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