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.

52.3 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 "GuMeshDepenetration.h"
#include "GuLocalCluster.h"
#include "geometry/PxTriangleMeshGeometry.h"
#include "geometry/PxMeshQuery.h"
#include "geometry/PxBoxGeometry.h"
#include "foundation/PxSIMDHelpers.h"
#include "foundation/PxFPU.h"
#include "foundation/PxMat34.h"
#include "foundation/PxVecMath.h"
#include "CmRandom.h"
#include "GuEdgeList.h"
#include <stdio.h>

#define VERIFY_SIMD_CODE		0
#define SIMD_EPILOG				1
#define TEST_SIMD_CROSS			0	// ### faster in some cases, slower in others
#define USE_PX_TRANSPOSE_44_34	0	// ### slower!
#define DEP_OFFSET				1.0f
#define USE_2D_SWEEP_BACKUP		1

// PT: this one is tricky and not equivalent to other epsilons used to test cross products.
// - if it is too small it creates inaccuracies in reported time of impact for small triangles.
// - If it is too large it discards too many small triangles and collisions can be missed entirely.
#define DIR_DOT_AXIS_EPSILON	1.0e-5f
//#define DIR_DOT_AXIS_EPSILON	1.0e-6f

//#define testSeparationAxes testSeparationAxes_Scalar
//#define testSeparationAxes testSeparationAxes_SIMD1
#define testSeparationAxes testSeparationAxes_SIMD2

using namespace physx;
using namespace Gu;
using namespace aos;

// PT: TODO: refactor with other versions in the codebase
static PX_FORCE_INLINE void projectTriangle(const PxVec3& dir, const PxVec3* PX_RESTRICT triangle, float& min1, float& max1)
{
	const float dp0 = triangle[0].dot(dir);
	const float dp1 = triangle[1].dot(dir);
	const float dp2 = triangle[2].dot(dir);

	const float partialMin1 = PxMin(dp0, dp1);
	const float partialMax1 = PxMax(dp0, dp1);
	min1 = PxMin(partialMin1, dp2);
	max1 = PxMax(partialMax1, dp2);
}

typedef	PxIntBool RetType;
typedef	PxIntBool MTDType;

#define TEST_OVERLAP									\
	const float d0 = triMin0 - triMax1;					\
	const float d1 = triMax0 - triMin1;					\
	const MTDType bIntersect = (d0<=0.0f && d1>=0.0f);	\
	bValidMTD &= bIntersect;

static PX_FORCE_INLINE RetType testAxis(
	const PxTriangle& tri0, const PxTriangle& tri1,
	const PxVec3& dir, const PxVec3& axis, MTDType& bValidMTD, float& tfirst, float& tlast)
{
	float triMin0, triMax0;
	projectTriangle(axis, tri0.verts, triMin0, triMax0);

	////////

	float triMin1, triMax1;
	projectTriangle(axis, tri1.verts, triMin1, triMax1);

	////////

	TEST_OVERLAP

	const float v = dir.dot(axis);
	if(PxAbs(v) < DIR_DOT_AXIS_EPSILON)
		return bIntersect;
	const float oneOverV = -1.0f / v;

	const float t0_ = d0 * oneOverV;
	const float t1_ = d1 * oneOverV;
	const float t0 = PxMin(t0_, t1_);
	const float t1 = PxMax(t0_, t1_);

	if(t0 > tlast)
		return false;
	if(t1 < tfirst)
		return false;

	tlast = PxMin(t1, tlast);

	tfirst = PxMax(t0, tfirst);

	return true;
}

#if USE_2D_SWEEP_BACKUP
// PT: force no-inline for this backup procedure, to avoid bloating the main codepath
static PX_NOINLINE int sweep2D(
	const PxTriangle& tri0, const PxTriangle& tri1,
	const PxVec3& normal0, const PxVec3& normal1,
	const PxVec3& dir, MTDType& bValidMTD, float& tfirst, float& tlast)
{
	// PT: only a scalar version for now. Probably ok, as it should rarely run.
	const float v0 = dir.dot(normal0);
	const float v1 = dir.dot(normal1);
	if(PxAbs(v0) < 1.0e-6f && PxAbs(v1) < 1.0e-6f)
	{
		const PxVec3 e0a = (tri0.verts[1] - tri0.verts[0]).cross(normal0);
		if((e0a.dot(e0a))>=1.0e-6f && !testAxis(tri0, tri1, dir, e0a, bValidMTD, tfirst, tlast))
			return 0;

		const PxVec3 e0b = (tri0.verts[2] - tri0.verts[1]).cross(normal0);
		if((e0b.dot(e0b))>=1.0e-6f && !testAxis(tri0, tri1, dir, e0b, bValidMTD, tfirst, tlast))
			return 0;

		const PxVec3 e0c = (tri0.verts[0] - tri0.verts[2]).cross(normal0);
		if((e0c.dot(e0c))>=1.0e-6f && !testAxis(tri0, tri1, dir, e0c, bValidMTD, tfirst, tlast))
			return 0;

		const PxVec3 e1a = (tri1.verts[1] - tri1.verts[0]).cross(normal1);
		if((e1a.dot(e1a))>=1.0e-6f && !testAxis(tri0, tri1, dir, e1a, bValidMTD, tfirst, tlast))
			return 0;

		const PxVec3 e1b = (tri1.verts[2] - tri1.verts[1]).cross(normal1);
		if((e1b.dot(e1b))>=1.0e-6f && !testAxis(tri0, tri1, dir, e1b, bValidMTD, tfirst, tlast))
			return 0;

		const PxVec3 e1c = (tri1.verts[0] - tri1.verts[2]).cross(normal1);
		if((e1c.dot(e1c))>=1.0e-6f && !testAxis(tri0, tri1, dir, e1c, bValidMTD, tfirst, tlast))
			return 0;
	}
	return 1;
}
#endif

static PX_FORCE_INLINE int processTimeOfImpactResults(
	const PxTriangle& tri0, const PxTriangle& tri1,
	const PxVec3& normal0, const PxVec3& normal1,
	const PxVec3& dir, float& tcoll, float tfirst, float tlast, float tmax, MTDType bValidMTD)
{
#if USE_2D_SWEEP_BACKUP
	// PT: we can reach this place with tfirst left to its initial value in 2D cases, e.g. when two 2D triangles in
	// a plane are swept against each-other. In most PhysX usages this does not create problems, but it does for the
	// mesh depenetration code. Running the extra 2D tests has a performance cost but in the context of clash detection
	// it is unfortunately necessary.
	if(tfirst == -FLT_MAX)
	{
		if(!sweep2D(tri0, tri1, normal0, normal1, dir, bValidMTD, tfirst, tlast))
			return 0;
	}
#else
	PX_UNUSED(tri0);
	PX_UNUSED(tri1);
	PX_UNUSED(normal0);
	PX_UNUSED(normal1);
	PX_UNUSED(dir);
#endif

	if(tfirst > tmax || tlast < 0.0f)
		return 0;

	if(tfirst <= 0.0f)
	{
		// PT: in very rare cases where everything falls below the 1.0e-6f limit we can reach this place with tfirst
		// left to its initial value. This does not mean we had an initial overlap, so we should not return "tcoll = 0"
		// in this case. Safest course of action is to pretend there were no collisions.
		if(tfirst == -FLT_MAX)
			return 0;

		if(!bValidMTD)
			return 0;
		tcoll = 0.0f;
//tcoll = tfirst;
	}
	else tcoll = tfirst;

	return 1;
}

