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; }