qt/src/3rdparty/PhysX/source/lowlevel/common/src/pipeline/PxcNpContactPrepShared.cpp

Revision control

- // 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-2026 NVIDIA Corporation. All rights reserved.
+ // Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
- // Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
+ // SPDX-FileCopyrightText: Copyright (c) 2008-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ // SPDX-License-Identifier: Apache-2.0
#include "foundation/PxAlloca.h"
#include "foundation/PxAtomic.h"
#include "foundation/PxErrors.h"
#include "foundation/PxPreprocessor.h"
#include "foundation/PxVecMath.h"
#include "geomutils/PxContactPoint.h"
#include "PxcNpContactPrepShared.h"
#include "PxcNpThreadContext.h"
#include "PxsMaterialManager.h"
#include "PxsMaterialCombiner.h"
#include "PxcNpContactPrepShared.h"
using namespace physx;
using namespace Gu;
using namespace aos;
static PX_FORCE_INLINE void copyContactPoint(PxContact* PX_RESTRICT point, const PxContactPoint* PX_RESTRICT cp)
{
// PT: TODO: consider moving "separation" right after "point" in both structures, to copy both at the same time.
// point->contact = cp->point;
const Vec4V contactV = V4LoadA(&cp->point.x); // PT: V4LoadA safe because 'point' is aligned.
V4StoreU(contactV, &point->contact.x);
point->separation = cp->separation;
}
void combineMaterials(const PxsMaterialManager* materialManager, PxU16 origMatIndex0, PxU16 origMatIndex1, PxReal& staticFriction, PxReal& dynamicFriction, PxReal& combinedRestitution, PxU32& materialFlags, PxReal& combinedDamping)
{
const PxsMaterialData& data0 = *materialManager->getMaterial(origMatIndex0);
const PxsMaterialData& data1 = *materialManager->getMaterial(origMatIndex1);
PxsCombineMaterials(data0, data1, staticFriction, dynamicFriction, combinedRestitution, materialFlags, combinedDamping);
}
struct StridePatch
{
PxU8 startIndex;
PxU8 endIndex;
PxU8 nextIndex;
PxU8 totalCount;
bool isRoot;
};
PxU32 physx::writeCompressedContact(const PxContactPoint* const PX_RESTRICT contactPoints, const PxU32 numContactPoints, PxcNpThreadContext* threadContext,
PxU16& writtenContactCount, PxU8*& outContactPatches, PxU8*& outContactPoints, PxU16& compressedContactSize, PxReal*& outContactForces, PxU32 contactForceByteSize,
PxU8*& outFrictionPatches, PxcDataStreamPool* frictionPatchesStreamPool,
const PxsMaterialManager* materialManager, bool hasModifiableContacts, bool forceNoResponse, const PxsMaterialInfo* PX_RESTRICT pMaterial, PxU8& numPatches,
PxU32 additionalHeaderSize, PxsConstraintBlockManager* manager, PxcConstraintBlockStream* blockStream, bool insertAveragePoint,
PxcDataStreamPool* contactStreamPool, PxcDataStreamPool* patchStreamPool, PxcDataStreamPool* forceStreamPool, const bool isMeshType)
{
if(numContactPoints == 0)
{
writtenContactCount = 0;
outContactPatches = NULL;
outContactPoints = NULL;
outContactForces = NULL;
compressedContactSize = 0;
numPatches = 0;
outFrictionPatches = NULL;
return 0;
}
//Calculate the size of the contact buffer...
PX_ALLOCA(strPatches, StridePatch, numContactPoints);
StridePatch* stridePatches = &strPatches[0];
PxU32 numStrideHeaders = 1;
PxU32 totalUniquePatches = 1;
PxU32 totalContactPoints = numContactPoints;
PxU32 strideStart = 0;
bool root = true;
StridePatch* parentRootPatch = NULL;
{
const PxReal closeNormalThresh = PXC_SAME_NORMAL;
//Go through and tag how many patches we have...
PxVec3 normal = contactPoints[0].normal;
PxU16 mat0 = pMaterial[0].mMaterialIndex0;
PxU16 mat1 = pMaterial[0].mMaterialIndex1;
for(PxU32 a = 1; a < numContactPoints; ++a)
{
if(normal.dot(contactPoints[a].normal) < closeNormalThresh ||
pMaterial[a].mMaterialIndex0 != mat0 || pMaterial[a].mMaterialIndex1 != mat1)
{
StridePatch& patch = stridePatches[numStrideHeaders-1];
patch.startIndex = PxU8(strideStart);
patch.endIndex = PxU8(a);
patch.nextIndex = 0xFF;
patch.totalCount = PxU8(a - strideStart);
patch.isRoot = root;
if(parentRootPatch)
parentRootPatch->totalCount += PxU8(a - strideStart);
root = true;
parentRootPatch = NULL;
for(PxU32 b = 1; b < numStrideHeaders; ++b)
{
StridePatch& thisPatch = stridePatches[b-1];
if(thisPatch.isRoot)
{
PxU32 ind = thisPatch.startIndex;
PxReal dp2 = contactPoints[a].normal.dot(contactPoints[ind].normal);
if(dp2 >= closeNormalThresh && pMaterial[a].mMaterialIndex0 == pMaterial[ind].mMaterialIndex0 &&
pMaterial[a].mMaterialIndex1 == pMaterial[ind].mMaterialIndex1)
{
PxU32 nextInd = b-1;
while(stridePatches[nextInd].nextIndex != 0xFF)
nextInd = stridePatches[nextInd].nextIndex;
stridePatches[nextInd].nextIndex = PxU8(numStrideHeaders);
root = false;
parentRootPatch = &stridePatches[b-1];
break;
}
}
}
normal = contactPoints[a].normal;
mat0 = pMaterial[a].mMaterialIndex0;
mat1 = pMaterial[a].mMaterialIndex1;
- totalContactPoints = insertAveragePoint && (a - strideStart) > 1 ? totalContactPoints + 1 : totalContactPoints;
strideStart = a;
numStrideHeaders++;
if(root)
totalUniquePatches++;
}
}
- totalContactPoints = insertAveragePoint &&(numContactPoints - strideStart) > 1 ? totalContactPoints + 1 : totalContactPoints;
- contactForceByteSize = insertAveragePoint && contactForceByteSize != 0 ? contactForceByteSize + sizeof(PxF32) * (totalContactPoints - numContactPoints) : contactForceByteSize;
}
{
StridePatch& patch = stridePatches[numStrideHeaders-1];
patch.startIndex = PxU8(strideStart);
patch.endIndex = PxU8(numContactPoints);
patch.nextIndex = 0xFF;
patch.totalCount = PxU8(numContactPoints - strideStart);
patch.isRoot = root;
if(parentRootPatch)
parentRootPatch->totalCount += PxU8(numContactPoints - strideStart);
}
+ // PT: the average points must be counted once the stride graph is complete, and per *root* patch - which is what
+ // the write loops below emit. Counting them per contiguous stride instead disagrees with the writers as soon as
+ // strides get merged into a root: a normal/material sequence like A,B,A reserves no average point but writes one
+ // (overrunning the contact & force buffers), while A,A,B,A,A reserves two but writes one (leaving a trailing
+ // contact that is counted but never written).
+ if(insertAveragePoint)
+ {
+ PxU32 nbAveragePoints = 0;
+ for(PxU32 a = 0; a < numStrideHeaders; a++)
+ {
+ const StridePatch& patch = stridePatches[a];
+ if(patch.isRoot && patch.totalCount > 1)
+ nbAveragePoints++;
+ }
+
+ totalContactPoints += nbAveragePoints;
+ if(contactForceByteSize)
+ contactForceByteSize += sizeof(PxF32) * nbAveragePoints;
+ }
+
numPatches = PxU8(totalUniquePatches);
//Calculate the number of patches/points required
const bool isModifiable = !forceNoResponse && hasModifiableContacts;
const PxU32 patchHeaderSize = sizeof(PxContactPatch) * (isModifiable ? totalContactPoints : totalUniquePatches) + additionalHeaderSize;
const PxU32 pointSize = totalContactPoints * (isModifiable ? sizeof(PxModifiableContact) : sizeof(PxContact));
const PxU32 requiredContactSize = pointSize;
const PxU32 requiredPatchSize = patchHeaderSize;
PxU32 totalRequiredSize;
PxU8* PX_RESTRICT contactData = NULL;
PxU8* PX_RESTRICT patchData = NULL;
PxReal* PX_RESTRICT forceData = NULL;
PxU32* PX_RESTRICT triangleIndice = NULL;
// Calculate friction data size
const PxU32 frictionPatchesSize = numPatches * sizeof(PxFrictionPatch);
PxU8* PX_RESTRICT frictionPatchesData = NULL;
if(contactStreamPool && !isModifiable && additionalHeaderSize == 0) //If the contacts are modifiable, we **DON'T** allocate them in GPU pinned memory. This will be handled later when they're modified
{
bool isOverflown = false;
PxU32 contactIndex = PxU32(PxAtomicAdd(&contactStreamPool->mSharedDataIndex, PxI32(requiredContactSize)));
if (contactStreamPool->isOverflown())
{
PX_WARN_ONCE("Contact buffer overflow detected, please increase its size in the scene desc!\n");
isOverflown = true;
}
contactData = contactStreamPool->mDataStream + contactStreamPool->mDataStreamSize - contactIndex;
const PxU32 patchIndex = PxU32(PxAtomicAdd(&patchStreamPool->mSharedDataIndex, PxI32(requiredPatchSize)));
if (patchStreamPool->isOverflown())
{
PX_WARN_ONCE("Patch buffer overflow detected, please increase its size in the scene desc!\n");
isOverflown = true;
}
patchData = patchStreamPool->mDataStream + patchStreamPool->mDataStreamSize - patchIndex;
PxU32 frictionPatchesIndex = PxTo32(PxAtomicAdd(&frictionPatchesStreamPool->mSharedDataIndex, PxI32(frictionPatchesSize)));
if (frictionPatchesStreamPool->isOverflown())
{
PX_WARN_ONCE("Friction patch buffer overflow detected, please increase its size in the scene desc!\n");
isOverflown = true;
}
frictionPatchesData = frictionPatchesStreamPool->mDataStream + frictionPatchesStreamPool->mDataStreamSize - frictionPatchesIndex;
if(contactForceByteSize)
{
contactForceByteSize = isMeshType ? contactForceByteSize * 2 : contactForceByteSize;
contactIndex = PxU32(PxAtomicAdd(&forceStreamPool->mSharedDataIndex, PxI32(contactForceByteSize)));
if (forceStreamPool->isOverflown())
{
PX_WARN_ONCE("Force buffer overflow detected, please increase its size in the scene desc!\n");
isOverflown = true;
}
forceData = reinterpret_cast<PxReal*>(forceStreamPool->mDataStream + forceStreamPool->mDataStreamSize - contactIndex);
if (isMeshType)
- triangleIndice = reinterpret_cast<PxU32*>(forceData + numContactPoints);
+ triangleIndice = reinterpret_cast<PxU32*>(forceData + totalContactPoints);
}
totalRequiredSize = requiredContactSize + requiredPatchSize;
if (isOverflown)
{
patchData = NULL;
contactData = NULL;
forceData = NULL;
triangleIndice = NULL;
}
}
else
{
const PxU32 alignedRequiredSize = computeAlignedSize(requiredContactSize + requiredPatchSize);
contactForceByteSize = (isMeshType ? contactForceByteSize * 2 : contactForceByteSize);
const PxU32 totalSize = alignedRequiredSize + contactForceByteSize + frictionPatchesSize;
PxU8* data = manager ? blockStream->reserve(totalSize, *manager) : threadContext->mContactBlockStream.reserve(totalSize);
if(data)
{
patchData = data;
contactData = data + requiredPatchSize;
if(contactForceByteSize)
{
forceData = reinterpret_cast<PxReal*>((data + alignedRequiredSize));
if (isMeshType)
- triangleIndice = reinterpret_cast<PxU32*>(forceData + numContactPoints);
+ triangleIndice = reinterpret_cast<PxU32*>(forceData + totalContactPoints);
PxMemZero(forceData, contactForceByteSize);
if (frictionPatchesSize)
{
frictionPatchesData = data + alignedRequiredSize + contactForceByteSize;
PxMemZero(frictionPatchesData, frictionPatchesSize);
}
}
}
totalRequiredSize = alignedRequiredSize;
}
if(patchData == NULL)
{
writtenContactCount = 0;
outContactPatches = NULL;
outContactPoints = NULL;
outContactForces = NULL;
compressedContactSize = 0;
numPatches = 0;
outFrictionPatches = NULL;
return 0;
}
PxPrefetchLine(patchData);
PxPrefetchLine(contactData);
#if PX_ENABLE_SIM_STATS
if(threadContext)
threadContext->mCompressedCacheSize += totalRequiredSize;
#else
PX_CATCH_UNDEFINED_ENABLE_SIM_STATS
#endif
compressedContactSize = PxTo16(totalRequiredSize);
//PxU32 startIndex = 0;
//Extract first material
PxU16 origMatIndex0 = pMaterial[0].mMaterialIndex0;
PxU16 origMatIndex1 = pMaterial[0].mMaterialIndex1;
PxReal staticFriction, dynamicFriction, combinedRestitution, combinedDamping;
PxU32 materialFlags;
combineMaterials(materialManager, origMatIndex0, origMatIndex1, staticFriction, dynamicFriction, combinedRestitution, materialFlags, combinedDamping);
PxU8* PX_RESTRICT dataPlusOffset = patchData + additionalHeaderSize;
PxContactPatch* PX_RESTRICT patches = reinterpret_cast<PxContactPatch*>(dataPlusOffset);
PxU32* PX_RESTRICT faceIndice = triangleIndice;
outContactPatches = patchData;
outContactPoints = contactData;
outContactForces = forceData;
outFrictionPatches = frictionPatchesData;
struct Local
{
static PX_FORCE_INLINE void fillPatch(PxContactPatch* PX_RESTRICT patch, const StridePatch& rootPatch, const PxVec3& normal,
PxU32 currentIndex, PxReal staticFriction_, PxReal dynamicFriction_, PxReal combinedRestitution_, PxReal combinedDamping_,
PxU32 materialFlags_, PxU32 flags, PxU16 matIndex0, PxU16 matIndex1
)
{
patch->mMassModification.linear0 = 1.0f;
patch->mMassModification.linear1 = 1.0f;
patch->mMassModification.angular0 = 1.0f;
patch->mMassModification.angular1 = 1.0f;
PX_ASSERT(PxAbs(normal.magnitude() - 1) < 1e-3f);
patch->normal = normal;
patch->restitution = combinedRestitution_;
patch->dynamicFriction = dynamicFriction_;
patch->staticFriction = staticFriction_;
patch->damping = combinedDamping_;
patch->startContactIndex = PxTo16(currentIndex);
//KS - we could probably compress this further into the header but the complexity might not be worth it
patch->nbContacts = rootPatch.totalCount;
patch->materialFlags = PxU8(materialFlags_);
patch->internalFlags = PxU8(flags);
patch->materialIndex0 = matIndex0;
patch->materialIndex1 = matIndex1;
}
};
if(isModifiable)
{
PxU32 flags = PxU32(isModifiable ? PxContactPatch::eMODIFIABLE : 0) |
(forceNoResponse ? PxContactPatch::eFORCE_NO_RESPONSE : 0) |
(isMeshType ? PxContactPatch::eHAS_FACE_INDICES : 0);
PxU32 currentIndex = 0;
PxModifiableContact* PX_RESTRICT point = reinterpret_cast<PxModifiableContact*>(contactData);
for(PxU32 a = 0; a < numStrideHeaders; ++a)
{
StridePatch& rootPatch = stridePatches[a];
if(rootPatch.isRoot)
{
const PxU32 startIndex = rootPatch.startIndex;
const PxU16 matIndex0 = pMaterial[startIndex].mMaterialIndex0;
const PxU16 matIndex1 = pMaterial[startIndex].mMaterialIndex1;
if(matIndex0 != origMatIndex0 || matIndex1 != origMatIndex1)
{
combineMaterials(materialManager, matIndex0, matIndex1, staticFriction, dynamicFriction, combinedRestitution, materialFlags, combinedDamping);
origMatIndex0 = matIndex0;
origMatIndex1 = matIndex1;
}
PxContactPatch* PX_RESTRICT patch = patches++;
Local::fillPatch(patch, rootPatch, contactPoints[startIndex].normal, currentIndex, staticFriction, dynamicFriction, combinedRestitution, combinedDamping, materialFlags, flags, matIndex0, matIndex1);
//const PxU32 endIndex = strideHeader[a];
const PxU32 totalCountThisPatch = rootPatch.totalCount;
if(insertAveragePoint && totalCountThisPatch > 1)
{
PxVec3 avgPt(0.0f);
PxF32 avgPen(0.0f);
PxF32 recipCount = 1.0f/(PxF32(rootPatch.totalCount));
PxU32 index = a;
while(index != 0xFF)
{
StridePatch& p = stridePatches[index];
for(PxU32 b = p.startIndex; b < p.endIndex; ++b)
{
avgPt += contactPoints[b].point;
avgPen += contactPoints[b].separation;
}
index = p.nextIndex;
}
if (faceIndice)
{
- StridePatch& p = stridePatches[index];
- *faceIndice = contactPoints[p.startIndex].internalFaceIndex1;
+ // PT: the average point inherits the face index of the patch's first contact.
+ *faceIndice = contactPoints[startIndex].internalFaceIndex1;
faceIndice++;
}
patch->nbContacts++;
point->contact = avgPt * recipCount;
point->separation = avgPen * recipCount;
point->normal = contactPoints[startIndex].normal;
point->maxImpulse = PX_MAX_REAL;
point->targetVelocity = PxVec3(0.0f);
point->staticFriction = staticFriction;
point->dynamicFriction = dynamicFriction;
point->restitution = combinedRestitution;
point->materialFlags = materialFlags;
point->materialIndex0 = matIndex0;
point->materialIndex1 = matIndex1;
point++;
currentIndex++;
PxPrefetchLine(point, 128);
}
PxU32 index = a;
while(index != 0xFF)
{
StridePatch& p = stridePatches[index];
for(PxU32 b = p.startIndex; b < p.endIndex; ++b)
{
copyContactPoint(point, &contactPoints[b]);
point->normal = contactPoints[b].normal;
point->maxImpulse = PX_MAX_REAL;
point->targetVelocity = PxVec3(0.0f);
point->staticFriction = staticFriction;
point->dynamicFriction = dynamicFriction;
point->restitution = combinedRestitution;
point->materialFlags = materialFlags;
point->materialIndex0 = matIndex0;
point->materialIndex1 = matIndex1;
if (faceIndice)
{
*faceIndice = contactPoints[b].internalFaceIndex1;
faceIndice++;
}
point++;
currentIndex++;
PxPrefetchLine(point, 128);
}
index = p.nextIndex;
}
}
}
}
else
{
PxU32 flags = PxU32(isMeshType ? PxContactPatch::eHAS_FACE_INDICES : 0);
PxContact* PX_RESTRICT point = reinterpret_cast<PxContact*>(contactData);
PxU32 currentIndex = 0;
{
for(PxU32 a = 0; a < numStrideHeaders; ++a)
{
StridePatch& rootPatch = stridePatches[a];
if(rootPatch.isRoot)
{
const PxU32 startIndex = rootPatch.startIndex;
const PxU16 matIndex0 = pMaterial[startIndex].mMaterialIndex0;
const PxU16 matIndex1 = pMaterial[startIndex].mMaterialIndex1;
if(matIndex0 != origMatIndex0 || matIndex1 != origMatIndex1)
{
combineMaterials(materialManager, matIndex0, matIndex1, staticFriction, dynamicFriction, combinedRestitution, materialFlags, combinedDamping);
origMatIndex0 = matIndex0;
origMatIndex1 = matIndex1;
}
PxContactPatch* PX_RESTRICT patch = patches++;
Local::fillPatch(patch, rootPatch, contactPoints[startIndex].normal, currentIndex, staticFriction, dynamicFriction, combinedRestitution, combinedDamping, materialFlags, flags, matIndex0, matIndex1);
if(insertAveragePoint && (rootPatch.totalCount) > 1)
{
patch->nbContacts++;
PxVec3 avgPt(0.0f);
PxF32 avgPen(0.0f);
PxF32 recipCount = 1.0f/(PxF32(rootPatch.totalCount));
PxU32 index = a;
while(index != 0xFF)
{
StridePatch& p = stridePatches[index];
for(PxU32 b = p.startIndex; b < p.endIndex; ++b)
{
avgPt += contactPoints[b].point;
avgPen += contactPoints[b].separation;
}
index = stridePatches[index].nextIndex;
}
if (faceIndice)
{
- StridePatch& p = stridePatches[index];
- *faceIndice = contactPoints[p.startIndex].internalFaceIndex1;
+ // PT: the average point inherits the face index of the patch's first contact.
+ *faceIndice = contactPoints[startIndex].internalFaceIndex1;
faceIndice++;
}
point->contact = avgPt * recipCount;
point->separation = avgPen * recipCount;
point++;
currentIndex++;
PxPrefetchLine(point, 128);
}
PxU32 index = a;
while(index != 0xFF)
{
StridePatch& p = stridePatches[index];
for(PxU32 b = p.startIndex; b < p.endIndex; ++b)
{
copyContactPoint(point, &contactPoints[b]);
if (faceIndice)
{
*faceIndice = contactPoints[b].internalFaceIndex1;
faceIndice++;
}
point++;
currentIndex++;
PxPrefetchLine(point, 128);
}
index = stridePatches[index].nextIndex;
}
}
}
}
}
writtenContactCount = PxTo16(totalContactPoints);
return totalRequiredSize;
}