394 lines
12 KiB
C++
394 lines
12 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-2019 NVIDIA Corporation. All rights reserved.
|
|
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
|
|
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
|
|
|
|
#include "PxcContactCache.h"
|
|
#include "PxsContactManager.h"
|
|
#include "PsUtilities.h"
|
|
#include "PxcNpCache.h"
|
|
|
|
using namespace physx;
|
|
using namespace Gu;
|
|
|
|
//#define ENABLE_CONTACT_CACHE_STATS
|
|
|
|
#ifdef ENABLE_CONTACT_CACHE_STATS
|
|
static PxU32 gNbCalls;
|
|
static PxU32 gNbHits;
|
|
#endif
|
|
|
|
void PxcClearContactCacheStats()
|
|
{
|
|
#ifdef ENABLE_CONTACT_CACHE_STATS
|
|
gNbCalls = 0;
|
|
gNbHits = 0;
|
|
#endif
|
|
}
|
|
|
|
void PxcDisplayContactCacheStats()
|
|
{
|
|
#ifdef ENABLE_CONTACT_CACHE_STATS
|
|
pxPrintf("%d|%d (%f)\n", gNbHits, gNbCalls, gNbCalls ? float(gNbHits)/float(gNbCalls) : 0.0f);
|
|
#endif
|
|
}
|
|
|
|
namespace physx
|
|
{
|
|
const bool g_CanUseContactCache[][PxGeometryType::eGEOMETRY_COUNT] =
|
|
{
|
|
//PxGeometryType::eSPHERE
|
|
{
|
|
false, //PxcContactSphereSphere
|
|
false, //PxcContactSpherePlane
|
|
true, //PxcContactSphereCapsule
|
|
false, //PxcContactSphereBox
|
|
true, //PxcContactSphereConvex
|
|
true, //PxcContactSphereMesh
|
|
true, //PxcContactSphereHeightField
|
|
},
|
|
|
|
//PxGeometryType::ePLANE
|
|
{
|
|
false, //-
|
|
false, //PxcInvalidContactPair
|
|
true, //PxcContactPlaneCapsule
|
|
true, //PxcContactPlaneBox
|
|
true, //PxcContactPlaneConvex
|
|
false, //PxcInvalidContactPair
|
|
false, //PxcInvalidContactPair
|
|
},
|
|
|
|
//PxGeometryType::eCAPSULE
|
|
{
|
|
false, //-
|
|
false, //-
|
|
true, //PxcContactCapsuleCapsule
|
|
true, //PxcContactCapsuleBox
|
|
true, //PxcContactCapsuleConvex
|
|
true, //PxcContactCapsuleMesh
|
|
true, //PxcContactCapsuleHeightField
|
|
},
|
|
|
|
//PxGeometryType::eBOX
|
|
{
|
|
false, //-
|
|
false, //-
|
|
false, //-
|
|
true, //PxcContactBoxBox
|
|
true, //PxcContactBoxConvex
|
|
true, //PxcContactBoxMesh
|
|
true, //PxcContactBoxHeightField
|
|
},
|
|
|
|
//PxGeometryType::eCONVEXMESH
|
|
{
|
|
false, //-
|
|
false, //-
|
|
false, //-
|
|
false, //-
|
|
true, //PxcContactConvexConvex
|
|
true, //PxcContactConvexMesh2
|
|
true, //PxcContactConvexHeightField
|
|
},
|
|
|
|
//PxGeometryType::eTRIANGLEMESH
|
|
{
|
|
false, //-
|
|
false, //-
|
|
false, //-
|
|
false, //-
|
|
false, //-
|
|
false, //PxcInvalidContactPair
|
|
false, //PxcInvalidContactPair
|
|
},
|
|
|
|
//PxGeometryType::eHEIGHTFIELD
|
|
{
|
|
false, //-
|
|
false, //-
|
|
false, //-
|
|
false, //-
|
|
false, //-
|
|
false, //-
|
|
false, //PxcInvalidContactPair
|
|
},
|
|
};
|
|
}
|
|
|
|
static PX_FORCE_INLINE void updateContact( Gu::ContactPoint& dst, const PxcLocalContactsCache& contactsData,
|
|
const Cm::Matrix34& world0, const Cm::Matrix34& world1,
|
|
const PxVec3& point, const PxVec3& normal, float separation)
|
|
{
|
|
const PxVec3 tmp0 = contactsData.mTransform0.transformInv(point);
|
|
const PxVec3 worldpt0 = world0.transform(tmp0);
|
|
|
|
const PxVec3 tmp1 = contactsData.mTransform1.transformInv(point);
|
|
const PxVec3 worldpt1 = world1.transform(tmp1);
|
|
|
|
const PxVec3 motion = worldpt0 - worldpt1;
|
|
dst.normal = normal;
|
|
dst.point = (worldpt0 + worldpt1)*0.5f;
|
|
//dst.point = point;
|
|
dst.separation = separation + motion.dot(normal);
|
|
}
|
|
|
|
static PX_FORCE_INLINE void prefetchData128(PxU8* PX_RESTRICT ptr, PxU32 size)
|
|
{
|
|
// PT: always prefetch the cache line containing our address (which unfortunately won't be aligned to 128 most of the time)
|
|
Ps::prefetchLine(ptr, 0);
|
|
// PT: compute start offset of our data within its cache line
|
|
const PxU32 startOffset = PxU32(size_t(ptr)&127);
|
|
// PT: prefetch next cache line if needed
|
|
if(startOffset+size>128)
|
|
Ps::prefetchLine(ptr+128, 0);
|
|
}
|
|
|
|
static PX_FORCE_INLINE PxU8* outputToCache(PxU8* PX_RESTRICT bytes, const PxVec3& v)
|
|
{
|
|
*reinterpret_cast<PxVec3*>(bytes) = v;
|
|
return bytes + sizeof(PxVec3);
|
|
}
|
|
|
|
static PX_FORCE_INLINE PxU8* outputToCache(PxU8* PX_RESTRICT bytes, PxReal v)
|
|
{
|
|
*reinterpret_cast<PxReal*>(bytes) = v;
|
|
return bytes + sizeof(PxReal);
|
|
}
|
|
|
|
static PX_FORCE_INLINE PxU8* outputToCache(PxU8* PX_RESTRICT bytes, PxU32 v)
|
|
{
|
|
*reinterpret_cast<PxU32*>(bytes) = v;
|
|
return bytes + sizeof(PxU32);
|
|
}
|
|
|
|
//PxU32 gContactCache_NbCalls = 0;
|
|
//PxU32 gContactCache_NbHits = 0;
|
|
|
|
static PX_FORCE_INLINE PxReal maxComponentDeltaPos(const PxTransform& t0, const PxTransform& t1)
|
|
{
|
|
PxReal delta = PxAbs(t0.p.x - t1.p.x);
|
|
delta = PxMax(delta, PxAbs(t0.p.y - t1.p.y));
|
|
delta = PxMax(delta, PxAbs(t0.p.z - t1.p.z));
|
|
return delta;
|
|
}
|
|
|
|
static PX_FORCE_INLINE PxReal maxComponentDeltaRot(const PxTransform& t0, const PxTransform& t1)
|
|
{
|
|
PxReal delta = PxAbs(t0.q.x - t1.q.x);
|
|
delta = PxMax(delta, PxAbs(t0.q.y - t1.q.y));
|
|
delta = PxMax(delta, PxAbs(t0.q.z - t1.q.z));
|
|
delta = PxMax(delta, PxAbs(t0.q.w - t1.q.w));
|
|
return delta;
|
|
}
|
|
|
|
bool physx::PxcCacheLocalContacts( PxcNpThreadContext& context, Gu::Cache& pairContactCache,
|
|
const PxTransform& tm0, const PxTransform& tm1,
|
|
const PxcContactMethod conMethod,
|
|
const Gu::GeometryUnion& shape0, const Gu::GeometryUnion& shape1)
|
|
{
|
|
const Gu::NarrowPhaseParams& params = context.mNarrowPhaseParams;
|
|
|
|
// gContactCache_NbCalls++;
|
|
|
|
if(pairContactCache.mCachedData)
|
|
prefetchData128(pairContactCache.mCachedData, pairContactCache.mCachedSize);
|
|
|
|
ContactBuffer& contactBuffer = context.mContactBuffer;
|
|
contactBuffer.reset();
|
|
|
|
PxcLocalContactsCache contactsData;
|
|
PxU32 nbCachedBytes;
|
|
const PxU8* cachedBytes = PxcNpCacheRead2(pairContactCache, contactsData, nbCachedBytes);
|
|
|
|
pairContactCache.mCachedData = NULL;
|
|
pairContactCache.mCachedSize = 0;
|
|
|
|
#ifdef ENABLE_CONTACT_CACHE_STATS
|
|
gNbCalls++;
|
|
#endif
|
|
|
|
const PxU32 payloadSize = (sizeof(PxcLocalContactsCache)+3)&~3;
|
|
|
|
if(cachedBytes)
|
|
{
|
|
// PT: we used to store the relative TM but it's better to save memory and recompute it
|
|
const PxTransform t0to1 = tm1.transformInv(tm0);
|
|
const PxTransform relTM = contactsData.mTransform1.transformInv(contactsData.mTransform0);
|
|
|
|
const PxReal epsilon = 0.01f;
|
|
if( maxComponentDeltaPos(t0to1, relTM)<epsilon*params.mToleranceLength
|
|
&& maxComponentDeltaRot(t0to1, relTM)<epsilon)
|
|
{
|
|
// gContactCache_NbHits++;
|
|
const PxU32 nbContacts = contactsData.mNbCachedContacts;
|
|
|
|
PxU8* ls = PxcNpCacheWriteInitiate(context.mNpCacheStreamPair, pairContactCache, contactsData, nbCachedBytes);
|
|
prefetchData128(ls, (payloadSize + 4 + nbCachedBytes + 0xF)&~0xF);
|
|
|
|
contactBuffer.count = nbContacts;
|
|
if(nbContacts)
|
|
{
|
|
Gu::ContactPoint* PX_RESTRICT dst = contactBuffer.contacts;
|
|
|
|
const Cm::Matrix34 world1(tm1);
|
|
const Cm::Matrix34 world0(tm0);
|
|
|
|
const bool sameNormal = contactsData.mSameNormal;
|
|
|
|
const PxU8* contacts = reinterpret_cast<const PxU8*>(cachedBytes);
|
|
const PxVec3* normal0 = NULL;
|
|
for(PxU32 i=0;i<nbContacts;i++)
|
|
{
|
|
if(i!=nbContacts-1)
|
|
Ps::prefetchLine(contacts, 128);
|
|
|
|
const PxVec3* cachedNormal;
|
|
if(!i || !sameNormal)
|
|
{
|
|
cachedNormal = reinterpret_cast<const PxVec3*>(contacts); contacts += sizeof(PxVec3);
|
|
normal0 = cachedNormal;
|
|
}
|
|
else
|
|
{
|
|
cachedNormal = normal0;
|
|
}
|
|
|
|
const PxVec3* cachedPoint = reinterpret_cast<const PxVec3*>(contacts); contacts += sizeof(PxVec3);
|
|
const PxReal* cachedPD = reinterpret_cast<const PxReal*>(contacts); contacts += sizeof(PxReal);
|
|
|
|
updateContact(*dst, contactsData, world0, world1, *cachedPoint, *cachedNormal, *cachedPD);
|
|
|
|
if(contactsData.mUseFaceIndices)
|
|
{
|
|
const PxU32* cachedIndex1 = reinterpret_cast<const PxU32*>(contacts); contacts += sizeof(PxU32);
|
|
|
|
dst->internalFaceIndex1 = *cachedIndex1;
|
|
}
|
|
else
|
|
{
|
|
dst->internalFaceIndex1 = PXC_CONTACT_NO_FACE_INDEX;
|
|
}
|
|
dst++;
|
|
}
|
|
}
|
|
if(ls)
|
|
PxcNpCacheWriteFinalize(ls, contactsData, nbCachedBytes, cachedBytes);
|
|
#ifdef ENABLE_CONTACT_CACHE_STATS
|
|
gNbHits++;
|
|
#endif
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
// PT: if we reach this point we cached the contacts but we couldn't use them next frame
|
|
// => waste of time and memory
|
|
}
|
|
}
|
|
|
|
conMethod(shape0, shape1, tm0, tm1, params, pairContactCache, context.mContactBuffer, &context.mRenderOutput);
|
|
|
|
//if(contactBuffer.count)
|
|
{
|
|
contactsData.mTransform0 = tm0;
|
|
contactsData.mTransform1 = tm1;
|
|
|
|
PxU32 nbBytes = 0;
|
|
const PxU8* bytes = NULL;
|
|
const PxU32 count = contactBuffer.count;
|
|
if(count)
|
|
{
|
|
const bool useFaceIndices = contactBuffer.contacts[0].internalFaceIndex1!=PXC_CONTACT_NO_FACE_INDEX;
|
|
contactsData.mNbCachedContacts = Ps::to16(count);
|
|
contactsData.mUseFaceIndices = useFaceIndices;
|
|
|
|
const Gu::ContactPoint* PX_RESTRICT srcContacts = contactBuffer.contacts;
|
|
// PT: this loop should not be here. We should output the contacts directly compressed, as we used to.
|
|
bool sameNormal = true;
|
|
{
|
|
const PxVec3 normal0 = srcContacts->normal;
|
|
for(PxU32 i=1;i<count;i++)
|
|
{
|
|
if(srcContacts[i].normal!=normal0)
|
|
{
|
|
sameNormal = false;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
contactsData.mSameNormal = sameNormal;
|
|
|
|
if(!sameNormal)
|
|
{
|
|
const PxU32 sizeof_CachedContactPoint = sizeof(PxVec3) + sizeof(PxVec3) + sizeof(PxReal);
|
|
const PxU32 sizeof_CachedContactPointAndFaceIndices = sizeof_CachedContactPoint + sizeof(PxU32);
|
|
const PxU32 sizeOfItem = useFaceIndices ? sizeof_CachedContactPointAndFaceIndices : sizeof_CachedContactPoint;
|
|
nbBytes = count * sizeOfItem;
|
|
}
|
|
else
|
|
{
|
|
const PxU32 sizeof_CachedContactPoint = sizeof(PxVec3) + sizeof(PxReal);
|
|
const PxU32 sizeof_CachedContactPointAndFaceIndices = sizeof_CachedContactPoint + sizeof(PxU32);
|
|
const PxU32 sizeOfItem = useFaceIndices ? sizeof_CachedContactPointAndFaceIndices : sizeof_CachedContactPoint;
|
|
nbBytes = sizeof(PxVec3) + count * sizeOfItem;
|
|
}
|
|
PxU8* ls = PxcNpCacheWriteInitiate(context.mNpCacheStreamPair, pairContactCache, contactsData, nbBytes);
|
|
if(ls)
|
|
{
|
|
*reinterpret_cast<PxcLocalContactsCache*>(ls) = contactsData;
|
|
*reinterpret_cast<PxU32*>(ls+payloadSize) = nbBytes;
|
|
bytes = ls+payloadSize+sizeof(PxU32);
|
|
PxU8* dest = const_cast<PxU8*>(bytes);
|
|
for(PxU32 i=0;i<count;i++)
|
|
{
|
|
if(!i || !sameNormal)
|
|
dest = outputToCache(dest, srcContacts[i].normal);
|
|
dest = outputToCache(dest, srcContacts[i].point);
|
|
dest = outputToCache(dest, srcContacts[i].separation);
|
|
if(useFaceIndices)
|
|
{
|
|
dest = outputToCache(dest, srcContacts[i].internalFaceIndex1);
|
|
}
|
|
}
|
|
PX_ASSERT(size_t(dest) - size_t(bytes)==nbBytes);
|
|
}
|
|
else
|
|
{
|
|
contactsData.mNbCachedContacts = 0;
|
|
PxcNpCacheWrite(context.mNpCacheStreamPair, pairContactCache, contactsData, 0, bytes);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
contactsData.mNbCachedContacts = 0;
|
|
contactsData.mUseFaceIndices = false;
|
|
PxcNpCacheWrite(context.mNpCacheStreamPair, pairContactCache, contactsData, nbBytes, bytes);
|
|
}
|
|
}
|
|
return false;
|
|
}
|