Initial commit

engine/math/mBox.cc

@@ -0,0 +1,161 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#include "math/mBox.h"
+#include "math/mMatrix.h"
+
+bool Box3F::collideLine(const Point3F& start, const Point3F& end, F32* t, Point3F* n) const
+{
+   // Collide against bounding box. Need at least this for the editor.
+   F32 st,et;
+   F32 fst = 0;
+   F32 fet = 1;
+   const F32* bmin = &min.x;
+   const F32* bmax = &max.x;
+   const F32* si   = &start.x;
+   const F32* ei   = &end.x;
+
+   Point3F na[3] = { Point3F(1, 0, 0), Point3F(0, 1, 0), Point3F(0, 0, 1) };
+   Point3F finalNormal;
+
+   for (int i = 0; i < 3; i++) {
+      Point3F normal = na[i];
+      if (si[i] < ei[i]) {
+         if (si[i] > bmax[i] || ei[i] < bmin[i])
+            return false;
+         F32 di = ei[i] - si[i];
+         st = (si[i] < bmin[i]) ? (bmin[i] - si[i]) / di : 0;
+         et = (ei[i] > bmax[i]) ? (bmax[i] - si[i]) / di : 1;
+         normal.neg();
+      }
+      else {
+         if (ei[i] > bmax[i] || si[i] < bmin[i])
+            return false;
+         F32 di = ei[i] - si[i];
+         st = (si[i] > bmax[i]) ? (bmax[i] - si[i]) / di : 0;
+         et = (ei[i] < bmin[i]) ? (bmin[i] - si[i]) / di : 1;
+      }
+      if (st > fst) {
+         fst = st;
+         finalNormal = normal;
+      }
+      if (et < fet)
+         fet = et;
+
+      if (fet < fst)
+         return false;
+   }
+
+   *t = fst;
+   *n = finalNormal;
+   return true;
+}
+
+
+bool Box3F::collideLine(const Point3F& start, const Point3F& end) const
+{
+   F32 t;
+   Point3F normal;
+   return collideLine(start, end, &t, &normal);
+}
+
+// returns true if "oriented" box collides with us
+// radiiB is dimension of incoming box (half x,y,z length
+// toA is transform that takes incoming box into our space
+// assumes incoming box is centered at origin of source space
+bool Box3F::collideOrientedBox(const Point3F & bRadii, const MatrixF & toA) const
+{
+   Point3F p;
+   toA.getColumn(3,&p);
+   Point3F aCenter = min + max;
+   aCenter *= 0.5f;
+   p -= aCenter; // this essentially puts origin of toA target space on the center of the current box
+   Point3F aRadii = max - min;
+   aRadii *= 0.5f;
+
+	F32 absXX,absXY,absXZ;
+	F32 absYX,absYY,absYZ;
+	F32 absZX,absZY,absZZ;
+
+   const F32 * f = toA;
+
+   absXX = mFabs(f[0]);
+   absYX = mFabs(f[1]);
+   absZX = mFabs(f[2]);
+
+	if (aRadii.x + bRadii.x * absXX + bRadii.y * absYX + bRadii.z * absZX - mFabs(p.x)<0.0f)
+		return false;
+
+   absXY = mFabs(f[4]);
+   absYY = mFabs(f[5]);
+   absZY = mFabs(f[6]);
+	if (aRadii.y + bRadii.x * absXY +	bRadii.y * absYY +	bRadii.z * absZY - mFabs(p.y)<0.0f)
+		return false;
+	
+   absXZ = mFabs(f[8]);
+   absYZ = mFabs(f[9]);
+   absZZ = mFabs(f[10]);
+	if (aRadii.z + bRadii.x * absXZ + bRadii.y * absYZ +	bRadii.z * absZZ - mFabs(p.z)<0.0f)
+		return false;
+
+	if (aRadii.x*absXX + aRadii.y*absXY + aRadii.z*absXZ + bRadii.x - mFabs(p.x*f[0] + p.y*f[4] + p.z*f[8])<0.0f)
+		return false;
+
+	if (aRadii.x*absYX + aRadii.y*absYY + aRadii.z*absYZ + bRadii.y - mFabs(p.x*f[1] + p.y*f[5] + p.z*f[9])<0.0f)
+		return false;		
+	
+	if (aRadii.x*absZX + aRadii.y*absZY + aRadii.z*absZZ + bRadii.z - mFabs(p.x*f[2] + p.y*f[6] + p.z*f[10])<0.0f)
+		return false;		
+	
+	if (mFabs(p.z*f[4] - p.y*f[8]) >
+				aRadii.y * absXZ + aRadii.z * absXY +
+				bRadii.y * absZX + bRadii.z * absYX)
+		return false;
+	
+	if (mFabs(p.z*f[5] - p.y*f[9]) >
+				aRadii.y * absYZ + aRadii.z * absYY +
+				bRadii.x * absZX + bRadii.z * absXX)
+		return false;
+	
+	if (mFabs(p.z*f[6] - p.y*f[10]) >
+				aRadii.y * absZZ + aRadii.z * absZY +
+				bRadii.x * absYX + bRadii.y * absXX)
+		return false;
+	
+	if (mFabs(p.x*f[8] - p.z*f[0]) >
+				aRadii.x * absXZ + aRadii.z * absXX +
+				bRadii.y * absZY + bRadii.z * absYY)
+		return false;
+	
+	if (mFabs(p.x*f[9] - p.z*f[1]) >
+				aRadii.x * absYZ + aRadii.z * absYX +
+				bRadii.x * absZY + bRadii.z * absXY)
+		return false;
+	
+	if (mFabs(p.x*f[10] - p.z*f[2]) >
+				aRadii.x * absZZ + aRadii.z * absZX +
+				bRadii.x * absYY + bRadii.y * absXY)
+		return false;
+	
+	if (mFabs(p.y*f[0] - p.x*f[4]) >
+				aRadii.x * absXY + aRadii.y * absXX +
+				bRadii.y * absZZ + bRadii.z * absYZ)
+		return false;
+	
+	if (mFabs(p.y*f[1] - p.x*f[5]) >
+				aRadii.x * absYY + aRadii.y * absYX +
+				bRadii.x * absZZ + bRadii.z * absXZ)
+		return false;
+	
+	if (mFabs(p.y*f[2] - p.x*f[6]) >
+				aRadii.x * absZY + aRadii.y * absZX +
+				bRadii.x * absYZ + bRadii.y * absXZ)
+		return false;
+	
+	return true;
+}
+
+
+

engine/math/mBox.h

@@ -0,0 +1,287 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#ifndef _MBOX_H_
+#define _MBOX_H_
+
+#ifndef _MPOINT_H_
+#include "math/mPoint.h"
+#endif
+
+//------------------------------------------------------------------------------
+/// Bounding Box
+///
+/// A helper class for working with boxes. It runs at F32 precision.
+///
+/// @see Box3D
+class Box3F
+{
+  public:
+   Point3F min; ///< Minimum extents of box
+   Point3F max; ///< Maximum extents of box
+
+  public:
+   Box3F() { }
+
+   /// Create a box from two points.
+   ///
+   /// Normally, this function will compensate for mismatched
+   /// min/max values. If you know your values are valid, you
+   /// can set in_overrideCheck to true and skip this.
+   ///
+   /// @param   in_rMin          Minimum extents of box.
+   /// @param   in_rMax          Maximum extents of box.
+   /// @param   in_overrideCheck  Pass true to skip check of extents.
+   Box3F(const Point3F& in_rMin, const Point3F& in_rMax, const bool in_overrideCheck = false);
+
+   /// Create a box from six extent values.
+   ///
+   /// No checking is performed as to the validity of these
+   /// extents, unlike the other constructor.
+   Box3F(F32 xmin, F32 ymin, F32 zmin, F32 max, F32 ymax, F32 zmax);
+
+   /// Check to see if a point is contained in this box.
+   bool isContained(const Point3F& in_rContained) const;
+
+   /// Check to see if another box overlaps this box.
+   bool isOverlapped(const Box3F&  in_rOverlap) const;
+
+   /// Check to see if another box is contained in this box.
+   bool isContained(const Box3F& in_rContained) const;
+
+   F32 len_x() const;
+   F32 len_y() const;
+   F32 len_z() const;
+
+   /// Perform an intersection operation with another box
+   /// and store the results in this box.
+   void intersect(const Box3F& in_rIntersect);
+   void intersect(const Point3F& in_rIntersect);
+
+   /// Get the center of this box.
+   ///
+   /// This is the average of min and max.
+   void getCenter(Point3F* center) const;
+
+   /// Collide a line against the box.
+   ///
+   /// @param   start   Start of line.
+   /// @param   end     End of line.
+   /// @param   t       Value from 0.0-1.0, indicating position
+   ///                  along line of collision.
+   /// @param   n       Normal of collision.
+   bool collideLine(const Point3F& start, const Point3F& end, F32*t, Point3F*n) const;
+
+   /// Collide a line against the box.
+   ///
+   /// Returns true on collision.
+   bool collideLine(const Point3F& start, const Point3F& end) const;
+
+   /// Collide an oriented box against the box.
+   ///
+   /// Returns true if "oriented" box collides with us.
+   /// Assumes incoming box is centered at origin of source space.
+   ///
+   /// @param   radii   The dimension of incoming box (half x,y,z length).
+   /// @param   toUs    A transform that takes incoming box into our space.
+   bool collideOrientedBox(const Point3F & radii, const MatrixF & toUs) const;
+
+   /// Check that the box is valid.
+   ///
+   /// Currently, this just means that min < max.
+   bool isValidBox() const { return (min.x <= max.x) &&
+                                    (min.y <= max.y) &&
+                                    (min.z <= max.z); }
+
+   /// Return the closest point of the box, relative to the passed point.
+   Point3F getClosestPoint(const Point3F& refPt) const;
+};
+
+inline Box3F::Box3F(const Point3F& in_rMin, const Point3F& in_rMax, const bool in_overrideCheck)
+ : min(in_rMin),
+   max(in_rMax)
+{
+   if (in_overrideCheck == false) {
+      min.setMin(in_rMax);
+      max.setMax(in_rMin);
+   }
+}
+
+inline Box3F::Box3F(F32 xMin, F32 yMin, F32 zMin, F32 xMax, F32 yMax, F32 zMax)
+   : min(xMin,yMin,zMin),
+     max(xMax,yMax,zMax)
+{
+}
+
+inline bool Box3F::isContained(const Point3F& in_rContained) const
+{
+   return (in_rContained.x >= min.x && in_rContained.x < max.x) &&
+          (in_rContained.y >= min.y && in_rContained.y < max.y) &&
+          (in_rContained.z >= min.z && in_rContained.z < max.z);
+}
+
+inline bool Box3F::isOverlapped(const Box3F& in_rOverlap) const
+{
+   if (in_rOverlap.min.x > max.x ||
+       in_rOverlap.min.y > max.y ||
+       in_rOverlap.min.z > max.z)
+      return false;
+   if (in_rOverlap.max.x < min.x ||
+       in_rOverlap.max.y < min.y ||
+       in_rOverlap.max.z < min.z)
+      return false;
+   return true;
+}
+
+inline bool Box3F::isContained(const Box3F& in_rContained) const
+{
+   return (min.x <= in_rContained.min.x) &&
+          (min.y <= in_rContained.min.y) &&
+          (min.z <= in_rContained.min.z) &&
+          (max.x >= in_rContained.max.x) &&
+          (max.y >= in_rContained.max.y) &&
+          (max.z >= in_rContained.max.z);
+}
+
+inline F32 Box3F::len_x() const
+{
+   return max.x - min.x;
+}
+
+inline F32 Box3F::len_y() const
+{
+   return max.y - min.y;
+}
+
+inline F32 Box3F::len_z() const
+{
+   return max.z - min.z;
+}
+
+inline void Box3F::intersect(const Box3F& in_rIntersect)
+{
+   min.setMin(in_rIntersect.min);
+   max.setMax(in_rIntersect.max);
+}
+
+inline void Box3F::intersect(const Point3F& in_rIntersect)
+{
+   min.setMin(in_rIntersect);
+   max.setMax(in_rIntersect);
+}
+
+inline void Box3F::getCenter(Point3F* center) const
+{
+   center->x = F32((min.x + max.x) * 0.5);
+   center->y = F32((min.y + max.y) * 0.5);
+   center->z = F32((min.z + max.z) * 0.5);
+}
+
+inline Point3F Box3F::getClosestPoint(const Point3F& refPt) const
+{
+   Point3F closest;
+   if      (refPt.x <= min.x) closest.x = min.x;
+   else if (refPt.x >  max.x) closest.x = max.x;
+   else                       closest.x = refPt.x;
+
+   if      (refPt.y <= min.y) closest.y = min.y;
+   else if (refPt.y >  max.y) closest.y = max.y;
+   else                       closest.y = refPt.y;
+
+   if      (refPt.z <= min.z) closest.z = min.z;
+   else if (refPt.z >  max.z) closest.z = max.z;
+   else                       closest.z = refPt.z;
+
+   return closest;
+}
+
+
+//------------------------------------------------------------------------------
+/// Clone of Box3F, using 3D types.
+///
+/// 3D types use F64.
+///
+/// @see Box3F
+class Box3D
+{
+  public:
+   Point3D min;
+   Point3D max;
+
+  public:
+   Box3D() { }
+   Box3D(const Point3D& in_rMin, const Point3D& in_rMax, const bool in_overrideCheck = false);
+
+   bool isContained(const Point3D& in_rContained) const;
+   bool isOverlapped(const Box3D&  in_rOverlap) const;
+
+   F64 len_x() const;
+   F64 len_y() const;
+   F64 len_z() const;
+
+   void intersect(const Box3D& in_rIntersect);
+   void getCenter(Point3D* center) const;
+};
+
+inline Box3D::Box3D(const Point3D& in_rMin, const Point3D& in_rMax, const bool in_overrideCheck)
+ : min(in_rMin),
+   max(in_rMax)
+{
+   if (in_overrideCheck == false) {
+      min.setMin(in_rMax);
+      max.setMax(in_rMin);
+   }
+}
+
+inline bool Box3D::isContained(const Point3D& in_rContained) const
+{
+   return (in_rContained.x >= min.x && in_rContained.x < max.x) &&
+          (in_rContained.y >= min.y && in_rContained.y < max.y) &&
+          (in_rContained.z >= min.z && in_rContained.z < max.z);
+}
+
+inline bool Box3D::isOverlapped(const Box3D& in_rOverlap) const
+{
+   if (in_rOverlap.min.x > max.x ||
+       in_rOverlap.min.y > max.y ||
+       in_rOverlap.min.z > max.z)
+      return false;
+   if (in_rOverlap.max.x < min.x ||
+       in_rOverlap.max.y < min.y ||
+       in_rOverlap.max.z < min.z)
+      return false;
+   return true;
+}
+
+inline F64 Box3D::len_x() const
+{
+   return max.x - min.x;
+}
+
+inline F64 Box3D::len_y() const
+{
+   return max.y - min.y;
+}
+
+inline F64 Box3D::len_z() const
+{
+   return max.z - min.z;
+}
+
+inline void Box3D::intersect(const Box3D& in_rIntersect)
+{
+   min.setMin(in_rIntersect.min);
+   max.setMax(in_rIntersect.max);
+}
+
+inline void Box3D::getCenter(Point3D* center) const
+{
+   center->x = (min.x + max.x) * 0.5;
+   center->y = (min.y + max.y) * 0.5;
+   center->z = (min.z + max.z) * 0.5;
+}
+
+
+#endif // _DBOX_H_

engine/math/mConsoleFunctions.cc

@@ -0,0 +1,137 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#include "platform/platform.h"
+#include "console/console.h"
+#include "math/mMathFn.h"
+#include "math/mRandom.h"
+
+ConsoleFunctionGroupBegin( GeneralMath, "General math functions. Use these whenever possible, as they'll run much faster than script equivalents.");
+
+ConsoleFunction( mSolveQuadratic, const char *, 4, 4, "(float a, float b, float c)"
+              "Solve a quadratic equation of form a*x^2 + b*x + c = 0.\n\n"
+              "@returns A triple, contanining: sol x0 x1. sol is the number of"
+              " solutions (being 0, 1, or 2), and x0 and x1 are the solutions, if any."
+              " Unused x's are undefined.")
+{
+   char * retBuffer = Con::getReturnBuffer(256);
+   F32 x[2];
+   U32 sol = mSolveQuadratic(dAtof(argv[1]), dAtof(argv[2]), dAtof(argv[3]), x);
+   dSprintf(retBuffer, 256, "%d %g %g", sol, x[0], x[1]);
+   return retBuffer;
+}
+
+ConsoleFunction( mSolveCubic, const char *, 5, 5, "(float a, float b, float c, float d)"
+              "Solve a cubic equation of form a*x^3 + b*x^2 + c*x + d = 0.\n\n"
+              "@returns A 4-tuple, contanining: sol x0 x1 x2. sol is the number of"
+              " solutions (being 0, 1, 2, or 3), and x0, x1, x2 are the solutions, if any."
+              " Unused x's are undefined.")
+{
+   char * retBuffer = Con::getReturnBuffer(256);
+   F32 x[3];
+   U32 sol = mSolveCubic(dAtof(argv[1]), dAtof(argv[2]), dAtof(argv[3]), dAtof(argv[4]), x);
+   dSprintf(retBuffer, 256, "%d %g %g %g", sol, x[0], x[1], x[2]);
+   return retBuffer;
+}
+
+ConsoleFunction( mSolveQuartic, const char *, 6, 6, "(float a, float b, float c, float d, float e)"
+              "Solve a quartic equation of form a*x^4 + b*x^3 + c*x^2 + d*x + e = 0.\n\n"
+              "@returns A 5-tuple, contanining: sol x0 x1 x2 x3. sol is the number of"
+              " solutions (ranging from 0-4), and x0, x1, x2 and x3 are the solutions, if any."
+              " Unused x's are undefined.")
+{
+   char * retBuffer = Con::getReturnBuffer(256);
+   F32 x[4];
+   U32 sol = mSolveQuartic(dAtof(argv[1]), dAtof(argv[2]), dAtof(argv[3]), dAtof(argv[4]), dAtof(argv[5]), x);
+   dSprintf(retBuffer, 256, "%d %g %g %g %g", sol, x[0], x[1], x[2], x[3]);
+   return retBuffer;
+}
+
+ConsoleFunction( mFloor, S32, 2, 2, "(float v) Round v down to the nearest whole number.")
+{
+   return (S32)mFloor(dAtof(argv[1]));
+}
+
+ConsoleFunction( mCeil, S32, 2, 2, "(float v) Round v up to the nearest whole number.")
+{
+   return (S32)mCeil(dAtof(argv[1]));
+}
+
+ConsoleFunction( mFloatLength, const char *, 3, 3, "(float v, int numDecimals)"
+              "Return a string containing v formatted with the specified number of decimal places.")
+{
+   char * outBuffer = Con::getReturnBuffer(256);
+   char fmtString[8] = "%.0f";
+   U32 precision = dAtoi(argv[2]);
+   if (precision > 9)
+      precision = 9;
+   fmtString[2] = '0' + precision;
+
+   dSprintf(outBuffer, 255, fmtString, dAtof(argv[1]));
+   return outBuffer;
+}
+
+//------------------------------------------------------------------------------
+ConsoleFunction( mAbs, F32, 2, 2, "(float v) Returns the absolute value of the argument.")
+{
+   return(mFabs(dAtof(argv[1])));
+}
+
+ConsoleFunction( mSqrt, F32, 2, 2, "(float v) Returns the square root of the argument.")
+{
+   return(mSqrt(dAtof(argv[1])));
+}
+
+ConsoleFunction( mPow, F32, 3, 3, "(float b, float p) Returns the b raised to the pth power.")
+{
+   return(mPow(dAtof(argv[1]), dAtof(argv[2])));
+}
+
+ConsoleFunction( mLog, F32, 2, 2, "(float v) Returns the natural logarithm of the argument.")
+{
+   return(mLog(dAtof(argv[1])));
+}
+
+ConsoleFunction( mSin, F32, 2, 2, "(float th) Returns the sine of th, which is in radians.")
+{
+   return(mSin(dAtof(argv[1])));
+}
+
+ConsoleFunction( mCos, F32, 2, 2, "(float th) Returns the cosine of th, which is in radians.")
+{
+   return(mCos(dAtof(argv[1])));
+}
+
+ConsoleFunction( mTan, F32, 2, 2, "(float th) Returns the tangent of th, which is in radians.")
+{
+   return(mTan(dAtof(argv[1])));
+}
+
+ConsoleFunction( mAsin, F32, 2, 2, "(float th) Returns the arc-sine of th, which is in radians.")
+{
+   return(mAsin(dAtof(argv[1])));
+}
+
+ConsoleFunction( mAcos, F32, 2, 2, "(float th) Returns the arc-cosine of th, which is in radians.")
+{
+   return(mAcos(dAtof(argv[1])));
+}
+
+ConsoleFunction( mAtan, F32, 3, 3, "(float rise, float run) Returns the slope in radians (the arc-tangent) of a line with the given rise and run.")
+{
+   return(mAtan(dAtof(argv[1]), dAtof(argv[2])));
+}
+
+ConsoleFunction( mRadToDeg, F32, 2, 2, "(float radians) Converts a measure in radians to degrees.")
+{
+   return(mRadToDeg(dAtof(argv[1])));
+}
+
+ConsoleFunction( mDegToRad, F32, 2, 2, "(float degrees) Convert a measure in degrees to radians.")
+{
+   return(mDegToRad(dAtof(argv[1])));
+}
+
+ConsoleFunctionGroupEnd( GeneralMath );

engine/math/mConstants.h

@@ -0,0 +1,22 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#ifndef _MCONSTANTS_H_
+#define _MCONSTANTS_H_
+
+#define M_PI         3.14159265358979323846
+#define M_SQRT2      1.41421356237309504880
+
+#define M_2PI        (3.1415926535897932384626433 * 2.0)
+#define M_SQRTHALF   0.7071067811865475244008443
+
+#define M_PI_F         3.14159265358979323846f
+#define M_SQRT2_F      1.41421356237309504880f
+
+#define M_2PI_F        (3.1415926535897932384626433f * 2.0f)
+#define M_SQRTHALF_F   0.7071067811865475244008443f
+
+
+#endif

engine/math/mMath.h

@@ -0,0 +1,42 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#ifndef _MMATH_H_
+#define _MMATH_H_
+
+
+#ifndef _MPOINT_H_
+#include "math/mPoint.h"
+#endif
+#ifndef _MRECT_H_
+#include "math/mRect.h"
+#endif
+#ifndef _MBOX_H_
+#include "math/mBox.h"
+#endif
+#ifndef _MPLANE_H_
+#include "math/mPlane.h"
+#endif
+#ifndef _MMATRIX_H_
+#include "math/mMatrix.h"
+#endif
+#ifndef _MQUAT_H_
+#include "math/mQuat.h"
+#endif
+#ifndef _MSPHERE_H_
+#include "math/mSphere.h"
+#endif
+#ifndef _MCONSTANTS_H_
+#include "math/mConstants.h"
+#endif
+#ifndef _MRANDOM_H_
+#include "math/mRandom.h"
+#endif
+#ifndef _MATHTYPES_H_
+#include "math/mathTypes.h"
+#endif
+
+
+#endif //_MMATH_H_

engine/math/mMathAMD.cc

