public static void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, PersistentManifold resultOut)
{
CapsuleShape capsule0 = (CapsuleShape)body0.getCollisionShape();
CapsuleShape capsule1 = (CapsuleShape)body1.getCollisionShape();
VIntTransform transform0 = body0.getWorldTransform();
VIntTransform transform1 = body1.getWorldTransform();
VInt3 p00 = transform0.TransformPoint(capsule0.getUpAxis() * capsule0.getHalfHeight());
VInt3 p01 = transform0.TransformPoint(capsule0.getUpAxis() * -capsule0.getHalfHeight());
VInt3 p10 = transform1.TransformPoint(capsule1.getUpAxis() * capsule1.getHalfHeight());
VInt3 p11 = transform1.TransformPoint(capsule1.getUpAxis() * -capsule1.getHalfHeight());
VInt3 x, y;
VFixedPoint dist2 = Distance.SegmentSegmentDist2(p00, p01 - p00, p10, p11 - p10, out x, out y);
VFixedPoint dist = FMath.Sqrt(dist2);
VFixedPoint penetration = dist - (capsule0.getRadius() + capsule1.getRadius());
if (penetration > Globals.getContactBreakingThreshold())
{
return;
}
VInt3 diff = (x - y) / dist;
VInt3 posWorldOnA = x - diff * capsule0.getRadius();
VInt3 posWorldOnB = y + diff * capsule1.getRadius();
ManifoldPoint contractPoint = new ManifoldPoint(posWorldOnA, posWorldOnB, diff, penetration);
resultOut.addManifoldPoint(contractPoint);
}
public static void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, PersistentManifold resultOut)
{
bool needSwap = body0.getCollisionShape() is CapsuleShape;
CollisionObject sphereObject = needSwap ? body1 : body0;
CollisionObject capsuleObject = needSwap ? body0 : body1;
SphereShape sphere = (SphereShape)sphereObject.getCollisionShape();
CapsuleShape capsule = (CapsuleShape)capsuleObject.getCollisionShape();
VInt3 spherePos = sphereObject.getWorldTransform().position;
VIntTransform capsuleTransform = capsuleObject.getWorldTransform();
VInt3 p0 = capsuleTransform.TransformPoint(capsule.getUpAxis() * capsule.getHalfHeight());
VInt3 p1 = capsuleTransform.TransformPoint(capsule.getUpAxis() * -capsule.getHalfHeight());
VFixedPoint param = VFixedPoint.Zero;
VFixedPoint dist2 = Distance.distancePointSegmentSquared(p0, p1, spherePos, ref param);
VFixedPoint dist = FMath.Sqrt(dist2);
VFixedPoint penetration = dist - (sphere.getRadius() + capsule.getRadius());
if (penetration > Globals.getContactBreakingThreshold())
{
return;
}
VInt3 lineWorldPos = p0 * (VFixedPoint.One - param) + p1 * param;
VInt3 diff = (spherePos - lineWorldPos) / dist;
VInt3 sphereWorldPos = spherePos - diff * sphere.getRadius();
VInt3 capsuleWorldPos = lineWorldPos + diff * capsule.getRadius();
ManifoldPoint contractPoint = new ManifoldPoint(needSwap ? capsuleWorldPos : sphereWorldPos, needSwap ? sphereWorldPos: capsuleWorldPos, diff * (needSwap ? -1 : 1), penetration);
resultOut.addManifoldPoint(contractPoint);
}
//sphere move to collide capsule
static bool sweepSphereCapsule(SphereShape sphere, VIntTransform sphereTransform, VInt3 end,
CapsuleShape capsule, VIntTransform capsuleTransform, ref VInt3 normal, ref VFixedPoint t)
{
VInt3 move = end - sphereTransform.position;
VFixedPoint radiusSum = sphere.getRadius() + capsule.getRadius();
VInt3 capsuleP0 = capsuleTransform.TransformPoint(capsule.getUpAxis() * capsule.getHalfHeight());
VInt3 capsuleP1 = capsuleTransform.TransformPoint(capsule.getUpAxis() * -capsule.getHalfHeight());
VInt3 spherePosition = sphereTransform.position;
VFixedPoint tmp = VFixedPoint.Zero;
if (Distance.distancePointSegmentSquared(capsuleP0, capsuleP1, spherePosition, ref tmp) < radiusSum * radiusSum)
{
t = VFixedPoint.Zero;
normal = -move.Normalize();
return(true);
}
VFixedPoint u0 = VFixedPoint.Zero;
VInt3 tmpNormal = VInt3.zero;
if (capsuleP0 == capsuleP1)
{
VInt3 ToPos = spherePosition + move;
if (SphereSphereSweepAlgorithm.sphereSphereSweep(sphere.getRadius(), spherePosition, ToPos, capsule.getRadius(), capsuleTransform.position, ref u0, ref tmp, ref tmpNormal))
{
t = u0;
normal = tmpNormal;
return(true);
}
else
{
return(false);
}
}
else if (CapsuleRaytestAlgorithm.raycastCapsule(spherePosition, end, capsuleP0, capsuleP1, radiusSum, ref normal, ref u0))
{
t = u0;
VFixedPoint param = VFixedPoint.Zero;
VInt3 movedSphereCenter = spherePosition + (end - spherePosition) * u0;
Distance.distancePointSegmentSquared(capsuleP0, capsuleP1, movedSphereCenter, ref param);
normal = movedSphereCenter - (capsuleP0 * (VFixedPoint.One - param) + capsuleP1 * param);
normal = normal.Normalize();
return(true);
}
return(false);
}
static int extrudeBox(VInt3 aabbMax, VIntTransform world, VInt3 extrusionDir, Triangle[] tris, VInt3 dir)
{
computeBoxPoints(aabbMax, -aabbMax);
for (int i = 0; i < 12; i++)
{
int VRef0 = Indices[i * 3 + 0];
int VRef1 = Indices[i * 3 + 1];
int VRef2 = Indices[i * 3 + 2];
Triangle atriangle = boxTris[i];
atriangle.verts[0] = world.TransformPoint(boxP[VRef0]);
atriangle.verts[1] = world.TransformPoint(boxP[VRef1]);
atriangle.verts[2] = world.TransformPoint(boxP[VRef2]);
}
return(extrudeMesh(boxTris, extrusionDir, dir, tris));
}
public static void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, PersistentManifold resultOut)
{
bool needSwap = body0.getCollisionShape() is CapsuleShape;
CollisionObject boxObject = needSwap ? body1 : body0;
CollisionObject capsuleObject = needSwap ? body0 : body1;
BoxShape boxShape = (BoxShape)boxObject.getCollisionShape();
CapsuleShape capsuleShape = (CapsuleShape)capsuleObject.getCollisionShape();
VIntTransform boxTransform = boxObject.getWorldTransform();
VIntTransform capsuleTransform = capsuleObject.getWorldTransform();
VInt3 p0 = capsuleTransform.TransformPoint(capsuleShape.getUpAxis() * capsuleShape.getHalfHeight()), p1 = capsuleTransform.TransformPoint(capsuleShape.getUpAxis() * -capsuleShape.getHalfHeight());
VFixedPoint lParam = VFixedPoint.Zero; VInt3 closestPointBoxLS = VInt3.zero;
VFixedPoint distSq = SegmentBoxDistance.distanceSegmentBoxSquared(p0, p1, boxShape.getHalfExtent(), boxTransform, ref lParam, ref closestPointBoxLS);
VInt3 closestPointBoxWS = boxTransform.TransformPoint(closestPointBoxLS);
VInt3 closestPointLineWS = p0 * (VFixedPoint.One - lParam) + p1 * lParam;
VFixedPoint dist = FMath.Sqrt(distSq) - capsuleShape.getRadius();
if (dist > VFixedPoint.Zero)
{
return;
}
if ((closestPointBoxWS - closestPointLineWS).sqrMagnitude > Globals.EPS2)
{
VInt3 normalOnBoxWS = (closestPointLineWS - closestPointBoxWS).Normalize();
ManifoldPoint contactPoint = new ManifoldPoint(needSwap ? closestPointLineWS - normalOnBoxWS * capsuleShape.getRadius() : closestPointBoxWS,
!needSwap ? closestPointLineWS - normalOnBoxWS * capsuleShape.getRadius() : closestPointBoxWS,
normalOnBoxWS * (needSwap ? 1 : -1), dist);
resultOut.addManifoldPoint(contactPoint);
}
else //box and line are intersected
{
//EPA
LineShape coreShape = new LineShape(capsuleShape);
VInt3 pa = VInt3.zero, pb = VInt3.zero, normal = VInt3.zero;
VFixedPoint depth = VFixedPoint.Zero;
PxGJKStatus result = EpaSolver.calcPenDepth(coreShape, boxShape, capsuleTransform, boxTransform, ref pa, ref pb, ref normal, ref depth);
if (result == PxGJKStatus.EPA_CONTACT)
{
ManifoldPoint contactPoint = new ManifoldPoint(needSwap ? pa - normal * capsuleShape.getRadius() : pb, !needSwap ? pa - normal * capsuleShape.getRadius() : pb, needSwap ? normal : -normal, depth - capsuleShape.getRadius());
resultOut.addManifoldPoint(contactPoint);
}
}
}
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 void refreshContactPoints(VIntTransform trA, VIntTransform trB)
{
VInt3 tmp = VInt3.zero;
for (int i = cachedPoints - 1; i >= 0; i--)
{
ManifoldPoint manifoldPoint = pointCache[i];
manifoldPoint.positionWorldOnA = trA.TransformPoint(manifoldPoint.localPointA);
manifoldPoint.positionWorldOnB = trB.TransformPoint(manifoldPoint.localPointB);
manifoldPoint.distance = VInt3.Dot(manifoldPoint.normalWorldOnB, manifoldPoint.positionWorldOnA - manifoldPoint.positionWorldOnB);
}
VFixedPoint distance2d = VFixedPoint.Zero;
VInt3 projectedDifference = VInt3.zero, projectedPoint = VInt3.zero;
for (int i = cachedPoints - 1; i >= 0; i--)
{
ManifoldPoint manifoldPoint = pointCache[i];
if (!validContactDistance(manifoldPoint))
{
removeContactPoint(i);
}
}
}
public static bool sweepCapsuleCapsule(CapsuleShape lss0, VIntTransform transform0, VInt3 toPos, CapsuleShape lss1, VIntTransform transform1, ref VFixedPoint dist, ref VInt3 hitNormal)
{
VInt3 FromPos = transform0.position;
VFixedPoint radiusSun = lss0.getRadius() + lss1.getRadius();
VFixedPoint length = (toPos - FromPos).magnitude;
VInt3 dir = (toPos - FromPos) / length;
VInt3 lss0p0 = transform0.TransformPoint(lss0.getUpAxis() * lss0.getHalfHeight()), lss0p1 = transform0.TransformPoint(lss0.getUpAxis() * -lss0.getHalfHeight());
VInt3 lss1p0 = transform1.TransformPoint(lss1.getUpAxis() * lss1.getHalfHeight()), lss1p1 = transform1.TransformPoint(lss1.getUpAxis() * -lss1.getHalfHeight());
bool initialOverlapStatus = false;
VFixedPoint tmp = VFixedPoint.Zero;
if (lss0.getHalfHeight() < Globals.EPS)
{
initialOverlapStatus = Distance.distancePointSegmentSquared(lss1p0, lss1p1, lss0p0, ref tmp) < radiusSun * radiusSun;
}
else if (lss1.getHalfHeight() < Globals.EPS)
{
initialOverlapStatus = Distance.distancePointSegmentSquared(lss0p0, lss0p1, lss1p0, ref tmp) < radiusSun * radiusSun;
}
else
{
VInt3 x, y;
initialOverlapStatus = Distance.SegmentSegmentDist2(lss0p0, lss0p1 - lss0p0, lss1p0, lss1p1 - lss1p0, out x, out y) < radiusSun * radiusSun;
}
if (initialOverlapStatus)
{
dist = VFixedPoint.Zero;
hitNormal = (FromPos - toPos).Normalize();
return(true);
}
// 1. Extrude lss0 by lss1's length
// 2. Inflate extruded shape by lss1's radius
// 3. Raycast against resulting quad
VInt3 D = (lss0p1 - lss0p0) * VFixedPoint.Half;
VInt3 p0 = lss1p0 - D, p1 = lss1p1 - D, p0b = lss1p0 + D, p1b = lss1p1 + D;
VInt3 normal = VInt3.Cross(p1b - p0b, p1 - p0b); normal = normal.Normalize();
dist = VFixedPoint.One; bool status = false;
VInt3 pa, pb, pc;
if (VInt3.Dot(normal, dir) >= VFixedPoint.Zero)
{
pc = p0 - normal * radiusSun;
pa = p1 - normal * radiusSun;
pb = p1b - normal * radiusSun;
}
else
{
pc = p0 + normal * radiusSun;
pa = p1 + normal * radiusSun;
pb = p1b + normal * radiusSun;
}
VFixedPoint t = VFixedPoint.Zero, u = VFixedPoint.Zero, v = VFixedPoint.Zero;
if (rayQuad(transform1.position, dir, pa, pb, pc, ref t, ref u, ref v, true) && t >= VFixedPoint.Zero && t < length)
{
dist = t / length;
status = true;
}
if (!status)
{
VInt3[] caps = new VInt3[]
{
p0, p1, p1, p1b, p1b, p0b, p0b, p0
};
VInt3 tmpNormal = VInt3.zero;
for (int i = 0; i < 4; i++)
{
VFixedPoint s = VFixedPoint.Zero;
if (CapsuleRaytestAlgorithm.raycastCapsule(FromPos, toPos, caps[i * 2], caps[i * 2 + 1], radiusSun, ref tmpNormal, ref s))
{
if (s > VFixedPoint.Zero && s < dist)
{
dist = s;
status = true;
}
}
}
}
if (status)
{
VInt3 x, y;
Distance.SegmentSegmentDist2(lss0p0 + dir * length * dist, lss0p1 - lss0p0, lss1p0, lss1p1 - lss1p0, out x, out y);
hitNormal = (x - y).Normalize();
}
return(status);
}
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);
}
static void calculateContacts(VFixedPoint extentX, VFixedPoint extentY, VFixedPoint extentZ, VInt3[] pts,
VInt3 incidentfaceNormalInNew, VInt3 localNormal, VFixedPoint contactDist, VIntTransform transformNew,
PersistentManifold resultOut, bool flip)
{
localNormal = transformNew.TransformDirection(localNormal);
VFixedPoint nExtentX = -extentX;
VFixedPoint nExtentY = -extentY;
bool[] penetration = new bool[4];
bool[] area = new bool[4];
VInt3 bmin = new VInt3(VFixedPoint.MaxValue, VFixedPoint.MaxValue, VFixedPoint.MaxValue);
VInt3 bmax = new VInt3(VFixedPoint.MinValue, VFixedPoint.MinValue, VFixedPoint.MinValue);
VInt3 bound = new VInt3(extentX, extentY, VFixedPoint.MaxValue);
for (int i = 0; i < 4; i++)
{
bmin = VInt3.Min(bmin, pts[i]);
bmax = VInt3.Max(bmax, pts[i]);
VFixedPoint z = -pts[i].z;
if (contactDist > z)
{
penetration[i] = true;
VInt3 absPt = pts[i].Abs();
bool con = bound >= absPt;
if (con)
{
area[i] = true;
VInt3 localPointA = pts[i]; localPointA.z = VFixedPoint.Zero; localPointA = transformNew.TransformPoint(localPointA);
VInt3 localPointB = pts[i]; localPointB = transformNew.TransformPoint(localPointB);
ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, z);
resultOut.addManifoldPoint(contactPoint);
}
else
{
area[i] = false;
}
}
else
{
penetration[i] = false;
area[i] = false;
}
}
if (resultOut.getContactPointsNum() == PersistentManifold.MANIFOLD_CACHE_SIZE)
{
return;
}
{
{
VFixedPoint denom = incidentfaceNormalInNew.z;
{
VInt3 q0 = new VInt3(extentX, extentY, VFixedPoint.Zero);
if (contains(pts, q0, bmin, bmax))
{
VFixedPoint nom = VInt3.Dot(incidentfaceNormalInNew, pts[0] - q0);
VFixedPoint t = nom / denom;
VFixedPoint pen = -t;
if (contactDist > pen)
{
VInt3 localPointA = q0; localPointA = transformNew.TransformPoint(localPointA);
VInt3 localPointB = q0; localPointB.z = t; localPointB = transformNew.TransformPoint(localPointB);
ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, pen);
resultOut.addManifoldPoint(contactPoint);
}
}
}
{
VInt3 q0 = new VInt3(extentX, nExtentY, VFixedPoint.Zero);
if (contains(pts, q0, bmin, bmax))
{
VFixedPoint nom = VInt3.Dot(incidentfaceNormalInNew, pts[0] - q0);
VFixedPoint t = nom / denom;
VFixedPoint pen = -t;
if (contactDist > pen)
{
VInt3 localPointA = q0; localPointA = transformNew.TransformPoint(localPointA);
VInt3 localPointB = q0; localPointB.z = t; localPointB = transformNew.TransformPoint(localPointB);
ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, pen);
resultOut.addManifoldPoint(contactPoint);
}
}
}
{
VInt3 q0 = new VInt3(nExtentX, extentY, VFixedPoint.Zero);
if (contains(pts, q0, bmin, bmax))
{
VFixedPoint nom = VInt3.Dot(incidentfaceNormalInNew, pts[0] - q0);
VFixedPoint t = nom / denom;
VFixedPoint pen = -t;
if (contactDist > pen)
{
VInt3 localPointA = q0; localPointA = transformNew.TransformPoint(localPointA);
VInt3 localPointB = q0; localPointB.z = t; localPointB = transformNew.TransformPoint(localPointB);
ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, pen);
resultOut.addManifoldPoint(contactPoint);
}
}
}
{
VInt3 q0 = new VInt3(nExtentX, nExtentY, VFixedPoint.Zero);
if (contains(pts, q0, bmin, bmax))
{
VFixedPoint nom = VInt3.Dot(incidentfaceNormalInNew, pts[0] - q0);
VFixedPoint t = nom / denom;
VFixedPoint pen = -t;
if (contactDist > pen)
{
VInt3 localPointA = q0; localPointA = transformNew.TransformPoint(localPointA);
VInt3 localPointB = q0; localPointB.z = t; localPointB = transformNew.TransformPoint(localPointB);
ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, pen);
resultOut.addManifoldPoint(contactPoint);
}
}
}
}
}
VInt3 ext = new VInt3(extentX, extentY, extentZ * VFixedPoint.Two);
VInt3 negExt = new VInt3(nExtentX, nExtentY, -(contactDist + Globals.EPS));
for (int start = 0, end = 3; start < 4; end = start++)
{
VInt3 p0 = pts[start];
VInt3 p1 = pts[end];
if (!penetration[start] && !penetration[end])
{
continue;
}
bool con0 = penetration[start] && area[start];
bool con1 = penetration[end] && area[end];
if (con0 && con1)
{
continue;
}
VFixedPoint tmin = VFixedPoint.Zero, tmax = VFixedPoint.Zero;
if (AabbUtils.RayAabb2(p0, p1, negExt, ext, ref tmin, ref tmax))
{
if (!con0 && tmin > VFixedPoint.Zero)
{
VInt3 intersectP = p0 * (VFixedPoint.One - tmin) + p1 * tmin;
VInt3 localPointA = intersectP; localPointA.z = VFixedPoint.Zero; localPointA = transformNew.TransformPoint(localPointA);
VInt3 localPointB = intersectP; localPointB = transformNew.TransformPoint(localPointB);
ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, -intersectP.z);
resultOut.addManifoldPoint(contactPoint);
}
if (!con1 && tmax < VFixedPoint.One)
{
VInt3 intersectP = p0 * (VFixedPoint.One - tmax) + p1 * tmax;
VInt3 localPointA = intersectP; localPointA.z = VFixedPoint.Zero; localPointA = transformNew.TransformPoint(localPointA);
VInt3 localPointB = intersectP; localPointB = transformNew.TransformPoint(localPointB);
ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, -intersectP.z);
resultOut.addManifoldPoint(contactPoint);
}
}
}
}