xuanchi/XCEngine
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
22a2b982ef9e369fe2a90ad3bd154f5e2b5e410c
XCEngine/engine/third_party/physx/source/geomutils/src/GuBVH.cpp

907 lines
26 KiB
C++
Raw Blame History

// 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 "foundation/PxFoundation.h"
#include "foundation/PxFPU.h"
#include "foundation/PxPlane.h"
#include "geometry/PxGeometryInternal.h"
#include "GuBVH.h"
#include "GuAABBTreeQuery.h"
#include "GuAABBTreeNode.h"
#include "GuAABBTreeBuildStats.h"
#include "GuMeshFactory.h"
#include "GuQuery.h"
#include "CmSerialize.h"
using namespace physx;
using namespace Gu;
using namespace Cm;
///////////////////////////////////////////////////////////////////////////////
// PT: these two functions moved from cooking
bool BVHData::build(PxU32 nbBounds, const void* boundsData, PxU32 boundsStride, float enlargement, PxU32 nbPrimsPerLeaf, BVHBuildStrategy bs)
{
if(!nbBounds || !boundsData || boundsStride<sizeof(PxBounds3) || enlargement<0.0f || nbPrimsPerLeaf>=16)
return false;
mBounds.init(nbBounds);
if(nbBounds)
{
const PxU8* sB = reinterpret_cast<const PxU8*>(boundsData);
for(PxU32 i=0; i<nbBounds-1; i++)
{
inflateBounds<true>(mBounds.getBounds()[i], *reinterpret_cast<const PxBounds3*>(sB), enlargement);
sB += boundsStride;
}
inflateBounds<false>(mBounds.getBounds()[nbBounds-1], *reinterpret_cast<const PxBounds3*>(sB), enlargement);
}
mNbIndices = nbBounds;
// build the BVH
BuildStats stats;
NodeAllocator nodeAllocator;
mIndices = buildAABBTree(AABBTreeBuildParams(nbPrimsPerLeaf, nbBounds, &mBounds, bs), nodeAllocator, stats);
if(!mIndices)
return false;
// store the computed hierarchy
mNbNodes = stats.getCount();
mNodes = PX_ALLOCATE(BVHNode, mNbNodes, "AABB tree nodes");
PX_ASSERT(mNbNodes==nodeAllocator.mTotalNbNodes);
// store the results into BVHNode list
if(nbPrimsPerLeaf==1)
{
// PT: with 1 prim/leaf we don't need the remap table anymore, we can just store the prim index in each tree node directly.
flattenTree(nodeAllocator, mNodes, mIndices);
PX_FREE(mIndices);
}
else
flattenTree(nodeAllocator, mNodes);
return true;
}
// A.B. move to load code
#define PX_BVH_STRUCTURE_VERSION 1
bool BVHData::save(PxOutputStream& stream, bool endian) const
{
// write header
if(!writeHeader('B', 'V', 'H', 'S', PX_BVH_STRUCTURE_VERSION, endian, stream))
return false;
// write mData members
writeDword(mNbIndices, endian, stream);
writeDword(mNbNodes, endian, stream);
// write indices and bounds
for(PxU32 i=0; i<mNbIndices; i++)
writeDword(mIndices[i], endian, stream);
const PxBounds3* bounds = mBounds.getBounds();
for(PxU32 i=0; i<mNbIndices; i++)
{
writeFloatBuffer(&bounds[i].minimum.x, 3, endian, stream);
writeFloatBuffer(&bounds[i].maximum.x, 3, endian, stream);
}
// write nodes
for(PxU32 i=0; i<mNbNodes; i++)
{
writeDword(mNodes[i].mData, endian, stream);
writeFloatBuffer(&mNodes[i].mBV.minimum.x, 3, endian, stream);
writeFloatBuffer(&mNodes[i].mBV.maximum.x, 3, endian, stream);
}
return true;
}
///////////////////////////////////////////////////////////////////////////////
// PT: temporary for Kit
BVH::BVH(const PxBVHInternalData& data) :
PxBVH (PxType(PxConcreteType::eBVH), PxBaseFlags(0)),
mMeshFactory (NULL)
{
mData.mNbIndices = data.mNbIndices;
mData.mNbNodes = data.mNbNodes;
mData.mIndices = data.mIndices;
mData.mNodes = reinterpret_cast<BVHNode*>(data.mNodes);
mData.mBounds.setBounds(reinterpret_cast<PxBounds3*>(data.mBounds));
}
bool BVH::getInternalData(PxBVHInternalData& data, bool takeOwnership) const
{
data.mNbIndices = mData.mNbIndices;
data.mNbNodes = mData.mNbNodes;
data.mNodeSize = sizeof(BVHNode);
data.mNodes = mData.mNodes;
data.mIndices = mData.mIndices;
data.mBounds = const_cast<PxBounds3*>(mData.mBounds.getBounds());
if(takeOwnership)
const_cast<BVH*>(this)->mData.mBounds.takeOwnership();
return true;
}
bool physx::PxGetBVHInternalData(PxBVHInternalData& data, const PxBVH& bvh, bool takeOwnership)
{
return static_cast<const BVH&>(bvh).getInternalData(data, takeOwnership);
}
//~ PT: temporary for Kit
BVH::BVH(MeshFactory* factory) :
PxBVH (PxType(PxConcreteType::eBVH), PxBaseFlag::eOWNS_MEMORY | PxBaseFlag::eIS_RELEASABLE),
mMeshFactory (factory)
{
}
BVH::BVH(MeshFactory* factory, BVHData& bvhData) :
PxBVH (PxType(PxConcreteType::eBVH), PxBaseFlag::eOWNS_MEMORY | PxBaseFlag::eIS_RELEASABLE),
mMeshFactory (factory),
mData (bvhData)
{
}
BVH::~BVH()
{
}
bool BVH::init(PxU32 nbPrims, AABBTreeBounds* bounds, const void* boundsData, PxU32 stride, BVHBuildStrategy bs, PxU32 nbPrimsPerLeaf, float enlargement)
{
if(!nbPrims)
return false;
if(bounds)
{
mData.mBounds.moveFrom(*bounds);
}
else
{
mData.mBounds.init(nbPrims);
PxBounds3* dst = mData.mBounds.getBounds();
if(stride==sizeof(PxBounds3))
{
PxMemCopy(dst, boundsData, sizeof(PxBounds3)*nbPrims);
}
else
{
if(nbPrims)
{
const PxU8* sB = reinterpret_cast<const PxU8*>(boundsData);
for(PxU32 i=0; i<nbPrims-1; i++)
{
inflateBounds<true>(mData.mBounds.getBounds()[i], *reinterpret_cast<const PxBounds3*>(sB), enlargement);
sB += stride;
}
inflateBounds<false>(mData.mBounds.getBounds()[nbPrims-1], *reinterpret_cast<const PxBounds3*>(sB), enlargement);
}
}
}
mData.mNbIndices = nbPrims;
// build the BVH
BuildStats stats;
NodeAllocator nodeAllocator;
mData.mIndices = buildAABBTree(AABBTreeBuildParams(nbPrimsPerLeaf, nbPrims, &mData.mBounds, bs), nodeAllocator, stats);
if(!mData.mIndices)
return false;
// store the computed hierarchy
mData.mNbNodes = stats.getCount();
mData.mNodes = PX_ALLOCATE(BVHNode, mData.mNbNodes, "AABB tree nodes");
PX_ASSERT(mData.mNbNodes==nodeAllocator.mTotalNbNodes);
// store the results into BVHNode list
if(nbPrimsPerLeaf==1)
{
// PT: with 1 prim/leaf we don't need the remap table anymore, we can just store the prim index in each tree node directly.
flattenTree(nodeAllocator, mData.mNodes, mData.mIndices);
PX_FREE(mData.mIndices);
}
else
flattenTree(nodeAllocator, mData.mNodes);
return true;
}
bool BVH::load(PxInputStream& stream)
{
// Import header
PxU32 version;
bool mismatch;
if(!readHeader('B', 'V', 'H', 'S', version, mismatch, stream))
return false;
// read numVolumes, numNodes together
//ReadDwordBuffer(&mData.mNbIndices, 2, mismatch, stream);
mData.mNbIndices = readDword(mismatch, stream);
mData.mNbNodes = readDword(mismatch, stream);
// read indices
mData.mIndices = PX_ALLOCATE(PxU32, mData.mNbIndices, "BVH indices");
ReadDwordBuffer(mData.mIndices, mData.mNbIndices, mismatch, stream);
// read bounds
mData.mBounds.init(mData.mNbIndices);
readFloatBuffer(&mData.mBounds.getBounds()->minimum.x, mData.mNbIndices*(3 + 3), mismatch, stream);
// read nodes
mData.mNodes = PX_ALLOCATE(BVHNode, mData.mNbNodes, "BVH nodes");
for(PxU32 i = 0; i < mData.mNbNodes; i++)
{
ReadDwordBuffer(&mData.mNodes[i].mData, 1, mismatch, stream);
readFloatBuffer(&mData.mNodes[i].mBV.minimum.x, 3 + 3, mismatch, stream);
}
return true;
}
void BVH::release()
{
decRefCount();
}
void BVH::onRefCountZero()
{
::onRefCountZero(this, mMeshFactory, false, "PxBVH::release: double deletion detected!");
}
namespace
{
struct BVHTree
{
PX_FORCE_INLINE BVHTree(const BVHData& data) : mRootNode(data.mNodes), mIndices(data.mIndices) {}
const BVHNode* getNodes() const { return mRootNode; }
const PxU32* getIndices() const { return mIndices; }
const BVHNode* mRootNode;
const PxU32* mIndices;
};
}
namespace
{
struct RaycastAdapter
{
RaycastAdapter(PxBVH::RaycastCallback& cb) : mCallback(cb), mAbort(false) {}
PX_FORCE_INLINE bool invoke(PxReal& distance, PxU32 index)
{
if(mAbort || !mCallback.reportHit(index, distance))
{
mAbort = true;
return false;
}
return true;
}
PxBVH::RaycastCallback& mCallback;
bool mAbort;
PX_NOCOPY(RaycastAdapter)
};
}
bool BVH::raycast(const PxVec3& origin, const PxVec3& unitDir, float distance, RaycastCallback& cb, PxGeometryQueryFlags flags) const
{
PX_SIMD_GUARD_CNDT(flags & PxGeometryQueryFlag::eSIMD_GUARD)
RaycastAdapter ra(cb);
if(mData.mIndices)
return AABBTreeRaycast<false, true, BVHTree, BVHNode, RaycastAdapter>()(mData.mBounds, BVHTree(mData), origin, unitDir, distance, PxVec3(0.0f), ra);
else
return AABBTreeRaycast<false, false, BVHTree, BVHNode, RaycastAdapter>()(mData.mBounds, BVHTree(mData), origin, unitDir, distance, PxVec3(0.0f), ra);
}
namespace
{
struct OverlapAdapter
{
OverlapAdapter(PxBVH::OverlapCallback& cb) : mCallback(cb), mAbort(false) {}
PX_FORCE_INLINE bool invoke(PxU32 index)
{
if(mAbort || !mCallback.reportHit(index))
{
mAbort = true;
return false;
}
return true;
}
PxBVH::OverlapCallback& mCallback;
bool mAbort;
PX_NOCOPY(OverlapAdapter)
};
}
bool BVH::overlap(const ShapeData& queryVolume, OverlapCallback& cb, PxGeometryQueryFlags flags) const
{
PX_SIMD_GUARD_CNDT(flags & PxGeometryQueryFlag::eSIMD_GUARD)
OverlapAdapter oa(cb);
switch(queryVolume.getType())
{
case PxGeometryType::eBOX:
{
if(queryVolume.isOBB())
{
const DefaultOBBAABBTest test(queryVolume);
if(mData.mIndices)
return AABBTreeOverlap<true, OBBAABBTest, BVHTree, BVHNode, OverlapAdapter>()(mData.mBounds, BVHTree(mData), test, oa);
else
return AABBTreeOverlap<false, OBBAABBTest, BVHTree, BVHNode, OverlapAdapter>()(mData.mBounds, BVHTree(mData), test, oa);
}
else
{
const DefaultAABBAABBTest test(queryVolume);
if(mData.mIndices)
return AABBTreeOverlap<true, AABBAABBTest, BVHTree, BVHNode, OverlapAdapter>()(mData.mBounds, BVHTree(mData), test, oa);
else
return AABBTreeOverlap<false, AABBAABBTest, BVHTree, BVHNode, OverlapAdapter>()(mData.mBounds, BVHTree(mData), test, oa);
}
}
case PxGeometryType::eCAPSULE:
{
const DefaultCapsuleAABBTest test(queryVolume, 1.0f);
if(mData.mIndices)
return AABBTreeOverlap<true, CapsuleAABBTest, BVHTree, BVHNode, OverlapAdapter>()(mData.mBounds, BVHTree(mData), test, oa);
else
return AABBTreeOverlap<false, CapsuleAABBTest, BVHTree, BVHNode, OverlapAdapter>()(mData.mBounds, BVHTree(mData), test, oa);
}
case PxGeometryType::eSPHERE:
{
const DefaultSphereAABBTest test(queryVolume);
if(mData.mIndices)
return AABBTreeOverlap<true, SphereAABBTest, BVHTree, BVHNode, OverlapAdapter>()(mData.mBounds, BVHTree(mData), test, oa);
else
return AABBTreeOverlap<false, SphereAABBTest, BVHTree, BVHNode, OverlapAdapter>()(mData.mBounds, BVHTree(mData), test, oa);
}
case PxGeometryType::eCONVEXMESH:
{
const DefaultOBBAABBTest test(queryVolume);
if(mData.mIndices)
return AABBTreeOverlap<true, OBBAABBTest, BVHTree, BVHNode, OverlapAdapter>()(mData.mBounds, BVHTree(mData), test, oa);
else
return AABBTreeOverlap<false, OBBAABBTest, BVHTree, BVHNode, OverlapAdapter>()(mData.mBounds, BVHTree(mData), test, oa);
}
default:
PX_ALWAYS_ASSERT_MESSAGE("unsupported overlap query volume geometry type");
}
return false;
}
bool BVH::overlap(const PxGeometry& geom, const PxTransform& pose, OverlapCallback& cb, PxGeometryQueryFlags flags) const
{
const ShapeData queryVolume(geom, pose, 0.0f);
return overlap(queryVolume, cb, flags);
}
bool BVH::sweep(const ShapeData& queryVolume, const PxVec3& unitDir, float distance, RaycastCallback& cb, PxGeometryQueryFlags flags) const
{
PX_SIMD_GUARD_CNDT(flags & PxGeometryQueryFlag::eSIMD_GUARD)
const PxBounds3& aabb = queryVolume.getPrunerInflatedWorldAABB();
RaycastAdapter ra(cb);
if(mData.mIndices)
return AABBTreeRaycast<true, true, BVHTree, BVHNode, RaycastAdapter>()(mData.mBounds, BVHTree(mData), aabb.getCenter(), unitDir, distance, aabb.getExtents(), ra);
else
return AABBTreeRaycast<true, false, BVHTree, BVHNode, RaycastAdapter>()(mData.mBounds, BVHTree(mData), aabb.getCenter(), unitDir, distance, aabb.getExtents(), ra);
}
bool BVH::sweep(const PxGeometry& geom, const PxTransform& pose, const PxVec3& unitDir, float distance, RaycastCallback& cb, PxGeometryQueryFlags flags) const
{
const ShapeData queryVolume(geom, pose, 0.0f);
return sweep(queryVolume, unitDir, distance, cb, flags);
}
namespace
{
PX_FORCE_INLINE bool planesAABBOverlap(const PxVec3& m, const PxVec3& d, const PxPlane* p, PxU32& outClipMask, PxU32 inClipMask)
{
PxU32 mask = 1;
PxU32 tmpOutClipMask = 0;
while(mask<=inClipMask)
{
if(inClipMask & mask)
{
const float NP = d.x*fabsf(p->n.x) + d.y*fabsf(p->n.y) + d.z*fabsf(p->n.z);
const float MP = m.x*p->n.x + m.y*p->n.y + m.z*p->n.z + p->d;
if(NP < MP)
return false;
if((-NP) < MP)
tmpOutClipMask |= mask;
}
mask+=mask;
p++;
}
outClipMask = tmpOutClipMask;
return true;
}
struct FrustumTest
{
FrustumTest(PxU32 nbPlanes, const PxPlane* planes) : mPlanes(planes), mMask((1<<nbPlanes)-1), mNbPlanes(nbPlanes), mOutClipMask(0)
{
}
PX_FORCE_INLINE PxIntBool operator()(const Vec3V boxCenter, const Vec3V boxExtents) const
{
// PT: TODO: rewrite all this in SIMD
PxVec3 center, extents;
V3StoreU(boxCenter, center);
V3StoreU(boxExtents, extents);
if(!planesAABBOverlap(center, extents, mPlanes, mOutClipMask, mMask))
return PxIntFalse;
// PT: unfortunately the AABBTreeOverlap template doesn't support this case where we know we can
// immediately dump the rest of the tree (i.e. the old "containment tests" in Opcode). We might
// want to revisit this at some point.
//
// In fact it's worse than this: we lost the necessary data to make this quick, in "flattenTree"
// when going from AABBTreeBuildNodes to BVHNodes. The BVHNodes lost the primitive-related info
// for internal (non-leaf) nodes so we cannot just dump a list of primitives when an internal
// node is fully visible (like we did in Opcode 1.x). Best we can do is keep traversing the tree
// and skip VFC tests.
//if(!outClipMask)
return PxIntTrue;
}
const PxPlane* mPlanes;
const PxU32 mMask;
const PxU32 mNbPlanes;
mutable PxU32 mOutClipMask;
PX_NOCOPY(FrustumTest)
};
}
static bool dumpNode(OverlapAdapter& oa, const BVHNode* const nodeBase, const BVHNode* node0, const PxU32* indices)
{
PxInlineArray<const BVHNode*, RAW_TRAVERSAL_STACK_SIZE> stack;
stack.forceSize_Unsafe(RAW_TRAVERSAL_STACK_SIZE);
stack[0] = node0;
PxU32 stackIndex = 1;
while(stackIndex > 0)
{
const BVHNode* node = stack[--stackIndex];
while(1)
{
if(node->isLeaf())
{
PxU32 nbPrims = node->getNbPrimitives();
const PxU32* prims = indices ? node->getPrimitives(indices) : NULL;
while(nbPrims--)
{
const PxU32 primIndex = indices ? *prims++ : node->getPrimitiveIndex();
if(!oa.invoke(primIndex))
return false;
}
break;
}
else
{
const BVHNode* children = node->getPos(nodeBase);
node = children;
stack[stackIndex++] = children + 1;
if(stackIndex == stack.capacity())
stack.resizeUninitialized(stack.capacity() * 2);
}
}
}
return true;
}
bool BVH::cull(PxU32 nbPlanes, const PxPlane* planes, OverlapCallback& cb, PxGeometryQueryFlags flags) const
{
PX_SIMD_GUARD_CNDT(flags & PxGeometryQueryFlag::eSIMD_GUARD)
OverlapAdapter oa(cb);
const FrustumTest test(nbPlanes, planes);
if(0)
{
// PT: this vanilla codepath is slower
if(mData.mIndices)
return AABBTreeOverlap<true, FrustumTest, BVHTree, BVHNode, OverlapAdapter>()(mData.mBounds, BVHTree(mData), test, oa);
else
return AABBTreeOverlap<false, FrustumTest, BVHTree, BVHNode, OverlapAdapter>()(mData.mBounds, BVHTree(mData), test, oa);
}
else
{
const PxBounds3* bounds = mData.mBounds.getBounds();
const bool hasIndices = mData.mIndices!=NULL;
PxInlineArray<const BVHNode*, RAW_TRAVERSAL_STACK_SIZE> stack;
stack.forceSize_Unsafe(RAW_TRAVERSAL_STACK_SIZE);
const BVHNode* const nodeBase = mData.mNodes;
stack[0] = nodeBase;
PxU32 stackIndex = 1;
while(stackIndex > 0)
{
const BVHNode* node = stack[--stackIndex];
Vec3V center, extents;
node->getAABBCenterExtentsV(&center, &extents);
while(test(center, extents))
{
if(!test.mOutClipMask)
{
if(!dumpNode(oa, nodeBase, node, mData.mIndices))
return false;
break;
}
else
{
if(node->isLeaf())
{
PxU32 nbPrims = node->getNbPrimitives();
const bool doBoxTest = nbPrims > 1;
const PxU32* prims = hasIndices ? node->getPrimitives(mData.mIndices) : NULL;
while(nbPrims--)
{
const PxU32 primIndex = hasIndices ? *prims++ : node->getPrimitiveIndex();
if(doBoxTest)
{
Vec4V center2, extents2;
getBoundsTimesTwo(center2, extents2, bounds, primIndex);
const float half = 0.5f;
const FloatV halfV = FLoad(half);
const Vec4V extents_ = V4Scale(extents2, halfV);
const Vec4V center_ = V4Scale(center2, halfV);
if(!test(Vec3V_From_Vec4V(center_), Vec3V_From_Vec4V(extents_)))
continue;
}
if(!oa.invoke(primIndex))
return false;
}
break;
}
const BVHNode* children = node->getPos(nodeBase);
node = children;
stack[stackIndex++] = children + 1;
if(stackIndex == stack.capacity())
stack.resizeUninitialized(stack.capacity() * 2);
node->getAABBCenterExtentsV(&center, &extents);
}
}
}
return true;
}
}
void BVH::refit()
{
mData.fullRefit(mData.mBounds.getBounds());
}
bool BVH::updateBoundsInternal(PxU32 localIndex, const PxBounds3& newBounds)
{
if(localIndex>=mData.mNbIndices)
return false;
PxBounds3* bounds = mData.mBounds.getBounds();
bounds[localIndex] = newBounds;
// Lazy-create update map
if(!mData.getUpdateMap())
mData.createUpdateMap(mData.mNbIndices);
PxU32* mMapping = mData.getUpdateMap();
if(mMapping)
{
const PxU32 treeNodeIndex = mMapping[localIndex];
if(treeNodeIndex!=0xffffffff)
{
mData.markNodeForRefit(treeNodeIndex);
return true;
}
}
return false;
}
bool BVH::updateBounds(PxU32 boundsIndex, const PxBounds3& newBounds)
{
return updateBoundsInternal(boundsIndex, newBounds);
}
void BVH::partialRefit()
{
mData.refitMarkedNodes(mData.mBounds.getBounds());
}
bool BVH::traverse(TraversalCallback& cb) const
{
// PT: copy-pasted from AABBTreeOverlap and modified
PxInlineArray<const BVHNode*, RAW_TRAVERSAL_STACK_SIZE> stack;
stack.forceSize_Unsafe(RAW_TRAVERSAL_STACK_SIZE);
const BVHNode* const nodeBase = mData.getNodes();
stack[0] = nodeBase;
PxU32 stackIndex = 1;
while(stackIndex > 0)
{
const BVHNode* node = stack[--stackIndex];
while(cb.visitNode(node->mBV))
{
if(node->isLeaf())
{
if(mData.getIndices())
{
if(!cb.reportLeaf(node->getNbPrimitives(), node->getPrimitives(mData.getIndices())))
return false;
}
else
{
PX_ASSERT(node->getNbPrimitives()==1);
const PxU32 primIndex = node->getPrimitiveIndex();
if(!cb.reportLeaf(node->getNbPrimitives(), &primIndex))
return false;
}
break;
}
const BVHNode* children = node->getPos(nodeBase);
node = children;
stack[stackIndex++] = children + 1;
if(stackIndex == stack.capacity())
stack.resizeUninitialized(stack.capacity() * 2);
}
}
return true;
}
#include "geometry/PxMeshQuery.h"
#define GU_BVH_STACK_SIZE 1024 // Default size of local stacks for non-recursive traversals.
static bool doLeafVsLeaf(PxReportCallback<PxGeomIndexPair>& callback, const BVHNode* node0, const PxBounds3* bounds0, const PxU32* indices0,
const BVHNode* node1, const PxBounds3* bounds1, const PxU32* indices1,
bool& abort)
{
PxGeomIndexPair* dst = callback.mBuffer;
PxU32 capacity = callback.mCapacity;
PxU32 currentSize = callback.mSize;
PX_ASSERT(currentSize<capacity);
bool foundHit = false;
abort = false;
const FloatV halfV = FLoad(0.5f);
PxU32 nbPrims0 = node0->getNbPrimitives();
const PxU32* prims0 = indices0 ? node0->getPrimitives(indices0) : NULL;
while(nbPrims0--)
{
const PxU32 primIndex0 = prims0 ? *prims0++ : node0->getPrimitiveIndex();
Vec3V center0, extents0;
{
Vec4V center2, extents2;
getBoundsTimesTwo(center2, extents2, bounds0, primIndex0);
extents0 = Vec3V_From_Vec4V(V4Scale(extents2, halfV));
center0 = Vec3V_From_Vec4V(V4Scale(center2, halfV));
}
PxU32 nbPrims1 = node1->getNbPrimitives();
const PxU32* prims1 = indices1 ? node1->getPrimitives(indices1) : NULL;
while(nbPrims1--)
{
const PxU32 primIndex1 = prims1 ? *prims1++ : node1->getPrimitiveIndex();
Vec3V center1, extents1;
{
Vec4V center2, extents2;
getBoundsTimesTwo(center2, extents2, bounds1, primIndex1);
extents1 = Vec3V_From_Vec4V(V4Scale(extents2, halfV));
center1 = Vec3V_From_Vec4V(V4Scale(center2, halfV));
}
if(PxIntBool(V3AllGrtrOrEq(V3Add(extents0, extents1), V3Abs(V3Sub(center1, center0)))))
{
foundHit = true;
// PT: TODO: refactor callback management code with BVH34
dst[currentSize].id0 = primIndex0;
dst[currentSize].id1 = primIndex1;
currentSize++;
if(currentSize==capacity)
{
callback.mSize = 0;
if(!callback.flushResults(currentSize, dst))
{
abort = true;
return foundHit;
}
dst = callback.mBuffer;
capacity = callback.mCapacity;
currentSize = callback.mSize;
}
}
}
}
callback.mSize = currentSize;
return foundHit;
}
static PX_FORCE_INLINE void pushChildren(PxGeomIndexPair* stack, PxU32& nb, PxU32 a, PxU32 b, PxU32 c, PxU32 d)
{
stack[nb].id0 = a;
stack[nb].id1 = b;
nb++;
stack[nb].id0 = c;
stack[nb].id1 = d;
nb++;
}
static PX_NOINLINE bool abortQuery(PxReportCallback<PxGeomIndexPair>& callback, bool& abort)
{
abort = true;
callback.mSize = 0;
return true;
}
static bool BVH_BVH(PxReportCallback<PxGeomIndexPair>& callback, const BVH& tree0, const BVH& tree1, bool& _abort)
{
const BVHNode* PX_RESTRICT node0 = tree0.getNodes();
const BVHNode* PX_RESTRICT node1 = tree1.getNodes();
PX_ASSERT(node0 && node1);
const PxBounds3* bounds0 = tree0.getData().mBounds.getBounds();
const PxBounds3* bounds1 = tree1.getData().mBounds.getBounds();
const PxU32* indices0 = tree0.getIndices();
const PxU32* indices1 = tree1.getIndices();
{
PxU32 nb=1;
PxGeomIndexPair stack[GU_BVH_STACK_SIZE];
stack[0].id0 = 0;
stack[0].id1 = 0;
bool status = false;
const BVHNode* const root0 = node0;
const BVHNode* const root1 = node1;
do
{
const PxGeomIndexPair& childData = stack[--nb];
node0 = root0 + childData.id0;
node1 = root1 + childData.id1;
if(node0->mBV.intersects(node1->mBV))
{
const PxU32 isLeaf0 = node0->isLeaf();
const PxU32 isLeaf1 = node1->isLeaf();
if(isLeaf0)
{
if(isLeaf1)
{
bool abort;
if(doLeafVsLeaf(callback, node0, bounds0, indices0, node1, bounds1, indices1, abort))
status = true;
if(abort)
return abortQuery(callback, _abort);
}
else
{
const PxU32 posIndex1 = node1->getPosIndex();
pushChildren(stack, nb, childData.id0, posIndex1, childData.id0, posIndex1 + 1);
}
}
else if(isLeaf1)
{
const PxU32 posIndex0 = node0->getPosIndex();
pushChildren(stack, nb, posIndex0, childData.id1, posIndex0 + 1, childData.id1);
}
else
{
const PxU32 posIndex0 = node0->getPosIndex();
const PxU32 posIndex1 = node1->getPosIndex();
pushChildren(stack, nb, posIndex0, posIndex1, posIndex0, posIndex1 + 1);
pushChildren(stack, nb, posIndex0 + 1, posIndex1, posIndex0 + 1, posIndex1 + 1);
}
}
}while(nb);
return status;
}
}
bool physx::PxFindOverlap(PxReportCallback<PxGeomIndexPair>& callback, const PxBVH& bvh0, const PxBVH& bvh1)
{
PX_SIMD_GUARD
// PT: TODO: refactor callback management code with BVH34
PxGeomIndexPair stackBuffer[256];
bool mustResetBuffer;
if(callback.mBuffer)
{
PX_ASSERT(callback.mCapacity);
mustResetBuffer = false;
}
else
{
callback.mBuffer = stackBuffer;
PX_ASSERT(callback.mCapacity<=256);
if(callback.mCapacity==0 || callback.mCapacity>256)
{
callback.mCapacity = 256;
}
callback.mSize = 0;
mustResetBuffer = true;
}
bool abort = false;
const bool status = BVH_BVH(callback, static_cast<const BVH&>(bvh0), static_cast<const BVH&>(bvh1), abort);
if(!abort)
{
const PxU32 currentSize = callback.mSize;
if(currentSize)
{
callback.mSize = 0;
callback.flushResults(currentSize, callback.mBuffer);
}
}
if(mustResetBuffer)
callback.mBuffer = NULL;
return status;
}
Reference in New Issue View Git Blame Copy Permalink