mathUtils.cpp

Engine/source/math/mathUtils.cpp

More...

Classes:

Namespaces:

namespace

Miscellaneous math utility functions.

Public Defines

define
MAX_TRIES() 20
define
MAX_TRIES() 20

Detailed Description

Public Defines

MAX_TRIES() 20
MAX_TRIES() 20
   1
   2//-----------------------------------------------------------------------------
   3// Copyright (c) 2012 GarageGames, LLC
   4//
   5// Permission is hereby granted, free of charge, to any person obtaining a copy
   6// of this software and associated documentation files (the "Software"), to
   7// deal in the Software without restriction, including without limitation the
   8// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
   9// sell copies of the Software, and to permit persons to whom the Software is
  10// furnished to do so, subject to the following conditions:
  11//
  12// The above copyright notice and this permission notice shall be included in
  13// all copies or substantial portions of the Software.
  14//
  15// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  16// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  17// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  18// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  19// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
  20// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
  21// IN THE SOFTWARE.
  22//-----------------------------------------------------------------------------
  23
  24#include "platform/platform.h"
  25#include "math/util/frustum.h"
  26#include "math/mathUtils.h"
  27
  28#include "math/mMath.h"
  29#include "math/mRandom.h"
  30#include "math/util/frustum.h"
  31#include "platform/profiler.h"
  32#include "core/tAlgorithm.h"
  33#include "gfx/gfxDevice.h"
  34
  35namespace MathUtils
  36{
  37
  38MRandomLCG sgRandom(0xdeadbeef); ///< Our random number generator.
  39
  40//-----------------------------------------------------------------------------
  41
  42bool capsuleCapsuleOverlap(const Point3F & a1, const Point3F & b1, F32 rad1, const Point3F & a2, const Point3F & b2, F32 rad2)
  43{
  44   F32 s,t;
  45   Point3F c1,c2;
  46   F32 dist = segmentSegmentNearest(a1,b1,a2,b2,s,t,c1,c2);
  47   return dist <= (rad1+rad2)*(rad1+rad2);
  48}
  49
  50//-----------------------------------------------------------------------------
  51
  52F32 segmentSegmentNearest(const Point3F & p1, const Point3F & q1, const Point3F & p2, const Point3F & q2, F32 & s, F32 & t, Point3F & c1, Point3F & c2)
  53{
  54   Point3F d1 = q1-p1;
  55   Point3F d2 = q2-p2;
  56   Point3F r = p1-p2;
  57   F32 a = mDot(d1,d1);
  58   F32 e = mDot(d2,d2);
  59   F32 f = mDot(d2,r);
  60   
  61   const F32 EPSILON = 0.001f;
  62   
  63   if (a <= EPSILON && e <= EPSILON)
  64   {
  65      s = t = 0.0f;
  66      c1 = p1;
  67      c2 = p2;
  68      return mDot(c1-c2,c1-c2);
  69   }
  70   
  71   if (a <= EPSILON)
  72   {
  73      s = 0.0f;
  74      t = mClampF(f/e,0.0f,1.0f);
  75   }
  76   else
  77   {
  78      F32 c = mDot(d1,r);
  79      if (e <= EPSILON)
  80      {
  81         t = 0.0f;
  82         s = mClampF(-c/<a href="/coding/file/pointer_8h/#pointer_8h_1aeeddce917cf130d62c370b8f216026dd">a</a>,0.0f,1.0f);
  83      }
  84      else
  85      {
  86         F32 b = mDot(d1,d2);
  87         F32 denom = a*e-b*b;
  88         if (denom != 0.0f)
  89            s = mClampF((b*f-c*e)/denom,0.0f,1.0f);
  90         else
  91            s = 0.0f;
  92         F32 tnom = b*s+f;
  93         if (tnom < 0.0f)
  94         {
  95            t = 0.0f;
  96            s = mClampF(-c/<a href="/coding/file/pointer_8h/#pointer_8h_1aeeddce917cf130d62c370b8f216026dd">a</a>,0.0f,1.0f);
  97         }
  98         else if (tnom>e)
  99         {
 100            t = 1.0f;
 101            s = mClampF((b-c)/<a href="/coding/file/pointer_8h/#pointer_8h_1aeeddce917cf130d62c370b8f216026dd">a</a>,0.0f,1.0f);
 102         }
 103         else
 104            t = tnom/e;
 105      }
 106   }
 107   
 108   c1 = p1 + d1*s;
 109   c2 = p2 + d2*t;
 110   return mDot(c1-c2,c1-c2);
 111}
 112
 113//-----------------------------------------------------------------------------
 114
 115bool capsuleSphereNearestOverlap(const Point3F & A0, const Point3F A1, F32 radA, const Point3F & B, F32 radB, F32 & t)
 116{
 117   Point3F V = A1-A0;
 118   Point3F A0B = A0-B;
 119   F32 d1 = mDot(A0B,V);
 120   F32 d2 = mDot(A0B,A0B);
 121   F32 d3 = mDot(V,V);
 122   F32 R2 = (radA+radB)*(radA+radB);
 123   if (d2<R2)
 124   {
 125      // starting in collision state
 126      t=0;
 127      return true;
 128   }
 129   if (d3<0.01f)
 130      // no movement, and don't start in collision state, so no collision
 131      return false;
 132
 133   F32 b24ac = mSqrt(d1*d1-d2*d3+d3*R2);
 134   F32 t1 = (-d1-b24ac)/d3;
 135   if (t1>0 && t1<1.0f)
 136   {
 137      t=t1;
 138      return true;
 139   }
 140   F32 t2 = (-d1+b24ac)/d3;
 141   if (t2>0 && t2<1.0f)
 142   {
 143      t=t2;
 144      return true;
 145   }
 146   if (t1<0 && t2>0)
 147   {
 148      t=0;
 149      return true;
 150   }
 151   return false;   
 152}
 153
 154//-----------------------------------------------------------------------------
 155
 156void vectorRotateZAxis( Point3F &vector, F32 radians )
 157{
 158   F32 sin, cos;
 159   mSinCos(radians, sin, cos);
 160   F32 x = cos * vector.x - sin * vector.y;
 161   F32 y = sin * vector.x + cos * vector.y;
 162   vector.x = x;
 163   vector.y = y;     
 164}
 165
 166void vectorRotateZAxis( F32 radians, Point3F *vectors, U32 count )
 167{
 168   F32 sin, cos;
 169   mSinCos(radians, sin, cos);
 170
 171   F32 x, y;
 172   const Point3F *end = vectors + count;
 173   for ( ; vectors != end; vectors++ )
 174   {
 175      x = cos * vectors->x - sin * vectors->y;
 176      y = sin * vectors->x + cos * vectors->y;
 177      vectors->x = x;
 178      vectors->y = y;      
 179   }
 180}
 181
 182//-----------------------------------------------------------------------------
 183
 184void getZBiasProjectionMatrix( F32 bias, const Frustum &frustum, MatrixF *outMat, bool rotate )
 185{
 186   Frustum temp(frustum);
 187   temp.setNearDist(frustum.getNearDist() + bias);
 188   temp.getProjectionMatrix(outMat, rotate);
 189}
 190
 191//-----------------------------------------------------------------------------
 192
 193MatrixF createOrientFromDir( const Point3F &direction )
 194{
 195   Point3F j = direction;
 196   Point3F k(0.0f, 0.0f, 1.0f);
 197   Point3F i;
 198   
 199   mCross( j, k, &i );
 200
 201   if( i.magnitudeSafe() == 0.0f )
 202   {
 203      i.set( 0.0f, -1.0f, 0.0f );
 204   }
 205
 206   i.normalizeSafe();
 207   mCross( i, j, &k );
 208
 209   MatrixF mat( true );
 210   mat.setColumn( 0, i );
 211   mat.setColumn( 1, j );
 212   mat.setColumn( 2, k );
 213
 214   return mat;
 215}
 216
 217//-----------------------------------------------------------------------------
 218
 219void getMatrixFromUpVector( const VectorF &up, MatrixF *outMat )
 220{
 221   AssertFatal( up.isUnitLength(), "MathUtils::getMatrixFromUpVector() - Up vector was not normalized!" );
 222   AssertFatal( outMat, "MathUtils::getMatrixFromUpVector() - Got null output matrix!" );
 223   AssertFatal( outMat->isAffine(), "MathUtils::getMatrixFromUpVector() - Got uninitialized matrix!" );
 224
 225   VectorF forward = mPerp( up );
 226   VectorF right = mCross( forward, up );
 227   right.normalize();
 228   forward = mCross( up, right );
 229   forward.normalize();
 230
 231   outMat->setColumn( 0, right );
 232   outMat->setColumn( 1, forward );
 233   outMat->setColumn( 2, up );
 234}
 235
 236//-----------------------------------------------------------------------------
 237
 238void getMatrixFromForwardVector( const VectorF &forward, MatrixF *outMat  )
 239{
 240   AssertFatal( forward.isUnitLength(), "MathUtils::getMatrixFromForwardVector() - Forward vector was not normalized!" );
 241   AssertFatal( outMat, "MathUtils::getMatrixFromForwardVector() - Got null output matrix!" );
 242   AssertFatal( outMat->isAffine(), "MathUtils::getMatrixFromForwardVector() - Got uninitialized matrix!" );
 243
 244   VectorF up = mPerp( forward );
 245   VectorF right = mCross( forward, up );
 246   right.normalize();
 247   up = mCross( right, forward );
 248   up.normalize();
 249
 250   outMat->setColumn( 0, right );
 251   outMat->setColumn( 1, forward );
 252   outMat->setColumn( 2, up );
 253}
 254
 255//-----------------------------------------------------------------------------
 256
 257Point3F randomDir( const Point3F &axis, F32 thetaAngleMin, F32 thetaAngleMax,
 258                                        F32 phiAngleMin, F32 phiAngleMax )
 259{
 260   MatrixF orient = createOrientFromDir( axis );
 261   Point3F axisx;
 262   orient.getColumn( 0, &axisx );
 263
 264   F32 theta = (thetaAngleMax - thetaAngleMin) * sgRandom.randF() + thetaAngleMin;
 265   F32 phi = (phiAngleMax - phiAngleMin) * sgRandom.randF() + phiAngleMin;
 266
 267   // Both phi and theta are in degs.  Create axis angles out of them, and create the
 268   //  appropriate rotation matrix...
 269   AngAxisF thetaRot(axisx, theta * (M_PI_F / 180.0f));
 270   AngAxisF phiRot(axis,    phi   * (M_PI_F / 180.0f));
 271
 272   Point3F ejectionAxis = axis;
 273
 274   MatrixF temp(true);
 275   thetaRot.setMatrix(&temp);
 276   temp.mulP(ejectionAxis);
 277   phiRot.setMatrix(&temp);
 278   temp.mulP(ejectionAxis);
 279
 280   return ejectionAxis;
 281}
 282
 283//-----------------------------------------------------------------------------
 284
 285Point3F randomPointInSphere( F32 radius )
 286{
 287   AssertFatal( radius > 0.0f, "MathUtils::randomPointInRadius - radius must be positive" );
 288
 289   #define MAX_TRIES 20
 290
 291   Point3F out;
 292   F32 radiusSq = radius * radius;
 293
 294   for ( S32 i = 0; i < MAX_TRIES; i++ )
 295   {
 296      out.x = sgRandom.randF(-radius,radius);
 297      out.y = sgRandom.randF(-radius,radius);
 298      out.z = sgRandom.randF(-radius,radius);
 299
 300      if ( out.lenSquared() < radiusSq )
 301         return out;
 302   }
 303
 304   AssertFatal( false, "MathUtils::randomPointInRadius - something is wrong, should not fail this many times." );
 305   return Point3F::Zero;
 306}
 307
 308//-----------------------------------------------------------------------------
 309
 310Point2F randomPointInCircle( F32 radius )
 311{
 312   AssertFatal( radius > 0.0f, "MathUtils::randomPointInRadius - radius must be positive" );
 313
 314   #define MAX_TRIES 20
 315
 316   Point2F out;
 317   F32 radiusSq = radius * radius;
 318
 319   for ( S32 i = 0; i < MAX_TRIES; i++ )
 320   {
 321      out.x = sgRandom.randF(-radius,radius);
 322      out.y = sgRandom.randF(-radius,radius);
 323
 324      if ( out.lenSquared() < radiusSq )
 325         return out;
 326   }
 327
 328   AssertFatal( false, "MathUtils::randomPointInRadius - something is wrong, should not fail this many times." );
 329   return Point2F::Zero;
 330}
 331
 332//-----------------------------------------------------------------------------
 333
 334void getAnglesFromVector( const VectorF &vec, F32 &yawAng, F32 &pitchAng )
 335{
 336   yawAng = mAtan2( vec.x, vec.y );
 337   if( yawAng < 0.0f )
 338      yawAng += M_2PI_F;
 339
 340   if( mFabs(vec.x) > mFabs(vec.y) )
 341      pitchAng = mAtan2( mFabs(vec.z), mFabs(vec.x) );
 342   else
 343      pitchAng = mAtan2( mFabs(vec.z), mFabs(vec.y) );
 344   if( vec.z < 0.0f )
 345      pitchAng = -pitchAng;
 346}
 347
 348//-----------------------------------------------------------------------------
 349
 350void getVectorFromAngles( VectorF &vec, F32 yawAng, F32 pitchAng )
 351{
 352   VectorF  pnt( 0.0f, 1.0f, 0.0f );
 353
 354   EulerF   rot( -pitchAng, 0.0f, 0.0f );
 355   MatrixF  mat( rot );
 356
 357   rot.set( 0.0f, 0.0f, yawAng );
 358   MatrixF   mat2( rot );
 359
 360   mat.mulV( pnt );
 361   mat2.mulV( pnt );
 362
 363   vec = pnt;
 364}
 365
 366F32 getAngleBetweenVectors(VectorF vecA, VectorF vecB)
 367{
 368   F32 dot = mDot(vecA, vecB);
 369   F32 lenSq1 = vecA.lenSquared();
 370   F32 lenSq2 = vecB.lenSquared();
 371   F32 angle = mAcos(dot / mSqrt(lenSq1 * lenSq2));
 372
 373   return angle;
 374}
 375
 376F32 getSignedAngleBetweenVectors(VectorF vecA, VectorF vecB, VectorF norm)
 377{
 378   // angle in 0-180
 379   F32 angle = getAngleBetweenVectors(vecA, vecB);
 380   F32 sign = mSign(mDot(norm, mCross(vecA, vecB)));
 381
 382   // angle in -179-180
 383   F32 signed_angle = angle * sign;
 384
 385   return signed_angle;
 386}
 387
 388//-----------------------------------------------------------------------------
 389
 390void transformBoundingBox(const Box3F &sbox, const MatrixF &mat, const Point3F scale, Box3F &dbox)
 391{
 392   Point3F center;
 393
 394   // set transformed center...
 395   sbox.getCenter(&center);
 396   center.convolve(scale);
 397   mat.mulP(center);
 398   dbox.minExtents = center;
 399   dbox.maxExtents = center;
 400
 401   Point3F val;
 402   for(U32 ix=0; ix<2; ix++)
 403   {
 404      if(ix & 0x1)
 405         val.x = sbox.minExtents.x;
 406      else
 407         val.x = sbox.maxExtents.x;
 408
 409      for(U32 iy=0; iy<2; iy++)
 410      {
 411         if(iy & 0x1)
 412            val.y = sbox.minExtents.y;
 413         else
 414            val.y = sbox.maxExtents.y;
 415
 416         for(U32 iz=0; iz<2; iz++)
 417         {
 418            if(iz & 0x1)
 419               val.z = sbox.minExtents.z;
 420            else
 421               val.z = sbox.maxExtents.z;
 422
 423            Point3F v1, v2;
 424            v1 = val;
 425            v1.convolve(scale);
 426            mat.mulP(v1, &v2);
 427            dbox.minExtents.setMin(v2);
 428            dbox.maxExtents.setMax(v2);
 429         }
 430      }
 431   }
 432}
 433
 434//-----------------------------------------------------------------------------
 435
 436bool mProjectWorldToScreen(   const Point3F &in, 
 437                              Point3F *out, 
 438                              const RectI &view, 
 439                              const MatrixF &world, 
 440                              const MatrixF &projection )
 441{
 442   MatrixF worldProjection = projection;
 443   worldProjection.mul(world);
 444
 445   return mProjectWorldToScreen( in, out, view, worldProjection );
 446}
 447
 448//-----------------------------------------------------------------------------
 449
 450bool mProjectWorldToScreen(   const Point3F &in, 
 451                              Point3F *out, 
 452                              const RectI &view, 
 453                              const MatrixF &worldProjection )
 454{
 455   Point4F temp(in.x,in.y,in.z,1.0f);
 456   worldProjection.mul(temp);
 457
 458   // Perform the perspective division.  For orthographic
 459   // projections, temp.w will be 1.
 460
 461   temp.x /= temp.w;
 462   temp.y /= temp.w;
 463   temp.z /= temp.w;
 464
 465   // Take the normalized device coordinates (NDC) and transform them
 466   // into device coordinates.
 467
 468   out->x = (temp.x + 1.0f) / 2.0f * view.extent.x + view.point.x;
 469   out->y = (1.0f - temp.y) / 2.0f * view.extent.y + view.point.y;
 470   out->z = temp.z;
 471
 472   if ( out->z < 0.0f || out->z > 1.0f || 
 473        out->x < (F32)view.point.x || out->x > (F32)view.point.x + (F32)view.extent.x ||
 474        out->y < (F32)view.point.y || out->y > (F32)view.point.y + (F32)view.extent.y )
 475      return false;
 476
 477   return true;
 478}
 479
 480//-----------------------------------------------------------------------------
 481
 482void mProjectScreenToWorld(   const Point3F &in, 
 483                              Point3F *out, 
 484                              const RectI &view, 
 485                              const MatrixF &world, 
 486                              const MatrixF &projection, 
 487                              F32 zfar, 
 488                              F32 znear )
 489{
 490   MatrixF invWorldProjection = projection;
 491   invWorldProjection.mul(world);
 492   invWorldProjection.inverse();
 493
 494   Point3F vec;
 495   vec.x = (in.x - view.point.x) * 2.0f / view.extent.x - 1.0f;
 496   vec.y = -(in.y - view.point.y) * 2.0f / view.extent.y + 1.0f;
 497   vec.z = (znear + in.z * (zfar - znear))/zfar;
 498
 499   invWorldProjection.mulV(vec);
 500   vec *= 1.0f + in.z * zfar;
 501
 502   invWorldProjection.getColumn(3, out);
 503   (*out) += vec;
 504}
 505
 506//-----------------------------------------------------------------------------
 507
 508bool pointInPolygon( const Point2F *verts, U32 vertCount, const Point2F &testPt )
 509{
 510  U32 i, j, c = 0;
 511  for ( i = 0, j = vertCount-1; i < vertCount; j = i++ ) 
 512  {
 513    if ( ( ( verts[i].y > testPt.y ) != ( verts[j].y > testPt.y ) ) &&
 514            ( testPt.x <   ( verts[j].x - verts[i].x ) * 
 515                           ( testPt.y - verts[i].y ) / 
 516                           ( verts[j].y - verts[i].y ) + verts[i].x ) )
 517       c = !c;
 518  }
 519  
 520  return c != 0;
 521}  
 522
 523//-----------------------------------------------------------------------------
 524
 525F32 mTriangleDistance( const Point3F &A, const Point3F &B, const Point3F &C, const Point3F &P, IntersectInfo* info )
 526{
 527    Point3F diff = A - P;
 528    Point3F edge0 = B - A;
 529    Point3F edge1 = C - A;
 530    F32 a00 = edge0.lenSquared();
 531    F32 a01 = mDot( edge0, edge1 );
 532    F32 a11 = edge1.lenSquared();
 533    F32 b0 = mDot( diff, edge0 );
 534    F32 b1 = mDot( diff, edge1 );
 535    F32 c = diff.lenSquared();
 536    F32 det = mFabs(a00*a11-a01*a01);
 537    F32 s = a01*b1-a11*b0;
 538    F32 t = a01*b0-a00*b1;
 539    F32 sqrDistance;
 540
 541    if (s + t <= det)
 542    {
 543        if (s < 0.0f)
 544        {
 545            if (t < 0.0f)  // region 4
 546            {
 547                if (b0 < 0.0f)
 548                {
 549                    t = 0.0f;
 550                    if (-b0 >= a00)
 551                    {
 552                        s = 1.0f;
 553                        sqrDistance = a00 + (2.0f)*b0 + c;
 554                    }
 555                    else
 556                    {
 557                        s = -b0/a00;
 558                        sqrDistance = b0*s + c;
 559                    }
 560                }
 561                else
 562                {
 563                    s = 0.0f;
 564                    if (b1 >= 0.0f)
 565                    {
 566                        t = 0.0f;
 567                        sqrDistance = c;
 568                    }
 569                    else if (-b1 >= a11)
 570                    {
 571                        t = 1.0f;
 572                        sqrDistance = a11 + 2.0f*b1 + c;
 573                    }
 574                    else
 575                    {
 576                        t = -b1/a11;
 577                        sqrDistance = b1*t + c;
 578                    }
 579                }
 580            }
 581            else  // region 3
 582            {
 583                s = 0.0f;
 584                if (b1 >= 0.0f)
 585                {
 586                    t = 0.0f;
 587                    sqrDistance = c;
 588                }
 589                else if (-b1 >= a11)
 590                {
 591                    t = 1.0f;
 592                    sqrDistance = a11 + 2.0f*b1 + c;
 593                }
 594                else
 595                {
 596                    t = -b1/a11;
 597                    sqrDistance = b1*t + c;
 598                }
 599            }
 600        }
 601        else if (t < 0.0f)  // region 5
 602        {
 603            t = 0.0f;
 604            if (b0 >= 0.0f)
 605            {
 606                s = 0.0f;
 607                sqrDistance = c;
 608            }
 609            else if (-b0 >= a00)
 610            {
 611                s = 1.0f;
 612                sqrDistance = a00 + 2.0f*b0 + c;
 613            }
 614            else
 615            {
 616                s = -b0/a00;
 617                sqrDistance = b0*s + c;
 618            }
 619        }
 620        else  // region 0
 621        {
 622            // minimum at interior point
 623            F32 invDet = 1.0f / det;
 624            s *= invDet;
 625            t *= invDet;
 626            sqrDistance = s * (a00*s + a01*t + 2.0f*b0) +
 627                t * (a01*s + a11*t + 2.0f*b1) + c;
 628        }
 629    }
 630    else
 631    {
 632        F32 tmp0, tmp1, numer, denom;
 633
 634        if (s < 0.0f)  // region 2
 635        {
 636            tmp0 = a01 + b0;
 637            tmp1 = a11 + b1;
 638            if (tmp1 > tmp0)
 639            {
 640                numer = tmp1 - tmp0;
 641                denom = a00 - 2.0f*a01 + a11;
 642                if (numer >= denom)
 643                {
 644                    s = 1.0f;
 645                    t = 0.0f;
 646                    sqrDistance = a00 + 2.0f*b0 + c;
 647                }
 648                else
 649                {
 650                    s = numer/denom;
 651                    t = 1.0f - s;
 652                    sqrDistance = s * (a00*s + a01*t + 2.0f*b0) +
 653                                  t * (a01*s + a11*t + 2.0f*b1) + c;
 654                }
 655            }
 656            else
 657            {
 658                s = 0.0f;
 659                if (tmp1 <= 0.0f)
 660                {
 661                    t = 1.0f;
 662                    sqrDistance = a11 + 2.0f*b1 + c;
 663                }
 664                else if (b1 >= 0.0f)
 665                {
 666                    t = 0.0f;
 667                    sqrDistance = c;
 668                }
 669                else
 670                {
 671                    t = -b1/a11;
 672                    sqrDistance = b1*t + c;
 673                }
 674            }
 675        }
 676        else if (t < 0.0f)  // region 6
 677        {
 678            tmp0 = a01 + b1;
 679            tmp1 = a00 + b0;
 680            if (tmp1 > tmp0)
 681            {
 682                numer = tmp1 - tmp0;
 683                denom = a00 - 2.0f*a01 + a11;
 684                if (numer >= denom)
 685                {
 686                    t = 1.0f;
 687                    s = 0.0f;
 688                    sqrDistance = a11 + 2.0f*b1 + c;
 689                }
 690                else
 691                {
 692                    t = numer/denom;
 693                    s = 1.0f - t;
 694                    sqrDistance = s * (a00*s + a01*t + 2.0f*b0) +
 695                                  t * (a01*s + a11*t + 2.0f*b1) + c;
 696                }
 697            }
 698            else
 699            {
 700                t = 0.0f;
 701                if (tmp1 <= 0.0f)
 702                {
 703                    s = 1.0f;
 704                    sqrDistance = a00 + 2.0f*b0 + c;
 705                }
 706                else if (b0 >= 0.0f)
 707                {
 708                    s = 0.0f;
 709                    sqrDistance = c;
 710                }
 711                else
 712                {
 713                    s = -b0/a00;
 714                    sqrDistance = b0*s + c;
 715                }
 716            }
 717        }
 718        else  // region 1
 719        {
 720            numer = a11 + b1 - a01 - b0;
 721            if (numer <= 0.0f)
 722            {
 723                s = 0.0f;
 724                t = 1.0f;
 725                sqrDistance = a11 + 2.0f*b1 + c;
 726            }
 727            else
 728            {
 729                denom = a00 - 2.0f*a01 + a11;
 730                if (numer >= denom)
 731                {
 732                    s = 1.0f;
 733                    t = 0.0f;
 734                    sqrDistance = a00 + 2.0f*b0 + c;
 735                }
 736                else
 737                {
 738                    s = numer/denom;
 739                    t = 1.0f - s;
 740                    sqrDistance = s * (a00*s + a01*t + 2.0f*b0) +
 741                                  t * (a01*s + a11*t + 2.0f*b1) + c;
 742                }
 743            }
 744        }
 745    }
 746
 747    // account for numerical round-off error
 748    if (sqrDistance < 0.0f)
 749        sqrDistance = 0.0f;
 750
 751    // This also calculates the barycentric coordinates and the closest point!
 752    //m_kClosestPoint0 = P;
 753    //m_kClosestPoint1 = A + s*edge0 + t*edge1;
 754    //m_afTriangleBary[1] = s;
 755    //m_afTriangleBary[2] = t;
 756    //m_afTriangleBary[0] = (Real)1.0 - fS - fT;
 757    if(info)
 758    {
 759       info->segment.p0 = P;
 760       info->segment.p1 = A + s*edge0 + t*edge1;
 761       info->bary.x = s;
 762       info->bary.y = t;
 763       info->bary.z = 1.0f - s - t;
 764    }
 765
 766    return sqrDistance;
 767}
 768
 769//-----------------------------------------------------------------------------
 770
 771Point3F mTriangleNormal( const Point3F &a, const Point3F &b, const Point3F &c )
 772{
 773   // Vector from b to a.
 774   const F32 ax = a.x-b.x;
 775   const F32 ay = a.y-b.y;
 776   const F32 az = a.z-b.z;
 777   // Vector from b to c.
 778   const F32 cx = c.x-b.x;
 779   const F32 cy = c.y-b.y;
 780   const F32 cz = c.z-b.z;
 781
 782   Point3F n;
 783
 784   // This is an in-line cross product.
 785   n.x = ay*cz - az*cy;
 786   n.y = az*cx - ax*cz;
 787   n.z = ax*cy - ay*cx;
 788   m_point3F_normalize( (F32*)(&n) );
 789
 790   return n;
 791}
 792
 793//-----------------------------------------------------------------------------
 794
 795Point3F mClosestPointOnSegment( const Point3F &a, const Point3F &b, const Point3F &p )
 796{
 797   Point3F c = p - a;               // Vector from a to Point
 798   Point3F v = (b - a);
 799   F32 d = v.len();           // Length of the line segment
 800   v.normalize();                   // Unit Vector from a to b
 801   F32 t = mDot( v, c );            // Intersection point Distance from a
 802
 803   // Check to see if the point is on the line
 804   // if not then return the endpoint
 805   if(t < 0) return a;
 806   if(t > d) return b;
 807
 808   // get the distance to move from point a
 809   v *= t;
 810
 811   // move from point a to the nearest point on the segment
 812   return a + v;
 813}
 814
 815//-----------------------------------------------------------------------------
 816
 817void mShortestSegmentBetweenLines( const Line &line0, const Line &line1, LineSegment *outSegment )
 818{   
 819   // compute intermediate parameters
 820   Point3F w0 = line0.origin - line1.origin;
 821   F32 a = mDot( line0.direction, line0.direction );
 822   F32 b = mDot( line0.direction, line1.direction );
 823   F32 c = mDot( line1.direction, line1.direction );
 824   F32 d = mDot( line0.direction, w0 );
 825   F32 e = mDot( line1.direction, w0 );
 826
 827   F32 denom = a*c - b*b;
 828
 829   if ( denom > -0.001f && denom < 0.001f )
 830   {
 831      outSegment->p0 = line0.origin;
 832      outSegment->p1 = line1.origin + (e/c)*line1.direction;
 833   }
 834   else
 835   {
 836      outSegment->p0 = line0.origin + ((b*e - c*d)/denom)*line0.direction;
 837      outSegment->p1 = line1.origin + ((a*e - b*d)/denom)*line1.direction;
 838   }
 839}
 840
 841//-----------------------------------------------------------------------------
 842
 843U32 greatestCommonDivisor( U32 u, U32 v )
 844{
 845   // http://en.wikipedia.org/wiki/Binary_GCD_algorithm
 846      
 847   S32 shift;
 848
 849   /* GCD(0,x) := x */
 850   if (u == 0 || v == 0)
 851      return u | v;
 852
 853   /* Left shift := lg K, where K is the greatest power of 2
 854   dividing both u and v. */
 855   for (shift = 0; ((u | v) & 1) == 0; ++shift) {
 856      u >>= 1;
 857      v >>= 1;
 858   }
 859
 860   while ((u & 1) == 0)
 861      u >>= 1;
 862
 863   /* From here on, u is always odd. */
 864   do {
 865      while ((v & 1) == 0)  /* Loop X */
 866         v >>= 1;
 867
 868      /* Now u and v are both odd, so diff(u, v) is even.
 869      Let u = min(u, v), v = diff(u, v)/2. */
 870      if (u < v) {
 871         v -= u;
 872      } else {
 873         U32 diff = u - v;
 874         u = v;
 875         v = diff;
 876      }
 877      v >>= 1;
 878   } while (v != 0);
 879
 880   return u << shift;
 881}
 882
 883//-----------------------------------------------------------------------------
 884
 885bool mLineTriangleCollide( const Point3F &p1, const Point3F &p2, 
 886                           const Point3F &t1, const Point3F &t2, const Point3F &t3,
 887                           Point3F *outUVW, F32 *outT )
 888{
 889   VectorF ab = t2 - t1;
 890   VectorF ac = t3 - t1;
 891   VectorF qp = p1 - p2;
 892
 893   // Compute triangle normal. Can be precalculated or cached if
 894   // intersecting multiple segments against the same triangle
 895   VectorF n = mCross( ab, ac );
 896
 897   // Compute denominator d. If d <= 0, segment is parallel to or points
 898   // away from triangle, so exit early
 899   F32 d = mDot( qp, n );
 900   if ( d <= 0.0f ) 
 901      return false;
 902
 903   // Compute intersection t value of pq with plane of triangle. A ray
 904   // intersects if 0 <= t. Segment intersects iff 0 <= t <= 1. Delay
 905   // dividing by d until intersection has been found to pierce triangle
 906   VectorF ap = p1 - t1;
 907   F32 t = mDot( ap, n );
 908   if ( t < 0.0f ) 
 909      return false;      
 910   if ( t > d ) 
 911      return false; // For segment; exclude this code line for a ray test
 912
 913   // Compute barycentric coordinate components and test if within bounds
 914   VectorF e = mCross( qp, ap );
 915   F32 v = mDot( ac, e );
 916   if ( v < 0.0f || v > d )
 917      return false;
 918   F32 w = -mDot( ab, e );
 919   if ( w < 0.0f || v + w > d ) 
 920      return false;
 921
 922   // Segment/ray intersects triangle. Perform delayed division and
 923   // compute the last barycentric coordinate component
 924   const F32 ood = 1.0f / d;
 925   
 926   if ( outT )
 927      *outT = t * ood;
 928      
 929   if ( outUVW )
 930   {
 931      v *= ood;
 932      w *= ood;
 933      outUVW->set( 1.0f - v - w, v, w );
 934   }
 935
 936   return true;
 937}
 938
 939//-----------------------------------------------------------------------------
 940
 941bool mRayQuadCollide(   const Quad &quad, 
 942                        const Ray &ray, 
 943                        Point2F *outUV,
 944                        F32 *outT )
 945{
 946   static const F32 eps = F32(10e-6);
 947
 948   // Rejects rays that are parallel to Q, and rays that intersect the plane of
 949   // Q either on the left of the line V00V01 or on the right of the line V00V10.
 950
 951   //   p01-----eXX-----p11
 952   //   ^            . ^ |
 953   //   |          .     |
 954   //   e03     e02     eXX
 955   //   |    .           |
 956   //   |  .             |
 957   //   p00-----e01---->p10
 958
 959   VectorF e01 = quad.p10 - quad.p00;
 960   VectorF e03 = quad.p01 - quad.p00;
 961
 962   // If the ray is perfectly perpendicular to e03, which 
 963   // represents the entire planes tangent, then the 
 964   // result of this cross product (P) will equal e01
 965   // If it is parallel it will result in a vector opposite e01.
 966
 967   // If the ray is heading DOWN the cross product will point to the RIGHT
 968   // If the ray is heading UP the cross product will point to the LEFT
 969   // We do not reject based on this though...
 970   //
 971   // In either case cross product will be more parallel to e01 the more
 972   // perpendicular the ray is to e03, and it will be more perpendicular to
 973   // e01 the more parallel it is to e03.
 974   VectorF P = mCross(ray.direction, e03);
 975
 976   // det can be seen as 'the amount of vector e01 in the direction P'
 977   F32 det = mDot(e01, P);
 978
 979   // Take a Abs of the dot because we do not care if the ray is heading up or down,
 980   // but if it is perfectly parallel to the quad we want to reject it.
 981   if ( mFabs(det) < eps ) 
 982      return false;
 983
 984   F32 inv_det = 1.0f / det;
 985
 986   VectorF T = ray.origin - quad.p00;
 987
 988   // alpha can be seen as 'the amount of vector T in the direction P'
 989   // T is a vector up from the quads corner point 00 to the ray's origin.
 990   // P is the cross product of the ray and e01, which should be "roughly"
 991   // parallel with e03 but might be of either positive or negative magnitude.
 992   F32 alpha = mDot(T, P) * inv_det;
 993   if ( alpha < 0.0f ) 
 994      return false;
 995
 996   // if (alpha > real(1.0)) return false; // Uncomment if VR is used.
 997
 998   // The cross product of T and e01 should be roughly parallel to e03
 999   // and of either positive or negative magnitude.
1000   VectorF Q = mCross(T, e01);
1001   F32 beta = mDot(ray.direction, Q) * inv_det;
1002   if ( beta < 0.0f ) 
1003      return false; 
1004
1005   // if (beta > real(1.0)) return false; // Uncomment if VR is used.
1006
1007   if ( alpha + beta > 1.0f ) 
1008   //if ( false )
1009   {
1010      // Rejects rays that intersect the plane of Q either on the
1011      // left of the line V11V10 or on the right of the line V11V01.
1012
1013      VectorF e23 = quad.p01 - quad.p11;
1014      VectorF e21 = quad.p10 - quad.p11;
1015      VectorF P_prime = mCross(ray.direction, e21);
1016      F32 det_prime = mDot(e23, P_prime);
1017      if ( mFabs(det_prime) < eps) 
1018         return false;
1019      F32 inv_det_prime = 1.0f / det_prime;
1020      VectorF T_prime = ray.origin - quad.p11;
1021      F32 alpha_prime = mDot(T_prime, P_prime) * inv_det_prime;
1022      if (alpha_prime < 0.0f) 
1023         return false;
1024      VectorF Q_prime = mCross(T_prime, e23);
1025      F32 beta_prime = mDot(ray.direction, Q_prime) * inv_det_prime;
1026      if (beta_prime < 0.0f) 
1027         return false;
1028   }
1029
1030   // Compute the ray parameter of the intersection point, and
1031   // reject the ray if it does not hit Q.
1032
1033   F32 t = mDot(e03, Q) * inv_det;
1034   if ( t < 0.0f ) 
1035      return false; 
1036
1037
1038   // Compute the barycentric coordinates of the fourth vertex.
1039   // These do not depend on the ray, and can be precomputed
1040   // and stored with the quadrilateral.  
1041
1042   F32 alpha_11, beta_11;
1043   VectorF e02 = quad.p11 - quad.p00;
1044   VectorF n = mCross(e01, e03);
1045
1046   if ( mFabs(n.x) >= mFabs(n.y) && 
1047      mFabs(n.x) >= mFabs(n.z) )
1048   {
1049      alpha_11 = ( e02.y * e03.z - e02.z * e03.y ) / n.x;
1050      beta_11  = ( e01.y * e02.z - e01.z * e02.y ) / n.x;
1051   }
1052   else if ( mFabs(n.y) >= mFabs(n.x) && 
1053      mFabs(n.y) >= mFabs(n.z) ) 
1054   {  
1055      alpha_11 = ((e02.z * e03.x) - (e02.x * e03.z)) / n.y;
1056      beta_11  = ((e01.z * e02.x) - (e01.x * e02.z)) / n.y;
1057   }
1058   else 
1059   {
1060      alpha_11 = ((e02.x * e03.y) - (e02.y * e03.x)) / n.z;
1061      beta_11  = ((e01.x * e02.y) - (e01.y * e02.x)) / n.z;
1062   }
1063
1064   // Compute the bilinear coordinates of the intersection point.
1065
1066   F32 u,v;
1067
1068   if ( mFabs(alpha_11 - 1.0f) < eps) 
1069   {    
1070      // Q is a trapezium.
1071      u = alpha;
1072      if ( mFabs(beta_11 - 1.0f) < eps) 
1073         v = beta; // Q is a parallelogram.
1074      else 
1075         v = beta / ((u * (beta_11 - 1.0f)) + 1.0f); // Q is a trapezium.
1076   }
1077   else if ( mFabs(beta_11 - 1.0f) < eps) 
1078   {
1079      // Q is a trapezium.
1080      v = beta;
1081      u = alpha / ((v * (alpha_11 - 1.0f)) + 1.0f);
1082   }
1083   else 
1084   {
1085      F32 A = 1.0f - beta_11;
1086      F32 B = (alpha * (beta_11 - 1.0f))
1087         - (beta * (alpha_11 - 1.0f)) - 1.0f;
1088      F32 C = alpha;
1089      F32 D = (B * B) - (4.0f * A * C);
1090      F32 F = -0.5f * (B + (B < 0.0f ? -1.0f : 1.0f) ) * mSqrt(D);
1091      u = F / A;
1092      if ((u < 0.0f) || (u > 1.0f)) u = C / F;
1093      v = beta / ((u * (beta_11 - 1.0f)) + 1.0f); 
1094   }
1095
1096   if ( outUV )
1097      outUV->set( u, v );  
1098   if ( outT )
1099      *outT = t;
1100
1101   return true;  
1102}
1103
1104//-----------------------------------------------------------------------------
1105
1106// Used by sortQuadWindingOrder.
1107struct QuadSortPoint
1108{
1109   U32 id;
1110   F32 theta;
1111};
1112
1113// Used by sortQuadWindingOrder.
1114S32 QSORT_CALLBACK cmpAngleAscending( const void *a, const void *b )
1115{
1116   const QuadSortPoint *p0 = (const QuadSortPoint*)a;
1117   const QuadSortPoint *p1 = (const QuadSortPoint*)b;   
1118
1119   F32 diff = p1->theta - p0->theta;
1120
1121   if ( diff > 0.0f )
1122      return -1;
1123   else if ( diff < 0.0f )
1124      return 1;
1125   else
1126      return 0;   
1127}
1128
1129// Used by sortQuadWindingOrder.
1130S32 QSORT_CALLBACK cmpAngleDescending( const void *a, const void *b )
1131{
1132   const QuadSortPoint *p0 = (const QuadSortPoint*)a;
1133   const QuadSortPoint *p1 = (const QuadSortPoint*)b;   
1134
1135   F32 diff = p1->theta - p0->theta;
1136
1137   if ( diff > 0.0f )
1138      return 1;
1139   else if ( diff < 0.0f )
1140      return -1;
1141   else
1142      return 0;   
1143}
1144
1145void sortQuadWindingOrder( const MatrixF &quadMat, bool clockwise, const Point3F *verts, U32 *vertMap, U32 count )
1146{
1147   PROFILE_SCOPE( MathUtils_sortQuadWindingOrder );
1148
1149   if ( count == 0 )
1150      return;   
1151   
1152   Point3F *quadPoints = new Point3F[count];
1153   
1154   for ( S32 i = 0; i < count; i++ )   
1155   {     
1156      quadMat.mulP( verts[i], &quadPoints[i] );
1157      quadPoints[i].normalizeSafe();
1158   }
1159
1160   sortQuadWindingOrder( clockwise, quadPoints, vertMap, count );
1161
1162   delete [] quadPoints;
1163}
1164
1165void sortQuadWindingOrder( bool clockwise, const Point3F *verts, U32 *vertMap, U32 count )
1166{
1167   QuadSortPoint *sortPoints = new QuadSortPoint[count];
1168
1169   for ( S32 i = 0; i < count; i++ )
1170   {
1171      QuadSortPoint &sortPnt = sortPoints[i];
1172      const Point3F &vec = verts[i];
1173
1174      sortPnt.id = i;
1175
1176      F32 theta = mAtan2( vec.y, vec.x );
1177
1178      if ( vec.y < 0.0f )
1179         theta = M_2PI_F + theta;
1180
1181      sortPnt.theta = theta;
1182   }
1183
1184   dQsort( sortPoints, count, sizeof( QuadSortPoint ), clockwise ? cmpAngleDescending : cmpAngleAscending ); 
1185
1186   for ( S32 i = 0; i < count; i++ )
1187      vertMap[i] = sortPoints[i].id;
1188
1189   delete [] sortPoints;
1190}
1191
1192//-----------------------------------------------------------------------------
1193
1194void buildMatrix( const VectorF *rvec, const VectorF *fvec, const VectorF *uvec, const VectorF *pos, MatrixF *outMat )
1195{     
1196   /// Work in Progress
1197
1198   /*
1199   AssertFatal( !rvec || rvec->isUnitLength(), "MathUtils::buildMatrix() - Right vector was not normalized!" );  
1200   AssertFatal( !fvec || fvec->isUnitLength(), "MathUtils::buildMatrix() - Forward vector was not normalized!" );  
1201   AssertFatal( !uvec || uvec->isUnitLength(), "MathUtils::buildMatrix() - Up vector was not normalized!" ); 
1202
1203   // Note this relationship:
1204   //
1205   // Column0  Column1  Column2
1206   // Axis X   Axis Y   Axis Z
1207   // Rvec     Fvec     Uvec
1208   //
1209
1210   enum 
1211   {
1212      RVEC = 1,
1213      FVEC = 1 << 1,
1214      UVEC = 1 << 2,
1215      ALL = RVEC | FVEC | UVEC
1216   };
1217
1218   U8 mask = 0;
1219   U8 count = 0;
1220   U8 axis0, axis1;
1221
1222   if ( rvec )
1223   {
1224      mask |= RVEC;
1225      axis0 == 0;      
1226      count++;
1227   }
1228   if ( fvec )
1229   {
1230      mask |= FVEC;      
1231      if ( count == 0 )
1232         axis0 = 1;
1233      else
1234         axis1 = 1;
1235      count++;
1236   }
1237   if ( uvec )
1238   {
1239      mask |= UVEC;      
1240      count++;
1241   }
1242
1243   U8 bR = 1;
1244   U8 bF = 1 << 1;
1245   U8 bU = 1 << 2;
1246   U8 bRF = bR | bF;
1247   U8 bRU = bR | bU;
1248   U8 bFU = bF | bU;
1249   U8 bRFU = bR | bF | bU;
1250
1251   
1252
1253   // Cross product map.
1254   U8 cpdMap[3][2] = 
1255   {
1256      { 1, 2 },
1257      { 2, 0 },
1258      { 0, 1 },
1259   }
1260
1261   if ( count == 1 )
1262   {
1263      if ( mask == bR )
1264      {
1265         
1266      }
1267      else if ( mask == bF )
1268      {
1269
1270      }
1271      else if ( mask == bU )
1272      {
1273
1274      }
1275   }
1276   else if ( count == 2 )
1277   {
1278      if ( mask == bRF )
1279      {
1280
1281      }
1282      else if ( mask == bRU )
1283      {
1284
1285      }
1286      else if ( mask == bFU )
1287      {
1288
1289      }
1290   }
1291   else // bRFU
1292   {
1293
1294   }
1295
1296   if ( rvec )
1297   {
1298      outMat->setColumn( 0, *rvec );
1299
1300      if ( fvec )
1301      {
1302         outMat->setColumn( 1, *fvec );
1303
1304         if ( uvec )         
1305            outMat->setColumn( 2, *uvec );         
1306         else
1307         {
1308            // Set uvec from rvec/fvec
1309            tmp = mCross( rvec, fvec );
1310            tmp.normalizeSafe();
1311            outMat->setColumn( 2, tmp );
1312         }
1313      }
1314      else if ( uvec )
1315      {
1316         // Set fvec from uvec/rvec
1317         tmp = mCross( uvec, rvec );
1318         tmp.normalizeSafe();
1319         outMat->setColumn( 1, tmp );
1320      }
1321      else
1322      {
1323         // Set fvec and uvec from rvec
1324         Point3F tempFvec = mPerp( rvec );
1325         Point3F tempUvec = mCross( )
1326
1327      }
1328   }
1329   AssertFatal( rvec->isUnitLength(), "MathUtils::buildMatrix() - Right vector was not normalized!" );
1330   AssertFatal( fvec->isUnitLength(), "MathUtils::buildMatrix() - Forward vector was not normalized!" );
1331   AssertFatal( uvec->isUnitLength(), "MathUtils::buildMatrix() - UpVector vector was not normalized!" );
1332   AssertFatal( outMat, "MathUtils::buildMatrix() - Got null output matrix!" );
1333   AssertFatal( outMat->isAffine(), "MathUtils::buildMatrix() - Got uninitialized matrix!" );
1334   */
1335}
1336
1337//-----------------------------------------------------------------------------
1338
1339bool reduceFrustum( const Frustum& frustum, const RectI& viewport, const RectF& area, Frustum& outFrustum )
1340{
1341   // Just to be safe, clamp the area to the viewport.
1342
1343   Point2F clampedMin;
1344   Point2F clampedMax;
1345
1346   clampedMin.x = mClampF( area.extent.x, ( F32 ) viewport.point.x, ( F32 ) viewport.point.x + viewport.extent.x );
1347   clampedMin.y = mClampF( area.extent.y, ( F32 ) viewport.point.y, ( F32 ) viewport.point.y + viewport.extent.y );
1348
1349   clampedMax.x = mClampF( area.extent.x, ( F32 ) viewport.point.x, ( F32 ) viewport.point.x + viewport.extent.x );
1350   clampedMax.y = mClampF( area.extent.y, ( F32 ) viewport.point.y, ( F32 ) viewport.point.y + viewport.extent.y );
1351
1352   // If we have ended up without a visible region on the screen,
1353   // terminate now.
1354   
1355   if(   mFloor( clampedMin.x ) == mFloor( clampedMax.x ) ||
1356         mFloor( clampedMin.y ) == mFloor( clampedMax.y ) )
1357      return false;
1358
1359   // Get the extents of the frustum.
1360
1361   const F32 frustumXExtent = mFabs( frustum.getNearRight() - frustum.getNearLeft() );
1362   const F32 frustumYExtent = mFabs( frustum.getNearTop() - frustum.getNearBottom() );
1363
1364   // Now, normalize the screen-space pixel coordinates to lie within the screen-centered
1365   // -1 to 1 coordinate space that is used for the frustum planes.
1366
1367   Point2F normalizedMin;
1368   Point2F normalizedMax;
1369
1370   normalizedMin.x = ( ( clampedMin.x / viewport.extent.x ) * frustumXExtent ) - ( frustumXExtent / 2.f );
1371   normalizedMin.y = ( ( clampedMin.y / viewport.extent.y ) * frustumYExtent ) - ( frustumYExtent / 2.f );
1372   normalizedMax.x = ( ( clampedMax.x / viewport.extent.x ) * frustumXExtent ) - ( frustumXExtent / 2.f );
1373   normalizedMax.y = ( ( clampedMax.y / viewport.extent.y ) * frustumYExtent ) - ( frustumYExtent / 2.f );
1374
1375   // Make sure the generated frustum metrics are somewhat sane.
1376
1377   if( normalizedMax.x - normalizedMin.x < 0.001f ||
1378       normalizedMax.y - normalizedMin.y < 0.001f )
1379      return false;
1380   
1381   // Finally, create the new frustum using the original's frustum
1382   // information except its left/right/top/bottom planes.
1383   //
1384   // Note that screen-space coordinates go upside down on Y whereas
1385   // camera-space frustum coordinates go downside up on Y which is
1386   // why we are inverting Y here.
1387
1388   outFrustum.set(
1389      frustum.isOrtho(),
1390      normalizedMin.x,
1391      normalizedMax.x,
1392      - normalizedMin.y,
1393      - normalizedMax.y,
1394      frustum.getNearDist(),
1395      frustum.getFarDist(),
1396      frustum.getTransform()
1397   );
1398
1399   return true;
1400}
1401
1402//-----------------------------------------------------------------------------
1403
1404void makeFrustum( F32 *outLeft,
1405                  F32 *outRight,
1406                  F32 *outTop,
1407                  F32 *outBottom,
1408                  F32 fovYInRadians, 
1409                  F32 aspectRatio, 
1410                  F32 nearPlane )
1411{
1412   F32 top = nearPlane * mTan( fovYInRadians / 2.0 );
1413   if ( outTop ) *outTop = top;
1414   if ( outBottom ) *outBottom = -top;
1415
1416   F32 left = top * aspectRatio;
1417   if ( outLeft ) *outLeft = -left;
1418   if ( outRight ) *outRight = left;
1419}
1420
1421//-----------------------------------------------------------------------------
1422
1423void makeProjection( MatrixF *outMatrix, 
1424                     F32 fovYInRadians, 
1425                     F32 aspectRatio, 
1426                     F32 nearPlane, 
1427                     F32 farPlane,
1428                     bool gfxRotate )
1429{
1430   F32 left, right, top, bottom;
1431   makeFrustum( &left, &right, &top, &bottom, fovYInRadians, aspectRatio, nearPlane );
1432   makeProjection( outMatrix, left, right, top, bottom, nearPlane, farPlane, gfxRotate );
1433}
1434
1435//-----------------------------------------------------------------------------
1436
1437void makeFovPortFrustum(
1438   Frustum *outFrustum,
1439   bool isOrtho,
1440   F32 nearDist,
1441   F32 farDist,
1442   const FovPort &inPort,
1443   const MatrixF &transform)
1444{
1445   F32 leftSize = nearDist * inPort.leftTan;
1446   F32 rightSize = nearDist * inPort.rightTan;
1447   F32 upSize = nearDist * inPort.upTan;
1448   F32 downSize = nearDist * inPort.downTan;
1449
1450   F32 left = -leftSize;
1451   F32 right = rightSize;
1452   F32 top = upSize;
1453   F32 bottom = -downSize;
1454
1455   outFrustum->set(isOrtho, left, right, top, bottom, nearDist, farDist, transform);
1456}
1457
1458//-----------------------------------------------------------------------------
1459
1460/// This is the special rotation matrix applied to
1461/// projection matricies for GFX.
1462///
1463/// It is a wart of the OGL to DX change over.
1464///
1465static const MatrixF sGFXProjRotMatrix( EulerF( (M_PI_F / 2.0f), 0.0f, 0.0f ) );
1466
1467void makeProjection( MatrixF *outMatrix,
1468                     F32 left,
1469                     F32 right,
1470                     F32 top,
1471                     F32 bottom,
1472                     F32 nearPlane,
1473                     F32 farPlane,
1474                     bool gfxRotate)
1475{
1476   const bool isGL = GFX->getAdapterType() == OpenGL;
1477   Point4F row;
1478   row.x = 2.0f * nearPlane / (right - left);
1479   row.y = 0.0f;
1480   row.z = 0.0f;
1481   row.w = 0.0f;
1482   outMatrix->setRow(0, row);
1483
1484   row.x = 0.0f;
1485   row.y = 2.0f * nearPlane / (top - bottom);
1486   row.z = 0.0f;
1487   row.w = 0.0f;
1488   outMatrix->setRow(1, row);
1489
1490   row.x = (left + right) / (right - left);
1491   row.y = (top + bottom) / (top - bottom);
1492   row.z = isGL ? (nearPlane + farPlane) / (nearPlane - farPlane) : farPlane / (nearPlane - farPlane);
1493   row.w = -1.0f;
1494   outMatrix->setRow(2, row);
1495
1496   row.x = 0.0f;
1497   row.y = 0.0f;
1498   row.z = isGL ? 2.0f * nearPlane * farPlane / (nearPlane - farPlane) : farPlane * nearPlane / (nearPlane - farPlane);
1499   row.w = 0.0f;
1500   outMatrix->setRow(3, row);
1501
1502   outMatrix->transpose();
1503
1504   if (gfxRotate)
1505      outMatrix->mul(sGFXProjRotMatrix);
1506}
1507
1508//-----------------------------------------------------------------------------
1509
1510void makeOrthoProjection(  MatrixF *outMatrix, 
1511                           F32 left, 
1512                           F32 right, 
1513                           F32 top, 
1514                           F32 bottom, 
1515                           F32 nearPlane, 
1516                           F32 farPlane,
1517                           bool gfxRotate )
1518{
1519   Point4F row;
1520   row.x = 2.0f / (right - left);
1521   row.y = 0.0f;
1522   row.z = 0.0f;
1523   row.w = 0.0f;
1524   outMatrix->setRow( 0, row );
1525
1526   row.x = 0.0f;
1527   row.y = 2.0f / (top - bottom);
1528   row.z = 0.0f;
1529   row.w = 0.0f;
1530   outMatrix->setRow( 1, row );
1531
1532   row.x = 0.0f;
1533   row.y = 0.0f;
1534   row.z = 1.0f / (nearPlane - farPlane);
1535   row.w = 0.0f;
1536
1537   outMatrix->setRow( 2, row );
1538
1539   row.x = (left + right) / (left - right);
1540   row.y = (top + bottom) / (bottom - top);
1541   row.z = nearPlane / (nearPlane - farPlane);
1542   row.w = 1.0f;
1543   outMatrix->setRow( 3, row );
1544
1545   outMatrix->transpose();
1546
1547   if ( gfxRotate )
1548      outMatrix->mul( sGFXProjRotMatrix );
1549}
1550
1551//-----------------------------------------------------------------------------
1552
1553bool edgeFaceIntersect( const Point3F &edgeA, const Point3F &edgeB, 
1554                        const Point3F &faceA, const Point3F &faceB, const Point3F &faceC, const Point3F &faceD, Point3F *intersection )
1555{
1556   VectorF edgeAB = edgeB - edgeA;
1557   VectorF edgeAFaceA = faceA - edgeA;
1558   VectorF edgeAFaceB = faceB - edgeA;
1559   VectorF edgeAFaceC = faceC - edgeA;
1560
1561   VectorF m = mCross( edgeAFaceC, edgeAB );
1562   F32 v = mDot( edgeAFaceA, m );
1563   if ( v >= 0.0f )
1564   {
1565      F32 u = -mDot( edgeAFaceB, m );
1566      if ( u < 0.0f )
1567         return false;
1568
1569      VectorF tmp = mCross( edgeAFaceB, edgeAB );
1570      F32 w = mDot( edgeAFaceA, tmp );
1571      if ( w < 0.0f )
1572         return false;
1573
1574      F32 denom = 1.0f / (u + v + w );
1575      u *= denom;
1576      v *= denom;
1577      w *= denom;
1578
1579      (*intersection) = u * faceA + v * faceB + w * faceC;
1580   }
1581   else
1582   {
1583      VectorF edgeAFaceD = faceD - edgeA;
1584      F32 u = mDot( edgeAFaceD, m );
1585      if ( u < 0.0f )
1586         return false;
1587
1588      VectorF tmp = mCross( edgeAFaceA, edgeAB );
1589      F32 w = mDot( edgeAFaceD, tmp );
1590      if ( w < 0.0f )
1591         return false;
1592
1593      v = -v;
1594
1595      F32 denom = 1.0f / ( u + v + w );
1596      u *= denom;
1597      v *= denom;
1598      w *= denom;
1599
1600      (*intersection) = u * faceA + v * faceD + w * faceC;
1601   }
1602
1603   return true;
1604}
1605
1606//-----------------------------------------------------------------------------
1607
1608bool isPlanarPolygon( const Point3F* vertices, U32 numVertices )
1609{
1610   AssertFatal( vertices != NULL, "MathUtils::isPlanarPolygon - Received NULL pointer" );
1611   AssertFatal( numVertices >= 3, "MathUtils::isPlanarPolygon - Must have at least three vertices" );
1612
1613   // Triangles are always planar.  Letting smaller numVertices
1614   // slip through provides robustness for errors in release builds.
1615   
1616   if( numVertices <= 3 )
1617      return true;
1618
1619   // Compute the normal of the first triangle in the polygon.
1620   
1621   Point3F triangle1Normal = mTriangleNormal( vertices[ 0 ], vertices[ 1 ], vertices[ 2 ] );
1622
1623   // Now go through all the remaining vertices and build triangles
1624   // with the first two vertices.  Then the normals of all these triangles
1625   // must be the same (minus some variance due to floating-point inaccuracies)
1626   // as the normal of the first triangle.
1627
1628   for( U32 i = 3; i < numVertices; ++ i )
1629   {
1630      Point3F triangle2Normal = mTriangleNormal( vertices[ 0 ], vertices[ 1 ], vertices[ i ] );
1631      if( !triangle1Normal.equal( triangle2Normal ) )
1632         return false;
1633   }
1634
1635   return true;
1636}
1637
1638//-----------------------------------------------------------------------------
1639
1640bool isConvexPolygon( const Point3F* vertices, U32 numVertices )
1641{
1642   AssertFatal( vertices != NULL, "MathUtils::isConvexPolygon - Received NULL pointer" );
1643   AssertFatal( numVertices >= 3, "MathUtils::isConvexPolygon - Must have at least three vertices" );
1644
1645   // Triangles are always convex.  Letting smaller numVertices
1646   // slip through provides robustness for errors in release builds.
1647
1648   if( numVertices <= 3 )
1649      return true;
1650
1651   U32 numPositive = 0;
1652   U32 numNegative = 0;
1653
1654   for( U32 i = 0; i < numVertices; ++ i )
1655   {
1656      const Point3F& a = vertices[ i ];
1657      const Point3F& b = vertices[ ( i + 1 ) % numVertices ];
1658      const Point3F& c = vertices[ ( i + 2 ) % numVertices ];
1659
1660      const F32 crossProductLength = mCross( b - a, c - b ).len();
1661      
1662      if( crossProductLength < 0.f )
1663         numNegative ++;
1664      else if( crossProductLength > 0.f )
1665         numPositive ++;
1666
1667      if( numNegative && numPositive )
1668         return false;
1669   }
1670
1671   return true;
1672}
1673
1674//-----------------------------------------------------------------------------
1675
1676bool clipFrustumByPolygon( const Point3F* points, U32 numPoints, const RectI& viewport, const MatrixF& world,
1677                           const MatrixF& projection, const Frustum& inFrustum, const Frustum& rootFrustum, Frustum& outFrustum )
1678{
1679   enum
1680   {
1681      MAX_RESULT_VERTICES = 64,
1682      MAX_INPUT_VERTICES = MAX_RESULT_VERTICES - Frustum::PlaneCount // Clipping against each plane may add a vertex.
1683   };
1684
1685   AssertFatal( numPoints <= MAX_INPUT_VERTICES, "MathUtils::clipFrustumByPolygon - Too many vertices!" );
1686   if( numPoints > MAX_INPUT_VERTICES )
1687      return false;
1688
1689   // First, we need to clip the polygon against inFrustum.
1690   //
1691   // Use two buffers here in interchanging roles as sources and targets
1692   // in clipping against the frustum planes.
1693
1694   Point3F polygonBuffer1[ MAX_RESULT_VERTICES ];
1695   Point3F polygonBuffer2[ MAX_RESULT_VERTICES ];
1696
1697   Point3F* tempPolygon = polygonBuffer1;
1698   Point3F* clippedPolygon = polygonBuffer2;
1699
1700   dMemcpy( clippedPolygon, points, numPoints * sizeof( points[ 0 ] ) );
1701
1702   U32 numClippedPolygonVertices = numPoints;
1703   U32 numTempPolygonVertices = 0;
1704
1705   for( U32 nplane = 0; nplane < Frustum::PlaneCount; ++ nplane )
1706   {
1707      // Make the output of the last iteration the
1708      // input of this iteration.
1709
1710     T3D::swap( tempPolygon, clippedPolygon );
1711      numTempPolygonVertices = numClippedPolygonVertices;
1712
1713      // Clip our current remainder of the original polygon
1714      // against the current plane.
1715
1716      const PlaneF& plane = inFrustum.getPlanes()[ nplane ];
1717      numClippedPolygonVertices = plane.clipPolygon( tempPolygon, numTempPolygonVertices, clippedPolygon );
1718
1719      // If the polygon was completely on the backside of the plane,
1720      // then polygon is outside the frustum.  In this case, return false
1721      // to indicate we haven't clipped anything.
1722
1723      if( !numClippedPolygonVertices )
1724         return false;
1725   }
1726
1727   // Project the clipped polygon into screen space.
1728
1729   MatrixF worldProjection = projection;
1730   worldProjection.mul( world ); // Premultiply world*projection so we don't have to do this over and over for each point.
1731
1732   Point3F projectedPolygon[ 10 ];
1733   for( U32 i = 0; i < numClippedPolygonVertices; ++ i )
1734      mProjectWorldToScreen(
1735         clippedPolygon[ i ],
1736         &projectedPolygon[ i ],
1737         viewport,
1738         worldProjection
1739      );
1740
1741   // Put an axis-aligned rectangle around our polygon.
1742
1743   Point2F minPoint( projectedPolygon[ 0 ].x, projectedPolygon[ 0 ].y );
1744   Point2F maxPoint( projectedPolygon[ 0 ].x, projectedPolygon[ 0 ].y );
1745
1746   for( U32 i = 1; i < numClippedPolygonVertices; ++ i )
1747   {
1748      minPoint.setMin( Point2F( projectedPolygon[ i ].x, projectedPolygon[ i ].y ) );
1749      maxPoint.setMax( Point2F( projectedPolygon[ i ].x, projectedPolygon[ i ].y ) );
1750   }
1751
1752   RectF area( minPoint, maxPoint - minPoint );
1753
1754   // Finally, reduce the input frustum to the given area.  Note that we
1755   // use rootFrustum here instead of inFrustum as the latter does not necessarily
1756   // represent the full viewport we are using here which thus would skew the mapping.
1757
1758   return reduceFrustum( rootFrustum, viewport, area, outFrustum );
1759}
1760
1761//-----------------------------------------------------------------------------
1762
1763U32 extrudePolygonEdges( const Point3F* vertices, U32 numVertices, const Point3F& direction, PlaneF* outPlanes )
1764{
1765   U32 numPlanes = 0;
1766   U32 lastVertex = numVertices - 1;
1767   bool invert = false;
1768
1769   for( U32 i = 0; i < numVertices; lastVertex = i, ++ i )
1770   {
1771      const Point3F& v1 = vertices[ i ];
1772      const Point3F& v2 = vertices[ lastVertex ];
1773
1774      // Skip the edge if it's length is really short.
1775
1776      const Point3F edgeVector = v2 - v1;
1777      if( edgeVector.len() < 0.05 )
1778         continue;
1779
1780      // Compute the plane normal.  The direction and the edge vector
1781      // basically define the orientation of the plane so their cross
1782      // product is the plane normal.
1783
1784      Point3F normal;
1785      if( !invert )
1786         normal = mCross( edgeVector, direction );
1787      else
1788         normal = mCross( direction, edgeVector );
1789
1790      // Create a plane for the edge.
1791
1792      outPlanes[ numPlanes ] = PlaneF( v1, normal );
1793      numPlanes ++;
1794
1795      // If this is the first plane that we have created, find out whether
1796      // the vertex ordering is giving us the plane orientations that we want
1797      // (facing inside).  If not, invert vertex order from now on.
1798
1799      if( i == 0 )
1800      {
1801         const PlaneF& plane = outPlanes[ numPlanes - 1 ];
1802         for( U32 n = i + 1; n < numVertices; ++ n )
1803         {
1804            const PlaneF::Side side = plane.whichSide( vertices[ n ] );
1805            if( side == PlaneF::On )
1806               continue;
1807
1808            if( side != PlaneF::Front )
1809               invert = true;
1810            break;
1811         }
1812      }
1813   }
1814
1815   return numPlanes;
1816}
1817
1818//-----------------------------------------------------------------------------
1819
1820U32 extrudePolygonEdgesFromPoint( const Point3F* vertices, U32 numVertices, const Point3F& fromPoint, PlaneF* outPlanes )
1821{
1822   U32 numPlanes = 0;
1823   U32 lastVertex = numVertices - 1;
1824   bool invert = false;
1825
1826   for( U32 i = 0; i < numVertices; lastVertex = i, ++ i )
1827   {
1828      const Point3F& v1 = vertices[ i ];
1829      const Point3F& v2 = vertices[ lastVertex ];
1830
1831      // Skip the edge if it's length is really short.
1832
1833      const Point3F edgeVector = v2 - v1;
1834      if( edgeVector.len() < 0.05 )
1835         continue;
1836
1837      // Create a plane for the edge.
1838
1839      if( !invert )
1840         outPlanes[ numPlanes ] = PlaneF( v1, fromPoint, v2 );
1841      else
1842         outPlanes[ numPlanes ] = PlaneF( v2, fromPoint, v1 );
1843      
1844      numPlanes ++;
1845
1846      // If this is the first plane that we have created, find out whether
1847      // the vertex ordering is giving us the plane orientations that we want
1848      // (facing inside).  If not, invert vertex order from now on.
1849
1850      if( i == 0 )
1851      {
1852         const PlaneF& plane = outPlanes[ numPlanes - 1 ];
1853         for( U32 n = i + 1; n < numVertices; ++ n )
1854         {
1855            const PlaneF::Side side = plane.whichSide( vertices[ n ] );
1856            if( side == PlaneF::On )
1857               continue;
1858
1859            if( side != PlaneF::Front )
1860               invert = true;
1861            break;
1862         }
1863      }
1864   }
1865
1866   return numPlanes;
1867}
1868
1869//-----------------------------------------------------------------------------
1870
1871void mBuildHull2D(const Vector<Point2F> _inPoints, Vector<Point2F> &hullPoints)
1872{
1873   /// Andrew's monotone chain convex hull algorithm implementation
1874
1875   struct Util
1876   {
1877      //compare by x and then by y   
1878      static int CompareLexicographic( const Point2F *a, const Point2F *b)
1879      {
1880         return a->x < b->x || (a->x == b->x && a->y < b->y);
1881      }
1882   };
1883
1884   hullPoints.clear();
1885   hullPoints.setSize( _inPoints.size()*2 );
1886
1887   // sort in points by x and then by y
1888   Vector<Point2F> inSortedPoints = _inPoints;
1889   inSortedPoints.sort( &Util::CompareLexicographic );
1890
1891   Point2F* lowerHullPtr = hullPoints.address();
1892   U32 lowerHullIdx = 0;
1893
1894   //lower part of hull
1895   for( int i = 0; i < inSortedPoints.size(); ++i )
1896   {      
1897      while( lowerHullIdx >= 2 && mCross( lowerHullPtr[ lowerHullIdx - 2], lowerHullPtr[lowerHullIdx - 1], inSortedPoints[i] ) <= 0 )
1898         --lowerHullIdx;
1899
1900      lowerHullPtr[lowerHullIdx++] = inSortedPoints[i];
1901   }
1902
1903   --lowerHullIdx; // last point are the same as first in upperHullPtr
1904
1905   Point2F* upperHullPtr = hullPoints.address() + lowerHullIdx;
1906   U32 upperHullIdx = 0;
1907
1908   //upper part of hull
1909   for( int i = inSortedPoints.size()-1; i >= 0; --i )
1910   {
1911      while( upperHullIdx >= 2 && mCross( upperHullPtr[ upperHullIdx - 2], upperHullPtr[upperHullIdx - 1], inSortedPoints[i] ) <= 0 )
1912         --upperHullIdx;
1913
1914      upperHullPtr[upperHullIdx++] = inSortedPoints[i];
1915   }
1916
1917   hullPoints.setSize( lowerHullIdx + upperHullIdx );
1918}
1919
1920} // namespace MathUtils
1921