public static bool ComputeBestFitPlane(Vector3[] points, float[] weights, ref Vector4 plane)
{
if (points == null || points.Length <= 0)
{
return(false);
}
if (weights != null && weights.Length != points.Length)
{
return(false);
}
Vector3 origin = Vector3.zero;
float wtotal = 0;
for (int i = 0; i < points.Length; i++)
{
float w = (weights != null) ? weights[i] : 1.0f;
origin += w * points[i];
wtotal += w;
}
float recip = 1.0f / wtotal; // reciprocol of total weighting
origin = recip * origin;
float sumXX = 0.0f;
float sumXY = 0.0f;
float sumXZ = 0.0f;
float sumYY = 0.0f;
float sumYZ = 0.0f;
float sumZZ = 0.0f;
for (int i = 0; i < points.Length; i++)
{
float w = (weights != null) ? weights[i] : 1.0f;
origin += w * points[i];
Vector3 diff = w * (points[i] - origin);
sumXX += diff[0] * diff[0]; // sume of the squares of the differences.
sumXY += diff[0] * diff[1]; // sume of the squares of the differences.
sumXZ += diff[0] * diff[2]; // sume of the squares of the differences.
sumYY += diff[1] * diff[1];
sumYZ += diff[1] * diff[2];
sumZZ += diff[2] * diff[2];
}
sumXX *= recip;
sumXY *= recip;
sumXZ *= recip;
sumYY *= recip;
sumYZ *= recip;
sumZZ *= recip;
// setup the eigensolver
Eigen ES = new Eigen();
ES.Elements[0, 0] = sumXX;
ES.Elements[0, 1] = sumXY;
ES.Elements[0, 2] = sumXZ;
ES.Elements[1, 0] = sumXY;
ES.Elements[1, 1] = sumYY;
ES.Elements[1, 2] = sumYZ;
ES.Elements[2, 0] = sumXZ;
ES.Elements[2, 1] = sumYZ;
ES.Elements[2, 2] = sumZZ;
// compute eigenstuff, smallest eigenvalue is in last position
ES.DecrSortEigenStuff();
Vector3 normal = new Vector3(ES.Elements[0, 2], ES.Elements[1, 2], ES.Elements[2, 2]);
// the minimum energy
plane[0] = normal[0];
plane[1] = normal[1];
plane[2] = normal[2];
plane[3] = 0.0f - fm_dot(normal, origin);
return(true);
}
/// <summary>
/// Compute a plane which best fits a series of input points.
/// </summary>
/// <param name="tPoints">The array of points to analyse.</param>
/// <param name="tWeights">The relative weightings to apply to each point.</param>
/// <returns>The plane of best fit.</returns>
public static Plane ComputeBestFit(Vector3[] tPoints, float[] tWeights)
{
Vector3 kOrigin = new Vector3();
float w = 1;
float wtotal = 0;
// Calculate the weighted center of all the points.
for (int i = 0, n = tPoints.Length; i < n; ++i)
{
w = 1;
if (tWeights != null)
{
w *= tWeights[i];
}
Vector3 p = tPoints[i];
kOrigin.X += p.X * w;
kOrigin.Y += p.Y * w;
kOrigin.Z += p.Z * w;
wtotal += w;
}
float recip = 1.0f / wtotal; // reciprocol of total weighting
kOrigin.X *= recip;
kOrigin.Y *= recip;
kOrigin.Z *= recip;
float fSumXX = 0;
float fSumXY = 0;
float fSumXZ = 0;
float fSumYY = 0;
float fSumYZ = 0;
float fSumZZ = 0;
//--
for (int i = 0, n = tPoints.Length; i < n; ++i)
{
w = 1;
if (tWeights != null)
{
w *= tWeights[i];
}
// Transform to center and apply vertex weighting.
Vector3 p = tPoints[i];
Vector3 kDiff = new Vector3(w * (p.X - kOrigin.X), w * (p.Y - kOrigin.Y), w * (p.Z - kOrigin.Z));
fSumXX += kDiff.X * kDiff.X; // sum of the squares of the differences.
fSumXY += kDiff.X * kDiff.Y; // sum of the squares of the differences.
fSumXZ += kDiff.X * kDiff.Z; // sum 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 an 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.
return(new Plane(kNormal, 0 - Vector3.Dot(kNormal, kOrigin)));
}
/// <summary>
/// Compute a plane which best fits a series of input points.
/// </summary>
/// <param name="tPoints">The array of points to analyse.</param>
/// <param name="tWeights">The relative weightings to apply to each point.</param>
/// <returns>The plane of best fit.</returns>
public static Plane ComputeBestFit(Vector3[] tPoints, float[] tWeights)
{
Vector3 kOrigin = new Vector3();
float w = 1;
float wtotal = 0;
// Calculate the weighted center of all the points.
for (int i = 0, n = tPoints.Length; i < n; ++i)
{
w = 1;
if (tWeights != null)
w *= tWeights[i];
Vector3 p = tPoints[i];
kOrigin.X += p.X * w;
kOrigin.Y += p.Y * w;
kOrigin.Z += p.Z * w;
wtotal += w;
}
float recip = 1.0f / wtotal; // reciprocol of total weighting
kOrigin.X *= recip;
kOrigin.Y *= recip;
kOrigin.Z *= recip;
float fSumXX = 0;
float fSumXY = 0;
float fSumXZ = 0;
float fSumYY = 0;
float fSumYZ = 0;
float fSumZZ = 0;
//--
for (int i = 0, n = tPoints.Length; i < n; ++i)
{
w = 1;
if (tWeights != null)
w *= tWeights[i];
// Transform to center and apply vertex weighting.
Vector3 p = tPoints[i];
Vector3 kDiff = new Vector3(w * (p.X - kOrigin.X), w * (p.Y - kOrigin.Y), w * (p.Z - kOrigin.Z));
fSumXX += kDiff.X * kDiff.X; // sum of the squares of the differences.
fSumXY += kDiff.X * kDiff.Y; // sum of the squares of the differences.
fSumXZ += kDiff.X * kDiff.Z; // sum 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 an 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.
return new Plane(kNormal, 0 - Vector3.Dot(kNormal, kOrigin));
}
