XCEngine/engine/third_party/physx/source/geomutils/src/GuGeometryQuery.cpp

// 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-2025 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 "geometry/PxGeometryQuery.h"
#include "geometry/PxSphereGeometry.h"
#include "geometry/PxBoxGeometry.h"
#include "geometry/PxPlaneGeometry.h"
#include "geometry/PxCapsuleGeometry.h"
#include "geometry/PxTriangleMeshGeometry.h"
#include "geometry/PxConvexMeshGeometry.h"
#include "geometry/PxHeightFieldGeometry.h"
#include "geometry/PxParticleSystemGeometry.h"
#include "geometry/PxCustomGeometry.h"
#include "geometry/PxConvexCoreGeometry.h"
#include "foundation/PxAtomic.h"

#include "GuInternal.h"
#include "GuOverlapTests.h"
#include "GuSweepTests.h"
#include "GuRaycastTests.h"
#include "GuBoxConversion.h"
#include "GuTriangleMesh.h"
#include "GuMTD.h"
#include "GuBounds.h"
#include "GuDistancePointSegment.h"
#include "GuConvexMesh.h"
#include "GuDistancePointBox.h"
#include "GuMidphaseInterface.h"
#include "foundation/PxFPU.h"

#include "GuConvexEdgeFlags.h"
#include "GuVecBox.h"
#include "GuVecConvexHull.h"
#include "GuPCMShapeConvex.h"
#include "GuPCMContactConvexCommon.h"
#include "GuConvexSupport.h"
#include "GuConvexGeometry.h"

using namespace physx;
using namespace Gu;

extern GeomSweepFuncs gGeomSweepFuncs;
extern GeomOverlapTable gGeomOverlapMethodTable[];
extern RaycastFunc gRaycastMap[PxGeometryType::eGEOMETRY_COUNT];

///////////////////////////////////////////////////////////////////////////////

bool PxGeometryQuery::isValid(const PxGeometry& g)
{
	switch(PxU32(g.getType()))
	{
		case PxGeometryType::eSPHERE:			return static_cast<const PxSphereGeometry&>(g).isValid();
		case PxGeometryType::ePLANE:			return static_cast<const PxPlaneGeometry&>(g).isValid();
		case PxGeometryType::eCAPSULE:			return static_cast<const PxCapsuleGeometry&>(g).isValid();
		case PxGeometryType::eBOX:				return static_cast<const PxBoxGeometry&>(g).isValid();
		case PxGeometryType::eCONVEXCORE:		return static_cast<const PxConvexCoreGeometry&>(g).isValid();
		case PxGeometryType::eCONVEXMESH:		return static_cast<const PxConvexMeshGeometry&>(g).isValid();
		case PxGeometryType::eTRIANGLEMESH:		return static_cast<const PxTriangleMeshGeometry&>(g).isValid();
		case PxGeometryType::eHEIGHTFIELD:		return static_cast<const PxHeightFieldGeometry&>(g).isValid();
		case PxGeometryType::eTETRAHEDRONMESH:	return static_cast<const PxTetrahedronMeshGeometry&>(g).isValid();
		case PxGeometryType::ePARTICLESYSTEM:	return static_cast<const PxParticleSystemGeometry&>(g).isValid();
		case PxGeometryType::eCUSTOM:			return static_cast<const PxCustomGeometry&>(g).isValid();
	}
	return false;
}

///////////////////////////////////////////////////////////////////////////////

