public static LineParams FitLine(List <PosXYZ> pos)
{
var points = pos.Select(p => new Vector3d(p.X, p.Y, p.Z));
var Origin = Vector3d.Zero;
int num1 = 0;
foreach (Vector3d point in points)
{
Origin += point;
++num1;
}
double num2 = 1.0 / num1;
Origin *= num2;
double num3 = 0.0;
double num4 = 0.0;
double num5 = 0.0;
double num6 = 0.0;
double num7 = 0.0;
double num8 = 0.0;
foreach (Vector3d point in points)
{
Vector3d vector3d = point - Origin;
num3 += vector3d[0] * vector3d[0];
num4 += vector3d[0] * vector3d[1];
num5 += vector3d[0] * vector3d[2];
num6 += vector3d[1] * vector3d[1];
num7 += vector3d[1] * vector3d[2];
num8 += vector3d[2] * vector3d[2];
}
double num9 = num3 * num2;
double num10 = num4 * num2;
double num11 = num5 * num2;
double num12 = num6 * num2;
double num13 = num7 * num2;
double num14 = num8 * num2;
double[] input = new double[9]
{
num9, num10, num11,
num10, num12, num13,
num11, num13, num14
};
SymmetricEigenSolver symmetricEigenSolver = new SymmetricEigenSolver(3, 4096);
int num15 = symmetricEigenSolver.Solve(input, SymmetricEigenSolver.SortType.Decreasing);
var ResultValid = num15 > 0 && num15 < int.MaxValue;
var Normal = new Vector3d(symmetricEigenSolver.GetEigenvector(0));
return(new LineParams()
{
Direction = Normal, Origin = Origin
});
}
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]);
}
public OrthogonalPlaneFit3(IEnumerable <Vector3D> points)
{
// Compute the mean of the points.
Origin = Vector3D.Zero;
int numPoints = 0;
foreach (Vector3D v in points)
{
Origin += v;
numPoints++;
}
double invNumPoints = (1.0) / numPoints;
Origin *= 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 - Origin;
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.Decreasing);
ResultValid = (iters > 0 && iters < SymmetricEigenSolver.NO_CONVERGENCE);
Normal = new Vector3D(solver.GetEigenvector(2));
}