static PX_FORCE_INLINE int testSeparationAxes_Scalar(
	const PxTriangle& tri0, const PxTriangle& tri1,
	const PxVec3& normal0, const PxVec3& normal1,
	const PxVec3& dir, float tmax, float& tcoll)
{
	MTDType bValidMTD = true;
	float tfirst = -FLT_MAX;
	float tlast  = FLT_MAX;

	// Triangle normals
	if(!testAxis(tri0, tri1, dir, normal0, bValidMTD, tfirst, tlast))
		return 0;
	if(!testAxis(tri0, tri1, dir, normal1, bValidMTD, tfirst, tlast))
		return 0;

	// Edges
	for(PxU32 i=0; i<3; i++)
	{
		int ip1 = int(i+1);
		if(i>=2)	ip1 = 0;
		const PxVec3 triEdge0 = tri0.verts[ip1] - tri0.verts[i];

		{
			const PxVec3 triEdge1 = tri1.verts[1] - tri1.verts[0];
			const PxVec3 sep = triEdge0.cross(triEdge1);
			if((sep.dot(sep))>=1.0e-6f && !testAxis(tri0, tri1, dir, sep, bValidMTD, tfirst, tlast))
				return 0;
		}
		{
			const PxVec3 triEdge1 = tri1.verts[2] - tri1.verts[1];
			const PxVec3 sep = triEdge0.cross(triEdge1);
			if((sep.dot(sep))>=1.0e-6f && !testAxis(tri0, tri1, dir, sep, bValidMTD, tfirst, tlast))
				return 0;
		}
		{
			const PxVec3 triEdge1 = tri1.verts[0] - tri1.verts[2];
			const PxVec3 sep = triEdge0.cross(triEdge1);
			if((sep.dot(sep))>=1.0e-6f && !testAxis(tri0, tri1, dir, sep, bValidMTD, tfirst, tlast))
				return 0;
		}
	}

	return processTimeOfImpactResults(tri0, tri1, normal0, normal1, dir, tcoll, tfirst, tlast, tmax, bValidMTD);
}

	static PX_FORCE_INLINE PxIntBool epilogue(
#if SIMD_EPILOG
		PxU32 cndt, float t0, float t1,
#else
		float d0, float d1, const PxVec3& dir, const PxVec3& axis,
#endif
		PxIntBool bIntersect, PxIntBool& bValidMTD, float& tfirst, float& tlast
	)
	{
		bValidMTD &= bIntersect;

#if SIMD_EPILOG
		if(cndt)
			return bIntersect;
#else
		const float v = dir.dot(axis);

		if(PxAbs(v) < DIR_DOT_AXIS_EPSILON)
			return bIntersect;

		const float oneOverV = -1.0f / v;
		const float t0_ = d0 * oneOverV;
		const float t1_ = d1 * oneOverV;
		const float t0 = PxMin(t0_, t1_);
		const float t1 = PxMax(t0_, t1_);
#endif

		if(t0 > tlast)
			return false;
		if(t1 < tfirst)
			return false;

		tlast = PxMin(t1, tlast);
		tfirst = PxMax(t0, tfirst);

		return true;
	}

	static PX_FORCE_INLINE void projectTriangle4(
		const Vec4VArg axesXV, const Vec4VArg axesYV, const Vec4VArg axesZV,
		const PxTrianglePadded& PX_RESTRICT triangle,
		Vec4V* PX_RESTRICT triMin, Vec4V* PX_RESTRICT triMax)
	{
		const Vec4V v0V = V4LoadU(&triangle.verts[0].x);
		Vec4V dp0V = V4Scale(axesXV, V4GetX(v0V));
		dp0V = V4ScaleAdd(axesYV, V4GetY(v0V), dp0V);
		dp0V = V4ScaleAdd(axesZV, V4GetZ(v0V), dp0V);

		const Vec4V v1V = V4LoadU(&triangle.verts[1].x);
		Vec4V dp1V = V4Scale(axesXV, V4GetX(v1V));
		dp1V = V4ScaleAdd(axesYV, V4GetY(v1V), dp1V);
		dp1V = V4ScaleAdd(axesZV, V4GetZ(v1V), dp1V);

		const Vec4V v2V = V4LoadU(&triangle.verts[2].x);
		Vec4V dp2V = V4Scale(axesXV, V4GetX(v2V));
		dp2V = V4ScaleAdd(axesYV, V4GetY(v2V), dp2V);
		dp2V = V4ScaleAdd(axesZV, V4GetZ(v2V), dp2V);

		// PT: this version is slower on platforms that support SSE2 but it could be faster on platforms where shuffles are slow
		/*
			Vec4V dp0V = V4Mul(V4Load(triangle.verts[0].x), axesXV);
			dp0V = V4Add(dp0V, V4Mul(V4Load(triangle.verts[0].y), axesYV));
			dp0V = V4Add(dp0V, V4Mul(V4Load(triangle.verts[0].z), axesZV));

			Vec4V dp1V = V4Mul(V4Load(triangle.verts[1].x), axesXV);
			dp1V = V4Add(dp1V, V4Mul(V4Load(triangle.verts[1].y), axesYV));
			dp1V = V4Add(dp1V, V4Mul(V4Load(triangle.verts[1].z), axesZV));

			Vec4V dp2V = V4Mul(V4Load(triangle.verts[2].x), axesXV);
			dp2V = V4Add(dp2V, V4Mul(V4Load(triangle.verts[2].y), axesYV));
			dp2V = V4Add(dp2V, V4Mul(V4Load(triangle.verts[2].z), axesZV));
		*/
		*triMin = V4Min(V4Min(dp0V, dp1V), dp2V);
		*triMax = V4Max(V4Max(dp0V, dp1V), dp2V);
	}

	static PX_FORCE_INLINE void testAxes(
		const PxTrianglePadded& tri0, const PxTrianglePadded& tri1,
		const PxVec3& axis0, const PxVec3& axis1,
		const PxVec3& axis2, const PxVec3& axis3,
		PxU32* intersect,
#if SIMD_EPILOG
		const PxVec3& dir,
		PxU32* cndt,
		float* t0,
		float* t1
#else
		float* d0,
		float* d1
#endif
		)
	{
#if USE_PX_TRANSPOSE_44_34
		Vec4V axis0V = V4LoadU(&axis0.x);
		Vec4V axis1V = V4LoadU(&axis1.x);
		Vec4V axis2V = V4LoadU(&axis2.x);
		Vec4V axis3V = V4LoadU(&axis3.x);

		Vec4V axesXV, axesYV, axesZV;
		PX_TRANSPOSE_44_34(axis0V, axis1V, axis2V, axis3V, axesXV, axesYV, axesZV);
#else
		const Vec4V axesXV = V4LoadXYZW(axis0.x, axis1.x, axis2.x, axis3.x);
		const Vec4V axesYV = V4LoadXYZW(axis0.y, axis1.y, axis2.y, axis3.y);
		const Vec4V axesZV = V4LoadXYZW(axis0.z, axis1.z, axis2.z, axis3.z);
#endif
		Vec4V triMin0, triMax0;
		projectTriangle4(axesXV, axesYV, axesZV, tri0, &triMin0, &triMax0);

		Vec4V triMin1, triMax1;
		projectTriangle4(axesXV, axesYV, axesZV, tri1, &triMin1, &triMax1);

		const Vec4V d0V = V4Sub(triMin0, triMax1);
		const Vec4V d1V = V4Sub(triMax0, triMin1);

#if !SIMD_EPILOG
		V4StoreA(d0V, d0);
		V4StoreA(d1V, d1);
#endif
		//const bool bIntersect = (d0<=0.0f && d1>=0.0f);
		const Vec4V zeroV = V4Zero();
		const BoolV d0b = V4IsGrtrOrEq(zeroV, d0V);
		const BoolV d1b = V4IsGrtrOrEq(d1V, zeroV);
		BStoreA(BAnd(d0b, d1b), intersect);

#if SIMD_EPILOG
		Vec4V vV = V4Mul(V4Load(dir.x), axesXV);
		vV = V4Add(vV, V4Mul(V4Load(dir.y), axesYV));
		vV = V4Add(vV, V4Mul(V4Load(dir.z), axesZV));	//const float v = dir.dot(axis);

		const Vec4V epsilonV = V4Load(DIR_DOT_AXIS_EPSILON);
		BStoreA(V4IsGrtr(epsilonV, V4Abs(vV)), cndt);	//if(PxAbs(v) < DIR_DOT_AXIS_EPSILON)

		const Vec4V oneOverVV = V4Neg(V4Recip(vV));		//const float oneOverV = -1.0f / v;
		const Vec4V t0_V = V4Mul(d0V, oneOverVV);		//const float t0_ = d0 * oneOverV;
		const Vec4V t1_V = V4Mul(d1V, oneOverVV);		//const float t1_ = d1 * oneOverV;
		const Vec4V t0V = V4Min(t0_V, t1_V);			//const float t0 = PxMin(t0_, t1_);
		const Vec4V t1V = V4Max(t0_V, t1_V);			//const float t1 = PxMax(t0_, t1_);
		V4StoreA(t0V, t0);
		V4StoreA(t1V, t1);
#endif
	}

	static PX_FORCE_INLINE int processAxes4(
		const PxVec3& sep0, const PxVec3& sep1, const PxVec3& sep2, const PxVec3& sep3,
		bool invalid0, bool invalid1, bool invalid2, bool invalid3,
		const PxTrianglePadded& tri0, const PxTrianglePadded& tri1, const PxVec3& dir,
		PxIntBool& bValidMTD, float& tfirst, float& tlast)
	{
		PX_ALIGN(16, PxU32 intersect[4]);
#if SIMD_EPILOG
		PX_ALIGN(16, PxU32 cndt[4]);
		PX_ALIGN(16, float t0[4]);
		PX_ALIGN(16, float t1[4]);
#else
		PX_ALIGN(16, float d0[4]);
		PX_ALIGN(16, float d1[4]);
#endif

		testAxes(tri0, tri1, sep0, sep1, sep2, sep3, intersect
#if SIMD_EPILOG
				, dir, cndt, t0, t1
#else
				, d0, d1
#endif
				);

		if(!invalid0 && !epilogue(
#if SIMD_EPILOG
			cndt[0], t0[0], t1[0],
#else
			d0[0], d1[0], dir, sep0,
#endif
			intersect[0], bValidMTD, tfirst, tlast))
			return 0;

		if(!invalid1 && !epilogue(
#if SIMD_EPILOG
			cndt[1], t0[1], t1[1],
#else
			d0[1], d1[1], dir, sep1,
#endif
			intersect[1], bValidMTD, tfirst, tlast))
			return 0;

		if(!invalid2 && !epilogue(
#if SIMD_EPILOG
			cndt[2], t0[2], t1[2],
#else
			d0[2], d1[2], dir, sep2,
#endif			
			intersect[2], bValidMTD, tfirst, tlast))
			return 0;

		if(!invalid3 && !epilogue(
#if SIMD_EPILOG
			cndt[3], t0[3], t1[3],
#else
			d0[3], d1[3], dir, sep3,
#endif	
			intersect[3], bValidMTD, tfirst, tlast))
			return 0;

		return 1;
	}

	static PX_FORCE_INLINE int test(PxU32& nbAxes, PxVec3* axes,
									const PxTrianglePadded& tri0, const PxTrianglePadded& tri1, const PxVec3& dir,
									PxIntBool& bValidMTD, float& tfirst, float& tlast)
	{
		const PxU32 savedNbAxes = nbAxes;
		nbAxes = 0;

		const bool validate1 = savedNbAxes > 1;
		const bool validate2 = savedNbAxes > 2;
		const bool validate3 = savedNbAxes > 3;

		const PxVec3& sep0 = axes[0];
		const PxVec3& sep1 = axes[1];
		const PxVec3& sep2 = axes[2];
		const PxVec3& sep3 = axes[3];

		return processAxes4(sep0, sep1, sep2, sep3,
							false, !validate1, !validate2, !validate3,
							tri0, tri1, dir, bValidMTD, tfirst, tlast);
	}

	static PX_FORCE_INLINE int test4(PxU32& nbAxes, PxVec3* axes,
									const PxTrianglePadded& tri0, const PxTrianglePadded& tri1, const PxVec3& dir,
									PxIntBool& bValidMTD, float& tfirst, float& tlast)
	{
		nbAxes = 0;
		return processAxes4(axes[0], axes[1], axes[2], axes[3],
							false, false, false, false,
							tri0, tri1, dir, bValidMTD, tfirst, tlast);
	}

	static PX_FORCE_INLINE int testSeparationAxes_SIMD1(
		const PxTrianglePadded& tri0, const PxTrianglePadded& tri1,
		const PxVec3& normal0, const PxVec3& normal1,
		const PxVec3& dir, float tmax, float& tcoll)
	{
		PxIntBool bValidMTD = true;
		float tfirst = -FLT_MAX;
		float tlast  = FLT_MAX;

		// Triangle normals
		if(!processAxes4(normal0, normal1, PxVec3(0.0f), PxVec3(0.0f),
						false, false, true, true,
						tri0, tri1, dir, bValidMTD, tfirst, tlast))
			return 0;

#if TEST_SIMD_CROSS
		const Vec4V epsilonV = V4Load(1.0e-6f);
#endif

		// Edges
		for(PxU32 i=0; i<3; i++)
		{
			int ip1 = int(i+1);
			if(i>=2)	ip1 = 0;
#if TEST_SIMD_CROSS
			const Vec4V v0_0V = V4LoadU(&tri0.verts[ip1].x);
			const Vec4V v0_1V = V4LoadU(&tri0.verts[i].x);
			const Vec4V triEdge0V = V4Sub(v0_1V, v0_0V);
#else
			const PxVec3 triEdge0 = tri0.verts[ip1] - tri0.verts[i];
#endif
			{
#if TEST_SIMD_CROSS
				const Vec4V v1_0V = V4LoadU(&tri1.verts[0].x);
				const Vec4V v1_1V = V4LoadU(&tri1.verts[1].x);
				const Vec4V v1_2V = V4LoadU(&tri1.verts[2].x);

				const Vec4V triEdge1aV = V4Sub(v1_1V, v1_0V);
				const Vec4V triEdge1bV = V4Sub(v1_2V, v1_1V);
				const Vec4V triEdge1cV = V4Sub(v1_0V, v1_2V);

				const Vec4V sep0V = V4Cross(triEdge0V, triEdge1aV);
				const Vec4V sep1V = V4Cross(triEdge0V, triEdge1bV);
				const Vec4V sep2V = V4Cross(triEdge0V, triEdge1cV);

				PxVec3Padded sep0, sep1, sep2;
				bool invalid0, invalid1, invalid2;

				if(BGetBitMask(V4IsGrtrOrEq(V4Mul(sep0V, sep0V), epsilonV)) & 7)
				{
					V4StoreU(sep0V, &sep0.x);
					invalid0 = false;
				}
				else
				{
					sep0 = PxVec3(0.0f);
					invalid0 = true;
				}

				if(BGetBitMask(V4IsGrtrOrEq(V4Mul(sep1V, sep1V), epsilonV)) & 7)
				{
					V4StoreU(sep1V, &sep1.x);
					invalid1 = false;
				}
				else
				{
					sep1 = PxVec3(0.0f);
					invalid1 = true;
				}

				if(BGetBitMask(V4IsGrtrOrEq(V4Mul(sep2V, sep2V), epsilonV)) & 7)
				{
					V4StoreU(sep2V, &sep2.x);
					invalid2 = false;
				}
				else
				{
					sep2 = PxVec3(0.0f);
					invalid2 = true;
				}
#else
				const PxVec3 triEdge1a = tri1.verts[1] - tri1.verts[0];
				const PxVec3 triEdge1b = tri1.verts[2] - tri1.verts[1];
				const PxVec3 triEdge1c = tri1.verts[0] - tri1.verts[2];

				PxVec3 sep0 = triEdge0.cross(triEdge1a);
				PxVec3 sep1 = triEdge0.cross(triEdge1b);
				PxVec3 sep2 = triEdge0.cross(triEdge1c);

				const bool invalid0 = sep0.dot(sep0) < 1.0e-6f;
				const bool invalid1 = sep1.dot(sep1) < 1.0e-6f;
				const bool invalid2 = sep2.dot(sep2) < 1.0e-6f;
#endif
				const bool invalid3 = true;
				const PxVec3 sep3(0.0f);

				if(!processAxes4(sep0, sep1, sep2, sep3,
								invalid0, invalid1, invalid2, invalid3,
								tri0, tri1, dir, bValidMTD, tfirst, tlast))
					return 0;
			}
		}

		return processTimeOfImpactResults(tri0, tri1, normal0, normal1, dir, tcoll, tfirst, tlast, tmax, bValidMTD);
	}

	static PX_FORCE_INLINE int testSeparationAxes_SIMD2(
		const PxTrianglePadded& tri0, const PxTrianglePadded& tri1,
		const PxVec3& normal0, const PxVec3& normal1,
		const PxVec3& dir, float tmax, float& tcoll)
	{
		PxIntBool bValidMTD = true;
		float tfirst = -FLT_MAX;
		float tlast  = FLT_MAX;

		PxVec3 axes[4+1];

		// Triangle normals
		axes[0] = normal0;
		axes[1] = normal1;
		PxU32 nbAxes = 2;

#if TEST_SIMD_CROSS
		const Vec4V epsilonV = V4Load(1.0e-6f);
#endif
		// Edges
		for(PxU32 i=0; i<3; i++)
		{
			int ip1 = int(i+1);
			if(i>=2)	ip1 = 0;
#if TEST_SIMD_CROSS
			const Vec4V v0_0V = V4LoadU(&tri0.verts[ip1].x);
			const Vec4V v0_1V = V4LoadU(&tri0.verts[i].x);
			const Vec4V triEdge0V = V4Sub(v0_1V, v0_0V);
#else
			const PxVec3 triEdge0 = tri0.verts[ip1] - tri0.verts[i];
#endif
			// TODO: try to swizzle right here when storing the axes
			{
#if TEST_SIMD_CROSS
				const Vec4V v1_0V = V4LoadU(&tri1.verts[0].x);
				const Vec4V v1_1V = V4LoadU(&tri1.verts[1].x);
				const Vec4V triEdge1aV = V4Sub(v1_1V, v1_0V);
				const Vec4V sep0V = V4Cross(triEdge0V, triEdge1aV);
				if(BGetBitMask(V4IsGrtrOrEq(V4Mul(sep0V, sep0V), epsilonV)) & 7)
#else
				const PxVec3 triEdge1a = tri1.verts[1] - tri1.verts[0];
				PxVec3 sep0 = triEdge0.cross(triEdge1a);
				if(sep0.dot(sep0) >= 1.0e-6f)
#endif
				{
#if TEST_SIMD_CROSS
					V4StoreU(sep0V, &axes[nbAxes++].x);
#else
					axes[nbAxes++] = sep0;
#endif
					if(nbAxes == 4 && !::test4(nbAxes, axes, tri0, tri1, dir, bValidMTD, tfirst, tlast))
						return 0;
				}

#if TEST_SIMD_CROSS
				const Vec4V v1_2V = V4LoadU(&tri1.verts[2].x);
				const Vec4V triEdge1bV = V4Sub(v1_2V, v1_1V);
				const Vec4V sep1V = V4Cross(triEdge0V, triEdge1bV);
				if(BGetBitMask(V4IsGrtrOrEq(V4Mul(sep1V, sep1V), epsilonV)) & 7)
#else
				const PxVec3 triEdge1b = tri1.verts[2] - tri1.verts[1];
				PxVec3 sep1 = triEdge0.cross(triEdge1b);
				if(sep1.dot(sep1) >= 1.0e-6f)
#endif
				{
#if TEST_SIMD_CROSS
					V4StoreU(sep1V, &axes[nbAxes++].x);
#else
					axes[nbAxes++] = sep1;
#endif
					if(nbAxes == 4 && !::test4(nbAxes, axes, tri0, tri1, dir, bValidMTD, tfirst, tlast))
						return 0;
				}

#if TEST_SIMD_CROSS
				const Vec4V triEdge1cV = V4Sub(v1_0V, v1_2V);
				const Vec4V sep2V = V4Cross(triEdge0V, triEdge1cV);
				if(BGetBitMask(V4IsGrtrOrEq(V4Mul(sep2V, sep2V), epsilonV)) & 7)
#else
				const PxVec3 triEdge1c = tri1.verts[0] - tri1.verts[2];
				PxVec3 sep2 = triEdge0.cross(triEdge1c);
				if(sep2.dot(sep2) >= 1.0e-6f)
#endif
				{
#if TEST_SIMD_CROSS
					V4StoreU(sep2V, &axes[nbAxes++].x);
#else
					axes[nbAxes++] = sep2;
#endif
					if(nbAxes == 4 && !::test4(nbAxes, axes, tri0, tri1, dir, bValidMTD, tfirst, tlast))
						return 0;
				}
			}
		}
		if(nbAxes && !::test(nbAxes, axes, tri0, tri1, dir, bValidMTD, tfirst, tlast))
			return 0;

		return processTimeOfImpactResults(tri0, tri1, normal0, normal1, dir, tcoll, tfirst, tlast, tmax, bValidMTD);
	}

