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); }