bool physx::Gu::computeCapsule_HeightFieldMTD(const PxHeightFieldGeometry& heightFieldGeom, const PxTransform& pose, Gu::CapsuleV& capsuleV, const PxReal inflatedRadius, const PxReal distance, const bool isDoubleSided, const PxU32 flags, PxSweepHit& hit)
{
using namespace Ps::aos;
const Vec3V zeroV = V3Zero();
Gu::MeshPersistentContact manifoldContacts[64];
PxU32 numContacts = 0;
FloatV distV = FLoad(distance);
//inflated the capsule by 1% in case of some disgreement between sweep and mtd calculation.If sweep said initial overlap, but mtd has a positive seperation,
//we are still be able to return a valid normal but we should zero the distance.
const FloatV inflatedRadiusV = FLoad(inflatedRadius*1.01f);
bool foundInitial = false;
static PxU32 iterations = 4;
LocalContainer(tempContainer, 128);
Gu::HeightFieldUtil hfUtil(heightFieldGeom);
Vec3V closestA=zeroV, closestB=zeroV, normal=zeroV;
PxU32 triangleIndex = 0xfffffff;
Vec3V translation = zeroV;
FloatV mtd;
for(PxU32 i=0; i<iterations; ++i)
{
tempContainer.Reset();
Gu::Capsule inflatedCapsule;
V3StoreU(capsuleV.p0, inflatedCapsule.p0);
V3StoreU(capsuleV.p1, inflatedCapsule.p1);
inflatedCapsule.radius = inflatedRadius;
Box capsuleBox;
computeBoxAroundCapsule(inflatedCapsule, capsuleBox);
const PxTransform capsuleBoxTransform = capsuleBox.getTransform();
PxBounds3 bounds = PxBounds3::poseExtent(capsuleBoxTransform, capsuleBox.extents);
midPhaseQueryHF(hfUtil, pose, bounds, tempContainer, flags);
// Get results
PxU32 nbTriangles = tempContainer.GetNbEntries();
if(!nbTriangles)
break;
PxU32* indices = tempContainer.GetEntries();
bool hadContacts = false;
PxU32 nbBatches = (nbTriangles + BATCH_TRIANGLE_NUMBER - 1)/BATCH_TRIANGLE_NUMBER;
mtd = FMax();
MTDTriangle triangles[BATCH_TRIANGLE_NUMBER];
for(PxU32 a = 0; a < nbBatches; ++a)
{
PxU32 startIndex = a * BATCH_TRIANGLE_NUMBER;
PxU32 nbTrigs = PxMin(nbTriangles - startIndex, BATCH_TRIANGLE_NUMBER);
for(PxU32 k=0; k<nbTrigs; k++)
{
//triangle vertext space
const PxU32 triangleIndex1 = indices[startIndex+k];
hfUtil.getTriangle(pose, triangles[k], NULL, NULL, triangleIndex1, true);
triangles[k].extraTriData = 0x38;
}
//ML: mtd has back face culling, so if the capsule's center is below the triangle, we won't generate any contacts
hadContacts = calculateMTD(capsuleV, inflatedRadiusV, isDoubleSided, triangles, nbTrigs, startIndex, manifoldContacts, numContacts, normal, closestA, closestB, triangleIndex, mtd) || hadContacts;
}
if(!hadContacts)
break;
triangleIndex = indices[triangleIndex];
foundInitial = true;
distV = FSub(mtd, capsuleV.radius);
if(FAllGrtrOrEq(FZero(), distV))
{
//move the capsule to depenerate it
Vec3V center = capsuleV.getCenter();
Vec3V t = V3Scale(normal, distV);
translation = V3Sub(translation, t);
center = V3Sub(center, t);
capsuleV.setCenter(center);
}
else
{
if(i == 0)
{
//First iteration so keep this normal
hit.distance = 0.f;
V3StoreU(closestA, hit.position);
V3StoreU(normal, hit.normal);
hit.faceIndex = triangleIndex;
return true;
}
break;
}
}
normal = V3Normalize(translation);
distV = FNeg(V3Length(translation));
if(foundInitial)
{
FStore(distV, &hit.distance);
V3StoreU(closestA, hit.position);
V3StoreU(normal, hit.normal);
hit.faceIndex = triangleIndex;
}
return foundInitial;
}
