///<summary>
/// Initializes the pair tester.
///</summary>
///<param name="shapeA">First shape in the pair.</param>
///<param name="shapeB">Second shape in the pair.</param>
public void Initialize(Collidable shapeA, Collidable shapeB)
{
collidableA = (ConvexCollidable)shapeA;
collidableB = (ConvexCollidable)shapeB;
cachedSimplex = new CachedSimplex {
State = SimplexState.Point
}; // new CachedSimplex(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform);
}
///<summary>
/// Cleans up the pair tester.
///</summary>
public void CleanUp()
{
state = CollisionState.Separated;
previousState = CollisionState.Separated;
cachedSimplex = new CachedSimplex();
localSeparatingAxis = new Vector3();
collidableA = null;
collidableB = null;
}
///<summary>
/// Gets the closest points between the shapes.
///</summary>
///<param name="shapeA">First shape of the pair.</param>
///<param name="shapeB">Second shape of the pair.</param>
///<param name="transformA">Transform to apply to the first shape.</param>
///<param name="transformB">Transform to apply to the second shape.</param>
/// <param name="cachedSimplex">Simplex from a previous updated used to warmstart the current attempt. Updated after each run.</param>
///<param name="closestPointA">Closest point on the first shape to the second shape.</param>
///<param name="closestPointB">Closest point on the second shape to the first shape.</param>
///<returns>Whether or not the objects were intersecting. If they are intersecting, then the closest points cannot be identified.</returns>
public static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB,
ref CachedSimplex cachedSimplex, out Vector3 closestPointA, out Vector3 closestPointB)
{
RigidTransform localtransformB;
MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB);
bool toReturn = GetClosestPoints(shapeA, shapeB, ref localtransformB, ref cachedSimplex, out closestPointA, out closestPointB);
RigidTransform.Transform(ref closestPointA, ref transformA, out closestPointA);
RigidTransform.Transform(ref closestPointB, ref transformA, out closestPointB);
return(toReturn);
}
///<summary>
/// Gets the closest points between the shapes.
///</summary>
///<param name="shapeA">First shape of the pair.</param>
///<param name="shapeB">Second shape of the pair.</param>
///<param name="transformA">Transform to apply to the first shape.</param>
///<param name="transformB">Transform to apply to the second shape.</param>
///<param name="closestPointA">Closest point on the first shape to the second shape.</param>
///<param name="closestPointB">Closest point on the second shape to the first shape.</param>
///<returns>Whether or not the objects were intersecting. If they are intersecting, then the closest points cannot be identified.</returns>
public static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB,
out Vector3 closestPointA, out Vector3 closestPointB)
{
//The cached simplex stores locations that are local to the shapes. A fairly decent initial state is between the centroids of the objects.
//In local space, the centroids are at the origins.
RigidTransform localtransformB;
MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB);
var simplex = new CachedSimplex {
State = SimplexState.Point
};
// new CachedSimplex(shapeA, shapeB, ref localtransformB);
bool toReturn = GetClosestPoints(shapeA, shapeB, ref localtransformB, ref simplex, out closestPointA, out closestPointB);
RigidTransform.Transform(ref closestPointA, ref transformA, out closestPointA);
RigidTransform.Transform(ref closestPointB, ref transformA, out closestPointB);
return(toReturn);
}
private static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB,
ref CachedSimplex cachedSimplex, out Vector3 localClosestPointA, out Vector3 localClosestPointB)
{
var simplex = new PairSimplex(ref cachedSimplex, ref localTransformB);
Vector3 closestPoint;
int count = 0;
while (true)
{
if (simplex.GetPointClosestToOrigin(out closestPoint) || //Also reduces the simplex and computes barycentric coordinates if necessary.
closestPoint.LengthSquared() <= Toolbox.Epsilon * simplex.errorTolerance)
{
//Intersecting.
localClosestPointA = Toolbox.ZeroVector;
localClosestPointB = Toolbox.ZeroVector;
simplex.UpdateCachedSimplex(ref cachedSimplex);
return(true);
}
if (++count > MaximumGJKIterations)
{
break; //Must break BEFORE a new vertex is added if we're over the iteration limit. This guarantees final simplex is not a tetrahedron.
}
if (simplex.GetNewSimplexPoint(shapeA, shapeB, count, ref closestPoint))
{
//No progress towards origin, not intersecting.
break;
}
}
//Compute closest points from the contributing simplexes and barycentric coordinates
simplex.GetClosestPoints(out localClosestPointA, out localClosestPointB);
//simplex.VerifyContributions();
//if (Vector3.Distance(localClosestPointA - localClosestPointB, closestPoint) > .00001f)
// Debug.WriteLine("break.");
simplex.UpdateCachedSimplex(ref cachedSimplex);
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);
CachedSimplex 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.
ContactData 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);
}
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);
}
private bool DoShallowContact(out ContactData contact)
{
Vector3 closestA, closestB;
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);
}
