Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

Last change on this file since 11071 was 11071, checked in by landauf, 8 years ago

merged branch cpp11_v3 back to trunk

  • Property svn:eol-style set to native
File size: 22.2 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        orxonox::Vector3 distance = targetposition - myposition;
354        float a = distance.squaredLength();
355        float b = 2 * (distance.x * targetvelocity.x + distance.y * targetvelocity.y + distance.z * targetvelocity.z);
356        float c = targetvelocity.squaredLength() - projectilespeed * projectilespeed;
357
358        float discriminant = b*b - 4*a*c;
359        if (discriminant < 0)
360            return orxonox::Vector3::ZERO; // solution is undefined (e.g. target faster than projectile)
361
362        if (a == 0.0f)
363            return orxonox::Vector3::ZERO; // avoid division by zero
364        float solution = (-b + sqrt(discriminant)) / (2 * a);
365
366        if (solution == 0.0f)
367            return orxonox::Vector3::ZERO; // avoid division by zero
368        float time = 1.0f / solution;
369
370        return (targetposition + targetvelocity * time);
371    }
372
373
374    namespace detail
375    {
376        std::mt19937 rngen;
377    }
378
379    /**
380        @brief Returns a unique number. This function will never return the same value twice.
381    */
382    unsigned long getUniqueNumber()
383    {
384        static unsigned long aNumber = 135;
385        return aNumber++;
386    }
387
388
389    //////////////////////////
390    // Conversion functions //
391    //////////////////////////
392
393    // std::string to Vector2
394    bool ConverterFallback<std::string, orxonox::Vector2>::convert(orxonox::Vector2* output, const std::string& input)
395    {
396        size_t opening_parenthesis, closing_parenthesis = input.find('}');
397        if ((opening_parenthesis = input.find('{')) == std::string::npos)
398            opening_parenthesis = 0;
399        else
400            opening_parenthesis++;
401
402        SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis),
403                         ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0');
404        if (tokens.size() >= 2)
405        {
406            if (!convertValue(&(output->x), tokens[0]))
407                return false;
408            if (!convertValue(&(output->y), tokens[1]))
409                return false;
410
411            return true;
412        }
413        return false;
414    }
415
416    // std::string to Vector3
417    bool ConverterFallback<std::string, orxonox::Vector3>::convert(orxonox::Vector3* output, const std::string& input)
418    {
419        size_t opening_parenthesis, closing_parenthesis = input.find('}');
420        if ((opening_parenthesis = input.find('{')) == std::string::npos)
421            opening_parenthesis = 0;
422        else
423            opening_parenthesis++;
424
425        SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis),
426                         ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0');
427        if (tokens.size() >= 3)
428        {
429            if (!convertValue(&(output->x), tokens[0]))
430                return false;
431            if (!convertValue(&(output->y), tokens[1]))
432                return false;
433            if (!convertValue(&(output->z), tokens[2]))
434                return false;
435
436            return true;
437        }
438        return false;
439    }
440
441    // std::string to Vector4
442    bool ConverterFallback<std::string, orxonox::Vector4>::convert(orxonox::Vector4* output, const std::string& input)
443    {
444        size_t opening_parenthesis, closing_parenthesis = input.find('}');
445        if ((opening_parenthesis = input.find('{')) == std::string::npos)
446            opening_parenthesis = 0;
447        else
448            opening_parenthesis++;
449
450        SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis),
451                         ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0');
452        if (tokens.size() >= 4)
453        {
454            if (!convertValue(&(output->x), tokens[0]))
455                return false;
456            if (!convertValue(&(output->y), tokens[1]))
457                return false;
458            if (!convertValue(&(output->z), tokens[2]))
459                return false;
460            if (!convertValue(&(output->w), tokens[3]))
461                return false;
462
463            return true;
464        }
465        return false;
466    }
467
468    // std::string to Quaternion
469    bool ConverterFallback<std::string, orxonox::Quaternion>::convert(orxonox::Quaternion* output, const std::string& input)
470    {
471        size_t opening_parenthesis, closing_parenthesis = input.find('}');
472        if ((opening_parenthesis = input.find('{')) == std::string::npos)
473            opening_parenthesis = 0;
474        else
475            opening_parenthesis++;
476
477        SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis), ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0');
478        if (tokens.size() >= 4)
479        {
480            if (!convertValue(&(output->w), tokens[0]))
481                return false;
482            if (!convertValue(&(output->x), tokens[1]))
483                return false;
484            if (!convertValue(&(output->y), tokens[2]))
485                return false;
486            if (!convertValue(&(output->z), tokens[3]))
487                return false;
488
489            return true;
490        }
491        return false;
492    }
493
494    // std::string to ColourValue
495    bool ConverterFallback<std::string, orxonox::ColourValue>::convert(orxonox::ColourValue* output, const std::string& input)
496    {
497        size_t opening_parenthesis, closing_parenthesis = input.find('}');
498        if ((opening_parenthesis = input.find('{')) == std::string::npos)
499            opening_parenthesis = 0;
500        else
501            opening_parenthesis++;
502
503        SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis), ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0');
504        if (tokens.size() >= 3)
505        {
506            if (!convertValue(&(output->r), tokens[0]))
507                return false;
508            if (!convertValue(&(output->g), tokens[1]))
509                return false;
510            if (!convertValue(&(output->b), tokens[2]))
511                return false;
512            if (tokens.size() >= 4)
513            {
514                if (!convertValue(&(output->a), tokens[3]))
515                    return false;
516            }
517            else
518                output->a = 1.0;
519
520            return true;
521        }
522        return false;
523    }
524}
Note: See TracBrowser for help on using the repository browser.