public float solve(out Vec3 outx, float svd_tol,
int svd_sweeps, float pinv_tol)
{
if (this.data.numPoints == 0)
{
throw new UnityException("...");
}
this.massPoint.set(this.data.massPoint_x, this.data.massPoint_y, this.data.massPoint_z);
VecUtils.scale(out this.massPoint, 1.0f / this.data.numPoints, this.massPoint);
this.setAta();
this.setAtb();
Vec3 tmpv;
MatUtils.vmul_symmetric(out tmpv, this.ata, this.massPoint);
VecUtils.sub(out this.atb, this.atb, tmpv);
this.x.clear();
float result = Svd.solveSymmetric(this.ata, this.atb, out this.x,
svd_tol, svd_sweeps, pinv_tol);
VecUtils.addScaled(out this.x, 1.0f, this.massPoint);
this.setAtb();
outx = x;
this.hasSolution = true;
return(result);
}
public static float solveLeastSquares(Mat3 a, Vec3 b, out Vec3 x,
float svd_tol, int svd_sweeps, float pinv_tol)
{
Mat3 at;
SMat3 ata;
Vec3 atb;
MatUtils.transpose(out at, a);
MatUtils.mmul_ata(out ata, a);
MatUtils.vmul(out atb, at, b);
return(Svd.solveSymmetric(ata, atb, out x, svd_tol, svd_sweeps, pinv_tol));
}