static PX_FORCE_INLINE int triTriSweep(
	const PxTrianglePadded& tri0, const PxTrianglePadded& tri1,
	const PxVec3& dir, float tmax, float& toi/*, PxU32 doBackfaceCulling*/)
{
	// Create triangle normal
	PxVec3 triNormal0;
	tri0.denormalizedNormal(triNormal0);

	PxVec3 triNormal1;
	tri1.denormalizedNormal(triNormal1);

	// Backface culling
//	if(doBackfaceCulling && (triNormal.dot(dir)) >= 0.0f)	// ">=" is important !
//		return 0;

	return testSeparationAxes(tri0, tri1, triNormal0, triNormal1, dir, tmax, toi);
}

namespace
{
	class TriangleProvider
	{
		const void*		mTriangles;
		const PxVec3*	mVerts;
		const PxU32		m16BitIndices;
		const bool		mIdtScale;
		PxMat33			mRot;

		public:
		TriangleProvider(const PxTriangleMeshGeometry& meshGeom, const PxQuat& rot);

		void getTriangle(PxTrianglePadded& tri, const PxTransform& meshPose, PxU32 triangleIndex)	const;
	};
};

TriangleProvider::TriangleProvider(const PxTriangleMeshGeometry& meshGeom, const PxQuat& rot) :
	mTriangles(meshGeom.triangleMesh->getTriangles()),
	mVerts(meshGeom.triangleMesh->getVertices()),
	m16BitIndices(meshGeom.triangleMesh->getTriangleMeshFlags() & PxTriangleMeshFlag::e16_BIT_INDICES),
	mIdtScale(meshGeom.scale.isIdentity())
{
	if(mIdtScale)
	{
		mRot = PxMat33Padded(rot);
	}
	else
	{
		const PxMat33Padded m33(rot);
		mRot = m33 * meshGeom.scale.toMat33();
	}
}

