Torque3D-clone/Engine/lib/bullet/examples/ThirdPartyLibs/BussIK/LinearR4.cpp

/*
*
* Mathematics Subpackage (VrMath)
*
*
* Author: Samuel R. Buss, [email protected].
* Web page: http://math.ucsd.edu/~sbuss/MathCG
*
*
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*
*
*/


#include "LinearR4.h"

#include <assert.h>

const VectorR4 VectorR4::Zero(0.0, 0.0, 0.0, 0.0);
const VectorR4 VectorR4::UnitX( 1.0, 0.0, 0.0, 0.0);
const VectorR4 VectorR4::UnitY( 0.0, 1.0, 0.0, 0.0);
const VectorR4 VectorR4::UnitZ( 0.0, 0.0, 1.0, 0.0);
const VectorR4 VectorR4::UnitW( 0.0, 0.0, 0.0, 1.0);
const VectorR4 VectorR4::NegUnitX(-1.0, 0.0, 0.0, 0.0);
const VectorR4 VectorR4::NegUnitY( 0.0,-1.0, 0.0, 0.0);
const VectorR4 VectorR4::NegUnitZ( 0.0, 0.0,-1.0, 0.0);
const VectorR4 VectorR4::NegUnitW( 0.0, 0.0, 0.0,-1.0);

const Matrix4x4 Matrix4x4::Identity(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
									0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0);


// ******************************************************
// * VectorR4 class - math library functions			*
// * * * * * * * * * * * * * * * * * * * * * * * * * * **

double VectorR4::MaxAbs() const
{
	register double m;
	m = (x>0.0) ? x : -x;
	if ( y>m ) m=y;
	else if ( -y >m ) m = -y;
	if ( z>m ) m=z;
	else if ( -z>m ) m = -z;
	if ( w>m ) m=w;
	else if ( -w>m ) m = -w;
	return m;
}

// ******************************************************
// * Matrix4x4 class - math library functions			*
// * * * * * * * * * * * * * * * * * * * * * * * * * * **


void Matrix4x4::operator*= (const Matrix4x4& B)	// Matrix product
{
	double t1, t2, t3;		// temporary values
	t1 =  m11*B.m11 + m12*B.m21 + m13*B.m31 + m14*B.m41;
	t2 =  m11*B.m12 + m12*B.m22 + m13*B.m32 + m14*B.m42;
	t3 =  m11*B.m13 + m12*B.m23 + m13*B.m33 + m14*B.m43;
	m14 = m11*B.m14 + m12*B.m24 + m13*B.m34 + m14*B.m44;
	m11 = t1;
	m12 = t2;
	m13 = t3;

	t1 =  m21*B.m11 + m22*B.m21 + m23*B.m31 + m24*B.m41;
	t2 =  m21*B.m12 + m22*B.m22 + m23*B.m32 + m24*B.m42;
	t3 =  m21*B.m13 + m22*B.m23 + m23*B.m33 + m24*B.m43;
	m24 = m21*B.m14 + m22*B.m24 + m23*B.m34 + m24*B.m44;
	m21 = t1;
	m22 = t2;
	m23 = t3;

	t1 =  m31*B.m11 + m32*B.m21 + m33*B.m31 + m34*B.m41;
	t2 =  m31*B.m12 + m32*B.m22 + m33*B.m32 + m34*B.m42;
	t3 =  m31*B.m13 + m32*B.m23 + m33*B.m33 + m34*B.m43;
	m34 = m31*B.m14 + m32*B.m24 + m33*B.m34 + m34*B.m44;
	m31 = t1;
	m32 = t2;
	m33 = t3;

	t1 =  m41*B.m11 + m42*B.m21 + m43*B.m31 + m44*B.m41;
	t2 =  m41*B.m12 + m42*B.m22 + m43*B.m32 + m44*B.m42;
	t3 =  m41*B.m13 + m42*B.m23 + m43*B.m33 + m44*B.m43;
	m44 = m41*B.m14 + m42*B.m24 + m43*B.m34 + m44*B.m44;
	m41 = t1;
	m42 = t2;
	m43 = t3;
}

inline void ReNormalizeHelper ( double &a, double &b, double &c, double &d )
{
	register double scaleF = a*a+b*b+c*c+d*d;		// Inner product of Vector-R4
	scaleF = 1.0-0.5*(scaleF-1.0);
	a *= scaleF;
	b *= scaleF;
	c *= scaleF;
	d *= scaleF;
}

