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