void TriangleProvider::getTriangle(PxTrianglePadded& tri, const PxTransform& meshPose, PxU32 triangleIndex) const
{
	PxU32 vref0, vref1, vref2;
	Gu::getVertexReferences(vref0, vref1, vref2, triangleIndex, mTriangles, m16BitIndices);

	tri.verts[0] = meshPose.p + mRot.transform(mVerts[vref0]);
	tri.verts[1] = meshPose.p + mRot.transform(mVerts[vref1]);
	tri.verts[2] = meshPose.p + mRot.transform(mVerts[vref2]);
}

static PX_FORCE_INLINE void accumulate(PxVec3& disp, PxTransform& meshPose0, const PxVec3& dir, float sep, float offset = 1e-3f)
{
	const PxVec3 accum = dir * (sep - offset);
	disp += accum;
	//meshPose0.p += accum;
	meshPose0.p = disp;	// we can use disp directly here because we moved to mesh0 space
}

static PX_FORCE_INLINE float computeBoundsExtent(const PxTriangle& tri)
{
	PxBounds3 bounds(tri.verts[0], tri.verts[0]);
	bounds.include(tri.verts[1]);
	bounds.include(tri.verts[2]);
	const PxVec3 extents = bounds.getExtents();
	return extents.magnitude();
}

PxVec3 Gu::depenetrateMeshesRef(const PxTriangleMeshGeometry& meshGeom0, const PxTriangleMeshGeometry& meshGeom1, const PxTransform& pose0, const PxTransform& pose1, const PxVec3& inputDir, PxU32 maxIter, PxU32* nbIter)
{
	if(inputDir.isZero())
		return PxVec3(0.0f);

	PX_SIMD_GUARD

	const TriangleProvider triangleProvider0(meshGeom0, pose0.q);
	const TriangleProvider triangleProvider1(meshGeom1, pose1.q);

	PxTransform meshPose0(PxVec3(0.0f), pose0.q);
	const PxTransform meshPose1(pose1.p - pose0.p, pose1.q);

	const PxVec3 dir = inputDir.getNormalized();

	//float tmax = dir.magnitude();	// ### does this really help?
	float tmax = FLT_MAX;
	//float tmax = 0.0f;

	//const PxGeometryQueryFlags queryFlags = PxGeometryQueryFlag::eDEFAULT;
	const PxGeometryQueryFlags queryFlags = PxGeometryQueryFlags(0);
	//const PxMeshMeshQueryFlags meshMeshFlags = PxMeshMeshQueryFlag::eDEFAULT;
	const PxMeshMeshQueryFlags meshMeshFlags = PxMeshMeshQueryFlag::eDEFAULT|PxMeshMeshQueryFlag::eRESERVED3;
	const float tolerance = 0.0f;

	PxArray<PxGeomIndexPair> overlaps;

	PxVec3 disp(0.0f);
	PxU32 iter = 0;

	while(iter < maxIter)
	{
		iter++;

		class MyPxReportCallback : public PxDynamicArrayReportCallback<PxGeomIndexPair>
		{
			public:
							MyPxReportCallback(PxArray<PxGeomIndexPair>& overlaps) : PxDynamicArrayReportCallback(overlaps)	{}

			PX_FORCE_INLINE	PxU32					getNbPairs()	const	{ return mResults.size();	}
			PX_FORCE_INLINE	const PxGeomIndexPair*	getPairs()		const	{ return mResults.begin();	}
		};

		overlaps.clear();
		MyPxReportCallback callback(overlaps);

		bool status = PxMeshQuery::findOverlapTriangleMesh(callback, meshGeom0, meshPose0, meshGeom1, meshPose1, queryFlags, meshMeshFlags, tolerance);
		PX_UNUSED(status);
		const PxU32 nbPairs = callback.getNbPairs();
		const PxGeomIndexPair* pairs = callback.getPairs();
	
		float sep = 0.0f;
		const bool log = false;
		const bool supportRestart = false;

		PxTrianglePadded tri0, tri1, offsetTri0;
		for(PxU32 i=0;i<nbPairs;i++)
		{
			triangleProvider0.getTriangle(tri0, meshPose0, pairs[i].id0);
			triangleProvider1.getTriangle(tri1, meshPose1, pairs[i].id1);

			float scale = 1.0f;
			bool tryAgain = true;

			float be0 = computeBoundsExtent(tri0);
			float be1 = computeBoundsExtent(tri1);
			
			while(tryAgain)
			{
				tryAgain = false;

				const float offset = be0 + be1 + DEP_OFFSET;

				const PxVec3 d = (sep*scale - offset) * dir;
				offsetTri0.verts[0] = tri0.verts[0] + d;
				offsetTri0.verts[1] = tri0.verts[1] + d;
				offsetTri0.verts[2] = tri0.verts[2] + d;

				float toi;
				if(triTriSweep(offsetTri0, tri1, dir, tmax + offset, toi))
				{
					toi -= offset;
					// PT: the displacement from the first pair might have resolved collisions for subsequent pairs already.
					// In this case we can get a positive TOI, which we should discard (as if the initial overlap query would not
					// have returned these pairs). Failing to do so (i.e. adding positive TOIs to sep) is a mistake, it undoes the
					// progress made so far, can make the query significantly slower, or make it fail entirely.
					if(toi < 0.0f)
					{
						// PT: the time-of-impact can become inaccurate for small triangles whose normals have the same magnitude as
						// our internal epsilon (DIR_DOT_AXIS_EPSILON). Tweaking the epsilon fixed a lot of problems but we can still
						// get incorrect results occasionally. We try to catch these cases here.
						const float limit = (be0 + be1)*2.0f;	// PT: we can't have a penetration depth larger than the size of both tris
						if(-toi > limit)
						{
							// PT: we detected a clearly invalid result and need to decide what to do about it. A strategy that works is
							// to scale up the source triangles by an order of magnitude and recompute the result. Another strategy is
							// simply to discard the result and ignore that pair. Due to the iterative nature of the algorithm other pairs
							// take over and depenetrate meshes anyway. The 'supportRestart' bool selects which strategy is applied.
							if(!tryAgain && scale == 1.0f)
							{
								if(log)
									printf("Invalid result detected: %f %f\n", double(-toi), double(limit));
								if(supportRestart)
								{
									tryAgain = true;

									scale = 10.0f;
									tri0.verts[0] *= scale;
									tri0.verts[1] *= scale;
									tri0.verts[2] *= scale;
									tri1.verts[0] *= scale;
									tri1.verts[1] *= scale;
									tri1.verts[2] *= scale;
									be0 *= scale;
									be1 *= scale;
								}
							}
							else if(log)
								printf("ERROR: invalid result still failing after rescale\n");
						}
						else
						{
							if(log && scale>1.0f)
								printf("Invalid result fixed (success) after rescale\n");

							toi /= scale;
							sep += toi;
						}
					}
					else
					{
						if(log && scale>1.0f)
							printf("Invalid result fixed (discarded) after rescale\n");
					}
				}
			}
		}

		if(sep != 0.0f)
			accumulate(disp, meshPose0, dir, sep);
		else
			break;
	}

	if(nbIter)
		*nbIter = iter;

	return disp;
}

