///<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>
/// Updates the cached simplex with the latest run's results.
///</summary>
///<param name="simplex">Simplex to update.</param>
public void UpdateCachedSimplex(ref CachedSimplex simplex)
{
simplex.LocalSimplexA = SimplexA;
switch (State)
{
case SimplexState.Point:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Quaternion conjugate;
Quaternion.Conjugate(ref LocalTransformB.Orientation, out conjugate);
Quaternion.Transform(ref simplex.LocalSimplexB.A, ref conjugate, out simplex.LocalSimplexB.A);
break;
case SimplexState.Segment:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);
Matrix3x3 transform;
Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B);
break;
case SimplexState.Triangle:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);
Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C);
Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform, out simplex.LocalSimplexB.C);
break;
case SimplexState.Tetrahedron:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);
Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C);
Vector3.Subtract(ref SimplexB.D, ref LocalTransformB.Position, out simplex.LocalSimplexB.D);
Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform, out simplex.LocalSimplexB.C);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.D, ref transform, out simplex.LocalSimplexB.D);
break;
}
simplex.State = State;
}
///<summary>
/// Constructs a new pair simplex.
///</summary>
///<param name="cachedSimplex">Cached simplex to use to warmstart the simplex.</param>
///<param name="localTransformB">Transform of shape B in the local space of A.</param>
public PairSimplex(ref CachedSimplex cachedSimplex, ref RigidTransform localTransformB)
{
//NOTE:
//USING A CACHED SIMPLEX INVALIDATES ASSUMPTIONS THAT ALLOW SIMPLEX CASES TO BE IGNORED!
//To get those assumptions back, either DO NOT USE CACHED SIMPLEXES, or
//VERIFY THE SIMPLEXES.
//-A point requires no verification.
//-A segment needs verification that the origin is in front of A in the direction of B.
//-A triangle needs verification that the origin is within the edge planes and in the direction of C.
//-A tetrahedron needs verification that the origin is within the edge planes of triangle ABC and is in the direction of D.
//This simplex implementation will not ignore any cases, so we can warm start safely with one problem.
//Due to relative movement, the simplex may become degenerate. Edges could become points, etc.
//Some protections are built into the simplex cases, but keep an eye out for issues.
//Most dangerous degeneracy seen so far is tetrahedron. It fails to find any points on opposing sides due to numerical problems and returns intersection.
previousDistanceToClosest = float.MaxValue;
errorTolerance = 0;
LocalTransformB = localTransformB;
//Transform the SimplexB into the working space of the simplex and compute the working space simplex.
State = cachedSimplex.State;
SimplexA = cachedSimplex.LocalSimplexA;
SimplexB = new ContributingShapeSimplex();
U = 0;
V = 0;
W = 0;
switch (State)
{
case SimplexState.Point:
Quaternion.Transform(ref cachedSimplex.LocalSimplexB.A, ref LocalTransformB.Orientation, out SimplexB.A);
Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);
Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
B = new Vector3();
C = new Vector3();
D = new Vector3();
break;
case SimplexState.Segment:
Matrix3x3 transform;
Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B);
Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);
Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B);
Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B);
C = new Vector3();
D = new Vector3();
////Test for degeneracy.
//float edgeLengthAB;
//Vector3.DistanceSquared(ref A, ref B, out edgeLengthAB);
//if (edgeLengthAB < Toolbox.Epsilon)
// State = SimplexState.Point;
break;
case SimplexState.Triangle:
Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.C, ref transform, out SimplexB.C);
Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);
Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B);
Vector3.Add(ref SimplexB.C, ref LocalTransformB.Position, out SimplexB.C);
Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B);
Vector3.Subtract(ref SimplexA.C, ref SimplexB.C, out C);
D = new Vector3();
////Test for degeneracy.
//Vector3 AB, AC;
//Vector3.Subtract(ref B, ref A, out AB);
//Vector3.Subtract(ref C, ref A, out AC);
//Vector3 cross;
//Vector3.Cross(ref AB, ref AC, out cross);
////If the area is small compared to a tolerance (adjusted by the partial perimeter), it's degenerate.
//if (cross.LengthSquared() < Toolbox.BigEpsilon * (AB.LengthSquared() + AC.LengthSquared()))
// State = SimplexState.Point;
break;
case SimplexState.Tetrahedron:
Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.C, ref transform, out SimplexB.C);
Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.D, ref transform, out SimplexB.D);
Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);
Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B);
Vector3.Add(ref SimplexB.C, ref LocalTransformB.Position, out SimplexB.C);
Vector3.Add(ref SimplexB.D, ref LocalTransformB.Position, out SimplexB.D);
Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B);
Vector3.Subtract(ref SimplexA.C, ref SimplexB.C, out C);
Vector3.Subtract(ref SimplexA.D, ref SimplexB.D, out D);
////Test for degeneracy.
//Vector3 AD;
//Vector3.Subtract(ref B, ref A, out AB);
//Vector3.Subtract(ref C, ref A, out AC);
//Vector3.Subtract(ref D, ref A, out AD);
//Vector3.Cross(ref AB, ref AC, out cross);
//float volume;
//Vector3.Dot(ref cross, ref AD, out volume);
////Volume is small compared to partial 'perimeter.'
//if (volume < Toolbox.BigEpsilon * (AB.LengthSquared() + AC.LengthSquared() + AD.LengthSquared()))
// State = SimplexState.Point;
break;
default:
A = new Vector3();
B = new Vector3();
C = new Vector3();
D = new Vector3();
break;
}
}
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);
}
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);
}
