//-------------------------------------------------------------- /// <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); }