bool PxGeometryQuery::sweep(const PxVec3& unitDir, const PxReal distance,
							const PxGeometry& geom0, const PxTransform& pose0,
							const PxGeometry& geom1, const PxTransform& pose1,
							PxGeomSweepHit& sweepHit, PxHitFlags hitFlags,
							const PxReal inflation, PxGeometryQueryFlags queryFlags, PxSweepThreadContext* threadContext)
{
	PX_SIMD_GUARD_CNDT(queryFlags & PxGeometryQueryFlag::eSIMD_GUARD)
	PX_CHECK_AND_RETURN_VAL(pose0.isValid(), "PxGeometryQuery::sweep(): pose0 is not valid.", false);
	PX_CHECK_AND_RETURN_VAL(pose1.isValid(), "PxGeometryQuery::sweep(): pose1 is not valid.", false);
	PX_CHECK_AND_RETURN_VAL(unitDir.isFinite(), "PxGeometryQuery::sweep(): unitDir is not valid.", false);
	PX_CHECK_AND_RETURN_VAL(PxIsFinite(distance), "PxGeometryQuery::sweep(): distance is not valid.", false);
	PX_CHECK_AND_RETURN_VAL((distance >= 0.0f && !(hitFlags & PxHitFlag::eASSUME_NO_INITIAL_OVERLAP)) || distance > 0.0f,
		"PxGeometryQuery::sweep(): sweep distance must be >=0 or >0 with eASSUME_NO_INITIAL_OVERLAP.", 0);
#if PX_CHECKED
	if(!PxGeometryQuery::isValid(geom0))
		return PxGetFoundation().error(PxErrorCode::eINVALID_PARAMETER, PX_FL, "Provided geometry 0 is not valid");

	if(!PxGeometryQuery::isValid(geom1))
		return PxGetFoundation().error(PxErrorCode::eINVALID_PARAMETER, PX_FL, "Provided geometry 1 is not valid");
#endif

	const GeomSweepFuncs& sf = gGeomSweepFuncs;

	switch(geom0.getType())
	{
		case PxGeometryType::eSPHERE:
		{
			const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom0);

			const PxCapsuleGeometry capsuleGeom(sphereGeom.radius, 0.0f);

			const Capsule worldCapsule(pose0.p, pose0.p, sphereGeom.radius);

			const bool precise = hitFlags & PxHitFlag::ePRECISE_SWEEP;
			const SweepCapsuleFunc func = precise ? sf.preciseCapsuleMap[geom1.getType()] : sf.capsuleMap[geom1.getType()];

			return func(geom1, pose1, capsuleGeom, pose0, worldCapsule, unitDir, distance, sweepHit, hitFlags, inflation, threadContext);
		}

		case PxGeometryType::eCAPSULE:
		{
			const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom0);

			Capsule worldCapsule;
			getCapsule(worldCapsule, capsuleGeom, pose0);

			const bool precise = hitFlags & PxHitFlag::ePRECISE_SWEEP;
			const SweepCapsuleFunc func = precise ? sf.preciseCapsuleMap[geom1.getType()] : sf.capsuleMap[geom1.getType()];

			return func(geom1, pose1, capsuleGeom, pose0, worldCapsule, unitDir, distance, sweepHit, hitFlags, inflation, threadContext);
		}

		case PxGeometryType::eBOX:
		{
			const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom0);

			Box box;
			buildFrom(box, pose0.p, boxGeom.halfExtents, pose0.q);

			const bool precise = hitFlags & PxHitFlag::ePRECISE_SWEEP;
			const SweepBoxFunc func = precise ? sf.preciseBoxMap[geom1.getType()] : sf.boxMap[geom1.getType()];

			return func(geom1, pose1, boxGeom, pose0, box, unitDir, distance, sweepHit, hitFlags, inflation, threadContext);
		}

		case PxGeometryType::eCONVEXMESH:
		{
			const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom0);

			const SweepConvexFunc func = sf.convexMap[geom1.getType()];

			return func(geom1, pose1, convexGeom, pose0, unitDir, distance, sweepHit, hitFlags, inflation, threadContext);
		}
		default:
			PX_CHECK_MSG(false, "PxGeometryQuery::sweep(): first geometry object parameter must be sphere, capsule, box or convex geometry.");
	}

	return false;
}

///////////////////////////////////////////////////////////////////////////////

bool PxGeometryQuery::overlap(	const PxGeometry& geom0, const PxTransform& pose0,
								const PxGeometry& geom1, const PxTransform& pose1,
								PxGeometryQueryFlags queryFlags, PxOverlapThreadContext* threadContext)
{
	PX_SIMD_GUARD_CNDT(queryFlags & PxGeometryQueryFlag::eSIMD_GUARD)
	return Gu::overlap(geom0, pose0, geom1, pose1, gGeomOverlapMethodTable, threadContext);
}

///////////////////////////////////////////////////////////////////////////////

