Revision control
++ // Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
++ // Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
++ // SPDX-FileCopyrightText: Copyright (c) 2008-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
++ // SPDX-License-Identifier: Apache-2.0
++
++ #include "foundation/PxBounds3.h"
++ #include "foundation/PxMemory.h"
++ #include "common/PxTolerancesScale.h"
++
++ #include "foundation/PxSimpleTypes.h"
++ #include "foundation/PxSort.h"
++ #include "foundation/PxAllocator.h"
++ #include "foundation/PxVecMath.h"
++ #include "foundation/PxInlineArray.h"
++ #include "GuRTree.h"
++ #include "GuRTreeCooking.h"
++
++ #define PRINT_RTREE_COOKING_STATS 0 // AP: keeping this frequently used macro for diagnostics/benchmarking
++
++ #if PRINT_RTREE_COOKING_STATS
++ #include <stdio.h>
++ #endif
++
++ using namespace physx::Gu;
++ using namespace physx::aos;
++
++ namespace physx
++ {
++
++ // Google "wikipedia QuickSelect" for algorithm explanation
++ namespace quickSelect {
++
++ #define SWAP32(x, y) { PxU32 tmp = y; y = x; x = tmp; }
++
++ // left is the index of the leftmost element of the subarray
++ // right is the index of the rightmost element of the subarray (inclusive)
++ // number of elements in subarray = right-left+1
++ template<typename LtEq>
++ PxU32 partition(PxU32* PX_RESTRICT a, PxU32 left, PxU32 right, PxU32 pivotIndex, const LtEq& cmpLtEq)
++ {
++ PX_ASSERT(pivotIndex >= left && pivotIndex <= right);
++ PxU32 pivotValue = a[pivotIndex];
++ SWAP32(a[pivotIndex], a[right]) // Move pivot to end
++ PxU32 storeIndex = left;
++ for (PxU32 i = left; i < right; i++) // left <= i < right
++ if (cmpLtEq(a[i], pivotValue))
++ {
++ SWAP32(a[i], a[storeIndex]);
++ storeIndex++;
++ }
++ SWAP32(a[storeIndex], a[right]); // Move pivot to its final place
++ for (PxU32 i = left; i < storeIndex; i++)
++ PX_ASSERT(cmpLtEq(a[i], a[storeIndex]));
++ for (PxU32 i = storeIndex+1; i <= right; i++)
++ PX_ASSERT(cmpLtEq(a[storeIndex], a[i]));
++ return storeIndex;
++ }
++
++ // left is the index of the leftmost element of the subarray
++ // right is the index of the rightmost element of the subarray (inclusive)
++ // number of elements in subarray = right-left+1
++ // recursive version
++ template<typename LtEq>
++ void quickFindFirstK(PxU32* PX_RESTRICT a, PxU32 left, PxU32 right, PxU32 k, const LtEq& cmpLtEq)
++ {
++ PX_ASSERT(k <= right-left+1);
++ if (right > left)
++ {
++ // select pivotIndex between left and right
++ PxU32 pivotIndex = (left + right) >> 1;
++ PxU32 pivotNewIndex = partition(a, left, right, pivotIndex, cmpLtEq);
++ // now all elements to the left of pivotNewIndex are < old value of a[pivotIndex] (bottom half values)
++ if (pivotNewIndex > left + k) // new condition
++ quickFindFirstK(a, left, pivotNewIndex-1, k, cmpLtEq);
++ if (pivotNewIndex < left + k)
++ quickFindFirstK(a, pivotNewIndex+1, right, k+left-pivotNewIndex-1, cmpLtEq);
++ }
++ }
++
++ // non-recursive version
++ template<typename LtEq>
++ void quickSelectFirstK(PxU32* PX_RESTRICT a, PxU32 left, PxU32 right, PxU32 k, const LtEq& cmpLtEq)
++ {
++ PX_ASSERT(k <= right-left+1);
++ for (;;)
++ {
++ PxU32 pivotIndex = (left+right) >> 1;
++ PxU32 pivotNewIndex = partition(a, left, right, pivotIndex, cmpLtEq);
++ PxU32 pivotDist = pivotNewIndex - left + 1;
++ if (pivotDist == k)
++ return;
++ else if (k < pivotDist)
++ {
++ PX_ASSERT(pivotNewIndex > 0);
++ right = pivotNewIndex - 1;
++ }
++ else
++ {
++ k = k - pivotDist;
++ left = pivotNewIndex+1;
++ }
++ }
++ }
++
++ } // namespace quickSelect
++
++ // Intermediate non-quantized representation for RTree node in a page (final format is SIMD transposed page)
++ struct RTreeNodeNQ
++ {
++ PxBounds3 bounds;
++ PxI32 childPageFirstNodeIndex; // relative to the beginning of all build tree nodes array
++ PxI32 leafCount; // -1 for empty nodes, 0 for non-terminal nodes, number of enclosed tris if non-zero (LeafTriangles), also means a terminal node
++
++ struct U {}; // selector struct for uninitialized constructor
++ RTreeNodeNQ(U) {} // uninitialized constructor
++ RTreeNodeNQ() : bounds(PxBounds3::empty()), childPageFirstNodeIndex(-1), leafCount(0) {}
++ };
++
++ // SIMD version of bounds class
++ struct PxBounds3V
++ {
++ struct U {}; // selector struct for uninitialized constructor
++ Vec3V mn, mx;
++ PxBounds3V(Vec3VArg mn_, Vec3VArg mx_) : mn(mn_), mx(mx_) {}
++ PxBounds3V(U) {} // uninitialized constructor
++
++ PX_FORCE_INLINE Vec3V getExtents() const { return V3Sub(mx, mn); }
++ PX_FORCE_INLINE void include(const PxBounds3V& other) { mn = V3Min(mn, other.mn); mx = V3Max(mx, other.mx); }
++
++ // convert vector extents to PxVec3
++ PX_FORCE_INLINE const PxVec3 getMinVec3() const { PxVec3 ret; V3StoreU(mn, ret); return ret; }
++ PX_FORCE_INLINE const PxVec3 getMaxVec3() const { PxVec3 ret; V3StoreU(mx, ret); return ret; }
++ };
++
++ static void buildFromBounds(
++ Gu::RTree& resultTree, const PxBounds3V* allBounds, PxU32 numBounds,
++ PxArray<PxU32>& resultPermute, RTreeCooker::RemapCallback* rc, Vec3VArg allMn, Vec3VArg allMx,
++ PxReal sizePerfTradeOff, PxMeshCookingHint::Enum hint);
++
++ /////////////////////////////////////////////////////////////////////////
++ void RTreeCooker::buildFromTriangles(
++ Gu::RTree& result, const PxVec3* verts, PxU32 numVerts, const PxU16* tris16, const PxU32* tris32, PxU32 numTris,
++ PxArray<PxU32>& resultPermute, RTreeCooker::RemapCallback* rc, PxReal sizePerfTradeOff01, PxMeshCookingHint::Enum hint)
++ {
++ PX_UNUSED(numVerts);
++ PxArray<PxBounds3V> allBounds;
++ allBounds.reserve(numTris);
++ Vec3V allMn = Vec3V_From_FloatV(FMax()), allMx = Vec3V_From_FloatV(FNegMax());
++ Vec3V eps = V3Splat(FLoad(5e-4f)); // AP scaffold: use PxTolerancesScale here?
++
++ // build RTree AABB bounds from triangles, conservative bound inflation is also performed here
++ for(PxU32 i = 0; i < numTris; i ++)
++ {
++ PxU32 i0, i1, i2;
++ PxU32 i3 = i*3;
++ if(tris16)
++ {
++ i0 = tris16[i3]; i1 = tris16[i3+1]; i2 = tris16[i3+2];
++ } else
++ {
++ i0 = tris32[i3]; i1 = tris32[i3+1]; i2 = tris32[i3+2];
++ }
++ PX_ASSERT_WITH_MESSAGE(i0 < numVerts && i1 < numVerts && i2 < numVerts ,"Input mesh triangle's vertex index exceeds specified numVerts.");
++ Vec3V v0 = V3LoadU(verts[i0]), v1 = V3LoadU(verts[i1]), v2 = V3LoadU(verts[i2]);
++ Vec3V mn = V3Sub(V3Min(V3Min(v0, v1), v2), eps); // min over 3 verts, subtract eps to inflate
++ Vec3V mx = V3Add(V3Max(V3Max(v0, v1), v2), eps); // max over 3 verts, add eps to inflate
++ allMn = V3Min(allMn, mn); allMx = V3Max(allMx, mx);
++ allBounds.pushBack(PxBounds3V(mn, mx));
++ }
++
++ buildFromBounds(result, allBounds.begin(), numTris, resultPermute, rc, allMn, allMx, sizePerfTradeOff01, hint);
++ }
++
++ /////////////////////////////////////////////////////////////////////////
++ // Fast but lower quality 4-way split sorting using repeated application of quickselect
++
++ // comparator template struct for sortin gbounds centers given a coordinate index (x,y,z=0,1,2)
++ struct BoundsLTE
++ {
++ PxU32 coordIndex;
++ const PxVec3* PX_RESTRICT boundCenters; // AP: precomputed centers are faster than recomputing the centers
++ BoundsLTE(PxU32 coordIndex_, const PxVec3* boundCenters_)
++ : coordIndex(coordIndex_), boundCenters(boundCenters_)
++ {}
++
++ PX_FORCE_INLINE bool operator()(const PxU32 & idx1, const PxU32 & idx2) const
++ {
++ PxF32 center1 = boundCenters[idx1][coordIndex];
++ PxF32 center2 = boundCenters[idx2][coordIndex];
++ return (center1 <= center2);
++ }
++ };
++
++ // ======================================================================
++ // Quick sorting method
++ // recursive sorting procedure:
++ // 1. find min and max extent along each axis for the current cluster
++ // 2. split input cluster into two 3 times using quickselect, splitting off a quarter of the initial cluster size each time
++ // 3. the axis is potentialy different for each split using the following
++ // approximate splitting heuristic - reduce max length by some estimated factor to encourage split along other axis
++ // since we cut off between a quarter to a half of elements in this direction per split
++ // the reduction for first split should be *0.75f but we use 0.8
++ // to account for some node overlap. This is somewhat of an arbitrary choice and there's room for improvement.
++ // 4. recurse on new clusters (goto step 1)
++ //
++ struct SubSortQuick
++ {
++ static const PxReal reductionFactors[RTREE_N-1];
++
++ enum { NTRADEOFF = 9 };
++ static const PxU32 stopAtTrisPerLeaf1[NTRADEOFF]; // presets for PxCookingParams::meshSizePerformanceTradeoff implementation
++
++ const PxU32* permuteEnd;
++ const PxU32* permuteStart;
++ const PxBounds3V* allBounds;
++ PxArray<PxVec3> boundCenters;
++ PxU32 maxBoundsPerLeafPage;
++
++ // initialize the context for the sorting routine
++ SubSortQuick(PxU32* permute, const PxBounds3V* allBounds_, PxU32 allBoundsSize, PxReal sizePerfTradeOff01)
++ : allBounds(allBounds_)
++ {
++ permuteEnd = permute + allBoundsSize;
++ permuteStart = permute;
++ PxU32 boundsCount = allBoundsSize;
++ boundCenters.reserve(boundsCount); // AP - measured that precomputing centers helps with perf significantly (~20% on 1k verts)
++ for(PxU32 i = 0; i < boundsCount; i++)
++ boundCenters.pushBack( allBounds[i].getMinVec3() + allBounds[i].getMaxVec3() );
++ PxU32 iTradeOff = PxMin<PxU32>( PxU32(PxMax<PxReal>(0.0f, sizePerfTradeOff01)*NTRADEOFF), NTRADEOFF-1 );
++ maxBoundsPerLeafPage = stopAtTrisPerLeaf1[iTradeOff];
++ }
++
++ // implements the sorting/splitting procedure
++ void sort4(
++ PxU32* PX_RESTRICT permute, const PxU32 clusterSize, // beginning and size of current recursively processed cluster
++ PxArray<RTreeNodeNQ>& resultTree, PxU32& maxLevels,
++ PxBounds3V& subTreeBound, PxU32 level = 0)
++ {
++ if(level == 0)
++ maxLevels = 1;
++ else
++ maxLevels = PxMax(maxLevels, level+1);
++
++ PX_ASSERT(permute + clusterSize <= permuteEnd);
++ PX_ASSERT(maxBoundsPerLeafPage >= RTREE_N-1);
++
++ const PxU32 cluster4 = PxMax<PxU32>(clusterSize/RTREE_N, 1);
++
++ PX_ASSERT(clusterSize > 0);
++ // find min and max world bound for current cluster
++ Vec3V mx = allBounds[permute[0]].mx, mn = allBounds[permute[0]].mn; PX_ASSERT(permute[0] < boundCenters.size());
++ for(PxU32 i = 1; i < clusterSize; i ++)
++ {
++ PX_ASSERT(permute[i] < boundCenters.size());
++ mx = V3Max(mx, allBounds[permute[i]].mx);
++ mn = V3Min(mn, allBounds[permute[i]].mn);
++ }
++ PX_ALIGN_PREFIX(16) PxReal maxElem[4] PX_ALIGN_SUFFIX(16);
++ V3StoreA(V3Sub(mx, mn), *reinterpret_cast<PxVec3*>(maxElem)); // compute the dimensions and store into a scalar maxElem array
++
++ // split along the longest axis
++ const PxU32 maxDiagElement = PxU32(maxElem[0] > maxElem[1] && maxElem[0] > maxElem[2] ? 0 : (maxElem[1] > maxElem[2] ? 1 : 2));
++ BoundsLTE cmpLte(maxDiagElement, boundCenters.begin());
++
++ const PxU32 startNodeIndex = resultTree.size();
++ resultTree.resizeUninitialized(startNodeIndex+RTREE_N); // at each recursion level we add 4 nodes to the tree
++
++ PxBounds3V childBound( (PxBounds3V::U()) ); // start off uninitialized for performance
++ const PxI32 leftover = PxMax<PxI32>(PxI32(clusterSize - cluster4*(RTREE_N-1)), 0);
++ PxU32 totalCount = 0;
++ for(PxU32 i = 0; i < RTREE_N; i++)
++ {
++ // split off cluster4 count nodes out of the entire cluster for each i
++ const PxU32 clusterOffset = cluster4*i;
++ PxU32 count1; // cluster4 or leftover depending on whether it's the last cluster
++ if(i < RTREE_N-1)
++ {
++ // only need to so quickSelect for the first pagesize-1 clusters
++ if(clusterOffset <= clusterSize-1)
++ {
++ quickSelect::quickSelectFirstK(permute, clusterOffset, clusterSize-1, cluster4, cmpLte);
++ // approximate heuristic - reduce max length by some estimated factor to encourage split along other axis
++ // since we cut off a quarter of elements in this direction the reduction should be *0.75f but we use 0.8
++ // to account for some node overlap. This is somewhat of an arbitrary choice though
++ maxElem[cmpLte.coordIndex] *= reductionFactors[i];
++ // recompute cmpLte.coordIndex from updated maxElements
++ cmpLte.coordIndex = PxU32(maxElem[0] > maxElem[1] && maxElem[0] > maxElem[2] ? 0 : (maxElem[1] > maxElem[2] ? 1 : 2));
++ }
++ count1 = cluster4;
++ } else
++ {
++ count1 = PxU32(leftover);
++ // verify that leftover + sum of previous clusters adds up to clusterSize or leftover is 0
++ // leftover can be 0 if clusterSize<RTREE_N, this is generally rare, can happen for meshes with < RTREE_N tris
++ PX_ASSERT(leftover == 0 || cluster4*i + count1 == clusterSize);
++ }
++
++ RTreeNodeNQ& curNode = resultTree[startNodeIndex+i];
++
++ totalCount += count1; // accumulate total node count
++ if(count1 <= maxBoundsPerLeafPage) // terminal page according to specified maxBoundsPerLeafPage
++ {
++ if(count1 && totalCount <= clusterSize)
++ {
++ // this will be true most of the time except when the total number of triangles in the mesh is < PAGESIZE
++ curNode.leafCount = PxI32(count1);
++ curNode.childPageFirstNodeIndex = PxI32(clusterOffset + PxU32(permute-permuteStart));
++ childBound = allBounds[permute[clusterOffset+0]];
++ for(PxU32 i1 = 1; i1 < count1; i1++)
++ {
++ const PxBounds3V& bnd = allBounds[permute[clusterOffset+i1]];
++ childBound.include(bnd);
++ }
++ } else
++ {
++ // since we are required to have PAGESIZE nodes per page for simd, we fill any leftover with empty nodes
++ // we should only hit this if the total number of triangles in the mesh is < PAGESIZE
++ childBound.mn = childBound.mx = V3Zero(); // shouldn't be necessary but setting just in case
++ curNode.bounds.setEmpty();
++ curNode.leafCount = -1;
++ curNode.childPageFirstNodeIndex = -1; // using -1 for empty node
++ }
++ } else // not a terminal page, recurse on count1 nodes cluster
++ {
++ curNode.childPageFirstNodeIndex = PxI32(resultTree.size());
++ curNode.leafCount = 0;
++ sort4(permute+cluster4*i, count1, resultTree, maxLevels, childBound, level+1);
++ }
++ if(i == 0)
++ subTreeBound = childBound; // initialize subTreeBound with first childBound
++ else
++ subTreeBound.include(childBound); // expand subTreeBound with current childBound
++
++ // can use curNode since the reference change due to resizing in recursive call, need to recompute the pointer
++ RTreeNodeNQ& curNode1 = resultTree[startNodeIndex+i];
++ curNode1.bounds.minimum = childBound.getMinVec3(); // update node bounds using recursively computed childBound
++ curNode1.bounds.maximum = childBound.getMaxVec3();
++ }
++ }
++ };
++
++ // heuristic size reduction factors for splitting heuristic (see how it's used above)
++ const PxReal SubSortQuick::reductionFactors[RTREE_N-1] = {0.8f, 0.7f, 0.6f};
++
++ // sizePerf trade-off presets for sorting routines
++ const PxU32 SubSortQuick::stopAtTrisPerLeaf1[SubSortQuick::NTRADEOFF] = {16, 14, 12, 10, 8, 7, 6, 5, 4};
++
++ /////////////////////////////////////////////////////////////////////////
++ // SAH sorting method
++ //
++ // Preset table: lower index=better size -> higher index = better perf
++ static const PxU32 NTRADEOFF = 15;
++ // % -24 -23 -17 -15 -10 -8 -5 -3 0 +3 +3 +5 +7 +8 +9 - % raycast MeshSurface*Random benchmark perf
++ // K 717 734 752 777 793 811 824 866 903 939 971 1030 1087 1139 1266 - testzone size in K
++ // # 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 - preset number
++ static const PxU32 stopAtTrisPerPage[NTRADEOFF] = { 64, 60, 56, 48, 46, 44, 40, 36, 32, 28, 24, 20, 16, 12, 12};
++ static const PxU32 stopAtTrisPerLeaf[NTRADEOFF] = { 16, 14, 12, 10, 9, 8, 8, 6, 5, 5, 5, 4, 4, 4, 2}; // capped at 2 anyway
++
++ /////////////////////////////////////////////////////////////////////////
++ // comparator struct for sorting the bounds along a specified coordIndex (coordIndex=0,1,2 for X,Y,Z)
++ struct SortBoundsPredicate
++ {
++ PxU32 coordIndex;
++ const PxBounds3V* allBounds;
++ SortBoundsPredicate(PxU32 coordIndex_, const PxBounds3V* allBounds_) : coordIndex(coordIndex_), allBounds(allBounds_)
++ {}
++
++ bool operator()(const PxU32 & idx1, const PxU32 & idx2) const
++ {
++ // using the bounds center for comparison
++ PxF32 center1 = V3ReadXYZ(allBounds[idx1].mn)[coordIndex] + V3ReadXYZ(allBounds[idx1].mx)[coordIndex];
++ PxF32 center2 = V3ReadXYZ(allBounds[idx2].mn)[coordIndex] + V3ReadXYZ(allBounds[idx2].mx)[coordIndex];
++ return (center1 < center2);
++ }
++ };
++
++
++ /////////////////////////////////////////////////////////////////////////
++ // auxiliary class for SAH build (SAH = surface area heuristic)
++ struct Interval
++ {
++ PxU32 start, count;
++ Interval(PxU32 s, PxU32 c) : start(s), count(c) {}
++ };
++
++ // SAH function - returns surface area for given AABB extents
++ static PX_FORCE_INLINE void PxSAH(const Vec3VArg v, PxF32& sah)
++ {
++ FStore(V3Dot(v, V3PermZXY(v)), &sah); // v.x*v.y + v.y*v.z + v.x*v.z;
++ }
++
++ struct SubSortSAH
++ {
++ PxU32* PX_RESTRICT permuteStart, *PX_RESTRICT tempPermute;
++ const PxBounds3V* PX_RESTRICT allBounds;
++ PxF32* PX_RESTRICT metricL;
++ PxF32* PX_RESTRICT metricR;
++ const PxU32* PX_RESTRICT xOrder, *PX_RESTRICT yOrder, *PX_RESTRICT zOrder;
++ const PxU32* PX_RESTRICT xRanks, *PX_RESTRICT yRanks, *PX_RESTRICT zRanks;
++ PxU32* PX_RESTRICT tempRanks;
++ PxU32 nbTotalBounds;
++ PxU32 iTradeOff;
++
++ // precompute various values used during sort
++ SubSortSAH(
++ PxU32* permute, const PxBounds3V* allBounds_, PxU32 numBounds,
++ const PxU32* xOrder_, const PxU32* yOrder_, const PxU32* zOrder_,
++ const PxU32* xRanks_, const PxU32* yRanks_, const PxU32* zRanks_, PxReal sizePerfTradeOff01)
++ : permuteStart(permute), allBounds(allBounds_),
++ xOrder(xOrder_), yOrder(yOrder_), zOrder(zOrder_),
++ xRanks(xRanks_), yRanks(yRanks_), zRanks(zRanks_), nbTotalBounds(numBounds)
++ {
++ metricL = PX_ALLOCATE(PxF32, numBounds, "metricL");
++ metricR = PX_ALLOCATE(PxF32, numBounds, "metricR");
++ tempPermute = PX_ALLOCATE(PxU32, (numBounds*2+1), "tempPermute");
++ tempRanks = PX_ALLOCATE(PxU32, numBounds, "tempRanks");
++ iTradeOff = PxMin<PxU32>( PxU32(PxMax<PxReal>(0.0f, sizePerfTradeOff01)*NTRADEOFF), NTRADEOFF-1 );
++ }
++
++ ~SubSortSAH() // release temporarily used memory
++ {
++ PX_FREE(metricL);
++ PX_FREE(metricR);
++ PX_FREE(tempPermute);
++ PX_FREE(tempRanks);
++ }
++
++ ////////////////////////////////////////////////////////////////////
++ // returns split position for second array start relative to permute ptr
++ PxU32 split(PxU32* permute, PxU32 clusterSize)
++ {
++ if(clusterSize <= 1)
++ return 0;
++ if(clusterSize == 2)
++ return 1;
++
++ PxI32 minCount = clusterSize >= 4 ? 2 : 1;
++ PxI32 splitStartL = minCount; // range=[startL->endL)
++ PxI32 splitEndL = PxI32(clusterSize-minCount);
++ PxI32 splitStartR = PxI32(clusterSize-splitStartL); // range=(endR<-startR], startR > endR
++ PxI32 splitEndR = PxI32(clusterSize-splitEndL);
++ PX_ASSERT(splitEndL-splitStartL == splitStartR-splitEndR);
++ PX_ASSERT(splitStartL <= splitEndL);
++ PX_ASSERT(splitStartR >= splitEndR);
++ PX_ASSERT(splitEndR >= 1);
++ PX_ASSERT(splitEndL < PxI32(clusterSize));
++
++ // pick the best axis with some splitting metric
++ // axis index is X=0, Y=1, Z=2
++ PxF32 minMetric[3];
++ PxU32 minMetricSplit[3];
++ const PxU32* ranks3[3] = { xRanks, yRanks, zRanks };
++ const PxU32* orders3[3] = { xOrder, yOrder, zOrder };
++ for(PxU32 coordIndex = 0; coordIndex <= 2; coordIndex++)
++ {
++ SortBoundsPredicate sortPredicateLR(coordIndex, allBounds);
++
++ const PxU32* rank = ranks3[coordIndex];
++ const PxU32* order = orders3[coordIndex];
++
++ // build ranks in tempPermute
++ if(clusterSize == nbTotalBounds) // AP: about 4% perf gain from this optimization
++ {
++ // if this is a full cluster sort, we already have it done
++ for(PxU32 i = 0; i < clusterSize; i ++)
++ tempPermute[i] = order[i];
++ } else
++ {
++ // sort the tempRanks
++ for(PxU32 i = 0; i < clusterSize; i ++)
++ tempRanks[i] = rank[permute[i]];
++ PxSort(tempRanks, clusterSize);
++ for(PxU32 i = 0; i < clusterSize; i ++) // convert back from ranks to indices
++ tempPermute[i] = order[tempRanks[i]];
++ }
++
++ // we consider overlapping intervals for minimum sum of metrics
++ // left interval is from splitStartL up to splitEndL
++ // right interval is from splitStartR down to splitEndR
++
++
++ // first compute the array metricL
++ Vec3V boundsLmn = allBounds[tempPermute[0]].mn; // init with 0th bound
++ Vec3V boundsLmx = allBounds[tempPermute[0]].mx; // init with 0th bound
++ PxI32 ii;
++ for(ii = 1; ii < splitStartL; ii++) // sweep right to include all bounds up to splitStartL-1
++ {
++ boundsLmn = V3Min(boundsLmn, allBounds[tempPermute[ii]].mn);
++ boundsLmx = V3Max(boundsLmx, allBounds[tempPermute[ii]].mx);
++ }
++
++ PxU32 countL0 = 0;
++ for(ii = splitStartL; ii <= splitEndL; ii++) // compute metric for inclusive bounds from splitStartL to splitEndL
++ {
++ boundsLmn = V3Min(boundsLmn, allBounds[tempPermute[ii]].mn);
++ boundsLmx = V3Max(boundsLmx, allBounds[tempPermute[ii]].mx);
++ PxSAH(V3Sub(boundsLmx, boundsLmn), metricL[countL0++]);
++ }
++ // now we have metricL
++
++ // now compute the array metricR
++ Vec3V boundsRmn = allBounds[tempPermute[clusterSize-1]].mn; // init with last bound
++ Vec3V boundsRmx = allBounds[tempPermute[clusterSize-1]].mx; // init with last bound
++ for(ii = PxI32(clusterSize-2); ii > splitStartR; ii--) // include bounds to the left of splitEndR down to splitStartR
++ {
++ boundsRmn = V3Min(boundsRmn, allBounds[tempPermute[ii]].mn);
++ boundsRmx = V3Max(boundsRmx, allBounds[tempPermute[ii]].mx);
++ }
++
++ PxU32 countR0 = 0;
++ for(ii = splitStartR; ii >= splitEndR; ii--) // continue sweeping left, including bounds and recomputing the metric
++ {
++ boundsRmn = V3Min(boundsRmn, allBounds[tempPermute[ii]].mn);
++ boundsRmx = V3Max(boundsRmx, allBounds[tempPermute[ii]].mx);
++ PxSAH(V3Sub(boundsRmx, boundsRmn), metricR[countR0++]);
++ }
++
++ PX_ASSERT((countL0 == countR0) && (countL0 == PxU32(splitEndL-splitStartL+1)));
++
++ // now iterate over splitRange and compute the minimum sum of SAHLeft*countLeft + SAHRight*countRight
++ PxU32 minMetricSplitPosition = 0;
++ PxF32 minMetricLocal = PX_MAX_REAL;
++ const PxI32 hsI32 = PxI32(clusterSize/2);
++ const PxI32 splitRange = (splitEndL-splitStartL+1);
++ for(ii = 0; ii < splitRange; ii++)
++ {
++ PxF32 countL = PxF32(ii+minCount); // need to add minCount since ii iterates over splitRange
++ PxF32 countR = PxF32(splitRange-ii-1+minCount);
++ PX_ASSERT(PxU32(countL + countR) == clusterSize);
++
++ const PxF32 metric = (countL*metricL[ii] + countR*metricR[splitRange-ii-1]);
++ const PxU32 splitPos = PxU32(ii+splitStartL);
++ if(metric < minMetricLocal ||
++ (metric <= minMetricLocal && // same metric but more even split
++ PxAbs(PxI32(splitPos)-hsI32) < PxAbs(PxI32(minMetricSplitPosition)-hsI32)))
++ {
++ minMetricLocal = metric;
++ minMetricSplitPosition = splitPos;
++ }
++ }
++
++ minMetric[coordIndex] = minMetricLocal;
++ minMetricSplit[coordIndex] = minMetricSplitPosition;
++
++ // sum of axis lengths for both left and right AABBs
++ }
++
++ PxU32 winIndex = 2;
++ if(minMetric[0] <= minMetric[1] && minMetric[0] <= minMetric[2])
++ winIndex = 0;
++ else if(minMetric[1] <= minMetric[2])
++ winIndex = 1;
++
++ const PxU32* rank = ranks3[winIndex];
++ const PxU32* order = orders3[winIndex];
++ if(clusterSize == nbTotalBounds) // AP: about 4% gain from this special case optimization
++ {
++ // if this is a full cluster sort, we already have it done
++ for(PxU32 i = 0; i < clusterSize; i ++)
++ permute[i] = order[i];
++ } else
++ {
++ // sort the tempRanks
++ for(PxU32 i = 0; i < clusterSize; i ++)
++ tempRanks[i] = rank[permute[i]];
++ PxSort(tempRanks, clusterSize);
++ for(PxU32 i = 0; i < clusterSize; i ++)
++ permute[i] = order[tempRanks[i]];
++ }
++
++ PxU32 splitPoint = minMetricSplit[winIndex];
++ if(clusterSize == 3 && splitPoint == 0)
++ splitPoint = 1; // special case due to rounding
++ return splitPoint;
++ }
++
++ // compute surface area for a given split
++ PxF32 computeSA(const PxU32* permute, const Interval& split) // both permute and i are relative
++ {
++ PX_ASSERT(split.count >= 1);
++ Vec3V bmn = allBounds[permute[split.start]].mn;
++ Vec3V bmx = allBounds[permute[split.start]].mx;
++ for(PxU32 i = 1; i < split.count; i++)
++ {
++ const PxBounds3V& b1 = allBounds[permute[split.start+i]];
++ bmn = V3Min(bmn, b1.mn); bmx = V3Max(bmx, b1.mx);
++ }
++
++ PxF32 ret; PxSAH(V3Sub(bmx, bmn), ret);
++ return ret;
++ }
++
++ ////////////////////////////////////////////////////////////////////
++ // main SAH sort routine
++ void sort4(PxU32* permute, PxU32 clusterSize,
++ PxArray<RTreeNodeNQ>& resultTree, PxU32& maxLevels, PxU32 level = 0, RTreeNodeNQ* parentNode = NULL)
++ {
++ PX_UNUSED(parentNode);
++
++ if(level == 0)
++ maxLevels = 1;
++ else
++ maxLevels = PxMax(maxLevels, level+1);
++
++ PxU32 splitPos[RTREE_N];
++ for(PxU32 j = 0; j < RTREE_N; j++)
++ splitPos[j] = j+1;
++
++ if(clusterSize >= RTREE_N)
++ {
++ // split into RTREE_N number of regions via RTREE_N-1 subsequent splits
++ // each split is represented as a current interval
++ // we iterate over currently active intervals and compute it's surface area
++ // then we split the interval with maximum surface area
++ // AP scaffold: possible optimization - seems like computeSA can be cached for unchanged intervals
++ PxInlineArray<Interval, 1024> splits;
++ splits.pushBack(Interval(0, clusterSize));
++ for(PxU32 iSplit = 0; iSplit < RTREE_N-1; iSplit++)
++ {
++ PxF32 maxSAH = -FLT_MAX;
++ PxU32 maxSplit = 0xFFFFffff;
++ for(PxU32 i = 0; i < splits.size(); i++)
++ {
++ if(splits[i].count == 1)
++ continue;
++ PxF32 SAH = computeSA(permute, splits[i])*splits[i].count;
++ if(SAH > maxSAH)
++ {
++ maxSAH = SAH;
++ maxSplit = i;
++ }
++ }
++ PX_ASSERT(maxSplit != 0xFFFFffff);
++
++ // maxSplit is now the index of the interval in splits array with maximum surface area
++ // we now split it into 2 using the split() function
++ Interval old = splits[maxSplit];
++ PX_ASSERT(old.count > 1);
++ PxU32 splitLocal = split(permute+old.start, old.count); // relative split pos
++
++ PX_ASSERT(splitLocal >= 1);
++ PX_ASSERT(old.count-splitLocal >= 1);
++ splits.pushBack(Interval(old.start, splitLocal));
++ splits.pushBack(Interval(old.start+splitLocal, old.count-splitLocal));
++ splits.replaceWithLast(maxSplit);
++ splitPos[iSplit] = old.start+splitLocal;
++ }
++
++ // verification code, make sure split counts add up to clusterSize
++ PX_ASSERT(splits.size() == RTREE_N);
++ PxU32 sum = 0;
++ PX_UNUSED(sum);
++ for(PxU32 j = 0; j < RTREE_N; j++)
++ sum += splits[j].count;
++ PX_ASSERT(sum == clusterSize);
++ }
++ else // clusterSize < RTREE_N
++ {
++ // make it so splitCounts based on splitPos add up correctly for small cluster sizes
++ for(PxU32 i = clusterSize; i < RTREE_N-1; i++)
++ splitPos[i] = clusterSize;
++ }
++
++ // sort splitPos index array using quicksort (just a few values)
++ PxSort(splitPos, RTREE_N-1);
++ splitPos[RTREE_N-1] = clusterSize; // splitCount[n] is computed as splitPos[n+1]-splitPos[n], so we need to add this last value
++
++ // now compute splitStarts and splitCounts from splitPos[] array. Also perform a bunch of correctness verification
++ PxU32 splitStarts[RTREE_N];
++ PxU32 splitCounts[RTREE_N];
++ splitStarts[0] = 0;
++ splitCounts[0] = splitPos[0];
++ PxU32 sumCounts = splitCounts[0];
++ PX_UNUSED(sumCounts);
++ for(PxU32 j = 1; j < RTREE_N; j++)
++ {
++ splitStarts[j] = splitPos[j-1];
++ PX_ASSERT(splitStarts[j-1]<=splitStarts[j]);
++ splitCounts[j] = splitPos[j]-splitPos[j-1];
++ PX_ASSERT(splitCounts[j] > 0 || clusterSize < RTREE_N);
++ sumCounts += splitCounts[j];
++ PX_ASSERT(splitStarts[j-1]+splitCounts[j-1]<=splitStarts[j]);
++ }
++ PX_ASSERT(sumCounts == clusterSize);
++ PX_ASSERT(splitStarts[RTREE_N-1]+splitCounts[RTREE_N-1]<=clusterSize);
++
++ // mark this cluster as terminal based on clusterSize <= stopAtTrisPerPage parameter for current iTradeOff user specified preset
++ bool terminalClusterByTotalCount = (clusterSize <= stopAtTrisPerPage[iTradeOff]);
++ // iterate over splitCounts for the current cluster, if any of counts exceed 16 (which is the maximum supported by LeafTriangles
++ // we cannot mark this cluster as terminal (has to be split more)
++ for(PxU32 s = 0; s < RTREE_N; s++)
++ if(splitCounts[s] > 16) // LeafTriangles doesn't support > 16 tris
++ terminalClusterByTotalCount = false;
++
++ // iterate over all the splits
++ for(PxU32 s = 0; s < RTREE_N; s++)
++ {
++ RTreeNodeNQ rtn;
++ PxU32 splitCount = splitCounts[s];
++ if(splitCount > 0) // splits shouldn't be empty generally
++ {
++ // sweep left to right and compute min and max SAH for each individual bound in current split
++ PxBounds3V b = allBounds[permute[splitStarts[s]]];
++ PxF32 sahMin; PxSAH(b.getExtents(), sahMin);
++ PxF32 sahMax = sahMin;
++ // AP scaffold - looks like this could be optimized (we are recomputing bounds top down)
++ for(PxU32 i = 1; i < splitCount; i++)
++ {
++ PxU32 localIndex = i + splitStarts[s];
++ const PxBounds3V& b1 = allBounds[permute[localIndex]];
++ PxF32 sah1; PxSAH(b1.getExtents(), sah1);
++ sahMin = PxMin(sahMin, sah1);
++ sahMax = PxMax(sahMax, sah1);
++ b.include(b1);
++ }
++
++ rtn.bounds.minimum = V3ReadXYZ(b.mn);
++ rtn.bounds.maximum = V3ReadXYZ(b.mx);
++
++ // if bounds differ widely (according to some heuristic preset), we continue splitting
++ // this is important for a mixed cluster with large and small triangles
++ bool okSAH = (sahMax/sahMin < 40.0f);
++ if(!okSAH)
++ terminalClusterByTotalCount = false; // force splitting this cluster
++
++ bool stopSplitting = // compute the final splitting criterion
++ splitCount <= 2 || (okSAH && splitCount <= 3) // stop splitting at 2 nodes or if SAH ratio is OK and splitCount <= 3
++ || terminalClusterByTotalCount || splitCount <= stopAtTrisPerLeaf[iTradeOff];
++ if(stopSplitting)
++ {
++ // this is a terminal page then, mark as such
++ // first node index is relative to the top level input array beginning
++ rtn.childPageFirstNodeIndex = PxI32(splitStarts[s]+(permute-permuteStart));
++ rtn.leafCount = PxI32(splitCount);
++ PX_ASSERT(splitCount <= 16); // LeafTriangles doesn't support more
++ }
++ else
++ {
++ // this is not a terminal page, we will recompute this later, after we recurse on subpages (label ZZZ)
++ rtn.childPageFirstNodeIndex = -1;
++ rtn.leafCount = 0;
++ }
++ }
++ else // splitCount == 0 at this point, this is an empty paddding node (with current presets it's very rare)
++ {
++ PX_ASSERT(splitCount == 0);
++ rtn.bounds.setEmpty();
++ rtn.childPageFirstNodeIndex = -1;
++ rtn.leafCount = -1;
++ }
++ resultTree.pushBack(rtn); // push the new node into the resultTree array
++ }
++
++ if(terminalClusterByTotalCount) // abort recursion if terminal cluster
++ return;
++
++ // recurse on subpages
++ PxU32 parentIndex = resultTree.size() - RTREE_N; // save the parentIndex as specified (array can be resized during recursion)
++ for(PxU32 s = 0; s<RTREE_N; s++)
++ {
++ RTreeNodeNQ* sParent = &resultTree[parentIndex+s]; // array can be resized and relocated during recursion
++ if(sParent->leafCount == 0) // only split pages that were marked as non-terminal during splitting (see "label ZZZ" above)
++ {
++ // all child nodes will be pushed inside of this recursive call,
++ // so we set the child pointer for parent node to resultTree.size()
++ sParent->childPageFirstNodeIndex = PxI32(resultTree.size());
++ sort4(permute+splitStarts[s], splitCounts[s], resultTree, maxLevels, level+1, sParent);
++ }
++ }
++ }
++ };
++
++
++
++
++ /////////////////////////////////////////////////////////////////////////
++ // initializes the input permute array with identity permutation
++ // and shuffles it so that new sorted index, newIndex = resultPermute[oldIndex]
++ static void buildFromBounds(
++ Gu::RTree& result, const PxBounds3V* allBounds, PxU32 numBounds,
++ PxArray<PxU32>& permute, RTreeCooker::RemapCallback* rc, Vec3VArg allMn, Vec3VArg allMx,
++ PxReal sizePerfTradeOff01, PxMeshCookingHint::Enum hint)
++ {
++ PX_UNUSED(sizePerfTradeOff01);
++ PxBounds3V treeBounds(allMn, allMx);
++
++ // start off with an identity permutation
++ permute.resize(0);
++ permute.reserve(numBounds+1);
++ for(PxU32 j = 0; j < numBounds; j ++)
++ permute.pushBack(j);
++ const PxU32 sentinel = 0xABCDEF01;
++ permute.pushBack(sentinel);
++
++ // load sorted nodes into an RTreeNodeNQ tree representation
++ // build the tree structure from sorted nodes
++ const PxU32 pageSize = RTREE_N;
++ PxArray<RTreeNodeNQ> resultTree;
++ resultTree.reserve(numBounds*2);
++
++ PxU32 maxLevels = 0;
++ if(hint == PxMeshCookingHint::eSIM_PERFORMANCE) // use high quality SAH build
++ {
++ PxArray<PxU32> xRanks(numBounds), yRanks(numBounds), zRanks(numBounds), xOrder(numBounds), yOrder(numBounds), zOrder(numBounds);
++ PxMemCopy(xOrder.begin(), permute.begin(), sizeof(xOrder[0])*numBounds);
++ PxMemCopy(yOrder.begin(), permute.begin(), sizeof(yOrder[0])*numBounds);
++ PxMemCopy(zOrder.begin(), permute.begin(), sizeof(zOrder[0])*numBounds);
++ // sort by shuffling the permutation, precompute sorted ranks for x,y,z-orders
++ PxSort(xOrder.begin(), xOrder.size(), SortBoundsPredicate(0, allBounds));
++ for(PxU32 i = 0; i < numBounds; i++) xRanks[xOrder[i]] = i;
++ PxSort(yOrder.begin(), yOrder.size(), SortBoundsPredicate(1, allBounds));
++ for(PxU32 i = 0; i < numBounds; i++) yRanks[yOrder[i]] = i;
++ PxSort(zOrder.begin(), zOrder.size(), SortBoundsPredicate(2, allBounds));
++ for(PxU32 i = 0; i < numBounds; i++) zRanks[zOrder[i]] = i;
++
++ SubSortSAH ss(permute.begin(), allBounds, numBounds,
++ xOrder.begin(), yOrder.begin(), zOrder.begin(), xRanks.begin(), yRanks.begin(), zRanks.begin(), sizePerfTradeOff01);
++ ss.sort4(permute.begin(), numBounds, resultTree, maxLevels);
++ } else
++ { // use fast cooking path
++ PX_ASSERT(hint == PxMeshCookingHint::eCOOKING_PERFORMANCE);
++ SubSortQuick ss(permute.begin(), allBounds, numBounds, sizePerfTradeOff01);
++ PxBounds3V discard((PxBounds3V::U()));
++ ss.sort4(permute.begin(), permute.size()-1, resultTree, maxLevels, discard); // AP scaffold: need to implement build speed/runtime perf slider
++ }
++
++ PX_ASSERT(permute[numBounds] == sentinel); // verify we didn't write past the array
++ permute.popBack(); // discard the sentinel value
++
++ #if PRINT_RTREE_COOKING_STATS // stats code
++ PxU32 totalLeafTris = 0;
++ PxU32 numLeaves = 0;
++ PxI32 maxLeafTris = 0;
++ PxU32 numEmpty = 0;
++ for(PxU32 i = 0; i < resultTree.size(); i++)
++ {
++ PxI32 leafCount = resultTree[i].leafCount;
++ numEmpty += (resultTree[i].bounds.isEmpty());
++ if(leafCount > 0)
++ {
++ numLeaves++;
++ totalLeafTris += leafCount;
++ if(leafCount > maxLeafTris)
++ maxLeafTris = leafCount;
++ }
++ }
++
++ printf("AABBs total/empty=%d/%d\n", resultTree.size(), numEmpty);
++ printf("numTris=%d, numLeafAABBs=%d, avgTrisPerLeaf=%.2f, maxTrisPerLeaf = %d\n",
++ numBounds, numLeaves, PxF32(totalLeafTris)/numLeaves, maxLeafTris);
++ #endif
++
++ PX_ASSERT(RTREE_N*sizeof(RTreeNodeQ) == sizeof(RTreePage)); // needed for nodePtrMultiplier computation to be correct
++ const int nodePtrMultiplier = sizeof(RTreeNodeQ); // convert offset as count in qnodes to page ptr
++
++ // Quantize the tree. AP scaffold - might be possible to merge this phase with the page pass below this loop
++ PxArray<RTreeNodeQ> qtreeNodes;
++ PxU32 firstEmptyIndex = PxU32(-1);
++ PxU32 resultCount = resultTree.size();
++ qtreeNodes.reserve(resultCount);
++
++ for(PxU32 i = 0; i < resultCount; i++) // AP scaffold - eliminate this pass
++ {
++ RTreeNodeNQ & u = resultTree[i];
++ RTreeNodeQ q;
++ q.setLeaf(u.leafCount > 0); // set the leaf flag
++ if(u.childPageFirstNodeIndex == -1) // empty node?
++ {
++ if(firstEmptyIndex == PxU32(-1))
++ firstEmptyIndex = qtreeNodes.size();
++ q.minx = q.miny = q.minz = FLT_MAX; // AP scaffold improvement - use empty 1e30 bounds instead and reference a valid leaf
++ q.maxx = q.maxy = q.maxz = -FLT_MAX; // that will allow to remove the empty node test from the runtime
++
++ q.ptr = firstEmptyIndex*nodePtrMultiplier; PX_ASSERT((q.ptr & 1) == 0);
++ q.setLeaf(true); // label empty node as leaf node
++ } else
++ {
++ // non-leaf node
++ q.minx = u.bounds.minimum.x;
++ q.miny = u.bounds.minimum.y;
++ q.minz = u.bounds.minimum.z;
++ q.maxx = u.bounds.maximum.x;
++ q.maxy = u.bounds.maximum.y;
++ q.maxz = u.bounds.maximum.z;
++ if(u.leafCount > 0)
++ {
++ q.ptr = PxU32(u.childPageFirstNodeIndex);
++ rc->remap(&q.ptr, q.ptr, PxU32(u.leafCount));
++ PX_ASSERT(q.isLeaf()); // remap is expected to set the isLeaf bit
++ }
++ else
++ {
++ // verify that all children bounds are included in the parent bounds
++ for(PxU32 s = 0; s < RTREE_N; s++)
++ {
++ const RTreeNodeNQ& child = resultTree[u.childPageFirstNodeIndex+s];
++ PX_UNUSED(child);
++ // is a sentinel node or is inside parent's bounds
++ PX_ASSERT(child.leafCount == -1 || child.bounds.isInside(u.bounds));
++ }
++
++ q.ptr = PxU32(u.childPageFirstNodeIndex * nodePtrMultiplier);
++ PX_ASSERT(q.ptr % RTREE_N == 0);
++ q.setLeaf(false);
++ }
++ }
++ qtreeNodes.pushBack(q);
++ }
++
++ // build the final rtree image
++ result.mInvDiagonal = PxVec4(1.0f);
++ PX_ASSERT(qtreeNodes.size() % RTREE_N == 0);
++ result.mTotalNodes = qtreeNodes.size();
++ result.mTotalPages = result.mTotalNodes / pageSize;
++ result.mPages = static_cast<RTreePage*>(
++ PxAlignedAllocator<128>().allocate(sizeof(RTreePage)*result.mTotalPages, PX_FL));
++ result.mBoundsMin = PxVec4(V3ReadXYZ(treeBounds.mn), 0.0f);
++ result.mBoundsMax = PxVec4(V3ReadXYZ(treeBounds.mx), 0.0f);
++ result.mDiagonalScaler = (result.mBoundsMax - result.mBoundsMin) / 65535.0f;
++ result.mPageSize = pageSize;
++ result.mNumLevels = maxLevels;
++ PX_ASSERT(result.mTotalNodes % pageSize == 0);
++ result.mNumRootPages = 1;
++
++ for(PxU32 j = 0; j < result.mTotalPages; j++)
++ {
++ RTreePage& page = result.mPages[j];
++ for(PxU32 k = 0; k < RTREE_N; k ++)
++ {
++ const RTreeNodeQ& n = qtreeNodes[j*RTREE_N+k];
++ page.maxx[k] = n.maxx;
++ page.maxy[k] = n.maxy;
++ page.maxz[k] = n.maxz;
++ page.minx[k] = n.minx;
++ page.miny[k] = n.miny;
++ page.minz[k] = n.minz;
++ page.ptrs[k] = n.ptr;
++ }
++ }
++
++ //printf("Tree size=%d\n", result.mTotalPages*sizeof(RTreePage));
++ #if PX_DEBUG
++ result.validate(); // make sure the child bounds are included in the parent and other validation
++ #endif
++ }
++
++ } // namespace physx