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 objectQuerySingle(CollisionObject castObject, VInt3 ToPos, CollisionObject collisionObject, List <CastResult> results, VFixedPoint allowedPenetration) { bool needSwap = collisionObject.getCollisionShape() is CapsuleShape; CollisionObject capsuleObject = needSwap ? collisionObject : castObject; CollisionObject boxObject = needSwap ? castObject : collisionObject; VInt3 dir = ToPos - castObject.getWorldTransform().position; VFixedPoint length = dir.magnitude; dir = dir / length * (needSwap ? -1 : 1); VFixedPoint fraction = VFixedPoint.Zero; VInt3 hitNormal = VInt3.zero; CapsuleShape capsule = (CapsuleShape)capsuleObject.getCollisionShape(); BoxShape box = (BoxShape)boxObject.getCollisionShape(); VInt3 p0 = capsuleObject.getWorldTransform().TransformPoint(capsule.getUpAxis() * -capsule.getHalfHeight()); VInt3 p1 = capsuleObject.getWorldTransform().TransformPoint(capsule.getUpAxis() * capsule.getHalfHeight()); if (sweepCapsuleBox(p0, p1, capsule.getRadius(), box.getHalfExtent(), boxObject.getWorldTransform(), dir, length, ref fraction, ref hitNormal)) { CastResult result = new CastResult(); result.fraction = fraction; result.hitObject = collisionObject; result.normal = hitNormal * (needSwap ? -1 : 1); results.Add(result); } }
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); }
//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); }
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 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 LineShape(CapsuleShape capsule) { p0 = capsule.getUpAxis() * -capsule.getHalfHeight(); p1 = capsule.getUpAxis() * capsule.getHalfHeight(); }
public static void rayTestSingle(VInt3 fromPos, VInt3 toPos, CollisionObject collisionObject, RayResultCallback resultCallback) { CapsuleShape capsule = (CapsuleShape)collisionObject.getCollisionShape(); VInt3 p0 = collisionObject.getWorldTransform().TransformPoint(capsule.getUpAxis() * capsule.getHalfHeight()); VInt3 p1 = collisionObject.getWorldTransform().TransformPoint(capsule.getUpAxis() * -capsule.getHalfHeight()); VInt3 normal = VInt3.zero; VFixedPoint t = VFixedPoint.Zero; if (raycastCapsule(fromPos, toPos, p0, p1, capsule.getRadius(), ref normal, ref t)) { resultCallback.addSingleResult(collisionObject, normal, t); } }