1237 lines
50 KiB
C++
1237 lines
50 KiB
C++
// Redistribution and use in source and binary forms, with or without
|
|
// modification, are permitted provided that the following conditions
|
|
// are met:
|
|
// * Redistributions of source code must retain the above copyright
|
|
// notice, this list of conditions and the following disclaimer.
|
|
// * Redistributions in binary form must reproduce the above copyright
|
|
// notice, this list of conditions and the following disclaimer in the
|
|
// documentation and/or other materials provided with the distribution.
|
|
// * Neither the name of NVIDIA CORPORATION nor the names of its
|
|
// contributors may be used to endorse or promote products derived
|
|
// from this software without specific prior written permission.
|
|
//
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
|
|
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
|
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
|
|
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
|
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
|
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
|
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
|
|
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
//
|
|
// Copyright (c) 2008-2025 NVIDIA Corporation. All rights reserved.
|
|
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
|
|
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
|
|
|
|
#include "ExtSqQuery.h"
|
|
|
|
using namespace physx;
|
|
using namespace Sq;
|
|
|
|
#include "common/PxProfileZone.h"
|
|
#include "foundation/PxFPU.h"
|
|
#include "GuBounds.h"
|
|
#include "GuIntersectionRayBox.h"
|
|
#include "GuIntersectionRay.h"
|
|
#include "GuBVH.h"
|
|
#include "geometry/PxGeometryQuery.h"
|
|
#include "geometry/PxSphereGeometry.h"
|
|
#include "geometry/PxBoxGeometry.h"
|
|
#include "geometry/PxCapsuleGeometry.h"
|
|
#include "geometry/PxConvexMeshGeometry.h"
|
|
#include "geometry/PxTriangleMeshGeometry.h"
|
|
//#include "geometry/PxBVH.h"
|
|
|
|
#include "PxQueryFiltering.h"
|
|
#include "PxRigidActor.h"
|
|
|
|
using namespace physx;
|
|
using namespace Sq;
|
|
using namespace Gu;
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
PX_IMPLEMENT_OUTPUT_ERROR
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
// PT: this is a customized version of physx::Sq::SceneQueries that supports more than 2 hardcoded pruners.
|
|
// It might not be possible to support the whole PxSceneQuerySystem API with an arbitrary number of pruners.
|
|
|
|
// See #MODIFIED tag for what changed in this file compared to the initial code in SqQuery.cpp
|
|
|
|
static PX_FORCE_INLINE void copy(PxRaycastHit* PX_RESTRICT dest, const PxRaycastHit* PX_RESTRICT src)
|
|
{
|
|
dest->faceIndex = src->faceIndex;
|
|
dest->flags = src->flags;
|
|
dest->position = src->position;
|
|
dest->normal = src->normal;
|
|
dest->distance = src->distance;
|
|
dest->u = src->u;
|
|
dest->v = src->v;
|
|
dest->actor = src->actor;
|
|
dest->shape = src->shape;
|
|
}
|
|
|
|
static PX_FORCE_INLINE void copy(PxSweepHit* PX_RESTRICT dest, const PxSweepHit* PX_RESTRICT src)
|
|
{
|
|
dest->faceIndex = src->faceIndex;
|
|
dest->flags = src->flags;
|
|
dest->position = src->position;
|
|
dest->normal = src->normal;
|
|
dest->distance = src->distance;
|
|
dest->actor = src->actor;
|
|
dest->shape = src->shape;
|
|
}
|
|
|
|
static PX_FORCE_INLINE void copy(PxOverlapHit* PX_RESTRICT dest, const PxOverlapHit* PX_RESTRICT src)
|
|
{
|
|
dest->faceIndex = src->faceIndex;
|
|
dest->actor = src->actor;
|
|
dest->shape = src->shape;
|
|
}
|
|
|
|
// these partial template specializations are used to generalize the query code to be reused for all permutations of
|
|
// hit type=(raycast, overlap, sweep) x query type=(ANY, SINGLE, MULTIPLE)
|
|
template <typename HitType> struct HitTypeSupport { enum { IsRaycast = 0, IsSweep = 0, IsOverlap = 0 }; };
|
|
template <> struct HitTypeSupport<PxRaycastHit>
|
|
{
|
|
enum { IsRaycast = 1, IsSweep = 0, IsOverlap = 0 };
|
|
static PX_FORCE_INLINE PxReal getDistance(const PxQueryHit& hit) { return static_cast<const PxRaycastHit&>(hit).distance; }
|
|
};
|
|
template <> struct HitTypeSupport<PxSweepHit>
|
|
{
|
|
enum { IsRaycast = 0, IsSweep = 1, IsOverlap = 0 };
|
|
static PX_FORCE_INLINE PxReal getDistance(const PxQueryHit& hit) { return static_cast<const PxSweepHit&>(hit).distance; }
|
|
};
|
|
template <> struct HitTypeSupport<PxOverlapHit>
|
|
{
|
|
enum { IsRaycast = 0, IsSweep = 0, IsOverlap = 1 };
|
|
static PX_FORCE_INLINE PxReal getDistance(const PxQueryHit&) { return -1.0f; }
|
|
};
|
|
|
|
#define HITDIST(hit) HitTypeSupport<HitType>::getDistance(hit)
|
|
|
|
template<typename HitType>
|
|
static PxU32 clipHitsToNewMaxDist(HitType* ppuHits, PxU32 count, PxReal newMaxDist)
|
|
{
|
|
PxU32 i=0;
|
|
while(i!=count)
|
|
{
|
|
if(HITDIST(ppuHits[i]) > newMaxDist)
|
|
ppuHits[i] = ppuHits[--count];
|
|
else
|
|
i++;
|
|
}
|
|
return count;
|
|
}
|
|
|
|
namespace physx
|
|
{
|
|
namespace Sq
|
|
{
|
|
struct ExtMultiQueryInput
|
|
{
|
|
const PxVec3* rayOrigin; // only valid for raycasts
|
|
const PxVec3* unitDir; // only valid for raycasts and sweeps
|
|
PxReal maxDistance; // only valid for raycasts and sweeps
|
|
const PxGeometry* geometry; // only valid for overlaps and sweeps
|
|
const PxTransform* pose; // only valid for overlaps and sweeps
|
|
PxReal inflation; // only valid for sweeps
|
|
|
|
// Raycast constructor
|
|
ExtMultiQueryInput(const PxVec3& aRayOrigin, const PxVec3& aUnitDir, PxReal aMaxDist)
|
|
{
|
|
rayOrigin = &aRayOrigin;
|
|
unitDir = &aUnitDir;
|
|
maxDistance = aMaxDist;
|
|
geometry = NULL;
|
|
pose = NULL;
|
|
inflation = 0.0f;
|
|
}
|
|
|
|
// Overlap constructor
|
|
ExtMultiQueryInput(const PxGeometry* aGeometry, const PxTransform* aPose)
|
|
{
|
|
geometry = aGeometry;
|
|
pose = aPose;
|
|
inflation = 0.0f;
|
|
rayOrigin = unitDir = NULL;
|
|
}
|
|
|
|
// Sweep constructor
|
|
ExtMultiQueryInput(
|
|
const PxGeometry* aGeometry, const PxTransform* aPose,
|
|
const PxVec3& aUnitDir, const PxReal aMaxDist, const PxReal aInflation)
|
|
{
|
|
rayOrigin = NULL;
|
|
maxDistance = aMaxDist;
|
|
unitDir = &aUnitDir;
|
|
geometry = aGeometry;
|
|
pose = aPose;
|
|
inflation = aInflation;
|
|
}
|
|
|
|
PX_FORCE_INLINE const PxVec3& getDir() const { PX_ASSERT(unitDir); return *unitDir; }
|
|
PX_FORCE_INLINE const PxVec3& getOrigin() const { PX_ASSERT(rayOrigin); return *rayOrigin; }
|
|
};
|
|
|
|
}
|
|
}
|
|
|
|
// performs a single geometry query for any HitType (PxSweepHit, PxOverlapHit, PxRaycastHit)
|
|
template<typename HitType>
|
|
struct ExtGeomQueryAny
|
|
{
|
|
static PX_FORCE_INLINE PxU32 geomHit(
|
|
const CachedFuncs& funcs, const ExtMultiQueryInput& input, const Gu::ShapeData* sd,
|
|
const PxGeometry& sceneGeom, const PxTransform& pose, PxHitFlags hitFlags,
|
|
PxU32 maxHits, HitType* hits, const PxReal shrunkMaxDistance, const PxBounds3* precomputedBounds,
|
|
PxQueryThreadContext* context)
|
|
{
|
|
using namespace Gu;
|
|
|
|
const PxGeometry& geom0 = *input.geometry;
|
|
const PxTransform& pose0 = *input.pose;
|
|
const PxGeometry& geom1 = sceneGeom;
|
|
const PxTransform& pose1 = pose;
|
|
|
|
// Handle raycasts
|
|
if(HitTypeSupport<HitType>::IsRaycast)
|
|
{
|
|
// the test for mesh AABB is archived in //sw/physx/dev/apokrovsky/graveyard/sqMeshAABBTest.cpp
|
|
// TODO: investigate performance impact (see US12801)
|
|
PX_CHECK_AND_RETURN_VAL(input.getDir().isFinite(), "PxScene::raycast(): rayDir is not valid.", 0);
|
|
PX_CHECK_AND_RETURN_VAL(input.getOrigin().isFinite(), "PxScene::raycast(): rayOrigin is not valid.", 0);
|
|
PX_CHECK_AND_RETURN_VAL(pose1.isValid(), "PxScene::raycast(): pose is not valid.", 0);
|
|
PX_CHECK_AND_RETURN_VAL(shrunkMaxDistance >= 0.0f, "PxScene::raycast(): maxDist is negative.", 0);
|
|
PX_CHECK_AND_RETURN_VAL(PxIsFinite(shrunkMaxDistance), "PxScene::raycast(): maxDist is not valid.", 0);
|
|
PX_CHECK_AND_RETURN_VAL(PxAbs(input.getDir().magnitudeSquared()-1)<1e-4f,
|
|
"PxScene::raycast(): ray direction must be unit vector.", 0);
|
|
|
|
// PT: TODO: investigate perf difference
|
|
const RaycastFunc func = funcs.mCachedRaycastFuncs[geom1.getType()];
|
|
return func(geom1, pose1, input.getOrigin(), input.getDir(), shrunkMaxDistance,
|
|
hitFlags, maxHits, reinterpret_cast<PxGeomRaycastHit*>(hits), sizeof(PxRaycastHit), context);
|
|
}
|
|
// Handle sweeps
|
|
else if(HitTypeSupport<HitType>::IsSweep)
|
|
{
|
|
PX_ASSERT(precomputedBounds != NULL);
|
|
PX_ASSERT(sd != NULL);
|
|
// b0 = query shape bounds
|
|
// b1 = scene shape bounds
|
|
// AP: Here we clip the sweep to bounds with sum of extents. This is needed for GJK stability.
|
|
// because sweep is equivalent to a raycast vs a scene shape with inflated bounds.
|
|
// This also may (or may not) provide an optimization for meshes because top level of rtree has multiple boxes
|
|
// and there is no bounds test for the whole mesh elsewhere
|
|
PxBounds3 b0 = *precomputedBounds, b1;
|
|
// compute the scene geometry bounds
|
|
// PT: TODO: avoid recomputing the bounds here
|
|
Gu::computeBounds(b1, sceneGeom, pose, 0.0f, 1.0f);
|
|
const PxVec3 combExt = (b0.getExtents() + b1.getExtents())*1.01f;
|
|
|
|
PxF32 tnear, tfar;
|
|
if(!intersectRayAABB2(-combExt, combExt, b0.getCenter() - b1.getCenter(), input.getDir(), shrunkMaxDistance, tnear, tfar)) // returns (tnear<tfar)
|
|
if(tnear>tfar) // this second test is needed because shrunkMaxDistance can be 0 for 0 length sweep
|
|
return 0;
|
|
PX_ASSERT(input.getDir().isNormalized());
|
|
// tfar is now the t where the ray exits the AABB. input.getDir() is normalized
|
|
|
|
const PxVec3& unitDir = input.getDir();
|
|
PxSweepHit& sweepHit = reinterpret_cast<PxSweepHit&>(hits[0]);
|
|
|
|
// if we don't start inside the AABB box, offset the start pos, because of precision issues with large maxDist
|
|
const bool offsetPos = (tnear > GU_RAY_SURFACE_OFFSET);
|
|
const PxReal offset = offsetPos ? (tnear - GU_RAY_SURFACE_OFFSET) : 0.0f;
|
|
const PxVec3 offsetVec(offsetPos ? (unitDir*offset) : PxVec3(0.0f));
|
|
// we move the geometry we sweep against, so that we avoid the Gu::Capsule/Box recomputation
|
|
const PxTransform pose1Offset(pose1.p - offsetVec, pose1.q);
|
|
|
|
const PxReal distance = PxMin(tfar, shrunkMaxDistance) - offset;
|
|
const PxReal inflation = input.inflation;
|
|
PX_CHECK_AND_RETURN_VAL(pose0.isValid(), "PxScene::sweep(): pose0 is not valid.", 0);
|
|
PX_CHECK_AND_RETURN_VAL(pose1Offset.isValid(), "PxScene::sweep(): pose1 is not valid.", 0);
|
|
PX_CHECK_AND_RETURN_VAL(unitDir.isFinite(), "PxScene::sweep(): unitDir is not valid.", 0);
|
|
PX_CHECK_AND_RETURN_VAL(PxIsFinite(distance), "PxScene::sweep(): distance is not valid.", 0);
|
|
PX_CHECK_AND_RETURN_VAL((distance >= 0.0f && !(hitFlags & PxHitFlag::eASSUME_NO_INITIAL_OVERLAP)) || distance > 0.0f,
|
|
"PxScene::sweep(): sweep distance must be >=0 or >0 with eASSUME_NO_INITIAL_OVERLAP.", 0);
|
|
|
|
PxU32 retVal = 0;
|
|
const GeomSweepFuncs& sf = funcs.mCachedSweepFuncs;
|
|
switch(geom0.getType())
|
|
{
|
|
case PxGeometryType::eSPHERE:
|
|
{
|
|
const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom0);
|
|
const PxCapsuleGeometry capsuleGeom(sphereGeom.radius, 0.0f);
|
|
const Capsule worldCapsule(pose0.p, pose0.p, sphereGeom.radius); // AP: precompute?
|
|
const bool precise = hitFlags & PxHitFlag::ePRECISE_SWEEP;
|
|
const SweepCapsuleFunc func = precise ? sf.preciseCapsuleMap[geom1.getType()] : sf.capsuleMap[geom1.getType()];
|
|
retVal = PxU32(func(geom1, pose1Offset, capsuleGeom, pose0, worldCapsule, unitDir, distance, sweepHit, hitFlags, inflation, context));
|
|
}
|
|
break;
|
|
|
|
case PxGeometryType::eCAPSULE:
|
|
{
|
|
const bool precise = hitFlags & PxHitFlag::ePRECISE_SWEEP;
|
|
const SweepCapsuleFunc func = precise ? sf.preciseCapsuleMap[geom1.getType()] : sf.capsuleMap[geom1.getType()];
|
|
retVal = PxU32(func(geom1, pose1Offset, static_cast<const PxCapsuleGeometry&>(geom0), pose0, sd->getGuCapsule(), unitDir, distance, sweepHit, hitFlags, inflation, context));
|
|
}
|
|
break;
|
|
|
|
case PxGeometryType::eBOX:
|
|
{
|
|
const bool precise = hitFlags & PxHitFlag::ePRECISE_SWEEP;
|
|
const SweepBoxFunc func = precise ? sf.preciseBoxMap[geom1.getType()] : sf.boxMap[geom1.getType()];
|
|
retVal = PxU32(func(geom1, pose1Offset, static_cast<const PxBoxGeometry&>(geom0), pose0, sd->getGuBox(), unitDir, distance, sweepHit, hitFlags, inflation, context));
|
|
}
|
|
break;
|
|
|
|
case PxGeometryType::eCONVEXMESH:
|
|
{
|
|
const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom0);
|
|
const SweepConvexFunc func = sf.convexMap[geom1.getType()];
|
|
retVal = PxU32(func(geom1, pose1Offset, convexGeom, pose0, unitDir, distance, sweepHit, hitFlags, inflation, context));
|
|
}
|
|
break;
|
|
default:
|
|
outputError<physx::PxErrorCode::eINVALID_PARAMETER>(__LINE__, "PxScene::sweep(): first geometry object parameter must be sphere, capsule, box or convex geometry.");
|
|
break;
|
|
}
|
|
if (retVal)
|
|
{
|
|
// we need to offset the distance back
|
|
sweepHit.distance += offset;
|
|
// we need to offset the hit position back as we moved the geometry we sweep against
|
|
sweepHit.position += offsetVec;
|
|
}
|
|
return retVal;
|
|
}
|
|
// Handle overlaps
|
|
else if(HitTypeSupport<HitType>::IsOverlap)
|
|
{
|
|
const GeomOverlapTable* overlapFuncs = funcs.mCachedOverlapFuncs;
|
|
return PxU32(Gu::overlap(geom0, pose0, geom1, pose1, overlapFuncs, context));
|
|
}
|
|
else
|
|
{
|
|
PX_ALWAYS_ASSERT_MESSAGE("Unexpected template expansion in GeomQueryAny::geomHit");
|
|
return 0;
|
|
}
|
|
}
|
|
};
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
static PX_FORCE_INLINE bool applyFilterEquation(const ExtQueryAdapter& adapter, const PrunerPayload& payload, const PxFilterData& queryFd)
|
|
{
|
|
// if the filterData field is non-zero, and the bitwise-AND value of filterData AND the shape's
|
|
// queryFilterData is zero, the shape is skipped.
|
|
if(queryFd.word0 | queryFd.word1 | queryFd.word2 | queryFd.word3)
|
|
{
|
|
// PT: TODO: revisit this, there's an obvious LHS here otherwise
|
|
// We could maybe make this more flexible and let the user do the filtering
|
|
// const PxFilterData& objFd = adapter.getFilterData(payload);
|
|
PxFilterData objFd;
|
|
adapter.getFilterData(payload, objFd);
|
|
|
|
const PxU32 keep = (queryFd.word0 & objFd.word0) | (queryFd.word1 & objFd.word1) | (queryFd.word2 & objFd.word2) | (queryFd.word3 & objFd.word3);
|
|
if(!keep)
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static PX_FORCE_INLINE bool applyAllPreFiltersSQ(
|
|
const ExtQueryAdapter& adapter, const PrunerPayload& payload, const PxActorShape& as, PxQueryHitType::Enum& shapeHitType, const PxQueryFlags& inFilterFlags,
|
|
const PxQueryFilterData& filterData, PxQueryFilterCallback* filterCall,
|
|
PxHitFlags& queryFlags/*, PxU32 maxNbTouches*/)
|
|
{
|
|
// #MODIFIED
|
|
// PT: we have to do the static / dynamic filtering here now, because we're operating on N pruners
|
|
// and we don't know which one(s) are "static", which ones are "dynamic", and which ones are a mix of both.
|
|
const bool doStatics = filterData.flags & PxQueryFlag::eSTATIC;
|
|
const bool doDynamic = filterData.flags & PxQueryFlag::eDYNAMIC;
|
|
const PxType actorType = as.actor->getConcreteType();
|
|
const bool isStatic = (actorType == PxConcreteType::eRIGID_STATIC);
|
|
if(isStatic && !doStatics)
|
|
return false;
|
|
if(!isStatic && !doDynamic)
|
|
return false;
|
|
//~#MODIFIED
|
|
|
|
if(!(filterData.flags & PxQueryFlag::eBATCH_QUERY_LEGACY_BEHAVIOUR) && !applyFilterEquation(adapter, payload, filterData.data))
|
|
return false;
|
|
|
|
if((inFilterFlags & PxQueryFlag::ePREFILTER) && (filterCall))
|
|
{
|
|
PxHitFlags outQueryFlags = queryFlags;
|
|
|
|
if(filterCall)
|
|
shapeHitType = filterCall->preFilter(filterData.data, as.shape, as.actor, outQueryFlags);
|
|
|
|
// AP: at this point the callback might return eTOUCH but the touch buffer can be empty, the hit will be discarded
|
|
//PX_CHECK_MSG(hitType == PxQueryHitType::eTOUCH ? maxNbTouches > 0 : true,
|
|
// "SceneQuery: preFilter returned eTOUCH but empty touch buffer was provided, hit discarded.");
|
|
|
|
queryFlags = (queryFlags & ~PxHitFlag::eMODIFIABLE_FLAGS) | (outQueryFlags & PxHitFlag::eMODIFIABLE_FLAGS);
|
|
|
|
if(shapeHitType == PxQueryHitType::eNONE)
|
|
return false;
|
|
}
|
|
// test passed, continue to return as;
|
|
return true;
|
|
}
|
|
|
|
static PX_NOINLINE void computeCompoundShapeTransform(PxTransform* PX_RESTRICT transform, const PxTransform* PX_RESTRICT compoundPose, const PxTransform* PX_RESTRICT transforms, PxU32 primIndex)
|
|
{
|
|
// PT:: tag: scalar transform*transform
|
|
*transform = (*compoundPose) * transforms[primIndex];
|
|
}
|
|
|
|
// struct to access protected data members in the public PxHitCallback API
|
|
template<typename HitType>
|
|
struct ExtMultiQueryCallback : public PrunerRaycastCallback, public PrunerOverlapCallback, public CompoundPrunerRaycastCallback, public CompoundPrunerOverlapCallback
|
|
{
|
|
const ExtSceneQueries& mScene;
|
|
const ExtMultiQueryInput& mInput;
|
|
PxHitCallback<HitType>& mHitCall;
|
|
const PxHitFlags mHitFlags;
|
|
const PxQueryFilterData& mFilterData;
|
|
PxQueryFilterCallback* mFilterCall;
|
|
PxReal mShrunkDistance;
|
|
const PxHitFlags mMeshAnyHitFlags;
|
|
bool mReportTouchesAgain;
|
|
bool mFarBlockFound; // this is to prevent repeated searches for far block
|
|
const bool mNoBlock;
|
|
const bool mAnyHit;
|
|
|
|
// The reason we need these bounds is because we need to know combined(inflated shape) bounds to clip the sweep path
|
|
// to be tolerable by GJK precision issues. This test is done for (queryShape vs touchedShapes)
|
|
// So it makes sense to cache the bounds for sweep query shape, otherwise we'd have to recompute them every time
|
|
// Currently only used for sweeps.
|
|
const PxBounds3* mQueryShapeBounds;
|
|
const ShapeData* mShapeData;
|
|
PxTransform mCompoundShapeTransform;
|
|
|
|
ExtMultiQueryCallback(
|
|
const ExtSceneQueries& scene, const ExtMultiQueryInput& input, bool anyHit, PxHitCallback<HitType>& hitCall, PxHitFlags hitFlags,
|
|
const PxQueryFilterData& filterData, PxQueryFilterCallback* filterCall, PxReal shrunkDistance) :
|
|
mScene (scene),
|
|
mInput (input),
|
|
mHitCall (hitCall),
|
|
mHitFlags (hitFlags),
|
|
mFilterData (filterData),
|
|
mFilterCall (filterCall),
|
|
mShrunkDistance (shrunkDistance),
|
|
mMeshAnyHitFlags ((hitFlags.isSet(PxHitFlag::eANY_HIT) || anyHit) ? PxHitFlag::eANY_HIT : PxHitFlag::Enum(0)),
|
|
mReportTouchesAgain (true),
|
|
mFarBlockFound (filterData.flags & PxQueryFlag::eNO_BLOCK),
|
|
mNoBlock (filterData.flags & PxQueryFlag::eNO_BLOCK),
|
|
mAnyHit (anyHit),
|
|
mQueryShapeBounds (NULL),
|
|
mShapeData (NULL)
|
|
{
|
|
}
|
|
|
|
bool processTouchHit(const HitType& hit, PxReal& aDist)
|
|
#if PX_WINDOWS_FAMILY
|
|
PX_RESTRICT
|
|
#endif
|
|
{
|
|
// -------------------------- handle eTOUCH hits ---------------------------------
|
|
// for qType=multiple, store the hit. For other qTypes ignore it.
|
|
// <= is important for initially overlapping sweeps
|
|
#if PX_CHECKED
|
|
if(mHitCall.maxNbTouches == 0 && !mFilterData.flags.isSet(PxQueryFlag::eRESERVED))
|
|
// issue a warning if eTOUCH was returned by the prefilter, we have 0 touch buffer and not a batch query
|
|
// not doing for BQ because the touches buffer can be overflown and thats ok by spec
|
|
// eRESERVED to avoid a warning from nested callback (closest blocking hit recursive search)
|
|
outputError<PxErrorCode::eINVALID_OPERATION>(__LINE__, "User filter returned PxQueryHitType::eTOUCH but the touches buffer was empty. Hit was discarded.");
|
|
#endif
|
|
|
|
if(mHitCall.maxNbTouches && mReportTouchesAgain && HITDIST(hit) <= mShrunkDistance)
|
|
{
|
|
// Buffer full: need to find the closest blocking hit, clip touch hits and flush the buffer
|
|
if(mHitCall.nbTouches == mHitCall.maxNbTouches)
|
|
{
|
|
// issue a second nested query just looking for the closest blocking hit
|
|
// could do better perf-wise by saving traversal state (start looking for blocking from this point)
|
|
// but this is not a perf critical case because users can provide a bigger buffer
|
|
// that covers non-degenerate cases
|
|
// far block search doesn't apply to overlaps because overlaps don't work with blocking hits
|
|
if(HitTypeSupport<HitType>::IsOverlap == 0)
|
|
{
|
|
// AP: the use of eRESERVED is a bit tricky, see other comments containing #LABEL1
|
|
PxQueryFilterData fd1 = mFilterData; fd1.flags |= PxQueryFlag::eRESERVED;
|
|
PxHitBuffer<HitType> buf1; // create a temp callback buffer for a single blocking hit
|
|
if(!mFarBlockFound && mHitCall.maxNbTouches > 0 && mScene.ExtSceneQueries::multiQuery<HitType>(mInput, buf1, mHitFlags, NULL, fd1, mFilterCall))
|
|
{
|
|
mHitCall.block = buf1.block;
|
|
mHitCall.hasBlock = true;
|
|
mHitCall.nbTouches =
|
|
clipHitsToNewMaxDist<HitType>(mHitCall.touches, mHitCall.nbTouches, HITDIST(buf1.block));
|
|
mShrunkDistance = HITDIST(buf1.block);
|
|
aDist = mShrunkDistance;
|
|
}
|
|
mFarBlockFound = true;
|
|
}
|
|
if(mHitCall.nbTouches == mHitCall.maxNbTouches)
|
|
{
|
|
mReportTouchesAgain = mHitCall.processTouches(mHitCall.touches, mHitCall.nbTouches);
|
|
if(!mReportTouchesAgain)
|
|
return false; // optimization - buffer is full
|
|
else
|
|
mHitCall.nbTouches = 0; // reset nbTouches so we can continue accumulating again
|
|
}
|
|
}
|
|
|
|
//if(hitCall.nbTouches < hitCall.maxNbTouches) // can be true if maxNbTouches is 0
|
|
mHitCall.touches[mHitCall.nbTouches++] = hit;
|
|
} // if(hitCall.maxNbTouches && reportTouchesAgain && HITDIST(hit) <= shrunkDistance)
|
|
|
|
return true;
|
|
}
|
|
|
|
template<const bool isCached> // is this call coming as a callback from the pruner or a single item cached callback?
|
|
bool _invoke(PxReal& aDist, PxU32 primIndex, const PrunerPayload* payloads, const PxTransform* transforms, const PxTransform* compoundPose)
|
|
#if PX_WINDOWS_FAMILY
|
|
PX_RESTRICT
|
|
#endif
|
|
{
|
|
PX_ASSERT(payloads);
|
|
const PrunerPayload& payload = payloads[primIndex];
|
|
const ExtQueryAdapter& adapter = static_cast<const ExtQueryAdapter&>(mScene.mSQManager.getAdapter());
|
|
|
|
PxActorShape actorShape;
|
|
adapter.getActorShape(payload, actorShape);
|
|
|
|
const PxQueryFlags filterFlags = mFilterData.flags;
|
|
|
|
// for no filter callback, default to eTOUCH for MULTIPLE, eBLOCK otherwise
|
|
// also always treat as eBLOCK if currently tested shape is cached
|
|
// Using eRESERVED flag as a special condition to default to eTOUCH hits while only looking for a single blocking hit
|
|
// from a nested query (see other comments containing #LABEL1)
|
|
PxQueryHitType::Enum shapeHitType =
|
|
((mHitCall.maxNbTouches || (mFilterData.flags & PxQueryFlag::eRESERVED)) && !isCached)
|
|
? PxQueryHitType::eTOUCH
|
|
: PxQueryHitType::eBLOCK;
|
|
|
|
// apply pre-filter
|
|
PxHitFlags filteredHitFlags = mHitFlags;
|
|
if(!isCached) // don't run filters on single item cache
|
|
{
|
|
if(!applyAllPreFiltersSQ(adapter, payload, actorShape, shapeHitType/*in&out*/, filterFlags, mFilterData, mFilterCall, filteredHitFlags/*, mHitCall.maxNbTouches*/))
|
|
return true; // skip this shape from reporting if prefilter said to do so
|
|
|
|
// if(shapeHitType == PxQueryHitType::eNONE)
|
|
// return true;
|
|
}
|
|
|
|
const PxGeometry& shapeGeom = adapter.getGeometry(payload);
|
|
|
|
PX_ASSERT(transforms);
|
|
const PxTransform* shapeTransform;
|
|
if(!compoundPose)
|
|
{
|
|
shapeTransform = transforms + primIndex;
|
|
}
|
|
else
|
|
{
|
|
computeCompoundShapeTransform(&mCompoundShapeTransform, compoundPose, transforms, primIndex);
|
|
shapeTransform = &mCompoundShapeTransform;
|
|
}
|
|
|
|
const PxU32 tempCount = 1;
|
|
HitType tempBuf[tempCount];
|
|
|
|
// Here we decide whether to use the user provided buffer in place or a local stack buffer
|
|
// see if we have more room left in the callback results buffer than in the parent stack buffer
|
|
// if so get subHits in-place in the hit buffer instead of the parent stack buffer
|
|
// nbTouches is the number of accumulated touch hits so far
|
|
// maxNbTouches is the size of the user buffer
|
|
PxU32 maxSubHits1;
|
|
HitType* subHits1;
|
|
if(mHitCall.nbTouches >= mHitCall.maxNbTouches)
|
|
// if there's no room left in the user buffer, use a stack buffer
|
|
{
|
|
// tried using 64 here - causes check stack code to get generated on xbox, perhaps because of guard page
|
|
// need this buffer in case the input buffer is full but we still want to correctly merge results from later hits
|
|
maxSubHits1 = tempCount;
|
|
subHits1 = reinterpret_cast<HitType*>(tempBuf);
|
|
}
|
|
else
|
|
{
|
|
maxSubHits1 = mHitCall.maxNbTouches - mHitCall.nbTouches; // how much room is left in the user buffer
|
|
subHits1 = mHitCall.touches + mHitCall.nbTouches; // pointer to the first free hit in the user buffer
|
|
}
|
|
|
|
// call the geometry specific intersection template
|
|
const PxU32 nbSubHits = ExtGeomQueryAny<HitType>::geomHit(
|
|
mScene.mCachedFuncs, mInput, mShapeData, shapeGeom,
|
|
*shapeTransform, filteredHitFlags | mMeshAnyHitFlags,
|
|
maxSubHits1, subHits1, mShrunkDistance, mQueryShapeBounds, &mHitCall);
|
|
|
|
// ------------------------- iterate over geometry subhits -----------------------------------
|
|
for (PxU32 iSubHit = 0; iSubHit < nbSubHits; iSubHit++)
|
|
{
|
|
HitType& hit = subHits1[iSubHit];
|
|
hit.actor = actorShape.actor;
|
|
hit.shape = actorShape.shape;
|
|
|
|
// some additional processing only for sweep hits with initial overlap
|
|
if(HitTypeSupport<HitType>::IsSweep && HITDIST(hit) == 0.0f && !(filteredHitFlags & PxHitFlag::eMTD))
|
|
// PT: necessary as some leaf routines are called with reversed params, thus writing +unitDir there.
|
|
// AP: apparently still necessary to also do in Gu because Gu can be used standalone (without SQ)
|
|
reinterpret_cast<PxSweepHit&>(hit).normal = -mInput.getDir();
|
|
|
|
// start out with hitType for this cached shape set to a pre-filtered hit type
|
|
PxQueryHitType::Enum hitType = shapeHitType;
|
|
|
|
// run the post-filter if specified in filterFlags and filterCall is non-NULL
|
|
if(!isCached && mFilterCall && (filterFlags & PxQueryFlag::ePOSTFILTER))
|
|
{
|
|
//if(mFilterCall)
|
|
hitType = mFilterCall->postFilter(mFilterData.data, hit, hit.shape, hit.actor);
|
|
}
|
|
|
|
// early out on any hit if eANY_HIT was specified, regardless of hit type
|
|
if(mAnyHit && hitType != PxQueryHitType::eNONE)
|
|
{
|
|
// block or touch qualifies for qType=ANY type hit => return it as blocking according to spec. Ignore eNONE.
|
|
//mHitCall.block = hit;
|
|
copy(&mHitCall.block, &hit);
|
|
mHitCall.hasBlock = true;
|
|
return false; // found a hit for ANY qType, can early exit now
|
|
}
|
|
|
|
if(mNoBlock && hitType==PxQueryHitType::eBLOCK)
|
|
hitType = PxQueryHitType::eTOUCH;
|
|
|
|
PX_WARN_ONCE_IF(HitTypeSupport<HitType>::IsOverlap && hitType == PxQueryHitType::eBLOCK,
|
|
"eBLOCK returned from user filter for overlap() query. This may cause undesired behavior. "
|
|
"Consider using PxQueryFlag::eNO_BLOCK for overlap queries.");
|
|
|
|
if(hitType == PxQueryHitType::eTOUCH)
|
|
{
|
|
if(!processTouchHit(hit, aDist))
|
|
return false;
|
|
} // if(hitType == PxQueryHitType::eTOUCH)
|
|
else if(hitType == PxQueryHitType::eBLOCK)
|
|
{
|
|
// -------------------------- handle eBLOCK hits ----------------------------------
|
|
// only eBLOCK qualifies as a closest hit candidate => compare against best distance and store
|
|
// <= is needed for eTOUCH hits to be recorded correctly vs same eBLOCK distance for overlaps
|
|
if(HITDIST(hit) <= mShrunkDistance)
|
|
{
|
|
if(HitTypeSupport<HitType>::IsOverlap == 0)
|
|
{
|
|
mShrunkDistance = HITDIST(hit);
|
|
aDist = mShrunkDistance;
|
|
}
|
|
//mHitCall.block = hit;
|
|
copy(&mHitCall.block, &hit);
|
|
mHitCall.hasBlock = true;
|
|
}
|
|
} // if(hitType == eBLOCK)
|
|
else {
|
|
PX_ASSERT(hitType == PxQueryHitType::eNONE);
|
|
}
|
|
} // for iSubHit
|
|
return true;
|
|
}
|
|
|
|
virtual bool invoke(PxReal& aDist, PxU32 primIndex, const PrunerPayload* payloads, const PxTransform* transforms) PX_OVERRIDE PX_FINAL
|
|
{
|
|
return _invoke<false>(aDist, primIndex, payloads, transforms, NULL);
|
|
}
|
|
|
|
virtual bool invoke(PxU32 primIndex, const PrunerPayload* payloads, const PxTransform* transforms) PX_OVERRIDE PX_FINAL
|
|
{
|
|
float unused = 0.0f;
|
|
return _invoke<false>(unused, primIndex, payloads, transforms, NULL);
|
|
}
|
|
|
|
virtual bool invoke(PxReal& aDist, PxU32 primIndex, const PrunerPayload* payloads, const PxTransform* transforms, const PxTransform* compoundPose) PX_OVERRIDE PX_FINAL
|
|
{
|
|
return _invoke<false>(aDist, primIndex, payloads, transforms, compoundPose);
|
|
}
|
|
|
|
virtual bool invoke(PxU32 primIndex, const PrunerPayload* payloads, const PxTransform* transforms, const PxTransform* compoundPose) PX_OVERRIDE PX_FINAL
|
|
{
|
|
float unused = 0.0f;
|
|
return _invoke<false>(unused, primIndex, payloads, transforms, compoundPose);
|
|
}
|
|
|
|
private:
|
|
ExtMultiQueryCallback<HitType>& operator=(const ExtMultiQueryCallback<HitType>&);
|
|
};
|
|
|
|
//========================================================================================================================
|
|
#if PX_SUPPORT_PVD
|
|
template<typename HitType>
|
|
struct ExtCapturePvdOnReturn : public PxHitCallback<HitType>
|
|
{
|
|
// copy the arguments of multiQuery into a struct, this is strictly for PVD recording
|
|
const ExtSceneQueries* mSQ;
|
|
const ExtMultiQueryInput& mInput;
|
|
const PxQueryFilterData& mFilterData;
|
|
PxArray<HitType> mAllHits;
|
|
PxHitCallback<HitType>& mParentCallback;
|
|
|
|
ExtCapturePvdOnReturn(
|
|
const ExtSceneQueries* sq, const ExtMultiQueryInput& input,
|
|
const PxQueryFilterData& filterData,
|
|
PxHitCallback<HitType>& parentCallback) :
|
|
PxHitCallback<HitType> (parentCallback.touches, parentCallback.maxNbTouches),
|
|
mSQ (sq),
|
|
mInput (input),
|
|
mFilterData (filterData),
|
|
mParentCallback (parentCallback)
|
|
{}
|
|
|
|
virtual PxAgain processTouches(const HitType* hits, PxU32 nbHits)
|
|
{
|
|
const PxAgain again = mParentCallback.processTouches(hits, nbHits);
|
|
for(PxU32 i=0; i<nbHits; i++)
|
|
mAllHits.pushBack(hits[i]);
|
|
return again;
|
|
}
|
|
|
|
~ExtCapturePvdOnReturn()
|
|
{
|
|
ExtPVDCapture* pvd = mSQ->mPVD;
|
|
if(!pvd || !pvd->transmitSceneQueries())
|
|
return;
|
|
|
|
if(mParentCallback.nbTouches)
|
|
{
|
|
for(PxU32 i = 0; i < mParentCallback.nbTouches; i++)
|
|
mAllHits.pushBack(mParentCallback.touches[i]);
|
|
}
|
|
|
|
if(mParentCallback.hasBlock)
|
|
mAllHits.pushBack(mParentCallback.block);
|
|
|
|
// PT: TODO: why do we need reinterpret_casts below?
|
|
if(HitTypeSupport<HitType>::IsRaycast)
|
|
pvd->raycast(mInput.getOrigin(), mInput.getDir(), mInput.maxDistance, reinterpret_cast<PxRaycastHit*>(mAllHits.begin()), mAllHits.size(), mFilterData, this->maxNbTouches!=0);
|
|
else if(HitTypeSupport<HitType>::IsOverlap)
|
|
pvd->overlap(*mInput.geometry, *mInput.pose, reinterpret_cast<PxOverlapHit*>(mAllHits.begin()), mAllHits.size(), mFilterData);
|
|
else if(HitTypeSupport<HitType>::IsSweep)
|
|
pvd->sweep (*mInput.geometry, *mInput.pose, mInput.getDir(), mInput.maxDistance, reinterpret_cast<PxSweepHit*>(mAllHits.begin()), mAllHits.size(), mFilterData, this->maxNbTouches!=0);
|
|
}
|
|
|
|
private:
|
|
ExtCapturePvdOnReturn<HitType>& operator=(const ExtCapturePvdOnReturn<HitType>&);
|
|
};
|
|
#endif // PX_SUPPORT_PVD
|
|
|
|
//========================================================================================================================
|
|
template<typename HitType>
|
|
struct ExtIssueCallbacksOnReturn
|
|
{
|
|
PxHitCallback<HitType>& hits;
|
|
bool again; // query was stopped by previous processTouches. This means that nbTouches is still non-zero
|
|
// but we don't need to issue processTouches again
|
|
PX_FORCE_INLINE ExtIssueCallbacksOnReturn(PxHitCallback<HitType>& aHits) : hits(aHits)
|
|
{
|
|
again = true;
|
|
}
|
|
|
|
~ExtIssueCallbacksOnReturn()
|
|
{
|
|
if(again)
|
|
// only issue processTouches if query wasn't stopped
|
|
// this is because nbTouches doesn't get reset to 0 in this case (according to spec)
|
|
// and the touches in touches array were already processed by the callback
|
|
{
|
|
if(hits.hasBlock && hits.nbTouches)
|
|
hits.nbTouches = clipHitsToNewMaxDist<HitType>(hits.touches, hits.nbTouches, HITDIST(hits.block));
|
|
if(hits.nbTouches)
|
|
{
|
|
bool again_ = hits.processTouches(hits.touches, hits.nbTouches);
|
|
if(again_)
|
|
hits.nbTouches = 0;
|
|
}
|
|
}
|
|
hits.finalizeQuery();
|
|
}
|
|
|
|
private:
|
|
ExtIssueCallbacksOnReturn<HitType>& operator=(const ExtIssueCallbacksOnReturn<HitType>&);
|
|
};
|
|
|
|
#undef HITDIST
|
|
|
|
//========================================================================================================================
|
|
|
|
template<typename HitType>
|
|
static bool doQueryVsCached(const PrunerHandle handle, PxU32 prunerIndex, const PrunerCompoundId cachedCompoundId, const ExtPrunerManager& manager, ExtMultiQueryCallback<HitType>& pcb, const ExtMultiQueryInput& input);
|
|
|
|
static PX_FORCE_INLINE PxCompoundPrunerQueryFlags convertFlags(PxQueryFlags inFlags)
|
|
{
|
|
PxCompoundPrunerQueryFlags outFlags(0);
|
|
if(inFlags.isSet(PxQueryFlag::eSTATIC))
|
|
outFlags.raise(PxCompoundPrunerQueryFlag::eSTATIC);
|
|
if(inFlags.isSet(PxQueryFlag::eDYNAMIC))
|
|
outFlags.raise(PxCompoundPrunerQueryFlag::eDYNAMIC);
|
|
return outFlags;
|
|
}
|
|
|
|
// #MODIFIED
|
|
static PX_FORCE_INLINE bool prunerFilter(const ExtQueryAdapter& adapter, PxU32 prunerIndex, const PxQueryThreadContext* context, const PxQueryFilterData& filterData, PxQueryFilterCallback* filterCall)
|
|
{
|
|
// PT: the internal PhysX code can skip an entire pruner by just testing one query flag, since there is a direct
|
|
// mapping between the static/dynamic flags and the static/dynamic pruners. This is not the case here anymore,
|
|
// so instead we call a user-provided callback to validate processing each pruner.
|
|
return adapter.processPruner(prunerIndex, context, filterData, filterCall);
|
|
}
|
|
//~#MODIFIED
|
|
|
|
// PT: the following local callbacks are for the "tree of pruners"
|
|
template<typename HitType>
|
|
struct LocalBaseCallback
|
|
{
|
|
LocalBaseCallback(ExtMultiQueryCallback<HitType>& pcb, const Sq::ExtPrunerManager& manager, const ExtQueryAdapter& adapter, PxHitCallback<HitType>& hits, const PxQueryFilterData& filterData, PxQueryFilterCallback* filterCall) :
|
|
mPCB (pcb),
|
|
mSQManager (manager),
|
|
mAdapter (adapter),
|
|
mHits (hits),
|
|
mFilterData (filterData),
|
|
mFilterCall (filterCall)
|
|
{}
|
|
|
|
ExtMultiQueryCallback<HitType>& mPCB;
|
|
const Sq::ExtPrunerManager& mSQManager;
|
|
const ExtQueryAdapter& mAdapter;
|
|
PxHitCallback<HitType>& mHits;
|
|
const PxQueryFilterData& mFilterData;
|
|
PxQueryFilterCallback* mFilterCall;
|
|
|
|
PX_FORCE_INLINE const Pruner* filtering(PxU32 prunerIndex)
|
|
{
|
|
if(!prunerFilter(mAdapter, prunerIndex, &mHits, mFilterData, mFilterCall))
|
|
return NULL;
|
|
|
|
return mSQManager.getPruner(prunerIndex);
|
|
}
|
|
|
|
PX_NOCOPY(LocalBaseCallback)
|
|
};
|
|
|
|
template<typename HitType>
|
|
struct LocalRaycastCallback : LocalBaseCallback<HitType>, PxBVH::RaycastCallback
|
|
{
|
|
LocalRaycastCallback(const ExtMultiQueryInput& input, ExtMultiQueryCallback<HitType>& pcb, const Sq::ExtPrunerManager& manager, const ExtQueryAdapter& adapter, PxHitCallback<HitType>& hits, const PxQueryFilterData& filterData, PxQueryFilterCallback* filterCall) :
|
|
LocalBaseCallback<HitType>(pcb, manager, adapter, hits, filterData, filterCall), mInput(input) {}
|
|
|
|
virtual bool reportHit(PxU32 boundsIndex, PxReal& distance)
|
|
{
|
|
const Pruner* pruner = LocalBaseCallback<HitType>::filtering(boundsIndex);
|
|
if(!pruner)
|
|
return true;
|
|
return pruner->raycast(mInput.getOrigin(), mInput.getDir(), distance, this->mPCB);
|
|
}
|
|
|
|
const ExtMultiQueryInput& mInput;
|
|
|
|
PX_NOCOPY(LocalRaycastCallback)
|
|
};
|
|
|
|
template<typename HitType>
|
|
struct LocalOverlapCallback : LocalBaseCallback<HitType>, PxBVH::OverlapCallback
|
|
{
|
|
LocalOverlapCallback(const ShapeData& shapeData, ExtMultiQueryCallback<HitType>& pcb, const Sq::ExtPrunerManager& manager, const ExtQueryAdapter& adapter, PxHitCallback<HitType>& hits, const PxQueryFilterData& filterData, PxQueryFilterCallback* filterCall) :
|
|
LocalBaseCallback<HitType>(pcb, manager, adapter, hits, filterData, filterCall), mShapeData(shapeData) {}
|
|
|
|
virtual bool reportHit(PxU32 boundsIndex)
|
|
{
|
|
const Pruner* pruner = LocalBaseCallback<HitType>::filtering(boundsIndex);
|
|
if(!pruner)
|
|
return true;
|
|
return pruner->overlap(mShapeData, this->mPCB);
|
|
}
|
|
|
|
const ShapeData& mShapeData;
|
|
|
|
PX_NOCOPY(LocalOverlapCallback)
|
|
};
|
|
|
|
template<typename HitType>
|
|
struct LocalSweepCallback : LocalBaseCallback<HitType>, PxBVH::RaycastCallback
|
|
{
|
|
LocalSweepCallback(const ShapeData& shapeData, const PxVec3& dir, ExtMultiQueryCallback<HitType>& pcb, const Sq::ExtPrunerManager& manager, const ExtQueryAdapter& adapter, PxHitCallback<HitType>& hits, const PxQueryFilterData& filterData, PxQueryFilterCallback* filterCall) :
|
|
LocalBaseCallback<HitType>(pcb, manager, adapter, hits, filterData, filterCall), mShapeData(shapeData), mDir(dir) {}
|
|
|
|
virtual bool reportHit(PxU32 boundsIndex, PxReal& distance)
|
|
{
|
|
const Pruner* pruner = LocalBaseCallback<HitType>::filtering(boundsIndex);
|
|
if(!pruner)
|
|
return true;
|
|
return pruner->sweep(mShapeData, mDir, distance, this->mPCB);
|
|
}
|
|
|
|
const ShapeData& mShapeData;
|
|
const PxVec3& mDir;
|
|
|
|
PX_NOCOPY(LocalSweepCallback)
|
|
};
|
|
|
|
// PT: TODO: revisit error messages without breaking UTs
|
|
template<typename HitType>
|
|
bool ExtSceneQueries::multiQuery(
|
|
const ExtMultiQueryInput& input, PxHitCallback<HitType>& hits, PxHitFlags hitFlags, const PxQueryCache* cache,
|
|
const PxQueryFilterData& filterData, PxQueryFilterCallback* filterCall) const
|
|
{
|
|
const bool anyHit = (filterData.flags & PxQueryFlag::eANY_HIT) == PxQueryFlag::eANY_HIT;
|
|
|
|
if(HitTypeSupport<HitType>::IsRaycast == 0)
|
|
{
|
|
PX_CHECK_AND_RETURN_VAL(input.pose != NULL, "NpSceneQueries::overlap/sweep pose is NULL.", 0);
|
|
PX_CHECK_AND_RETURN_VAL(input.pose->isValid(), "NpSceneQueries::overlap/sweep pose is not valid.", 0);
|
|
}
|
|
else
|
|
{
|
|
PX_CHECK_AND_RETURN_VAL(input.getOrigin().isFinite(), "NpSceneQueries::raycast pose is not valid.", 0);
|
|
}
|
|
|
|
if(HitTypeSupport<HitType>::IsOverlap == 0)
|
|
{
|
|
PX_CHECK_AND_RETURN_VAL(input.getDir().isFinite(), "NpSceneQueries multiQuery input check: unitDir is not valid.", 0);
|
|
PX_CHECK_AND_RETURN_VAL(input.getDir().isNormalized(), "NpSceneQueries multiQuery input check: direction must be normalized", 0);
|
|
}
|
|
|
|
if(HitTypeSupport<HitType>::IsRaycast)
|
|
{
|
|
PX_CHECK_AND_RETURN_VAL(input.maxDistance > 0.0f, "NpSceneQueries::multiQuery input check: distance cannot be negative or zero", 0);
|
|
}
|
|
|
|
if(HitTypeSupport<HitType>::IsOverlap && !anyHit)
|
|
{
|
|
PX_CHECK_AND_RETURN_VAL(hits.maxNbTouches > 0, "PxScene::overlap() calls without eANY_HIT flag require a touch hit buffer for return results.", 0);
|
|
}
|
|
|
|
if(HitTypeSupport<HitType>::IsSweep)
|
|
{
|
|
PX_CHECK_AND_RETURN_VAL(input.maxDistance >= 0.0f, "NpSceneQueries multiQuery input check: distance cannot be negative", 0);
|
|
PX_CHECK_AND_RETURN_VAL(input.maxDistance != 0.0f || !(hitFlags & PxHitFlag::eASSUME_NO_INITIAL_OVERLAP),
|
|
"NpSceneQueries multiQuery input check: zero-length sweep only valid without the PxHitFlag::eASSUME_NO_INITIAL_OVERLAP flag", 0);
|
|
}
|
|
|
|
PX_CHECK_MSG(!cache || (cache && cache->shape && cache->actor), "Raycast cache specified but shape or actor pointer is NULL!");
|
|
PrunerCompoundId cachedCompoundId = INVALID_COMPOUND_ID;
|
|
// PT: this is similar to the code in the SqRefFinder so we could share that code maybe. But here we later retrieve the payload from the PrunerData,
|
|
// i.e. we basically go back to the same pointers we started from. I suppose it's to make sure they get properly invalidated when an object is deleted etc,
|
|
// but we could still probably find a more efficient way to do that here. Isn't it exactly why we had the Signature class initially?
|
|
//
|
|
// how can this work anyway? if the actor has been deleted the lookup won't work either => doc says it's up to users to manage that....
|
|
const ExtQueryAdapter& adapter = static_cast<const ExtQueryAdapter&>(mSQManager.getAdapter());
|
|
PxU32 prunerIndex = 0xffffffff;
|
|
const PrunerHandle cacheData = cache ? adapter.findPrunerHandle(*cache, cachedCompoundId, prunerIndex) : INVALID_PRUNERHANDLE;
|
|
|
|
// this function is logically const for the SDK user, as flushUpdates() will not have an API-visible effect on this object
|
|
// internally however, flushUpdates() changes the states of the Pruners in mSQManager
|
|
// because here is the only place we need this, const_cast instead of making SQM mutable
|
|
const_cast<ExtSceneQueries*>(this)->mSQManager.flushUpdates();
|
|
|
|
#if PX_SUPPORT_PVD
|
|
ExtCapturePvdOnReturn<HitType> pvdCapture(this, input, filterData, hits);
|
|
#endif
|
|
|
|
ExtIssueCallbacksOnReturn<HitType> cbr(hits); // destructor will execute callbacks on return from this function
|
|
hits.hasBlock = false;
|
|
hits.nbTouches = 0;
|
|
|
|
PxReal shrunkDistance = HitTypeSupport<HitType>::IsOverlap ? PX_MAX_REAL : input.maxDistance; // can be progressively shrunk as we go over the list of shapes
|
|
if(HitTypeSupport<HitType>::IsSweep)
|
|
shrunkDistance = PxMin(shrunkDistance, PX_MAX_SWEEP_DISTANCE);
|
|
|
|
ExtMultiQueryCallback<HitType> pcb(*this, input, anyHit, hits, hitFlags, filterData, filterCall, shrunkDistance);
|
|
|
|
if(cacheData!=INVALID_PRUNERHANDLE && hits.maxNbTouches == 0) // don't use cache for queries that can return touch hits
|
|
{
|
|
if(!doQueryVsCached(cacheData, prunerIndex, cachedCompoundId, mSQManager, pcb, input))
|
|
return hits.hasAnyHits();
|
|
}
|
|
|
|
const PxU32 nbPruners = mSQManager.getNbPruners();
|
|
const CompoundPruner* compoundPruner = mSQManager.getCompoundPruner();
|
|
|
|
const PxCompoundPrunerQueryFlags compoundPrunerQueryFlags = convertFlags(filterData.flags);
|
|
|
|
const BVH* treeOfPruners = mSQManager.getTreeOfPruners();
|
|
|
|
if(HitTypeSupport<HitType>::IsRaycast)
|
|
{
|
|
// #MODIFIED
|
|
bool again = true;
|
|
if(treeOfPruners)
|
|
{
|
|
LocalRaycastCallback<HitType> prunerRaycastCB(input, pcb, mSQManager, adapter, hits, filterData, filterCall);
|
|
again = treeOfPruners->raycast(input.getOrigin(), input.getDir(), pcb.mShrunkDistance, prunerRaycastCB, PxGeometryQueryFlag::Enum(0));
|
|
if(!again)
|
|
{
|
|
cbr.again = again; // update the status to avoid duplicate processTouches()
|
|
return hits.hasAnyHits();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for(PxU32 i=0;i<nbPruners;i++)
|
|
{
|
|
if(prunerFilter(adapter, i, &hits, filterData, filterCall))
|
|
{
|
|
const Pruner* pruner = mSQManager.getPruner(i);
|
|
again = pruner->raycast(input.getOrigin(), input.getDir(), pcb.mShrunkDistance, pcb);
|
|
if(!again)
|
|
{
|
|
cbr.again = again; // update the status to avoid duplicate processTouches()
|
|
return hits.hasAnyHits();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
//~#MODIFIED
|
|
|
|
if(again && compoundPruner)
|
|
again = compoundPruner->raycast(input.getOrigin(), input.getDir(), pcb.mShrunkDistance, pcb, compoundPrunerQueryFlags);
|
|
|
|
cbr.again = again; // update the status to avoid duplicate processTouches()
|
|
return hits.hasAnyHits();
|
|
}
|
|
else if(HitTypeSupport<HitType>::IsOverlap)
|
|
{
|
|
PX_ASSERT(input.geometry);
|
|
|
|
const ShapeData sd(*input.geometry, *input.pose, input.inflation);
|
|
pcb.mShapeData = &sd;
|
|
|
|
// #MODIFIED
|
|
bool again = true;
|
|
if(treeOfPruners)
|
|
{
|
|
LocalOverlapCallback<HitType> prunerOverlapCB(sd, pcb, mSQManager, adapter, hits, filterData, filterCall);
|
|
again = treeOfPruners->overlap(*input.geometry, *input.pose, prunerOverlapCB, PxGeometryQueryFlag::Enum(0));
|
|
if(!again)
|
|
{
|
|
cbr.again = again; // update the status to avoid duplicate processTouches()
|
|
return hits.hasAnyHits();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for(PxU32 i=0;i<nbPruners;i++)
|
|
{
|
|
if(prunerFilter(adapter, i, &hits, filterData, filterCall))
|
|
{
|
|
const Pruner* pruner = mSQManager.getPruner(i);
|
|
again = pruner->overlap(sd, pcb);
|
|
if(!again)
|
|
{
|
|
cbr.again = again; // update the status to avoid duplicate processTouches()
|
|
return hits.hasAnyHits();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
//~#MODIFIED
|
|
|
|
if(again && compoundPruner)
|
|
again = compoundPruner->overlap(sd, pcb, compoundPrunerQueryFlags);
|
|
|
|
cbr.again = again; // update the status to avoid duplicate processTouches()
|
|
return hits.hasAnyHits();
|
|
}
|
|
else
|
|
{
|
|
PX_ASSERT(HitTypeSupport<HitType>::IsSweep);
|
|
PX_ASSERT(input.geometry);
|
|
|
|
const ShapeData sd(*input.geometry, *input.pose, input.inflation);
|
|
pcb.mQueryShapeBounds = &sd.getPrunerInflatedWorldAABB();
|
|
pcb.mShapeData = &sd;
|
|
|
|
// #MODIFIED
|
|
bool again = true;
|
|
if(treeOfPruners)
|
|
{
|
|
LocalSweepCallback<HitType> prunerSweepCB(sd, input.getDir(), pcb, mSQManager, adapter, hits, filterData, filterCall);
|
|
again = treeOfPruners->sweep(*input.geometry, *input.pose, input.getDir(), pcb.mShrunkDistance, prunerSweepCB, PxGeometryQueryFlag::Enum(0));
|
|
if(!again)
|
|
{
|
|
cbr.again = again; // update the status to avoid duplicate processTouches()
|
|
return hits.hasAnyHits();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for(PxU32 i=0;i<nbPruners;i++)
|
|
{
|
|
if(prunerFilter(adapter, i, &hits, filterData, filterCall))
|
|
{
|
|
const Pruner* pruner = mSQManager.getPruner(i);
|
|
again = pruner->sweep(sd, input.getDir(), pcb.mShrunkDistance, pcb);
|
|
if(!again)
|
|
{
|
|
cbr.again = again; // update the status to avoid duplicate processTouches()
|
|
return hits.hasAnyHits();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
//~#MODIFIED
|
|
|
|
if(again && compoundPruner)
|
|
again = compoundPruner->sweep(sd, input.getDir(), pcb.mShrunkDistance, pcb, compoundPrunerQueryFlags);
|
|
|
|
cbr.again = again; // update the status to avoid duplicate processTouches()
|
|
return hits.hasAnyHits();
|
|
}
|
|
}
|
|
|
|
//explicit template instantiation
|
|
template bool ExtSceneQueries::multiQuery<PxRaycastHit>(const ExtMultiQueryInput&, PxHitCallback<PxRaycastHit>&, PxHitFlags, const PxQueryCache*, const PxQueryFilterData&, PxQueryFilterCallback*) const;
|
|
template bool ExtSceneQueries::multiQuery<PxOverlapHit>(const ExtMultiQueryInput&, PxHitCallback<PxOverlapHit>&, PxHitFlags, const PxQueryCache*, const PxQueryFilterData&, PxQueryFilterCallback*) const;
|
|
template bool ExtSceneQueries::multiQuery<PxSweepHit>(const ExtMultiQueryInput&, PxHitCallback<PxSweepHit>&, PxHitFlags, const PxQueryCache*, const PxQueryFilterData&, PxQueryFilterCallback*) const;
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
bool ExtSceneQueries::_raycast(
|
|
const PxVec3& origin, const PxVec3& unitDir, const PxReal distance,
|
|
PxHitCallback<PxRaycastHit>& hits, PxHitFlags hitFlags, const PxQueryFilterData& filterData, PxQueryFilterCallback* filterCall,
|
|
const PxQueryCache* cache, PxGeometryQueryFlags flags) const
|
|
{
|
|
PX_PROFILE_ZONE("SceneQuery.raycast", getContextId());
|
|
PX_SIMD_GUARD_CNDT(flags & PxGeometryQueryFlag::eSIMD_GUARD)
|
|
|
|
ExtMultiQueryInput input(origin, unitDir, distance);
|
|
return multiQuery<PxRaycastHit>(input, hits, hitFlags, cache, filterData, filterCall);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
|
|
bool ExtSceneQueries::_overlap(
|
|
const PxGeometry& geometry, const PxTransform& pose, PxOverlapCallback& hits,
|
|
const PxQueryFilterData& filterData, PxQueryFilterCallback* filterCall,
|
|
const PxQueryCache* cache, PxGeometryQueryFlags flags) const
|
|
{
|
|
PX_PROFILE_ZONE("SceneQuery.overlap", getContextId());
|
|
PX_SIMD_GUARD_CNDT(flags & PxGeometryQueryFlag::eSIMD_GUARD)
|
|
|
|
ExtMultiQueryInput input(&geometry, &pose);
|
|
return multiQuery<PxOverlapHit>(input, hits, PxHitFlags(), cache, filterData, filterCall);
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
bool ExtSceneQueries::_sweep(
|
|
const PxGeometry& geometry, const PxTransform& pose, const PxVec3& unitDir, const PxReal distance,
|
|
PxHitCallback<PxSweepHit>& hits, PxHitFlags hitFlags, const PxQueryFilterData& filterData, PxQueryFilterCallback* filterCall,
|
|
const PxQueryCache* cache, const PxReal inflation, PxGeometryQueryFlags flags) const
|
|
{
|
|
PX_PROFILE_ZONE("SceneQuery.sweep", getContextId());
|
|
PX_SIMD_GUARD_CNDT(flags & PxGeometryQueryFlag::eSIMD_GUARD)
|
|
|
|
#if PX_CHECKED
|
|
if(!PxGeometryQuery::isValid(geometry))
|
|
return outputError<PxErrorCode::eINVALID_PARAMETER>(__LINE__, "Provided geometry is not valid");
|
|
#endif
|
|
|
|
if((hitFlags & PxHitFlag::ePRECISE_SWEEP) && (hitFlags & PxHitFlag::eMTD))
|
|
{
|
|
outputError<PxErrorCode::eINVALID_PARAMETER>(__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))
|
|
{
|
|
outputError<PxErrorCode::eINVALID_PARAMETER>(__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;
|
|
outputError<PxErrorCode::eINVALID_PARAMETER>(__LINE__, " Precise sweep doesn't support inflation, inflation will be overwritten to be zero");
|
|
}
|
|
ExtMultiQueryInput input(&geometry, &pose, unitDir, distance, realInflation);
|
|
return multiQuery<PxSweepHit>(input, hits, hitFlags, cache, filterData, filterCall);
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
template<typename HitType>
|
|
static bool doQueryVsCached(const PrunerHandle handle, PxU32 prunerIndex, const PrunerCompoundId cachedCompoundId, const ExtPrunerManager& manager, ExtMultiQueryCallback<HitType>& pcb, const ExtMultiQueryInput& input)
|
|
{
|
|
// this block is only executed for single shape cache
|
|
const PrunerPayload* payloads;
|
|
const PxTransform* compoundPosePtr;
|
|
|
|
PxTransform* transform;
|
|
PxTransform compoundPose;
|
|
if(cachedCompoundId == INVALID_COMPOUND_ID)
|
|
{
|
|
const Pruner* pruner = manager.getPruner(PruningIndex::Enum(prunerIndex));
|
|
PX_ASSERT(pruner);
|
|
|
|
PrunerPayloadData ppd;
|
|
const PrunerPayload& cachedPayload = pruner->getPayloadData(handle, &ppd);
|
|
|
|
payloads = &cachedPayload;
|
|
compoundPosePtr = NULL;
|
|
transform = ppd.mTransform;
|
|
}
|
|
else
|
|
{
|
|
const CompoundPruner* pruner = manager.getCompoundPruner();
|
|
PX_ASSERT(pruner);
|
|
|
|
PrunerPayloadData ppd;
|
|
const PrunerPayload& cachedPayload = pruner->getPayloadData(handle, cachedCompoundId, &ppd);
|
|
|
|
compoundPose = pruner->getTransform(cachedCompoundId);
|
|
|
|
payloads = &cachedPayload;
|
|
compoundPosePtr = &compoundPose;
|
|
transform = ppd.mTransform;
|
|
}
|
|
PxReal dummyDist;
|
|
|
|
bool againAfterCache;
|
|
if(HitTypeSupport<HitType>::IsSweep)
|
|
{
|
|
// AP: for sweeps we cache the bounds because we need to know them for the test to clip the sweep to bounds
|
|
// otherwise GJK becomes unstable. The bounds can be used multiple times so this is an optimization.
|
|
const ShapeData sd(*input.geometry, *input.pose, input.inflation);
|
|
pcb.mQueryShapeBounds = &sd.getPrunerInflatedWorldAABB();
|
|
pcb.mShapeData = &sd;
|
|
// againAfterCache = pcb.invoke(dummyDist, 0);
|
|
againAfterCache = pcb.template _invoke<true>(dummyDist, 0, payloads, transform, compoundPosePtr);
|
|
pcb.mQueryShapeBounds = NULL;
|
|
pcb.mShapeData = NULL;
|
|
} else
|
|
// againAfterCache = pcb.invoke(dummyDist, 0);
|
|
againAfterCache = pcb.template _invoke<true>(dummyDist, 0, payloads, transform, compoundPosePtr);
|
|
|
|
return againAfterCache;
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
ExtSceneQueries::ExtSceneQueries(ExtPVDCapture* pvd, PxU64 contextID, float inflation, const ExtQueryAdapter& adapter, bool usesTreeOfPruners) :
|
|
mSQManager (contextID, inflation, adapter, usesTreeOfPruners),
|
|
mPVD (pvd)
|
|
{
|
|
}
|
|
|
|
ExtSceneQueries::~ExtSceneQueries()
|
|
{
|
|
}
|
|
|