bool sweepConvex_SphereGeom(const PxGeometry& geom, const PxTransform& pose, const PxConvexMeshGeometry& convexGeom, const PxTransform& convexPose, const PxVec3& unitDir, const PxReal distance, PxSweepHit& sweepHit, PxHitFlags hintFlags, const PxReal inflation) { PX_ASSERT(geom.getType() == PxGeometryType::eSPHERE); const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom); FETCH_CONVEX_HULL_DATA(convexGeom) const Vec3V zeroV = V3Zero(); const FloatV zero= FZero(); const Vec3V vScale = Vec3V_From_Vec4V(V4LoadU(&convexGeom.scale.scale.x)); const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x); const FloatV sphereRadius = FLoad(sphereGeom.radius); const PsTransformV sphereTransf = loadTransformU(pose); const PsTransformV convexTransf = loadTransformU(convexPose); const PsMatTransformV aToB(convexTransf.transformInv(sphereTransf)); const Vec3V worldDir = V3LoadU(unitDir); const FloatV dist = FLoad(distance); const Vec3V dir = convexTransf.rotateInv(V3Scale(worldDir, dist)); ConvexHullV convexHull(hullData, zeroV, vScale, vQuat); //CapsuleV capsule(zeroV, sphereRadius); CapsuleV capsule(aToB.p, sphereRadius); const bool isMtd = hintFlags & PxHitFlag::eMTD; FloatV toi; Vec3V closestA, normal; //bool hit = GJKRelativeRayCast(capsule, convexHull, aToB, zero, zeroV, dir, toi, normal, closestA, inflation); bool hit = GJKLocalRayCast(capsule, convexHull, zero, zeroV, dir, toi, normal, closestA, sphereGeom.radius+inflation, isMtd); if(hit) { sweepHit.faceIndex = 0xffffffff; //closestA = V3NegScaleSub(normal, sphereRadius, closestA); const Vec3V destWorldPointA = convexTransf.transform(closestA); sweepHit.flags = PxHitFlag::eDISTANCE | PxHitFlag::eNORMAL; if(FAllGrtrOrEq(zero, toi)) { //ML: initial overlap if(!(PX_IS_SPU) && isMtd) { sweepHit.flags |= PxHitFlag::ePOSITION; const Vec3V destNormal = V3Neg(V3Normalize(convexTransf.rotate(normal))); const FloatV length = toi; V3StoreU(destNormal, sweepHit.normal); V3StoreU(destWorldPointA, sweepHit.position); FStore(length, &sweepHit.distance); } else { sweepHit.distance = 0.0f; sweepHit.normal = -unitDir; } } else { sweepHit.flags |= PxHitFlag::ePOSITION; const Vec3V destNormal = V3Neg(V3Normalize(convexTransf.rotate(normal))); const FloatV length = FMul(dist, toi); V3StoreU(destNormal, sweepHit.normal); V3StoreU(destWorldPointA, sweepHit.position); FStore(length, &sweepHit.distance); } return true; } return false; }
bool sweepConvex_ConvexGeom(const PxGeometry& geom, const PxTransform& pose, const PxConvexMeshGeometry& convexGeom, const PxTransform& convexPose, const PxVec3& unitDir, const PxReal distance, PxSweepHit& sweepHit, PxHitFlags hintFlags, const PxReal inflation) { using namespace Ps::aos; PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH); const PxConvexMeshGeometry& otherConvexGeom = static_cast<const PxConvexMeshGeometry&>(geom); ConvexMesh& otherConvexMesh = *static_cast<ConvexMesh*>(otherConvexGeom.convexMesh); FETCH_CONVEX_HULL_DATA(convexGeom) // PT: TODO: find a way to use the FETCH_CONVEX_HULL_DATA macro for the second hull as well #ifdef __SPU__ PX_COMPILE_TIME_ASSERT(&((ConvexMesh*)NULL)->getHull()==NULL); PX_ALIGN_PREFIX(16) PxU8 otherconvexMeshBuffer[sizeof(ConvexMesh)+32] PX_ALIGN_SUFFIX(16); ConvexMesh* otherMesh = memFetchAsync<ConvexMesh>(otherconvexMeshBuffer, (uintptr_t)(&otherConvexMesh), sizeof(ConvexMesh),1); memFetchWait(1); // convexMesh PxU32 otherNPolys = otherMesh->getNbPolygonsFast(); const HullPolygonData* PX_RESTRICT otherPolysEA = otherMesh->getPolygons(); const PxU32 otherPolysSize = sizeof(HullPolygonData)*otherNPolys + sizeof(PxVec3)*otherMesh->getNbVerts(); //TODO: Need optimization with dma cache --jiayang void* otherHullBuffer = PxAlloca(CELL_ALIGN_SIZE_16(otherPolysSize+32)); HullPolygonData* otherPolys = memFetchAsync<HullPolygonData>(otherHullBuffer, (uintptr_t)(otherPolysEA), otherPolysSize, 1); ConvexHullData* otherHullData = &otherMesh->getHull(); otherHullData->mPolygons = otherPolys; memFetchWait(1); // convexMesh #else ConvexHullData* otherHullData = &otherConvexMesh.getHull(); #endif const Vec3V zeroV = V3Zero(); const FloatV zero = FZero(); const Vec3V otherVScale = V3LoadU(otherConvexGeom.scale.scale); const QuatV otherVQuat = QuatVLoadU(&otherConvexGeom.scale.rotation.x); const Vec3V vScale = Vec3V_From_Vec4V(V4LoadU(&convexGeom.scale.scale.x)); const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x); const PsTransformV otherTransf = loadTransformU(pose); const PsTransformV convexTransf = loadTransformU(convexPose); const Vec3V worldDir = V3LoadU(unitDir); const FloatV dist = FLoad(distance); const Vec3V dir = convexTransf.rotateInv(V3Scale(worldDir, dist)); const PsMatTransformV aToB(convexTransf.transformInv(otherTransf)); ConvexHullV otherConvexHull(otherHullData, zeroV, otherVScale, otherVQuat); ConvexHullV convexHull(hullData, zeroV, vScale, vQuat); bool isMtd = hintFlags & PxHitFlag::eMTD; FloatV toi; Vec3V closestA, normal; bool hit = GJKRelativeRayCast(otherConvexHull, convexHull, aToB, zero, zeroV, dir, toi, normal, closestA, inflation, isMtd); if(hit) { sweepHit.flags = PxHitFlag::eDISTANCE | PxHitFlag::eNORMAL; if(FAllGrtrOrEq(zero, toi)) { //initial overlap if(!(PX_IS_SPU) && isMtd) { sweepHit.flags |= PxHitFlag::ePOSITION; const Vec3V worldPointA = convexTransf.transform(closestA); const Vec3V destNormal = V3Neg(V3Normalize(convexTransf.rotate(normal))); const FloatV length = toi; V3StoreU(destNormal, sweepHit.normal); V3StoreU(worldPointA, sweepHit.position); FStore(length, &sweepHit.distance); } else { sweepHit.distance = 0.0f; sweepHit.normal = -unitDir; } } else { sweepHit.flags |= PxHitFlag::ePOSITION; const Vec3V worldPointA = convexTransf.transform(closestA); const Vec3V destNormal = V3Neg(V3Normalize(convexTransf.rotate(normal))); const FloatV length = FMul(dist, toi); V3StoreU(destNormal, sweepHit.normal); V3StoreU(worldPointA, sweepHit.position); FStore(length, &sweepHit.distance); } // PT: compute closest polygon using the same tweak as in swept-capsule-vs-mesh sweepHit.faceIndex = computeSweepConvexPlane(convexGeom,hullData,nbPolys,pose,sweepHit.position,unitDir); return true; } return false; }
bool sweepCapsule_ConvexGeom(GU_CAPSULE_SWEEP_FUNC_PARAMS) { PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH); using namespace Ps::aos; PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH); const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom); FETCH_CONVEX_HULL_DATA(convexGeom) PxReal _capsuleHalfHeight = 0.0f; const PxTransform capTransform = getCapsuleTransform(lss, _capsuleHalfHeight); const Vec3V zeroV = V3Zero(); const FloatV zero = FZero(); const FloatV dist = FLoad(distance); const Vec3V worldDir = V3LoadU(unitDir); const PsTransformV capPose = loadTransformU(capTransform); const PsTransformV convexPose = loadTransformU(pose); const PsMatTransformV aToB(convexPose.transformInv(capPose)); //const PsMatTransformV aToB(pose.transformInv(capsuleTransform)); const FloatV capsuleHalfHeight = FLoad(_capsuleHalfHeight); const FloatV capsuleRadius = FLoad(lss.radius); const Vec3V vScale = Vec3V_From_Vec4V(V4LoadU(&convexGeom.scale.scale.x)); const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x); CapsuleV capsule(aToB.p, aToB.rotate( V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius); //CapsuleV capsule(zeroV, V3Scale(V3UnitX(), capsuleHalfHeight), capsuleRadius); ConvexHullV convexHull(hullData, zeroV, vScale, vQuat); const Vec3V dir = convexPose.rotateInv(V3Neg(V3Scale(worldDir, dist))); bool isMtd = hintFlags & PxHitFlag::eMTD; FloatV toi; Vec3V closestA, normal;//closestA and normal is in the local space of convex hull bool hit = GJKLocalRayCast(capsule, convexHull, zero, zeroV, dir, toi, normal, closestA, lss.radius + inflation, isMtd); if(hit) { sweepHit.flags = PxHitFlag::eDISTANCE | PxHitFlag::eNORMAL; if(FAllGrtrOrEq(zero, toi)) { if(!(PX_IS_SPU) && isMtd) { sweepHit.flags |= PxHitFlag::ePOSITION; const FloatV length = toi; const Vec3V destNormal = V3Normalize(convexPose.rotate(normal)); const Vec3V worldPointA = convexPose.transform(closestA); const Vec3V destWorldPointA = V3NegScaleSub(destNormal, length, worldPointA); V3StoreU(destNormal, sweepHit.normal); V3StoreU(destWorldPointA, sweepHit.position); FStore(length, &sweepHit.distance); } else { sweepHit.distance = 0.f; sweepHit.normal = -unitDir; } } else { sweepHit.flags |= PxHitFlag::ePOSITION; const Vec3V worldPointA = convexPose.transform(closestA); const FloatV length = FMul(dist, toi); const Vec3V destNormal = V3Normalize(convexPose.rotate(normal)); const Vec3V destWorldPointA = V3ScaleAdd(worldDir, length, worldPointA); V3StoreU(destNormal, sweepHit.normal); V3StoreU(destWorldPointA, sweepHit.position); FStore(length, &sweepHit.distance); } // PT: compute closest polygon using the same tweak as in swept-capsule-vs-mesh sweepHit.faceIndex = computeSweepConvexPlane(convexGeom,hullData,nbPolys,pose,sweepHit.position,unitDir); //pxPrintf("fi = %d, pos=%.7f %.7f %.7f\n", // sweepHit.faceIndex, sweepHit.position.x, sweepHit.position.y, sweepHit.position.z); return true; } return false; }
bool sweepBox_ConvexGeom(GU_BOX_SWEEP_FUNC_PARAMS) { using namespace Ps::aos; PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH); const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom); PX_ALIGN_PREFIX(16) PxTransform boxTransform PX_ALIGN_SUFFIX(16); boxTransform = box.getTransform(); FETCH_CONVEX_HULL_DATA(convexGeom) const Vec3V zeroV = V3Zero(); const FloatV zero = FZero(); const PsTransformV boxPose = loadTransformA(boxTransform); const PsTransformV convexPose = loadTransformU(pose); const PsMatTransformV aToB(convexPose.transformInv(boxPose)); const Vec3V boxExtents = V3LoadU(box.extents); const Vec3V vScale = V3LoadU(convexGeom.scale.scale); const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x); BoxV boxV(zeroV, boxExtents); ConvexHullV convexHull(hullData, zeroV, vScale, vQuat); const Vec3V worldDir = V3LoadU(unitDir); const FloatV dist = FLoad(distance); const Vec3V dir =convexPose.rotateInv(V3Neg(V3Scale(worldDir, dist))); bool isMtd = hintFlags & PxHitFlag::eMTD; FloatV toi; Vec3V closestA, normal; bool hit = GJKRelativeRayCast(boxV, convexHull, aToB, zero, zeroV, dir, toi, normal, closestA, inflation, isMtd); if(hit) { const Vec3V worldPointA = convexPose.transform(closestA); sweepHit.flags = PxHitFlag::eDISTANCE | PxHitFlag::eNORMAL; if(FAllGrtrOrEq(zero, toi)) { //ML: initial overlap if(!(PX_IS_SPU) && isMtd) { sweepHit.flags |= PxHitFlag::ePOSITION; const Vec3V destNormal = V3Normalize(convexPose.rotate(normal)); const FloatV length = toi; const Vec3V destWorldPointA =V3NegScaleSub(destNormal, length, worldPointA); V3StoreU(destNormal, sweepHit.normal); V3StoreU(destWorldPointA, sweepHit.position); FStore(length, &sweepHit.distance); } else { sweepHit.distance = 0.0f; sweepHit.normal = -unitDir; } } else { sweepHit.flags |= PxHitFlag::ePOSITION; const Vec3V destNormal = V3Normalize(convexPose.rotate(normal)); const FloatV length = FMul(dist, toi); const Vec3V destWorldPointA = V3ScaleAdd(worldDir, length, worldPointA); V3StoreU(destNormal, sweepHit.normal); V3StoreU(destWorldPointA, sweepHit.position); FStore(length, &sweepHit.distance); } // PT: compute closest polygon using the same tweak as in swept-capsule-vs-mesh sweepHit.faceIndex = computeSweepConvexPlane(convexGeom,hullData,nbPolys,pose,sweepHit.position,unitDir); return true; } return false; }
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; }
bool sweepCapsule_BoxGeom(GU_CAPSULE_SWEEP_FUNC_PARAMS) { PX_UNUSED(hintFlags); using namespace Ps::aos; PX_ASSERT(geom.getType() == PxGeometryType::eBOX); const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom); const FloatV zero = FZero(); const Vec3V zeroV = V3Zero(); const Vec3V boxExtents0 = V3LoadU(boxGeom.halfExtents); const FloatV dist = FLoad(distance); const Vec3V worldDir = V3LoadU(unitDir); PxReal _capsuleHalfHeight = 0.0f; const PxTransform capTransform = getCapsuleTransform(lss, _capsuleHalfHeight); const PsTransformV capPos = loadTransformU(capTransform); const PsTransformV boxPos = loadTransformU(pose); const PsMatTransformV aToB(boxPos.transformInv(capPos)); const FloatV capsuleHalfHeight = FLoad(_capsuleHalfHeight); const FloatV capsuleRadius = FLoad(lss.radius); BoxV box(zeroV, boxExtents0); CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius); const Vec3V dir = boxPos.rotateInv(V3Neg(V3Scale(worldDir, dist))); const bool isMtd = hintFlags & PxHitFlag::eMTD; FloatV toi = FMax(); Vec3V closestA, normal;//closestA and normal is in the local space of box bool hit = GJKLocalRayCast(capsule, box, zero, zeroV, dir, toi, normal, closestA, lss.radius + inflation, isMtd); if(hit) { sweepHit.flags = PxHitFlag::eDISTANCE | PxHitFlag::eNORMAL; if(FAllGrtrOrEq(zero, toi)) { //initial overlap if((!PX_IS_SPU) && isMtd) { sweepHit.flags |= PxHitFlag::ePOSITION; const Vec3V worldPointA = boxPos.transform(closestA); const Vec3V destNormal = boxPos.rotate(normal); const FloatV length = toi; const Vec3V destWorldPointA = V3NegScaleSub(destNormal, length, worldPointA); V3StoreU(destWorldPointA, sweepHit.position); V3StoreU(destNormal, sweepHit.normal); FStore(length, &sweepHit.distance); } else { sweepHit.distance = 0.0f; sweepHit.normal = -unitDir; } } else { sweepHit.flags |= PxHitFlag::ePOSITION; const Vec3V worldPointA = boxPos.transform(closestA); const Vec3V destNormal = boxPos.rotate(normal); const FloatV length = FMul(dist, toi); const Vec3V destWorldPointA = V3ScaleAdd(worldDir, length, worldPointA); V3StoreU(destNormal, sweepHit.normal); V3StoreU(destWorldPointA, sweepHit.position); FStore(length, &sweepHit.distance); } return true; } return false; }
bool sweepBox_BoxGeom(GU_BOX_SWEEP_FUNC_PARAMS) { PX_ASSERT(geom.getType() == PxGeometryType::eBOX); const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom); const FloatV zero = FZero(); const Vec3V zeroV = V3Zero(); const Vec3V boxExtents0 = V3LoadU(boxGeom.halfExtents); const Vec3V boxExtents1 = V3LoadU(box.extents); const FloatV worldDist = FLoad(distance); const Vec3V unitDirV = V3LoadU(unitDir); const PxTransform boxWorldPose = box.getTransform(); const PsTransformV boxTrans0 = loadTransformU(pose); const PsTransformV boxTrans1 = loadTransformU(boxWorldPose); const PsMatTransformV aToB(boxTrans1.transformInv(boxTrans0)); BoxV box0(zeroV, boxExtents0); BoxV box1(zeroV, boxExtents1); //transform into b space const Vec3V dir = boxTrans1.rotateInv(V3Scale(unitDirV, worldDist)); const bool isMtd = hintFlags & PxHitFlag::eMTD; FloatV toi; Vec3V closestA, normal;//closestA and normal is in the local space of box bool hit = GJKRelativeRayCast(box0, box1, aToB, zero, zeroV, dir, toi, normal, closestA, inflation, isMtd); if(hit) { sweepHit.flags = PxHitFlag::eDISTANCE | PxHitFlag::eNORMAL; if(FAllGrtrOrEq(zero, toi)) { if((!PX_IS_SPU) && isMtd) { sweepHit.flags |= PxHitFlag::ePOSITION; const FloatV length = toi; const Vec3V destWorldPointA = boxTrans1.transform(closestA); const Vec3V destNormal = V3Normalize(boxTrans1.rotate(normal)); V3StoreU(V3Neg(destNormal), sweepHit.normal); V3StoreU(destWorldPointA, sweepHit.position); FStore(length, &sweepHit.distance); } else { sweepHit.distance = 0.0f; sweepHit.normal = -unitDir; } } else { sweepHit.flags |= PxHitFlag::ePOSITION; const Vec3V destWorldPointA = boxTrans1.transform(closestA); const Vec3V destNormal = V3Normalize(boxTrans1.rotate(normal)); const FloatV length = FMul(worldDist, toi); V3StoreU(V3Neg(destNormal), sweepHit.normal); V3StoreU(destWorldPointA, sweepHit.position); FStore(length, &sweepHit.distance); } return true; } return false; }
bool sweepBox_CapsuleGeom(GU_BOX_SWEEP_FUNC_PARAMS) { using namespace Ps::aos; PX_ASSERT(geom.getType() == PxGeometryType::eCAPSULE); PX_UNUSED(hintFlags); const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom); const FloatV capsuleHalfHeight = FLoad(capsuleGeom.halfHeight); const FloatV capsuleRadius = FLoad(capsuleGeom.radius); const FloatV zero = FZero(); const Vec3V zeroV = V3Zero(); const Vec3V boxExtents = V3LoadU(box.extents); const FloatV worldDist = FLoad(distance); const Vec3V unitDirV = V3LoadU(unitDir); const PxTransform boxWorldPose = box.getTransform(); const PsTransformV capPos = loadTransformU(pose); const PsTransformV boxPos = loadTransformU(boxWorldPose); const PsMatTransformV aToB(boxPos.transformInv(capPos)); BoxV boxV(zeroV, boxExtents); CapsuleV capsuleV(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius); //transform into b space const Vec3V dir = boxPos.rotateInv(V3Scale(unitDirV, worldDist)); const bool isMtd = hintFlags & PxHitFlag::eMTD; FloatV toi; Vec3V closestA, normal;//closestA and normal is in the local space of box bool hit = GJKLocalRayCast(capsuleV, boxV, zero, zeroV, dir, toi, normal, closestA, capsuleGeom.radius+inflation, isMtd); if(hit) { sweepHit.flags = PxHitFlag::eDISTANCE | PxHitFlag::eNORMAL; //initial overlap if(FAllGrtrOrEq(zero, toi)) { if((!PX_IS_SPU) && isMtd) { sweepHit.flags |= PxHitFlag::ePOSITION; //initial overlap is toi < 0 const FloatV length = toi; const Vec3V destWorldPointA = boxPos.transform(closestA); const Vec3V destNormal = boxPos.rotate(normal); V3StoreU(V3Neg(destNormal), sweepHit.normal); V3StoreU(destWorldPointA, sweepHit.position); FStore(length, &sweepHit.distance); } else { sweepHit.distance = 0.0f; sweepHit.normal = -unitDir; } return true; } else { sweepHit.flags |= PxHitFlag::ePOSITION; const Vec3V destWorldPointA = boxPos.transform(closestA); const Vec3V destNormal = boxPos.rotate(normal); const FloatV length = FMul(worldDist, toi); V3StoreU(V3Neg(destNormal), sweepHit.normal); V3StoreU(destWorldPointA, sweepHit.position); FStore(length, &sweepHit.distance); } return true; } return false; }
bool sweepBox_SphereGeom(GU_BOX_SWEEP_FUNC_PARAMS) { PX_ASSERT(geom.getType() == PxGeometryType::eSPHERE); PX_UNUSED(hintFlags); const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom); const FloatV zero = FZero(); const Vec3V zeroV = V3Zero(); const Vec3V boxExtents = V3LoadU(box.extents); const FloatV worldDist = FLoad(distance); const Vec3V unitDirV = V3LoadU(unitDir); const FloatV sphereRadius = FLoad(sphereGeom.radius); /* DE10168 original code: const PxTransform boxWorldPose = box.getTransform(); starting with SDK 1.700 it breaks various PS4 non-debug tests: SqTestSweep and RecoveryModuleTest for some unknown reason, the vector elements end up in the wrong location (in y where it should be in x it seems) while we investigate the real cause, use the equivalent code directly */ const PxTransform boxWorldPose = PxTransform(box.center, PxQuat(box.rot)); const PsTransformV spherePos = loadTransformU(pose); const PsTransformV boxPos = loadTransformU(boxWorldPose); const PsMatTransformV aToB(boxPos.transformInv(spherePos)); BoxV boxV(zeroV, boxExtents); CapsuleV capsuleV(aToB.p, sphereRadius); //transform into b space const Vec3V dir = boxPos.rotateInv(V3Scale(unitDirV, worldDist)); bool isMtd = hintFlags & PxHitFlag::eMTD; FloatV toi; Vec3V closestA, normal;//closestA and normal is in the local space of box bool hit = GJKLocalRayCast(capsuleV, boxV, zero, zeroV, dir, toi, normal, closestA, sphereGeom.radius+inflation, isMtd); if(hit) { sweepHit.flags = PxHitFlag::eDISTANCE | PxHitFlag::eNORMAL; //initial overlap if(FAllGrtrOrEq(zero, toi)) { if((!PX_IS_SPU) && isMtd) { sweepHit.flags |= PxHitFlag::ePOSITION; const Vec3V destWorldPointA = boxPos.transform(closestA); const Vec3V destNormal = V3Neg(boxPos.rotate(normal)); const FloatV length = toi; V3StoreU(destNormal, sweepHit.normal); V3StoreU(destWorldPointA, sweepHit.position); FStore(length, &sweepHit.distance); } else { sweepHit.distance = 0.0f; sweepHit.normal = -unitDir; } } else { sweepHit.flags |= PxHitFlag::ePOSITION; const Vec3V destWorldPointA = boxPos.transform(closestA); const Vec3V destNormal = V3Neg(boxPos.rotate(normal)); const FloatV length = FMul(worldDist, toi); V3StoreU(destNormal, sweepHit.normal); V3StoreU(destWorldPointA, sweepHit.position); FStore(length, &sweepHit.distance); } return true; } return false; }