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