public static void ComputeBoundingCapsule(IList <Vector3> 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.ToMatrix(); 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, Vector3.Cross(v.GetColumn(0), v.GetColumn(1))); // Make local Y the largest axis. (Y is the long capsule axis.) Vector3 eigenValues = evd.RealEigenvalues.ToVector3(); int largestComponentIndex = eigenValues.IndexOfLargestComponent; if (largestComponentIndex != 1) { // Swap two columns to create a right handed rotation matrix. Vector3 colLargest = v.GetColumn(largestComponentIndex); Vector3 col1 = v.GetColumn(1); v.SetColumn(1, colLargest); v.SetColumn(largestComponentIndex, col1); v.SetColumn(2, Vector3.Cross(v.GetColumn(0), v.GetColumn(1))); } // Compute capsule for the orientation given by v. Vector3 center; ComputeBoundingCapsule(points, v, out radius, out height, out center); pose = new Pose(center, v); }
public static void ComputeBoundingBox(IList <Vector3> points, out Vector3 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.ToMatrix(); 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, Vector3.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. // Vector3 col1 = v.GetColumn(2); // Vector3 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); Vector3 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. Vector3 αBest = Vector3.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: Vector3 α = α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) { Vector3 centerNew; Vector3 boxExtentNew; Matrix vNew = Quaternion.CreateFromRotationMatrix(αX, Vector3.UnitX, αY, Vector3.UnitY, αZ, Vector3.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 Vector3(α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); }