LinearR4.cpp

/*
 *
 * RayTrace Software Package, release 1.0.1,  May 9, 2002.
 *
 * Mathematics Subpackage (VrMath)
 *
 * Author: Samuel R. Buss
 *
 * Software accompanying the book
 *		3D Computer Graphics: A Mathematical Introduction with OpenGL,
 *		by S. Buss, Cambridge University Press, 2003.
 *
 * Software is "as-is" and carries no warranty.  It may be used without
 *   restriction, but if you modify it, please change the filenames to
 *   prevent confusion between different versions.  Please acknowledge
 *   all use of the software in any publications or products based on it.
 *
 * Bug reports: Sam Buss, sbuss@ucsd.edu.
 * Web page: http://math.ucsd.edu/~sbuss/MathCG
 *
 */

#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 << ">");
}