495 lines
15 KiB
C++
495 lines
15 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.
|
|
|
|
#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
|