public static void GetSymmetricSvd(SMat3 a, SMat3 vtav, Mat3 v, float tol, int max_sweeps)
{
vtav.SetSymmetric(a);
v.Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
float delta = tol * MatUtils.Fnorm(vtav);
for (int i = 0; i < max_sweeps && MatUtils.Off(vtav) > delta; ++i)
{
Rotate01(vtav, v);
Rotate02(vtav, v);
Rotate12(vtav, v);
}
}
public static void Pseudoinverse(Mat3 mout, SMat3 d, Mat3 v, float tol)
{
float d0 = pinv(d.m00, tol), d1 = pinv(d.m11, tol), d2 = pinv(d.m22, tol);
mout.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);
}
public void SetSymmetric(SMat3 rhs)
{
this.SetSymmetric(rhs.m00, rhs.m01, rhs.m02, rhs.m11, rhs.m12, rhs.m22);
}
private SMat3(SMat3 rhs)
{
this.SetSymmetric(rhs);
}
public static float Off(SMat3 a)
{
return(Mathf.Sqrt(2 * ((a.m01 * a.m01) + (a.m02 * a.m02) + (a.m12 * a.m12))));
}
public static float Fnorm(SMat3 a)
{
return(Mathf.Sqrt((a.m00 * a.m00) + (a.m01 * a.m01) + (a.m02 * a.m02)
+ (a.m01 * a.m01) + (a.m11 * a.m11) + (a.m12 * a.m12)
+ (a.m02 * a.m02) + (a.m12 * a.m12) + (a.m22 * a.m22)));
}