PxU32 PxGeometryQuery::raycast(	const PxVec3& rayOrigin, const PxVec3& rayDir,
								const PxGeometry& geom, const PxTransform& pose,
								PxReal maxDist, PxHitFlags hitFlags, PxU32 maxHits, PxGeomRaycastHit* PX_RESTRICT rayHits, PxU32 stride,
								PxGeometryQueryFlags queryFlags, PxRaycastThreadContext* threadContext)
{
	PX_SIMD_GUARD_CNDT(queryFlags & PxGeometryQueryFlag::eSIMD_GUARD)
	PX_CHECK_AND_RETURN_VAL(rayDir.isFinite(), "PxGeometryQuery::raycast(): rayDir is not valid.", 0);
	PX_CHECK_AND_RETURN_VAL(rayOrigin.isFinite(), "PxGeometryQuery::raycast(): rayOrigin is not valid.", 0);
	PX_CHECK_AND_RETURN_VAL(pose.isValid(), "PxGeometryQuery::raycast(): pose is not valid.", 0);
	PX_CHECK_AND_RETURN_VAL(maxDist >= 0.0f, "PxGeometryQuery::raycast(): maxDist is negative.", false);
	PX_CHECK_AND_RETURN_VAL(PxIsFinite(maxDist), "PxGeometryQuery::raycast(): maxDist is not valid.", false);
	PX_CHECK_AND_RETURN_VAL(PxAbs(rayDir.magnitudeSquared()-1)<1e-4f, "PxGeometryQuery::raycast(): ray direction must be unit vector.", false);

	const RaycastFunc func = gRaycastMap[geom.getType()];
	return func(geom, pose, rayOrigin, rayDir, maxDist, hitFlags, maxHits, rayHits, stride, threadContext);
}

///////////////////////////////////////////////////////////////////////////////

bool pointConvexDistance(PxVec3& normal_, PxVec3& closestPoint_, PxReal& sqDistance, const PxVec3& pt, const ConvexMesh* convexMesh, const PxMeshScale& meshScale, const PxTransform32& convexPose);

PxReal PxGeometryQuery::pointDistance(const PxVec3& point, const PxGeometry& geom, const PxTransform& pose, PxVec3* closestPoint, PxU32* closestIndex, PxGeometryQueryFlags queryFlags)
{
	PX_SIMD_GUARD_CNDT(queryFlags & PxGeometryQueryFlag::eSIMD_GUARD)
	PX_CHECK_AND_RETURN_VAL(pose.isValid(), "PxGeometryQuery::pointDistance(): pose is not valid.", -1.0f);

	switch(geom.getType())
	{
		case PxGeometryType::eSPHERE:
		{
			const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom);

			const PxReal r = sphereGeom.radius;

			PxVec3 delta = point - pose.p;
			const PxReal d = delta.magnitude();
			if(d<=r)
				return 0.0f;

			if(closestPoint)
			{
				delta /= d;
				*closestPoint = pose.p + delta * r;
			}

			return (d - r)*(d - r);
		}
		case PxGeometryType::eCAPSULE:
		{
			const PxCapsuleGeometry& capsGeom = static_cast<const PxCapsuleGeometry&>(geom);

			Capsule capsule;
			getCapsule(capsule, capsGeom, pose);

			const PxReal r = capsGeom.radius;

			PxReal param;
			const PxReal sqDistance = distancePointSegmentSquared(capsule, point, &param);
			if(sqDistance<=r*r)
				return 0.0f;

			const PxReal d = physx::intrinsics::sqrt(sqDistance);

			if(closestPoint)
			{
				const PxVec3 cp = capsule.getPointAt(param);

				PxVec3 delta = point - cp;
				delta.normalize();

				*closestPoint = cp + delta * r;
			}
			return (d - r)*(d - r);
		}
		case PxGeometryType::eBOX:
		{
			const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom);

			Box obb;
			buildFrom(obb, pose.p, boxGeom.halfExtents, pose.q);

			PxVec3 boxParam;
			const PxReal sqDistance = distancePointBoxSquared(point, obb, &boxParam);
			if(closestPoint && sqDistance!=0.0f)
			{
				*closestPoint = obb.transform(boxParam);
			}
			return sqDistance;
		}
		case PxGeometryType::eCONVEXMESH:
		{
			const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom);

			const PxTransform32 poseA(pose);

			PxVec3 normal, cp;
			PxReal sqDistance;
			const bool intersect = pointConvexDistance(normal, cp, sqDistance, point, static_cast<ConvexMesh*>(convexGeom.convexMesh), convexGeom.scale, poseA);
			if(!intersect && closestPoint)
				*closestPoint = cp;
			return sqDistance;
		}
		case PxGeometryType::eCONVEXCORE:
		{
			const PxConvexCoreGeometry& convexGeom = static_cast<const PxConvexCoreGeometry&>(geom);

			// Create a point support
			Gu::ConvexShape pointShape;
			pointShape.coreType = Gu::ConvexCore::Type::ePOINT;
			pointShape.pose = PxTransform(PxIdentity);
			pointShape.margin = 0.0f;

			// Create convex core shape
			Gu::ConvexShape convexShape;
			Gu::makeConvexShape(convexGeom, pose, convexShape);
			PX_ASSERT(convexShape.isValid());

			// Compute distance using GJK
			PxVec3 pointA, pointB, axis;
			const PxReal distance = Gu::RefGjkEpa::computeGjkDistance(pointShape, convexShape, PxTransform(PxIdentity), pose, FLT_MAX, pointA, pointB, axis);

			if(closestPoint)
				*closestPoint = pointB;

			return distance * distance;
		}
		case PxGeometryType::eTRIANGLEMESH:
		{
			const PxTriangleMeshGeometry& meshGeom = static_cast<const PxTriangleMeshGeometry&>(geom);

			PxU32 index;
			float dist;
			PxVec3 cp;
			Midphase::pointMeshDistance(static_cast<TriangleMesh*>(meshGeom.triangleMesh), meshGeom, pose, point, FLT_MAX, index, dist, cp);
			if(closestPoint)
				*closestPoint = cp;
			if(closestIndex)
				*closestIndex = index;
			return dist*dist;
		}
		default:
			PX_CHECK_MSG(false, "PxGeometryQuery::pointDistance(): geometry object parameter must be sphere, capsule, box, convex or mesh geometry.");
			break;
	}
	return -1.0f;
}