#include "GuMidphaseInterface.h"
#include "GuTriangleMeshBV4.h"
#include "GuBV4_Common.h"

static PX_FORCE_INLINE void setIdentity(PxMat44& m)
{
	m.column0 = PxVec4(1.0f, 0.0f, 0.0f, 0.0f);
	m.column1 = PxVec4(0.0f, 1.0f, 0.0f, 0.0f);
	m.column2 = PxVec4(0.0f, 0.0f, 1.0f, 0.0f);
	m.column3 = PxVec4(0.0f, 0.0f, 0.0f, 1.0f);
}

static PX_FORCE_INLINE void setRotation(PxMat44& m, const PxQuat& q)
{
	const QuatV qV = V4LoadU(&q.x);
	Vec3V column0V, column1V, column2V;
	QuatGetMat33V(qV, column0V, column1V, column2V);

	V4StoreU(Vec4V_From_Vec3V(column0V), &m.column0.x);
	V4StoreU(Vec4V_From_Vec3V(column1V), &m.column1.x);
	V4StoreU(Vec4V_From_Vec3V(column2V), &m.column2.x);
}

bool BV4_OverlapMeshVsMesh(	PxReportCallback<PxGeomIndexPair>& callback,
							const BV4Tree& tree0, const BV4Tree& tree1, const PxMat44* mat0to1, const PxMat44* mat1to0,
							const PxTransform& meshPose0, const PxTransform& meshPose1,
							const PxMeshScale& meshScale0, const PxMeshScale& meshScale1,
							PxMeshMeshQueryFlags meshMeshFlags, float tolerance);

static PX_FORCE_INLINE void updatePos(PxVec4& dst, const PxTransform& src, const PxVec3& p, const PxQuat& qinv)
{
	const PxVec3 v = qinv.rotate(src.p - p);
	dst.x = v.x;
	dst.y = v.y;
	dst.z = v.z;
}

PxVec3 Gu::depenetrateMeshes(const PxTriangleMeshGeometry& meshGeom0, const PxTriangleMeshGeometry& meshGeom1, const PxTransform& pose0, const PxTransform& pose1, const PxVec3& inputDir, PxU32 maxIter, PxU32* nbIter)
{
	if(inputDir.isZero())
		return PxVec3(0.0f);

	PX_SIMD_GUARD

	const TriangleMesh* tm0 = static_cast<const TriangleMesh*>(meshGeom0.triangleMesh);
	const TriangleMesh* tm1 = static_cast<const TriangleMesh*>(meshGeom1.triangleMesh);

	if(!tm0 || !tm1 || tm0->getConcreteType()!=PxConcreteType::eTRIANGLE_MESH_BVH34 || tm1->getConcreteType()!=PxConcreteType::eTRIANGLE_MESH_BVH34)
	{
		PxGetFoundation().error(PxErrorCode::eINVALID_OPERATION, PX_FL, "Gu::depenetrateMeshes(): only available between two BVH34 triangles meshes.");
		return PxVec3(0.0f);
	}

	/////

	const BV4Tree& tree0 = static_cast<const BV4TriangleMesh*>(tm0)->getBV4Tree();
	const BV4Tree& tree1 = static_cast<const BV4TriangleMesh*>(tm1)->getBV4Tree();

	/////

	class Sweeper : public PxReportCallback<PxGeomIndexPair>
	{
		public:
		const TriangleProvider	mTriangleProvider0;
		const TriangleProvider	mTriangleProvider1;
		PxTransform				mMeshPose0;
		PxTransform				mMeshPose1;
		PxVec3					mDir;

		Sweeper(const PxTriangleMeshGeometry& meshGeom0, const PxTriangleMeshGeometry& meshGeom1, const PxTransform& pose0, const PxTransform& pose1, const PxVec3& dir) :
			mTriangleProvider0(meshGeom0, pose0.q), mTriangleProvider1(meshGeom1, pose1.q)
		{
			this->mBuffer = &mPair;
			this->mCapacity = 1;

			mMeshPose0 = PxTransform(PxVec3(0.0f), pose0.q);
			mMeshPose1 = PxTransform(pose1.p - pose0.p, pose1.q);
			mDir = dir;
			mSep = 0.0f;
		}

		void	reset()
		{
			mSep = 0.0f;
		}

		virtual	bool	flushResults(PxU32 nbItems, const PxGeomIndexPair* pair) PX_OVERRIDE
		{
			PX_UNUSED(nbItems);
			PX_ASSERT(nbItems == 1);
			PxTrianglePadded tri0, tri1, offsetTri0;
			mTriangleProvider0.getTriangle(tri0, mMeshPose0, pair->id0);
			mTriangleProvider1.getTriangle(tri1, mMeshPose1, pair->id1);

			//const float offset = computeBoundsExtent(tri0) + computeBoundsExtent(tri1) + DEP_OFFSET;
			const float be0 = computeBoundsExtent(tri0);
			const float be1 = computeBoundsExtent(tri1);
			const float offset = be0 + be1 + DEP_OFFSET;

			const PxVec3 dirOffset = mDir * offset;

			offsetTri0.verts[0] = tri0.verts[0] - dirOffset;
			offsetTri0.verts[1] = tri0.verts[1] - dirOffset;
			offsetTri0.verts[2] = tri0.verts[2] - dirOffset;

			//float tmax = dir.magnitude();	// ### does this really help?
			const float tmax = FLT_MAX;
			//float tmax = 0.0f;

			float toi;
			if(!triTriSweep(offsetTri0, tri1, mDir, tmax + offset, toi/*, 0.0f*/))
				return true;

			toi -= offset;

			// PT: in this version the test is not needed anymore in theory, but we keep it to
			// fight FPU accuracy issues that could potentially make the code produce a +epsilon TOI.
			if(toi < 0.0f)
			{
				// PT: see reference code for why we have this test here
				const float limit = (be0 + be1)*2.0f;	// PT: we can't have a penetration depth larger than the size of both tris
				if(-toi > limit)
				{
					if(0)
						printf("Invalid result detected: %f %f\n", double(-toi), double(limit));
					return true;
				}

				mSep += toi;
				return false;
			}
			return true;
		}

		PxGeomIndexPair	mPair;
		float			mSep;
	};

	//const PxMeshMeshQueryFlags meshMeshFlags = PxMeshMeshQueryFlag::eDEFAULT;
	const PxMeshMeshQueryFlags meshMeshFlags = PxMeshMeshQueryFlag::eDEFAULT|PxMeshMeshQueryFlag::eRESERVED3;

	PxVec3 disp(0.0f);

	PxU32 iter = 0;

	const PxVec3 dir = inputDir.getNormalized();

	Sweeper sweeper(meshGeom0, meshGeom1, pose0, pose1, dir);

/*	const PxU32 nbPasses = 3;
	//const float offsets[3] = { 2e-1f, 1e-2f, 1e-3f };
	const float offsets[3] = { 1e-1f, 1e-2f, 1e-3f };
//	const float offsets[3] = { 1e-1f, 1e-2f, 1e-4f };*/

	const PxU32 nbPasses = 2;
	const float offsets[2] = { 1e-1f, 1e-3f };

//	const PxU32 nbPasses = 2;
//	const float offsets[2] = { 1e-2f, 1e-5f };

//	const PxU32 nbPasses = 1;
//	const float offsets[2] = { 1e-3f };

	PxVec3 prevDisp(0.0f);

	BV4_ALIGN16(PxMat44 World0to1);
	BV4_ALIGN16(PxMat44 World1to0);

	setIdentity(World0to1);
	setIdentity(World1to0);

	const PxMat44* TM0to1 = &World0to1;
	const PxMat44* TM1to0 = &World1to0;
	{
		const PxTransform t0to1 = sweeper.mMeshPose1.transformInv(sweeper.mMeshPose0);
		const PxTransform t1to0 = sweeper.mMeshPose0.transformInv(sweeper.mMeshPose1);

		setRotation(World0to1, t0to1.q);
		setRotation(World1to0, t1to0.q);

		World0to1.column3.x = t0to1.p.x;
		World0to1.column3.y = t0to1.p.y;
		World0to1.column3.z = t0to1.p.z;

		World1to0.column3.x = t1to0.p.x;
		World1to0.column3.y = t1to0.p.y;
		World1to0.column3.z = t1to0.p.z;
	}

	const PxQuat qinv0 = sweeper.mMeshPose0.q.getConjugate();
	const PxQuat qinv1 = sweeper.mMeshPose1.q.getConjugate();

	for(PxU32 pass=0; pass<nbPasses; pass++)
	{
		float offset = offsets[pass];

		while(iter < maxIter)
		{
			iter++;

			sweeper.reset();

			{
				updatePos(World0to1.column3, sweeper.mMeshPose0, sweeper.mMeshPose1.p, qinv1);
				updatePos(World1to0.column3, sweeper.mMeshPose1, sweeper.mMeshPose0.p, qinv0);
				BV4_OverlapMeshVsMesh(sweeper, tree0, tree1, TM0to1, TM1to0, sweeper.mMeshPose0, sweeper.mMeshPose1, meshGeom0.scale, meshGeom1.scale, meshMeshFlags, 0.0f);
			}

			if(sweeper.mSep != 0.0f)
			{
				prevDisp = disp;
				accumulate(disp, sweeper.mMeshPose0, dir, sweeper.mSep, offset);
			}
			else
			{
				//const float offset = 1e-5f;
				const float offset2 = 1e-3f;
				//const float offset = 1e-2f;	// ### 2x faster but "floating" results
				//const float offset = 1e-1f;	// ### 2x faster but "floating" results
				//const float offset = 0.0f;
				const PxVec3 accum = - dir * offset2;
				//disp += accum;
				sweeper.mMeshPose0.p = disp + accum;

				sweeper.reset();

				{
					updatePos(World0to1.column3, sweeper.mMeshPose0, sweeper.mMeshPose1.p, qinv1);
					updatePos(World1to0.column3, sweeper.mMeshPose1, sweeper.mMeshPose0.p, qinv0);
					BV4_OverlapMeshVsMesh(sweeper, tree0, tree1, TM0to1, TM1to0, sweeper.mMeshPose0, sweeper.mMeshPose1, meshGeom0.scale, meshGeom1.scale, meshMeshFlags, 0.0f);
				}

				if(sweeper.mSep != 0.0f)
				{
					// This was a false alarm
					prevDisp = disp;
					disp += accum;
				}
				else
				{
					if(pass != nbPasses - 1)
					{
						disp = prevDisp;
						sweeper.mMeshPose0.p = disp;
					}
					break;
				}
			}
		}
	}

	if(nbIter)
		*nbIter = iter;

	return disp;
}

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

