public static void vmul(out Vec3 Out, DMat3 a, Vec3 v)
{
Out = new Vec3(
(float)((a.m00 * v.X) + (a.m01 * v.Y) + (a.m02 * v.Z)),
(float)((a.m10 * v.X) + (a.m11 * v.Y) + (a.m12 * v.Z)),
(float)((a.m20 * v.X) + (a.m21 * v.Y) + (a.m22 * v.Z)));
}
public static double calcError(DMat3 A, Vec3 x, Vec3 b)
{
Vec3 vtmp;
MatUtils.vmul(out vtmp, A, x);
vtmp = b - vtmp;
return(vtmp.Dot(vtmp));
}
public static double calcError(SMat3 origA, Vec3 x, Vec3 b)
{
var A = new DMat3();
Vec3 vtmp;
A.setSymmetric(origA);
MatUtils.vmul(out vtmp, A, x);
vtmp = b - vtmp;
return(vtmp.Dot(vtmp));
}
public static void mmul_ata(out SMat3 Out, DMat3 a)
{
Out = new SMat3();
Out.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 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 solveLeastSquares(DMat3 a, Vec3 b, Vec3 x, double svd_tol, int svd_sweeps,
double pinv_tol)
{
DMat3 at;
SMat3 ata;
Vec3 atb;
MatUtils.transpose(out at, a);
MatUtils.mmul_ata(out ata, a);
MatUtils.vmul(out atb, at, b);
return(solveSymmetric(ata, atb, x, svd_tol, svd_sweeps, pinv_tol));
}
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 mmul(out DMat3 Out, DMat3 a, DMat3 b)
{
Out = new DMat3();
Out.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 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 void transpose(out DMat3 Out, DMat3 a)
{
Out = new DMat3();
Out.set(a.m00, a.m10, a.m20, a.m01, a.m11, a.m21, a.m02, a.m12, a.m22);
}
public static double fnorm(DMat3 a)
{
return(Math.Sqrt((a.m00 * a.m00) + (a.m01 * a.m01) + (a.m02 * a.m02)
+ (a.m10 * a.m10) + (a.m11 * a.m11) + (a.m12 * a.m12)
+ (a.m20 * a.m20) + (a.m21 * a.m21) + (a.m22 * a.m22)));
}
public static double off(DMat3 a)
{
return(Math.Sqrt((a.m01 * a.m01) + (a.m02 * a.m02) + (a.m10 * a.m10) + (a.m12 * a.m12) + (a.m20 * a.m20) +
(a.m21 * a.m21)));
}
