public static bool ComputeBestFitPlane(Vector3[] points, out Plane plane)
{
Vector3 origin = new Vector3(0, 0, 0);
for (int i = 0; i < points.Length; i++)
{
origin = origin + points[i];
}
float recip = 1.0f / points.Length; // reciprocol of total weighting
origin.X *= recip;
origin.Y *= recip;
origin.Z *= recip;
float fSumXX = 0;
float fSumXY = 0;
float fSumXZ = 0;
float fSumYY = 0;
float fSumYZ = 0;
float fSumZZ = 0;
Vector3 kDiff;
for (int i = 0; i < points.Length; i++)
{
Vector3 p = points[i];
kDiff.X = (p.X - origin.X); // apply vertex weighting!
kDiff.Y = (p.Y - origin.Y);
kDiff.Z = (p.Z - origin.Z);
fSumXX += kDiff.X * kDiff.X; // sume of the squares of the differences.
fSumXY += kDiff.X * kDiff.Y; // sume of the squares of the differences.
fSumXZ += kDiff.X * kDiff.Z; // sume of the squares of the differences.
fSumYY += kDiff.Y * kDiff.Y;
fSumYZ += kDiff.Y * kDiff.Z;
fSumZZ += kDiff.Z * kDiff.Z;
}
fSumXX *= recip;
fSumXY *= recip;
fSumXZ *= recip;
fSumYY *= recip;
fSumYZ *= recip;
fSumZZ *= recip;
// setup the eigensolver
Eigen kES = new Eigen();
kES.mElement[0][0] = fSumXX;
kES.mElement[0][1] = fSumXY;
kES.mElement[0][2] = fSumXZ;
kES.mElement[1][0] = fSumXY;
kES.mElement[1][1] = fSumYY;
kES.mElement[1][2] = fSumYZ;
kES.mElement[2][0] = fSumXZ;
kES.mElement[2][1] = fSumYZ;
kES.mElement[2][2] = fSumZZ;
// compute eigenstuff, smallest eigenvalue is in last position
kES.DecrSortEigenStuff();
Vector3 kNormal = new Vector3();
kNormal.X = kES.mElement[0][2];
kNormal.Y = kES.mElement[1][2];
kNormal.Z = kES.mElement[2][2];
// the minimum energy
plane = new Plane(kNormal, 0 - Vector3.Dot(kNormal, origin));
return(true);
}
public static bool ComputeBestFitPlane(Vector3[] points, out Plane plane)
{
Vector3 origin = new Vector3(0, 0, 0);
for (int i = 0; i < points.Length; i++)
{
origin = origin + points[i];
}
float recip = 1.0f / points.Length; // reciprocol of total weighting
origin.X *= recip;
origin.Y *= recip;
origin.Z *= recip;
float fSumXX = 0;
float fSumXY = 0;
float fSumXZ = 0;
float fSumYY = 0;
float fSumYZ = 0;
float fSumZZ = 0;
Vector3 kDiff;
for (int i = 0; i < points.Length; i++)
{
Vector3 p = points[i];
kDiff.X = (p.X - origin.X); // apply vertex weighting!
kDiff.Y = (p.Y - origin.Y);
kDiff.Z = (p.Z - origin.Z);
fSumXX += kDiff.X * kDiff.X; // sume of the squares of the differences.
fSumXY += kDiff.X * kDiff.Y; // sume of the squares of the differences.
fSumXZ += kDiff.X * kDiff.Z; // sume of the squares of the differences.
fSumYY += kDiff.Y * kDiff.Y;
fSumYZ += kDiff.Y * kDiff.Z;
fSumZZ += kDiff.Z * kDiff.Z;
}
fSumXX *= recip;
fSumXY *= recip;
fSumXZ *= recip;
fSumYY *= recip;
fSumYZ *= recip;
fSumZZ *= recip;
// setup the eigensolver
Eigen kES = new Eigen();
kES.mElement[0][0] = fSumXX;
kES.mElement[0][1] = fSumXY;
kES.mElement[0][2] = fSumXZ;
kES.mElement[1][0] = fSumXY;
kES.mElement[1][1] = fSumYY;
kES.mElement[1][2] = fSumYZ;
kES.mElement[2][0] = fSumXZ;
kES.mElement[2][1] = fSumYZ;
kES.mElement[2][2] = fSumZZ;
// compute eigenstuff, smallest eigenvalue is in last position
kES.DecrSortEigenStuff();
Vector3 kNormal = new Vector3();
kNormal.X = kES.mElement[0][2];
kNormal.Y = kES.mElement[1][2];
kNormal.Z = kES.mElement[2][2];
// the minimum energy
plane = new Plane(kNormal, 0 - Vector3.Dot(kNormal, origin));
return true;
}