@@ -0,0 +1,242 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#include "math/mMathFn.h"
+#include "math/mPlane.h"
+#include "math/mMatrix.h"
+
+
+// extern void (*m_matF_x_point3F)(const F32 *m, const F32 *p, F32 *presult);
+// extern void (*m_matF_x_vectorF)(const F32 *m, const F32 *v, F32 *vresult);
+
+/* not currently implemented.
+void Athlon_MatrixF_x_Point3F(const F32 *m, const F32 *p, F32 *presult)
+{
+   m;
+   p;
+   presult;
+}
+*/
+
+//============================================================
+//  Here's the C code for MatF_x_MatF:
+//  note that the code below does it in a different order (optimal asm, after all!)
+//
+// r[0] = a[0]*b[0] + a[1]*b[4] + a[2]*b[8]  + a[3]*b[12];
+// r[1] = a[0]*b[1] + a[1]*b[5] + a[2]*b[9]  + a[3]*b[13];
+// r[2] = a[0]*b[2] + a[1]*b[6] + a[2]*b[10] + a[3]*b[14];
+// r[3] = a[0]*b[3] + a[1]*b[7] + a[2]*b[11] + a[3]*b[15];
+//
+// r[4] = a[4]*b[0] + a[5]*b[4] + a[6]*b[8]  + a[7]*b[12];
+// r[5] = a[4]*b[1] + a[5]*b[5] + a[6]*b[9]  + a[7]*b[13];
+// r[6] = a[4]*b[2] + a[5]*b[6] + a[6]*b[10] + a[7]*b[14];
+// r[7] = a[4]*b[3] + a[5]*b[7] + a[6]*b[11] + a[7]*b[15];
+//
+// r[8] = a[8]*b[0] + a[9]*b[4] + a[10]*b[8] + a[11]*b[12];
+// r[9] = a[8]*b[1] + a[9]*b[5] + a[10]*b[9] + a[11]*b[13];
+// r[10]= a[8]*b[2] + a[9]*b[6] + a[10]*b[10]+ a[11]*b[14];
+// r[11]= a[8]*b[3] + a[9]*b[7] + a[10]*b[11]+ a[11]*b[15];
+//
+// r[12]= a[12]*b[0]+ a[13]*b[4]+ a[14]*b[8] + a[15]*b[12];
+// r[13]= a[12]*b[1]+ a[13]*b[5]+ a[14]*b[9] + a[15]*b[13];
+// r[14]= a[12]*b[2]+ a[13]*b[6]+ a[14]*b[10]+ a[15]*b[14];
+// r[15]= a[12]*b[3]+ a[13]*b[7]+ a[14]*b[11]+ a[15]*b[15];
+//============================================================
+
+#if defined(TORQUE_SUPPORTS_NASM)
+#define ADD_3DNOW_FUNCS
+extern "C"
+{
+   void Athlon_MatrixF_x_MatrixF(const F32 *matA, const F32 *matB, F32 *result);
+}
+
+#elif defined(TORQUE_SUPPORTS_VC_INLINE_X86_ASM)
+#define ADD_3DNOW_FUNCS
+// inlined version here.
+void Athlon_MatrixF_x_MatrixF(const F32 *matA, const F32 *matB, F32 *result)
+{
+   __asm
+   {
+      femms
+
+      mov         ecx, matA
+      mov         edx, matB
+      mov         eax, result
+
+      prefetch    [ecx+32]       ;// These may help -
+      prefetch    [edx+32]       ;//    and probably don't hurt
+
+      movq        mm0,[ecx]      ;// a21   | a11
+      movq        mm1,[ecx+8]      ;// a41   | a31
+      movq        mm4,[edx]      ;// b21   | b11
+      punpckhdq   mm2,mm0         ;// a21   |
+      movq        mm5,[edx+16]   ;// b22   | b12
+      punpckhdq   mm3,mm1         ;// a41   |
+      movq        mm6,[edx+32]   ;// b23   | b13
+      punpckldq   mm0,mm0         ;// a11   | a11
+      punpckldq   mm1,mm1         ;// a31   | a31
+      pfmul       mm4,mm0         ;// a11*b21 | a11*b11
+      punpckhdq   mm2,mm2         ;// a21   | a21
+      pfmul       mm0,[edx+8]      ;// a11*b41 | a11*b31
+      movq        mm7,[edx+48]   ;// b24   | b14
+      pfmul       mm5,mm2         ;// a21*b22 | a21*b12
+      punpckhdq   mm3,mm3         ;// a41   | a41
+      pfmul       mm2,[edx+24]   ;// a21*b42 | a21*b32
+      pfmul       mm6,mm1         ;// a31*b23 | a31*b13
+      pfadd       mm5,mm4         ;// a21*b22 + a11*b21 | a21*b12 + a11*b11
+      pfmul       mm1,[edx+40]   ;// a31*b43 | a31*b33
+      pfadd       mm2,mm0         ;// a21*b42 + a11*b41 | a21*b32 + a11*b31
+      pfmul       mm7,mm3         ;// a41*b24 | a41*b14
+      pfadd       mm6,mm5         ;// a21*b22 + a11*b21 + a31*b23 | a21*b12 + a11*b11 + a31*b13
+      pfmul       mm3,[edx+56]   ;// a41*b44 | a41*b34
+      pfadd       mm2,mm1         ;// a21*b42 + a11*b41 + a31*b43 | a21*b32 + a11*b31 + a31*b33
+      pfadd       mm7,mm6         ;// a41*b24 + a21*b22 + a11*b21 + a31*b23 |  a41*b14 + a21*b12 + a11*b11 + a31*b13
+      movq        mm0,[ecx+16]   ;// a22   | a12
+      pfadd       mm3,mm2         ;// a41*b44 + a21*b42 + a11*b41 + a31*b43 | a41*b34 + a21*b32 + a11*b31 + a31*b33
+      movq        mm1,[ecx+24]   ;// a42   | a32
+      movq        [eax],mm7      ;// r21   | r11
+      movq        mm4,[edx]      ;// b21   | b11
+      movq        [eax+8],mm3      ;// r41   | r31
+
+      punpckhdq   mm2,mm0         ;// a22   | XXX
+      movq        mm5,[edx+16]   ;// b22   | b12
+      punpckhdq   mm3,mm1         ;// a42   | XXX
+      movq        mm6,[edx+32]   ;// b23   | b13
+      punpckldq   mm0,mm0         ;// a12   | a12
+      punpckldq   mm1,mm1         ;// a32   | a32
+      pfmul       mm4,mm0         ;// a12*b21 | a12*b11
+      punpckhdq   mm2,mm2         ;// a22   | a22
+      pfmul       mm0,[edx+8]      ;// a12*b41 | a12*b31
+      movq        mm7,[edx+48]   ;// b24   | b14
+      pfmul       mm5,mm2         ;// a22*b22 | a22*b12
+      punpckhdq   mm3,mm3         ;// a42   | a42
+      pfmul       mm2,[edx+24]   ;// a22*b42 | a22*b32
+      pfmul       mm6,mm1         ;// a32*b23 | a32*b13
+      pfadd       mm5,mm4         ;// a12*b21 + a22*b22 | a12*b11 + a22*b12
+      pfmul       mm1,[edx+40]   ;// a32*b43 | a32*b33
+      pfadd       mm2,mm0         ;// a12*b41 + a22*b42 | a12*b11 + a22*b32
+      pfmul       mm7,mm3         ;// a42*b24 | a42*b14
+      pfadd       mm6,mm5         ;// a32*b23 + a12*b21 + a22*b22 | a32*b13 + a12*b11 + a22*b12
+      pfmul       mm3,[edx+56]   ;// a42*b44 | a42*b34
+      pfadd       mm2,mm1         ;// a32*b43 + a12*b41 + a22*b42 | a32*b33 + a12*b11 + a22*b32
+      pfadd       mm7,mm6         ;// a42*b24 + a32*b23 + a12*b21 + a22*b22 | a42*b14 + a32*b13 + a12*b11 + a22*b12
+      movq        mm0,[ecx+32]   ;// a23 | a13
+      pfadd       mm3,mm2         ;// a42*b44 + a32*b43 + a12*b41 + a22*b42 | a42*b34 + a32*b33 + a12*b11 + a22*b32
+      movq        mm1,[ecx+40]   ;// a43 | a33
+      movq        [eax+16],mm7   ;// r22 | r12
+      movq        mm4,[edx]      ;// b21   | b11
+      movq        [eax+24],mm3   ;// r42 | r32
+
+      punpckhdq   mm2,mm0         ;// a23 | XXX
+      movq        mm5,[edx+16]   ;// b22 | b12
+      punpckhdq   mm3,mm1         ;// a43 | XXX
+      movq        mm6,[edx+32]   ;// b23 | b13
+      punpckldq   mm0,mm0         ;// a13 | a13
+      punpckldq   mm1,mm1         ;// a33 | a33
+      pfmul       mm4,mm0         ;// a13*b21 | a13*b11
+      punpckhdq   mm2,mm2         ;// a23 | a23
+      pfmul       mm0,[edx+8]      ;// a13*b41 | a13*b31
+      movq        mm7,[edx+48]   ;// b24 | b14
+      pfmul       mm5,mm2         ;// a23*b22 | a23*b12
+      punpckhdq   mm3,mm3         ;// a43 | a43
+      pfmul       mm2,[edx+24]   ;// a23*b42 | a23*b32
+      pfmul       mm6,mm1         ;// a33*b23 | a33*b13
+      pfadd       mm5,mm4         ;// a23*b22 + a13*b21 | a23*b12 + a13*b11
+      pfmul       mm1,[edx+40]   ;// a33*b43 | a33*b33
+      pfadd       mm2,mm0         ;// a13*b41 + a23*b42 | a13*b31 + a23*b32
+      pfmul       mm7,mm3         ;// a43*b24 | a43*b14
+      pfadd       mm6,mm5         ;// a33*b23 + a23*b22 + a13*b21 | a33*b13 + a23*b12 + a13*b11
+      pfmul       mm3,[edx+56]   ;// a43*b44 | a43*b34
+      pfadd       mm2,mm1         ;// a33*b43*a13*b41 + a23*b42 | a33*b33 + a13*b31 + a23*b32
+      pfadd       mm7,mm6         ;// a43*b24 + a33*b23 + a23*b22 + a13*b21 | a43*b14 + a33*b13 + a23*b12 + a13*b11
+      movq        mm0,[ecx+48]   ;// a24 | a14
+      pfadd       mm3,mm2         ;// a43*b44 + a33*b43*a13*b41 + a23*b42 | a43*b34 + a33*b33 + a13*b31 + a23*b32
+      movq        mm1,[ecx+56]   ;// a44 | a34
+      movq        [eax+32],mm7   ;// r23 | r13
+      movq        mm4,[edx]      ;// b21 | b11
+      movq        [eax+40],mm3   ;// r43 | r33
+
+      punpckhdq   mm2,mm0         ;// a24 | XXX
+      movq        mm5,[edx+16]   ;// b22 | b12
+      punpckhdq   mm3,mm1         ;// a44 | XXX
+      movq        mm6,[edx+32]   ;// b23 | b13
+      punpckldq   mm0,mm0         ;// a14 | a14
+      punpckldq   mm1,mm1         ;// a34 | a34
+      pfmul       mm4,mm0         ;// a14*b21 | a14*b11
+      punpckhdq   mm2,mm2         ;// a24 | a24
+      pfmul       mm0,[edx+8]      ;// a14*b41 | a14*b31
+      movq        mm7,[edx+48]   ;// b24 | b14
+      pfmul       mm5,mm2         ;// a24*b22 | a24*b12
+      punpckhdq   mm3,mm3         ;// a44 | a44
+      pfmul       mm2,[edx+24]   ;// a24*b 42 | a24*b32
+      pfmul       mm6,mm1         ;// a34*b23 | a34*b13
+      pfadd       mm5,mm4         ;// a14*b21 + a24*b22 | a14*b11 + a24*b12
+      pfmul       mm1,[edx+40]   ;// a34*b43 | a34*b33
+      pfadd       mm2,mm0         ;// a14*b41 + a24*b 42 | a14*b31 + a24*b32
+      pfmul       mm7,mm3         ;// a44*b24 | a44*b14
+      pfadd       mm6,mm5         ;// a34*b23 + a14*b21 + a24*b22 | a34*b13 + a14*b11 + a24*b12
+      pfmul       mm3,[edx+56]   ;// a44*b44 | a44*b34
+      pfadd       mm2,mm1         ;// a34*b43 + a14*b41 + a24*b 42 | a34*b33 + a14*b31 + a24*b32
+      pfadd       mm7,mm6         ;// a44*b24 + a14*b23 + a24*b 42 | a44*b14 + a14*b31 + a24*b32
+      pfadd       mm3,mm2         ;// a44*b44 + a34*b43 + a14*b41 + a24*b42 | a44*b34 + a34*b33 + a14*b31 + a24*b32
+      movq        [eax+48],mm7   ;// r24 | r14
+      movq        [eax+56],mm3   ;// r44 | r34
+      femms
+   }
+}
+#endif
+
+
+#if 0
+/* this isn't currently used/implemented.
+void Athlon_MatrixF_x_VectorF(const F32 *matrix, const F32 *vector, F32 *result)
+{
+   __asm {
+      femms
+      mov         eax,result
+      mov         ecx,vector
+      mov         edx,matrix
+
+      // Here's what we're doing:
+      // result[0] = M[0] * v[0]    +  M[1] * v[1]    +  M[2] * v[2];
+      // result[1] = M[4] * v[0]    +  M[5] * v[1]    +  M[6] * v[2];
+      // result[2] = M[8] * v[0]    +  M[9] * v[1]    +  M[10]* v[2];
+
+      movq        mm0,[ecx]         //     y   |  x
+      movd        mm1,[ecx+8]       //     0   |  z
+      movd        mm4,[edx+8]       //     0   | m_13
+      movq        mm3,mm0           //     y   |  x
+      movd        mm2,[edx+40]      //     0   | m_33 (M[10])
+      punpckldq   mm0,mm0           //     x   |  x
+      punpckldq   mm4,[edx+20]      //    m_31 | m_23
+      pfmul       mm0,[edx]         //     x * m_12 | x * m_11
+      punpckhdq   mm3,mm3           //     y   |  y
+      pfmul       mm2,mm1           //     0   |  z * m_33
+      punpckldq   mm1,mm1           //     z   |  z
+      pfmul       mm4,[ecx]         //    y * m_31 | x * m_23
+      pfmul       mm3,[edx+12]      //    y * m_22 | y * m_21
+      pfmul       mm1,[edx+24]      //    z * m_32 | z * m_32
+      pfacc       mm4,mm4           //     ?   | y * m_31 + x * m_23
+      pfadd       mm3,mm0           //     x * m_12 + y * m_22 | x * m_11 + y * m_21
+      pfadd       mm4,mm2           //     ?   | y * m_31 + x * m_23 + z * m_33
+      pfadd       mm3,mm1           //     x * m_12 + y * m_22 + z * m_32 | x * m_11 + y * m_21 + z * m_32
+      movd        [eax+8],mm4       //    r_z
+      movq        [eax],mm3         //    r_y  | r_x
+      femms
+   }
+}
+*/
+#endif
+
+
+void mInstall_AMD_Math()
+{
+#if defined(ADD_3DNOW_FUNCS)
+   m_matF_x_matF           = Athlon_MatrixF_x_MatrixF;
+#endif
+   // m_matF_x_point3F = Athlon_MatrixF_x_Point3F;
+   // m_matF_x_vectorF = Athlon_MatrixF_x_VectorF;
+}
+

engine/math/mMathAMD_ASM.asm

@@ -0,0 +1,160 @@
+;-----------------------------------------------------------------------------
+; Torque Game Engine
+; Copyright (C) GarageGames.com, Inc.
+;-----------------------------------------------------------------------------
+
+
+segment .data
+
+matA     dd 0
+result   dd 0
+matB     dd 0
+
+segment .text
+
+%macro export_fn 1 
+   %ifdef LINUX
+   ; No underscore needed for ELF object files
+   global %1
+   %1:
+   %else
+   global _%1
+   _%1:
+   %endif
+%endmacro
+
+
+%define arg(x) [esp+(x*4)] 
+
+
+
+;void Athlon_MatrixF_x_MatrixF(const F32 *matA, const F32 *matB, F32 *result)
+
+export_fn Athlon_MatrixF_x_MatrixF
+
+      mov         ecx, arg(1)
+      mov         edx, arg(2)
+      mov         eax, arg(3)
+      
+      femms
+      prefetch    [ecx+32]       ; These may help -
+      prefetch    [edx+32]       ;    and probably don't hurt
+      
+      movq        mm0,[ecx]		; a21	| a11
+      movq        mm1,[ecx+8]		; a41	| a31
+      movq        mm4,[edx]		; b21	| b11
+      punpckhdq   mm2,mm0			; a21	| 
+      movq        mm5,[edx+16]	; b22	| b12
+      punpckhdq   mm3,mm1			; a41	| 
+      movq        mm6,[edx+32]	; b23	| b13
+      punpckldq   mm0,mm0			; a11	| a11
+      punpckldq   mm1,mm1			; a31	| a31
+      pfmul       mm4,mm0			; a11*b21 | a11*b11
+      punpckhdq   mm2,mm2			; a21	| a21
+      pfmul       mm0,[edx+8]		; a11*b41 | a11*b31
+      movq        mm7,[edx+48]	; b24	| b14
+      pfmul       mm5,mm2			; a21*b22 | a21*b12
+      punpckhdq   mm3,mm3			; a41	| a41
+      pfmul       mm2,[edx+24]	; a21*b42 | a21*b32
+      pfmul       mm6,mm1			; a31*b23 | a31*b13 
+      pfadd       mm5,mm4			; a21*b22 + a11*b21 | a21*b12 + a11*b11
+      pfmul       mm1,[edx+40]	; a31*b43 | a31*b33
+      pfadd       mm2,mm0			; a21*b42 + a11*b41 | a21*b32 + a11*b31
+      pfmul       mm7,mm3			; a41*b24 | a41*b14 
+      pfadd       mm6,mm5			; a21*b22 + a11*b21 + a31*b23 | a21*b12 + a11*b11 + a31*b13
+      pfmul       mm3,[edx+56]	; a41*b44 | a41*b34
+      pfadd       mm2,mm1			; a21*b42 + a11*b41 + a31*b43 | a21*b32 + a11*b31 + a31*b33 
+      pfadd       mm7,mm6			; a41*b24 + a21*b22 + a11*b21 + a31*b23 |  a41*b14 + a21*b12 + a11*b11 + a31*b13
+      movq        mm0,[ecx+16]	; a22	| a12
+      pfadd       mm3,mm2			; a41*b44 + a21*b42 + a11*b41 + a31*b43 | a41*b34 + a21*b32 + a11*b31 + a31*b33 
+      movq        mm1,[ecx+24]	; a42	| a32
+      movq        [eax],mm7		; r21	| r11 
+      movq        mm4,[edx]		; b21	| b11
+      movq        [eax+8],mm3		; r41	| r31
+
+      punpckhdq   mm2,mm0			; a22	| XXX
+      movq        mm5,[edx+16]	; b22	| b12
+      punpckhdq   mm3,mm1			; a42	| XXX
+      movq        mm6,[edx+32]	; b23	| b13
+      punpckldq   mm0,mm0			; a12	| a12
+      punpckldq   mm1,mm1			; a32	| a32
+      pfmul       mm4,mm0			; a12*b21 | a12*b11
+      punpckhdq   mm2,mm2			; a22	| a22
+      pfmul       mm0,[edx+8]		; a12*b41 | a12*b31
+      movq        mm7,[edx+48]	; b24	| b14
+      pfmul       mm5,mm2			; a22*b22 | a22*b12
+      punpckhdq   mm3,mm3			; a42	| a42
+      pfmul       mm2,[edx+24]	; a22*b42 | a22*b32
+      pfmul       mm6,mm1			; a32*b23 | a32*b13
+      pfadd       mm5,mm4			; a12*b21 + a22*b22 | a12*b11 + a22*b12
+      pfmul       mm1,[edx+40]	; a32*b43 | a32*b33
+      pfadd       mm2,mm0			; a12*b41 + a22*b42 | a12*b11 + a22*b32
+      pfmul       mm7,mm3			; a42*b24 | a42*b14
+      pfadd       mm6,mm5			; a32*b23 + a12*b21 + a22*b22 | a32*b13 + a12*b11 + a22*b12
+      pfmul       mm3,[edx+56]	; a42*b44 | a42*b34
+      pfadd       mm2,mm1			; a32*b43 + a12*b41 + a22*b42 | a32*b33 + a12*b11 + a22*b32
+      pfadd       mm7,mm6			; a42*b24 + a32*b23 + a12*b21 + a22*b22 | a42*b14 + a32*b13 + a12*b11 + a22*b12
+      movq        mm0,[ecx+32]	; a23 | a13
+      pfadd       mm3,mm2			; a42*b44 + a32*b43 + a12*b41 + a22*b42 | a42*b34 + a32*b33 + a12*b11 + a22*b32
+      movq        mm1,[ecx+40]	; a43 | a33
+      movq        [eax+16],mm7	; r22 | r12
+      movq        mm4,[edx]		; b21	| b11
+      movq        [eax+24],mm3	; r42 | r32
+
+      punpckhdq   mm2,mm0			; a23 | XXX
+      movq        mm5,[edx+16]	; b22 | b12
+      punpckhdq   mm3,mm1			; a43 | XXX
+      movq        mm6,[edx+32]	; b23 | b13
+      punpckldq   mm0,mm0			; a13 | a13
+      punpckldq   mm1,mm1			; a33 | a33
+      pfmul       mm4,mm0			; a13*b21 | a13*b11
+      punpckhdq   mm2,mm2			; a23 | a23
+      pfmul       mm0,[edx+8]		; a13*b41 | a13*b31
+      movq        mm7,[edx+48]	; b24 | b14
+      pfmul       mm5,mm2			; a23*b22 | a23*b12
+      punpckhdq   mm3,mm3			; a43 | a43
+      pfmul       mm2,[edx+24]	; a23*b42 | a23*b32
+      pfmul       mm6,mm1			; a33*b23 | a33*b13
+      pfadd       mm5,mm4			; a23*b22 + a13*b21 | a23*b12 + a13*b11
+      pfmul       mm1,[edx+40]	; a33*b43 | a33*b33 
+      pfadd       mm2,mm0			; a13*b41 + a23*b42 | a13*b31 + a23*b32
+      pfmul       mm7,mm3			; a43*b24 | a43*b14
+      pfadd       mm6,mm5			; a33*b23 + a23*b22 + a13*b21 | a33*b13 + a23*b12 + a13*b11
+      pfmul       mm3,[edx+56]	; a43*b44 | a43*b34
+      pfadd       mm2,mm1			; a33*b43*a13*b41 + a23*b42 | a33*b33 + a13*b31 + a23*b32
+      pfadd       mm7,mm6			; a43*b24 + a33*b23 + a23*b22 + a13*b21 | a43*b14 + a33*b13 + a23*b12 + a13*b11
+      movq        mm0,[ecx+48]	; a24 | a14
+      pfadd       mm3,mm2			; a43*b44 + a33*b43*a13*b41 + a23*b42 | a43*b34 + a33*b33 + a13*b31 + a23*b32
+      movq        mm1,[ecx+56]	; a44 | a34
+      movq        [eax+32],mm7	; r23 | r13
+      movq        mm4,[edx]		; b21 | b11
+      movq        [eax+40],mm3	; r43 | r33
+
+      punpckhdq   mm2,mm0			; a24 | XXX
+      movq        mm5,[edx+16]	; b22 | b12
+      punpckhdq   mm3,mm1			; a44 | XXX
+      movq        mm6,[edx+32]	; b23 | b13
+      punpckldq   mm0,mm0			; a14 | a14
+      punpckldq   mm1,mm1			; a34 | a34
+      pfmul       mm4,mm0			; a14*b21 | a14*b11
+      punpckhdq   mm2,mm2			; a24 | a24
+      pfmul       mm0,[edx+8]		; a14*b41 | a14*b31
+      movq        mm7,[edx+48]	; b24 | b14
+      pfmul       mm5,mm2			; a24*b22 | a24*b12
+      punpckhdq   mm3,mm3			; a44 | a44
+      pfmul       mm2,[edx+24]	; a24*b 42 | a24*b32
+      pfmul       mm6,mm1			; a34*b23 | a34*b13
+      pfadd       mm5,mm4			; a14*b21 + a24*b22 | a14*b11 + a24*b12
+      pfmul       mm1,[edx+40]	; a34*b43 | a34*b33
+      pfadd       mm2,mm0			; a14*b41 + a24*b 42 | a14*b31 + a24*b32
+      pfmul       mm7,mm3			; a44*b24 | a44*b14
+      pfadd       mm6,mm5			; a34*b23 + a14*b21 + a24*b22 | a34*b13 + a14*b11 + a24*b12
+      pfmul       mm3,[edx+56]	; a44*b44 | a44*b34
+      pfadd       mm2,mm1			; a34*b43 + a14*b41 + a24*b 42 | a34*b33 + a14*b31 + a24*b32
+      pfadd       mm7,mm6			; a44*b24 + a14*b23 + a24*b 42 | a44*b14 + a14*b31 + a24*b32
+      pfadd       mm3,mm2			; a44*b44 + a34*b43 + a14*b41 + a24*b42 | a44*b34 + a34*b33 + a14*b31 + a24*b32
+      movq        [eax+48],mm7	; r24 | r14
+      movq        [eax+56],mm3	; r44 | r34
+      femms
+
+      ret      

engine/math/mMathFn.cc

@@ -0,0 +1,19 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#include "math/mMathFn.h"
+#include "math/mPlane.h"
+#include "math/mMatrix.h"
+
+
+void mTransformPlane(const MatrixF& mat,
+                     const Point3F& scale,
+                     const PlaneF&  plane,
+                     PlaneF*        result)
+{
+   m_matF_x_scale_x_planeF(mat, &scale.x, &plane.x, &result->x);
+}
+
+

engine/math/mMathFn.h

@@ -0,0 +1,454 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#ifndef _MMATHFN_H_
+#define _MMATHFN_H_
+
+#ifndef _PLATFORM_H_
+#include "platform/platform.h"
+#endif
+#ifndef _MCONSTANTS_H_
+#include "math/mConstants.h"
+#endif
+#include <math.h>
+
+// Remove a couple of annoying macros, if they are present (In VC 6, they are.)
+#ifdef min
+#undef min
+#endif
+#ifdef max
+#undef max
+#endif
+
+class MatrixF;
+class PlaneF;
+
+extern void MathConsoleInit();
+
+//--------------------------------------
+// Installable Library Prototypes
+extern S32  (*m_mulDivS32)(S32 a, S32 b, S32 c);
+extern U32  (*m_mulDivU32)(S32 a, S32 b, U32 c);
+
+extern F32  (*m_catmullrom)(F32 t, F32 p0, F32 p1, F32 p2, F32 p3);
+
+extern void (*m_point2F_normalize)(F32 *p);
+extern void (*m_point2F_normalize_f)(F32 *p, F32 len);
+extern void (*m_point2D_normalize)(F64 *p);
+extern void (*m_point2D_normalize_f)(F64 *p, F64 len);
+extern void (*m_point3F_normalize)(F32 *p);
+extern void (*m_point3F_normalize_f)(F32 *p, F32 len);
+extern void (*m_point3F_interpolate)(const F32 *from, const F32 *to, F32 factor, F32 *result);
+
+extern void (*m_point3D_normalize)(F64 *p);
+extern void (*m_point3D_normalize_f)(F64 *p, F64 len);
+extern void (*m_point3D_interpolate)(const F64 *from, const F64 *to, F64 factor, F64 *result);
+
+extern void (*m_point3F_bulk_dot)(const F32* refVector,
+                                  const F32* dotPoints,
+                                  const U32  numPoints,
+                                  const U32  pointStride,
+                                  F32*       output);
+extern void (*m_point3F_bulk_dot_indexed)(const F32* refVector,
+                                          const F32* dotPoints,
+                                          const U32  numPoints,
+                                          const U32  pointStride,
+                                          const U32* pointIndices,
+                                          F32*       output);
+
+extern void (*m_quatF_set_matF)( F32 x, F32 y, F32 z, F32 w, F32* m );
+
+extern void (*m_matF_set_euler)(const F32 *e, F32 *result);
+extern void (*m_matF_set_euler_point)(const F32 *e, const F32 *p, F32 *result);
+extern void (*m_matF_identity)(F32 *m);
+extern void (*m_matF_inverse)(F32 *m);
+extern void (*m_matF_affineInverse)(F32 *m);
+extern void (*m_matF_transpose)(F32 *m);
+extern void (*m_matF_scale)(F32 *m,const F32* p);
+extern void (*m_matF_normalize)(F32 *m);
+extern F32  (*m_matF_determinant)(const F32 *m);
+extern void (*m_matF_x_matF)(const F32 *a, const F32 *b, F32 *mresult);
+// extern void (*m_matF_x_point3F)(const F32 *m, const F32 *p, F32 *presult);
+// extern void (*m_matF_x_vectorF)(const F32 *m, const F32 *v, F32 *vresult);
+extern void (*m_matF_x_point4F)(const F32 *m, const F32 *p, F32 *presult);
+extern void (*m_matF_x_scale_x_planeF)(const F32 *m, const F32* s, const F32 *p, F32 *presult);
+extern void (*m_matF_x_box3F)(const F32 *m, F32 *min, F32 *max);
+
+// Note that x must point to at least 4 values for quartics, and 3 for cubics
+extern U32 (*mSolveQuadratic)(F32 a, F32 b, F32 c, F32* x);
+extern U32 (*mSolveCubic)(F32 a, F32 b, F32 c, F32 d, F32* x);
+extern U32 (*mSolveQuartic)(F32 a, F32 b, F32 c, F32 d, F32 e, F32* x);
+
+extern S32 mRandI(S32 i1, S32 i2); // random # from i1 to i2 inclusive
+extern F32 mRandF(F32 f1, F32 f2); // random # from f1 to f2 inclusive
+
+
+inline void m_matF_x_point3F(const F32 *m, const F32 *p, F32 *presult)
+{
+   AssertFatal(p != presult, "Error, aliasing matrix mul pointers not allowed here!");
+#if defined(TORQUE_SUPPORTS_GCC_INLINE_X86_ASM)
+   // inline assembly version because gcc's math optimization isn't as good
+   // JMQ: the profiler shows that with g++ 2.96, the difference
+   // between the asm and optimized c versions is minimal.
+   int u0, u1, u2;
+   __asm__ __volatile__ (
+   "flds   0x8(%%eax)\n"
+   "fmuls  0x8(%%ecx)\n"
+   "flds   0x4(%%eax)\n"
+   "fmuls  0x4(%%ecx)\n"
+   "faddp  %%st,%%st(1)\n"
+   "flds   (%%eax)\n"
+   "fmuls  (%%ecx)\n"
+   "faddp  %%st,%%st(1)\n"
+   "fadds  0xc(%%eax)\n"
+   "fstps  (%%edx)\n"
+   "flds   0x18(%%eax)\n"
+   "fmuls  0x8(%%ecx)\n"
+   "flds   0x10(%%eax)\n"
+   "fmuls  (%%ecx)\n"
+   "faddp  %%st,%%st(1)\n"
+   "flds   0x14(%%eax)\n"
+   "fmuls  0x4(%%ecx)\n"
+   "faddp  %%st,%%st(1)\n"
+   "fadds  0x1c(%%eax)\n"
+   "fstps  0x4(%%edx)\n"
+   "flds   0x28(%%eax)\n"
+   "fmuls  0x8(%%ecx)\n"
+   "flds   0x20(%%eax)\n"
+   "fmuls  (%%ecx)\n"
+   "faddp  %%st,%%st(1)\n"
+   "flds   0x24(%%eax)\n"
+   "fmuls  0x4(%%ecx)\n"
+   "faddp  %%st,%%st(1)\n"
+   "fadds  0x2c(%%eax)\n"
+   "fstps  0x8(%%edx)\n"
+     : "=&a" (u0), "=&c" (u1), "=&d" (u2)
+     : "0"   (m),  "1"   (p),  "2"   (presult)
+     : "memory" );
+#else
+   presult[0] = m[0]*p[0] + m[1]*p[1] + m[2]*p[2]  + m[3];
+   presult[1] = m[4]*p[0] + m[5]*p[1] + m[6]*p[2]  + m[7];
+   presult[2] = m[8]*p[0] + m[9]*p[1] + m[10]*p[2] + m[11];
+#endif
+}
+
+
+//--------------------------------------
+inline void m_matF_x_vectorF(const F32 *m, const F32 *v, F32 *vresult)
+{
+   AssertFatal(v != vresult, "Error, aliasing matrix mul pointers not allowed here!");
+#if defined(TORQUE_SUPPORTS_GCC_INLINE_X86_ASM)
+   // inline assembly version because gcc's math optimization isn't as good
+   // JMQ: the profiler shows that with g++ 2.96, the difference
+   // between the asm and optimized c versions is minimal.
+   int u0, u1, u2;
+   __asm__ __volatile__ (
+        "flds   0x8(%%ecx)\n"
+        "fmuls  0x8(%%eax)\n"
+        "flds   0x4(%%ecx)\n"
+        "fmuls  0x4(%%eax)\n"
+        "faddp  %%st,%%st(1)\n"
+        "flds   (%%ecx)\n"
+        "fmuls  (%%eax)\n"
+        "faddp  %%st,%%st(1)\n"
+        "fstps  (%%edx)\n"
+        "flds   0x18(%%ecx)\n"
+        "fmuls  0x8(%%eax)\n"
+        "flds   0x10(%%ecx)\n"
+        "fmuls  (%%eax)\n"
+        "faddp  %%st,%%st(1)\n"
+        "flds   0x14(%%ecx)\n"
+        "fmuls  0x4(%%eax)\n"
+        "faddp  %%st,%%st(1)\n"
+        "fstps  0x4(%%edx)\n"
+        "flds   0x28(%%ecx)\n"
+        "fmuls  0x8(%%eax)\n"
+        "flds   0x20(%%ecx)\n"
+        "fmuls  (%%eax)\n"
+        "faddp  %%st,%%st(1)\n"
+        "flds   0x24(%%ecx)\n"
+        "fmuls  0x4(%%eax)\n"
+        "faddp  %%st,%%st(1)\n"
+        "fstps  0x8(%%edx)\n"
+     : "=&c" (u0), "=&a" (u1), "=&d" (u2)
+     : "0"   (m),  "1"   (v),  "2"   (vresult)
+     : "memory" );
+#else
+   vresult[0] = m[0]*v[0] + m[1]*v[1] + m[2]*v[2];
+   vresult[1] = m[4]*v[0] + m[5]*v[1] + m[6]*v[2];
+   vresult[2] = m[8]*v[0] + m[9]*v[1] + m[10]*v[2];
+#endif
+}
+
+
+//--------------------------------------
+// Inlines
+inline F32 mFloor(const F32 val)
+{
+   return (F32) floor(val);
+}
+
+inline F32 mCeil(const F32 val)
+{
+   return (F32) ceil(val);
+}
+
+inline F32 mFabs(const F32 val)
+{
+   return (F32) fabs(val);
+}
+
+inline F32 mFmod(const F32 val, const F32 mod)
+{
+   return (F32) fmod(val, mod);
+}
+
+inline S32 mAbs(const S32 val)
+{
+   // Kinda lame, and disallows intrinsic inlining by the compiler.  Maybe fix?
+   //  DMM
+   if (val < 0)
+      return -val;
+
+   return val;
+}
+
+inline S32 mClamp(S32 val, S32 low, S32 high)
+{
+   return getMax(getMin(val, high), low);
+}
+
+inline F32 mClampF(F32 val, F32 low, F32 high)
+{
+   return (F32) getMax(getMin(val, high), low);
+}
+
+inline S32 mMulDiv(S32 a, S32 b, S32 c)
+{
+   return m_mulDivS32(a, b, c);
+}
+
+inline U32 mMulDiv(S32 a, S32 b, U32 c)
+{
+   return m_mulDivU32(a, b, c);
+}
+
+
+inline F32 mSin(const F32 angle)
+{
+   return (F32) sin(angle);
+}
+
+inline F32 mCos(const F32 angle)
+{
+   return (F32) cos(angle);
+}
+
+inline F32 mTan(const F32 angle)
+{
+   return (F32) tan(angle);
+}
+
+inline F32 mAsin(const F32 val)
+{
+   return (F32) asin(val);
+}
+
+inline F32 mAcos(const F32 val)
+{
+   return (F32) acos(val);
+}
+
+inline F32 mAtan(const F32 x, const F32 y)
+{
+   return (F32) atan2(x, y);
+}
+
+inline void mSinCos(const F32 angle, F32 &s, F32 &c)
+{
+   s = mSin(angle);
+   c = mCos(angle);
+}
+
+inline F32 mTanh(const F32 angle)
+{
+   return (F32) tanh(angle);
+}
+
+inline F32 mSqrt(const F32 val)
+{
+   return (F32) sqrt(val);
+}
+
+inline F32 mPow(const F32 x, const F32 y)
+{
+   return (F32) pow(x, y);
+}
+
+inline F32 mLog(const F32 val)
+{
+   return (F32) log(val);
+}
+
+
+inline F64 mSin(const F64 angle)
+{
+   return (F64) sin(angle);
+}
+
+inline F64 mCos(const F64 angle)
+{
+   return (F64) cos(angle);
+}
+
+inline F64 mTan(const F64 angle)
+{
+   return (F64) tan(angle);
+}
+
+inline F64 mAsin(const F64 val)
+{
+   return (F64) asin(val);
+}
+
+inline F64 mAcos(const F64 val)
+{
+   return (F64) acos(val);
+}
+
+inline F64 mAtan(const F64 x, const F64 y)
+{
+   return (F64) atan2(x, y);
+}
+
+inline void mSinCos(const F64 angle, F64 &sin, F64 &cos)
+{
+   sin = mSin(angle);
+   cos = mCos(angle);
+}
+
+inline F64 mTanh(const F64 angle)
+{
+   return (F64) tanh(angle);
+}
+
+inline F64 mPow(const F64 x, const F64 y)
+{
+   return (F64) pow(x, y);
+}
+
+inline F64 mLog(const F64 val)
+{
+   return (F64) log(val);
+}
+
+
+inline F32 mCatmullrom(F32 t, F32 p0, F32 p1, F32 p2, F32 p3)
+{
+   return m_catmullrom(t, p0, p1, p2, p3);
+}
+
+
+inline F64 mFabsD(const F64 val)
+{
+   return (F64) fabs(val);
+}
+
+inline F64 mFmodD(const F64 val, const F64 mod)
+{
+   return (F64) fmod(val, mod);
+}
+
+inline F64 mSqrtD(const F64 val)
+{
+   return (F64) sqrt(val);
+}
+
+inline F64 mFloorD(const F64 val)
+{
+   return (F64) floor(val);
+}
+
+inline F64 mCeilD(const F64 val)
+{
+   return (F64) ceil(val);
+}
+
+
+//--------------------------------------
+#ifndef _MPOINT_H_
+#include "math/mPoint.h"
+#endif
+
+inline F32 mDot(const Point3F &p1, const Point3F &p2)
+{
+   return (p1.x*p2.x + p1.y*p2.y + p1.z*p2.z);
+}
+
+inline void mCross(const Point3F &a, const Point3F &b, Point3F *res)
+{
+   res->x = (a.y * b.z) - (a.z * b.y);
+   res->y = (a.z * b.x) - (a.x * b.z);
+   res->z = (a.x * b.y) - (a.y * b.x);
+}
+
+inline F64 mDot(const Point3D &p1, const Point3D &p2)
+{
+   return (p1.x*p2.x + p1.y*p2.y + p1.z*p2.z);
+}
+
+inline void mCross(const Point3D &a, const Point3D &b, Point3D *res)
+{
+   res->x = (a.y * b.z) - (a.z * b.y);
+   res->y = (a.z * b.x) - (a.x * b.z);
+   res->z = (a.x * b.y) - (a.y * b.x);
+}
+
+inline Point3F mCross(const Point3F &a, const Point3F &b)
+{
+   Point3F ret;
+   mCross(a,b,&ret);
+   return ret;
+}
+
+inline void mCross(const F32* a, const F32* b, F32 *res)
+{
+   res[0] = (a[1] * b[2]) - (a[2] * b[1]);
+   res[1] = (a[2] * b[0]) - (a[0] * b[2]);
+   res[2] = (a[0] * b[1]) - (a[1] * b[0]);
+}
+
+inline void mCross(const F64* a, const F64* b, F64* res)
+{
+   res[0] = (a[1] * b[2]) - (a[2] * b[1]);
+   res[1] = (a[2] * b[0]) - (a[0] * b[2]);
+   res[2] = (a[0] * b[1]) - (a[1] * b[0]);
+}
+
+void mTransformPlane(const MatrixF& mat, const Point3F& scale, const PlaneF& plane, PlaneF* result);
+
+
+//--------------------------------------
+inline F32 mDegToRad(F32 d)
+{
+   return F32((d * M_PI) / F32(180));
+}
+
+inline F32 mRadToDeg(F32 r)
+{
+   return F32((r * 180.0) / M_PI);
+}
+
+inline F64 mDegToRad(F64 d)
+{
+   return (d * M_PI) / F64(180);
+}
+
+inline F64 mRadToDeg(F64 r)
+{
+   return (r * 180.0) / M_PI;
+}
+
+#endif //_MMATHFN_H_

