static void Rotate12(ref SymmetricMatrix3x3 vtav, ref Matrix3x3 v)
{
if (vtav.m12 == 0)
{
return;
}
float c, s;
Schur2.Rot12(ref vtav, out c, out s);
Givens.Rot12_post(ref v, c, s);
}
public static float SolveSymmetric(SymmetricMatrix3x3 A, Vector3 b, out Vector3 x, float svd_tol, int svd_sweeps, float pinv_tol)
{
SymmetricMatrix3x3 VTAV;
Matrix3x3 V;
GetSymmetricSvd(A, out VTAV, out V, svd_tol, svd_sweeps);
Matrix3x3 pseudoInverse;
Pseudoinverse(out pseudoInverse, VTAV, V, pinv_tol);
x = pseudoInverse * b;
return ComputeError(A, x, b);
}
static void GetSymmetricSvd(SymmetricMatrix3x3 a, out SymmetricMatrix3x3 vtav, out Matrix3x3 v, float tol, int max_sweeps)
{
vtav = a;
v = Matrix3x3.identity;
float delta = tol * vtav.FNorm;
for (int i = 0; i < max_sweeps && vtav.Off > delta; ++i)
{
Rotate01(ref vtav, ref v);
Rotate02(ref vtav, ref v);
Rotate12(ref vtav, ref v);
}
}
public void Set(SymmetricMatrix3x3 m)
{
m00 = m.m00;
m01 = m.m01;
m02 = m.m02;
m10 = m.m01;
m11 = m.m11;
m12 = m.m12;
m20 = m.m02;
m21 = m.m12;
m22 = m.m22;
}
public static void Rot12(ref SymmetricMatrix3x3 m, out float c, out float s)
{
CalcSymmetricGivensCoefficients(m.m11, m.m12, m.m22, out c, out s);
float cc = c * c;
float ss = s * s;
float mix = 2 * c * s * m.m12;
m.m01 = c * m.m01 - s * m.m02;
m.m02 = s * m.m01 + c * m.m02;
m.m11 = cc * m.m11 - mix + ss * m.m22;
m.m12 = 0;
m.m22 = ss * m.m11 + mix + cc * m.m22;
}
public static void Rot01(ref SymmetricMatrix3x3 m, out float c, out float s)
{
CalcSymmetricGivensCoefficients(m.m00, m.m01, m.m11, out c, out s);
float cc = c * c;
float ss = s * s;
float mix = 2 * c * s * m.m01;
m.m00 = cc * m.m00 - mix + ss * m.m11;
m.m01 = 0;
m.m02 = c * m.m02 - s * m.m12;
m.m11 = ss * m.m00 + mix + cc * m.m11;
m.m12 = s * m.m02 + c * m.m12;
}
static void Pseudoinverse(out Matrix3x3 outm, SymmetricMatrix3x3 d, Matrix3x3 v, float tol)
{
float d0 = Pinv(d.m00, tol);
float d1 = Pinv(d.m11, tol);
float d2 = Pinv(d.m22, tol);
outm = new Matrix3x3();
outm.m00 = v.m00 * d0 * v.m00 + v.m01 * d1 * v.m01 + v.m02 * d2 * v.m02;
outm.m01 = v.m00 * d0 * v.m10 + v.m01 * d1 * v.m11 + v.m02 * d2 * v.m12;
outm.m02 = v.m00 * d0 * v.m20 + v.m01 * d1 * v.m21 + v.m02 * d2 * v.m22;
outm.m10 = v.m10 * d0 * v.m00 + v.m11 * d1 * v.m01 + v.m12 * d2 * v.m02;
outm.m11 = v.m10 * d0 * v.m10 + v.m11 * d1 * v.m11 + v.m12 * d2 * v.m12;
outm.m12 = v.m10 * d0 * v.m20 + v.m11 * d1 * v.m21 + v.m12 * d2 * v.m22;
outm.m20 = v.m20 * d0 * v.m00 + v.m21 * d1 * v.m01 + v.m22 * d2 * v.m02;
outm.m21 = v.m20 * d0 * v.m10 + v.m21 * d1 * v.m11 + v.m22 * d2 * v.m12;
outm.m22 = v.m20 * d0 * v.m20 + v.m21 * d1 * v.m21 + v.m22 * d2 * v.m22;
}
static float ComputeError(SymmetricMatrix3x3 A, Vector3 x, Vector3 b)
{
Vector3 vtmp = A * x;
vtmp = b - vtmp;
return vtmp.sqrMagnitude;
}
