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