// Adds a contact to the contact set. private void AddContact(ref Vector3 vertex, ref Plane planeWorld, float penetrationDepth, bool swapped, ContactSet contactSet, CollisionQueryType type) { Vector3 position = vertex + planeWorld.Normal * (penetrationDepth / 2); Vector3 normal = (swapped) ? -planeWorld.Normal : planeWorld.Normal; // Update contact set. Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); }
public void UpdateContacts() { CollisionObject a = new CollisionObject { GeometricObject = new GeometricObject { Shape = new SphereShape(1) } }; CollisionObject b = new CollisionObject(new GeometricObject(new SphereShape(2), new Pose(new Vector3F(2, 0, 0)))); a.Changed = false; b.Changed = false; ContactSet set = ContactSet.Create(a, b); ContactHelper.UpdateContacts(set, 0.01f, 0.1f); set.Add(ContactHelper.CreateContact(set, new Vector3F(1, 0, 0), new Vector3F(1, 0, 0), 0, false)); set.Add(ContactHelper.CreateContact(set, new Vector3F(1, 1, 0), new Vector3F(0, 0, 1), 0, false)); ContactHelper.UpdateContacts(set, 0.01f, 0.1f); Assert.AreEqual(0.01f, set[0].Lifetime); Assert.AreEqual(0.01f, set[1].Lifetime); ((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(1.8f, 0.1f, -0.1f)); ContactHelper.UpdateContacts(set, 0.01f, 0.3f); Assert.AreEqual(0.02f, set[0].Lifetime); Assert.AreEqual(0.02f, set[1].Lifetime); Assert.IsTrue(Numeric.AreEqual(0.2f, set[0].PenetrationDepth)); Assert.IsTrue(Numeric.AreEqual(0.1f, set[1].PenetrationDepth)); // Remove first contact because it separates. ((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(2.1f, 0, -0.1f)); ContactHelper.UpdateContacts(set, 0.01f, 0.4f); Assert.AreEqual(0.03f, set[0].Lifetime); Assert.IsTrue(Numeric.AreEqual(0.1f, set[0].PenetrationDepth)); // Remove second contact because of horizontal drift. ((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(2.2f, 0.0f, -0.1f)); ContactHelper.UpdateContacts(set, 0.01f, 0.1f); Assert.AreEqual(0, set.Count); // Test surface contacts (ray casts). ((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(2, 0, 0)); set.Add(ContactHelper.CreateContact(set, new Vector3F(1, 0, 0), new Vector3F(1, 0, 0), 0, true)); set.Add(ContactHelper.CreateContact(set, new Vector3F(1, 0, 0), new Vector3F(1, 0, 0), 0, false)); ((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(1.9f, 0, 0)); ContactHelper.UpdateContacts(set, 0.01f, 0.2f); Assert.AreEqual(2, set.Count); Assert.IsTrue(Numeric.AreEqual(0f, set[0].PenetrationDepth)); Assert.IsTrue(Numeric.AreEqual(0.1f, set[1].PenetrationDepth)); ContactHelper.UpdateContacts(set, 0.01f, 0.1f); Assert.AreEqual(1, set.Count); Assert.IsTrue(Numeric.AreEqual(0.1f, set[0].PenetrationDepth)); }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { contactSet.Clear(); contactSet.Add(ContactHelper.CreateContact(contactSet, new Vector3(1, 2, 3), Vector3.UnitZ, 1, false)); if (type == CollisionQueryType.Contacts) { contactSet.Add(ContactHelper.CreateContact(contactSet, new Vector3(2, 2, 3), Vector3.UnitZ, 1.2f, false)); } contactSet.HaveContact = true; }
public void CreateContact() { Contact c = ContactHelper.CreateContact( new CollisionObject { GeometricObject = new GeometricObject { Pose = new Pose(new Vector3F(1, 2, 3)) } }, new CollisionObject { GeometricObject = new GeometricObject { Pose = new Pose(new Vector3F(4, 5, 6)) } }, new Vector3F(10, 10, 10), new Vector3F(0, 0, 1), 10, false); Assert.AreEqual(new Vector3F(10, 10, 10), c.Position); Assert.AreEqual(new Vector3F(0, 0, 1), c.Normal); Assert.AreEqual(10, c.PenetrationDepth); Assert.AreEqual(false, c.IsRayHit); Assert.AreEqual(new Vector3F(9, 8, 12), c.PositionALocal); Assert.AreEqual(new Vector3F(6, 5, -1), c.PositionBLocal); Contact surfaceC = ContactHelper.CreateContact( new CollisionObject { GeometricObject = new GeometricObject { Pose = new Pose(new Vector3F(1, 2, 3)) } }, new CollisionObject { GeometricObject = new GeometricObject { Pose = new Pose(new Vector3F(4, 5, 6)) } }, new Vector3F(10, 10, 10), new Vector3F(0, 0, 1), 10, true); Assert.AreEqual(new Vector3F(10, 10, 10), surfaceC.Position); Assert.AreEqual(new Vector3F(0, 0, 1), surfaceC.Normal); Assert.AreEqual(10, surfaceC.PenetrationDepth); Assert.AreEqual(true, surfaceC.IsRayHit); Assert.AreEqual(new Vector3F(9, 8, 7), surfaceC.PositionALocal); Assert.AreEqual(new Vector3F(6, 5, 4), surfaceC.PositionBLocal); }
public void Swapped() { CollisionObject a = new CollisionObject(); CollisionObject b = new CollisionObject(); ContactSet set = ContactSet.Create(a, b); set.Add(ContactHelper.CreateContact(set, new Vector3(1, 2, 3), new Vector3(0, 0, 1), 10, false)); set.Add(ContactHelper.CreateContact(set, new Vector3(4, 5, 6), new Vector3(0, 0, 1), 10, false)); set.Add(ContactHelper.CreateContact(set, new Vector3(7, 8, 9), new Vector3(0, 0, 1), 10, false)); ContactSet swapped = set.Swapped; Assert.AreEqual(set.ObjectA, swapped.ObjectB); Assert.AreEqual(set.ObjectB, swapped.ObjectA); Assert.AreEqual(set[0].PositionALocal, swapped[0].PositionBLocal); Assert.AreEqual(set[1].Normal, -swapped[1].Normal); }
public void TestNoContact() { SphereSphereAlgorithm algo = new SphereSphereAlgorithm(new CollisionDetection()); CollisionObject objectA = new CollisionObject(); ((GeometricObject)objectA.GeometricObject).Shape = new SphereShape(1); CollisionObject objectB = new CollisionObject(); ((GeometricObject)objectB.GeometricObject).Shape = new SphereShape(0.5f); ((GeometricObject)objectA.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0)); ((GeometricObject)objectB.GeometricObject).Pose = new Pose(new Vector3F(1.6f, 0, 0)); ContactSet cs = ContactSet.Create(objectA, objectB); cs.Add(ContactHelper.CreateContact(cs, Vector3F.Zero, Vector3F.UnitX, 0, false)); algo.UpdateContacts(cs, 0); Assert.AreEqual(objectA, cs.ObjectA); Assert.AreEqual(objectB, cs.ObjectB); Assert.AreEqual(0, cs.Count); }
public void UpdateClosestPoints() { ContactSet set = ContactSet.Create(_objectA, _objectB); _collisionDetection.UpdateClosestPoints(set, 0); Assert.AreEqual(1, set.Count); Assert.AreEqual(_objectA, set.ObjectA); Assert.AreEqual(_objectB, set.ObjectB); set = ContactSet.Create(_objectA, _objectC); set.Add(ContactHelper.CreateContact(set, new Vector3F(1, 0, 0), new Vector3F(1, 0, 0), -10, false)); _collisionDetection.UpdateClosestPoints(set, 0); Assert.AreEqual(1, set.Count); Assert.AreEqual(_objectA, set.ObjectA); Assert.AreEqual(_objectC, set.ObjectB); _collisionDetection.CollisionFilter = new CollisionFilter(); ((CollisionFilter)_collisionDetection.CollisionFilter).Set(_objectA, _objectB, false); set = ContactSet.Create(_objectA, _objectB); _collisionDetection.UpdateClosestPoints(set, 0); Assert.AreEqual(1, set.Count); Assert.AreEqual(_objectA, set.ObjectA); Assert.AreEqual(_objectB, set.ObjectB); }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { // Object A should be the plane. // Object B should be the other object. IGeometricObject planeObject = contactSet.ObjectA.GeometricObject; IGeometricObject convexObject = contactSet.ObjectB.GeometricObject; // Swap objects if necessary. bool swapped = (convexObject.Shape is PlaneShape); if (swapped) { MathHelper.Swap(ref planeObject, ref convexObject); } PlaneShape planeShape = planeObject.Shape as PlaneShape; ConvexShape convexShape = convexObject.Shape as ConvexShape; // Check if shapes are correct. if (planeShape == null || convexShape == null) { throw new ArgumentException("The contact set must contain a plane and a convex shape.", "contactSet"); } // Get transformations. Vector3F scalePlane = planeObject.Scale; Vector3F scaleB = convexObject.Scale; Pose planePose = planeObject.Pose; Pose poseB = convexObject.Pose; // Apply scale to plane and transform plane into world space. Plane planeWorld = new Plane(planeShape); planeWorld.Scale(ref scalePlane); // Scale plane. planeWorld.ToWorld(ref planePose); // Transform plane to world space. // Transform plane normal to local space of convex. Vector3F planeNormalLocalB = poseB.ToLocalDirection(planeWorld.Normal); // Get support vertex nearest to the plane. Vector3F supportVertexBLocal = convexShape.GetSupportPoint(-planeNormalLocalB, scaleB); // Transform support vertex into world space. Vector3F supportVertexBWorld = poseB.ToWorldPosition(supportVertexBLocal); // Project vertex onto separating axis (given by plane normal). float distance = Vector3F.Dot(supportVertexBWorld, planeWorld.Normal); // Check for collision. float penetrationDepth = planeWorld.DistanceFromOrigin - distance; contactSet.HaveContact = (penetrationDepth >= 0); if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact)) { // HaveContact queries can exit here. // GetContacts queries can exit here if we don't have a contact. return; } // Position is between support vertex and plane. Vector3F position = supportVertexBWorld + planeWorld.Normal * (penetrationDepth / 2); Vector3F normal = (swapped) ? -planeWorld.Normal : planeWorld.Normal; // Update contact set. Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); if (CollisionDetection.FullContactSetPerFrame && type == CollisionQueryType.Contacts && contactSet.Count > 0 && contactSet.Count < 4) { // Special treatment for tetrahedra: Test all vertices against plane. IList <Vector3F> vertices = null; if (convexShape is ConvexHullOfPoints) { var convexHullOfPoints = (ConvexHullOfPoints)convexShape; vertices = convexHullOfPoints.Points; } else if (convexShape is ConvexPolyhedron) { var convexPolyhedron = (ConvexPolyhedron)convexShape; vertices = convexPolyhedron.Vertices; } if (vertices != null && vertices.Count <= 8) { // Convex has 8 or less vertices. Explicitly test all vertices against the plane. int numberOfVertices = vertices.Count; for (int i = 0; i < numberOfVertices; i++) { // Test is the same as above. var vertex = vertices[i]; Vector3F scaledVertex = vertex * scaleB; if (scaledVertex != supportVertexBLocal) // supportVertexBLocal has already been added. { Vector3F vertexWorld = poseB.ToWorldPosition(scaledVertex); distance = Vector3F.Dot(vertexWorld, planeWorld.Normal); penetrationDepth = planeWorld.DistanceFromOrigin - distance; if (penetrationDepth >= 0) { position = vertexWorld + planeWorld.Normal * (penetrationDepth / 2); normal = (swapped) ? -planeWorld.Normal : planeWorld.Normal; contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); } } } } else { // Convex is a complex shape with more than 4 vertices. ContactHelper.TestWithPerturbations( CollisionDetection, contactSet, !swapped, // Perturb the convex object, not the plane. _computeContactsMethod); } } }
/// <summary> /// Computes the collision between line vs. line. /// </summary> /// <param name="contactSet">The contact set.</param> /// <param name="type">The type of collision query.</param> private void ComputeLineVsLine(ContactSet contactSet, CollisionQueryType type) { IGeometricObject objectA = contactSet.ObjectA.GeometricObject; IGeometricObject objectB = contactSet.ObjectB.GeometricObject; Debug.Assert(objectA.Shape is LineShape && objectB.Shape is LineShape, "LineAlgorithm.ComputeLineVsLine should only be called for 2 line shapes."); Debug.Assert(contactSet.Count <= 1, "Two lines should have at max 1 contact point."); // Get transformations. Vector3F scaleA = objectA.Scale; Vector3F scaleB = objectB.Scale; Pose poseA = objectA.Pose; Pose poseB = objectB.Pose; // Create two line objects in world space. var lineA = new Line((LineShape)objectA.Shape); lineA.Scale(ref scaleA); lineA.ToWorld(ref poseA); var lineB = new Line((LineShape)objectB.Shape); lineB.Scale(ref scaleB); lineB.ToWorld(ref poseB); // Get closest points. Vector3F pointA; Vector3F pointB; contactSet.HaveContact = GeometryHelper.GetClosestPoints(lineA, lineB, out pointA, out pointB); if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact)) { // HaveContact queries can exit here. // GetContacts queries can exit here if we don't have a contact. return; } // Create contact information. Vector3F position = (pointA + pointB) / 2; Vector3F normal = pointB - pointA; float length = normal.Length; if (Numeric.IsZero(length)) { // Create normal from cross product of both lines. normal = Vector3F.Cross(lineA.Direction, lineB.Direction); if (!normal.TryNormalize()) { normal = Vector3F.UnitY; } } else { // Normalize vector normal = normal / length; } Contact contact = ContactHelper.CreateContact(contactSet, position, normal, -length, false); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { // Object A should be the plane. // Object B should be the sphere. IGeometricObject planeObject = contactSet.ObjectA.GeometricObject; IGeometricObject sphereObject = contactSet.ObjectB.GeometricObject; // A should be the plane, swap objects if necessary. bool swapped = (sphereObject.Shape is PlaneShape); if (swapped) { MathHelper.Swap(ref planeObject, ref sphereObject); } PlaneShape planeShape = planeObject.Shape as PlaneShape; SphereShape sphereShape = sphereObject.Shape as SphereShape; // Check if collision object shapes are correct. if (planeShape == null || sphereShape == null) { throw new ArgumentException("The contact set must contain a plane and a sphere.", "contactSet"); } // Get scalings. Vector3F planeScale = planeObject.Scale; Vector3F sphereScale = Vector3F.Absolute(sphereObject.Scale); // Call other algorithm for non-uniformly scaled spheres. if (sphereScale.X != sphereScale.Y || sphereScale.Y != sphereScale.Z) { if (_fallbackAlgorithm == null) { _fallbackAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(PlaneShape), typeof(ConvexShape)]; } _fallbackAlgorithm.ComputeCollision(contactSet, type); return; } // Get poses. Pose planePose = planeObject.Pose; Pose spherePose = sphereObject.Pose; // Apply scaling to plane and transform plane to world space. Plane plane = new Plane(planeShape); plane.Scale(ref planeScale); // Scale plane. plane.ToWorld(ref planePose); // Transform plane to world space. // Calculate distance from plane to sphere surface. float sphereRadius = sphereShape.Radius * sphereScale.X; Vector3F sphereCenter = spherePose.Position; float planeToSphereDistance = Vector3F.Dot(sphereCenter, plane.Normal) - sphereRadius - plane.DistanceFromOrigin; float penetrationDepth = -planeToSphereDistance; contactSet.HaveContact = (penetrationDepth >= 0); if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact)) { // HaveContact queries can exit here. // GetContacts queries can exit here if we don't have a contact. return; } // Compute contact details. Vector3F position = sphereCenter - plane.Normal * (sphereRadius - penetrationDepth / 2); Vector3F normal = (swapped) ? -plane.Normal : plane.Normal; // Update contact set. Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); }
// The parameters 'testXxx' are initialized objects which are re-used to avoid a lot of GC garbage. private void AddTriangleTriangleContacts( ContactSet contactSet, int triangleIndexA, int triangleIndexB, CollisionQueryType type, ContactSet testContactSet, CollisionObject testCollisionObjectA, TestGeometricObject testGeometricObjectA, TriangleShape testTriangleA, CollisionObject testCollisionObjectB, TestGeometricObject testGeometricObjectB, TriangleShape testTriangleB) { CollisionObject collisionObjectA = contactSet.ObjectA; CollisionObject collisionObjectB = contactSet.ObjectB; IGeometricObject geometricObjectA = collisionObjectA.GeometricObject; IGeometricObject geometricObjectB = collisionObjectB.GeometricObject; TriangleMeshShape triangleMeshShapeA = (TriangleMeshShape)geometricObjectA.Shape; Triangle triangleA = triangleMeshShapeA.Mesh.GetTriangle(triangleIndexA); TriangleMeshShape triangleMeshShapeB = (TriangleMeshShape)geometricObjectB.Shape; Triangle triangleB = triangleMeshShapeB.Mesh.GetTriangle(triangleIndexB); Pose poseA = geometricObjectA.Pose; Pose poseB = geometricObjectB.Pose; Vector3F scaleA = geometricObjectA.Scale; Vector3F scaleB = geometricObjectB.Scale; // Apply SRT. Triangle transformedTriangleA; transformedTriangleA.Vertex0 = poseA.ToWorldPosition(triangleA.Vertex0 * scaleA); transformedTriangleA.Vertex1 = poseA.ToWorldPosition(triangleA.Vertex1 * scaleA); transformedTriangleA.Vertex2 = poseA.ToWorldPosition(triangleA.Vertex2 * scaleA); Triangle transformedTriangleB; transformedTriangleB.Vertex0 = poseB.ToWorldPosition(triangleB.Vertex0 * scaleB); transformedTriangleB.Vertex1 = poseB.ToWorldPosition(triangleB.Vertex1 * scaleB); transformedTriangleB.Vertex2 = poseB.ToWorldPosition(triangleB.Vertex2 * scaleB); // Make super-fast boolean check first. This is redundant if we have to compute // a contact with SAT below. But in stochastic benchmarks it seems to be 10% faster. bool haveContact = GeometryHelper.HaveContact(ref transformedTriangleA, ref transformedTriangleB); if (type == CollisionQueryType.Boolean) { contactSet.HaveContact = (contactSet.HaveContact || haveContact); return; } if (haveContact) { // Make sure the scaled triangles have the correct normal. // (A negative scale changes the normal/winding order. See unit test in TriangleTest.cs.) if (scaleA.X * scaleA.Y * scaleA.Z < 0) { MathHelper.Swap(ref transformedTriangleA.Vertex0, ref transformedTriangleA.Vertex1); } if (scaleB.X * scaleB.Y * scaleB.Z < 0) { MathHelper.Swap(ref transformedTriangleB.Vertex0, ref transformedTriangleB.Vertex1); } // Compute contact. Vector3F position, normal; float penetrationDepth; haveContact = TriangleTriangleAlgorithm.GetContact( ref transformedTriangleA, ref transformedTriangleB, !triangleMeshShapeA.IsTwoSided, !triangleMeshShapeB.IsTwoSided, out position, out normal, out penetrationDepth); if (haveContact) { contactSet.HaveContact = true; // In deep interpenetrations we might get no contact (penDepth = NaN). if (!Numeric.IsNaN(penetrationDepth)) { Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false); contact.FeatureA = triangleIndexA; contact.FeatureB = triangleIndexB; ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); } return; } // We might come here if the boolean test reports contact but the SAT test // does not because of numerical errors. } Debug.Assert(!haveContact); if (type == CollisionQueryType.Contacts) { return; } Debug.Assert(type == CollisionQueryType.ClosestPoints); if (contactSet.HaveContact) { // These triangles are separated but other parts of the meshes touches. // --> Abort. return; } // We do not have a specialized triangle-triangle closest points algorithm. // Fall back to the default algorithm (GJK). // Initialize temporary test contact set and test objects. // Note: We assume the triangle-triangle does not care about front/back faces. testTriangleA.Vertex0 = transformedTriangleA.Vertex0; testTriangleA.Vertex1 = transformedTriangleA.Vertex1; testTriangleA.Vertex2 = transformedTriangleA.Vertex2; testGeometricObjectA.Shape = testTriangleA; Debug.Assert(testGeometricObjectA.Scale == Vector3F.One); Debug.Assert(testGeometricObjectA.Pose == Pose.Identity); testCollisionObjectA.SetInternal(collisionObjectA, testGeometricObjectA); testTriangleB.Vertex0 = transformedTriangleB.Vertex0; testTriangleB.Vertex1 = transformedTriangleB.Vertex1; testTriangleB.Vertex2 = transformedTriangleB.Vertex2; testGeometricObjectB.Shape = testTriangleB; Debug.Assert(testGeometricObjectB.Scale == Vector3F.One); Debug.Assert(testGeometricObjectB.Pose == Pose.Identity); testCollisionObjectB.SetInternal(collisionObjectB, testGeometricObjectB); Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared."); testContactSet.Reset(testCollisionObjectA, testCollisionObjectB); testContactSet.IsPerturbationTestAllowed = false; _triangleTriangleAlgorithm.ComputeCollision(testContactSet, type); // Note: We expect no contact but because of numerical differences the triangle-triangle // algorithm could find a shallow surface contact. contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact); #region ----- Merge testContactSet into contactSet ----- if (testContactSet.Count > 0) { // Set the shape feature of the new contacts. int numberOfContacts = testContactSet.Count; for (int i = 0; i < numberOfContacts; i++) { Contact contact = testContactSet[i]; //if (contact.Lifetime.Ticks == 0) // Currently, this check is not necessary because triangleSet does not contain old contacts. //{ contact.FeatureA = triangleIndexA; contact.FeatureB = triangleIndexB; //} } // Merge the contact info. ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance); } #endregion }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { // Ray vs. sphere has at max 1 contact. Debug.Assert(contactSet.Count <= 1); // Object A should be the ray. // Object B should be the sphere. IGeometricObject rayObject = contactSet.ObjectA.GeometricObject; IGeometricObject sphereObject = contactSet.ObjectB.GeometricObject; // Swap if necessary. bool swapped = (sphereObject.Shape is RayShape); if (swapped) { MathHelper.Swap(ref rayObject, ref sphereObject); } RayShape rayShape = rayObject.Shape as RayShape; SphereShape sphereShape = sphereObject.Shape as SphereShape; // Check if shapes are correct. if (rayShape == null || sphereShape == null) { throw new ArgumentException("The contact set must contain a ray and a sphere.", "contactSet"); } // Get transformations. Vector3 rayScale = rayObject.Scale; Vector3 sphereScale = Vector3.Absolute(sphereObject.Scale); Pose rayPose = rayObject.Pose; Pose spherePose = sphereObject.Pose; // Call other algorithm for non-uniformly scaled spheres. if (sphereScale.X != sphereScale.Y || sphereScale.Y != sphereScale.Z) { if (_fallbackAlgorithm == null) { _fallbackAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(RayShape), typeof(ConvexShape)]; } _fallbackAlgorithm.ComputeCollision(contactSet, type); return; } // Scale ray and transform ray to world space. Ray rayWorld = new Ray(rayShape); rayWorld.Scale(ref rayScale); rayWorld.ToWorld(ref rayPose); // Scale sphere. float sphereRadius = sphereShape.Radius * sphereScale.X; float sphereRadiusSquared = sphereRadius * sphereRadius; // Call line segment vs. sphere for closest points queries. if (type == CollisionQueryType.ClosestPoints || type == CollisionQueryType.Boolean) { // ----- Find point on ray closest to the sphere. // Make a line segment vs. point (sphere center) check. Debug.Assert(rayWorld.Direction.IsNumericallyNormalized, "The ray direction should be normalized."); LineSegment segment = new LineSegment(rayWorld.Origin, rayWorld.Origin + rayWorld.Direction * rayWorld.Length); Vector3 segmentPoint; Vector3 sphereCenter = spherePose.Position; GeometryHelper.GetClosestPoints(segment, sphereCenter, out segmentPoint); Vector3 normal = sphereCenter - segmentPoint; float distanceSquared = normal.LengthSquared(); float penetrationDepthSquared = sphereRadiusSquared - distanceSquared; contactSet.HaveContact = penetrationDepthSquared >= 0; if (type == CollisionQueryType.Boolean) { return; } if (penetrationDepthSquared <= 0) { // Separated or touching objects. Vector3 position; if (normal.TryNormalize()) { Vector3 spherePoint = sphereCenter - normal * sphereRadius; position = (spherePoint + segmentPoint) / 2; } else { position = segmentPoint; normal = Vector3.UnitY; } if (swapped) { normal = -normal; } float penetrationDepth = -((float)Math.Sqrt(distanceSquared) - sphereRadius); // Update contact set. Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, contactSet.HaveContact); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); return; } // Otherwise, we have a penetrating contact. // Compute 1 contact ... } // First assume no contact. contactSet.HaveContact = false; // See SOLID and Bergen: "Collision Detection in Interactive 3D Environments", pp. 70. // Compute in sphere local space. // Transform ray to local space of sphere. Ray ray = rayWorld; ray.ToLocal(ref spherePose); Vector3 s = ray.Origin; // ray source Vector3 d = ray.Direction; // ray direction Vector3 r = d * ray.Length; // ray float rayLengthSquared = ray.Length * ray.Length; // ||r||² float δ = -Vector3.Dot(s, r); // -s∙r float σ = δ * δ - rayLengthSquared * (s.LengthSquared() - sphereRadiusSquared); if (σ >= 0) { // The infinite ray intersects. float sqrtσ = (float)Math.Sqrt(σ); float λ2 = (δ + sqrtσ) /* / rayLengthSquared */; // Division can be ignored. Only sign is relevant. if (λ2 >= 0) { // Ray shoots to sphere. float λ1 = (δ - sqrtσ) / rayLengthSquared; if (λ1 <= 1) { // Ray hits sphere. contactSet.HaveContact = true; Debug.Assert(type != CollisionQueryType.Boolean); // Was handled before. float penetrationDepth; Vector3 normal; if (λ1 > 0) { // λ1 shows the entry point. λ2 shows the exit point. penetrationDepth = λ1 * ray.Length; // Distance from ray origin to entry point (hit). normal = -(s + r * λ1); // Entry point (hit). } else { // Ray origin is in the sphere. penetrationDepth = 0; normal = -s; } Vector3 position = rayWorld.Origin + rayWorld.Direction * penetrationDepth; normal = spherePose.ToWorldDirection(normal); if (!normal.TryNormalize()) { normal = Vector3.UnitY; } if (swapped) { normal = -normal; } // Update contact set. Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, true); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); } } } }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { // Object A should be the ray. // Object B should be the triangle. IGeometricObject rayObject = contactSet.ObjectA.GeometricObject; IGeometricObject triangleObject = contactSet.ObjectB.GeometricObject; // Swap if necessary. bool swapped = (triangleObject.Shape is RayShape); if (swapped) { MathHelper.Swap(ref rayObject, ref triangleObject); } RayShape rayShape = rayObject.Shape as RayShape; TriangleShape triangleShape = triangleObject.Shape as TriangleShape; // Check if shapes are correct. if (rayShape == null || triangleShape == null) { throw new ArgumentException("The contact set must contain a ray and a triangle.", "contactSet"); } // See SOLID and Bergen: "Collision Detection in Interactive 3D Environments", pp. 84. // Note: All computations are done in triangle local space. // Get transformations. Vector3F rayScale = rayObject.Scale; Vector3F triangleScale = triangleObject.Scale; Pose rayPose = rayObject.Pose; Pose trianglePose = triangleObject.Pose; // Scale triangle. Vector3F v0 = triangleShape.Vertex0 * triangleScale; Vector3F v1 = triangleShape.Vertex1 * triangleScale; Vector3F v2 = triangleShape.Vertex2 * triangleScale; // Scale ray and transform ray to local space of triangle. Ray rayWorld = new Ray(rayShape); rayWorld.Scale(ref rayScale); // Scale ray. rayWorld.ToWorld(ref rayPose); // Transform to world space. Ray ray = rayWorld; ray.ToLocal(ref trianglePose); // Transform to local space of triangle. Vector3F d1 = (v1 - v0); Vector3F d2 = (v2 - v0); Vector3F n = Vector3F.Cross(d1, d2); // Tolerance value, see SOLID, Bergen: "Collision Detection in Interactive 3D Environments". float ε = n.Length * Numeric.EpsilonFSquared; Vector3F r = ray.Direction * ray.Length; float δ = -Vector3F.Dot(r, n); if (ε == 0.0f || Numeric.IsZero(δ, ε)) { // The triangle is degenerate or the ray is parallel to triangle. if (type == CollisionQueryType.Contacts || type == CollisionQueryType.Boolean) { contactSet.HaveContact = false; } else if (type == CollisionQueryType.ClosestPoints) { GetClosestPoints(contactSet, rayWorld.Origin); } return; } Vector3F triangleToRayOrigin = ray.Origin - v0; float λ = Vector3F.Dot(triangleToRayOrigin, n) / δ; // Assume no contact. contactSet.HaveContact = false; if (λ < 0 || λ > 1) { // The ray does not hit. if (type == CollisionQueryType.ClosestPoints) { GetClosestPoints(contactSet, rayWorld.Origin); } } else { Vector3F u = Vector3F.Cross(triangleToRayOrigin, r); float μ1 = Vector3F.Dot(d2, u) / δ; float μ2 = Vector3F.Dot(-d1, u) / δ; if (μ1 + μ2 <= 1 + ε && μ1 >= -ε && μ2 >= -ε) { // Hit! contactSet.HaveContact = true; if (type == CollisionQueryType.Boolean) { return; } float penetrationDepth = λ * ray.Length; // Create contact info. Vector3F position = rayWorld.Origin + rayWorld.Direction * penetrationDepth; n = trianglePose.ToWorldDirection(n); Debug.Assert(!n.IsNumericallyZero, "Degenerate cases of ray vs. triangle should be treated above."); n.Normalize(); if (δ > 0) { n = -n; } if (swapped) { n = -n; } Contact contact = ContactHelper.CreateContact(contactSet, position, n, penetrationDepth, true); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); } else { // No Hit! if (type == CollisionQueryType.ClosestPoints) { GetClosestPoints(contactSet, rayWorld.Origin); } } } }
// Returns true if a contact was added. private bool AddContact(ContactSet contactSet, bool swapped, CollisionQueryType type, ref Ray rayWorld, // The ray in world space. ref Ray rayInField, // The ray in the scaled height field space. ref Triangle triangle, // The unscaled triangle in the mesh space. int triangleIndex, ref Pose trianglePose, ref Vector3F triangleScale) { // This code is from GeometryHelper_Triangles.cs. Sync changes! Vector3F v0 = triangle.Vertex0 * triangleScale; Vector3F v1 = triangle.Vertex1 * triangleScale; Vector3F v2 = triangle.Vertex2 * triangleScale; Vector3F d1 = (v1 - v0); Vector3F d2 = (v2 - v0); Vector3F n = Vector3F.Cross(d1, d2); // Tolerance value, see SOLID, Bergen: "Collision Detection in Interactive 3D Environments". float ε = n.Length * Numeric.EpsilonFSquared; Vector3F r = rayInField.Direction * rayInField.Length; float δ = -Vector3F.Dot(r, n); // Degenerate triangle --> No hit. if (ε == 0.0f || Numeric.IsZero(δ, ε)) { return(false); } Vector3F triangleToRayOrigin = rayInField.Origin - v0; float λ = Vector3F.Dot(triangleToRayOrigin, n) / δ; if (λ < 0 || λ > 1) { return(false); } // The ray hit the triangle plane. Vector3F u = Vector3F.Cross(triangleToRayOrigin, r); float μ1 = Vector3F.Dot(d2, u) / δ; float μ2 = Vector3F.Dot(-d1, u) / δ; if (μ1 + μ2 <= 1 + ε && μ1 >= -ε && μ2 >= -ε) { // Hit! contactSet.HaveContact = true; if (type == CollisionQueryType.Boolean) { return(false); } if (δ < 0) { return(false); // Shooting into the back of a one-sided triangle - no contact. } float penetrationDepth = λ * rayInField.Length; // Create contact info. Vector3F position = rayWorld.Origin + rayWorld.Direction * penetrationDepth; n = trianglePose.ToWorldDirection(n); Debug.Assert(!n.IsNumericallyZero, "Degenerate cases of ray vs. triangle should be treated above."); n.Normalize(); if (δ < 0) { n = -n; } if (swapped) { n = -n; } Contact contact = ContactHelper.CreateContact(contactSet, position, n, penetrationDepth, true); if (swapped) { contact.FeatureB = triangleIndex; } else { contact.FeatureA = triangleIndex; } Debug.Assert( contactSet.ObjectA.GeometricObject.Shape is RayShape && contact.FeatureA == -1 || contactSet.ObjectB.GeometricObject.Shape is RayShape && contact.FeatureB == -1, "RayHeightFieldAlgorithm has set the wrong feature property."); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); return(true); } return(false); }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { if (type == CollisionQueryType.Contacts) { throw new GeometryException("GJK cannot handle contact queries. Use MPR instead."); } IGeometricObject objectA = contactSet.ObjectA.GeometricObject; ConvexShape shapeA = objectA.Shape as ConvexShape; Vector3F scaleA = objectA.Scale; Pose poseA = objectA.Pose; IGeometricObject objectB = contactSet.ObjectB.GeometricObject; ConvexShape shapeB = objectB.Shape as ConvexShape; Vector3F scaleB = objectB.Scale; Pose poseB = objectB.Pose; if (shapeA == null || shapeB == null) { throw new ArgumentException("The contact set must contain two convex shapes.", "contactSet"); } // GJK builds a simplex of the CSO (A-B). This simplex is managed in a GjkSimplexSolver. var simplex = GjkSimplexSolver.Create(); bool foundSeparatingAxis = false; try { // v is the separating axis or the CSO point nearest to the origin. // We start with last known separating axis or with an arbitrary CSO point. Vector3F v; if (contactSet.Count > 0) { // Use last separating axis. // The contact normal points from A to B. This is the direction we want to sample first. // If the frame-to-frame coherence is high we should get a point close to the origin. // Note: To sample in the normal direction, we need to initialize the CSO point v with // -normal. v = -contactSet[0].Normal; } else { // Use difference of inner points. Vector3F vA = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA); Vector3F vB = poseB.ToWorldPosition(shapeB.InnerPoint * scaleB); v = vA - vB; } // If the inner points overlap, then we have already found a contact. // We don't expect this case to happen often, so we simply choose an arbitrary separating // axis to continue with the normal GJK code. if (v.IsNumericallyZero) { v = Vector3F.UnitZ; } // Cache inverted rotations. var orientationAInverse = poseA.Orientation.Transposed; var orientationBInverse = poseB.Orientation.Transposed; int iterationCount = 0; float distanceSquared = float.MaxValue; float distanceEpsilon; // Assume we have no contact. contactSet.HaveContact = false; do { // TODO: Translate A and B close to the origin to avoid numerical problems. // This optimization is done in Bullet: The offset (a.Pose.Position + b.Pose.Position) / 2 // is subtracted from a.Pose and b.Pose. This offset is added when the Contact info is // computed (also in EPA if the poses are still translated). // Compute a new point w on the simplex. We seek for the point that is closest to the origin. // Therefore, we get the support points on the current separating axis v. Vector3F p = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -v, scaleA)); Vector3F q = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * v, scaleB)); Vector3F w = p - q; // w projected onto the separating axis. float delta = Vector3F.Dot(w, v); // If v∙w > 0 then the objects do not overlap. if (delta > 0) { // We have found a separating axis. foundSeparatingAxis = true; // Early exit for boolean and contact queries. if (type == CollisionQueryType.Boolean || type == CollisionQueryType.Contacts) { // TODO: We could cache the separating axis n in ContactSet for future collision checks. return; } // We continue for closest point queries because we don't know if there are other // points closer than p and q. } // If the new w is already part of the simplex. We cannot improve any further. if (simplex.Contains(w)) { break; } // If the new w is not closer to the origin (within numerical tolerance), we stop. if (distanceSquared - delta <= distanceSquared * Numeric.EpsilonF) // SOLID uses Epsilon = 10^-6 { break; } // Add the new point to the simplex. simplex.Add(w, p, q); // Update the simplex. (Unneeded simplex points are removed). simplex.Update(); // Get new point of simplex closest to the origin. v = simplex.ClosestPoint; float previousDistanceSquared = distanceSquared; distanceSquared = v.LengthSquared; if (previousDistanceSquared < distanceSquared) { // If the result got worse, we use the previous result. This happens for // degenerate cases for example when the simplex is a tetrahedron with all // 4 vertices in a plane. distanceSquared = previousDistanceSquared; simplex.Backup(); break; } // If the new simplex is invalid, we stop. // Example: A simplex gets invalid if a fourth vertex is added to create a tetrahedron // simplex but all vertices are in a plane. This can happen if a box corner nearly touches a // face of another box. if (!simplex.IsValid) { break; } // Compare the distance of v to the origin with the distance of the last iteration. // We stop if the improvement is less than the numerical tolerance. if (previousDistanceSquared - distanceSquared <= previousDistanceSquared * Numeric.EpsilonF) { break; } // If we reach the iteration limit, we stop. iterationCount++; if (iterationCount > MaxNumberOfIterations) { Debug.Assert(false, "GJK reached the iteration limit."); break; } // Compute a scaled epsilon. distanceEpsilon = Numeric.EpsilonFSquared * Math.Max(1, simplex.MaxVertexDistanceSquared); // Loop until the simplex is full (i.e. it contains the origin) or we have come // sufficiently close to the origin. } while (!simplex.IsFull && distanceSquared > distanceEpsilon); Debug.Assert(simplex.IsEmpty == false, "The GJK simplex must contain at least 1 point."); // Compute contact normal and separation. Vector3F normal = -simplex.ClosestPoint; // simplex.ClosestPoint = ClosestPointA-ClosestPointB float distance; distanceEpsilon = Numeric.EpsilonFSquared * Math.Max(1, simplex.MaxVertexDistanceSquared); if (distanceSquared <= distanceEpsilon) { // Distance is approximately 0. // --> Objects are in contact. if (simplex.IsValid && normal.TryNormalize()) { // Normal can be used but we have to invert it because for contact we // have to compute normal as pointOnA - pointOnB. normal = -normal; } else { // No useful normal. Use direction between inner points as a fallback. Vector3F innerA = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA); normal = simplex.ClosestPointOnA - innerA; if (!normal.TryNormalize()) { Vector3F innerB = poseB.ToWorldPosition(shapeB.InnerPoint * scaleB); normal = innerB - innerA; if (!normal.TryNormalize()) { normal = Vector3F.UnitY; // TODO: We could use better normal: e.g. normal of old contact or PreferredNormal? } } } distance = 0; contactSet.HaveContact = true; } else { // Distance is greater than 0. distance = (float)Math.Sqrt(distanceSquared); normal /= distance; // If the simplex is valid and full, then we have a contact. if (simplex.IsFull && simplex.IsValid) { // Let's use the current result as an estimated contact info for // shallow contacts. // TODO: The following IF was added because this can occur for valid // triangle vs. triangle separation. Check this. if (!foundSeparatingAxis) { contactSet.HaveContact = true; // Distance is a penetration depth distance = -distance; // Since the simplex tetrahedron can have any position in the Minkowsky difference, // we do not know the real normal. Let's use the current normal and make // sure that it points away from A. - This is only a heuristic... Vector3F innerA = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA); if (Vector3F.Dot(simplex.ClosestPointOnA - innerA, normal) < 0) { normal = -normal; } } } } Debug.Assert(normal.IsNumericallyZero == false); if (type != CollisionQueryType.Boolean) { Vector3F position = (simplex.ClosestPointOnA + simplex.ClosestPointOnB) / 2; Contact contact = ContactHelper.CreateContact(contactSet, position, normal, -distance, false); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); } } finally { simplex.Recycle(); } }
private void ComputeCollisionFast(ContactSet contactSet, CollisionQueryType type, IGeometricObject heightFieldGeometricObject, IGeometricObject convexGeometricObject, HeightField heightField, ConvexShape convex, bool swapped) { Debug.Assert(type != CollisionQueryType.ClosestPoints); // Assume no contact. contactSet.HaveContact = false; // Get scales and poses Pose heightFieldPose = heightFieldGeometricObject.Pose; Vector3F heightFieldScale = heightFieldGeometricObject.Scale; if (heightFieldScale.X < 0 || heightFieldScale.Y < 0 || heightFieldScale.Z < 0) { throw new NotSupportedException("Computing collisions for height fields with a negative scaling is not supported."); } Pose convexPose = convexGeometricObject.Pose; Vector3F convexScale = convexGeometricObject.Scale; // Get a point in the convex. (Could also use center of AABB.) var convexPoint = convexPose.ToWorldPosition(convex.InnerPoint * convexScale); // Get height field coordinates. convexPoint = heightFieldPose.ToLocalPosition(convexPoint); float xUnscaled = convexPoint.X / heightFieldScale.X; float zUnscaled = convexPoint.Z / heightFieldScale.Z; // If convex point is outside height field, abort. var originX = heightField.OriginX; var originZ = heightField.OriginZ; if (xUnscaled < originX || xUnscaled > originX + heightField.WidthX || zUnscaled < originZ || zUnscaled > originZ + heightField.WidthZ) { return; } // Get height and normal. float height; Vector3F normal; int featureIndex; GetHeight(heightField, xUnscaled, zUnscaled, out height, out normal, out featureIndex); // Check for holes. if (Numeric.IsNaN(height)) { return; } // Apply scaling. height *= heightFieldScale.Y; // Normals are transformed with the inverse transposed matrix --> 1 / scale. normal = normal / heightFieldScale; normal.Normalize(); // ----- Now we test convex vs. plane. // Convert normal to convex space. normal = heightFieldPose.ToWorldDirection(normal); var normalInConvex = convexPose.ToLocalDirection(normal); // Convert plane point to convex space. Vector3F planePoint = new Vector3F(convexPoint.X, height, convexPoint.Z); planePoint = heightFieldPose.ToWorldPosition(planePoint); planePoint = convexPose.ToLocalPosition(planePoint); // Get convex support point in plane normal direction. Vector3F supportPoint = convex.GetSupportPoint(-normalInConvex, convexScale); // Get penetration depth. float penetrationDepth = Vector3F.Dot((planePoint - supportPoint), normalInConvex); // Abort if there is no contact. if (penetrationDepth < 0) { return; } // Abort if object is too deep under the height field. // This is important for height fields with holes/caves. Without this check // no objects could enter the cave. if (penetrationDepth > heightField.Depth) { return; } // We have contact. contactSet.HaveContact = true; // Return for boolean queries. if (type == CollisionQueryType.Boolean) { return; } // Contact position is in the "middle of the penetration". Vector3F position = convexPose.ToWorldPosition(supportPoint + normalInConvex * (penetrationDepth / 2)); if (swapped) { normal = -normal; } // Add contact var contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false); if (swapped) { contact.FeatureB = featureIndex; } else { contact.FeatureA = featureIndex; } ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); if (CollisionDetection.FullContactSetPerFrame && contactSet.Count < 3) { // Trying to create a full contact set. // We use arbitrary orthonormal values to perturb the normal direction. var ortho1 = normalInConvex.Orthonormal1; var ortho2 = normalInConvex.Orthonormal2; // Test 4 perturbed support directions. for (int i = 0; i < 4; i++) { Vector3F direction; switch (i) { case 0: direction = -normalInConvex + ortho1; break; case 1: direction = -normalInConvex - ortho1; break; case 2: direction = -normalInConvex + ortho2; break; default: direction = -normalInConvex - ortho2; break; } // Support point vs. plane test as above: supportPoint = convex.GetSupportPoint(direction, convexScale); penetrationDepth = Vector3F.Dot((planePoint - supportPoint), normalInConvex); if (penetrationDepth >= 0) { position = convexPose.ToWorldPosition(supportPoint + normalInConvex * (penetrationDepth / 2)); contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); } } } }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { Debug.Assert(contactSet.Count <= 1, "Ray vs. plane should have at max 1 contact."); // Object A should be the plane. // Object B should be the ray. IGeometricObject planeObject = contactSet.ObjectA.GeometricObject; IGeometricObject rayObject = contactSet.ObjectB.GeometricObject; // Swap objects if necessary. bool swapped = (rayObject.Shape is PlaneShape); if (swapped) { MathHelper.Swap(ref planeObject, ref rayObject); } PlaneShape planeShape = planeObject.Shape as PlaneShape; RayShape rayShape = rayObject.Shape as RayShape; // Check if A is really a plane and B is a ray. if (planeShape == null || rayShape == null) { throw new ArgumentException("The contact set must contain a plane and a ray.", "contactSet"); } // Get transformations. Vector3F planeScale = planeObject.Scale; Vector3F rayScale = rayObject.Scale; Pose rayPose = rayObject.Pose; Pose planePose = planeObject.Pose; // Apply scale to plane. Plane plane = new Plane(planeShape); plane.Scale(ref planeScale); // Apply scale to ray and transform ray into local space of plane. Ray ray = new Ray(rayShape); ray.Scale(ref rayScale); // Scale ray. ray.ToWorld(ref rayPose); // Transform ray to world space. ray.ToLocal(ref planePose); // Transform ray to local space of plane. // Convert ray into a line segment. LineSegment segment = new LineSegment { Start = ray.Origin, End = ray.Origin + ray.Direction * ray.Length }; // Check if ray origin is inside the plane. Otherwise call plane vs. ray query. Vector3F linePoint; Vector3F planePoint = Vector3F.Zero; if (Vector3F.Dot(segment.Start, plane.Normal) <= plane.DistanceFromOrigin) { // The origin of the ray is below the plane. linePoint = segment.Start; contactSet.HaveContact = true; } else { // The origin of the ray is above the plane. contactSet.HaveContact = GeometryHelper.GetClosestPoints(plane, segment, out linePoint, out planePoint); } if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact)) { // HaveContact queries can exit here. // GetContacts queries can exit here if we don't have a contact. return; } // ----- Create contact info. Vector3F position; float penetrationDepth; if (contactSet.HaveContact) { // We have a contact. position = planePose.ToWorldPosition(linePoint); penetrationDepth = (linePoint - segment.Start).Length; } else { // Closest points, but separated. position = planePose.ToWorldPosition((planePoint + linePoint) / 2); penetrationDepth = -(linePoint - planePoint).Length; } Vector3F normal = planePose.ToWorldDirection(plane.Normal); if (swapped) { normal = -normal; } Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, contactSet.HaveContact); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); }
/// <summary> /// Guesses the closest pair. /// </summary> /// <param name="contactSet">The contact set.</param> /// <param name="swapped"> /// Object A in <paramref name="contactSet"/> should be the height field. This parameter /// indicates whether object A and object B in the contact set are swapped. /// </param> /// <param name="isOverHole"> /// <see langword="true"/> if the guessed contact is over a hole and probably shouldn't be used. /// </param> /// <returns>Guess for closest pair.</returns> /// <remarks> /// For general shapes: Inner point of B to height field point "under" inner point of B. /// For convex shapes: Support point of B in the "down" direction to height field point "under" /// this support point. /// </remarks> private static Contact GuessClosestPair(ContactSet contactSet, bool swapped, out bool isOverHole) { // Object A should be the height field. IGeometricObject objectA = contactSet.ObjectA.GeometricObject; IGeometricObject objectB = contactSet.ObjectB.GeometricObject; // Swap if necessary. if (swapped) { MathHelper.Swap(ref objectA, ref objectB); } HeightField heightFieldA = (HeightField)objectA.Shape; Shape shapeB = objectB.Shape; Vector3 scaleA = objectA.Scale; Vector3 scaleB = objectB.Scale; Pose poseA = objectA.Pose; Pose poseB = objectB.Pose; // Get the height field up-axis in world space. Vector3 heightFieldUpAxis = poseA.ToWorldDirection(Vector3.UnitY); // Get point on other object. Vector3 positionBLocal; ConvexShape shapeBAsConvex = shapeB as ConvexShape; if (shapeBAsConvex != null) { // Use support point for convex shapes. positionBLocal = shapeBAsConvex.GetSupportPoint(poseB.ToLocalDirection(-heightFieldUpAxis), scaleB); } else { // Use inner point for general shapes. positionBLocal = shapeB.InnerPoint * scaleB; } // Convert point (object B) into height field space (object A). Vector3 positionB = poseB.ToWorldPosition(positionBLocal); Vector3 positionBInA = poseA.ToLocalPosition(positionB); // Get point on the surface of the height field (object A): // Clamp x and z coordinate to height field widths. // For y coordinate get the height of the height field at the x-z position. float originX = heightFieldA.OriginX; float originZ = heightFieldA.OriginZ; float x = MathHelper.Clamp(positionBInA.X, originX * scaleA.X, (originX + heightFieldA.WidthX) * scaleA.X); float z = MathHelper.Clamp(positionBInA.Z, originZ * scaleA.Z, (originZ + heightFieldA.WidthZ) * scaleA.Z); float y = heightFieldA.GetHeight(x / scaleA.X, z / scaleA.Z) * scaleA.Y; // Inverse scale applied in GetHeight() parameters. Vector3 positionALocal = new Vector3(x, y, z); // Special handling of holes. isOverHole = Numeric.IsNaN(y); if (isOverHole) { positionALocal = heightFieldA.InnerPoint * scaleA; } // Convert point on height field to world space. Vector3 positionA = poseA.ToWorldPosition(positionALocal); // Use the world positions (positionA, positionB) as our closest-pair/contact guess. // Compute contact information. Vector3 position = (positionA + positionB) / 2; float penetrationDepth = (positionA - positionB).Length; bool haveContact = (positionALocal.Y >= positionBInA.Y); Vector3 normal = positionA - positionB; if (!normal.TryNormalize()) { normal = heightFieldUpAxis; } if (swapped) { normal = -normal; } bool isRayHit = haveContact && shapeB is RayShape; if (!haveContact) { // For separation: Switch the normal and make the penetration depth negative to indicate // separation. normal = -normal; penetrationDepth = -penetrationDepth; } return(ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, isRayHit)); }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { // From Coutinho: "Dynamic Simulations of Multibody Systems" and // Bergen: "Collision Detection in Interactive 3D Environments". // Object A should be the box. // Object B should be the sphere. IGeometricObject boxObject = contactSet.ObjectA.GeometricObject; IGeometricObject sphereObject = contactSet.ObjectB.GeometricObject; // Swap objects if necessary. bool swapped = (sphereObject.Shape is BoxShape); if (swapped) { MathHelper.Swap(ref boxObject, ref sphereObject); } BoxShape boxShape = boxObject.Shape as BoxShape; SphereShape sphereShape = sphereObject.Shape as SphereShape; // Check if collision objects shapes are correct. if (boxShape == null || sphereShape == null) { throw new ArgumentException("The contact set must contain a box and a sphere.", "contactSet"); } Vector3F scaleBox = Vector3F.Absolute(boxObject.Scale); Vector3F scaleSphere = Vector3F.Absolute(sphereObject.Scale); // Call other algorithm for non-uniformly scaled spheres. if (scaleSphere.X != scaleSphere.Y || scaleSphere.Y != scaleSphere.Z) { if (_fallbackAlgorithm == null) { _fallbackAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(BoxShape), typeof(ConvexShape)]; } _fallbackAlgorithm.ComputeCollision(contactSet, type); return; } // Apply scale. Vector3F boxExtent = boxShape.Extent * scaleBox; float sphereRadius = sphereShape.Radius * scaleSphere.X; // ----- First transform sphere center into the local space of the box. Pose boxPose = boxObject.Pose; Vector3F sphereCenterWorld = sphereObject.Pose.Position; Vector3F sphereCenter = boxPose.ToLocalPosition(sphereCenterWorld); Vector3F p = Vector3F.Zero; bool sphereCenterIsContainedInBox = true; // When sphere center is on a box surface we have to choose a suitable normal. // otherwise the normal will be computed later. Vector3F normal = Vector3F.Zero; #region ----- Look for the point p of the box that is closest to center of the sphere. ----- Vector3F boxHalfExtent = 0.5f * boxExtent; // x component if (sphereCenter.X < -boxHalfExtent.X) { p.X = -boxHalfExtent.X; sphereCenterIsContainedInBox = false; } else if (sphereCenter.X > boxHalfExtent.X) { p.X = boxHalfExtent.X; sphereCenterIsContainedInBox = false; } else { p.X = sphereCenter.X; } // y component if (sphereCenter.Y < -boxHalfExtent.Y) { p.Y = -boxHalfExtent.Y; sphereCenterIsContainedInBox = false; } else if (sphereCenter.Y > boxHalfExtent.Y) { p.Y = boxHalfExtent.Y; sphereCenterIsContainedInBox = false; } else { p.Y = sphereCenter.Y; } // z component if (sphereCenter.Z < -boxHalfExtent.Z) { p.Z = -boxHalfExtent.Z; sphereCenterIsContainedInBox = false; } else if (sphereCenter.Z > boxHalfExtent.Z) { p.Z = boxHalfExtent.Z; sphereCenterIsContainedInBox = false; } else { p.Z = sphereCenter.Z; } if (sphereCenterIsContainedInBox || (sphereCenter - p).IsNumericallyZero) { // Special case: Sphere center is within box. In this case p == center. // Lets return a point on the surface of the box. // Lets find the axis with the smallest way out (penetration depth). Vector3F diff = boxHalfExtent - Vector3F.Absolute(sphereCenter); if (diff.X <= diff.Y && diff.X <= diff.Z) { // Point on one of the x surfaces is nearest. // Check whether positive or negative surface. bool positive = (sphereCenter.X > 0); p.X = positive ? boxHalfExtent.X : -boxHalfExtent.X; if (Numeric.IsZero(diff.X)) { // Sphere center is on box surface. normal = positive ? Vector3F.UnitX : -Vector3F.UnitX; } } else if (diff.Y <= diff.X && diff.Y <= diff.Z) { // Point on one of the y surfaces is nearest. // Check whether positive or negative surface. bool positive = (sphereCenter.Y > 0); p.Y = positive ? boxHalfExtent.Y : -boxHalfExtent.Y; if (Numeric.IsZero(diff.Y)) { // Sphere center is on box surface. normal = positive ? Vector3F.UnitY : -Vector3F.UnitY; } } else { // Point on one of the z surfaces is nearest. // Check whether positive or negative surface. bool positive = (sphereCenter.Z > 0); p.Z = positive ? boxHalfExtent.Z : -boxHalfExtent.Z; if (Numeric.IsZero(diff.Z)) { // Sphere center is on box surface. normal = positive ? Vector3F.UnitZ : -Vector3F.UnitZ; } } } #endregion // ----- Convert back to world space p = boxPose.ToWorldPosition(p); Vector3F sphereCenterToP = p - sphereCenterWorld; // Compute penetration depth. float penetrationDepth = sphereCenterIsContainedInBox ? sphereRadius + sphereCenterToP.Length : sphereRadius - sphereCenterToP.Length; contactSet.HaveContact = (penetrationDepth >= 0); if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact)) { // HaveContact queries can exit here. // GetContacts queries can exit here if we don't have a contact. return; } // ----- Create collision info. // Compute normal if we haven't set one yet. if (normal == Vector3F.Zero) { Debug.Assert(!sphereCenterToP.IsNumericallyZero, "When the center of the sphere lies on the box surface a normal should be have been set explicitly."); normal = sphereCenterIsContainedInBox ? sphereCenterToP : -sphereCenterToP; normal.Normalize(); } else { normal = boxPose.ToWorldDirection(normal); } // Position = point between sphere and box surface. Vector3F position = p - normal * (penetrationDepth / 2); if (swapped) { normal = -normal; } // Update contact set. Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); }
private Vector3 DoMpr(CollisionQueryType type, ContactSet contactSet, Vector3 v0) { int iterationCount = 0; const int iterationLimit = 100; CollisionObject collisionObjectA = contactSet.ObjectA; IGeometricObject geometricObjectA = collisionObjectA.GeometricObject; ConvexShape shapeA = (ConvexShape)geometricObjectA.Shape; Vector3 scaleA = geometricObjectA.Scale; Pose poseA = geometricObjectA.Pose; CollisionObject collisionObjectB = contactSet.ObjectB; IGeometricObject geometricObjectB = collisionObjectB.GeometricObject; ConvexShape shapeB = (ConvexShape)geometricObjectB.Shape; Vector3 scaleB = geometricObjectB.Scale; Pose poseB = geometricObjectB.Pose; // Cache inverted rotations. var orientationAInverse = poseA.Orientation.Transposed; var orientationBInverse = poseB.Orientation.Transposed; Vector3 n = -v0; // Shoot from v0 to the origin. Vector3 v1A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA)); Vector3 v1B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB)); Vector3 v1 = v1B - v1A; // Separating axis test: if (Vector3.Dot(v1, n) < 0) { // TODO: We could cache the separating axis n in ContactSet for future collision checks. // Also in the separating axis tests below. return(Vector3.Zero); } // Second support direction = perpendicular to plane of origin, v0 and v1. n = Vector3.Cross(v1, v0); // If n is a zero vector, then origin, v0 and v1 are on a line with the origin inside the support plane. if (n.IsNumericallyZero) { // Contact found. contactSet.HaveContact = true; if (type == CollisionQueryType.Boolean) { return(Vector3.Zero); } // Compute contact information. // (v0 is an inner point. v1 is a support point on the CSO. => The contact normal is -v1. // However, v1 could be close to the origin. To avoid numerical // problems we use v0 - v1, which is the same direction.) Vector3 normal = v0 - v1; if (!normal.TryNormalize()) { // This happens for Point vs. flat object when they are on the same position. // Maybe we could even find a better normal. normal = Vector3.UnitY; } Vector3 position = (v1A + v1B) / 2; float penetrationDepth = v1.Length; Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); return(Vector3.Zero); } Vector3 v2A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA)); Vector3 v2B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB)); Vector3 v2 = v2B - v2A; // Separating axis test: if (Vector3.Dot(v2, n) < 0) { return(Vector3.Zero); } // Third support direction = perpendicular to plane of v0, v1 and v2. n = Vector3.Cross(v1 - v0, v2 - v0); // If the origin is on the negative side of the plane, then reverse the plane direction. // n must point into the origin direction and not away... if (Vector3.Dot(n, v0) > 0) { MathHelper.Swap(ref v1, ref v2); MathHelper.Swap(ref v1A, ref v2A); MathHelper.Swap(ref v1B, ref v2B); n = -n; } if (n.IsNumericallyZero) { // Degenerate case: // Interpretation (HelmutG): v2 is on the line with v0 and v1. I think this can only happen // if the CSO is flat and in the plane of (origin, v0, v1). // This happens for example in Point vs. Line Segment, or triangle vs. triangle when both // triangles are in the same plane. // Simply ignore this case (Infinite small/flat objects do not touch). return(Vector3.Zero); } // Search for a valid portal. Vector3 v3, v3A, v3B; while (true) { iterationCount++; // Abort if we cannot find a valid portal. if (iterationCount > iterationLimit) { return(Vector3.Zero); } // Get next support point. //v3A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA)); //v3B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB)); //v3 = v3B - v3A; // ----- Optimized version: Vector3 supportDirectionA; supportDirectionA.X = -(orientationAInverse.M00 * n.X + orientationAInverse.M01 * n.Y + orientationAInverse.M02 * n.Z); supportDirectionA.Y = -(orientationAInverse.M10 * n.X + orientationAInverse.M11 * n.Y + orientationAInverse.M12 * n.Z); supportDirectionA.Z = -(orientationAInverse.M20 * n.X + orientationAInverse.M21 * n.Y + orientationAInverse.M22 * n.Z); Vector3 supportPointA = shapeA.GetSupportPoint(supportDirectionA, scaleA); v3A.X = poseA.Orientation.M00 * supportPointA.X + poseA.Orientation.M01 * supportPointA.Y + poseA.Orientation.M02 * supportPointA.Z + poseA.Position.X; v3A.Y = poseA.Orientation.M10 * supportPointA.X + poseA.Orientation.M11 * supportPointA.Y + poseA.Orientation.M12 * supportPointA.Z + poseA.Position.Y; v3A.Z = poseA.Orientation.M20 * supportPointA.X + poseA.Orientation.M21 * supportPointA.Y + poseA.Orientation.M22 * supportPointA.Z + poseA.Position.Z; Vector3 supportDirectionB; supportDirectionB.X = orientationBInverse.M00 * n.X + orientationBInverse.M01 * n.Y + orientationBInverse.M02 * n.Z; supportDirectionB.Y = orientationBInverse.M10 * n.X + orientationBInverse.M11 * n.Y + orientationBInverse.M12 * n.Z; supportDirectionB.Z = orientationBInverse.M20 * n.X + orientationBInverse.M21 * n.Y + orientationBInverse.M22 * n.Z; Vector3 supportPointB = shapeB.GetSupportPoint(supportDirectionB, scaleB); v3B.X = poseB.Orientation.M00 * supportPointB.X + poseB.Orientation.M01 * supportPointB.Y + poseB.Orientation.M02 * supportPointB.Z + poseB.Position.X; v3B.Y = poseB.Orientation.M10 * supportPointB.X + poseB.Orientation.M11 * supportPointB.Y + poseB.Orientation.M12 * supportPointB.Z + poseB.Position.Y; v3B.Z = poseB.Orientation.M20 * supportPointB.X + poseB.Orientation.M21 * supportPointB.Y + poseB.Orientation.M22 * supportPointB.Z + poseB.Position.Z; v3 = v3B - v3A; // Separating axis test: //if (Vector3.Dot(v3, n) < 0) if (v3.X * n.X + v3.Y * n.Y + v3.Z * n.Z < 0) { return(Vector3.Zero); } // v0, v1, v2, v3 form a tetrahedron. // v0 is an inner point of the CSO and v1, v2, v3 are support points. // v1, v2, v3 should form a valid portal. // If origin is outside the plane of v0, v1, v3 then the portal is invalid and we choose a new n. //if (Vector3.Dot(Vector3.Cross(v1, v3), v0) < 0) // ORIENT3D test, see Ericson: "Real-Time Collision Detection" if ((v1.Y * v3.Z - v1.Z * v3.Y) * v0.X + (v1.Z * v3.X - v1.X * v3.Z) * v0.Y + (v1.X * v3.Y - v1.Y * v3.X) * v0.Z < 0) { v2 = v3; // Get rid of v2. A new v3 will be chosen in the next iteration. v2A = v3A; v2B = v3B; //n = Vector3.Cross(v1 - v0, v3 - v0); // ----- Optimized version: Vector3 v1MinusV0; v1MinusV0.X = v1.X - v0.X; v1MinusV0.Y = v1.Y - v0.Y; v1MinusV0.Z = v1.Z - v0.Z; Vector3 v3MinusV0; v3MinusV0.X = v3.X - v0.X; v3MinusV0.Y = v3.Y - v0.Y; v3MinusV0.Z = v3.Z - v0.Z; n.X = v1MinusV0.Y * v3MinusV0.Z - v1MinusV0.Z * v3MinusV0.Y; n.Y = v1MinusV0.Z * v3MinusV0.X - v1MinusV0.X * v3MinusV0.Z; n.Z = v1MinusV0.X * v3MinusV0.Y - v1MinusV0.Y * v3MinusV0.X; continue; } // If origin is outside the plane of v0, v2, v3 then the portal is invalid and we choose a new n. //if (Vector3.Dot(Vector3.Cross(v3, v2), v0) < 0) if ((v3.Y * v2.Z - v3.Z * v2.Y) * v0.X + (v3.Z * v2.X - v3.X * v2.Z) * v0.Y + (v3.X * v2.Y - v3.Y * v2.X) * v0.Z < 0) { v1 = v3; // Get rid of v1. A new v3 will be chosen in the next iteration. v1A = v3A; v1B = v3B; //n = Vector3.Cross(v3 - v0, v2 - v0); // ----- Optimized version: Vector3 v3MinusV0; v3MinusV0.X = v3.X - v0.X; v3MinusV0.Y = v3.Y - v0.Y; v3MinusV0.Z = v3.Z - v0.Z; Vector3 v2MinusV0; v2MinusV0.X = v2.X - v0.X; v2MinusV0.Y = v2.Y - v0.Y; v2MinusV0.Z = v2.Z - v0.Z; n.X = v3MinusV0.Y * v2MinusV0.Z - v3MinusV0.Z * v2MinusV0.Y; n.Y = v3MinusV0.Z * v2MinusV0.X - v3MinusV0.X * v2MinusV0.Z; n.Z = v3MinusV0.X * v2MinusV0.Y - v3MinusV0.Y * v2MinusV0.X; continue; } // If come to here, then we have found a valid portal to begin with. // (We have a tetrahedron that contains the ray (v0 to origin)). break; } // Refine the portal while (true) { iterationCount++; // Store old n. Numerical inaccuracy can lead to endless loops where n is constant. Vector3 oldN = n; // Compute outward pointing normal of the portal //n = Vector3.Cross(v2 - v1, v3 - v1); Vector3 v2MinusV1; v2MinusV1.X = v2.X - v1.X; v2MinusV1.Y = v2.Y - v1.Y; v2MinusV1.Z = v2.Z - v1.Z; Vector3 v3MinusV1; v3MinusV1.X = v3.X - v1.X; v3MinusV1.Y = v3.Y - v1.Y; v3MinusV1.Z = v3.Z - v1.Z; n.X = v2MinusV1.Y * v3MinusV1.Z - v2MinusV1.Z * v3MinusV1.Y; n.Y = v2MinusV1.Z * v3MinusV1.X - v2MinusV1.X * v3MinusV1.Z; n.Z = v2MinusV1.X * v3MinusV1.Y - v2MinusV1.Y * v3MinusV1.X; //if (!n.TryNormalize()) // ----- Optimized version: float nLengthSquared = n.LengthSquared(); if (nLengthSquared < Numeric.EpsilonFSquared) { // The portal is degenerate (some vertices of v1, v2, v3 are identical). // This can happen for coplanar shapes, e.g. long thin triangles in the // same plane. The portal (v1, v2, v3) is a line segment. // This might be a contact or not. We use the GJK as a fallback to check this case. if (_gjk == null) { _gjk = new Gjk(CollisionDetection); } _gjk.ComputeCollision(contactSet, CollisionQueryType.Boolean); if (contactSet.HaveContact == false) { return(Vector3.Zero); } // GJK reports a contact - but it cannot compute contact positions. // We use the best point on the current portal as the contact point. // Find the point closest to the origin. float u, v, w; GeometryHelper.GetClosestPoint(new Triangle(v1, v2, v3), Vector3.Zero, out u, out v, out w); Vector3 vClosest = u * v1 + v * v2 + w * v3; // We have not found a separating axis so far. --> Contact. contactSet.HaveContact = true; if (type == CollisionQueryType.Boolean) { return(Vector3.Zero); } // The points on the objects have the same barycentric coordinates. Vector3 pointOnA = u * v1A + v * v2A + w * v3A; Vector3 pointOnB = u * v1B + v * v2B + w * v3B; Vector3 normal = pointOnA - pointOnB; if (!normal.TryNormalize()) { if (contactSet.IsPreferredNormalAvailable) { normal = contactSet.PreferredNormal; } else { normal = Vector3.UnitY; } } Vector3 position = (pointOnA + pointOnB) / 2; float penetrationDepth = vClosest.Length; Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); return(Vector3.Zero); } // ----- Optimized version: Rest of n.TryNormalize(): float nLength = (float)Math.Sqrt(nLengthSquared); float scale = 1.0f / nLength; n.X *= scale; n.Y *= scale; n.Z *= scale; // Separating axis test: // Testing > instead of >= is important otherwise coplanar triangles may report false contacts // because the portal is in the same plane as the origin. if (!contactSet.HaveContact && v1.X * n.X + v1.Y * n.Y + v1.Z * n.Z > 0) // Optimized version of && Vector3.Dot(v1, n) > 0) { // Portal points aways from origin --> Origin is in the tetrahedron. contactSet.HaveContact = true; if (type == CollisionQueryType.Boolean) { return(Vector3.Zero); } } // Find new support point. //Vector3 v4A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA)); //Vector3 v4B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB)); //Vector3 v4 = v4B - v4A; // ----- Optimized version: Vector3 supportDirectionA; supportDirectionA.X = -(orientationAInverse.M00 * n.X + orientationAInverse.M01 * n.Y + orientationAInverse.M02 * n.Z); supportDirectionA.Y = -(orientationAInverse.M10 * n.X + orientationAInverse.M11 * n.Y + orientationAInverse.M12 * n.Z); supportDirectionA.Z = -(orientationAInverse.M20 * n.X + orientationAInverse.M21 * n.Y + orientationAInverse.M22 * n.Z); Vector3 supportPointA = shapeA.GetSupportPoint(supportDirectionA, scaleA); Vector3 v4A; v4A.X = poseA.Orientation.M00 * supportPointA.X + poseA.Orientation.M01 * supportPointA.Y + poseA.Orientation.M02 * supportPointA.Z + poseA.Position.X; v4A.Y = poseA.Orientation.M10 * supportPointA.X + poseA.Orientation.M11 * supportPointA.Y + poseA.Orientation.M12 * supportPointA.Z + poseA.Position.Y; v4A.Z = poseA.Orientation.M20 * supportPointA.X + poseA.Orientation.M21 * supportPointA.Y + poseA.Orientation.M22 * supportPointA.Z + poseA.Position.Z; Vector3 supportDirectionB; supportDirectionB.X = orientationBInverse.M00 * n.X + orientationBInverse.M01 * n.Y + orientationBInverse.M02 * n.Z; supportDirectionB.Y = orientationBInverse.M10 * n.X + orientationBInverse.M11 * n.Y + orientationBInverse.M12 * n.Z; supportDirectionB.Z = orientationBInverse.M20 * n.X + orientationBInverse.M21 * n.Y + orientationBInverse.M22 * n.Z; Vector3 supportPointB = shapeB.GetSupportPoint(supportDirectionB, scaleB); Vector3 v4B; v4B.X = poseB.Orientation.M00 * supportPointB.X + poseB.Orientation.M01 * supportPointB.Y + poseB.Orientation.M02 * supportPointB.Z + poseB.Position.X; v4B.Y = poseB.Orientation.M10 * supportPointB.X + poseB.Orientation.M11 * supportPointB.Y + poseB.Orientation.M12 * supportPointB.Z + poseB.Position.Y; v4B.Z = poseB.Orientation.M20 * supportPointB.X + poseB.Orientation.M21 * supportPointB.Y + poseB.Orientation.M22 * supportPointB.Z + poseB.Position.Z; Vector3 v4 = v4B - v4A; // Separating axis test: if (!contactSet.HaveContact && // <--- New (see below). v4.X * n.X + v4.Y * n.Y + v4.Z * n.Z < 0) // Optimized version of && Vector3.Dot(v4, n) < 0) { // Following assert can fail. For example if the above dot product returns -0.000000001 // for nearly perfectly touching objects. Therefore I have added the condition // hit == false to the condition. return(Vector3.Zero); } // Test if we have refined more than the collision epsilon. // Condition 1: Project the point difference v4-v3 onto normal n and check whether we have // improved in this direction. // Condition 2: If n has not changed, then we couldn't improve anymore. This is caused // by numerical problems, e.g. when a large object (>10000) is checked. //if (Vector3.Dot(v4 - v3, n) <= CollisionDetection.Epsilon // ----- Optimized version: if ((v4.X - v3.X) * n.X + (v4.Y - v3.Y) * n.Y + (v4.Z - v3.Z) * n.Z <= CollisionDetection.Epsilon || Vector3.AreNumericallyEqual(n, oldN) || iterationCount >= iterationLimit) { // We have the final portal. if (!contactSet.HaveContact) { return(Vector3.Zero); } if (type == CollisionQueryType.Boolean) { return(Vector3.Zero); } // Find the point closest to the origin. float u, v, w; GeometryHelper.GetClosestPoint(new Triangle(v1, v2, v3), Vector3.Zero, out u, out v, out w); // Note: If u, v or w is 0 or 1, then the point was probably outside portal triangle. // We can use the returned data, but re-running MPR will give us a better contact. Vector3 closest = u * v1 + v * v2 + w * v3; // The points on the objects have the same barycentric coordinates. Vector3 pointOnA = u * v1A + v * v2A + w * v3A; Vector3 pointOnB = u * v1B + v * v2B + w * v3B; // Use difference between points as normal direction, only if it can be normalized. Vector3 normal = pointOnA - pointOnB; if (!normal.TryNormalize()) { normal = -n; // Else use the inverted normal of the portal. } Vector3 position = (pointOnA + pointOnB) / 2; float penetrationDepth = closest.Length; Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); // If real closest point is outside the portal triangle, then one of u, v, w will // be exactly 0 or 1. In this case we should run a new MPR with the portal ray n. if (u == 0 || v == 0 || w == 0 || u == 1 || v == 1 || w == 1) { return(n); } return(Vector3.Zero); } // Now we have a new point v4 and have to make a new portal by eliminating v1, v2 or v3. // The possible new tetrahedron faces are: (v0, v1, v4), (v0, v4, v2), (v0, v4, v3) // We don't know the orientation yet. // Test with the ORIENT3D test. //Vector3 cross = Vector3.Cross(v4, v0); // ----- Optimized version: Vector3 cross; cross.X = v4.Y * v0.Z - v4.Z * v0.Y; cross.Y = v4.Z * v0.X - v4.X * v0.Z; cross.Z = v4.X * v0.Y - v4.Y * v0.X; //if (Vector3.Dot(v1, cross) > 0) if (v1.X * cross.X + v1.Y * cross.Y + v1.Z * cross.Z > 0) { // Eliminate v3 or v1. //if (Vector3.Dot(v2, cross) > 0) if (v2.X * cross.X + v2.Y * cross.Y + v2.Z * cross.Z > 0) { v1 = v4; v1A = v4A; v1B = v4B; } else { v3 = v4; v3A = v4A; v3B = v4B; } } else { // Eliminate v1 or v2. //if (Vector3.Dot(v3, cross) > 0) if (v3.X * cross.X + v3.Y * cross.Y + v3.Z * cross.Z > 0) { v2 = v4; v2A = v4A; v2B = v4B; } else { v1 = v4; v1A = v4A; v1B = v4B; } } } }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { if (type == CollisionQueryType.ClosestPoints) { // Just use normal height field shape algorithm. _heightFieldAlgorithm.ComputeCollision(contactSet, type); return; } Debug.Assert(type != CollisionQueryType.ClosestPoints, "Closest point queries should have already been handled!"); // HeightField = A, Ray = B IGeometricObject heightFieldObject = contactSet.ObjectA.GeometricObject; IGeometricObject rayObject = contactSet.ObjectB.GeometricObject; // Object A should be the height field, swap objects if necessary. bool swapped = (heightFieldObject.Shape is RayShape); if (swapped) { MathHelper.Swap(ref rayObject, ref heightFieldObject); } RayShape rayShape = rayObject.Shape as RayShape; HeightField heightField = heightFieldObject.Shape as HeightField; // Check if shapes are correct. if (rayShape == null || heightField == null) { throw new ArgumentException("The contact set must contain a ray and a height field.", "contactSet"); } // Assume no contact. contactSet.HaveContact = false; // Get transformations. Vector3F rayScale = rayObject.Scale; Pose rayPose = rayObject.Pose; Vector3F heightFieldScale = heightFieldObject.Scale; Pose heightFieldPose = heightFieldObject.Pose; // We do not support negative scaling. It is not clear what should happen when y is // scaled with a negative factor and triangle orders would be wrong... Not worth the trouble. if (heightFieldScale.X < 0 || heightFieldScale.Y < 0 || heightFieldScale.Z < 0) { throw new NotSupportedException("Computing collisions for height fields with a negative scaling is not supported."); } // Ray in world space. Ray rayWorld = new Ray(rayShape); rayWorld.Scale(ref rayScale); rayWorld.ToWorld(ref rayPose); // Ray in local scaled space of the height field. Ray rayScaled = rayWorld; rayScaled.ToLocal(ref heightFieldPose); // Ray in local unscaled space of the mesh. Ray rayUnscaled = rayScaled; var inverseCompositeScale = Vector3F.One / heightFieldScale; rayUnscaled.Scale(ref inverseCompositeScale); // Get height field and basic info. int arrayLengthX = heightField.NumberOfSamplesX; int arrayLengthZ = heightField.NumberOfSamplesZ; int numberOfCellsX = arrayLengthX - 1; int numberOfCellsZ = arrayLengthZ - 1; Debug.Assert(arrayLengthX > 1 && arrayLengthZ > 1, "A height field should contain at least 2 x 2 elements (= 1 cell)."); float cellWidthX = heightField.WidthX / numberOfCellsX; // Unscaled! float cellWidthZ = heightField.WidthZ / numberOfCellsZ; // Unscaled! // We use a 2D-DDA traversal of the height field cells. In other words: Look at it from // above. The height field is our screen and we will select the cells as if we draw // a pixel line. This could be made more efficient when we do not recompute values and // reuse values and make incremental steps Bresenham-style. // See GeometryHelper_Casts.cs method HaveContact(Aabb, ray) for explanation of the // ray parameter formula. var rayUnscaledDirectionInverse = new Vector3F( 1 / rayUnscaled.Direction.X, 1 / rayUnscaled.Direction.Y, 1 / rayUnscaled.Direction.Z); // The position where the ray enters the current cell. var cellEnter = rayUnscaled.Origin; // Unscaled!!! var originX = heightField.OriginX; var originZ = heightField.OriginZ; // ----- Find first cell. int indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1; // (int)(...) does not return the desired result for negative values! if (indexX < 0) { if (rayUnscaled.Direction.X <= 0) { return; } float parameter = (originX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X; if (parameter > rayUnscaled.Length) { return; // The ray does not reach the height field. } cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction; indexX = 0; } else if (indexX >= numberOfCellsX) { if (rayUnscaled.Direction.X >= 0) { return; } float parameter = (originX + heightField.WidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X; if (parameter > rayUnscaled.Length) { return; // The ray does not reach the height field. } cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction; indexX = numberOfCellsX - 1; } int indexZ = (cellEnter.Z >= originZ) ? (int)((cellEnter.Z - originZ) / cellWidthZ) : -1; if (indexZ < 0) { if (rayUnscaled.Direction.Z <= 0) { return; } float parameter = (originZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z; if (parameter > rayUnscaled.Length) { return; // The ray does not reach the next height field. } cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction; // We also have to correct the indexX! indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1; indexZ = 0; } else if (indexZ >= numberOfCellsZ) { if (rayUnscaled.Direction.Z >= 0) { return; } float parameter = (originZ + heightField.WidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z; if (parameter > rayUnscaled.Length) { return; // The ray does not reach the next height field. } cellEnter = rayUnscaled.Origin + parameter * rayUnscaled.Direction; indexX = (cellEnter.X >= originX) ? (int)((cellEnter.X - originX) / cellWidthX) : -1; indexZ = numberOfCellsZ - 1; } if (indexX < 0 || indexX >= numberOfCellsX || indexZ < 0 || indexZ >= numberOfCellsZ) { return; } while (true) { // ----- Get triangles of current cell. var triangle0 = heightField.GetTriangle(indexX, indexZ, false); var triangle1 = heightField.GetTriangle(indexX, indexZ, true); // Index of first triangle. var triangleIndex = (indexZ * numberOfCellsX + indexX) * 2; float xRelative = (cellEnter.X - originX) / cellWidthX - indexX; float zRelative = (cellEnter.Z - originZ) / cellWidthZ - indexZ; bool enterSecondTriangle = (xRelative + zRelative) > 1; // The diagonal is where xRel + zRel == 1. // ----- Find cell exit and move indices to next cell. // The position where the ray leaves the current cell. Vector3F cellExit; float nextXParameter = float.PositiveInfinity; if (rayUnscaled.Direction.X > 0) { nextXParameter = (originX + (indexX + 1) * cellWidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X; } else if (rayUnscaled.Direction.X < 0) { nextXParameter = (originX + indexX * cellWidthX - rayUnscaled.Origin.X) * rayUnscaledDirectionInverse.X; } float nextZParameter = float.PositiveInfinity; if (rayUnscaled.Direction.Z > 0) { nextZParameter = (originZ + (indexZ + 1) * cellWidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z; } else if (rayUnscaled.Direction.Z < 0) { nextZParameter = (originZ + indexZ * cellWidthZ - rayUnscaled.Origin.Z) * rayUnscaledDirectionInverse.Z; } bool isLastCell = false; if (nextXParameter < nextZParameter) { if (rayUnscaled.Direction.X > 0) { indexX++; if (indexX >= numberOfCellsX) // Abort if we have left the height field. { isLastCell = true; } } else { indexX--; if (indexX < 0) { isLastCell = true; } } if (nextXParameter > rayUnscaled.Length) { isLastCell = true; // The ray does not reach the next cell. nextXParameter = rayUnscaled.Length; } cellExit = rayUnscaled.Origin + nextXParameter * rayUnscaled.Direction; } else { if (rayUnscaled.Direction.Z > 0) { indexZ++; if (indexZ >= numberOfCellsZ) { isLastCell = true; } } else { indexZ--; if (indexZ < 0) { isLastCell = true; } } if (nextZParameter > rayUnscaled.Length) { isLastCell = true; nextZParameter = rayUnscaled.Length; } cellExit = rayUnscaled.Origin + nextZParameter * rayUnscaled.Direction; } // ----- We can skip cell if cell AABB is below the ray. var rayMinY = Math.Min(cellEnter.Y, cellExit.Y) - CollisionDetection.Epsilon; // Apply to avoid missing collisions when ray hits a cell border. // The ray is above if no height field height is higher the ray height. // (This check handles NaN height values (holes) correctly.) bool rayIsAbove = !(triangle0.Vertex0.Y >= rayMinY || triangle0.Vertex1.Y >= rayMinY || triangle0.Vertex2.Y >= rayMinY || triangle1.Vertex1.Y >= rayMinY); // Vertex1 of triangle1 is the fourth quad vertex! // ----- Test ray against the 2 triangles of the cell. bool triangle0IsHole = false; bool triangle1IsHole = false; if (!rayIsAbove) { // Abort if a height value is NaN (hole). triangle0IsHole = Numeric.IsNaN(triangle0.Vertex0.Y * triangle0.Vertex1.Y * triangle0.Vertex2.Y); triangle1IsHole = Numeric.IsNaN(triangle1.Vertex0.Y * triangle1.Vertex1.Y * triangle1.Vertex2.Y); bool contactAdded = false; if (enterSecondTriangle) { // Test second triangle first. if (!triangle1IsHole) { contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle1, triangleIndex + 1, ref heightFieldPose, ref heightFieldScale); } if (!contactAdded && !triangle0IsHole) { contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle0, triangleIndex, ref heightFieldPose, ref heightFieldScale); } } else { // Test first triangle first. if (!triangle0IsHole) { contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle0, triangleIndex, ref heightFieldPose, ref heightFieldScale); } if (!contactAdded && !triangle1IsHole) { contactAdded = AddContact(contactSet, swapped, type, ref rayWorld, ref rayScaled, ref triangle1, triangleIndex + 1, ref heightFieldPose, ref heightFieldScale); } } if (contactAdded) { return; } // We have contact and stop for boolean queries. if (contactSet.HaveContact && type == CollisionQueryType.Boolean) { return; } } // ----- Return simplified contact if cellEnter is below the cell. if (!rayIsAbove) { if (!enterSecondTriangle && !triangle0IsHole && GeometryHelper.IsInFront(triangle0, cellEnter) < 0 || enterSecondTriangle && !triangle1IsHole && GeometryHelper.IsInFront(triangle1, cellEnter) < 0) { contactSet.HaveContact = true; if (type == CollisionQueryType.Boolean) { return; } var position = heightFieldPose.ToWorldPosition(cellEnter * heightFieldScale); var normal = heightFieldPose.ToWorldDirection(Vector3F.UnitY); if (swapped) { normal = -normal; } float penetrationDepth = (position - rayWorld.Origin).Length; Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, true); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); return; } } // ----- Move to next cell. if (isLastCell) { return; } cellEnter = cellExit; } }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { // Ray vs. box has at max 1 contact. Debug.Assert(contactSet.Count <= 1); // Object A should be the ray. // Object B should be the box. IGeometricObject rayObject = contactSet.ObjectA.GeometricObject; IGeometricObject boxObject = contactSet.ObjectB.GeometricObject; // Swap objects if necessary. bool swapped = (boxObject.Shape is RayShape); if (swapped) { MathHelper.Swap(ref rayObject, ref boxObject); } RayShape rayShape = rayObject.Shape as RayShape; BoxShape boxShape = boxObject.Shape as BoxShape; // Check if shapes are correct. if (rayShape == null || boxShape == null) { throw new ArgumentException("The contact set must contain a ray and a box.", "contactSet"); } // Call line segment vs. box for closest points queries. if (type == CollisionQueryType.ClosestPoints) { // Find point on ray closest to the box. // Call GJK. _gjk.ComputeCollision(contactSet, type); if (contactSet.HaveContact == false) { return; } // Otherwise compute 1 contact ... // GJK result is invalid for penetration. foreach (var contact in contactSet) { contact.Recycle(); } contactSet.Clear(); } // Assume no contact. contactSet.HaveContact = false; // Get transformations. Vector3F rayScale = rayObject.Scale; Vector3F boxScale = Vector3F.Absolute(boxObject.Scale); Pose rayPose = rayObject.Pose; Pose boxPose = boxObject.Pose; // Apply scale to box. Vector3F boxExtent = boxShape.Extent * boxScale; // See SOLID and Bergen: "Collision Detection in Interactive 3D Environments", p. 75. // Note: Compute in box local space. // Apply scale to ray and transform to local space of box. Ray rayWorld = new Ray(rayShape); rayWorld.Scale(ref rayScale); // Apply scale to ray. rayWorld.ToWorld(ref rayPose); // Transform ray to world space. Ray ray = rayWorld; ray.ToLocal(ref boxPose); // Transform ray from world to local space. uint startOutcode = GeometryHelper.GetOutcode(boxExtent, ray.Origin); uint endOutcode = GeometryHelper.GetOutcode(boxExtent, ray.Origin + ray.Direction * ray.Length); if ((startOutcode & endOutcode) != 0) { // A face of the box is a separating plane. return; } // Assertion: The ray can intersect with the box but may not... float λEnter = 0; // ray parameter where ray enters box float λExit = 1; // ray parameter where ray exits box uint bit = 1; Vector3F r = ray.Direction * ray.Length; // ray vector Vector3F halfExtent = 0.5f * boxExtent; // Box half-extent vector. Vector3F normal = Vector3F.Zero; // normal vector for (int i = 0; i < 3; i++) { if ((startOutcode & bit) != 0) { // Intersection is an entering point. float λ = (-ray.Origin[i] - halfExtent[i]) / r[i]; if (λEnter < λ) { λEnter = λ; normal = new Vector3F(); normal[i] = 1; } } else if ((endOutcode & bit) != 0) { // Intersection is an exciting point. float λ = (-ray.Origin[i] - halfExtent[i]) / r[i]; if (λExit > λ) { λExit = λ; } } bit <<= 1; if ((startOutcode & bit) != 0) { // Intersection is an entering point. float λ = (-ray.Origin[i] + halfExtent[i]) / r[i]; if (λEnter < λ) { λEnter = λ; normal = new Vector3F(); normal[i] = -1; } } else if ((endOutcode & bit) != 0) { // Intersection is an exciting point. float λ = (-ray.Origin[i] + halfExtent[i]) / r[i]; if (λExit > λ) { λExit = λ; } } bit <<= 1; } if (λEnter <= λExit) { // The ray intersects the box. contactSet.HaveContact = true; if (type == CollisionQueryType.Boolean) { return; } float penetrationDepth = λEnter * ray.Length; if (normal == Vector3F.Zero) { normal = Vector3F.UnitX; } // Create contact info. Vector3F position = rayWorld.Origin + rayWorld.Direction * penetrationDepth; normal = boxPose.ToWorldDirection(normal); if (swapped) { normal = -normal; } // Update contact set. Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, true); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); } }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { // Invoke GJK for closest points. if (type == CollisionQueryType.ClosestPoints) { throw new GeometryException("This collision algorithm cannot handle closest-point queries. Use GJK instead."); } CollisionObject collisionObjectA = contactSet.ObjectA; CollisionObject collisionObjectB = contactSet.ObjectB; IGeometricObject geometricObjectA = collisionObjectA.GeometricObject; IGeometricObject geometricObjectB = collisionObjectB.GeometricObject; BoxShape boxA = geometricObjectA.Shape as BoxShape; BoxShape boxB = geometricObjectB.Shape as BoxShape; // Check if collision objects shapes are correct. if (boxA == null || boxB == null) { throw new ArgumentException("The contact set must contain box shapes.", "contactSet"); } Vector3F scaleA = Vector3F.Absolute(geometricObjectA.Scale); Vector3F scaleB = Vector3F.Absolute(geometricObjectB.Scale); Pose poseA = geometricObjectA.Pose; Pose poseB = geometricObjectB.Pose; // We perform the separating axis test in the local space of A. // The following variables are in local space of A. // Center of box B. Vector3F cB = poseA.ToLocalPosition(poseB.Position); // Orientation matrix of box B Matrix33F mB = poseA.Orientation.Transposed * poseB.Orientation; // Absolute of mB. Matrix33F aMB = Matrix33F.Absolute(mB); // Half extent vectors of the boxes. Vector3F eA = 0.5f * boxA.Extent * scaleA; Vector3F eB = 0.5f * boxB.Extent * scaleB; // ----- Separating Axis tests // If the boxes are separated, we immediately return. // For the case of interpenetration, we store the smallest penetration depth. float smallestPenetrationDepth = float.PositiveInfinity; int separatingAxisNumber = 0; Vector3F normal = Vector3F.UnitX; bool isNormalInverted = false; contactSet.HaveContact = false; // Assume no contact. #region ----- Case 1: Separating Axis: (1, 0, 0) ----- float separation = Math.Abs(cB.X) - (eA.X + eB.X * aMB.M00 + eB.Y * aMB.M01 + eB.Z * aMB.M02); if (separation > 0) { return; } if (type != CollisionQueryType.Boolean && -separation < smallestPenetrationDepth) { normal = Vector3F.UnitX; smallestPenetrationDepth = -separation; isNormalInverted = cB.X < 0; separatingAxisNumber = 1; } #endregion #region ----- Case 2: Separating Axis: (0, 1, 0) ----- separation = Math.Abs(cB.Y) - (eA.Y + eB.X * aMB.M10 + eB.Y * aMB.M11 + eB.Z * aMB.M12); if (separation > 0) { return; } if (type != CollisionQueryType.Boolean && -separation < smallestPenetrationDepth) { normal = Vector3F.UnitY; smallestPenetrationDepth = -separation; isNormalInverted = cB.Y < 0; separatingAxisNumber = 2; } #endregion #region ----- Case 3: Separating Axis: (0, 0, 1) ----- separation = Math.Abs(cB.Z) - (eA.Z + eB.X * aMB.M20 + eB.Y * aMB.M21 + eB.Z * aMB.M22); if (separation > 0) { return; } if (type != CollisionQueryType.Boolean && -separation < smallestPenetrationDepth) { normal = Vector3F.UnitZ; smallestPenetrationDepth = -separation; isNormalInverted = cB.Z < 0; separatingAxisNumber = 3; } #endregion #region ----- Case 4: Separating Axis: OrientationB * (1, 0, 0) ----- float expression = cB.X * mB.M00 + cB.Y * mB.M10 + cB.Z * mB.M20; separation = Math.Abs(expression) - (eB.X + eA.X * aMB.M00 + eA.Y * aMB.M10 + eA.Z * aMB.M20); if (separation > 0) { return; } if (type != CollisionQueryType.Boolean && -separation < smallestPenetrationDepth) { normal = mB.GetColumn(0); smallestPenetrationDepth = -separation; isNormalInverted = expression < 0; separatingAxisNumber = 4; } #endregion #region ----- Case 5: Separating Axis: OrientationB * (0, 1, 0) ----- expression = cB.X * mB.M01 + cB.Y * mB.M11 + cB.Z * mB.M21; separation = Math.Abs(expression) - (eB.Y + eA.X * aMB.M01 + eA.Y * aMB.M11 + eA.Z * aMB.M21); if (separation > 0) { return; } if (type != CollisionQueryType.Boolean && -separation < smallestPenetrationDepth) { normal = mB.GetColumn(1); smallestPenetrationDepth = -separation; isNormalInverted = expression < 0; separatingAxisNumber = 5; } #endregion #region ----- Case 6: Separating Axis: OrientationB * (0, 0, 1) ----- expression = cB.X * mB.M02 + cB.Y * mB.M12 + cB.Z * mB.M22; separation = Math.Abs(expression) - (eB.Z + eA.X * aMB.M02 + eA.Y * aMB.M12 + eA.Z * aMB.M22); if (separation > 0) { return; } if (type != CollisionQueryType.Boolean && -separation < smallestPenetrationDepth) { normal = mB.GetColumn(2); smallestPenetrationDepth = -separation; isNormalInverted = expression < 0; separatingAxisNumber = 6; } #endregion // The next 9 tests are edge-edge cases. The normal vector has to be normalized // to get the right penetration depth. // normal = Normalize(edgeA x edgeB) Vector3F separatingAxis; float length; #region ----- Case 7: Separating Axis: (1, 0, 0) x (OrientationB * (1, 0, 0)) ----- expression = cB.Z * mB.M10 - cB.Y * mB.M20; separation = Math.Abs(expression) - (eA.Y * aMB.M20 + eA.Z * aMB.M10 + eB.Y * aMB.M02 + eB.Z * aMB.M01); if (separation > 0) { return; } if (type != CollisionQueryType.Boolean) { separatingAxis = new Vector3F(0, -mB.M20, mB.M10); length = separatingAxis.Length; separation /= length; if (-separation < smallestPenetrationDepth) { normal = separatingAxis / length; smallestPenetrationDepth = -separation; isNormalInverted = expression < 0; separatingAxisNumber = 7; } } #endregion #region ----- Case 8: Separating Axis: (1, 0, 0) x (OrientationB * (0, 1, 0)) ----- expression = cB.Z * mB.M11 - cB.Y * mB.M21; separation = Math.Abs(expression) - (eA.Y * aMB.M21 + eA.Z * aMB.M11 + eB.X * aMB.M02 + eB.Z * aMB.M00); if (separation > 0) { return; } if (type != CollisionQueryType.Boolean) { separatingAxis = new Vector3F(0, -mB.M21, mB.M11); length = separatingAxis.Length; separation /= length; if (-separation < smallestPenetrationDepth) { normal = separatingAxis / length; smallestPenetrationDepth = -separation; isNormalInverted = expression < 0; separatingAxisNumber = 8; } } #endregion #region ----- Case 9: Separating Axis: (1, 0, 0) x (OrientationB * (0, 0, 1)) ----- expression = cB.Z * mB.M12 - cB.Y * mB.M22; separation = Math.Abs(expression) - (eA.Y * aMB.M22 + eA.Z * aMB.M12 + eB.X * aMB.M01 + eB.Y * aMB.M00); if (separation > 0) { return; } if (type != CollisionQueryType.Boolean) { separatingAxis = new Vector3F(0, -mB.M22, mB.M12); length = separatingAxis.Length; separation /= length; if (-separation < smallestPenetrationDepth) { normal = separatingAxis / length; smallestPenetrationDepth = -separation; isNormalInverted = expression < 0; separatingAxisNumber = 9; } } #endregion #region ----- Case 10: Separating Axis: (0, 1, 0) x (OrientationB * (1, 0, 0)) ----- expression = cB.X * mB.M20 - cB.Z * mB.M00; separation = Math.Abs(expression) - (eA.X * aMB.M20 + eA.Z * aMB.M00 + eB.Y * aMB.M12 + eB.Z * aMB.M11); if (separation > 0) { return; } if (type != CollisionQueryType.Boolean) { separatingAxis = new Vector3F(mB.M20, 0, -mB.M00); length = separatingAxis.Length; separation /= length; if (-separation < smallestPenetrationDepth) { normal = separatingAxis / length; smallestPenetrationDepth = -separation; isNormalInverted = expression < 0; separatingAxisNumber = 10; } } #endregion #region ----- Case 11: Separating Axis: (0, 1, 0) x (OrientationB * (0, 1, 0)) ----- expression = cB.X * mB.M21 - cB.Z * mB.M01; separation = Math.Abs(expression) - (eA.X * aMB.M21 + eA.Z * aMB.M01 + eB.X * aMB.M12 + eB.Z * aMB.M10); if (separation > 0) { return; } if (type != CollisionQueryType.Boolean) { separatingAxis = new Vector3F(mB.M21, 0, -mB.M01); length = separatingAxis.Length; separation /= length; if (-separation < smallestPenetrationDepth) { normal = separatingAxis / length; smallestPenetrationDepth = -separation; isNormalInverted = expression < 0; separatingAxisNumber = 11; } } #endregion #region ----- Case 12: Separating Axis: (0, 1, 0) x (OrientationB * (0, 0, 1)) ----- expression = cB.X * mB.M22 - cB.Z * mB.M02; separation = Math.Abs(expression) - (eA.X * aMB.M22 + eA.Z * aMB.M02 + eB.X * aMB.M11 + eB.Y * aMB.M10); if (separation > 0) { return; } if (type != CollisionQueryType.Boolean) { separatingAxis = new Vector3F(mB.M22, 0, -mB.M02); length = separatingAxis.Length; separation /= length; if (-separation < smallestPenetrationDepth) { normal = separatingAxis / length; smallestPenetrationDepth = -separation; isNormalInverted = expression < 0; separatingAxisNumber = 12; } } #endregion #region ----- Case 13: Separating Axis: (0, 0, 1) x (OrientationB * (1, 0, 0)) ----- expression = cB.Y * mB.M00 - cB.X * mB.M10; separation = Math.Abs(expression) - (eA.X * aMB.M10 + eA.Y * aMB.M00 + eB.Y * aMB.M22 + eB.Z * aMB.M21); if (separation > 0) { return; } if (type != CollisionQueryType.Boolean) { separatingAxis = new Vector3F(-mB.M10, mB.M00, 0); length = separatingAxis.Length; separation /= length; if (-separation < smallestPenetrationDepth) { normal = separatingAxis / length; smallestPenetrationDepth = -separation; isNormalInverted = expression < 0; separatingAxisNumber = 13; } } #endregion #region ----- Case 14: Separating Axis: (0, 0, 1) x (OrientationB * (0, 1, 0)) ----- expression = cB.Y * mB.M01 - cB.X * mB.M11; separation = Math.Abs(expression) - (eA.X * aMB.M11 + eA.Y * aMB.M01 + eB.X * aMB.M22 + eB.Z * aMB.M20); if (separation > 0) { return; } if (type != CollisionQueryType.Boolean) { separatingAxis = new Vector3F(-mB.M11, mB.M01, 0); length = separatingAxis.Length; separation /= length; if (-separation < smallestPenetrationDepth) { normal = separatingAxis / length; smallestPenetrationDepth = -separation; isNormalInverted = expression < 0; separatingAxisNumber = 14; } } #endregion #region ----- Case 15: Separating Axis: (0, 0, 1) x (OrientationB * (0, 0, 1)) ----- expression = cB.Y * mB.M02 - cB.X * mB.M12; separation = Math.Abs(expression) - (eA.X * aMB.M12 + eA.Y * aMB.M02 + eB.X * aMB.M21 + eB.Y * aMB.M20); if (separation > 0) { return; } if (type != CollisionQueryType.Boolean) { separatingAxis = new Vector3F(-mB.M12, mB.M02, 0); length = separatingAxis.Length; separation /= length; if (-separation < smallestPenetrationDepth) { normal = separatingAxis / length; smallestPenetrationDepth = -separation; isNormalInverted = expression < 0; separatingAxisNumber = 15; } } #endregion // We have a contact. contactSet.HaveContact = true; // HaveContact queries can exit here. if (type == CollisionQueryType.Boolean) { return; } // Lets find the contact info. Debug.Assert(smallestPenetrationDepth >= 0, "The smallest penetration depth should be greater than or equal to 0."); if (isNormalInverted) { normal = -normal; } // Transform normal from local space of A to world space. Vector3F normalWorld = poseA.ToWorldDirection(normal); if (separatingAxisNumber > 6) { // The intersection was detected by an edge-edge test. // Get the intersecting edges. // Separating axes: // 7 = x edge on A, x edge on B // 8 = x edge on A, y edge on B // 9 = x edge on A, Z edge on B // 10 = y edge on A, x edge on B // ... // 15 = z edge on A, z edge on B var edgeA = boxA.GetEdge((separatingAxisNumber - 7) / 3, normal, scaleA); var edgeB = boxB.GetEdge((separatingAxisNumber - 7) % 3, Matrix33F.MultiplyTransposed(mB, -normal), scaleB); edgeB.Start = mB * edgeB.Start + cB; edgeB.End = mB * edgeB.End + cB; Vector3F position; Vector3F dummy; GeometryHelper.GetClosestPoints(edgeA, edgeB, out position, out dummy); position = position - normal * (smallestPenetrationDepth / 2); // Position is between the positions of the box surfaces. // Convert back position from local space of A to world space; position = poseA.ToWorldPosition(position); Contact contact = ContactHelper.CreateContact(contactSet, position, normalWorld, smallestPenetrationDepth, false); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); } else if (1 <= separatingAxisNumber && separatingAxisNumber <= 6) { // The intersection was detected by a face vs. * test. // The separating axis is perpendicular to a face. #region ----- Credits ----- // The face vs. * test is based on the algorithm of the Bullet Continuous Collision // Detection and Physics Library. DigitalRune Geometry contains a new and improved // implementation of the original algorithm. // // The box-box detector in Bullet contains the following remarks: // // Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith // Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. // All rights reserved. Email: [email protected] Web: www.q12.org // // Bullet Continuous Collision Detection and Physics Library // Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ // // This software is provided 'as-is', without any express or implied warranty. // In no event will the authors be held liable for any damages arising from the use of this software. // Permission is granted to anyone to use this software for any purpose, // including commercial applications, and to alter it and redistribute it freely, // subject to the following restrictions: // // 1. The origin of this software must not be misrepresented; you must not claim that you wrote the // original software. If you use this software in a product, an acknowledgment in the product // documentation would be appreciated but is not required. // 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being // the original software. // 3. This notice may not be removed or altered from any source distribution. #endregion // We define the face perpendicular to the separating axis to be the "reference face". // The face of the other box closest to the reference face is called the "incident face". // Accordingly, we will call the box containing the reference face the "reference box" and // the box containing the incident face the "incident box". // // We will transform the incident face into the 2D space of reference face. Then we will // clip the incident face against the reference face. The polygon resulting from the // intersection will be transformed back into world space and the points of the polygon will // be the candidates for the contact points. Pose poseR; // Pose of reference box. Pose poseI; // Pose of incident box. Vector3F boxExtentR; // Half extent of reference box. Vector3F boxExtentI; // Half extent of incident box. // Contact normal (= normal of reference face) in world space. if (separatingAxisNumber <= 3) { poseR = poseA; poseI = poseB; boxExtentR = eA; boxExtentI = eB; isNormalInverted = false; } else { poseR = poseB; poseI = poseA; boxExtentR = eB; boxExtentI = eA; normalWorld = -normalWorld; isNormalInverted = true; } // Contact normal in local space of incident box. Vector3F normalI = poseI.ToLocalDirection(normalWorld); Vector3F absNormal = normalI; absNormal.Absolute(); Vector3F xAxisInc, yAxisInc; // The basis of the incident-face space. float absFaceOffsetI; // The offset of the incident face to the center of the box. Vector2F faceExtentI; // The half extent of the incident face. Vector3F faceNormal; // The normal of the incident face in world space. float faceDirection; // A value indicating the direction of the incident face. // Find the largest component of the normal. The largest component indicates which face is // the incident face. switch (Vector3F.Absolute(normalI).IndexOfLargestComponent) { case 0: faceExtentI.X = boxExtentI.Y; faceExtentI.Y = boxExtentI.Z; absFaceOffsetI = boxExtentI.X; faceNormal = poseI.Orientation.GetColumn(0); xAxisInc = poseI.Orientation.GetColumn(1); yAxisInc = poseI.Orientation.GetColumn(2); faceDirection = normalI.X; break; case 1: faceExtentI.X = boxExtentI.X; faceExtentI.Y = boxExtentI.Z; absFaceOffsetI = boxExtentI.Y; faceNormal = poseI.Orientation.GetColumn(1); xAxisInc = poseI.Orientation.GetColumn(0); yAxisInc = poseI.Orientation.GetColumn(2); faceDirection = normalI.Y; break; // case 2: default: faceExtentI.X = boxExtentI.X; faceExtentI.Y = boxExtentI.Y; absFaceOffsetI = boxExtentI.Z; faceNormal = poseI.Orientation.GetColumn(2); xAxisInc = poseI.Orientation.GetColumn(0); yAxisInc = poseI.Orientation.GetColumn(1); faceDirection = normalI.Z; break; } // Compute center of incident face relative to the center of the reference box in world space. float faceOffset = (faceDirection < 0) ? absFaceOffsetI : -absFaceOffsetI; Vector3F centerOfFaceI = faceNormal * faceOffset + poseI.Position - poseR.Position; // (Note: We will use the center of the incident face to compute the points of the incident // face and transform the points into the reference-face frame. The center of the incident // face is relative to the center of the reference box. We could also get center of the // incident face relative to the center of the reference face. But since we are projecting // the points from 3D to 2D this does not matter.) Vector3F xAxisR, yAxisR; // The basis of the reference-face space. float faceOffsetR; // The offset of the reference face to the center of the box. Vector2F faceExtentR; // The half extent of the reference face. switch (separatingAxisNumber) { case 1: case 4: faceExtentR.X = boxExtentR.Y; faceExtentR.Y = boxExtentR.Z; faceOffsetR = boxExtentR.X; xAxisR = poseR.Orientation.GetColumn(1); yAxisR = poseR.Orientation.GetColumn(2); break; case 2: case 5: faceExtentR.X = boxExtentR.X; faceExtentR.Y = boxExtentR.Z; faceOffsetR = boxExtentR.Y; xAxisR = poseR.Orientation.GetColumn(0); yAxisR = poseR.Orientation.GetColumn(2); break; // case 3: // case 6: default: faceExtentR.X = boxExtentR.X; faceExtentR.Y = boxExtentR.Y; faceOffsetR = boxExtentR.Z; xAxisR = poseR.Orientation.GetColumn(0); yAxisR = poseR.Orientation.GetColumn(1); break; } // Compute the center of the incident face in the reference-face frame. // We can simply project centerOfFaceI onto the x- and y-axis of the reference // face. Vector2F centerOfFaceIInR; //centerOfFaceIInR.X = Vector3F.Dot(centerOfFaceI, xAxisR); // ----- Optimized version: centerOfFaceIInR.X = centerOfFaceI.X * xAxisR.X + centerOfFaceI.Y * xAxisR.Y + centerOfFaceI.Z * xAxisR.Z; //centerOfFaceIInR.Y = Vector3F.Dot(centerOfFaceI, yAxisR); // ----- Optimized version: centerOfFaceIInR.Y = centerOfFaceI.X * yAxisR.X + centerOfFaceI.Y * yAxisR.Y + centerOfFaceI.Z * yAxisR.Z; // Now, we have the center of the incident face in reference-face coordinates. // To compute the corners of the incident face in reference-face coordinates, we need // transform faceExtentI (the half extent vector of the incident face) from the incident- // face frame to the reference-face frame to compute the corners. // // The reference-face frame has the basis // mR = (xAxisR, yAxisR, ?) // // The incident-face frame has the basis // mI = (xAxisI, yAxisI, ?) // // Rotation from incident-face frame to reference-face frame is // mIToR = mR^-1 * mI // // The corner offsets in incident-face space is are vectors (x, y, 0). To transform these // vectors from incident-face space to reference-face space we need to calculate: // mIToR * v // // Since the z-components are 0 and we are only interested in the resulting x, y coordinates // in reference-space when can reduce the rotation to a 2 x 2 matrix. (The other components // are not needed.) // ----- Optimized version: (Original on the right) Matrix22F mIToR; mIToR.M00 = xAxisR.X * xAxisInc.X + xAxisR.Y * xAxisInc.Y + xAxisR.Z * xAxisInc.Z; // mIToR.M00 = Vector3F.Dot(xAxisR, xAxisInc); mIToR.M01 = xAxisR.X * yAxisInc.X + xAxisR.Y * yAxisInc.Y + xAxisR.Z * yAxisInc.Z; // mIToR.M01 = Vector3F.Dot(xAxisR, yAxisInc); mIToR.M10 = yAxisR.X * xAxisInc.X + yAxisR.Y * xAxisInc.Y + yAxisR.Z * xAxisInc.Z; // mIToR.M10 = Vector3F.Dot(yAxisR, xAxisInc); mIToR.M11 = yAxisR.X * yAxisInc.X + yAxisR.Y * yAxisInc.Y + yAxisR.Z * yAxisInc.Z; // mIToR.M11 = Vector3F.Dot(yAxisR, yAxisInc); // The corner offsets in incident-face space are: // (-faceExtentI.X, -faceExtentI.Y) ... left, bottom corner // ( faceExtentI.X, -faceExtentI.Y) ... right, bottom corner // ( faceExtentI.X, faceExtentI.Y) ... right, top corner // (-faceExtentI.X, faceExtentI.Y) ... left, top corner // // Instead of transforming each corner offset, we can optimize the computation: Do the // matrix-vector multiplication once, keep the intermediate products, apply the sign // of the components when adding the intermediate results. float k1 = mIToR.M00 * faceExtentI.X; // Products of matrix-vector multiplication. float k2 = mIToR.M01 * faceExtentI.Y; float k3 = mIToR.M10 * faceExtentI.X; float k4 = mIToR.M11 * faceExtentI.Y; List <Vector2F> quad = DigitalRune.ResourcePools <Vector2F> .Lists.Obtain(); quad.Add(new Vector2F(centerOfFaceIInR.X - k1 - k2, centerOfFaceIInR.Y - k3 - k4)); quad.Add(new Vector2F(centerOfFaceIInR.X + k1 - k2, centerOfFaceIInR.Y + k3 - k4)); quad.Add(new Vector2F(centerOfFaceIInR.X + k1 + k2, centerOfFaceIInR.Y + k3 + k4)); quad.Add(new Vector2F(centerOfFaceIInR.X - k1 + k2, centerOfFaceIInR.Y - k3 + k4)); // Clip incident face (quadrilateral) against reference face (rectangle). List <Vector2F> contacts2D = ClipQuadrilateralAgainstRectangle(faceExtentR, quad); // Transform contact points back to world space and compute penetration depths. int numberOfContacts = contacts2D.Count; List <Vector3F> contacts3D = DigitalRune.ResourcePools <Vector3F> .Lists.Obtain(); List <float> penetrationDepths = DigitalRune.ResourcePools <float> .Lists.Obtain(); Matrix22F mRToI = mIToR.Inverse; for (int i = numberOfContacts - 1; i >= 0; i--) { Vector2F contact2DR = contacts2D[i]; // Contact in reference-face space. Vector2F contact2DI = mRToI * (contact2DR - centerOfFaceIInR); // Contact in incident-face space. // Transform point in incident-face space to world (relative to center of reference box). // contact3D = mI * (x, y, 0) + centerOfFaceI Vector3F contact3D; contact3D.X = xAxisInc.X * contact2DI.X + yAxisInc.X * contact2DI.Y + centerOfFaceI.X; contact3D.Y = xAxisInc.Y * contact2DI.X + yAxisInc.Y * contact2DI.Y + centerOfFaceI.Y; contact3D.Z = xAxisInc.Z * contact2DI.X + yAxisInc.Z * contact2DI.Y + centerOfFaceI.Z; // Compute penetration depth. //float penetrationDepth = faceOffsetR - Vector3F.Dot(normalWorld, contact3D); // ----- Optimized version: float penetrationDepth = faceOffsetR - (normalWorld.X * contact3D.X + normalWorld.Y * contact3D.Y + normalWorld.Z * contact3D.Z); if (penetrationDepth >= 0) { // Valid contact. contacts3D.Add(contact3D); penetrationDepths.Add(penetrationDepth); } else { // Remove bad contacts from the 2D contacts. // (We might still need the 2D contacts, if we need to reduce the contacts.) contacts2D.RemoveAt(i); } } numberOfContacts = contacts3D.Count; if (numberOfContacts == 0) { return; // Should never happen. } // Revert normal back to original direction. normal = (isNormalInverted) ? -normalWorld : normalWorld; // Note: normal ........ contact normal pointing from box A to B. // normalWorld ... contact normal pointing from reference box to incident box. if (numberOfContacts <= MaxNumberOfContacts) { // Add all contacts to contact set. for (int i = 0; i < numberOfContacts; i++) { float penetrationDepth = penetrationDepths[i]; // Position is between the positions of the box surfaces. Vector3F position = contacts3D[i] + poseR.Position + normalWorld * (penetrationDepth / 2); Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); } } else { // Reduce number of contacts, keep the contact with the max penetration depth. int indexOfDeepest = 0; float maxPenetrationDepth = penetrationDepths[0]; for (int i = 1; i < numberOfContacts; i++) { float penetrationDepth = penetrationDepths[i]; if (penetrationDepth > maxPenetrationDepth) { maxPenetrationDepth = penetrationDepth; indexOfDeepest = i; } } List <int> indicesOfContacts = ReduceContacts(contacts2D, indexOfDeepest, MaxNumberOfContacts); // Add selected contacts to contact set. numberOfContacts = indicesOfContacts.Count; for (int i = 0; i < numberOfContacts; i++) { int index = indicesOfContacts[i]; float penetrationDepth = penetrationDepths[index]; // Position is between the positions of the box surfaces. Vector3F position = contacts3D[index] + poseR.Position + normalWorld * (penetrationDepth / 2); Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepths[index], false); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); } DigitalRune.ResourcePools <int> .Lists.Recycle(indicesOfContacts); } DigitalRune.ResourcePools <Vector2F> .Lists.Recycle(contacts2D); DigitalRune.ResourcePools <Vector3F> .Lists.Recycle(contacts3D); DigitalRune.ResourcePools <float> .Lists.Recycle(penetrationDepths); } }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { IGeometricObject sphereObjectA = contactSet.ObjectA.GeometricObject; IGeometricObject sphereObjectB = contactSet.ObjectB.GeometricObject; SphereShape sphereShapeA = sphereObjectA.Shape as SphereShape; SphereShape sphereShapeB = sphereObjectB.Shape as SphereShape; // Check if collision objects are spheres. if (sphereShapeA == null || sphereShapeB == null) { throw new ArgumentException("The contact set must contain sphere shapes.", "contactSet"); } Vector3F scaleA = Vector3F.Absolute(sphereObjectA.Scale); Vector3F scaleB = Vector3F.Absolute(sphereObjectB.Scale); // Call MPR for non-uniformly scaled spheres. if (scaleA.X != scaleA.Y || scaleA.Y != scaleA.Z || scaleB.X != scaleB.Y || scaleB.Y != scaleB.Z) { if (_fallbackAlgorithm == null) { _fallbackAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(ConvexShape), typeof(ConvexShape)]; } _fallbackAlgorithm.ComputeCollision(contactSet, type); return; } // Apply uniform scale. float radiusA = sphereShapeA.Radius * scaleA.X; float radiusB = sphereShapeB.Radius * scaleB.X; // Vector from center of A to center of B. Vector3F centerA = sphereObjectA.Pose.Position; Vector3F centerB = sphereObjectB.Pose.Position; Vector3F aToB = centerB - centerA; float lengthAToB = aToB.Length; // Check radius of spheres. float penetrationDepth = radiusA + radiusB - lengthAToB; contactSet.HaveContact = penetrationDepth >= 0; if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact)) { // HaveContact queries can exit here. // GetContacts queries can exit here if we don't have a contact. return; } // ----- Create contact information. Vector3F normal; if (Numeric.IsZero(lengthAToB)) { // Spheres are on the same position, we can choose any normal vector. // Possibly it would be better to consider the object movement (velocities), but // it is not important since this case should be VERY rare. normal = Vector3F.UnitY; } else { normal = aToB.Normalized; } // The contact point lies in the middle of the intersecting volume. Vector3F position = centerA + normal * (radiusA - penetrationDepth / 2); // Update contact set. Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { // Ray vs. convex has at max 1 contact. Debug.Assert(contactSet.Count <= 1); // Object A should be the ray. // Object B should be the convex. IGeometricObject rayObject = contactSet.ObjectA.GeometricObject; IGeometricObject convexObject = contactSet.ObjectB.GeometricObject; // Swap object if necessary. bool swapped = (convexObject.Shape is RayShape); if (swapped) { MathHelper.Swap(ref rayObject, ref convexObject); } RayShape rayShape = rayObject.Shape as RayShape; ConvexShape convexShape = convexObject.Shape as ConvexShape; // Check if shapes are correct. if (rayShape == null || convexShape == null) { throw new ArgumentException("The contact set must contain a ray and a convex shape.", "contactSet"); } // Call line segment vs. convex for closest points queries. if (type == CollisionQueryType.ClosestPoints) { // Find point on ray closest to the convex shape. // Call GJK. _gjk.ComputeCollision(contactSet, type); if (contactSet.HaveContact == false) { return; } // Otherwise compute 1 contact ... // GJK result is invalid for penetration. foreach (var contact in contactSet) { contact.Recycle(); } contactSet.Clear(); } // Assume no contact. contactSet.HaveContact = false; // Get transformations. Vector3 rayScale = rayObject.Scale; Vector3 convexScale = convexObject.Scale; Pose convexPose = convexObject.Pose; Pose rayPose = rayObject.Pose; // See Raycasting paper of van den Bergen or Bullet. // Note: Compute in local space of convex (object B). // Scale ray and transform ray to local space of convex. Ray rayWorld = new Ray(rayShape); rayWorld.Scale(ref rayScale); // Scale ray. rayWorld.ToWorld(ref rayPose); // Transform ray to world space. Ray ray = rayWorld; ray.ToLocal(ref convexPose); // Transform ray to local space of convex. var simplex = GjkSimplexSolver.Create(); try { Vector3 s = ray.Origin; // source Vector3 r = ray.Direction * ray.Length; // ray float λ = 0; // ray parameter Vector3 x = s; // hit spot (on ray) Vector3 n = new Vector3(); // normal Vector3 v = x - convexShape.GetSupportPoint(ray.Direction, convexScale); // v = x - arbitrary point. Vector used for support mapping. float distanceSquared = v.LengthSquared(); // ||v||² int iterationCount = 0; while (distanceSquared > Numeric.EpsilonF && iterationCount < MaxNumberOfIterations) { iterationCount++; Vector3 p = convexShape.GetSupportPoint(v, convexScale); // point on convex Vector3 w = x - p; // simplex/Minkowski difference point float vDotW = Vector3.Dot(v, w); // v∙w if (vDotW > 0) { float vDotR = Vector3.Dot(v, r); // v∙r if (vDotR >= 0) // TODO: vDotR >= - Epsilon^2 ? { return; // No Hit. } λ = λ - vDotW / vDotR; x = s + λ * r; simplex.Clear(); // Configuration space obstacle (CSO) is translated whenever x is updated. w = x - p; n = v; } simplex.Add(w, x, p); simplex.Update(); v = simplex.ClosestPoint; distanceSquared = (simplex.IsValid && !simplex.IsFull) ? v.LengthSquared() : 0; } // We have a contact if the hit is inside the ray length. contactSet.HaveContact = (0 <= λ && λ <= 1); if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact)) { // HaveContact queries can exit here. // GetContacts queries can exit here if we don't have a contact. return; } float penetrationDepth = λ * ray.Length; Debug.Assert(contactSet.HaveContact, "Separation was not detected by GJK above."); // Convert back to world space. Vector3 position = rayWorld.Origin + rayWorld.Direction * penetrationDepth; n = convexPose.ToWorldDirection(n); if (!n.TryNormalize()) { n = Vector3.UnitY; } if (swapped) { n = -n; } // Update contact set. Contact contact = ContactHelper.CreateContact(contactSet, position, -n, penetrationDepth, true); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); } finally { simplex.Recycle(); } }
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type) { // Invoke GJK for closest points. if (type == CollisionQueryType.ClosestPoints) { throw new GeometryException("This collision algorithm cannot handle closest-point queries. Use GJK instead."); } //if (UseMpr) //{ // if (_mpr == null) // _mpr = new MinkowskiPortalRefinement(CollisionDetection); // _mpr.ComputeCollision(contactSet, type); // return; //} CollisionObject collisionObjectA = contactSet.ObjectA; CollisionObject collisionObjectB = contactSet.ObjectB; IGeometricObject geometricObjectA = collisionObjectA.GeometricObject; IGeometricObject geometricObjectB = collisionObjectB.GeometricObject; TriangleShape triangleShapeA = geometricObjectA.Shape as TriangleShape; TriangleShape triangleShapeB = geometricObjectB.Shape as TriangleShape; // Check if collision objects shapes are correct. if (triangleShapeA == null || triangleShapeB == null) { throw new ArgumentException("The contact set must contain triangle shapes.", "contactSet"); } Vector3F scaleA = Vector3F.Absolute(geometricObjectA.Scale); Vector3F scaleB = Vector3F.Absolute(geometricObjectB.Scale); Pose poseA = geometricObjectA.Pose; Pose poseB = geometricObjectB.Pose; // Get triangles in world space. Triangle triangleA; triangleA.Vertex0 = poseA.ToWorldPosition(triangleShapeA.Vertex0 * scaleA); triangleA.Vertex1 = poseA.ToWorldPosition(triangleShapeA.Vertex1 * scaleA); triangleA.Vertex2 = poseA.ToWorldPosition(triangleShapeA.Vertex2 * scaleA); Triangle triangleB; triangleB.Vertex0 = poseB.ToWorldPosition(triangleShapeB.Vertex0 * scaleB); triangleB.Vertex1 = poseB.ToWorldPosition(triangleShapeB.Vertex1 * scaleB); triangleB.Vertex2 = poseB.ToWorldPosition(triangleShapeB.Vertex2 * scaleB); if (type == CollisionQueryType.Boolean) { contactSet.HaveContact = GeometryHelper.HaveContact(ref triangleA, ref triangleB); return; } Debug.Assert(type == CollisionQueryType.Contacts, "TriangleTriangleAlgorithm cannot handle closest point queries."); // Assume no contact. contactSet.HaveContact = false; Vector3F position, normal; float penetrationDepth; if (!GetContact(ref triangleA, ref triangleB, false, false, out position, out normal, out penetrationDepth)) { return; } contactSet.HaveContact = true; Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false); ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance); }