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