+Unity Runtime codeHEADmaster

1 files changed, 596 insertions, 0 deletions

diff --git a/Runtime/Math/Matrix3x3.cpp b/Runtime/Math/Matrix3x3.cpp
new file mode 100644
index 0000000..ac92b70
--- /dev/null
+++ b/Runtime/Math/Matrix3x3.cpp
@@ -0,0 +1,596 @@
+#include "UnityPrefix.h"
+#include "Matrix3x3.h"
+#include "Matrix4x4.h"
+using namespace std;
+
+namespace
+{
+	Matrix3x3f CreateIdentityMatrix3x3f ()
+	{
+		Matrix3x3f temp; 
+		temp.SetIdentity (); 
+		return temp;
+	}
+
+	Matrix3x3f CreateZeroMatrix3x3f ()
+	{
+		Matrix3x3f temp; 
+		temp.SetZero (); 
+		return temp;
+	}
+}
+
+const Matrix3x3f Matrix3x3f::identity = CreateIdentityMatrix3x3f ();
+const Matrix3x3f Matrix3x3f::zero = CreateZeroMatrix3x3f ();
+
+void GetRotMatrixNormVec (float* out, const float* inVec, float radians);
+
+Matrix3x3f& Matrix3x3f::operator = (const Matrix4x4f& other)
+{
+	m_Data[0] = other.m_Data[0];
+	m_Data[1] = other.m_Data[1];
+	m_Data[2] = other.m_Data[2];
+
+	m_Data[3] = other.m_Data[4];
+	m_Data[4] = other.m_Data[5];
+	m_Data[5] = other.m_Data[6];
+
+	m_Data[6] = other.m_Data[8];
+	m_Data[7] = other.m_Data[9];
+	m_Data[8] = other.m_Data[10];
+	return *this;
+}
+
+Matrix3x3f::Matrix3x3f (const Matrix4x4f& other)
+{
+	m_Data[0] = other.m_Data[0];
+	m_Data[1] = other.m_Data[1];
+	m_Data[2] = other.m_Data[2];
+
+	m_Data[3] = other.m_Data[4];
+	m_Data[4] = other.m_Data[5];
+	m_Data[5] = other.m_Data[6];
+
+	m_Data[6] = other.m_Data[8];
+	m_Data[7] = other.m_Data[9];
+	m_Data[8] = other.m_Data[10];
+}
+
+Matrix3x3f& Matrix3x3f::SetIdentity ()
+{
+	Get (0, 0) = 1.0F;	Get (0, 1) = 0.0F;	Get (0, 2) = 0.0F;
+	Get (1, 0) = 0.0F;	Get (1, 1) = 1.0F;	Get (1, 2) = 0.0F;
+	Get (2, 0) = 0.0F;	Get (2, 1) = 0.0F;	Get (2, 2) = 1.0F;
+	return *this;
+}
+
+Matrix3x3f& Matrix3x3f::SetZero ()
+{
+	Get (0, 0) = 0.0F;	Get (0, 1) = 0.0F;	Get (0, 2) = 0.0F;
+	Get (1, 0) = 0.0F;	Get (1, 1) = 0.0F;	Get (1, 2) = 0.0F;
+	Get (2, 0) = 0.0F;	Get (2, 1) = 0.0F;	Get (2, 2) = 0.0F;
+	return *this;
+}
+
+Matrix3x3f& Matrix3x3f::SetOrthoNormalBasis (const Vector3f& inX, const Vector3f& inY, const Vector3f& inZ)
+{
+	Get (0, 0) = inX[0];	Get (0, 1) = inY[0];	Get (0, 2) = inZ[0];
+	Get (1, 0) = inX[1];	Get (1, 1) = inY[1];	Get (1, 2) = inZ[1];
+	Get (2, 0) = inX[2];	Get (2, 1) = inY[2];	Get (2, 2) = inZ[2];
+	return *this;
+}
+
+Matrix3x3f& Matrix3x3f::SetOrthoNormalBasisInverse (const Vector3f& inX, const Vector3f& inY, const Vector3f& inZ)
+{
+	Get (0, 0) = inX[0];	Get (1, 0) = inY[0];	Get (2, 0) = inZ[0];
+	Get (0, 1) = inX[1];	Get (1, 1) = inY[1];	Get (2, 1) = inZ[1];
+	Get (0, 2) = inX[2];	Get (1, 2) = inY[2];	Get (2, 2) = inZ[2];
+	return *this;
+}
+
+Matrix3x3f& Matrix3x3f::SetScale (const Vector3f& inScale)
+{
+	Get (0, 0) = inScale[0];	Get (0, 1) = 0.0F;			Get (0, 2) = 0.0F;
+	Get (1, 0) = 0.0F;			Get (1, 1) = inScale[1];	Get (1, 2) = 0.0F;
+	Get (2, 0) = 0.0F;			Get (2, 1) = 0.0F;			Get (2, 2) = inScale[2];
+	return *this;
+}
+
+bool Matrix3x3f::IsIdentity (float threshold) {
+	if (CompareApproximately (Get (0,0),1.0f, threshold) && CompareApproximately (Get (0,1),0.0f, threshold) && CompareApproximately (Get (0,2),0.0f, threshold) &&
+	CompareApproximately (Get (1,0),0.0f, threshold) && CompareApproximately (Get (1,1),1.0f, threshold) && CompareApproximately (Get (1,2),0.0f, threshold) &&
+	CompareApproximately (Get (2,0),0.0f, threshold) && CompareApproximately (Get (2,1),0.0f, threshold) && CompareApproximately (Get (2,2),1.0f, threshold))
+		return true;
+	return false;
+}
+
+
+Matrix3x3f& Matrix3x3f::Scale (const Vector3f& inScale)
+{
+	Get (0, 0) *= inScale[0];
+	Get (1, 0) *= inScale[0];
+	Get (2, 0) *= inScale[0];
+
+	Get (0, 1) *= inScale[1];
+	Get (1, 1) *= inScale[1];
+	Get (2, 1) *= inScale[1];
+
+	Get (0, 2) *= inScale[2];
+	Get (1, 2) *= inScale[2];
+	Get (2, 2) *= inScale[2];
+	return *this;
+}
+
+float Matrix3x3f::GetDeterminant () const
+{
+	float fCofactor0 = Get (0, 0) * Get (1, 1) * Get (2, 2);
+	float fCofactor1 = Get (0, 1) * Get (1, 2) * Get (2, 0);
+	float fCofactor2 = Get (0, 2) * Get (1, 0) * Get (2, 1);
+
+	float fCofactor3 = Get (0, 2) * Get (1, 1) * Get (2, 0);
+	float fCofactor4 = Get (0, 1) * Get (1, 0) * Get (2, 2);
+	float fCofactor5 = Get (0, 0) * Get (1, 2) * Get (2, 1);
+	
+	return fCofactor0 + fCofactor1 + fCofactor2 - fCofactor3 - fCofactor4 - fCofactor5;
+}
+
+Matrix3x3f& Matrix3x3f::Transpose ()
+{
+	swap (Get (0, 1), Get (1, 0));
+	swap (Get (0, 2), Get (2, 0));
+	swap (Get (2, 1), Get (1, 2));
+	return *this;
+}
+/*
+Matrix3x3f& Matrix3x3f::Transpose (const Matrix3x3f& inMat)
+{
+	int i;
+	for (i=0;i<3;i++)
+	{
+		Get (i, 0) = inMat.Get (0, i);
+		Get (i, 1) = inMat.Get (1, i);
+		Get (i, 2) = inMat.Get (2, i);
+	}
+	return *this;
+}
+*/
+
+bool Matrix3x3f::Invert ()
+{
+	///@TODO make a fast but robust inverse matrix 3x3
+	Matrix4x4f m = *this;
+	bool success = InvertMatrix4x4_Full( m.GetPtr(), m.GetPtr() );
+	*this = m;
+	return success;
+
+#if 0
+	////// THIS METHOD IS NUMERICALLY LESS ROBUST
+	// Invert a 3x3 using cofactors.  This is faster than using a generic
+	// Gaussian elimination because of the loop overhead of such a method.
+
+	Matrix3x3f kInverse;
+
+	kInverse.Get (0, 0) = Get (1, 1) * Get (2, 2) - Get (1, 2) * Get (2, 1);
+	kInverse.Get (0, 1) = Get (0, 2) * Get (2, 1) - Get (0, 1) * Get (2, 2);
+	kInverse.Get (0, 2) = Get (0, 1) * Get (1, 2) - Get (0, 2) * Get (1, 1);
+	kInverse.Get (1, 0) = Get (1, 2) * Get (2, 0) - Get (1, 0) * Get (2, 2);
+	kInverse.Get (1, 1) = Get (0, 0) * Get (2, 2) - Get (0, 2) * Get (2, 0);
+	kInverse.Get (1, 2) = Get (0, 2) * Get (1, 0) - Get (0, 0) * Get (1, 2);
+	kInverse.Get (2, 0) = Get (1, 0) * Get (2, 1) - Get (1, 1) * Get (2, 0);
+	kInverse.Get (2, 1) = Get (0, 1) * Get (2, 0) - Get (0, 0) * Get (2, 1);
+	kInverse.Get (2, 2) = Get (0, 0) * Get (1, 1) - Get (0, 1) * Get (1, 0);
+
+	float fDet = Get (0, 0) * kInverse.Get (0, 0) + Get (0, 1) * kInverse.Get (1, 0) + Get (0, 2) * kInverse.Get (2, 0);
+
+	if (Abs (fDet) > Vector3f::epsilon)
+	{
+		kInverse /= fDet;
+		*this = kInverse;
+		return true;
+	}
+	else
+	{
+		SetZero ();
+		return false;
+	}
+	#endif
+}
+
+void Matrix3x3f::InvertTranspose ()
+{	
+	Invert ();
+	Transpose ();
+}
+
+Matrix3x3f& Matrix3x3f::operator *= (float f)
+{
+	for (int i=0;i<9;i++)
+		m_Data[i] *= f;
+	return *this;
+}
+
+Matrix3x3f& Matrix3x3f::operator *= (const Matrix3x3f& inM)
+{
+	int i;
+	for (i=0;i<3;i++)
+	{
+		float v[3] = {Get (i, 0), Get (i, 1), Get (i, 2)};
+		Get (i, 0) = v[0] * inM.Get (0, 0) + v[1] * inM.Get (1, 0) + v[2] * inM.Get (2, 0);
+		Get (i, 1) = v[0] * inM.Get (0, 1) + v[1] * inM.Get (1, 1) + v[2] * inM.Get (2, 1);
+		Get (i, 2) = v[0] * inM.Get (0, 2) + v[1] * inM.Get (1, 2) + v[2] * inM.Get (2, 2);
+	}
+	return *this;
+}
+
+Matrix3x3f& Matrix3x3f::operator *= (const Matrix4x4f& inM)
+{
+	int i;
+	for (i=0;i<3;i++)
+	{
+		float v[3] = {Get (i, 0), Get (i, 1), Get (i, 2)};
+		Get (i, 0) = v[0] * inM.Get (0, 0) + v[1] * inM.Get (1, 0) + v[2] * inM.Get (2, 0);
+		Get (i, 1) = v[0] * inM.Get (0, 1) + v[1] * inM.Get (1, 1) + v[2] * inM.Get (2, 1);
+		Get (i, 2) = v[0] * inM.Get (0, 2) + v[1] * inM.Get (1, 2) + v[2] * inM.Get (2, 2);
+	}
+	return *this;
+}
+
+Matrix3x3f& Matrix3x3f::SetAxisAngle (const Vector3f& rotationAxis, float radians)
+{
+	GetRotMatrixNormVec (m_Data, rotationAxis.GetPtr (), radians);
+	return *this;
+}
+
+void fromToRotation(const float from[3], const float to[3],float mtx[3][3]);
+
+Matrix3x3f& Matrix3x3f::SetFromToRotation (const Vector3f& from, const Vector3f& to)
+{
+	float mtx[3][3];
+	fromToRotation (from.GetPtr (), to.GetPtr (), mtx);
+	Get (0, 0) = mtx[0][0];	Get (0, 1) = mtx[0][1];	Get (0, 2) = mtx[0][2];
+	Get (1, 0) = mtx[1][0];	Get (1, 1) = mtx[1][1];	Get (1, 2) = mtx[1][2];
+	Get (2, 0) = mtx[2][0];	Get (2, 1) = mtx[2][1];	Get (2, 2) = mtx[2][2];
+	return *this;
+}
+
+inline void MakePositive (Vector3f& euler)
+{
+	const float negativeFlip = -0.0001F;
+	const float positiveFlip = (kPI * 2.0F) - 0.0001F;
+	
+	if (euler.x < negativeFlip)
+		euler.x += 2.0 * kPI;
+	else if (euler.x > positiveFlip)
+		euler.x -= 2.0 * kPI;
+    
+	if (euler.y < negativeFlip)
+		euler.y += 2.0 * kPI;
+	else if (euler.y > positiveFlip)
+		euler.y -= 2.0 * kPI;
+    
+	if (euler.z < negativeFlip)
+		euler.z += 2.0 * kPI;
+	else if (euler.z > positiveFlip)
+		euler.z -= 2.0 * kPI;
+}
+
+inline void SanitizeEuler (Vector3f& euler)
+{
+	MakePositive (euler);
+    /*
+     Vector3f option0 = euler;
+     option0.x = kPI - option0.x;
+     option0.y = kPI - option0.y;
+     option0.z = kPI - option0.z;
+     
+     MakePositive (euler);
+     MakePositive (option0);
+     if (option0.x+option0.y+option0.z < euler.x+euler.y+euler.z)
+     euler = option0;
+     */
+}
+
+void EulerToMatrix (const Vector3f& v, Matrix3x3f& matrix)
+{
+	float cx = cos (v.x);
+	float sx = sin (v.x);
+	float cy = cos (v.y);
+	float sy = sin (v.y);
+	float cz = cos (v.z);
+	float sz = sin (v.z);
+	
+	matrix.Get(0,0) = cy*cz + sx*sy*sz;
+	matrix.Get(0,1) = cz*sx*sy - cy*sz;
+	matrix.Get(0,2) = cx*sy;
+    
+	matrix.Get(1,0) = cx*sz;
+	matrix.Get(1,1) = cx*cz;
+	matrix.Get(1,2) = -sx;
+    
+	matrix.Get(2,0) = -cz*sy + cy*sx*sz;
+	matrix.Get(2,1) = cy*cz*sx + sy*sz;
+	matrix.Get(2,2) = cx*cy;
+}
+
+/// This is YXZ euler conversion
+bool MatrixToEuler (const Matrix3x3f& matrix, Vector3f& v)
+{
+	// from http://www.geometrictools.com/Documentation/EulerAngles.pdf
+	// YXZ order
+	if ( matrix.Get(1,2) < 0.999F ) // some fudge for imprecision
+	{
+		if ( matrix.Get(1,2) > -0.999F ) // some fudge for imprecision
+		{
+			v.x = asin(-matrix.Get(1,2));
+			v.y = atan2(matrix.Get(0,2), matrix.Get(2,2));
+			v.z = atan2(matrix.Get(1,0), matrix.Get(1,1));
+			SanitizeEuler (v);
+            return true;
+        }
+        else
+        {
+            // WARNING.  Not unique.  YA - ZA = atan2(r01,r00)
+            v.x = kPI * 0.5F;
+            v.y = atan2(matrix.Get (0,1), matrix.Get(0,0));
+            v.z = 0.0F;
+			SanitizeEuler (v);
+            
+            return false;
+        }
+    }
+    else
+    {
+        // WARNING.  Not unique.  YA + ZA = atan2(-r01,r00)
+        v.x = -kPI * 0.5F;
+        v.y = atan2(-matrix.Get(0,1),matrix.Get(0,0));
+        v.z = 0.0F;
+ 		SanitizeEuler (v);
+        return false;
+    }
+}
+
+
+#define EPSILON 0.000001
+
+#define CROSS(dest,v1,v2){                 \
+          dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
+          dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
+          dest[2]=v1[0]*v2[1]-v1[1]*v2[0];}
+
+#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])
+
+#define SUB(dest,v1,v2){       \
+          dest[0]=v1[0]-v2[0]; \
+          dest[1]=v1[1]-v2[1]; \
+          dest[2]=v1[2]-v2[2];}
+
+/*
+ * A function for creating a rotation matrix that rotates a vector called
+ * "from" into another vector called "to".
+ * Input : from[3], to[3] which both must be *normalized* non-zero vectors
+ * Output: mtx[3][3] -- a 3x3 matrix in colum-major form
+ * Author: Tomas Möller, 1999
+ */
+void fromToRotation(const float* from, const float* to,float mtx[3][3])
+{	
+	float v[3];
+	float e,h;
+	CROSS(v,from,to);
+	e=DOT(from,to);
+	if(e>1.0-EPSILON)     /* "from" almost or equal to "to"-vector? */
+	{
+		/* return identity */
+		mtx[0][0]=1.0; mtx[0][1]=0.0; mtx[0][2]=0.0;
+		mtx[1][0]=0.0; mtx[1][1]=1.0; mtx[1][2]=0.0;
+		mtx[2][0]=0.0; mtx[2][1]=0.0; mtx[2][2]=1.0;
+	}
+	else if(e<-1.0+EPSILON) /* "from" almost or equal to negated "to"? */
+	{
+		float up[3],left[3];
+		float invlen;
+		float fxx,fyy,fzz,fxy,fxz,fyz;
+		float uxx,uyy,uzz,uxy,uxz,uyz;
+		float lxx,lyy,lzz,lxy,lxz,lyz;
+		/* left=CROSS(from, (1,0,0)) */
+		left[0]=0.0; left[1]=from[2]; left[2]=-from[1];
+		if(DOT(left,left)<EPSILON) /* was left=CROSS(from,(1,0,0)) a good choice? */
+		{
+			/* here we now that left = CROSS(from, (1,0,0)) will be a good choice */
+			left[0]=-from[2]; left[1]=0.0; left[2]=from[0];
+		}
+		/* normalize "left" */
+		invlen=1.0f/sqrt(DOT(left,left));
+		left[0]*=invlen;
+		left[1]*=invlen;
+		left[2]*=invlen;
+		CROSS(up,left,from);
+		/* now we have a coordinate system, i.e., a basis;    */
+		/* M=(from, up, left), and we want to rotate to:      */
+		/* N=(-from, up, -left). This is done with the matrix:*/
+		/* N*M^T where M^T is the transpose of M              */
+		fxx=-from[0]*from[0]; fyy=-from[1]*from[1]; fzz=-from[2]*from[2];
+		fxy=-from[0]*from[1]; fxz=-from[0]*from[2]; fyz=-from[1]*from[2];
+
+		uxx=up[0]*up[0]; uyy=up[1]*up[1]; uzz=up[2]*up[2];
+		uxy=up[0]*up[1]; uxz=up[0]*up[2]; uyz=up[1]*up[2];
+
+		lxx=-left[0]*left[0]; lyy=-left[1]*left[1]; lzz=-left[2]*left[2];
+		lxy=-left[0]*left[1]; lxz=-left[0]*left[2]; lyz=-left[1]*left[2];
+		/* symmetric matrix */
+		mtx[0][0]=fxx+uxx+lxx; mtx[0][1]=fxy+uxy+lxy; mtx[0][2]=fxz+uxz+lxz;
+		mtx[1][0]=mtx[0][1];   mtx[1][1]=fyy+uyy+lyy; mtx[1][2]=fyz+uyz+lyz;
+		mtx[2][0]=mtx[0][2];   mtx[2][1]=mtx[1][2];   mtx[2][2]=fzz+uzz+lzz;
+	}
+	else  /* the most common case, unless "from"="to", or "from"=-"to" */
+	{
+		/* ...otherwise use this hand optimized version (9 mults less) */
+		float hvx,hvz,hvxy,hvxz,hvyz;
+		h=(1.0f-e)/DOT(v,v);
+		hvx=h*v[0];
+		hvz=h*v[2];
+		hvxy=hvx*v[1];
+		hvxz=hvx*v[2];
+		hvyz=hvz*v[1];
+		mtx[0][0]=e+hvx*v[0]; mtx[0][1]=hvxy-v[2];     mtx[0][2]=hvxz+v[1];
+		mtx[1][0]=hvxy+v[2];  mtx[1][1]=e+h*v[1]*v[1]; mtx[1][2]=hvyz-v[0];
+		mtx[2][0]=hvxz-v[1];  mtx[2][1]=hvyz+v[0];     mtx[2][2]=e+hvz*v[2];
+	}
+}
+
+// Right handed
+bool LookRotationToMatrix (const Vector3f& viewVec, const Vector3f& upVec, Matrix3x3f* m)
+{
+	Vector3f z = viewVec;
+	// compute u0
+	float mag = Magnitude (z);
+	if (mag < Vector3f::epsilon)
+	{
+		m->SetIdentity();
+		return false;
+	}
+	z /= mag;
+
+	Vector3f x = Cross (upVec, z);
+	mag = Magnitude (x);
+	if (mag < Vector3f::epsilon)
+	{
+		m->SetIdentity();
+		return false;
+	}
+	x /= mag;
+	
+	Vector3f y (Cross (z, x));
+	if (!CompareApproximately (SqrMagnitude (y), 1.0F))
+		return false;
+	
+	m->SetOrthoNormalBasis (x, y, z);
+	return true;	
+}
+/*
+//Left handed
+bool LookRotationToMatrixLeftHanded (const Vector3f& viewVec, const Vector3f& upVec, Matrix3x3f* m)
+{
+	Vector3f z = viewVec;
+	// compute u0
+	float mag = Magnitude (z);
+	if (mag < Vector3f::epsilon)
+		return false;
+	z /= mag;
+
+	Vector3f x = Cross (z, upVec);
+	mag = Magnitude (x);
+	if (mag < Vector3f::epsilon)
+		return false;
+	x /= mag;
+	
+	Vector3f y (Cross (x, z));
+	if (!CompareApproximately (SqrMagnitude (y), 1.0F))
+		return false;
+	
+	m->SetOrthoNormalBasis (x, y, z);
+	return true;
+}
+*/
+
+void GetRotMatrixNormVec (float* out, const float* inVec, float radians) 
+{
+  /* This function contributed by Erich Boleyn (erich@uruk.org) */
+  /* This function used from the Mesa OpenGL code (matrix.c)  */
+  float s, c;
+  float vx, vy, vz, xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
+  
+  s = sin (radians);
+  c = cos (radians);
+    
+  vx = inVec[0];
+  vy = inVec[1];
+  vz = inVec[2];
+  
+#define M(row,col)  out[row*3 + col]
+  /*
+  *     Arbitrary axis rotation matrix.
+  *
+  *  This is composed of 5 matrices, Rz, Ry, T, Ry', Rz', multiplied
+  *  like so:  Rz * Ry * T * Ry' * Rz'.  T is the final rotation
+  *  (which is about the X-axis), and the two composite transforms
+  *  Ry' * Rz' and Rz * Ry are (respectively) the rotations necessary
+  *  from the arbitrary axis to the X-axis then back.  They are
+  *  all elementary rotations.
+  *
+  *  Rz' is a rotation about the Z-axis, to bring the axis vector
+  *  into the x-z plane.  Then Ry' is applied, rotating about the
+  *  Y-axis to bring the axis vector parallel with the X-axis.  The
+  *  rotation about the X-axis is then performed.  Ry and Rz are
+  *  simply the respective inverse transforms to bring the arbitrary
+  *  axis back to its original orientation.  The first transforms
+  *  Rz' and Ry' are considered inverses, since the data from the
+  *  arbitrary axis gives you info on how to get to it, not how
+  *  to get away from it, and an inverse must be applied.
+  *
+  *  The basic calculation used is to recognize that the arbitrary
+  *  axis vector (x, y, z), since it is of unit length, actually
+  *  represents the sines and cosines of the angles to rotate the
+  *  X-axis to the same orientation, with theta being the angle about
+  *  Z and phi the angle about Y (in the order described above)
+  *  as follows:
+  *
+  *  cos ( theta ) = x / sqrt ( 1 - z^2 )
+  *  sin ( theta ) = y / sqrt ( 1 - z^2 )
+  *
+  *  cos ( phi ) = sqrt ( 1 - z^2 )
+  *  sin ( phi ) = z
+  *
+  *  Note that cos ( phi ) can further be inserted to the above
+  *  formulas:
+  *
+  *  cos ( theta ) = x / cos ( phi )
+  *  sin ( theta ) = y / cos ( phi )
+  *
+  *  ...etc.  Because of those relations and the standard trigonometric
+  *  relations, it is pssible to reduce the transforms down to what
+  *  is used below.  It may be that any primary axis chosen will give the
+  *  same results (modulo a sign convention) using thie method.
+  *
+  *  Particularly nice is to notice that all divisions that might
+  *  have caused trouble when parallel to certain planes or
+  *  axis go away with care paid to reducing the expressions.
+  *  After checking, it does perform correctly under all cases, since
+  *  in all the cases of division where the denominator would have
+  *  been zero, the numerator would have been zero as well, giving
+  *  the expected result.
+  */
+  
+  xx = vx * vx;
+  yy = vy * vy;
+  zz = vz * vz;
+  xy = vx * vy;
+  yz = vy * vz;
+  zx = vz * vx;
+  xs = vx * s;
+  ys = vy * s;
+  zs = vz * s;
+  one_c = 1.0F - c;
+  
+  M(0,0) = (one_c * xx) + c;
+  M(1,0) = (one_c * xy) - zs;
+  M(2,0) = (one_c * zx) + ys;
+  
+  M(0,1) = (one_c * xy) + zs;
+  M(1,1) = (one_c * yy) + c;
+  M(2,1) = (one_c * yz) - xs;
+  
+  M(0,2) = (one_c * zx) - ys;
+  M(1,2) = (one_c * yz) + xs;
+  M(2,2) = (one_c * zz) + c;
+  
+#undef M
+}
+
+
+void OrthoNormalize (Matrix3x3f& matrix)
+{
+	Vector3f* c0 = (Vector3f*)matrix.GetPtr () + 0;
+	Vector3f* c1 = (Vector3f*)matrix.GetPtr () + 3;
+	Vector3f* c2 = (Vector3f*)matrix.GetPtr () + 6;
+	OrthoNormalize (c0, c1, c2);
+}