public Vector3 Solve(double svd_tol, int svd_sweeps, double pinv_tol)
{
if (QefData.numPoints == 0)
{
throw new ArgumentException("...");
}
massPoint = QefData.massPoint;
massPoint /= QefData.numPoints;
SetAta();
SetAtb();
atb -= MatUtils.VMuSymmetric(ata, massPoint);
x = Vector3.Zero;
double result = SVD.SolveSymmetric(ata, atb, out x, svd_tol, svd_sweeps, pinv_tol);
if (double.IsNaN(result))
{
x = massPoint;
}
else
{
x += massPoint;
}
SetAtb();
hasSolution = true;
return(x);
}
public double GetError(Vector3 pos)
{
if (!hasSolution)
{
SetAta();
SetAtb();
}
Vector3 atax = MatUtils.VMuSymmetric(ata, pos);
return(pos.Dot(atax) - 2 * pos.Dot(atb) + QefData.btb);
}
