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)); }