Skip to content

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.

8.4 KB
Raw Blame
// 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

#include "PxPhysXConfig.h"
#include "DyCorrelationBuffer.h"
#include "PxsMaterialManager.h"
#include "foundation/PxUtilities.h"
#include "foundation/PxBounds3.h"
#include "foundation/PxVecMath.h"
#include "GuBounds.h"

using namespace physx;
using namespace aos;

namespace physx
{
namespace Dy
{
static PX_FORCE_INLINE void initContactPatch(CorrelationBuffer::ContactPatchData& patch, PxU16 index, PxReal restitution, PxReal staticFriction, PxReal dynamicFriction,
	PxU8 flags)
{
	patch.start = index;
	patch.count = 1;
	patch.next = 0;
	patch.flags = flags;
	patch.restitution = restitution;
	patch.staticFriction = staticFriction;
	patch.dynamicFriction = dynamicFriction;
}

bool createContactPatches(CorrelationBuffer& fb, const PxContactPoint* cb, PxU32 contactCount, PxReal normalTolerance)
{
	// PT: this rewritten version below doesn't have LHS

	PxU32 contactPatchCount = fb.contactPatchCount;
	if(contactPatchCount == PxContactBuffer::MAX_CONTACTS)
		return false;
	if(contactCount>0)
	{
		CorrelationBuffer::ContactPatchData* PX_RESTRICT currentPatchData = fb.contactPatches + contactPatchCount;
		const PxContactPoint* PX_RESTRICT contacts = cb;

		initContactPatch(fb.contactPatches[contactPatchCount++], PxTo16(0), contacts[0].restitution, 
			contacts[0].staticFriction, contacts[0].dynamicFriction, PxU8(contacts[0].materialFlags));

		Vec4V minV = V4LoadA(&contacts[0].point.x);
		Vec4V maxV = minV;

		PxU32 patchIndex = 0;
		PxU8 count = 1;

		for (PxU32 i = 1; i<contactCount; i++)
		{
			const PxContactPoint& curContact = contacts[i];
			const PxContactPoint& preContact = contacts[patchIndex];

			if(curContact.staticFriction == preContact.staticFriction
				&& curContact.dynamicFriction == preContact.dynamicFriction
				&& curContact.restitution == preContact.restitution
				&& curContact.normal.dot(preContact.normal)>=normalTolerance)
			{
				const Vec4V ptV = V4LoadA(&curContact.point.x);
				minV = V4Min(minV, ptV);
				maxV = V4Max(maxV, ptV);

				count++;
			}
			else
			{
				if(contactPatchCount == PxContactBuffer::MAX_CONTACTS)
					return false;
				patchIndex = i;
				currentPatchData->count = count;
				count = 1;
				StoreBounds(currentPatchData->patchBounds, minV, maxV);
				currentPatchData = fb.contactPatches + contactPatchCount;

				initContactPatch(fb.contactPatches[contactPatchCount++], PxTo16(i), curContact.restitution,
					curContact.staticFriction, curContact.dynamicFriction, PxU8(curContact.materialFlags));

				minV = V4LoadA(&curContact.point.x);
				maxV = minV;
			}
		}
		if(count!=1)
			currentPatchData->count = count;

		StoreBounds(currentPatchData->patchBounds, minV, maxV);
	}
	fb.contactPatchCount = contactPatchCount;
	return true;
}

static PX_FORCE_INLINE void initFrictionPatch(FrictionPatch& p, const PxVec3& worldNormal, const PxTransform& body0Pose, const PxTransform& body1Pose, 
	PxReal restitution, PxReal staticFriction, PxReal dynamicFriction, PxU8 materialFlags)
{
	p.body0Normal = body0Pose.rotateInv(worldNormal);
	p.body1Normal = body1Pose.rotateInv(worldNormal);
	p.relativeQuat = body0Pose.q.getConjugate() * body1Pose.q;
	p.anchorCount = 0;
	p.broken = 0;
	p.staticFriction = staticFriction;
	p.dynamicFriction = dynamicFriction;
	p.restitution = restitution;
	p.materialFlags = materialFlags;
}

bool correlatePatches(CorrelationBuffer& fb, 
					  const PxContactPoint* cb,
					  const PxTransform& bodyFrame0,
					  const PxTransform& bodyFrame1,
					  PxReal normalTolerance,
					  PxU32 startContactPatchIndex,
					  PxU32 startFrictionPatchIndex)
{
	bool overflow = false;
	PxU32 frictionPatchCount = fb.frictionPatchCount;

	for(PxU32 i=startContactPatchIndex;i<fb.contactPatchCount;i++)
	{
		CorrelationBuffer::ContactPatchData &c = fb.contactPatches[i];
		const PxVec3 patchNormal = cb[c.start].normal;

		PxU32 j=startFrictionPatchIndex;
		for(;j<frictionPatchCount && ((patchNormal.dot(fb.frictionPatchWorldNormal[j]) < normalTolerance) 
			|| fb.frictionPatches[j].restitution != c.restitution|| fb.frictionPatches[j].staticFriction != c.staticFriction || 
			fb.frictionPatches[j].dynamicFriction != c.dynamicFriction);j++)
			;

		if(j==frictionPatchCount)
		{
			overflow |= j==CorrelationBuffer::MAX_FRICTION_PATCHES;
			if(overflow)
				continue;

			initFrictionPatch(fb.frictionPatches[frictionPatchCount], patchNormal, bodyFrame0, bodyFrame1, c.restitution, c.staticFriction, c.dynamicFriction, c.flags);
			fb.frictionPatchWorldNormal[j] = patchNormal;
			fb.frictionPatchContactCounts[frictionPatchCount] = c.count;
			fb.patchBounds[frictionPatchCount] = c.patchBounds;
			fb.contactID[frictionPatchCount][0] = 0xffff;
			fb.contactID[frictionPatchCount++][1] = 0xffff;
			c.next = CorrelationBuffer::LIST_END;
		}
		else
		{
			fb.patchBounds[j].include(c.patchBounds);
			fb.frictionPatchContactCounts[j] += c.count;
			c.next = PxTo16(fb.correlationListHeads[j]);
		}

		fb.correlationListHeads[j] = i;
	}

	fb.frictionPatchCount = frictionPatchCount;

	return overflow;
}

// run over the friction patches, trying to find two anchors per patch. If we already have
// anchors that are close, we keep them, which gives us persistent spring behavior

void growPatches(CorrelationBuffer& fb,
				 const PxContactPoint* cb,
				 const PxTransform& bodyFrame0,
				 const PxTransform& bodyFrame1,
				 PxU32 frictionPatchStartIndex,
				 PxReal frictionOffsetThreshold)
{
	for(PxU32 i=frictionPatchStartIndex;i<fb.frictionPatchCount;i++)
	{
		FrictionPatch& fp = fb.frictionPatches[i];

		if (fp.anchorCount == 2 || fb.correlationListHeads[i] == CorrelationBuffer::LIST_END)
		{
			const PxReal frictionPatchDiagonalSq = fb.patchBounds[i].getDimensions().magnitudeSquared();
			const PxReal anchorSqDistance = (fp.body0Anchors[0] - fp.body0Anchors[1]).magnitudeSquared();

			//If the squared distance between the anchors is more than a quarter of the patch diagonal, we can keep, 
			//otherwise the anchors are potentially clustered around a corner so force a rebuild of the patch
			if (fb.frictionPatchContactCounts[i] == 0 || (anchorSqDistance * 4.f) >= frictionPatchDiagonalSq)
				continue;

			fp.anchorCount = 0;
		}

		PxVec3 worldAnchors[2];
		PxU16 anchorCount = 0;
		PxReal pointDistSq = 0.0f, dist0, dist1;

		// if we have an anchor already, keep it
		if(fp.anchorCount == 1)
		{
			worldAnchors[anchorCount++] = bodyFrame0.transform(fp.body0Anchors[0]);
		}

		const PxReal eps = 1e-8f;

		for(PxU32 patch = fb.correlationListHeads[i]; 
			patch!=CorrelationBuffer::LIST_END; 
			patch = fb.contactPatches[patch].next)
		{
			CorrelationBuffer::ContactPatchData& cp = fb.contactPatches[patch];
			for(PxU16 j=0;j<cp.count;j++)
			{
				const PxVec3& worldPoint = cb[cp.start+j].point;

				if(cb[cp.start+j].separation < frictionOffsetThreshold)
				{
					switch(anchorCount)
					{
					case 0:
						fb.contactID[i][0] = PxU16(cp.start+j);
						worldAnchors[0] = worldPoint;
						anchorCount++;
						break;
					case 1:
						pointDistSq = (worldPoint-worldAnchors[0]).magnitudeSquared(); 
						if (pointDistSq > eps)
						{
							fb.contactID[i][1] = PxU16(cp.start+j);
							worldAnchors[1] = worldPoint;
							anchorCount++;
						}
						break;
					default: //case 2
						dist0 = (worldPoint-worldAnchors[0]).magnitudeSquared();
						dist1 = (worldPoint-worldAnchors[1]).magnitudeSquared();
						if (dist0 > dist1)
						{
							if(dist0 > pointDistSq)
							{
								fb.contactID[i][1] = PxU16(cp.start+j);
								worldAnchors[1] = worldPoint;
								pointDistSq = dist0;
							}
						}
						else if (dist1 > pointDistSq)
						{
							fb.contactID[i][0] = PxU16(cp.start+j);
							worldAnchors[0] = worldPoint;
							pointDistSq = dist1;
						}
					}
				}
			}
		}

		//PX_ASSERT(anchorCount > 0);

		// add the new anchor(s) to the patch
		for(PxU32 j = fp.anchorCount; j < anchorCount; j++)
		{
			fp.body0Anchors[j] = bodyFrame0.transformInv(worldAnchors[j]);
			fp.body1Anchors[j] = bodyFrame1.transformInv(worldAnchors[j]);
		}

		// the block contact solver always reads at least one anchor per patch for performance reasons even if there are no valid patches,
		// so we need to initialize this in the unexpected case that we have no anchors

		if(anchorCount==0)
			fp.body0Anchors[0] = fp.body1Anchors[0] = PxVec3(0);

		fp.anchorCount = anchorCount;
	}
}

}

}