void Gu::computeDepenetrationVectors(PxU32 nbDirs, PxVec3* dirs)
{
	const int n = int(nbDirs);
	const float goldenRatio = (1.0f + sqrtf(5.0f)) * 0.5f;
	for(int i=0; i<n; i++)
	{
		const float theta = 2.0f * PxPi * i / goldenRatio;
		const float phi = acosf(1.0f - 2.0f * (i + 0.5f) / n);
		const PxVec3 dir(cosf(theta) * sinf(phi), sinf(theta) * sinf(phi), cosf(phi));
		dirs[i] = dir.getNormalized();
	}

	Cm::BasicRandom rnd(42);
	for(int i=0; i<n*100; i++)
	{
		const PxU32 j = rnd.randomize() % n;
		const PxU32 k = rnd.randomize() % n;

		if(j != k)
			PxSwap(dirs[j], dirs[k]);
	}
}

static void getTriangle(PxVec3& p0, PxVec3& p1, PxVec3& p2, PxU32 triangleIndex, const PxMeshScale& scale, const PxTransform* meshPose, const PxVec3* verts, const void* triangles, PxU32 has16BitIndices)
{
	PxU32 vref0, vref1, vref2;
	getVertexReferences(vref0, vref1, vref2, triangleIndex, triangles, has16BitIndices);

	if(scale.isIdentity())
	{
		if(meshPose)
		{
			p0 = meshPose->transform(verts[vref0]);
			p1 = meshPose->transform(verts[vref1]);
			p2 = meshPose->transform(verts[vref2]);
		}
		else
		{
			p0 = verts[vref0];
			p1 = verts[vref1];
			p2 = verts[vref2];
		}
	}
	else
	{
		if(meshPose)
		{
			const PxMat33Padded m33(meshPose->q);
			const PxMat34 absPose(m33 * scale.toMat33(), meshPose->p);

			p0 = absPose.transform(verts[vref0]);
			p1 = absPose.transform(verts[vref1]);
			p2 = absPose.transform(verts[vref2]);
		}
		else
		{
			const PxMat33 absPose = scale.toMat33();

			p0 = absPose.transform(verts[vref0]);
			p1 = absPose.transform(verts[vref1]);
			p2 = absPose.transform(verts[vref2]);
		}
	}
}

void Gu::getTriangle(PxVec3& p0, PxVec3& p1, PxVec3& p2, PxU32 triangleIndex, const PxTriangleMeshGeometry& meshGeom, const PxTransform* meshPose)
{
	const PxTriangleMesh* tm = meshGeom.triangleMesh;
	::getTriangle(p0, p1, p2, triangleIndex, meshGeom.scale, meshPose, tm->getVertices(), tm->getTriangles(), tm->getTriangleMeshFlags() & PxTriangleMeshFlag::e16_BIT_INDICES);
}

void LocalCluster::reset()
{
	mTris.clear();
	mNormals.clear();
	mTris.setExtraSize(4);	// Allocate 4 more bytes to safely SIMD-load the last vertex
}

static PX_FORCE_INLINE void addTriangleData(LocalCluster& cluster, const PxTriangle& tri)
{
	cluster.mTris.pushBack(tri);

    PxVec3 triNormal;
	tri.normal(triNormal);
    cluster.mNormals.pushBack(triNormal);
}

void LocalCluster::init(const float* src, PxU32 nbFloats)
{
	const PxU32 nbTris = nbFloats / 9;

	PxTriangle tri;
	for(PxU32 i=0; i<nbTris; i++)
	{
		tri.verts[0].x = *src++;
		tri.verts[0].y = *src++;
		tri.verts[0].z = *src++;

		tri.verts[1].x = *src++;
		tri.verts[1].y = *src++;
		tri.verts[1].z = *src++;

		tri.verts[2].x = *src++;
		tri.verts[2].y = *src++;
		tri.verts[2].z = *src++;

		addTriangleData(*this, tri);
	}
}

void LocalCluster::addTriangle(PxU32 triangleIndex, const PxMeshScale& scale, const PxTransform* meshPose, const PxVec3* verts, const void* triangles, PxU32 has16BitIndices)
{
	PxTriangle tri;
	::getTriangle(tri.verts[0], tri.verts[1], tri.verts[2], triangleIndex, scale, meshPose, verts, triangles, has16BitIndices);

	addTriangleData(*this, tri);
}

static void processAdjacent(LocalCluster& mesh, const PxTriangleMeshGeometry& meshGeom, const PxTransform* meshPose, const PxU32* faceByEdge, PxU32 nbTris, PxU32 triangleIndex)
{
	PxTriangleMesh* tm = meshGeom.triangleMesh;
	const PxVec3* verts = tm->getVertices();
	const void* triangles = tm->getTriangles();
	const PxU32 has16BitIndices = tm->getTriangleMeshFlags() & PxTriangleMeshFlag::e16_BIT_INDICES;
	const PxMeshScale& scale = meshGeom.scale;

	for(PxU32 i=0; i<nbTris; i++)
	{
		const PxU32 candidateTriIndex = faceByEdge[i];
		if(candidateTriIndex != triangleIndex)
			mesh.addTriangle(candidateTriIndex, scale, meshPose, verts, triangles, has16BitIndices);
	}
}

static PX_FORCE_INLINE bool canConnect(PxU32 vref0, PxU32 vref1, PxU32 vref0b, PxU32 vref1b)
{
	PX_ASSERT(vref0b <= vref1b);
	return (vref0 == vref0b && vref1 == vref1b);
}

static PX_FORCE_INLINE bool canConnect(PxU32 vref0, PxU32 vref1, PxU32 vref0b, PxU32 vref1b, PxU32 vref2b)
{
	PX_ASSERT(vref0 <= vref1);
	if(canConnect(vref0, vref1, vref0b, vref1b))
		return true;
	if(canConnect(vref0, vref1, vref1b, vref2b))
		return true;
	if(canConnect(vref0, vref1, vref0b, vref2b))
		return true;
	return false;
}

static PX_FORCE_INLINE void sort3(PxU32& vref0, PxU32& vref1, PxU32& vref2)
{
	if(vref0 > vref1)
		PxSwap(vref0, vref1);
	if(vref1 > vref2)
		PxSwap(vref1, vref2);
	if(vref0 > vref1)
		PxSwap(vref0, vref1);
}

