/// <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 VisualizeContactSet(Microsoft.Xna.Framework.Game game)
: base(game)
{
SampleFramework.IsMouseVisible = false;
GraphicsScreen.ClearBackground = true;
SetCamera(new Vector3F(0, 1, 10), 0, 0);
CreateObjects();
_contactSet = _collisionDetection.GetContacts(_objectA, _objectB);
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
if (type == CollisionQueryType.ClosestPoints)
{
// Closest point queries.
_closestPointsAlgorithm.ComputeCollision(contactSet, type);
if (contactSet.HaveContact)
{
// Penetration.
// Remember the closest point info in case we run into troubles.
// Get the last contact. We assume that this is the newest. In most cases there will
// only be one contact in the contact set.
Contact fallbackContact = (contactSet.Count > 0)
? contactSet[contactSet.Count - 1]
: null;
// Call the contact query algorithm.
_contactAlgorithm.ComputeCollision(contactSet, CollisionQueryType.Contacts);
if (!contactSet.HaveContact)
{
// Problem!
// The closest-point algorithm reported contact. The contact algorithm didn't find a contact.
// This can happen, for example, because of numerical inaccuracies in GJK vs. MPR.
// Keep the result of the closest-point computation, but decrease the penetration depth
// to indicate separation.
if (fallbackContact != null)
{
Debug.Assert(fallbackContact.PenetrationDepth == 0);
fallbackContact.PenetrationDepth = -Math.Min(10 * Numeric.EpsilonF, CollisionDetection.Epsilon);
foreach (var contact in contactSet)
if (contact != fallbackContact)
contact.Recycle();
contactSet.Clear();
contactSet.Add(fallbackContact);
}
contactSet.HaveContact = false;
}
}
}
else
{
// Boolean or contact queries.
_contactAlgorithm.ComputeCollision(contactSet, type);
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
bool isLineA = contactSet.ObjectA.GeometricObject.Shape is LineShape;
bool isLineB = contactSet.ObjectB.GeometricObject.Shape is LineShape;
if (isLineA && isLineB)
{
ComputeLineVsLine(contactSet, type);
}
else if (isLineA || isLineB)
{
ComputeLineVsOther(contactSet, type, isLineA);
}
else
{
throw new ArgumentException("The contact set must contain a line.", "contactSet");
}
}
// TODO: Make the max. number of contact points configurable or experiment to find out the optimal number.
// TODO: We could also keep the convex hull of contact points in the 2D plane defined by the (average) normal vector.
/// <summary>
/// Filters the specified contact set.
/// </summary>
/// <param name="contactSet">The contact set.</param>
public void Filter(ContactSet contactSet)
{
if (contactSet == null)
return;
int numberOfContacts = contactSet.Count;
// Nothing to do if we have 4 or less contacts.
if (numberOfContacts <= 4)
return;
// First of all we keep the best contact: This is the contact with the deepest penetration.
// Note: We could also argue that the newest contact is the best, but then following
// problem occurs: Two composite objects touch with 5 contacts. 1 is removed. In
// the next step the same 5 contacts are found. The one that was removed in the last
// frame is now the newest contact, so another contact is removed. --> 1 Contact will
// always toggle and is not persistent.
// Find the deepest contact
int deepestContactIndex = 0;
float deepestPenetrationDepth = contactSet[0].PenetrationDepth;
for (int i = 1; i < numberOfContacts; i++)
{
Contact contact = contactSet[i];
if (contact.PenetrationDepth > deepestPenetrationDepth)
{
deepestPenetrationDepth = contact.PenetrationDepth;
deepestContactIndex = i;
}
}
// Move deepest contact to index 0.
if (deepestContactIndex != 0)
{
Contact deepest = contactSet[deepestContactIndex];
contactSet[deepestContactIndex] = contactSet[0];
contactSet[0] = deepest;
}
// Iteratively, remove single contacts until 4 are left.
while (contactSet.Count > 4)
Reduce(contactSet);
}
private static void CheckResult(ContactSet contactSet, bool isClosestPointResult)
{
if (isClosestPointResult && contactSet.Count > 1)
{
throw new GeometryException("Closest point query result contains more than one contact.");
}
if (contactSet.HaveContact)
{
if (contactSet.Any(contact => contact.PenetrationDepth < 0))
{
throw new GeometryException("HaveContact is true but the contactSet contains a separated contact.");
}
if (contactSet.ObjectA.GeometricObject.Shape is RayShape || contactSet.ObjectB.GeometricObject.Shape is RayShape)
{
if (contactSet.Count > 1)
{
throw new GeometryException("Ray casts must not contain more than 1 contact.");
}
if (contactSet.Any(contact => !contact.IsRayHit))
{
throw new GeometryException("IsRayHit is not set for a ray hit.");
}
}
}
if (isClosestPointResult == false && contactSet.HaveContact == false && contactSet.Count > 0)
{
throw new GeometryException("A contact query returned contact details for a separated pair.");
}
if (contactSet.HaveContact == false && contactSet.Any(contact => contact.PenetrationDepth >= 0))
{
throw new GeometryException("HaveContact is false but the contactSet contains a touching/penetrating contact.");
}
}
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 TestZeroSphere()
{
SphereSphereAlgorithm algo = new SphereSphereAlgorithm(new CollisionDetection());
CollisionObject objectA = new CollisionObject();
((GeometricObject)objectA.GeometricObject).Shape = new SphereShape(0);
CollisionObject objectB = new CollisionObject();
((GeometricObject)objectB.GeometricObject).Shape = new SphereShape(0);
((GeometricObject)objectA.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 1));
((GeometricObject)objectB.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 1));
ContactSet cs = ContactSet.Create(objectA, objectB);
algo.UpdateContacts(cs, 0);
Assert.AreEqual(1, cs.Count);
Assert.AreEqual(new Vector3F(0, 0, 1), cs[0].Position);
//Assert.AreEqual(new Vector3F(0, 0, 1), cs.Contacts[0].Normal);
Assert.IsTrue(Numeric.AreEqual(0, cs[0].PenetrationDepth));
}
public void TestInfiniteSphere()
{
SphereSphereAlgorithm algo = new SphereSphereAlgorithm(new CollisionDetection());
CollisionObject objectA = new CollisionObject();
((GeometricObject)objectA.GeometricObject).Shape = new SphereShape(float.PositiveInfinity);
CollisionObject objectB = new CollisionObject();
((GeometricObject)objectB.GeometricObject).Shape = new SphereShape(1f);
((GeometricObject)objectA.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0));
((GeometricObject)objectB.GeometricObject).Pose = new Pose(new Vector3F(0, 2f, 0));
ContactSet cs = ContactSet.Create(objectA, objectB);
algo.UpdateContacts(cs, 0);
Assert.AreEqual(1, cs.Count);
//Assert.AreEqual(new Vector3F(0, 1f, 0), cs.Contacts[0].Position); // Undefined when a sphere is infinite.
Assert.AreEqual(new Vector3F(0, 1, 0), cs[0].Normal);
Assert.IsTrue(float.IsPositiveInfinity(cs[0].PenetrationDepth));
}
public void TestContainment()
{
PlaneSphereAlgorithm algo = new PlaneSphereAlgorithm(new CollisionDetection());
CollisionObject objectA = new CollisionObject();
((GeometricObject)objectA.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)objectA.GeometricObject).Pose = new Pose(new Vector3(0, -2, 0));
CollisionObject objectB = new CollisionObject();
((GeometricObject)objectB.GeometricObject).Shape = new PlaneShape(new Vector3(0, 1, 0).Normalized, 0);
((GeometricObject)objectB.GeometricObject).Pose = new Pose(new Vector3(0, 0, 0));
ContactSet cs = ContactSet.Create(objectA, objectB);
algo.UpdateContacts(cs, 0);
Assert.AreEqual(objectA, cs.ObjectA);
Assert.AreEqual(objectB, cs.ObjectB);
Assert.AreEqual(1, cs.Count);
Assert.AreEqual(new Vector3(0, -1.5f, 0), cs[0].Position);
Assert.IsTrue(Vector3.AreNumericallyEqual(new Vector3(0, -1, 0), cs[0].Normal));
Assert.IsTrue(Numeric.AreEqual(3, cs[0].PenetrationDepth));
// Test swapped case:
cs = ContactSet.Create(objectB, objectA);
algo.UpdateContacts(cs, 0);
Assert.AreEqual(objectB, cs.ObjectA);
Assert.AreEqual(objectA, cs.ObjectB);
Assert.AreEqual(1, cs.Count);
Assert.AreEqual(new Vector3(0, -1.5f, 0), cs[0].Position);
Assert.IsTrue(Vector3.AreNumericallyEqual(new Vector3(0, 1, 0), cs[0].Normal));
Assert.IsTrue(Numeric.AreEqual(3, cs[0].PenetrationDepth));
}
public DumpContactSetSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
GraphicsScreen.ClearBackground = true;
// Create two collision objects with triangle mesh shapes.
var meshA = new SphereShape(1).GetMesh(0.01f, 4);
var shapeA = new TriangleMeshShape(meshA, true)
{
Partition = new CompressedAabbTree()
};
var poseA = new Pose(new Vector3(-1, 0, 0), RandomHelper.Random.NextQuaternion());
var collisionObjectA = new CollisionObject(new GeometricObject(shapeA, poseA));
var meshB = new BoxShape(0.2f, 2, 1f).GetMesh(0.01f, 4);
var shapeB = new TriangleMeshShape(meshB, true)
{
Partition = new CompressedAabbTree()
};
var poseB = new Pose(new Vector3(0.1f, 0, 0), RandomHelper.Random.NextQuaternion());
var collisionObjectB = new CollisionObject(new GeometricObject(shapeB, poseB));
// Explicitly create a contact set. (Normally you would get the contact set
// from the collision domain...)
var contactSet = ContactSet.Create(collisionObjectA, collisionObjectB);
// Create a C# sample which visualizes the contact set.
const string Filename = "DumpedContactSet001.cs";
DumpContactSet(contactSet, Filename);
GraphicsScreen.DebugRenderer2D.DrawText(
"Contact set dumped into the file: " + Filename,
new Vector2F(300, 300),
Color.Black);
}
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 Update(GameTime gameTime)
{
_domain.EnableMultithreading = InputService.IsDown(Keys.Space);
MessageBox.Show("Start");
_deltaTime = 1 / 60f;
for (int bla = 0; bla < 10000; bla++)
{
if (ClosestPointQueriesEnabled)
{
// Here we run closest point queries on all object pairs.
// We compare the results with the contact queries.
for (int i = 0; i < _domain.CollisionObjects.Count; i++)
{
for (int j = i + 1; j < _domain.CollisionObjects.Count; j++)
{
CollisionObject a = _domain.CollisionObjects[i];
CollisionObject b = _domain.CollisionObjects[j];
ContactSet closestPointQueryResult = _domain.CollisionDetection.GetClosestPoints(a, b);
ContactSet contactSet = _domain.GetContacts(a, b);
// Ignore height fields and rays.
if (a.GeometricObject.Shape is HeightField || b.GeometricObject.Shape is HeightField)
break;
if (a.GeometricObject.Shape is RayShape || b.GeometricObject.Shape is RayShape)
break;
if (contactSet == null || !contactSet.HaveContact)
{
// No contact in contactSet
if (closestPointQueryResult.HaveContact)
{
// Contact in closest point query. Results are inconsistent.
if (closestPointQueryResult.Count > 0
&& closestPointQueryResult[0].PenetrationDepth > 0.001f)
Debugger.Break();
}
}
else if (!closestPointQueryResult.HaveContact)
{
// contact in contact query, but no contact in closest point query.
// We allow a deviation within a small tolerance.
if (closestPointQueryResult.Count > 0 && contactSet.Count > 0
&& closestPointQueryResult[0].PenetrationDepth + contactSet[0].PenetrationDepth > 0.001f)
Debugger.Break();
}
}
}
}
// Reflect velocity if objects collide:
// The collision domain contains a ContactSet for each pair of touching objects.
foreach (var contactSet in _domain.ContactSets)
{
// Get the touching objects.
var moA = (MovingGeometricObject)contactSet.ObjectA.GeometricObject;
var moB = (MovingGeometricObject)contactSet.ObjectB.GeometricObject;
// Reflect only at boundary objects.
if (!(moA.Shape is PlaneShape) && !(moB.Shape is PlaneShape)
&& !(moA.Shape is HeightField) && !(moB.Shape is HeightField))
continue;
// Get normal vector. If objects are sensors, the contact set does not tell us
// the right normal.
Vector3 normal = Vector3.Zero;
if (contactSet.Count > 0)
{
// Take normal from contact set.
normal = contactSet[0].Normal;
}
else
{
// If we use Trigger CollisionObjects we do not have contacts. --> Reflect at
// bounding planes.
if (moA.Shape is PlaneShape)
normal = ((PlaneShape)moA.Shape).Normal;
else if (moB.Shape is PlaneShape)
normal = -((PlaneShape)moB.Shape).Normal;
else if (moA.Shape is HeightField)
normal = Vector3.UnitY;
else
normal = -Vector3.UnitY;
}
//else if (moA.Shape is Plane || moB.Shape is Plane )
//{
// // Use plane normal.
// IGeometricObject plane = moA.Shape is Plane ? moA : moB;
// normal = plane.Pose.ToWorldDirection(((Plane)plane.Shape).Normal);
// if (moB == plane)
// normal = -normal;
//}
//else if (moA.Shape is HeightField || moB.Shape is HeightField)
//{
// // Use up-vector for height field contacts.
// normal = Vector3.UnitY;
// if (moB.Shape is HeightField)
// normal = -normal;
//}
//else
//{
// // Use random normal.
// normal = RandomHelper.NextVector3(-1, 1).Normalized;
//}
// Check if the objects move towards or away from each other in the direction of the normal.
if (normal != Vector3.Zero && Vector3.Dot(moB.LinearVelocity - moA.LinearVelocity, normal) <= 0)
{
// Objects move towards each other. --> Reflect their velocities.
moA.LinearVelocity -= 2 * Vector3.ProjectTo(moA.LinearVelocity, normal);
moB.LinearVelocity -= 2 * Vector3.ProjectTo(moB.LinearVelocity, normal);
moA.AngularVelocity = -moA.AngularVelocity;
moB.AngularVelocity = -moB.AngularVelocity;
}
}
// Get the size of the current time step.
float timeStep = (float)gameTime.ElapsedGameTime.TotalSeconds;
// Move objects.
var objects = _domain.CollisionObjects.Select(co => co.GeometricObject).OfType<MovingGeometricObject>();
foreach (var obj in objects)
{
// Update position.
Vector3 position = obj.Pose.Position + obj.LinearVelocity * timeStep;
// Update rotation.
Vector3 rotationAxis = obj.AngularVelocity;
float angularSpeed = obj.AngularVelocity.Length;
Matrix rotation = (Numeric.IsZero(angularSpeed))
? Matrix.Identity
: Matrix.CreateRotation(rotationAxis, angularSpeed * timeStep);
var orientation = rotation * obj.Pose.Orientation;
// Incrementally updating the rotation matrix will eventually create a
// matrix which is not a rotation matrix anymore because of numerical
// problems. Re-othogonalization make sure that the matrix represents a
// rotation.
orientation.Orthogonalize();
obj.Pose = new Pose(position, orientation);
}
// Update collision domain. This computes new contact information.
_domain.Update(timeStep);
MessageBox.Show("Finished");
Exit();
// Record some statistics.
int numberOfObjects = _domain.CollisionObjects.Count;
Profiler.SetFormat("NumObjects", 1, "The total number of objects.");
Profiler.AddValue("NumObjects", numberOfObjects);
// If there are n objects, we can have max. n * (n - 1) / 2 collisions.
Profiler.SetFormat("NumObjectPairs", 1, "The number of objects pairs, which have to be checked.");
Profiler.AddValue("NumObjectPairs", numberOfObjects * (numberOfObjects - 1f) / 2f);
// The first part of the collision detection is the "broad-phase" which
// filters out objects that cannot collide (e.g. using a fast bounding box test).
Profiler.SetFormat("BroadPhasePairs", 1, "The number of overlaps reported by the broad phase.");
Profiler.AddValue("BroadPhasePairs", _domain.NumberOfBroadPhaseOverlaps);
// Finally, the collision detection computes the exact contact information and creates
// a ContactSet with the Contacts for each pair of colliding objects.
Profiler.SetFormat("ContactSetCount", 1, "The number of actual collisions.");
Profiler.AddValue("ContactSetCount", _domain.ContactSets.Count);
// Draw objects using the DebugRenderer of the graphics screen.
var debugRenderer = GraphicsScreen.DebugRenderer;
debugRenderer.Clear();
foreach (var collisionObject in _domain.CollisionObjects)
debugRenderer.DrawObject(collisionObject.GeometricObject, GraphicsHelper.GetUniqueColor(collisionObject), false, false);
}
}
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);
}
// OnUpdate() is called once per frame.
protected override void OnUpdate(TimeSpan deltaTime)
{
if (_spring != null
&& !_inputService.IsDown(MouseButtons.Left)
&& !_inputService.IsDown(Buttons.LeftTrigger, LogicalPlayerIndex.One))
{
// The user has released the object.
_simulation.Constraints.Remove(_spring);
_spring = null;
}
if (!_inputService.IsMouseOrTouchHandled
&& !_inputService.IsGamePadHandled(LogicalPlayerIndex.Any)
&& (_inputService.IsPressed(MouseButtons.Left, false)
|| _inputService.IsPressed(Buttons.LeftTrigger, false, LogicalPlayerIndex.One)))
{
// The user has pressed the grab button and the input was not already handled
// by another game object.
// Remove the old joint, in case anything is grabbed.
if (_spring != null)
{
_simulation.Constraints.Remove(_spring);
_spring = null;
}
// The spring is attached at the position that is targeted with the cross-hair.
// We can perform a ray hit-test to find the position. The ray starts at the camera
// position and shoots forward (-z direction).
var cameraGameObject = (CameraObject)_gameObjectService.Objects["Camera"];
var cameraNode = cameraGameObject.CameraNode;
Vector3F cameraPosition = cameraNode.PoseWorld.Position;
Vector3F cameraDirection = cameraNode.PoseWorld.ToWorldDirection(Vector3F.Forward);
// Create a ray for picking.
RayShape ray = new RayShape(cameraPosition, cameraDirection, 1000);
// The ray should stop at the first hit. We only want the first object.
ray.StopsAtFirstHit = true;
// The collision detection requires a CollisionObject.
CollisionObject rayCollisionObject = new CollisionObject(new GeometricObject(ray, Pose.Identity));
// Assign the collision object to collision group 2. (In SampleGame.cs a
// collision filter based on collision groups was set. Objects for hit-testing
// are in group 2.)
rayCollisionObject.CollisionGroup = 2;
// Get the first object that has contact with the ray.
ContactSet contactSet = _simulation.CollisionDomain.GetContacts(rayCollisionObject).FirstOrDefault();
if (contactSet != null && contactSet.Count > 0)
{
// The ray has hit something.
// The contact set contains all detected contacts between the ray and the rigid body.
// Get the first contact in the contact set. (A ray hit usually contains exactly 1 contact.)
Contact contact = contactSet[0];
// The contact set contains the object pair of the collision. One object is the ray.
// The other is the object we want to grab.
CollisionObject hitCollisionObject = (contactSet.ObjectA == rayCollisionObject) ? contactSet.ObjectB : contactSet.ObjectA;
// Check whether a dynamic rigid body was hit.
RigidBody hitBody = hitCollisionObject.GeometricObject as RigidBody;
if (hitBody != null && hitBody.MotionType == MotionType.Dynamic)
{
// Attach the rigid body at the cursor position using a ball-socket joint.
// (Note: We could also use a FixedJoint, if we don't want any rotations.)
// The penetration depth tells us the distance from the ray origin to the rigid body.
_springAttachmentDistanceFromObserver = contact.PenetrationDepth;
// Get the position where the ray hits the other object.
// (The position is defined in the local space of the object.)
Vector3F hitPositionLocal = (contactSet.ObjectA == rayCollisionObject) ? contact.PositionBLocal : contact.PositionALocal;
_spring = new BallJoint
{
BodyA = hitBody,
AnchorPositionALocal = hitPositionLocal,
// We need to attach the grabbed object to a second body. In this case we just want to
// anchor the object at a specific point in the world. To achieve this we can use the
// special rigid body "World", which is defined in the simulation.
BodyB = _simulation.World,
// AnchorPositionBLocal is set below.
// Some constraint adjustments.
ErrorReduction = 0.3f,
// We set a softness > 0. This makes the joint "soft" and it will act like
// damped spring.
Softness = 0.00001f,
// We limit the maximal force. This reduces the ability of this joint to violate
// other constraints.
MaxForce = 1e6f
};
// Add the spring to the simulation.
_simulation.Constraints.Add(_spring);
}
}
}
if (_spring != null)
{
// User has grabbed something.
// Update the position of the object by updating the anchor position of
// the ball-socket joint.
var cameraGameObject = (CameraObject)_gameObjectService.Objects["Camera"];
var cameraNode = cameraGameObject.CameraNode;
Vector3F cameraPosition = cameraNode.PoseWorld.Position;
Vector3F cameraDirection = cameraNode.PoseWorld.ToWorldDirection(-Vector3F.UnitZ);
_spring.AnchorPositionBLocal = cameraPosition + cameraDirection * _springAttachmentDistanceFromObserver;
// Reduce the angular velocity by a certain factor. (This acts like a damping because we
// do not want the object to rotate like crazy.)
_spring.BodyA.AngularVelocity *= 0.9f;
}
}
private static void UpdateWheelContactInfo(ConstraintWheel wheel, ContactSet contactSet)
{
if (contactSet != null && contactSet.HaveContact && contactSet.Count > 0)
{
// ----- Ray has contact.
var contact = contactSet[0];
wheel.HasGroundContact = true;
wheel.GroundPosition = contact.Position;
if (wheel.CollisionObject == contactSet.ObjectA)
{
wheel.GroundNormal = -contact.Normal;
wheel.TouchedBody = contactSet.ObjectB.GeometricObject as RigidBody;
}
else
{
wheel.GroundNormal = contact.Normal;
wheel.TouchedBody = contactSet.ObjectA.GeometricObject as RigidBody;
}
// If the ray is nearly parallel to the ground, then the contact is not
// useful and we ignore it.
Vector3F up = wheel.Vehicle.Chassis.Pose.Orientation.GetColumn(1);
float normalDotUp = Vector3F.Dot(wheel.GroundNormal, up);
if (Numeric.IsGreater(normalDotUp, 0))
{
wheel.Tag = 1; // Tag = 1 means that this wheel has a useful ground contact.
float hitDistance = contact.PenetrationDepth;
wheel.SuspensionLength = Math.Max(hitDistance - wheel.Radius, wheel.MinSuspensionLength);
wheel.Constraint.BodyB = wheel.TouchedBody ?? wheel.Vehicle.Simulation.World;
wheel.Constraint.Enabled = true;
wheel.GroundRight = wheel.Vehicle.Chassis.Pose.ToWorldDirection(Matrix33F.CreateRotationY(wheel.SteeringAngle) * Vector3F.UnitX);
wheel.GroundForward = Vector3F.Cross(wheel.GroundNormal, wheel.GroundRight).Normalized;
}
}
else
{
// ----- The wheel is in the air.
wheel.Constraint.Enabled = false;
wheel.Constraint.BodyB = wheel.Vehicle.Simulation.World;
wheel.HasGroundContact = false;
wheel.TouchedBody = null;
wheel.SuspensionLength = wheel.SuspensionRestLength;
}
}
/// <summary>
/// Performs a collision query to update the contact information in the contact set.
/// </summary>
/// <param name="contactSet">The contact set.</param>
/// <param name="deltaTime">
/// The time step size in seconds. (The elapsed simulation time since the contact set was
/// updated the last time.)
/// </param>
/// <remarks>
/// <para>
/// This method updates contact information stored in the given contact set. This method is
/// usually faster than <see cref="GetContacts"/> because the information in
/// <paramref name="contactSet"/> is reused and updated.
/// </para>
/// <para>
/// The life time counter of persistent contacts is increased.
/// </para>
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="contactSet"/> is <see langword="null"/>.
/// </exception>
public void UpdateContacts(ContactSet contactSet, float deltaTime)
{
if (contactSet == null)
{
throw new ArgumentNullException("contactSet");
}
// Remove separated contacts - the contact set could contain separated
// closest points of a GetClosestPoints query.
ContactHelper.RemoveSeparatedContacts(contactSet);
// Update cached information.
ContactHelper.UpdateContacts(contactSet, deltaTime, CollisionDetection.ContactPositionTolerance);
// We do not compute the detailed contact information for triggers.
bool ignoreContactInfo = (contactSet.ObjectA.Type == CollisionObjectType.Trigger ||
contactSet.ObjectB.Type == CollisionObjectType.Trigger);
// We have to make the full contact query if one object is a ray with StopsAtFirstHit because
// we need the PenetrationDepth for sorting the hits.
if (contactSet.ObjectA.IsRayThatStopsAtFirstHit || contactSet.ObjectB.IsRayThatStopsAtFirstHit)
{
ignoreContactInfo = false;
}
if (ignoreContactInfo)
{
// Check cached flag. If necessary, make only boolean check.
if (contactSet.HaveContact == false || contactSet.Count == 0)
{
ComputeCollision(contactSet, CollisionQueryType.Boolean);
}
}
else
{
// Compute new contact info.
ComputeCollision(contactSet, CollisionQueryType.Contacts);
}
if (contactSet.HaveContact == false)
{
// No contact: Remove old contacts if objects do not touch.
foreach (var contact in contactSet)
{
contact.Recycle();
}
contactSet.Clear();
}
else
{
// Reduce ray cast results to 1 contact.
if (contactSet.ObjectA.IsRay || contactSet.ObjectB.IsRay)
{
ContactHelper.ReduceRayHits(contactSet);
}
// We have contact: Call contact filter.
if (CollisionDetection.ContactFilter != null)
{
CollisionDetection.ContactFilter.Filter(contactSet);
}
}
CheckResult(contactSet, false);
contactSet.IsValid = true;
}
/// <summary>
/// Updates the contact geometry of a contact set.
/// </summary>
/// <param name="contactSet">The contact set.</param>
/// <param name="deltaTime">
/// The time step in seconds. (The simulation time that has elapsed since the last time that an
/// Update-method was called.)
/// </param>
/// <param name="contactPositionTolerance">The contact position tolerance.</param>
/// <remarks>
/// <para>
/// The objects can still move relative to each other. This method updates the contact
/// information if the objects have moved. The <see cref="Contact.Lifetime"/> of persistent
/// contacts is increased. Invalid contacts are removed. Closest point pairs will be removed if
/// they start touching. Penetrating or touching contacts are removed if the objects have moved
/// more than <paramref name="contactPositionTolerance"/> or the contacts have separated.
/// </para>
/// <para>
/// Note: Only the info of the cached contacts is updated. New contacts are not discovered in
/// this method.
/// </para>
/// </remarks>
internal static void UpdateContacts(ContactSet contactSet, float deltaTime, float contactPositionTolerance)
{
Debug.Assert(contactPositionTolerance >= 0, "The contact position tolerance must be greater than or equal to 0");
int numberOfContacts = contactSet.Count;
if (numberOfContacts == 0)
return;
if (contactSet.ObjectA.Changed || contactSet.ObjectB.Changed)
{
// Objects have moved.
for (int i = numberOfContacts - 1; i >= 0; i--) // Reverse loop, because contacts might be removed.
{
Contact contact = contactSet[i];
// Increase Lifetime.
contact.Lifetime += deltaTime;
// Update all contacts and remove invalid contacts.
bool shouldRemove = UpdateContact(contactSet, contact, contactPositionTolerance);
if (shouldRemove)
{
contactSet.RemoveAt(i);
contact.Recycle();
}
}
}
else
{
// Nothing has moved.
for (int i = 0; i < numberOfContacts; i++)
{
// Increase Lifetime.
contactSet[i].Lifetime += deltaTime;
}
}
}
public static void Merge(ContactSet target, ContactSet newContacts, CollisionQueryType type, float contactPositionTolerance)
{
Debug.Assert(target != null);
Debug.Assert(newContacts != null);
Debug.Assert(contactPositionTolerance >= 0, "The contact position tolerance must be greater than or equal to 0");
int numberOfNewContacts = newContacts.Count;
for (int i = 0; i < numberOfNewContacts; i++)
Merge(target, newContacts[i], type, contactPositionTolerance);
newContacts.Clear();
}
/// <summary>
/// Removes separated contacts.
/// </summary>
/// <param name="contactSet">The contact set.</param>
internal static void RemoveSeparatedContacts(ContactSet contactSet)
{
for (int i = contactSet.Count - 1; i >= 0; i--)
{
Contact contact = contactSet[i];
if (contact.PenetrationDepth < 0)
{
contactSet.RemoveAt(i);
contact.Recycle();
}
}
}
/// <summary>
/// Performs more collision tests while slightly rotating one collision object.
/// </summary>
/// <param name="collisionDetection">The collision detection.</param>
/// <param name="contactSet">
/// The contact set; must contain at least 1 <see cref="Contact"/>.
/// </param>
/// <param name="perturbB">
/// if set to <see langword="true"/> collision object B will be rotated; otherwise collision
/// object A will be rotated.
/// </param>
/// <param name="testMethod">The test method that is called to compute contacts.</param>
/// <remarks>
/// This method rotates one object 3 times and calls contact computation for the new
/// orientations. It is recommended to call this method only when the contact set has 1 new
/// contact.
/// </remarks>
internal static void TestWithPerturbations(CollisionDetection collisionDetection, ContactSet contactSet, bool perturbB, Action<ContactSet> testMethod)
{
Debug.Assert(contactSet != null);
Debug.Assert(contactSet.Count > 0 && contactSet.HaveContact || !contactSet.IsPerturbationTestAllowed);
Debug.Assert(testMethod != null);
// Make this test only if there is 1 contact.
// If there are 0 contacts, we assume that the contact pair is separated.
// If there are more than 3 contacts, then we already have a lot of contacts to work with, no
// need to search for more.
if (!contactSet.HaveContact || contactSet.Count == 0 || contactSet.Count >= 4 || !contactSet.IsPerturbationTestAllowed)
return;
// Get data of object that will be rotated.
var collisionObject = (perturbB) ? contactSet.ObjectB : contactSet.ObjectA;
var geometricObject = collisionObject.GeometricObject;
var pose = geometricObject.Pose;
// Get normal, pointing to the test object.
var normal = contactSet[0].Normal;
if (!perturbB)
normal = -normal;
var contactPosition = contactSet[0].Position;
var centerToContact = contactPosition - pose.Position;
// Compute a perturbation angle proportional to the dimension of the object.
var radius = geometricObject.Aabb.Extent.Length;
var angle = collisionDetection.ContactPositionTolerance / radius;
// axis1 is in the contact tangent plane, orthogonal to normal.
var axis1 = Vector3F.Cross(normal, centerToContact);
// If axis1 is zero then normal and centerToContact are collinear. This happens
// for example for spheres or cone tips against flat faces. In these cases we assume
// that there will be max. 1 contact.
if (axis1.IsNumericallyZero)
return;
var axis1Local = pose.ToLocalDirection(axis1);
var rotation = Matrix33F.CreateRotation(axis1Local, -angle);
// Use temporary test objects.
var testGeometricObject = TestGeometricObject.Create();
testGeometricObject.Shape = geometricObject.Shape;
testGeometricObject.Scale = geometricObject.Scale;
testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation);
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObject.SetInternal(collisionObject, testGeometricObject);
var testContactSet = perturbB ? ContactSet.Create(contactSet.ObjectA, testCollisionObject)
: ContactSet.Create(testCollisionObject, contactSet.ObjectB);
testContactSet.IsPerturbationTestAllowed = false; // Avoid recursive perturbation tests!
testContactSet.PreferredNormal = contactSet.PreferredNormal;
// Compute next contacts.
testMethod(testContactSet);
if (testContactSet.Count > 0)
{
// axis2 is in the contact tangent plane, orthogonal to axis1.
var axis2 = Vector3F.Cross(axis1, normal);
var axis2Local = pose.ToLocalDirection(axis2);
var rotation2 = Matrix33F.CreateRotation(axis2Local, -angle);
testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation2);
// Compute next contacts.
testMethod(testContactSet);
// Invert rotation2.
rotation2.Transpose();
testGeometricObject.Pose = new Pose(pose.Position, pose.Orientation * rotation2);
// Compute next contacts.
testMethod(testContactSet);
}
// Set HaveContact. It is reset when a perturbation separates the objects.
testContactSet.HaveContact = true;
// TODO: Test if we need this:
// The contact world positions are not really correct because one object was rotated.
// UpdateContacts recomputes the world positions from the local positions.
UpdateContacts(testContactSet, 0, collisionDetection.ContactPositionTolerance);
// Merge contacts of testContactSet into contact set, but do not change existing contacts.
foreach (var contact in testContactSet)
{
// We call TryMerge() to see if the contact is similar to an existing contact.
bool exists = TryMergeWithNearestContact(
contactSet,
contact,
collisionDetection.ContactPositionTolerance,
false); // The existing contact must no be changed!
if (exists)
{
// We can throw away the new contact because a similar is already in the contact set.
contact.Recycle();
}
else
{
// Add new contact.
contactSet.Add(contact);
}
}
// Recycle temporary objects.
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
testGeometricObject.Recycle();
}
/// <summary>
/// Reduces the number of contacts in the contact set to 1 contact.
/// </summary>
/// <param name="contactSet">
/// The contact set. One shape in the contact set must be a <see cref="RayShape"/>!
/// </param>
/// <remarks>
/// The best ray hit is kept.
/// </remarks>
internal static void ReduceRayHits(ContactSet contactSet)
{
Debug.Assert(
contactSet.ObjectA.GeometricObject.Shape is RayShape
|| contactSet.ObjectB.GeometricObject.Shape is RayShape,
"ReduceRayHits was called for a contact set without a ray.");
// For separated contacts keep the contact with the smallest separation.
// If we have contact, keep the contact with the SMALLEST penetration (= shortest ray length)
// and remove all invalid contacts (with separation).
bool haveContact = contactSet.HaveContact;
float bestPenetrationDepth = haveContact ? float.PositiveInfinity : float.NegativeInfinity;
Contact bestContact = null;
int numberOfContacts = contactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = contactSet[i];
float penetrationDepth = contact.PenetrationDepth;
if (haveContact)
{
// Search for positive and smallest penetration depth.
if (penetrationDepth >= 0 && penetrationDepth < bestPenetrationDepth)
{
bestContact = contact;
bestPenetrationDepth = penetrationDepth;
}
}
else
{
// Search for negative and largest penetration depth (Separation!)
Debug.Assert(penetrationDepth < 0, "HaveContact is false, but contact shows penetration.");
if (penetrationDepth > bestPenetrationDepth)
{
bestContact = contact;
bestPenetrationDepth = penetrationDepth;
}
}
}
// Keep best contact.
// Note: In some situations HaveContact is true, but the contact set does not contains any
// contacts with penetration. This happen, for example, when testing a ray inside a triangle
// mesh. The TriangleMeshAlgorithm automatically filters contacts with bad normals.
// When HaveContact is false, we should always have a contact (closest point).
// Throw away other contacts.
foreach (var contact in contactSet)
if (contact != bestContact)
contact.Recycle();
contactSet.Clear();
if (bestContact != null)
contactSet.Add(bestContact);
}
/// <summary>
/// Reduces the number of contacts in the contact set to 1 contact.
/// </summary>
/// <param name="contactSet">The contact set.</param>
/// <remarks>
/// The contact with the biggest penetration depth is kept.
/// </remarks>
internal static void ReduceClosestPoints(ContactSet contactSet)
{
// Reduce to 1 contact.
int numberOfContacts = contactSet.Count;
if (numberOfContacts > 1)
{
// Keep the contact with the deepest penetration depth.
Contact bestContact = contactSet[0];
for (int i = 1; i < numberOfContacts; i++)
{
if (contactSet[i].PenetrationDepth > bestContact.PenetrationDepth)
{
bestContact = contactSet[i];
}
}
// Throw away other contacts.
foreach (var contact in contactSet)
if (contact != bestContact)
contact.Recycle();
contactSet.Clear();
contactSet.Add(bestContact);
}
Debug.Assert(contactSet.Count == 0 || contactSet.Count == 1);
// If we HaveContact but the contact shows a separation, delete contact.
// This can happen for TriangleMesh vs. TriangleMesh because the triangle mesh algorithm
// filters contacts if they have a bad normal. It can happen that all contacts are filtered
// and only a separated contact remains in the contact set.
if (contactSet.HaveContact && contactSet.Count > 0 && contactSet[0].PenetrationDepth < 0)
{
contactSet[0].Recycle();
contactSet.Clear();
}
}
public static void RemoveBadContacts(ContactSet contactSet, Vector3F normal, float minDotProduct)
{
for (int i = contactSet.Count - 1; i >= 0; i--)
{
Contact contact = contactSet[i];
Vector3F contactNormal = contact.Normal;
// float dot = Vector3F.Dot(contactNormal, normal);
// ----- Optimized version:
float dot = contactNormal.X * normal.X + contactNormal.Y * normal.Y + contactNormal.Z * normal.Z;
if (dot < minDotProduct)
{
contactSet.RemoveAt(i);
contact.Recycle();
}
}
}
// Compute contacts between a shape and the child shapes of a <see cref="CompositeShape"/>.
// testXxx are initialized objects which are re-used to avoid a lot of GC garbage.
private void AddChildContacts(ContactSet contactSet,
bool swapped,
int childIndex,
CollisionQueryType type,
ContactSet testContactSet,
CollisionObject testCollisionObject,
TestGeometricObject testGeometricObject)
{
// This method is taken from CompositeShapeAlgorithm.cs and slightly modified. Keep changes
// in sync with CompositeShapeAlgorithm.cs!
// !!! Object A should be the composite. - This is different then in ComputeContacts() above!!!
CollisionObject collisionObjectA = (swapped) ? contactSet.ObjectB : contactSet.ObjectA;
CollisionObject collisionObjectB = (swapped) ? contactSet.ObjectA : contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
Vector3F scaleA = geometricObjectA.Scale;
IGeometricObject childA = ((CompositeShape)geometricObjectA.Shape).Children[childIndex];
// Find collision algorithm.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[childA, geometricObjectB];
// ----- Set the shape temporarily to the current child.
// (Note: The scaling is either uniform or the child has no local rotation. Therefore, we only
// need to apply the scale of the parent to the scale and translation of the child. We can
// ignore the rotation.)
Debug.Assert(
(scaleA.X == scaleA.Y && scaleA.Y == scaleA.Z) || !childA.Pose.HasRotation,
"CompositeShapeAlgorithm should have thrown an exception. Non-uniform scaling is not supported for rotated children.");
var childPose = childA.Pose;
childPose.Position *= scaleA; // Apply scaling to local translation.
testGeometricObject.Pose = geometricObjectA.Pose * childPose;
testGeometricObject.Shape = childA.Shape;
testGeometricObject.Scale = scaleA * childA.Scale; // Apply scaling to local scale.
testCollisionObject.SetInternal(collisionObjectA, testGeometricObject);
// Create a temporary contact set.
// (ObjectA and ObjectB should have the same order as in contactSet; otherwise we couldn't
// simply merge them.)
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
if (swapped)
{
testContactSet.Reset(collisionObjectB, testCollisionObject);
}
else
{
testContactSet.Reset(testCollisionObject, collisionObjectB);
}
if (type == CollisionQueryType.Boolean)
{
// Boolean queries.
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
}
else
{
// No perturbation test. Most composite shapes are either complex and automatically
// have more contacts. Or they are complex and will not be used for stacking
// where full contact sets would be needed.
testContactSet.IsPerturbationTestAllowed = false;
// Make collision check. As soon as we have found contact, we can make faster
// contact queries instead of closest-point queries.
CollisionQueryType queryType = (contactSet.HaveContact) ? CollisionQueryType.Contacts : type;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
// Transform contacts into space of composite shape.
// And set the shape feature of the contact.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
if (swapped)
{
contact.PositionBLocal = childPose.ToWorldPosition(contact.PositionBLocal);
contact.FeatureB = childIndex;
}
else
{
contact.PositionALocal = childPose.ToWorldPosition(contact.PositionALocal);
contact.FeatureA = childIndex;
}
}
// Merge child contacts.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
}
/// <summary>
/// Tries to the merge the contact with the nearest contact in the given
/// <see cref="ContactSet"/>.
/// </summary>
/// <param name="contactSet">The contact set. (Must not be <see langword="null"/>.)</param>
/// <param name="contact">The contact. (Must not be <see langword="null"/>.)</param>
/// <param name="contactPositionTolerance">The contact position tolerance.</param>
/// <param name="updateMerged">
/// If set to <see langword="true"/> the merged contact is updated with the data of
/// <paramref name="contact"/>. If set to <see langword="false"/> the merged contact keeps the
/// data of the old contact.
/// </param>
/// <returns>
/// <see langword="true"/> if the contact was merged successfully; otherwise
/// <see langword="false"/>.
/// </returns>
private static bool TryMergeWithNearestContact(ContactSet contactSet, Contact contact, float contactPositionTolerance, bool updateMerged)
{
Debug.Assert(contactPositionTolerance >= 0, "The contact position tolerance must be greater than or equal to 0");
Debug.Assert(contactSet.Count > 0, "Cannot merge nearest contact to empty contact set.");
// Find the cached contact that is closest.
int nearestContactIndex = -1;
// Near contact must be within contact position tolerance.
float minDistance = contactPositionTolerance + Numeric.EpsilonF;
float minDistanceSquared = minDistance * minDistance;
int numberOfContacts = contactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact otherContact = contactSet[i];
// Do not merge contacts with different features because user should be notified by
// a new contact that a new feature is touched.
if (contact.FeatureA == otherContact.FeatureA && contact.FeatureB == otherContact.FeatureB)
{
// Check position difference.
float distanceSquared = (otherContact.Position - contact.Position).LengthSquared;
if (distanceSquared < minDistanceSquared)
{
minDistanceSquared = distanceSquared;
nearestContactIndex = i;
}
}
}
if (nearestContactIndex >= 0)
{
if (updateMerged)
{
// Merge with an existing contact.
// We take the geometry of the new contact and keep the other data of the old contact.
// We also keep the old contact, so that references to this contact stay valid.
Contact nearestContact = contactSet[nearestContactIndex];
nearestContact.IsRayHit = contact.IsRayHit;
nearestContact.PositionALocal = contact.PositionALocal;
nearestContact.PositionBLocal = contact.PositionBLocal;
nearestContact.Normal = contact.Normal;
nearestContact.PenetrationDepth = contact.PenetrationDepth;
nearestContact.Position = contact.Position;
}
return true;
}
return false;
}
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);
}
/// <summary>
/// Updates the contact geometry for a single contact.
/// </summary>
/// <param name="contactSet">The contact set.</param>
/// <param name="contact">The contact to be updated.</param>
/// <param name="contactPositionTolerance">The contact position tolerance.</param>
/// <returns>
/// <see langword="true"/> if the contact is invalid and should be removed.
/// </returns>
private static bool UpdateContact(ContactSet contactSet, Contact contact, float contactPositionTolerance)
{
Pose poseA = contactSet.ObjectA.GeometricObject.Pose;
Pose poseB = contactSet.ObjectB.GeometricObject.Pose;
// Get local positions in world space.
//Vector3F positionA = poseA.ToWorldPosition(contact.PositionALocal);
//Vector3F positionB = poseB.ToWorldPosition(contact.PositionBLocal);
// ----- Optimized version:
Vector3F positionALocal = contact.PositionALocal;
Vector3F positionA;
positionA.X = poseA.Orientation.M00 * positionALocal.X + poseA.Orientation.M01 * positionALocal.Y + poseA.Orientation.M02 * positionALocal.Z + poseA.Position.X;
positionA.Y = poseA.Orientation.M10 * positionALocal.X + poseA.Orientation.M11 * positionALocal.Y + poseA.Orientation.M12 * positionALocal.Z + poseA.Position.Y;
positionA.Z = poseA.Orientation.M20 * positionALocal.X + poseA.Orientation.M21 * positionALocal.Y + poseA.Orientation.M22 * positionALocal.Z + poseA.Position.Z;
Vector3F positionBLocal = contact.PositionBLocal;
Vector3F positionB;
positionB.X = poseB.Orientation.M00 * positionBLocal.X + poseB.Orientation.M01 * positionBLocal.Y + poseB.Orientation.M02 * positionBLocal.Z + poseB.Position.X;
positionB.Y = poseB.Orientation.M10 * positionBLocal.X + poseB.Orientation.M11 * positionBLocal.Y + poseB.Orientation.M12 * positionBLocal.Z + poseB.Position.Y;
positionB.Z = poseB.Orientation.M20 * positionBLocal.X + poseB.Orientation.M21 * positionBLocal.Y + poseB.Orientation.M22 * positionBLocal.Z + poseB.Position.Z;
// Update Position.
contact.Position = (positionA + positionB) / 2;
// Update contacts and closest points differently:
if (contact.PenetrationDepth >= 0)
{
// ----- Contact.
Vector3F bToA = positionA - positionB; // Vector from contact on A to contact on B
if (!contact.IsRayHit)
{
// ----- Normal contact.
// Update penetration depth: Difference of world position projected onto normal.
//contact.PenetrationDepth = Vector3F.Dot(bToA, contact.Normal);
// ----- Optimized version:
Vector3F contactNormal = contact.Normal;
contact.PenetrationDepth = bToA.X * contactNormal.X + bToA.Y * contactNormal.Y + bToA.Z * contactNormal.Z;
}
else
{
// ----- Ray hit.
// Update penetration depth: Contact position to ray origin projected onto ray direction.
// Get ray. Only one shape is a ray because ray vs. ray do normally not collide.
RayShape ray = contactSet.ObjectA.GeometricObject.Shape as RayShape;
float rayScale = contactSet.ObjectA.GeometricObject.Scale.X; // Non-uniformly scaled rays are not support, so we only need Scale.X!
Vector3F hitPositionLocal; // Hit position in local space of ray.
if (ray != null)
{
hitPositionLocal = poseA.ToLocalPosition(contact.Position);
}
else
{
// The other object must be the ray.
ray = contactSet.ObjectB.GeometricObject.Shape as RayShape;
rayScale = contactSet.ObjectB.GeometricObject.Scale.X; // Non-uniformly scaled rays are not support, so we only need Scale.X!
hitPositionLocal = poseB.ToLocalPosition(contact.Position);
}
// Now, we have found the ray, unless there is a composite shape with a child ray - which
// is not supported.
if (ray != null)
{
contact.PenetrationDepth = Vector3F.Dot(hitPositionLocal - ray.Origin * rayScale, ray.Direction);
// If the new penetration depth is negative or greater than the ray length,
// the objects have separated along the ray direction.
if (contact.PenetrationDepth < 0 || contact.PenetrationDepth > ray.Length * rayScale)
return true;
}
}
// Remove points with negative penetration depth.
if (contact.PenetrationDepth < 0)
return true;
// Check drift.
float driftSquared;
if (contact.IsRayHit)
{
// For ray casts: Remove contact if movement in any direction is too large.
driftSquared = bToA.LengthSquared;
}
else
{
// For contacts: Remove contacts if horizontal movement (perpendicular to contact normal)
// is too large.
driftSquared = (bToA - contact.Normal * contact.PenetrationDepth).LengthSquared;
}
// Remove contact if drift is too large.
return driftSquared > contactPositionTolerance * contactPositionTolerance;
}
else
{
// ----- Closest point pair.
// Update distance. Since we do not check the geometric objects, the new distance
// could be a separation or a penetration. We assume it is a separation and
// use a "-" sign.
// We have no problem if we are wrong and this is actually a penetration because this
// contact is automatically updated or removed when new contacts are computed in
// the narrow phase.
Vector3F aToB = positionB - positionA; // Vector from contact on A to contact on B
contact.PenetrationDepth = -aToB.Length;
// If points moved into contact, remove this pair, because we don't have a valid
// contact normal.
if (Numeric.IsZero(contact.PenetrationDepth))
return true;
// Update normal.
contact.Normal = aToB.Normalized;
return false;
}
}
/// <summary>
/// Filters the specified contact set.
/// </summary>
/// <param name="contactSet">The contact set.</param>
public void Filter(ContactSet contactSet)
{
// Call the default contact filter.
DefaultContactFilter.Filter(contactSet);
// Abort if there are no contacts in this contact set.
if (contactSet.Count == 0)
return;
// If this is a sphere vs. * contact set, then we correct the position of the
// contact point to make sure that the contact position is in line with the sphere center.
var sphere = contactSet.ObjectA.GeometricObject.Shape as SphereShape;
if (sphere != null)
{
float radius = sphere.Radius;
foreach (var contact in contactSet)
contact.Position = contactSet.ObjectA.GeometricObject.Pose.Position + contact.Normal * (radius - contact.PenetrationDepth / 2);
return;
}
sphere = contactSet.ObjectB.GeometricObject.Shape as SphereShape;
if (sphere != null)
{
float radius = sphere.Radius;
foreach (var contact in contactSet)
contact.Position = contactSet.ObjectB.GeometricObject.Pose.Position - contact.Normal * (radius - contact.PenetrationDepth / 2);
}
}
public static Contact CreateContact(ContactSet contactSet,
Vector3F position,
Vector3F normal,
float penetrationDepth,
bool isRayHit)
{
Debug.Assert(contactSet != null);
return CreateContact(contactSet.ObjectA, contactSet.ObjectB, position, normal, penetrationDepth, isRayHit);
}
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();
}
}
public override void Update(GameTime gameTime)
{
var debugRenderer = GraphicsScreen.DebugRenderer;
debugRenderer.Clear();
// Draw coordinate cross at world origin.
debugRenderer.DrawAxes(Pose.Identity, 10, false);
// Draw objects.
debugRenderer.DrawObject(_objectA.GeometricObject, GraphicsHelper.GetUniqueColor(_objectA), _drawWireframe, false);
debugRenderer.DrawObject(_objectB.GeometricObject, GraphicsHelper.GetUniqueColor(_objectB), _drawWireframe, false);
// Draw contact info.
debugRenderer.DrawContacts(_contactSet, 0.1f, null, true);
// Toggle wireframe rendering.
if (InputService.IsPressed(Keys.Space, true))
_drawWireframe = !_drawWireframe;
// Change background color if we have a contact.
GraphicsScreen.BackgroundColor = (_contactSet != null && _contactSet.HaveContact)
? new Color(220, 200, 200, 255)
: new Color(200, 220, 200, 255);
// Move one object with keyboard NumPad.
var translation = new Vector3F();
if (InputService.IsDown(Keys.NumPad4))
translation.X -= 1;
if (InputService.IsDown(Keys.NumPad6))
translation.X += 1;
if (InputService.IsDown(Keys.NumPad8))
translation.Y += 1;
if (InputService.IsDown(Keys.NumPad5))
translation.Y -= 1;
if (InputService.IsDown(Keys.NumPad7))
translation.Z -= 1;
if (InputService.IsDown(Keys.NumPad9))
translation.Z += 1;
if (!translation.IsNumericallyZero)
{
var go = (GeometricObject)_objectA.GeometricObject;
float scale = go.Aabb.Extent.Length * 0.1f;
translation *= scale * (float)gameTime.ElapsedGameTime.TotalSeconds;
go.Pose = new Pose(go.Pose.Position + translation, go.Pose.Orientation);
if (_contactSet != null)
_collisionDetection.UpdateContacts(_contactSet, 0.001f);
else
_contactSet = _collisionDetection.GetContacts(_objectA, _objectB);
}
}
/// <summary> /// Computes the collision. - This method should only be used by /// <see cref="CollisionAlgorithm"/> instances! /// </summary> /// <param name="contactSet">The contact set.</param> /// <param name="type">The type of collision query.</param> /// <remarks> /// <para> /// This method does the real work. It is called from the other public methods of a /// <see cref="CollisionAlgorithm"/>. Also, if one <see cref="CollisionAlgorithm"/> uses another /// <see cref="CollisionAlgorithm"/> internally, this method should be called directly instead /// of <see cref="CollisionAlgorithm.UpdateClosestPoints"/> or /// <see cref="CollisionAlgorithm.UpdateContacts"/>. /// </para> /// <para> /// <strong>Notes to Inheritors:</strong> This is the central method which has to be implemented /// in derived classes. <paramref name="contactSet"/> is never <see langword="null"/>. This /// method has to add new contact/closest-point info with /// <see cref="ContactHelper.Merge(ContactSet,Contact,CollisionQueryType,float)"/>. It is not /// necessary to remove old contacts. At the beginning of the method /// <see cref="ContactSet.HaveContact"/> in <paramref name="contactSet"/> indicates the result /// of the last narrow phase algorithm that was run on <paramref name="contactSet"/>. This /// method must set <see cref="ContactSet.HaveContact"/> to <see langword="false"/> if it /// doesn't find a contact or to <see langword="true"/> if it finds a contact. /// </para> /// </remarks> public abstract void ComputeCollision(ContactSet contactSet, CollisionQueryType type);
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
if (type == CollisionQueryType.ClosestPoints)
{
// Just use normal composite shape algorithm.
_triangleMeshAlgorithm.ComputeCollision(contactSet, type);
return;
}
Debug.Assert(type != CollisionQueryType.ClosestPoints, "Closest point queries should have already been handled!");
// Mesh = A, Ray = B
IGeometricObject meshObject = contactSet.ObjectA.GeometricObject;
IGeometricObject rayObject = contactSet.ObjectB.GeometricObject;
// Object A should be the mesh, swap objects if necessary.
bool swapped = (meshObject.Shape is RayShape);
if (swapped)
MathHelper.Swap(ref rayObject, ref meshObject);
RayShape rayShape = rayObject.Shape as RayShape;
TriangleMeshShape meshShape = meshObject.Shape as TriangleMeshShape;
// Check if shapes are correct.
if (rayShape == null || meshShape == null)
throw new ArgumentException("The contact set must contain a ray and a triangle mesh shape.", "contactSet");
// Assume no contact.
contactSet.HaveContact = false;
// Get transformations.
Vector3F rayScale = rayObject.Scale;
Pose rayPose = rayObject.Pose;
Vector3F meshScale = meshObject.Scale;
Pose meshPose = meshObject.Pose;
// Ray in world space.
Ray rayWorld = new Ray(rayShape);
rayWorld.Scale(ref rayScale); // Scale ray.
rayWorld.ToWorld(ref rayPose); // Transform ray to world space.
// Ray in local scaled space of the mesh.
Ray ray = rayWorld;
ray.ToLocal(ref meshPose); // Transform ray to local space of composite.
// Ray in local unscaled space of the mesh.
Ray rayUnscaled = ray;
var inverseCompositeScale = Vector3F.One / meshScale;
rayUnscaled.Scale(ref inverseCompositeScale);
ITriangleMesh triangleMesh = meshShape.Mesh;
bool isTwoSided = meshShape.IsTwoSided;
if (meshShape.Partition != null)
{
// ----- Mesh with BVH vs. Ray -----
foreach (var childIndex in meshShape.Partition.GetOverlaps(rayUnscaled))
{
Triangle triangle = triangleMesh.GetTriangle(childIndex);
AddContact(contactSet, swapped, type, ref rayWorld, ref ray, ref triangle, childIndex, ref meshPose, ref meshScale, isTwoSided);
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
break; // We can abort early.
}
}
else
{
// ----- Mesh vs. Ray -----
var rayUnscaledDirectionInverse = new Vector3F(
1 / rayUnscaled.Direction.X,
1 / rayUnscaled.Direction.Y,
1 / rayUnscaled.Direction.Z);
float epsilon = Numeric.EpsilonF * (1 + meshObject.Aabb.Extent.Length);
int numberOfTriangles = triangleMesh.NumberOfTriangles;
for (int i = 0; i < numberOfTriangles; i++)
{
Triangle triangle = triangleMesh.GetTriangle(i);
// Make ray vs AABB check first. We could skip this because the ray vs. triangle test
// is also fast. But experiments (ray vs sphere mesh) have shown that making an
// additional ray vs. AABB test first makes the worst case more than 20% faster.
if (GeometryHelper.HaveContact(triangle.Aabb, rayUnscaled.Origin, rayUnscaledDirectionInverse, rayUnscaled.Length, epsilon))
{
AddContact(contactSet, swapped, type, ref rayWorld, ref ray, ref triangle, i, ref meshPose, ref meshScale, isTwoSided);
// We have contact and stop for boolean queries.
if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
break;
}
}
}
}
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();
}
}
private void ComputeLineVsOther(ContactSet contactSet, CollisionQueryType type, bool objectAIsLine)
{
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
Shape shapeA = geometricObjectA.Shape;
Shape shapeB = geometricObjectB.Shape;
Debug.Assert(
shapeA is LineShape && !(shapeB is LineShape) ||
shapeB is LineShape && !(shapeA is LineShape),
"LineAlgorithm.ComputeLineVsOther should only be called for a line and another shape.");
CollisionObject lineCollisionObject;
IGeometricObject lineGeometricObject;
IGeometricObject otherGeometricObject;
LineShape lineShape;
Shape otherShape;
if (objectAIsLine)
{
lineCollisionObject = collisionObjectA;
lineGeometricObject = geometricObjectA;
lineShape = (LineShape)shapeA;
otherGeometricObject = geometricObjectB;
otherShape = shapeB;
}
else
{
lineCollisionObject = collisionObjectB;
lineGeometricObject = geometricObjectB;
lineShape = (LineShape)shapeB;
otherGeometricObject = geometricObjectA;
otherShape = shapeA;
}
// Apply scaling to line.
Line line = new Line(lineShape);
Vector3F lineScale = lineGeometricObject.Scale;
line.Scale(ref lineScale);
// Step 1: Get any bounding sphere that encloses the other object.
Aabb aabb = otherGeometricObject.Aabb;
Vector3F center = (aabb.Minimum + aabb.Maximum) / 2;
float radius = (aabb.Maximum - aabb.Minimum).Length; // A large safe radius. (Exact size does not matter.)
// Step 2: Get the closest point of line vs. center.
// All computations in local space of the line.
Vector3F closestPointOnLine;
Pose linePose = lineGeometricObject.Pose;
GeometryHelper.GetClosestPoint(line, linePose.ToLocalPosition(center), out closestPointOnLine);
// Step 3: Crop the line to a line segment that will contain the closest point.
var lineSegment = ResourcePools.LineSegmentShapes.Obtain();
lineSegment.Start = closestPointOnLine - line.Direction * radius;
lineSegment.End = closestPointOnLine + line.Direction * radius;
// Use temporary test objects.
var testGeometricObject = TestGeometricObject.Create();
testGeometricObject.Shape = lineSegment;
testGeometricObject.Scale = Vector3F.One;
testGeometricObject.Pose = linePose;
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObject.SetInternal(lineCollisionObject, testGeometricObject);
var testContactSet = objectAIsLine ? ContactSet.Create(testCollisionObject, collisionObjectB)
: ContactSet.Create(collisionObjectA, testCollisionObject);
testContactSet.IsPerturbationTestAllowed = contactSet.IsPerturbationTestAllowed;
// Step 4: Call another collision algorithm.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[lineSegment, otherShape];
// Step 5: Manually chosen preferred direction for MPR.
// For the MPR we choose the best ray direction ourselves. The ray should be normal
// to the line, otherwise MPR could try to push the line segment out of the other object
// in the line direction - this cannot work for infinite lines.
// Results without a manual MPR ray were ok for normal cases. Problems were only observed
// for cases where the InnerPoints overlap or for deep interpenetrations.
Vector3F v0A = geometricObjectA.Pose.ToWorldPosition(shapeA.InnerPoint * geometricObjectA.Scale);
Vector3F v0B = geometricObjectB.Pose.ToWorldPosition(shapeB.InnerPoint * geometricObjectB.Scale);
Vector3F n = v0B - v0A; // This is the default MPR ray direction.
// Make n normal to the line.
n = n - Vector3F.ProjectTo(n, linePose.ToWorldDirection(lineShape.Direction));
if (!n.TryNormalize())
{
n = lineShape.Direction.Orthonormal1;
}
testContactSet.PreferredNormal = n;
collisionAlgorithm.ComputeCollision(testContactSet, type);
if (testContactSet.HaveContact)
{
contactSet.HaveContact = true;
}
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
// Recycle temporary objects.
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
testGeometricObject.Recycle();
ResourcePools.LineSegmentShapes.Recycle(lineSegment);
}
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);
}
// See FAST paper: "Interactive Continuous Collision Detection for Non-Convex Polyhedra", Kim Young et al.
/// <summary>
/// Gets the time of impact using Conservative Advancement (ignoring rotational movement).
/// </summary>
/// <param name="objectA">The object A.</param>
/// <param name="targetPoseA">The target pose of A.</param>
/// <param name="objectB">The object B.</param>
/// <param name="targetPoseB">The target pose of B.</param>
/// <param name="allowedPenetration">The allowed penetration depth.</param>
/// <param name="collisionDetection">The collision detection.</param>
/// <returns>
/// The time of impact in the range [0, 1].
/// </returns>
/// <remarks>
/// This algorithm does not work for concave objects.
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="objectA"/>, <paramref name="objectB"/> or
/// <paramref name="collisionDetection"/> is <see langword="null"/>.
/// </exception>
internal static float GetTimeOfImpactLinearCA(CollisionObject objectA, Pose targetPoseA,
CollisionObject objectB, Pose targetPoseB, float allowedPenetration,
CollisionDetection collisionDetection) // Required for collision algorithm matrix.
{
if (objectA == null)
{
throw new ArgumentNullException("objectA");
}
if (objectB == null)
{
throw new ArgumentNullException("objectB");
}
if (collisionDetection == null)
{
throw new ArgumentNullException("collisionDetection");
}
IGeometricObject geometricObjectA = objectA.GeometricObject;
IGeometricObject geometricObjectB = objectB.GeometricObject;
Pose startPoseA = geometricObjectA.Pose;
Pose startPoseB = geometricObjectB.Pose;
// Compute relative linear velocity.
// (linearRelVel ∙ normal > 0 if objects are getting closer.)
Vector3F linearVelocityA = targetPoseA.Position - startPoseA.Position;
Vector3F linearVelocityB = targetPoseB.Position - startPoseB.Position;
Vector3F linearVelocityRelative = linearVelocityA - linearVelocityB;
// Abort if relative movement is zero.
if (Numeric.IsZero(linearVelocityRelative.Length))
{
return(1);
}
var distanceAlgorithm = collisionDetection.AlgorithmMatrix[objectA, objectB];
// Use temporary test objects.
var testGeometricObjectA = TestGeometricObject.Create();
testGeometricObjectA.Shape = geometricObjectA.Shape;
testGeometricObjectA.Scale = geometricObjectA.Scale;
testGeometricObjectA.Pose = startPoseA;
var testGeometricObjectB = TestGeometricObject.Create();
testGeometricObjectB.Shape = geometricObjectB.Shape;
testGeometricObjectB.Scale = geometricObjectB.Scale;
testGeometricObjectB.Pose = startPoseB;
var testCollisionObjectA = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObjectA.SetInternal(objectA, testGeometricObjectA);
var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObjectB.SetInternal(objectB, testGeometricObjectB);
var testContactSet = ContactSet.Create(testCollisionObjectA, testCollisionObjectB);
try
{
distanceAlgorithm.UpdateClosestPoints(testContactSet, 0);
if (testContactSet.Count < 0)
{
// No closest-distance result. --> Abort.
return(1);
}
Vector3F normal = testContactSet[0].Normal;
float distance = -testContactSet[0].PenetrationDepth;
float λ = 0;
float λPrevious = 0;
for (int i = 0; i < MaxNumberOfIterations && distance > 0; i++)
{
// |v∙n|
float velocityProjected = Vector3F.Dot(linearVelocityRelative, normal);
// Abort for separating objects.
if (Numeric.IsLess(velocityProjected, 0))
{
break;
}
// Increase TOI.
float μ = (distance + allowedPenetration) / velocityProjected;
λ = λ + μ;
if (λ < 0 || λ > 1)
{
break;
}
Debug.Assert(λPrevious < λ);
if (λ <= λPrevious)
{
break;
}
// Get new interpolated poses - only positions are changed.
Vector3F positionA = startPoseA.Position + λ * (targetPoseA.Position - startPoseA.Position);
testGeometricObjectA.Pose = new Pose(positionA, startPoseA.Orientation);
Vector3F positionB = startPoseB.Position + λ * (targetPoseB.Position - startPoseB.Position);
testGeometricObjectB.Pose = new Pose(positionB, startPoseB.Orientation);
// Get new closest point distance.
distanceAlgorithm.UpdateClosestPoints(testContactSet, 0);
if (testContactSet.Count == 0)
{
break;
}
normal = testContactSet[0].Normal;
distance = -testContactSet[0].PenetrationDepth;
λPrevious = λ;
}
if (testContactSet.HaveContact && λ > 0 && λ < 1 && testContactSet.Count > 0)
{
return(λ);
// We already have a contact that we could use.
// result.Contact = testContactSet[0];
}
}
finally
{
// Recycle temporary objects.
testContactSet.Recycle(true);
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectA);
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB);
testGeometricObjectA.Recycle();
testGeometricObjectB.Recycle();
}
return(1);
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
contactSet.HaveContact = true;
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Note: When comparing this implementation with the GJK (see Gjk.cs), be aware that the
// GJK implementation computes the CSO as the Minkowski difference A-B whereas the MPR uses
// B-A. Both representations of the CSO are equivalent, we just have to invert the vectors
// here and there. (B-A was chosen because the original description of the MPR used B-A.)
if (type == CollisionQueryType.ClosestPoints)
{
throw new GeometryException("MPR cannot handle closest-point queries. Use GJK instead.");
}
CollisionObject collisionObjectA = contactSet.ObjectA;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
ConvexShape shapeA = geometricObjectA.Shape as ConvexShape;
Vector3 scaleA = geometricObjectA.Scale;
Pose poseA = geometricObjectA.Pose;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
ConvexShape shapeB = geometricObjectB.Shape as ConvexShape;
Vector3 scaleB = geometricObjectB.Scale;
Pose poseB = geometricObjectB.Pose;
if (shapeA == null || shapeB == null)
{
throw new ArgumentException("The contact set must contain convex shapes.", "contactSet");
}
// Assume no contact.
contactSet.HaveContact = false;
Vector3 v0;
if (contactSet.IsPreferredNormalAvailable && type == CollisionQueryType.Contacts)
{
// Set v0, so to shoot into preferred direction.
v0 = contactSet.PreferredNormal;
// Perform only 1 MPR iteration.
DoMpr(type, contactSet, v0);
return;
}
// Find first point v0 (which determines the ray direction).
// Inner point in CSO (Minkowski difference B-A).
Vector3 v0A = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA);
Vector3 v0B = poseB.ToWorldPosition(shapeB.InnerPoint * scaleB);
v0 = v0B - v0A;
// If v0 == origin then we have contact.
if (v0.IsNumericallyZero)
{
// The inner points overlap. Probably there are two objects centered on the same point.
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
{
return;
}
// Choose a v0 different from Zero. Any direction is ok.
// The point should still be in the Minkowski difference.
v0.X = CollisionDetection.Epsilon / 10;
}
// Call MPR in iteration until the MPR ray has converged.
int iterationCount = 0;
const int iterationLimit = 10;
Vector3 oldMprRay;
// Use a temporary contact set.
var testContactSet = ContactSet.Create(collisionObjectA, collisionObjectB);
testContactSet.IsPerturbationTestAllowed = contactSet.IsPerturbationTestAllowed;
testContactSet.PreferredNormal = contactSet.PreferredNormal;
Contact oldContact = null;
do
{
oldMprRay = v0;
if (iterationCount == 0)
{
oldMprRay.TryNormalize();
}
// Call MPR. v0 of the next iteration is simply -returned portal normal.
Debug.Assert(testContactSet.Count == 0 || testContactSet.Count == 1, "testContactSet in MPR should have 0 or 1 contacts.");
Debug.Assert(testContactSet.Count == 0 || testContactSet[0] == oldContact);
testContactSet.Clear();
// Because of numerical problems (for example with long thin ellipse vs. capsule)
// it is possible that the last iteration was a contact but in this iteration
// no contact is found. Therefore we also reset the HaveContact flag to avoid
// an end result where HaveContact is set but no Contact is in the ContactSet.
testContactSet.HaveContact = false;
v0 = -DoMpr(type, testContactSet, v0);
if (testContactSet.Count > 0)
{
var newContact = testContactSet[0];
if (oldContact != null)
{
if (oldContact.PenetrationDepth < newContact.PenetrationDepth)
{
// The new penetration depth is larger then the old penetration depth.
// In this case we keep the old contact.
// This can happen for nearly parallel boxes. First we get a good contact.
// Then we get a contact another side. Normal has changed 90�. The new
// penetration depth can be nearly the whole box side length :-(.
newContact.Recycle();
testContactSet[0] = oldContact;
break;
}
}
if (newContact != oldContact)
{
if (oldContact != null)
{
oldContact.Recycle();
}
oldContact = newContact;
}
}
iterationCount++;
} while (testContactSet.HaveContact && // Separation? - No contact which we could refine.
iterationCount < iterationLimit && // Iteration limit reached?
v0 != Vector3.Zero && // Is normal useful to go on?
!Vector3.AreNumericallyEqual(-v0, oldMprRay, CollisionDetection.Epsilon));
// Normal hasn't converged yet?
if (testContactSet.Count > 0)
{
// Recycle oldContact if not used.
if (testContactSet[0] != oldContact)
{
if (oldContact != null)
{
oldContact.Recycle();
oldContact = null;
}
}
}
if (CollisionDetection.FullContactSetPerFrame &&
type == CollisionQueryType.Contacts &&
testContactSet.Count > 0 &&
contactSet.Count < 3)
{
// Try to find full contact set.
var wrapper = TestMethodWrappers.Obtain();
wrapper.OriginalMethod = _doMprMethod;
wrapper.V0 = testContactSet[0].Normal; // The MPR ray will point along the normal of the first contact.
ContactHelper.TestWithPerturbations(
CollisionDetection,
testContactSet,
true,
wrapper.Method);
TestMethodWrappers.Recycle(wrapper);
}
contactSet.HaveContact = testContactSet.HaveContact;
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
// Recycle temporary objects.
testContactSet.Recycle();
}
// 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)
{
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)
{
if (type == CollisionQueryType.ClosestPoints)
{
// Just use normal composite shape algorithm.
_compositeAlgorithm.ComputeCollision(contactSet, type);
return;
}
Debug.Assert(type != CollisionQueryType.ClosestPoints, "Closest point queries should have already been handled!");
// Composite = A, Ray = B
IGeometricObject compositeObject = contactSet.ObjectA.GeometricObject;
IGeometricObject rayObject = contactSet.ObjectB.GeometricObject;
// Object A should be the composite, swap objects if necessary.
bool swapped = (compositeObject.Shape is RayShape);
if (swapped)
{
MathHelper.Swap(ref rayObject, ref compositeObject);
}
RayShape rayShape = rayObject.Shape as RayShape;
CompositeShape compositeShape = compositeObject.Shape as CompositeShape;
// Check if shapes are correct.
if (rayShape == null || compositeShape == null)
{
throw new ArgumentException("The contact set must contain a ray and a composite shape.", "contactSet");
}
// Assume no contact.
contactSet.HaveContact = false;
// Get transformations.
Vector3F rayScale = rayObject.Scale;
Pose rayPose = rayObject.Pose;
Vector3F compositeScale = compositeObject.Scale;
Pose compositePose = compositeObject.Pose;
// Check if transforms are supported.
// Same check for object B.
if (compositeShape != null &&
(compositeScale.X != compositeScale.Y || compositeScale.Y != compositeScale.Z) &&
compositeShape.Children.Any(child => child.Pose.HasRotation)) // Note: Any() creates garbage, but non-uniform scalings should not be used anyway.
{
throw new NotSupportedException("Computing collisions for composite shapes with non-uniform scaling and rotated children is not supported.");
}
// ----- A few fixed objects which are reused to avoid GC garbage.
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
var testGeometricObject = TestGeometricObject.Create();
// Create a test contact set and initialize with dummy objects.
// (The actual collision objects are set below.)
var testContactSet = ContactSet.Create(testCollisionObject, contactSet.ObjectB); // Dummy arguments! They are changed later.
// Scale ray and transform ray to local unscaled space of composite.
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 compositePose); // Transform ray to local space of composite.
var inverseCompositeScale = Vector3F.One / compositeScale;
ray.Scale(ref inverseCompositeScale);
try
{
if (compositeShape.Partition != null)
{
#region ----- Composite with BVH vs. * -----
foreach (var childIndex in compositeShape.Partition.GetOverlaps(ray))
{
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
{
break; // We can abort early.
}
AddChildContacts(
contactSet,
swapped,
childIndex,
type,
testContactSet,
testCollisionObject,
testGeometricObject);
}
#endregion
}
else
{
#region ----- Composite vs. *-----
var rayDirectionInverse = new Vector3F(
1 / ray.Direction.X,
1 / ray.Direction.Y,
1 / ray.Direction.Z);
float epsilon = Numeric.EpsilonF * (1 + compositeObject.Aabb.Extent.Length);
// Go through list of children and find contacts.
int numberOfChildGeometries = compositeShape.Children.Count;
for (int i = 0; i < numberOfChildGeometries; i++)
{
IGeometricObject child = compositeShape.Children[i];
if (GeometryHelper.HaveContact(child.Shape.GetAabb(child.Scale, child.Pose), ray.Origin, rayDirectionInverse, ray.Length, epsilon))
{
AddChildContacts(
contactSet,
swapped,
i,
type,
testContactSet,
testCollisionObject,
testGeometricObject);
// We have contact and stop for boolean queries.
if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
{
break;
}
}
}
#endregion
}
}
finally
{
Debug.Assert(compositeObject.Shape == compositeShape, "Shape was altered and not restored.");
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
testGeometricObject.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.");
}
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);
}
}
private void AddContact(ContactSet contactSet,
bool swapped,
CollisionQueryType type,
ref Ray rayWorld, // The ray in world space.
ref Ray rayInMesh, // The ray in the scaled triangle mesh space.
ref Triangle triangle, // The unscaled triangle in the mesh space.
int triangleIndex,
ref Pose trianglePose,
ref Vector3F triangleScale,
bool isTwoSided)
{
// 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 = rayInMesh.Direction * rayInMesh.Length;
float δ = -Vector3F.Dot(r, n);
// Degenerate triangle --> No hit.
if (ε == 0.0f || Numeric.IsZero(δ, ε))
return;
Vector3F triangleToRayOrigin = rayInMesh.Origin - v0;
float λ = Vector3F.Dot(triangleToRayOrigin, n) / δ;
if (λ < 0 || λ > 1)
return;
// 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;
if (δ < 0 && !isTwoSided)
return; // Shooting into the back of a one-sided triangle - no contact.
float penetrationDepth = λ * rayInMesh.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,
"RayTriangleMeshAlgorithm has set the wrong feature property.");
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
}
public static void Merge(ContactSet contactSet, Contact newContact, CollisionQueryType type, float contactPositionTolerance)
{
Debug.Assert(contactSet != null);
Debug.Assert(newContact != null);
Debug.Assert(contactPositionTolerance >= 0, "The contact position tolerance must be greater than or equal to 0");
Debug.Assert(type == CollisionQueryType.ClosestPoints || type == CollisionQueryType.Contacts);
if (type == CollisionQueryType.Contacts && newContact.PenetrationDepth < 0)
return; // Do not merge separated contacts.
// ----- Simplest case: Contact set is empty.
if (contactSet.Count == 0)
{
// Simply add the new contact.
contactSet.Add(newContact);
return;
}
// ----- Try to merge with nearest old contact.
bool merged = TryMergeWithNearestContact(contactSet, newContact, contactPositionTolerance, true);
if (merged)
{
newContact.Recycle();
return;
}
// ----- Default: Add the new contact.
contactSet.Add(newContact);
}
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);
}
// Compute contacts between a shape and the child shapes of a <see cref="CompositeShape"/>.
// testXxx are initialized objects which are re-used to avoid a lot of GC garbage.
private void AddChildContacts(ContactSet contactSet,
bool swapped,
int childIndex,
CollisionQueryType type,
ContactSet testContactSet,
CollisionObject testCollisionObject,
TestGeometricObject testGeometricObject)
{
// This method is also used in RayCompositeAlgorithm.cs. Keep changes in sync!
// Object A should be the CompositeShape.
CollisionObject collisionObjectA = (swapped) ? contactSet.ObjectB : contactSet.ObjectA;
CollisionObject collisionObjectB = (swapped) ? contactSet.ObjectA : contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
Vector3F scaleA = geometricObjectA.Scale;
IGeometricObject childA = ((CompositeShape)geometricObjectA.Shape).Children[childIndex];
// Find collision algorithm.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[childA, geometricObjectB];
// ----- Set the shape temporarily to the current child.
// (Note: The scaling is either uniform or the child has no local rotation. Therefore, we only
// need to apply the scale of the parent to the scale and translation of the child. We can
// ignore the rotation.)
Debug.Assert(
(scaleA.X == scaleA.Y && scaleA.Y == scaleA.Z) || !childA.Pose.HasRotation,
"CompositeShapeAlgorithm should have thrown an exception. Non-uniform scaling is not supported for rotated children.");
var childPose = childA.Pose;
childPose.Position *= scaleA; // Apply scaling to local translation.
testGeometricObject.Pose = geometricObjectA.Pose * childPose;
testGeometricObject.Shape = childA.Shape;
testGeometricObject.Scale = scaleA * childA.Scale; // Apply scaling to local scale.
testCollisionObject.SetInternal(collisionObjectA, testGeometricObject);
// Create a temporary contact set.
// (ObjectA and ObjectB should have the same order as in contactSet; otherwise we couldn't
// simply merge them.)
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
if (swapped)
testContactSet.Reset(collisionObjectB, testCollisionObject);
else
testContactSet.Reset(testCollisionObject, collisionObjectB);
if (type == CollisionQueryType.Boolean)
{
// Boolean queries.
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
}
else
{
// No perturbation test. Most composite shapes are either complex and automatically
// have more contacts. Or they are complex and will not be used for stacking
// where full contact sets would be needed.
testContactSet.IsPerturbationTestAllowed = false;
// TODO: We could add existing contacts with the same child shape to childContactSet.
// Collision algorithms could take advantage of existing contact information to speed up
// calculations. However, at the moment the collision algorithms ignore existing contacts.
// If we add the exiting contacts to childContactSet we need to uncomment the comment
// code lines below.
// Transform contacts into space of child shape.
//foreach (Contact c in childContactSet)
//{
// if (childContactSet.ObjectA == childCollisionObject)
// c.PositionALocal = childPose.ToLocalPosition(c.PositionALocal);
// else
// c.PositionBLocal = childPose.ToLocalPosition(c.PositionBLocal);
//}
// Make collision check. As soon as we have found contact, we can make faster
// contact queries instead of closest-point queries.
CollisionQueryType queryType = (contactSet.HaveContact) ? CollisionQueryType.Contacts : type;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
// Transform contacts into space of composite shape.
// And set the shape feature of the contact.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
if (swapped)
{
contact.PositionBLocal = childPose.ToWorldPosition(contact.PositionBLocal);
//if (contact.Lifetime.Ticks == 0) // Currently, all contacts are new, so this check is not necessary.
//{
contact.FeatureB = childIndex;
//}
}
else
{
contact.PositionALocal = childPose.ToWorldPosition(contact.PositionALocal);
//if (contact.Lifetime.Ticks == 0) // Currently, all contacts are new, so this check is not necessary.
//{
contact.FeatureA = childIndex;
//}
}
}
// Merge child contacts.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
}
/// <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);
}
// Compute contacts between child shapes of two <see cref="CompositeShape"/>s.
// testXxx are initialized objects which are re-used to avoid a lot of GC garbage.
private void AddChildChildContacts(ContactSet contactSet,
int childIndexA,
int childIndexB,
CollisionQueryType type,
ContactSet testContactSet,
CollisionObject testCollisionObjectA,
TestGeometricObject testGeometricObjectA,
CollisionObject testCollisionObjectB,
TestGeometricObject testGeometricObjectB)
{
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
CompositeShape shapeA = (CompositeShape)geometricObjectA.Shape;
CompositeShape shapeB = (CompositeShape)geometricObjectB.Shape;
Vector3F scaleA = geometricObjectA.Scale;
Vector3F scaleB = geometricObjectB.Scale;
IGeometricObject childA = shapeA.Children[childIndexA];
IGeometricObject childB = shapeB.Children[childIndexB];
// Find collision algorithm.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[childA, childB];
// ----- Set the shape temporarily to the current children.
// (Note: The scaling is either uniform or the child has no local rotation. Therefore, we only
// need to apply the scale of the parent to the scale and translation of the child. We can
// ignore the rotation.)
Debug.Assert(
(scaleA.X == scaleA.Y && scaleA.Y == scaleA.Z) || !childA.Pose.HasRotation,
"CompositeShapeAlgorithm should have thrown an exception. Non-uniform scaling is not supported for rotated children.");
Debug.Assert(
(scaleB.X == scaleB.Y && scaleB.Y == scaleB.Z) || !childB.Pose.HasRotation,
"CompositeShapeAlgorithm should have thrown an exception. Non-uniform scaling is not supported for rotated children.");
var childAPose = childA.Pose;
childAPose.Position *= scaleA; // Apply scaling to local translation.
testGeometricObjectA.Pose = geometricObjectA.Pose * childAPose;
testGeometricObjectA.Shape = childA.Shape;
testGeometricObjectA.Scale = scaleA * childA.Scale; // Apply scaling to local scale.
testCollisionObjectA.SetInternal(collisionObjectA, testGeometricObjectA);
var childBPose = childB.Pose;
childBPose.Position *= scaleB; // Apply scaling to local translation.
testGeometricObjectB.Pose = geometricObjectB.Pose * childBPose;
testGeometricObjectB.Shape = childB.Shape;
testGeometricObjectB.Scale = scaleB * childB.Scale; // Apply scaling to local scale.
testCollisionObjectB.SetInternal(collisionObjectB, testGeometricObjectB);
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
testContactSet.Reset(testCollisionObjectA, testCollisionObjectB);
if (type == CollisionQueryType.Boolean)
{
// Boolean queries.
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
}
else
{
// TODO: We could add existing contacts with the same child shape to childContactSet.
// No perturbation test. Most composite shapes are either complex and automatically
// have more contacts. Or they are complex and will not be used for stacking
// where full contact sets would be needed.
testContactSet.IsPerturbationTestAllowed = false;
// Make collision check. As soon as we have found contact, we can make faster
// contact queries instead of closest-point queries.
CollisionQueryType queryType = (contactSet.HaveContact) ? CollisionQueryType.Contacts : type;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
contactSet.HaveContact = (contactSet.HaveContact || testContactSet.HaveContact);
// Transform contacts into space of composite shape.
// And set the shape feature of the contact.
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
contact.PositionALocal = childAPose.ToWorldPosition(contact.PositionALocal);
//if (contact.Lifetime.Ticks == 0) // Currently, all contacts are new, so this check is not necessary.
//{
contact.FeatureA = childIndexA;
//}
contact.PositionBLocal = childBPose.ToWorldPosition(contact.PositionBLocal);
//if (contact.Lifetime.Ticks == 0) // Currently, all contacts are new, so this check is not necessary.
//{
contact.FeatureB = childIndexB;
//}
}
// Merge child contacts.
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
}
}
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);
}
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
// Object A should be the composite, swap objects if necessary.
// When testing CompositeShape vs. CompositeShape with BVH, object A should be the
// CompositeShape with BVH.
CompositeShape compositeShapeA = geometricObjectA.Shape as CompositeShape;
CompositeShape compositeShapeB = geometricObjectB.Shape as CompositeShape;
bool swapped = false;
if (compositeShapeA == null)
{
// Object A is something else. Object B must be a composite shape.
swapped = true;
}
else if (compositeShapeA.Partition == null
&& compositeShapeB != null
&& compositeShapeB.Partition != null)
{
// Object A has no BVH, object B is CompositeShape with BVH.
swapped = true;
}
if (swapped)
{
MathHelper.Swap(ref collisionObjectA, ref collisionObjectB);
MathHelper.Swap(ref geometricObjectA, ref geometricObjectB);
MathHelper.Swap(ref compositeShapeA, ref compositeShapeB);
}
// Check if collision objects shapes are correct.
if (compositeShapeA == null)
throw new ArgumentException("The contact set must contain a composite shape.", "contactSet");
// Assume no contact.
contactSet.HaveContact = false;
Vector3F scaleA = geometricObjectA.Scale;
Vector3F scaleB = geometricObjectB.Scale;
// Check if transforms are supported.
if (compositeShapeA != null // When object A is a CompositeShape
&& (scaleA.X != scaleA.Y || scaleA.Y != scaleA.Z) // non-uniform scaling is not supported
&& compositeShapeA.Children.Any(child => child.Pose.HasRotation)) // when a child has a local rotation.
{ // Note: Any() creates garbage, but non-uniform scalings should not be used anyway.
throw new NotSupportedException("Computing collisions for composite shapes with non-uniform scaling and rotated children is not supported.");
}
// Same check for object B.
if (compositeShapeB != null
&& (scaleB.X != scaleB.Y || scaleB.Y != scaleB.Z)
&& compositeShapeB.Children.Any(child => child.Pose.HasRotation)) // Note: Any() creates garbage, but non-uniform scalings should not be used anyway.
{
throw new NotSupportedException("Computing collisions for composite shapes with non-uniform scaling and rotated children is not supported.");
}
// ----- A few fixed objects which are reused to avoid GC garbage.
var testCollisionObjectA = ResourcePools.TestCollisionObjects.Obtain();
var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain();
// Create a test contact set and initialize with dummy objects.
// (The actual collision objects are set below.)
var testContactSet = ContactSet.Create(testCollisionObjectA, testCollisionObjectB);
var testGeometricObjectA = TestGeometricObject.Create();
var testGeometricObjectB = TestGeometricObject.Create();
try
{
if (compositeShapeA.Partition != null
&& (type != CollisionQueryType.ClosestPoints
|| compositeShapeA.Partition is ISupportClosestPointQueries<int>))
{
if (compositeShapeB != null && compositeShapeB.Partition != null)
{
#region ----- Composite with BVH vs. Composite with BVH -----
Debug.Assert(swapped == false, "Why did we swap the objects? Order of objects is fine.");
if (type != CollisionQueryType.ClosestPoints)
{
// ----- Boolean or Contact Query
// Heuristic: Test large BVH vs. small BVH.
Aabb aabbOfA = geometricObjectA.Aabb;
Aabb aabbOfB = geometricObjectB.Aabb;
float largestExtentA = aabbOfA.Extent.LargestComponent;
float largestExtentB = aabbOfB.Extent.LargestComponent;
IEnumerable<Pair<int>> overlaps;
bool overlapsSwapped = largestExtentA < largestExtentB;
if (overlapsSwapped)
{
overlaps = compositeShapeB.Partition.GetOverlaps(
scaleB,
geometricObjectB.Pose,
compositeShapeA.Partition,
scaleA,
geometricObjectA.Pose);
}
else
{
overlaps = compositeShapeA.Partition.GetOverlaps(
scaleA,
geometricObjectA.Pose,
compositeShapeB.Partition,
scaleB,
geometricObjectB.Pose);
}
foreach (var overlap in overlaps)
{
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
break; // We can abort early.
AddChildChildContacts(
contactSet,
overlapsSwapped ? overlap.Second : overlap.First,
overlapsSwapped ? overlap.First : overlap.Second,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA,
testCollisionObjectB,
testGeometricObjectB);
}
}
else
{
// Closest-Point Query
var callback = ClosestPointsCallbacks.Obtain();
callback.CollisionAlgorithm = this;
callback.Swapped = false;
callback.ContactSet = contactSet;
callback.TestCollisionObjectA = testCollisionObjectA;
callback.TestCollisionObjectB = testCollisionObjectB;
callback.TestGeometricObjectA = testGeometricObjectA;
callback.TestGeometricObjectB = testGeometricObjectB;
callback.TestContactSet = testContactSet;
((ISupportClosestPointQueries<int>)compositeShapeA.Partition)
.GetClosestPointCandidates(
scaleA,
geometricObjectA.Pose,
compositeShapeB.Partition,
scaleB,
geometricObjectB.Pose,
callback.HandlePair);
ClosestPointsCallbacks.Recycle(callback);
}
#endregion
}
else
{
#region ----- Composite with BVH vs. * -----
// Compute AABB of B in local space of the CompositeShape.
Aabb aabbBInA = geometricObjectB.Shape.GetAabb(
scaleB, geometricObjectA.Pose.Inverse * geometricObjectB.Pose);
// Apply inverse scaling to do the AABB checks in the unscaled local space of A.
aabbBInA.Scale(Vector3F.One / scaleA);
if (type != CollisionQueryType.ClosestPoints)
{
// Boolean or Contact Query
foreach (var childIndex in compositeShapeA.Partition.GetOverlaps(aabbBInA))
{
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
break; // We can abort early.
AddChildContacts(
contactSet,
swapped,
childIndex,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA);
}
}
else if (type == CollisionQueryType.ClosestPoints)
{
// Closest-Point Query
var callback = ClosestPointsCallbacks.Obtain();
callback.CollisionAlgorithm = this;
callback.Swapped = swapped;
callback.ContactSet = contactSet;
callback.TestCollisionObjectA = testCollisionObjectA;
callback.TestCollisionObjectB = testCollisionObjectB;
callback.TestGeometricObjectA = testGeometricObjectA;
callback.TestGeometricObjectB = testGeometricObjectB;
callback.TestContactSet = testContactSet;
((ISupportClosestPointQueries<int>)compositeShapeA.Partition)
.GetClosestPointCandidates(
aabbBInA,
float.PositiveInfinity,
callback.HandleItem);
ClosestPointsCallbacks.Recycle(callback);
}
#endregion
}
}
else
{
#region ----- Composite vs. *-----
// Compute AABB of B in local space of the composite.
Aabb aabbBInA = geometricObjectB.Shape.GetAabb(scaleB, geometricObjectA.Pose.Inverse * geometricObjectB.Pose);
// Apply inverse scaling to do the AABB checks in the unscaled local space of A.
aabbBInA.Scale(Vector3F.One / scaleA);
// Go through list of children and find contacts.
int numberOfChildGeometries = compositeShapeA.Children.Count;
for (int i = 0; i < numberOfChildGeometries; i++)
{
IGeometricObject child = compositeShapeA.Children[i];
// NOTE: For closest-point queries we could be faster estimating a search space.
// See TriangleMeshAlgorithm or BVH queries.
// But the current implementation is sufficient. If the CompositeShape is more complex
// the user should be using spatial partitions anyway.
// For boolean or contact queries, we make an AABB test first.
// For closest points where we have not found a contact yet, we have to search
// all children.
if ((type == CollisionQueryType.ClosestPoints && !contactSet.HaveContact)
|| GeometryHelper.HaveContact(aabbBInA, child.Shape.GetAabb(child.Scale, child.Pose)))
{
// TODO: We could compute the minDistance of the child AABB and the AABB of the
// other shape. If the minDistance is greater than the current closestPairDistance
// we can ignore this pair. - This could be a performance boost.
// Get contacts/closest pairs of this child.
AddChildContacts(
contactSet,
swapped,
i,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA);
// We have contact and stop for boolean queries.
if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
break;
}
}
#endregion
}
}
finally
{
Debug.Assert(collisionObjectA.GeometricObject.Shape == compositeShapeA, "Shape was altered and not restored.");
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectA);
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB);
testGeometricObjectB.Recycle();
testGeometricObjectA.Recycle();
}
}
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)
{
contactSet.Clear();
contactSet.Add(ContactHelper.CreateContact(contactSet, new Vector3F(1, 2, 3), Vector3F.UnitZ, 1, false));
if (type == CollisionQueryType.Contacts)
contactSet.Add(ContactHelper.CreateContact(contactSet, new Vector3F(2, 2, 3), Vector3F.UnitZ, 1.2f, false));
contactSet.HaveContact = true;
}
// 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.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)
{
// 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);
}
}
}
}
// 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 Update(GameTime gameTime)
{
var debugRenderer = GraphicsScreen.DebugRenderer;
debugRenderer.Clear();
// Draw coordinate cross at world origin.
debugRenderer.DrawAxes(Pose.Identity, 10, false);
// Draw objects.
debugRenderer.DrawObject(_objectA.GeometricObject, GraphicsHelper.GetUniqueColor(_objectA), _drawWireframe, false);
debugRenderer.DrawObject(_objectB.GeometricObject, GraphicsHelper.GetUniqueColor(_objectB), _drawWireframe, false);
// Draw contact info.
debugRenderer.DrawContacts(_contactSet, 0.1f, null, true);
// Toggle wireframe rendering.
if (InputService.IsPressed(Keys.Space, true))
{
_drawWireframe = !_drawWireframe;
}
// Change background color if we have a contact.
GraphicsScreen.BackgroundColor = (_contactSet != null && _contactSet.HaveContact)
? new Color(220, 200, 200, 255)
: new Color(200, 220, 200, 255);
// Move one object with keyboard NumPad.
var translation = new Vector3F();
if (InputService.IsDown(Keys.NumPad4))
{
translation.X -= 1;
}
if (InputService.IsDown(Keys.NumPad6))
{
translation.X += 1;
}
if (InputService.IsDown(Keys.NumPad8))
{
translation.Y += 1;
}
if (InputService.IsDown(Keys.NumPad5))
{
translation.Y -= 1;
}
if (InputService.IsDown(Keys.NumPad7))
{
translation.Z -= 1;
}
if (InputService.IsDown(Keys.NumPad9))
{
translation.Z += 1;
}
if (!translation.IsNumericallyZero)
{
var go = (GeometricObject)_objectA.GeometricObject;
float scale = go.Aabb.Extent.Length * 0.1f;
translation *= scale * (float)gameTime.ElapsedGameTime.TotalSeconds;
go.Pose = new Pose(go.Pose.Position + translation, go.Pose.Orientation);
if (_contactSet != null)
{
_collisionDetection.UpdateContacts(_contactSet, 0.001f);
}
else
{
_contactSet = _collisionDetection.GetContacts(_objectA, _objectB);
}
}
}
public CollisionAlgorithm this[ContactSet pair]
{
get
{
if (pair == null)
throw new ArgumentNullException("pair");
Debug.Assert(pair.ObjectA != null, "ContactSet needs to ensure that ObjectA is not null.");
Debug.Assert(pair.ObjectB != null, "ContactSet needs to ensure that ObjectB is not null.");
Debug.Assert(pair.ObjectA.GeometricObject != null, "CollisionObject needs to ensure that GeometricObject is not null.");
Debug.Assert(pair.ObjectB.GeometricObject != null, "CollisionObject needs to ensure that GeometricObject is not null.");
Debug.Assert(pair.ObjectA.GeometricObject.Shape != null, "IGeometricObject needs to ensure that Shape is not null.");
Debug.Assert(pair.ObjectB.GeometricObject.Shape != null, "IGeometricObject needs to ensure that Shape is not null.");
return this[pair.ObjectA.GeometricObject.Shape.GetType(), pair.ObjectB.GeometricObject.Shape.GetType()];
}
set
{
if (pair == null)
throw new ArgumentNullException("pair");
Debug.Assert(pair.ObjectA != null, "ContactSet needs to ensure that ObjectA is not null.");
Debug.Assert(pair.ObjectB != null, "ContactSet needs to ensure that ObjectB is not null.");
Debug.Assert(pair.ObjectA.GeometricObject != null, "CollisionObject needs to ensure that GeometricObject is not null.");
Debug.Assert(pair.ObjectB.GeometricObject != null, "CollisionObject needs to ensure that GeometricObject is not null.");
Debug.Assert(pair.ObjectA.GeometricObject.Shape != null, "IGeometricObject needs to ensure that Shape is not null.");
Debug.Assert(pair.ObjectB.GeometricObject.Shape != null, "IGeometricObject needs to ensure that Shape is not null.");
this[pair.ObjectA.GeometricObject.Shape.GetType(), pair.ObjectB.GeometricObject.Shape.GetType()] = value;
}
}
