public SMat3 MulATA()
{
SMat3 m = new SMat3();
m.SetSymmetric(m00 * m00 + m10 * m10 + m20 * m20,
m00 * m01 + m10 * m11 + m20 * m21,
m00 * m02 + m10 * m12 + m20 * m22,
m01 * m01 + m11 * m11 + m21 * m21,
m01 * m02 + m11 * m12 + m21 * m22,
m02 * m02 + m12 * m12 + m22 * m22);
return(m);
}
public static void Rotate01(ref SMat3 vtav, ref Mat3 v)
{
if (vtav.m01 == 0)
{
return;
}
float c = 0, s = 0;
vtav.Rot01(ref c, ref s);
c = 0; s = 0;
v.Rot01_post(c, s);
}
public static void GetSymmetricSvd(ref SMat3 a, ref SMat3 vtav, ref Mat3 v, float tol, int max_sweeps)
{
vtav.SetSymmetric(a);
v.Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
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 static void Rotate12(ref SMat3 vtav, ref Mat3 v)
{
if (vtav.m12 == 0)
{
return;
}
float c = 0, s = 0;
vtav.Rot12(ref c, ref s);
c = 0; s = 0;
v.Rot12_post(c, s);
}
public static Mat3 PseudoInverse(SMat3 d, Mat3 v, float tol)
{
Mat3 m = new Mat3();
float d0 = Pinv(d.m00, tol), d1 = Pinv(d.m11, tol), d2 = Pinv(d.m22,
tol);
m.Set(v.m00 * d0 * v.m00 + v.m01 * d1 * v.m01 + v.m02 * d2 * v.m02,
v.m00 * d0 * v.m10 + v.m01 * d1 * v.m11 + v.m02 * d2 * v.m12,
v.m00 * d0 * v.m20 + v.m01 * d1 * v.m21 + v.m02 * d2 * v.m22,
v.m10 * d0 * v.m00 + v.m11 * d1 * v.m01 + v.m12 * d2 * v.m02,
v.m10 * d0 * v.m10 + v.m11 * d1 * v.m11 + v.m12 * d2 * v.m12,
v.m10 * d0 * v.m20 + v.m11 * d1 * v.m21 + v.m12 * d2 * v.m22,
v.m20 * d0 * v.m00 + v.m21 * d1 * v.m01 + v.m22 * d2 * v.m02,
v.m20 * d0 * v.m10 + v.m21 * d1 * v.m11 + v.m22 * d2 * v.m12,
v.m20 * d0 * v.m20 + v.m21 * d1 * v.m21 + v.m22 * d2 * v.m22);
return(m);
}
public void SetSymmetric(SMat3 rhs)
{
SetSymmetric(rhs.m00, rhs.m01, rhs.m02, rhs.m11, rhs.m12, rhs.m22);
}