Matrix4x4& Matrix4x4::ReNormalize() {
	ReNormalizeHelper( m11, m21, m31, m41 );	// Renormalize first column
	ReNormalizeHelper( m12, m22, m32, m42 );	// Renormalize second column
	ReNormalizeHelper( m13, m23, m33, m43 );	// Renormalize third column
	ReNormalizeHelper( m14, m24, m34, m44 );	// Renormalize fourth column
	double alpha = 0.5*(m11*m12 + m21*m22 + m31*m32 + m41*m42);	//1st and 2nd cols
	double beta  = 0.5*(m11*m13 + m21*m23 + m31*m33 + m41*m43);	//1st and 3rd cols
	double gamma = 0.5*(m11*m14 + m21*m24 + m31*m34 + m41*m44);	//1st and 4nd cols
	double delta = 0.5*(m12*m13 + m22*m23 + m32*m33 + m42*m43);	//2nd and 3rd cols
	double eps   = 0.5*(m12*m14 + m22*m24 + m32*m34 + m42*m44);	//2nd and 4nd cols
	double phi   = 0.5*(m13*m14 + m23*m24 + m33*m34 + m43*m44);	//3rd and 4nd cols
	double temp1, temp2, temp3;
	temp1 = m11 - alpha*m12 - beta*m13 - gamma*m14;
	temp2 = m12 - alpha*m11 - delta*m13 - eps*m14;
	temp3 = m13 - beta*m11 - delta*m12 - phi*m14;
	m14 -= (gamma*m11 + eps*m12 + phi*m13);
	m11 = temp1;
	m12 = temp2;
	m13 = temp3;
	temp1 = m21 - alpha*m22 - beta*m23 - gamma*m24;
	temp2 = m22 - alpha*m21 - delta*m23 - eps*m24;
	temp3 = m23 - beta*m21 - delta*m22 - phi*m24;
	m24 -= (gamma*m21 + eps*m22 + phi*m23);
	m21 = temp1;
	m22 = temp2;
	m23 = temp3;
	temp1 = m31 - alpha*m32 - beta*m33 - gamma*m34;
	temp2 = m32 - alpha*m31 - delta*m33 - eps*m34;
	temp3 = m33 - beta*m31 - delta*m32 - phi*m34;
	m34 -= (gamma*m31 + eps*m32 + phi*m33);
	m31 = temp1;
	m32 = temp2;
	m33 = temp3;
	temp1 = m41 - alpha*m42 - beta*m43 - gamma*m44;
	temp2 = m42 - alpha*m41 - delta*m43 - eps*m44;
	temp3 = m43 - beta*m41 - delta*m42 - phi*m44;
	m44 -= (gamma*m41 + eps*m42 + phi*m43);
	m41 = temp1;
	m42 = temp2;
	m43 = temp3;
	return *this;
}

// ******************************************************
// * LinearMapR4 class - math library functions			*
// * * * * * * * * * * * * * * * * * * * * * * * * * * **


double LinearMapR4::Determinant () const		// Returns the determinant
{
	double Tbt34C12 = m31*m42-m32*m41;		// 2x2 subdeterminants
	double Tbt34C13 = m31*m43-m33*m41;
	double Tbt34C14 = m31*m44-m34*m41;
	double Tbt34C23 = m32*m43-m33*m42;
	double Tbt34C24 = m32*m44-m34*m42;
	double Tbt34C34 = m33*m44-m34*m43;

	double sd11 = m22*Tbt34C34 - m23*Tbt34C24 + m24*Tbt34C23;	// 3x3 subdeterminants
	double sd12 = m21*Tbt34C34 - m23*Tbt34C14 + m24*Tbt34C13;
	double sd13 = m21*Tbt34C24 - m22*Tbt34C14 + m24*Tbt34C12;
	double sd14 = m21*Tbt34C23 - m22*Tbt34C13 + m23*Tbt34C12;

	return ( m11*sd11 - m12*sd12 + m13*sd13 - m14*sd14 );
}

