private void ComputeTimeOfImpact_Singlethreaded(ContactSet contactSet)
{
RigidBody bodyA = contactSet.ObjectA.GeometricObject as RigidBody;
RigidBody bodyB = contactSet.ObjectB.GeometricObject as RigidBody;
// Note: We do CCD only between rigid bodies.
// TODO: Support CCD between RigidBody and general CollisionObject of the user.
if (bodyA != null && bodyB != null)
{
// Check if at least one object needs CCD.
if (bodyA.IsCcdActive || bodyB.IsCcdActive)
{
// Check CCD filter. (We check this filter first because it hopefully filters out a lot
// of unnecessary checks (e.g. body against debris).)
Func<RigidBody, RigidBody, bool> ccdFilter = Settings.Motion.CcdFilter;
if (ccdFilter != null && !ccdFilter(bodyA, bodyB))
return;
// Check collision filter. (Using the internal method CanCollide that uses a cache.)
if (!CollisionDomain.CanCollide(contactSet))
return;
// If convex bodies are touching we do not need to compute TOI because they are either
// moving more toward each other (TOI = 0) or separating (TOI = 1). If they are moving
// toward each other, then the contact constraint has failed and CCD is not the problem.
// For objects vs. concave objects we still have to make a check because the bullet
// could be separating and colliding with another part of the concave object.
// To avoid that objects stick: GetTimeOfImpact() should not return 0 for separating
// objects and we use 2 * AllowedPenetration so that the current contact does not
// count.
if (!(bodyA.Shape is ConvexShape)
|| !(bodyB.Shape is ConvexShape)
|| !CollisionDomain.HaveContact(contactSet.ObjectA, contactSet.ObjectB))
{
// Get time of impact.
float timeOfImpact = CollisionDomain.CollisionDetection.GetTimeOfImpact(
contactSet.ObjectA,
bodyA.IsCcdActive ? bodyA.TargetPose : bodyA.Pose,
contactSet.ObjectB,
bodyB.IsCcdActive ? bodyB.TargetPose : bodyB.Pose,
Settings.Constraints.AllowedPenetration * 2f);
// Store minimal time of impact.
if (timeOfImpact < bodyA.TimeOfImpact)
bodyA.TimeOfImpact = timeOfImpact;
if (timeOfImpact < bodyB.TimeOfImpact)
bodyB.TimeOfImpact = timeOfImpact;
}
}
}
}
private void ComputeTimeOfImpact_Multithreaded(int i)
{
var contactSet = CollisionDomain.InternalBroadPhase.CandidatePairs[i];
if (contactSet == null)
return;
RigidBody bodyA = contactSet.ObjectA.GeometricObject as RigidBody;
RigidBody bodyB = contactSet.ObjectB.GeometricObject as RigidBody;
if (bodyA != null && bodyB != null)
{
if (bodyA.IsCcdActive || bodyB.IsCcdActive)
{
Func<RigidBody, RigidBody, bool> ccdFilter = Settings.Motion.CcdFilter;
if (ccdFilter != null && !ccdFilter(bodyA, bodyB))
return;
if (!CollisionDomain.CanCollide(contactSet))
return;
if (!(bodyA.Shape is ConvexShape)
|| !(bodyB.Shape is ConvexShape)
|| !CollisionDomain.HaveContact(contactSet.ObjectA, contactSet.ObjectB))
{
float timeOfImpact = CollisionDomain.CollisionDetection.GetTimeOfImpact(
contactSet.ObjectA,
bodyA.IsCcdActive ? bodyA.TargetPose : bodyA.Pose,
contactSet.ObjectB,
bodyB.IsCcdActive ? bodyB.TargetPose : bodyB.Pose,
Settings.Constraints.AllowedPenetration * 2f);
float initialToi;
do
{
initialToi = bodyA.TimeOfImpact;
} while (timeOfImpact < initialToi && Interlocked.CompareExchange(ref bodyA.TimeOfImpact, timeOfImpact, initialToi) != initialToi);
do
{
initialToi = bodyB.TimeOfImpact;
} while (timeOfImpact < initialToi && Interlocked.CompareExchange(ref bodyB.TimeOfImpact, timeOfImpact, initialToi) != initialToi);
#else
if (timeOfImpact < bodyA.TimeOfImpact) // Double-check to avoid unnecessary locking.
{
lock (bodyA)
{
if (timeOfImpact < bodyA.TimeOfImpact)
bodyA.TimeOfImpact = timeOfImpact;
}
}
if (timeOfImpact < bodyB.TimeOfImpact) // Double-check to avoid unnecessary locking.
{
lock (bodyB)
{
if (timeOfImpact < bodyB.TimeOfImpact)
bodyB.TimeOfImpact = timeOfImpact;
}
}
}
}
}
}