// Calculate the Eigensystem of a 4x4 matrix
int EigenSystem(const Matrix44& rOrig, Vector4& rValues, Matrix44& rVectors)
{
rValues.Zero();
rVectors.One();
int nRot = 0;
Matrix44 a(rOrig); // Copy Input Matrix
double sm, tresh, d;
for(int g=0;g<50;++g)
{
// Test for convergence
sm = (fabs(a(0,1)) + fabs(a(0,2)) + fabs(a(0,3))
+ fabs(a(1,2)) + fabs(a(1,3)) + fabs(a(2,3)));
if(sm == 0.0)
{
rValues[0] = nearZero(a(0,0));
rValues[1] = nearZero(a(1,1));
rValues[2] = nearZero(a(2,2));
rValues[3] = nearZero(a(3,3));
for(Matrix44::Pos i=0;i<3;++i)
for(Matrix44::Pos j=0;j<3;++j)
rVectors(i,j) = nearZero(rVectors(i,j));
return nRot;
}
tresh = ((g < 4) ? 0.0125*sm : 0.0);
// Rotate each off diagonal if greater than threshold
for(int i=0;i<4;++i)
{
for(int j=i+1;j<4;++j)
{
d = 100.0*fabs(a(i,j));
if(g > 4 && fabs(a(i,i))+d == fabs(a(i,i)) && fabs(a(j,j))+d == fabs(a(j,j)))
a(i,j) = 0.0;
else if( fabs(a(i,j)) > tresh)
JacobiRot44(i, j, a, rVectors);
}
}
nRot+=6;
}
return -1; // too many iterations
}
// Normalise functions for various SIMD types.
void NormaliseC( Vector4 &p_Vec )
{
ZED_FLOAT32 LengthSq = p_Vec[ 0 ]*p_Vec[ 0 ] +
p_Vec[ 1 ]*p_Vec[ 1 ] + p_Vec[ 2 ]*p_Vec[ 2 ] +
p_Vec[ 3 ]*p_Vec[ 3 ];
// Check to make sure the value is workable
if( ZED::Arithmetic::IsZero( LengthSq ) )
{
p_Vec.Zero( );
}
else
{
ZED_FLOAT32 Factor =
ZED::Arithmetic::InvSquareRoot(
LengthSq );
p_Vec[ 0 ] *= Factor;
p_Vec[ 1 ] *= Factor;
p_Vec[ 2 ] *= Factor;
p_Vec[ 3 ] *= Factor;
}
}