public Vector3 Solve(float svd_tol, int svd_sweeps, float pinv_tol) { if (this.data.numPoints == 0) { throw new Exception("..."); } MassPoint = new Vector3(this.data.massPoint_x, this.data.massPoint_y, this.data.massPoint_z); MassPoint /= (float)data.numPoints; this.SetAta(); this.SetAtb(); Vector3 tmpv = ata.Vmul(MassPoint); atb = atb - tmpv; x = Vector3.Zero; float result = SVD.SolveSymmetric(this.ata, this.atb, ref this.x, svd_tol, svd_sweeps, pinv_tol); if (float.IsNaN(result)) { x = MassPoint; } else { x += MassPoint; } this.SetAtb(); //output = x; this.hasSolution = true; //return result; return(x); return(MassPoint); }