void Gu::BoxV::create(const Gu::CapsuleV& capsule)
{
using namespace Ps::aos;
const FloatV half = FloatV_From_F32(0.5f);
const FloatV cRadius = capsule.radius;
// Box center = center of the two LSS's endpoints
center = capsule.getCenter();
const Vec3V dir = capsule.computeDirection(); //capsule.p1 - capsule.p0;
const FloatV d = V3Length(dir);
rot.col0 = V3Scale(dir, FRecip(d));
//Box extent
extents = cRadius;
V3SetX(extents, FMulAdd(d, half, cRadius));
// Box orientation
computeBasis2(rot.col0, rot.col1, rot.col2);
}
bool calculateMTD(const Gu::CapsuleV& capsuleV, const Ps::aos::FloatVArg inflatedRadiusV, const bool isDoubleSide, const MTDTriangle* triangles, const PxU32 nbTriangles, const PxU32 startIndex, Gu::MeshPersistentContact* manifoldContacts,
PxU32& numContacts, Ps::aos::Vec3V& normal, Ps::aos::Vec3V& closestA, Ps::aos::Vec3V& closestB, PxU32& faceIndex, Ps::aos::FloatV& mtd)
{
using namespace Ps::aos;
const FloatV zero = FZero();
bool hadContacts = false;
FloatV deepestPen = mtd;
for(PxU32 j=0; j<nbTriangles; ++j)
{
numContacts = 0;
const MTDTriangle& curTri = triangles[j];
TriangleV triangleV;
triangleV.verts[0] = V3LoadU(curTri.verts[0]);
triangleV.verts[1] = V3LoadU(curTri.verts[1]);
triangleV.verts[2] = V3LoadU(curTri.verts[2]);
const PxU8 triFlag = curTri.extraTriData;
const Vec3V triangleNormal = triangleV.normal();
const Vec3V v = V3Sub(capsuleV.getCenter(), triangleV.verts[0]);
const FloatV dotV = V3Dot(triangleNormal, v);
// Backface culling
const bool culled = !isDoubleSide && (FAllGrtr(zero, dotV));
if(culled)
continue;
Gu::PCMCapsuleVsMeshContactGeneration::processTriangle(triangleV, j+startIndex, capsuleV, inflatedRadiusV, triFlag, manifoldContacts, numContacts);
if(numContacts ==0)
continue;
hadContacts = true;
getMTDPerTriangle(manifoldContacts, numContacts, j + startIndex, normal, closestA, closestB, faceIndex, deepestPen);
}
mtd = deepestPen;
return hadContacts;
}
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;
}