// 
// Diagonalization of a Symmetric 3x3 Matrix
// by S Melax  2006
//
// see http://www.melax.com/diag
//

Quaternion Diagonalizer(const float3x3 &A)
{
	// A must be a symmetric matrix.
	// returns quaternion q such that its corresponding matrix Q 
	// can be used to Diagonalize A
	// Diagonal matrix D = Q * A * Transpose(Q);  and  A = QT*D*Q
	// The rows of q are the eigenvectors D's diagonal is the eigenvalues
	// As per 'row' convention if float3x3 Q = q.getmatrix(); then v*Q = q*v*conj(q)
	int maxsteps=24;  // certainly wont need that many.
	int i;
	Quaternion q(0,0,0,1);
	for(i=0;i<maxsteps;i++)
	{
		float3x3 Q  = q.getmatrix(); // v*Q == q*v*conj(q)
		float3x3 D  = Q * A * Transpose(Q);  // A = Q^T*D*Q
		float3 offdiag(D[1][2],D[0][2],D[0][1]); // elements not on the diagonal
		float3 om(fabsf(offdiag.x),fabsf(offdiag.y),fabsf(offdiag.z)); // mag of each offdiag elem
		int k = (om.x>om.y&&om.x>om.z)?0: (om.y>om.z)? 1 : 2; // index of largest element of offdiag
		int k1 = (k+1)%3;
		int k2 = (k+2)%3;
		if(offdiag[k]==0.0f) break;  // diagonal already
		float thet = (D[k2][k2]-D[k1][k1])/(2.0f*offdiag[k]);
		float sgn = (thet>0.0f)?1.0f:-1.0f;
		thet    *= sgn; // make it positive
		float t = sgn /(thet +((thet<1.E6f)?sqrtf(sqr(thet)+1.0f):thet)) ; // sign(T)/(|T|+sqrt(T^2+1))
		float c = 1.0f/sqrtf(sqr(t)+1.0f); //  c= 1/(t^2+1) , t=s/c 
		if(c==1.0f) break;  // no room for improvement - reached machine precision.
		Quaternion jr(0,0,0,0); // jacobi rotation for this iteration.
		jr[k] = sgn*sqrtf((1.0f-c)/2.0f);  // using 1/2 angle identity sin(a/2) = sqrt((1-cos(a))/2)  
		jr[k] *= -1.0f; // since our quat-to-matrix convention was for v*M instead of M*v
		jr.w  = sqrtf(1.0f - sqr(jr[k]));
		if(jr.w==1.0f) break; // reached limits of floating point precision
		q =  q*jr;  
		q.Normalize();
	} 
	return q;
}