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 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 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 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 Gu::SweepBoxTriangles( PxU32 nbTris, const PxTriangle* triangles, bool isDoubleSided,
const PxBoxGeometry& boxGeom, const PxTransform& boxPose, const PxVec3& dir, const PxReal length, PxVec3& _hit,
PxVec3& _normal, float& _d, PxU32& _index, const PxU32* cachedIndex, const PxReal inflation, PxHitFlags hintFlags)
{
PX_UNUSED(hintFlags);
if(!nbTris)
return false;
const bool meshBothSides = hintFlags & PxHitFlag::eMESH_BOTH_SIDES;
const bool doBackfaceCulling = !isDoubleSided && !meshBothSides;
Box box;
buildFrom1(box, boxPose.p, boxGeom.halfExtents, boxPose.q);
PxSweepHit sweepHit;
// Move to AABB space
Matrix34 worldToBox;
computeWorldToBoxMatrix(worldToBox, box);
const PxVec3 localDir = worldToBox.rotate(dir);
const PxVec3 localMotion = localDir * length;
const Vec3V base0 = V3LoadU(worldToBox.base0);
const Vec3V base1 = V3LoadU(worldToBox.base1);
const Vec3V base2 = V3LoadU(worldToBox.base2);
const Mat33V matV(base0, base1, base2);
const Vec3V p = V3LoadU(worldToBox.base3);
const PsMatTransformV worldToBoxV(p, matV);
const FloatV zero = FZero();
const Vec3V zeroV = V3Zero();
const BoolV bTrue = BTTTT();
const Vec3V boxExtents = V3LoadU(box.extents);
const Vec3V boxDir = V3LoadU(localDir);
const FloatV inflationV = FLoad(inflation);
const Vec3V absBoxDir = V3Abs(boxDir);
const FloatV boxRadiusV = FAdd(V3Dot(absBoxDir, boxExtents), inflationV);
BoxV boxV(zeroV, boxExtents);
#ifdef PX_DEBUG
PxU32 totalTestsExpected = nbTris;
PxU32 totalTestsReal = 0;
PX_UNUSED(totalTestsExpected);
PX_UNUSED(totalTestsReal);
#endif
Vec3V boxLocalMotion = V3LoadU(localMotion);
Vec3V minClosestA = zeroV, minNormal = zeroV;
PxU32 minTriangleIndex = 0;
PxVec3 bestTriNormal(0.0f);
FloatV dist = FLoad(length);
const PsTransformV boxPos = loadTransformU(boxPose);
bool status = false;
const PxU32 idx = cachedIndex ? *cachedIndex : 0;
for(PxU32 ii=0;ii<nbTris;ii++)
{
const PxU32 triangleIndex = getTriangleIndex(ii, idx);
const Vec3V localV0 = V3LoadU(triangles[triangleIndex].verts[0]);
const Vec3V localV1 = V3LoadU(triangles[triangleIndex].verts[1]);
const Vec3V localV2 = V3LoadU(triangles[triangleIndex].verts[2]);
const Vec3V triV0 = worldToBoxV.transform(localV0);
const Vec3V triV1 = worldToBoxV.transform(localV1);
const Vec3V triV2 = worldToBoxV.transform(localV2);
const Vec3V triNormal = V3Cross(V3Sub(triV2, triV1),V3Sub(triV0, triV1));
if(doBackfaceCulling && FAllGrtrOrEq(V3Dot(triNormal, boxLocalMotion), zero)) // backface culling
continue;
const FloatV dp0 = V3Dot(triV0, boxDir);
const FloatV dp1 = V3Dot(triV1, boxDir);
const FloatV dp2 = V3Dot(triV2, boxDir);
const FloatV dp = FMin(dp0, FMin(dp1, dp2));
const Vec3V dpV = V3Merge(dp0, dp1, dp2);
const FloatV temp1 = FAdd(boxRadiusV, dist);
const BoolV con0 = FIsGrtr(dp, temp1);
const BoolV con1 = V3IsGrtr(zeroV, dpV);
if(BAllEq(BOr(con0, con1), bTrue))
continue;
#ifdef PX_DEBUG
totalTestsReal++;
#endif
TriangleV triangleV(triV0, triV1, triV2);
FloatV lambda;
Vec3V closestA, normal;//closestA and normal is in the local space of convex hull
bool hit = GJKLocalRayCast(triangleV, boxV, zero, zeroV, boxLocalMotion, lambda, normal, closestA, inflation, false);
if(hit)
{
//hitCount++;
if(FAllGrtrOrEq(zero, lambda))
{
_d = 0.0f;
_index = triangleIndex;
_normal = -dir;
return true;
}
dist = FMul(dist,lambda);
boxLocalMotion = V3Scale(boxDir, dist);
minClosestA = closestA;
minNormal = normal;
minTriangleIndex = triangleIndex;
V3StoreU(triNormal, bestTriNormal);
status = true;
}
}
if(status)
{
_index = minTriangleIndex;
const Vec3V destNormal = V3Neg(V3Normalize(boxPos.rotate(minNormal)));
const Vec3V destWorldPointA = boxPos.transform(minClosestA);
V3StoreU(destNormal, _normal);
V3StoreU(destWorldPointA, _hit);
FStore(dist, &_d);
// PT: by design, returned normal is opposed to the sweep direction.
if(shouldFlipNormal(_normal, meshBothSides, isDoubleSided, bestTriNormal, dir))
_normal = -_normal;
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;
}
