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 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 double getError(Vec3 pos)
{
if (!hasSolution)
{
setAta();
setAtb();
}
Vec3 atax;
MatUtils.vmul_symmetric(out atax, ata, pos);
return(pos.Dot(atax) - 2 * pos.Dot(atb) + data.btb);
}
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 double solve(out Vec3 outx, double svd_tol, int svd_sweeps, double pinv_tol)
{
if (data.numPoints == 0)
{
throw new ArgumentException("...");
}
massPoint = new Vec3((float)data.massPoint_x, (float)data.massPoint_y, (float)data.massPoint_z);
massPoint *= (1.0f / data.numPoints);
setAta();
setAtb();
Vec3 tmpv;
MatUtils.vmul_symmetric(out tmpv, ata, massPoint);
atb = atb - tmpv;
x = Vec3.Zero;
var result = SVD.solveSymmetric(ata, atb, x, svd_tol, svd_sweeps, pinv_tol);
x += massPoint * 1;
setAtb();
outx = x;
hasSolution = true;
return(result);
}