//=================================================================================
// Single closest hit compound sweep
bool PxRigidBodyExt::linearSweepSingle(
PxRigidBody& body, PxScene& scene, const PxVec3& unitDir, const PxReal distance,
PxHitFlags outputFlags, PxSweepHit& closestHit, PxU32& shapeIndex,
const PxQueryFilterData& filterData, PxQueryFilterCallback* filterCall,
const PxQueryCache* cache, const PxReal inflation)
{
shapeIndex = 0xFFFFffff;
PxReal closestDist = distance;
PxU32 nbShapes = body.getNbShapes();
for(PxU32 i=0; i < nbShapes; i++)
{
PxShape* shape = NULL;
body.getShapes(&shape, 1, i);
PX_ASSERT(shape != NULL);
PxTransform pose = PxShapeExt::getGlobalPose(*shape, body);
PxQueryFilterData fd;
fd.flags = filterData.flags;
PxU32 or4 = (filterData.data.word0 | filterData.data.word1 | filterData.data.word2 | filterData.data.word3);
fd.data = or4 ? filterData.data : shape->getSimulationFilterData();
PxGeometryHolder anyGeom = shape->getGeometry();
PxSweepBuffer subHit; // touching hits are not allowed to be returned from the filters
scene.sweep(anyGeom.any(), pose, unitDir, distance, subHit, outputFlags, fd, filterCall, cache, inflation);
if (subHit.hasBlock && subHit.block.distance < closestDist)
{
closestDist = subHit.block.distance;
closestHit = subHit.block;
shapeIndex = i;
}
}
return (shapeIndex != 0xFFFFffff);
}
static void sweepRigidBody(PxRigidBody& body, PxBatchQuery& batchQuery, bool closestObject, const PxVec3& unitDir, const PxReal distance, PxSceneQueryFilterFlags filterFlags,
bool useShapeFilterData, PxFilterData* filterDataList, PxU32 filterDataCount, void* userData, const PxSweepCache* sweepCache)
{
if (body.getNbShapes() == 0)
return;
const char* outOfMemMsg = "PxRigidBodyExt: LinearSweep: Out of memory, call failed.";
bool succeeded = true;
PX_ALLOCA(shapes, PxShape*, body.getNbShapes());
if (!shapes)
{
Ps::getFoundation().error(PxErrorCode::eOUT_OF_MEMORY, __FILE__, __LINE__, outOfMemMsg);
succeeded = false;
}
PxU32 nbShapes = body.getShapes(shapes, body.getNbShapes());
PX_ALLOCA(geoms, const PxGeometry*, nbShapes);
if (!geoms)
{
Ps::getFoundation().error(PxErrorCode::eOUT_OF_MEMORY, __FILE__, __LINE__, outOfMemMsg);
succeeded = false;
}
PX_ALLOCA(poses, PxTransform, nbShapes);
if (!poses)
{
Ps::getFoundation().error(PxErrorCode::eOUT_OF_MEMORY, __FILE__, __LINE__, outOfMemMsg);
succeeded = false;
}
PxFilterData* filterData = NULL;
PX_ALLOCA(filterDataBuffer, PxFilterData, nbShapes);
if (useShapeFilterData)
{
filterData = filterDataBuffer;
if (!filterDataBuffer)
{
Ps::getFoundation().error(PxErrorCode::eOUT_OF_MEMORY, __FILE__, __LINE__, outOfMemMsg);
succeeded = false;
}
}
else if (filterDataList)
{
if (filterDataCount == nbShapes)
{
filterData = filterDataList;
}
else
{
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, "PxRigidBodyExt: LinearSweep: Number of filter data entries does not match number of shapes, call failed.");
succeeded = false;
}
}
if (succeeded)
{
PxU32 geomByteSize = 0;
for(PxU32 i=0; i < nbShapes; i++)
{
poses[i] = PxShapeExt::getGlobalPose(*shapes[i]);
if (useShapeFilterData)
filterData[i] = shapes[i]->getSimulationFilterData();
// Copy the non-supported geometry types too, to make sure the closest geometry index maps to the shapes
switch(shapes[i]->getGeometryType())
{
case PxGeometryType::eSPHERE :
{
geomByteSize += sizeof(PxSphereGeometry);
}
break;
case PxGeometryType::eBOX :
{
geomByteSize += sizeof(PxBoxGeometry);
}
break;
case PxGeometryType::eCAPSULE :
{
geomByteSize += sizeof(PxCapsuleGeometry);
}
break;
case PxGeometryType::eCONVEXMESH :
{
geomByteSize += sizeof(PxConvexMeshGeometry);
}
break;
case PxGeometryType::ePLANE :
{
geomByteSize += sizeof(PxPlaneGeometry);
}
break;
case PxGeometryType::eTRIANGLEMESH :
{
geomByteSize += sizeof(PxTriangleMeshGeometry);
}
break;
case PxGeometryType::eHEIGHTFIELD :
{
geomByteSize += sizeof(PxHeightFieldGeometry);
}
break;
default:
break;
}
}
PX_ALLOCA(geomBuffer, PxU8, geomByteSize);
if (geomBuffer)
{
PxU8* currBufferPos = geomBuffer;
for(PxU32 i=0; i < nbShapes; i++)
{
// Copy the non-supported geometry types too, to make sure the closest geometry index maps to the shapes
switch(shapes[i]->getGeometryType())
{
case PxGeometryType::eSPHERE :
{
PxSphereGeometry* g = reinterpret_cast<PxSphereGeometry*>(currBufferPos);
geoms[i] = g;
shapes[i]->getSphereGeometry(*g);
currBufferPos += sizeof(PxSphereGeometry);
}
break;
case PxGeometryType::eBOX :
{
PxBoxGeometry* g = reinterpret_cast<PxBoxGeometry*>(currBufferPos);
geoms[i] = g;
shapes[i]->getBoxGeometry(*g);
currBufferPos += sizeof(PxBoxGeometry);
}
break;
case PxGeometryType::eCAPSULE :
{
PxCapsuleGeometry* g = reinterpret_cast<PxCapsuleGeometry*>(currBufferPos);
geoms[i] = g;
shapes[i]->getCapsuleGeometry(*g);
currBufferPos += sizeof(PxCapsuleGeometry);
}
break;
case PxGeometryType::eCONVEXMESH :
{
PxConvexMeshGeometry* g = reinterpret_cast<PxConvexMeshGeometry*>(currBufferPos);
geoms[i] = g;
shapes[i]->getConvexMeshGeometry(*g);
currBufferPos += sizeof(PxConvexMeshGeometry);
}
break;
case PxGeometryType::ePLANE :
{
PxPlaneGeometry* g = reinterpret_cast<PxPlaneGeometry*>(currBufferPos);
geoms[i] = g;
shapes[i]->getPlaneGeometry(*g);
currBufferPos += sizeof(PxPlaneGeometry);
}
break;
case PxGeometryType::eTRIANGLEMESH :
{
PxTriangleMeshGeometry* g = reinterpret_cast<PxTriangleMeshGeometry*>(currBufferPos);
geoms[i] = g;
shapes[i]->getTriangleMeshGeometry(*g);
currBufferPos += sizeof(PxTriangleMeshGeometry);
}
break;
case PxGeometryType::eHEIGHTFIELD :
{
PxHeightFieldGeometry* g = reinterpret_cast<PxHeightFieldGeometry*>(currBufferPos);
geoms[i] = g;
shapes[i]->getHeightFieldGeometry(*g);
currBufferPos += sizeof(PxHeightFieldGeometry);
}
break;
default:
break;
}
}
if (closestObject)
batchQuery.linearCompoundGeometrySweepSingle(geoms, poses, filterData, nbShapes, unitDir, distance, filterFlags, PxSceneQueryFlag::eIMPACT|PxSceneQueryFlag::eNORMAL|PxSceneQueryFlag::eDISTANCE|PxSceneQueryFlag::eUV, userData, sweepCache);
else
batchQuery.linearCompoundGeometrySweepMultiple(geoms, poses, filterData, nbShapes, unitDir, distance, filterFlags, PxSceneQueryFlag::eIMPACT|PxSceneQueryFlag::eNORMAL|PxSceneQueryFlag::eDISTANCE|PxSceneQueryFlag::eUV, userData, sweepCache);
}
}
}
static bool computeMassAndInertia(bool multipleMassOrDensity, PxRigidBody& body, const PxReal* densities, const PxReal* masses, PxU32 densityOrMassCount, Ext::InertiaTensorComputer& computer)
{
PX_ASSERT(!densities || !masses);
PX_ASSERT((densities || masses) && (densityOrMassCount > 0));
Ext::InertiaTensorComputer inertiaComp(true);
Ps::InlineArray<PxShape*, 16> shapes("PxShape*"); shapes.resize(body.getNbShapes());
body.getShapes(shapes.begin(), shapes.size());
PxU32 validShapeIndex = 0;
PxReal currentMassOrDensity;
const PxReal* massOrDensityArray;
if (densities)
{
massOrDensityArray = densities;
currentMassOrDensity = densities[0];
}
else
{
massOrDensityArray = masses;
currentMassOrDensity = masses[0];
}
if (!PxIsFinite(currentMassOrDensity))
{
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__,
"computeMassAndInertia: Provided mass or density has no valid value");
return false;
}
for(PxU32 i=0; i < shapes.size(); i++)
{
if (!(shapes[i]->getFlags() & PxShapeFlag::eSIMULATION_SHAPE))
continue;
if (multipleMassOrDensity)
{
if (validShapeIndex < densityOrMassCount)
{
currentMassOrDensity = massOrDensityArray[validShapeIndex];
if (!PxIsFinite(currentMassOrDensity))
{
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__,
"computeMassAndInertia: Provided mass or density has no valid value");
return false;
}
}
else
{
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__,
"computeMassAndInertia: Not enough mass/density values provided for all simulation shapes");
return false;
}
}
Ext::InertiaTensorComputer it(false);
switch(shapes[i]->getGeometryType())
{
case PxGeometryType::eSPHERE :
{
PxSphereGeometry g;
bool ok = shapes[i]->getSphereGeometry(g);
PX_ASSERT(ok);
PX_UNUSED(ok);
PxTransform temp(shapes[i]->getLocalPose());
it.setSphere(g.radius, &temp);
}
break;
case PxGeometryType::eBOX :
{
PxBoxGeometry g;
bool ok = shapes[i]->getBoxGeometry(g);
PX_ASSERT(ok);
PX_UNUSED(ok);
PxTransform temp(shapes[i]->getLocalPose());
it.setBox(g.halfExtents, &temp);
}
break;
case PxGeometryType::eCAPSULE :
{
PxCapsuleGeometry g;
bool ok = shapes[i]->getCapsuleGeometry(g);
PX_ASSERT(ok);
PX_UNUSED(ok);
PxTransform temp(shapes[i]->getLocalPose());
it.setCapsule(0, g.radius, g.halfHeight, &temp);
}
break;
case PxGeometryType::eCONVEXMESH :
{
PxConvexMeshGeometry g;
bool ok = shapes[i]->getConvexMeshGeometry(g);
PX_ASSERT(ok);
PX_UNUSED(ok);
PxConvexMesh& convMesh = *g.convexMesh;
PxReal convMass;
PxMat33 convInertia;
PxVec3 convCoM;
convMesh.getMassInformation(convMass, reinterpret_cast<PxMat33&>(convInertia), convCoM);
//scale the mass:
convMass *= (g.scale.scale.x * g.scale.scale.y * g.scale.scale.z);
convCoM = g.scale.rotation.rotateInv(g.scale.scale.multiply(g.scale.rotation.rotate(convCoM)));
convInertia = Ext::MassProps::scaleInertia(convInertia, g.scale.rotation, g.scale.scale);
it = Ext::InertiaTensorComputer(convInertia, convCoM, convMass);
it.transform(shapes[i]->getLocalPose());
}
break;
default :
{
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__,
"computeMassAndInertia: Dynamic actor with illegal collision shapes");
return false;
}
}
if (densities)
it.scaleDensity(currentMassOrDensity);
else if (multipleMassOrDensity) // mass per shape -> need to scale density per shape
it.scaleDensity(currentMassOrDensity / it.getMass());
inertiaComp.add(it);
validShapeIndex++;
}
if (validShapeIndex && masses && (!multipleMassOrDensity)) // at least one simulation shape and single mass for all shapes -> scale density at the end
{
inertiaComp.scaleDensity(currentMassOrDensity / inertiaComp.getMass());
}
computer = inertiaComp;
return true;
}
//=================================================================================
// Multiple hits compound sweep
// AP: we might be able to improve the return results API but no time for it in 3.3
PxU32 PxRigidBodyExt::linearSweepMultiple(
PxRigidBody& body, PxScene& scene, const PxVec3& unitDir, const PxReal distance, PxHitFlags outputFlags,
PxSweepHit* hitBuffer, PxU32* hitShapeIndices, PxU32 hitBufferSize, PxSweepHit& block, PxI32& blockingHitShapeIndex,
bool& overflow, const PxQueryFilterData& filterData, PxQueryFilterCallback* filterCall,
const PxQueryCache* cache, const PxReal inflation)
{
overflow = false;
blockingHitShapeIndex = -1;
for (PxU32 i = 0; i < hitBufferSize; i++)
hitShapeIndices[i] = 0xFFFFffff;
PxI32 sumNbResults = 0;
PxU32 nbShapes = body.getNbShapes();
PxF32 shrunkMaxDistance = distance;
for(PxU32 i=0; i < nbShapes; i++)
{
PxShape* shape = NULL;
body.getShapes(&shape, 1, i);
PX_ASSERT(shape != NULL);
PxTransform pose = PxShapeExt::getGlobalPose(*shape, body);
PxQueryFilterData fd;
fd.flags = filterData.flags;
PxU32 or4 = (filterData.data.word0 | filterData.data.word1 | filterData.data.word2 | filterData.data.word3);
fd.data = or4 ? filterData.data : shape->getSimulationFilterData();
PxGeometryHolder anyGeom = shape->getGeometry();
PxU32 bufSizeLeft = hitBufferSize-sumNbResults;
PxSweepHit extraHit;
PxSweepBuffer buffer(bufSizeLeft == 0 ? &extraHit : hitBuffer+sumNbResults, bufSizeLeft == 0 ? 1 : hitBufferSize-sumNbResults);
scene.sweep(anyGeom.any(), pose, unitDir, shrunkMaxDistance, buffer, outputFlags, fd, filterCall, cache, inflation);
// Check and abort on overflow. Assume overflow if result count is bufSize.
PxU32 nbNewResults = buffer.getNbTouches();
overflow |= (nbNewResults >= bufSizeLeft);
if (bufSizeLeft == 0) // this is for when we used the extraHit buffer
nbNewResults = 0;
// set hitShapeIndices for each new non-blocking hit
for (PxU32 j = 0; j < nbNewResults; j++)
if (sumNbResults + PxU32(j) < hitBufferSize)
hitShapeIndices[sumNbResults+j] = i;
if (buffer.hasBlock) // there's a blocking hit in the most recent sweepMultiple results
{
// overwrite the return result blocking hit with the new blocking hit if under
if (blockingHitShapeIndex == -1 || buffer.block.distance < block.distance)
{
blockingHitShapeIndex = (PxI32)i;
block = buffer.block;
}
// Remove all the old touching hits below the new maxDist
// sumNbResults is not updated yet at this point
// and represents the count accumulated so far excluding the very last query
PxI32 nbNewResultsSigned = PxI32(nbNewResults); // need a signed version, see nbNewResultsSigned-- below
for (PxI32 j = sumNbResults-1; j >= 0; j--) // iterate over "old" hits (up to shapeIndex-1)
if (buffer.block.distance < hitBuffer[j].distance)
{
// overwrite with last "new" hit
PxI32 sourceIndex = PxI32(sumNbResults)+nbNewResultsSigned-1; PX_ASSERT(sourceIndex >= j);
hitBuffer[j] = hitBuffer[sourceIndex];
hitShapeIndices[j] = hitShapeIndices[sourceIndex];
nbNewResultsSigned--; // can get negative, that means we are shifting the last results array
}
sumNbResults += nbNewResultsSigned;
} else // if there was no new blocking hit we don't need to do anything special, simply append all results to touch array
sumNbResults += nbNewResults;
PX_ASSERT(sumNbResults >= 0 && sumNbResults <= PxI32(hitBufferSize));
}
return (PxU32)sumNbResults;
}