feat(physics): wire physx sdk into build
This commit is contained in:
494
engine/third_party/physx/source/geomutils/src/contact/GuContactReduction.h
vendored
Normal file
494
engine/third_party/physx/source/geomutils/src/contact/GuContactReduction.h
vendored
Normal file
@@ -0,0 +1,494 @@
|
||||
// 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
|
||||
Reference in New Issue
Block a user