static void doSupport(CollisionShape a, CollisionShape b, VIntTransform transformA, VIntTransform transformB, VInt3 dir
, out VInt3 supportA, out VInt3 supportB, out VInt3 support)
{
VInt3 dirInA = dir;
dirInA = transformA.InverseTransformDirection(dirInA);
VInt3 dirInB = -dir;
dirInB = transformB.InverseTransformDirection(dirInB);
VInt3 pInA = new VInt3();
VInt3 qInB = new VInt3();
pInA = a.support(dirInA);
qInB = b.support(dirInB);
supportA = transformA.TransformPoint(pInA);
supportB = transformB.TransformPoint(qInB);
support = supportA - supportB;
}
public static PxGJKStatus calcPenDepth(CollisionShape a, CollisionShape b, VIntTransform transformA, VIntTransform transformB,
ref VInt3 pa, ref VInt3 pb, ref VInt3 normal, ref VFixedPoint penDepth)
{
simplexSolver.reset();
//Do simple GJK
VFixedPoint minMargin = FMath.Min(a.getMargin(), b.getMargin());
VInt3 initialSearchDir = transformA.position - transformB.position;
VInt3 v = initialSearchDir.sqrMagnitude > Globals.EPS2 ? initialSearchDir : VInt3.right;
bool notTerminated = true;
VFixedPoint sDist = VFixedPoint.MaxValue;
VFixedPoint minDist = sDist;
int iter = 0;
while (notTerminated && iter < MaxGJKIteration)
{
minDist = sDist;
VInt3 supportA = transformA.TransformPoint(a.support(transformA.InverseTransformDirection(v)));
VInt3 supportB = transformB.TransformPoint(b.support(transformB.InverseTransformDirection(-v)));
VInt3 support = supportA - supportB;
simplexSolver.addVertex(support, supportA, supportB);
VInt3 p0, p1;
bool con = !simplexSolver.compute_points(out p0, out p1);
v = p1 - p0;
sDist = v.sqrMagnitude;
notTerminated = con && sDist < minDist;
iter++;
}
if (notTerminated)
{
return(PxGJKStatus.EPA_FAIL);
}
simplexSolver.getSimplex(aBuf, bBuf, Q);
VFixedPoint upper_bound = VFixedPoint.MaxValue;
VFixedPoint lower_bound = VFixedPoint.MinValue;
int numVertsLocal = 0;
heap.Clear();
facetManager.freeAll();
switch (simplexSolver.numVertices())
{
case 1:
if (!expandPoint(a, b, transformA, transformB, ref numVertsLocal, lower_bound, upper_bound))
{
return(PxGJKStatus.EPA_FAIL);
}
break;
case 2:
if (!expandSegment(a, b, transformA, transformB, ref numVertsLocal, lower_bound, upper_bound))
{
return(PxGJKStatus.EPA_FAIL);
}
break;
case 3:
if (!expandTriangle(a, b, transformA, transformB, ref numVertsLocal, lower_bound, upper_bound))
{
return(PxGJKStatus.EPA_FAIL);
}
break;
case 4:
//check for input face normal. All face normals in this tetrahedron should be all pointing either inwards or outwards. If all face normals are pointing outward, we are good to go. Otherwise, we need to
//shuffle the input vertexes and make sure all face normals are pointing outward
VFixedPoint planeDist0 = calculatePlaneDist(0, 1, 2, aBuf, bBuf);
if (planeDist0 < VFixedPoint.Zero)
{
//shuffle the input vertexes
VInt3 tempA = aBuf[2];
VInt3 tempB = bBuf[2];
aBuf[2] = aBuf[1];
bBuf[2] = bBuf[1];
aBuf[1] = tempA;
bBuf[1] = tempB;
}
Facet f0 = addFacet(0, 1, 2, lower_bound, upper_bound);
Facet f1 = addFacet(0, 3, 1, lower_bound, upper_bound);
Facet f2 = addFacet(0, 2, 3, lower_bound, upper_bound);
Facet f3 = addFacet(1, 3, 2, lower_bound, upper_bound);
if ((f0 == null) || (f1 == null) || (f2 == null) || (f3 == null) || heap.IsEmpty())
{
return(PxGJKStatus.EPA_FAIL);
}
f0.link(0, f1, 2);
f0.link(1, f3, 2);
f0.link(2, f2, 0);
f1.link(0, f2, 2);
f1.link(1, f3, 0);
f2.link(1, f3, 1);
numVertsLocal = 4;
break;
}
Facet facet = null;
Facet bestFacet = null;
bool hasMoreFacets = false;
VInt3 tempa;
VInt3 tempb;
VInt3 q;
do
{
facetManager.processDeferredIDs();
facet = heap.pop(); //get the shortest distance triangle of origin from the list
facet.m_inHeap = false;
if (!facet.isObsolete())
{
bestFacet = facet;
VInt3 planeNormal = facet.getPlaneNormal();
VFixedPoint planeDist = facet.getPlaneDist();
doSupport(a, b, transformA, transformB, planeNormal, out tempa, out tempb, out q);
//calculate the distance from support point to the origin along the plane normal. Because the support point is search along
//the plane normal, which means the distance should be positive. However, if the origin isn't contained in the polytope, dist
//might be negative
VFixedPoint dist = VInt3.Dot(q, planeNormal);
//update the upper bound to the minimum between exisiting upper bound and the distance if distance is positive
upper_bound = dist >= VFixedPoint.Zero ? FMath.Min(upper_bound, dist) : upper_bound;
//lower bound is the plane distance, which will be the smallest among all the facets in the prority queue
lower_bound = planeDist;
//if the plane distance and the upper bound is within a small tolerance, which means we found the MTD
bool con0 = upper_bound != VFixedPoint.MaxValue && lower_bound != VFixedPoint.MinValue && (upper_bound - lower_bound) <= Globals.EPS2;
if (con0)
{
calculateContactInformation(aBuf, bBuf, facet, a, b, ref pa, ref pb, ref normal, ref penDepth);
return(PxGJKStatus.EPA_CONTACT);
}
//if planeDist is the same as dist, which means the support point we get out from the Mincowsky sum is one of the point
//in the triangle facet, we should exist because we can't progress further.
VFixedPoint dif = dist - planeDist;
bool degeneratedCondition = dif < Globals.EPS;
if (degeneratedCondition)
{
return(PxGJKStatus.EPA_DEGENERATE);
}
aBuf[numVertsLocal] = tempa;
bBuf[numVertsLocal] = tempb;
int index = numVertsLocal++;
// Compute the silhouette cast by the new vertex
// Note that the new vertex is on the positive side
// of the current facet, so the current facet will
// not be in the polytope. Start local search
// from this facet.
edgeBuffer.MakeEmpty();
facet.silhouette(q, aBuf, bBuf, edgeBuffer, facetManager);
Edge edge = edgeBuffer.Get(0);
int bufferSize = edgeBuffer.Size();
//check to see whether we have enough space in the facet manager to create new facets
if (bufferSize > facetManager.getNumRemainingIDs())
{
return(PxGJKStatus.EPA_DEGENERATE);
}
Facet firstFacet = addFacet(edge.getTarget(), edge.getSource(), index, lower_bound, upper_bound);
firstFacet.link(0, edge.getFacet(), edge.getIndex());
Facet lastFacet = firstFacet;
bool degenerate = false;
for (int i = 1; (i < bufferSize) && (!degenerate); ++i)
{
edge = edgeBuffer.Get(i);
Facet newFacet = addFacet(edge.getTarget(), edge.getSource(), index, lower_bound, upper_bound);
bool b0 = newFacet.link(0, edge.getFacet(), edge.getIndex());
bool b1 = newFacet.link(2, lastFacet, 1);
degenerate = degenerate || !b0 || !b1;
lastFacet = newFacet;
}
if (degenerate)
{
return(PxGJKStatus.EPA_DEGENERATE);
}
firstFacet.link(2, lastFacet, 1);
}
hasMoreFacets = (heap.size() > 0);
if (hasMoreFacets && heap.peak().getPlaneDist() > upper_bound)
{
calculateContactInformation(aBuf, bBuf, bestFacet, a, b, ref pa, ref pb, ref normal, ref penDepth);
return(PxGJKStatus.EPA_CONTACT);
}
} while (hasMoreFacets && numVertsLocal != MaxSupportPoints);
return(PxGJKStatus.EPA_DEGENERATE);
}
public static VFixedPoint distanceLineBoxSquared(VInt3 lineOrigin, VInt3 lineDirection,
VInt3 boxExtent, VIntTransform boxTransform, ref VFixedPoint lineParam, ref VInt3 boxParam)
{
// compute coordinates of line in box coordinate system
VInt3 pnt = boxTransform.InverseTransformPoint(lineOrigin);
VInt3 dir = boxTransform.InverseTransformDirection(lineDirection);
// Apply reflections so that direction vector has nonnegative components.
bool[] reflect = new bool[3];
for (int i = 0; i < 3; i++)
{
if (dir[i] < VFixedPoint.Zero)
{
pnt[i] = -pnt[i];
dir[i] = -dir[i];
reflect[i] = true;
}
else
{
reflect[i] = false;
}
}
VFixedPoint sqrDistance = VFixedPoint.Zero;
if (dir.x > VFixedPoint.Zero)
{
if (dir.y > VFixedPoint.Zero)
{
if (dir.z > VFixedPoint.Zero)
{
caseNoZero(ref pnt, dir, boxExtent, ref lineParam, ref sqrDistance);
}
else
{
case0(0, 1, 2, ref pnt, dir, boxExtent, ref lineParam, ref sqrDistance);
}
}
else
{
if (dir.z > VFixedPoint.Zero)
{
case0(0, 2, 1, ref pnt, dir, boxExtent, ref lineParam, ref sqrDistance); // (+,0,+)
}
else
{
case00(0, 1, 2, ref pnt, dir, boxExtent, ref lineParam, ref sqrDistance); // (+,0,0)
}
}
}
else
{
if (dir.y > VFixedPoint.Zero)
{
if (dir.z > VFixedPoint.Zero)
{
case0(1, 2, 0, ref pnt, dir, boxExtent, ref lineParam, ref sqrDistance); // (0,+,+)
}
else
{
case00(1, 0, 2, ref pnt, dir, boxExtent, ref lineParam, ref sqrDistance); // (0,+,0)
}
}
else
{
if (dir.z > VFixedPoint.Zero)
{
case00(2, 0, 1, ref pnt, dir, boxExtent, ref lineParam, ref sqrDistance); // (0,0,+)
}
else
{
case000(ref pnt, boxExtent, ref sqrDistance); // (0,0,0)
lineParam = VFixedPoint.Zero;
}
}
}
for (int i = 0; i < 3; i++)
{
boxParam[i] = pnt[i] * (reflect[i] ? -1 : 1);
}
return(sqrDistance);
}