///Diagonalizes a symmetric matrix (M = M^T)
void Diagonalize(Mat3Ptr matrix)
{
if (matrix == NULL)
ErrorIf(matrix == NULL, "Matrix3 - Null pointer passed for matrix.");
Matrix3 quatMatrix = ToMatrix3(CreateDiagonalizer(*matrix));
*matrix = Concat(Concat(quatMatrix, *matrix), quatMatrix.Transposed());
}
Quaternion CreateDiagonalizer(Mat3Param matrix)
{
const unsigned cMaxSteps = 50;
const float cThetaLimit = 1.0e6f;
Quaternion quat(0.0f, 0.0f, 0.0f, 1.0f);
Matrix3 quatMatrix;
Matrix3 diagMatrix;
for(unsigned i = 0; i < cMaxSteps; ++i)
{
ToMatrix3(quat, &quatMatrix);
diagMatrix = Concat(Concat(quatMatrix, matrix), quatMatrix.Transposed());
//Elements not on the diagonal
Vector3 offDiag(diagMatrix(1, 2), diagMatrix(0, 2), diagMatrix(0, 1));
//Magnitude of the off-diagonal elements
Vector3 magDiag = Abs(offDiag);
//Index of the largest element
unsigned k = ((magDiag.x > magDiag.y) && (magDiag.x > magDiag.z)) ? 0 :
((magDiag.y > magDiag.z) ? 1 : 2);
unsigned k1 = (k + 1) % 3;
unsigned k2 = (k + 2) % 3;
//Diagonal already
if(offDiag[k] == 0.0f)
{
break;
}
float theta = (diagMatrix(k2, k2) - diagMatrix(k1, k1)) /
(2.0f * offDiag[k]);
float sign = Math::GetSign(theta);
//Make theta positive
theta *= sign;
//Large term in T
float thetaTerm = theta < 1e6f ? Math::Sqrt(Math::Sq(theta) + 1.0f)
: theta;
//Sign(T) / (|T| + sqrt(T^2 + 1))
float t = sign / (theta + thetaTerm);
//c = 1 / (t^2 + 1) t = s / c
float c = 1.0f / Math::Sqrt(Math::Sq(t) + 1.0f);
//No room for improvement - reached machine precision.
if(c == 1.0f)
{
break;
}
//Jacobi rotation for this iteration
Quaternion jacobi(0.0f, 0.0f, 0.0f, 0.0f);
//Using 1/2 angle identity sin(a/2) = sqrt((1-cos(a))/2)
jacobi[k] = sign * Math::Sqrt((1.0f - c) / 2.0f);
//Since our quat-to-matrix convention was for v*M instead of M*v
jacobi.w = Math::Sqrt(1.0f - Math::Sq(jacobi[k]));
//Reached limits of floating point precision
if(jacobi.w == 1.0f)
{
break;
}
quat *= jacobi;
Normalize(&quat);
}
return quat;
}