public static void Rot12_post(Mat3 m, float c, float s) { float m01 = m.m01, m02 = m.m02, m11 = m.m11, m12 = m.m12, m21 = m.m21, m22 = m.m22; m.Set(m.m00, c * m01 - s * m02, s * m01 + c * m02, m.m10, c * m11 - s * m12, s * m11 + c * m12, m.m20, c * m21 - s * m22, s * m21 + c * m22); }
public static void Rot02_post(Mat3 m, float c, float s) { float m00 = m.m00, m02 = m.m02, m10 = m.m10, m12 = m.m12, m20 = m.m20, m22 = m.m22; m.Set(c * m00 - s * m02, m.m01, s * m00 + c * m02, c * m10 - s * m12, m.m11, s * m10 + c * m12, c * m20 - s * m22, m.m21, s * m20 + c * m22); }
public static void Mmul(Mat3 mout, Mat3 a, Mat3 b) { mout.Set(a.m00 * b.m00 + a.m01 * b.m10 + a.m02 * b.m20, a.m00 * b.m01 + a.m01 * b.m11 + a.m02 * b.m21, a.m00 * b.m02 + a.m01 * b.m12 + a.m02 * b.m22, a.m10 * b.m00 + a.m11 * b.m10 + a.m12 * b.m20, a.m10 * b.m01 + a.m11 * b.m11 + a.m12 * b.m21, a.m10 * b.m02 + a.m11 * b.m12 + a.m12 * b.m22, a.m20 * b.m00 + a.m21 * b.m10 + a.m22 * b.m20, a.m20 * b.m01 + a.m21 * b.m11 + a.m22 * b.m21, a.m20 * b.m02 + a.m21 * b.m12 + a.m22 * b.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); } }
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 static void Transpose(Mat3 mout, Mat3 a) { mout.Set(a.m00, a.m10, a.m20, a.m01, a.m11, a.m21, a.m02, a.m12, a.m22); }