bool sweepConvex_ConvexGeom(const PxGeometry& geom, const PxTransform& pose, const PxConvexMeshGeometry& convexGeom, const PxTransform& convexPose,
const PxVec3& unitDir, const PxReal distance, PxSweepHit& sweepHit, PxHitFlags hintFlags, const PxReal inflation)
{
using namespace Ps::aos;
PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);
const PxConvexMeshGeometry& otherConvexGeom = static_cast<const PxConvexMeshGeometry&>(geom);
ConvexMesh& otherConvexMesh = *static_cast<ConvexMesh*>(otherConvexGeom.convexMesh);
FETCH_CONVEX_HULL_DATA(convexGeom)
// PT: TODO: find a way to use the FETCH_CONVEX_HULL_DATA macro for the second hull as well
#ifdef __SPU__
PX_COMPILE_TIME_ASSERT(&((ConvexMesh*)NULL)->getHull()==NULL);
PX_ALIGN_PREFIX(16) PxU8 otherconvexMeshBuffer[sizeof(ConvexMesh)+32] PX_ALIGN_SUFFIX(16);
ConvexMesh* otherMesh = memFetchAsync<ConvexMesh>(otherconvexMeshBuffer, (uintptr_t)(&otherConvexMesh), sizeof(ConvexMesh),1);
memFetchWait(1); // convexMesh
PxU32 otherNPolys = otherMesh->getNbPolygonsFast();
const HullPolygonData* PX_RESTRICT otherPolysEA = otherMesh->getPolygons();
const PxU32 otherPolysSize = sizeof(HullPolygonData)*otherNPolys + sizeof(PxVec3)*otherMesh->getNbVerts();
//TODO: Need optimization with dma cache --jiayang
void* otherHullBuffer = PxAlloca(CELL_ALIGN_SIZE_16(otherPolysSize+32));
HullPolygonData* otherPolys = memFetchAsync<HullPolygonData>(otherHullBuffer, (uintptr_t)(otherPolysEA), otherPolysSize, 1);
ConvexHullData* otherHullData = &otherMesh->getHull();
otherHullData->mPolygons = otherPolys;
memFetchWait(1); // convexMesh
#else
ConvexHullData* otherHullData = &otherConvexMesh.getHull();
#endif
const Vec3V zeroV = V3Zero();
const FloatV zero = FZero();
const Vec3V otherVScale = V3LoadU(otherConvexGeom.scale.scale);
const QuatV otherVQuat = QuatVLoadU(&otherConvexGeom.scale.rotation.x);
const Vec3V vScale = Vec3V_From_Vec4V(V4LoadU(&convexGeom.scale.scale.x));
const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x);
const PsTransformV otherTransf = loadTransformU(pose);
const PsTransformV convexTransf = loadTransformU(convexPose);
const Vec3V worldDir = V3LoadU(unitDir);
const FloatV dist = FLoad(distance);
const Vec3V dir = convexTransf.rotateInv(V3Scale(worldDir, dist));
const PsMatTransformV aToB(convexTransf.transformInv(otherTransf));
ConvexHullV otherConvexHull(otherHullData, zeroV, otherVScale, otherVQuat);
ConvexHullV convexHull(hullData, zeroV, vScale, vQuat);
bool isMtd = hintFlags & PxHitFlag::eMTD;
FloatV toi;
Vec3V closestA, normal;
bool hit = GJKRelativeRayCast(otherConvexHull, convexHull, aToB, zero, zeroV, dir, toi, normal, closestA,
inflation, isMtd);
if(hit)
{
sweepHit.flags = PxHitFlag::eDISTANCE | PxHitFlag::eNORMAL;
if(FAllGrtrOrEq(zero, toi))
{
//initial overlap
if(!(PX_IS_SPU) && isMtd)
{
sweepHit.flags |= PxHitFlag::ePOSITION;
const Vec3V worldPointA = convexTransf.transform(closestA);
const Vec3V destNormal = V3Neg(V3Normalize(convexTransf.rotate(normal)));
const FloatV length = toi;
V3StoreU(destNormal, sweepHit.normal);
V3StoreU(worldPointA, sweepHit.position);
FStore(length, &sweepHit.distance);
}
else
{
sweepHit.distance = 0.0f;
sweepHit.normal = -unitDir;
}
}
else
{
sweepHit.flags |= PxHitFlag::ePOSITION;
const Vec3V worldPointA = convexTransf.transform(closestA);
const Vec3V destNormal = V3Neg(V3Normalize(convexTransf.rotate(normal)));
const FloatV length = FMul(dist, toi);
V3StoreU(destNormal, sweepHit.normal);
V3StoreU(worldPointA, sweepHit.position);
FStore(length, &sweepHit.distance);
}
// PT: compute closest polygon using the same tweak as in swept-capsule-vs-mesh
sweepHit.faceIndex = computeSweepConvexPlane(convexGeom,hullData,nbPolys,pose,sweepHit.position,unitDir);
return true;
}
return false;
}
bool sweepBox_ConvexGeom(GU_BOX_SWEEP_FUNC_PARAMS)
{
using namespace Ps::aos;
PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);
const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom);
PX_ALIGN_PREFIX(16) PxTransform boxTransform PX_ALIGN_SUFFIX(16); boxTransform = box.getTransform();
FETCH_CONVEX_HULL_DATA(convexGeom)
const Vec3V zeroV = V3Zero();
const FloatV zero = FZero();
const PsTransformV boxPose = loadTransformA(boxTransform);
const PsTransformV convexPose = loadTransformU(pose);
const PsMatTransformV aToB(convexPose.transformInv(boxPose));
const Vec3V boxExtents = V3LoadU(box.extents);
const Vec3V vScale = V3LoadU(convexGeom.scale.scale);
const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x);
BoxV boxV(zeroV, boxExtents);
ConvexHullV convexHull(hullData, zeroV, vScale, vQuat);
const Vec3V worldDir = V3LoadU(unitDir);
const FloatV dist = FLoad(distance);
const Vec3V dir =convexPose.rotateInv(V3Neg(V3Scale(worldDir, dist)));
bool isMtd = hintFlags & PxHitFlag::eMTD;
FloatV toi;
Vec3V closestA, normal;
bool hit = GJKRelativeRayCast(boxV, convexHull, aToB, zero, zeroV, dir, toi, normal, closestA,
inflation, isMtd);
if(hit)
{
const Vec3V worldPointA = convexPose.transform(closestA);
sweepHit.flags = PxHitFlag::eDISTANCE | PxHitFlag::eNORMAL;
if(FAllGrtrOrEq(zero, toi))
{
//ML: initial overlap
if(!(PX_IS_SPU) && isMtd)
{
sweepHit.flags |= PxHitFlag::ePOSITION;
const Vec3V destNormal = V3Normalize(convexPose.rotate(normal));
const FloatV length = toi;
const Vec3V destWorldPointA =V3NegScaleSub(destNormal, length, worldPointA);
V3StoreU(destNormal, sweepHit.normal);
V3StoreU(destWorldPointA, sweepHit.position);
FStore(length, &sweepHit.distance);
}
else
{
sweepHit.distance = 0.0f;
sweepHit.normal = -unitDir;
}
}
else
{
sweepHit.flags |= PxHitFlag::ePOSITION;
const Vec3V destNormal = V3Normalize(convexPose.rotate(normal));
const FloatV length = FMul(dist, toi);
const Vec3V destWorldPointA = V3ScaleAdd(worldDir, length, worldPointA);
V3StoreU(destNormal, sweepHit.normal);
V3StoreU(destWorldPointA, sweepHit.position);
FStore(length, &sweepHit.distance);
}
// PT: compute closest polygon using the same tweak as in swept-capsule-vs-mesh
sweepHit.faceIndex = computeSweepConvexPlane(convexGeom,hullData,nbPolys,pose,sweepHit.position,unitDir);
return true;
}
return false;
}
bool sweepConvex_SphereGeom(const PxGeometry& geom, const PxTransform& pose, const PxConvexMeshGeometry& convexGeom, const PxTransform& convexPose,
const PxVec3& unitDir, const PxReal distance, PxSweepHit& sweepHit, PxHitFlags hintFlags, const PxReal inflation)
{
PX_ASSERT(geom.getType() == PxGeometryType::eSPHERE);
const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom);
FETCH_CONVEX_HULL_DATA(convexGeom)
const Vec3V zeroV = V3Zero();
const FloatV zero= FZero();
const Vec3V vScale = Vec3V_From_Vec4V(V4LoadU(&convexGeom.scale.scale.x));
const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x);
const FloatV sphereRadius = FLoad(sphereGeom.radius);
const PsTransformV sphereTransf = loadTransformU(pose);
const PsTransformV convexTransf = loadTransformU(convexPose);
const PsMatTransformV aToB(convexTransf.transformInv(sphereTransf));
const Vec3V worldDir = V3LoadU(unitDir);
const FloatV dist = FLoad(distance);
const Vec3V dir = convexTransf.rotateInv(V3Scale(worldDir, dist));
ConvexHullV convexHull(hullData, zeroV, vScale, vQuat);
//CapsuleV capsule(zeroV, sphereRadius);
CapsuleV capsule(aToB.p, sphereRadius);
const bool isMtd = hintFlags & PxHitFlag::eMTD;
FloatV toi;
Vec3V closestA, normal;
//bool hit = GJKRelativeRayCast(capsule, convexHull, aToB, zero, zeroV, dir, toi, normal, closestA, inflation);
bool hit = GJKLocalRayCast(capsule, convexHull, zero, zeroV, dir, toi, normal, closestA,
sphereGeom.radius+inflation, isMtd);
if(hit)
{
sweepHit.faceIndex = 0xffffffff;
//closestA = V3NegScaleSub(normal, sphereRadius, closestA);
const Vec3V destWorldPointA = convexTransf.transform(closestA);
sweepHit.flags = PxHitFlag::eDISTANCE | PxHitFlag::eNORMAL;
if(FAllGrtrOrEq(zero, toi))
{
//ML: initial overlap
if(!(PX_IS_SPU) && isMtd)
{
sweepHit.flags |= PxHitFlag::ePOSITION;
const Vec3V destNormal = V3Neg(V3Normalize(convexTransf.rotate(normal)));
const FloatV length = toi;
V3StoreU(destNormal, sweepHit.normal);
V3StoreU(destWorldPointA, sweepHit.position);
FStore(length, &sweepHit.distance);
}
else
{
sweepHit.distance = 0.0f;
sweepHit.normal = -unitDir;
}
}
else
{
sweepHit.flags |= PxHitFlag::ePOSITION;
const Vec3V destNormal = V3Neg(V3Normalize(convexTransf.rotate(normal)));
const FloatV length = FMul(dist, toi);
V3StoreU(destNormal, sweepHit.normal);
V3StoreU(destWorldPointA, sweepHit.position);
FStore(length, &sweepHit.distance);
}
return true;
}
return false;
}
bool sweepCapsule_ConvexGeom(GU_CAPSULE_SWEEP_FUNC_PARAMS)
{
PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);
using namespace Ps::aos;
PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);
const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom);
FETCH_CONVEX_HULL_DATA(convexGeom)
PxReal _capsuleHalfHeight = 0.0f;
const PxTransform capTransform = getCapsuleTransform(lss, _capsuleHalfHeight);
const Vec3V zeroV = V3Zero();
const FloatV zero = FZero();
const FloatV dist = FLoad(distance);
const Vec3V worldDir = V3LoadU(unitDir);
const PsTransformV capPose = loadTransformU(capTransform);
const PsTransformV convexPose = loadTransformU(pose);
const PsMatTransformV aToB(convexPose.transformInv(capPose));
//const PsMatTransformV aToB(pose.transformInv(capsuleTransform));
const FloatV capsuleHalfHeight = FLoad(_capsuleHalfHeight);
const FloatV capsuleRadius = FLoad(lss.radius);
const Vec3V vScale = Vec3V_From_Vec4V(V4LoadU(&convexGeom.scale.scale.x));
const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x);
CapsuleV capsule(aToB.p, aToB.rotate( V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius);
//CapsuleV capsule(zeroV, V3Scale(V3UnitX(), capsuleHalfHeight), capsuleRadius);
ConvexHullV convexHull(hullData, zeroV, vScale, vQuat);
const Vec3V dir = convexPose.rotateInv(V3Neg(V3Scale(worldDir, dist)));
bool isMtd = hintFlags & PxHitFlag::eMTD;
FloatV toi;
Vec3V closestA, normal;//closestA and normal is in the local space of convex hull
bool hit = GJKLocalRayCast(capsule, convexHull, zero, zeroV, dir, toi, normal, closestA,
lss.radius + inflation, isMtd);
if(hit)
{
sweepHit.flags = PxHitFlag::eDISTANCE | PxHitFlag::eNORMAL;
if(FAllGrtrOrEq(zero, toi))
{
if(!(PX_IS_SPU) && isMtd)
{
sweepHit.flags |= PxHitFlag::ePOSITION;
const FloatV length = toi;
const Vec3V destNormal = V3Normalize(convexPose.rotate(normal));
const Vec3V worldPointA = convexPose.transform(closestA);
const Vec3V destWorldPointA = V3NegScaleSub(destNormal, length, worldPointA);
V3StoreU(destNormal, sweepHit.normal);
V3StoreU(destWorldPointA, sweepHit.position);
FStore(length, &sweepHit.distance);
}
else
{
sweepHit.distance = 0.f;
sweepHit.normal = -unitDir;
}
}
else
{
sweepHit.flags |= PxHitFlag::ePOSITION;
const Vec3V worldPointA = convexPose.transform(closestA);
const FloatV length = FMul(dist, toi);
const Vec3V destNormal = V3Normalize(convexPose.rotate(normal));
const Vec3V destWorldPointA = V3ScaleAdd(worldDir, length, worldPointA);
V3StoreU(destNormal, sweepHit.normal);
V3StoreU(destWorldPointA, sweepHit.position);
FStore(length, &sweepHit.distance);
}
// PT: compute closest polygon using the same tweak as in swept-capsule-vs-mesh
sweepHit.faceIndex = computeSweepConvexPlane(convexGeom,hullData,nbPolys,pose,sweepHit.position,unitDir);
//pxPrintf("fi = %d, pos=%.7f %.7f %.7f\n",
// sweepHit.faceIndex, sweepHit.position.x, sweepHit.position.y, sweepHit.position.z);
return true;
}
return false;
}
bool sweepCapsule_BoxGeom(GU_CAPSULE_SWEEP_FUNC_PARAMS)
{
PX_UNUSED(hintFlags);
using namespace Ps::aos;
PX_ASSERT(geom.getType() == PxGeometryType::eBOX);
const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom);
const FloatV zero = FZero();
const Vec3V zeroV = V3Zero();
const Vec3V boxExtents0 = V3LoadU(boxGeom.halfExtents);
const FloatV dist = FLoad(distance);
const Vec3V worldDir = V3LoadU(unitDir);
PxReal _capsuleHalfHeight = 0.0f;
const PxTransform capTransform = getCapsuleTransform(lss, _capsuleHalfHeight);
const PsTransformV capPos = loadTransformU(capTransform);
const PsTransformV boxPos = loadTransformU(pose);
const PsMatTransformV aToB(boxPos.transformInv(capPos));
const FloatV capsuleHalfHeight = FLoad(_capsuleHalfHeight);
const FloatV capsuleRadius = FLoad(lss.radius);
BoxV box(zeroV, boxExtents0);
CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius);
const Vec3V dir = boxPos.rotateInv(V3Neg(V3Scale(worldDir, dist)));
const bool isMtd = hintFlags & PxHitFlag::eMTD;
FloatV toi = FMax();
Vec3V closestA, normal;//closestA and normal is in the local space of box
bool hit = GJKLocalRayCast(capsule, box, zero, zeroV, dir, toi, normal, closestA, lss.radius + inflation, isMtd);
if(hit)
{
sweepHit.flags = PxHitFlag::eDISTANCE | PxHitFlag::eNORMAL;
if(FAllGrtrOrEq(zero, toi))
{
//initial overlap
if((!PX_IS_SPU) && isMtd)
{
sweepHit.flags |= PxHitFlag::ePOSITION;
const Vec3V worldPointA = boxPos.transform(closestA);
const Vec3V destNormal = boxPos.rotate(normal);
const FloatV length = toi;
const Vec3V destWorldPointA = V3NegScaleSub(destNormal, length, worldPointA);
V3StoreU(destWorldPointA, sweepHit.position);
V3StoreU(destNormal, sweepHit.normal);
FStore(length, &sweepHit.distance);
}
else
{
sweepHit.distance = 0.0f;
sweepHit.normal = -unitDir;
}
}
else
{
sweepHit.flags |= PxHitFlag::ePOSITION;
const Vec3V worldPointA = boxPos.transform(closestA);
const Vec3V destNormal = boxPos.rotate(normal);
const FloatV length = FMul(dist, toi);
const Vec3V destWorldPointA = V3ScaleAdd(worldDir, length, worldPointA);
V3StoreU(destNormal, sweepHit.normal);
V3StoreU(destWorldPointA, sweepHit.position);
FStore(length, &sweepHit.distance);
}
return true;
}
return false;
}
bool sweepBox_BoxGeom(GU_BOX_SWEEP_FUNC_PARAMS)
{
PX_ASSERT(geom.getType() == PxGeometryType::eBOX);
const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom);
const FloatV zero = FZero();
const Vec3V zeroV = V3Zero();
const Vec3V boxExtents0 = V3LoadU(boxGeom.halfExtents);
const Vec3V boxExtents1 = V3LoadU(box.extents);
const FloatV worldDist = FLoad(distance);
const Vec3V unitDirV = V3LoadU(unitDir);
const PxTransform boxWorldPose = box.getTransform();
const PsTransformV boxTrans0 = loadTransformU(pose);
const PsTransformV boxTrans1 = loadTransformU(boxWorldPose);
const PsMatTransformV aToB(boxTrans1.transformInv(boxTrans0));
BoxV box0(zeroV, boxExtents0);
BoxV box1(zeroV, boxExtents1);
//transform into b space
const Vec3V dir = boxTrans1.rotateInv(V3Scale(unitDirV, worldDist));
const bool isMtd = hintFlags & PxHitFlag::eMTD;
FloatV toi;
Vec3V closestA, normal;//closestA and normal is in the local space of box
bool hit = GJKRelativeRayCast(box0, box1, aToB, zero, zeroV, dir, toi, normal, closestA, inflation, isMtd);
if(hit)
{
sweepHit.flags = PxHitFlag::eDISTANCE | PxHitFlag::eNORMAL;
if(FAllGrtrOrEq(zero, toi))
{
if((!PX_IS_SPU) && isMtd)
{
sweepHit.flags |= PxHitFlag::ePOSITION;
const FloatV length = toi;
const Vec3V destWorldPointA = boxTrans1.transform(closestA);
const Vec3V destNormal = V3Normalize(boxTrans1.rotate(normal));
V3StoreU(V3Neg(destNormal), sweepHit.normal);
V3StoreU(destWorldPointA, sweepHit.position);
FStore(length, &sweepHit.distance);
}
else
{
sweepHit.distance = 0.0f;
sweepHit.normal = -unitDir;
}
}
else
{
sweepHit.flags |= PxHitFlag::ePOSITION;
const Vec3V destWorldPointA = boxTrans1.transform(closestA);
const Vec3V destNormal = V3Normalize(boxTrans1.rotate(normal));
const FloatV length = FMul(worldDist, toi);
V3StoreU(V3Neg(destNormal), sweepHit.normal);
V3StoreU(destWorldPointA, sweepHit.position);
FStore(length, &sweepHit.distance);
}
return true;
}
return false;
}
bool sweepBox_CapsuleGeom(GU_BOX_SWEEP_FUNC_PARAMS)
{
using namespace Ps::aos;
PX_ASSERT(geom.getType() == PxGeometryType::eCAPSULE);
PX_UNUSED(hintFlags);
const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom);
const FloatV capsuleHalfHeight = FLoad(capsuleGeom.halfHeight);
const FloatV capsuleRadius = FLoad(capsuleGeom.radius);
const FloatV zero = FZero();
const Vec3V zeroV = V3Zero();
const Vec3V boxExtents = V3LoadU(box.extents);
const FloatV worldDist = FLoad(distance);
const Vec3V unitDirV = V3LoadU(unitDir);
const PxTransform boxWorldPose = box.getTransform();
const PsTransformV capPos = loadTransformU(pose);
const PsTransformV boxPos = loadTransformU(boxWorldPose);
const PsMatTransformV aToB(boxPos.transformInv(capPos));
BoxV boxV(zeroV, boxExtents);
CapsuleV capsuleV(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius);
//transform into b space
const Vec3V dir = boxPos.rotateInv(V3Scale(unitDirV, worldDist));
const bool isMtd = hintFlags & PxHitFlag::eMTD;
FloatV toi;
Vec3V closestA, normal;//closestA and normal is in the local space of box
bool hit = GJKLocalRayCast(capsuleV, boxV, zero, zeroV, dir, toi, normal, closestA, capsuleGeom.radius+inflation, isMtd);
if(hit)
{
sweepHit.flags = PxHitFlag::eDISTANCE | PxHitFlag::eNORMAL;
//initial overlap
if(FAllGrtrOrEq(zero, toi))
{
if((!PX_IS_SPU) && isMtd)
{
sweepHit.flags |= PxHitFlag::ePOSITION;
//initial overlap is toi < 0
const FloatV length = toi;
const Vec3V destWorldPointA = boxPos.transform(closestA);
const Vec3V destNormal = boxPos.rotate(normal);
V3StoreU(V3Neg(destNormal), sweepHit.normal);
V3StoreU(destWorldPointA, sweepHit.position);
FStore(length, &sweepHit.distance);
}
else
{
sweepHit.distance = 0.0f;
sweepHit.normal = -unitDir;
}
return true;
}
else
{
sweepHit.flags |= PxHitFlag::ePOSITION;
const Vec3V destWorldPointA = boxPos.transform(closestA);
const Vec3V destNormal = boxPos.rotate(normal);
const FloatV length = FMul(worldDist, toi);
V3StoreU(V3Neg(destNormal), sweepHit.normal);
V3StoreU(destWorldPointA, sweepHit.position);
FStore(length, &sweepHit.distance);
}
return true;
}
return false;
}
bool sweepBox_SphereGeom(GU_BOX_SWEEP_FUNC_PARAMS)
{
PX_ASSERT(geom.getType() == PxGeometryType::eSPHERE);
PX_UNUSED(hintFlags);
const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom);
const FloatV zero = FZero();
const Vec3V zeroV = V3Zero();
const Vec3V boxExtents = V3LoadU(box.extents);
const FloatV worldDist = FLoad(distance);
const Vec3V unitDirV = V3LoadU(unitDir);
const FloatV sphereRadius = FLoad(sphereGeom.radius);
/*
DE10168
original code:
const PxTransform boxWorldPose = box.getTransform();
starting with SDK 1.700 it breaks various PS4 non-debug tests:
SqTestSweep and RecoveryModuleTest
for some unknown reason, the vector elements end up in the wrong location
(in y where it should be in x it seems)
while we investigate the real cause, use the equivalent code directly
*/
const PxTransform boxWorldPose = PxTransform(box.center, PxQuat(box.rot));
const PsTransformV spherePos = loadTransformU(pose);
const PsTransformV boxPos = loadTransformU(boxWorldPose);
const PsMatTransformV aToB(boxPos.transformInv(spherePos));
BoxV boxV(zeroV, boxExtents);
CapsuleV capsuleV(aToB.p, sphereRadius);
//transform into b space
const Vec3V dir = boxPos.rotateInv(V3Scale(unitDirV, worldDist));
bool isMtd = hintFlags & PxHitFlag::eMTD;
FloatV toi;
Vec3V closestA, normal;//closestA and normal is in the local space of box
bool hit = GJKLocalRayCast(capsuleV, boxV, zero, zeroV, dir, toi, normal, closestA, sphereGeom.radius+inflation, isMtd);
if(hit)
{
sweepHit.flags = PxHitFlag::eDISTANCE | PxHitFlag::eNORMAL;
//initial overlap
if(FAllGrtrOrEq(zero, toi))
{
if((!PX_IS_SPU) && isMtd)
{
sweepHit.flags |= PxHitFlag::ePOSITION;
const Vec3V destWorldPointA = boxPos.transform(closestA);
const Vec3V destNormal = V3Neg(boxPos.rotate(normal));
const FloatV length = toi;
V3StoreU(destNormal, sweepHit.normal);
V3StoreU(destWorldPointA, sweepHit.position);
FStore(length, &sweepHit.distance);
}
else
{
sweepHit.distance = 0.0f;
sweepHit.normal = -unitDir;
}
}
else
{
sweepHit.flags |= PxHitFlag::ePOSITION;
const Vec3V destWorldPointA = boxPos.transform(closestA);
const Vec3V destNormal = V3Neg(boxPos.rotate(normal));
const FloatV length = FMul(worldDist, toi);
V3StoreU(destNormal, sweepHit.normal);
V3StoreU(destWorldPointA, sweepHit.position);
FStore(length, &sweepHit.distance);
}
return true;
}
return false;
}
