public static void Mmul_ata(SMat3 mout, Mat3 a) { mout.SetSymmetric(a.m00 * a.m00 + a.m10 * a.m10 + a.m20 * a.m20, a.m00 * a.m01 + a.m10 * a.m11 + a.m20 * a.m21, a.m00 * a.m02 + a.m10 * a.m12 + a.m20 * a.m22, a.m01 * a.m01 + a.m11 * a.m11 + a.m21 * a.m21, a.m01 * a.m02 + a.m11 * a.m12 + a.m21 * a.m22, a.m02 * a.m02 + a.m12 * a.m12 + a.m22 * a.m22); }
public static void Rot12(SMat3 m, float c, float s) { float [] cs = Svd.CalcSymmetricGivensCoefficients(m.m11, m.m12, m.m22, c, s); float cc = cs[0] * cs[0]; float ss = cs[1] * cs[1]; float mix = 2 * c * s * m.m12; m.SetSymmetric(m.m00, c * m.m01 - s * m.m02, s * m.m01 + c * m.m02, cc * m.m11 - mix + ss * m.m22, 0, ss * m.m11 + mix + cc * m.m22); }
public static void Rot01(SMat3 m, float c, float s) { float [] cs = Svd.CalcSymmetricGivensCoefficients(m.m00, m.m01, m.m11, c, s); float cc = cs[0] * cs[0]; float ss = cs[1] * cs[1]; float mix = 2 * c * s * m.m01; m.SetSymmetric(cc * m.m00 - mix + ss * m.m11, 0, c * m.m02 - s * m.m12, ss * m.m00 + mix + cc * m.m11, s * m.m02 + c * m.m12, m.m22); }
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); } }