engine/math/mMathSSE.cc

@@ -0,0 +1,114 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#include "math/mMathFn.h"
+#include "math/mPlane.h"
+#include "math/mMatrix.h"
+
+
+// if we set our flag, we always try to build the inlined asm.
+// EXCEPT if we're in an old version of Codewarrior that can't handle SSE code.
+#if defined(TORQUE_SUPPORTS_NASM)
+#define ADD_SSE_FN
+extern "C"
+{
+   void SSE_MatrixF_x_MatrixF(const F32 *matA, const F32 *matB, F32 *result);
+}
+
+#elif defined(TORQUE_SUPPORTS_VC_INLINE_X86_ASM)
+#define ADD_SSE_FN
+// inlined version here.
+void SSE_MatrixF_x_MatrixF(const F32 *matA, const F32 *matB, F32 *result)
+{
+   __asm
+   {
+      mov         ecx, matA
+      mov         edx, matB
+      mov         eax, result
+
+      movss       xmm0, [edx]
+      movups      xmm1, [ecx]
+      shufps      xmm0, xmm0, 0
+      movss       xmm2, [edx+4]
+      mulps       xmm0, xmm1
+      shufps      xmm2, xmm2, 0
+      movups      xmm3, [ecx+10h]
+      movss       xmm7, [edx+8]
+      mulps       xmm2, xmm3
+      shufps      xmm7, xmm7, 0
+      addps       xmm0, xmm2
+      movups      xmm4, [ecx+20h]
+      movss       xmm2, [edx+0Ch]
+      mulps       xmm7, xmm4
+      shufps      xmm2, xmm2, 0
+      addps       xmm0, xmm7
+      movups      xmm5, [ecx+30h]
+      movss       xmm6, [edx+10h]
+      mulps       xmm2, xmm5
+      movss       xmm7, [edx+14h]
+      shufps      xmm6, xmm6, 0
+      addps       xmm0, xmm2
+      shufps      xmm7, xmm7, 0
+      movlps      [eax], xmm0
+      movhps      [eax+8], xmm0
+      mulps       xmm7, xmm3
+      movss       xmm0, [edx+18h]
+      mulps       xmm6, xmm1
+      shufps      xmm0, xmm0, 0
+      addps       xmm6, xmm7
+      mulps       xmm0, xmm4
+      movss       xmm2, [edx+24h]
+      addps       xmm6, xmm0
+      movss       xmm0, [edx+1Ch]
+      movss       xmm7, [edx+20h]
+      shufps      xmm0, xmm0, 0
+      shufps      xmm7, xmm7, 0
+      mulps       xmm0, xmm5
+      mulps       xmm7, xmm1
+      addps       xmm6, xmm0
+      shufps      xmm2, xmm2, 0
+      movlps      [eax+10h], xmm6
+      movhps      [eax+18h], xmm6
+      mulps       xmm2, xmm3
+      movss       xmm6, [edx+28h]
+      addps       xmm7, xmm2
+      shufps      xmm6, xmm6, 0
+      movss       xmm2, [edx+2Ch]
+      mulps       xmm6, xmm4
+      shufps      xmm2, xmm2, 0
+      addps       xmm7, xmm6
+      mulps       xmm2, xmm5
+      movss       xmm0, [edx+34h]
+      addps       xmm7, xmm2
+      shufps      xmm0, xmm0, 0
+      movlps      [eax+20h], xmm7
+      movss       xmm2, [edx+30h]
+      movhps      [eax+28h], xmm7
+      mulps       xmm0, xmm3
+      shufps      xmm2, xmm2, 0
+      movss       xmm6, [edx+38h]
+      mulps       xmm2, xmm1
+      shufps      xmm6, xmm6, 0
+      addps       xmm2, xmm0
+      mulps       xmm6, xmm4
+      movss       xmm7, [edx+3Ch]
+      shufps      xmm7, xmm7, 0
+      addps       xmm2, xmm6
+      mulps       xmm7, xmm5
+      addps       xmm2, xmm7
+      movups      [eax+30h], xmm2
+   }
+}
+#endif
+
+
+void mInstall_Library_SSE()
+{
+#if defined(ADD_SSE_FN)
+   m_matF_x_matF           = SSE_MatrixF_x_MatrixF;
+   // m_matF_x_point3F = Athlon_MatrixF_x_Point3F;
+   // m_matF_x_vectorF = Athlon_MatrixF_x_VectorF;
+#endif
+}

engine/math/mMathSSE_ASM.asm

@@ -0,0 +1,111 @@
+;-----------------------------------------------------------------------------
+; Torque Game Engine
+; Copyright (C) GarageGames.com, Inc.
+;-----------------------------------------------------------------------------
+
+
+segment .data
+
+matA     dd 0
+result   dd 0
+matB     dd 0
+
+segment .text
+
+%macro export_fn 1 
+   %ifdef LINUX
+   ; No underscore needed for ELF object files
+   global %1
+   %1:
+   %else
+   global _%1
+   _%1:
+   %endif
+%endmacro
+
+
+%define arg(x) [esp+(x*4)] 
+
+
+
+;void SSE_MatrixF_x_MatrixF(const F32 *matA, const F32 *matB, F32 *result)
+
+export_fn SSE_MatrixF_x_MatrixF
+
+      mov         edx, arg(1) 
+      mov         ecx, arg(2)
+      mov         eax, arg(3)
+
+      movss       xmm0, [edx]
+      movups      xmm1, [ecx]
+      shufps      xmm0, xmm0, 0
+      movss       xmm2, [edx+4]
+      mulps       xmm0, xmm1
+      shufps      xmm2, xmm2, 0
+      movups      xmm3, [ecx+10h]
+      movss       xmm7, [edx+8]
+      mulps       xmm2, xmm3
+      shufps      xmm7, xmm7, 0
+      addps       xmm0, xmm2
+      movups      xmm4, [ecx+20h]
+      movss       xmm2, [edx+0Ch]
+      mulps       xmm7, xmm4
+      shufps      xmm2, xmm2, 0
+      addps       xmm0, xmm7
+      movups      xmm5, [ecx+30h]
+      movss       xmm6, [edx+10h]
+      mulps       xmm2, xmm5
+      movss       xmm7, [edx+14h]
+      shufps      xmm6, xmm6, 0
+      addps       xmm0, xmm2
+      shufps      xmm7, xmm7, 0
+      movlps      [eax], xmm0
+      movhps      [eax+8], xmm0
+      mulps       xmm7, xmm3
+      movss       xmm0, [edx+18h]
+      mulps       xmm6, xmm1
+      shufps      xmm0, xmm0, 0
+      addps       xmm6, xmm7
+      mulps       xmm0, xmm4
+      movss       xmm2, [edx+24h]
+      addps       xmm6, xmm0
+      movss       xmm0, [edx+1Ch]
+      movss       xmm7, [edx+20h]
+      shufps      xmm0, xmm0, 0
+      shufps      xmm7, xmm7, 0
+      mulps       xmm0, xmm5
+      mulps       xmm7, xmm1
+      addps       xmm6, xmm0
+      shufps      xmm2, xmm2, 0
+      movlps      [eax+10h], xmm6
+      movhps      [eax+18h], xmm6
+      mulps       xmm2, xmm3
+      movss       xmm6, [edx+28h]
+      addps       xmm7, xmm2
+      shufps      xmm6, xmm6, 0
+      movss       xmm2, [edx+2Ch]
+      mulps       xmm6, xmm4
+      shufps      xmm2, xmm2, 0
+      addps       xmm7, xmm6
+      mulps       xmm2, xmm5
+      movss       xmm0, [edx+34h]
+      addps       xmm7, xmm2
+      shufps      xmm0, xmm0, 0
+      movlps      [eax+20h], xmm7
+      movss       xmm2, [edx+30h]
+      movhps      [eax+28h], xmm7
+      mulps       xmm0, xmm3
+      shufps      xmm2, xmm2, 0
+      movss       xmm6, [edx+38h]
+      mulps       xmm2, xmm1
+      shufps      xmm6, xmm6, 0
+      addps       xmm2, xmm0
+      mulps       xmm6, xmm4
+      movss       xmm7, [edx+3Ch]
+      shufps      xmm7, xmm7, 0
+      addps       xmm2, xmm6
+      mulps       xmm7, xmm5
+      addps       xmm2, xmm7
+      movups      [eax+30h], xmm2
+
+      ret

engine/math/mMath_ASM.asm

@@ -0,0 +1,271 @@
+;-----------------------------------------------------------------------------
+; Torque Game Engine
+; Copyright (C) GarageGames.com, Inc.
+;-----------------------------------------------------------------------------
+;
+; NASM version of optimized funcs in mMath_C
+;
+
+; The following funcs are included:
+;  m_ceil_ASM, m_ceilD_ASM, m_floor_ASM, m_floorD_ASM
+;  m_fmod_ASM, m_fmodD_ASM, m_mulDivS32_ASM, m_mulDivU32_ASM
+;  m_sincos_ASM, m_sincosD_ASM
+
+; The other funcs from mMath_C were determined to compile into fast
+;  code using MSVC --Paul Bowman
+
+
+segment .data
+
+
+temp_int64			dq		0.0
+const_0pt5_D		dq		0.4999999999995
+temp_int32			dd		0
+const_0pt5			dd		0.49999995
+const_neg1			dd		-1.0
+
+
+segment .text
+
+
+%define rnd_adjD	qword [const_0pt5_D]
+%define rnd_adj		dword [const_0pt5]
+
+
+%define val		dword [esp+4]
+%define val64	qword [esp+4]
+;
+; static F32 m_ceil_ASM(F32 val)
+;
+%ifdef __linux
+global m_ceil_ASM
+m_ceil_ASM:
+%else
+global _m_ceil_ASM
+_m_ceil_ASM:
+%endif
+    fld		val
+    fadd	rnd_adj
+    fistp	qword [temp_int64]
+    fild	qword [temp_int64]
+	ret
+
+;
+; static F64 m_ceilD_ASM(F64 val64)
+;
+%ifdef __linux
+global m_ceilD_ASM
+m_ceilD_ASM:
+%else
+global _m_ceilD_ASM
+_m_ceilD_ASM:
+%endif
+    fld		val64
+    fadd	rnd_adjD
+    fistp	qword [temp_int64]
+    fild	qword [temp_int64]
+	ret
+
+; 
+; static F32 m_floor_ASM(F32 val)
+; 
+%ifdef __linux
+global m_floor_ASM
+m_floor_ASM:
+%else
+global _m_floor_ASM
+_m_floor_ASM:
+%endif
+    fld		val
+    fsub	rnd_adj
+    fistp	qword [temp_int64]
+    fild	qword [temp_int64] 
+	ret
+
+
+;
+; static F32 m_floorD_ASM( F64 val64 )
+;
+%ifdef __linux
+global m_floorD_ASM
+m_floorD_ASM:
+%else
+global _m_floorD_ASM
+_m_floorD_ASM:
+%endif
+    fld		val64
+    fsub	rnd_adjD
+    fistp	qword [temp_int64]
+    fild	qword [temp_int64] 
+	ret
+
+
+
+%define arg_a		dword [esp+4]
+%define arg_b		dword [esp+8]
+%define arg_c		dword [esp+12]
+
+;
+; static S32 m_mulDivS32_ASM( S32 a, S32 b, S32 c )
+;
+;    // Note: this returns different (but correct) values than the C
+;    //  version.  C code must be overflowing...returns -727
+;    //  if a b and c are 1 million, for instance.  This version returns
+;    //  1 million.
+; return (S32) ((S64)a*(S64)b) / (S64)c;
+;
+%ifdef __linux
+global m_mulDivS32_ASM
+m_mulDivS32_ASM:
+%else
+global _m_mulDivS32_ASM
+_m_mulDivS32_ASM:
+%endif
+    mov     eax, arg_a
+    imul    arg_b
+    idiv    arg_c
+	ret
+
+;
+; static U32 m_mulDivU32_ASM( U32 a, U32 b, U32 c )
+;
+;    // Note: again, C version overflows
+;
+%ifdef __linux
+global m_mulDivU32_ASM
+m_mulDivU32_ASM:
+%else
+global _m_mulDivU32_ASM
+_m_mulDivU32_ASM:
+%endif
+    mov     eax, arg_a
+    mul     arg_b
+    div     arg_c
+	ret
+
+
+
+; val is already defined above to be esp+4
+%define		modulo	dword [esp+8]
+
+
+;
+; static F32 m_fmod_ASM(F32 val, F32 modulo)
+;
+%ifdef __linux
+global m_fmod_ASM
+m_fmod_ASM:
+%else
+global _m_fmod_ASM
+_m_fmod_ASM:
+%endif
+    mov     eax, val
+    fld     modulo
+    fabs
+    fld     val
+    fabs
+    fdiv    st0, st1
+    fld     st0
+    fsub	rnd_adj
+    fistp   qword [temp_int64]
+    fild    qword [temp_int64]
+    fsubp   st1, st0
+    fmulp   st1, st0
+
+;    // sign bit can be read as integer high bit, 
+;    //  as long as # isn't 0x80000000
+    cmp     eax, 0x80000000
+    jbe     notneg
+
+    fmul    dword [const_neg1]
+
+notneg:
+	ret
+
+
+%define val64hi		dword [esp+8]
+%define val64		qword [esp+4]
+%define modulo64	qword [esp+12]
+
+;
+; static F32 m_fmodD_ASM(F64 val, F64 modulo)
+;
+%ifdef __linux
+global m_fmodD_ASM
+m_fmodD_ASM:
+%else
+global _m_fmodD_ASM
+_m_fmodD_ASM:
+%endif
+    mov     eax, val64hi
+    fld     modulo64
+    fabs
+    fld     val64
+    fabs
+    fdiv    st0, st1
+    fld     st0
+    fsub	rnd_adjD
+    fistp   qword [temp_int64]
+    fild    qword [temp_int64]
+    fsubp   st1, st0
+    fmulp   st1, st0
+
+;    // sign bit can be read as integer high bit, 
+;    //  as long as # isn't 0x80000000
+    cmp     eax, 0x80000000
+    jbe     notnegD
+
+    fmul    dword [const_neg1]
+
+notnegD:
+	ret
+
+	 
+
+%define angle		dword [esp+4]
+%define res_sin		dword [esp+8]
+%define res_cos		dword [esp+12]
+
+;
+;static void m_sincos_ASM( F32 angle, F32 *s, F32 *c )
+;
+%ifdef __linux
+global m_sincos_ASM
+m_sincos_ASM:
+%else
+global _m_sincos_ASM
+_m_sincos_ASM:
+%endif
+    mov     eax, res_cos
+    fld     angle
+    fsincos
+    fstp    dword [eax]
+    mov     eax, res_sin
+    fstp    dword [eax]
+	ret
+
+
+
+%define angle64		qword [esp+4]
+%define res_sin64	dword [esp+12]
+%define res_cos64	dword [esp+16]
+;
+;static void m_sincosD_ASM( F64 angle, F64 *s, F64 *c )
+;
+%ifdef __linux
+global m_sincosD_ASM
+m_sincosD_ASM:
+%else
+global _m_sincosD_ASM
+_m_sincosD_ASM:
+%endif
+    mov     eax, res_cos64
+    fld     angle64
+    fsincos
+    fstp    qword [eax]
+    mov     eax, res_sin64
+    fstp    qword [eax]
+	ret
+
+
+

engine/math/mMath_C.cc