///////////////////////////////////////////////////////////////////////////////

void PxGeometryQuery::computeGeomBounds(PxBounds3& bounds, const PxGeometry& geom, const PxTransform& pose, float offset, float inflation, PxGeometryQueryFlags queryFlags)
{
	PX_SIMD_GUARD_CNDT(queryFlags & PxGeometryQueryFlag::eSIMD_GUARD)
	PX_CHECK_AND_RETURN(pose.isValid(), "PxGeometryQuery::computeGeomBounds(): pose is not valid.");

	Gu::computeBounds(bounds, geom, pose, offset, inflation);
	PX_ASSERT(bounds.isValid());
}

///////////////////////////////////////////////////////////////////////////////

extern GeomMTDFunc gGeomMTDMethodTable[][PxGeometryType::eGEOMETRY_COUNT];

bool PxGeometryQuery::computePenetration(	PxVec3& mtd, PxF32& depth,
											const PxGeometry& geom0, const PxTransform& pose0,
											const PxGeometry& geom1, const PxTransform& pose1, PxGeometryQueryFlags queryFlags)
{
	PX_SIMD_GUARD_CNDT(queryFlags & PxGeometryQueryFlag::eSIMD_GUARD)
	PX_CHECK_AND_RETURN_VAL(pose0.isValid(), "PxGeometryQuery::computePenetration(): pose0 is not valid.", false);
	PX_CHECK_AND_RETURN_VAL(pose1.isValid(), "PxGeometryQuery::computePenetration(): pose1 is not valid.", false);

	const PxTransform32 pose0A(pose0);
	const PxTransform32 pose1A(pose1);

	if(geom0.getType() > geom1.getType())
	{
		GeomMTDFunc mtdFunc = gGeomMTDMethodTable[geom1.getType()][geom0.getType()];
		PX_ASSERT(mtdFunc);
		if(!mtdFunc(mtd, depth, geom1, pose1A, geom0, pose0A))
			return false;
		mtd = -mtd;
		return true;
	}
	else
	{
		GeomMTDFunc mtdFunc = gGeomMTDMethodTable[geom0.getType()][geom1.getType()];
		PX_ASSERT(mtdFunc);
		return mtdFunc(mtd, depth, geom0, pose0A, geom1, pose1A);
	}
}

///////////////////////////////////////////////////////////////////////////////

