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 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;
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);
}