LinearMapR4 LinearMapR4::Inverse() const			// Returns inverse
{

	double Tbt34C12 = m31*m42-m32*m41;		// 2x2 subdeterminants
	double Tbt34C13 = m31*m43-m33*m41;
	double Tbt34C14 = m31*m44-m34*m41;
	double Tbt34C23 = m32*m43-m33*m42;
	double Tbt34C24 = m32*m44-m34*m42;
	double Tbt34C34 = m33*m44-m34*m43;
	double Tbt24C12 = m21*m42-m22*m41;		// 2x2 subdeterminants
	double Tbt24C13 = m21*m43-m23*m41;
	double Tbt24C14 = m21*m44-m24*m41;
	double Tbt24C23 = m22*m43-m23*m42;
	double Tbt24C24 = m22*m44-m24*m42;
	double Tbt24C34 = m23*m44-m24*m43;
	double Tbt23C12 = m21*m32-m22*m31;		// 2x2 subdeterminants
	double Tbt23C13 = m21*m33-m23*m31;
	double Tbt23C14 = m21*m34-m24*m31;
	double Tbt23C23 = m22*m33-m23*m32;
	double Tbt23C24 = m22*m34-m24*m32;
	double Tbt23C34 = m23*m34-m24*m33;

	double sd11 = m22*Tbt34C34 - m23*Tbt34C24 + m24*Tbt34C23;	// 3x3 subdeterminants
	double sd12 = m21*Tbt34C34 - m23*Tbt34C14 + m24*Tbt34C13;
	double sd13 = m21*Tbt34C24 - m22*Tbt34C14 + m24*Tbt34C12;
	double sd14 = m21*Tbt34C23 - m22*Tbt34C13 + m23*Tbt34C12;
	double sd21 = m12*Tbt34C34 - m13*Tbt34C24 + m14*Tbt34C23;	// 3x3 subdeterminants
	double sd22 = m11*Tbt34C34 - m13*Tbt34C14 + m14*Tbt34C13;
	double sd23 = m11*Tbt34C24 - m12*Tbt34C14 + m14*Tbt34C12;
	double sd24 = m11*Tbt34C23 - m12*Tbt34C13 + m13*Tbt34C12;
	double sd31 = m12*Tbt24C34 - m13*Tbt24C24 + m14*Tbt24C23;	// 3x3 subdeterminants
	double sd32 = m11*Tbt24C34 - m13*Tbt24C14 + m14*Tbt24C13;
	double sd33 = m11*Tbt24C24 - m12*Tbt24C14 + m14*Tbt24C12;
	double sd34 = m11*Tbt24C23 - m12*Tbt24C13 + m13*Tbt24C12;
	double sd41 = m12*Tbt23C34 - m13*Tbt23C24 + m14*Tbt23C23;	// 3x3 subdeterminants
	double sd42 = m11*Tbt23C34 - m13*Tbt23C14 + m14*Tbt23C13;
	double sd43 = m11*Tbt23C24 - m12*Tbt23C14 + m14*Tbt23C12;
	double sd44 = m11*Tbt23C23 - m12*Tbt23C13 + m13*Tbt23C12;


	register double detInv = 1.0/(m11*sd11 - m12*sd12 + m13*sd13 - m14*sd14);

	return( LinearMapR4( sd11*detInv, -sd12*detInv, sd13*detInv, -sd14*detInv,
						 -sd21*detInv, sd22*detInv, -sd23*detInv, sd24*detInv,
						 sd31*detInv, -sd32*detInv, sd33*detInv, -sd34*detInv,
						 -sd41*detInv, sd42*detInv, -sd43*detInv, sd44*detInv ) );
}

