/// <summary>
/// Gets the enclosed volume of a triangle mesh.
/// </summary>
/// <param name="triangleMesh">The triangle mesh.</param>
/// <returns>
/// The enclosed volume of the given triangle mesh.
/// </returns>
/// <remarks>
/// <para>
/// This method assumes that the given triangle mesh is a closed mesh without holes.
/// </para>
/// <para>
/// Remember: To compute the volume of a scaled mesh, you can compute the volume of the
/// unscaled mesh and multiply the result with the scaling factors:
/// </para>
/// <para>
/// <i>volume<sub>scaled</sub> = volume<sub>unscaled</sub> * scale<sub>X</sub> * scale<sub>Y</sub> * scale<sub>Z</sub></i>
/// </para>
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="triangleMesh"/> is <see langword="null"/>.
/// </exception>
public static float GetVolume(this ITriangleMesh triangleMesh)
{
if (triangleMesh == null)
{
throw new ArgumentNullException("triangleMesh");
}
int numberOfTriangles = triangleMesh.NumberOfTriangles;
if (numberOfTriangles == 0)
{
return(0);
}
// The reference points is on the first triangle. So the first tetrahedron has no volume.
var p = triangleMesh.GetTriangle(0).Vertex0;
float volume = 0;
for (int i = 1; i < numberOfTriangles; i++)
{
var triangle = triangleMesh.GetTriangle(i);
float tetrahedronVolume = GetSignedTetrahedronVolume(p, ref triangle);
volume += tetrahedronVolume;
}
return(volume);
}
/**
* Flip triangle normal orientation.
* */
public static void Flip(ITriangleMesh mesh)
{
for (int i = 0; i < mesh.GetNumberOfTriangles(); i++)
{
mesh.GetTriangle(i).Flip();
}
}
/// <inheritdoc/>
public override Aabb GetAabb(Vector3 scale, Pose pose)
{
if (Numeric.IsNaN(_aabbLocal.Minimum.X))
{
// ----- Recompute local cached AABB if it is invalid.
if (Partition == null)
{
// ----- No spatial partition.
_aabbLocal = new Aabb();
if (_mesh != null && _mesh.NumberOfTriangles > 0)
{
bool isFirst = true;
// Get AABB that contains all triangles.
for (int i = 0; i < _mesh.NumberOfTriangles; i++)
{
Triangle triangle = _mesh.GetTriangle(i);
for (int j = 0; j < 3; j++)
{
Vector3 vertex = triangle[j];
if (isFirst)
{
isFirst = false;
_aabbLocal = new Aabb(vertex, vertex);
}
else
{
_aabbLocal.Grow(vertex);
}
}
}
}
}
else
{
// ----- With spatial partition.
// Use spatial partition to determine local AABB.
_aabbLocal = Partition.Aabb;
}
}
// Apply scale and pose to AABB.
return(_aabbLocal.GetAabb(scale, pose));
}
/**
* Create the VBO for the triangles
* */
void CreateTrianglesVBO()
{
trianglesVBO.Invalidate();
List <RenderVertex> renderVertices = new List <RenderVertex>();
for (int triangleIndex = 0; triangleIndex < mesh.GetNumberOfTriangles(); triangleIndex++)
{
Triangle t = mesh.GetTriangle(triangleIndex);
for (int vertexIndex = 0; vertexIndex < 3; vertexIndex++)
{
Vector3 pos = mesh.GetVertex(t.GetVertexIndex(vertexIndex));
Vector2 textCoords = mesh.GetTextureCoordinate(t.GetTexCoordIndex(vertexIndex));
renderVertices.Add(new RenderVertex(pos, t.Normal, color, textCoords));
}
}
trianglesVBO.Setup(renderVertices, PrimitiveType.Triangles);
}
// Handling of non-uniform scaling:
// http://en.wikipedia.org/wiki/Scaling_(geometry) about non-uniform scaling:
// "Such a scaling changes ... the volume by the product of all three [scale factors]."
/// <summary>
/// Gets the contact of ray with a triangle mesh.
/// </summary>
/// <param name="triangleMesh">The triangle mesh.</param>
/// <param name="ray">The ray.</param>
/// <param name="hitDistance">
/// The hit distance. This is the distance on the ray from the ray origin to the contact.
/// </param>
/// <returns>
/// <see langword="true"/> if the ray hits the front face of a triangle mesh triangle;
/// otherwise, <see langword="false"/>.
/// </returns>
/// <remarks>
/// This method returns any contact, not necessarily the first contact of the ray with the
/// triangle mesh! The mesh triangles are treated as one-sided.
/// </remarks>
internal static bool GetContact(ITriangleMesh triangleMesh, Ray ray, out float hitDistance)
{
for (int i = 0; i < triangleMesh.NumberOfTriangles; i++)
{
var triangle = triangleMesh.GetTriangle(i);
bool hit = GetContact(ray, triangle, false, out hitDistance);
if (hit)
{
return(true);
}
}
hitDistance = float.NaN;
return(false);
}
public void Add(ITriangleMesh mesh, bool weldVerticesBruteForce)
{
var triangleMesh = mesh as TriangleMesh;
if (triangleMesh != null && !weldVerticesBruteForce)
{
// Special: mesh is TriangleMesh and no welding.
if (triangleMesh.Vertices == null)
{
return;
}
if (triangleMesh.Indices == null)
{
return;
}
if (Vertices == null)
{
Vertices = new List <Vector3>(triangleMesh.Vertices.Count);
}
int numberOfNewIndices = triangleMesh.Indices.Count;
if (Indices == null)
{
Indices = new List <int>(numberOfNewIndices);
}
// Add new vertices.
int oldNumberOfVertices = Vertices.Count;
Vertices.AddRange(triangleMesh.Vertices);
// Add new indices. Add offset to all indices.
for (int i = 0; i < numberOfNewIndices; i++)
{
Indices.Add(triangleMesh.Indices[i] + oldNumberOfVertices);
}
return;
}
int numberOfTriangles = mesh.NumberOfTriangles;
for (int i = 0; i < numberOfTriangles; i++)
{
Add(mesh.GetTriangle(i), weldVerticesBruteForce);
}
}
private void ValidateInput()
{
int numberOfTriangles = _mesh.NumberOfTriangles;
for (int i = 0; i < numberOfTriangles; i++)
{
var triangle = _mesh.GetTriangle(i);
// Check for NaN or infinity. If we sum up all values, we only have to make one check.
float value = triangle.Vertex0.X + triangle.Vertex0.Y + triangle.Vertex0.Z
+ triangle.Vertex1.X + triangle.Vertex1.Y + triangle.Vertex1.Z
+ triangle.Vertex2.X + triangle.Vertex2.Y + triangle.Vertex2.Z;
if (!Numeric.IsFinite(value))
{
throw new GeometryException("Cannot compute convex decomposition because the vertex positions are invalid (e.g. NaN or infinity).");
}
}
}
public static ITriangleMesh Snap(ITriangleMesh mesh, float epsilon)
{
Dictionary <int, int> vertexMapping = new Dictionary <int, int>();
ITriangleMesh result = new TriangleMesh();
for (int i = 0; i < mesh.GetNumberOfVertices(); i++)
{
Vector3 vi = mesh.GetVertex(i);
bool found = false;
for (int j = 0; j < result.GetNumberOfVertices(); j++)
{
Vector3 vj = result.GetVertex(j);
if (Vector3.Subtract(vi, vj).Length < epsilon)
{
vertexMapping[i] = j;
found = true;
}
}
if (!found)
{
int idx = result.AddVertex(vi);
vertexMapping[i] = idx;
}
}
for (int i = 0; i < mesh.GetNumberOfTexCoords(); i++)
{
result.AddTextureCoordinate(mesh.GetTextureCoordinate(i));
}
for (int i = 0; i < mesh.GetNumberOfTriangles(); i++)
{
Triangle t = mesh.GetTriangle(i).Clone();
t.A = vertexMapping[t.A];
t.B = vertexMapping[t.B];
t.C = vertexMapping[t.C];
result.AddTriangle(t);
}
Console.WriteLine("Mesh snapping: " + mesh.GetNumberOfVertices() + " -> " + result.GetNumberOfVertices() + " verts.");
return(result);
}
/**
* Generated the unification of two meshes. Attention: no new mesh is generated, in fact the
* geometry of mesh2 is added to mesh1.
*/
public static void Unite(ITriangleMesh mesh1, ITriangleMesh mesh2)
{
int numberOfVertsMesh1 = mesh1.GetNumberOfVertices();
int numberOfTexCoordsMesh1 = mesh1.GetNumberOfTexCoords();
for (int i = 0; i < mesh2.GetNumberOfVertices(); i++)
{
mesh1.AddVertex(mesh2.GetVertex(i));
}
for (int i = 0; i < mesh2.GetNumberOfTexCoords(); i++)
{
mesh1.AddTextureCoordinate(mesh2.GetTextureCoordinate(i));
}
for (int i = 0; i < mesh2.GetNumberOfTriangles(); i++)
{
Triangle t = mesh2.GetTriangle(i).Clone();
t.VertexIndexOffset(numberOfVertsMesh1);
t.TexCoordsIndexOffset(numberOfTexCoordsMesh1);
mesh1.AddTriangle(t);
}
mesh1.ComputeTriangleNormals();
}
public void Add(ITriangleMesh mesh, bool weldVerticesBruteForce)
{
var triangleMesh = mesh as TriangleMesh;
if (triangleMesh != null && !weldVerticesBruteForce)
{
// Special: mesh is TriangleMesh and no welding.
if (triangleMesh.Vertices == null)
return;
if (triangleMesh.Indices == null)
return;
if (Vertices == null)
Vertices = new List<Vector3F>(triangleMesh.Vertices.Count);
int numberOfNewIndices = triangleMesh.Indices.Count;
if (Indices == null)
Indices = new List<int>(numberOfNewIndices);
// Add new vertices.
int oldNumberOfVertices = Vertices.Count;
Vertices.AddRange(triangleMesh.Vertices);
// Add new indices. Add offset to all indices.
for (int i = 0; i < numberOfNewIndices; i++)
Indices.Add(triangleMesh.Indices[i] + oldNumberOfVertices);
return;
}
int numberOfTriangles = mesh.NumberOfTriangles;
for (int i = 0; i < numberOfTriangles; i++)
Add(mesh.GetTriangle(i), weldVerticesBruteForce);
}
// Computes the surface covariance matrix for a convex hull of the given points.
private static MatrixF ComputeCovarianceMatrixFromSurface(ITriangleMesh mesh)
{
// Compute covariance matrix for the surface of the triangle mesh.
// See Physics-Based Animation for a derivation. Variable names are the same as in
// the book.
// ... Better look at Real-Time Collision Detection p. 108. The Physics-Based Animation
// book has errors.
MatrixF C = new MatrixF(3, 3); // The covariance matrix.
float A = 0; // Total surface area.
Vector3F mS = Vector3F.Zero; // Mean point of the entire surface.
for (int k = 0; k < mesh.NumberOfTriangles; k++)
{
var triangle = mesh.GetTriangle(k);
var pK = triangle.Vertex0;
var qK = triangle.Vertex1;
var rK = triangle.Vertex2;
var mK = 1f / 3f * (pK + qK + rK);
var uK = qK - pK;
var vK = rK - pK;
var Ak = 0.5f * Vector3F.Cross(uK, vK).Length;
A += Ak;
mS += Ak * mK;
for (int i = 0; i < 3; i++)
for (int j = i; j < 3; j++)
C[i, j] += Ak / 12f * (9 * mK[i] * mK[j]
+ pK[i] * pK[j]
+ qK[i] * qK[j]
+ rK[i] * rK[j]);
}
mS /= A;
for (int i = 0; i < 3; i++)
for (int j = i; j < 3; j++)
C[i, j] = 1 / A * C[i, j] - mS[i] * mS[j];
// Set the other half of the symmetric matrix.
for (int i = 0; i < 3; i++)
for (int j = i + 1; j < 3; j++)
C[j, i] = C[i, j];
return C;
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
if (type == CollisionQueryType.ClosestPoints)
{
// Just use normal composite shape algorithm.
_triangleMeshAlgorithm.ComputeCollision(contactSet, type);
return;
}
Debug.Assert(type != CollisionQueryType.ClosestPoints, "Closest point queries should have already been handled!");
// Mesh = A, Ray = B
IGeometricObject meshObject = contactSet.ObjectA.GeometricObject;
IGeometricObject rayObject = contactSet.ObjectB.GeometricObject;
// Object A should be the mesh, swap objects if necessary.
bool swapped = (meshObject.Shape is RayShape);
if (swapped)
{
MathHelper.Swap(ref rayObject, ref meshObject);
}
RayShape rayShape = rayObject.Shape as RayShape;
TriangleMeshShape meshShape = meshObject.Shape as TriangleMeshShape;
// Check if shapes are correct.
if (rayShape == null || meshShape == null)
{
throw new ArgumentException("The contact set must contain a ray and a triangle mesh shape.", "contactSet");
}
// Assume no contact.
contactSet.HaveContact = false;
// Get transformations.
Vector3F rayScale = rayObject.Scale;
Pose rayPose = rayObject.Pose;
Vector3F meshScale = meshObject.Scale;
Pose meshPose = meshObject.Pose;
// Ray in world space.
Ray rayWorld = new Ray(rayShape);
rayWorld.Scale(ref rayScale); // Scale ray.
rayWorld.ToWorld(ref rayPose); // Transform ray to world space.
// Ray in local scaled space of the mesh.
Ray ray = rayWorld;
ray.ToLocal(ref meshPose); // Transform ray to local space of composite.
// Ray in local unscaled space of the mesh.
Ray rayUnscaled = ray;
var inverseCompositeScale = Vector3F.One / meshScale;
rayUnscaled.Scale(ref inverseCompositeScale);
ITriangleMesh triangleMesh = meshShape.Mesh;
bool isTwoSided = meshShape.IsTwoSided;
if (meshShape.Partition != null)
{
// ----- Mesh with BVH vs. Ray -----
foreach (var childIndex in meshShape.Partition.GetOverlaps(rayUnscaled))
{
Triangle triangle = triangleMesh.GetTriangle(childIndex);
AddContact(contactSet, swapped, type, ref rayWorld, ref ray, ref triangle, childIndex, ref meshPose, ref meshScale, isTwoSided);
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
{
break; // We can abort early.
}
}
}
else
{
// ----- Mesh vs. Ray -----
var rayUnscaledDirectionInverse = new Vector3F(
1 / rayUnscaled.Direction.X,
1 / rayUnscaled.Direction.Y,
1 / rayUnscaled.Direction.Z);
float epsilon = Numeric.EpsilonF * (1 + meshObject.Aabb.Extent.Length);
int numberOfTriangles = triangleMesh.NumberOfTriangles;
for (int i = 0; i < numberOfTriangles; i++)
{
Triangle triangle = triangleMesh.GetTriangle(i);
// Make ray vs AABB check first. We could skip this because the ray vs. triangle test
// is also fast. But experiments (ray vs sphere mesh) have shown that making an
// additional ray vs. AABB test first makes the worst case more than 20% faster.
if (GeometryHelper.HaveContact(triangle.Aabb, rayUnscaled.Origin, rayUnscaledDirectionInverse, rayUnscaled.Length, epsilon))
{
AddContact(contactSet, swapped, type, ref rayWorld, ref ray, ref triangle, i, ref meshPose, ref meshScale, isTwoSided);
// We have contact and stop for boolean queries.
if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
{
break;
}
}
}
}
}
// Handling of non-uniform scaling:
// http://en.wikipedia.org/wiki/Scaling_(geometry) about non-uniform scaling:
// "Such a scaling changes ... the volume by the product of all three [scale factors]."
/// <summary>
/// Gets the contact of ray with a triangle mesh.
/// </summary>
/// <param name="triangleMesh">The triangle mesh.</param>
/// <param name="ray">The ray.</param>
/// <param name="hitDistance">
/// The hit distance. This is the distance on the ray from the ray origin to the contact.
/// </param>
/// <returns>
/// <see langword="true"/> if the ray hits the front face of a triangle mesh triangle;
/// otherwise, <see langword="false"/>.
/// </returns>
/// <remarks>
/// This method returns any contact, not necessarily the first contact of the ray with the
/// triangle mesh! The mesh triangles are treated as one-sided.
/// </remarks>
internal static bool GetContact(ITriangleMesh triangleMesh, Ray ray, out float hitDistance)
{
for (int i = 0; i < triangleMesh.NumberOfTriangles; i++)
{
var triangle = triangleMesh.GetTriangle(i);
bool hit = GetContact(ray, triangle, false, out hitDistance);
if (hit)
return true;
}
hitDistance = float.NaN;
return false;
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
// Object A should be the triangle mesh, swap objects if necessary.
// When testing TriangleMeshShape vs. TriangleMeshShape with BVH, object A should be the
// TriangleMeshShape with BVH - except when the TriangleMeshShape with BVH is a lot smaller
// than the other TriangleMeshShape. (See tests below.)
TriangleMeshShape triangleMeshShapeA = geometricObjectA.Shape as TriangleMeshShape;
TriangleMeshShape triangleMeshShapeB = geometricObjectB.Shape as TriangleMeshShape;
bool swapped = false;
// Check if collision objects shapes are correct.
if (triangleMeshShapeA == null && triangleMeshShapeB == null)
{
throw new ArgumentException("The contact set must contain a triangle mesh.", "contactSet");
}
Pose poseA = geometricObjectA.Pose;
Pose poseB = geometricObjectB.Pose;
Vector3F scaleA = geometricObjectA.Scale;
Vector3F scaleB = geometricObjectB.Scale;
// First we assume that we do not have a contact.
contactSet.HaveContact = false;
// ----- Use temporary test objects.
var testCollisionObjectA = ResourcePools.TestCollisionObjects.Obtain();
var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain();
// Create a test contact set and initialize with dummy objects.
// (The actual collision objects are set below.)
var testContactSet = ContactSet.Create(testCollisionObjectA, testCollisionObjectB);
var testGeometricObjectA = TestGeometricObject.Create();
var testGeometricObjectB = TestGeometricObject.Create();
var testTriangleShapeA = ResourcePools.TriangleShapes.Obtain();
var testTriangleShapeB = ResourcePools.TriangleShapes.Obtain();
try
{
if (triangleMeshShapeA != null &&
triangleMeshShapeA.Partition != null &&
triangleMeshShapeB != null &&
triangleMeshShapeB.Partition != null &&
(type != CollisionQueryType.ClosestPoints ||
triangleMeshShapeA.Partition is ISupportClosestPointQueries <int>))
{
#region ----- TriangleMesh with BVH vs. TriangleMesh with BVH -----
Debug.Assert(swapped == false, "Why did we swap the objects? Order of objects is fine.");
// Find collision algorithm for triangle vs. triangle (used in AddTriangleTriangleContacts()).
if (_triangleTriangleAlgorithm == null)
{
_triangleTriangleAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(TriangleShape), typeof(TriangleShape)];
}
if (type != CollisionQueryType.ClosestPoints)
{
// ----- Boolean or Contact Query
// Heuristic: Test large BVH vs. small BVH.
Aabb aabbOfA = geometricObjectA.Aabb;
Aabb aabbOfB = geometricObjectB.Aabb;
float largestExtentA = aabbOfA.Extent.LargestComponent;
float largestExtentB = aabbOfB.Extent.LargestComponent;
IEnumerable <Pair <int> > overlaps;
bool overlapsSwapped = largestExtentA < largestExtentB;
if (overlapsSwapped)
{
overlaps = triangleMeshShapeB.Partition.GetOverlaps(
scaleB,
geometricObjectB.Pose,
triangleMeshShapeA.Partition,
scaleA,
geometricObjectA.Pose);
}
else
{
overlaps = triangleMeshShapeA.Partition.GetOverlaps(
scaleA,
geometricObjectA.Pose,
triangleMeshShapeB.Partition,
scaleB,
geometricObjectB.Pose);
}
foreach (var overlap in overlaps)
{
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
{
break; // We can abort early.
}
AddTriangleTriangleContacts(
contactSet,
overlapsSwapped ? overlap.Second : overlap.First,
overlapsSwapped ? overlap.First : overlap.Second,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA,
testTriangleShapeA,
testCollisionObjectB,
testGeometricObjectB,
testTriangleShapeB);
}
}
else
{
// ----- Closest-Point Query
var callback = ClosestPointCallbacks.Obtain();
callback.CollisionAlgorithm = this;
callback.Swapped = false;
callback.ContactSet = contactSet;
callback.TestContactSet = testContactSet;
callback.TestCollisionObjectA = testCollisionObjectA;
callback.TestCollisionObjectB = testCollisionObjectB;
callback.TestGeometricObjectA = testGeometricObjectA;
callback.TestGeometricObjectB = testGeometricObjectB;
callback.TestTriangleA = testTriangleShapeA;
callback.TestTriangleB = testTriangleShapeB;
((ISupportClosestPointQueries <int>)triangleMeshShapeA.Partition)
.GetClosestPointCandidates(
scaleA,
geometricObjectA.Pose,
triangleMeshShapeB.Partition,
scaleB,
geometricObjectB.Pose,
callback.HandlePair);
ClosestPointCallbacks.Recycle(callback);
}
#endregion
}
else
{
Aabb aabbOfA = geometricObjectA.Aabb;
Aabb aabbOfB = geometricObjectB.Aabb;
float largestExtentA = aabbOfA.Extent.LargestComponent;
float largestExtentB = aabbOfB.Extent.LargestComponent;
// Choose which object should be A.
if (triangleMeshShapeA == null)
{
// A is no TriangleMesh. B must be a TriangleMesh.
swapped = true;
}
else if (triangleMeshShapeB == null)
{
// A is a TriangleMesh and B is no TriangleMesh.
}
else if (triangleMeshShapeA.Partition != null)
{
// A is a TriangleMesh with BVH and B is a TriangleMesh.
// We want to test TriangleMesh with BVH vs. * - unless the TriangleMesh with BVH is a lot
// smaller than the TriangleMesh.
if (largestExtentA * 2 < largestExtentB)
{
swapped = true;
}
}
else if (triangleMeshShapeB.Partition != null)
{
// A is a TriangleMesh and B is a TriangleMesh with BVH.
// We want to test TriangleMesh with BVH vs. * - unless the TriangleMesh BVH is a lot
// smaller than the TriangleMesh.
if (largestExtentB * 2 >= largestExtentA)
{
swapped = true;
}
}
else
{
// A and B are normal triangle meshes. A should be the larger object.
if (largestExtentA < largestExtentB)
{
swapped = true;
}
}
if (swapped)
{
// Swap all variables.
MathHelper.Swap(ref collisionObjectA, ref collisionObjectB);
MathHelper.Swap(ref geometricObjectA, ref geometricObjectB);
MathHelper.Swap(ref aabbOfA, ref aabbOfB);
MathHelper.Swap(ref largestExtentA, ref largestExtentB);
MathHelper.Swap(ref triangleMeshShapeA, ref triangleMeshShapeB);
MathHelper.Swap(ref poseA, ref poseB);
MathHelper.Swap(ref scaleA, ref scaleB);
}
if (triangleMeshShapeB == null &&
type != CollisionQueryType.ClosestPoints &&
largestExtentA * 2 < largestExtentB)
{
// B is a very large object and no triangle mesh.
// Make a AABB vs. Shape of B test for quick rejection.
BoxShape testBoxShape = ResourcePools.BoxShapes.Obtain();
testBoxShape.Extent = aabbOfA.Extent;
testGeometricObjectA.Shape = testBoxShape;
testGeometricObjectA.Scale = Vector3F.One;
testGeometricObjectA.Pose = new Pose(aabbOfA.Center);
testCollisionObjectA.SetInternal(collisionObjectA, testGeometricObjectA);
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
testContactSet.Reset(testCollisionObjectA, collisionObjectB);
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[testContactSet];
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
ResourcePools.BoxShapes.Recycle(testBoxShape);
if (!testContactSet.HaveContact)
{
contactSet.HaveContact = false;
return;
}
}
if (triangleMeshShapeA.Partition != null &&
(type != CollisionQueryType.ClosestPoints ||
triangleMeshShapeA.Partition is ISupportClosestPointQueries <int>))
{
#region ----- TriangleMesh BVH vs. * -----
// Get AABB of B in local space of A.
var aabbBInA = geometricObjectB.Shape.GetAabb(scaleB, poseA.Inverse * poseB);
// Apply inverse scaling to do the AABB-tree checks in the unscaled local space of A.
aabbBInA.Scale(Vector3F.One / scaleA);
if (type != CollisionQueryType.ClosestPoints)
{
// Boolean or Contact Query
foreach (var triangleIndex in triangleMeshShapeA.Partition.GetOverlaps(aabbBInA))
{
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
{
break; // We can abort early.
}
AddTriangleContacts(
contactSet,
swapped,
triangleIndex,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA,
testTriangleShapeA);
}
}
else if (type == CollisionQueryType.ClosestPoints)
{
// Closest-Point Query
var callback = ClosestPointCallbacks.Obtain();
callback.CollisionAlgorithm = this;
callback.Swapped = swapped;
callback.ContactSet = contactSet;
callback.TestContactSet = testContactSet;
callback.TestCollisionObjectA = testCollisionObjectA;
callback.TestCollisionObjectB = testCollisionObjectB;
callback.TestGeometricObjectA = testGeometricObjectA;
callback.TestGeometricObjectB = testGeometricObjectB;
callback.TestTriangleA = testTriangleShapeA;
callback.TestTriangleB = testTriangleShapeB;
((ISupportClosestPointQueries <int>)triangleMeshShapeA.Partition)
.GetClosestPointCandidates(
aabbBInA,
float.PositiveInfinity,
callback.HandleItem);
ClosestPointCallbacks.Recycle(callback);
}
#endregion
}
else
{
#region ----- TriangleMesh vs. * -----
// Find an upper bound for the distance we have to search.
// If object are in contact or we make contact/boolean query, then the distance is 0.
float closestPairDistance;
if (contactSet.HaveContact || type != CollisionQueryType.ClosestPoints)
{
closestPairDistance = 0;
}
else
{
// Make first guess for closest pair: inner point of B to inner point of mesh.
Vector3F innerPointA = poseA.ToWorldPosition(geometricObjectA.Shape.InnerPoint * scaleA);
Vector3F innerPointB = poseB.ToWorldPosition(geometricObjectB.Shape.InnerPoint * scaleB);
closestPairDistance = (innerPointB - innerPointA).Length + CollisionDetection.Epsilon;
}
// The search-space is a space where the closest points must lie in.
Vector3F minimum = aabbOfB.Minimum - new Vector3F(closestPairDistance);
Vector3F maximum = aabbOfB.Maximum + new Vector3F(closestPairDistance);
Aabb searchSpaceAabb = new Aabb(minimum, maximum);
// Test all triangles.
ITriangleMesh triangleMeshA = triangleMeshShapeA.Mesh;
int numberOfTriangles = triangleMeshA.NumberOfTriangles;
for (int i = 0; i < numberOfTriangles; i++)
{
// TODO: GetTriangle is performed twice! Here and in AddTriangleContacts() below!
Triangle triangle = triangleMeshA.GetTriangle(i);
testTriangleShapeA.Vertex0 = triangle.Vertex0 * scaleA;
testTriangleShapeA.Vertex1 = triangle.Vertex1 * scaleA;
testTriangleShapeA.Vertex2 = triangle.Vertex2 * scaleA;
// Make AABB test with search space.
if (GeometryHelper.HaveContact(searchSpaceAabb, testTriangleShapeA.GetAabb(poseA)))
{
// IMPORTANT: Info in triangleShape is destroyed in this method!
// Triangle is given to the method so that method does not allocate garbage.
AddTriangleContacts(
contactSet,
swapped,
i,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA,
testTriangleShapeA);
// We have contact and stop for boolean queries.
if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
{
break;
}
if (closestPairDistance > 0 && contactSet.HaveContact ||
contactSet.Count > 0 && -contactSet[contactSet.Count - 1].PenetrationDepth < closestPairDistance)
{
// Reduce search space
// Note: contactSet can contain several contacts. We assume that the last contact
// is the newest one and check only this.
if (contactSet.Count > 0)
{
closestPairDistance = Math.Max(0, -contactSet[contactSet.Count - 1].PenetrationDepth);
}
else
{
closestPairDistance = 0;
}
searchSpaceAabb.Minimum = aabbOfB.Minimum - new Vector3F(closestPairDistance);
searchSpaceAabb.Maximum = aabbOfB.Maximum + new Vector3F(closestPairDistance);
}
}
}
#endregion
}
}
}
finally
{
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectA);
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB);
testGeometricObjectB.Recycle();
testGeometricObjectA.Recycle();
ResourcePools.TriangleShapes.Recycle(testTriangleShapeB);
ResourcePools.TriangleShapes.Recycle(testTriangleShapeA);
}
}
/// <inheritdoc/>
/// <exception cref="ArgumentNullException">
/// <paramref name="objectA"/> or <paramref name="objectB"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// Neither <paramref name="objectA"/> nor <paramref name="objectB"/> is a
/// <see cref="TriangleMeshShape"/>.
/// </exception>
public override float GetTimeOfImpact(CollisionObject objectA, Pose targetPoseA, CollisionObject objectB, Pose targetPoseB, float allowedPenetration)
{
if (objectA == null)
{
throw new ArgumentNullException("objectA");
}
if (objectB == null)
{
throw new ArgumentNullException("objectB");
}
// Object A should be the triangle mesh, swap objects if necessary.
if (!(objectA.GeometricObject.Shape is TriangleMeshShape))
{
MathHelper.Swap(ref objectA, ref objectB);
MathHelper.Swap(ref targetPoseA, ref targetPoseB);
}
IGeometricObject geometricObjectA = objectA.GeometricObject;
IGeometricObject geometricObjectB = objectB.GeometricObject;
TriangleMeshShape triangleMeshShapeA = geometricObjectA.Shape as TriangleMeshShape;
// Check if collision objects shapes are correct.
if (triangleMeshShapeA == null)
{
throw new ArgumentException("One object must be a triangle mesh.");
}
// Currently mesh vs. mesh CCD is not supported.
if (objectB.GeometricObject.Shape is TriangleMeshShape)
{
return(1);
}
ITriangleMesh triangleMeshA = triangleMeshShapeA.Mesh;
Pose startPoseA = geometricObjectA.Pose;
Pose startPoseB = geometricObjectB.Pose;
Vector3F scaleA = geometricObjectA.Scale;
Vector3F scaleB = geometricObjectB.Scale;
// Get an AABB of the swept B in the space of A.
// This simplified AABB can miss some rotational movement.
// To simplify, we assume that A is static and B is moving relative to A.
// In general, this is not correct! But for CCD we make this simplification.
// We convert everything to the space of A.
var aabbSweptBInA = geometricObjectB.Shape.GetAabb(scaleB, startPoseA.Inverse * startPoseB);
aabbSweptBInA.Grow(geometricObjectB.Shape.GetAabb(scaleB, targetPoseA.Inverse * targetPoseB));
// Use temporary object.
var triangleShape = ResourcePools.TriangleShapes.Obtain();
// (Vertices will be set in the loop below.)
var testGeometricObject = TestGeometricObject.Create();
testGeometricObject.Shape = triangleShape;
testGeometricObject.Scale = Vector3F.One;
testGeometricObject.Pose = startPoseA;
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObject.SetInternal(objectA, testGeometricObject);
var collisionAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(TriangleShape), geometricObjectB.Shape.GetType()];
float timeOfImpact = 1;
if (triangleMeshShapeA.Partition != null)
{
// Apply inverse scaling to do the AABB-tree checks in the unscaled local space of A.
aabbSweptBInA.Scale(Vector3F.One / scaleA);
foreach (var triangleIndex in triangleMeshShapeA.Partition.GetOverlaps(aabbSweptBInA))
{
Triangle triangle = triangleMeshA.GetTriangle(triangleIndex);
// Apply scale.
triangle.Vertex0 = triangle.Vertex0 * scaleA;
triangle.Vertex1 = triangle.Vertex1 * scaleA;
triangle.Vertex2 = triangle.Vertex2 * scaleA;
triangleShape.Vertex0 = triangle.Vertex0;
triangleShape.Vertex1 = triangle.Vertex1;
triangleShape.Vertex2 = triangle.Vertex2;
float triangleTimeOfImpact = collisionAlgorithm.GetTimeOfImpact(
testCollisionObject, targetPoseA,
objectB, targetPoseB,
allowedPenetration);
timeOfImpact = Math.Min(timeOfImpact, triangleTimeOfImpact);
}
}
else
{
// Test all triangles.
int numberOfTriangles = triangleMeshA.NumberOfTriangles;
for (int triangleIndex = 0; triangleIndex < numberOfTriangles; triangleIndex++)
{
Triangle triangle = triangleMeshA.GetTriangle(triangleIndex);
// Apply scale.
triangle.Vertex0 = triangle.Vertex0 * scaleA;
triangle.Vertex1 = triangle.Vertex1 * scaleA;
triangle.Vertex2 = triangle.Vertex2 * scaleA;
// Make AABB test of triangle vs. sweep of B.
if (!GeometryHelper.HaveContact(aabbSweptBInA, triangle.Aabb))
{
continue;
}
triangleShape.Vertex0 = triangle.Vertex0;
triangleShape.Vertex1 = triangle.Vertex1;
triangleShape.Vertex2 = triangle.Vertex2;
float triangleTimeOfImpact = collisionAlgorithm.GetTimeOfImpact(
testCollisionObject, targetPoseA,
objectB, targetPoseB,
allowedPenetration);
timeOfImpact = Math.Min(timeOfImpact, triangleTimeOfImpact);
}
}
// Recycle temporary objects.
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
testGeometricObject.Recycle();
ResourcePools.TriangleShapes.Recycle(triangleShape);
return(timeOfImpact);
}
public static void GetMass(ITriangleMesh mesh, out float mass, out Vector3F centerOfMass, out Matrix33F inertia)
{
if (mesh == null)
{
throw new ArgumentNullException("mesh");
}
// Integral variables.
float i0 = 0, i1 = 0, i2 = 0, i3 = 0, i4 = 0, i5 = 0, i6 = 0, i7 = 0, i8 = 0, i9 = 0;
int numberOfTriangles = mesh.NumberOfTriangles;
for (int triangleIndex = 0; triangleIndex < numberOfTriangles; triangleIndex++)
{
var triangle = mesh.GetTriangle(triangleIndex);
// Vertex coordinates.
Vector3F v0 = triangle.Vertex0;
Vector3F v1 = triangle.Vertex1;
Vector3F v2 = triangle.Vertex2;
// Edges and cross products of edges
Vector3F a = v1 - v0;
Vector3F b = v2 - v0;
Vector3F d = Vector3F.Cross(a, b);
// Compute integral terms.
float f1x, f2x, f3x, g0x, g1x, g2x;
ComputePolyhedronMassSubExpressions(v0.X, v1.X, v2.X, out f1x, out f2x, out f3x, out g0x, out g1x, out g2x);
float f1y, f2y, f3y, g0y, g1y, g2y;
ComputePolyhedronMassSubExpressions(v0.Y, v1.Y, v2.Y, out f1y, out f2y, out f3y, out g0y, out g1y, out g2y);
float f1z, f2z, f3z, g0z, g1z, g2z;
ComputePolyhedronMassSubExpressions(v0.Z, v1.Z, v2.Z, out f1z, out f2z, out f3z, out g0z, out g1z, out g2z);
// Update integrals.
i0 += d.X * f1x;
i1 += d.X * f2x;
i2 += d.Y * f2y;
i3 += d.Z * f2z;
i4 += d.X * f3x;
i5 += d.Y * f3y;
i6 += d.Z * f3z;
i7 += d.X * (v0.Y * g0x + v1.Y * g1x + v2.Y * g2x);
i8 += d.Y * (v0.Z * g0y + v1.Z * g1y + v2.Z * g2y);
i9 += d.Z * (v0.X * g0z + v1.X * g1z + v2.X * g2z);
}
i0 /= 6.0f;
i1 /= 24.0f;
i2 /= 24.0f;
i3 /= 24.0f;
i4 /= 60.0f;
i5 /= 60.0f;
i6 /= 60.0f;
i7 /= 120.0f;
i8 /= 120.0f;
i9 /= 120.0f;
mass = i0;
centerOfMass = 1.0f / mass * new Vector3F(i1, i2, i3);
// Clamp to zero.
if (Numeric.IsZero(centerOfMass.X))
{
centerOfMass.X = 0;
}
if (Numeric.IsZero(centerOfMass.Y))
{
centerOfMass.Y = 0;
}
if (Numeric.IsZero(centerOfMass.Z))
{
centerOfMass.Z = 0;
}
// Inertia around the world origin.
inertia.M00 = i5 + i6;
inertia.M11 = i4 + i6;
inertia.M22 = i4 + i5;
inertia.M01 = inertia.M10 = Numeric.IsZero(i7) ? 0 : -i7;
inertia.M12 = inertia.M21 = Numeric.IsZero(i8) ? 0 : -i8;
inertia.M02 = inertia.M20 = Numeric.IsZero(i9) ? 0 : -i9;
// Inertia around center of mass.
inertia = GetUntranslatedMassInertia(mass, inertia, centerOfMass);
}
// Computes the surface covariance matrix for a convex hull of the given points.
private static MatrixF ComputeCovarianceMatrixFromSurface(ITriangleMesh mesh)
{
// Compute covariance matrix for the surface of the triangle mesh.
// See Physics-Based Animation for a derivation. Variable names are the same as in
// the book.
// ... Better look at Real-Time Collision Detection p. 108. The Physics-Based Animation
// book has errors.
MatrixF C = new MatrixF(3, 3); // The covariance matrix.
float A = 0; // Total surface area.
Vector3 mS = Vector3.Zero; // Mean point of the entire surface.
for (int k = 0; k < mesh.NumberOfTriangles; k++)
{
var triangle = mesh.GetTriangle(k);
var pK = triangle.Vertex0;
var qK = triangle.Vertex1;
var rK = triangle.Vertex2;
var mK = 1f / 3f * (pK + qK + rK);
var uK = qK - pK;
var vK = rK - pK;
var Ak = 0.5f * Vector3.Cross(uK, vK).Length;
A += Ak;
mS += Ak * mK;
for (int i = 0; i < 3; i++)
{
for (int j = i; j < 3; j++)
{
C[i, j] += Ak / 12f * (9 * mK[i] * mK[j]
+ pK[i] * pK[j]
+ qK[i] * qK[j]
+ rK[i] * rK[j]);
}
}
}
mS /= A;
for (int i = 0; i < 3; i++)
{
for (int j = i; j < 3; j++)
{
C[i, j] = 1 / A * C[i, j] - mS[i] * mS[j];
}
}
// Set the other half of the symmetric matrix.
for (int i = 0; i < 3; i++)
{
for (int j = i + 1; j < 3; j++)
{
C[j, i] = C[i, j];
}
}
return(C);
}
public static void GetMass(ITriangleMesh mesh, out float mass, out Vector3F centerOfMass, out Matrix33F inertia)
{
if (mesh == null)
throw new ArgumentNullException("mesh");
// Integral variables.
float i0 = 0, i1 = 0, i2 = 0, i3 = 0, i4 = 0, i5 = 0, i6 = 0, i7 = 0, i8 = 0, i9 = 0;
int numberOfTriangles = mesh.NumberOfTriangles;
for (int triangleIndex = 0; triangleIndex < numberOfTriangles; triangleIndex++)
{
var triangle = mesh.GetTriangle(triangleIndex);
// Vertex coordinates.
Vector3F v0 = triangle.Vertex0;
Vector3F v1 = triangle.Vertex1;
Vector3F v2 = triangle.Vertex2;
// Edges and cross products of edges
Vector3F a = v1 - v0;
Vector3F b = v2 - v0;
Vector3F d = Vector3F.Cross(a, b);
// Compute integral terms.
float f1x, f2x, f3x, g0x, g1x, g2x;
ComputePolyhedronMassSubExpressions(v0.X, v1.X, v2.X, out f1x, out f2x, out f3x, out g0x, out g1x, out g2x);
float f1y, f2y, f3y, g0y, g1y, g2y;
ComputePolyhedronMassSubExpressions(v0.Y, v1.Y, v2.Y, out f1y, out f2y, out f3y, out g0y, out g1y, out g2y);
float f1z, f2z, f3z, g0z, g1z, g2z;
ComputePolyhedronMassSubExpressions(v0.Z, v1.Z, v2.Z, out f1z, out f2z, out f3z, out g0z, out g1z, out g2z);
// Update integrals.
i0 += d.X * f1x;
i1 += d.X * f2x;
i2 += d.Y * f2y;
i3 += d.Z * f2z;
i4 += d.X * f3x;
i5 += d.Y * f3y;
i6 += d.Z * f3z;
i7 += d.X * (v0.Y * g0x + v1.Y * g1x + v2.Y * g2x);
i8 += d.Y * (v0.Z * g0y + v1.Z * g1y + v2.Z * g2y);
i9 += d.Z * (v0.X * g0z + v1.X * g1z + v2.X * g2z);
}
i0 /= 6.0f;
i1 /= 24.0f;
i2 /= 24.0f;
i3 /= 24.0f;
i4 /= 60.0f;
i5 /= 60.0f;
i6 /= 60.0f;
i7 /= 120.0f;
i8 /= 120.0f;
i9 /= 120.0f;
mass = i0;
centerOfMass = 1.0f / mass * new Vector3F(i1, i2, i3);
// Clamp to zero.
if (Numeric.IsZero(centerOfMass.X))
centerOfMass.X = 0;
if (Numeric.IsZero(centerOfMass.Y))
centerOfMass.Y = 0;
if (Numeric.IsZero(centerOfMass.Z))
centerOfMass.Z = 0;
// Inertia around the world origin.
inertia.M00 = i5 + i6;
inertia.M11 = i4 + i6;
inertia.M22 = i4 + i5;
inertia.M01 = inertia.M10 = Numeric.IsZero(i7) ? 0 : -i7;
inertia.M12 = inertia.M21 = Numeric.IsZero(i8) ? 0 : -i8;
inertia.M02 = inertia.M20 = Numeric.IsZero(i9) ? 0 : -i9;
// Inertia around center of mass.
inertia = GetUntranslatedMassInertia(mass, inertia, centerOfMass);
}
/// <summary>
/// Draws the triangles of the given mesh (with counter-clockwise winding for front faces).
/// </summary>
/// <param name="mesh">The triangle mesh.</param>
/// <param name="pose">The pose.</param>
/// <param name="scale">The scale.</param>
/// <param name="color">The color.</param>
/// <param name="drawWireFrame">
/// If set to <see langword="true"/> the wire-frame is drawn; otherwise the object is drawn
/// with solid faces.
/// </param>
/// <param name="drawOverScene">
/// If set to <see langword="true"/> the object is drawn over the graphics scene (depth-test
/// disabled).
/// </param>
/// <remarks>
/// Warning: Calling this method every frame to render the same triangle mesh is very
/// inefficient! If the triangle mesh does not change, call <see cref="DrawShape"/> with a
/// <see cref="TriangleMeshShape"/> instead!
/// </remarks>
/// <exception cref="NotSupportedException">
/// Drawing solid objects with disabled depth test is not yet supported.
/// </exception>
public void DrawTriangles(ITriangleMesh mesh, Pose pose, Vector3F scale,
Color color, bool drawWireFrame, bool drawOverScene)
{
if (!Enabled || mesh == null)
return;
if (!drawWireFrame && drawOverScene)
throw new NotSupportedException("Drawing solid objects with disabled depth test is not yet supported.");
TriangleBatch batch;
if (drawOverScene)
batch = OverSceneWireframeTriangleBatch;
else if (drawWireFrame)
batch = InSceneWireframeTriangleBatch;
else if (color.A == 255)
batch = OpaqueTriangleBatch;
else
batch = TransparentTriangleBatch;
if (Vector3F.AreNumericallyEqual(scale, Vector3F.One) && !pose.HasRotation && !pose.HasTranslation)
{
int numberOfTriangles = mesh.NumberOfTriangles;
for (int i = 0; i < numberOfTriangles; i++)
{
var triangle = mesh.GetTriangle(i);
var normal = Vector3F.Cross(triangle.Vertex1 - triangle.Vertex0, triangle.Vertex2 - triangle.Vertex0);
// (normal is normalized in the BasicEffect HLSL.)
// Draw with swapped winding order!
batch.Add(ref triangle.Vertex0, ref triangle.Vertex2, ref triangle.Vertex1, ref normal, ref color);
}
}
else
{
var transform = pose * Matrix44F.CreateScale(scale);
int numberOfTriangles = mesh.NumberOfTriangles;
for (int i = 0; i < numberOfTriangles; i++)
{
var triangle = mesh.GetTriangle(i);
// Transform to world space.
triangle.Vertex0 = transform.TransformPosition(triangle.Vertex0);
triangle.Vertex1 = transform.TransformPosition(triangle.Vertex1);
triangle.Vertex2 = transform.TransformPosition(triangle.Vertex2);
var normal = Vector3F.Cross(triangle.Vertex1 - triangle.Vertex0, triangle.Vertex2 - triangle.Vertex0);
// (normal is normalized in the BasicEffect HLSL.)
// Draw with swapped winding order!
batch.Add(ref triangle.Vertex0, ref triangle.Vertex2, ref triangle.Vertex1, ref normal, ref color);
}
}
}