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
++
++ #ifndef GU_SDF_CONTACT_REDUCTION_H
++ #define GU_SDF_CONTACT_REDUCTION_H
++
++ #include "foundation/PxArray.h"
++ #include "foundation/PxAssert.h"
++ #include "foundation/PxErrors.h"
++ #include "foundation/PxFoundation.h"
++ #include "foundation/PxPreprocessor.h"
++ #include "foundation/PxSort.h"
++ #include "foundation/PxSimpleTypes.h"
++ #include "foundation/PxVec3.h"
++ #include "geomutils/PxContactBuffer.h"
++ #include "geomutils/PxContactPoint.h"
++ #include "PxContact.h"
++
++ namespace physx
++ {
++ namespace Gu
++ {
++
++ const float PATCH_ACCEPT_DOTP = 0.9995f;
++ const float PATCH_REJECT_DOTP = 0.85f;
++ const float PXS_SEPARATION_TOLERANCE = 0.001f;
++
++ // SDF contact reduction on the CPU takes place in three steps:
++ // (1) assign a contact to an existing patch or create a new one
++ // (2) once the patches grow too large, reduce them
++ // (3) order patches by penetration depth and select a diverse set
++ struct TinyContact
++ {
++ TinyContact() : mNormal(0.0f), mSeparation(0.0f), mPoint(0.0f) { }
++ TinyContact(const PxVec3& normal, PxReal separation, const PxVec3& point) : mNormal(normal), mSeparation(separation), mPoint(point) {}
++ PxVec3 mNormal;
++ PxReal mSeparation;
++ PxVec3 mPoint;
++ };
++
++ template <PxU8 TMaxContactsPerPatch>
++ struct TinyContactPatch
++ {
++ TinyContact mContacts[TMaxContactsPerPatch];
++ PxU8 mNbContacts;
++ const TinyContact* begin() const { return mContacts; }
++ const TinyContact* end() const { return mContacts + mNbContacts; }
++
++ TinyContactPatch() : mNbContacts(0) { }
++ };
++
++ PX_FORCE_INLINE static float square(float x) { return x * x; }
++
++ // proto-patch that may need to be reduced
++ template <PxU8 TMaxContactsPerPatch, PxU32 TContactBufSz, bool planarize = true>
++ class BufferedPatch
++ {
++ public:
++ BufferedPatch() {}
++ explicit BufferedPatch(const TinyContact& contact) : mRootNormal(contact.mNormal), mMinSeparation(contact.mSeparation)
++ {
++ addContact(contact);
++ }
++
++ // copy, skipping unused elements
++ BufferedPatch(const BufferedPatch& other) : mRootNormal(other.mRootNormal), mMinSeparation(other.mMinSeparation), mNbContacts(other.mNbContacts)
++ {
++ for (PxU32 i = 0; i < mNbContacts; ++i)
++ {
++ mPointX[i] = other.mPointX[i];
++ mPointY[i] = other.mPointY[i];
++ mPointZ[i] = other.mPointZ[i];
++
++ mNormalX[i] = other.mNormalX[i];
++ mNormalY[i] = other.mNormalY[i];
++ mNormalZ[i] = other.mNormalZ[i];
++
++ mSeparation[i] = other.mSeparation[i];
++ }
++ }
++
++ // add a contact
++ PX_INLINE void addContact(const TinyContact& contact)
++ {
++ PX_ASSERT(mNbContacts < TContactBufSz);
++ mPointX[mNbContacts] = contact.mPoint.x;
++ mPointY[mNbContacts] = contact.mPoint.y;
++ mPointZ[mNbContacts] = contact.mPoint.z;
++
++ mNormalX[mNbContacts] = contact.mNormal.x;
++ mNormalY[mNbContacts] = contact.mNormal.y;
++ mNormalZ[mNbContacts] = contact.mNormal.z;
++
++ mSeparation[mNbContacts] = contact.mSeparation;
++
++ // in case of <= 4 contacts per patch, the deepest one will not usually be conserved
++ if (contact.mSeparation < mMinSeparation)
++ mMinSeparation = contact.mSeparation;
++
++ ++mNbContacts;
++ if (mNbContacts == TContactBufSz)
++ reduce();
++ }
++
++ PX_INLINE void asTinyContactPatch(TinyContactPatch<TMaxContactsPerPatch>& patch)
++ {
++ reduce();
++ patch.mNbContacts = static_cast<PxU8>(mNbContacts);
++
++ PX_ASSERT(mNbContacts <= TMaxContactsPerPatch);
++ TinyContact* contacts = patch.mContacts;
++ for (PxU32 contactIdx = 0; contactIdx < mNbContacts; ++contactIdx, ++contacts)
++ {
++ contacts->mPoint = PxVec3(mPointX[contactIdx], mPointY[contactIdx], mPointZ[contactIdx]);
++ contacts->mNormal = PxVec3(mNormalX[contactIdx], mNormalY[contactIdx], mNormalZ[contactIdx]);
++ contacts->mSeparation = mSeparation[contactIdx];
++ }
++ }
++
++ // store reduced contacts into `contacts`, returning the number of contacts
++ PX_INLINE PxU32 getContacts(TinyContact* contacts)
++ {
++ reduce();
++ PX_ASSERT(mNbContacts <= TMaxContactsPerPatch);
++ for (PxU32 contactIdx = 0; contactIdx < mNbContacts; ++contactIdx, ++contacts)
++ {
++ contacts->mPoint = PxVec3(mPointX[contactIdx], mPointY[contactIdx], mPointZ[contactIdx]);
++ contacts->mNormal = PxVec3(mNormalX[contactIdx], mNormalY[contactIdx], mNormalZ[contactIdx]);
++ contacts->mSeparation = mSeparation[contactIdx];
++ }
++ return mNbContacts;
++ }
++
++ PX_INLINE PxVec3 getPoint(PxU32 index) const
++ {
++ return PxVec3(mPointX[index], mPointY[index], mPointZ[index]);
++ }
++
++ // reduce the contacts to `TMaxContactsPerPatch`.
++ PX_INLINE void reduce()
++ {
++ if (mNbContacts <= TMaxContactsPerPatch)
++ return;
++ // P0: most extreme point
++ float maxDistOrigSq = -1;
++ PxU32 P0Idx = 0;
++ PxVec3 P0;
++ for (PxU32 i = 0; i < mNbContacts; ++i)
++ {
++ const PxVec3 p = getPoint(i);
++ float distOrigSq = p.magnitudeSquared();
++ if (planarize)
++ distOrigSq -= square(p.dot(mRootNormal));
++ if (distOrigSq > maxDistOrigSq)
++ {
++ maxDistOrigSq = distOrigSq;
++ P0Idx = i;
++ P0 = p;
++ }
++ }
++ // P1: most distant point from P0
++ float maxDistP0Sq = -1;
++ PxU32 P1Idx = 0;
++ PxVec3 P1;
++ for (PxU32 i = 0; i < mNbContacts; ++i)
++ {
++ const PxVec3 p = getPoint(i);
++ const PxVec3 v = p - P0;
++ float distP0Sq = v.magnitudeSquared();
++ if (planarize)
++ distP0Sq -= square(v.dot(mRootNormal));
++ if (distP0Sq > maxDistP0Sq)
++ {
++ maxDistP0Sq = distP0Sq;
++ P1Idx = i;
++ P1 = p;
++ }
++ }
++
++ if (P0Idx == P1Idx) // P0 == P1 => all points equal. keep only the first
++ {
++ // TODO(CA): account for differences in penetration? should not occur
++ mNbContacts = 1;
++ return;
++ }
++
++ PxVec3 P2, P3;
++ // P2 & P3: most distant from P0-P1 segment in both directions
++ const PxVec3 segNormal = (P0 - P1).cross(mRootNormal);
++ float maxDistPos = -PX_MAX_REAL, maxDistNeg = PX_MAX_REAL;
++ PxU32 P2Idx = PX_MAX_U32, P3Idx = PX_MAX_U32;
++ for (PxU32 i = 0; i < mNbContacts; ++i)
++ {
++ if (i == P0Idx || i == P1Idx) // ensure that we have contacts distinct from P0/P1
++ continue;
++ const PxVec3 p = getPoint(i);
++ const PxReal dist = (p - P0).dot(segNormal);
++ if (dist > maxDistPos)
++ {
++ maxDistPos = dist;
++ P2Idx = i;
++ P2 = p;
++ }
++ if (dist <= maxDistNeg)
++ {
++ maxDistNeg = dist;
++ P3Idx = i;
++ P3 = p;
++ }
++ }
++ // cluster the points
++ PxReal anchorSeparation[TMaxContactsPerPatch];
++ PxU32 anchorDeepestIdx[TMaxContactsPerPatch];
++ const PxU32 anchorIndices[4] = {P0Idx, P1Idx, P2Idx, P3Idx};
++ const PxVec3 anchorPoints[4] = {P0, P1, P2, P3};
++ for(PxU32 i = 0; i < 4; ++i)
++ {
++ const PxU32 index = anchorIndices[i];
++ anchorDeepestIdx[i] = index;
++ anchorSeparation[i] = mSeparation[index] - PXS_SEPARATION_TOLERANCE;
++ }
++ for (PxU32 i = 0; i < mNbContacts; ++i)
++ {
++ const PxVec3 p = getPoint(i);
++ PxReal dMin = PX_MAX_REAL;
++ PxU32 anchorIdx = 0;
++ for(PxU32 c = 0; c < 4; ++c) // assign to anchors
++ {
++ const PxReal dist = (anchorPoints[c] - p).magnitudeSquared();
++ if(dist < dMin)
++ {
++ dMin = dist;
++ anchorIdx = c;
++ }
++ }
++ if(mSeparation[i] < anchorSeparation[anchorIdx]) // pick deepest
++ {
++ anchorDeepestIdx[anchorIdx] = i;
++ anchorSeparation[anchorIdx] = mSeparation[i];
++ }
++ }
++ PxU32 chosenPoints[TMaxContactsPerPatch] =
++ {
++ anchorDeepestIdx[0], anchorDeepestIdx[1], anchorDeepestIdx[2], anchorDeepestIdx[3]
++ };
++
++ PxReal chosenSeparations[TMaxContactsPerPatch-4]; // only relevant for extra points
++ for (PxU32 i = 0; i < TMaxContactsPerPatch-4; ++i)
++ chosenSeparations[i] = PX_MAX_REAL;
++
++ for (PxU32 i = 0; i < mNbContacts; ++i)
++ {
++ bool alreadyChosen = false;
++ for (PxU32 j = 0; j < 4; ++j)
++ {
++ if (i == chosenPoints[j])
++ {
++ alreadyChosen = true;
++ break;
++ }
++ }
++ if(alreadyChosen)
++ continue;
++ PxReal sep = mSeparation[i];
++ for (PxU32 slotIdx = 4; slotIdx < TMaxContactsPerPatch; ++slotIdx)
++ {
++ if (sep < chosenSeparations[slotIdx-4])
++ {
++ // drop out largest contact
++ for (PxU32 k = TMaxContactsPerPatch-1; k > slotIdx; --k)
++ {
++ chosenSeparations[k-4] = chosenSeparations[k-1-4];
++ chosenPoints[k] = chosenPoints[k-1];
++ }
++ // assign to this slot
++ chosenSeparations[slotIdx-4] = sep;
++ chosenPoints[slotIdx] = i;
++ break;
++ }
++ }
++ }
++ float pointXNew[TMaxContactsPerPatch],
++ pointYNew[TMaxContactsPerPatch],
++ pointZNew[TMaxContactsPerPatch],
++ normalXNew[TMaxContactsPerPatch],
++ normalYNew[TMaxContactsPerPatch],
++ normalZNew[TMaxContactsPerPatch],
++ separationNew[TMaxContactsPerPatch];
++
++ for (PxU32 dst = 0; dst < TMaxContactsPerPatch; ++dst)
++ {
++ const PxU32 src = chosenPoints[dst];
++ pointXNew[dst] = mPointX[src];
++ pointYNew[dst] = mPointY[src];
++ pointZNew[dst] = mPointZ[src];
++
++ normalXNew[dst] = mNormalX[src];
++ normalYNew[dst] = mNormalY[src];
++ normalZNew[dst] = mNormalZ[src];
++
++ separationNew[dst] = mSeparation[src];
++ }
++ for (PxU32 i = 0; i < TMaxContactsPerPatch; ++i)
++ {
++ mPointX[i] = pointXNew[i];
++ mPointY[i] = pointYNew[i];
++ mPointZ[i] = pointZNew[i];
++
++ mNormalX[i] = normalXNew[i];
++ mNormalY[i] = normalYNew[i];
++ mNormalZ[i] = normalZNew[i];
++
++ mSeparation[i] = separationNew[i];
++ }
++ mNbContacts = TMaxContactsPerPatch;
++ }
++ protected:
++
++ float mNormalX[TContactBufSz];
++ float mNormalY[TContactBufSz];
++ float mNormalZ[TContactBufSz];
++
++ float mSeparation[TContactBufSz];
++
++ float mPointX[TContactBufSz];
++ float mPointY[TContactBufSz];
++ float mPointZ[TContactBufSz];
++
++ public:
++ PxVec3 mRootNormal;
++ PxReal mMinSeparation = PX_MAX_REAL;
++ protected:
++ PxU32 mNbContacts = 0;
++ PxU32 pad0;
++ PxU32 pad1;
++ PxU32 pad2;
++
++ PX_COMPILE_TIME_ASSERT((TContactBufSz * sizeof(float)) % 32 == 0);
++ PX_COMPILE_TIME_ASSERT(TContactBufSz > TMaxContactsPerPatch);
++ };
++
++ template <PxU8 TMaxContactsPerPatch, PxU32 TMaxPatches, PxU32 TPatchBufSz>
++ class SDFContactReduction
++ {
++ public:
++ using Patch = BufferedPatch<TMaxContactsPerPatch, TPatchBufSz>;
++ using TinyPatch = TinyContactPatch<TMaxContactsPerPatch>;
++
++ // attempt to add a contact to one of the patches, adding a new one if necessary
++ // return false if the contact was dropped
++ PX_INLINE bool addContact(const TinyContact& contact)
++ {
++ ++mNbContactsConsumed;
++ for (Patch& patch: mPatchesBuffer)
++ {
++ const PxReal dot = patch.mRootNormal.dot(contact.mNormal);
++ if (dot >= PATCH_ACCEPT_DOTP)
++ return patch.addContact(contact), true;
++ }
++ mPatchesBuffer.pushBack(Patch(contact));
++ // TODO(CA): make this more robust, taking into account the max number of surviving patches
++ if (mPatchesBuffer.size() > mPatchCountLimit)
++ cullPatches(1-(1-PATCH_REJECT_DOTP)/1);
++
++ return true;
++ }
++
++ struct PatchPenetrationPredicate
++ {
++ bool operator()(const Patch* a, const Patch* b) const { return a->mMinSeparation < b->mMinSeparation; }
++ };
++
++ // cull the existing patches
++ PX_INLINE void cullPatches(float rejectDotP)
++ {
++ PxArray<Patch*> patchesSorted;
++ patchesSorted.reserve(mPatchesBuffer.size());
++ for (Patch& patch: mPatchesBuffer)
++ patchesSorted.pushBack(&patch);
++
++ PxSort(patchesSorted.begin(), mPatchesBuffer.size(), PatchPenetrationPredicate());
++
++ // drop patches that have > nbAllowedNeighbors neighbors that were selected
++ // allowed neighbors = 0 seems to work best, still, 3 is worst, and >> 10 deactivates
++ // isotropy
++ const PxU32 nbAllowedNeighbors = 0;
++
++ PxArray<PxVec3> patchNormals;
++ // Geometrical upper bound for the number of patches based on the area "reserved" for each patch
++ const PxU32 nbPatchesBound = static_cast<PxU32>(PxFloor(1.0f/square(PxSin(0.25f * PxAcos(rejectDotP)))));
++ patchNormals.reserve(PxMin(mPatchesBuffer.size(), nbPatchesBound));
++
++ mPatchesBufferTmp.clear();
++ for (Patch* patch: patchesSorted)
++ {
++ PxU32 neighborPatches = 0;
++ for (const PxVec3& rootNormal: patchNormals)
++ if (rootNormal.dot(patch->mRootNormal) > rejectDotP)
++ if (++neighborPatches > nbAllowedNeighbors)
++ break;
++ if (neighborPatches > nbAllowedNeighbors)
++ continue;
++
++ // patch->reduce();
++ patchNormals.pushBack(patch->mRootNormal);
++ mPatchesBufferTmp.pushBack(*patch);
++ }
++ PxSwap(mPatchesBuffer, mPatchesBufferTmp);
++ mPatchesBufferTmp.clear();
++ }
++
++ // reduce each buffered patch, sort them, and create contacts
++ PX_INLINE void flushContacts()
++ {
++ cullPatches(PATCH_REJECT_DOTP);
++ for (Patch& patch: mPatchesBuffer)
++ {
++ TinyContactPatch<TMaxContactsPerPatch>& finalPatch = mPatchesFinal.pushBack({});
++ patch.getContacts(finalPatch.mContacts);
++ finalPatch.mNbContacts = patch.mNbContacts;
++ }
++ }
++
++ PX_INLINE PxU32 flushToContactBuffer(PxContactBuffer& contactBuffer)
++ {
++ cullPatches(PATCH_REJECT_DOTP);
++ PxU32 nbContactsKept = 0;
++ for (Patch& patch: mPatchesBuffer)
++ {
++ TinyPatch finalPatch;
++ patch.asTinyContactPatch(finalPatch);
++ for (PxU32 i = 0; i < finalPatch.mNbContacts; ++i)
++ {
++ const TinyContact& contact = finalPatch.mContacts[i];
++ PxContactPoint* c = contactBuffer.contact();
++ if (c == NULL)
++ {
++ #if PX_CHECKED
++ PxGetFoundation().error(
++ physx::PxErrorCode::eDEBUG_WARNING,
++ PX_FL,
++ "Dropping contacts in contact reduction due to full contact buffer.");
++ #endif
++ break;
++ }
++ ++nbContactsKept;
++ c->normal = contact.mNormal;
++ c->point = contact.mPoint;
++ c->separation = contact.mSeparation;
++ c->internalFaceIndex1 = PXC_CONTACT_NO_FACE_INDEX;
++ }
++ }
++
++ return nbContactsKept;
++ }
++
++ protected:
++ static const PxU32 mPatchCountLimit = 4000000/sizeof(Patch); // 4 MB limit on patch size
++ PxArray<Patch> mPatchesBuffer; // store patches up to MaxPatches
++ PxArray<Patch> mPatchesBufferTmp;
++ PxArray<TinyPatch> mPatchesFinal; // store culled patches
++
++ PxU32 mNbContactsConsumed = 0;
++ };
++
++ }
++ }
++
++ #endif