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 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);
}