void Gu::createLocalCluster(LocalCluster& mesh, const PxTriangleMeshGeometry& meshGeom, const PxTransform* meshPose, PxU32 triIndex, const EdgeList* edgeList)
{
	PxTriangleMesh* tm = meshGeom.triangleMesh;
	const PxVec3* verts = tm->getVertices();
	const void* triangles = tm->getTriangles();
	const PxU32 has16BitIndices = tm->getTriangleMeshFlags() & PxTriangleMeshFlag::e16_BIT_INDICES;
	const PxMeshScale& scale = meshGeom.scale;

	mesh.addTriangle(triIndex, scale, meshPose, verts, triangles, has16BitIndices);

	if(edgeList)
	{
		// PT: TODO: revisit that edgelist code, it's been butchered over the years
		const EdgeTriangleData* edgeTriangleData = edgeList->getEdgeTriangles();
		const EdgeDescData* edgeToTriangle = edgeList->getEdgeToTriangles();
		const PxU32* faceByEdge = edgeList->getFacesByEdges();

		const EdgeTriangleData& edgeTri = edgeTriangleData[triIndex];
		const EdgeDescData& edgeData0 = edgeToTriangle[edgeTri.mLink[0] & MSH_EDGE_LINK_MASK];
		const EdgeDescData& edgeData1 = edgeToTriangle[edgeTri.mLink[1] & MSH_EDGE_LINK_MASK];
		const EdgeDescData& edgeData2 = edgeToTriangle[edgeTri.mLink[2] & MSH_EDGE_LINK_MASK];

		//printf("%d %d %d\n", edgeData0.Count, edgeData1.Count, edgeData2.Count);

		processAdjacent(mesh, meshGeom, meshPose, faceByEdge + edgeData0.Offset, edgeData0.Count, triIndex);
		processAdjacent(mesh, meshGeom, meshPose, faceByEdge + edgeData1.Offset, edgeData1.Count, triIndex);
		processAdjacent(mesh, meshGeom, meshPose, faceByEdge + edgeData2.Offset, edgeData2.Count, triIndex);
	}
	else
	{
		PxU32 vref0, vref1, vref2;
		Gu::getVertexReferences(vref0, vref1, vref2, triIndex, triangles, has16BitIndices);

		const PxVec3& v0 = verts[vref0];
		const PxVec3& v1 = verts[vref1];
		const PxVec3& v2 = verts[vref2];

		PxBounds3 bounds(v0, v0);
		bounds.include(v1);
		bounds.include(v2);

		bounds.fattenFast(1e-3f);

		const PxBoxGeometry boxGeom(bounds.getExtents());
		const PxTransform boxPose(bounds.getCenter());

		const PxTriangleMeshGeometry meshGeomNoScale(tm);

		const PxGeometryQueryFlags queryFlags = PxGeometryQueryFlag::eDEFAULT;
		//const PxGeometryQueryFlags queryFlags = PxGeometryQueryFlag::Enum(0);

		// PT: TODO: why don't we have a callback version for box-vs-mesh findOverlapTriangleMesh ?
		PxU32 results[1024];
		bool overflow;
		const PxU32 nbTris = PxMeshQuery::findOverlapTriangleMesh(boxGeom, boxPose, meshGeomNoScale, PxTransform(PxIdentity), results, 1024, 0, overflow, queryFlags);
		if(nbTris)
		{
			sort3(vref0, vref1, vref2);

			for(PxU32 i=0; i<nbTris; i++)
			{
				const PxU32 candidateTriIndex = results[i];
				if(candidateTriIndex == triIndex)
					continue;

				PxU32 vref0b, vref1b, vref2b;
				Gu::getVertexReferences(vref0b, vref1b, vref2b, candidateTriIndex, triangles, has16BitIndices);

				sort3(vref0b, vref1b, vref2b);

				if(		canConnect(vref0, vref1, vref0b, vref1b, vref2b)
					||	canConnect(vref1, vref2, vref0b, vref1b, vref2b)
					||	canConnect(vref0, vref2, vref0b, vref1b, vref2b)
					)
				{
					// PT: TODO: we fetch the vertex refs again inside addTriangle
					mesh.addTriangle(candidateTriIndex, scale, meshPose, verts, triangles, has16BitIndices);
				}
			}
		}
	}

	{
		const PxU32 nbTris = mesh.mTris.size();
		const PxTriangle* tris = mesh.mTris.begin();

		PxBounds3 bounds = PxBounds3::empty();
		for(PxU32 i=0; i<nbTris; i++)
		{
			const PxTriangle& tri = tris[i];
			bounds.include(tri.verts[0]);
			bounds.include(tri.verts[1]);
			bounds.include(tri.verts[2]);
		}

		mesh.mExtent = bounds.getExtents().magnitude();
	}
}

// PT: this is safe when using the "extra size" parameter in LocalCluster's inline arrays
static PX_FORCE_INLINE const PxTrianglePadded& getPaddedTriangle(const PxTriangle& triangle)
{
	return static_cast<const PxTrianglePadded&>(triangle);
}

float Gu::depenetrateLocalClusters(const LocalCluster& mesh0, const LocalCluster& mesh1, const PxVec3& inputDir, bool iterative)
{
	const PxVec3 dir = inputDir.getNormalized();
	const PxVec3 ndir = -dir;

	const PxU32 nbTris0 = mesh0.mTris.size();
	const PxTriangle* PX_RESTRICT tris0 = mesh0.mTris.begin();
	const PxVec3* PX_RESTRICT normals0 = mesh0.mNormals.begin();
	const PxU32 nbTris1 = mesh1.mTris.size();
	const PxTriangle* PX_RESTRICT tris1 = mesh1.mTris.begin();
    const PxVec3* PX_RESTRICT normals1 = mesh1.mNormals.begin();

	// ### TODO: add early exit if already larger than best

	float minDist = PX_MAX_F32;
	if(!iterative)
	{
		const float offset = mesh0.mExtent + mesh1.mExtent + DEP_OFFSET;

		const PxVec3 dirOffset = dir * offset;

		PxTrianglePadded offsetTri0;
		for(PxU32 i=0; i<nbTris0; i++)
		{
			const PxTriangle& tri0 = tris0[i];

			offsetTri0.verts[0] = tri0.verts[0] + dirOffset;
			offsetTri0.verts[1] = tri0.verts[1] + dirOffset;
			offsetTri0.verts[2] = tri0.verts[2] + dirOffset;

			const PxVec3& triNormal0 = normals0[i];

			for(PxU32 j=0; j<nbTris1; j++)
			{
				const PxTrianglePadded& tri1 = getPaddedTriangle(tris1[j]);

				const PxVec3& triNormal1 = normals1[j];

#if VERIFY_SIMD_CODE
				float toi_ref;
				int status_ref = testSeparationAxes_Scalar(offsetTri0, tri1, triNormal0, triNormal1, ndir, FLT_MAX, toi_ref);
#endif
				float toi;
				int status = testSeparationAxes(offsetTri0, tri1, /*edge0, edge1, */triNormal0, triNormal1, ndir, FLT_MAX, toi);

#if VERIFY_SIMD_CODE
				if((status != status_ref ) || (status && status_ref && toi != toi_ref))
					printf("ERROR! %f %f\n", toi_ref, toi);
#endif
				if(status)
				{
					toi -= offset;
					minDist = PxMin(minDist, toi);
				}
			}
		}
	}
	else
	{
		float disp = 0.0f;

		PxU32 nbIter = 0;

		while(nbIter < 16)
		{
			nbIter++;

			PxU32 nb = 0;

			bool found = false;

			for(PxU32 i=0; i<nbTris0; i++)
			{
				if(found)
					break;

				const PxTriangle& tri0 = tris0[i];
				const PxVec3& triNormal0 = normals0[i];

				const PxVec3 dispVector = dir * disp;
				const PxVec3 v00 = tri0.verts[0] + dispVector;
				const PxVec3 v10 = tri0.verts[1] + dispVector;
				const PxVec3 v20 = tri0.verts[2] + dispVector;

				PxTrianglePadded dispTri0;
				dispTri0.verts[0] = v00;
				dispTri0.verts[1] = v10;
				dispTri0.verts[2] = v20;

				for(PxU32 j=0; j<nbTris1; j++)
				{
					const PxTrianglePadded& tri1 = getPaddedTriangle(tris1[j]);

					const PxVec3& triNormal1 = normals1[j];

					float toi;
					if(testSeparationAxes(dispTri0, tri1, triNormal0, triNormal1, ndir, FLT_MAX, toi))
					{
						//printf("toi %f\n", toi);
						if(toi == 0.0f)
						{
							nb++;

							const float offset = computeBoundsExtent(tri0) + computeBoundsExtent(tri1) + DEP_OFFSET;

							const PxVec3 dirOffset = dir * offset;

							PxTrianglePadded offsetTri0;
							offsetTri0.verts[0] = v00 + dirOffset;
							offsetTri0.verts[1] = v10 + dirOffset;
							offsetTri0.verts[2] = v20 + dirOffset;

							if(testSeparationAxes(offsetTri0, tri1, triNormal0, triNormal1, ndir, FLT_MAX, toi))
							{
								toi -= offset;
								//minDist = PxMin(minDist, toi);

								if(toi < 0.0f)
								{
									disp -= toi;
									disp += 1e-3f;
									found = true;
									break;
								}
							}
						}
					}
				}
			}
			if(!found)
				break;

			//printf("%d %d %f\n", nbIter, nb, disp);
		}

		minDist = -disp;
	}

	//printf("minDist: %f\n", minDist);
	return minDist;
}

