/** SGX

The   sss    gggg x   x   
     s      g      x x    Engine
      sss   g  gg   x
         s  g   g  x x          Copyright 1995-2009.
     ssss    gggg x   x         Steven Goodwin. All Rights Reserved.
    
This version has been released under the GNU Public License v2.0.

Please see the license files for details.

File: sgx/src/sgx/core/math/matrix43.cpp

*/

#include "sgx/core/math/math.hpp"

void sgxMatrix43::setRotateX(const tREAL32 theta)
{
tREAL32 s=sgxSin(theta);
tREAL32 c=sgxCos(theta);

	right = sgxVector3::UnitX;
	up.x = 0; up.y = c; up.z = s;
	at.x = 0; at.y = -s; at.z = c;
}

void sgxMatrix43::setRotateY(const tREAL32 theta)
{
tREAL32 s=sgxSin(theta);
tREAL32 c=sgxCos(theta);

	right.x = c; right.y = 0; right.z = -s; 
	up = sgxVector3::UnitY;
	at.x = s; at.y = 0; at.z =  c;
}

void sgxMatrix43::setRotateZ(const tREAL32 theta)
{
tREAL32 s=sgxSin(theta);
tREAL32 c=sgxCos(theta);

	right.x = c; right.y = s; right.z = 0;
	up.x = -s; up.y = c; up.z = 0;
	at = sgxVector3::UnitZ;
}

void
sgxMatrix43::setRotateAxis(const tREAL32 theta, const sgxVector3 &axis)
{
tREAL32 ct = sgxCos(theta);
tREAL32 st = sgxSin(theta);
tREAL32 xx = axis.x * axis.x;
tREAL32 xy = axis.x * axis.y;
tREAL32 xz = axis.x * axis.z;
tREAL32 yy = axis.y * axis.y;
tREAL32 yz = axis.y * axis.z;
tREAL32 zz = axis.z * axis.z;

    /* R = uu' + cos(theta)*(I-uu') + sin(theta)*S
     *
     * S =  0  -z   y    u' = (x, y, z)
     *	    z   0  -x
     *	   -y   x   0
     */
	right.x = xx + ct*(1-xx);
	up.x = xy + ct*(0-xy) - st*axis.z;
	at.x = xz + ct*(0-xz) + st*axis.y;

	right.y = xy + ct*(0-xy) + st*axis.z;
	up.y = yy + ct*(1-yy);
	at.y = yz + ct*(0-yz) - st*axis.x;

	right.z = xz + ct*(0-xz) - st*axis.y;
	up.z = yz + ct*(0-yz) + st*axis.x;
	at.z = zz + ct*(1-zz);
}

sgxMatrix43 sgxMatrix43::operator*(const sgxMatrix43 &m)
{
	sgxMatrix43 out;

	out.right.x = right.x*m.right.x + right.y*m.up.x + right.z*m.at.x + 0;
	out.right.y = right.x*m.right.y + right.y*m.up.y + right.z*m.at.y + 0;
	out.right.z = right.x*m.right.z + right.y*m.up.z + right.z*m.at.z + 0;

	out.up.x = up.x*m.right.x + up.y*m.up.x + up.z*m.at.x;
	out.up.y = up.x*m.right.y + up.y*m.up.y + up.z*m.at.y;
	out.up.z = up.x*m.right.z + up.y*m.up.z + up.z*m.at.z;

	out.at.x = at.x*m.right.x + at.y*m.up.x + at.z*m.at.x;
	out.at.y = at.x*m.right.y + at.y*m.up.y + at.z*m.at.y;
	out.at.z = at.x*m.right.z + at.y*m.up.z + at.z*m.at.z;

	out.pos.x = pos.x*m.right.x + pos.y*m.up.x + pos.z*m.at.x + m.pos.x;
	out.pos.y = pos.x*m.right.y + pos.y*m.up.y + pos.z*m.at.y + m.pos.y;
	out.pos.z = pos.x*m.right.z + pos.y*m.up.z + pos.z*m.at.z + m.pos.z;

	return out;
}