@@ -0,0 +1,823 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#include "math/mMath.h"
+#include <math.h>    // Caution!!! Possible platform specific include
+
+
+//------------------------------------------------------------------------------
+// C version of Math Library
+
+// asm externals
+extern "C" {
+
+   S32 m_mulDivS32_ASM( S32 a, S32 b, S32 c );
+   U32 m_mulDivU32_ASM( U32 a, U32 b, U32 c );
+   F32 m_fmod_ASM(F32 val, F32 modulo);
+   F32 m_fmodD_ASM(F64 val, F64 modulo);
+   void m_sincos_ASM( F32 angle, F32 *s, F32 *c );
+   void m_sincosD_ASM( F64 angle, F64 *s, F64 *c );
+
+}
+//--------------------------------------
+
+static S32 m_mulDivS32_C(S32 a, S32 b, S32 c)
+{
+   // S64/U64 support in most 32-bit compilers generate
+   // horrible code, the C version are here just for porting
+   // assembly implementation is recommended
+   return (S32) ((S64)a*(S64)b) / (S64)c;
+}
+
+static U32 m_mulDivU32_C(S32 a, S32 b, U32 c)
+{
+   return (U32) ((S64)a*(S64)b) / (U64)c;
+}
+
+
+//--------------------------------------
+static F32 m_catmullrom_C(F32 t, F32 p0, F32 p1, F32 p2, F32 p3)
+{
+   return 0.5f * ((3.0f*p1 - 3.0f*p2 + p3 - p0)*t*t*t
+                  +  (2.0f*p0 - 5.0f*p1 + 4.0f*p2 - p3)*t*t
+                  +  (p2-p0)*t
+                  +  2.0f*p1);
+}
+
+
+//--------------------------------------
+static void m_point2F_normalize_C(F32 *p)
+{
+   F32 factor = 1.0f / mSqrt(p[0]*p[0] + p[1]*p[1] );
+   p[0] *= factor;
+   p[1] *= factor;
+}
+
+//--------------------------------------
+static void m_point2F_normalize_f_C(F32 *p, F32 val)
+{
+   F32 factor = val / mSqrt(p[0]*p[0] + p[1]*p[1] );
+   p[0] *= factor;
+   p[1] *= factor;
+}
+
+//--------------------------------------
+static void m_point2D_normalize_C(F64 *p)
+{
+   F64 factor = 1.0f / mSqrtD(p[0]*p[0] + p[1]*p[1] );
+   p[0] *= factor;
+   p[1] *= factor;
+}
+
+//--------------------------------------
+static void m_point2D_normalize_f_C(F64 *p, F64 val)
+{
+   F64 factor = val / mSqrtD(p[0]*p[0] + p[1]*p[1] );
+   p[0] *= factor;
+   p[1] *= factor;
+}
+
+//--------------------------------------
+static void m_point3D_normalize_f_C(F64 *p, F64 val)
+{
+   F64 factor = val / mSqrtD(p[0]*p[0] + p[1]*p[1] );
+   p[0] *= factor;
+   p[1] *= factor;
+   p[2] *= factor;
+}
+
+//--------------------------------------
+static void m_point3F_normalize_C(F32 *p)
+{
+   F32 squared = p[0]*p[0] + p[1]*p[1] + p[2]*p[2];
+   // This can happen in Container::castRay -> ForceFieldBare::castRay
+   //AssertFatal(squared != 0.0, "Error, zero length vector normalized!");
+   if (squared != 0.0) {
+      F32 factor = 1.0f / mSqrt(squared);
+      p[0] *= factor;
+      p[1] *= factor;
+      p[2] *= factor;
+   } else {
+      p[0] = 0;
+      p[1] = 0;
+      p[2] = 1;
+   }
+}
+
+//--------------------------------------
+static void m_point3F_normalize_f_C(F32 *p, F32 val)
+{
+   F32 factor = val / mSqrt(p[0]*p[0] + p[1]*p[1] + p[2]*p[2] );
+   p[0] *= factor;
+   p[1] *= factor;
+   p[2] *= factor;
+}
+
+//--------------------------------------
+static void m_point3F_interpolate_C(const F32 *from, const F32 *to, F32 factor, F32 *result )
+{
+   F32 inverse = 1.0f - factor;
+   result[0] = from[0] * inverse + to[0] * factor;
+   result[1] = from[1] * inverse + to[1] * factor;
+   result[2] = from[2] * inverse + to[2] * factor;
+}
+
+//--------------------------------------
+static void m_point3D_normalize_C(F64 *p)
+{
+   F64 factor = 1.0f / mSqrtD(p[0]*p[0] + p[1]*p[1] + p[2]*p[2] );
+   p[0] *= factor;
+   p[1] *= factor;
+   p[2] *= factor;
+}
+
+
+//--------------------------------------
+static void m_point3D_interpolate_C(const F64 *from, const F64 *to, F64 factor, F64 *result )
+{
+   F64 inverse = 1.0f - factor;
+   result[0] = from[0] * inverse + to[0] * factor;
+   result[1] = from[1] * inverse + to[1] * factor;
+   result[2] = from[2] * inverse + to[2] * factor;
+}
+
+
+static void m_quatF_set_matF_C( F32 x, F32 y, F32 z, F32 w, F32* m )
+{
+#define qidx(r,c) (r*4 + c)
+      F32 xs = x * 2.0f;
+      F32 ys = y * 2.0f;
+      F32 zs = z * 2.0f;
+      F32 wx = w * xs;
+      F32 wy = w * ys;
+      F32 wz = w * zs;
+      F32 xx = x * xs;
+      F32 xy = x * ys;
+      F32 xz = x * zs;
+      F32 yy = y * ys;
+      F32 yz = y * zs;
+      F32 zz = z * zs;
+      m[qidx(0,0)] = 1.0f - (yy + zz);
+      m[qidx(1,0)] = xy - wz;
+      m[qidx(2,0)] = xz + wy;
+      m[qidx(3,0)] = 0.0f;
+      m[qidx(0,1)] = xy + wz;
+      m[qidx(1,1)] = 1.0f - (xx + zz);
+      m[qidx(2,1)] = yz - wx;
+      m[qidx(3,1)] = 0.0f;
+      m[qidx(0,2)] = xz - wy;
+      m[qidx(1,2)] = yz + wx;
+      m[qidx(2,2)] = 1.0f - (xx + yy);
+      m[qidx(3,2)] = 0.0f;
+
+      m[qidx(0,3)] = 0.0f;
+      m[qidx(1,3)] = 0.0f;
+      m[qidx(2,3)] = 0.0f;
+      m[qidx(3,3)] = 1.0f;
+#undef qidx
+}
+
+
+//--------------------------------------
+static void m_matF_set_euler_C(const F32 *e, F32 *result)
+{
+   enum {
+      AXIS_X   = (1<<0),
+      AXIS_Y   = (1<<1),
+      AXIS_Z   = (1<<2)
+   };
+
+   U32 axis = 0;
+   if (e[0] != 0.0f) axis |= AXIS_X;
+   if (e[1] != 0.0f) axis |= AXIS_Y;
+   if (e[2] != 0.0f) axis |= AXIS_Z;
+
+   switch (axis)
+   {
+      case 0:
+         m_matF_identity(result);
+         break;
+
+      case AXIS_X:
+      {
+         F32 cx,sx;
+         mSinCos( e[0], sx, cx );
+
+         result[0] = 1.0f;
+         result[1] = 0.0f;
+         result[2] = 0.0f;
+         result[3] = 0.0f;
+
+         result[4] = 0.0f;
+         result[5] = cx;
+         result[6] = sx;
+         result[7] = 0.0f;
+
+         result[8] = 0.0f;
+         result[9] = -sx;
+         result[10]= cx;
+         result[11]= 0.0f;
+
+         result[12]= 0.0f;
+         result[13]= 0.0f;
+         result[14]= 0.0f;
+         result[15]= 1.0f;
+         break;
+      }
+
+      case AXIS_Y:
+      {
+         F32 cy,sy;
+         mSinCos( e[1], sy, cy );
+
+         result[0] = cy;
+         result[1] = 0.0f;
+         result[2] = -sy;
+         result[3] = 0.0f;
+
+         result[4] = 0.0f;
+         result[5] = 1.0f;
+         result[6] = 0.0f;
+         result[7] = 0.0f;
+
+         result[8] = sy;
+         result[9] = 0.0f;
+         result[10]= cy;
+         result[11]= 0.0f;
+
+         result[12]= 0.0f;
+         result[13]= 0.0f;
+         result[14]= 0.0f;
+         result[15]= 1.0f;
+         break;
+      }
+
+      case AXIS_Z:
+      {
+         // the matrix looks like this:
+         //  r1 - (r4 * sin(x))     r2 + (r3 * sin(x))   -cos(x) * sin(y)
+         //  -cos(x) * sin(z)       cos(x) * cos(z)      sin(x)
+         //  r3 + (r2 * sin(x))     r4 - (r1 * sin(x))   cos(x) * cos(y)
+         //
+         // where:
+         //  r1 = cos(y) * cos(z)
+         //  r2 = cos(y) * sin(z)
+         //  r3 = sin(y) * cos(z)
+         //  r4 = sin(y) * sin(z)
+         F32 cz,sz;
+         mSinCos( e[2], sz, cz );
+         F32 r1 = cz;
+         F32 r2 = sz;
+         F32 r3 = 0.0f;
+         F32 r4 = 0.0f;
+
+         result[0] = cz;
+         result[1] = sz;
+         result[2] = 0.0f;
+         result[3] = 0.0f;
+
+         result[4] = -sz;
+         result[5] = cz;
+         result[6] = 0.0f;
+         result[7] = 0.0f;
+
+         result[8] = 0.0f;
+         result[9] = 0.0f;
+         result[10]= 1.0f;
+         result[11]= 0.0f;
+
+         result[12]= 0.0f;
+         result[13]= 0.0f;
+         result[14]= 0.0f;
+         result[15]= 1.0f;
+         break;
+      }
+
+      default:
+         // the matrix looks like this:
+         //  r1 - (r4 * sin(x))     r2 + (r3 * sin(x))   -cos(x) * sin(y)
+         //  -cos(x) * sin(z)       cos(x) * cos(z)      sin(x)
+         //  r3 + (r2 * sin(x))     r4 - (r1 * sin(x))   cos(x) * cos(y)
+         //
+         // where:
+         //  r1 = cos(y) * cos(z)
+         //  r2 = cos(y) * sin(z)
+         //  r3 = sin(y) * cos(z)
+         //  r4 = sin(y) * sin(z)
+         F32 cx,sx;
+         mSinCos( e[0], sx, cx );
+         F32 cy,sy;
+         mSinCos( e[1], sy, cy );
+         F32 cz,sz;
+         mSinCos( e[2], sz, cz );
+         F32 r1 = cy * cz;
+         F32 r2 = cy * sz;
+         F32 r3 = sy * cz;
+         F32 r4 = sy * sz;
+
+         result[0] = r1 - (r4 * sx);
+         result[1] = r2 + (r3 * sx);
+         result[2] = -cx * sy;
+         result[3] = 0.0f;
+
+         result[4] = -cx * sz;
+         result[5] = cx * cz;
+         result[6] = sx;
+         result[7] = 0.0f;
+
+         result[8] = r3 + (r2 * sx);
+         result[9] = r4 - (r1 * sx);
+         result[10]= cx * cy;
+         result[11]= 0.0f;
+
+         result[12]= 0.0f;
+         result[13]= 0.0f;
+         result[14]= 0.0f;
+         result[15]= 1.0f;
+         break;
+   }
+}
+
+
+//--------------------------------------
+static void m_matF_set_euler_point_C(const F32 *e, const F32 *p, F32 *result)
+{
+   m_matF_set_euler(e, result);
+   result[3] = p[0];
+   result[7] = p[1];
+   result[11]= p[2];
+}
+
+//--------------------------------------
+static void m_matF_identity_C(F32 *m)
+{
+   *m++ = 1.0f;
+   *m++ = 0.0f;
+   *m++ = 0.0f;
+   *m++ = 0.0f;
+
+   *m++ = 0.0f;
+   *m++ = 1.0f;
+   *m++ = 0.0f;
+   *m++ = 0.0f;
+
+   *m++ = 0.0f;
+   *m++ = 0.0f;
+   *m++ = 1.0f;
+   *m++ = 0.0f;
+
+   *m++ = 0.0f;
+   *m++ = 0.0f;
+   *m++ = 0.0f;
+   *m   = 1.0f;
+}
+
+
+//--------------------------------------
+static void m_matF_inverse_C(F32 *m)
+{
+   // using Cramers Rule find the Inverse
+   // Minv = (1/det(M)) * adjoint(M)
+   F32 det = m_matF_determinant( m );
+   AssertFatal( det != 0.0f, "MatrixF::inverse: non-singular matrix, no inverse.");
+
+   F32 invDet = 1.0f/det;
+   F32 temp[16];
+
+   temp[0] = (m[5] * m[10]- m[6] * m[9]) * invDet;
+   temp[1] = (m[9] * m[2] - m[10]* m[1]) * invDet;
+   temp[2] = (m[1] * m[6] - m[2] * m[5]) * invDet;
+
+   temp[4] = (m[6] * m[8] - m[4] * m[10])* invDet;
+   temp[5] = (m[10]* m[0] - m[8] * m[2]) * invDet;
+   temp[6] = (m[2] * m[4] - m[0] * m[6]) * invDet;
+
+   temp[8] = (m[4] * m[9] - m[5] * m[8]) * invDet;
+   temp[9] = (m[8] * m[1] - m[9] * m[0]) * invDet;
+   temp[10]= (m[0] * m[5] - m[1] * m[4]) * invDet;
+
+   m[0] = temp[0];
+   m[1] = temp[1];
+   m[2] = temp[2];
+
+   m[4] = temp[4];
+   m[5] = temp[5];
+   m[6] = temp[6];
+
+   m[8] = temp[8];
+   m[9] = temp[9];
+   m[10] = temp[10];
+
+   // invert the translation
+   temp[0] = -m[3];
+   temp[1] = -m[7];
+   temp[2] = -m[11];
+   m_matF_x_vectorF(m, temp, &temp[4]);
+   m[3] = temp[4];
+   m[7] = temp[5];
+   m[11]= temp[6];
+}
+
+//--------------------------------------
+static void m_matF_affineInverse_C(F32 *m)
+{
+   // Matrix class checks to make sure this is an affine transform before calling
+   //  this function, so we can proceed assuming it is...
+   F32 temp[16];
+   dMemcpy(temp, m, 16 * sizeof(F32));
+
+   // Transpose rotation
+   m[1] = temp[4];
+   m[4] = temp[1];
+   m[2] = temp[8];
+   m[8] = temp[2];
+   m[6] = temp[9];
+   m[9] = temp[6];
+
+   m[3]  = -(temp[0]*temp[3] + temp[4]*temp[7] + temp[8]*temp[11]);
+   m[7]  = -(temp[1]*temp[3] + temp[5]*temp[7] + temp[9]*temp[11]);
+   m[11] = -(temp[2]*temp[3] + temp[6]*temp[7] + temp[10]*temp[11]);
+}
+
+inline void swap(F32 &a, F32 &b)
+{
+   F32 temp = a;
+   a = b;
+   b = temp;
+}
+
+//--------------------------------------
+static void m_matF_transpose_C(F32 *m)
+{
+   swap(m[1], m[4]);
+   swap(m[2], m[8]);
+   swap(m[3], m[12]);
+   swap(m[6], m[9]);
+   swap(m[7], m[13]);
+   swap(m[11],m[14]);
+}
+
+//--------------------------------------
+static void m_matF_scale_C(F32 *m,const F32 *p)
+{
+   // Note, doesn't allow scaling w...
+
+   m[0]  *= p[0];  m[1]  *= p[1];  m[2]  *= p[2];
+   m[4]  *= p[0];  m[5]  *= p[1];  m[6]  *= p[2];
+   m[8]  *= p[0];  m[9]  *= p[1];  m[10] *= p[2];
+   m[12] *= p[0];  m[13] *= p[1];  m[14] *= p[2];
+}
+
+//--------------------------------------
+static void m_matF_normalize_C(F32 *m)
+{
+   F32 col0[3], col1[3], col2[3];
+   // extract columns 0 and 1
+   col0[0] = m[0];
+   col0[1] = m[4];
+   col0[2] = m[8];
+
+   col1[0] = m[1];
+   col1[1] = m[5];
+   col1[2] = m[9];
+
+   // assure their relationsips to one another
+   mCross(col0, col1, col2);
+   mCross(col2, col0, col1);
+
+   // assure their lengh is 1.0f
+   m_point3F_normalize( col0 );
+   m_point3F_normalize( col1 );
+   m_point3F_normalize( col2 );
+
+   // store the normalized columns
+   m[0] = col0[0];
+   m[4] = col0[1];
+   m[8] = col0[2];
+
+   m[1] = col1[0];
+   m[5] = col1[1];
+   m[9] = col1[2];
+
+   m[2] = col2[0];
+   m[6] = col2[1];
+   m[10]= col2[2];
+}
+
+
+//--------------------------------------
+static F32 m_matF_determinant_C(const F32 *m)
+{
+   return m[0] * (m[5] * m[10] - m[6] * m[9])  +
+      m[4] * (m[2] * m[9]  - m[1] * m[10]) +
+      m[8] * (m[1] * m[6]  - m[2] * m[5])  ;
+}
+
+
+//--------------------------------------
+// Removed static in order to write benchmarking code (that compares against
+// specialized SSE/AMD versions) elsewhere.
+void default_matF_x_matF_C(const F32 *a, const F32 *b, F32 *mresult)
+{
+   mresult[0] = a[0]*b[0] + a[1]*b[4] + a[2]*b[8]  + a[3]*b[12];
+   mresult[1] = a[0]*b[1] + a[1]*b[5] + a[2]*b[9]  + a[3]*b[13];
+   mresult[2] = a[0]*b[2] + a[1]*b[6] + a[2]*b[10] + a[3]*b[14];
+   mresult[3] = a[0]*b[3] + a[1]*b[7] + a[2]*b[11] + a[3]*b[15];
+
+   mresult[4] = a[4]*b[0] + a[5]*b[4] + a[6]*b[8]  + a[7]*b[12];
+   mresult[5] = a[4]*b[1] + a[5]*b[5] + a[6]*b[9]  + a[7]*b[13];
+   mresult[6] = a[4]*b[2] + a[5]*b[6] + a[6]*b[10] + a[7]*b[14];
+   mresult[7] = a[4]*b[3] + a[5]*b[7] + a[6]*b[11] + a[7]*b[15];
+
+   mresult[8] = a[8]*b[0] + a[9]*b[4] + a[10]*b[8] + a[11]*b[12];
+   mresult[9] = a[8]*b[1] + a[9]*b[5] + a[10]*b[9] + a[11]*b[13];
+   mresult[10]= a[8]*b[2] + a[9]*b[6] + a[10]*b[10]+ a[11]*b[14];
+   mresult[11]= a[8]*b[3] + a[9]*b[7] + a[10]*b[11]+ a[11]*b[15];
+
+   mresult[12]= a[12]*b[0]+ a[13]*b[4]+ a[14]*b[8] + a[15]*b[12];
+   mresult[13]= a[12]*b[1]+ a[13]*b[5]+ a[14]*b[9] + a[15]*b[13];
+   mresult[14]= a[12]*b[2]+ a[13]*b[6]+ a[14]*b[10]+ a[15]*b[14];
+   mresult[15]= a[12]*b[3]+ a[13]*b[7]+ a[14]*b[11]+ a[15]*b[15];
+}
+
+
+// //--------------------------------------
+// static void m_matF_x_point3F_C(const F32 *m, const F32 *p, F32 *presult)
+// {
+//    AssertFatal(p != presult, "Error, aliasing matrix mul pointers not allowed here!");
+//    presult[0] = m[0]*p[0] + m[1]*p[1] + m[2]*p[2]  + m[3];
+//    presult[1] = m[4]*p[0] + m[5]*p[1] + m[6]*p[2]  + m[7];
+//    presult[2] = m[8]*p[0] + m[9]*p[1] + m[10]*p[2] + m[11];
+// }
+
+
+// //--------------------------------------
+// static void m_matF_x_vectorF_C(const F32 *m, const F32 *v, F32 *vresult)
+// {
+//    AssertFatal(v != vresult, "Error, aliasing matrix mul pointers not allowed here!");
+//    vresult[0] = m[0]*v[0] + m[1]*v[1] + m[2]*v[2];
+//    vresult[1] = m[4]*v[0] + m[5]*v[1] + m[6]*v[2];
+//    vresult[2] = m[8]*v[0] + m[9]*v[1] + m[10]*v[2];
+// }
+
+
+//--------------------------------------
+static void m_matF_x_point4F_C(const F32 *m, const F32 *p, F32 *presult)
+{
+   AssertFatal(p != presult, "Error, aliasing matrix mul pointers not allowed here!");
+   presult[0] = m[0]*p[0] + m[1]*p[1] + m[2]*p[2]  + m[3]*p[3];
+   presult[1] = m[4]*p[0] + m[5]*p[1] + m[6]*p[2]  + m[7]*p[3];
+   presult[2] = m[8]*p[0] + m[9]*p[1] + m[10]*p[2] + m[11]*p[3];
+   presult[3] = m[12]*p[0]+ m[13]*p[1]+ m[14]*p[2] + m[15]*p[3];
+}
+
+//--------------------------------------
+static void m_matF_x_scale_x_planeF_C(const F32* m, const F32* s, const F32* p, F32* presult)
+{
+   // We take in a matrix, a scale factor, and a plane equation.  We want to output
+   //  the resultant normal
+   // We have T = m*s
+   // To multiply the normal, we want Inv(Tr(m*s))
+   //  Inv(Tr(ms)) = Inv(Tr(s) * Tr(m))
+   //              = Inv(Tr(m)) * Inv(Tr(s))
+   //
+   //  Inv(Tr(s)) = Inv(s) = [ 1/x   0   0  0]
+   //                        [   0 1/y   0  0]
+   //                        [   0   0 1/z  0]
+   //                        [   0   0   0  1]
+   //
+   // Since m is an affine matrix,
+   //  Tr(m) = [ [       ] 0 ]
+   //          [ [   R   ] 0 ]
+   //          [ [       ] 0 ]
+   //          [ [ x y z ] 1 ]
+   //
+   // Inv(Tr(m)) = [ [    -1 ] 0 ]
+   //              [ [   R   ] 0 ]
+   //              [ [       ] 0 ]
+   //              [ [ A B C ] 1 ]
+   // Where:
+   //
+   //  P = (x, y, z)
+   //  A = -(Row(0, r) * P);
+   //  B = -(Row(1, r) * P);
+   //  C = -(Row(2, r) * P);
+
+   MatrixF invScale(true);
+   F32* pScaleElems = invScale;
+   pScaleElems[MatrixF::idx(0, 0)] = 1.0 / s[0];
+   pScaleElems[MatrixF::idx(1, 1)] = 1.0 / s[1];
+   pScaleElems[MatrixF::idx(2, 2)] = 1.0 / s[2];
+
+   Point3F shear;
+   shear.x = m[MatrixF::idx(3, 0)];
+   shear.y = m[MatrixF::idx(3, 1)];
+   shear.z = m[MatrixF::idx(3, 2)];
+
+   Point3F row0(m[MatrixF::idx(0, 0)], m[MatrixF::idx(0, 1)], m[MatrixF::idx(0, 2)]);
+   Point3F row1(m[MatrixF::idx(1, 0)], m[MatrixF::idx(1, 1)], m[MatrixF::idx(1, 2)]);
+   Point3F row2(m[MatrixF::idx(2, 0)], m[MatrixF::idx(2, 1)], m[MatrixF::idx(2, 2)]);
+
+   F32 A = -mDot(row0, shear);
+   F32 B = -mDot(row1, shear);
+   F32 C = -mDot(row2, shear);
+
+   MatrixF invTrMatrix(true);
+   F32* destMat = invTrMatrix;
+   destMat[MatrixF::idx(0, 0)] = m[MatrixF::idx(0, 0)];
+   destMat[MatrixF::idx(1, 0)] = m[MatrixF::idx(1, 0)];
+   destMat[MatrixF::idx(2, 0)] = m[MatrixF::idx(2, 0)];
+   destMat[MatrixF::idx(0, 1)] = m[MatrixF::idx(0, 1)];
+   destMat[MatrixF::idx(1, 1)] = m[MatrixF::idx(1, 1)];
+   destMat[MatrixF::idx(2, 1)] = m[MatrixF::idx(2, 1)];
+   destMat[MatrixF::idx(0, 2)] = m[MatrixF::idx(0, 2)];
+   destMat[MatrixF::idx(1, 2)] = m[MatrixF::idx(1, 2)];
+   destMat[MatrixF::idx(2, 2)] = m[MatrixF::idx(2, 2)];
+   destMat[MatrixF::idx(0, 3)] = A;
+   destMat[MatrixF::idx(1, 3)] = B;
+   destMat[MatrixF::idx(2, 3)] = C;
+   invTrMatrix.mul(invScale);
+
+   Point3F norm(p[0], p[1], p[2]);
+   Point3F point = norm * -p[3];
+   invTrMatrix.mulP(norm);
+   norm.normalize();
+
+   MatrixF temp;
+   dMemcpy(temp, m, sizeof(F32) * 16);
+   point.x *= s[0];
+   point.y *= s[1];
+   point.z *= s[2];
+   temp.mulP(point);
+
+   PlaneF resultPlane(point, norm);
+   presult[0] = resultPlane.x;
+   presult[1] = resultPlane.y;
+   presult[2] = resultPlane.z;
+   presult[3] = resultPlane.d;
+}
+
+static void m_matF_x_box3F_C(const F32 *m, F32* min, F32* max)
+{
+   // Algorithm for axis aligned bounding box adapted from
+   //  Graphic Gems I, pp 548-550
+   //
+   F32 originalMin[3];
+   F32 originalMax[3];
+   originalMin[0] = min[0];
+   originalMin[1] = min[1];
+   originalMin[2] = min[2];
+   originalMax[0] = max[0];
+   originalMax[1] = max[1];
+   originalMax[2] = max[2];
+
+   min[0] = max[0] = m[3];
+   min[1] = max[1] = m[7];
+   min[2] = max[2] = m[11];
+
+   const F32 * row = &m[0];
+   for (U32 i = 0; i < 3; i++)
+   {
+      #define  Do_One_Row(j)   {                         \
+         F32    a = (row[j] * originalMin[j]);           \
+         F32    b = (row[j] * originalMax[j]);           \
+         if (a < b) { *min += a;  *max += b; }           \
+         else       { *min += b;  *max += a; }     }
+
+      // Simpler addressing (avoiding things like [ecx+edi*4]) might be worthwhile (LH):
+      Do_One_Row(0);
+      Do_One_Row(1);
+      Do_One_Row(2);
+      row += 4;
+      min++;
+      max++;
+   }
+}
+
+
+void m_point3F_bulk_dot_C(const F32* refVector,
+                          const F32* dotPoints,
+                          const U32  numPoints,
+                          const U32  pointStride,
+                          F32*       output)
+{
+   for (U32 i = 0; i < numPoints; i++)
+   {
+      F32* pPoint = (F32*)(((U8*)dotPoints) + (pointStride * i));
+      output[i] = ((refVector[0] * pPoint[0]) +
+                   (refVector[1] * pPoint[1]) +
+                   (refVector[2] * pPoint[2]));
+   }
+}
+
+void m_point3F_bulk_dot_indexed_C(const F32* refVector,
+                                  const F32* dotPoints,
+                                  const U32  numPoints,
+                                  const U32  pointStride,
+                                  const U32* pointIndices,
+                                  F32*       output)
+{
+   for (U32 i = 0; i < numPoints; i++)
+   {
+      F32* pPoint = (F32*)(((U8*)dotPoints) + (pointStride * pointIndices[i]));
+      output[i] = ((refVector[0] * pPoint[0]) +
+                   (refVector[1] * pPoint[1]) +
+                   (refVector[2] * pPoint[2]));
+   }
+}
+
+
+//------------------------------------------------------------------------------
+// Math function pointer declarations
+
+S32  (*m_mulDivS32)(S32 a, S32 b, S32 c) = m_mulDivS32_C;
+U32  (*m_mulDivU32)(S32 a, S32 b, U32 c) = m_mulDivU32_C;
+
+F32  (*m_catmullrom)(F32 t, F32 p0, F32 p1, F32 p2, F32 p3) = m_catmullrom_C;
+
+void (*m_point2F_normalize)(F32 *p) = m_point2F_normalize_C;
+void (*m_point2F_normalize_f)(F32 *p, F32 val) = m_point2F_normalize_f_C;
+void (*m_point2D_normalize)(F64 *p) = m_point2D_normalize_C;
+void (*m_point2D_normalize_f)(F64 *p, F64 val) = m_point2D_normalize_f_C;
+void (*m_point3D_normalize_f)(F64 *p, F64 val) = m_point3D_normalize_f_C;
+void (*m_point3F_normalize)(F32 *p) = m_point3F_normalize_C;
+void (*m_point3F_normalize_f)(F32 *p, F32 len) = m_point3F_normalize_f_C;
+void (*m_point3F_interpolate)(const F32 *from, const F32 *to, F32 factor, F32 *result) = m_point3F_interpolate_C;
+
+void (*m_point3D_normalize)(F64 *p) = m_point3D_normalize_C;
+void (*m_point3D_interpolate)(const F64 *from, const F64 *to, F64 factor, F64 *result) = m_point3D_interpolate_C;
+
+void (*m_point3F_bulk_dot)(const F32* refVector,
+                           const F32* dotPoints,
+                           const U32  numPoints,
+                           const U32  pointStride,
+                           F32*       output) = m_point3F_bulk_dot_C;
+void (*m_point3F_bulk_dot_indexed)(const F32* refVector,
+                                   const F32* dotPoints,
+                                   const U32  numPoints,
+                                   const U32  pointStride,
+                                   const U32* pointIndices,
+                                   F32*       output) = m_point3F_bulk_dot_indexed_C;
+
+void (*m_quatF_set_matF)( F32 x, F32 y, F32 z, F32 w, F32* m ) = m_quatF_set_matF_C;
+
+void (*m_matF_set_euler)(const F32 *e, F32 *result) = m_matF_set_euler_C;
+void (*m_matF_set_euler_point)(const F32 *e, const F32 *p, F32 *result) = m_matF_set_euler_point_C;
+void (*m_matF_identity)(F32 *m)  = m_matF_identity_C;
+void (*m_matF_inverse)(F32 *m)   = m_matF_inverse_C;
+void (*m_matF_affineInverse)(F32 *m)   = m_matF_affineInverse_C;
+void (*m_matF_transpose)(F32 *m) = m_matF_transpose_C;
+void (*m_matF_scale)(F32 *m,const F32* p) = m_matF_scale_C;
+void (*m_matF_normalize)(F32 *m) = m_matF_normalize_C;
+F32  (*m_matF_determinant)(const F32 *m) = m_matF_determinant_C;
+void (*m_matF_x_matF)(const F32 *a, const F32 *b, F32 *mresult)    = default_matF_x_matF_C;
+// void (*m_matF_x_point3F)(const F32 *m, const F32 *p, F32 *presult) = m_matF_x_point3F_C;
+// void (*m_matF_x_vectorF)(const F32 *m, const F32 *v, F32 *vresult) = m_matF_x_vectorF_C;
+void (*m_matF_x_point4F)(const F32 *m, const F32 *p, F32 *presult) = m_matF_x_point4F_C;
+void (*m_matF_x_scale_x_planeF)(const F32 *m, const F32* s, const F32 *p, F32 *presult) = m_matF_x_scale_x_planeF_C;
+void (*m_matF_x_box3F)(const F32 *m, F32 *min, F32 *max)    = m_matF_x_box3F_C;
+
+
+//------------------------------------------------------------------------------
+void mInstallLibrary_C()
+{
+   m_mulDivS32             = m_mulDivS32_C;
+   m_mulDivU32             = m_mulDivU32_C;
+
+   m_catmullrom            = m_catmullrom_C;
+
+   m_point2F_normalize     = m_point2F_normalize_C;
+   m_point2F_normalize_f   = m_point2F_normalize_f_C;
+   m_point2D_normalize     = m_point2D_normalize_C;
+   m_point2D_normalize_f   = m_point2D_normalize_f_C;
+   m_point3F_normalize     = m_point3F_normalize_C;
+   m_point3F_normalize_f   = m_point3F_normalize_f_C;
+   m_point3F_interpolate   = m_point3F_interpolate_C;
+
+   m_point3D_normalize     = m_point3D_normalize_C;
+   m_point3D_interpolate   = m_point3D_interpolate_C;
+
+   m_point3F_bulk_dot      = m_point3F_bulk_dot_C;
+   m_point3F_bulk_dot_indexed = m_point3F_bulk_dot_indexed_C;
+
+   m_quatF_set_matF        = m_quatF_set_matF_C;
+
+   m_matF_set_euler        = m_matF_set_euler_C;
+   m_matF_set_euler_point  = m_matF_set_euler_point_C;
+   m_matF_identity         = m_matF_identity_C;
+   m_matF_inverse          = m_matF_inverse_C;
+   m_matF_affineInverse    = m_matF_affineInverse_C;
+   m_matF_transpose        = m_matF_transpose_C;
+   m_matF_scale            = m_matF_scale_C;
+   m_matF_normalize        = m_matF_normalize_C;
+   m_matF_determinant      = m_matF_determinant_C;
+   m_matF_x_matF           = default_matF_x_matF_C;
+//    m_matF_x_point3F        = m_matF_x_point3F_C;
+//    m_matF_x_vectorF        = m_matF_x_vectorF_C;
+   m_matF_x_point4F        = m_matF_x_point4F_C;
+   m_matF_x_scale_x_planeF = m_matF_x_scale_x_planeF_C;
+   m_matF_x_box3F          = m_matF_x_box3F_C;
+}
+

engine/math/mMatrix.cc

@@ -0,0 +1,174 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#include "math/mMath.h"
+#include "math/mMatrix.h"
+#include "console/console.h"
+#include "core/frameAllocator.h"
+
+// idx(i,j) is index to element in column i, row j
+
+void MatrixF::transposeTo(F32 *matrix) const
+{
+   matrix[idx(0,0)] = m[idx(0,0)];
+   matrix[idx(0,1)] = m[idx(1,0)];
+   matrix[idx(0,2)] = m[idx(2,0)];
+   matrix[idx(0,3)] = m[idx(3,0)];
+   matrix[idx(1,0)] = m[idx(0,1)];
+   matrix[idx(1,1)] = m[idx(1,1)];
+   matrix[idx(1,2)] = m[idx(2,1)];
+   matrix[idx(1,3)] = m[idx(3,1)];
+   matrix[idx(2,0)] = m[idx(0,2)];
+   matrix[idx(2,1)] = m[idx(1,2)];
+   matrix[idx(2,2)] = m[idx(2,2)];
+   matrix[idx(2,3)] = m[idx(3,2)];
+   matrix[idx(3,0)] = m[idx(0,3)];
+   matrix[idx(3,1)] = m[idx(1,3)];
+   matrix[idx(3,2)] = m[idx(2,3)];
+   matrix[idx(3,3)] = m[idx(3,3)];
+}
+
+bool MatrixF::isAffine() const
+{
+   // An affine transform is defined by the following structure
+   //
+   // [ X X X P ]
+   // [ X X X P ]
+   // [ X X X P ]
+   // [ 0 0 0 1 ]
+   //
+   //  Where X is an orthonormal 3x3 submatrix and P is an arbitrary translation
+   //  We'll check in the following order:
+   //   1: [3][3] must be 1
+   //   2: Shear portion must be zero
+   //   3: Dot products of rows and columns must be zero
+   //   4: Length of rows and columns must be 1
+   //
+   if (m[idx(3,3)] != 1.0f)
+      return false;
+
+   if (m[idx(0,3)] != 0.0f ||
+       m[idx(1,3)] != 0.0f ||
+       m[idx(2,3)] != 0.0f)
+      return false;
+
+   Point3F one, two, three;
+   getColumn(0, &one);
+   getColumn(1, &two);
+   getColumn(2, &three);
+   if (mDot(one, two)   > 0.0001f ||
+       mDot(one, three) > 0.0001f ||
+       mDot(two, three) > 0.0001f)
+      return false;
+
+   if (mFabs(1.0f - one.lenSquared()) > 0.0001f ||
+       mFabs(1.0f - two.lenSquared()) > 0.0001f ||
+       mFabs(1.0f - three.lenSquared()) > 0.0001f)
+      return false;
+
+   getRow(0, &one);
+   getRow(1, &two);
+   getRow(2, &three);
+   if (mDot(one, two)   > 0.0001f ||
+       mDot(one, three) > 0.0001f ||
+       mDot(two, three) > 0.0001f)
+      return false;
+
+   if (mFabs(1.0f - one.lenSquared()) > 0.0001f ||
+       mFabs(1.0f - two.lenSquared()) > 0.0001f ||
+       mFabs(1.0f - three.lenSquared()) > 0.0001f)
+      return false;
+
+   // We're ok.
+   return true;
+}
+
+// Perform inverse on full 4x4 matrix.  Used in special cases only, so not at all optimized.
+bool MatrixF::fullInverse()
+{
+   Point4F a,b,c,d;
+   getRow(0,&a);
+   getRow(1,&b);
+   getRow(2,&c);
+   getRow(3,&d);
+
+   // det = a0*b1*c2*d3 - a0*b1*c3*d2 - a0*c1*b2*d3 + a0*c1*b3*d2 + a0*d1*b2*c3 - a0*d1*b3*c2 -
+   //       b0*a1*c2*d3 + b0*a1*c3*d2 + b0*c1*a2*d3 - b0*c1*a3*d2 - b0*d1*a2*c3 + b0*d1*a3*c2 +
+   //       c0*a1*b2*d3 - c0*a1*b3*d2 - c0*b1*a2*d3 + c0*b1*a3*d2 + c0*d1*a2*b3 - c0*d1*a3*b2 -
+   //       d0*a1*b2*c3 + d0*a1*b3*c2 + d0*b1*a2*c3 - d0*b1*a3*c2 - d0*c1*a2*b3 + d0*c1*a3*b2
+   F32 det = a.x*b.y*c.z*d.w - a.x*b.y*c.w*d.z - a.x*c.y*b.z*d.w + a.x*c.y*b.w*d.z + a.x*d.y*b.z*c.w - a.x*d.y*b.w*c.z
+           - b.x*a.y*c.z*d.w + b.x*a.y*c.w*d.z + b.x*c.y*a.z*d.w - b.x*c.y*a.w*d.z - b.x*d.y*a.z*c.w + b.x*d.y*a.w*c.z
+           + c.x*a.y*b.z*d.w - c.x*a.y*b.w*d.z - c.x*b.y*a.z*d.w + c.x*b.y*a.w*d.z + c.x*d.y*a.z*b.w - c.x*d.y*a.w*b.z
+           - d.x*a.y*b.z*c.w + d.x*a.y*b.w*c.z + d.x*b.y*a.z*c.w - d.x*b.y*a.w*c.z - d.x*c.y*a.z*b.w + d.x*c.y*a.w*b.z;
+
+   if (mFabs(det)<0.00001f)
+      return false;
+
+   Point4F aa,bb,cc,dd;
+   aa.x =  b.y*c.z*d.w - b.y*c.w*d.z - c.y*b.z*d.w + c.y*b.w*d.z + d.y*b.z*c.w - d.y*b.w*c.z;
+   aa.y = -a.y*c.z*d.w + a.y*c.w*d.z + c.y*a.z*d.w - c.y*a.w*d.z - d.y*a.z*c.w + d.y*a.w*c.z;
+   aa.z =  a.y*b.z*d.w - a.y*b.w*d.z - b.y*a.z*d.w + b.y*a.w*d.z + d.y*a.z*b.w - d.y*a.w*b.z;
+   aa.w = -a.y*b.z*c.w + a.y*b.w*c.z + b.y*a.z*c.w - b.y*a.w*c.z - c.y*a.z*b.w + c.y*a.w*b.z;
+
+   bb.x = -b.x*c.z*d.w + b.x*c.w*d.z + c.x*b.z*d.w - c.x*b.w*d.z - d.x*b.z*c.w + d.x*b.w*c.z;
+   bb.y =  a.x*c.z*d.w - a.x*c.w*d.z - c.x*a.z*d.w + c.x*a.w*d.z + d.x*a.z*c.w - d.x*a.w*c.z;
+   bb.z = -a.x*b.z*d.w + a.x*b.w*d.z + b.x*a.z*d.w - b.x*a.w*d.z - d.x*a.z*b.w + d.x*a.w*b.z;
+   bb.w =  a.x*b.z*c.w - a.x*b.w*c.z - b.x*a.z*c.w + b.x*a.w*c.z + c.x*a.z*b.w - c.x*a.w*b.z;
+
+   cc.x =  b.x*c.y*d.w - b.x*c.w*d.y - c.x*b.y*d.w + c.x*b.w*d.y + d.x*b.y*c.w - d.x*b.w*c.y;
+   cc.y = -a.x*c.y*d.w + a.x*c.w*d.y + c.x*a.y*d.w - c.x*a.w*d.y - d.x*a.y*c.w + d.x*a.w*c.y;
+   cc.z =  a.x*b.y*d.w - a.x*b.w*d.y - b.x*a.y*d.w + b.x*a.w*d.y + d.x*a.y*b.w - d.x*a.w*b.y;
+   cc.w = -a.x*b.y*c.w + a.x*b.w*c.y + b.x*a.y*c.w - b.x*a.w*c.y - c.x*a.y*b.w + c.x*a.w*b.y;
+
+   dd.x = -b.x*c.y*d.z + b.x*c.z*d.y + c.x*b.y*d.z - c.x*b.z*d.y - d.x*b.y*c.z + d.x*b.z*c.y;
+   dd.y =  a.x*c.y*d.z - a.x*c.z*d.y - c.x*a.y*d.z + c.x*a.z*d.y + d.x*a.y*c.z - d.x*a.z*c.y;
+   dd.z = -a.x*b.y*d.z + a.x*b.z*d.y + b.x*a.y*d.z - b.x*a.z*d.y - d.x*a.y*b.z + d.x*a.z*b.y;
+   dd.w =  a.x*b.y*c.z - a.x*b.z*c.y - b.x*a.y*c.z + b.x*a.z*c.y + c.x*a.y*b.z - c.x*a.z*b.y;
+
+   setRow(0,aa);
+   setRow(1,bb);
+   setRow(2,cc);
+   setRow(3,dd);
+
+   mul(1.0f/det);
+
+   return true;
+}
+
+EulerF MatrixF::toEuler() const
+{
+   const F32 * mat = m;
+
+   EulerF r;
+   r.x = mAsin(mat[MatrixF::idx(2,1)]);
+
+   if(mCos(r.x) != 0.f)
+   {
+      r.y = mAtan(-mat[MatrixF::idx(2,0)], mat[MatrixF::idx(2,2)]);
+      r.z = mAtan(-mat[MatrixF::idx(0,1)], mat[MatrixF::idx(1,1)]);
+   }
+   else
+   {
+      r.y = 0.f;
+      r.z = mAtan(mat[MatrixF::idx(1,0)], mat[MatrixF::idx(0,0)]);
+   }
+
+   return r;
+}
+
+void MatrixF::dumpMatrix(const char *caption /* =NULL */) const
+{
+   U32 size = dStrlen(caption);
+   FrameTemp<char> spacer(size+1);
+   char *spacerRef = spacer;
+
+   dMemset(spacerRef, ' ', size);
+   spacerRef[size] = 0;
+
+   Con::printf("%s = | %-8.4f %-8.4f %-8.4f %-8.4f |", caption,    m[idx(0,0)], m[idx(0, 1)], m[idx(0, 2)], m[idx(0, 3)]);
+   Con::printf("%s   | %-8.4f %-8.4f %-8.4f %-8.4f |", spacerRef,  m[idx(1,0)], m[idx(1, 1)], m[idx(1, 2)], m[idx(1, 3)]);
+   Con::printf("%s   | %-8.4f %-8.4f %-8.4f %-8.4f |", spacerRef,  m[idx(2,0)], m[idx(2, 1)], m[idx(2, 2)], m[idx(2, 3)]);
+   Con::printf("%s   | %-8.4f %-8.4f %-8.4f %-8.4f |", spacerRef,  m[idx(3,0)], m[idx(3, 1)], m[idx(3, 2)], m[idx(3, 3)]);
+}

