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