static FVector FindHeightFieldOpposingNormal(const PxLocationHit& PHit, const FVector& TraceDirectionDenorm, const FVector InNormal)
{
if (IsInvalidFaceIndex(PHit.faceIndex))
{
return InNormal;
}
PxHeightFieldGeometry PHeightFieldGeom;
const bool bReadGeomSuccess = PHit.shape->getHeightFieldGeometry(PHeightFieldGeom);
check(bReadGeomSuccess); //we should only call this function when we have a heightfield
if (PHeightFieldGeom.heightField)
{
const PxU32 TriIndex = PHit.faceIndex;
const PxTransform PShapeWorldPose = PxShapeExt::getGlobalPose(*PHit.shape, *PHit.actor);
PxTriangle Tri;
PxMeshQuery::getTriangle(PHeightFieldGeom, PShapeWorldPose, TriIndex, Tri);
PxVec3 TriNormal;
Tri.normal(TriNormal);
return P2UVector(TriNormal);
}
return InNormal;
}
// PT: SAT-based version, in box space
// AP: uninlining on SPU doesn't help
int Gu::triBoxSweepTestBoxSpace(const PxTriangle& tri, const PxVec3& extents, const PxVec3& dir, const PxVec3& oneOverDir, float tmax, float& toi, bool doBackfaceCulling)
{
// Create triangle normal
PxVec3 triNormal;
tri.denormalizedNormal(triNormal);
// Backface culling
if(doBackfaceCulling && (triNormal.dot(dir)) >= 0.0f) // ">=" is important !
return 0;
// The SAT test will properly detect initial overlaps, no need for extra tests
return testSeparationAxes(tri, extents, triNormal, dir, oneOverDir, tmax, toi);
}
static bool FindHeightFieldOpposingNormal(const PxLocationHit& PHit, const FVector& TraceDirectionDenorm, FVector& OutNormal)
{
PxHeightFieldGeometry PHeightFieldGeom;
bool bSuccess = PHit.shape->getHeightFieldGeometry(PHeightFieldGeom);
check(bSuccess); //we should only call this function when we have a heightfield
if (PHeightFieldGeom.heightField)
{
const PxU32 TriIndex = PHit.faceIndex;
const PxTransform PShapeWorldPose = PxShapeExt::getGlobalPose(*PHit.shape, *PHit.actor);
PxTriangle Tri;
PxMeshQuery::getTriangle(PHeightFieldGeom, PShapeWorldPose, TriIndex, Tri);
PxVec3 TriNormal;
Tri.normal(TriNormal);
OutNormal = P2UVector(TriNormal);
return true;
}
return false;
}
static PxU32 createInvisibleWalls(const CCTParams& params, const PxTriangle& currentTriangle, TriArray& worldTriangles, IntArray& triIndicesArray)
{
const PxF32 wallHeight = params.mInvisibleWallHeight;
if(wallHeight==0.0f)
return 0;
PxU32 nbNewTris = 0; // Number of newly created tris
const PxVec3& upDirection = params.mUpDirection;
PxVec3 normal;
currentTriangle.normal(normal);
if(testSlope(normal, upDirection, params.mSlopeLimit))
{
const PxVec3 upWall = upDirection*wallHeight;
PxVec3 v0p = currentTriangle.verts[0] + upWall;
PxVec3 v1p = currentTriangle.verts[1] + upWall;
PxVec3 v2p = currentTriangle.verts[2] + upWall;
// Extrude edge 0-1
PxVec3 faceNormal01;
{
// 0-1-0p
const PxTriangle tri0_1_0p(currentTriangle.verts[0], currentTriangle.verts[1], v0p);
worldTriangles.pushBack(tri0_1_0p);
// 0p-1-1p
const PxTriangle tri0p_1_1p(v0p, currentTriangle.verts[1], v1p);
worldTriangles.pushBack(tri0p_1_1p);
tri0p_1_1p.normal(faceNormal01);
}
// Extrude edge 1-2
PxVec3 faceNormal12;
{
// 1p-1-2p
const PxTriangle tri1p_1_2p(v1p, currentTriangle.verts[1], v2p);
worldTriangles.pushBack(tri1p_1_2p);
// 2p-1-2
const PxTriangle tri2p_1_2(v2p, currentTriangle.verts[1], currentTriangle.verts[2]);
worldTriangles.pushBack(tri2p_1_2);
tri2p_1_2.normal(faceNormal12);
}
// Extrude edge 2-0
PxVec3 faceNormal20;
{
// 0p-2-0
const PxTriangle tri0p_2_0(v0p, currentTriangle.verts[2], currentTriangle.verts[0]);
worldTriangles.pushBack(tri0p_2_0);
// 0p-2p-2
const PxTriangle tri0p_2p_2(v0p, v2p, currentTriangle.verts[2]);
worldTriangles.pushBack(tri0p_2p_2);
tri0p_2p_2.normal(faceNormal20);
}
const PxU32 triIndex = PX_INVALID_U32;
for(PxU32 i=0;i<6;i++)
triIndicesArray.pushBack(triIndex);
nbNewTris += 6;
}
return nbNewTris;
}
// 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;
}