void do_solve(double sumXX, double sumXY, double sumXZ, double sumYY, double sumYZ, double sumZZ, double invSumMultiplier) { sumXX *= invSumMultiplier; sumXY *= invSumMultiplier; sumXZ *= invSumMultiplier; sumYY *= invSumMultiplier; sumYZ *= invSumMultiplier; sumZZ *= invSumMultiplier; double[] matrix = new double[] { sumXX, sumXY, sumXZ, sumXY, sumYY, sumYZ, sumXZ, sumYZ, sumZZ }; // Setup the eigensolver. SymmetricEigenSolver solver = new SymmetricEigenSolver(3, 4096); int iters = solver.Solve(matrix, SymmetricEigenSolver.SortType.Increasing); ResultValid = (iters > 0 && iters < SymmetricEigenSolver.NO_CONVERGENCE); if (ResultValid) { Box.Extent = new Vector3d(solver.GetEigenvalues()); double[] evectors = solver.GetEigenvectors(); Box.AxisX = new Vector3d(evectors[0], evectors[1], evectors[2]); Box.AxisY = new Vector3d(evectors[3], evectors[4], evectors[5]); Box.AxisZ = new Vector3d(evectors[6], evectors[7], evectors[8]); } }
public GaussPointsFit3(IEnumerable <Vector3d> points) { Box = new Box3d(Vector3d.Zero, Vector3d.One); // Compute the mean of the points. int numPoints = 0; foreach (Vector3d v in points) { Box.Center += v; numPoints++; } double invNumPoints = (1.0) / numPoints; Box.Center *= invNumPoints; // Compute the covariance matrix of the points. double sumXX = (double)0, sumXY = (double)0, sumXZ = (double)0; double sumYY = (double)0, sumYZ = (double)0, sumZZ = (double)0; foreach (Vector3d p in points) { Vector3d diff = p - Box.Center; sumXX += diff[0] * diff[0]; sumXY += diff[0] * diff[1]; sumXZ += diff[0] * diff[2]; sumYY += diff[1] * diff[1]; sumYZ += diff[1] * diff[2]; sumZZ += diff[2] * diff[2]; } sumXX *= invNumPoints; sumXY *= invNumPoints; sumXZ *= invNumPoints; sumYY *= invNumPoints; sumYZ *= invNumPoints; sumZZ *= invNumPoints; double[] matrix = new double[] { sumXX, sumXY, sumXZ, sumXY, sumYY, sumYZ, sumXZ, sumYZ, sumZZ }; // Setup the eigensolver. SymmetricEigenSolver solver = new SymmetricEigenSolver(3, 4096); int iters = solver.Solve(matrix, SymmetricEigenSolver.SortType.Increasing); ResultValid = (iters > 0 && iters < SymmetricEigenSolver.NO_CONVERGENCE); Box.Extent = new Vector3d(solver.GetEigenvalues()); double[] evectors = solver.GetEigenvectors(); Box.AxisX = new Vector3d(evectors[0], evectors[1], evectors[2]); Box.AxisY = new Vector3d(evectors[3], evectors[4], evectors[5]); Box.AxisZ = new Vector3d(evectors[6], evectors[7], evectors[8]); }