public float solve(out Vec3 outx, float svd_tol, int svd_sweeps, float pinv_tol) { if (this.data.numPoints == 0) { throw new UnityException("..."); } this.massPoint.set(this.data.massPoint_x, this.data.massPoint_y, this.data.massPoint_z); VecUtils.scale(out this.massPoint, 1.0f / this.data.numPoints, this.massPoint); this.setAta(); this.setAtb(); Vec3 tmpv; MatUtils.vmul_symmetric(out tmpv, this.ata, this.massPoint); VecUtils.sub(out this.atb, this.atb, tmpv); this.x.clear(); float result = Svd.solveSymmetric(this.ata, this.atb, out this.x, svd_tol, svd_sweeps, pinv_tol); VecUtils.addScaled(out this.x, 1.0f, this.massPoint); this.setAtb(); outx = x; this.hasSolution = true; return(result); }
private static float calcError(Mat3 A, Vec3 x, Vec3 b) { Vec3 vtmp; MatUtils.vmul(out vtmp, A, x); VecUtils.sub(out vtmp, b, vtmp); return(VecUtils.dot(vtmp, vtmp)); }
private static float calcError(SMat3 origA, Vec3 x, Vec3 b) { Mat3 A = new Mat3(); Vec3 vtmp; A.setSymmetric(origA); MatUtils.vmul(out vtmp, A, x); VecUtils.sub(out vtmp, b, vtmp); return(VecUtils.dot(vtmp, vtmp)); }