public override void Update(float dt)
{
//Now, generate a contact between the two shapes.
float distance;
Vector3 axis;
var manifold = new TinyStructList <BoxContactData>();
if (BoxBoxCollider.AreBoxesColliding(boxA.Shape, boxB.Shape, ref boxA.worldTransform, ref boxB.worldTransform, out distance, out axis, out manifold))
{
Vector3.Negate(ref axis, out axis);
TinyList <int> toRemove = new TinyList <int>();
BoxContactData data;
for (int i = 0; i < contacts.Count; i++)
{
bool found = false;
for (int j = manifold.Count - 1; j >= 0; j--)
{
manifold.Get(j, out data);
if (contacts.Elements[i].Id == data.Id)
{
found = true;
//Update contact...
contacts.Elements[i].Position = data.Position;
contacts.Elements[i].PenetrationDepth = -data.Depth;
contacts.Elements[i].Normal = axis;
contacts.Elements[i].Validate();
//Remove manifold entry
manifold.RemoveAt(j);
break;
}
}
if (!found)
{//No match found
toRemove.Add(i);
}
}
////Go through the indices to remove.
////For each one, replace the removal index with a contact in the new manifold.
//int removalIndex;
//for (removalIndex = toRemove.count - 1; removalIndex >= 0 && manifold.count > 0; removalIndex--)
//{
// int indexToReplace = toRemove[removalIndex];
// toRemove.RemoveAt(removalIndex);
// manifold.Get(manifold.count - 1, out data);
// //Update contact...
// contacts.Elements[indexToReplace].Position = data.Position;
// contacts.Elements[indexToReplace].PenetrationDepth = -data.Depth;
// contacts.Elements[indexToReplace].Normal = axis;
// contacts.Elements[indexToReplace].Id = data.Id;
// //Remove manifold entry
// manifold.RemoveAt(manifold.count - 1);
//}
//Alright, we ran out of contacts to replace (if, in fact, toRemove isn't empty now). Just remove the remainder.
//toRemove is sorted by increasing index. Go backwards along it so that the indices are valid all the way through.
for (int i = toRemove.Count - 1; i >= 0; i--)
{
Remove(toRemove[i]);
}
//Add new contacts.
for (int i = 0; i < manifold.Count; i++)
{
manifold.Get(i, out data);
ContactData newContact = new ContactData();
newContact.Position = data.Position;
newContact.PenetrationDepth = -data.Depth;
newContact.Normal = axis;
newContact.Id = data.Id;
Add(ref newContact);
}
}
else
{
//Not colliding, so get rid of it.
for (int i = contacts.Count - 1; i >= 0; i--)
{
Remove(i);
}
}
}
///<summary>
/// Updates the manifold.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//Now, generate a contact between the two shapes.
float distance;
Vector3 axis;
BoxContactDataCache manifold;
if (BoxBoxCollider.AreBoxesColliding(boxA.Shape, boxB.Shape, ref boxA.worldTransform, ref boxB.worldTransform, out distance, out axis, out manifold))
{
unsafe
{
BoxContactData *manifoldPointer = &manifold.D1;
Vector3.Negate(ref axis, out axis);
var toRemove = new TinyList <int>();
for (int i = 0; i < contacts.Count; i++)
{
bool found = false;
for (int j = manifold.Count - 1; j >= 0; j--)
{
if (contacts.Elements[i].Id == manifoldPointer[j].Id)
{
found = true;
contacts.Elements[i].Validate();
//Update contact...
contacts.Elements[i].Position = manifoldPointer[j].Position;
contacts.Elements[i].PenetrationDepth = -manifoldPointer[j].Depth;
contacts.Elements[i].Normal = axis;
//Remove manifold entry
contacts.Elements[i].Validate();
manifold.RemoveAt(j);
break;
}
}
if (!found)
{//No match found
toRemove.Add(i);
}
}
//toRemove is sorted by increasing index. Go backwards along it so that the indices are valid all the way through.
for (int i = toRemove.Count - 1; i >= 0; i--)
{
Remove(toRemove[i]);
}
//Add new contacts.
for (int i = 0; i < manifold.Count; i++)
{
var newContact = new ContactData
{
Position = manifoldPointer[i].Position,
PenetrationDepth = -manifoldPointer[i].Depth,
Normal = axis,
Id = manifoldPointer[i].Id
};
Add(ref newContact);
}
}
}
else
{
//Not colliding, so get rid of it.
for (int i = contacts.Count - 1; i >= 0; i--)
{
Remove(i);
}
}
}
bool AnalyzeCandidate(ref TriangleIndices indices, TriangleShape triangle, TrianglePairTester pairTester, ref ContactData contact, ref BoundarySets sets)
{
switch (pairTester.GetRegion(triangle, ref contact))
{
case VoronoiRegion.A:
//Add the contact.
VertexContact vertexContact;
GetNormal(ref contact.Normal, triangle, out vertexContact.CorrectedNormal);
vertexContact.ContactData = contact;
vertexContact.Vertex = indices.A;
vertexContact.ShouldCorrect = pairTester.ShouldCorrectContactNormal;
sets.VertexContacts.Add(ref vertexContact);
//Block all of the other voronoi regions.
sets.BlockedEdgeRegions.Add(new Edge(indices.A, indices.B));
sets.BlockedEdgeRegions.Add(new Edge(indices.B, indices.C));
sets.BlockedEdgeRegions.Add(new Edge(indices.A, indices.C));
sets.BlockedVertexRegions.Add(indices.B);
sets.BlockedVertexRegions.Add(indices.C);
break;
case VoronoiRegion.B:
//Add the contact.
GetNormal(ref contact.Normal, triangle, out vertexContact.CorrectedNormal);
vertexContact.ContactData = contact;
vertexContact.Vertex = indices.B;
vertexContact.ShouldCorrect = pairTester.ShouldCorrectContactNormal;
sets.VertexContacts.Add(ref vertexContact);
//Block all of the other voronoi regions.
sets.BlockedEdgeRegions.Add(new Edge(indices.A, indices.B));
sets.BlockedEdgeRegions.Add(new Edge(indices.B, indices.C));
sets.BlockedEdgeRegions.Add(new Edge(indices.A, indices.C));
sets.BlockedVertexRegions.Add(indices.A);
sets.BlockedVertexRegions.Add(indices.C);
break;
case VoronoiRegion.C:
//Add the contact.
GetNormal(ref contact.Normal, triangle, out vertexContact.CorrectedNormal);
vertexContact.ContactData = contact;
vertexContact.Vertex = indices.C;
vertexContact.ShouldCorrect = pairTester.ShouldCorrectContactNormal;
sets.VertexContacts.Add(ref vertexContact);
//Block all of the other voronoi regions.
sets.BlockedEdgeRegions.Add(new Edge(indices.A, indices.B));
sets.BlockedEdgeRegions.Add(new Edge(indices.B, indices.C));
sets.BlockedEdgeRegions.Add(new Edge(indices.A, indices.C));
sets.BlockedVertexRegions.Add(indices.A);
sets.BlockedVertexRegions.Add(indices.B);
break;
case VoronoiRegion.AB:
//Add the contact.
EdgeContact edgeContact;
GetNormal(ref contact.Normal, triangle, out edgeContact.CorrectedNormal);
edgeContact.Edge = new Edge(indices.A, indices.B);
edgeContact.ContactData = contact;
edgeContact.ShouldCorrect = pairTester.ShouldCorrectContactNormal;
sets.EdgeContacts.Add(ref edgeContact);
//Block all of the other voronoi regions.
sets.BlockedEdgeRegions.Add(new Edge(indices.B, indices.C));
sets.BlockedEdgeRegions.Add(new Edge(indices.A, indices.C));
sets.BlockedVertexRegions.Add(indices.A);
sets.BlockedVertexRegions.Add(indices.B);
sets.BlockedVertexRegions.Add(indices.C);
break;
case VoronoiRegion.AC:
//Add the contact.
GetNormal(ref contact.Normal, triangle, out edgeContact.CorrectedNormal);
edgeContact.Edge = new Edge(indices.A, indices.C);
edgeContact.ContactData = contact;
edgeContact.ShouldCorrect = pairTester.ShouldCorrectContactNormal;
sets.EdgeContacts.Add(ref edgeContact);
//Block all of the other voronoi regions.
sets.BlockedEdgeRegions.Add(new Edge(indices.A, indices.B));
sets.BlockedEdgeRegions.Add(new Edge(indices.B, indices.C));
sets.BlockedVertexRegions.Add(indices.A);
sets.BlockedVertexRegions.Add(indices.B);
sets.BlockedVertexRegions.Add(indices.C);
break;
case VoronoiRegion.BC:
//Add the contact.
GetNormal(ref contact.Normal, triangle, out edgeContact.CorrectedNormal);
edgeContact.Edge = new Edge(indices.B, indices.C);
edgeContact.ContactData = contact;
edgeContact.ShouldCorrect = pairTester.ShouldCorrectContactNormal;
sets.EdgeContacts.Add(ref edgeContact);
//Block all of the other voronoi regions.
sets.BlockedEdgeRegions.Add(new Edge(indices.A, indices.B));
sets.BlockedEdgeRegions.Add(new Edge(indices.A, indices.C));
sets.BlockedVertexRegions.Add(indices.A);
sets.BlockedVertexRegions.Add(indices.B);
sets.BlockedVertexRegions.Add(indices.C);
break;
default:
//Block all of the other voronoi regions.
sets.BlockedEdgeRegions.Add(new Edge(indices.A, indices.B));
sets.BlockedEdgeRegions.Add(new Edge(indices.B, indices.C));
sets.BlockedEdgeRegions.Add(new Edge(indices.A, indices.C));
sets.BlockedVertexRegions.Add(indices.A);
sets.BlockedVertexRegions.Add(indices.B);
sets.BlockedVertexRegions.Add(indices.C);
//Should add the contact.
return(true);
}
return(false);
}
///<summary>
/// Tests if a box and sphere are colliding.
///</summary>
///<param name="box">Box to test.</param>
///<param name="sphere">Sphere to test.</param>
///<param name="boxTransform">Transform to apply to the box.</param>
///<param name="spherePosition">Transform to apply to the sphere.</param>
///<param name="contact">Contact point between the shapes, if any.</param>
///<returns>Whether or not the shapes were colliding.</returns>
public static bool AreShapesColliding(BoxShape box, SphereShape sphere, ref RigidTransform boxTransform, ref Vector3 spherePosition, out ContactData contact)
{
contact = new ContactData();
Vector3 localPosition;
RigidTransform.TransformByInverse(ref spherePosition, ref boxTransform, out localPosition);
#if !WINDOWS
Vector3 localClosestPoint = new Vector3();
#else
Vector3 localClosestPoint;
#endif
localClosestPoint.X = MathHelper.Clamp(localPosition.X, -box.halfWidth, box.halfWidth);
localClosestPoint.Y = MathHelper.Clamp(localPosition.Y, -box.halfHeight, box.halfHeight);
localClosestPoint.Z = MathHelper.Clamp(localPosition.Z, -box.halfLength, box.halfLength);
RigidTransform.Transform(ref localClosestPoint, ref boxTransform, out contact.Position);
Vector3 offset;
Vector3.Subtract(ref spherePosition, ref contact.Position, out offset);
float offsetLength = offset.LengthSquared();
if (offsetLength > (sphere.collisionMargin + CollisionDetectionSettings.maximumContactDistance) * (sphere.collisionMargin + CollisionDetectionSettings.maximumContactDistance))
{
return(false);
}
//Colliding.
if (offsetLength > Toolbox.Epsilon)
{
offsetLength = (float)Math.Sqrt(offsetLength);
//Outside of the box.
Vector3.Divide(ref offset, offsetLength, out contact.Normal);
contact.PenetrationDepth = sphere.collisionMargin - offsetLength;
}
else
{
//Inside of the box.
Vector3 penetrationDepths;
penetrationDepths.X = localClosestPoint.X < 0 ? localClosestPoint.X + box.halfWidth : box.halfWidth - localClosestPoint.X;
penetrationDepths.Y = localClosestPoint.Y < 0 ? localClosestPoint.Y + box.halfHeight : box.halfHeight - localClosestPoint.Y;
penetrationDepths.Z = localClosestPoint.Z < 0 ? localClosestPoint.Z + box.halfLength : box.halfLength - localClosestPoint.Z;
if (penetrationDepths.X < penetrationDepths.Y && penetrationDepths.X < penetrationDepths.Z)
{
contact.Normal = localClosestPoint.X > 0 ? Toolbox.RightVector : Toolbox.LeftVector;
contact.PenetrationDepth = penetrationDepths.X;
}
else if (penetrationDepths.Y < penetrationDepths.Z)
{
contact.Normal = localClosestPoint.Y > 0 ? Toolbox.UpVector : Toolbox.DownVector;
contact.PenetrationDepth = penetrationDepths.Y;
}
else
{
contact.Normal = localClosestPoint.Z > 0 ? Toolbox.BackVector : Toolbox.ForwardVector;
contact.PenetrationDepth = penetrationDepths.Z;
}
contact.PenetrationDepth += sphere.collisionMargin;
Quaternion.Transform(ref contact.Normal, ref boxTransform.Orientation, out contact.Normal);
}
return(true);
}
//This works in the specific case of 4 contacts and 1 contact candidate.
///<summary>
/// Reduces a 4-contact manifold and contact candidate to 4 total contacts.
///</summary>
///<param name="contacts">Contacts to reduce.</param>
///<param name="contactCandidate">Contact candidate to include in the reduction process.</param>
///<param name="toRemove">Contacts that need to be removed to reduce the manifold.</param>
///<param name="addCandidate">Whether or not to add the contact candidate to reach the reduced manifold.</param>
///<exception cref="ArgumentException">Thrown when the contact manifold being reduced doesn't have 4 contacts.</exception>
public static void ReduceContacts(RawList <Contact> contacts, ref ContactData contactCandidate, RawList <int> toRemove, out bool addCandidate)
{
if (contacts.Count != 4)
{
throw new ArgumentException("Can only use this method to reduce contact lists with four contacts and a contact candidate.");
}
//Find the deepest point of all contacts/candidates, as well as a compounded 'normal' vector.
float maximumDepth = -float.MaxValue;
int deepestIndex = -1;
for (int i = 0; i < 4; i++)
{
if (contacts.Elements[i].PenetrationDepth > maximumDepth)
{
deepestIndex = i;
maximumDepth = contacts.Elements[i].PenetrationDepth;
}
}
if (contactCandidate.PenetrationDepth > maximumDepth)
{
deepestIndex = 4;
}
//Find the contact (candidate) that is furthest away from the deepest contact (candidate).
Vector3 deepestPosition;
if (deepestIndex < 4)
{
deepestPosition = contacts.Elements[deepestIndex].Position;
}
else
{
deepestPosition = contactCandidate.Position;
}
float distanceSquared;
float furthestDistance = 0;
int furthestIndex = -1;
for (int i = 0; i < 4; i++)
{
Vector3.DistanceSquared(ref contacts.Elements[i].Position, ref deepestPosition, out distanceSquared);
if (distanceSquared > furthestDistance)
{
furthestDistance = distanceSquared;
furthestIndex = i;
}
}
Vector3.DistanceSquared(ref contactCandidate.Position, ref deepestPosition, out distanceSquared);
if (distanceSquared > furthestDistance)
{
furthestIndex = 4;
}
Vector3 furthestPosition;
if (furthestIndex < contacts.Count)
{
furthestPosition = contacts.Elements[furthestIndex].Position;
}
else
{
furthestPosition = contactCandidate.Position;
}
Vector3 xAxis;
Vector3.Subtract(ref deepestPosition, ref furthestPosition, out xAxis);
//Create the second axis of the 2d 'coordinate system' of the manifold.
Vector3 yAxis;
Vector3.Cross(ref xAxis, ref contacts.Elements[0].Normal, out yAxis);
//Determine the furthest points along the axis.
float minYAxisDot = float.MaxValue, maxYAxisDot = -float.MaxValue;
int minYAxisIndex = -1, maxYAxisIndex = -1;
float dot;
for (int i = 0; i < 4; i++)
{
Vector3.Dot(ref contacts.Elements[i].Position, ref yAxis, out dot);
if (dot < minYAxisDot)
{
minYAxisIndex = i;
minYAxisDot = dot;
}
if (dot > maxYAxisDot)
{
maxYAxisIndex = i;
maxYAxisDot = dot;
}
}
Vector3.Dot(ref contactCandidate.Position, ref yAxis, out dot);
if (dot < minYAxisDot)
{
minYAxisIndex = 4;
}
if (dot > maxYAxisDot)
{
maxYAxisIndex = 4;
}
//the deepestIndex, furthestIndex, minYAxisIndex, and maxYAxisIndex are the extremal points.
//Cycle through the existing contacts. If any DO NOT MATCH the existing candidates, add them to the toRemove list.
//Cycle through the candidates. If any match, add them to the toAdd list.
//Repeated entries in the reduced manifold aren't a problem.
//-Contacts list does not include repeats with itself.
//-A contact is only removed if it doesn't match anything.
//-Contact candidates do not repeat with themselves.
//-Contact candidates do not repeat with contacts.
//-Contact candidates are added if they match any of the indices.
if (4 == deepestIndex || 4 == furthestIndex || 4 == minYAxisIndex || 4 == maxYAxisIndex)
{
addCandidate = true;
//Only reduce when we are going to add a new contact, and only get rid of one.
for (int i = 0; i < 4; i++)
{
if (!(i == deepestIndex || i == furthestIndex || i == minYAxisIndex || i == maxYAxisIndex))
{
//This contact is not present in the new manifold. Remove it.
toRemove.Add(i);
break;
}
}
}
else
{
addCandidate = false;
}
}
public static void Validate(this ContactData contact)
{
contact.Normal.Validate();
contact.Position.Validate();
contact.PenetrationDepth.Validate();
}
public override VoronoiRegion GetRegion(TriangleShape triangle, ref ContactData contact)
{
//Deep contact can produce non-triangle normals while still being within the triangle.
//To solve this problem, find the voronoi region to which the contact belongs using its normal.
//The voronoi region will be either the most extreme vertex, or the edge that includes
//the first and second most extreme vertices.
//If the normal dotted with an extreme edge direction is near 0, then it belongs to the edge.
//Otherwise, it belongs to the vertex.
//MPR tends to produce 'approximate' normals, though.
//Use a fairly forgiving epsilon.
float dotA, dotB, dotC;
Vector3.Dot(ref triangle.vA, ref contact.Normal, out dotA);
Vector3.Dot(ref triangle.vB, ref contact.Normal, out dotB);
Vector3.Dot(ref triangle.vC, ref contact.Normal, out dotC);
//Since normal points from convex to triangle always, reverse dot signs.
dotA = -dotA;
dotB = -dotB;
dotC = -dotC;
float faceEpsilon = .01f;
const float edgeEpsilon = .01f;
float edgeDot;
Vector3 edgeDirection;
if (dotA > dotB && dotA > dotC)
{
//A is extreme.
if (dotB > dotC)
{
//B is second most extreme.
if (Math.Abs(dotA - dotC) < faceEpsilon)
{
//The normal is basically a face normal. This can happen at the edges occasionally.
return(VoronoiRegion.ABC);
}
else
{
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out edgeDirection);
Vector3.Dot(ref edgeDirection, ref contact.Normal, out edgeDot);
if (edgeDot * edgeDot < edgeDirection.LengthSquared() * edgeEpsilon)
{
return(VoronoiRegion.AB);
}
else
{
return(VoronoiRegion.A);
}
}
}
else
{
//C is second most extreme.
if (Math.Abs(dotA - dotB) < faceEpsilon)
{
//The normal is basically a face normal. This can happen at the edges occasionally.
return(VoronoiRegion.ABC);
}
else
{
Vector3.Subtract(ref triangle.vC, ref triangle.vA, out edgeDirection);
Vector3.Dot(ref edgeDirection, ref contact.Normal, out edgeDot);
if (edgeDot * edgeDot < edgeDirection.LengthSquared() * edgeEpsilon)
{
return(VoronoiRegion.AC);
}
else
{
return(VoronoiRegion.A);
}
}
}
}
else if (dotB > dotC)
{
//B is extreme.
if (dotC > dotA)
{
//C is second most extreme.
if (Math.Abs(dotB - dotA) < faceEpsilon)
{
//The normal is basically a face normal. This can happen at the edges occasionally.
return(VoronoiRegion.ABC);
}
else
{
Vector3.Subtract(ref triangle.vC, ref triangle.vB, out edgeDirection);
Vector3.Dot(ref edgeDirection, ref contact.Normal, out edgeDot);
if (edgeDot * edgeDot < edgeDirection.LengthSquared() * edgeEpsilon)
{
return(VoronoiRegion.BC);
}
else
{
return(VoronoiRegion.B);
}
}
}
else
{
//A is second most extreme.
if (Math.Abs(dotB - dotC) < faceEpsilon)
{
//The normal is basically a face normal. This can happen at the edges occasionally.
return(VoronoiRegion.ABC);
}
else
{
Vector3.Subtract(ref triangle.vA, ref triangle.vB, out edgeDirection);
Vector3.Dot(ref edgeDirection, ref contact.Normal, out edgeDot);
if (edgeDot * edgeDot < edgeDirection.LengthSquared() * edgeEpsilon)
{
return(VoronoiRegion.AB);
}
else
{
return(VoronoiRegion.B);
}
}
}
}
else
{
//C is extreme.
if (dotA > dotB)
{
//A is second most extreme.
if (Math.Abs(dotC - dotB) < faceEpsilon)
{
//The normal is basically a face normal. This can happen at the edges occasionally.
return(VoronoiRegion.ABC);
}
else
{
Vector3.Subtract(ref triangle.vA, ref triangle.vC, out edgeDirection);
Vector3.Dot(ref edgeDirection, ref contact.Normal, out edgeDot);
if (edgeDot * edgeDot < edgeDirection.LengthSquared() * edgeEpsilon)
{
return(VoronoiRegion.AC);
}
else
{
return(VoronoiRegion.C);
}
}
}
else
{
//B is second most extreme.
if (Math.Abs(dotC - dotA) < faceEpsilon)
{
//The normal is basically a face normal. This can happen at the edges occasionally.
return(VoronoiRegion.ABC);
}
else
{
Vector3.Subtract(ref triangle.vB, ref triangle.vC, out edgeDirection);
Vector3.Dot(ref edgeDirection, ref contact.Normal, out edgeDot);
if (edgeDot * edgeDot < edgeDirection.LengthSquared() * edgeEpsilon)
{
return(VoronoiRegion.BC);
}
else
{
return(VoronoiRegion.C);
}
}
}
}
}
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;
}
}
}
private bool DoPlaneTest(TriangleShape triangle, out TinyStructList <ContactData> contactList)
{
//Find closest point between object and plane.
Vector3 reverseNormal;
Vector3 ab, ac;
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out ab);
Vector3.Subtract(ref triangle.vC, ref triangle.vA, out ac);
Vector3.Cross(ref ac, ref ab, out reverseNormal);
//Convex position dot normal is ALWAYS zero. The thing to look at is the plane's 'd'.
//If the distance along the normal is positive, then the convex is 'behind' that normal.
float dotA;
Vector3.Dot(ref triangle.vA, ref reverseNormal, out dotA);
contactList = new TinyStructList <ContactData>();
switch (triangle.sidedness)
{
case TriangleSidedness.DoubleSided:
if (dotA < 0)
{
//The reverse normal is pointing towards the convex.
//It needs to point away from the convex so that the direction
//will get the proper extreme point.
Vector3.Negate(ref reverseNormal, out reverseNormal);
dotA = -dotA;
}
break;
case TriangleSidedness.Clockwise:
//if (dotA < 0)
//{
// //The reverse normal is pointing towards the convex.
// return false;
//}
break;
case TriangleSidedness.Counterclockwise:
//if (dotA > 0)
//{
// //The reverse normal is pointing away from the convex.
// return false;
//}
//The reverse normal is pointing towards the convex.
//It needs to point away from the convex so that the direction
//will get the proper extreme point.
Vector3.Negate(ref reverseNormal, out reverseNormal);
dotA = -dotA;
break;
}
Vector3 extremePoint;
convex.GetLocalExtremePointWithoutMargin(ref reverseNormal, out extremePoint);
//See if the extreme point is within the face or not.
//It might seem like the easy "depth" test should come first, since a barycentric
//calculation takes a bit more time. However, transferring from plane to depth is 'rare'
//(like all transitions), and putting this test here is logically closer to its requirements'
//computation.
if (GetVoronoiRegion(triangle, ref extremePoint) != VoronoiRegion.ABC)
{
state = CollisionState.ExternalSeparated;
return(DoExternalSeparated(triangle, out contactList));
}
float dotE;
Vector3.Dot(ref extremePoint, ref reverseNormal, out dotE);
float t = (dotA - dotE) / reverseNormal.LengthSquared();
Vector3 offset;
Vector3.Multiply(ref reverseNormal, t, out offset);
//Compare the distance from the plane to the convex object.
float distanceSquared = offset.LengthSquared();
float marginSum = triangle.collisionMargin + convex.collisionMargin;
//TODO: Could just normalize early and avoid computing point plane before it's necessary.
//Exposes a sqrt but...
if (t <= 0 || distanceSquared < marginSum * marginSum)
{
//The convex object is in the margin of the plane.
//All that's left is to create the contact.
var contact = new ContactData();
//Displacement is from A to B. point = A + t * AB, where t = marginA / margin.
if (marginSum > Toolbox.Epsilon) //This can be zero! It would cause a NaN is unprotected.
{
Vector3.Multiply(ref offset, convex.collisionMargin / marginSum, out contact.Position); //t * AB
}
else
{
contact.Position = new Vector3();
}
Vector3.Add(ref extremePoint, ref contact.Position, out contact.Position); //A + t * AB.
float normalLength = reverseNormal.Length();
Vector3.Divide(ref reverseNormal, normalLength, out contact.Normal);
float distance = normalLength * t;
contact.PenetrationDepth = marginSum - distance;
if (contact.PenetrationDepth > marginSum)
{
//Check to see if the inner sphere is touching the plane.
//This does not override other tests; there can be more than one contact from a single triangle.
ContactData alternateContact;
if (TryInnerSphereContact(triangle, out alternateContact))// && alternateContact.PenetrationDepth > contact.PenetrationDepth)
{
contactList.Add(ref alternateContact);
}
//The convex object is stuck deep in the plane!
//The most problematic case for this is when
//an object is right on top of a cliff.
//The lower, vertical triangle may occasionally detect
//a contact with the object, but would compute an extremely
//deep depth if the normal plane test was used.
//Verify that the depth is correct by trying another approach.
CollisionState previousState = state;
state = CollisionState.ExternalNear;
TinyStructList <ContactData> alternateContacts;
if (DoExternalNear(triangle, out alternateContacts))
{
alternateContacts.Get(0, out alternateContact);
if (alternateContact.PenetrationDepth + .01f < contact.PenetrationDepth) //Bias against the subtest's result, since the plane version will probably have a better position.
{
//It WAS a bad contact.
contactList.Add(ref alternateContact);
//DoDeepContact (which can be called from within DoExternalNear) can generate two contacts, but the second contact would just be an inner sphere (which we already generated).
//DoExternalNear can only generate one contact. So we only need the first contact!
//TODO: This is a fairly fragile connection between the two stages. Consider robustifying. (Also, the TryInnerSphereContact is done twice! This process is very rare for marginful pairs, though)
}
else
{
//Well, it really is just that deep.
contactList.Add(ref contact);
state = previousState;
}
}
else
{
//If the external near test finds that there was no collision at all,
//just return to plane testing. If the point turns up outside the face region
//next time, the system will adapt.
state = previousState;
return(false);
}
}
else
{
contactList.Add(ref contact);
}
return(true);
}
return(false);
}
private bool TryInnerSphereContact(TriangleShape triangle, out ContactData contact)
{
Vector3 closestPoint;
Toolbox.GetClosestPointOnTriangleToPoint(ref triangle.vA, ref triangle.vB, ref triangle.vC, ref Toolbox.ZeroVector, out closestPoint);
float length = closestPoint.LengthSquared();
float minimumRadius = convex.MinimumRadius * (MotionSettings.CoreShapeScaling + .01f);
if (length < minimumRadius * minimumRadius)
{
Vector3 triangleNormal, ab, ac;
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out ab);
Vector3.Subtract(ref triangle.vC, ref triangle.vA, out ac);
Vector3.Cross(ref ab, ref ac, out triangleNormal);
float dot;
Vector3.Dot(ref closestPoint, ref triangleNormal, out dot);
if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
{
//Normal was facing the wrong way.
contact = new ContactData();
return(false);
}
length = (float)Math.Sqrt(length);
contact.Position = closestPoint;
if (length > Toolbox.Epsilon) //Watch out for NaN's!
{
Vector3.Divide(ref closestPoint, length, out contact.Normal);
}
else
{
//The direction is undefined. Use the triangle's normal.
//One sided triangles can only face in the appropriate direction.
float normalLength = triangleNormal.LengthSquared();
if (triangleNormal.LengthSquared() > Toolbox.Epsilon)
{
Vector3.Divide(ref triangleNormal, (float)Math.Sqrt(normalLength), out triangleNormal);
if (triangle.sidedness == TriangleSidedness.Clockwise)
{
contact.Normal = triangleNormal;
}
else
{
Vector3.Negate(ref triangleNormal, out contact.Normal);
}
}
else
{
//Degenerate triangle!
contact = new ContactData();
return(false);
}
}
//Compute the actual depth of the contact.
//This is conservative; the minimum radius is guaranteed to be no larger than the shape itself.
//But that's ok- this is strictly a deep contact protection scheme. Other contacts will make the objects separate.
contact.PenetrationDepth = convex.MinimumRadius - length;
contact.Id = -1;
return(true);
}
contact = new ContactData();
return(false);
}
private bool DoExternalNear(TriangleShape triangle, out TinyStructList <ContactData> contactList)
{
Vector3 closestA, closestB;
//Don't bother trying to do any clever caching. The continually transforming simplex makes it very rarely useful.
//TODO: Initialize the simplex of the GJK method using the 'true' center of the triangle.
//If left unmodified, the simplex that is used in GJK will just be a point at 0,0,0, which of course is at the origin.
//This causes an instant-out, always. Not good!
//By giving the contributing simplex the average centroid, it has a better guess.
Vector3 triangleCentroid;
Vector3.Add(ref triangle.vA, ref triangle.vB, out triangleCentroid);
Vector3.Add(ref triangleCentroid, ref triangle.vC, out triangleCentroid);
Vector3.Multiply(ref triangleCentroid, .33333333f, out triangleCentroid);
var initialSimplex = new CachedSimplex {
State = SimplexState.Point, LocalSimplexB = { A = triangleCentroid }
};
if (GJKToolbox.GetClosestPoints(convex, triangle, ref Toolbox.RigidIdentity, ref Toolbox.RigidIdentity, ref initialSimplex, out closestA, out closestB))
{
state = CollisionState.Deep;
return(DoDeepContact(triangle, out contactList));
}
Vector3 displacement;
Vector3.Subtract(ref closestB, ref closestA, out displacement);
float distanceSquared = displacement.LengthSquared();
float margin = convex.collisionMargin + triangle.collisionMargin;
contactList = new TinyStructList <ContactData>();
if (distanceSquared < margin * margin)
{
//Try to generate a contact.
var contact = new ContactData();
//Determine if the normal points in the appropriate direction given the sidedness of the triangle.
if (triangle.sidedness != TriangleSidedness.DoubleSided)
{
Vector3 triangleNormal, ab, ac;
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out ab);
Vector3.Subtract(ref triangle.vC, ref triangle.vA, out ac);
Vector3.Cross(ref ab, ref ac, out triangleNormal);
float dot;
Vector3.Dot(ref triangleNormal, ref displacement, out dot);
if (triangle.sidedness == TriangleSidedness.Clockwise && dot > 0)
{
return(false);
}
if (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0)
{
return(false);
}
}
//Displacement is from A to B. point = A + t * AB, where t = marginA / margin.
if (margin > Toolbox.Epsilon) //This can be zero! It would cause a NaN if unprotected.
{
Vector3.Multiply(ref displacement, convex.collisionMargin / margin, out contact.Position); //t * AB
}
else
{
contact.Position = new Vector3();
}
Vector3.Add(ref closestA, ref contact.Position, out contact.Position); //A + t * AB.
contact.Normal = displacement;
float distance = (float)Math.Sqrt(distanceSquared);
Vector3.Divide(ref contact.Normal, distance, out contact.Normal);
contact.PenetrationDepth = margin - distance;
contactList.Add(ref contact);
TryToEscape(triangle, ref contact.Position);
return(true);
}
//Too far to make a contact- move back to separation.
state = CollisionState.ExternalSeparated;
return(false);
}
bool TryToStepUsingContact(ref ContactData contact, ref Vector3 down, out Vector3 newPosition)
{
Vector3 position = characterBody.Position;
//The normal of the contact may not be facing perfectly out to the side.
//The detection process allows a bit of slop.
//Correct it by removing any component of the normal along the local up vector.
Vector3 normal = contact.Normal;
float dot;
Vector3.Dot(ref normal, ref down, out dot);
Vector3 error;
Vector3.Multiply(ref down, dot, out error);
Vector3.Subtract(ref normal, ref error, out normal);
normal.Normalize();
//Now we need to ray cast out from the center of the character in the direction of this normal to check for obstructions.
//Compute the ray origin location. Fire it out of the top of the character; if we're stepping, this must be a valid location.
//Putting it as high as possible helps to reject more invalid step geometry.
Ray ray;
float downRayLength = characterBody.Height;// MaximumStepHeight + upStepMargin;
Vector3.Multiply(ref down, characterBody.Height * .5f - downRayLength, out ray.Position);
Vector3.Add(ref ray.Position, ref position, out ray.Position);
ray.Direction = normal;
//Include a little margin in the length.
//Technically, the character only needs to teleport horizontally by the complicated commented expression.
//That puts it just far enough to have traction on the new surface.
//In practice, the current contact refreshing approach used for many pair types causes contacts to persist horizontally a bit,
//which can cause side effects for the character.
float horizontalOffsetAmount = characterBody.CollisionInformation.Shape.CollisionMargin; // (float)((1 - character.SupportFinder.sinMaximumSlope) * character.Body.CollisionInformation.Shape.CollisionMargin + 0);
float length = characterBody.Radius + horizontalOffsetAmount; // -contact.PenetrationDepth;
if (QueryManager.RayCastHitAnything(ray, length))
{
//The step is obstructed!
newPosition = new Vector3();
return(false);
}
//The down-cast ray origin has been verified by the previous ray cast.
//Let's look for a support!
Vector3 horizontalOffset;
Vector3.Multiply(ref normal, length, out horizontalOffset);
Vector3.Add(ref ray.Position, ref horizontalOffset, out ray.Position);
ray.Direction = down;
//Find the earliest hit, if any.
RayHit earliestHit;
if (!QueryManager.RayCast(ray, downRayLength, out earliestHit) || //Can't do anything if it didn't hit.
earliestHit.T <= 0 || //Can't do anything if the hit was invalid.
earliestHit.T - downRayLength > -minimumUpStepHeight || //Don't bother doing anything if the step is too small.
earliestHit.T - downRayLength < -maximumStepHeight - upStepMargin) //Can't do anything if the step is too tall.
{
//No valid hit was detected.
newPosition = new Vector3();
return(false);
}
//Ensure the candidate surface supports traction.
Vector3 supportNormal;
Vector3.Normalize(ref earliestHit.Normal, out supportNormal);
//Calibrate the normal to face in the same direction as the down vector for consistency.
Vector3.Dot(ref supportNormal, ref down, out dot);
if (dot < 0)
{
Vector3.Negate(ref supportNormal, out supportNormal);
dot = -dot;
}
//If the new surface does not have traction, do not attempt to step up.
if (dot < ContactCategorizer.TractionThreshold)
{
newPosition = new Vector3();
return(false);
}
//Since contact queries are frequently expensive compared to ray cast tests,
//do one more ray cast test. This time, starting from the same position, cast upwards.
//In order to step up, the previous down-ray hit must be at least a character height away from the result of the up-ray.
Vector3.Negate(ref down, out ray.Direction);
//Find the earliest hit, if any.
//RayHit earliestHitUp = new RayHit();
//earliestHitUp.T = float.MaxValue;
float upLength = characterBody.Height - earliestHit.T;
//If the sum of the up and down distances is less than the height, the character can't fit.
if (QueryManager.RayCastHitAnything(ray, upLength))
{
newPosition = new Vector3();
return(false);
}
//By now, a valid ray hit has been found. Now we need to validate it using contact queries.
//This process is very similar in concept to the down step verification, but it has some extra
//requirements.
//Predict a hit location based on the time of impact and the normal at the intersection.
//Take into account the radius of the character (don't forget the collision margin!)
RigidTransform transform = characterBody.CollisionInformation.WorldTransform;
//The transform must be modified to position the query body at the right location.
//The horizontal offset of the queries ensures that a tractionable part of the character will be put onto the new support.
Vector3.Multiply(ref normal, horizontalOffsetAmount, out horizontalOffset);
Vector3.Add(ref transform.Position, ref horizontalOffset, out transform.Position);
Vector3 verticalOffset;
Vector3.Multiply(ref down, -downRayLength, out verticalOffset);
Vector3.Add(ref transform.Position, ref verticalOffset, out transform.Position);
//We know that the closest point to the plane will be the extreme point in the plane's direction.
//Use it as the ray origin.
Ray downRay;
characterBody.CollisionInformation.Shape.GetExtremePoint(supportNormal, ref transform, out downRay.Position);
downRay.Direction = down;
//Intersect the ray against the plane defined by the support hit.
Vector3 intersection;
Vector3.Dot(ref earliestHit.Location, ref supportNormal, out dot);
Plane plane = new Plane(supportNormal, dot);
Vector3 candidatePosition;
//Define the interval bounds to be used later.
//The words 'highest' and 'lowest' here refer to the position relative to the character's body.
//The ray cast points downward relative to the character's body.
float highestBound = -maximumStepHeight;
float lowestBound = characterBody.CollisionInformation.Shape.CollisionMargin - downRayLength + earliestHit.T;
float currentOffset = lowestBound;
float hintOffset;
var tractionContacts = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);
var supportContacts = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);
var sideContacts = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);
var headContacts = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);
try
{
//This guess may either win immediately, or at least give us a better idea of where to search.
float hitT;
if (Toolbox.GetRayPlaneIntersection(ref downRay, ref plane, out hitT, out intersection))
{
hitT = -downRayLength + hitT + CollisionDetectionSettings.AllowedPenetration;
if (hitT < highestBound)
{
//Don't try a location known to be too high.
hitT = highestBound;
}
currentOffset = hitT;
if (currentOffset > lowestBound)
{
lowestBound = currentOffset;
}
candidatePosition = characterBody.Position + down * currentOffset + horizontalOffset;
switch (TryUpStepPosition(ref normal, ref candidatePosition, ref down,
ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts,
out hintOffset))
{
case CharacterContactPositionState.Accepted:
currentOffset += hintOffset;
//Only use the new position location if the movement distance was the right size.
if (currentOffset < 0 && currentOffset > -maximumStepHeight - CollisionDetectionSettings.AllowedPenetration)
{
//It's possible that we let a just-barely-too-high step occur, limited by the allowed penetration.
//Just clamp the overall motion and let it penetrate a bit.
newPosition = characterBody.Position + Math.Max(-maximumStepHeight, currentOffset) * down + horizontalOffset;
return(true);
}
else
{
newPosition = new Vector3();
return(false);
}
case CharacterContactPositionState.Rejected:
newPosition = new Vector3();
return(false);
case CharacterContactPositionState.NoHit:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.Obstructed:
lowestBound = currentOffset;
currentOffset = (highestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.HeadObstructed:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.TooDeep:
currentOffset += hintOffset;
lowestBound = currentOffset;
break;
}
} //TODO: If the ray cast doesn't hit, that could be used to early out... Then again, it pretty much can't happen.
//Our guesses failed.
//Begin the regular process. Start at the time of impact of the ray itself.
//How about trying the time of impact of the ray itself?
//Since we wouldn't be here unless there were no contacts at the body's current position,
//testing the ray cast location gives us the second bound we need to do an informed binary search.
int attempts = 0;
//Don't keep querying indefinitely. If we fail to reach it in a few informed steps, it's probably not worth continuing.
//The bound size check prevents the system from continuing to search a meaninglessly tiny interval.
while (attempts++ < 5 && lowestBound - highestBound > Toolbox.BigEpsilon)
{
candidatePosition = characterBody.Position + currentOffset * down + horizontalOffset;
switch (TryUpStepPosition(ref normal, ref candidatePosition, ref down,
ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts,
out hintOffset))
{
case CharacterContactPositionState.Accepted:
currentOffset += hintOffset;
//Only use the new position location if the movement distance was the right size.
if (currentOffset < 0 && currentOffset > -maximumStepHeight - CollisionDetectionSettings.AllowedPenetration)
{
//It's possible that we let a just-barely-too-high step occur, limited by the allowed penetration.
//Just clamp the overall motion and let it penetrate a bit.
newPosition = characterBody.Position + Math.Max(-maximumStepHeight, currentOffset) * down + horizontalOffset;
return(true);
}
else
{
newPosition = new Vector3();
return(false);
}
case CharacterContactPositionState.Rejected:
newPosition = new Vector3();
return(false);
case CharacterContactPositionState.NoHit:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + highestBound) * .5f;
break;
case CharacterContactPositionState.Obstructed:
lowestBound = currentOffset;
currentOffset = (highestBound + lowestBound) * .5f;
break;
case CharacterContactPositionState.HeadObstructed:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.TooDeep:
currentOffset += hintOffset;
lowestBound = currentOffset;
break;
}
}
}
finally
{
tractionContacts.Dispose();
supportContacts.Dispose();
sideContacts.Dispose();
headContacts.Dispose();
}
//Couldn't find a candidate.
newPosition = new Vector3();
return(false);
}
private bool DoDeepContact(out ContactData contact)
{
#region Informed search
if (previousState == CollisionState.Separated) //If it was shallow before, then its closest points will be used to find the normal.
{
//It's overlapping! Find the relative velocity at the point relative to the two objects. The point is still in local space!
//Vector3 velocityA;
//Vector3.Cross(ref contact.Position, ref collidableA.entity.angularVelocity, out velocityA);
//Vector3.Add(ref velocityA, ref collidableA.entity.linearVelocity, out velocityA);
//Vector3 velocityB;
//Vector3.Subtract(ref contact.Position, ref localTransformB.Position, out velocityB);
//Vector3.Cross(ref velocityB, ref collidableB.entity.angularVelocity, out velocityB);
//Vector3.Add(ref velocityB, ref collidableB.entity.linearVelocity, out velocityB);
////The velocity is negated because the direction so point backwards along the velocity.
//Vector3.Subtract(ref velocityA, ref velocityB, out localDirection);
//The above takes into account angular velocity, but linear velocity alone is a lot more stable and does the job just fine.
if (collidableA.entity != null && collidableB.entity != null)
{
Vector3.Subtract(ref collidableA.entity.linearVelocity, ref collidableB.entity.linearVelocity, out localDirection);
}
else
{
localDirection = localSeparatingAxis;
}
if (localDirection.LengthSquared() < Toolbox.Epsilon)
{
localDirection = Vector3.Up;
}
}
if (MPRToolbox.GetContact(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref localDirection, out contact))
{
if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin)
{
state = CollisionState.ShallowContact;
}
return(true);
}
//This is rare, but could happen.
state = CollisionState.Separated;
return(false);
//if (MPRTesting.GetLocalOverlapPosition(collidableA.Shape, collidableB.Shape, ref localTransformB, out contact.Position))
//{
// //First, try to use the heuristically found direction. This comes from either the GJK shallow contact separating axis or from the relative velocity.
// Vector3 rayCastDirection;
// float lengthSquared = localDirection.LengthSquared();
// if (lengthSquared > Toolbox.Epsilon)
// {
// Vector3.Divide(ref localDirection, (float)Math.Sqrt(lengthSquared), out rayCastDirection);// (Vector3.Normalize(localDirection) + Vector3.Normalize(collidableB.worldTransform.Position - collidableA.worldTransform.Position)) / 2;
// MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref rayCastDirection, out contact.PenetrationDepth, out contact.Normal);
// }
// else
// {
// contact.PenetrationDepth = float.MaxValue;
// contact.Normal = Toolbox.UpVector;
// }
// //Try the offset between the origins as a second option. Sometimes this is a better choice than the relative velocity.
// //TODO: Could use the position-finding MPR iteration to find the A-B direction hit by continuing even after the origin has been found (optimization).
// Vector3 normalCandidate;
// float depthCandidate;
// lengthSquared = localTransformB.Position.LengthSquared();
// if (lengthSquared > Toolbox.Epsilon)
// {
// Vector3.Divide(ref localTransformB.Position, (float)Math.Sqrt(lengthSquared), out rayCastDirection);
// MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref rayCastDirection, out depthCandidate, out normalCandidate);
// if (depthCandidate < contact.PenetrationDepth)
// {
// contact.Normal = normalCandidate;
// }
// }
// //Correct the penetration depth.
// MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref contact.Normal, out contact.PenetrationDepth, out rayCastDirection);
// ////The local casting can optionally continue. Eventually, it will converge to the local minimum.
// //while (true)
// //{
// // MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref contact.Normal, out depthCandidate, out normalCandidate);
// // if (contact.PenetrationDepth - depthCandidate <= Toolbox.BigEpsilon)
// // break;
// // contact.PenetrationDepth = depthCandidate;
// // contact.Normal = normalCandidate;
// //}
// contact.Id = -1;
// //we're still in local space! transform it all back.
// Matrix3X3 orientation;
// Matrix3X3.CreateFromQuaternion(ref collidableA.worldTransform.Orientation, out orientation);
// Matrix3X3.Transform(ref contact.Normal, ref orientation, out contact.Normal);
// //Vector3.Negate(ref contact.Normal, out contact.Normal);
// Matrix3X3.Transform(ref contact.Position, ref orientation, out contact.Position);
// Vector3.Add(ref contact.Position, ref collidableA.worldTransform.Position, out contact.Position);
// if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin)
// state = CollisionState.ShallowContact;
// return true;
//}
////This is rare, but could happen.
//state = CollisionState.Separated;
//contact = new ContactData();
//return false;
#endregion
#region Testing
//RigidTransform localTransformB;
//MinkowskiToolbox.GetLocalTransform(ref collidableA.worldTransform, ref collidableB.worldTransform, out localTransformB);
//contact.Id = -1;
//if (MPRTesting.GetLocalOverlapPosition(collidableA.Shape, collidableB.Shape, ref localTransformB, out contact.Position))
//{
// Vector3 rayCastDirection = localTransformB.Position;
// MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref rayCastDirection, out contact.PenetrationDepth, out contact.Normal);
// MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref contact.Normal, out contact.PenetrationDepth, out rayCastDirection);
// RigidTransform.Transform(ref contact.Position, ref collidableA.worldTransform, out contact.Position);
// Vector3.Transform(ref contact.Normal, ref collidableA.worldTransform.Orientation, out contact.Normal);
// return true;
//}
//contact.Normal = new Vector3();
//contact.PenetrationDepth = 0;
//return false;
#endregion
#region v0.15.2 and before
//if (MPRToolbox.AreObjectsColliding(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, out contact))
//{
// if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin)
// state = CollisionState.ShallowContact; //If it's emerged from the deep contact, we can go back to using the preferred GJK method.
// return true;
//}
////This is rare, but could happen.
//state = CollisionState.Separated;
//return false;
#endregion
}
private bool DoShallowContact(out ContactData contact)
{
Vector3 closestA, closestB;
//RigidTransform transform = RigidTransform.Identity;
//Vector3 closestAnew, closestBnew;
//CachedSimplex cachedTest = cachedSimplex;
//bool intersecting = GJKToolbox.GetClosestPoints(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, ref cachedTest, out closestAnew, out closestBnew);
////bool otherIntersecting = OldGJKVerifier.GetClosestPointsBetweenObjects(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, 0, 0, out closestA, out closestB);
//bool otherIntersecting = GJKToolbox.GetClosestPoints(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, out closestA, out closestB);
//Vector3 closestAold, closestBold;
//bool oldIntersecting = OldGJKVerifier.GetClosestPointsBetweenObjects(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, 0, 0, out closestAold, out closestBold);
//if (otherIntersecting != intersecting || (!otherIntersecting && !intersecting &&
// Vector3.DistanceSquared(closestAnew, closestBnew) - Vector3.DistanceSquared(closestA, closestB) > .0001f &&
// (Vector3.DistanceSquared(closestA, closestAnew) > .0001f ||
// Vector3.DistanceSquared(closestB, closestBnew) > .0001f)))// ||
// //Math.Abs(Vector3.Dot(closestB - closestA, closestBnew - closestAnew) - Vector3.Dot(closestB - closestA, closestB - closestA)) > Toolbox.Epsilon)))
// Debug.WriteLine("Break.");
//Vector3 sub;
//Vector3.Subtract(ref closestA, ref closestB, out sub);
//if (sub.LengthSquared() < Toolbox.Epsilon)
if (UseSimplexCaching)
{
GJKToolbox.GetClosestPoints(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref cachedSimplex, out closestA, out closestB);
}
else
{
//The initialization of the pair creates a pretty decent simplex to start from.
//Just don't try to update it.
CachedSimplex preInitializedSimplex = cachedSimplex;
GJKToolbox.GetClosestPoints(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref preInitializedSimplex, out closestA, out closestB);
}
Vector3 displacement;
Vector3.Subtract(ref closestB, ref closestA, out displacement);
float distanceSquared = displacement.LengthSquared();
if (distanceSquared < Toolbox.Epsilon)
{
state = CollisionState.DeepContact;
return(DoDeepContact(out contact));
}
localDirection = displacement; //Use this as the direction for future deep contacts.
float margin = collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin;
if (distanceSquared < margin * margin)
{
//Generate a contact.
contact = new ContactData();
//Displacement is from A to B. point = A + t * AB, where t = marginA / margin.
if (margin > Toolbox.Epsilon) //Avoid a NaN!
{
Vector3.Multiply(ref displacement, collidableA.Shape.collisionMargin / margin, out contact.Position); //t * AB
}
else
{
contact.Position = new Vector3();
}
Vector3.Add(ref closestA, ref contact.Position, out contact.Position); //A + t * AB.
contact.Normal = displacement;
float distance = (float)Math.Sqrt(distanceSquared);
Vector3.Divide(ref contact.Normal, distance, out contact.Normal);
contact.PenetrationDepth = margin - distance;
return(true);
}
//Too shallow to make a contact- move back to separation.
state = CollisionState.Separated;
contact = new ContactData();
return(false);
}
/// <summary>
/// Computes contact data for two spheres.
/// </summary>
/// <param name="a">First sphere.</param>
/// <param name="b">Second sphere.</param>
/// <param name="positionA">Position of the first sphere.</param>
/// <param name="positionB">Position of the second sphere.</param>
/// <param name="contact">Contact data between the spheres, if any.</param>
/// <returns>Whether or not the spheres are touching.</returns>
public static bool AreSpheresColliding(SphereShape a, SphereShape b, ref Vector3 positionA, ref Vector3 positionB, out ContactData contact)
{
contact = new ContactData();
float radiusSum = a.collisionMargin + b.collisionMargin;
Vector3 centerDifference;
Vector3.Subtract(ref positionB, ref positionA, out centerDifference);
float centerDistance = centerDifference.LengthSquared();
if (centerDistance < (radiusSum + CollisionDetectionSettings.maximumContactDistance) * (radiusSum + CollisionDetectionSettings.maximumContactDistance))
{
//In collision!
if (radiusSum > Toolbox.Epsilon) //This would be weird, but it is still possible to cause a NaN.
{
Vector3.Multiply(ref centerDifference, a.collisionMargin / (radiusSum), out contact.Position);
}
else
{
contact.Position = new Vector3();
}
Vector3.Add(ref contact.Position, ref positionA, out contact.Position);
centerDistance = (float)Math.Sqrt(centerDistance);
if (centerDistance > Toolbox.BigEpsilon)
{
Vector3.Divide(ref centerDifference, centerDistance, out contact.Normal);
}
else
{
contact.Normal = Toolbox.UpVector;
}
contact.PenetrationDepth = radiusSum - centerDistance;
return(true);
}
return(false);
}