sgxVector3  sgxMatrix43::operator*(const sgxVector3 &v3) {
	sgxVector3 r(
		right.x*v3.x + up.x*v3.y + at.x*v3.z + pos.x,	// x
		right.y*v3.x + up.y*v3.y + at.y*v3.z + pos.y,	// y
		right.x*v3.x + up.z*v3.y + at.z*v3.z + pos.z 	// z
	);

	return r;
}


sgxVector2  sgxMatrix43::operator*(const sgxVector2 &v2) {
	sgxVector2 r(
		right.x*v2.x + up.x*v2.y + pos.x,	// x
		right.y*v2.x + up.y*v2.y + pos.y	// y
	);

	return r;
}

sgxVector2  sgxMatrix43::mul(const tREAL32 x, const tREAL32 y) {
	sgxVector2 r(
		right.x*x + up.x*y + pos.x,	// x
		right.y*x + up.y*y + pos.y	// y
	);

	return r;
}

sgxVector2  sgxMatrix43::mul(const tINT32 x, const tINT32 y) {
	sgxVector2 r(
		right.x*x + up.x*y + pos.x,	// x
		right.y*x + up.y*y + pos.y	// y
	);

	return r;
}



tREAL32 
sgxMatrix43::determinant() const {
	tREAL32 a0 = right.x*up.y - up.x*right.y;
	tREAL32 a1 = right.x*at.y - at.x*right.y;
	tREAL32 a3 = up.x*at.y - at.x*up.y;

	return a0*at.z - a1*up.z + a3*right.z;
}

tBOOL 
sgxMatrix43::inverse(sgxMatrix43 &result) const {
	// Use the temporary, pre-compute, idea from:
	// http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix4.inl

	tREAL32 a0 = right.x*up.y - up.x*right.y;
	tREAL32 a1 = right.x*at.y - at.x*right.y;
	tREAL32 a2 = right.x*pos.y - pos.x*right.y;
	tREAL32 a3 = up.x*at.y - at.x*up.y;
	tREAL32 a4 = up.x*pos.y - pos.x*up.y;
	tREAL32 a5 = at.x*pos.y - pos.x*at.y;

	// we still have 0 0 0 1 in pseudo-bottom line of the matrix
	//tREAL32 b0 = 0;
	//tREAL32 b1 = 0;
	tREAL32 b2 = right.z;
	//tREAL32 b3 = 0;
	tREAL32 b4 = up.z;
	tREAL32 b5 = at.z;

	tREAL32 det = a0*b5 - a1*b4 + a3*b2;
	if (det == 0) {
		return FALSE;
	}
	tREAL32 invDet = 1.0f / det;

	result.right.x = up.y*b5 - at.y*b4;
	result.right.y = -right.y*b5 + at.y*b2;
	result.right.z = right.y*b4 - up.y*b2;

	result.up.x = -up.x*b5 + at.x*b4;
	result.up.y = right.x*b5 - at.x*b2;
	result.up.z = -right.x*b4 + up.x*b2;

	result.at.x = a3;		// reduced from  up.w*a5 - at.w*a4 + pos.w*a3;
	result.at.y = -a1;		// reduced from -right.w*a5 + at.w*a2 - pos.w*a1;
	result.at.z = a0;		// reduced from right.w*a4 - up.w*a2 + pos.w*a0;

	result.pos.x = -up.z*a5 + at.z*a4 - pos.z*a3;
	result.pos.y = right.z*a5 - at.z*a2 + pos.z*a1;
	result.pos.z = -right.z*a4 + up.z*a2 - pos.z*a0;

	result.right *= invDet;
	result.up *= invDet;
	result.at *= invDet;
	result.pos *= invDet;

	return TRUE;
}

sgxMatrix43 	sgxMatrix43::inverse() const {
	sgxMatrix43 ans;
	inverse(ans);
	return ans;
}

sgxMatrix44 	sgxMatrix43::inverse44() const {
	sgxMatrix44 ans44(inverse());
	return ans44;
}

