private bool IsContactUnique(ref ContactData contactCandidate, ref QuickList <ContactData> candidatesToAdd)
{
contactCandidate.Validate();
float distanceSquared;
RigidTransform meshTransform = MeshTransform;
for (int i = 0; i < contacts.Count; i++)
{
Vector3.DistanceSquared(ref contacts.Elements[i].Position, ref contactCandidate.Position, out distanceSquared);
if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
{
//This is a nonconvex manifold. There will be times where a an object will be shoved into a corner such that
//a single position will have two reasonable normals. If the normals aren't mostly aligned, they should NOT be considered equivalent.
Vector3.Dot(ref contacts.Elements[i].Normal, ref contactCandidate.Normal, out distanceSquared);
if (Math.Abs(distanceSquared) >= CollisionDetectionSettings.nonconvexNormalDotMinimum)
{
//Update the existing 'redundant' contact with the new information.
//This works out because the new contact is the deepest contact according to the previous collision detection iteration.
contacts.Elements[i].Normal = contactCandidate.Normal;
contacts.Elements[i].Position = contactCandidate.Position;
contacts.Elements[i].PenetrationDepth = contactCandidate.PenetrationDepth;
supplementData.Elements[i].BasePenetrationDepth = contactCandidate.PenetrationDepth;
RigidTransform.TransformByInverse(ref contactCandidate.Position, ref convex.worldTransform, out supplementData.Elements[i].LocalOffsetA);
RigidTransform.TransformByInverse(ref contactCandidate.Position, ref meshTransform, out supplementData.Elements[i].LocalOffsetB);
return(false);
}
}
}
for (int i = 0; i < candidatesToAdd.Count; i++)
{
Vector3.DistanceSquared(ref candidatesToAdd.Elements[i].Position, ref contactCandidate.Position, out distanceSquared);
if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
{
//This is a nonconvex manifold. There will be times where a an object will be shoved into a corner such that
//a single position will have two reasonable normals. If the normals aren't mostly aligned, they should NOT be considered equivalent.
Vector3.Dot(ref candidatesToAdd.Elements[i].Normal, ref contactCandidate.Normal, out distanceSquared);
if (Math.Abs(distanceSquared) >= CollisionDetectionSettings.nonconvexNormalDotMinimum)
{
return(false);
}
}
}
//for (int i = 0; i < edgeContacts.count; i++)
//{
// Vector3.DistanceSquared(ref edgeContacts.Elements[i].ContactData.Position, ref contactCandidate.Position, out distanceSquared);
// if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
// {
// return false;
// }
//}
//for (int i = 0; i < vertexContacts.count; i++)
//{
// Vector3.DistanceSquared(ref vertexContacts.Elements[i].ContactData.Position, ref contactCandidate.Position, out distanceSquared);
// if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
// {
// return false;
// }
//}
return(true);
}
///<summary>
/// Sets up the contact with new information.
///</summary>
///<param name="candidate">Contact data to initialize the contact with.</param>
public void Setup(ref ContactData candidate)
{
candidate.Validate();
Position = candidate.Position;
Normal = candidate.Normal;
PenetrationDepth = candidate.PenetrationDepth;
Id = candidate.Id;
}
protected override void Add(ref ContactData contactCandidate)
{
contactCandidate.Validate();
contact.Normal = contactCandidate.Normal;
contact.PenetrationDepth = contactCandidate.PenetrationDepth;
contact.Position = contactCandidate.Position;
contacts.Add(contact);
OnAdded(contact);
}
protected override void ProcessCandidates(ref QuickList <ContactData> candidates)
{
//If the candidates list is empty, then let's see if the convex is in the 'thickness' of the terrain.
if (candidates.Count == 0 & terrain.thickness > 0)
{
RayHit rayHit;
Ray ray = new Ray {
Position = convex.worldTransform.Position, Direction = terrain.worldTransform.LinearTransform.Up
};
ray.Direction.Normalize();
//The raycast has to use doublesidedness, since we're casting from the bottom up.
if (terrain.Shape.RayCast(ref ray, terrain.thickness, ref terrain.worldTransform, TriangleSidedness.DoubleSided, out rayHit))
{
//Found a hit!
rayHit.Normal.Normalize();
float dot;
Vector3.Dot(ref ray.Direction, ref rayHit.Normal, out dot);
var newContact = new ContactData
{
Normal = rayHit.Normal,
Position = convex.worldTransform.Position,
Id = 2,
PenetrationDepth = -rayHit.T * dot + convex.Shape.MinimumRadius
};
newContact.Validate();
bool found = false;
for (int i = 0; i < contacts.Count; i++)
{
if (contacts.Elements[i].Id == 2)
{
//As set above, an id of 2 corresponds to a contact created from this raycast process.
contacts.Elements[i].Normal = newContact.Normal;
contacts.Elements[i].Position = newContact.Position;
contacts.Elements[i].PenetrationDepth = newContact.PenetrationDepth;
supplementData.Elements[i].BasePenetrationDepth = newContact.PenetrationDepth;
supplementData.Elements[i].LocalOffsetA = new Vector3();
supplementData.Elements[i].LocalOffsetB = ray.Position; //convex local position in mesh.
found = true;
break;
}
}
if (!found)
{
candidates.Add(ref newContact);
}
}
}
}
private bool IsContactUnique(ref ContactData contactCandidate)
{
contactCandidate.Validate();
for (int i = 0; i < contacts.Count; i++)
{
float distanceSquared;
Vector3.DistanceSquared(ref contacts.Elements[i].Position, ref contactCandidate.Position, out distanceSquared);
if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
{
//Update the existing 'redundant' contact with the new information.
//This works out because the new contact is the deepest contact according to the previous collision detection iteration.
contacts.Elements[i].Normal = contactCandidate.Normal;
contacts.Elements[i].Position = contactCandidate.Position;
contacts.Elements[i].PenetrationDepth = contactCandidate.PenetrationDepth;
supplementData.Elements[i].BasePenetrationDepth = contactCandidate.PenetrationDepth;
RigidTransform.TransformByInverse(ref contactCandidate.Position, ref collidableA.worldTransform, out supplementData.Elements[i].LocalOffsetA);
RigidTransform.TransformByInverse(ref contactCandidate.Position, ref collidableB.worldTransform, out supplementData.Elements[i].LocalOffsetB);
return(false);
}
}
return(true);
}
protected override void ProcessCandidates(ref QuickList <ContactData> candidates)
{
if (candidates.Count == 0 && parentContactCount == 0 && Mesh.Shape.solidity == MobileMeshSolidity.Solid)
{
//If there's no new contacts on the mesh and it's supposed to be a solid,
//then we must check the convex for containment within the shell.
//We already know that it's not on the shell, meaning that the shape is either
//far enough away outside the shell that there's no contact (and we're done),
//or it's far enough inside the shell that the triangles cannot create contacts.
//To find out which it is, raycast against the shell.
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref mesh.worldTransform.Orientation, out orientation);
Ray ray;
Vector3.Subtract(ref convex.worldTransform.Position, ref mesh.worldTransform.Position, out ray.Position);
Matrix3x3.TransformTranspose(ref ray.Position, ref orientation, out ray.Position);
//Cast from the current position back to the previous position.
Vector3.Subtract(ref lastValidConvexPosition, ref ray.Position, out ray.Direction);
float rayDirectionLength = ray.Direction.LengthSquared();
if (rayDirectionLength < Toolbox.Epsilon)
{
//The object may not have moved enough to normalize properly. If so, choose something arbitrary.
//Try the direction from the center of the object to the convex's position.
ray.Direction = ray.Position;
rayDirectionLength = ray.Direction.LengthSquared();
if (rayDirectionLength < Toolbox.Epsilon)
{
//This is unlikely; just pick something completely arbitrary then.
ray.Direction = Vector3.Up;
rayDirectionLength = 1;
}
}
Vector3.Divide(ref ray.Direction, (float)Math.Sqrt(rayDirectionLength), out ray.Direction);
RayHit hit;
if (mesh.Shape.IsLocalRayOriginInMesh(ref ray, out hit))
{
ContactData newContact = new ContactData {
Id = 2
};
//Give it a special id so that we know that it came from the inside.
Matrix3x3.Transform(ref ray.Position, ref orientation, out newContact.Position);
Vector3.Add(ref newContact.Position, ref mesh.worldTransform.Position, out newContact.Position);
newContact.Normal = hit.Normal;
newContact.Normal.Normalize();
float factor;
Vector3.Dot(ref ray.Direction, ref newContact.Normal, out factor);
newContact.PenetrationDepth = -factor * hit.T + convex.Shape.MinimumRadius;
Matrix3x3.Transform(ref newContact.Normal, ref orientation, out newContact.Normal);
newContact.Validate();
//Do not yet create a new contact. Check to see if an 'inner contact' with id == 2 already exists.
bool addContact = true;
for (int i = 0; i < contacts.Count; i++)
{
if (contacts.Elements[i].Id == 2)
{
contacts.Elements[i].Position = newContact.Position;
contacts.Elements[i].Normal = newContact.Normal;
contacts.Elements[i].PenetrationDepth = newContact.PenetrationDepth;
supplementData.Elements[i].BasePenetrationDepth = newContact.PenetrationDepth;
supplementData.Elements[i].LocalOffsetA = new Vector3();
supplementData.Elements[i].LocalOffsetB = ray.Position; //convex local position in mesh.
addContact = false;
break;
}
}
if (addContact && contacts.Count == 0)
{
Add(ref newContact);
}
previousDepth = newContact.PenetrationDepth;
}
else
{
//It's possible that we had a false negative. The previous frame may have been in deep intersection, and this frame just failed to come to the same conclusion.
//If we set the target location to the current location, the object will never escape the mesh. Instead, only do that if two frames agree that we are no longer colliding.
if (previousDepth > 0)
{
//We're not touching the mesh.
lastValidConvexPosition = ray.Position;
}
previousDepth = 0;
}
}
}
