public static void processCollision(CollisionObject col0, CollisionObject col1, DispatcherInfo dispatchInfo,
PersistentManifold resultOut)
{
SphereShape sphere0 = (SphereShape)col0.getCollisionShape();
SphereShape sphere1 = (SphereShape)col1.getCollisionShape();
VInt3 diff = col0.getWorldTransform().position - col1.getWorldTransform().position;
VFixedPoint len = diff.magnitude;
VFixedPoint radius0 = sphere0.getRadius();
VFixedPoint radius1 = sphere1.getRadius();
// if distance positive, don't generate a new contact
if (len > (radius0 + radius1))
{
return;
}
// distance (negative means penetration)
VFixedPoint dist = len - (radius0 + radius1);
VInt3 normalOnSurfaceB = VInt3.zero;
if (dist < Globals.EPS)
{
normalOnSurfaceB = diff / len;
}
// report a contact. internally this will be kept persistent, and contact reduction is done
ManifoldPoint contactPoint = new ManifoldPoint(col0.getWorldTransform().position - normalOnSurfaceB * radius0, col1.getWorldTransform().position + normalOnSurfaceB * radius1, normalOnSurfaceB, dist);
resultOut.addManifoldPoint(contactPoint);
}
protected bool recoverFromPenetration(CollisionWorld collisionWorld)
{
bool penetration = false;
manifolds.Clear();
collisionWorld.OverlapTest(me, manifolds);
for (int i = 0; i < manifolds.Count; i++)
{
PersistentManifold aresult = manifolds[i];
int directionSign = aresult.body0 == me ? 1 : -1;
for (int j = 0; j < aresult.getContactPointsNum(); j++)
{
ManifoldPoint apoint = aresult.getManifoldPoint(j);
VFixedPoint pen = apoint.distance;
if (pen >= VFixedPoint.Zero)
{
continue;
}
currentPosition += apoint.normalWorldOnB * directionSign * pen * alpha;
penetration = true;
}
}
return(penetration);
}
public override bool process(BroadphaseProxy proxy)
{
///terminate further ray tests, once the closestHitFraction reached zero
if (aabbMin == aabbMax)
{
return(false);
}
CollisionObject collisionObject = proxy.clientObject;
PersistentManifold result = new PersistentManifold(this.collisionObject, collisionObject);
//only perform raycast if filterMask matches
CollisionAlgorithm algorithm = dispatcher.findAlgorithm(collisionObject, this.collisionObject);
algorithm(collisionObject, this.collisionObject,
dispatcher.getDispatchInfo(),
result);
if (result.getContactPointsNum() > 0)
{
results.Add(result);
}
return(true);
}
public static void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, PersistentManifold resultOut)
{
}
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);
}
public static void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, PersistentManifold resultOut)
{
bool isSwapped = body1.getCollisionShape() is SphereShape;
CollisionObject sphereObj = isSwapped ? body1 : body0;
CollisionObject boxObj = isSwapped ? body0 : body1;
VInt3 normalOnSurfaceB;
VFixedPoint penetrationDepth;
VInt3 sphereCenter = sphereObj.getWorldTransform().position;
SphereShape sphere0 = (SphereShape)sphereObj.getCollisionShape();
VFixedPoint radius = sphere0.getRadius();
if (getSphereDistance((BoxShape)boxObj.getCollisionShape(), boxObj.getWorldTransform(), sphereCenter, radius, out normalOnSurfaceB, out penetrationDepth))
{
VInt3 worldPosOnSphere = sphereCenter - normalOnSurfaceB * radius;
VInt3 worldPosOnBox = worldPosOnSphere - normalOnSurfaceB * penetrationDepth;
ManifoldPoint contactPoint = new ManifoldPoint(isSwapped ? worldPosOnBox : worldPosOnSphere, isSwapped ? worldPosOnSphere : worldPosOnBox, normalOnSurfaceB * (isSwapped ? -1 : 1), penetrationDepth);
resultOut.addManifoldPoint(contactPoint);
}
}
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);
}
}
}
public BroadphasePair(BroadphaseProxy pProxy0, BroadphaseProxy pProxy1)
{
this.pProxy0 = pProxy0;
this.pProxy1 = pProxy1;
manifold = new PersistentManifold(pProxy0.clientObject, pProxy1.clientObject);
}
public static void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, PersistentManifold resultOut)
{
BoxShape box0 = (BoxShape)body0.getCollisionShape();
BoxShape box1 = (BoxShape)body1.getCollisionShape();
VIntTransform transform0 = body0.getWorldTransform();
VIntTransform transform1 = body1.getWorldTransform();
VInt3 box0Extent = box0.getHalfExtent();
VInt3 box1Extent = box1.getHalfExtent();
doBoxBoxGenerateContacts(box0Extent, box1Extent, transform0, transform1, resultOut);
if (resultOut.getContactPointsNum() == 0)
{
VInt3 pa = VInt3.zero, pb = VInt3.zero, normal = VInt3.zero;
VFixedPoint depth = VFixedPoint.Zero;
PxGJKStatus result = EpaSolver.calcPenDepth(box0, box1, transform0, transform1, ref pa, ref pb, ref normal, ref depth);
if (result == PxGJKStatus.EPA_CONTACT)
{
ManifoldPoint contactPoint = new ManifoldPoint(pa, pb, normal, depth);
resultOut.addManifoldPoint(contactPoint);
}
}
}
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 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;
}
}
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);
}
