// -------------------------------------------------------------------------------
public float Solve(Vector3 outx, float svd_tol, int svd_sweeps, float pinv_tol)
{
if (this.data.numPoints == 0)
{
// Debug.LogError("Invalid Argument");
}
this.massPoint = new Vector3(this.data.massPoint_x, this.data.massPoint_y, this.data.massPoint_z);
this.massPoint = VecUtils.Scale(this.massPoint, (1.0f / (float)this.data.numPoints));
this.SetAta();
this.SetAtb();
Vector3 tmpv = MatUtils.Vmul_symmetric(this.ata, this.massPoint);
atb = VecUtils.Sub(atb, tmpv);
this.x = new Vector3(0, 0, 0);
float result = Svd.SolveSymmetric(this.ata, this.atb, this.x, svd_tol, svd_sweeps, pinv_tol);
VecUtils.AddScaled(this.x, 1, this.massPoint);
this.SetAtb();
outx.Set(x.x, x.y, x.z);
this.hasSolution = true;
return(result);
}
public static float Solve(
Mat3 ata, Vec3 atb, Vec4 pointaccum, out Vec3 x)
{
Vec3 masspoint =
new Vec3(pointaccum.x, pointaccum.y, pointaccum.z) /
pointaccum.w;
atb -= Svd.VmulSym(ata, masspoint);
Svd.SolveAtaAtb(ata, atb, out x);
float result = CalcError(ata, x, atb);
x += masspoint;
return(result);
}
public static float CalcError(Mat3 a, Vec3 x, Vec3 b)
{
Vec3 vtmp = b - Svd.VmulSym(a, x);
return(Vec3.Dot(vtmp, vtmp));
}