engine/math/mMatrix.h

@@ -0,0 +1,434 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#ifndef _MMATRIX_H_
+#define _MMATRIX_H_
+
+
+#ifndef _MMATH_H_
+#include "math/mMath.h"
+#endif
+
+/// 4x4 Matrix Class
+///
+/// This runs at F32 precision.
+class MatrixF
+{
+private:
+   F32 m[16];     ///< Note: this is stored in ROW MAJOR format.  OpenGL is
+                  ///  COLUMN MAJOR.  Transpose before sending down.
+
+public:
+   /// Create an uninitialized matrix.
+   ///
+   /// @param   identity    If true, initialize to the identity matrix.
+   explicit MatrixF(bool identity=false);
+
+   /// Create a matrix to rotate about origin by e.
+   /// @see set
+   explicit MatrixF( const EulerF &e);
+
+   /// Create a matrix to rotate about p by e.
+   /// @see set
+   MatrixF( const EulerF &e, const Point3F& p);
+
+   /// Get the index in m to element in column i, row j
+   ///
+   /// This is necessary as we have m as a one dimensional array.
+   ///
+   /// @param   i   Column desired.
+   /// @param   j   Row desired.
+   static U32 idx(U32 i, U32 j) { return (i + j*4); }
+
+   /// Initialize matrix to rotate about origin by e.
+   MatrixF& set( const EulerF &e);
+
+   /// Initialize matrix to rotate about p by e.
+   MatrixF& set( const EulerF &e, const Point3F& p);
+
+   /// Initialize matrix with a cross product of p.
+   MatrixF& setCrossProduct( const Point3F &p);
+
+   /// Initialize matrix with a tensor product of p.
+   MatrixF& setTensorProduct( const Point3F &p, const Point3F& q);
+
+   operator F32*() { return (m); }              ///< Allow people to get at m.
+   operator F32*() const { return (F32*)(m); }  ///< Allow people to get at m.
+
+   bool isAffine() const;                       ///< Check to see if this is an affine matrix.
+   bool isIdentity() const;                     ///< Checks for identity matrix.
+
+   /// Make this an identity matrix.
+   MatrixF& identity();
+
+   /// Invert m.
+   MatrixF& inverse();
+
+   /// Take inverse without disturbing position data.
+   ///
+   /// Ie, take inverse of 3x3 submatrix.
+   MatrixF& affineInverse();
+   MatrixF& transpose();                        ///< Swap rows and columns.
+   MatrixF& scale(const Point3F& p);            ///< M * Matrix(p) -> M
+
+   EulerF toEuler() const;
+
+   /// Compute the inverse of the matrix.
+   ///
+   /// Computes inverse of full 4x4 matrix. Returns false and performs no inverse if
+   /// the determinant is 0.
+   ///
+   /// Note: In most cases you want to use the normal inverse function.  This method should
+   ///       be used if the matrix has something other than (0,0,0,1) in the bottom row.
+   bool fullInverse();
+
+   /// Swaps rows and columns into matrix.
+   void transposeTo(F32 *matrix) const;
+
+   /// Normalize the matrix.
+   void normalize();
+
+   /// Copy the requested column into a Point4F.
+   void getColumn(S32 col, Point4F *cptr) const;
+
+   /// Copy the requested column into a Point3F.
+   ///
+   /// This drops the bottom-most row.
+   void getColumn(S32 col, Point3F *cptr) const;
+
+   /// Set the specified column from a Point4F.
+   void setColumn(S32 col, const Point4F& cptr);
+
+   /// Set the specified column from a Point3F.
+   ///
+   /// The bottom-most row is not set.
+   void setColumn(S32 col, const Point3F& cptr);
+
+   /// Copy the specified row into a Point4F.
+   void getRow(S32 row, Point4F *cptr) const;
+
+   /// Copy the specified row into a Point3F.
+   ///
+   /// Right-most item is dropped.
+   void getRow(S32 row, Point3F *cptr) const;
+
+   /// Set the specified row from a Point4F.
+   void setRow(S32 row, const Point4F& cptr);
+
+   /// Set the specified row from a Point3F.
+   ///
+   /// The right-most item is not set.
+   void setRow(S32 row, const Point3F& cptr);
+
+   /// Get the position of the matrix.
+   ///
+   /// This is the 4th column of the matrix.
+   Point3F   getPosition() const;
+
+   /// Set the position of the matrix.
+   ///
+   /// This is the 4th column of the matrix.
+   void      setPosition( const Point3F &pos ){ setColumn( 3, pos ); }
+
+   MatrixF&  mul(const MatrixF &a);                    ///< M * a -> M
+   MatrixF&  mul(const MatrixF &a, const MatrixF &b);  ///< a * b -> M
+
+   // Scalar multiplies
+   MatrixF&  mul(const F32 a);                         ///< M * a -> M
+   MatrixF&  mul(const MatrixF &a, const F32 b);       ///< a * b -> M
+
+
+   void mul( Point4F& p ) const;                       ///< M * p -> p (full [4x4] * [1x4])
+   void mulP( Point3F& p ) const;                      ///< M * p -> p (assume w = 1.0f)
+   void mulP( const Point3F &p, Point3F *d) const;     ///< M * p -> d (assume w = 1.0f)
+   void mulV( VectorF& p ) const;                      ///< M * v -> v (assume w = 0.0f)
+   void mulV( const VectorF &p, Point3F *d) const;     ///< M * v -> d (assume w = 0.0f)
+
+   void mul(Box3F& b) const;                           ///< Axial box -> Axial Box
+
+   /// Convenience function to allow people to treat this like an array.
+   F32& operator ()(S32 row, S32 col) { return m[idx(col,row)]; }
+
+   void dumpMatrix(const char *caption=NULL) const;
+};
+
+//--------------------------------------
+// Inline Functions
+
+inline MatrixF::MatrixF(bool _identity)
+{
+   if (_identity)
+      identity();
+}
+
+inline MatrixF::MatrixF( const EulerF &e )
+{
+   set(e);
+}
+
+inline MatrixF::MatrixF( const EulerF &e, const Point3F& p )
+{
+   set(e,p);
+}
+
+inline MatrixF& MatrixF::set( const EulerF &e)
+{
+   m_matF_set_euler( e, *this );
+   return (*this);
+}
+
+
+inline MatrixF& MatrixF::set( const EulerF &e, const Point3F& p)
+{
+   m_matF_set_euler_point( e, p, *this );
+   return (*this);
+}
+
+inline MatrixF& MatrixF::setCrossProduct( const Point3F &p)
+{
+   m[1] = -(m[4] = p.z);
+   m[8] = -(m[2] = p.y);
+   m[6] = -(m[9] = p.x);
+   m[0] = m[3] = m[5] = m[7] = m[10] = m[11] =
+      m[12] = m[13] = m[14] = 0;
+   m[15] = 1;
+   return (*this);
+}
+
+inline MatrixF& MatrixF::setTensorProduct( const Point3F &p, const Point3F &q)
+{
+   m[0] = p.x * q.x;
+   m[1] = p.x * q.y;
+   m[2] = p.x * q.z;
+   m[4] = p.y * q.x;
+   m[5] = p.y * q.y;
+   m[6] = p.y * q.z;
+   m[8] = p.z * q.x;
+   m[9] = p.z * q.y;
+   m[10] = p.z * q.z;
+   m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0;
+   m[15] = 1;
+   return (*this);
+}
+
+inline bool MatrixF::isIdentity() const
+{
+   return
+   m[0]  == 1.0f &&
+   m[1]  == 0.0f &&
+   m[2]  == 0.0f &&
+   m[3]  == 0.0f &&
+   m[4]  == 0.0f &&
+   m[5]  == 1.0f &&
+   m[6]  == 0.0f &&
+   m[7]  == 0.0f &&
+   m[8]  == 0.0f &&
+   m[9]  == 0.0f &&
+   m[10] == 1.0f &&
+   m[11] == 0.0f &&
+   m[12] == 0.0f &&
+   m[13] == 0.0f &&
+   m[14] == 0.0f &&
+   m[15] == 1.0f;
+}
+
+inline MatrixF& MatrixF::identity()
+{
+   m[0]  = 1.0f;
+   m[1]  = 0.0f;
+   m[2]  = 0.0f;
+   m[3]  = 0.0f;
+   m[4]  = 0.0f;
+   m[5]  = 1.0f;
+   m[6]  = 0.0f;
+   m[7]  = 0.0f;
+   m[8]  = 0.0f;
+   m[9]  = 0.0f;
+   m[10] = 1.0f;
+   m[11] = 0.0f;
+   m[12] = 0.0f;
+   m[13] = 0.0f;
+   m[14] = 0.0f;
+   m[15] = 1.0f;
+   return (*this);
+}
+
+
+inline MatrixF& MatrixF::inverse()
+{
+   m_matF_inverse(m);
+   return (*this);
+}
+
+inline MatrixF& MatrixF::affineInverse()
+{
+//   AssertFatal(isAffine() == true, "Error, this matrix is not an affine transform");
+   m_matF_affineInverse(m);
+   return (*this);
+}
+
+inline MatrixF& MatrixF::transpose()
+{
+   m_matF_transpose(m);
+   return (*this);
+}
+
+inline MatrixF& MatrixF::scale(const Point3F& p)
+{
+   m_matF_scale(m,p);
+   return *this;
+}
+
+inline void MatrixF::normalize()
+{
+   m_matF_normalize(m);
+}
+
+inline MatrixF& MatrixF::mul( const MatrixF &a )
+{  // M * a -> M
+   MatrixF tempThis(*this);
+   m_matF_x_matF(tempThis, a, *this);
+   return (*this);
+}
+
+
+inline MatrixF& MatrixF::mul( const MatrixF &a, const MatrixF &b )
+{  // a * b -> M
+   m_matF_x_matF(a, b, *this);
+   return (*this);
+}
+
+
+inline MatrixF& MatrixF::mul(const F32 a)
+{
+   for (U32 i = 0; i < 16; i++)
+      m[i] *= a;
+
+   return *this;
+}
+
+
+inline MatrixF& MatrixF::mul(const MatrixF &a, const F32 b)
+{
+   *this = a;
+   mul(b);
+
+   return *this;
+}
+
+inline void MatrixF::mul( Point4F& p ) const
+{
+   Point4F temp;
+   m_matF_x_point4F(*this, &p.x, &temp.x);
+   p = temp;
+}
+
+inline void MatrixF::mulP( Point3F& p) const
+{
+   // M * p -> d
+   Point3F d;
+   m_matF_x_point3F(*this, &p.x, &d.x);
+   p = d;
+}
+
+inline void MatrixF::mulP( const Point3F &p, Point3F *d) const
+{
+   // M * p -> d
+   m_matF_x_point3F(*this, &p.x, &d->x);
+}
+
+inline void MatrixF::mulV( VectorF& v) const
+{
+   // M * v -> v
+   VectorF temp;
+   m_matF_x_vectorF(*this, &v.x, &temp.x);
+   v = temp;
+}
+
+inline void MatrixF::mulV( const VectorF &v, Point3F *d) const
+{
+   // M * v -> d
+   m_matF_x_vectorF(*this, &v.x, &d->x);
+}
+
+inline void MatrixF::mul(Box3F& b) const
+{
+   m_matF_x_box3F(*this, &b.min.x, &b.max.x);
+}
+
+inline void MatrixF::getColumn(S32 col, Point4F *cptr) const
+{
+   cptr->x = m[col];
+   cptr->y = m[col+4];
+   cptr->z = m[col+8];
+   cptr->w = m[col+12];
+}
+
+inline void MatrixF::getColumn(S32 col, Point3F *cptr) const
+{
+   cptr->x = m[col];
+   cptr->y = m[col+4];
+   cptr->z = m[col+8];
+}
+
+inline void MatrixF::setColumn(S32 col, const Point4F &cptr)
+{
+   m[col]   = cptr.x;
+   m[col+4] = cptr.y;
+   m[col+8] = cptr.z;
+   m[col+12]= cptr.w;
+}
+
+inline void MatrixF::setColumn(S32 col, const Point3F &cptr)
+{
+   m[col]   = cptr.x;
+   m[col+4] = cptr.y;
+   m[col+8] = cptr.z;
+}
+
+
+inline void MatrixF::getRow(S32 col, Point4F *cptr) const
+{
+   col *= 4;
+   cptr->x = m[col++];
+   cptr->y = m[col++];
+   cptr->z = m[col++];
+   cptr->w = m[col];
+}
+
+inline void MatrixF::getRow(S32 col, Point3F *cptr) const
+{
+   col *= 4;
+   cptr->x = m[col++];
+   cptr->y = m[col++];
+   cptr->z = m[col];
+}
+
+inline void MatrixF::setRow(S32 col, const Point4F &cptr)
+{
+   col *= 4;
+   m[col++] = cptr.x;
+   m[col++] = cptr.y;
+   m[col++] = cptr.z;
+   m[col]   = cptr.w;
+}
+
+inline void MatrixF::setRow(S32 col, const Point3F &cptr)
+{
+   col *= 4;
+   m[col++] = cptr.x;
+   m[col++] = cptr.y;
+   m[col]   = cptr.z;
+}
+
+// not too speedy, but convienient
+inline Point3F MatrixF::getPosition() const
+{
+   Point3F pos;
+   getColumn( 3, &pos );
+   return pos;
+}
+
+#endif //_MMATRIX_H_

engine/math/mPlane.h

@@ -0,0 +1,297 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#ifndef _MPLANE_H_
+#define _MPLANE_H_
+
+#ifndef _MMATHFN_H_
+#include "math/mMathFn.h"
+#endif
+#ifndef _MPOINT_H_
+#include "math/mPoint.h"
+#endif
+
+//---------------------------------------------------------------------------
+
+class PlaneF: public Point3F
+{
+public:
+   F32 d;
+
+   PlaneF();
+   PlaneF( const Point3F& p, const Point3F& n );
+   PlaneF( F32 _x, F32 _y, F32 _z, F32 _d);
+   PlaneF( const Point3F& j, const Point3F& k, const Point3F& l );
+
+   // Comparison operators
+   bool operator==(const PlaneF&a) const
+   {
+      return (a.x == x && a.y == y && a.z == z && a.d == d);
+   }
+
+   bool operator!=(const PlaneF&a) const
+   {
+      return (a.x != x || a.y != y || a.z != z || a.d == d);
+   }
+
+   // Methods
+   void set(const F32 _x, const F32 _y, const F32 _z);
+
+   void     set( const Point3F& p, const Point3F& n);
+   void     set( const Point3F& k, const Point3F& j, const Point3F& l );
+   void     setPoint(const Point3F &p); // assumes the x,y,z fields are already set
+                                        // creates an un-normalized plane
+
+   void     setXY(F32 zz);
+   void     setYZ(F32 xx);
+   void     setXZ(F32 yy);
+   void     setXY(const Point3F& P, F32 dir);
+   void     setYZ(const Point3F& P, F32 dir);
+   void     setXZ(const Point3F& P, F32 dir);
+   void     shiftX(F32 xx);
+   void     shiftY(F32 yy);
+   void     shiftZ(F32 zz);
+   void     invert();
+   void     neg();
+   Point3F  project(Point3F pt); // project's the point onto the plane.
+
+   F32      distToPlane( const Point3F& cp ) const;
+
+   enum Side
+   {
+      Front = 1,
+      On    = 0,
+      Back  = -1
+   };
+
+   Side whichSide(const Point3F& cp) const;
+   F32  intersect(const Point3F &start, const Point3F &end) const;
+
+   bool     isHorizontal() const;
+   bool     isVertical() const;
+
+   Side whichSideBox(const Point3F& center,
+                     const Point3F& axisx,
+                     const Point3F& axisy,
+                     const Point3F& axisz,
+                     const Point3F& offset) const;
+};
+#define PARALLEL_PLANE  1e20f
+
+#define PlaneSwitchCode(s, e) (s * 3 + e)
+
+
+//---------------------------------------------------------------------------
+
+inline PlaneF::PlaneF()
+{
+}
+
+inline PlaneF::
+   PlaneF( F32 _x, F32 _y, F32 _z, F32 _d )
+{
+   x = _x; y = _y; z = _z; d = _d;
+}
+
+inline PlaneF::PlaneF( const Point3F& p, const Point3F& n )
+{
+   set(p,n);
+}
+
+inline PlaneF::PlaneF( const Point3F& j, const Point3F& k, const Point3F& l )
+{
+   set(j,k,l);
+}
+
+inline void PlaneF::setXY( F32 zz )
+{
+   x = y = 0; z = 1; d = -zz;
+}
+
+inline void PlaneF::setYZ( F32 xx )
+{
+   x = 1; z = y = 0; d = -xx;
+}
+
+inline void PlaneF::setXZ( F32 yy )
+{
+   x = z = 0; y = 1; d = -yy;
+}
+
+inline void PlaneF::setXY(const Point3F& point, F32 dir)       // Normal = (0, 0, -1|1)
+{
+   x = y = 0;
+   d = -((z = dir) * point.z);
+}
+
+inline void PlaneF::setYZ(const Point3F& point, F32 dir)       // Normal = (-1|1, 0, 0)
+{
+   z = y = 0;
+   d = -((x = dir) * point.x);
+}
+
+inline void PlaneF::setXZ(const Point3F& point, F32 dir)       // Normal = (0, -1|1, 0)
+{
+   x = z = 0;
+   d = -((y = dir) * point.y);
+}
+
+inline void PlaneF::shiftX( F32 xx )
+{
+   d -= xx * x;
+}
+
+inline void PlaneF::shiftY( F32 yy )
+{
+   d -= yy * y;
+}
+
+inline void PlaneF::shiftZ( F32 zz )
+{
+   d -= zz * z;
+}
+
+inline bool PlaneF::isHorizontal() const
+{
+   return (x == 0 && y == 0) ? true : false;
+}
+
+inline bool PlaneF::isVertical() const
+{
+    return ((x != 0 || y != 0) && z == 0) ? true : false;
+}
+
+inline Point3F PlaneF::project(Point3F pt)
+{
+   F32 dist = distToPlane(pt);
+   return Point3F(pt.x - x * dist, pt.y - y * dist, pt.z - z * dist);
+}
+
+inline F32 PlaneF::distToPlane( const Point3F& cp ) const
+{
+   // return mDot(*this,cp) + d;
+   return (x * cp.x + y * cp.y + z * cp.z) + d;
+}
+
+inline PlaneF::Side PlaneF::whichSide(const Point3F& cp) const
+{
+   F32 dist = distToPlane(cp);
+   if (dist >= 0.005f)                 // if (mFabs(dist) < 0.005f)
+      return Front;                    //    return On;
+   else if (dist <= -0.005f)           // else if (dist > 0.0f)
+      return Back;                     //    return Front;
+   else                                // else
+      return On;                       //    return Back;
+}
+
+inline void PlaneF::set(const F32 _x, const F32 _y, const F32 _z)
+{
+    Point3F::set(_x,_y,_z);
+}
+
+//---------------------------------------------------------------------------
+/// Calculate the coefficients of the plane passing through
+/// a point with the given normal.
+inline void PlaneF::setPoint(const Point3F &p)
+{
+   d = -(p.x * x + p.y * y + p.z * z);
+}
+
+inline void PlaneF::set( const Point3F& p, const Point3F& n )
+{
+   x = n.x; y = n.y; z = n.z;
+   normalize();
+
+   // Calculate the last plane coefficient.
+   d = -(p.x * x + p.y * y + p.z * z);
+}
+
+//---------------------------------------------------------------------------
+/// Calculate the coefficients of the plane passing through
+/// three points.  Basically it calculates the normal to the three
+/// points then calculates a plane through the middle point with that
+/// normal.
+inline void PlaneF::set( const Point3F& k, const Point3F& j, const Point3F& l )
+{
+//   Point3F kj,lj,pv;
+//   kj = k;
+//   kj -= j;
+//   lj = l;
+//   lj -= j;
+//   mCross( kj, lj, &pv );
+//   set(j,pv);
+
+// Above ends up making function calls up the %*#...
+// This is called often enough to be a little more direct
+// about it (sqrt should become intrinsic in the future)...
+   F32 ax = k.x-j.x;
+   F32 ay = k.y-j.y;
+   F32 az = k.z-j.z;
+   F32 bx = l.x-j.x;
+   F32 by = l.y-j.y;
+   F32 bz = l.z-j.z;
+   x = ay*bz - az*by;
+   y = az*bx - ax*bz;
+   z = ax*by - ay*bx;
+   F32 squared = x*x + y*y + z*z;
+   AssertFatal(squared != 0.0, "Error, no plane possible!");
+
+   // In non-debug mode
+   if (squared != 0) {
+      F32 invSqrt = 1.0f / mSqrt(x*x + y*y + z*z);
+      x *= invSqrt;
+      y *= invSqrt;
+      z *= invSqrt;
+      d = -(j.x * x + j.y * y + j.z * z);
+   } else {
+      x = 0;
+      y = 0;
+      z = 1;
+      d = -(j.x * x + j.y * y + j.z * z);
+   }
+}
+
+inline void PlaneF::invert()
+{
+   x = -x;
+   y = -y;
+   z = -z;
+   d = -d;
+}
+
+inline void PlaneF::neg()
+{
+   invert();
+}
+
+inline F32 PlaneF::intersect(const Point3F &p1, const Point3F &p2) const
+{
+   F32 den = mDot(p2 - p1, *this);
+   if(den == 0)
+      return PARALLEL_PLANE;
+   return -distToPlane(p1) / den;
+}
+
+inline PlaneF::Side PlaneF::whichSideBox(const Point3F& center,
+                                         const Point3F& axisx,
+                                         const Point3F& axisy,
+                                         const Point3F& axisz,
+                                         const Point3F& /*offset*/) const
+{
+   F32 baseDist = distToPlane(center);
+
+   F32 compDist = mFabs(mDot(axisx, *this)) +
+                  mFabs(mDot(axisy, *this)) +
+                  mFabs(mDot(axisz, *this));
+
+   if (baseDist >= compDist)
+      return Front;
+   else if (baseDist <= -compDist)
+      return Back;
+   else
+      return On;
+}
+
+#endif

engine/math/mPlaneTransformer.cc

@@ -0,0 +1,52 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#include "math/mPlaneTransformer.h"
+#include "math/mMathFn.h"
+
+void PlaneTransformer::set(const MatrixF& xform, const Point3F& scale)
+{
+   mTransform = xform;
+   mScale     = scale;
+
+   MatrixF scaleMat(true);
+   F32* m = scaleMat;
+   m[MatrixF::idx(0, 0)] = scale.x;
+   m[MatrixF::idx(1, 1)] = scale.y;
+   m[MatrixF::idx(2, 2)] = scale.z;
+
+   mTransposeInverse = xform;
+   mTransposeInverse.mul(scaleMat);
+   mTransposeInverse.transpose();
+   mTransposeInverse.inverse();
+}
+
+void PlaneTransformer::transform(const PlaneF& plane, PlaneF& result)
+{
+   Point3F point = plane;
+   point *= -plane.d;
+   point.convolve(mScale);
+   mTransform.mulP(point);
+
+   Point3F normal = plane;
+   mTransposeInverse.mulV(normal);
+
+   result.set(point, normal);
+//   mTransformPlane(mTransform, mScale, plane, &result);
+}
+
+void PlaneTransformer::setIdentity()
+{
+   static struct MakeIdentity {
+      PlaneTransformer  transformer;
+      MakeIdentity() {
+         MatrixF  defMat(true);
+         Point3F  defScale(1, 1, 1);
+         transformer.set(defMat, defScale);
+      }
+   } sMakeIdentity;
+
+   * this = sMakeIdentity.transformer;
+}

engine/math/mPlaneTransformer.h

@@ -0,0 +1,33 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#ifndef _MPLANETRANSFORMER_H_
+#define _MPLANETRANSFORMER_H_
+
+#ifndef _MMATRIX_H_
+#include "math/mMatrix.h"
+#endif
+#ifndef _MPOINT_H_
+#include "math/mPoint.h"
+#endif
+#ifndef _MPLANE_H_
+#include "math/mPlane.h"
+#endif
+
+// =========================================================
+class PlaneTransformer
+{
+   MatrixF mTransform;
+   Point3F mScale;
+
+   MatrixF mTransposeInverse;
+
+  public:
+   void set(const MatrixF& xform, const Point3F& scale);
+   void transform(const PlaneF& plane, PlaneF& result);
+   void setIdentity();
+};
+
+#endif

engine/math/mQuadPatch.cc

@@ -0,0 +1,59 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#include "math/mQuadPatch.h"
+
+
+//******************************************************************************
+// Quadratic spline patch
+//******************************************************************************
+QuadPatch::QuadPatch()
+{
+   setNumReqControlPoints(3);
+}
+
+//------------------------------------------------------------------------------
+//------------------------------------------------------------------------------
+void QuadPatch::calcABC( const Point3F *points )
+{
+   a = points[2] - points[1];
+   b = points[1] - points[0];
+   c = points[0];
+}
+
+
+//------------------------------------------------------------------------------
+//------------------------------------------------------------------------------
+void QuadPatch::submitControlPoints( SplCtrlPts &points )
+{
+   Parent::submitControlPoints( points );
+   calcABC( points.getPoint(0) );
+};
+
+
+//------------------------------------------------------------------------------
+//------------------------------------------------------------------------------
+void QuadPatch::setControlPoint( Point3F &point, int index )
+{
+   ( (SplCtrlPts*) getControlPoints() )->setPoint( point, index );
+   calcABC( getControlPoint(0) );
+}
+
+//------------------------------------------------------------------------------
+//------------------------------------------------------------------------------
+void QuadPatch::calc( F32 t, Point3F &result )
+{
+   F32 t2 = t*t;
+   result = a*t2 + b*t + c;
+}
+
+
+//------------------------------------------------------------------------------
+//------------------------------------------------------------------------------
+void QuadPatch::calc( Point3F *points, F32 t, Point3F &result )
+{
+   calcABC( points );
+   calc( t, result );
+}

