source: OGRE/trunk/ogrenew/OgreMain/src/OgreMath.cpp @ 657

Revision 657, 23.9 KB checked in by mattausch, 19 years ago (diff)

added ogre dependencies and patched ogre sources
Line 
1/*
2-----------------------------------------------------------------------------
3This source file is part of OGRE
4    (Object-oriented Graphics Rendering Engine)
5For the latest info, see http://www.ogre3d.org/
6
7Copyright (c) 2000-2005 The OGRE Team
8Also see acknowledgements in Readme.html
9
10This program is free software; you can redistribute it and/or modify it under
11the terms of the GNU Lesser General Public License as published by the Free Software
12Foundation; either version 2 of the License, or (at your option) any later
13version.
14
15This program is distributed in the hope that it will be useful, but WITHOUT
16ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
17FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.
18
19You should have received a copy of the GNU Lesser General Public License along with
20this program; if not, write to the Free Software Foundation, Inc., 59 Temple
21Place - Suite 330, Boston, MA 02111-1307, USA, or go to
22http://www.gnu.org/copyleft/lesser.txt.
23-----------------------------------------------------------------------------
24*/
25#include "OgreStableHeaders.h"
26
27#include "OgreMath.h"
28#include "asm_math.h"
29#include "OgreVector3.h"
30#include "OgreVector4.h"
31#include "OgreRay.h"
32#include "OgreSphere.h"
33#include "OgreAxisAlignedBox.h"
34#include "OgrePlane.h"
35
36
37namespace Ogre
38{
39
40    const Real Math::POS_INFINITY = std::numeric_limits<Real>::infinity();
41    const Real Math::NEG_INFINITY = -std::numeric_limits<Real>::infinity();
42    const Real Math::PI = Real( 4.0 * atan( 1.0 ) );
43    const Real Math::TWO_PI = Real( 2.0 * PI );
44    const Real Math::HALF_PI = Real( 0.5 * PI );
45        const Real Math::fDeg2Rad = PI / Real(180.0);
46        const Real Math::fRad2Deg = Real(180.0) / PI;
47
48    int Math::mTrigTableSize;
49   Math::AngleUnit Math::msAngleUnit;
50
51    Real  Math::mTrigTableFactor;
52    Real *Math::mSinTable = NULL;
53    Real *Math::mTanTable = NULL;
54
55    //-----------------------------------------------------------------------
56    Math::Math( unsigned int trigTableSize )
57    {
58        msAngleUnit = AU_DEGREE;
59
60        mTrigTableSize = trigTableSize;
61        mTrigTableFactor = mTrigTableSize / Math::TWO_PI;
62
63        mSinTable = new Real[mTrigTableSize];
64        mTanTable = new Real[mTrigTableSize];
65
66        buildTrigTables();
67    }
68
69    //-----------------------------------------------------------------------
70    Math::~Math()
71    {
72        delete [] mSinTable;
73        delete [] mTanTable;
74    }
75
76    //-----------------------------------------------------------------------
77    void Math::buildTrigTables(void)
78    {
79        // Build trig lookup tables
80        // Could get away with building only PI sized Sin table but simpler this
81        // way. Who cares, it'll ony use an extra 8k of memory anyway and I like
82        // simplicity.
83        Real angle;
84        for (int i = 0; i < mTrigTableSize; ++i)
85        {
86            angle = Math::TWO_PI * i / mTrigTableSize;
87            mSinTable[i] = sin(angle);
88            mTanTable[i] = tan(angle);
89        }
90    }
91        //-----------------------------------------------------------------------       
92        Real Math::SinTable (Real fValue)
93    {
94        // Convert range to index values, wrap if required
95        int idx;
96        if (fValue >= 0)
97        {
98            idx = int(fValue * mTrigTableFactor) % mTrigTableSize;
99        }
100        else
101        {
102            idx = mTrigTableSize - (int(-fValue * mTrigTableFactor) % mTrigTableSize) - 1;
103        }
104
105        return mSinTable[idx];
106    }
107        //-----------------------------------------------------------------------
108        Real Math::TanTable (Real fValue)
109    {
110        // Convert range to index values, wrap if required
111                int idx = int(fValue *= mTrigTableFactor) % mTrigTableSize;
112                return mTanTable[idx];
113    }
114    //-----------------------------------------------------------------------
115    int Math::ISign (int iValue)
116    {
117        return ( iValue > 0 ? +1 : ( iValue < 0 ? -1 : 0 ) );
118    }
119    //-----------------------------------------------------------------------
120    Radian Math::ACos (Real fValue)
121    {
122        if ( -1.0 < fValue )
123        {
124            if ( fValue < 1.0 )
125                return Radian(acos(fValue));
126            else
127                return Radian(0.0);
128        }
129        else
130        {
131            return Radian(PI);
132        }
133    }
134    //-----------------------------------------------------------------------
135    Radian Math::ASin (Real fValue)
136    {
137        if ( -1.0 < fValue )
138        {
139            if ( fValue < 1.0 )
140                return Radian(asin(fValue));
141            else
142                return Radian(HALF_PI);
143        }
144        else
145        {
146            return Radian(-HALF_PI);
147        }
148    }
149    //-----------------------------------------------------------------------
150    Real Math::Sign (Real fValue)
151    {
152        if ( fValue > 0.0 )
153            return 1.0;
154
155        if ( fValue < 0.0 )
156            return -1.0;
157
158        return 0.0;
159    }
160        //-----------------------------------------------------------------------
161        Real Math::InvSqrt(Real fValue)
162        {
163                return Real(asm_rsq(fValue));
164        }
165    //-----------------------------------------------------------------------
166    Real Math::UnitRandom ()
167    {
168        return asm_rand() / asm_rand_max();
169    }
170   
171    //-----------------------------------------------------------------------
172    Real Math::RangeRandom (Real fLow, Real fHigh)
173    {
174        return (fHigh-fLow)*UnitRandom() + fLow;
175    }
176
177    //-----------------------------------------------------------------------
178    Real Math::SymmetricRandom ()
179    {
180                return 2.0f * UnitRandom() - 1.0f;
181    }
182
183   //-----------------------------------------------------------------------
184    void Math::setAngleUnit(Math::AngleUnit unit)
185   {
186       msAngleUnit = unit;
187   }
188   //-----------------------------------------------------------------------
189   Math::AngleUnit Math::getAngleUnit(void)
190   {
191       return msAngleUnit;
192   }
193    //-----------------------------------------------------------------------
194    Real Math::AngleUnitsToRadians(Real angleunits)
195    {
196       if (msAngleUnit == AU_DEGREE)
197           return angleunits * fDeg2Rad;
198       else
199           return angleunits;
200    }
201
202    //-----------------------------------------------------------------------
203    Real Math::RadiansToAngleUnits(Real radians)
204    {
205       if (msAngleUnit == AU_DEGREE)
206           return radians * fRad2Deg;
207       else
208           return radians;
209    }
210
211    //-----------------------------------------------------------------------
212    Real Math::AngleUnitsToDegrees(Real angleunits)
213    {
214       if (msAngleUnit == AU_RADIAN)
215           return angleunits * fRad2Deg;
216       else
217           return angleunits;
218    }
219
220    //-----------------------------------------------------------------------
221    Real Math::DegreesToAngleUnits(Real degrees)
222    {
223       if (msAngleUnit == AU_RADIAN)
224           return degrees * fDeg2Rad;
225       else
226           return degrees;
227    }
228
229    //-----------------------------------------------------------------------
230    bool Math::pointInTri2D( Real px, Real py, Real ax, Real ay, Real bx, Real by, Real cx, Real cy )
231    {
232        Real v1x, v2x, v1y, v2y;
233        bool bClockwise;
234
235        v1x = bx - ax;
236        v1y = by - ay;
237
238        v2x = px - bx;
239        v2y = py - by;
240
241        // For the sake of readability
242        #define Clockwise ( v1x * v2y - v1y * v2x >= 0.0 )
243
244        bClockwise = Clockwise;
245
246        v1x = cx - bx;
247        v1y = cy - by;
248
249        v2x = px - cx;
250        v2y = py - cy;
251
252        if( Clockwise != bClockwise )
253            return false;
254
255        v1x = ax - cx;
256        v1y = ay - cy;
257
258        v2x = px - ax;
259        v2y = py - ay;
260
261        if( Clockwise != bClockwise )
262            return false;
263
264        return true;
265
266        // Clean up the #defines
267        #undef Clockwise
268    }
269
270    //-----------------------------------------------------------------------
271    bool Math::RealEqual( Real a, Real b, Real tolerance )
272    {
273        if (fabs(b-a) <= tolerance)
274            return true;
275        else
276            return false;
277    }
278
279    //-----------------------------------------------------------------------
280    std::pair<bool, Real> Math::intersects(const Ray& ray, const Plane& plane)
281    {
282
283        Real denom = plane.normal.dotProduct(ray.getDirection());
284        if (Math::Abs(denom) < std::numeric_limits<Real>::epsilon())
285        {
286            // Parallel
287            return std::pair<bool, Real>(false, 0);
288        }
289        else
290        {
291            Real nom = plane.normal.dotProduct(ray.getOrigin()) + plane.d;
292            Real t = -(nom/denom);
293            return std::pair<bool, Real>(t >= 0, t);
294        }
295       
296    }
297    //-----------------------------------------------------------------------
298    std::pair<bool, Real> Math::intersects(const Ray& ray,
299        const std::vector<Plane>& planes, bool normalIsOutside)
300    {
301        std::vector<Plane>::const_iterator planeit, planeitend;
302        planeitend = planes.end();
303        bool allInside = true;
304        std::pair<bool, Real> ret;
305        ret.first = false;
306        ret.second = 0.0f;
307
308        // derive side
309        // NB we don't pass directly since that would require Plane::Side in
310        // interface, which results in recursive includes since Math is so fundamental
311        Plane::Side outside = normalIsOutside ? Plane::POSITIVE_SIDE : Plane::NEGATIVE_SIDE;
312
313        for (planeit = planes.begin(); planeit != planeitend; ++planeit)
314        {
315            const Plane& plane = *planeit;
316            // is origin outside?
317            if (plane.getSide(ray.getOrigin()) == outside)
318            {
319                allInside = false;
320                // Test single plane
321                std::pair<bool, Real> planeRes =
322                    ray.intersects(plane);
323                if (planeRes.first)
324                {
325                    // Ok, we intersected
326                    ret.first = true;
327                    // Use the most distant result since convex volume
328                    ret.second = std::max(ret.second, planeRes.second);
329                }
330            }
331        }
332
333        if (allInside)
334        {
335            // Intersecting at 0 distance since inside the volume!
336            ret.first = true;
337            ret.second = 0.0f;
338        }
339
340        return ret;
341    }
342    //-----------------------------------------------------------------------
343    std::pair<bool, Real> Math::intersects(const Ray& ray,
344        const std::list<Plane>& planes, bool normalIsOutside)
345    {
346        std::list<Plane>::const_iterator planeit, planeitend;
347        planeitend = planes.end();
348        bool allInside = true;
349        std::pair<bool, Real> ret;
350        ret.first = false;
351        ret.second = 0.0f;
352
353        // derive side
354        // NB we don't pass directly since that would require Plane::Side in
355        // interface, which results in recursive includes since Math is so fundamental
356        Plane::Side outside = normalIsOutside ? Plane::POSITIVE_SIDE : Plane::NEGATIVE_SIDE;
357
358        for (planeit = planes.begin(); planeit != planeitend; ++planeit)
359        {
360            const Plane& plane = *planeit;
361            // is origin outside?
362            if (plane.getSide(ray.getOrigin()) == outside)
363            {
364                allInside = false;
365                // Test single plane
366                std::pair<bool, Real> planeRes =
367                    ray.intersects(plane);
368                if (planeRes.first)
369                {
370                    // Ok, we intersected
371                    ret.first = true;
372                    // Use the most distant result since convex volume
373                    ret.second = std::max(ret.second, planeRes.second);
374                }
375            }
376        }
377
378        if (allInside)
379        {
380            // Intersecting at 0 distance since inside the volume!
381            ret.first = true;
382            ret.second = 0.0f;
383        }
384
385        return ret;
386    }
387    //-----------------------------------------------------------------------
388    std::pair<bool, Real> Math::intersects(const Ray& ray, const Sphere& sphere,
389        bool discardInside)
390    {
391        const Vector3& raydir = ray.getDirection();
392        // Adjust ray origin relative to sphere center
393        const Vector3& rayorig = ray.getOrigin() - sphere.getCenter();
394        Real radius = sphere.getRadius();
395
396        // Check origin inside first
397        if (rayorig.squaredLength() <= radius*radius && discardInside)
398        {
399            return std::pair<bool, Real>(true, 0);
400        }
401
402        // Mmm, quadratics
403        // Build coeffs which can be used with std quadratic solver
404        // ie t = (-b +/- sqrt(b*b + 4ac)) / 2a
405        Real a = raydir.dotProduct(raydir);
406        Real b = 2 * rayorig.dotProduct(raydir);
407        Real c = rayorig.dotProduct(rayorig) - radius*radius;
408
409        // Calc determinant
410        Real d = (b*b) - (4 * a * c);
411        if (d < 0)
412        {
413            // No intersection
414            return std::pair<bool, Real>(false, 0);
415        }
416        else
417        {
418            // BTW, if d=0 there is one intersection, if d > 0 there are 2
419            // But we only want the closest one, so that's ok, just use the
420            // '-' version of the solver
421            Real t = ( -b - Math::Sqrt(d) ) / (2 * a);
422            if (t < 0)
423                t = ( -b + Math::Sqrt(d) ) / (2 * a);
424            return std::pair<bool, Real>(true, t);
425        }
426
427
428    }
429    //-----------------------------------------------------------------------
430    std::pair<bool, Real> Math::intersects(const Ray& ray, const AxisAlignedBox& box)
431    {
432        if (box.isNull()) return std::pair<bool, Real>(false, 0);
433
434        Real lowt = 0.0f;
435        Real t;
436        bool hit = false;
437        Vector3 hitpoint;
438        const Vector3& min = box.getMinimum();
439        const Vector3& max = box.getMaximum();
440        const Vector3& rayorig = ray.getOrigin();
441        const Vector3& raydir = ray.getDirection();
442
443        // Check origin inside first
444        if ( rayorig > min && rayorig < max )
445        {
446            return std::pair<bool, Real>(true, 0);
447        }
448
449        // Check each face in turn, only check closest 3
450        // Min x
451        if (rayorig.x < min.x && raydir.x > 0)
452        {
453            t = (min.x - rayorig.x) / raydir.x;
454            if (t > 0)
455            {
456                // Substitute t back into ray and check bounds and dist
457                hitpoint = rayorig + raydir * t;
458                if (hitpoint.y >= min.y && hitpoint.y <= max.y &&
459                    hitpoint.z >= min.z && hitpoint.z <= max.z &&
460                    (!hit || t < lowt))
461                {
462                    hit = true;
463                    lowt = t;
464                }
465            }
466        }
467        // Max x
468        if (rayorig.x > max.x && raydir.x < 0)
469        {
470            t = (max.x - rayorig.x) / raydir.x;
471            if (t > 0)
472            {
473                // Substitute t back into ray and check bounds and dist
474                hitpoint = rayorig + raydir * t;
475                if (hitpoint.y >= min.y && hitpoint.y <= max.y &&
476                    hitpoint.z >= min.z && hitpoint.z <= max.z &&
477                    (!hit || t < lowt))
478                {
479                    hit = true;
480                    lowt = t;
481                }
482            }
483        }
484        // Min y
485        if (rayorig.y < min.y && raydir.y > 0)
486        {
487            t = (min.y - rayorig.y) / raydir.y;
488            if (t > 0)
489            {
490                // Substitute t back into ray and check bounds and dist
491                hitpoint = rayorig + raydir * t;
492                if (hitpoint.x >= min.x && hitpoint.x <= max.x &&
493                    hitpoint.z >= min.z && hitpoint.z <= max.z &&
494                    (!hit || t < lowt))
495                {
496                    hit = true;
497                    lowt = t;
498                }
499            }
500        }
501        // Max y
502        if (rayorig.y > max.y && raydir.y < 0)
503        {
504            t = (max.y - rayorig.y) / raydir.y;
505            if (t > 0)
506            {
507                // Substitute t back into ray and check bounds and dist
508                hitpoint = rayorig + raydir * t;
509                if (hitpoint.x >= min.x && hitpoint.x <= max.x &&
510                    hitpoint.z >= min.z && hitpoint.z <= max.z &&
511                    (!hit || t < lowt))
512                {
513                    hit = true;
514                    lowt = t;
515                }
516            }
517        }
518        // Min z
519        if (rayorig.z < min.z && raydir.z > 0)
520        {
521            t = (min.z - rayorig.z) / raydir.z;
522            if (t > 0)
523            {
524                // Substitute t back into ray and check bounds and dist
525                hitpoint = rayorig + raydir * t;
526                if (hitpoint.x >= min.x && hitpoint.x <= max.x &&
527                    hitpoint.y >= min.y && hitpoint.y <= max.y &&
528                    (!hit || t < lowt))
529                {
530                    hit = true;
531                    lowt = t;
532                }
533            }
534        }
535        // Max z
536        if (rayorig.z > max.z && raydir.z < 0)
537        {
538            t = (max.z - rayorig.z) / raydir.z;
539            if (t > 0)
540            {
541                // Substitute t back into ray and check bounds and dist
542                hitpoint = rayorig + raydir * t;
543                if (hitpoint.x >= min.x && hitpoint.x <= max.x &&
544                    hitpoint.y >= min.y && hitpoint.y <= max.y &&
545                    (!hit || t < lowt))
546                {
547                    hit = true;
548                    lowt = t;
549                }
550            }
551        }
552
553        return std::pair<bool, Real>(hit, lowt);
554
555    }
556    //-----------------------------------------------------------------------
557    bool Math::intersects(const Sphere& sphere, const AxisAlignedBox& box)
558    {
559        if (box.isNull()) return false;
560
561        // Use splitting planes
562        const Vector3& center = sphere.getCenter();
563        Real radius = sphere.getRadius();
564        const Vector3& min = box.getMinimum();
565        const Vector3& max = box.getMaximum();
566
567        // just test facing planes, early fail if sphere is totally outside
568        if (center.x < min.x &&
569            min.x - center.x > radius)
570        {
571            return false;
572        }
573        if (center.x > max.x &&
574            center.x  - max.x > radius)
575        {
576            return false;
577        }
578
579        if (center.y < min.y &&
580            min.y - center.y > radius)
581        {
582            return false;
583        }
584        if (center.y > max.y &&
585            center.y  - max.y > radius)
586        {
587            return false;
588        }
589
590        if (center.z < min.z &&
591            min.z - center.z > radius)
592        {
593            return false;
594        }
595        if (center.z > max.z &&
596            center.z  - max.z > radius)
597        {
598            return false;
599        }
600
601        // Must intersect
602        return true;
603
604    }
605    //-----------------------------------------------------------------------
606    bool Math::intersects(const Plane& plane, const AxisAlignedBox& box)
607    {
608        if (box.isNull()) return false;
609
610        // Get corners of the box
611        const Vector3* pCorners = box.getAllCorners();
612
613
614        // Test which side of the plane the corners are
615        // Intersection occurs when at least one corner is on the
616        // opposite side to another
617        Plane::Side lastSide = plane.getSide(pCorners[0]);
618        for (int corner = 1; corner < 8; ++corner)
619        {
620            if (plane.getSide(pCorners[corner]) != lastSide)
621            {
622                return true;
623            }
624        }
625
626        return false;
627    }
628    //-----------------------------------------------------------------------
629    bool Math::intersects(const Sphere& sphere, const Plane& plane)
630    {
631        return (
632            Math::Abs(plane.normal.dotProduct(sphere.getCenter()))
633            <= sphere.getRadius() );
634    }
635    //-----------------------------------------------------------------------
636    Vector3 Math::calculateTangentSpaceVector(
637        const Vector3& position1, const Vector3& position2, const Vector3& position3,
638        Real u1, Real v1, Real u2, Real v2, Real u3, Real v3)
639    {
640            //side0 is the vector along one side of the triangle of vertices passed in,
641            //and side1 is the vector along another side. Taking the cross product of these returns the normal.
642            Vector3 side0 = position1 - position2;
643            Vector3 side1 = position3 - position1;
644            //Calculate face normal
645            Vector3 normal = side1.crossProduct(side0);
646            normal.normalise();
647            //Now we use a formula to calculate the tangent.
648            Real deltaV0 = v1 - v2;
649            Real deltaV1 = v3 - v1;
650            Vector3 tangent = deltaV1 * side0 - deltaV0 * side1;
651            tangent.normalise();
652            //Calculate binormal
653            Real deltaU0 = u1 - u2;
654            Real deltaU1 = u3 - u1;
655            Vector3 binormal = deltaU1 * side0 - deltaU0 * side1;
656            binormal.normalise();
657            //Now, we take the cross product of the tangents to get a vector which
658            //should point in the same direction as our normal calculated above.
659            //If it points in the opposite direction (the dot product between the normals is less than zero),
660            //then we need to reverse the s and t tangents.
661            //This is because the triangle has been mirrored when going from tangent space to object space.
662            //reverse tangents if necessary
663            Vector3 tangentCross = tangent.crossProduct(binormal);
664            if (tangentCross.dotProduct(normal) < 0.0f)
665            {
666                    tangent = -tangent;
667                    binormal = -binormal;
668            }
669
670        return tangent;
671
672    }
673    //-----------------------------------------------------------------------
674    Matrix4 Math::buildReflectionMatrix(const Plane& p)
675    {
676        return Matrix4(
677            -2 * p.normal.x * p.normal.x + 1,   -2 * p.normal.x * p.normal.y,       -2 * p.normal.x * p.normal.z,       -2 * p.normal.x * p.d,
678            -2 * p.normal.y * p.normal.x,       -2 * p.normal.y * p.normal.y + 1,   -2 * p.normal.y * p.normal.z,       -2 * p.normal.y * p.d,
679            -2 * p.normal.z * p.normal.x,       -2 * p.normal.z * p.normal.y,       -2 * p.normal.z * p.normal.z + 1,   -2 * p.normal.z * p.d,
680            0,                                  0,                                  0,                                  1);
681    }
682    //-----------------------------------------------------------------------
683    Vector4 Math::calculateFaceNormal(const Vector3& v1, const Vector3& v2, const Vector3& v3)
684    {
685        Vector3 normal = calculateBasicFaceNormal(v1, v2, v3);
686        // Now set up the w (distance of tri from origin
687        return Vector4(normal.x, normal.y, normal.z, -(normal.dotProduct(v1)));
688    }
689    //-----------------------------------------------------------------------
690    Vector3 Math::calculateBasicFaceNormal(const Vector3& v1, const Vector3& v2, const Vector3& v3)
691    {
692        Vector3 normal = (v2 - v1).crossProduct(v3 - v1);
693        normal.normalise();
694        return normal;
695    }
696    //-----------------------------------------------------------------------
697    Vector4 Math::calculateFaceNormalWithoutNormalize(const Vector3& v1, const Vector3& v2, const Vector3& v3)
698    {
699        Vector3 normal = calculateBasicFaceNormalWithoutNormalize(v1, v2, v3);
700        // Now set up the w (distance of tri from origin)
701        return Vector4(normal.x, normal.y, normal.z, -(normal.dotProduct(v1)));
702    }
703    //-----------------------------------------------------------------------
704    Vector3 Math::calculateBasicFaceNormalWithoutNormalize(const Vector3& v1, const Vector3& v2, const Vector3& v3)
705    {
706        Vector3 normal = (v2 - v1).crossProduct(v3 - v1);
707        return normal;
708    }
709}
Note: See TracBrowser for help on using the repository browser.