Torque3D/Engine/source/math/mathUtils.cpp

//-----------------------------------------------------------------------------
// Copyright (c) 2012 GarageGames, LLC
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
//-----------------------------------------------------------------------------

#include "platform/platform.h"
#include "math/util/frustum.h"
#include "math/mathUtils.h"

#include "math/mMath.h"
#include "math/mRandom.h"
#include "math/util/frustum.h"
#include "platform/profiler.h"
#include "core/tAlgorithm.h"
#include "gfx/gfxDevice.h"

namespace MathUtils
{

MRandomLCG sgRandom(0xdeadbeef); ///< Our random number generator.

//-----------------------------------------------------------------------------

bool capsuleCapsuleOverlap(const Point3F & a1, const Point3F & b1, F32 rad1, const Point3F & a2, const Point3F & b2, F32 rad2)
{
   F32 s,t;
   Point3F c1,c2;
   F32 dist = segmentSegmentNearest(a1,b1,a2,b2,s,t,c1,c2);
   return dist <= (rad1+rad2)*(rad1+rad2);
}

//-----------------------------------------------------------------------------

F32 segmentSegmentNearest(const Point3F & p1, const Point3F & q1, const Point3F & p2, const Point3F & q2, F32 & s, F32 & t, Point3F & c1, Point3F & c2)
{
   Point3F d1 = q1-p1;
   Point3F d2 = q2-p2;
   Point3F r = p1-p2;
   F32 a = mDot(d1,d1);
   F32 e = mDot(d2,d2);
   F32 f = mDot(d2,r);
   
   const F32 EPSILON = 0.001f;
   
   if (a <= EPSILON && e <= EPSILON)
   {
      s = t = 0.0f;
      c1 = p1;
      c2 = p2;
      return mDot(c1-c2,c1-c2);
   }
   
   if (a <= EPSILON)
   {
      s = 0.0f;
      t = mClampF(f/e,0.0f,1.0f);
   }
   else
   {
      F32 c = mDot(d1,r);
      if (e <= EPSILON)
      {
         t = 0.0f;
         s = mClampF(-c/a,0.0f,1.0f);
      }
      else
      {
         F32 b = mDot(d1,d2);
         F32 denom = a*e-b*b;
         if (denom != 0.0f)
            s = mClampF((b*f-c*e)/denom,0.0f,1.0f);
         else
            s = 0.0f;
         F32 tnom = b*s+f;
         if (tnom < 0.0f)
         {
            t = 0.0f;
            s = mClampF(-c/a,0.0f,1.0f);
         }
         else if (tnom>e)
         {
            t = 1.0f;
            s = mClampF((b-c)/a,0.0f,1.0f);
         }
         else
            t = tnom/e;
      }
   }
   
   c1 = p1 + d1*s;
   c2 = p2 + d2*t;
   return mDot(c1-c2,c1-c2);
}

//-----------------------------------------------------------------------------

bool capsuleSphereNearestOverlap(const Point3F & A0, const Point3F A1, F32 radA, const Point3F & B, F32 radB, F32 & t)
{
   Point3F V = A1-A0;
   Point3F A0B = A0-B;
   F32 d1 = mDot(A0B,V);
   F32 d2 = mDot(A0B,A0B);
   F32 d3 = mDot(V,V);
   F32 R2 = (radA+radB)*(radA+radB);
   if (d2<R2)
   {
      // starting in collision state
      t=0;
      return true;
   }
   if (d3<0.01f)
      // no movement, and don't start in collision state, so no collision
      return false;

   F32 b24ac = mSqrt(d1*d1-d2*d3+d3*R2);
   F32 t1 = (-d1-b24ac)/d3;
   if (t1>0 && t1<1.0f)
   {
      t=t1;
      return true;
   }
   F32 t2 = (-d1+b24ac)/d3;
   if (t2>0 && t2<1.0f)
   {
      t=t2;
      return true;
   }
   if (t1<0 && t2>0)
   {
      t=0;
      return true;
   }
   return false;   
}

//-----------------------------------------------------------------------------

void vectorRotateZAxis( Point3F &vector, F32 radians )
{
   F32 sin, cos;
   mSinCos(radians, sin, cos);
   F32 x = cos * vector.x - sin * vector.y;
   F32 y = sin * vector.x + cos * vector.y;
   vector.x = x;
   vector.y = y;     
}

void vectorRotateZAxis( F32 radians, Point3F *vectors, U32 count )
{
   F32 sin, cos;
   mSinCos(radians, sin, cos);

   F32 x, y;
   const Point3F *end = vectors + count;
   for ( ; vectors != end; vectors++ )
   {
      x = cos * vectors->x - sin * vectors->y;
      y = sin * vectors->x + cos * vectors->y;
      vectors->x = x;
      vectors->y = y;      
   }
}

//-----------------------------------------------------------------------------

void getZBiasProjectionMatrix( F32 bias, const Frustum &frustum, MatrixF *outMat, bool rotate )
{
   Frustum temp(frustum);
   temp.setNearDist(frustum.getNearDist() + bias);
   temp.getProjectionMatrix(outMat, rotate);
}

//-----------------------------------------------------------------------------

MatrixF createOrientFromDir( const Point3F &direction )
{
	Point3F j = direction;
	Point3F k(0.0f, 0.0f, 1.0f);
	Point3F i;
	
	mCross( j, k, &i );

	if( i.magnitudeSafe() == 0.0f )
	{
		i.set( 0.0f, -1.0f, 0.0f );
	}

	i.normalizeSafe();
	mCross( i, j, &k );

   MatrixF mat( true );
   mat.setColumn( 0, i );
   mat.setColumn( 1, j );
   mat.setColumn( 2, k );

	return mat;
}

//-----------------------------------------------------------------------------

void getMatrixFromUpVector( const VectorF &up, MatrixF *outMat )
{
   AssertFatal( up.isUnitLength(), "MathUtils::getMatrixFromUpVector() - Up vector was not normalized!" );
   AssertFatal( outMat, "MathUtils::getMatrixFromUpVector() - Got null output matrix!" );
   AssertFatal( outMat->isAffine(), "MathUtils::getMatrixFromUpVector() - Got uninitialized matrix!" );

   VectorF forward = mPerp( up );
   VectorF right = mCross( forward, up );
   right.normalize();
   forward = mCross( up, right );
   forward.normalize();

   outMat->setColumn( 0, right );
   outMat->setColumn( 1, forward );
   outMat->setColumn( 2, up );
}

//-----------------------------------------------------------------------------

void getMatrixFromForwardVector( const VectorF &forward, MatrixF *outMat  )
{
   AssertFatal( forward.isUnitLength(), "MathUtils::getMatrixFromForwardVector() - Forward vector was not normalized!" );
   AssertFatal( outMat, "MathUtils::getMatrixFromForwardVector() - Got null output matrix!" );
   AssertFatal( outMat->isAffine(), "MathUtils::getMatrixFromForwardVector() - Got uninitialized matrix!" );

   VectorF up = mPerp( forward );
   VectorF right = mCross( forward, up );
   right.normalize();
   up = mCross( right, forward );
   up.normalize();

   outMat->setColumn( 0, right );
   outMat->setColumn( 1, forward );
   outMat->setColumn( 2, up );
}

//-----------------------------------------------------------------------------

Point3F randomDir( const Point3F &axis, F32 thetaAngleMin, F32 thetaAngleMax,
                                        F32 phiAngleMin, F32 phiAngleMax )
{
   MatrixF orient = createOrientFromDir( axis );
   Point3F axisx;
   orient.getColumn( 0, &axisx );

   F32 theta = (thetaAngleMax - thetaAngleMin) * sgRandom.randF() + thetaAngleMin;
   F32 phi = (phiAngleMax - phiAngleMin) * sgRandom.randF() + phiAngleMin;

   // Both phi and theta are in degs.  Create axis angles out of them, and create the
   //  appropriate rotation matrix...
   AngAxisF thetaRot(axisx, theta * (M_PI_F / 180.0f));
   AngAxisF phiRot(axis,    phi   * (M_PI_F / 180.0f));

   Point3F ejectionAxis = axis;

   MatrixF temp(true);
   thetaRot.setMatrix(&temp);
   temp.mulP(ejectionAxis);
   phiRot.setMatrix(&temp);
   temp.mulP(ejectionAxis);

   return ejectionAxis;
}

//-----------------------------------------------------------------------------

Point3F randomPointInSphere( F32 radius )
{
   AssertFatal( radius > 0.0f, "MathUtils::randomPointInRadius - radius must be positive" );

   #define MAX_TRIES 20

   Point3F out;
   F32 radiusSq = radius * radius;

   for ( S32 i = 0; i < MAX_TRIES; i++ )
   {
      out.x = sgRandom.randF(-radius,radius);
      out.y = sgRandom.randF(-radius,radius);
      out.z = sgRandom.randF(-radius,radius);

      if ( out.lenSquared() < radiusSq )
         return out;
   }

   AssertFatal( false, "MathUtils::randomPointInRadius - something is wrong, should not fail this many times." );
   return Point3F::Zero;
}

//-----------------------------------------------------------------------------

Point2F randomPointInCircle( F32 radius )
{
   AssertFatal( radius > 0.0f, "MathUtils::randomPointInRadius - radius must be positive" );

   #define MAX_TRIES 20

   Point2F out;
   F32 radiusSq = radius * radius;

   for ( S32 i = 0; i < MAX_TRIES; i++ )
   {
      out.x = sgRandom.randF(-radius,radius);
      out.y = sgRandom.randF(-radius,radius);

      if ( out.lenSquared() < radiusSq )
         return out;
   }

   AssertFatal( false, "MathUtils::randomPointInRadius - something is wrong, should not fail this many times." );
   return Point2F::Zero;
}

//-----------------------------------------------------------------------------

void getAnglesFromVector( const VectorF &vec, F32 &yawAng, F32 &pitchAng )
{
   yawAng = mAtan2( vec.x, vec.y );
   if( yawAng < 0.0f )
      yawAng += M_2PI_F;

   if( mFabs(vec.x) > mFabs(vec.y) )
      pitchAng = mAtan2( mFabs(vec.z), mFabs(vec.x) );
   else
      pitchAng = mAtan2( mFabs(vec.z), mFabs(vec.y) );
   if( vec.z < 0.0f )
      pitchAng = -pitchAng;
}

//-----------------------------------------------------------------------------

void getVectorFromAngles( VectorF &vec, F32 yawAng, F32 pitchAng )
{
   VectorF  pnt( 0.0f, 1.0f, 0.0f );

   EulerF   rot( -pitchAng, 0.0f, 0.0f );
   MatrixF  mat( rot );

   rot.set( 0.0f, 0.0f, yawAng );
   MatrixF   mat2( rot );

   mat.mulV( pnt );
   mat2.mulV( pnt );

   vec = pnt;
}

F32 getAngleBetweenVectors(VectorF vecA, VectorF vecB)
{
   F32 dot = mDot(vecA, vecB);
   F32 lenSq1 = vecA.lenSquared();
   F32 lenSq2 = vecB.lenSquared();
   F32 angle = mAcos(dot / mSqrt(lenSq1 * lenSq2));

   return angle;
}

F32 getSignedAngleBetweenVectors(VectorF vecA, VectorF vecB, VectorF norm)
{
   // angle in 0-180
   F32 angle = getAngleBetweenVectors(vecA, vecB);
   F32 sign = mSign(mDot(norm, mCross(vecA, vecB)));

   // angle in -179-180
   F32 signed_angle = angle * sign;

   return signed_angle;
}

//-----------------------------------------------------------------------------

void transformBoundingBox(const Box3F &sbox, const MatrixF &mat, const Point3F scale, Box3F &dbox)
{
	Point3F center;

	// set transformed center...
	sbox.getCenter(&center);
   center.convolve(scale);
	mat.mulP(center);
	dbox.minExtents = center;
	dbox.maxExtents = center;

	Point3F val;
	for(U32 ix=0; ix<2; ix++)
	{
		if(ix & 0x1)
			val.x = sbox.minExtents.x;
		else
			val.x = sbox.maxExtents.x;

		for(U32 iy=0; iy<2; iy++)
		{
			if(iy & 0x1)
				val.y = sbox.minExtents.y;
			else
				val.y = sbox.maxExtents.y;

			for(U32 iz=0; iz<2; iz++)
			{
				if(iz & 0x1)
					val.z = sbox.minExtents.z;
				else
					val.z = sbox.maxExtents.z;

				Point3F v1, v2;
            v1 = val;
            v1.convolve(scale);
				mat.mulP(v1, &v2);
				dbox.minExtents.setMin(v2);
				dbox.maxExtents.setMax(v2);
			}
		}
	}
}

//-----------------------------------------------------------------------------

bool mProjectWorldToScreen(   const Point3F &in, 
                              Point3F *out, 
                              const RectI &view, 
                              const MatrixF &world, 
                              const MatrixF &projection )
{
   MatrixF worldProjection = projection;
   worldProjection.mul(world);

   return mProjectWorldToScreen( in, out, view, worldProjection );
}

//-----------------------------------------------------------------------------

bool mProjectWorldToScreen(   const Point3F &in, 
                              Point3F *out, 
                              const RectI &view, 
                              const MatrixF &worldProjection )
{
   Point4F temp(in.x,in.y,in.z,1.0f);
   worldProjection.mul(temp);

   // Perform the perspective division.  For orthographic
   // projections, temp.w will be 1.

   temp.x /= temp.w;
   temp.y /= temp.w;
   temp.z /= temp.w;

   // Take the normalized device coordinates (NDC) and transform them
   // into device coordinates.

   out->x = (temp.x + 1.0f) / 2.0f * view.extent.x + view.point.x;
   out->y = (1.0f - temp.y) / 2.0f * view.extent.y + view.point.y;
   out->z = temp.z;

   if ( out->z < 0.0f || out->z > 1.0f || 
        out->x < (F32)view.point.x || out->x > (F32)view.point.x + (F32)view.extent.x ||
        out->y < (F32)view.point.y || out->y > (F32)view.point.y + (F32)view.extent.y )
      return false;

   return true;
}

//-----------------------------------------------------------------------------

void mProjectScreenToWorld(   const Point3F &in, 
                              Point3F *out, 
                              const RectI &view, 
                              const MatrixF &world, 
                              const MatrixF &projection, 
                              F32 zfar, 
                              F32 znear )
{
   MatrixF invWorldProjection = projection;
   invWorldProjection.mul(world);
   invWorldProjection.inverse();

   Point3F vec;
   vec.x = (in.x - view.point.x) * 2.0f / view.extent.x - 1.0f;
   vec.y = -(in.y - view.point.y) * 2.0f / view.extent.y + 1.0f;
   vec.z = (znear + in.z * (zfar - znear))/zfar;

   invWorldProjection.mulV(vec);
   vec *= 1.0f + in.z * zfar;

   invWorldProjection.getColumn(3, out);
   (*out) += vec;
}

//-----------------------------------------------------------------------------

bool pointInPolygon( const Point2F *verts, U32 vertCount, const Point2F &testPt )
{
  U32 i, j, c = 0;
  for ( i = 0, j = vertCount-1; i < vertCount; j = i++ ) 
  {
    if ( ( ( verts[i].y > testPt.y ) != ( verts[j].y > testPt.y ) ) &&
	         ( testPt.x <   ( verts[j].x - verts[i].x ) * 
                           ( testPt.y - verts[i].y ) / 
                           ( verts[j].y - verts[i].y ) + verts[i].x ) )
       c = !c;
  }
  
  return c != 0;
}  

//-----------------------------------------------------------------------------

F32 mTriangleDistance( const Point3F &A, const Point3F &B, const Point3F &C, const Point3F &P, IntersectInfo* info )
{
    Point3F diff = A - P;
    Point3F edge0 = B - A;
    Point3F edge1 = C - A;
    F32 a00 = edge0.lenSquared();
    F32 a01 = mDot( edge0, edge1 );
    F32 a11 = edge1.lenSquared();
    F32 b0 = mDot( diff, edge0 );
    F32 b1 = mDot( diff, edge1 );
    F32 c = diff.lenSquared();
    F32 det = mFabs(a00*a11-a01*a01);
    F32 s = a01*b1-a11*b0;
    F32 t = a01*b0-a00*b1;
    F32 sqrDistance;

    if (s + t <= det)
    {
        if (s < 0.0f)
        {
            if (t < 0.0f)  // region 4
            {
                if (b0 < 0.0f)
                {
                    t = 0.0f;
                    if (-b0 >= a00)
                    {
                        s = 1.0f;
                        sqrDistance = a00 + (2.0f)*b0 + c;
                    }
                    else
                    {
                        s = -b0/a00;
                        sqrDistance = b0*s + c;
                    }
                }
                else
                {
                    s = 0.0f;
                    if (b1 >= 0.0f)
                    {
                        t = 0.0f;
                        sqrDistance = c;
                    }
                    else if (-b1 >= a11)
                    {
                        t = 1.0f;
                        sqrDistance = a11 + 2.0f*b1 + c;
                    }
                    else
                    {
                        t = -b1/a11;
                        sqrDistance = b1*t + c;
                    }
                }
            }
            else  // region 3
            {
                s = 0.0f;
                if (b1 >= 0.0f)
                {
                    t = 0.0f;
                    sqrDistance = c;
                }
                else if (-b1 >= a11)
                {
                    t = 1.0f;
                    sqrDistance = a11 + 2.0f*b1 + c;
                }
                else
                {
                    t = -b1/a11;
                    sqrDistance = b1*t + c;
                }
            }
        }
        else if (t < 0.0f)  // region 5
        {
            t = 0.0f;
            if (b0 >= 0.0f)
            {
                s = 0.0f;
                sqrDistance = c;
            }
            else if (-b0 >= a00)
            {
                s = 1.0f;
                sqrDistance = a00 + 2.0f*b0 + c;
            }
            else
            {
                s = -b0/a00;
                sqrDistance = b0*s + c;
            }
        }
        else  // region 0
        {
            // minimum at interior point
            F32 invDet = 1.0f / det;
            s *= invDet;
            t *= invDet;
            sqrDistance = s * (a00*s + a01*t + 2.0f*b0) +
                t * (a01*s + a11*t + 2.0f*b1) + c;
        }
    }
    else
    {
        F32 tmp0, tmp1, numer, denom;

        if (s < 0.0f)  // region 2
        {
            tmp0 = a01 + b0;
            tmp1 = a11 + b1;
            if (tmp1 > tmp0)
            {
                numer = tmp1 - tmp0;
                denom = a00 - 2.0f*a01 + a11;
                if (numer >= denom)
                {
                    s = 1.0f;
                    t = 0.0f;
                    sqrDistance = a00 + 2.0f*b0 + c;
                }
                else
                {
                    s = numer/denom;
                    t = 1.0f - s;
                    sqrDistance = s * (a00*s + a01*t + 2.0f*b0) +
                                  t * (a01*s + a11*t + 2.0f*b1) + c;
                }
            }
            else
            {
                s = 0.0f;
                if (tmp1 <= 0.0f)
                {
                    t = 1.0f;
                    sqrDistance = a11 + 2.0f*b1 + c;
                }
                else if (b1 >= 0.0f)
                {
                    t = 0.0f;
                    sqrDistance = c;
                }
                else
                {
                    t = -b1/a11;
                    sqrDistance = b1*t + c;
                }
            }
        }
        else if (t < 0.0f)  // region 6
        {
            tmp0 = a01 + b1;
            tmp1 = a00 + b0;
            if (tmp1 > tmp0)
            {
                numer = tmp1 - tmp0;
                denom = a00 - 2.0f*a01 + a11;
                if (numer >= denom)
                {
                    t = 1.0f;
                    s = 0.0f;
                    sqrDistance = a11 + 2.0f*b1 + c;
                }
                else
                {
                    t = numer/denom;
                    s = 1.0f - t;
                    sqrDistance = s * (a00*s + a01*t + 2.0f*b0) +
                                  t * (a01*s + a11*t + 2.0f*b1) + c;
                }
            }
            else
            {
                t = 0.0f;
                if (tmp1 <= 0.0f)
                {
                    s = 1.0f;
                    sqrDistance = a00 + 2.0f*b0 + c;
                }
                else if (b0 >= 0.0f)
                {
                    s = 0.0f;
                    sqrDistance = c;
                }
                else
                {
                    s = -b0/a00;
                    sqrDistance = b0*s + c;
                }
            }
        }
        else  // region 1
        {
            numer = a11 + b1 - a01 - b0;
            if (numer <= 0.0f)
            {
                s = 0.0f;
                t = 1.0f;
                sqrDistance = a11 + 2.0f*b1 + c;
            }
            else
            {
                denom = a00 - 2.0f*a01 + a11;
                if (numer >= denom)
                {
                    s = 1.0f;
                    t = 0.0f;
                    sqrDistance = a00 + 2.0f*b0 + c;
                }
                else
                {
                    s = numer/denom;
                    t = 1.0f - s;
                    sqrDistance = s * (a00*s + a01*t + 2.0f*b0) +
                                  t * (a01*s + a11*t + 2.0f*b1) + c;
                }
            }
        }
    }

    // account for numerical round-off error
    if (sqrDistance < 0.0f)
        sqrDistance = 0.0f;

    // This also calculates the barycentric coordinates and the closest point!
    //m_kClosestPoint0 = P;
    //m_kClosestPoint1 = A + s*edge0 + t*edge1;
    //m_afTriangleBary[1] = s;
    //m_afTriangleBary[2] = t;
    //m_afTriangleBary[0] = (Real)1.0 - fS - fT;
    if(info)
    {
       info->segment.p0 = P;
       info->segment.p1 = A + s*edge0 + t*edge1;
       info->bary.x = s;
       info->bary.y = t;
       info->bary.z = 1.0f - s - t;
    }

    return sqrDistance;
}

//-----------------------------------------------------------------------------

Point3F mTriangleNormal( const Point3F &a, const Point3F &b, const Point3F &c )
{
   // Vector from b to a.
   const F32 ax = a.x-b.x;
   const F32 ay = a.y-b.y;
   const F32 az = a.z-b.z;
   // Vector from b to c.
   const F32 cx = c.x-b.x;
   const F32 cy = c.y-b.y;
   const F32 cz = c.z-b.z;

   Point3F n;

   // This is an in-line cross product.
   n.x = ay*cz - az*cy;
   n.y = az*cx - ax*cz;
   n.z = ax*cy - ay*cx;
   m_point3F_normalize( (F32*)(&n) );

   return n;
}

//-----------------------------------------------------------------------------

Point3F mClosestPointOnSegment( const Point3F &a, const Point3F &b, const Point3F &p )
{
	Point3F c = p - a;               // Vector from a to Point
   Point3F v = (b - a);
	F32 d = v.len();           // Length of the line segment
   v.normalize();	                  // Unit Vector from a to b
	F32 t = mDot( v, c );            // Intersection point Distance from a

	// Check to see if the point is on the line
	// if not then return the endpoint
	if(t < 0) return a;
	if(t > d) return b;

	// get the distance to move from point a
	v *= t;

	// move from point a to the nearest point on the segment
	return a + v;
}

//-----------------------------------------------------------------------------

void mShortestSegmentBetweenLines( const Line &line0, const Line &line1, LineSegment *outSegment )
{   
   // compute intermediate parameters
   Point3F w0 = line0.origin - line1.origin;
   F32 a = mDot( line0.direction, line0.direction );
   F32 b = mDot( line0.direction, line1.direction );
   F32 c = mDot( line1.direction, line1.direction );
   F32 d = mDot( line0.direction, w0 );
   F32 e = mDot( line1.direction, w0 );

   F32 denom = a*c - b*b;

   if ( denom > -0.001f && denom < 0.001f )
   {
      outSegment->p0 = line0.origin;
      outSegment->p1 = line1.origin + (e/c)*line1.direction;
   }
   else
   {
      outSegment->p0 = line0.origin + ((b*e - c*d)/denom)*line0.direction;
      outSegment->p1 = line1.origin + ((a*e - b*d)/denom)*line1.direction;
   }
}

//-----------------------------------------------------------------------------

U32 greatestCommonDivisor( U32 u, U32 v )
{
   // http://en.wikipedia.org/wiki/Binary_GCD_algorithm
      
   S32 shift;

   /* GCD(0,x) := x */
   if (u == 0 || v == 0)
      return u | v;

   /* Left shift := lg K, where K is the greatest power of 2
   dividing both u and v. */
   for (shift = 0; ((u | v) & 1) == 0; ++shift) {
      u >>= 1;
      v >>= 1;
   }

   while ((u & 1) == 0)
      u >>= 1;

   /* From here on, u is always odd. */
   do {
      while ((v & 1) == 0)  /* Loop X */
         v >>= 1;

      /* Now u and v are both odd, so diff(u, v) is even.
      Let u = min(u, v), v = diff(u, v)/2. */
      if (u < v) {
         v -= u;
      } else {
         U32 diff = u - v;
         u = v;
         v = diff;
      }
      v >>= 1;
   } while (v != 0);

   return u << shift;
}

//-----------------------------------------------------------------------------

bool mLineTriangleCollide( const Point3F &p1, const Point3F &p2, 
                           const Point3F &t1, const Point3F &t2, const Point3F &t3,
                           Point3F *outUVW, F32 *outT )
{
   VectorF ab = t2 - t1;
   VectorF ac = t3 - t1;
   VectorF qp = p1 - p2;

   // Compute triangle normal. Can be precalculated or cached if
   // intersecting multiple segments against the same triangle
   VectorF n = mCross( ab, ac );

   // Compute denominator d. If d <= 0, segment is parallel to or points
   // away from triangle, so exit early
   F32 d = mDot( qp, n );
   if ( d <= 0.0f ) 
      return false;

   // Compute intersection t value of pq with plane of triangle. A ray
   // intersects if 0 <= t. Segment intersects iff 0 <= t <= 1. Delay
   // dividing by d until intersection has been found to pierce triangle
   VectorF ap = p1 - t1;
   F32 t = mDot( ap, n );
   if ( t < 0.0f ) 
      return false;      
   if ( t > d ) 
      return false; // For segment; exclude this code line for a ray test

   // Compute barycentric coordinate components and test if within bounds
   VectorF e = mCross( qp, ap );
   F32 v = mDot( ac, e );
   if ( v < 0.0f || v > d )
      return false;
   F32 w = -mDot( ab, e );
   if ( w < 0.0f || v + w > d ) 
      return false;

   // Segment/ray intersects triangle. Perform delayed division and
   // compute the last barycentric coordinate component
   const F32 ood = 1.0f / d;
   
   if ( outT )
      *outT = t * ood;
      
   if ( outUVW )
   {
      v *= ood;
      w *= ood;
      outUVW->set( 1.0f - v - w, v, w );
   }

   return true;
}

//-----------------------------------------------------------------------------

bool mRayQuadCollide(   const Quad &quad, 
                        const Ray &ray, 
                        Point2F *outUV,
                        F32 *outT )
{
   static const F32 eps = F32(10e-6);

   // Rejects rays that are parallel to Q, and rays that intersect the plane of
   // Q either on the left of the line V00V01 or on the right of the line V00V10.

   //   p01-----eXX-----p11
   //   ^            . ^ |
   //   |          .     |
   //   e03     e02     eXX
   //   |    .           |
   //   |  .             |
   //   p00-----e01---->p10

   VectorF e01 = quad.p10 - quad.p00;
   VectorF e03 = quad.p01 - quad.p00;

   // If the ray is perfectly perpendicular to e03, which 
   // represents the entire planes tangent, then the 
   // result of this cross product (P) will equal e01
   // If it is parallel it will result in a vector opposite e01.

   // If the ray is heading DOWN the cross product will point to the RIGHT
   // If the ray is heading UP the cross product will point to the LEFT
   // We do not reject based on this though...
   //
   // In either case cross product will be more parallel to e01 the more
   // perpendicular the ray is to e03, and it will be more perpendicular to
   // e01 the more parallel it is to e03.
   VectorF P = mCross(ray.direction, e03);

   // det can be seen as 'the amount of vector e01 in the direction P'
   F32 det = mDot(e01, P);

   // Take a Abs of the dot because we do not care if the ray is heading up or down,
   // but if it is perfectly parallel to the quad we want to reject it.
   if ( mFabs(det) < eps ) 
      return false;

   F32 inv_det = 1.0f / det;

   VectorF T = ray.origin - quad.p00;

   // alpha can be seen as 'the amount of vector T in the direction P'
   // T is a vector up from the quads corner point 00 to the ray's origin.
   // P is the cross product of the ray and e01, which should be "roughly"
   // parallel with e03 but might be of either positive or negative magnitude.
   F32 alpha = mDot(T, P) * inv_det;
   if ( alpha < 0.0f ) 
      return false;

   // if (alpha > real(1.0)) return false; // Uncomment if VR is used.

   // The cross product of T and e01 should be roughly parallel to e03
   // and of either positive or negative magnitude.
   VectorF Q = mCross(T, e01);
   F32 beta = mDot(ray.direction, Q) * inv_det;
   if ( beta < 0.0f ) 
      return false; 

   // if (beta > real(1.0)) return false; // Uncomment if VR is used.

   if ( alpha + beta > 1.0f ) 
   //if ( false )
   {
      // Rejects rays that intersect the plane of Q either on the
      // left of the line V11V10 or on the right of the line V11V01.

      VectorF e23 = quad.p01 - quad.p11;
      VectorF e21 = quad.p10 - quad.p11;
      VectorF P_prime = mCross(ray.direction, e21);
      F32 det_prime = mDot(e23, P_prime);
      if ( mFabs(det_prime) < eps) 
         return false;
      F32 inv_det_prime = 1.0f / det_prime;
      VectorF T_prime = ray.origin - quad.p11;
      F32 alpha_prime = mDot(T_prime, P_prime) * inv_det_prime;
      if (alpha_prime < 0.0f) 
         return false;
      VectorF Q_prime = mCross(T_prime, e23);
      F32 beta_prime = mDot(ray.direction, Q_prime) * inv_det_prime;
      if (beta_prime < 0.0f) 
         return false;
   }

   // Compute the ray parameter of the intersection point, and
   // reject the ray if it does not hit Q.

   F32 t = mDot(e03, Q) * inv_det;
   if ( t < 0.0f ) 
      return false; 


   // Compute the barycentric coordinates of the fourth vertex.
   // These do not depend on the ray, and can be precomputed
   // and stored with the quadrilateral.  

   F32 alpha_11, beta_11;
   VectorF e02 = quad.p11 - quad.p00;
   VectorF n = mCross(e01, e03);

   if ( mFabs(n.x) >= mFabs(n.y) && 
      mFabs(n.x) >= mFabs(n.z) )
   {
      alpha_11 = ( e02.y * e03.z - e02.z * e03.y ) / n.x;
      beta_11  = ( e01.y * e02.z - e01.z * e02.y ) / n.x;
   }
   else if ( mFabs(n.y) >= mFabs(n.x) && 
      mFabs(n.y) >= mFabs(n.z) ) 
   {  
      alpha_11 = ((e02.z * e03.x) - (e02.x * e03.z)) / n.y;
      beta_11  = ((e01.z * e02.x) - (e01.x * e02.z)) / n.y;
   }
   else 
   {
      alpha_11 = ((e02.x * e03.y) - (e02.y * e03.x)) / n.z;
      beta_11  = ((e01.x * e02.y) - (e01.y * e02.x)) / n.z;
   }

   // Compute the bilinear coordinates of the intersection point.

   F32 u,v;

   if ( mFabs(alpha_11 - 1.0f) < eps) 
   {    
      // Q is a trapezium.
      u = alpha;
      if ( mFabs(beta_11 - 1.0f) < eps) 
         v = beta; // Q is a parallelogram.
      else 
         v = beta / ((u * (beta_11 - 1.0f)) + 1.0f); // Q is a trapezium.
   }
   else if ( mFabs(beta_11 - 1.0f) < eps) 
   {
      // Q is a trapezium.
      v = beta;
      u = alpha / ((v * (alpha_11 - 1.0f)) + 1.0f);
   }
   else 
   {
      F32 A = 1.0f - beta_11;
      F32 B = (alpha * (beta_11 - 1.0f))
         - (beta * (alpha_11 - 1.0f)) - 1.0f;
      F32 C = alpha;
      F32 D = (B * B) - (4.0f * A * C);
      F32 F = -0.5f * (B + (B < 0.0f ? -1.0f : 1.0f) ) * mSqrt(D);
      u = F / A;
      if ((u < 0.0f) || (u > 1.0f)) u = C / F;
      v = beta / ((u * (beta_11 - 1.0f)) + 1.0f); 
   }

   if ( outUV )
      outUV->set( u, v );  
   if ( outT )
      *outT = t;

   return true;  
}

//-----------------------------------------------------------------------------

// Used by sortQuadWindingOrder.
struct QuadSortPoint
{
   U32 id;
   F32 theta;
};

// Used by sortQuadWindingOrder.
S32 QSORT_CALLBACK cmpAngleAscending( const void *a, const void *b )
{
   const QuadSortPoint *p0 = (const QuadSortPoint*)a;
   const QuadSortPoint *p1 = (const QuadSortPoint*)b;   

	F32 diff = p1->theta - p0->theta;

	if ( diff > 0.0f )
		return -1;
	else if ( diff < 0.0f )
		return 1;
	else
		return 0;   
}

// Used by sortQuadWindingOrder.
S32 QSORT_CALLBACK cmpAngleDescending( const void *a, const void *b )
{
	const QuadSortPoint *p0 = (const QuadSortPoint*)a;
	const QuadSortPoint *p1 = (const QuadSortPoint*)b;   

	F32 diff = p1->theta - p0->theta;

	if ( diff > 0.0f )
		return 1;
	else if ( diff < 0.0f )
		return -1;
	else
		return 0;   
}

void sortQuadWindingOrder( const MatrixF &quadMat, bool clockwise, const Point3F *verts, U32 *vertMap, U32 count )
{
   PROFILE_SCOPE( MathUtils_sortQuadWindingOrder );

   if ( count == 0 )
      return;   
   
   Point3F *quadPoints = new Point3F[count];
   
   for ( S32 i = 0; i < count; i++ )   
	{		
      quadMat.mulP( verts[i], &quadPoints[i] );
		quadPoints[i].normalizeSafe();
	}

   sortQuadWindingOrder( clockwise, quadPoints, vertMap, count );

   delete [] quadPoints;
}

void sortQuadWindingOrder( bool clockwise, const Point3F *verts, U32 *vertMap, U32 count )
{
   QuadSortPoint *sortPoints = new QuadSortPoint[count];

   for ( S32 i = 0; i < count; i++ )
   {
      QuadSortPoint &sortPnt = sortPoints[i];
      const Point3F &vec = verts[i];

      sortPnt.id = i;

      F32 theta = mAtan2( vec.y, vec.x );

      if ( vec.y < 0.0f )
         theta = M_2PI_F + theta;

      sortPnt.theta = theta;
   }

   dQsort( sortPoints, count, sizeof( QuadSortPoint ), clockwise ? cmpAngleDescending : cmpAngleAscending ); 

   for ( S32 i = 0; i < count; i++ )
      vertMap[i] = sortPoints[i].id;

   delete [] sortPoints;
}

//-----------------------------------------------------------------------------

void buildMatrix( const VectorF *rvec, const VectorF *fvec, const VectorF *uvec, const VectorF *pos, MatrixF *outMat )
{     
   /// Work in Progress

   /*
   AssertFatal( !rvec || rvec->isUnitLength(), "MathUtils::buildMatrix() - Right vector was not normalized!" );  
   AssertFatal( !fvec || fvec->isUnitLength(), "MathUtils::buildMatrix() - Forward vector was not normalized!" );  
   AssertFatal( !uvec || uvec->isUnitLength(), "MathUtils::buildMatrix() - Up vector was not normalized!" ); 

   // Note this relationship:
   //
   // Column0  Column1  Column2
   // Axis X   Axis Y   Axis Z
   // Rvec     Fvec     Uvec
   //

   enum 
   {
      RVEC = 1,
      FVEC = 1 << 1,
      UVEC = 1 << 2,
      ALL = RVEC | FVEC | UVEC
   };

   U8 mask = 0;
   U8 count = 0;
   U8 axis0, axis1;

   if ( rvec )
   {
      mask |= RVEC;
      axis0 == 0;      
      count++;
   }
   if ( fvec )
   {
      mask |= FVEC;      
      if ( count == 0 )
         axis0 = 1;
      else
         axis1 = 1;
      count++;
   }
   if ( uvec )
   {
      mask |= UVEC;      
      count++;
   }

   U8 bR = 1;
   U8 bF = 1 << 1;
   U8 bU = 1 << 2;
   U8 bRF = bR | bF;
   U8 bRU = bR | bU;
   U8 bFU = bF | bU;
   U8 bRFU = bR | bF | bU;

   

   // Cross product map.
   U8 cpdMap[3][2] = 
   {
      { 1, 2 },
      { 2, 0 },
      { 0, 1 },
   }

   if ( count == 1 )
   {
      if ( mask == bR )
      {
         
      }
      else if ( mask == bF )
      {

      }
      else if ( mask == bU )
      {

      }
   }
   else if ( count == 2 )
   {
      if ( mask == bRF )
      {

      }
      else if ( mask == bRU )
      {

      }
      else if ( mask == bFU )
      {

      }
   }
   else // bRFU
   {

   }

   if ( rvec )
   {
      outMat->setColumn( 0, *rvec );

      if ( fvec )
      {
         outMat->setColumn( 1, *fvec );

         if ( uvec )         
            outMat->setColumn( 2, *uvec );         
         else
         {
            // Set uvec from rvec/fvec
            tmp = mCross( rvec, fvec );
            tmp.normalizeSafe();
            outMat->setColumn( 2, tmp );
         }
      }
      else if ( uvec )
      {
         // Set fvec from uvec/rvec
         tmp = mCross( uvec, rvec );
         tmp.normalizeSafe();
         outMat->setColumn( 1, tmp );
      }
      else
      {
         // Set fvec and uvec from rvec
         Point3F tempFvec = mPerp( rvec );
         Point3F tempUvec = mCross( )

      }
   }
   AssertFatal( rvec->isUnitLength(), "MathUtils::buildMatrix() - Right vector was not normalized!" );
   AssertFatal( fvec->isUnitLength(), "MathUtils::buildMatrix() - Forward vector was not normalized!" );
   AssertFatal( uvec->isUnitLength(), "MathUtils::buildMatrix() - UpVector vector was not normalized!" );
   AssertFatal( outMat, "MathUtils::buildMatrix() - Got null output matrix!" );
   AssertFatal( outMat->isAffine(), "MathUtils::buildMatrix() - Got uninitialized matrix!" );
   */
}

//-----------------------------------------------------------------------------

bool reduceFrustum( const Frustum& frustum, const RectI& viewport, const RectF& area, Frustum& outFrustum )
{
   // Just to be safe, clamp the area to the viewport.

   Point2F clampedMin;
   Point2F clampedMax;

   clampedMin.x = mClampF( area.extent.x, ( F32 ) viewport.point.x, ( F32 ) viewport.point.x + viewport.extent.x );
   clampedMin.y = mClampF( area.extent.y, ( F32 ) viewport.point.y, ( F32 ) viewport.point.y + viewport.extent.y );

   clampedMax.x = mClampF( area.extent.x, ( F32 ) viewport.point.x, ( F32 ) viewport.point.x + viewport.extent.x );
   clampedMax.y = mClampF( area.extent.y, ( F32 ) viewport.point.y, ( F32 ) viewport.point.y + viewport.extent.y );

   // If we have ended up without a visible region on the screen,
   // terminate now.
   
   if(   mFloor( clampedMin.x ) == mFloor( clampedMax.x ) ||
         mFloor( clampedMin.y ) == mFloor( clampedMax.y ) )
      return false;

   // Get the extents of the frustum.

   const F32 frustumXExtent = mFabs( frustum.getNearRight() - frustum.getNearLeft() );
   const F32 frustumYExtent = mFabs( frustum.getNearTop() - frustum.getNearBottom() );

   // Now, normalize the screen-space pixel coordinates to lie within the screen-centered
   // -1 to 1 coordinate space that is used for the frustum planes.

   Point2F normalizedMin;
   Point2F normalizedMax;

   normalizedMin.x = ( ( clampedMin.x / viewport.extent.x ) * frustumXExtent ) - ( frustumXExtent / 2.f );
   normalizedMin.y = ( ( clampedMin.y / viewport.extent.y ) * frustumYExtent ) - ( frustumYExtent / 2.f );
   normalizedMax.x = ( ( clampedMax.x / viewport.extent.x ) * frustumXExtent ) - ( frustumXExtent / 2.f );
   normalizedMax.y = ( ( clampedMax.y / viewport.extent.y ) * frustumYExtent ) - ( frustumYExtent / 2.f );

   // Make sure the generated frustum metrics are somewhat sane.

   if( normalizedMax.x - normalizedMin.x < 0.001f ||
       normalizedMax.y - normalizedMin.y < 0.001f )
      return false;
   
   // Finally, create the new frustum using the original's frustum
   // information except its left/right/top/bottom planes.
   //
   // Note that screen-space coordinates go upside down on Y whereas
   // camera-space frustum coordinates go downside up on Y which is
   // why we are inverting Y here.

   outFrustum.set(
      frustum.isOrtho(),
      normalizedMin.x,
      normalizedMax.x,
      - normalizedMin.y,
      - normalizedMax.y,
      frustum.getNearDist(),
      frustum.getFarDist(),
      frustum.getTransform()
   );

   return true;
}

//-----------------------------------------------------------------------------

void makeFrustum( F32 *outLeft,
                  F32 *outRight,
                  F32 *outTop,
                  F32 *outBottom,
                  F32 fovYInRadians, 
                  F32 aspectRatio, 
                  F32 nearPlane )
{
   F32 top = nearPlane * mTan( fovYInRadians / 2.0 );
   if ( outTop ) *outTop = top;
   if ( outBottom ) *outBottom = -top;

   F32 left = top * aspectRatio;
   if ( outLeft ) *outLeft = -left;
   if ( outRight ) *outRight = left;
}

//-----------------------------------------------------------------------------

void makeProjection( MatrixF *outMatrix, 
                     F32 fovYInRadians, 
                     F32 aspectRatio, 
                     F32 nearPlane, 
                     F32 farPlane,
                     bool gfxRotate )
{
   F32 left, right, top, bottom;
   makeFrustum( &left, &right, &top, &bottom, fovYInRadians, aspectRatio, nearPlane );
   makeProjection( outMatrix, left, right, top, bottom, nearPlane, farPlane, gfxRotate );
}

//-----------------------------------------------------------------------------

void makeFovPortFrustum(
   Frustum *outFrustum,
   bool isOrtho,
   F32 nearDist,
   F32 farDist,
   const FovPort &inPort,
   const MatrixF &transform)
{
   F32 leftSize = nearDist * inPort.leftTan;
   F32 rightSize = nearDist * inPort.rightTan;
   F32 upSize = nearDist * inPort.upTan;
   F32 downSize = nearDist * inPort.downTan;

   F32 left = -leftSize;
   F32 right = rightSize;
   F32 top = upSize;
   F32 bottom = -downSize;

   outFrustum->set(isOrtho, left, right, top, bottom, nearDist, farDist, transform);
}

//-----------------------------------------------------------------------------

/// This is the special rotation matrix applied to
/// projection matricies for GFX.
///
/// It is a wart of the OGL to DX change over.
///
static const MatrixF sGFXProjRotMatrix( EulerF( (M_PI_F / 2.0f), 0.0f, 0.0f ) );

void makeProjection( MatrixF *outMatrix,
                     F32 left,
                     F32 right,
                     F32 top,
                     F32 bottom,
                     F32 nearPlane,
                     F32 farPlane,
                     bool gfxRotate)
{
   const bool isGL = GFX->getAdapterType() == OpenGL;
   Point4F row;
   row.x = 2.0f * nearPlane / (right - left);
   row.y = 0.0f;
   row.z = 0.0f;
   row.w = 0.0f;
   outMatrix->setRow(0, row);

   row.x = 0.0f;
   row.y = 2.0f * nearPlane / (top - bottom);
   row.z = 0.0f;
   row.w = 0.0f;
   outMatrix->setRow(1, row);

   row.x = (left + right) / (right - left);
   row.y = (top + bottom) / (top - bottom);
   row.z = isGL ? (nearPlane + farPlane) / (nearPlane - farPlane) : farPlane / (nearPlane - farPlane);
   row.w = -1.0f;
   outMatrix->setRow(2, row);

   row.x = 0.0f;
   row.y = 0.0f;
   row.z = isGL ? 2.0f * nearPlane * farPlane / (nearPlane - farPlane) : farPlane * nearPlane / (nearPlane - farPlane);
   row.w = 0.0f;
   outMatrix->setRow(3, row);

   outMatrix->transpose();

   if (gfxRotate)
      outMatrix->mul(sGFXProjRotMatrix);
}

//-----------------------------------------------------------------------------

void makeOrthoProjection(  MatrixF *outMatrix, 
                           F32 left, 
                           F32 right, 
                           F32 top, 
                           F32 bottom, 
                           F32 nearPlane, 
                           F32 farPlane,
                           bool gfxRotate )
{
   Point4F row;
   row.x = 2.0f / (right - left);
   row.y = 0.0f;
   row.z = 0.0f;
   row.w = 0.0f;
   outMatrix->setRow( 0, row );

   row.x = 0.0f;
   row.y = 2.0f / (top - bottom);
   row.z = 0.0f;
   row.w = 0.0f;
   outMatrix->setRow( 1, row );

   row.x = 0.0f;
   row.y = 0.0f;
   row.z = 1.0f / (nearPlane - farPlane);
   row.w = 0.0f;

   outMatrix->setRow( 2, row );

   row.x = (left + right) / (left - right);
   row.y = (top + bottom) / (bottom - top);
   row.z = nearPlane / (nearPlane - farPlane);
   row.w = 1.0f;
   outMatrix->setRow( 3, row );

   outMatrix->transpose();

   if ( gfxRotate )
      outMatrix->mul( sGFXProjRotMatrix );
}

//-----------------------------------------------------------------------------

bool edgeFaceIntersect( const Point3F &edgeA, const Point3F &edgeB, 
                        const Point3F &faceA, const Point3F &faceB, const Point3F &faceC, const Point3F &faceD, Point3F *intersection )
{
   VectorF edgeAB = edgeB - edgeA;
   VectorF edgeAFaceA = faceA - edgeA;
   VectorF edgeAFaceB = faceB - edgeA;
   VectorF edgeAFaceC = faceC - edgeA;

   VectorF m = mCross( edgeAFaceC, edgeAB );
   F32 v = mDot( edgeAFaceA, m );
   if ( v >= 0.0f )
   {
      F32 u = -mDot( edgeAFaceB, m );
      if ( u < 0.0f )
         return false;

      VectorF tmp = mCross( edgeAFaceB, edgeAB );
      F32 w = mDot( edgeAFaceA, tmp );
      if ( w < 0.0f )
         return false;

      F32 denom = 1.0f / (u + v + w );
      u *= denom;
      v *= denom;
      w *= denom;

      (*intersection) = u * faceA + v * faceB + w * faceC;
   }
   else
   {
      VectorF edgeAFaceD = faceD - edgeA;
      F32 u = mDot( edgeAFaceD, m );
      if ( u < 0.0f )
         return false;

      VectorF tmp = mCross( edgeAFaceA, edgeAB );
      F32 w = mDot( edgeAFaceD, tmp );
      if ( w < 0.0f )
         return false;

      v = -v;

      F32 denom = 1.0f / ( u + v + w );
      u *= denom;
      v *= denom;
      w *= denom;

      (*intersection) = u * faceA + v * faceD + w * faceC;
   }

   return true;
}

//-----------------------------------------------------------------------------

bool isPlanarPolygon( const Point3F* vertices, U32 numVertices )
{
   AssertFatal( vertices != NULL, "MathUtils::isPlanarPolygon - Received NULL pointer" );
   AssertFatal( numVertices >= 3, "MathUtils::isPlanarPolygon - Must have at least three vertices" );

   // Triangles are always planar.  Letting smaller numVertices
   // slip through provides robustness for errors in release builds.
   
   if( numVertices <= 3 )
      return true;

   // Compute the normal of the first triangle in the polygon.
   
   Point3F triangle1Normal = mTriangleNormal( vertices[ 0 ], vertices[ 1 ], vertices[ 2 ] );

   // Now go through all the remaining vertices and build triangles
   // with the first two vertices.  Then the normals of all these triangles
   // must be the same (minus some variance due to floating-point inaccuracies)
   // as the normal of the first triangle.

   for( U32 i = 3; i < numVertices; ++ i )
   {
      Point3F triangle2Normal = mTriangleNormal( vertices[ 0 ], vertices[ 1 ], vertices[ i ] );
      if( !triangle1Normal.equal( triangle2Normal ) )
         return false;
   }

   return true;
}

//-----------------------------------------------------------------------------

bool isConvexPolygon( const Point3F* vertices, U32 numVertices )
{
   AssertFatal( vertices != NULL, "MathUtils::isConvexPolygon - Received NULL pointer" );
   AssertFatal( numVertices >= 3, "MathUtils::isConvexPolygon - Must have at least three vertices" );

   // Triangles are always convex.  Letting smaller numVertices
   // slip through provides robustness for errors in release builds.

   if( numVertices <= 3 )
      return true;

   U32 numPositive = 0;
   U32 numNegative = 0;

   for( U32 i = 0; i < numVertices; ++ i )
   {
      const Point3F& a = vertices[ i ];
      const Point3F& b = vertices[ ( i + 1 ) % numVertices ];
      const Point3F& c = vertices[ ( i + 2 ) % numVertices ];

      const F32 crossProductLength = mCross( b - a, c - b ).len();
      
      if( crossProductLength < 0.f )
         numNegative ++;
      else if( crossProductLength > 0.f )
         numPositive ++;

      if( numNegative && numPositive )
         return false;
   }

   return true;
}

//-----------------------------------------------------------------------------

bool clipFrustumByPolygon( const Point3F* points, U32 numPoints, const RectI& viewport, const MatrixF& world,
                           const MatrixF& projection, const Frustum& inFrustum, const Frustum& rootFrustum, Frustum& outFrustum )
{
   enum
   {
      MAX_RESULT_VERTICES = 64,
      MAX_INPUT_VERTICES = MAX_RESULT_VERTICES - Frustum::PlaneCount // Clipping against each plane may add a vertex.
   };

   AssertFatal( numPoints <= MAX_INPUT_VERTICES, "MathUtils::clipFrustumByPolygon - Too many vertices!" );
   if( numPoints > MAX_INPUT_VERTICES )
      return false;

   // First, we need to clip the polygon against inFrustum.
   //
   // Use two buffers here in interchanging roles as sources and targets
   // in clipping against the frustum planes.

   Point3F polygonBuffer1[ MAX_RESULT_VERTICES ];
   Point3F polygonBuffer2[ MAX_RESULT_VERTICES ];

   Point3F* tempPolygon = polygonBuffer1;
   Point3F* clippedPolygon = polygonBuffer2;

   dMemcpy( clippedPolygon, points, numPoints * sizeof( points[ 0 ] ) );

   U32 numClippedPolygonVertices = numPoints;
   U32 numTempPolygonVertices = 0;

   for( U32 nplane = 0; nplane < Frustum::PlaneCount; ++ nplane )
   {
      // Make the output of the last iteration the
      // input of this iteration.

	  T3D::swap( tempPolygon, clippedPolygon );
      numTempPolygonVertices = numClippedPolygonVertices;

      // Clip our current remainder of the original polygon
      // against the current plane.

      const PlaneF& plane = inFrustum.getPlanes()[ nplane ];
      numClippedPolygonVertices = plane.clipPolygon( tempPolygon, numTempPolygonVertices, clippedPolygon );

      // If the polygon was completely on the backside of the plane,
      // then polygon is outside the frustum.  In this case, return false
      // to indicate we haven't clipped anything.

      if( !numClippedPolygonVertices )
         return false;
   }

   // Project the clipped polygon into screen space.

   MatrixF worldProjection = projection;
   worldProjection.mul( world ); // Premultiply world*projection so we don't have to do this over and over for each point.

   Point3F projectedPolygon[ 10 ];
   for( U32 i = 0; i < numClippedPolygonVertices; ++ i )
      mProjectWorldToScreen(
         clippedPolygon[ i ],
         &projectedPolygon[ i ],
         viewport,
         worldProjection
      );

   // Put an axis-aligned rectangle around our polygon.

   Point2F minPoint( projectedPolygon[ 0 ].x, projectedPolygon[ 0 ].y );
   Point2F maxPoint( projectedPolygon[ 0 ].x, projectedPolygon[ 0 ].y );

   for( U32 i = 1; i < numClippedPolygonVertices; ++ i )
   {
      minPoint.setMin( Point2F( projectedPolygon[ i ].x, projectedPolygon[ i ].y ) );
      maxPoint.setMax( Point2F( projectedPolygon[ i ].x, projectedPolygon[ i ].y ) );
   }

   RectF area( minPoint, maxPoint - minPoint );

   // Finally, reduce the input frustum to the given area.  Note that we
   // use rootFrustum here instead of inFrustum as the latter does not necessarily
   // represent the full viewport we are using here which thus would skew the mapping.

   return reduceFrustum( rootFrustum, viewport, area, outFrustum );
}

//-----------------------------------------------------------------------------

U32 extrudePolygonEdges( const Point3F* vertices, U32 numVertices, const Point3F& direction, PlaneF* outPlanes )
{
   U32 numPlanes = 0;
   U32 lastVertex = numVertices - 1;
   bool invert = false;

   for( U32 i = 0; i < numVertices; lastVertex = i, ++ i )
   {
      const Point3F& v1 = vertices[ i ];
      const Point3F& v2 = vertices[ lastVertex ];

      // Skip the edge if it's length is really short.

      const Point3F edgeVector = v2 - v1;
      if( edgeVector.len() < 0.05 )
         continue;

      // Compute the plane normal.  The direction and the edge vector
      // basically define the orientation of the plane so their cross
      // product is the plane normal.

      Point3F normal;
      if( !invert )
         normal = mCross( edgeVector, direction );
      else
         normal = mCross( direction, edgeVector );

      // Create a plane for the edge.

      outPlanes[ numPlanes ] = PlaneF( v1, normal );
      numPlanes ++;

      // If this is the first plane that we have created, find out whether
      // the vertex ordering is giving us the plane orientations that we want
      // (facing inside).  If not, invert vertex order from now on.

      if( i == 0 )
      {
         const PlaneF& plane = outPlanes[ numPlanes - 1 ];
         for( U32 n = i + 1; n < numVertices; ++ n )
         {
            const PlaneF::Side side = plane.whichSide( vertices[ n ] );
            if( side == PlaneF::On )
               continue;

            if( side != PlaneF::Front )
               invert = true;
            break;
         }
      }
   }

   return numPlanes;
}

//-----------------------------------------------------------------------------

U32 extrudePolygonEdgesFromPoint( const Point3F* vertices, U32 numVertices, const Point3F& fromPoint, PlaneF* outPlanes )
{
   U32 numPlanes = 0;
   U32 lastVertex = numVertices - 1;
   bool invert = false;

   for( U32 i = 0; i < numVertices; lastVertex = i, ++ i )
   {
      const Point3F& v1 = vertices[ i ];
      const Point3F& v2 = vertices[ lastVertex ];

      // Skip the edge if it's length is really short.

      const Point3F edgeVector = v2 - v1;
      if( edgeVector.len() < 0.05 )
         continue;

      // Create a plane for the edge.

      if( !invert )
         outPlanes[ numPlanes ] = PlaneF( v1, fromPoint, v2 );
      else
         outPlanes[ numPlanes ] = PlaneF( v2, fromPoint, v1 );
      
      numPlanes ++;

      // If this is the first plane that we have created, find out whether
      // the vertex ordering is giving us the plane orientations that we want
      // (facing inside).  If not, invert vertex order from now on.

      if( i == 0 )
      {
         const PlaneF& plane = outPlanes[ numPlanes - 1 ];
         for( U32 n = i + 1; n < numVertices; ++ n )
         {
            const PlaneF::Side side = plane.whichSide( vertices[ n ] );
            if( side == PlaneF::On )
               continue;

            if( side != PlaneF::Front )
               invert = true;
            break;
         }
      }
   }

   return numPlanes;
}

//-----------------------------------------------------------------------------

void mBuildHull2D(const Vector<Point2F> _inPoints, Vector<Point2F> &hullPoints)
{
   /// Andrew's monotone chain convex hull algorithm implementation

   struct Util
   {
      //compare by x and then by y   
      static int CompareLexicographic( const Point2F *a, const Point2F *b)
      {
         return a->x < b->x || (a->x == b->x && a->y < b->y);
      }
   };

   hullPoints.clear();
   hullPoints.setSize( _inPoints.size()*2 );

   // sort in points by x and then by y
   Vector<Point2F> inSortedPoints = _inPoints;
   inSortedPoints.sort( &Util::CompareLexicographic );

   Point2F* lowerHullPtr = hullPoints.address();
   U32 lowerHullIdx = 0;

   //lower part of hull
   for( int i = 0; i < inSortedPoints.size(); ++i )
   {      
      while( lowerHullIdx >= 2 && mCross( lowerHullPtr[ lowerHullIdx - 2], lowerHullPtr[lowerHullIdx - 1], inSortedPoints[i] ) <= 0 )
         --lowerHullIdx;

      lowerHullPtr[lowerHullIdx++] = inSortedPoints[i];
   }

   --lowerHullIdx; // last point are the same as first in upperHullPtr

   Point2F* upperHullPtr = hullPoints.address() + lowerHullIdx;
   U32 upperHullIdx = 0;

   //upper part of hull
   for( int i = inSortedPoints.size()-1; i >= 0; --i )
   {
      while( upperHullIdx >= 2 && mCross( upperHullPtr[ upperHullIdx - 2], upperHullPtr[upperHullIdx - 1], inSortedPoints[i] ) <= 0 )
         --upperHullIdx;

      upperHullPtr[upperHullIdx++] = inSortedPoints[i];
   }

   hullPoints.setSize( lowerHullIdx + upperHullIdx );
}

} // namespace MathUtils