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_PCM_CONTACT_CONVEX_COMMON_H
++ #define GU_PCM_CONTACT_CONVEX_COMMON_H
++
++ #define PCM_MAX_CONTACTPATCH_SIZE 32
++
++ #include "geomutils/PxContactBuffer.h"
++ #include "GuVecCapsule.h"
++ #include "GuPCMTriangleContactGen.h"
++ #include "GuTriangleCache.h"
++ #include "foundation/PxInlineArray.h"
++
++ namespace physx
++ {
++ namespace Gu
++ {
++
++ #define MAX_CACHE_SIZE 128
++
++ //sizeof(PCMDeferredPolyData)/sizeof(PxU32) = 15, 960/15 = 64 triangles in the local array
++ #define LOCAL_PCM_CONTACTS_SIZE 960
++
++ class PCMMeshContactGeneration
++ {
++ PX_NOCOPY(PCMMeshContactGeneration)
++ public:
++ PCMContactPatch mContactPatch[PCM_MAX_CONTACTPATCH_SIZE];
++ PCMContactPatch* mContactPatchPtr[PCM_MAX_CONTACTPATCH_SIZE];
++ const aos::FloatV mContactDist;
++ const aos::FloatV mReplaceBreakingThreshold;
++ const aos::PxTransformV& mConvexTransform;
++ const aos::PxTransformV& mMeshTransform;
++ Gu::MultiplePersistentContactManifold& mMultiManifold;
++ PxContactBuffer& mContactBuffer;
++ aos::FloatV mAcceptanceEpsilon;
++ aos::FloatV mSqReplaceBreakingThreshold;
++ aos::PxMatTransformV mMeshToConvex;
++ Gu::MeshPersistentContact* mManifoldContacts;
++ PxU32 mNumContacts;
++ PxU32 mNumContactPatch;
++ PxU32 mNumCalls;
++ Gu::CacheMap<Gu::CachedEdge, MAX_CACHE_SIZE> mEdgeCache;
++ Gu::CacheMap<Gu::CachedVertex, MAX_CACHE_SIZE> mVertexCache;
++ PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE>* mDeferredContacts;
++ PxRenderOutput* mRenderOutput;
++
++ PCMMeshContactGeneration(
++ const aos::FloatVArg contactDist,
++ const aos::FloatVArg replaceBreakingThreshold,
++ const aos::PxTransformV& convexTransform,
++ const aos::PxTransformV& meshTransform,
++ Gu::MultiplePersistentContactManifold& multiManifold,
++ PxContactBuffer& contactBuffer,
++ PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE>* deferredContacts,
++ PxRenderOutput* renderOutput
++ ) :
++ mContactDist(contactDist),
++ mReplaceBreakingThreshold(replaceBreakingThreshold),
++ mConvexTransform(convexTransform),
++ mMeshTransform(meshTransform),
++ mMultiManifold(multiManifold),
++ mContactBuffer(contactBuffer),
++ mDeferredContacts(deferredContacts),
++ mRenderOutput(renderOutput)
++ {
++ using namespace aos;
++ mNumContactPatch = 0;
++ mNumContacts = 0;
++ mNumCalls = 0;
++
++ mMeshToConvex = mConvexTransform.transformInv(mMeshTransform);
++
++ //Assign the PCMContactPatch to the PCMContactPathPtr
++ for(PxU32 i=0; i<PCM_MAX_CONTACTPATCH_SIZE; ++i)
++ {
++ mContactPatchPtr[i] = &mContactPatch[i];
++ }
++ mManifoldContacts = PX_CP_TO_MPCP(contactBuffer.contacts);
++
++ mSqReplaceBreakingThreshold = FMul(replaceBreakingThreshold, replaceBreakingThreshold);
++
++ mAcceptanceEpsilon = FLoad(0.996f);//5 degree
++ //mAcceptanceEpsilon = FloatV_From_F32(0.9999);//5 degree
++ }
++
++ template <PxU32 TriangleCount, typename Derived>
++ bool processTriangleCache(Gu::TriangleCache<TriangleCount>& cache)
++ {
++ PxU32 count = cache.mNumTriangles;
++ PxVec3* verts = cache.mVertices;
++ PxU32* vertInds = cache.mIndices;
++ PxU32* triInds = cache.mTriangleIndex;
++ PxU8* edgeFlags = cache.mEdgeFlags;
++ while(count--)
++ {
++ (static_cast<Derived*>(this))->processTriangle(verts, *triInds, *edgeFlags, vertInds);
++ verts += 3;
++ vertInds += 3;
++ triInds++;
++ edgeFlags++;
++ }
++ return true;
++ }
++ void prioritizeContactPatches();
++ void addManifoldPointToPatch(const aos::Vec3VArg currentPatchNormal, const aos::FloatVArg maxPen, PxU32 previousNumContacts);
++ void processContacts(PxU8 maxContactPerManifold, const bool isNotLastPatch = true);
++ };
++
++ // This function is based on the current patch normal to either create a new patch or merge the manifold contacts in this patch with the manifold contacts in the last existing
++ // patch. This means there might be more than GU_SINGLE_MANIFOLD_CACHE_SIZE in a SinglePersistentContactManifold.
++ PX_FORCE_INLINE void PCMMeshContactGeneration::addManifoldPointToPatch(const aos::Vec3VArg currentPatchNormal, const aos::FloatVArg maxPen, PxU32 previousNumContacts)
++ {
++ using namespace aos;
++
++ bool foundPatch = false;
++ //we have existing patch
++ if(mNumContactPatch > 0)
++ {
++ //if the direction between the last existing patch normal and the current patch normal are within acceptance epsilon, which means we will be
++ //able to merge the last patch's contacts with the current patch's contacts. This is just to avoid to create an extra patch. We have some logic
++ //later to refine the patch again
++ if(FAllGrtr(V3Dot(mContactPatch[mNumContactPatch-1].mPatchNormal, currentPatchNormal), mAcceptanceEpsilon))
++ {
++ //get the last patch
++ PCMContactPatch& patch = mContactPatch[mNumContactPatch-1];
++
++ //remove duplicate contacts
++ for(PxU32 i = patch.mStartIndex; i<patch.mEndIndex; ++i)
++ {
++ for(PxU32 j = previousNumContacts; j<mNumContacts; ++j)
++ {
++ Vec3V dif = V3Sub(mManifoldContacts[j].mLocalPointB, mManifoldContacts[i].mLocalPointB);
++ FloatV d = V3Dot(dif, dif);
++ if(FAllGrtr(mSqReplaceBreakingThreshold, d))
++ {
++ if(FAllGrtr(V4GetW(mManifoldContacts[i].mLocalNormalPen), V4GetW(mManifoldContacts[j].mLocalNormalPen)))
++ {
++ //The new contact is deeper than the old contact so we keep the deeper contact
++ mManifoldContacts[i] = mManifoldContacts[j];
++ }
++ mManifoldContacts[j] = mManifoldContacts[mNumContacts-1];
++ mNumContacts--;
++ j--;
++ }
++ }
++ }
++ patch.mEndIndex = mNumContacts;
++ patch.mPatchMaxPen = FMin(patch.mPatchMaxPen, maxPen);
++ foundPatch = true;
++ }
++ }
++
++ //If there are no existing patch which match the currentPatchNormal, we will create a new patch
++ if(!foundPatch)
++ {
++ mContactPatch[mNumContactPatch].mStartIndex = previousNumContacts;
++ mContactPatch[mNumContactPatch].mEndIndex = mNumContacts;
++ mContactPatch[mNumContactPatch].mPatchMaxPen = maxPen;
++ mContactPatch[mNumContactPatch++].mPatchNormal = currentPatchNormal;
++ }
++ }
++
++ // This function sort the contact patch based on the max penetration so that deepest penetration contact patch will be in front of the less penetration contact
++ // patch
++ PX_FORCE_INLINE void PCMMeshContactGeneration::prioritizeContactPatches()
++ {
++ //we are using insertion sort to prioritize contact patchs
++ using namespace aos;
++ //sort the contact patch based on the max penetration
++ for(PxU32 i=1; i<mNumContactPatch; ++i)
++ {
++ const PxU32 indexi = i-1;
++ if(FAllGrtr(mContactPatchPtr[indexi]->mPatchMaxPen, mContactPatchPtr[i]->mPatchMaxPen))
++ {
++ //swap
++ PCMContactPatch* tmp = mContactPatchPtr[indexi];
++ mContactPatchPtr[indexi] = mContactPatchPtr[i];
++ mContactPatchPtr[i] = tmp;
++
++ for(PxI32 j=PxI32(i-2); j>=0; j--)
++ {
++ const PxU32 indexj = PxU32(j+1);
++ if(FAllGrtrOrEq(mContactPatchPtr[indexj]->mPatchMaxPen, mContactPatchPtr[j]->mPatchMaxPen))
++ break;
++ //swap
++ PCMContactPatch* temp = mContactPatchPtr[indexj];
++ mContactPatchPtr[indexj] = mContactPatchPtr[j];
++ mContactPatchPtr[j] = temp;
++ }
++ }
++ }
++ }
++
++ PX_FORCE_INLINE void PCMMeshContactGeneration::processContacts(PxU8 maxContactPerManifold, bool isNotLastPatch)
++ {
++ using namespace aos;
++
++ if(mNumContacts != 0)
++ {
++ //reorder the contact patches based on the max penetration
++ prioritizeContactPatches();
++ //connect the patches which's angle between patch normals are within 5 degree
++ mMultiManifold.refineContactPatchConnective(mContactPatchPtr, mNumContactPatch, mManifoldContacts, mAcceptanceEpsilon);
++ //get rid of duplicate manifold contacts in connected contact patches
++ mMultiManifold.reduceManifoldContactsInDifferentPatches(mContactPatchPtr, mNumContactPatch, mManifoldContacts, mNumContacts, mSqReplaceBreakingThreshold);
++ //add the manifold contact to the corresponding manifold
++ mMultiManifold.addManifoldContactPoints(mManifoldContacts, mNumContacts, mContactPatchPtr, mNumContactPatch, mSqReplaceBreakingThreshold, mAcceptanceEpsilon, maxContactPerManifold);
++
++ mNumContacts = 0;
++ mNumContactPatch = 0;
++
++ if(isNotLastPatch)
++ {
++ //remap the contact patch pointer to contact patch
++ for(PxU32 i=0; i<PCM_MAX_CONTACTPATCH_SIZE; ++i)
++ {
++ mContactPatchPtr[i] = &mContactPatch[i];
++ }
++ }
++ }
++ }
++
++ struct PCMDeferredPolyData
++ {
++ public:
++ PxVec3 mVerts[3]; //36
++ PxU32 mInds[3]; //48
++ PxU32 mTriangleIndex; //52
++ PxU32 mFeatureIndex; //56
++ PxU32 triFlags32; //60
++ };
++
++ #if PX_VC
++ #pragma warning(push)
++ #pragma warning( disable : 4324 ) // Padding was added at the end of a structure because of a __declspec(align) value.
++ #endif
++
++ class PCMConvexVsMeshContactGeneration : public PCMMeshContactGeneration
++ {
++ PCMConvexVsMeshContactGeneration &operator=(PCMConvexVsMeshContactGeneration &);
++
++ public:
++ aos::Vec3V mHullCenterMesh;
++ const Gu::PolygonalData& mPolyData;
++ const SupportLocal* mPolyMap;
++ const Cm::FastVertex2ShapeScaling& mConvexScaling;
++ bool mIdtConvexScale;
++ bool mSilhouetteEdgesAreActive;
++
++ PCMConvexVsMeshContactGeneration(
++ const aos::FloatVArg contactDistance,
++ const aos::FloatVArg replaceBreakingThreshold,
++ const aos::PxTransformV& convexTransform,
++ const aos::PxTransformV& meshTransform,
++ Gu::MultiplePersistentContactManifold& multiManifold,
++ PxContactBuffer& contactBuffer,
++ const Gu::PolygonalData& polyData,
++ const SupportLocal* polyMap,
++ PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE>* delayedContacts,
++ const Cm::FastVertex2ShapeScaling& convexScaling,
++ bool idtConvexScale,
++ bool silhouetteEdgesAreActive,
++ PxRenderOutput* renderOutput
++
++ ) : PCMMeshContactGeneration(contactDistance, replaceBreakingThreshold, convexTransform, meshTransform, multiManifold, contactBuffer,
++ delayedContacts, renderOutput),
++ mPolyData(polyData),
++ mPolyMap(polyMap),
++ mConvexScaling(convexScaling),
++ mIdtConvexScale(idtConvexScale),
++ mSilhouetteEdgesAreActive(silhouetteEdgesAreActive)
++ {
++ using namespace aos;
++
++ // Hull center in local space
++ const Vec3V hullCenterLocal = V3LoadU(mPolyData.mCenter);
++ // Hull center in mesh space
++ mHullCenterMesh = mMeshToConvex.transformInv(hullCenterLocal);
++ }
++
++ bool generateTriangleFullContactManifold(const Gu::TriangleV& localTriangle, PxU32 triangleIndex, const PxU32* triIndices, PxU8 triFlags, const Gu::PolygonalData& polyData, const Gu::SupportLocalImpl<Gu::TriangleV>* localTriMap, const Gu::SupportLocal* polyMap, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts,
++ const aos::FloatVArg contactDist, aos::Vec3V& patchNormal);
++
++ bool generatePolyDataContactManifold(const Gu::TriangleV& localTriangle, PxU32 featureIndex, PxU32 triangleIndex, PxU8 triFlags, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts, const aos::FloatVArg contactDist, aos::Vec3V& patchNormal);
++ void generateLastContacts();
++ void addContactsToPatch(const aos::Vec3VArg patchNormal, PxU32 previousNumContacts);
++
++ bool processTriangle(const PxVec3* verts, PxU32 triangleIndex, PxU8 triFlags, const PxU32* vertInds);
++
++ static bool generateTriangleFullContactManifold(const Gu::TriangleV& localTriangle, PxU32 triangleIndex, PxU8 triFlags, const Gu::PolygonalData& polyData, const Gu::SupportLocalImpl<Gu::TriangleV>* localTriMap, const Gu::SupportLocal* polyMap, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts,
++ const aos::FloatVArg contactDist, aos::Vec3V& patchNormal, PxRenderOutput* renderOutput = NULL);
++
++ static bool processTriangle(const Gu::PolygonalData& polyData, const SupportLocal* polyMap, const PxVec3* verts, PxU32 triangleIndex, PxU8 triFlags, const aos::FloatVArg inflation, bool isDoubleSided,
++ const aos::PxTransformV& convexTransform, const aos::PxMatTransformV& meshToConvex, Gu::MeshPersistentContact* manifoldContact, PxU32& numContacts);
++ };
++
++ #if PX_VC
++ #pragma warning(pop)
++ #endif
++
++ struct SortedTriangle
++ {
++ aos::FloatV mSquareDist;
++ PxU32 mIndex;
++
++ PX_FORCE_INLINE bool operator < (const SortedTriangle& data) const
++ {
++ return aos::FAllGrtrOrEq(mSquareDist, data.mSquareDist) ==0;
++ }
++ };
++
++ class PCMSphereVsMeshContactGeneration : public PCMMeshContactGeneration
++ {
++ public:
++ aos::Vec3V mSphereCenter;
++ aos::FloatV mSphereRadius;
++ aos::FloatV mSqInflatedSphereRadius;
++ PxInlineArray<SortedTriangle, 64> mSortedTriangle;
++
++ PCMSphereVsMeshContactGeneration(
++ const aos::Vec3VArg sphereCenter,
++ const aos::FloatVArg sphereRadius,
++ const aos::FloatVArg contactDist,
++ const aos::FloatVArg replaceBreakingThreshold,
++ const aos::PxTransformV& sphereTransform,
++ const aos::PxTransformV& meshTransform,
++ MultiplePersistentContactManifold& multiManifold,
++ PxContactBuffer& contactBuffer,
++ PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE>* deferredContacts,
++ PxRenderOutput* renderOutput = NULL
++
++ ) : PCMMeshContactGeneration(contactDist, replaceBreakingThreshold, sphereTransform, meshTransform, multiManifold,
++ contactBuffer, deferredContacts, renderOutput),
++ mSphereCenter(sphereCenter),
++ mSphereRadius(sphereRadius)
++ {
++ using namespace aos;
++ const FloatV inflatedSphereRadius = FAdd(sphereRadius, contactDist);
++ mSqInflatedSphereRadius = FMul(inflatedSphereRadius, inflatedSphereRadius);
++ }
++
++ bool processTriangle(const PxVec3* verts, PxU32 triangleIndex, PxU8 triFlags, const PxU32* vertInds);
++ void generateLastContacts();
++ void addToPatch(const aos::Vec3VArg contactP, const aos::Vec3VArg patchNormal, const aos::FloatV pen, PxU32 triangleIndex);
++ };
++
++ class PCMCapsuleVsMeshContactGeneration : public PCMMeshContactGeneration
++ {
++ PCMCapsuleVsMeshContactGeneration &operator=(PCMCapsuleVsMeshContactGeneration &);
++ public:
++ aos::FloatV mInflatedRadius;
++ aos::FloatV mSqInflatedRadius;
++ const CapsuleV& mCapsule;
++
++ PCMCapsuleVsMeshContactGeneration(
++ const CapsuleV& capsule,
++ const aos::FloatVArg contactDist,
++ const aos::FloatVArg replaceBreakingThreshold,
++ const aos::PxTransformV& sphereTransform,
++ const aos::PxTransformV& meshTransform,
++ Gu::MultiplePersistentContactManifold& multiManifold,
++ PxContactBuffer& contactBuffer,
++ PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE>* deferredContacts,
++ PxRenderOutput* renderOutput = NULL
++
++ ) : PCMMeshContactGeneration(contactDist, replaceBreakingThreshold, sphereTransform, meshTransform, multiManifold, contactBuffer,
++ deferredContacts, renderOutput),
++ mCapsule(capsule)
++ {
++ using namespace aos;
++ mInflatedRadius = FAdd(capsule.radius, contactDist);
++ mSqInflatedRadius = FMul(mInflatedRadius, mInflatedRadius);
++ }
++
++ void generateEEContacts(const aos::Vec3VArg a, const aos::Vec3VArg b,const aos::Vec3VArg c, const aos::Vec3VArg normal, PxU32 triangleIndex,
++ const aos::Vec3VArg p, const aos::Vec3VArg q, const aos::FloatVArg sqInflatedRadius, PxU32 previousNumContacts, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts);
++
++ void generateEE(const aos::Vec3VArg p, const aos::Vec3VArg q, const aos::FloatVArg sqInflatedRadius, const aos::Vec3VArg normal, PxU32 triangleIndex,
++ const aos::Vec3VArg a, const aos::Vec3VArg b, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts);
++
++ static void generateContacts(const aos::Vec3VArg a, const aos::Vec3VArg b,const aos::Vec3VArg c, const aos::Vec3VArg planeNormal, const aos::Vec3VArg normal,
++ PxU32 triangleIndex, const aos::Vec3VArg p, const aos::Vec3VArg q, const aos::FloatVArg inflatedRadius, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts);
++
++ static void generateEEContactsMTD(const aos::Vec3VArg a, const aos::Vec3VArg b,const aos::Vec3VArg c, const aos::Vec3VArg normal, PxU32 triangleIndex,
++ const aos::Vec3VArg p, const aos::Vec3VArg q, const aos::FloatVArg inflatedRadius, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts);
++
++ static void generateEEMTD(const aos::Vec3VArg p, const aos::Vec3VArg q, const aos::FloatVArg inflatedRadius, const aos::Vec3VArg normal, PxU32 triangleIndex,
++ const aos::Vec3VArg a, const aos::Vec3VArg b, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts);
++
++ bool processTriangle(const PxVec3* verts, PxU32 triangleIndex, PxU8 triFlags, const PxU32* vertInds);
++
++ static bool processTriangle(const TriangleV& triangle, PxU32 triangleIndex, const CapsuleV& capsule, const aos::FloatVArg inflatedRadius, const PxU8 triFlag, Gu::MeshPersistentContact* manifoldContacts, PxU32& numContacts);
++ };
++
++ }
++ }
++
++ #endif