bool PxGeometryQuery::generateTriangleContacts(const PxGeometry& geom, const PxTransform& pose, const PxVec3 triangleVertices[3], PxU32 triangleIndex, PxReal contactDistance, PxReal meshContactMargin, PxReal toleranceLength, PxContactBuffer& contactBuffer)
{
	using namespace aos;

	const PxU32 triangleIndices[3]{ 0, 1, 2 };
	PxInlineArray<PxU32, LOCAL_PCM_CONTACTS_SIZE> deferredContacts;
	Gu::MultiplePersistentContactManifold multiManifold;
	multiManifold.initialize();
	PxContactBuffer contactBuffer0; contactBuffer0.reset();
	const PxTransformV geomTransform = loadTransformU(pose);
	const PxTransformV triangleTransform = loadTransformU(PxTransform(PxIdentity));
	float radius0 = 0;
	float radius1 = meshContactMargin;

	PxU32 oldCount = contactBuffer.count;

	switch (geom.getType())
	{
		case PxGeometryType::eCAPSULE:
		{
			const PxCapsuleGeometry& capsule = static_cast<const PxCapsuleGeometry&>(geom);
			radius0 = capsule.radius;

			const FloatV capsuleRadius = FLoad(capsule.radius);
			const FloatV contactDist = FLoad(contactDistance + meshContactMargin);
			const FloatV replaceBreakingThreshold = FMul(capsuleRadius, FLoad(0.001f));

			const PxTransformV capsuleTransform = geomTransform;
			const PxTransformV meshTransform = triangleTransform;

			multiManifold.setRelativeTransform(capsuleTransform);

			const Gu::CapsuleV capsuleV(V3LoadU(pose.p), V3LoadU(pose.q.rotate(PxVec3(capsule.halfHeight, 0, 0))), capsuleRadius);

			Gu::PCMCapsuleVsMeshContactGeneration contactGeneration(capsuleV, contactDist, replaceBreakingThreshold, capsuleTransform, meshTransform, multiManifold, contactBuffer0, &deferredContacts);
			contactGeneration.processTriangle(triangleVertices, triangleIndex, Gu::ETD_CONVEX_EDGE_ALL, triangleIndices);
			contactGeneration.processContacts(GU_CAPSULE_MANIFOLD_CACHE_SIZE, false);

			break;
		}
		case PxGeometryType::eBOX:
		{
			const PxBoxGeometry& box = static_cast<const PxBoxGeometry&>(geom);

			const PxBounds3 hullAABB(-box.halfExtents, box.halfExtents);
			const Vec3V boxExtents = V3LoadU(box.halfExtents);
			const FloatV minMargin = Gu::CalculatePCMBoxMargin(boxExtents, toleranceLength, GU_PCM_MESH_MANIFOLD_EPSILON);

			Cm::FastVertex2ShapeScaling idtScaling;

			const FloatV contactDist = FLoad(contactDistance + meshContactMargin);
			const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));

			const BoxV boxV(V3Zero(), boxExtents);

			const PxTransformV boxTransform = geomTransform;
			const PxTransformV meshTransform = triangleTransform;

			PolygonalData polyData;
			PCMPolygonalBox polyBox(box.halfExtents);
			polyBox.getPolygonalData(&polyData);

			const Mat33V identity = M33Identity();
			SupportLocalImpl<BoxV> boxMap(boxV, boxTransform, identity, identity, true);

			Gu::PCMConvexVsMeshContactGeneration contactGeneration(contactDist, replaceBreakingThreshold, boxTransform, meshTransform, multiManifold, contactBuffer0, polyData, &boxMap, &deferredContacts, idtScaling, true, true, NULL);
			contactGeneration.processTriangle(triangleVertices, triangleIndex, Gu::ETD_CONVEX_EDGE_ALL, triangleIndices);
			contactGeneration.generateLastContacts();
			contactGeneration.processContacts(GU_SINGLE_MANIFOLD_CACHE_SIZE, false);

			break;
		}
		case PxGeometryType::eCONVEXMESH:
		{
			const PxConvexMeshGeometry& convex = static_cast<const PxConvexMeshGeometry&>(geom);

			const ConvexHullData* hullData = _getHullData(convex);

			Cm::FastVertex2ShapeScaling convexScaling;
			PxBounds3 hullAABB;
			PolygonalData polyData;
			const bool idtConvexScale = getPCMConvexData(convex, convexScaling, hullAABB, polyData);
			const QuatV vQuat = QuatVLoadU(&convex.scale.rotation.x);
			const Vec3V vScale = V3LoadU_SafeReadW(convex.scale.scale);
			const FloatV minMargin = CalculatePCMConvexMargin(hullData, vScale, toleranceLength, GU_PCM_MESH_MANIFOLD_EPSILON);

			const ConvexHullV convexHull(hullData, V3Zero(), vScale, vQuat, idtConvexScale);

			const FloatV contactDist = FLoad(contactDistance + meshContactMargin);
			const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));

			const PxTransformV convexTransform = geomTransform;
			const PxTransformV meshTransform = triangleTransform;

			SupportLocalImpl<Gu::ConvexHullV> convexMap(convexHull, convexTransform, convexHull.vertex2Shape, convexHull.shape2Vertex, false);

			Gu::PCMConvexVsMeshContactGeneration contactGeneration(contactDist, replaceBreakingThreshold, convexTransform, meshTransform, multiManifold, contactBuffer0, polyData, &convexMap, &deferredContacts, convexScaling, idtConvexScale, true, NULL);
			contactGeneration.processTriangle(triangleVertices, triangleIndex, Gu::ETD_CONVEX_EDGE_ALL, triangleIndices);
			contactGeneration.generateLastContacts();
			contactGeneration.processContacts(GU_SINGLE_MANIFOLD_CACHE_SIZE, false);

			break;
		}
		case PxGeometryType::eCONVEXCORE:
		{
			const PxConvexCoreGeometry& convex = static_cast<const PxConvexCoreGeometry&>(geom);

			Gu::ConvexShape convexShape;
			Gu::makeConvexShape(convex, pose, convexShape);
			PX_ASSERT(convexShape.isValid());

			// Create the triangle shape as a points-based convex shape
			Gu::ConvexShape triShape;
			triShape.coreType = Gu::ConvexCore::Type::ePOINTS;
			triShape.pose = PxTransform(PxIdentity);
			triShape.margin = meshContactMargin;
			// Initialize the points core data
			Gu::ConvexCore::PointsCore triCore;
			triCore.points = triangleVertices;
			triCore.numPoints = 3;
			triCore.stride = sizeof(PxVec3);
			triCore.S = PxVec3(1.0f);
			triCore.R = PxQuat(PxIdentity);
			// Copy the core data into the shape's core data buffer
			PX_ASSERT(sizeof(triCore) <= Gu::ConvexCore::MAX_CORE_SIZE);
			PxMemCopy(triShape.coreData, &triCore, sizeof(triCore));

			PxVec3 normal, points[Gu::MAX_CONVEX_CONTACTS];
			PxReal dists[Gu::MAX_CONVEX_CONTACTS];
			if (PxU32 count = Gu::generateContacts(convexShape, triShape, contactDistance + meshContactMargin, normal, points, dists))
			{
				for (PxU32 i = 0; i < count; ++i)
				{
					PxContactPoint contact;
					contact.point = points[i];
					contact.normal = normal;
					contact.separation = dists[i];
					contact.internalFaceIndex1 = triangleIndex;
					contactBuffer.contact(contact);
				}
			}

			break;
		}
		default:
			PX_ASSERT(0); // Unsupported geometry type
			break;
	}

	for (PxU32 manifoldIndex = 0; manifoldIndex < multiManifold.mNumManifolds; ++manifoldIndex)
	{
		Gu::SinglePersistentContactManifold& manifold = *multiManifold.getManifold(manifoldIndex);
		PxVec3 normal; V3StoreU(manifold.getWorldNormal(triangleTransform), normal);
		for (PxU32 contactIndex = 0; contactIndex < manifold.getNumContacts(); ++contactIndex)
		{
			Gu::MeshPersistentContact& meshContact = manifold.getContactPoint(contactIndex);
			PxContactPoint contact;
			PxVec3 p0; V3StoreU(geomTransform.transform(meshContact.mLocalPointA), p0); p0 -= normal * radius0;
			PxVec3 p1; V3StoreU(meshContact.mLocalPointB, p1); p1 += normal * radius1;
			contact.point = (p0 + p1) * 0.5f;
			contact.normal = normal;
			contact.separation = normal.dot(p0 - p1);
			contact.internalFaceIndex1 = triangleIndex;
			contactBuffer.contact(contact);
		}
	}

	return oldCount < contactBuffer.count;
}

///////////////////////////////////////////////////////////////////////////////

PxU32 PxCustomGeometry::getUniqueID()
{
    static PxU32 uniqueID(0);
    PxAtomicIncrement(reinterpret_cast<volatile PxI32*>(&uniqueID));
    return uniqueID;
}