LinearMapR4& LinearMapR4::Invert() 			// Converts into inverse.
{
	double Tbt34C12 = m31*m42-m32*m41;		// 2x2 subdeterminants
	double Tbt34C13 = m31*m43-m33*m41;
	double Tbt34C14 = m31*m44-m34*m41;
	double Tbt34C23 = m32*m43-m33*m42;
	double Tbt34C24 = m32*m44-m34*m42;
	double Tbt34C34 = m33*m44-m34*m43;
	double Tbt24C12 = m21*m42-m22*m41;		// 2x2 subdeterminants
	double Tbt24C13 = m21*m43-m23*m41;
	double Tbt24C14 = m21*m44-m24*m41;
	double Tbt24C23 = m22*m43-m23*m42;
	double Tbt24C24 = m22*m44-m24*m42;
	double Tbt24C34 = m23*m44-m24*m43;
	double Tbt23C12 = m21*m32-m22*m31;		// 2x2 subdeterminants
	double Tbt23C13 = m21*m33-m23*m31;
	double Tbt23C14 = m21*m34-m24*m31;
	double Tbt23C23 = m22*m33-m23*m32;
	double Tbt23C24 = m22*m34-m24*m32;
	double Tbt23C34 = m23*m34-m24*m33;

	double sd11 = m22*Tbt34C34 - m23*Tbt34C24 + m24*Tbt34C23;	// 3x3 subdeterminants
	double sd12 = m21*Tbt34C34 - m23*Tbt34C14 + m24*Tbt34C13;
	double sd13 = m21*Tbt34C24 - m22*Tbt34C14 + m24*Tbt34C12;
	double sd14 = m21*Tbt34C23 - m22*Tbt34C13 + m23*Tbt34C12;
	double sd21 = m12*Tbt34C34 - m13*Tbt34C24 + m14*Tbt34C23;	// 3x3 subdeterminants
	double sd22 = m11*Tbt34C34 - m13*Tbt34C14 + m14*Tbt34C13;
	double sd23 = m11*Tbt34C24 - m12*Tbt34C14 + m14*Tbt34C12;
	double sd24 = m11*Tbt34C23 - m12*Tbt34C13 + m13*Tbt34C12;
	double sd31 = m12*Tbt24C34 - m13*Tbt24C24 + m14*Tbt24C23;	// 3x3 subdeterminants
	double sd32 = m11*Tbt24C34 - m13*Tbt24C14 + m14*Tbt24C13;
	double sd33 = m11*Tbt24C24 - m12*Tbt24C14 + m14*Tbt24C12;
	double sd34 = m11*Tbt24C23 - m12*Tbt24C13 + m13*Tbt24C12;
	double sd41 = m12*Tbt23C34 - m13*Tbt23C24 + m14*Tbt23C23;	// 3x3 subdeterminants
	double sd42 = m11*Tbt23C34 - m13*Tbt23C14 + m14*Tbt23C13;
	double sd43 = m11*Tbt23C24 - m12*Tbt23C14 + m14*Tbt23C12;
	double sd44 = m11*Tbt23C23 - m12*Tbt23C13 + m13*Tbt23C12;

	register double detInv = 1.0/(m11*sd11 - m12*sd12 + m13*sd13 - m14*sd14);

	m11 = sd11*detInv;
	m12 = -sd21*detInv;
	m13 = sd31*detInv;
	m14 = -sd41*detInv;
	m21 = -sd12*detInv;
	m22 = sd22*detInv;
	m23 = -sd32*detInv;
	m24 = sd42*detInv;
	m31 = sd13*detInv;
	m32 = -sd23*detInv;
	m33 = sd33*detInv;
	m34 = -sd43*detInv;
	m41 = -sd14*detInv;
	m42 = sd24*detInv;
	m43 = -sd34*detInv;
	m44 = sd44*detInv;

	return ( *this );
}

VectorR4 LinearMapR4::Solve(const VectorR4& u) const	// Returns solution
{												
	// Just uses Inverse() for now.
	return ( Inverse()*u );
}

// ******************************************************
// * RotationMapR4 class - math library functions		*
// * * * * * * * * * * * * * * * * * * * * * * * * * * **


// ***************************************************************
// * 4-space vector and matrix utilities						 *
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *

// Returns u * v^T
LinearMapR4 TimesTranspose( const VectorR4& u, const VectorR4& v)
{
	LinearMapR4 result;
	TimesTranspose( u, v, result );
	return result;
}

// The following routines are use to obtain 
// a righthanded orthonormal basis to complement vectors u,v,w.
//		The vectors u,v,w must be unit and orthonormal.
// The value is returned in "rotmat" with the first column(s) of
//    rotmat equal to u,v,w as appropriate.

void GetOrtho( const VectorR4& u,  RotationMapR4& rotmat )
{
	rotmat.SetColumn1(u);
	GetOrtho( 1, rotmat );
}

void GetOrtho( const VectorR4& u,  const VectorR4& v, RotationMapR4& rotmat )
{
	rotmat.SetColumn1(u);
	rotmat.SetColumn2(v);
	GetOrtho( 2, rotmat );
}

void GetOrtho( const VectorR4& u,  const VectorR4& v, const VectorR4& s,
					RotationMapR4& rotmat )
{
	rotmat.SetColumn1(u);
	rotmat.SetColumn2(v);
	rotmat.SetColumn3(s);
	GetOrtho( 3, rotmat );
}

