#include "rparticle/Utilities/Matrix3x3.h"
#include "rparticle/Math/FloatConversion.h"

using namespace std;
USING_NS_RRP;

namespace
{
	Matrix3x3f CreateIdentityMatrix3x3f ()
	{
		Matrix3x3f temp; 
		temp.SetIdentity (); 
		return temp;
	}

	Matrix3x3f CreateZeroMatrix3x3f ()
	{
		Matrix3x3f temp; 
		temp.SetZero (); 
		return temp;
	}
}

void QuaternionToMatrix (const Quaternionf& q, Matrix3x3f& m)
{
        // If q is guaranteed to be a unit quaternion, s will always
        // be 1.  In that case, this calculation can be optimized out.
#if DEBUGMODE
    if (!CompareApproximately (SqrMagnitude (q), 1.0F, Vector3f::epsilon))
        {
        AssertString(Format("Quaternion To Matrix conversion failed because input Quaternion is invalid {%f, %f, %f, %f} l=%f", q.x, q.y, q.z, q.w, SqrMagnitude(q)));
        }
#endif
        //float norm = GetNorm (q);
        //float s = (norm > 0.0) ? 2.0/norm : 0;
    
        // Precalculate coordinate products
    float x = q.x * 2.0F;
    float y = q.y * 2.0F;
    float z = q.z * 2.0F;
    float xx = q.x * x;
    float yy = q.y * y;
    float zz = q.z * z;
    float xy = q.x * y;
    float xz = q.x * z;
    float yz = q.y * z;
    float wx = q.w * x;
    float wy = q.w * y;
    float wz = q.w * z;
    
        // Calculate 3x3 matrix from orthonormal basis
    m.m_Data[0] = 1.0f - (yy + zz);
    m.m_Data[1] = xy + wz;
    m.m_Data[2] = xz - wy;
    
    m.m_Data[3] = xy - wz;
    m.m_Data[4] = 1.0f - (xx + zz);
    m.m_Data[5] = yz + wx;
    
    m.m_Data[6]  = xz + wy;
    m.m_Data[7]  = yz - wx;
    m.m_Data[8] = 1.0f - (xx + yy);
}


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[0];
	m_Data[1] = other.m[1];
	m_Data[2] = other.m[2];

	m_Data[3] = other.m[4];
	m_Data[4] = other.m[5];
	m_Data[5] = other.m[6];

	m_Data[6] = other.m[8];
	m_Data[7] = other.m[9];
	m_Data[8] = other.m[10];
	return *this;
}

Matrix3x3f::Matrix3x3f (const Matrix4x4f& other)
{
	m_Data[0] = other.m[0];
	m_Data[1] = other.m[1];
	m_Data[2] = other.m[2];

	m_Data[3] = other.m[4];
	m_Data[4] = other.m[5];
	m_Data[5] = other.m[6];

	m_Data[6] = other.m[8];
	m_Data[7] = other.m[9];
	m_Data[8] = other.m[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.x;	Get (0, 1) = inY.x;	Get (0, 2) = inZ.x;
    Get (1, 0) = inX.y;	Get (1, 1) = inY.y;	Get (1, 2) = inZ.y;
	Get (2, 0) = inX.z;	Get (2, 1) = inY.z;	Get (2, 2) = inZ.z;
	return *this;
}

Matrix3x3f& Matrix3x3f::SetOrthoNormalBasisInverse (const Vector3f& inX, const Vector3f& inY, const Vector3f& inZ)
{
	Get (0, 0) = inX.x;	Get (1, 0) = inY.x;	Get (2, 0) = inZ.x;
	Get (0, 1) = inX.y;	Get (1, 1) = inY.y;	Get (2, 1) = inZ.y;
	Get (0, 2) = inX.z;	Get (1, 2) = inY.z;	Get (2, 2) = inZ.z;
	return *this;
}

Matrix3x3f& Matrix3x3f::SetScale (const Vector3f& inScale)
{
	Get (0, 0) = inScale.x;	Get (0, 1) = 0.0F;			Get (0, 2) = 0.0F;
	Get (1, 0) = 0.0F;			Get (1, 1) = inScale.y;	Get (1, 2) = 0.0F;
	Get (2, 0) = 0.0F;			Get (2, 1) = 0.0F;			Get (2, 2) = inScale.z;
	return *this;
}

bool Matrix3x3f::IsIdentity (float threshold) {

	return false;
}


Matrix3x3f& Matrix3x3f::Scale (const Vector3f& inScale)
{
	Get (0, 0) *= inScale.x;
	Get (1, 0) *= inScale.x;
	Get (2, 0) *= inScale.x;

	Get (0, 1) *= inScale.y;
	Get (1, 1) *= inScale.y;
	Get (2, 1) *= inScale.y;

	Get (0, 2) *= inScale.z;
	Get (1, 2) *= inScale.z;
	Get (2, 2) *= inScale.z;
	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 1
	////// 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) > 0.00001F)
	{
		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)
{
    float rotationAxisPtr[3] = {rotationAxis.x, rotationAxis.y, rotationAxis.z};
	GetRotMatrixNormVec (m_Data, rotationAxisPtr, 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];
    float fromPtr[3] = {from.x, from.y, from.z};
    float toPtr[3] = {to.x, to.y, to.z};
	fromToRotation (fromPtr, toPtr, 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 = z / mag;

	Vector3f x = Cross (upVec, z);
	mag = Magnitude (x);
	if (mag < Vector3f_Epsilon)
	{
		m->SetIdentity();
		return false;
	}
	x = 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);
}


Matrix4x4f& converterMatrix(Matrix4x4f& self, const Matrix3x3f& other)
{
    self.m[0] = other.m_Data[0];
    self.m[1] = other.m_Data[1];
    self.m[2] = other.m_Data[2];
    self.m[3] = 0.0F;
    
    self.m[4] = other.m_Data[3];
    self.m[5] = other.m_Data[4];
    self.m[6] = other.m_Data[5];
    self.m[7] = 0.0F;
    
    self.m[8] = other.m_Data[6];
    self.m[9] = other.m_Data[7];
    self.m[10] = other.m_Data[8];
    self.m[11] = 0.0F;
    
    self.m[12] = 0.0F;
    self.m[13] = 0.0F;
    self.m[14] = 0.0F;
    self.m[15] = 1.0F;
    return self;
}