static bool testDir(const LocalCluster& mesh0, const LocalCluster& mesh1, const PxVec3& dir, float& dmin, bool testOpposite, bool iterative)
{
	float M0 = depenetrateLocalClusters(mesh0, mesh1, dir, iterative);
	if(M0 < 0.0f)
	{
		M0 = -M0;
	}
	else
	{
		M0 = 0.0f;
		return false;
	}
	dmin = PxMin(dmin, M0);

	if(testOpposite)
	{
		float M1 = depenetrateLocalClusters(mesh0, mesh1, -dir, iterative);
		if(M1 < 0.0f)
		{
			M1 = -M1;
		}
		else
		{
			M1 = 0.0f;
			return false;
		}
		dmin = PxMin(dmin, M1);
	}
	return true;
}

static PX_FORCE_INLINE bool isAlmostZero(const PxVec3& v)
{
	if(fabsf(v.x)>1e-6f || fabsf(v.y)>1e-6f || fabsf(v.z)>1e-6f)
		return false;
	return true;
}

static bool testAxes(const LocalCluster& mesh0, const LocalCluster& mesh1, float& minDistMiniMesh, const MaxLocalDepthContext& context)
{
	const bool iterative = context.mIterative;
	const float minDepthCutoff = context.mMinDepthCutoff;

	if(context.mIncludeSATAxes)
	{
        const PxTriangle& tri0 = mesh0.mTris[0];
        const PxVec3& n0 = mesh0.mNormals[0];
		if(!::testDir(mesh0, mesh1, n0, minDistMiniMesh, true, iterative))
			return false;
		if(minDistMiniMesh <= minDepthCutoff)
			return true;

        const PxTriangle& tri1 = mesh1.mTris[0];
        const PxVec3& n1 = mesh1.mNormals[0];
		if(!::testDir(mesh0, mesh1, n1, minDistMiniMesh, true, iterative))
			return false;
		if(minDistMiniMesh <= minDepthCutoff)
			return true;

		for(int e0=0;e0<3;e0++)
		{
			const PxVec3 edge0 = tri0.verts[e0] - tri0.verts[(e0+1)%3];
			for(int e1=0;e1<3;e1++)
			{
				const PxVec3 edge1 = tri1.verts[e1] - tri1.verts[(e1+1)%3];
				PxVec3 cross = edge0.cross(edge1);
				if(!::isAlmostZero(cross) && !::testDir(mesh0, mesh1, cross.getNormalized(), minDistMiniMesh, true, iterative))
					return false;
				if(minDistMiniMesh <= minDepthCutoff)
					return true;
			}
		}
	}

	const PxVec3* userDirs = context.mUserDirs;
	if(userDirs)
	{
		const PxU32 nbUserDirs = context.mNbUserDirs;
		for(PxU32 i=0; i<nbUserDirs; i++)
		{
			if(!::testDir(mesh0, mesh1, userDirs[i], minDistMiniMesh, false, iterative))
				return false;
			if(minDistMiniMesh <= minDepthCutoff)
				return true;
		}
	}

	return true;
}

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

float Gu::computeMaxLocalDepth(
	const PxTriangleMeshGeometry& meshGeom0, const PxTransform& meshPose0, const EdgeList* edgeList0,
	const PxTriangleMeshGeometry& meshGeom1, const PxTransform& meshPose1, const EdgeList* edgeList1,
	PxU32 nbPairs, const PxGeomIndexPair* pairs, const MaxLocalDepthContext& context)
{
	float maxLocalDepth = 0.0f;
	const float maxDepthCutoff = context.mMaxDepthCutoff;

	const PxU32 cacheSize = 64;
	LocalCluster mesh0[cacheSize];
	LocalCluster mesh1[cacheSize];
	PxU32 cacheId0[cacheSize];
	PxU32 cacheId1[cacheSize];
	for(PxU32 k=0; k<cacheSize; k++)
		cacheId0[k] = cacheId1[k] = PX_INVALID_U32;

	const bool runTests = context.mIncludeSATAxes || (context.mNbUserDirs!=0);

	while(nbPairs--)
	{
		const PxU32 id0 = pairs->id0;
		const PxU32 id1 = pairs->id1;
		pairs++;

		//printf("%d %d\n", id0, id1);

		const PxU32 cachedIndex0 = id0 & (cacheSize - 1);
		if(cacheId0[cachedIndex0] != id0)
		{
			cacheId0[cachedIndex0] = id0;
			mesh0[cachedIndex0].reset();
			createLocalCluster(mesh0[cachedIndex0], meshGeom0, &meshPose0, id0, edgeList0);
		}

		const PxU32 cachedIndex1 = id1 & (cacheSize - 1);
		if(cacheId1[cachedIndex1] != id1)
		{
			cacheId1[cachedIndex1] = id1;
			mesh1[cachedIndex1].reset();
			createLocalCluster(mesh1[cachedIndex1], meshGeom1, &meshPose1, id1, edgeList1);
		}

		if(runTests)
		{
			float minDistMiniMesh = FLT_MAX;
			if(!testAxes(mesh0[cachedIndex0], mesh1[cachedIndex1], minDistMiniMesh, context))
				continue;

			if(minDistMiniMesh > maxLocalDepth)
			{
				maxLocalDepth = minDistMiniMesh;
				if(maxLocalDepth >= maxDepthCutoff)
					break;
			}
		}
	}

	return maxLocalDepth;
}

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

static bool testSepAxis(const PxVec3& sep_axis, const PxVec3* triangle0, const PxVec3* triangle1, float& depth)
{
	float Min0,Max0;
	projectTriangle(sep_axis, triangle0, Min0, Max0);

	float Min1,Max1;
	projectTriangle(sep_axis, triangle1, Min1, Max1);

	if(Max0<Min1 || Max1<Min0)
		return false;

	const float d0 = Max0 - Min1;
	PX_ASSERT(d0>=0.0f);
	const float d1 = Max1 - Min0;
	PX_ASSERT(d1>=0.0f);
	depth = d0<d1 ? d0:d1;
	return true;
}

static int triTriSAT(const PxTriangle& tri0, const PxTriangle& tri1, float& depth, PxVec3& dd)
{
	float dmin = FLT_MAX;
	PxVec3 sep;
	int code = 0;

	{
		PxVec3 n0;
		tri0.normal(n0);

		float d;
		if(!testSepAxis(n0, tri0.verts, tri1.verts, d))
			return false;

		if(d<dmin)
		{
			dmin = d;
			sep = n0;
			code = 1;
		}
	}

	{
		PxVec3 n1;
		tri1.normal(n1);

		float d;
		if(!testSepAxis(n1, tri0.verts, tri1.verts, d))
			return false;

		if(d<dmin)
		{
			dmin = d;
			sep = n1;
			code = 2;
		}
	}

	if(1)
	{
		for(int e0=0;e0<3;e0++)
		{
			const PxVec3 edge0 = tri0.verts[e0] - tri0.verts[(e0+1)%3];

			for(int e1=0;e1<3;e1++)
			{
				const PxVec3 edge1 = tri1.verts[e1] - tri1.verts[(e1+1)%3];

				PxVec3 cross = edge0.cross(edge1);
				if(!::isAlmostZero(cross))
				{
					cross.normalize();

					float d;
					if(!testSepAxis(cross, tri0.verts, tri1.verts, d))
						return false;

					if(d < dmin)
					{
						dmin = d;
						sep = cross;
						code = 3;
					}
				}
			}
		}
	}

    depth = dmin;
	dd = sep;

	return code;
}

float Gu::computeMaxLocalDepthBasic(
	const PxTriangleMeshGeometry& meshGeom0, const PxTransform& meshPose0,
	const PxTriangleMeshGeometry& meshGeom1, const PxTransform& meshPose1,
	PxU32 nbPairs, const PxGeomIndexPair* pairs)
{
	float maxLocalDepth = 0.0f;

	PxTriangle tri0;
	PxTriangle tri1;

	for(PxU32 k=0; k<nbPairs; k++)
	{
		const PxU32 id0 = pairs[k].id0;
		const PxU32 id1 = pairs[k].id1;

		Gu::getTriangle(tri0.verts[0], tri0.verts[1], tri0.verts[2], id0, meshGeom0, &meshPose0);
		Gu::getTriangle(tri1.verts[0], tri1.verts[1], tri1.verts[2], id1, meshGeom1, &meshPose1);

		float depth = 0.0f;
		PxVec3 dd;
		const int code = triTriSAT(tri0, tri1, depth, dd);
		PX_UNUSED(code);

		if(depth > maxLocalDepth)
			maxLocalDepth = depth;
	}
	return maxLocalDepth;
}