public static void rotate12(SMat3 vtav, DMat3 v) { if (vtav.m12 == 0) { return; } double c = 0, s = 0; Schur2.rot12(vtav, c, s); Givens.rot12_post(v, c, s); }
public static void rotate01(SMat3 vtav, DMat3 v) { if (vtav.m01 == 0) { return; } double c = 0, s = 0; Schur2.rot01(vtav, c, s); Givens.rot01_post(v, c, s); }
public static double solveSymmetric(SMat3 A, Vec3 b, Vec3 x, double svd_tol, int svd_sweeps, double pinv_tol) { DMat3 pinv; var V = new DMat3(); var VTAV = new SMat3(); getSymmetricSvd(A, VTAV, V, svd_tol, svd_sweeps); pseudoinverse(out pinv, VTAV, V, pinv_tol); MatUtils.vmul(out x, pinv, b); return(calcError(A, x, b)); }
public static void getSymmetricSvd(SMat3 a, SMat3 vtav, DMat3 v, double tol, int max_sweeps) { vtav.setSymmetric(a); v.set(1, 0, 0, 0, 1, 0, 0, 0, 1); var delta = tol * MatUtils.fnorm(vtav); for (var i = 0; i < max_sweeps && MatUtils.off(vtav) > delta; ++i) { rotate01(vtav, v); rotate02(vtav, v); rotate12(vtav, v); } }
public static void pseudoinverse(out DMat3 Out, SMat3 d, DMat3 v, double tol) { double d0 = pinv(d.m00, tol), d1 = pinv(d.m11, tol), d2 = pinv(d.m22, tol); Out = new DMat3(); Out.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 double off(SMat3 a) { return(Math.Sqrt(2 * ((a.m01 * a.m01) + (a.m02 * a.m02) + (a.m12 * a.m12)))); }
public static double fnorm(SMat3 a) { return(Math.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))); }