Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/libraries/util/Math.cc @ 10057

Last change on this file since 10057 was 9960, checked in by landauf, 11 years ago

fixed warning (msvc): the cast affected only '50' and not the whole expression. added braces and used a static_cast

  • Property svn:eol-style set to native
File size: 21.9 KB
Line 
1/*
2 *   ORXONOX - the hottest 3D action shooter ever to exist
3 *                    > www.orxonox.net <
4 *
5 *
6 *   License notice:
7 *
8 *   This program is free software; you can redistribute it and/or
9 *   modify it under the terms of the GNU General Public License
10 *   as published by the Free Software Foundation; either version 2
11 *   of the License, or (at your option) any later version.
12 *
13 *   This program is distributed in the hope that it will be useful,
14 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
15 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16 *   GNU General Public License for more details.
17 *
18 *   You should have received a copy of the GNU General Public License
19 *   along with this program; if not, write to the Free Software
20 *   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
21 *
22 *   Author:
23 *      Fabian 'x3n' Landau
24 *   Co-authors:
25 *      Wolfgang Roenninger
26 *
27 */
28
29/**
30    @file
31    @brief Implementation of several math-functions.
32*/
33
34#include "Math.h"
35
36#include <OgrePlane.h>
37
38#include "MathConvert.h"
39#include "SubString.h"
40
41namespace orxonox
42{
43#if OGRE_VERSION < 0x010603
44    /**
45        @brief Function for writing a Radian to a stream.
46    */
47    std::ostream& operator<<(std::ostream& out, const orxonox::Radian& radian)
48    {
49        out << radian.valueRadians();
50        return out;
51    }
52
53    /**
54        @brief Function for writing a Degree to a stream.
55    */
56    std::ostream& operator<<(std::ostream& out, const orxonox::Degree& degree)
57    {
58        out << degree.valueDegrees();
59        return out;
60    }
61#endif
62
63    /**
64        @brief Function for reading a Radian from a stream.
65    */
66    std::istream& operator>>(std::istream& in, orxonox::Radian& radian)
67    {
68        float temp;
69        in >> temp;
70        radian = temp;
71        return in;
72    }
73
74    /**
75        @brief Function for reading a Degree from a stream.
76    */
77    std::istream& operator>>(std::istream& in, orxonox::Degree& degree)
78    {
79        float temp;
80        in >> temp;
81        degree = temp;
82        return in;
83    }
84
85
86    /**
87        @brief Gets the angle between my viewing direction and the direction to the position of the other object.
88        @param myposition My position
89        @param mydirection My viewing direction
90        @param otherposition The position of the other object
91        @return The angle in radian
92
93        Examples:
94         - If the other object is exactly in front of me, the function returns 0.
95         - If the other object is exactly behind me, the function returns pi.
96         - If the other object is exactly right/left to me (or above/below), the function returns pi/2.
97    */
98    float getAngle(const orxonox::Vector3& myposition, const orxonox::Vector3& mydirection, const orxonox::Vector3& otherposition)
99    {
100        orxonox::Vector3 distance = otherposition - myposition;
101        float distancelength = distance.length();
102        if (distancelength == 0)
103            return 0;
104        else
105            return acos(clamp<float>(mydirection.dotProduct(distance) / distancelength, -1, 1));
106    }
107
108    /**
109        @brief Gets the 2D viewing direction (up/down, left/right) to the position of the other object.
110        @param myposition My position
111        @param mydirection My viewing direction
112        @param myorthonormal My orthonormalvector (pointing upwards through my head)
113        @param otherposition The position of the other object
114        @return The viewing direction
115
116        Examples:
117         - If the other object is exactly in front of me, the function returns <tt>Vector2(0, 0)</tt>.
118         - If the other object is exactly at my left, the function returns <tt>Vector2(-1, 0)</tt>.
119         - If the other object is exactly at my right, the function returns <tt>Vector2(1, 0)</tt>.
120         - If the other object is only a bit at my right, the function still returns <tt>Vector2(1, 0)</tt>.
121         - If the other object is exactly above me, the function returns <tt>Vector2(0, 1)</tt>.
122    */
123    orxonox::Vector2 get2DViewdirection(const orxonox::Vector3& myposition, const orxonox::Vector3& mydirection, const orxonox::Vector3& myorthonormal, const orxonox::Vector3& otherposition)
124    {
125        orxonox::Vector3 distance = otherposition - myposition;
126
127        // project difference vector on our plane
128        orxonox::Vector3 projection = Ogre::Plane(mydirection, myposition).projectVector(distance);
129
130        float projectionlength = projection.length();
131        if (projectionlength == 0)
132        {
133            if (myposition.dotProduct(otherposition) >= 0)
134                return orxonox::Vector2(0, 0);
135            else
136                return orxonox::Vector2(0, 1);
137        }
138
139        float cos_value = clamp<float>(myorthonormal.dotProduct(projection) / projectionlength, -1, 1);
140        float sin_value = sqrt( 1 - cos_value*cos_value );
141
142        if ((mydirection.crossProduct(myorthonormal)).dotProduct(distance) > 0)
143            return orxonox::Vector2( sin_value, cos_value );
144        else
145            return orxonox::Vector2( -sin_value, cos_value );
146    }
147
148    /**
149        @brief Gets the 2D viewing direction (up/down, left/right) to the position of the other object, multiplied with the viewing distance to the object (0° = 0, 180° = 1).
150        @param myposition My position
151        @param mydirection My viewing direction
152        @param myorthonormal My orthonormalvector (pointing upwards through my head)
153        @param otherposition The position of the other object
154        @return The viewing direction
155
156        Examples:
157         - If the other object is exactly in front of me, the function returns <tt>Vector2(0, 0)</tt>.
158         - If the other object is exactly at my left, the function returns <tt>Vector2(-0.5, 0)</tt>.
159         - If the other object is exactly at my right, the function returns <tt>Vector2(0.5, 0)</tt>.
160         - If the other object is only a bit at my right, the function still returns <tt>Vector2(0.01, 0)</tt>.
161         - If the other object is exactly above me, the function returns <tt>Vector2(0, 0.5)</tt>.
162    */
163    orxonox::Vector2 get2DViewcoordinates(const orxonox::Vector3& myposition, const orxonox::Vector3& mydirection, const orxonox::Vector3& myorthonormal, const orxonox::Vector3& otherposition)
164    {
165        orxonox::Vector3 distance = otherposition - myposition;
166
167        // project difference vector on our plane
168        orxonox::Vector3 projection = Ogre::Plane(mydirection, myposition).projectVector(distance);
169
170        float projectionlength = projection.length();
171        if (projectionlength == 0)
172        {
173            if (myposition.dotProduct(otherposition) >= 0)
174                return orxonox::Vector2(0, 0);
175            else
176                return orxonox::Vector2(0, 1);
177        }
178        //float angle = acos(clamp<float>(myorthonormal.dotProduct(projection) / projectionlength, -1, 1));
179
180        float cos_value = clamp<float>(myorthonormal.dotProduct(projection) / projectionlength, -1, 1);
181        float sin_value = sqrt( 1 - cos_value*cos_value );
182
183        float distancelength = distance.length();
184        if (distancelength == 0) return orxonox::Vector2(0, 0);
185        float radius = acos(clamp<float>(mydirection.dotProduct(distance) / distancelength, -1, 1)) / math::pi;
186
187        if ((mydirection.crossProduct(myorthonormal)).dotProduct(distance) > 0)
188            return orxonox::Vector2( sin_value * radius, cos_value * radius);
189        else
190            return orxonox::Vector2( -sin_value * radius, cos_value * radius);
191    }
192
193
194    /**
195            @brief Gets the 2D project vector for the 3D Radar .
196            @param myposition My position
197            @param mydirection My viewing direction
198            @param myorthonormal My orthonormalvector (pointing upwards through my head)
199            @param otherposition The position of the other object
200            @param mapangle The angle between line of sight on screen and the 3Dmap-x/z-plain in radian
201            @param detectionlimit The limit in which objects are shown on the map
202            @return The viewing direction
203    */
204    orxonox::Vector2 get3DProjection(const orxonox::Vector3& myposition, const orxonox::Vector3& mydirection, const orxonox::Vector3& myorthonormal, const orxonox::Vector3& otherposition, const float mapangle, const float detectionlimit)
205    {
206        // Orxonox Vectors: x_direction you are looking, y_direction points up, z_direction points to the right
207        orxonox::Vector3 distance = otherposition - myposition; // get vector from Ship to object
208
209        // new coordinate system: x_axsis: mydirection   (points front)
210        //                        y_axsis: myorthonormal (points up)
211        //                        z_axsis: myside        (points right)
212
213        orxonox::Vector3 myside = mydirection.crossProduct(myorthonormal); // get 3. base vector
214
215        distance = 4*distance / detectionlimit; // shrink vector on map
216        if(distance.length() > 1.0f) // if object would wander outside of the map
217        {
218            distance = distance / distance.length();
219        }
220
221        // perform a coordinate transformation to get distance in relation of the position of the ship
222        orxonox::Vector3 distanceShip = getTransformedVector(distance, mydirection, myorthonormal, myside);
223
224        // calculate 2D vector on the map (with angle between x/z - plain and line of sight)
225        float xcoordinate = distanceShip.z; // z; cause x direction on screen is to the right side
226        float ycoordinate = distanceShip.x*sin(mapangle)+distanceShip.y*cos(mapangle);
227        return orxonox::Vector2(xcoordinate , ycoordinate);
228    }
229
230    /**
231               @brief Gets if a object is over the x/z - plain on map
232               @param myposition My position
233               @param mydirection My viewing direction
234               @param myorthonormal My orthonormalvector (pointing upwards through my head)
235               @param otherposition The position of the other object
236               @param mapangle The angle you look on the 3Dmap in radian
237               @return If distancevector to the other object has a positive y-coordinate
238
239               Examples:
240                Returns true if object is over x/z - plain
241                Returns false if object is below x/z -plain
242    */
243    bool isObjectHigherThanShipOnMap(const orxonox::Vector3& myposition, const orxonox::Vector3& mydirection, const orxonox::Vector3& myorthonormal, const orxonox::Vector3& otherposition, const float mapangle)
244    {
245        // Orxonox Vectors: x_direction you are looking, y_direction points up, z_direction points to the right
246        orxonox::Vector3 distance = otherposition - myposition;
247
248        // new coordinate system: x_axsis: mydirection   (points front)
249        //                        y_axsis: myorthonormal (points up)
250        //                        z_axsis: myside        (points right)
251
252        orxonox::Vector3 myside = mydirection.crossProduct(myorthonormal); // get vector from Ship to object
253
254
255        // perform a coordinate transformation to get distance in relation of the position of the ship
256        orxonox::Vector3 distanceShip = getTransformedVector(distance, mydirection, myorthonormal, myside);
257
258        if(distanceShip.y >= 0)
259            return true;
260        else
261            return false;
262    }
263
264    /**
265                   @brief A value between 0 and 10, in order how other object is in front or in back
266                   @param myposition My position
267                   @param mydirection My viewing direction
268                   @param myorthonormal My orthonormalvector (pointing upwards through my head)
269                   @param otherposition The position of the other object
270                   @param detectionlimit The limit in which objects are shown on the map
271                   @return value between 0 and 100
272    */
273    int determineMap3DZOrder(const orxonox::Vector3& myposition, const orxonox::Vector3& mydirection, const orxonox::Vector3& myorthonormal, const orxonox::Vector3& otherposition, const float detectionlimit)
274    {
275        orxonox::Vector3 distance = otherposition - myposition; // get vector from Ship to object
276        orxonox::Vector3 myside = mydirection.crossProduct(myorthonormal); // get vector to the side
277
278        distance = 4*distance / detectionlimit; // shrink vector on map
279        if(distance.length() > 1.0f) // if object would wander outside of the map
280        {
281            distance = distance / distance.length();
282        }
283
284        // perform a coordinate transformation to get distance in relation of the position of the ship
285        orxonox::Vector3 distanceShip = getTransformedVector(distance, mydirection, myorthonormal, myside);
286
287        return static_cast<int>(50 - 100*distanceShip.x);
288    }
289
290
291    /**
292                @brief Gets the new vector after a coordinate transformation
293                @param distance Vector which will be transformed
294                @param mydirection New x basevector
295                @param myorthonormal New y basevector
296                @param otherposition New z basevector
297                @return direction in the new coordinates
298
299                x is vector in old coordinates
300                y is vector in old coordinates
301                T is transform matrix with:
302                    T = (t1 , t2 , t3)
303                    t1 = mydirection
304                    t2 = myorthonormal
305                    t3 = myside
306
307                y = T^(-1)*x
308            */
309    orxonox::Vector3 getTransformedVector(const orxonox::Vector3& distance, const orxonox::Vector3& mydirection, const orxonox::Vector3& myorthonormal, const orxonox::Vector3& myside)
310    {
311        // inverse of the transform matrix
312        float determinant = +mydirection.x * (myorthonormal.y*myside.z - myside.y*myorthonormal.z)
313                            -mydirection.y * (myorthonormal.x*myside.z - myorthonormal.z*myside.x)
314                            +mydirection.z * (myorthonormal.x*myside.y - myorthonormal.y*myside.x);
315        float invdet = 1/determinant;
316
317        // transform matrix
318        orxonox::Vector3 xinvtransform;
319        orxonox::Vector3 yinvtransform;
320        orxonox::Vector3 zinvtransform;
321
322        xinvtransform.x = (myorthonormal.y * myside.z        - myside.y        * myorthonormal.z)*invdet;
323        xinvtransform.y = (mydirection.z   * myside.y        - mydirection.y   * myside.z       )*invdet;
324        xinvtransform.z = (mydirection.y   * myorthonormal.z - mydirection.z   * myorthonormal.y)*invdet;
325        yinvtransform.x = (myorthonormal.z * myside.x        - myorthonormal.x * myside.z       )*invdet;
326        yinvtransform.y = (mydirection.x   * myside.z        - mydirection.z   * myside.x       )*invdet;
327        yinvtransform.z = (myorthonormal.x * mydirection.z   - mydirection.x   * myorthonormal.z)*invdet;
328        zinvtransform.x = (myorthonormal.x * myside.y        - myside.x        * myorthonormal.y)*invdet;
329        zinvtransform.y = (myside.x        * mydirection.y   - mydirection.x   * myside.y       )*invdet;
330        zinvtransform.z = (mydirection.x   * myorthonormal.y - myorthonormal.x * mydirection.)*invdet;
331
332        // coordinate transformation
333        orxonox::Vector3 distanceShip;
334        distanceShip.x = xinvtransform.x * distance.x + yinvtransform.x * distance.y + zinvtransform.x * distance.z;
335        distanceShip.y = xinvtransform.y * distance.x + yinvtransform.y * distance.y + zinvtransform.y * distance.z;
336        distanceShip.z = xinvtransform.z * distance.x + yinvtransform.z * distance.y + zinvtransform.z * distance.z;
337
338        return distanceShip;
339    }
340
341    /**
342        @brief Returns the predicted position I have to aim at, if I want to hit a moving target with a moving projectile.
343        @param myposition My position
344        @param projectilespeed The speed of my projectile
345        @param targetposition The position of my target
346        @param targetvelocity The velocity of my target
347        @return The predicted position
348
349        The function predicts the position based on a linear velocity of the target. If the target changes speed or direction, the projectile will miss.
350    */
351    orxonox::Vector3 getPredictedPosition(const orxonox::Vector3& myposition, float projectilespeed, const orxonox::Vector3& targetposition, const orxonox::Vector3& targetvelocity)
352    {
353        float squaredProjectilespeed = projectilespeed * projectilespeed;
354        orxonox::Vector3 distance = targetposition - myposition;
355        float a = distance.squaredLength();
356        float b = 2 * (distance.x + distance.y + distance.z) * (targetvelocity.x + targetvelocity.y + targetvelocity.z);
357        float c = targetvelocity.squaredLength();
358
359        float temp = 4*squaredProjectilespeed*c + a*a - 4*b*c;
360        if (temp < 0)
361            return orxonox::Vector3::ZERO;
362
363        temp = sqrt(temp);
364        float time = (temp + a) / (2 * (squaredProjectilespeed - b));
365        return (targetposition + targetvelocity * time);
366    }
367
368    /**
369        @brief Returns a unique number. This function will never return the same value twice.
370    */
371    unsigned long getUniqueNumber()
372    {
373        static unsigned long aNumber = 135;
374        return aNumber++;
375    }
376
377
378    //////////////////////////
379    // Conversion functions //
380    //////////////////////////
381
382    // std::string to Vector2
383    bool ConverterFallback<std::string, orxonox::Vector2>::convert(orxonox::Vector2* output, const std::string& input)
384    {
385        size_t opening_parenthesis, closing_parenthesis = input.find('}');
386        if ((opening_parenthesis = input.find('{')) == std::string::npos)
387            opening_parenthesis = 0;
388        else
389            opening_parenthesis++;
390
391        SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis),
392                         ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0');
393        if (tokens.size() >= 2)
394        {
395            if (!convertValue(&(output->x), tokens[0]))
396                return false;
397            if (!convertValue(&(output->y), tokens[1]))
398                return false;
399
400            return true;
401        }
402        return false;
403    }
404
405    // std::string to Vector3
406    bool ConverterFallback<std::string, orxonox::Vector3>::convert(orxonox::Vector3* output, const std::string& input)
407    {
408        size_t opening_parenthesis, closing_parenthesis = input.find('}');
409        if ((opening_parenthesis = input.find('{')) == std::string::npos)
410            opening_parenthesis = 0;
411        else
412            opening_parenthesis++;
413
414        SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis),
415                         ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0');
416        if (tokens.size() >= 3)
417        {
418            if (!convertValue(&(output->x), tokens[0]))
419                return false;
420            if (!convertValue(&(output->y), tokens[1]))
421                return false;
422            if (!convertValue(&(output->z), tokens[2]))
423                return false;
424
425            return true;
426        }
427        return false;
428    }
429
430    // std::string to Vector4
431    bool ConverterFallback<std::string, orxonox::Vector4>::convert(orxonox::Vector4* output, const std::string& input)
432    {
433        size_t opening_parenthesis, closing_parenthesis = input.find('}');
434        if ((opening_parenthesis = input.find('{')) == std::string::npos)
435            opening_parenthesis = 0;
436        else
437            opening_parenthesis++;
438
439        SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis),
440                         ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0');
441        if (tokens.size() >= 4)
442        {
443            if (!convertValue(&(output->x), tokens[0]))
444                return false;
445            if (!convertValue(&(output->y), tokens[1]))
446                return false;
447            if (!convertValue(&(output->z), tokens[2]))
448                return false;
449            if (!convertValue(&(output->w), tokens[3]))
450                return false;
451
452            return true;
453        }
454        return false;
455    }
456
457    // std::string to Quaternion
458    bool ConverterFallback<std::string, orxonox::Quaternion>::convert(orxonox::Quaternion* output, const std::string& input)
459    {
460        size_t opening_parenthesis, closing_parenthesis = input.find('}');
461        if ((opening_parenthesis = input.find('{')) == std::string::npos)
462            opening_parenthesis = 0;
463        else
464            opening_parenthesis++;
465
466        SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis), ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0');
467        if (tokens.size() >= 4)
468        {
469            if (!convertValue(&(output->w), tokens[0]))
470                return false;
471            if (!convertValue(&(output->x), tokens[1]))
472                return false;
473            if (!convertValue(&(output->y), tokens[2]))
474                return false;
475            if (!convertValue(&(output->z), tokens[3]))
476                return false;
477
478            return true;
479        }
480        return false;
481    }
482
483    // std::string to ColourValue
484    bool ConverterFallback<std::string, orxonox::ColourValue>::convert(orxonox::ColourValue* output, const std::string& input)
485    {
486        size_t opening_parenthesis, closing_parenthesis = input.find('}');
487        if ((opening_parenthesis = input.find('{')) == std::string::npos)
488            opening_parenthesis = 0;
489        else
490            opening_parenthesis++;
491
492        SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis), ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0');
493        if (tokens.size() >= 3)
494        {
495            if (!convertValue(&(output->r), tokens[0]))
496                return false;
497            if (!convertValue(&(output->g), tokens[1]))
498                return false;
499            if (!convertValue(&(output->b), tokens[2]))
500                return false;
501            if (tokens.size() >= 4)
502            {
503                if (!convertValue(&(output->a), tokens[3]))
504                    return false;
505            }
506            else
507                output->a = 1.0;
508
509            return true;
510        }
511        return false;
512    }
513}
Note: See TracBrowser for help on using the repository browser.