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