static bool expandTriangle(CollisionShape a, CollisionShape b, VIntTransform transformA, VIntTransform transformB, ref int numVerts, VFixedPoint lowerBound, VFixedPoint upperBound)
{
numVerts = 3;
Facet f0 = addFacet(0, 1, 2, lowerBound, upperBound);
Facet f1 = addFacet(1, 0, 2, lowerBound, upperBound);
f0.link(0, f1, 0);
f0.link(1, f1, 2);
f0.link(2, f1, 1);
return(true);
}
static bool expandPoint(CollisionShape a, CollisionShape b, VIntTransform transformA, VIntTransform transformB, ref int numVerts, VFixedPoint lowerBound, VFixedPoint upperBound)
{
VInt3 x = VInt3.right;
VInt3 q0;
doSupport(a, b, transformA, transformB, x, out aBuf[1], out bBuf[1], out q0);
return(expandSegment(a, b, transformA, transformB, ref numVerts, lowerBound, upperBound));
}
static bool expandSegment(CollisionShape a, CollisionShape b, VIntTransform transformA, VIntTransform transformB, ref int numVerts, VFixedPoint lowerBound, VFixedPoint upperBound)
{
VInt3 q0 = aBuf[0] - bBuf[0];
VInt3 q1 = aBuf[1] - bBuf[1];
VInt3 v = q1 - q0;
VInt3 absV = v.Abs();
VFixedPoint x = absV.x, y = absV.y, z = absV.z;
VInt3 axis = VInt3.right;
if (x > y && z > y)
{
axis = VInt3.up;
}
else if (x > z)
{
axis = VInt3.forward;
}
VInt3 n = VInt3.Cross(axis, v).Normalize();
VInt3 q2;
doSupport(a, b, transformA, transformB, n, out aBuf[2], out bBuf[2], out q2);
return(expandTriangle(a, b, transformA, transformB, ref numVerts, lowerBound, upperBound));
}
public static VFixedPoint getContactBreakingThreshold()
{
return(VFixedPoint.Create(4) / VFixedPoint.Create(100));
}
public abstract void updateAction(CollisionWorld collisionWorld, VFixedPoint deltaTimeStep);
public bool isValid2(int i0, int i1, int i2, VInt3[] aBuf, VInt3[] bBuf, VFixedPoint lower, VFixedPoint upper)
{
VInt3 pa0 = aBuf[i0];
VInt3 pa1 = aBuf[i1];
VInt3 pa2 = aBuf[i2];
VInt3 pb0 = bBuf[i0];
VInt3 pb1 = bBuf[i1];
VInt3 pb2 = bBuf[i2];
VInt3 p0 = pa0 - pb0;
VInt3 p1 = pa1 - pb1;
VInt3 p2 = pa2 - pb2;
VInt3 v1 = p1 - p0;
VInt3 v2 = p2 - p0;
VInt3 denormalizedNormal = VInt3.Cross(v1, v2);
VInt3 planeNormal = denormalizedNormal.Normalize();
VFixedPoint planeDist = VInt3.Dot(planeNormal, p0);
m_planeNormal = planeNormal;
m_planeDist = planeDist;
return(planeDist >= lower && upper >= planeDist);
}
public override void setMargin(VFixedPoint margin)
{
this.margin = margin;
}
static void case00(int i0, int i1, int i2, ref VInt3 pnt, VInt3 dir, VInt3 extents, ref VFixedPoint lineParam, ref VFixedPoint sqrDistance)
{
VFixedPoint fDelta = VFixedPoint.Zero;
lineParam = (extents[i0] - pnt[i0]) / dir[i0];
pnt[i0] = extents[i0];
if (pnt[i1] < -extents[i1])
{
fDelta = pnt[i1] + extents[i1];
sqrDistance += fDelta * fDelta;
pnt[i1] = -extents[i1];
}
else if (pnt[i1] > extents[i1])
{
fDelta = pnt[i1] - extents[i1];
sqrDistance += fDelta * fDelta;
pnt[i1] = extents[i1];
}
if (pnt[i2] < -extents[i2])
{
fDelta = pnt[i2] + extents[i2];
sqrDistance += fDelta * fDelta;
pnt[i2] = -extents[i2];
}
else if (pnt[i2] > extents[i2])
{
fDelta = pnt[i2] - extents[i2];
sqrDistance += fDelta * fDelta;
pnt[i2] = extents[i2];
}
}
static void face(int i0, int i1, int i2, ref VInt3 rkPnt, VInt3 rkDir, VInt3 extents, VInt3 rkPmE, ref VFixedPoint pfLParam, ref VFixedPoint rfSqrDistance)
{
VInt3 kPpE = VInt3.zero;
VFixedPoint fLSqr = VFixedPoint.Zero, fInv = VFixedPoint.Zero, fTmp = VFixedPoint.Zero, fParam = VFixedPoint.Zero, fT = VFixedPoint.Zero, fDelta = VFixedPoint.Zero;
kPpE[i1] = rkPnt[i1] + extents[i1];
kPpE[i2] = rkPnt[i2] + extents[i2];
if (rkDir[i0] * kPpE[i1] >= rkDir[i1] * rkPmE[i0])
{
if (rkDir[i0] * kPpE[i2] >= rkDir[i2] * rkPmE[i0])
{
rkPnt[i0] = extents[i0];
fInv = VFixedPoint.One / rkDir[i0];
rkPnt[i1] -= rkDir[i1] * rkPmE[i0] * fInv;
rkPnt[i2] -= rkDir[i2] * rkPmE[i0] * fInv;
pfLParam = -rkPmE[i0] * fInv;
}
else
{
// v[i1] >= -e[i1], v[i2] < -e[i2]
fLSqr = rkDir[i0] * rkDir[i0] + rkDir[i2] * rkDir[i2];
fTmp = fLSqr * kPpE[i1] - rkDir[i1] * (rkDir[i0] * rkPmE[i0] + rkDir[i2] * kPpE[i2]);
if (fTmp <= VFixedPoint.Two * fLSqr * extents[i1])
{
fT = fTmp / fLSqr;
fLSqr += rkDir[i1] * rkDir[i1];
fTmp = kPpE[i1] - fT;
fDelta = rkDir[i0] * rkPmE[i0] + rkDir[i1] * fTmp + rkDir[i2] * kPpE[i2];
fParam = -fDelta / fLSqr;
rfSqrDistance += rkPmE[i0] * rkPmE[i0] + fTmp * fTmp + kPpE[i2] * kPpE[i2] + fDelta * fParam;
pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = fT - extents[i1];
rkPnt[i2] = -extents[i2];
}
else
{
fLSqr += rkDir[i1] * rkDir[i1];
fDelta = rkDir[i0] * rkPmE[i0] + rkDir[i1] * rkPmE[i1] + rkDir[i2] * kPpE[i2];
fParam = -fDelta / fLSqr;
rfSqrDistance += rkPmE[i0] * rkPmE[i0] + rkPmE[i1] * rkPmE[i1] + kPpE[i2] * kPpE[i2] + fDelta * fParam;
pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = extents[i1];
rkPnt[i2] = -extents[i2];
}
}
}
else
{
if (rkDir[i0] * kPpE[i2] >= rkDir[i2] * rkPmE[i0])
{
// v[i1] < -e[i1], v[i2] >= -e[i2]
fLSqr = rkDir[i0] * rkDir[i0] + rkDir[i1] * rkDir[i1];
fTmp = fLSqr * kPpE[i2] - rkDir[i2] * (rkDir[i0] * rkPmE[i0] + rkDir[i1] * kPpE[i1]);
if (fTmp <= VFixedPoint.Two * fLSqr * extents[i2])
{
fT = fTmp / fLSqr;
fLSqr += rkDir[i2] * rkDir[i2];
fTmp = kPpE[i2] - fT;
fDelta = rkDir[i0] * rkPmE[i0] + rkDir[i1] * kPpE[i1] + rkDir[i2] * fTmp;
fParam = -fDelta / fLSqr;
rfSqrDistance += rkPmE[i0] * rkPmE[i0] + kPpE[i1] * kPpE[i1] + fTmp * fTmp + fDelta * fParam;
pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = -extents[i1];
rkPnt[i2] = fT - extents[i2];
}
else
{
fLSqr += rkDir[i2] * rkDir[i2];
fDelta = rkDir[i0] * rkPmE[i0] + rkDir[i1] * kPpE[i1] + rkDir[i2] * rkPmE[i2];
fParam = -fDelta / fLSqr;
rfSqrDistance += rkPmE[i0] * rkPmE[i0] + kPpE[i1] * kPpE[i1] + rkPmE[i2] * rkPmE[i2] + fDelta * fParam;
pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = -extents[i1];
rkPnt[i2] = extents[i2];
}
}
else
{
// v[i1] < -e[i1], v[i2] < -e[i2]
fLSqr = rkDir[i0] * rkDir[i0] + rkDir[i2] * rkDir[i2];
fTmp = fLSqr * kPpE[i1] - rkDir[i1] * (rkDir[i0] * rkPmE[i0] + rkDir[i2] * kPpE[i2]);
if (fTmp >= VFixedPoint.Zero)
{
// v[i1]-edge is closest
if (fTmp <= VFixedPoint.Two * fLSqr * extents[i1])
{
fT = fTmp / fLSqr;
fLSqr += rkDir[i1] * rkDir[i1];
fTmp = kPpE[i1] - fT;
fDelta = rkDir[i0] * rkPmE[i0] + rkDir[i1] * fTmp + rkDir[i2] * kPpE[i2];
fParam = -fDelta / fLSqr;
rfSqrDistance += rkPmE[i0] * rkPmE[i0] + fTmp * fTmp + kPpE[i2] * kPpE[i2] + fDelta * fParam;
pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = fT - extents[i1];
rkPnt[i2] = -extents[i2];
}
else
{
fLSqr += rkDir[i1] * rkDir[i1];
fDelta = rkDir[i0] * rkPmE[i0] + rkDir[i1] * rkPmE[i1] + rkDir[i2] * kPpE[i2];
fParam = -fDelta / fLSqr;
rfSqrDistance += rkPmE[i0] * rkPmE[i0] + rkPmE[i1] * rkPmE[i1] + kPpE[i2] * kPpE[i2] + fDelta * fParam;
pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = extents[i1];
rkPnt[i2] = -extents[i2];
}
return;
}
fLSqr = rkDir[i0] * rkDir[i0] + rkDir[i1] * rkDir[i1];
fTmp = fLSqr * kPpE[i2] - rkDir[i2] * (rkDir[i0] * rkPmE[i0] + rkDir[i1] * kPpE[i1]);
if (fTmp >= VFixedPoint.Zero)
{
// v[i2]-edge is closest
if (fTmp <= VFixedPoint.Two * fLSqr * extents[i2])
{
fT = fTmp / fLSqr;
fLSqr += rkDir[i2] * rkDir[i2];
fTmp = kPpE[i2] - fT;
fDelta = rkDir[i0] * rkPmE[i0] + rkDir[i1] * kPpE[i1] + rkDir[i2] * fTmp;
fParam = -fDelta / fLSqr;
rfSqrDistance += rkPmE[i0] * rkPmE[i0] + kPpE[i1] * kPpE[i1] + fTmp * fTmp + fDelta * fParam;
pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = -extents[i1];
rkPnt[i2] = fT - extents[i2];
}
else
{
fLSqr += rkDir[i2] * rkDir[i2];
fDelta = rkDir[i0] * rkPmE[i0] + rkDir[i1] * kPpE[i1] + rkDir[i2] * rkPmE[i2];
fParam = -fDelta / fLSqr;
rfSqrDistance += rkPmE[i0] * rkPmE[i0] + kPpE[i1] * kPpE[i1] + rkPmE[i2] * rkPmE[i2] + fDelta * fParam;
pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = -extents[i1];
rkPnt[i2] = extents[i2];
}
return;
}
// (v[i1],v[i2])-corner is closest
fLSqr += rkDir[i2] * rkDir[i2];
fDelta = rkDir[i0] * rkPmE[i0] + rkDir[i1] * kPpE[i1] + rkDir[i2] * kPpE[i2];
fParam = -fDelta / fLSqr;
rfSqrDistance += rkPmE[i0] * rkPmE[i0] + kPpE[i1] * kPpE[i1] + kPpE[i2] * kPpE[i2] + fDelta * fParam;
pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = -extents[i1];
rkPnt[i2] = -extents[i2];
}
}
}
static void caseNoZero(ref VInt3 pnt, VInt3 dir, VInt3 extent, ref VFixedPoint lineParam, ref VFixedPoint sqrDistance)
{
VInt3 kPmE = pnt - extent;
VFixedPoint fProdDxPy = VFixedPoint.Zero, fProdDyPx = VFixedPoint.Zero, fProdDzPx = VFixedPoint.Zero, fProdDxPz = VFixedPoint.Zero, fProdDzPy = VFixedPoint.Zero, fProdDyPz = VFixedPoint.Zero;
fProdDxPy = dir.x * kPmE.y;
fProdDyPx = dir.y * kPmE.x;
if (fProdDxPy >= fProdDyPx)
{
fProdDzPx = dir.z * kPmE.x;
fProdDxPz = dir.x * kPmE.z;
if (fProdDzPx >= fProdDxPz)
{
// line intersects x = e0
face(0, 1, 2, ref pnt, dir, extent, kPmE, ref lineParam, ref sqrDistance);
}
else
{
// line intersects z = e2
face(2, 0, 1, ref pnt, dir, extent, kPmE, ref lineParam, ref sqrDistance);
}
}
else
{
fProdDzPy = dir.z * kPmE.y;
fProdDyPz = dir.y * kPmE.z;
if (fProdDzPy >= fProdDyPz)
{
// line intersects y = e1
face(1, 2, 0, ref pnt, dir, extent, kPmE, ref lineParam, ref sqrDistance);
}
else
{
// line intersects z = e2
face(2, 0, 1, ref pnt, dir, extent, kPmE, ref lineParam, ref sqrDistance);
}
}
}
static void case0(int i0, int i1, int i2, ref VInt3 pnt, VInt3 dir, VInt3 extents, ref VFixedPoint lineParam, ref VFixedPoint sqrDistance)
{
VFixedPoint fPmE0 = pnt[i0] - extents[i0];
VFixedPoint fPmE1 = pnt[i1] - extents[i1];
VFixedPoint fProd0 = dir[i1] * fPmE0;
VFixedPoint fProd1 = dir[i0] * fPmE1;
VFixedPoint fDelta = VFixedPoint.Zero, fInvLSqr = VFixedPoint.Zero, fInv = VFixedPoint.Zero;
if (fProd0 >= fProd1)
{
// line intersects P[i0] = e[i0]
pnt[i0] = extents[i0];
VFixedPoint fPpE1 = pnt[i1] + extents[i1];
fDelta = fProd0 - dir[i0] * fPpE1;
if (fDelta >= VFixedPoint.Zero)
{
fInvLSqr = VFixedPoint.One / (dir[i0] * dir[i0] + dir[i1] * dir[i1]);
sqrDistance += fDelta * fDelta * fInvLSqr;
pnt[i1] = -extents[i1];
lineParam = -(dir[i0] * fPmE0 + dir[i1] * fPpE1) * fInvLSqr;
}
else
{
fInv = VFixedPoint.One / dir[i0];
pnt[i1] -= fProd0 * fInv;
lineParam = -fPmE0 * fInv;
}
}
else
{
// line intersects P[i1] = e[i1]
pnt[i1] = extents[i1];
VFixedPoint fPpE0 = pnt[i0] + extents[i0];
fDelta = fProd1 - dir[i1] * fPpE0;
if (fDelta >= VFixedPoint.Zero)
{
fInvLSqr = VFixedPoint.One / (dir[i0] * dir[i0] + dir[i1] * dir[i1]);
sqrDistance += fDelta * fDelta * fInvLSqr;
pnt[i0] = -extents[i0];
lineParam = -(dir[i0] * fPpE0 + dir[i1] * fPmE1) * fInvLSqr;
}
else
{
fInv = VFixedPoint.One / dir[i1];
pnt[i0] -= fProd1 * fInv;
lineParam = -fPmE1 * fInv;
}
}
if (pnt[i2] < -extents[i2])
{
fDelta = pnt[i2] + extents[i2];
sqrDistance += fDelta * fDelta;
pnt[i2] = -extents[i2];
}
else if (pnt[i2] > extents[i2])
{
fDelta = pnt[i2] - extents[i2];
sqrDistance += fDelta * fDelta;
pnt[i2] = extents[i2];
}
}
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);
}
}
}
}
static void getIncidentPolygon(ref VInt3[] pts, ref VInt3 faceNormal, VInt3 axis, VIntTransform transf1To0, VInt3 extents)
{
VFixedPoint ex = extents.x, ey = extents.y, ez = extents.z;
VInt3 u0 = transf1To0.right, u1 = transf1To0.up, u2 = transf1To0.forward;
VFixedPoint d0 = VInt3.Dot(u0, axis), d1 = VInt3.Dot(u1, axis), d2 = VInt3.Dot(u2, axis);
VFixedPoint absd0 = d0.Abs(), absd1 = d1.Abs(), absd2 = d2.Abs();
VInt3 r0 = VInt3.zero, r1 = VInt3.zero, r2 = VInt3.zero;
if (absd0 >= absd1 && absd0 >= absd2)
{
bool con = d0 > VFixedPoint.Zero;
faceNormal = con ? -u0 : u0;
ex = con ? -ex : ex;
r0 = u0 * ex;
r1 = u1 * ey;
r2 = u2 * ez;
VInt3 temp0 = transf1To0.position + r0;
VInt3 temp1 = r1 + r2;
VInt3 temp2 = r1 - r2;
pts[0] = temp0 + temp1;
pts[1] = temp0 + temp2;
pts[2] = temp0 - temp1;
pts[3] = temp0 - temp2;
}
else if (absd1 >= absd2)
{
bool con = d1 > VFixedPoint.Zero;
faceNormal = con ? -u1 : u1;
ey = con ? -ey : ey;
r0 = u0 * ex;
r1 = u1 * ey;
r2 = u2 * ez;
VInt3 temp0 = transf1To0.position + r1;
VInt3 temp1 = r0 + r2;
VInt3 temp2 = r0 - r2;
pts[0] = temp0 + temp1;
pts[1] = temp0 + temp2;
pts[2] = temp0 - temp1;
pts[3] = temp0 - temp2;
}
else
{
bool con = d2 > VFixedPoint.Zero;
faceNormal = con ? -u2 : u2;
ez = con ? -ez : ez;
r0 = u0 * ex;
r1 = u1 * ey;
r2 = u2 * ez;
VInt3 temp0 = transf1To0.position + r2;
VInt3 temp1 = r0 + r1;
VInt3 temp2 = r0 - r1;
pts[0] = temp0 + temp1;
pts[1] = temp0 + temp2;
pts[2] = temp0 - temp1;
pts[3] = temp0 - temp2;
}
}
static void doBoxBoxGenerateContacts(VInt3 box0Extent, VInt3 box1Extent, VIntTransform transform0, VIntTransform transform1, PersistentManifold resultOut)
{
VFixedPoint ea0 = box0Extent.x, ea1 = box0Extent.y, ea2 = box0Extent.z;
VFixedPoint eb0 = box1Extent.x, eb1 = box1Extent.y, eb2 = box1Extent.z;
VIntTransform transform1To0 = transform0.Transform(transform1);
VInt3 position1To0 = transform1To0.position;
VFixedPoint tx = position1To0.x;
VFixedPoint ty = position1To0.y;
VFixedPoint tz = position1To0.z;
VInt3 col0 = transform1To0.right;
VInt3 col1 = transform1To0.up;
VInt3 col2 = transform1To0.forward;
VInt3 abs1To0Col0 = col0.Abs() + VInt3.one * Globals.EPS;
VInt3 abs1To0Col1 = col1.Abs() + VInt3.one * Globals.EPS;
VInt3 abs1To0Col2 = col2.Abs() + VInt3.one * Globals.EPS;
VInt3[] transBasis = transform1To0.getTransposeBasis();
VInt3 abs0To1Col0 = transBasis[0].Abs() + VInt3.one * Globals.EPS;
VInt3 abs0To1Col1 = transBasis[1].Abs() + VInt3.one * Globals.EPS;
VInt3 abs0To1Col2 = transBasis[2].Abs() + VInt3.one * Globals.EPS;
VFixedPoint[] sign = new VFixedPoint[6];
VFixedPoint[] overlap = new VFixedPoint[6];
VFixedPoint ra = VFixedPoint.Zero, rb = VFixedPoint.Zero, radiusSum = VFixedPoint.Zero;
//ua0
{
sign[0] = tx;
rb = VInt3.Dot(abs0To1Col0, box1Extent);
radiusSum = ea0 + rb;
overlap[0] = radiusSum - sign[0].Abs();
if (overlap[0] < VFixedPoint.Zero)
{
return;
}
}
//ua1
{
sign[1] = ty;
rb = VInt3.Dot(abs0To1Col1, box1Extent);
radiusSum = ea1 + rb;
overlap[1] = radiusSum - sign[1].Abs();
if (overlap[1] < VFixedPoint.Zero)
{
return;
}
}
//ua2
{
sign[2] = tz;
rb = VInt3.Dot(abs0To1Col2, box1Extent);
radiusSum = ea2 + rb;
overlap[2] = radiusSum - sign[2].Abs();
if (overlap[2] < VFixedPoint.Zero)
{
return;
}
}
//ub0
{
sign[3] = VInt3.Dot(position1To0, col0);
ra = VInt3.Dot(abs1To0Col0, box0Extent);
radiusSum = ra + eb0;
overlap[3] = radiusSum - sign[3].Abs();
if (overlap[3] < VFixedPoint.Zero)
{
return;
}
}
//ub1
{
sign[4] = VInt3.Dot(position1To0, col1);
ra = VInt3.Dot(abs1To0Col1, box0Extent);
radiusSum = ra + eb1;
overlap[4] = radiusSum - sign[4].Abs();
if (overlap[4] < VFixedPoint.Zero)
{
return;
}
}
//ub2
{
sign[5] = VInt3.Dot(position1To0, col2);
ra = VInt3.Dot(abs1To0Col2, box0Extent);
radiusSum = ra + eb2;
overlap[5] = radiusSum - sign[5].Abs();
if (overlap[5] < VFixedPoint.Zero)
{
return;
}
}
//ua0 x ub0
{
VFixedPoint absSign = (col0.y * tz - col0.z * ty).Abs();
VFixedPoint vtemp0 = abs1To0Col0.z * ea1;
VFixedPoint vtemp1 = abs1To0Col0.y * ea2;
ra = vtemp0 + vtemp1;
VFixedPoint vtemp01 = abs0To1Col0.z * eb1;
VFixedPoint vtemp02 = abs0To1Col0.y * eb2;
rb = vtemp01 + vtemp02;
radiusSum = ra + rb;
if (absSign > radiusSum)
{
return;
}
}
//ua0 x ub1
{
VFixedPoint absSign = (col1.y * tz - col1.z * ty).Abs();
VFixedPoint vtemp0 = abs1To0Col1.z * ea1;
VFixedPoint vtemp1 = abs1To0Col1.y * ea2;
ra = vtemp0 + vtemp1;
VFixedPoint vtemp01 = abs0To1Col0.z * eb0;
VFixedPoint vtemp02 = abs0To1Col0.x * eb2;
rb = vtemp01 + vtemp02;
radiusSum = ra + rb;
if (absSign > radiusSum)
{
return;
}
}
//ua0 x ub2
{
VFixedPoint absSign = (col2.y * tz - col2.z * ty).Abs();
VFixedPoint vtemp0 = abs1To0Col2.z * ea1;
VFixedPoint vtemp1 = abs1To0Col2.y * ea2;
ra = vtemp0 + vtemp1;
VFixedPoint vtemp01 = abs0To1Col0.y * eb0;
VFixedPoint vtemp02 = abs0To1Col0.x * eb1;
rb = vtemp01 + vtemp02;
radiusSum = ra + rb;
if (absSign > radiusSum)
{
return;
}
}
//ua1 x ub0
{
VFixedPoint absSign = (col0.z * tx - col0.x * tz).Abs();
VFixedPoint vtemp0 = abs1To0Col0.z * ea0;
VFixedPoint vtemp1 = abs1To0Col0.x * ea2;
ra = vtemp0 + vtemp1;
VFixedPoint vtemp01 = abs0To1Col1.z * eb1;
VFixedPoint vtemp02 = abs0To1Col1.y * eb2;
rb = vtemp01 + vtemp02;
radiusSum = ra + rb;
if (absSign > radiusSum)
{
return;
}
}
//ua1 x ub1
{
VFixedPoint absSign = (col1.z * tx - col1.x * tz).Abs();
VFixedPoint vtemp0 = abs1To0Col1.z * ea0;
VFixedPoint vtemp1 = abs1To0Col1.x * ea2;
ra = vtemp0 + vtemp1;
VFixedPoint vtemp01 = abs0To1Col1.z * eb0;
VFixedPoint vtemp02 = abs0To1Col1.x * eb2;
rb = vtemp01 + vtemp02;
radiusSum = ra + rb;
if (absSign > radiusSum)
{
return;
}
}
//ua1 x ub2
{
VFixedPoint absSign = (col2.z * tx - col2.x * tz).Abs();
VFixedPoint vtemp0 = abs1To0Col2.z * ea0;
VFixedPoint vtemp1 = abs1To0Col2.x * ea2;
ra = vtemp0 + vtemp1;
VFixedPoint vtemp01 = abs0To1Col1.y * eb0;
VFixedPoint vtemp02 = abs0To1Col1.x * eb1;
rb = vtemp01 + vtemp02;
radiusSum = ra + rb;
if (absSign > radiusSum)
{
return;
}
}
//ua2 x ub0
{
VFixedPoint absSign = (col0.x * ty - col0.y * tx).Abs();
VFixedPoint vtemp0 = abs1To0Col0.y * ea0;
VFixedPoint vtemp1 = abs1To0Col0.x * ea1;
ra = vtemp0 + vtemp1;
VFixedPoint vtemp01 = abs0To1Col2.z * eb1;
VFixedPoint vtemp02 = abs0To1Col2.y * eb2;
rb = vtemp01 + vtemp02;
radiusSum = ra + rb;
if (absSign > radiusSum)
{
return;
}
}
//ua2 x ub1
{
VFixedPoint absSign = (col1.x * ty - col1.y * tx).Abs();
VFixedPoint vtemp0 = abs1To0Col1.y * ea0;
VFixedPoint vtemp1 = abs1To0Col1.x * ea1;
ra = vtemp0 + vtemp1;
VFixedPoint vtemp01 = abs0To1Col2.z * eb0;
VFixedPoint vtemp02 = abs0To1Col2.x * eb2;
rb = vtemp01 + vtemp02;
radiusSum = ra + rb;
if (absSign > radiusSum)
{
return;
}
}
//ua2 x ub2
{
VFixedPoint absSign = (col2.x * ty - col2.y * tx).Abs();
VFixedPoint vtemp0 = abs1To0Col2.y * ea0;
VFixedPoint vtemp1 = abs1To0Col2.x * ea1;
ra = vtemp0 + vtemp1;
VFixedPoint vtemp01 = abs0To1Col2.y * eb0;
VFixedPoint vtemp02 = abs0To1Col2.x * eb1;
rb = vtemp01 + vtemp02;
radiusSum = ra + rb;
if (absSign > radiusSum)
{
return;
}
}
VInt3 mtd = VInt3.zero;
int feature = 0;
VFixedPoint minOverlap = overlap[0];
for (int i = 1; i < 6; i++)
{
if (minOverlap > overlap[i])
{
feature = i;
minOverlap = overlap[i];
}
}
VIntTransform newTransformV;
VInt3 axis00 = transform0.right;
VInt3 axis01 = transform0.up;
VInt3 axis02 = transform0.forward;
VInt3 axis10 = transform1.right;
VInt3 axis11 = transform1.up;
VInt3 axis12 = transform1.forward;
VInt3 incidentFaceNormalInNew = VInt3.zero;
VInt3[] pts = new VInt3[4];
switch (feature)
{
case 0:
{
if (sign[0] <= VFixedPoint.Zero)
{
mtd = axis00;
newTransformV = new VIntTransform((transform0.position - axis00 * ea0), -axis02, axis01, axis00);
}
else
{
VInt3 nAxis00 = -axis00;
mtd = nAxis00;
newTransformV = new VIntTransform((transform0.position + axis00 * ea0), axis02, axis01, nAxis00);
}
VIntTransform transform1ToNew = newTransformV.Transform(transform1);
VInt3 localNormal = newTransformV.InverseTransformDirection(mtd);
getIncidentPolygon(ref pts, ref incidentFaceNormalInNew, -localNormal, transform1ToNew, box1Extent);
calculateContacts(ea2, ea1, ea0, pts, incidentFaceNormalInNew, localNormal, Globals.EPS, newTransformV, resultOut, false);
break;
}
case 1:
{
if (sign[1] <= VFixedPoint.Zero)
{
mtd = axis01;
newTransformV = new VIntTransform((transform0.position - axis01 * ea1), axis00, -axis02, axis01);
}
else
{
VInt3 nAxis01 = -axis01;
mtd = nAxis01;
newTransformV = new VIntTransform((transform0.position + axis01 * ea1), axis00, axis02, nAxis01);
}
VIntTransform transform1ToNew = newTransformV.Transform(transform1);
VInt3 localNormal = newTransformV.InverseTransformDirection(mtd);
getIncidentPolygon(ref pts, ref incidentFaceNormalInNew, -localNormal, transform1ToNew, box1Extent);
calculateContacts(ea0, ea2, ea1, pts, incidentFaceNormalInNew, localNormal, Globals.EPS, newTransformV, resultOut, false);
break;
}
case 2:
{
if (sign[2] <= VFixedPoint.Zero)
{
mtd = axis02;
newTransformV = new VIntTransform((transform0.position - axis02 * ea2), axis00, axis01, axis02);
}
else
{
VInt3 nAxis02 = -axis02;
mtd = nAxis02;
newTransformV = new VIntTransform((transform0.position + axis02 * ea2), axis00, -axis01, nAxis02);
}
VIntTransform transform1ToNew = newTransformV.Transform(transform1);
VInt3 localNormal = newTransformV.InverseTransformDirection(mtd);
getIncidentPolygon(ref pts, ref incidentFaceNormalInNew, -localNormal, transform1ToNew, box1Extent);
calculateContacts(ea0, ea1, ea2, pts, incidentFaceNormalInNew, localNormal, Globals.EPS, newTransformV, resultOut, false);
break;
}
case 3:
{
if (sign[3] <= VFixedPoint.Zero)
{
mtd = axis10;
newTransformV = new VIntTransform((transform1.position + axis10 * eb0), axis12, axis11, -axis10);
}
else
{
mtd = -axis10;
newTransformV = new VIntTransform((transform1.position - axis10 * eb0), -axis12, axis11, axis10);
}
VIntTransform transform1ToNew = newTransformV.Transform(transform0);
VInt3 localNormal = newTransformV.InverseTransformDirection(mtd);
getIncidentPolygon(ref pts, ref incidentFaceNormalInNew, localNormal, transform1ToNew, box0Extent);
calculateContacts(eb2, eb1, eb0, pts, incidentFaceNormalInNew, localNormal, Globals.EPS, newTransformV, resultOut, true);
break;
}
case 4:
{
if (sign[4] <= VFixedPoint.Zero)
{
mtd = axis11;
newTransformV = new VIntTransform((transform1.position + axis11 * eb1), axis10, axis12, -axis11);
}
else
{
mtd = -axis11;
newTransformV = new VIntTransform((transform1.position - axis11 * eb1), axis10, -axis12, axis11);
}
VIntTransform transform1ToNew = newTransformV.Transform(transform0);
VInt3 localNormal = newTransformV.InverseTransformDirection(mtd);
getIncidentPolygon(ref pts, ref incidentFaceNormalInNew, localNormal, transform1ToNew, box0Extent);
calculateContacts(eb0, eb2, eb1, pts, incidentFaceNormalInNew, localNormal, Globals.EPS, newTransformV, resultOut, true);
break;
}
case 5:
{
if (sign[5] <= VFixedPoint.Zero)
{
mtd = axis12;
newTransformV = new VIntTransform((transform1.position + axis12 * eb2), axis10, -axis11, -axis12);
}
else
{
mtd = -axis12;
newTransformV = new VIntTransform((transform1.position - axis12 * eb2), axis10, axis11, axis12);
}
VIntTransform transform1ToNew = newTransformV.Transform(transform0);
VInt3 localNormal = newTransformV.InverseTransformDirection(mtd);
getIncidentPolygon(ref pts, ref incidentFaceNormalInNew, localNormal, transform1ToNew, box0Extent);
calculateContacts(eb0, eb1, eb2, pts, incidentFaceNormalInNew, localNormal, Globals.EPS, newTransformV, resultOut, true);
break;
}
default:
return;
}
}
static void calculateContactInformation(VInt3[] aBuf, VInt3[] bBuf, Facet facet, CollisionShape a, CollisionShape b, ref VInt3 pa, ref VInt3 pb, ref VInt3 normal, ref VFixedPoint penDepth)
{
VInt3 _pa = new VInt3();
VInt3 _pb = new VInt3();
facet.getClosestPoint(aBuf, bBuf, ref _pa, ref _pb);
//dist > 0 means two shapes are penetrated. If dist < 0(when origin isn't inside the polytope), two shapes status are unknown
VFixedPoint dist = facet.getPlaneDist();
//planeNormal is pointing from B to A, however, the normal we are expecting is from A to B which match the GJK margin intersect case, therefore,
//we need to flip the normal
VInt3 n = -facet.getPlaneNormal();
pa = _pa;
pb = _pb;
normal = n;
penDepth = -dist;
}
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);
}
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 distanceSegmentBoxSquared(VInt3 p0, VInt3 p1, VInt3 boxHalfExtent, VIntTransform boxTransform, ref VFixedPoint segmentParam,
ref VInt3 boxParam)
{
VFixedPoint lp = VFixedPoint.Zero;
VInt3 bp = VInt3.zero;
VFixedPoint sqrDistance = distanceLineBoxSquared(p0, p1 - p0, boxHalfExtent, boxTransform, ref lp, ref bp);
if (lp >= VFixedPoint.Zero)
{
if (lp <= VFixedPoint.One)
{
segmentParam = lp;
boxParam = bp;
return(sqrDistance);
}
else
{
segmentParam = VFixedPoint.One;
return(BoxPointDistance.distancePointBoxSquared(p1, boxTransform, boxHalfExtent, ref boxParam));
}
}
else
{
segmentParam = VFixedPoint.Zero;
return(BoxPointDistance.distancePointBoxSquared(p0, boxTransform, boxHalfExtent, ref boxParam));
}
}
public static bool sweepSphereTriangles(Triangle[] triangles, int nbTris,
VInt3 center, VFixedPoint radius,
VInt3 unitDir, VFixedPoint distance,
ref VFixedPoint fraction, ref VInt3 hitNormal,
bool testInitialOverlap)
{
if (triangles.Length == 0)
{
return(false);
}
VFixedPoint curT = distance;
VFixedPoint dpc0 = VInt3.Dot(center, unitDir);
VFixedPoint bestAlignment = VFixedPoint.Two;
VInt3 bestTriNormal = VInt3.zero;
int index = -1;
for (int i = 0; i < nbTris; i++)
{
Triangle currentTri = triangles[i];
if (rejectTriangle(center, unitDir, curT, radius, currentTri.verts, dpc0))
{
continue;
}
VInt3 triNormal = currentTri.normal;
if (VInt3.Dot(triNormal, unitDir) > VFixedPoint.Zero)
{
continue;
}
VFixedPoint magnitude = triNormal.magnitude;
if (magnitude == VFixedPoint.Zero)
{
continue;
}
triNormal /= magnitude;
VFixedPoint currentDistance = VFixedPoint.Zero;
bool unused = false;
if (!sweepSphereVsTri(currentTri.verts, triNormal, center, radius, unitDir, distance, ref currentDistance, ref unused, testInitialOverlap))
{
continue;
}
VFixedPoint hitDot = -VInt3.Dot(triNormal, unitDir).Abs();
if (!keepTriangle(currentDistance, hitDot, curT, bestAlignment, distance, Globals.EPS))
{
continue;
}
if (currentDistance == VFixedPoint.Zero)
{
hitNormal = -unitDir;
fraction = VFixedPoint.Zero;
return(true);
}
curT = currentDistance;
bestAlignment = hitDot;
bestTriNormal = triNormal;
index = i;
}
curT = curT / distance;
return(computeSphereTriangleImpactData(index, curT, center, unitDir, bestTriNormal, triangles, ref fraction, ref hitNormal));
}
public void addSingleResult(CollisionObject collisionObject, VInt3 hitNormalWorld, VFixedPoint hitFraction)
{
hasHit = true;
if (hitFraction < closestHitFraction)
{
closestHitFraction = hitFraction;
this.hitNormalWorld = hitNormalWorld;
hitObject = collisionObject;
}
}