engine/math/mQuadPatch.h

@@ -0,0 +1,45 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#ifndef _MQUADPATCH_H_
+#define _MQUADPATCH_H_
+
+#ifndef _PLATFORM_H_
+#include "platform/platform.h"
+#endif
+#ifndef _MPOINT_H_
+#include "math/mPoint.h"
+#endif
+#ifndef _MSPLINEPATCH_H_
+#include "math/mSplinePatch.h"
+#endif
+
+//------------------------------------------------------------------------------
+/// Quadratic spline patch.  This is a special type of spline that only had 3 control points.
+/// @see SplinePatch
+class QuadPatch : public SplinePatch
+{
+   typedef SplinePatch Parent;
+
+private:
+   Point3F a, b, c;
+
+   void calcABC( const Point3F *points );
+
+public:
+
+   QuadPatch();
+
+   virtual void   calc( F32 t, Point3F &result );
+   virtual void   calc( Point3F *points, F32 t, Point3F &result );
+   virtual void   setControlPoint( Point3F &point, int index );
+   virtual void   submitControlPoints( SplCtrlPts &points );
+
+
+};
+
+
+
+#endif

engine/math/mQuat.cc

@@ -0,0 +1,345 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#include "math/mQuat.h"
+#include "math/mMatrix.h"
+
+QuatF& QuatF::set( const EulerF & e )
+{
+   F32 cx, sx;
+   F32 cy, sy;
+   F32 cz, sz;
+   mSinCos( -e.x * F32(0.5), sx, cx );
+   mSinCos( -e.y * F32(0.5), sy, cy );
+   mSinCos( -e.z * F32(0.5), sz, cz );
+
+   // Qyaw(z)   = [ (0, 0, sin z/2), cos z/2 ]
+   // Qpitch(x) = [ (sin x/2, 0, 0), cos x/2 ]
+   // Qroll(y)  = [ (0, sin y/2, 0), cos y/2 ]
+   // this = Qresult = Qyaw*Qpitch*Qroll  ZXY
+   //
+   // The code that folows is a simplification of:
+   //    roll*=pitch;
+   //    roll*=yaw;
+   //    *this = roll;
+   F32 cycz, sysz, sycz, cysz;
+   cycz = cy*cz;
+   sysz = sy*sz;
+   sycz = sy*cz;
+   cysz = cy*sz;
+   w = cycz*cx + sysz*sx;
+   x = cycz*sx + sysz*cx;
+   y = sycz*cx - cysz*sx;
+   z = cysz*cx - sycz*sx;
+
+   return *this;
+}
+
+AngAxisF & AngAxisF::set( const QuatF & q )
+{
+   angle = mAcos( q.w ) * 2;
+   F32 sinHalfAngle = mSqrt(1 - q.w * q.w);
+   if (sinHalfAngle != 0)
+   	axis.set( q.x / sinHalfAngle, q.y / sinHalfAngle, q.z / sinHalfAngle );
+   else
+      axis.set(1,0,0);
+   return *this;
+}
+
+AngAxisF & AngAxisF::set( const MatrixF & mat )
+{
+   QuatF q( mat );
+   set( q );
+   return *this;
+}
+
+MatrixF * AngAxisF::setMatrix( MatrixF * mat ) const
+{
+   QuatF q( *this );
+   return q.setMatrix( mat );
+}
+
+QuatF& QuatF::operator *=( const QuatF & b )
+{
+   QuatF prod;
+   prod.w = w * b.w - x * b.x - y * b.y - z * b.z;
+   prod.x = w * b.x + x * b.w + y * b.z - z * b.y;
+   prod.y = w * b.y + y * b.w + z * b.x - x * b.z;
+   prod.z = w * b.z + z * b.w + x * b.y - y * b.x;
+   *this = prod;
+   return (*this);
+}
+
+QuatF& QuatF::operator /=( const QuatF & c )
+{
+   QuatF temp = c;
+   return ( (*this) *= temp.inverse() );
+}
+
+QuatF& QuatF::operator +=( const QuatF & c )
+{
+   x += c.x;
+   y += c.y;
+   z += c.z;
+   w += c.w;
+   return *this;
+}
+
+QuatF& QuatF::operator -=( const QuatF & c )
+{
+   x -= c.x;
+   y -= c.y;
+   z -= c.z;
+   w -= c.w;
+   return *this;
+}
+
+QuatF& QuatF::operator *=( F32 a )
+{
+   x *= a;
+   y *= a;
+   z *= a;
+   w *= a;
+   return *this;
+}
+
+QuatF& QuatF::operator /=( F32 a )
+{
+   x /= a;
+   y /= a;
+   z /= a;
+   w /= a;
+   return *this;
+}
+
+QuatF& QuatF::square()
+{
+   F32 t = w*2.0f;
+   w = (w*w) - (x*x + y*y + z*z);
+   x *= t;
+   y *= t;
+   z *= t;
+   return *this;
+}
+
+QuatF& QuatF::inverse()
+{
+   F32 magnitude = w*w + x*x + y*y + z*z;
+   F32 invMagnitude;
+   if( magnitude == 1.0f )    // special case unit quaternion
+   {
+      x = -x;
+      y = -y;
+      z = -z;
+   }
+   else                       // else scale
+   {
+      if( magnitude == 0.0f )
+         invMagnitude = 1.0f;
+      else
+         invMagnitude = 1.0f / magnitude;
+      w *= invMagnitude;
+      x *= -invMagnitude;
+      y *= -invMagnitude;
+      z *= -invMagnitude;
+   }
+	return *this;
+}
+
+QuatF& QuatF::set( const AngAxisF & a )
+{
+   F32 sinHalfAngle, cosHalfAngle;
+   mSinCos( a.angle * F32(0.5), sinHalfAngle, cosHalfAngle );
+   x = a.axis.x * sinHalfAngle;
+   y = a.axis.y * sinHalfAngle;
+   z = a.axis.z * sinHalfAngle;
+   w = cosHalfAngle;
+   return *this;
+}
+
+QuatF & QuatF::normalize()
+{
+   F32 l = mSqrt( x*x + y*y + z*z + w*w );
+   if( l == F32(0.0) )
+      identity();
+   else
+   {
+      x /= l;
+      y /= l;
+      z /= l;
+      w /= l;
+   }
+   return *this;
+}
+
+#define idx(r,c) (r*4 + c)
+
+QuatF& QuatF::set( const MatrixF & mat )
+{
+   F32 const *m = mat;
+
+   F32 trace = m[idx(0, 0)] + m[idx(1, 1)] + m[idx(2, 2)];
+   if (trace > 0.0) {
+      F32 s = mSqrt(trace + F32(1));
+      w = s * 0.5;
+      s = 0.5 / s;
+      x = (m[idx(1,2)] - m[idx(2,1)]) * s;
+      y = (m[idx(2,0)] - m[idx(0,2)]) * s;
+      z = (m[idx(0,1)] - m[idx(1,0)]) * s;
+   } else {
+      F32* q = &x;
+      U32 i = 0;
+      if (m[idx(1, 1)] > m[idx(0, 0)]) i = 1;
+      if (m[idx(2, 2)] > m[idx(i, i)]) i = 2;
+      U32 j = (i + 1) % 3;
+      U32 k = (j + 1) % 3;
+
+      F32 s = mSqrt((m[idx(i, i)] - (m[idx(j, j)] + m[idx(k, k)])) + 1.0);
+      q[i] = s * 0.5;
+      s = 0.5 / s;
+      q[j] = (m[idx(i,j)] + m[idx(j,i)]) * s;
+      q[k] = (m[idx(i,k)] + m[idx(k, i)]) * s;
+      w = (m[idx(j,k)] - m[idx(k, j)]) * s;
+   }
+
+   return *this;
+}
+
+MatrixF * QuatF::setMatrix( MatrixF * mat ) const
+{
+   if( x*x + y*y + z*z < 10E-20f) // isIdentity() -- substituted code a little more stringent but a lot faster
+      mat->identity();
+   else
+      m_quatF_set_matF( x, y, z, w, *mat );
+   return mat;
+}
+
+QuatF & QuatF::slerp( const QuatF & q, F32 t )
+{
+   return interpolate( *this, q, t );
+}
+
+QuatF & QuatF::extrapolate( const QuatF & q1, const QuatF & q2, F32 t )
+{
+   // assert t >= 0 && t <= 1
+   // q1 is value at time = 0
+   // q2 is value at time = t
+   // Computes quaternion at time = 1
+   F64 flip,cos = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
+   if (cos < 0) {
+      cos = -cos;
+      flip = -1;
+   }
+   else
+      flip = 1;
+
+   F64 s1,s2;
+   if ((1.0 - cos) > 0.00001) {
+      F64 om = mAcos(cos) / t;
+      F64 sd = 1 / mSin(t * om);
+      s1 = flip * mSin(om) * sd;
+      s2 = mSin((1 - t) * om) * sd;
+   }
+   else {
+      // If quats are very close, do linear interpolation
+      s1 = flip / t;
+      s2 = (1 - t) / t;
+   }
+
+   x = F32(s1 * q2.x - s2 * q1.x);
+   y = F32(s1 * q2.y - s2 * q1.y);
+   z = F32(s1 * q2.z - s2 * q1.z);
+   w = F32(s1 * q2.w - s2 * q1.w);
+
+   return *this;
+}
+
+QuatF & QuatF::interpolate( const QuatF & q1, const QuatF & q2, F32 t )
+{
+   //-----------------------------------
+   // Calculate the cosine of the angle:
+
+   double cosOmega = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
+
+   //-----------------------------------
+   // adjust signs if necessary:
+
+   F32 sign2;
+   if ( cosOmega < 0.0 )
+   {
+      cosOmega = -cosOmega;
+      sign2 = -1.0f;
+   }
+   else
+      sign2 = 1.0f;
+
+   //-----------------------------------
+   // calculate interpolating coeffs:
+
+   double scale1, scale2;
+   if ( (1.0 - cosOmega) > 0.00001 )
+   {
+      // standard case
+      double omega = mAcos(cosOmega);
+      double sinOmega = mSin(omega);
+      scale1 = mSin((1.0 - t) * omega) / sinOmega;
+      scale2 = sign2 * mSin(t * omega) / sinOmega;
+   }
+   else
+   {
+      // if quats are very close, just do linear interpolation
+      scale1 = 1.0 - t;
+      scale2 = sign2 * t;
+   }
+
+
+   //-----------------------------------
+   // actually do the interpolation:
+
+   x = F32(scale1 * q1.x + scale2 * q2.x);
+   y = F32(scale1 * q1.y + scale2 * q2.y);
+   z = F32(scale1 * q1.z + scale2 * q2.z);
+   w = F32(scale1 * q1.w + scale2 * q2.w);
+   return *this;
+}
+
+Point3F& QuatF::mulP(const Point3F& p, Point3F* r)
+{
+   QuatF qq;
+   QuatF qi = *this;
+   QuatF qv( p.x, p.y, p.z, 0);
+
+   qi.inverse();
+   qq.mul(qi, qv);
+   qv.mul(qq, *this);
+   r->set(qv.x, qv.y, qv.z);
+   return *r;
+}
+
+QuatF& QuatF::mul( const QuatF &a, const QuatF &b)
+{
+	AssertFatal( &a != this && &b != this, "QuatF::mul: dest should not be same as source" );
+   w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
+   x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y;
+   y = a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z;
+   z = a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x;
+   return *this;
+}
+
+Point3F& TQuatF::mulP(const Point3F& pt, Point3F* r)
+{
+   QuatF a;
+   QuatF i = *this;
+   QuatF v( pt.x, pt.y, pt.z, 0.0f);
+   i.inverse();
+   a.mul(i, v);
+   v.mul(a, *this);
+   v.normalize();
+   r->set(v.x, v.y, v.z);
+
+   *r += p;
+   return ( *r );
+}
+

engine/math/mQuat.h

@@ -0,0 +1,329 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#ifndef _MQUAT_H_
+#define _MQUAT_H_
+
+#ifndef _MMATH_H_
+#include "math/mMath.h"
+#endif
+
+class MatrixF;
+class QuatF;
+
+inline F32 QuatIsEqual(F32 a,F32 b,F32 epsilon = 0.0001f)
+{
+   return mFabs(a-b) < epsilon;
+}
+
+inline F32 QuatIsZero(F32 a,F32 epsilon = 0.0001f)
+{
+   return mFabs(a) < epsilon;
+}
+
+//----------------------------------------------------------------------------
+// rotation about an arbitrary axis through the origin:
+
+class AngAxisF
+{
+  public:
+   Point3F axis;
+   F32  angle;
+
+   AngAxisF();
+   AngAxisF( const Point3F & _axis, F32 _angle );
+   explicit AngAxisF( const MatrixF &m );
+   explicit AngAxisF( const QuatF &q );
+
+   AngAxisF& set( const Point3F & _axis, F32 _angle );
+   AngAxisF& set( const MatrixF & m );
+   AngAxisF& set( const QuatF & q );
+
+   int operator ==( const AngAxisF & c ) const;
+   int operator !=( const AngAxisF & c ) const;
+
+   MatrixF * setMatrix( MatrixF * mat ) const;
+};
+
+//----------------------------------------------------------------------------
+// unit quaternion class:
+
+class QuatF
+{
+  public:
+   F32  x,y,z,w;
+
+   QuatF();
+   QuatF( F32 _x, F32 _y, F32 _z, F32 w );
+   QuatF( const MatrixF & m );
+   QuatF( const AngAxisF & a );
+   QuatF( const EulerF & e );
+
+   QuatF& set( F32 _x, F32 _y, F32 _z, F32 _w );
+   QuatF& set( const MatrixF & m );
+   QuatF& set( const AngAxisF & m );
+   QuatF& set( const EulerF & e );
+
+   int operator ==( const QuatF & c ) const;
+   int operator !=( const QuatF & c ) const;
+   QuatF& operator *=( const QuatF & c );
+   QuatF& operator /=( const QuatF & c );
+   QuatF& operator +=( const QuatF & c );
+   QuatF& operator -=( const QuatF & c );
+   QuatF& operator *=( F32 a );
+   QuatF& operator /=( F32 a );
+
+   QuatF& square();
+   QuatF& neg();
+   F32  dot( const QuatF &q ) const;
+
+   MatrixF* setMatrix( MatrixF * mat ) const;
+   QuatF& normalize();
+   QuatF& inverse();
+   QuatF& identity();
+   int    isIdentity() const;
+   QuatF& slerp( const QuatF & q, F32 t );
+   QuatF& extrapolate( const QuatF & q1, const QuatF & q2, F32 t );
+   QuatF& interpolate( const QuatF & q1, const QuatF & q2, F32 t );
+   F32  angleBetween( const QuatF & q );
+
+   Point3F& mulP(const Point3F& a, Point3F* b);   // r = p * this
+   QuatF& mul(const QuatF& a, const QuatF& b);    // This = a * b
+};
+
+
+//----------------------------------------------------------------------------
+// AngAxisF implementation:
+
+inline AngAxisF::AngAxisF()
+{
+}
+
+inline AngAxisF::AngAxisF( const Point3F & _axis, F32 _angle )
+{
+   set(_axis,_angle);
+}
+
+inline AngAxisF::AngAxisF( const MatrixF & mat )
+{
+   set(mat);
+}
+
+inline AngAxisF::AngAxisF( const QuatF & quat )
+{
+   set(quat);
+}
+
+inline AngAxisF& AngAxisF::set( const Point3F & _axis, F32 _angle )
+{
+   axis = _axis;
+   angle = _angle;
+   return *this;
+}
+
+inline int AngAxisF::operator ==( const AngAxisF & c ) const
+{
+   return QuatIsEqual(angle, c.angle) && (axis == c.axis);
+}
+
+inline int AngAxisF::operator !=( const AngAxisF & c ) const
+{
+   return !QuatIsEqual(angle, c.angle) || (axis != c.axis);
+}
+
+//----------------------------------------------------------------------------
+// quaternion implementation:
+
+inline QuatF::QuatF()
+{
+}
+
+inline QuatF::QuatF( F32 _x, F32 _y, F32 _z, F32 _w )
+{
+   set( _x, _y, _z, _w );
+}
+
+inline QuatF::QuatF( const AngAxisF & a )
+{
+   set( a );
+}
+
+inline QuatF::QuatF( const EulerF & e )
+{
+   set(e);
+}
+
+inline QuatF& QuatF::set( F32 _x, F32 _y, F32 _z, F32 _w )
+{
+   x = _x;
+   y = _y;
+   z = _z;
+   w = _w;
+   return *this;
+}
+
+inline int QuatF::operator ==( const QuatF & c ) const
+{
+   QuatF a = *this;
+   QuatF b = c;
+   a.normalize();
+   b.normalize();
+   b.inverse();
+   a *= b;
+   return a.isIdentity();
+}
+
+inline int QuatF::isIdentity() const
+{
+   return QuatIsZero( x ) && QuatIsZero( y ) && QuatIsZero( z );
+}
+
+inline QuatF& QuatF::identity()
+{
+   x = 0.0f;
+   y = 0.0f;
+   z = 0.0f;
+   w = 1.0f;
+   return *this;
+}
+
+inline int QuatF::operator !=( const QuatF & c ) const
+{
+   return ! operator==( c );
+}
+
+inline QuatF::QuatF( const MatrixF & m )
+{
+   set( m );
+}
+
+
+inline QuatF& QuatF::neg()
+{
+   x = -x;
+   y = -y;
+   z = -z;
+   w = -w;
+   return *this;
+}
+
+inline F32 QuatF::dot( const QuatF &q ) const
+{
+   return (w*q.w + x*q.x + y*q.y + z*q.z);
+}
+
+
+inline F32 QuatF::angleBetween( const QuatF & q )
+{
+   // angle between to quaternions
+   return mAcos(x * q.x + y * q.y + z * q.z + w * q.w);
+}
+
+
+//----------------------------------------------------------------------------
+// TQuatF classes:
+
+class TQuatF : public QuatF
+{
+  public:
+   enum
+   {
+      Matrix_HasRotation = 1,
+      Matrix_HasTranslation = 2,
+      Matrix_HasScale = 4
+   };
+
+   Point3F p;
+   U32 flags;
+
+   TQuatF();
+   TQuatF( bool ident );
+   TQuatF( const EulerF & e, const Point3F & p );
+   TQuatF( const AngAxisF & aa, const Point3F & p );
+   TQuatF( const QuatF & q, const Point3F & p );
+
+   TQuatF& set( const EulerF & euler, const Point3F & p );
+   TQuatF& set( const AngAxisF & aa, const Point3F & p );
+   TQuatF& set( const QuatF & quat, const Point3F & p );
+
+   TQuatF& inverse( void );
+   TQuatF& identity( void );
+
+   Point3F& mulP(const Point3F& p, Point3F* r);   // r = p * this
+};
+
+//---------------------------------------------------------------------------
+
+inline TQuatF::TQuatF()
+{
+   // Beware: no initialization is done!
+}
+
+inline TQuatF::TQuatF( bool ident )
+{
+   if( ident )
+      identity();
+}
+
+inline TQuatF::TQuatF( const EulerF & euler, const Point3F & p )
+{
+   set( euler, p );
+}
+
+inline TQuatF::TQuatF( const AngAxisF & aa, const Point3F & p )
+{
+   set( aa, p );
+}
+
+inline TQuatF::TQuatF( const QuatF & quat, const Point3F & p )
+{
+   set( quat, p );
+}
+
+inline TQuatF& TQuatF::set( const EulerF & e, const Point3F & t )
+{
+   p = t;
+   QuatF::set( e );
+   flags |= Matrix_HasTranslation;
+   return *this;
+}
+
+inline TQuatF& TQuatF::set( const AngAxisF & aa, const Point3F & t )
+{
+   p = t;
+   QuatF::set( aa );
+   flags |= Matrix_HasTranslation;
+   return *this;
+}
+
+inline TQuatF& TQuatF::set( const QuatF & q, const Point3F & t )
+{
+   p = t;
+   QuatF::set( q.x, q.y, q.z, q.w );
+   flags |= Matrix_HasTranslation;
+   return *this;
+}
+
+inline TQuatF& TQuatF::inverse( void )
+{
+   QuatF::inverse();
+   if( flags & Matrix_HasTranslation )
+   {
+      Point3F p2 = p;
+      p2.neg();
+      QuatF::mulP(p2,&p);
+   }
+   return *this;
+}
+
+inline TQuatF& TQuatF::identity( void )
+{
+   QuatF::identity();
+   p.set(0,0,0);
+   flags &= ~U32(Matrix_HasTranslation);
+   return *this;
+}
+
+#endif

engine/math/mRandom.cc

@@ -0,0 +1,155 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#include "platform/platform.h"
+#include "platform/event.h"
+#include "platform/gameInterface.h"
+#include "math/mRandom.h"
+
+MRandomLCG gRandGen;
+U32 gRandGenSeed = 1376312589;
+
+void MRandomLCG::setGlobalRandSeed(U32 seed)
+{
+	if (Game->isJournalReading())
+		Game->journalRead(&gRandGenSeed);
+	else
+	{
+		gRandGenSeed = seed;
+		if (Game->isJournalWriting())
+			Game->journalWrite(gRandGenSeed);
+	}
+
+	//now actually set the seed
+	gRandGen.setSeed(gRandGenSeed);
+}
+
+static U32 msSeed = 1376312589;
+
+inline U32 generateSeed()
+{
+   // A very, VERY crude LCG but good enough to generate
+   // a nice range of seed values
+   msSeed = (msSeed * 0x015a4e35L) + 1;
+   msSeed = (msSeed>>16)&0x7fff;
+   return (msSeed);
+}
+
+//--------------------------------------
+void MRandomGenerator::setSeed()
+{
+   setSeed(generateSeed());
+}
+
+
+//--------------------------------------
+const S32 MRandomLCG::msQuotient  = S32_MAX / 16807L;
+const S32 MRandomLCG::msRemainder = S32_MAX % 16807L;
+
+
+//--------------------------------------
+MRandomLCG::MRandomLCG()
+{
+   setSeed(generateSeed());
+}
+
+MRandomLCG::MRandomLCG(S32 s)
+{
+   setSeed(s);
+}
+
+
+//--------------------------------------
+void MRandomLCG::setSeed(S32 s)
+{
+   mSeed = s;
+}
+
+
+//--------------------------------------
+U32 MRandomLCG::randI()
+{
+   if ( mSeed <= msQuotient )
+      mSeed = (mSeed * 16807L) % S32_MAX;
+   else
+   {
+      S32 high_part = mSeed / msQuotient;
+      S32 low_part  = mSeed % msQuotient;
+
+      S32 test = (16807L * low_part) - (msRemainder * high_part);
+
+      if ( test > 0 )
+         mSeed = test;
+      else
+         mSeed = test + S32_MAX;
+
+   }
+   return mSeed;
+}
+
+
+
+//--------------------------------------
+MRandomR250::MRandomR250()
+{
+   setSeed(generateSeed());
+}
+
+MRandomR250::MRandomR250(S32 s)
+{
+   setSeed(s);
+}
+
+
+//--------------------------------------
+void MRandomR250::setSeed(S32 s)
+{
+   mSeed = s;
+   MRandomLCG lcg( s );
+   mIndex = 0;
+
+   S32 j;
+   for (j = 0; j < 250; j++)        // fill r250 buffer with bit values
+      mBuffer[j] = lcg.randI();
+
+   for (j = 0; j < 250; j++)        // set some MSBs to 1
+      if ( lcg.randI() > 0x40000000L )
+         mBuffer[j] |= 0x80000000L;
+
+
+   U32 msb  = 0x80000000;           // turn on diagonal bit
+   U32 mask = 0xffffffff;           // turn off the leftmost bits
+
+   for (j = 0; j < 32; j++)
+   {
+      S32 k = 7 * j + 3;            // select a word to operate on
+      mBuffer[k] &= mask;           // turn off bits left of the diagonal
+      mBuffer[k] |= msb;            // turn on the diagonal bit
+      mask >>= 1;
+      msb  >>= 1;
+   }
+}
+
+
+//--------------------------------------
+U32 MRandomR250::randI()
+{
+   S32 j;
+
+   // wrap pointer around
+   if ( mIndex >= 147 ) j = mIndex - 147;
+   else                 j = mIndex + 103;
+
+   U32 new_rand = mBuffer[ mIndex ] ^ mBuffer[ j ];
+   mBuffer[ mIndex ] = new_rand;
+
+   // increment pointer for next time
+   if ( mIndex >= 249 ) mIndex = 0;
+   else                 mIndex++;
+
+   return new_rand >> 1;
+}
+
+

engine/math/mRandom.h

@@ -0,0 +1,120 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#ifndef _MRANDOM_H_
+#define _MRANDOM_H_
+
+
+#ifndef _PLATFORM_H_
+#include "platform/platform.h"
+#endif
+
+
+//--------------------------------------
+/// Base class for random number generators
+class MRandomGenerator
+{
+protected:
+   MRandomGenerator() {}
+   S32  mSeed;
+
+public:
+   void setSeed();
+   S32  getSeed() { return mSeed; }
+   virtual void setSeed(S32 s) = 0;
+
+   virtual U32 randI( void ) = 0;      ///< 0..2^31 random number generator
+   virtual F32  randF( void );         ///< 0.0 .. 1.0 F32 random number generator
+   S32 randI(S32 i, S32 n);            ///< i..n integer random number generator
+   F32 randF(F32 i, F32 n);            ///< i..n F32 random number generator
+};
+
+
+//--------------------------------------
+inline F32 MRandomGenerator::randF()
+{
+   // default: multiply by 1/(2^31)
+   return  F32(randI()) * F32(1.0/2147483647.0);
+}
+
+inline S32 MRandomGenerator::randI(S32 i, S32 n)
+{
+   AssertFatal(i<=n, "MRandomGenerator::randi: inverted range.");
+   return (S32)(i + (randI() % (n - i + 1)) );
+}
+
+inline F32 MRandomGenerator::randF(F32 i, F32 n)
+{
+   AssertFatal(i<=n, "MRandomGenerator::randf: inverted range.");
+   return (i + (n - i) * randF());
+}
+
+
+//--------------------------------------
+/// Linear Congruential Method, the "minimal standard generator"
+///
+/// Fast, farly good random numbers (better than using rand)
+///
+/// @author Park & Miller, 1988, Comm of the ACM, 31(10), pp. 1192-1201
+class MRandomLCG : public MRandomGenerator
+{
+protected:
+   static const S32 msQuotient;
+   static const S32 msRemainder;
+
+public:
+   MRandomLCG();
+   MRandomLCG(S32 s);
+
+   static void setGlobalRandSeed(U32 seed);
+
+   void setSeed(S32 s);
+//   using MRandomGenerator::randI;
+    S32 randI(S32 i, S32 n);            ///< i..n integer generator
+
+   U32 randI( void );
+
+};
+
+// Solution to "using" problem.
+inline S32 MRandomLCG::randI(S32 i, S32 n)
+{
+    return( MRandomGenerator::randI(i,n) );
+}
+
+
+//--------------------------------------
+/// Fast, very good random numbers
+///
+/// Period = 2^249
+///
+/// Kirkpatrick, S., and E. Stoll, 1981; A Very Fast Shift-Register
+///       Sequence Random Number Generator, Journal of Computational Physics,
+/// V. 40.
+///
+/// Maier, W.L., 1991; A Fast Pseudo Random Number Generator,
+/// Dr. Dobb's Journal, May, pp. 152 - 157
+class MRandomR250: public MRandomGenerator
+{
+private:
+   U32 mBuffer[250];
+   S32 mIndex;
+
+public:
+   MRandomR250();
+   MRandomR250(S32 s);
+
+   void setSeed(S32 s);
+// using MRandomGenerator::randI;
+   U32 randI();
+};
+
+
+typedef MRandomLCG MRandom;
+
+extern MRandomLCG gRandGen;
+
+
+#endif //_MRANDOM_H_

engine/math/mRect.h

