// ------------------------------------------------------------------------------- public float Solve(Vector3 outx, float svd_tol, int svd_sweeps, float pinv_tol) { if (this.data.numPoints == 0) { // Debug.LogError("Invalid Argument"); } this.massPoint = new Vector3(this.data.massPoint_x, this.data.massPoint_y, this.data.massPoint_z); this.massPoint = VecUtils.Scale(this.massPoint, (1.0f / (float)this.data.numPoints)); this.SetAta(); this.SetAtb(); Vector3 tmpv = MatUtils.Vmul_symmetric(this.ata, this.massPoint); atb = VecUtils.Sub(atb, tmpv); this.x = new Vector3(0, 0, 0); float result = Svd.SolveSymmetric(this.ata, this.atb, this.x, svd_tol, svd_sweeps, pinv_tol); VecUtils.AddScaled(this.x, 1, this.massPoint); this.SetAtb(); outx.Set(x.x, x.y, x.z); this.hasSolution = true; return(result); }
// ------------------------------------------------------------------------------- public float GetError(Vector3 pos) { if (!this.hasSolution) { this.SetAta(); this.SetAtb(); } Vector3 atax = MatUtils.Vmul_symmetric(this.ata, pos); return(VecUtils.Dot(pos, atax) - 2 * VecUtils.Dot(pos, this.atb) + this.data.btb); }