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 void Add(
Vec3 n, Vec3 p, ref Mat3 ata, ref Vec3 atb, ref Vec4 pointaccum)
{
ata[0, 0] += n.x * n.x;
ata[0, 1] += n.x * n.y;
ata[0, 2] += n.x * n.z;
ata[1, 1] += n.y * n.y;
ata[1, 2] += n.y * n.z;
ata[2, 2] += n.z * n.z;
float b = Vec3.Dot(p, n);
atb += n * b;
pointaccum += new Vec4(p, 1.0f);
}