@@ -0,0 +1,325 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#ifndef _MRECT_H_
+#define _MRECT_H_
+
+//Includes
+#ifndef _MPOINT_H_
+#include "math/mPoint.h"
+#endif
+
+class RectI
+{
+  public:
+   Point2I  point;
+   Point2I  extent;
+
+  public:
+   RectI() { }
+   RectI(const Point2I& in_rMin,
+         const Point2I& in_rExtent);
+   RectI(const S32 in_left,  const S32 in_top,
+         const S32 in_width, const S32 in_height);
+
+   void set(const Point2I& in_rMin, const Point2I& in_rExtent);
+   void set(const S32 in_left,  const S32 in_top,
+         const S32 in_width, const S32 in_height);
+
+   bool intersect(const RectI& clipRect);
+   bool pointInRect(const Point2I& pt) const;
+   bool contains(const RectI& R) const;
+   bool overlaps(RectI R) const;
+   void inset(S32 x, S32 y);
+
+   void unionRects(const RectI&);
+
+   S32   len_x() const;
+   S32   len_y() const;
+
+   bool operator==(const RectI&) const;
+   bool operator!=(const RectI&) const;
+
+   bool isValidRect() const { return (extent.x > 0 && extent.y > 0); }
+};
+
+class RectF
+{
+  public:
+   Point2F  point;
+   Point2F  extent;
+
+  public:
+   RectF() { }
+   RectF(const Point2F& in_rMin,
+         const Point2F& in_rExtent);
+   RectF(const F32 in_left,  const F32 in_top,
+         const F32 in_width, const F32 in_height);
+   void inset(F32 x, F32 y);
+
+   bool intersect(const RectF& clipRect);
+   bool overlaps(const RectF&) const;
+   F32 len_x() const;
+   F32 len_y() const;
+
+   bool isValidRect() const { return (extent.x > 0 && extent.y > 0); }
+};
+
+class RectD
+{
+  public:
+   Point2D  point;
+   Point2D  extent;
+
+  public:
+   RectD() { }
+   RectD(const Point2D& in_rMin,
+         const Point2D& in_rExtent);
+   RectD(const F64 in_left,  const F64 in_top,
+         const F64 in_width, const F64 in_height);
+   void inset(F64 x, F64 y);
+
+   bool intersect(const RectD& clipRect);
+   F64 len_x() const;
+   F64 len_y() const;
+
+   bool isValidRect() const { return (extent.x > 0 && extent.y > 0); }
+};
+
+//------------------------------------------------------------------------------
+//-------------------------------------- INLINES (RectI)
+//
+inline
+RectI::RectI(const Point2I& in_rMin,
+             const Point2I& in_rExtent)
+ : point(in_rMin),
+   extent(in_rExtent)
+{
+   //
+}
+
+inline
+RectI::RectI(const S32 in_left,  const S32 in_top,
+             const S32 in_width, const S32 in_height)
+ : point(in_left,  in_top),
+   extent(in_width, in_height)
+{
+   //
+}
+
+inline void RectI::set(const Point2I& in_rMin, const Point2I& in_rExtent)
+{
+   point = in_rMin;
+   extent = in_rExtent;
+}
+
+inline void RectI::set(const S32 in_left,  const S32 in_top,
+                      const S32 in_width, const S32 in_height)
+{
+   point.set(in_left,  in_top);
+   extent.set(in_width, in_height);
+}
+
+inline bool RectI::intersect(const RectI& clipRect)
+{
+   Point2I bottomL;
+   bottomL.x = getMin(point.x + extent.x - 1, clipRect.point.x + clipRect.extent.x - 1);
+   bottomL.y = getMin(point.y + extent.y - 1, clipRect.point.y + clipRect.extent.y - 1);
+
+   point.x = getMax(point.x, clipRect.point.x);
+   point.y = getMax(point.y, clipRect.point.y);
+
+   extent.x = bottomL.x - point.x + 1;
+   extent.y = bottomL.y - point.y + 1;
+
+   return isValidRect();
+}
+
+inline bool RectI::pointInRect(const Point2I &pt) const
+{
+   return (pt.x >= point.x && pt.x < point.x + extent.x && pt.y >= point.y && pt.y < point.y + extent.y);
+}
+
+inline bool RectI::contains(const RectI& R) const
+{
+   if (point.x <= R.point.x && point.y <= R.point.y)
+    if (R.point.x + R.extent.x <= point.x + extent.x)
+     if (R.point.y + R.extent.y <= point.y + extent.y)
+      return true;
+   return false;
+}
+
+inline bool RectI::overlaps(RectI R) const
+{
+   return R.intersect (* this);
+}
+
+inline void RectI::inset(S32 x, S32 y)
+{
+   point.x += x;
+   point.y += y;
+   extent.x -= 2 * x;
+   extent.y -= 2 * y;
+}
+
+inline void RectF::inset(F32 x, F32 y)
+{
+   point.x += x;
+   point.y += y;
+   extent.x -= 2 * x;
+   extent.y -= 2 * y;
+}
+
+inline void RectD::inset(F64 x, F64 y)
+{
+   point.x += x;
+   point.y += y;
+   extent.x -= 2 * x;
+   extent.y -= 2 * y;
+}
+
+
+inline void RectI::unionRects(const RectI& u)
+{
+   S32 minx = point.x < u.point.x ? point.x : u.point.x;
+   S32 miny = point.y < u.point.y ? point.y : u.point.y;
+   S32 maxx = (point.x + extent.x) > (u.point.x + u.extent.x) ? (point.x + extent.x) : (u.point.x + u.extent.x);
+   S32 maxy = (point.y + extent.y) > (u.point.y + u.extent.y) ? (point.y + extent.y) : (u.point.y + u.extent.y);
+
+   point.x  = minx;
+   point.y  = miny;
+   extent.x = maxx - minx;
+   extent.y = maxy - miny;
+}
+
+inline S32
+RectI::len_x() const
+{
+   return extent.x;
+}
+
+inline S32
+RectI::len_y() const
+{
+   return extent.y;
+}
+
+inline bool
+RectI::operator==(const RectI& in_rCompare) const
+{
+   return (point == in_rCompare.point) && (extent == in_rCompare.extent);
+}
+
+inline bool
+RectI::operator!=(const RectI& in_rCompare) const
+{
+   return (operator==(in_rCompare) == false);
+}
+
+//------------------------------------------------------------------------------
+//-------------------------------------- INLINES (RectF)
+//
+inline
+RectF::RectF(const Point2F& in_rMin,
+             const Point2F& in_rExtent)
+ : point(in_rMin),
+   extent(in_rExtent)
+{
+   //
+}
+
+inline
+RectF::RectF(const F32 in_left,  const F32 in_top,
+             const F32 in_width, const F32 in_height)
+ : point(in_left,  in_top),
+   extent(in_width, in_height)
+{
+   //
+}
+
+inline F32
+RectF::len_x() const
+{
+   return extent.x;
+}
+
+inline F32
+RectF::len_y() const
+{
+   return extent.y;
+}
+
+inline bool RectF::intersect(const RectF& clipRect)
+{
+   Point2F bottomL;
+   bottomL.x = getMin(point.x + extent.x, clipRect.point.x + clipRect.extent.x);
+   bottomL.y = getMin(point.y + extent.y, clipRect.point.y + clipRect.extent.y);
+
+   point.x = getMax(point.x, clipRect.point.x);
+   point.y = getMax(point.y, clipRect.point.y);
+
+   extent.x = bottomL.x - point.x;
+   extent.y = bottomL.y - point.y;
+
+   return isValidRect();
+}
+
+
+inline bool RectF::overlaps(const RectF& clipRect) const
+{
+   RectF test = *this;
+   return test.intersect(clipRect);
+}
+
+
+//------------------------------------------------------------------------------
+//-------------------------------------- INLINES (RectD)
+//
+inline
+RectD::RectD(const Point2D& in_rMin,
+             const Point2D& in_rExtent)
+ : point(in_rMin),
+   extent(in_rExtent)
+{
+   //
+}
+
+inline
+RectD::RectD(const F64 in_left,  const F64 in_top,
+             const F64 in_width, const F64 in_height)
+ : point(in_left,  in_top),
+   extent(in_width, in_height)
+{
+   //
+}
+
+inline F64
+RectD::len_x() const
+{
+   return extent.x;
+}
+
+inline F64
+RectD::len_y() const
+{
+   return extent.y;
+}
+
+inline bool RectD::intersect(const RectD& clipRect)
+{
+   Point2D bottomL;
+   bottomL.x = getMin(point.x + extent.x, clipRect.point.x + clipRect.extent.x);
+   bottomL.y = getMin(point.y + extent.y, clipRect.point.y + clipRect.extent.y);
+
+   point.x = getMax(point.x, clipRect.point.x);
+   point.y = getMax(point.y, clipRect.point.y);
+
+   extent.x = bottomL.x - point.x;
+   extent.y = bottomL.y - point.y;
+
+   return isValidRect();
+}
+
+#endif //_RECT_H_

engine/math/mSolver.cc

@@ -0,0 +1,241 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#include "platform/platform.h"
+#include "math/mMathFn.h"
+
+//--------------------------------------------------------------------------
+#define EQN_EPSILON     (1e-8)
+
+static inline void swap(F32 & a, F32 & b)
+{
+   F32 t = b;
+   b = a;
+   a = t;
+}
+
+static inline bool mIsZero(F32 val)
+{
+   return((val > -EQN_EPSILON) && (val < EQN_EPSILON));
+}
+
+static inline F32 mCbrt(F32 val)
+{
+   if(val < 0.f)
+      return(-mPow(-val, F32(1.f/3.f)));
+   else
+      return(mPow(val, F32(1.f/3.f)));
+}
+
+static inline U32 mSolveLinear(F32 a, F32 b, F32 * x)
+{
+   if(mIsZero(a))
+      return(0);
+
+   x[0] = -b/a;
+   return(1);
+}
+
+static U32 mSolveQuadratic_c(F32 a, F32 b, F32 c, F32 * x)
+{
+   // really linear?
+   if(mIsZero(a))
+      return(mSolveLinear(b, c, x));
+
+   // get the descriminant:   (b^2 - 4ac)
+   F32 desc = (b * b) - (4.f * a * c);
+
+   // solutions:
+   // desc < 0:   two imaginary solutions
+   // desc > 0:   two real solutions (b +- sqrt(desc)) / 2a
+   // desc = 0:   one real solution (b / 2a)
+   if(mIsZero(desc))
+   {
+      x[0] = b / (2.f * a);
+      return(1);
+   }
+   else if(desc > 0.f)
+   {
+      F32 sqrdesc = mSqrt(desc);
+      F32 den = (2.f * a);
+      x[0] = (b + sqrdesc) / den;
+      x[1] = (b - sqrdesc) / den;
+
+      if(x[1] < x[0])
+         swap(x[0], x[1]);
+
+      return(2);
+   }
+   else
+      return(0);
+}
+
+//--------------------------------------------------------------------------
+// from Graphics Gems I: pp 738-742
+U32 mSolveCubic_c(F32 a, F32 b, F32 c, F32 d, F32 * x)
+{
+   if(mIsZero(a))
+      return(mSolveQuadratic(b, c, d, x));
+
+   // normal form: x^3 + Ax^2 + BX + C = 0
+   F32 A = b / a;
+   F32 B = c / a;
+   F32 C = d / a;
+
+   // substitute x = y - A/3 to eliminate quadric term and depress
+   // the cubic equation to (x^3 + px + q = 0)
+   F32 A2 = A * A;
+   F32 A3 = A2 * A;
+
+   F32 p = (1.f/3.f) * (((-1.f/3.f) * A2) + B);
+   F32 q = (1.f/2.f) * (((2.f/27.f) * A3) - ((1.f/3.f) * A * B) + C);
+
+   // use Cardano's fomula to solve the depressed cubic
+   F32 p3 = p * p * p;
+   F32 q2 = q * q;
+
+   F32 D = q2 + p3;
+
+   U32 num = 0;
+
+   if(mIsZero(D))          // 1 or 2 solutions
+   {
+      if(mIsZero(q)) // 1 triple solution
+      {
+         x[0] = 0.f;
+         num = 1;
+      }
+      else // 1 single and 1 double
+      {
+         F32 u = mCbrt(-q);
+         x[0] = 2.f * u;
+         x[1] = -u;
+         num = 2;
+      }
+   }
+   else if(D < 0.f)        // 3 solutions: casus irreducibilis
+   {
+      F32 phi = (1.f/3.f) * mAcos(-q / mSqrt(-p3));
+      F32 t = 2.f * mSqrt(-p);
+
+      x[0] = t * mCos(phi);
+      x[1] = -t * mCos(phi + (M_PI / 3.f));
+      x[2] = -t * mCos(phi - (M_PI / 3.f));
+      num = 3;
+   }
+   else                    // 1 solution
+   {
+      F32 sqrtD = mSqrt(D);
+      F32 u = mCbrt(sqrtD - q);
+      F32 v = -mCbrt(sqrtD + q);
+
+      x[0] = u + v;
+      num = 1;
+   }
+
+   // resubstitute
+   F32 sub = (1.f/3.f) * A;
+   for(U32 i = 0; i < num; i++)
+      x[i] -= sub;
+
+   // sort the roots
+   for(S32 j = 0; j < (num - 1); j++)
+      for(S32 k = j + 1; k < num; k++)
+         if(x[k] < x[j])
+            swap(x[k], x[j]);
+
+   return(num);
+}
+
+//--------------------------------------------------------------------------
+// from Graphics Gems I: pp 738-742
+U32 mSolveQuartic_c(F32 a, F32 b, F32 c, F32 d, F32 e, F32 * x)
+{
+   if(mIsZero(a))
+      return(mSolveCubic(b, c, d, e, x));
+
+   // normal form: x^4 + ax^3 + bx^2 + cx + d = 0
+   F32 A = b / a;
+   F32 B = c / a;
+   F32 C = d / a;
+   F32 D = e / a;
+
+   // substitue x = y - A/4 to eliminate cubic term:
+   // x^4 + px^2 + qx + r = 0
+   F32 A2 = A * A;
+   F32 A3 = A2 * A;
+   F32 A4 = A2 * A2;
+
+   F32 p = ((-3.f/8.f) * A2) + B;
+   F32 q = ((1.f/8.f) * A3) - ((1.f/2.f) * A * B) + C;
+   F32 r = ((-3.f/256.f) * A4) + ((1.f/16.f) * A2 * B) - ((1.f/4.f) * A * C) + D;
+
+   U32 num = 0;
+   if(mIsZero(r)) // no absolute term: y(y^3 + py + q) = 0
+   {
+      num = mSolveCubic(1.f, 0.f, p, q, x);
+      x[num++] = 0.f;
+   }
+   else
+   {
+      // solve the resolvent cubic
+      F32 q2 = q * q;
+
+      a = 1.f;
+      b = (-1.f/2.f) * p;
+      c = -r;
+      d = ((1.f/2.f) * r * p) - ((1.f/8.f) * q2);
+
+      mSolveCubic(a, b, c, d, x);
+
+      F32 z = x[0];
+
+      // build 2 quadratic equations from the one solution
+      F32 u = (z * z) - r;
+      F32 v = (2.f * z) - p;
+
+      if(mIsZero(u))
+         u = 0.f;
+      else if(u > 0.f)
+         u = mSqrt(u);
+      else
+         return(0);
+
+      if(mIsZero(v))
+         v = 0.f;
+      else if(v > 0.f)
+         v = mSqrt(v);
+      else
+         return(0);
+
+      // solve the two quadratics
+      a = 1.f;
+      b = v;
+      c = z - u;
+      num = mSolveQuadratic(a, b, c, x);
+
+      a = 1.f;
+      b = -v;
+      c = z + u;
+      num += mSolveQuadratic(a, b, c, x + num);
+   }
+
+   // resubstitute
+   F32 sub = (1.f/4.f) * A;
+   for(U32 i = 0; i < num; i++)
+      x[i] -= sub;
+
+   // sort the roots
+   for(S32 j = 0; j < (num - 1); j++)
+      for(S32 k = j + 1; k < num; k++)
+         if(x[k] < x[j])
+            swap(x[k], x[j]);
+
+   return(num);
+}
+
+U32 (*mSolveQuadratic)( F32 a, F32 b, F32 c, F32* x ) = mSolveQuadratic_c;
+U32 (*mSolveCubic)( F32 a, F32 b, F32 c, F32 d, F32* x ) = mSolveCubic_c;
+U32 (*mSolveQuartic)( F32 a, F32 b, F32 c, F32 d, F32 e, F32* x ) = mSolveQuartic_c;

engine/math/mSphere.h

@@ -0,0 +1,70 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#ifndef _MSPHERE_H_
+#define _MSPHERE_H_
+
+//Includes
+#ifndef _MPOINT_H_
+#include "math/mPoint.h"
+#endif
+
+class SphereF
+{
+  public:
+   Point3F center;
+   F32     radius;
+
+  public:
+   SphereF() { }
+   SphereF(const Point3F& in_rPosition,
+           const F32    in_rRadius)
+    : center(in_rPosition),
+      radius(in_rRadius)
+   {
+      if (radius < 0.0f)
+         radius = 0.0f;
+   }
+
+   bool isContained(const Point3F& in_rContain) const;
+   bool isContained(const SphereF& in_rContain) const;
+   bool isIntersecting(const SphereF& in_rIntersect) const;
+};
+
+//-------------------------------------- INLINES
+//
+inline bool
+SphereF::isContained(const Point3F& in_rContain) const
+{
+   F32 distSq = (center - in_rContain).lenSquared();
+
+   return (distSq <= (radius * radius));
+}
+
+inline bool
+SphereF::isContained(const SphereF& in_rContain) const
+{
+   if (radius < in_rContain.radius)
+      return false;
+
+   // Since our radius is guaranteed to be >= other's, we
+   //  can dodge the sqrt() here.
+   //
+   F32 dist = (in_rContain.center - center).lenSquared();
+
+   return (dist <= ((radius - in_rContain.radius) *
+                    (radius - in_rContain.radius)));
+}
+
+inline bool
+SphereF::isIntersecting(const SphereF& in_rIntersect) const
+{
+   F32 distSq = (in_rIntersect.center - center).lenSquared();
+
+   return (distSq <= ((in_rIntersect.radius + radius) *
+                      (in_rIntersect.radius + radius)));
+}
+
+#endif //_SPHERE_H_

engine/math/mSplinePatch.cc

@@ -0,0 +1,97 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#include "math/mSplinePatch.h"
+
+
+//******************************************************************************
+// Spline control points
+//******************************************************************************
+SplCtrlPts::SplCtrlPts()
+{
+}
+
+//------------------------------------------------------------------------------
+// Destructor
+//------------------------------------------------------------------------------
+SplCtrlPts::~SplCtrlPts()
+{
+}
+
+//------------------------------------------------------------------------------
+// Get point
+//------------------------------------------------------------------------------
+const Point3F * SplCtrlPts::getPoint( U32 pointNum )
+{
+   return &mPoints[pointNum];
+}
+
+//------------------------------------------------------------------------------
+// Set point
+//------------------------------------------------------------------------------
+void SplCtrlPts::setPoint( Point3F &point, U32 pointNum )
+{
+   mPoints[pointNum] = point;
+}
+
+//------------------------------------------------------------------------------
+// Add point
+//------------------------------------------------------------------------------
+void SplCtrlPts::addPoint( Point3F &point )
+{
+   mPoints.push_back( point );
+}
+
+//------------------------------------------------------------------------------
+// Submit control points
+//------------------------------------------------------------------------------
+void SplCtrlPts::submitPoints( Point3F *pts, U32 num )
+{
+   mPoints.clear();
+
+   for( int i=0; i<num; i++ )
+   {
+      mPoints.push_back( pts[i] );
+   }
+}
+
+
+//******************************************************************************
+// Spline patch  - base class
+//******************************************************************************
+SplinePatch::SplinePatch()
+{
+   mNumReqControlPoints = 0;
+}
+
+//------------------------------------------------------------------------------
+// This method is the only way to modify control points once they have been
+// submitted to the patch.  This was done to make sure the patch has a chance
+// to re-calc any pre-calculated data it may be using from the submitted control
+// points.
+//------------------------------------------------------------------------------
+void SplinePatch::setControlPoint( Point3F &point, int index )
+{
+   mControlPoints.setPoint( point, index );
+}
+
+//------------------------------------------------------------------------------
+// Calc point on spline using already submitted points
+//------------------------------------------------------------------------------
+void SplinePatch::calc( F32 t, Point3F &result )
+{
+   t = t; // kill compiler warning;
+   result.x = result.y = result.z = 0.0;
+}
+
+//------------------------------------------------------------------------------
+// Calc point on spline using passed-in points
+//------------------------------------------------------------------------------
+void SplinePatch::calc( Point3F *points, F32 t, Point3F &result )
+{
+   points = points;  // kill compiler warning
+   t = t;            // kill compiler warning;
+   result.x = result.y = result.z = 0.0;
+}

engine/math/mSplinePatch.h

@@ -0,0 +1,110 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#ifndef _MSPLINEPATCH_H_
+#define _MSPLINEPATCH_H_
+
+#ifndef _PLATFORM_H_
+#include "platform/platform.h"
+#endif
+#ifndef _MPOINT_H_
+#include "math/mPoint.h"
+#endif
+#ifndef _TVECTOR_H_
+#include "core/tVector.h"
+#endif
+
+//------------------------------------------------------------------------------
+// Spline control points
+//------------------------------------------------------------------------------
+
+/// Class for spline control points.
+/// @see SplinePatch
+class SplCtrlPts
+{
+private:
+   /// Vector of points in the spline
+   Vector <Point3F > mPoints;
+
+public:
+
+   SplCtrlPts();
+   virtual ~SplCtrlPts();
+
+   /// Gets the number of points in the spline
+   U32               getNumPoints(){ return mPoints.size(); }
+   /// Gets the point at the given index
+   /// @param pointNum index of the point in question
+   const Point3F *   getPoint( U32 pointNum );
+   /// Sets a point at the given index to the point given
+   /// @param point New value for the given point
+   /// @param pointNum index of the given point
+   void              setPoint( Point3F &point, U32 pointNum );
+   /// Adds a point to the end of the spline
+   /// @param point New point to be added
+   void              addPoint( Point3F &point );
+   /// Clears existing points and enters new points
+   /// @param pts List of points to be added
+   /// @param num Number of points to be added
+   void              submitPoints( Point3F *pts, U32 num );
+};
+
+//------------------------------------------------------------------------------
+// Base class for spline patches
+//------------------------------------------------------------------------------
+
+/// Base class for spline patches.  The only child of this class is QuadPatch.
+///
+/// Spline utility class for drawing nice pretty splines.  In order to draw a spline,
+/// you need to create a SplCtrlPts data structure, which contains all control
+/// points on the spline.  See SplCtrlPts for more information on how to submit
+/// points to the spline utility.  Next, submit the SplCtrlPts structure to the
+/// spline utility.
+/// @code
+/// SplinePatch patch;
+/// patch.submitControlPoints(ctrlPts);
+/// @endcode
+/// Next, use the SplineUtil namespace to draw your spline.
+/// @code
+/// SplineUtil::drawSplineBeam(camPos, numSegments, width, patch[, uvOffset, numTexRep]);
+/// @endcode
+///
+/// You can also create a SplineBeamInfo structure (SplineUtil::SplineBeamInfo)
+/// and just pass the SplineBeamInfo structure to the SplineUtil::drawSplineBeam function.
+/// @see SplCtrlPts
+/// @see SplineUtil
+class SplinePatch
+{
+private:
+   U32         mNumReqControlPoints;
+   SplCtrlPts  mControlPoints;
+
+protected:
+   void     setNumReqControlPoints( U32 numPts ){ mNumReqControlPoints = numPts; }
+
+public:
+
+   SplinePatch();
+
+   U32                  getNumReqControlPoints(){ return mNumReqControlPoints; }
+   const SplCtrlPts *   getControlPoints(){ return &mControlPoints; }
+   const Point3F *      getControlPoint( U32 index ){ return mControlPoints.getPoint( index ); }
+
+   // virtuals
+   virtual void         setControlPoint( Point3F &point, int index );
+   /// If you have a preconstructed "SplCtrlPts" class, submit it with this function.
+   /// @see SplCtrlPts
+   virtual void         submitControlPoints( SplCtrlPts &points ){ mControlPoints = points; }
+   /// Recalc function.  Do not call this ever - only SplineUtil needs this.
+   /// @see SplineUtil
+   virtual void         calc( F32 t, Point3F &result) = 0;
+   /// Recalc function.  Do not call this ever - only SplineUtil needs this.
+   /// @see SplineUtil
+   virtual void         calc( Point3F *points, F32 t, Point3F &result ) = 0;
+
+};
+
+
+#endif

engine/math/mTrig.h

@@ -0,0 +1,41 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#ifndef _MTRIG_H_
+#define _MTRIG_H_
+
+#ifndef _PLATFORM_H_
+#include "platform/platform.h"
+#endif
+#ifndef _MMATHFN_H_
+#include "math/mMathFn.h"
+#endif
+
+//-------------------------------------- External assembly helpers...
+//
+//#ifdef WIN32
+//
+//extern "C" {
+//   F32 m_reduceAngle_asm(const F32*);
+//}
+//
+//inline F32
+//m_reduceAngle(const F32 in_angle)
+//{
+//   F32 initial = m_reduceAngle_asm(&in_angle);
+//   if (initial < 0.0)
+//      initial += Float_2Pi;
+//   return initial;
+//}
+//
+//#else
+//
+//F32 m_reduceAngle(const F32 in_angle);
+//
+//#endif
+
+
+
+#endif //_TRIG_H_

engine/math/mathIO.h

@@ -0,0 +1,231 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#ifndef _MATHIO_H_
+#define _MATHIO_H_
+
+//Includes
+#ifndef _PLATFORM_H_
+#include "platform/platform.h"
+#endif
+#ifndef _STREAM_H_
+#include "core/stream.h"
+#endif
+#ifndef _MMATH_H_
+#include "math/mMath.h"
+#endif
+
+//------------------------------------------------------------------------------
+//-------------------------------------- READING
+//
+inline bool mathRead(Stream& stream, Point2I* p)
+{
+   bool success = stream.read(&p->x);
+   success     &= stream.read(&p->y);
+   return success;
+}
+
+inline bool mathRead(Stream& stream, Point3I* p)
+{
+   bool success = stream.read(&p->x);
+   success     &= stream.read(&p->y);
+   success     &= stream.read(&p->z);
+   return success;
+}
+
+inline bool mathRead(Stream& stream, Point2F* p)
+{
+   bool success = stream.read(&p->x);
+   success     &= stream.read(&p->y);
+   return success;
+}
+
+inline bool mathRead(Stream& stream, Point3F* p)
+{
+   bool success = stream.read(&p->x);
+   success     &= stream.read(&p->y);
+   success     &= stream.read(&p->z);
+   return success;
+}
+
+inline bool mathRead(Stream& stream, Point4F* p)
+{
+   bool success = stream.read(&p->x);
+   success     &= stream.read(&p->y);
+   success     &= stream.read(&p->z);
+   success     &= stream.read(&p->w);
+   return success;
+}
+
+inline bool mathRead(Stream& stream, Point3D* p)
+{
+   bool success = stream.read(&p->x);
+   success     &= stream.read(&p->y);
+   success     &= stream.read(&p->z);
+   return success;
+}
+
+inline bool mathRead(Stream& stream, PlaneF* p)
+{
+   bool success = stream.read(&p->x);
+   success     &= stream.read(&p->y);
+   success     &= stream.read(&p->z);
+   success     &= stream.read(&p->d);
+   return success;
+}
+
+inline bool mathRead(Stream& stream, Box3F* b)
+{
+   bool success = mathRead(stream, &b->min);
+   success     &= mathRead(stream, &b->max);
+   return success;
+}
+
+inline bool mathRead(Stream& stream, SphereF* s)
+{
+   bool success = mathRead(stream, &s->center);
+   success     &= stream.read(&s->radius);
+   return success;
+}
+
+inline bool mathRead(Stream& stream, RectI* r)
+{
+   bool success = mathRead(stream, &r->point);
+   success     &= mathRead(stream, &r->extent);
+   return success;
+}
+
+inline bool mathRead(Stream& stream, RectF* r)
+{
+   bool success = mathRead(stream, &r->point);
+   success     &= mathRead(stream, &r->extent);
+   return success;
+}
+
+inline bool mathRead(Stream& stream, MatrixF* m)
+{
+   bool success = true;
+   F32* pm    = *m;
+   for (U32 i = 0; i < 16; i++)
+      success &= stream.read(&pm[i]);
+   return success;
+}
+
+inline bool mathRead(Stream& stream, QuatF* q)
+{
+   bool success = stream.read(&q->x);
+   success     &= stream.read(&q->y);
+   success     &= stream.read(&q->z);
+   success     &= stream.read(&q->w);
+   return success;
+}
+
+//------------------------------------------------------------------------------
+//-------------------------------------- WRITING
+//
+inline bool mathWrite(Stream& stream, const Point2I& p)
+{
+   bool success = stream.write(p.x);
+   success     &= stream.write(p.y);
+   return success;
+}
+
+inline bool mathWrite(Stream& stream, const Point3I& p)
+{
+   bool success = stream.write(p.x);
+   success     &= stream.write(p.y);
+   success     &= stream.write(p.z);
+   return success;
+}
+
+inline bool mathWrite(Stream& stream, const Point2F& p)
+{
+   bool success = stream.write(p.x);
+   success     &= stream.write(p.y);
+   return success;
+}
+
+inline bool mathWrite(Stream& stream, const Point3F& p)
+{
+   bool success = stream.write(p.x);
+   success     &= stream.write(p.y);
+   success     &= stream.write(p.z);
+   return success;
+}
+
+inline bool mathWrite(Stream& stream, const Point4F& p)
+{
+   bool success = stream.write(p.x);
+   success     &= stream.write(p.y);
+   success     &= stream.write(p.z);
+   success     &= stream.write(p.w);
+   return success;
+}
+
+inline bool mathWrite(Stream& stream, const Point3D& p)
+{
+   bool success = stream.write(p.x);
+   success     &= stream.write(p.y);
+   success     &= stream.write(p.z);
+   return success;
+}
+
+inline bool mathWrite(Stream& stream, const PlaneF& p)
+{
+   bool success = stream.write(p.x);
+   success     &= stream.write(p.y);
+   success     &= stream.write(p.z);
+   success     &= stream.write(p.d);
+   return success;
+}
+
+inline bool mathWrite(Stream& stream, const Box3F& b)
+{
+   bool success = mathWrite(stream, b.min);
+   success     &= mathWrite(stream, b.max);
+   return success;
+}
+
+inline bool mathWrite(Stream& stream, const SphereF& s)
+{
+   bool success = mathWrite(stream, s.center);
+   success     &= stream.write(s.radius);
+   return success;
+}
+
+inline bool mathWrite(Stream& stream, const RectI& r)
+{
+   bool success = mathWrite(stream, r.point);
+   success     &= mathWrite(stream, r.extent);
+   return success;
+}
+
+inline bool mathWrite(Stream& stream, const RectF& r)
+{
+   bool success = mathWrite(stream, r.point);
+   success     &= mathWrite(stream, r.extent);
+   return success;
+}
+
+inline bool mathWrite(Stream& stream, const MatrixF& m)
+{
+   bool success    = true;
+   const F32* pm = m;
+   for (U32 i = 0; i < 16; i++)
+      success &= stream.write(pm[i]);
+   return success;
+}
+
+inline bool mathWrite(Stream& stream, const QuatF& q)
+{
+   bool success = stream.write(q.x);
+   success     &= stream.write(q.y);
+   success     &= stream.write(q.z);
+   success     &= stream.write(q.w);
+   return success;
+}
+
+#endif //_MATHIO_H_
+

engine/math/mathTypes.cc

