PxU32 physx::PxFindFaceIndex(const PxConvexMeshGeometry& convexGeom, const PxTransform& pose,
const PxVec3& impactPos, const PxVec3& unitDir)
{
PX_ASSERT(unitDir.isFinite());
PX_ASSERT(unitDir.isNormalized());
PX_ASSERT(impactPos.isFinite());
PX_ASSERT(pose.isFinite());
const PxVec3 impact = impactPos - unitDir * gEpsilon;
const PxVec3 localPoint = pose.transformInv(impact);
const PxVec3 localDir = pose.rotateInv(unitDir);
// Create shape to vertex scale transformation matrix
const PxMeshScale& meshScale = convexGeom.scale;
const PxMat33 rot(meshScale.rotation);
PxMat33 shape2VertexSkew = rot.getTranspose();
const PxMat33 diagonal = PxMat33::createDiagonal(PxVec3(1.0f / meshScale.scale.x, 1.0f / meshScale.scale.y, 1.0f / meshScale.scale.z));
shape2VertexSkew = shape2VertexSkew * diagonal;
shape2VertexSkew = shape2VertexSkew * rot;
const PxU32 nbPolys = convexGeom.convexMesh->getNbPolygons();
PxU32 minIndex = 0;
PxReal minD = PX_MAX_REAL;
for (PxU32 j = 0; j < nbPolys; j++)
{
PxHullPolygon hullPolygon;
convexGeom.convexMesh->getPolygonData(j, hullPolygon);
// transform hull plane into shape space
PxPlane plane;
const PxVec3 tmp = shape2VertexSkew.transformTranspose(PxVec3(hullPolygon.mPlane[0],hullPolygon.mPlane[1],hullPolygon.mPlane[2]));
const PxReal denom = 1.0f / tmp.magnitude();
plane.n = tmp * denom;
plane.d = hullPolygon.mPlane[3] * denom;
PxReal d = plane.distance(localPoint);
if (d < 0.0f)
continue;
const PxReal tweak = plane.n.dot(localDir) * gEpsilon;
d += tweak;
if (d < minD)
{
minIndex = j;
minD = d;
}
}
return minIndex;
}
//returns the maximal vertex in shape space
// PT: this function should be removed. We already have 2 different project hull functions in PxcShapeConvex & GuGJKObjectSupport, this one looks like a weird mix of both!
static PxVec3 projectHull_( const ConvexHullData& hull,
float& minimum,
float& maximum,
const PxVec3& localDir, // expected to be normalized
const PxMat33& vert2ShapeSkew)
{
PX_ASSERT(localDir.isNormalized());
//use property that x|My == Mx|y for symmetric M to avoid having to transform vertices.
const PxVec3 vertexSpaceDir = vert2ShapeSkew * localDir;
const PxVec3* Verts = hull.getHullVertices();
const PxVec3* bestVert = NULL;
if(!hull.mBigConvexRawData) // Brute-force, local space. Experiments show break-even point is around 32 verts.
{
PxU32 NbVerts = hull.mNbHullVertices;
float min_ = PX_MAX_F32;
float max_ = -PX_MAX_F32;
while(NbVerts--)
{
const float dp = (*Verts).dot(vertexSpaceDir);
min_ = physx::intrinsics::selectMin(min_, dp);
if(dp > max_) { max_ = dp; bestVert = Verts; }
Verts++;
}
minimum = min_;
maximum = max_;
PX_ASSERT(bestVert != NULL);
return vert2ShapeSkew * *bestVert;
}
else //*/if(1) // This version is better for objects with a lot of vertices
{
const PxU32 Offset = ComputeCubemapNearestOffset(vertexSpaceDir, hull.mBigConvexRawData->mSubdiv);
PxU32 MinID = hull.mBigConvexRawData->mSamples[Offset];
PxU32 MaxID = hull.mBigConvexRawData->getSamples2()[Offset];
localSearch(MinID, -vertexSpaceDir, Verts, hull.mBigConvexRawData);
localSearch(MaxID, vertexSpaceDir, Verts, hull.mBigConvexRawData);
minimum = (Verts[MinID].dot(vertexSpaceDir));
maximum = (Verts[MaxID].dot(vertexSpaceDir));
PX_ASSERT(maximum >= minimum);
return vert2ShapeSkew * Verts[MaxID];
}
}
void NpBatchQuery::sweep(
const PxGeometry& geometry, const PxTransform& pose, const PxVec3& unitDir, const PxReal distance, PxU16 maxTouchHits,
PxHitFlags hitFlags, const PxQueryFilterData& fd, void* userData, const PxQueryCache* cache, const PxReal inflation)
{
PX_CHECK_AND_RETURN(pose.isValid(), "Batch sweep input check: pose is not valid.");
PX_CHECK_AND_RETURN(unitDir.isFinite(), "Batch sweep input check: unitDir is not valid.");
PX_CHECK_AND_RETURN(unitDir.isNormalized(), "Batch sweep input check: direction must be normalized");
PX_CHECK_AND_RETURN(distance >= 0.0f, "Batch sweep input check: distance cannot be negative");
PX_CHECK_AND_RETURN(distance != 0.0f || !(hitFlags & PxHitFlag::eASSUME_NO_INITIAL_OVERLAP),
"Batch sweep input check: zero-length sweep only valid without the PxHitFlag::eASSUME_NO_INITIAL_OVERLAP flag");
if (mNbSweeps >= mDesc.queryMemory.getMaxSweepsPerExecute())
{
PX_CHECK_AND_RETURN(mNbSweeps < mDesc.queryMemory.getMaxSweepsPerExecute(),
"PxBatchQuery: number of sweep() calls exceeds PxBatchQueryMemory::sweepResultBufferSize, query discarded");
return;
}
CHECK_RUNNING("PxBatchQuery::sweep: This batch is still executing, skipping query.")
mNbSweeps++;
writeBatchHeader(BatchStreamHeader(hitFlags, cache, fd, userData, maxTouchHits, QTypeROS::eSWEEP));
//set the MTD flag
mHasMtdSweep |= !!(hitFlags & PxHitFlag::eMTD);
if((hitFlags & PxHitFlag::ePRECISE_SWEEP) && (hitFlags & PxHitFlag::eMTD))
{
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, " Precise sweep doesn't support MTD. Perform MTD with default sweep");
hitFlags &= ~PxHitFlag::ePRECISE_SWEEP;
}
if((hitFlags & PxHitFlag::eASSUME_NO_INITIAL_OVERLAP) && (hitFlags & PxHitFlag::eMTD))
{
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, " eMTD cannot be used in conjunction with eASSUME_NO_INITIAL_OVERLAP. eASSUME_NO_INITIAL_OVERLAP will be ignored");
hitFlags &= ~PxHitFlag::eASSUME_NO_INITIAL_OVERLAP;
}
PxReal realInflation = inflation;
if((hitFlags & PxHitFlag::ePRECISE_SWEEP)&& inflation > 0.f)
{
realInflation = 0.f;
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, " Precise sweep doesn't support inflation, inflation will be overwritten to be zero");
}
writeQueryInput(mStream, MultiQueryInput(&geometry, &pose, unitDir, distance, realInflation));
Ps::atomicExchange(&mBatchQueryIsRunning, 0);
}
void NpSpatialIndex::raycast(const PxVec3& origin,
const PxVec3& unitDir,
PxReal maxDist,
PxSpatialLocationCallback& callback) const
{
PX_SIMD_GUARD;
PX_CHECK_AND_RETURN(origin.isFinite(), "PxSpatialIndex::raycast: origin is not valid.");
PX_CHECK_AND_RETURN(unitDir.isFinite() && unitDir.isNormalized(), "PxSpatialIndex::raycast: unitDir is not valid.");
PX_CHECK_AND_RETURN(maxDist > 0.0f, "PxSpatialIndex::raycast: distance must be positive");
flushUpdates();
LocationCallback cb(callback);
mPruner->raycast(origin, unitDir, maxDist, cb);
}
void NpSpatialIndex::sweep(const PxBounds3& aabb,
const PxVec3& unitDir,
PxReal maxDist,
PxSpatialLocationCallback& callback) const
{
PX_SIMD_GUARD;
PX_CHECK_AND_RETURN(aabb.isValid(), "PxSpatialIndex::sweep: aabb is not valid.");
PX_CHECK_AND_RETURN(unitDir.isFinite() && unitDir.isNormalized(), "PxSpatialIndex::sweep: unitDir is not valid.");
PX_CHECK_AND_RETURN(maxDist > 0.0f, "PxSpatialIndex::sweep: distance must be positive");
flushUpdates();
LocationCallback cb(callback);
PxBoxGeometry boxGeom(aabb.getExtents());
PxTransform xf(aabb.getCenter());
Sq::ShapeData shapeData(boxGeom, xf, 0.0f); // temporary rvalue not compatible with PX_NOCOPY
mPruner->sweep(shapeData, unitDir, maxDist, cb);
}
void Controller::setUpDirectionInternal(const PxVec3& up)
{
PX_CHECK_MSG(up.isNormalized(), "CCT: up direction must be normalized");
if(mUserParams.mUpDirection==up)
return;
// const PxQuat q = Ps::computeQuatFromNormal(up);
const PxQuat q = Ps::rotationArc(PxVec3(1.0f, 0.0f, 0.0f), up);
mUserParams.mQuatFromUp = q;
mUserParams.mUpDirection = up;
// Update kinematic actor
if(mKineActor)
{
PxTransform pose = mKineActor->getGlobalPose();
pose.q = q;
mKineActor->setGlobalPose(pose);
}
}
void NpBatchQuery::raycast(
const PxVec3& origin, const PxVec3& unitDir, PxReal distance, PxU16 maxTouchHits,
PxHitFlags hitFlags, const PxQueryFilterData& fd, void* userData, const PxQueryCache* cache)
{
PX_CHECK_AND_RETURN(distance>0, "PxBatchQuery::raycast: The maximum distance must be greater than zero!");
PX_CHECK_AND_RETURN(unitDir.isNormalized(), "PxBatchQuery::raycast: Direction must be normalized");
PX_CHECK_AND_RETURN(origin.isFinite(), "PxBatchQuery::raycast: origin is not valid");
if (mNbRaycasts >= mDesc.queryMemory.getMaxRaycastsPerExecute())
{
PX_CHECK_AND_RETURN(mNbRaycasts < mDesc.queryMemory.getMaxRaycastsPerExecute(),
"PxBatchQuery: number of raycast() calls exceeds PxBatchQueryMemory::raycastResultBufferSize, query discarded");
return;
}
CHECK_RUNNING("PxBatchQuery::raycast: This batch is still executing, skipping query.");
mNbRaycasts++;
writeBatchHeader(BatchStreamHeader(hitFlags, cache, fd, userData, maxTouchHits, QTypeROS::eRAYCAST));
writeQueryInput(mStream, MultiQueryInput(origin, unitDir, distance));
Ps::atomicExchange(&mBatchQueryIsRunning, 0);
}