// This final version of GetOrtho is mainly for internal use.
//    It uses a Gram-Schmidt procedure to extend a partial orthonormal
//    basis to a complete orthonormal basis.
//    j = number of columns of rotmat that have already been set.
void GetOrtho( int j, RotationMapR4& rotmat)
{
	if ( j==0 ) {
		rotmat.SetIdentity();
		return;
	}
	if ( j==1 ) {
		rotmat.SetColumn2( -rotmat.m21, rotmat.m11, -rotmat.m41, rotmat.m31 );
		j = 2;
	}

	assert ( rotmat.Column1().Norm()<1.0001 && 0.9999<rotmat.Column1().Norm()
			 && rotmat.Column1().Norm()<1.0001 && 0.9999<rotmat.Column1().Norm()
			 && (rotmat.Column1()^rotmat.Column2()) < 0.001
			 && (rotmat.Column1()^rotmat.Column2()) > -0.001 );

	// 2x2 subdeterminants in first 2 columns

	double d12 = rotmat.m11*rotmat.m22-rotmat.m12*rotmat.m21;	
	double d13 = rotmat.m11*rotmat.m32-rotmat.m12*rotmat.m31;
	double d14 = rotmat.m11*rotmat.m42-rotmat.m12*rotmat.m41;
	double d23 = rotmat.m21*rotmat.m32-rotmat.m22*rotmat.m31;
	double d24 = rotmat.m21*rotmat.m42-rotmat.m22*rotmat.m41;
	double d34 = rotmat.m31*rotmat.m42-rotmat.m32*rotmat.m41;
	VectorR4 vec3;

	if ( j==2 ) {
		if ( d12>0.4 || d12<-0.4 || d13>0.4 || d13<-0.4 
								 || d23>0.4 || d23<-0.4 ) {
			vec3.Set( d23, -d13, d12, 0.0);
		}
		else if ( d24>0.4 || d24<-0.4 || d14>0.4 || d14<-0.4 ) {
			vec3.Set( d24, -d14, 0.0, d12 );
		}
		else {
			vec3.Set( d34, 0.0, -d14, d13 );
		}
		vec3.Normalize();
		rotmat.SetColumn3(vec3);
	}

	// Do the final column

	rotmat.SetColumn4 (
				-rotmat.m23*d34 + rotmat.m33*d24 - rotmat.m43*d23,
				 rotmat.m13*d34 - rotmat.m33*d14 + rotmat.m43*d13,
				-rotmat.m13*d24 + rotmat.m23*d14 - rotmat.m43*d12,
				 rotmat.m13*d23 - rotmat.m23*d13 + rotmat.m33*d12 );

	assert ( 0.99 < (((LinearMapR4)rotmat)).Determinant()
			 && (((LinearMapR4)rotmat)).Determinant() < 1.01 );

}



// *********************************************************************
// Rotation routines												   *
// *********************************************************************

// Rotate unit vector x in the direction of "dir": length of dir is rotation angle.
//		x must be a unit vector.  dir must be perpindicular to x.
VectorR4& VectorR4::RotateUnitInDirection ( const VectorR4& dir)
{	
	assert ( this->Norm()<1.0001 && this->Norm()>0.9999 &&
				(dir^(*this))<0.0001 && (dir^(*this))>-0.0001 );

	double theta = dir.NormSq();
	if ( theta==0.0 ) {
		return *this;
	}
	else {
		theta = sqrt(theta);
		double costheta = cos(theta);
		double sintheta = sin(theta);
		VectorR4 dirUnit = dir/theta;
		*this = costheta*(*this) + sintheta*dirUnit;
		// this->NormalizeFast();
		return ( *this );
	}
}

// RotateToMap returns a RotationMapR4 that rotates fromVec to toVec,
//		leaving the orthogonal subspace fixed.
// fromVec and toVec should be unit vectors
RotationMapR4 RotateToMap( const VectorR4& fromVec, const VectorR4& toVec)
{
	LinearMapR4 result;		
	result.SetIdentity();
	LinearMapR4 temp;
	VectorR4 vPerp = ProjectPerpUnitDiff( toVec, fromVec );
	double sintheta = vPerp.Norm();		// theta = angle between toVec and fromVec
	VectorR4 vProj = toVec-vPerp;
	double costheta = vProj^fromVec;
	if ( sintheta == 0.0 ) {
		// The vectors either coincide (return identity) or directly oppose
		if ( costheta < 0.0 ) {
			result = -result;		// Vectors directly oppose: return -identity.
		}
	}
	else {
		vPerp /= sintheta;						// Normalize
		VectorProjectMap ( fromVec, temp );		// project in fromVec direction
		temp *= (costheta-1.0);		
		result += temp;
		VectorProjectMap ( vPerp, temp );		// Project in vPerp direction
		temp *= (costheta-1.0);
		result += temp;
		TimesTranspose ( vPerp, fromVec, temp ); // temp = (vPerp)*(fromVec^T)
		temp *= sintheta;
		result += temp;
		temp.MakeTranspose();
		result -= temp;							 // (-sintheta)*(fromVec)*(vPerp^T)
	}
	RotationMapR4 rotationResult;
	rotationResult.Set(result);				 // Make explicitly a RotationMapR4
	return rotationResult;
}


// ***************************************************************
//  Stream Output Routines										 *
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *

ostream& operator<< ( ostream& os, const VectorR4& u )
{
	return (os << "<" << u.x << "," << u.y << "," << u.z << "," << u.w << ">");
}