//--------------------------------------------------------------
/// <summary>
/// Creates the principal component analysis for the given list of points.
/// </summary>
/// <param name="points">
/// The list of data points. All points must have the same
/// <see cref="VectorF.NumberOfElements"/>.
/// </param>
/// <exception cref="ArgumentNullException">
/// <paramref name="points"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// <paramref name="points"/> is empty.
/// </exception>
public PrincipalComponentAnalysisF(IList<VectorF> points)
{
if (points == null)
throw new ArgumentNullException("points");
if (points.Count == 0)
throw new ArgumentException("The list of points is empty.");
// Compute covariance matrix.
MatrixF covarianceMatrix = StatisticsHelper.ComputeCovarianceMatrix(points);
// Perform Eigenvalue decomposition.
EigenvalueDecompositionF evd = new EigenvalueDecompositionF(covarianceMatrix);
int numberOfElements = evd.RealEigenvalues.NumberOfElements;
Variances = new VectorF(numberOfElements);
V = new MatrixF(numberOfElements, numberOfElements);
// Sort eigenvalues by decreasing value.
// Since covarianceMatrix is symmetric, we have no imaginary eigenvalues.
for (int i = 0; i < Variances.NumberOfElements; i++)
{
int index = evd.RealEigenvalues.IndexOfLargestElement;
Variances[i] = evd.RealEigenvalues[index];
V.SetColumn(i, evd.V.GetColumn(index));
evd.RealEigenvalues[index] = float.NegativeInfinity;
}
}
/// <summary>
/// Diagonalizes the inertia matrix.
/// </summary>
/// <param name="inertia">The inertia matrix.</param>
/// <param name="inertiaDiagonal">The inertia of the principal axes.</param>
/// <param name="rotation">
/// The rotation that rotates from principal axis space to parent/world space.
/// </param>
/// <remarks>
/// All valid inertia matrices can be transformed into a coordinate space where all elements
/// non-diagonal matrix elements are 0. The axis of this special space are the principal axes.
/// </remarks>
internal static void DiagonalizeInertia(Matrix33F inertia, out Vector3F inertiaDiagonal, out Matrix33F rotation)
{
// Alternatively we could use Jacobi transformation (iterative method, see Bullet/btMatrix3x3.diagonalize()
// and Numerical Recipes book) or we could find the eigenvalues using the characteristic
// polynomial which is a cubic polynomial and then solve for the eigenvectors (see Numeric
// Recipes and "Mathematics for 3D Game Programming and Computer Graphics" chapter ray-tracing
// for cubic equations and computation of bounding boxes.
// Perform eigenvalue decomposition.
var eigenValueDecomposition = new EigenvalueDecompositionF(inertia.ToMatrixF());
inertiaDiagonal = eigenValueDecomposition.RealEigenvalues.ToVector3F();
rotation = eigenValueDecomposition.V.ToMatrix33F();
if (!rotation.IsRotation)
{
// V is orthogonal but not necessarily a rotation. If it is no rotation
// we have to swap two columns.
MathHelper.Swap(ref inertiaDiagonal.Y, ref inertiaDiagonal.Z);
Vector3F dummy = rotation.GetColumn(1);
rotation.SetColumn(1, rotation.GetColumn(2));
rotation.SetColumn(2, dummy);
Debug.Assert(rotation.IsRotation);
}
}
public static void ComputeBoundingBox(IList<Vector3F> points, out Vector3F extent, out Pose pose)
{
// PCA of the convex hull is used (see "Physics-Based Animation", pp. 483, and others.)
// in addition to a brute force search. The optimum is returned.
if (points == null)
throw new ArgumentNullException("points");
if (points.Count == 0)
throw new ArgumentException("The list of 'points' is empty.");
// Covariance matrix.
MatrixF cov = null;
// ReSharper disable EmptyGeneralCatchClause
try
{
if (points.Count > 4)
{
// Reduce point list to convex hull.
DcelMesh dcelMesh = CreateConvexHull(points);
TriangleMesh mesh = dcelMesh.ToTriangleMesh();
// Use reduced point list - if we have found a useful one. (Line objects do not have
// a useful triangle mesh.)
if (mesh.Vertices.Count > 0)
{
points = mesh.Vertices;
cov = ComputeCovarianceMatrixFromSurface(mesh);
}
}
}
catch
{
}
// ReSharper restore EmptyGeneralCatchClause
// If anything happens in the convex hull creation, we can still go on including the
// interior points and compute the covariance matrix for the points instead of the
// surface.
if (cov == null || Numeric.IsNaN(cov.Determinant))
{
// Make copy of points list because ComputeBoundingBox() will reorder the points.
points = points.ToList();
cov = ComputeCovarianceMatrixFromPoints(points);
}
// Perform Eigenvalue decomposition.
EigenvalueDecompositionF evd = new EigenvalueDecompositionF(cov);
// v transforms from local coordinate space of the box into world space.
var v = evd.V.ToMatrix33F();
Debug.Assert(v.GetColumn(0).IsNumericallyNormalized);
Debug.Assert(v.GetColumn(1).IsNumericallyNormalized);
Debug.Assert(v.GetColumn(2).IsNumericallyNormalized);
// v is like a rotation matrix, but the coordinate system is not necessarily right handed.
// --> Make sure it is right-handed.
v.SetColumn(2, Vector3F.Cross(v.GetColumn(0), v.GetColumn(1)));
// Another way to do this:
//// Make sure that V is a rotation matrix. (V could be an orthogonal matrix that
//// contains a mirror operation. In other words, V could be a rotation matrix for
//// a left handed coordinate system.)
//if (!v.IsRotation)
//{
// // Swap two columns to create a right handed rotation matrix.
// Vector3F col1 = v.GetColumn(2);
// Vector3F col2 = v.GetColumn(2);
// v.SetColumn(1, col2);
// v.SetColumn(2, col1);
//}
// If the box axes are parallel to the world axes, create a box with NO rotation.
TryToMakeIdentityMatrix(ref v);
Vector3F center;
float volume = ComputeBoundingBox(points, v, float.PositiveInfinity, out extent, out center);
// Brute force search for better box.
// This was inspired by the OBB algorithm of John Ratcliff, www.codesuppository.com.
Vector3F αBest = Vector3F.Zero; // Search for optimal angles.
float αMax = MathHelper.ToRadians(45); // On each axis we rotate from -αMax to +αMax.
float αMin = MathHelper.ToRadians(1); // We abort when αMax == 1°.
const float numberOfSteps = 7; // In each iteration we divide αMax in this number of steps.
// In each loop we test angles between -αMax and +αMax.
// Then we half αMax and search again.
while (αMax >= αMin)
{
bool foundBetterAngles = false; // Better angles found?
float αStep = αMax / numberOfSteps;
// We test around this angle:
Vector3F α = αBest;
for (float αX = α.X - αMax; αX <= α.X + αMax; αX += αStep)
{
for (float αY = α.Y - αMax; αY <= α.Y + αMax; αY += αStep)
{
for (float αZ = α.Z - αMax; αZ <= α.Z + αMax; αZ += αStep)
{
Vector3F centerNew;
Vector3F boxExtentNew;
Matrix33F vNew = QuaternionF.CreateRotation(αX, Vector3F.UnitX, αY, Vector3F.UnitY, αZ, Vector3F.UnitZ, true).ToRotationMatrix33();
float volumeNew = ComputeBoundingBox(points, vNew, volume, out boxExtentNew, out centerNew);
if (volumeNew < volume)
{
foundBetterAngles = true;
center = centerNew;
extent = boxExtentNew;
v = vNew;
volume = volumeNew;
αBest = new Vector3F(αX, αY, αZ);
}
}
}
}
// Search again in half the interval around the best angles or abort.
if (foundBetterAngles)
αMax *= 0.5f;
else
αMax = 0;
}
pose = new Pose(center, v);
}
public static void ComputeBoundingCapsule(IList<Vector3F> points, out float radius, out float height, out Pose pose)
{
if (points == null)
throw new ArgumentNullException("points");
// Covariance matrix.
MatrixF cov = null;
// ReSharper disable EmptyGeneralCatchClause
try
{
if (points.Count > 4)
{
// Reduce point list to convex hull.
DcelMesh dcelMesh = CreateConvexHull(points);
TriangleMesh mesh = dcelMesh.ToTriangleMesh();
// Use reduced point list - if we have found a useful one. (Line objects
// have not useful triangle mesh.)
if (mesh.Vertices.Count > 0)
points = mesh.Vertices;
cov = ComputeCovarianceMatrixFromSurface(mesh);
}
}
catch
{
}
// ReSharper restore EmptyGeneralCatchClause
// If anything happens in the convex hull creation, we can still go on including the
// interior points and compute the covariance matrix for the points instead of the
// surface.
if (cov == null || Numeric.IsNaN(cov.Determinant))
cov = ComputeCovarianceMatrixFromPoints(points);
// Perform Eigenvalue decomposition.
EigenvalueDecompositionF evd = new EigenvalueDecompositionF(cov);
// v transforms from local coordinate space of the capsule into world space.
var v = evd.V.ToMatrix33F();
Debug.Assert(v.GetColumn(0).IsNumericallyNormalized);
Debug.Assert(v.GetColumn(1).IsNumericallyNormalized);
Debug.Assert(v.GetColumn(2).IsNumericallyNormalized);
// v is like a rotation matrix, but the coordinate system is not necessarily right handed.
// --> Make sure it is right-handed.
v.SetColumn(2, Vector3F.Cross(v.GetColumn(0), v.GetColumn(1)));
// Make local Y the largest axis. (Y is the long capsule axis.)
Vector3F eigenValues = evd.RealEigenvalues.ToVector3F();
int largestComponentIndex = eigenValues.IndexOfLargestComponent;
if (largestComponentIndex != 1)
{
// Swap two columns to create a right handed rotation matrix.
Vector3F colLargest = v.GetColumn(largestComponentIndex);
Vector3F col1 = v.GetColumn(1);
v.SetColumn(1, colLargest);
v.SetColumn(largestComponentIndex, col1);
v.SetColumn(2, Vector3F.Cross(v.GetColumn(0), v.GetColumn(1)));
}
// Compute capsule for the orientation given by v.
Vector3F center;
ComputeBoundingCapsule(points, v, out radius, out height, out center);
pose = new Pose(center, v);
}
