///<summary>
/// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA, without a margin.
///</summary>
///<param name="shapeA">First shape.</param>
///<param name="shapeB">Second shape.</param>
///<param name="direction">Extreme point direction in local space.</param>
///<param name="localTransformB">Transform of shapeB in the local space of A.</param>
///<param name="extremePoint">The extreme point in the local space of A.</param>
public static void GetLocalMinkowskiExtremePointWithoutMargin(ConvexShape shapeA, ConvexShape shapeB, ref System.Numerics.Vector3 direction, ref RigidTransform localTransformB, out System.Numerics.Vector3 extremePoint)
{
//Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D)
shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePoint);
System.Numerics.Vector3 extremePointB;
System.Numerics.Vector3 negativeN;
Vector3Ex.Negate(ref direction, out negativeN);
shapeB.GetExtremePointWithoutMargin(negativeN, ref localTransformB, out extremePointB);
Vector3Ex.Subtract(ref extremePoint, ref extremePointB, out extremePoint);
}
///<summary>
/// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA.
///</summary>
///<param name="shapeA">First shape.</param>
///<param name="shapeB">Second shape.</param>
///<param name="direction">Extreme point direction in local space.</param>
///<param name="localTransformB">Transform of shapeB in the local space of A.</param>
/// <param name="extremePointA">The extreme point on shapeA.</param>
/// <param name="extremePointB">The extreme point on shapeB.</param>
///<param name="extremePoint">The extreme point in the local space of A.</param>
public static void GetLocalMinkowskiExtremePoint(ConvexShape shapeA, ConvexShape shapeB, ref System.Numerics.Vector3 direction, ref RigidTransform localTransformB,
out System.Numerics.Vector3 extremePointA, out System.Numerics.Vector3 extremePointB, out System.Numerics.Vector3 extremePoint)
{
//Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D)
shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePointA);
System.Numerics.Vector3 v;
Vector3Ex.Negate(ref direction, out v);
shapeB.GetExtremePointWithoutMargin(v, ref localTransformB, out extremePointB);
ExpandMinkowskiSum(shapeA.collisionMargin, shapeB.collisionMargin, direction, ref extremePointA, ref extremePointB);
Vector3Ex.Subtract(ref extremePointA, ref extremePointB, out extremePoint);
}
///<summary>
/// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA.
///</summary>
///<param name="shapeA">First shape.</param>
///<param name="shapeB">Second shape.</param>
///<param name="direction">Extreme point direction in local space.</param>
///<param name="localTransformB">Transform of shapeB in the local space of A.</param>
///<param name="extremePoint">The extreme point in the local space of A.</param>
public static void GetLocalMinkowskiExtremePoint(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 direction, ref RigidTransform localTransformB, out Vector3 extremePoint)
{
//Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D)
shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePoint);
Vector3 v;
Vector3 negativeN;
Vector3.Negate(ref direction, out negativeN);
shapeB.GetExtremePointWithoutMargin(negativeN, ref localTransformB, out v);
Vector3.Subtract(ref extremePoint, ref v, out extremePoint);
ExpandMinkowskiSum(shapeA.collisionMargin, shapeB.collisionMargin, ref direction, out v);
Vector3.Add(ref extremePoint, ref v, out extremePoint);
}
///<summary>
/// Casts a fat (sphere expanded) ray against the shape.
///</summary>
///<param name="ray">Ray to test against the shape.</param>
///<param name="radius">Radius of the ray.</param>
///<param name="shape">Shape to test against.</param>
///<param name="shapeTransform">Transform to apply to the shape for the test.</param>
///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
///<param name="hit">Hit data of the sphere cast, if any.</param>
///<returns>Whether or not the sphere cast hit the shape.</returns>
public static bool SphereCast(Ray ray, float radius, ConvexShape shape, ref RigidTransform shapeTransform,
float maximumLength,
out RayHit hit)
{
//Transform the ray into the object's local space.
Vector3.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position);
Quaternion conjugate;
Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate);
Quaternion.Transform(ref ray.Position, ref conjugate, out ray.Position);
Quaternion.Transform(ref ray.Direction, ref conjugate, out ray.Direction);
Vector3 w, p;
hit.T = 0;
hit.Location = ray.Position;
hit.Normal = Toolbox.ZeroVector;
Vector3 v = hit.Location;
RaySimplex simplex = new RaySimplex();
float vw, vdir;
int count = 0;
//This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position))
{
if (++count > MaximumGJKIterations)
{
//It's taken too long to find a hit. Numerical problems are probable; quit.
hit = new RayHit();
return(false);
}
shape.GetLocalExtremePointWithoutMargin(ref v, out p);
Vector3 contribution;
MinkowskiToolbox.ExpandMinkowskiSum(shape.collisionMargin, radius, ref v, out contribution);
Vector3.Add(ref p, ref contribution, out p);
Vector3.Subtract(ref hit.Location, ref p, out w);
Vector3.Dot(ref v, ref w, out vw);
if (vw > 0)
{
Vector3.Dot(ref v, ref ray.Direction, out vdir);
hit.T = hit.T - vw / vdir;
if (vdir >= 0)
//We would have to back up!
{
return(false);
}
if (hit.T > maximumLength)
//If we've gone beyond where the ray can reach, there's obviously no hit.
{
return(false);
}
//Shift the ray up.
Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
hit.Normal = v;
}
RaySimplex shiftedSimplex;
simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex);
shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v);
}
//Transform the hit data into world space.
Quaternion.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal);
Quaternion.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location);
Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location);
return(true);
}
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:
break;
case TriangleSidedness.Counterclockwise:
//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.
ContactData 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);
}
///<summary>
/// Adds a new point to the simplex.
///</summary>
///<param name="shapeA">First shape in the pair.</param>
///<param name="shapeB">Second shape in the pair.</param>
///<param name="iterationCount">Current iteration count.</param>
///<param name="closestPoint">Current point on simplex closest to origin.</param>
///<returns>Whether or not GJK should exit due to a lack of progression.</returns>
public bool GetNewSimplexPoint(ConvexShape shapeA, ConvexShape shapeB, int iterationCount, ref Vector3 closestPoint)
{
Vector3 negativeDirection;
Vector3.Negate(ref closestPoint, out negativeDirection);
Vector3 sa, sb;
shapeA.GetLocalExtremePointWithoutMargin(ref negativeDirection, out sa);
shapeB.GetExtremePointWithoutMargin(closestPoint, ref LocalTransformB, out sb);
Vector3 S;
Vector3.Subtract(ref sa, ref sb, out S);
//If S is not further towards the origin along negativeDirection than closestPoint, then we're done.
Fix64 dotS;
Vector3.Dot(ref S, ref negativeDirection, out dotS); //-P * S
Fix64 distanceToClosest = closestPoint.LengthSquared();
Fix64 progression = dotS + distanceToClosest;
//It's likely that the system is oscillating between two or more states, usually because of a degenerate simplex.
//Rather than detect specific problem cases, this approach just lets it run and catches whatever falls through.
//During oscillation, one of the states is usually just BARELY outside of the numerical tolerance.
//After a bunch of iterations, the system lets it pick the 'better' one.
if (iterationCount > GJKToolbox.HighGJKIterations && distanceToClosest - previousDistanceToClosest < DistanceConvergenceEpsilon * errorTolerance)
{
return(true);
}
if (distanceToClosest < previousDistanceToClosest)
{
previousDistanceToClosest = distanceToClosest;
}
//If "A" is the new point always, then the switch statement can be removed
//in favor of just pushing three points up.
switch (State)
{
case SimplexState.Point:
if (progression <= (errorTolerance = MathHelper.Max(A.LengthSquared(), S.LengthSquared())) * ProgressionEpsilon)
{
return(true);
}
State = SimplexState.Segment;
B = S;
SimplexA.B = sa;
SimplexB.B = sb;
return(false);
case SimplexState.Segment:
if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), S.LengthSquared())) * ProgressionEpsilon)
{
return(true);
}
State = SimplexState.Triangle;
C = S;
SimplexA.C = sa;
SimplexB.C = sb;
return(false);
case SimplexState.Triangle:
if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), MathHelper.Max(C.LengthSquared(), S.LengthSquared()))) * ProgressionEpsilon)
{
return(true);
}
State = SimplexState.Tetrahedron;
D = S;
SimplexA.D = sa;
SimplexB.D = sb;
return(false);
}
return(false);
}
