public void Normalize_Instance(QuaternionD quat, QuaternionD expected)
{
var actual = quat.Normalize();
Assert.Equal(expected.x, actual.x, 14);
Assert.Equal(expected.y, actual.y, 14);
Assert.Equal(expected.z, actual.z, 14);
Assert.Equal(expected.w, actual.w, 14);
}
public void Normalize_Static(QuaternionD quat, QuaternionD expected)
{
var actual = QuaternionD.Normalize(quat);
Assert.Equal(expected.x, actual.x, 14);
Assert.Equal(expected.y, actual.y, 14);
Assert.Equal(expected.z, actual.z, 14);
Assert.Equal(expected.w, actual.w, 14);
}
/// <summary>
/// Diagonalizes a given covariance matrix with double precision.
///
/// <para>
/// Credits to: https://github.com/melax/sandbox
/// http://melax.github.io/diag.html
/// A must be a symmetric matrix.
/// returns quaternion q such that its corresponding matrix Q
/// can be used to Diagonalize A
/// Diagonal matrix D = transpose(Q) * A * (Q); thus A == Q * D * QT
/// The directions of Q (cols of Q) are the eigenvectors D's diagonal is the eigenvalues.
/// As per 'col' convention if float3x3 Q = qgetmatrix(q); then Q*v = q*v*conj(q).
/// </para>
/// </summary>
/// <param name="A">The covariance matrix</param>
/// <returns></returns>
public static EigenD Diagonalizer(double4x4 A)
{
const int maxsteps = 512; // certainly wont need that many.
var q = new QuaternionD(0, 0, 0, 1);
var D = double4x4.Identity;
var Q = double4x4.Identity;
for (var i = 0; i < maxsteps; i++)
{
// Q = float4x4.CreateRotation(q); // v*Q == q*v*conj(q)
Q = q.ToRotMat(); // v*Q == q*v*conj(q)
D = Q.Transpose() * A * Q; // A = Q^T*D*Q
var offDiagonal = new double3(D.M23, D.M13, D.M12); // elements not on the diagonal
var om = new double3(System.Math.Abs(offDiagonal.x), System.Math.Abs(offDiagonal.y), System.Math.Abs(offDiagonal.z)); // mag of each offdiag elem
var k = (om.x > om.y && om.x > om.z) ? 0 : (om.y > om.z) ? 1 : 2; // index of largest element of offdiag
var k1 = (k + 1) % 3;
var k2 = (k + 2) % 3;
if (offDiagonal[k].Equals(0.0f))
{
break; // diagonal already
}
var theta = (D[k2, k2] - D[k1, k1]) / (2.0f * offDiagonal[k]);
var sgn = (theta > 0.0f) ? 1.0f : -1.0f;
theta *= sgn; // make it positive
var t = sgn / (theta + ((theta < 1.0e+6f) ? System.Math.Sqrt(theta * theta + 1.0f) : theta)); // sign(T)/(|T|+sqrt(T^2+1))
var c = 1.0f / System.Math.Sqrt(t * t + 1.0f); // c= 1/(t^2+1) , t=s/c
if (c.Equals(1.0f))
{
break; // no room for improvement - reached machine precision.
}
var jr = new QuaternionD(0, 0, 0, 0) // jacobi rotation for this iteration.
{
[k] = (sgn * System.Math.Sqrt((1.0f - c) / 2.0f))
};
// using 1/2 angle identity sin(a/2) = sqrt((1-cos(a))/2)
jr[k] *= -1.0f; // note we want a final result semantic that takes D to A, not A to D
jr.w = System.Math.Sqrt(1.0f - (jr[k] * jr[k]));
if (jr.w.Equals(1.0f))
{
break; // reached limits of floating point precision
}
q *= jr;
q.Normalize();
}
var vectorMat = Q;
return(new EigenD
{
Values = new[]
{
D.M11,
D.M22,
D.M33
},
Vectors = new double3[] { vectorMat.Row0.xyz, vectorMat.Row1.xyz, vectorMat.Row2.xyz }
});
}