/// <summary>
/// Returns true if A is successfully diagonalized within the specified number of steps.
/// </summary>
/// <param name="A"></param>
/// <param name="V"></param>
/// <param name="epsilon"></param>
/// <param name="maxSteps"></param>
/// <returns></returns>
private static bool DiagonalizeJacobi(ref Matrix3d A, ref Matrix3d V, double epsilon = D.ZeroTolerance, int maxSteps = 16)
{
// impl ref
// https://www2.units.it/ipl/students_area/imm2/files/Numerical_Recipes.pdf (11.1)
// TODO optimize - no need to build the full 3x3 rotation matrix at each step
Matrix3d P;
while (maxSteps-- > 0)
{
var a01 = Math.Abs(A.M01);
var a02 = Math.Abs(A.M02);
var a12 = Math.Abs(A.M12);
// Create Jacobi rotation P from max off-diagonal value of A
if (a01 > a02 && a01 > a12)
{
if (a01 < epsilon)
{
return(true);
}
GetJacobiRotation01(ref A, out P);
}
else if (a02 > a12)
{
if (a02 < epsilon)
{
return(true);
}
GetJacobiRotation02(ref A, out P);
}
else
{
if (a12 < epsilon)
{
return(true);
}
GetJacobiRotation12(ref A, out P);
}
// Apply Jacobi rotation
A = P.ApplyTranspose(A.Apply(ref P)); // A' = Pt A P
V = V.Apply(ref P); // V' = V P
}
return(false);
}
/// <summary>
/// Performs a singular value decomposition of the given matrix A.
/// Returns the rank of A.
/// Note that this implementation ensures that U and V are proper rotations (i.e. no reflections).
/// </summary>
public static int SingularValue(ref Matrix3d A, out Matrix3d U, out Vector3d sigma, out Matrix3d V, double epsilon = D.ZeroTolerance)
{
// U -> proper rotation (no reflection)
// sigma -> singular values
// V -> proper rotation (no reflection)
// impl ref
// https://www.math.ucla.edu/~jteran/papers/ITF04.pdf
var AtA = A.ApplyTranspose(ref A);
EigenSymmetricPSD(AtA, out V, out sigma, epsilon);
// handle reflection in V
if (V.Determinant < 0.0)
{
V.Column2 *= -1.0;
}
// U = A V inv(sigma)
// must handle cases where singular values are zero
if (sigma.X < epsilon)
{
// all zero singular values
// | 0 - - |
// Σ = | - 0 - |
// | - - 0 |
sigma.X = sigma.Y = sigma.Z = 0.0;
U = Identity;
return(0);
}
else if (sigma.Y < epsilon)
{
// one non-zero singular value
// | x - - |
// Σ = | - 0 - |
// | - - 0 |
sigma.X = Math.Sqrt(sigma.X);
sigma.Y = sigma.Z = 0.0;
var u0 = A.Apply(V.Column0 / sigma.X);
var u1 = u0.Y > 0.0 ? u0.CrossX : u0.CrossY;
u1.Unitize();
U = new Matrix3d(u0, u1, Vector3d.Cross(u0, u1));
return(1);
}
else if (sigma.Z < epsilon)
{
// two non-zero singular values
// | x - - |
// Σ = | - y - |
// | - - 0 |
sigma.X = Math.Sqrt(sigma.X);
sigma.Y = Math.Sqrt(sigma.Y);
sigma.Z = 0.0;
var u0 = A.Apply(V.Column0 / sigma.X);
var u1 = A.Apply(V.Column1 / sigma.Y);
U = new Matrix3d(u0, u1, Vector3d.Cross(u0, u1));
return(2);
}
// all non-zero singular values
// | x - - |
// Σ = | - y - |
// | - - z |
sigma.X = Math.Sqrt(sigma.X);
sigma.Y = Math.Sqrt(sigma.Y);
sigma.Z = Math.Sqrt(sigma.Z);
U = new Matrix3d(
A.Apply(V.Column0 / sigma.X),
A.Apply(V.Column1 / sigma.Y),
A.Apply(V.Column2 / sigma.Z));
// handle reflection in U
if (U.Determinant < 0.0)
{
U.Column2 *= -1.0;
sigma.Z *= -1.0;
}
return(3);
}
