예제 #1
0
	///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());
	}
예제 #2
0
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;
}