public Vector3 Solve2(float svd_tol, int sweeps, float pinv_tol)
{
if (Intersections.Count == 0)
{
this.Error = 100000;
return Vector3.Zero;
}
Vector3 x = mass_point / (float)Intersections.Count;
float error = GetDistanceSquared(x);
this.Error = error;
//return x;
if (Math.Abs(error) >= 0.0001f)
{
for (int i = 0; i < deltas.Length; i++)
{
Vector3 new_point = new Vector3(x.X + deltas[i].X, x.Y + deltas[i].Y, x.Z + deltas[i].Z);
new_point = Vector3.Clamp(new_point, Vector3.Zero, Vector3.One);
float e = GetDistanceSquared(new_point);
if (e <= error)
{
x = new_point;
if (Math.Abs(e) < 0.0001f)
break;
error = e;
}
}
}
if (x.X > 1 || x.Y > 1 || x.Z > 1 || x.X < 0 || x.Y < 0 || x.Z < 0)
return mass_point / (float)Intersections.Count;
return Vector3.Clamp(x, Vector3.Zero, Vector3.One);
}