public void Solve(Matrix3d matrix, double epsilon = MathUtil.Epsilon, int jacobiIters = -1)
{
if (jacobiIters != -1)
{
NumJacobiIterations = jacobiIters;
}
if (ATA == null)
{
ATA = new SymmetricMatrix3d();
}
ATA.SetATA(ref matrix);
Vector4d v = jacobiDiagonalize(ATA);
if (AV == null)
{
AV = new double[9];
}
computeAV(ref matrix, ref v, AV);
Vector4d u = Vector4d.Zero;
QRFactorize(AV, ref v, epsilon, ref S, ref u);
//u,v are quaternions in (s, x, y, z) order
U = new Quaterniond(u[1], u[2], u[3], u[0]);
V = new Quaterniond(v[1], v[2], v[3], v[0]);
}
Vector4d jacobiDiagonalize(SymmetricMatrix3d ATA)
{
var V = new Vector4d(1, 0, 0, 0);
for (int i = 0; i < NumJacobiIterations; ++i)
{
Vector2d givens = givensAngles(ATA, 0, 1);
ATA.quatConjugate01(givens.x, givens.y);
quatTimesEqualCoordinateAxis(ref V, givens.x, givens.y, 2);
givens = givensAngles(ATA, 1, 2);
ATA.quatConjugate12(givens.x, givens.y);
quatTimesEqualCoordinateAxis(ref V, givens.x, givens.y, 0);
givens = givensAngles(ATA, 0, 2);
ATA.quatConjugate02(givens.x, givens.y);
quatTimesEqualCoordinateAxis(ref V, givens.x, givens.y, 1);
}
return(V);
}
/// <summary>
/// compute givens angles of B for (p,q). Only
/// ever called with p,q as [0,1], [0,2], or [1,2]
/// </summary>
Vector2d givensAngles(SymmetricMatrix3d B, int p, int q)
{
double ch = 0, sh = 0;
if (p == 0)
{
if (q == 1)
{
ch = B.entries[p] - B.entries[q];
sh = 0.5 * B.entries[3];
}
else
{
ch = B.entries[q] - B.entries[p];
sh = 0.5 * B.entries[4];
}
}
else if (p == 1 /* && q == 2 */)
{
ch = B.entries[p] - B.entries[q];
sh = 0.5 * B.entries[5];
}
// [TODO] can use fast reciprocal square root here...
double omega = 1.0 / Math.Sqrt(ch * ch + sh * sh);
ch *= omega;
sh *= omega;
bool approxValid = (gamma * sh * sh) < (ch * ch);
ch = approxValid ? ch : cosBackup;
sh = approxValid ? sh : sinBackup;
return(new Vector2d(ch, sh));
}