Пример #1
0
// 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
}
Пример #2
0
		// 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;
			}
		}