private void Initialize(RigidBody bodyA, RigidBody bodyB, Contact contact)
{
Debug.Assert(bodyA != null && bodyB != null && contact != null);
_bodyA = bodyA;
_bodyB = bodyB;
Contact = contact;
_simulation = _bodyA.Simulation ?? _bodyB.Simulation;
Debug.Assert(_simulation != null);
ErrorReduction = _simulation.Settings.Constraints.ContactErrorReduction;
_minConstraintImpulse = _simulation.Settings.Constraints.MinConstraintImpulse;
Vector3F n = Contact.Normal;
_rA = Contact.PositionAWorld - BodyA.PoseCenterOfMass.Position;
_rB = Contact.PositionBWorld - BodyB.PoseCenterOfMass.Position;
_rALengthSquared = -1;
_rBLengthSquared = -1;
Vector3F vRel = RelativeVelocity;
float vRelN = Vector3F.Dot(vRel, n);
// Get material-dependent values.
var materialA = BodyA.Material.GetProperties(BodyA, Contact.PositionALocal, Contact.FeatureA);
var materialB = BodyB.Material.GetProperties(BodyB, Contact.PositionBLocal, Contact.FeatureB);
var materialCombiner = _simulation.Settings.MaterialPropertyCombiner;
// Restitution is clamped to 0 if bodies are slow or if restitution is very low.
if (Math.Abs(vRelN) > _simulation.Settings.Constraints.RestingVelocityLimit)
{
_restitution = materialCombiner.CombineRestitution(materialA.Restitution, materialB.Restitution);
if (_restitution < _simulation.Settings.Constraints.RestitutionThreshold)
_restitution = 0;
}
else
{
// The contacts must be created BEFORE forces are applied! If the bodies are moving less
// than the RestingVelocityLimit, we set the Restitution to zero.
// Bodies that do not move relative to each other must have a restitution of 0 to get
// stable stacks!
// RestingVelocityLimit can even be zero!
_restitution = 0;
}
_staticFriction = materialCombiner.CombineFriction(materialA.StaticFriction, materialB.StaticFriction);
_dynamicFriction = materialCombiner.CombineFriction(materialA.DynamicFriction, materialB.DynamicFriction);
_surfaceMotionAEnabled = materialA.SupportsSurfaceMotion;
_surfaceMotionBEnabled = materialB.SupportsSurfaceMotion;
// Get friction directions. The first direction is in direction of the greatest tangent
// velocity.
_t0 = vRel - vRelN * n;
if (!_t0.TryNormalize())
_t0 = n.Orthonormal1;
_t1 = Vector3F.Cross(_t0, n);
// Reset cached impulses.
_penetrationConstraint.ConstraintImpulse = 0;
_frictionConstraint0.ConstraintImpulse = 0;
_frictionConstraint1.ConstraintImpulse = 0;
}
/// <summary>
/// Creates an instance of the <see cref="ContactConstraint"/> class. (This method reuses a
/// previously recycled instance or allocates a new instance if necessary.)
/// </summary>
/// <param name="bodyA">The first body.</param>
/// <param name="bodyB">The second body.</param>
/// <param name="contact">The contact.</param>
/// <returns>A new or reusable instance of the <see cref="ContactConstraint"/> class.</returns>
/// <remarks>
/// <para>
/// This method tries to obtain a previously recycled instance from a resource pool if resource
/// pooling is enabled (see <see cref="ResourcePool.Enabled">ResourcePool.Enabled</see>). If no
/// object is available, a new instance is automatically allocated on the heap.
/// </para>
/// <para>
/// The owner of the object should call <see cref="Recycle"/> when the instance is no longer
/// needed.
/// </para>
/// </remarks>
internal static ContactConstraint Create(RigidBody bodyA, RigidBody bodyB, Contact contact)
{
var contactConstraint = Pool.Obtain();
contactConstraint.Initialize(bodyA, bodyB, contact);
return contactConstraint;
}
/// <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>
/// 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 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);
}
/// <summary>
/// Draws a contact.
/// </summary>
/// <param name="contact">The contact.</param>
/// <param name="normalLength">The length of the normal vector in world space.</param>
/// <param name="color">The color.</param>
/// <param name="drawOverScene">
/// If set to <see langword="true"/> the object is drawn over the graphics scene (depth-test
/// disabled).
/// </param>
/// <remarks>
/// The penetration depth is visualized with a dark red line.
/// </remarks>
public void DrawContact(Contact contact, float normalLength, Color color, bool drawOverScene)
{
if (!Enabled || contact == null)
return;
DrawPoint(contact.Position, color, drawOverScene);
DrawLine(contact.Position, contact.Position + contact.Normal * normalLength, color, drawOverScene);
// Draw a red line that visualizes the penetration depth.
var halfPenetration = contact.Normal * contact.PenetrationDepth / 2;
DrawLine(contact.Position - halfPenetration, contact.Position + halfPenetration, Color.DarkRed, drawOverScene);
}
