A fast 2D and 3D game engine — TypeScript SDK, C++/WebAssembly core, visual editor. Ship one project to web, desktop, WeChat MiniGames, playable ads, and native Android / iOS.
TypeScript 2d-game-engine3d3d-game-engineandroidcppecsgame-editorgame-enginegamedeviostypescriptwebassemblywebglwebgpuwechat-minigame
// 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_PERSISTENTCONTACTMANIFOLD_H
#define Gu_PERSISTENTCONTACTMANIFOLD_H
#include "common/PxPhysXCommonConfig.h"
#include "foundation/PxUnionCast.h"
#include "foundation/PxMemory.h"
#include "foundation/PxVecTransform.h"
#define PCM_LOW_LEVEL_DEBUG 0
namespace physx
{
#define VISUALIZE_PERSISTENT_CONTACT 1
//This is for primitives vs primitives
#define GU_MANIFOLD_CACHE_SIZE 4
//These are for primitives vs mesh
#define GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE 5
#define GU_SINGLE_MANIFOLD_CACHE_SIZE 6
#define GU_SPHERE_MANIFOLD_CACHE_SIZE 1
#define GU_CAPSULE_MANIFOLD_CACHE_SIZE 3
#define GU_MAX_MANIFOLD_SIZE 6 // PT: max nb of manifolds (e.g. for multi-manifolds), NOT the max size of a single manifold
#define GU_MESH_CONTACT_REDUCTION_THRESHOLD 16
#define GU_MANIFOLD_INVALID_INDEX 0xffffffff
//ML: this is used to compared with the shape's margin to decide the final tolerance used in the manifold to validate the existing contacts.
//In the case of big shape and relatively speaking small triangles in the mesh, we need to take a smaller margin. This helps because the PCM
//recycling thresholds are proportionate to margin so it makes it less likely to discard previous contacts due to separation
#define GU_PCM_MESH_MANIFOLD_EPSILON 0.05f
class PxRenderOutput;
class PxContactBuffer;
namespace Gu
{
struct GjkOutput;
extern const PxF32 invalidateThresholds[5];
extern const PxF32 invalidateQuatThresholds[5];
extern const PxF32 invalidateThresholds2[3];
extern const PxF32 invalidateQuatThresholds2[3];
aos::Mat33V findRotationMatrixFromZAxis(const aos::Vec3VArg to);
//This contact is used in the primitives vs primitives contact gen
class PersistentContact
{
public:
PersistentContact()
{
}
PersistentContact(aos::Vec3V _localPointA, aos::Vec3V _localPointB, aos::Vec4V _localNormalPen) :
mLocalPointA(_localPointA), mLocalPointB(_localPointB), mLocalNormalPen(_localNormalPen)
{
}
aos::Vec3V mLocalPointA;
aos::Vec3V mLocalPointB;
aos::Vec4V mLocalNormalPen; // the (x, y, z) is the local normal, and the w is the penetration depth
};
//This contact is used in the mesh contact gen to store an extra variable
class MeshPersistentContact : public PersistentContact
{
public:
PxU32 mFaceIndex;
};
//This contact is used in the compress stream buffer(NpCacheStreamPair in the PxcNpThreadContext) to store the data we need
PX_ALIGN_PREFIX(16)
class CachedMeshPersistentContact
{
public:
PxVec3 mLocalPointA; //16 byte aligned
PxU32 mFaceIndex; //face index
PxVec3 mLocalPointB; //16 byte aligned
PxU32 mPad; //pad
PxVec3 mLocalNormal; //16 byte aligned
PxReal mPen; //penetration
}PX_ALIGN_SUFFIX(16);
#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
//This class is used to store the start and end index of the mesh persistent contacts in an array.
struct PCMContactPatch
{
public:
PCMContactPatch()
{
mNextPatch = NULL;
mEndPatch = NULL;
mRoot = this;
mPatchMaxPen = aos::FMax();
}
aos::Vec3V mPatchNormal;
PCMContactPatch* mNextPatch;//store the next patch pointer in the patch list
PCMContactPatch* mEndPatch;//store the last patch pointer in the patch list
PCMContactPatch* mRoot;//if this is the start of the patch list which has very similar patch normal, the root will be itself
aos::FloatV mPatchMaxPen;//store the deepest penetration of the whole patch
PxU32 mStartIndex;//store the start index of the manifold contacts in the manifold contacts stream
PxU32 mEndIndex;//store the end index of the manifold contacts in the manifold contacts stream
PxU32 mTotalSize;//if this is the root, the total size will store the total number of manifold contacts in the whole patch list
};
#if PX_VC
#pragma warning(pop)
#endif
//ML: this is needed because it seems NEON doesn't force the alignment on SIMD type, which cause some of the PCM unit tests fail
PX_ALIGN_PREFIX(16)
class PersistentContactManifold
{
public:
PersistentContactManifold(PersistentContact* contactPointsBuff, PxU8 capacity): mNumContacts(0), mCapacity(capacity), mNumWarmStartPoints(0), mContactPoints(contactPointsBuff)
{
using namespace physx::aos;
mRelativeTransform.invalidate();
mQuatA = QuatIdentity();
mQuatB = QuatIdentity();
}
PX_FORCE_INLINE PxU32 getNumContacts() const { return mNumContacts;}
PX_FORCE_INLINE bool isEmpty() const { return mNumContacts==0; }
PX_FORCE_INLINE PersistentContact& getContactPoint(const PxU32 index)
{
PX_ASSERT(index < GU_MANIFOLD_CACHE_SIZE);
return mContactPoints[index];
}
PX_FORCE_INLINE aos::FloatV maxTransformPositionDelta(const aos::Vec3V& curP) const
{
using namespace aos;
const Vec3V deltaP = V3Sub(curP, mRelativeTransform.p);
const Vec4V delta = Vec4V_From_Vec3V(V3Abs(deltaP));
//need to work out max from a single vector...
return V4ExtractMax(delta);
}
PX_FORCE_INLINE void setRelativeTransform(const aos::PxTransformV& transform)
{
mRelativeTransform = transform;
}
PX_FORCE_INLINE void setRelativeTransform(const aos::PxTransformV& transform, const aos::QuatV quatA, const aos::QuatV quatB)
{
mRelativeTransform = transform;
mQuatA = quatA;
mQuatB = quatB;
}
//This is used for the box/convexhull vs box/convexhull contact gen to decide whether the relative movement of a pair of objects are
//small enough. In this case, we can skip the collision detection all together
PX_FORCE_INLINE PxU32 invalidate_BoxConvex(const aos::PxTransformV& curRTrans, const aos::QuatV& quatA, const aos::QuatV& quatB,
const aos::FloatVArg minMargin, const aos::FloatVArg radiusA, const aos::FloatVArg radiusB) const
{
using namespace aos;
PX_ASSERT(mNumContacts <= GU_MANIFOLD_CACHE_SIZE);
const FloatV ratio = FLoad(invalidateThresholds[mNumContacts]);
const FloatV thresholdP = FMul(minMargin, ratio);
const FloatV deltaP = maxTransformPositionDelta(curRTrans.p);
const FloatV thresholdQ = FLoad(invalidateQuatThresholds[mNumContacts]);
const FloatV deltaQA = QuatDot(quatA, mQuatA);
const FloatV deltaQB = QuatDot(quatB, mQuatB);
const BoolV con0 = BOr(FIsGrtr(deltaP, thresholdP), BOr(FIsGrtr(thresholdQ, deltaQA), FIsGrtr(thresholdQ, deltaQB)));
PxU32 generateContacts = BAllEqTTTT(con0);
if (!generateContacts)
{
PxReal dqA, dqB;
FStore(deltaQA, &dqA);
FStore(deltaQB, &dqB);
const PxReal aRadian = dqA < 1.0f ? PxAcos(dqA) : 0.f;
const FloatV travelDistA = FMul(FLoad(aRadian), radiusA);
const PxReal bRadian = dqB < 1.0f ? PxAcos(dqB) : 0.f;
const FloatV travelDistB = FMul(FLoad(bRadian), radiusB);
const BoolV con = BOr(FIsGrtr(travelDistA, thresholdP), FIsGrtr(travelDistB, thresholdP));
generateContacts = BAllEqTTTT(con);
}
return generateContacts;
}
//This is used for the sphere/capsule vs other primitives contact gen to decide whether the relative movement of a pair of objects are
//small enough. In this case, we can skip the collision detection all together
PX_FORCE_INLINE PxU32 invalidate_SphereCapsule(const aos::PxTransformV& curRTrans, const aos::FloatVArg minMargin) const
{
using namespace aos;
PX_ASSERT(mNumContacts <= 2);
const FloatV ratio = FLoad(invalidateThresholds2[mNumContacts]);
const FloatV thresholdP = FMul(minMargin, ratio);
const FloatV deltaP = maxTransformPositionDelta(curRTrans.p);
const FloatV thresholdQ = FLoad(invalidateQuatThresholds2[mNumContacts]);
const FloatV deltaQ = QuatDot(curRTrans.q, mRelativeTransform.q);
const BoolV con = BOr(FIsGrtr(deltaP, thresholdP), FIsGrtr(thresholdQ, deltaQ));
return BAllEqTTTT(con);
}
//This is used for plane contact gen to decide whether the relative movement of a pair of objects are small enough. In this case,
//we can skip the collision detection all together
PX_FORCE_INLINE PxU32 invalidate_PrimitivesPlane(const aos::PxTransformV& curRTrans, const aos::FloatVArg minMargin, const aos::FloatVArg ratio) const
{
using namespace aos;
const FloatV thresholdP = FMul(minMargin, ratio);
const FloatV deltaP = maxTransformPositionDelta(curRTrans.p);
const FloatV thresholdQ = FLoad(0.99996f);//about 0.5 degree
const FloatV deltaQ = QuatDot(curRTrans.q, mRelativeTransform.q);
const BoolV con = BOr(FIsGrtr(deltaP, thresholdP), FIsGrtr(thresholdQ, deltaQ));
return BAllEqTTTT(con);
}
PX_FORCE_INLINE void removeContactPoint(PxU32 index)
{
mNumContacts--;
mContactPoints[index] = mContactPoints[mNumContacts];
}
/*bool validContactDistance(const PersistentContact& pt, const aos::FloatVArg breakingThreshold) const
{
using namespace aos;
const FloatV dist = V4GetW(pt.mLocalNormalPen);
return FAllGrtr(breakingThreshold, dist) != 0;
}*/
PX_FORCE_INLINE void clearManifold()
{
mNumWarmStartPoints = 0;
mNumContacts = 0;
mRelativeTransform.invalidate();
}
PX_FORCE_INLINE void initialize()
{
clearManifold();
}
//This function is used to replace the existing contact with the newly created contact if their distance are within some threshold
bool replaceManifoldPoint(const aos::Vec3VArg localPointA, const aos::Vec3VArg localPointB, const aos::Vec4VArg localNormalPen, const aos::FloatVArg replaceBreakingThreshold);
//This function is to add a point(in box/convexhull contact gen) to the exising manifold. If the number of manifold is more than 4, we need to do contact reduction
PxU32 addManifoldPoint(const aos::Vec3VArg localPointA, const aos::Vec3VArg localPointB, const aos::Vec4VArg localNormalAPen, const aos::FloatVArg replaceBreakingThreshold);
//This function is to add a point(in capsule contact gen) to the exising manifold. If the number of manifold is more than 4, we need to do contact reduction
PxU32 addManifoldPoint2(const aos::Vec3VArg localPointA, const aos::Vec3VArg localPointB, const aos::Vec4VArg localNormalAPen, const aos::FloatVArg replaceBreakingThreshold);//max two points of contacts
//This function is used in box-plane contact gen to add the plane contacts to the manifold
void addBatchManifoldContactsCluster(const PersistentContact* manifoldPoints, PxU32 numPoints);
//This function is used in the capsule full manifold contact genenation(maximum 2 points).
void addBatchManifoldContacts2(const PersistentContact* manifoldPoints, PxU32 numPoints);//max two points of contacts
//This function is used in the box/convexhull full manifold contact generation(maximum 4 points).
void addBatchManifoldContacts(const PersistentContact* manifoldPoints, PxU32 numPoints, PxReal toleranceLength);
//This function is using the cluster algorithm to reduce contacts
void reduceBatchContactsCluster(const PersistentContact* manifoldPoints, PxU32 numPoints);
//This function is called by addBatchManifoldContacts2 to reduce the manifold contacts to 2 points;
void reduceBatchContacts2(const PersistentContact* manifoldPoints, PxU32 numPoints);
//This function is called by addBatchManifoldContacts to reduce the manifold contacts to 4 points
void reduceBatchContacts(const PersistentContact* manifoldPoints, PxU32 numPoints, PxReal toleranceLength);
//This function is used for incremental manifold contact reduction for box/convexhull
PxU32 reduceContactsForPCM(const aos::Vec3VArg localPointA, const aos::Vec3VArg localPointB, const aos::Vec4VArg localNormalPen);
//This function is used for incremental manifold contact reduction for capsule
PxU32 reduceContactSegment(const aos::Vec3VArg localPointA, const aos::Vec3VArg localPointB, const aos::Vec4VArg localNormalPen);
// This function recalculate the contacts in the manifold based on the current relative transform between a pair of objects. If the recalculated contacts are within some threshold,
// we will keep the contacts; Otherwise, we will remove the contacts.
void refreshContactPoints(const aos::PxMatTransformV& relTra, const aos::FloatVArg projectBreakingThreshold, const aos::FloatVArg contactOffset);
//This function is just used in boxbox contact gen for fast transform
void addManifoldContactsToContactBuffer(PxContactBuffer& contactBuffer, const aos::Vec3VArg normal, const aos::PxMatTransformV& transf1);
//This function is for adding box/convexhull manifold contacts to the contact buffer
void addManifoldContactsToContactBuffer(PxContactBuffer& contactBuffer, const aos::Vec3VArg normal, const aos::PxTransformV& transf1, const aos::FloatVArg contactOffset);
//This function is for adding sphere/capsule manifold contacts to the contact buffer
void addManifoldContactsToContactBuffer(PxContactBuffer& contactBuffer, const aos::Vec3VArg normal, const aos::Vec3VArg projectionNormal, const aos::PxTransformV& transf0, const aos::FloatVArg radius, const aos::FloatVArg contactOffset);
//get the average normal in the manifold in world space
aos::Vec3V getWorldNormal(const aos::PxTransformV& trB) const;
//get the average normal in the manifold in local B object space
aos::Vec3V getLocalNormal() const;
void recordWarmStart(PxU8* aIndices, PxU8* bIndices, PxU8& nbWarmStartPoints);
void setWarmStart(const PxU8* aIndices, const PxU8* bIndices, const PxU8 nbWarmStartPoints);
void drawManifold(PxRenderOutput& out, const aos::PxTransformV& trA, const aos::PxTransformV& trB) const;
void drawManifold(PxRenderOutput& out, const aos::PxTransformV& trA, const aos::PxTransformV& trB, const aos::FloatVArg radius) const;
void drawManifold(const PersistentContact& m, PxRenderOutput& out, const aos::PxTransformV& trA, const aos::PxTransformV& trB) const;
static void drawPoint(PxRenderOutput& out, const aos::Vec3VArg p, const PxF32 size, PxU32 color = 0x0000ff00);
static void drawLine(PxRenderOutput& out, const aos::Vec3VArg p0, const aos::Vec3VArg p1, PxU32 color = 0xff00ffff);
static void drawTriangle(PxRenderOutput& out, const aos::Vec3VArg p0, const aos::Vec3VArg p1, const aos::Vec3VArg p2, PxU32 color = 0xffff0000);
static void drawTetrahedron(PxRenderOutput& out, const aos::Vec3VArg p0, const aos::Vec3VArg p1, const aos::Vec3VArg p2, const aos::Vec3VArg p3, PxU32 color = 0xffff0000);
static void drawPolygon(PxRenderOutput& out, const aos::PxTransformV& transform, const aos::Vec3V* points, PxU32 numVerts, PxU32 color = 0xff00ffff);
static void drawPolygon(PxRenderOutput& out, const aos::PxMatTransformV& transform, const aos::Vec3V* points, PxU32 numVerts, PxU32 color = 0xff00ffff);
aos::PxTransformV mRelativeTransform;//aToB
aos::QuatV mQuatA;
aos::QuatV mQuatB;
PxU8 mNumContacts;
PxU8 mCapacity;
PxU8 mNumWarmStartPoints;
PxU8 mAIndice[4];
PxU8 mBIndice[4];
PersistentContact* mContactPoints;
} PX_ALIGN_SUFFIX(16);
PX_ALIGN_PREFIX(16)
class LargePersistentContactManifold : public PersistentContactManifold
{
public:
LargePersistentContactManifold() : PersistentContactManifold(mContactPointsBuff, GU_MANIFOLD_CACHE_SIZE)
{
}
PersistentContact mContactPointsBuff[GU_MANIFOLD_CACHE_SIZE];
}PX_ALIGN_SUFFIX(16);
PX_ALIGN_PREFIX(16)
class SpherePersistentContactManifold : public PersistentContactManifold
{
public:
SpherePersistentContactManifold() : PersistentContactManifold(mContactPointsBuff, GU_SPHERE_MANIFOLD_CACHE_SIZE)
{
}
PersistentContact mContactPointsBuff[GU_SPHERE_MANIFOLD_CACHE_SIZE];
}PX_ALIGN_SUFFIX(16);
PX_ALIGN_PREFIX(16)
class SinglePersistentContactManifold
{
public:
SinglePersistentContactManifold(): mNumContacts(0)
{
}
PX_FORCE_INLINE PxU32 getNumContacts() const { return mNumContacts;}
PX_FORCE_INLINE bool isEmpty() const { return mNumContacts==0; }
PX_FORCE_INLINE MeshPersistentContact& getContactPoint(const PxU32 index)
{
PX_ASSERT(index < GU_SINGLE_MANIFOLD_CACHE_SIZE);
return mContactPoints[index];
}
PX_FORCE_INLINE void removeContactPoint (PxU32 index)
{
mNumContacts--;
mContactPoints[index] = mContactPoints[mNumContacts];
}
PX_FORCE_INLINE void clearManifold()
{
mNumContacts = 0;
}
PX_FORCE_INLINE void initialize()
{
clearManifold();
}
PX_FORCE_INLINE aos::Vec3V getWorldNormal(const aos::PxTransformV& trB) const
{
using namespace aos;
Vec4V nPen = mContactPoints[0].mLocalNormalPen;
for(PxU32 i=1; i<mNumContacts; ++i)
{
nPen = V4Add(nPen, mContactPoints[i].mLocalNormalPen);
}
const Vec3V n = Vec3V_From_Vec4V(nPen);
return trB.rotateAndNormalize(n);
}
PX_FORCE_INLINE aos::Vec3V getLocalNormal() const
{
using namespace aos;
Vec4V nPen = mContactPoints[0].mLocalNormalPen;
for(PxU32 i=1; i<mNumContacts; ++i)
{
nPen = V4Add(nPen, mContactPoints[i].mLocalNormalPen);
}
return V3Normalize(Vec3V_From_Vec4V(nPen));
}
//This function reduces the manifold contact list in a patch for box/convexhull vs mesh
aos::FloatV reduceBatchContactsConvex(const MeshPersistentContact* manifoldContactExt, PxU32 numContacts, PCMContactPatch& patch);
//This function reduces the manifold contact list in a patch for sphere vs mesh
aos::FloatV reduceBatchContactsSphere(const MeshPersistentContact* manifoldContactExt, PxU32 numContacts, PCMContactPatch& patch);
//This function reduces the manifold contact list in a patch for capsuel vs mesh
aos::FloatV reduceBatchContactsCapsule(const MeshPersistentContact* manifoldContactExt, PxU32 numContacts, PCMContactPatch& patch);
//This function adds the manifold contact list in a patch for box/convexhull vs mesh
aos::FloatV addBatchManifoldContactsConvex(const MeshPersistentContact* manifoldContact, PxU32 numContacts, PCMContactPatch& patch, const aos::FloatVArg replaceBreakingThreshold);
//This function adds the manifold contact list in a patch for sphere vs mesh
aos::FloatV addBatchManifoldContactsSphere(const MeshPersistentContact* manifoldContact, PxU32 numContacts, PCMContactPatch& patch, const aos::FloatVArg replaceBreakingThreshold);
//This function adds the manifold contact list in a patch for capsule vs mesh
aos::FloatV addBatchManifoldContactsCapsule(const MeshPersistentContact* manifoldContact, PxU32 numContacts, PCMContactPatch& patch, const aos::FloatVArg replaceBreakingThreshold);
//This is used for in the addContactsToPatch for convex mesh contact gen.
static PxU32 reduceContacts(MeshPersistentContact* manifoldContactExt, PxU32 numContacts);
//This function is to recalculate the contacts based on the relative transform between a pair of objects
aos::FloatV refreshContactPoints(const aos::PxMatTransformV& relTra, const aos::FloatVArg projectBreakingThreshold, const aos::FloatVArg contactOffset);
void drawManifold(PxRenderOutput& out, const aos::PxTransformV& trA, const aos::PxTransformV& trB) const;
MeshPersistentContact mContactPoints[GU_SINGLE_MANIFOLD_CACHE_SIZE];//384 bytes
PxU32 mNumContacts;//400 bytes
} PX_ALIGN_SUFFIX(16);
//This is a structure used to cache a multi-persistent-manifold in the cache stream
struct MultiPersistentManifoldHeader
{
aos::PxTransformV mRelativeTransform;//aToB
PxU32 mNumManifolds;
PxU32 pad[3];
};
struct SingleManifoldHeader
{
PxU32 mNumContacts;
PxU32 pad[3];
};
#if PX_VC
#pragma warning(push)
#pragma warning( disable : 4251 ) // class needs to have dll-interface to be used by clients of class
#endif
PX_ALIGN_PREFIX(16)
class PX_PHYSX_COMMON_API MultiplePersistentContactManifold
{
public:
MultiplePersistentContactManifold():mNumManifolds(0), mNumTotalContacts(0)
{
mRelativeTransform.invalidate();
}
PX_FORCE_INLINE void setRelativeTransform(const aos::PxTransformV& transform)
{
mRelativeTransform = transform;
}
PX_FORCE_INLINE aos::FloatV maxTransformPositionDelta(const aos::Vec3V& curP) const
{
using namespace aos;
const Vec3V deltaP = V3Sub(curP, mRelativeTransform.p);
const Vec4V delta = Vec4V_From_Vec3V(V3Abs(deltaP));
//need to work out max from a single vector...
return V4ExtractMax(delta);
}
PX_FORCE_INLINE PxU32 invalidate(const aos::PxTransformV& curRTrans, const aos::FloatVArg minMargin, const aos::FloatVArg ratio) const
{
using namespace aos;
const FloatV thresholdP = FMul(minMargin, ratio);
const FloatV deltaP = maxTransformPositionDelta(curRTrans.p);
const FloatV thresholdQ = FLoad(0.9998f);//about 1 degree
const FloatV deltaQ = QuatDot(curRTrans.q, mRelativeTransform.q);
const BoolV con = BOr(FIsGrtr(deltaP, thresholdP), FIsGrtr(thresholdQ, deltaQ));
return BAllEqTTTT(con);
}
PX_FORCE_INLINE PxU32 invalidate(const aos::PxTransformV& curRTrans, const aos::FloatVArg minMargin) const
{
using namespace aos;
return invalidate(curRTrans, minMargin, FLoad(0.2f));
}
// This function work out the contact patch connectivity. If two patches's normal are within 5 degree, we would link these two patches together and reset the total size.
PX_FORCE_INLINE void refineContactPatchConnective(PCMContactPatch** contactPatch, PxU32 numContactPatch, MeshPersistentContact* manifoldContacts, const aos::FloatVArg acceptanceEpsilon) const
{
PX_UNUSED(manifoldContacts);
using namespace aos;
//work out the contact patch connectivity, the patchNormal should be in the local space of mesh
for(PxU32 i=0; i<numContactPatch; ++i)
{
PCMContactPatch* patch = contactPatch[i];
patch->mRoot = patch;
patch->mEndPatch = patch;
patch->mTotalSize = patch->mEndIndex - patch->mStartIndex;
patch->mNextPatch = NULL;
for(PxU32 j=i; j>0; --j)
{
PCMContactPatch* other = contactPatch[j-1];
const FloatV d = V3Dot(patch->mPatchNormal, other->mRoot->mPatchNormal);
if(FAllGrtrOrEq(d, acceptanceEpsilon))//less than 5 degree
{
other->mNextPatch = patch;
other->mRoot->mEndPatch = patch;
patch->mRoot = other->mRoot;
other->mRoot->mTotalSize += patch->mEndIndex - patch->mStartIndex;
break;
}
}
}
}
// This function uses to reduce the manifold contacts which are in different connected patchs but are within replace breaking threshold
PX_FORCE_INLINE PxU32 reduceManifoldContactsInDifferentPatches(PCMContactPatch** contactPatch, PxU32 numContactPatch, MeshPersistentContact* manifoldContacts, PxU32 numContacts, const aos::FloatVArg sqReplaceBreaking) const
{
using namespace aos;
for(PxU32 i=0; i<numContactPatch; ++i)
{
PCMContactPatch* currentPatch = contactPatch[i];
//this make sure the patch is the root before we do the contact reduction, otherwise, we will do duplicate work
if(currentPatch->mRoot == currentPatch)
{
while(currentPatch)
{
PCMContactPatch* nextPatch = currentPatch->mNextPatch;
if(nextPatch)
{
for(PxU32 k = currentPatch->mStartIndex; k<currentPatch->mEndIndex; ++k)
{
for(PxU32 l = nextPatch->mStartIndex; l < nextPatch->mEndIndex; ++l)
{
Vec3V dif = V3Sub(manifoldContacts[l].mLocalPointB, manifoldContacts[k].mLocalPointB);
FloatV d = V3Dot(dif, dif);
if(FAllGrtr(sqReplaceBreaking, d))
{
//if two manifold contacts are within threshold, we will get rid of the manifold contacts in the other contact patch
manifoldContacts[l] = manifoldContacts[nextPatch->mEndIndex-1];
nextPatch->mEndIndex--;
numContacts--;
l--;
}
}
}
}
currentPatch = nextPatch;
}
}
}
return numContacts;
}
// This function is for the multiple manifold loop through each individual single manifold to recalculate the contacts based on the relative transform between a pair of objects
PX_FORCE_INLINE void refreshManifold(const aos::PxMatTransformV& relTra, const aos::FloatVArg projectBreakingThreshold, const aos::FloatVArg contactDist)
{
using namespace aos;
//refresh manifold contacts
for(PxU32 i=0; i<mNumManifolds; ++i)
{
const PxU8 ind = mManifoldIndices[i];
PX_ASSERT(mManifoldIndices[i] < GU_MAX_MANIFOLD_SIZE);
const PxU32 nextInd = PxMin(i, mNumManifolds-2u)+1;
PxPrefetchLine(&mManifolds[mManifoldIndices[nextInd]]);
PxPrefetchLine(&mManifolds[mManifoldIndices[nextInd]],128);
PxPrefetchLine(&mManifolds[mManifoldIndices[nextInd]],256);
const FloatV _maxPen = mManifolds[ind].refreshContactPoints(relTra, projectBreakingThreshold, contactDist);
if(mManifolds[ind].isEmpty())
{
//swap the index with the next manifolds
const PxU8 index = mManifoldIndices[--mNumManifolds];
mManifoldIndices[mNumManifolds] = ind;
mManifoldIndices[i] = index;
i--;
}
else
{
FStore(_maxPen, &mMaxPen[ind]);
}
}
}
PX_FORCE_INLINE void initialize()
{
mNumManifolds = 0;
mNumTotalContacts = 0;
mRelativeTransform.invalidate();
for(PxU8 i=0; i<GU_MAX_MANIFOLD_SIZE; ++i)
{
mManifolds[i].initialize();
mManifoldIndices[i] = i;
}
}
PX_FORCE_INLINE void clearManifold()
{
for(PxU8 i=0; i<mNumManifolds; ++i)
{
mManifolds[i].clearManifold();
}
mNumManifolds = 0;
mNumTotalContacts = 0;
mRelativeTransform.invalidate();
}
PX_FORCE_INLINE const SinglePersistentContactManifold* getManifold(PxU32 index) const
{
PX_ASSERT(index < GU_MAX_MANIFOLD_SIZE);
return &mManifolds[mManifoldIndices[index]];
}
PX_FORCE_INLINE SinglePersistentContactManifold* getManifold(PxU32 index)
{
PX_ASSERT(index < GU_MAX_MANIFOLD_SIZE);
return &mManifolds[mManifoldIndices[index]];
}
PX_FORCE_INLINE SinglePersistentContactManifold* getEmptyManifold()
{
if(mNumManifolds < GU_MAX_MANIFOLD_SIZE)
return &mManifolds[mManifoldIndices[mNumManifolds]];
return NULL;
}
//This function adds the manifold contacts with different patches into the corresponding single persistent contact manifold
void addManifoldContactPoints(MeshPersistentContact* manifoldContact, PxU32 numManifoldContacts, PCMContactPatch** contactPatch, PxU32 numPatch,
const aos::FloatVArg sqReplaceBreakingThreshold, const aos::FloatVArg acceptanceEpsilon, PxU8 maxContactsPerManifold);
//This function adds the box/convexhull manifold contacts to the contact buffer
bool addManifoldContactsToContactBuffer(PxContactBuffer& contactBuffer, const aos::PxTransformV& transf1);
//This function adds the sphere/capsule manifold contacts to the contact buffer
bool addManifoldContactsToContactBuffer(PxContactBuffer& contactBuffer, const aos::PxTransformV& trA, const aos::PxTransformV& trB, const aos::FloatVArg radius);
void drawManifold(PxRenderOutput& out, const aos::PxTransformV& trA, const aos::PxTransformV& trB) const;
//Code to load from a buffer and store to a buffer.
void fromBuffer(PxU8* PX_RESTRICT buffer);
void toBuffer(PxU8* PX_RESTRICT buffer) const;
static void drawLine(PxRenderOutput& out, const aos::Vec3VArg p0, const aos::Vec3VArg p1, PxU32 color = 0xff00ffff);
static void drawLine(PxRenderOutput& out, const PxVec3 p0, const PxVec3 p1, PxU32 color = 0xff00ffff);
static void drawPoint(PxRenderOutput& out, const aos::Vec3VArg p, const PxF32 size, PxU32 color = 0x00ff0000);
static void drawPolygon(PxRenderOutput& out, const aos::PxTransformV& transform, aos::Vec3V* points, PxU32 numVerts, PxU32 color = 0xff00ffff);
aos::PxTransformV mRelativeTransform;//aToB
PxF32 mMaxPen[GU_MAX_MANIFOLD_SIZE];
PxU8 mManifoldIndices[GU_MAX_MANIFOLD_SIZE];
PxU8 mNumManifolds;
PxU8 mNumTotalContacts;
SinglePersistentContactManifold mManifolds[GU_MAX_MANIFOLD_SIZE];
} PX_ALIGN_SUFFIX(16);
#if PX_VC
#pragma warning(pop)
#endif
// This function calculates the average normal in the manifold in world space
PX_FORCE_INLINE aos::Vec3V PersistentContactManifold::getWorldNormal(const aos::PxTransformV& trB) const
{
using namespace aos;
Vec4V nPen = mContactPoints[0].mLocalNormalPen;
for(PxU32 i=1; i<mNumContacts; ++i)
nPen = V4Add(nPen, mContactPoints[i].mLocalNormalPen);
const Vec3V n = Vec3V_From_Vec4V(nPen);
const FloatV sqLength = V3Dot(n, n);
const Vec3V nn = V3Sel(FIsGrtr(sqLength, FEps()), n, Vec3V_From_Vec4V(mContactPoints[0].mLocalNormalPen));
return trB.rotateAndNormalize(nn);
}
// This function calculates the average normal in the manifold in local B space
PX_FORCE_INLINE aos::Vec3V PersistentContactManifold::getLocalNormal() const
{
using namespace aos;
Vec4V nPen = mContactPoints[0].mLocalNormalPen;
for(PxU32 i=1; i<mNumContacts; ++i)
nPen = V4Add(nPen, mContactPoints[i].mLocalNormalPen);
return V3Normalize(Vec3V_From_Vec4V(nPen));
}
// This function recalculates the contacts in the manifold based on the current relative transform between a pair of objects. If the recalculated contacts are within some threshold,
// we will keep the contacts; Otherwise, we will remove the contacts.
PX_FORCE_INLINE void PersistentContactManifold::refreshContactPoints(const aos::PxMatTransformV& aToB, const aos::FloatVArg projectBreakingThreshold, const aos::FloatVArg /*contactOffset*/)
{
using namespace aos;
const FloatV sqProjectBreakingThreshold = FMul(projectBreakingThreshold, projectBreakingThreshold);
// first refresh worldspace positions and distance
for (PxU32 i=mNumContacts; i > 0; --i)
{
PersistentContact& manifoldPoint = mContactPoints[i-1];
const Vec3V localAInB = aToB.transform( manifoldPoint.mLocalPointA ); // from a to b
const Vec3V localBInB = manifoldPoint.mLocalPointB;
const Vec3V v = V3Sub(localAInB, localBInB);
const Vec3V localNormal = Vec3V_From_Vec4V(manifoldPoint.mLocalNormalPen); // normal in b space
const FloatV dist = V3Dot(v, localNormal);
const Vec3V projectedPoint = V3NegScaleSub(localNormal, dist, localAInB);//manifoldPoint.worldPointA - manifoldPoint.worldPointB * manifoldPoint.m_distance1;
const Vec3V projectedDifference = V3Sub(localBInB, projectedPoint);
const FloatV distance2d = V3Dot(projectedDifference, projectedDifference);
//const BoolV con = BOr(FIsGrtr(dist, contactOffset), FIsGrtr(distance2d, sqProjectBreakingThreshold));
const BoolV con = FIsGrtr(distance2d, sqProjectBreakingThreshold);
if(BAllEqTTTT(con))
{
removeContactPoint(i-1);
}
else
{
manifoldPoint.mLocalNormalPen = V4SetW(Vec4V_From_Vec3V(localNormal), dist);
}
}
}
#define PX_CP_TO_PCP(contactPoint) (reinterpret_cast<PersistentContact*>(contactPoint)) //this is used in the normal pcm contact gen
#define PX_CP_TO_MPCP(contactPoint) (reinterpret_cast<MeshPersistentContact*>(contactPoint))//this is used in the mesh pcm contact gen
void addManifoldPoint(PersistentContact* manifoldContacts, PersistentContactManifold& manifold, GjkOutput& output,
const aos::PxMatTransformV& aToB, const aos::FloatV replaceBreakingThreshold);
}//Gu
}//physx
#endif