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;
}
// PT: test: new version for CCT, based on code for general sweeps. Just to check it works or not with rotations
// TODO: refactor this and the similar code in sweptBox for box-vs-mesh. Not so easy though.
static bool sweepBoxVsTriangles(PxU32 nbTris, const PxTriangle* triangles, const Box& box, const PxVec3& unitDir, const PxReal distance, PxSweepHit& sweepHit,
PxHitFlags hintFlags, bool isDoubleSided, const PxU32* cachedIndex)
{
if(!nbTris)
return false;
const bool meshBothSides = hintFlags & PxHitFlag::eMESH_BOTH_SIDES;
const bool doBackfaceCulling = !isDoubleSided && !meshBothSides;
// Move to AABB space
Matrix34 worldToBox;
computeWorldToBoxMatrix(worldToBox, box);
const PxVec3 localDir = worldToBox.rotate(unitDir);
const PxVec3 localMotion = localDir * distance;
bool status = false;
sweepHit.distance = distance; //was PX_MAX_F32, but that may trigger an assert in the caller!
const PxVec3 oneOverMotion(
localDir.x!=0.0f ? 1.0f/localMotion.x : 0.0f,
localDir.y!=0.0f ? 1.0f/localMotion.y : 0.0f,
localDir.z!=0.0f ? 1.0f/localMotion.z : 0.0f);
// PT: experimental code, don't clean up before I test it more and validate it
// Project box
/*float boxRadius0 =
PxAbs(dir.x) * box.extents.x
+ PxAbs(dir.y) * box.extents.y
+ PxAbs(dir.z) * box.extents.z;*/
float boxRadius =
PxAbs(localDir.x) * box.extents.x
+ PxAbs(localDir.y) * box.extents.y
+ PxAbs(localDir.z) * box.extents.z;
if(gValidateBoxRadiusComputation) // PT: run this to check the box radius is correctly computed
{
PxVec3 boxVertices2[8];
box.computeBoxPoints(boxVertices2);
float dpmin = FLT_MAX;
float dpmax = -FLT_MAX;
for(int i=0;i<8;i++)
{
const float dp = boxVertices2[i].dot(unitDir);
if(dp<dpmin) dpmin = dp;
if(dp>dpmax) dpmax = dp;
}
const float goodRadius = (dpmax-dpmin)/2.0f;
PX_UNUSED(goodRadius);
}
const float dpc0 = box.center.dot(unitDir);
float localMinDist = 1.0f;
#ifdef PX_DEBUG
PxU32 totalTestsExpected = nbTris;
PxU32 totalTestsReal = 0;
PX_UNUSED(totalTestsExpected);
PX_UNUSED(totalTestsReal);
#endif
const PxU32 idx = cachedIndex ? *cachedIndex : 0;
PxVec3 bestTriNormal(0.0f);
for(PxU32 ii=0;ii<nbTris;ii++)
{
const PxU32 triangleIndex = getTriangleIndex(ii, idx);
const PxTriangle& tri = triangles[triangleIndex];
#ifdef _XBOX
if(cullTriangle(tri, unitDir, boxRadius, localMinDist*distance, dpc0)==0.0f)
continue;
#else
if(!cullTriangle(tri, unitDir, boxRadius, localMinDist*distance, dpc0))
continue;
#endif
#ifdef PX_DEBUG
totalTestsReal++;
#endif
// Move to box space
const PxTriangle currentTriangle(
worldToBox.transform(tri.verts[0]),
worldToBox.transform(tri.verts[1]),
worldToBox.transform(tri.verts[2]));
PxF32 t = PX_MAX_F32; // could be better!
if(triBoxSweepTestBoxSpace(currentTriangle, box.extents, localMotion, oneOverMotion, localMinDist, t, doBackfaceCulling))
{
if(t <= localMinDist)
{
// PT: test if shapes initially overlap
if(t==0.0f)
{
sweepHit.flags = PxHitFlag::eDISTANCE|PxHitFlag::eNORMAL;
sweepHit.distance = 0.0f;
sweepHit.faceIndex = triangleIndex;
sweepHit.normal = -unitDir;
return true;
}
localMinDist = t;
sweepHit.distance = t * distance;
sweepHit.faceIndex = triangleIndex;
status = true;
// PT: TODO: optimize this.... already computed in triBoxSweepTestBoxSpace...
currentTriangle.denormalizedNormal(bestTriNormal);
}
}
}
if(status)
{
sweepHit.flags = PxHitFlag::eDISTANCE;
if(hintFlags & (PxHitFlag::eNORMAL|PxHitFlag::ePOSITION))
{
const PxTriangle& tri = triangles[sweepHit.faceIndex];
// Move to box space
const PxTriangle currentTriangle(
worldToBox.transform(tri.verts[0]),
worldToBox.transform(tri.verts[1]),
worldToBox.transform(tri.verts[2]));
computeBoxTriImpactData(sweepHit.position, sweepHit.normal, box.extents, localDir, localMotion, oneOverMotion, currentTriangle);
if(hintFlags & PxHitFlag::eNORMAL)
{
sweepHit.normal.normalize();
if(shouldFlipNormal(sweepHit.normal, meshBothSides, isDoubleSided, bestTriNormal, localDir))
sweepHit.normal = -sweepHit.normal;
sweepHit.normal = box.rotate(sweepHit.normal);
sweepHit.flags |= PxHitFlag::eNORMAL;
}
if(hintFlags & PxHitFlag::ePOSITION)
{
sweepHit.position = box.rotate(sweepHit.position) + box.center;
sweepHit.flags |= PxHitFlag::ePOSITION;
}
}
}
return status;
}
