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 bool getSphereDistance(BoxShape boxShape, VIntTransform m44T, VInt3 sphereCenter, VFixedPoint radius, out VInt3 normal, out VFixedPoint penetrationDepth) { VInt3 boxHalfExtent = boxShape.getHalfExtent(); VFixedPoint boxMargin = boxShape.getMargin(); penetrationDepth = VFixedPoint.Zero; // convert the sphere position to the box's local space VInt3 sphereRelPos = m44T.InverseTransformPoint(sphereCenter); // Determine the closest point to the sphere center in the box VInt3 closestPoint = sphereRelPos; closestPoint.x = (FMath.Min(boxHalfExtent.x, closestPoint.x)); closestPoint.x = (FMath.Max(-boxHalfExtent.x, closestPoint.x)); closestPoint.y = (FMath.Min(boxHalfExtent.y, closestPoint.y)); closestPoint.y = (FMath.Max(-boxHalfExtent.y, closestPoint.y)); closestPoint.z = (FMath.Min(boxHalfExtent.z, closestPoint.z)); closestPoint.z = (FMath.Max(-boxHalfExtent.z, closestPoint.z)); VFixedPoint intersectionDist = radius + boxMargin; VFixedPoint contactDist = intersectionDist; normal = sphereRelPos - closestPoint; //if there is no penetration, we are done VFixedPoint dist2 = normal.sqrMagnitude; if (dist2 > contactDist * contactDist) { return(false); } VFixedPoint distance; //special case if the sphere center is inside the box if (dist2 <= Globals.EPS) { distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, ref closestPoint, out normal); } else //compute the penetration details { distance = normal.magnitude; normal /= distance; } // v3PointOnSphere = sphereRelPos - (normal * fRadius); penetrationDepth = distance - intersectionDist; VInt3 tmp = m44T.TransformDirection(normal); normal = tmp; return(true); }
public static bool sweepSphereBox(SphereShape sphere, VInt3 fromPos, VInt3 ToPos, BoxShape box, VIntTransform boxTransform, ref VFixedPoint dist, ref VInt3 normal) { VFixedPoint tmp; VInt3 dir = ToPos - fromPos; if (SphereBoxCollisionAlgorithm.getSphereDistance(box, boxTransform, fromPos, sphere.getRadius(), out normal, out tmp)) { normal = (fromPos - ToPos).Normalize(); dist = VFixedPoint.Zero; return(true); } VInt3 aabbMin = boxTransform.position - box.getHalfExtent(), aabbMax = boxTransform.position + box.getHalfExtent(); aabbMax += VInt3.one * sphere.getRadius(); aabbMin -= VInt3.one * sphere.getRadius(); return(BoxRaytestAlgorithm.rayTestBox(fromPos, ToPos, aabbMax, boxTransform, ref dist, ref normal)); }
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); } } }
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); } } }