@@ -0,0 +1,555 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#include "console/consoleTypes.h"
+#include "console/console.h"
+#include "math/mPoint.h"
+#include "math/mMatrix.h"
+#include "math/mRect.h"
+#include "math/mBox.h"
+#include "math/mathTypes.h"
+#include "math/mRandom.h"
+
+//////////////////////////////////////////////////////////////////////////
+// TypePoint2I
+//////////////////////////////////////////////////////////////////////////
+ConsoleType( Point2I, TypePoint2I, sizeof(Point2I) )
+
+ConsoleGetType( TypePoint2I )
+{
+   Point2I *pt = (Point2I *) dptr;
+   char* returnBuffer = Con::getReturnBuffer(256);
+   dSprintf(returnBuffer, 256, "%d %d", pt->x, pt->y);
+   return returnBuffer;
+}
+
+ConsoleSetType( TypePoint2I )
+{
+   if(argc == 1)
+      dSscanf(argv[0], "%d %d", &((Point2I *) dptr)->x, &((Point2I *) dptr)->y);
+   else if(argc == 2)
+      *((Point2I *) dptr) = Point2I(dAtoi(argv[0]), dAtoi(argv[1]));
+   else
+      Con::printf("Point2I must be set as { x, y } or \"x y\"");
+}
+
+//////////////////////////////////////////////////////////////////////////
+// TypePoint2F
+//////////////////////////////////////////////////////////////////////////
+ConsoleType( Point2F, TypePoint2F, sizeof(Point2F) )
+
+ConsoleGetType( TypePoint2F )
+{
+   Point2F *pt = (Point2F *) dptr;
+   char* returnBuffer = Con::getReturnBuffer(256);
+   dSprintf(returnBuffer, 256, "%g %g", pt->x, pt->y);
+   return returnBuffer;
+}
+
+ConsoleSetType( TypePoint2F )
+{
+   if(argc == 1)
+      dSscanf(argv[0], "%g %g", &((Point2F *) dptr)->x, &((Point2F *) dptr)->y);
+   else if(argc == 2)
+      *((Point2F *) dptr) = Point2F(dAtof(argv[0]), dAtof(argv[1]));
+   else
+      Con::printf("Point2F must be set as { x, y } or \"x y\"");
+}
+
+//////////////////////////////////////////////////////////////////////////
+// TypePoint3F
+//////////////////////////////////////////////////////////////////////////
+ConsoleType( Point3F, TypePoint3F, sizeof(Point3F) )
+
+ConsoleGetType( TypePoint3F )
+{
+   Point3F *pt = (Point3F *) dptr;
+   char* returnBuffer = Con::getReturnBuffer(256);
+   dSprintf(returnBuffer, 256, "%g %g %g", pt->x, pt->y, pt->z);
+   return returnBuffer;
+}
+
+ConsoleSetType( TypePoint3F )
+{
+   if(argc == 1)
+      dSscanf(argv[0], "%g %g %g", &((Point3F *) dptr)->x, &((Point3F *) dptr)->y, &((Point3F *) dptr)->z);
+   else if(argc == 3)
+      *((Point3F *) dptr) = Point3F(dAtof(argv[0]), dAtof(argv[1]), dAtof(argv[2]));
+   else
+      Con::printf("Point3F must be set as { x, y, z } or \"x y z\"");
+}
+
+//////////////////////////////////////////////////////////////////////////
+// TypePoint4F
+//////////////////////////////////////////////////////////////////////////
+ConsoleType( Point4F, TypePoint4F, sizeof(Point4F) )
+
+ConsoleGetType( TypePoint4F )
+{
+   Point4F *pt = (Point4F *) dptr;
+   char* returnBuffer = Con::getReturnBuffer(256);
+   dSprintf(returnBuffer, 256, "%g %g %g %g", pt->x, pt->y, pt->z, pt->w);
+   return returnBuffer;
+}
+
+ConsoleSetType( TypePoint4F )
+{
+   if(argc == 1)
+      dSscanf(argv[0], "%g %g %g %g", &((Point4F *) dptr)->x, &((Point4F *) dptr)->y, &((Point4F *) dptr)->z, &((Point4F *) dptr)->w);
+   else if(argc == 4)
+      *((Point4F *) dptr) = Point4F(dAtof(argv[0]), dAtof(argv[1]), dAtof(argv[2]), dAtof(argv[3]));
+   else
+      Con::printf("Point4F must be set as { x, y, z, w } or \"x y z w\"");
+}
+
+//////////////////////////////////////////////////////////////////////////
+// TypeRectI
+//////////////////////////////////////////////////////////////////////////
+ConsoleType( RectI, TypeRectI, sizeof(RectI) )
+
+ConsoleGetType( TypeRectI )
+{
+   RectI *rect = (RectI *) dptr;
+   char* returnBuffer = Con::getReturnBuffer(256);
+   dSprintf(returnBuffer, 256, "%d %d %d %d", rect->point.x, rect->point.y,
+            rect->extent.x, rect->extent.y);
+   return returnBuffer;
+}
+
+ConsoleSetType( TypeRectI )
+{
+   if(argc == 1)
+      dSscanf(argv[0], "%d %d %d %d", &((RectI *) dptr)->point.x, &((RectI *) dptr)->point.y,
+              &((RectI *) dptr)->extent.x, &((RectI *) dptr)->extent.y);
+   else if(argc == 4)
+      *((RectI *) dptr) = RectI(dAtoi(argv[0]), dAtoi(argv[1]), dAtoi(argv[2]), dAtoi(argv[3]));
+   else
+      Con::printf("RectI must be set as { x, y, w, h } or \"x y w h\"");
+}
+
+//////////////////////////////////////////////////////////////////////////
+// TypeRectF
+//////////////////////////////////////////////////////////////////////////
+ConsoleType( RectF, TypeRectF, sizeof(RectF) )
+
+ConsoleGetType( TypeRectF )
+{
+   RectF *rect = (RectF *) dptr;
+   char* returnBuffer = Con::getReturnBuffer(256);
+   dSprintf(returnBuffer, 256, "%g %g %g %g", rect->point.x, rect->point.y,
+            rect->extent.x, rect->extent.y);
+   return returnBuffer;
+}
+
+ConsoleSetType( TypeRectF )
+{
+   if(argc == 1)
+      dSscanf(argv[0], "%g %g %g %g", &((RectF *) dptr)->point.x, &((RectF *) dptr)->point.y,
+              &((RectF *) dptr)->extent.x, &((RectF *) dptr)->extent.y);
+   else if(argc == 4)
+      *((RectF *) dptr) = RectF(dAtof(argv[0]), dAtof(argv[1]), dAtof(argv[2]), dAtof(argv[3]));
+   else
+      Con::printf("RectF must be set as { x, y, w, h } or \"x y w h\"");
+}
+
+//////////////////////////////////////////////////////////////////////////
+// TypeMatrixPosition
+//////////////////////////////////////////////////////////////////////////
+ConsoleType( MatrixPosition, TypeMatrixPosition, sizeof(4*sizeof(F32)) )
+
+ConsoleGetType( TypeMatrixPosition )
+{
+   F32 *col = (F32 *) dptr + 3;
+   char* returnBuffer = Con::getReturnBuffer(256);
+   if(col[12] == 1.f)
+      dSprintf(returnBuffer, 256, "%g %g %g", col[0], col[4], col[8]);
+   else
+      dSprintf(returnBuffer, 256, "%g %g %g %g", col[0], col[4], col[8], col[12]);
+   return returnBuffer;
+}
+
+ConsoleSetType( TypeMatrixPosition )
+{
+   F32 *col = ((F32 *) dptr) + 3;
+   if (argc == 1)
+   {
+      col[0] = col[4] = col[8] = 0.f;
+      col[12] = 1.f;
+      dSscanf(argv[0], "%g %g %g %g", &col[0], &col[4], &col[8], &col[12]);
+   }
+   else if (argc <= 4) 
+   {
+         for (S32 i = 0; i < argc; i++)
+            col[i << 2] = dAtoi(argv[i]);
+   }
+   else
+      Con::printf("Matrix position must be set as { x, y, z, w } or \"x y z w\"");
+}
+
+//////////////////////////////////////////////////////////////////////////
+// TypeMatrixRotation
+//////////////////////////////////////////////////////////////////////////
+ConsoleType( MatrixRotation, TypeMatrixRotation, sizeof(MatrixF) )
+
+ConsoleGetType( TypeMatrixRotation )
+{
+   AngAxisF aa(*(MatrixF *) dptr);
+   aa.axis.normalize();
+   char* returnBuffer = Con::getReturnBuffer(256);
+   dSprintf(returnBuffer,256,"%g %g %g %g",aa.axis.x,aa.axis.y,aa.axis.z,mRadToDeg(aa.angle));
+   return returnBuffer;
+}
+
+ConsoleSetType( TypeMatrixRotation )
+{
+   // DMM: Note that this will ONLY SET the ULeft 3x3 submatrix.
+   //
+   AngAxisF aa(Point3F(0,0,0),0);
+   if (argc == 1)
+   {
+      dSscanf(argv[0], "%g %g %g %g", &aa.axis.x, &aa.axis.y, &aa.axis.z, &aa.angle);
+      aa.angle = mDegToRad(aa.angle);
+   }
+   else if (argc == 4) 
+   {
+         for (S32 i = 0; i < argc; i++)
+            ((F32*)&aa)[i] = dAtof(argv[i]);
+         aa.angle = mDegToRad(aa.angle);
+   }
+   else
+      Con::printf("Matrix rotation must be set as { x, y, z, angle } or \"x y z angle\"");
+
+   //
+   MatrixF temp;
+   aa.setMatrix(&temp);
+
+   F32* pDst = *(MatrixF *)dptr;
+   const F32* pSrc = temp;
+   for (U32 i = 0; i < 3; i++)
+      for (U32 j = 0; j < 3; j++)
+         pDst[i*4 + j] = pSrc[i*4 + j];
+}
+
+
+
+//////////////////////////////////////////////////////////////////////////
+// TypeBox3F
+//////////////////////////////////////////////////////////////////////////
+ConsoleType( Box3F, TypeBox3F, sizeof(Box3F) )
+
+ConsoleGetType( TypeBox3F )
+{
+   const Box3F* pBox = (const Box3F*)dptr;
+
+   char* returnBuffer = Con::getReturnBuffer(256);
+   dSprintf(returnBuffer, 256, "%g %g %g %g %g %g",
+            pBox->min.x, pBox->min.y, pBox->min.z,
+            pBox->max.x, pBox->max.y, pBox->max.z);
+
+   return returnBuffer;
+}
+
+ConsoleSetType( TypeBox3F )
+{
+   Box3F* pDst = (Box3F*)dptr;
+
+   if (argc == 1) 
+   {
+      U32 args = dSscanf(argv[0], "%g %g %g %g %g %g",
+                         &pDst->min.x, &pDst->min.y, &pDst->min.z,
+                         &pDst->max.x, &pDst->max.y, &pDst->max.z);
+      AssertWarn(args == 6, "Warning, box probably not read properly");
+   } 
+   else 
+   {
+      Con::printf("Box3F must be set as \"xMin yMin zMin xMax yMax zMax\"");
+   }
+}
+
+
+//----------------------------------------------------------------------------
+
+ConsoleFunctionGroupBegin( VectorMath, "Vector manipulation functions.");
+
+ConsoleFunction( VectorAdd, const char*, 3, 3, "(Vector3F a, Vector3F b) Returns a+b.")
+{
+   VectorF v1(0,0,0),v2(0,0,0);
+   dSscanf(argv[1],"%g %g %g",&v1.x,&v1.y,&v1.z);
+   dSscanf(argv[2],"%g %g %g",&v2.x,&v2.y,&v2.z);
+   VectorF v;
+   v = v1 + v2;
+   char* returnBuffer = Con::getReturnBuffer(256);
+   dSprintf(returnBuffer,256,"%g %g %g",v.x,v.y,v.z);
+   return returnBuffer;
+}
+
+ConsoleFunction( VectorSub, const char*, 3, 3, "(Vector3F a, Vector3F b) Returns a-b.")
+{
+   VectorF v1(0,0,0),v2(0,0,0);
+   dSscanf(argv[1],"%g %g %g",&v1.x,&v1.y,&v1.z);
+   dSscanf(argv[2],"%g %g %g",&v2.x,&v2.y,&v2.z);
+   VectorF v;
+   v = v1 - v2;
+   char* returnBuffer = Con::getReturnBuffer(256);
+   dSprintf(returnBuffer,256,"%g %g %g",v.x,v.y,v.z);
+   return returnBuffer;
+}
+
+ConsoleFunction( VectorScale, const char*, 3, 3, "(Vector3F a, float scalar) Returns a scaled by scalar (ie, a*scalar).")
+{
+   VectorF v(0,0,0);
+   dSscanf(argv[1],"%g %g %g",&v.x,&v.y,&v.z);
+   v *= dAtof(argv[2]);
+   char* returnBuffer = Con::getReturnBuffer(256);
+   dSprintf(returnBuffer,256,"%g %g %g",v.x,v.y,v.z);
+   return returnBuffer;
+}
+
+ConsoleFunction( VectorNormalize, const char*, 2, 2, "(Vector3F a) Returns a scaled such that length(a) = 1.")
+{
+   VectorF v(0,0,0);
+   dSscanf(argv[1],"%g %g %g",&v.x,&v.y,&v.z);
+   if (v.len() != 0)
+      v.normalize();
+   char* returnBuffer = Con::getReturnBuffer(256);
+   dSprintf(returnBuffer,256,"%g %g %g",v.x,v.y,v.z);
+   return returnBuffer;
+}
+
+ConsoleFunction( VectorDot, F32, 3, 3, "(Vector3F a, Vector3F b) Calculate the dot product of a and b.")
+{
+   VectorF v1(0,0,0),v2(0,0,0);
+   dSscanf(argv[1],"%g %g %g",&v1.x,&v1.y,&v1.z);
+   dSscanf(argv[2],"%g %g %g",&v2.x,&v2.y,&v2.z);
+   return mDot(v1,v2);
+}
+
+ConsoleFunction(VectorCross, const char*, 3, 3, "(Vector3F a, Vector3F b) Calculate the cross product of a and b.")
+{
+   VectorF v1(0,0,0),v2(0,0,0);
+   dSscanf(argv[1],"%g %g %g",&v1.x,&v1.y,&v1.z);
+   dSscanf(argv[2],"%g %g %g",&v2.x,&v2.y,&v2.z);
+   VectorF v;
+   mCross(v1,v2,&v);
+   char* returnBuffer = Con::getReturnBuffer(256);
+   dSprintf(returnBuffer,256,"%g %g %g",v.x,v.y,v.z);
+   return returnBuffer;
+}
+
+ConsoleFunction(VectorDist, F32, 3, 3, "(Vector3F a, Vector3F b) Calculate the distance between a and b.")
+{
+   VectorF v1(0,0,0),v2(0,0,0);
+   dSscanf(argv[1],"%g %g %g",&v1.x,&v1.y,&v1.z);
+   dSscanf(argv[2],"%g %g %g",&v2.x,&v2.y,&v2.z);
+   VectorF v = v2 - v1;
+   return mSqrt((v.x * v.x) + (v.y * v.y) + (v.z * v.z));
+}
+
+ConsoleFunction(VectorLen, F32, 2, 2, "(Vector3F v) Calculate the length of a vector.")
+{
+   VectorF v(0,0,0);
+   dSscanf(argv[1],"%g %g %g",&v.x,&v.y,&v.z);
+   return mSqrt((v.x * v.x) + (v.y * v.y) + (v.z * v.z));
+}
+
+ConsoleFunction( VectorOrthoBasis, const char*, 2, 2, "(AngAxisF aaf) Create an orthogonal basis from the given vector. Return a matrix.")
+{
+   AngAxisF aa;
+   dSscanf(argv[1],"%g %g %g %g", &aa.axis.x,&aa.axis.y,&aa.axis.z,&aa.angle);
+   MatrixF mat;
+   aa.setMatrix(&mat);
+   Point3F col0, col1, col2;
+   mat.getColumn(0, &col0);
+   mat.getColumn(1, &col1);
+   mat.getColumn(2, &col2);
+   char* returnBuffer = Con::getReturnBuffer(256);
+   dSprintf(returnBuffer,256,"%g %g %g %g %g %g %g %g %g",
+            col0.x, col0.y, col0.z, col1.x, col1.y, col1.z, col2.x, col2.y, col2.z);
+   return returnBuffer;
+}
+
+ConsoleFunctionGroupEnd(VectorMath);
+
+ConsoleFunctionGroupBegin(MatrixMath, "Matrix manipulation functions.");
+
+ConsoleFunction( MatrixCreate, const char*, 3, 3, "(Vector3F pos, Vector3F rot) Create a matrix representing the given translation and rotation.")
+{
+   Point3F pos;
+   dSscanf(argv[1], "%g %g %g", &pos.x, &pos.y, &pos.z);
+
+   AngAxisF aa(Point3F(0,0,0),0);
+   dSscanf(argv[2], "%g %g %g %g", &aa.axis.x, &aa.axis.y, &aa.axis.z, &aa.angle);
+   aa.angle = mDegToRad(aa.angle);
+
+   char* returnBuffer = Con::getReturnBuffer(256);
+   dSprintf(returnBuffer, 255, "%g %g %g %g %g %g %g",
+            pos.x, pos.y, pos.z,
+            aa.axis.x, aa.axis.y, aa.axis.z,
+            aa.angle);
+   return returnBuffer;
+}
+
+ConsoleFunction( MatrixCreateFromEuler, const char*, 2, 2, "(Vector3F e) Create a matrix from the given rotations.")
+{
+   EulerF rot;
+   dSscanf(argv[1], "%g %g %g", &rot.x, &rot.y, &rot.z);
+
+   QuatF rotQ(rot);
+   AngAxisF aa;
+   aa.set(rotQ);
+
+   char* ret = Con::getReturnBuffer(256);
+   dSprintf(ret, 255, "0 0 0 %g %g %g %g",aa.axis.x,aa.axis.y,aa.axis.z,aa.angle);
+   return ret;
+}
+
+
+ConsoleFunction( MatrixMultiply, const char*, 3, 3, "(Matrix4F left, Matrix4F right) Multiply the two matrices.")
+{
+   Point3F pos1;
+   AngAxisF aa1(Point3F(0,0,0),0);
+   dSscanf(argv[1], "%g %g %g %g %g %g %g", &pos1.x, &pos1.y, &pos1.z, &aa1.axis.x, &aa1.axis.y, &aa1.axis.z, &aa1.angle);
+
+   MatrixF temp1(true);
+   aa1.setMatrix(&temp1);
+   temp1.setColumn(3, pos1);
+
+   Point3F pos2;
+   AngAxisF aa2(Point3F(0,0,0),0);
+   dSscanf(argv[2], "%g %g %g %g %g %g %g", &pos2.x, &pos2.y, &pos2.z, &aa2.axis.x, &aa2.axis.y, &aa2.axis.z, &aa2.angle);
+
+   MatrixF temp2(true);
+   aa2.setMatrix(&temp2);
+   temp2.setColumn(3, pos2);
+
+   temp1.mul(temp2);
+
+
+   Point3F pos;
+   AngAxisF aa(temp1);
+
+   aa.axis.normalize();
+   temp1.getColumn(3, &pos);
+
+   char* ret = Con::getReturnBuffer(256);
+   dSprintf(ret, 255, "%g %g %g %g %g %g %g",
+            pos.x, pos.y, pos.z,
+            aa.axis.x, aa.axis.y, aa.axis.z,
+            aa.angle);
+   return ret;
+}
+
+
+ConsoleFunction( MatrixMulVector, const char*, 3, 3, "(MatrixF xfrm, Point3F vector) Multiply the vector by the transform.")
+{
+   Point3F pos1;
+   AngAxisF aa1(Point3F(0,0,0),0);
+   dSscanf(argv[1], "%g %g %g %g %g %g %g", &pos1.x, &pos1.y, &pos1.z, &aa1.axis.x, &aa1.axis.y, &aa1.axis.z, &aa1.angle);
+
+   MatrixF temp1(true);
+   aa1.setMatrix(&temp1);
+   temp1.setColumn(3, pos1);
+
+   Point3F vec1;
+   dSscanf(argv[2], "%g %g %g", &vec1.x, &vec1.y, &vec1.z);
+
+   Point3F result;
+   temp1.mulV(vec1, &result);
+
+   char* ret = Con::getReturnBuffer(256);
+   dSprintf(ret, 255, "%g %g %g", result.x, result.y, result.z);
+   return ret;
+}
+
+ConsoleFunction( MatrixMulPoint, const char*, 3, 3, "(MatrixF xfrm, Point3F pnt) Multiply pnt by xfrm.")
+{
+   Point3F pos1;
+   AngAxisF aa1(Point3F(0,0,0),0);
+   dSscanf(argv[1], "%g %g %g %g %g %g %g", &pos1.x, &pos1.y, &pos1.z, &aa1.axis.x, &aa1.axis.y, &aa1.axis.z, &aa1.angle);
+
+   MatrixF temp1(true);
+   aa1.setMatrix(&temp1);
+   temp1.setColumn(3, pos1);
+
+   Point3F vec1;
+   dSscanf(argv[2], "%g %g %g", &vec1.x, &vec1.y, &vec1.z);
+
+   Point3F result;
+   temp1.mulP(vec1, &result);
+
+   char* ret = Con::getReturnBuffer(256);
+   dSprintf(ret, 255, "%g %g %g", result.x, result.y, result.z);
+   return ret;
+}
+
+ConsoleFunctionGroupEnd(MatrixMath);
+
+//------------------------------------------------------------------------------
+
+ConsoleFunction( getBoxCenter, const char*, 2, 2, "(Box b) Get the center point of a box.")
+{
+   Box3F box;
+   box.min.set(0,0,0);
+   box.max.set(0,0,0);
+   dSscanf(argv[1],"%g %g %g %g %g %g",
+           &box.min.x,&box.min.y,&box.min.z,
+           &box.max.x,&box.max.y,&box.max.z);
+   Point3F p;
+   box.getCenter(&p);
+   char* returnBuffer = Con::getReturnBuffer(256);
+   dSprintf(returnBuffer,256,"%g %g %g",p.x,p.y,p.z);
+   return returnBuffer;
+}
+
+
+//------------------------------------------------------------------------------
+ConsoleFunctionGroupBegin(RandomNumbers, "Functions relating to the generation of random numbers.");
+
+ConsoleFunction(setRandomSeed, void, 1, 2, "(int seed=-1) Set the current random seed. If no seed is provided, then the current time in ms is used.")
+{
+	U32 seed = Platform::getRealMilliseconds();
+	if (argc == 2)
+		seed = dAtoi(argv[1]);
+	MRandomLCG::setGlobalRandSeed(seed);
+}
+
+ConsoleFunction(getRandomSeed, S32, 1, 1, "Return the current random seed.")
+{
+   return gRandGen.getSeed();
+}
+
+S32 mRandI(S32 i1, S32 i2)
+{
+   return gRandGen.randI(i1, i2);
+}
+
+F32 mRandF(F32 f1, F32 f2)
+{
+   return gRandGen.randF(f1, f2);
+}
+
+ConsoleFunction(getRandom, F32, 1, 3, "(int a=1, int b=0)"
+                "Get a random number between a and b.")
+{
+   if (argc == 2)
+      return F32(gRandGen.randI(0,dAtoi(argv[1])));
+   else
+   {
+      if (argc == 3) {
+         S32 min = dAtoi(argv[1]);
+         S32 max = dAtoi(argv[2]);
+         if (min > max) {
+            S32 t = min;
+            min = max;
+            max = t;
+         }
+         return F32(gRandGen.randI(min,max));
+      }
+   }
+   return gRandGen.randF();
+}
+
+ConsoleFunctionGroupEnd(RandomNumbers);
+//------------------------------------------------------------------------------

engine/math/mathTypes.h

@@ -0,0 +1,27 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#ifndef _MATHTYPES_H_
+#define _MATHTYPES_H_
+
+#ifndef _DYNAMIC_CONSOLETYPES_H_
+#include "console/dynamicTypes.h"
+#endif
+
+void RegisterMathFunctions(void);
+
+// Define Math Console Types
+DefineConsoleType( TypePoint2I )
+DefineConsoleType( TypePoint2F )
+DefineConsoleType( TypePoint3F )
+DefineConsoleType( TypePoint4F )
+DefineConsoleType( TypeRectI )
+DefineConsoleType( TypeRectF )
+DefineConsoleType( TypeMatrixPosition )
+DefineConsoleType( TypeMatrixRotation )
+DefineConsoleType( TypeBox3F )
+
+
+#endif

engine/math/mathUtils.cc

@@ -0,0 +1,131 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#include "math/mMath.h"
+#include "math/mathUtils.h"
+#include "math/mRandom.h"
+
+namespace MathUtils
+{
+
+MRandomLCG sgRandom(0xdeadbeef); ///< Our random number generator.
+
+//------------------------------------------------------------------------------
+// Creates orientation matrix from a direction vector.  Assumes ( 0 0 1 ) is up.
+//------------------------------------------------------------------------------
+MatrixF createOrientFromDir( Point3F &direction )
+{
+	Point3F j = direction;
+	Point3F k(0.0, 0.0, 1.0);
+	Point3F i;
+	
+	mCross( j, k, &i );
+
+	if( i.magnitudeSafe() == 0.0 )
+	{
+		i = Point3F( 0.0, -1.0, 0.0 );
+	}
+
+	i.normalizeSafe();
+	mCross( i, j, &k );
+
+   MatrixF mat( true );
+   mat.setColumn( 0, i );
+   mat.setColumn( 1, j );
+   mat.setColumn( 2, k );
+
+	return mat;
+}
+
+
+//------------------------------------------------------------------------------
+// Creates random direction given angle parameters similar to the particle system.
+// The angles are relative to the specified axis.
+//------------------------------------------------------------------------------
+Point3F randomDir( 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 / 180.0));
+   AngAxisF phiRot(axis,    phi   * (M_PI / 180.0));
+
+   Point3F ejectionAxis = axis;
+
+   MatrixF temp(true);
+   thetaRot.setMatrix(&temp);
+   temp.mulP(ejectionAxis);
+   phiRot.setMatrix(&temp);
+   temp.mulP(ejectionAxis);
+
+   return ejectionAxis;
+}
+
+
+//------------------------------------------------------------------------------
+// Returns yaw and pitch angles from a given vector.  Angles are in RADIANS.
+// Assumes north is (0.0, 1.0, 0.0), the degrees move upwards clockwise.
+// The range of yaw is 0 - 2PI.  The range of pitch is -PI/2 - PI/2.
+// ASSUMES Z AXIS IS UP
+//------------------------------------------------------------------------------
+void getAnglesFromVector( VectorF &vec, F32 &yawAng, F32 &pitchAng )
+{
+   yawAng = mAtan( vec.x, vec.y );
+
+   if( yawAng < 0.0 )
+   {
+      yawAng += M_2PI;
+   }
+
+   if( fabs(vec.x) > fabs(vec.y) )
+   {
+      pitchAng = mAtan( fabs(vec.z), fabs(vec.x) );
+   }
+   else
+   {
+      pitchAng = mAtan( fabs(vec.z), fabs(vec.y) );
+   }
+
+   if( vec.z < 0.0 )
+   {
+      pitchAng = -pitchAng;
+   }
+
+}
+
+
+//------------------------------------------------------------------------------
+// Returns vector from given yaw and pitch angles.  Angles are in RADIANS.
+// Assumes north is (0.0, 1.0, 0.0), the degrees move upwards clockwise.
+// The range of yaw is 0 - 2PI.  The range of pitch is -PI/2 - PI/2.
+// ASSUMES Z AXIS IS UP
+//------------------------------------------------------------------------------
+void getVectorFromAngles( VectorF &vec, F32 &yawAng, F32 &pitchAng )
+{
+   VectorF  pnt( 0.0, 1.0, 0.0 );
+
+   EulerF   rot( -pitchAng, 0.0, 0.0 );
+   MatrixF  mat( rot );
+
+   rot.set( 0.0, 0.0, yawAng );
+   MatrixF   mat2( rot );
+
+   mat.mulV( pnt );
+   mat2.mulV( pnt );
+
+   vec = pnt;
+
+}
+
+
+
+} // end namespace MathUtils

engine/math/mathUtils.h

@@ -0,0 +1,47 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#ifndef _MATHUTILS_H_
+#define _MATHUTILS_H_
+
+class Point3F;
+class MatrixF;
+
+/// Miscellaneous math utility functions.
+namespace MathUtils
+{
+
+    /// Creates orientation matrix from a direction vector.  Assumes ( 0 0 1 ) is up.
+    MatrixF createOrientFromDir( Point3F &direction );
+
+    /// Creates random direction given angle parameters similar to the particle system.
+    ///
+    /// The angles are relative to the specified axis. Both phi and theta are in degrees.
+    Point3F randomDir( Point3F &axis, F32 thetaAngleMin, F32 thetaAngleMax, F32 phiAngleMin = 0.0, F32 phiAngleMax = 360.0 );
+
+    /// Returns yaw and pitch angles from a given vector.
+    ///
+    /// Angles are in RADIANS.
+    ///
+    /// Assumes north is (0.0, 1.0, 0.0), the degrees move upwards clockwise.
+    ///
+    /// The range of yaw is 0 - 2PI.  The range of pitch is -PI/2 - PI/2.
+    ///
+    /// <b>ASSUMES Z AXIS IS UP</b>
+    void    getAnglesFromVector( VectorF &vec, F32 &yawAng, F32 &pitchAng );
+
+    /// Returns vector from given yaw and pitch angles.
+    ///
+    /// Angles are in RADIANS.
+    ///
+    /// Assumes north is (0.0, 1.0, 0.0), the degrees move upwards clockwise.
+    ///
+    /// The range of yaw is 0 - 2PI.  The range of pitch is -PI/2 - PI/2.
+    ///
+    /// <b>ASSUMES Z AXIS IS UP</b>
+    void    getVectorFromAngles( VectorF &vec, F32 &yawAng, F32 &pitchAng );
+}
+
+#endif // _MATHUTILS_H_