Init

physx/source/geomutils/include/GuBox.h

@@ -0,0 +1,246 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_BOX_H
+#define GU_BOX_H
+
+/** \addtogroup geomutils
+@{
+*/
+
+#include "foundation/PxTransform.h"
+#include "foundation/PxMat33.h"
+#include "common/PxPhysXCommonConfig.h"
+#include "CmPhysXCommon.h"
+#include "CmScaling.h"
+
+namespace physx
+{
+namespace Gu
+{
+	class Capsule;
+
+	PX_PHYSX_COMMON_API void computeOBBPoints(PxVec3* PX_RESTRICT pts, const PxVec3& center, const PxVec3& extents, const PxVec3& base0, const PxVec3& base1, const PxVec3& base2);
+
+
+	/**
+	\brief Represents an oriented bounding box. 
+
+	As a center point, extents(radii) and a rotation. i.e. the center of the box is at the center point, 
+	the box is rotated around this point with the rotation and it is 2*extents in width, height and depth.
+	*/
+
+	/**
+	Box geometry
+
+	The rot member describes the world space orientation of the box.
+	The center member gives the world space position of the box.
+	The extents give the local space coordinates of the box corner in the positive octant.
+	Dimensions of the box are: 2*extent.
+	Transformation to world space is: worldPoint = rot * localPoint + center
+	Transformation to local space is: localPoint = T(rot) * (worldPoint - center)
+	Where T(M) denotes the transpose of M.
+	*/
+#if PX_VC 
+    #pragma warning(push)
+	#pragma warning( disable : 4251 ) // class needs to have dll-interface to be used by clients of class
+#endif
+	class PX_PHYSX_COMMON_API Box
+	{
+	public:
+		/**
+		\brief Constructor
+		*/
+		PX_FORCE_INLINE Box()
+		{
+		}
+
+		/**
+		\brief Constructor
+
+		\param origin Center of the OBB
+		\param extent Extents/radii of the obb.
+		\param base rotation to apply to the obb.
+		*/
+		//! Construct from center, extent and rotation
+		PX_FORCE_INLINE Box(const PxVec3& origin, const PxVec3& extent, const PxMat33& base) : rot(base), center(origin), extents(extent)
+		{}
+
+		//! Copy constructor
+		PX_FORCE_INLINE Box(const Box& other) : rot(other.rot), center(other.center), extents(other.extents)
+		{}
+
+		/**
+		\brief Destructor
+		*/
+		PX_FORCE_INLINE ~Box()
+		{
+		}
+
+		//! Assignment operator
+		PX_FORCE_INLINE const Box& operator=(const Box& other)
+		{
+			rot		= other.rot;
+			center	= other.center;
+			extents	= other.extents;
+			return *this;
+		}
+
+		/**
+		\brief Setups an empty box.
+		*/
+		PX_INLINE void setEmpty()
+		{
+			center = PxVec3(0);
+			extents = PxVec3(-PX_MAX_REAL, -PX_MAX_REAL, -PX_MAX_REAL);
+			rot = PxMat33(PxIdentity);
+		}
+
+		/**
+		\brief Checks the box is valid.
+
+		\return	true if the box is valid
+		*/
+		PX_INLINE bool isValid() const
+		{
+			// Consistency condition for (Center, Extents) boxes: Extents >= 0.0f
+			if(extents.x < 0.0f)	return false;
+			if(extents.y < 0.0f)	return false;
+			if(extents.z < 0.0f)	return false;
+			return true;
+		}
+
+/////////////
+		PX_FORCE_INLINE	void	setAxes(const PxVec3& axis0, const PxVec3& axis1, const PxVec3& axis2)
+		{
+			rot.column0 = axis0;
+			rot.column1 = axis1;
+			rot.column2 = axis2;
+		}
+
+		PX_FORCE_INLINE	PxVec3	rotate(const PxVec3& src)	const
+		{
+			return rot * src;
+		}
+
+		PX_FORCE_INLINE	PxVec3	rotateInv(const PxVec3& src)	const
+		{
+			return rot.transformTranspose(src);
+		}
+
+		PX_FORCE_INLINE	PxVec3	transform(const PxVec3& src)	const
+		{
+			return rot * src + center;
+		}
+
+		PX_FORCE_INLINE	PxTransform getTransform()	const
+		{
+			return PxTransform(center, PxQuat(rot));
+		}
+
+		PX_INLINE PxVec3 computeAABBExtent() const
+		{
+			const PxReal a00 = PxAbs(rot[0][0]);
+			const PxReal a01 = PxAbs(rot[0][1]);
+			const PxReal a02 = PxAbs(rot[0][2]);
+
+			const PxReal a10 = PxAbs(rot[1][0]);
+			const PxReal a11 = PxAbs(rot[1][1]);
+			const PxReal a12 = PxAbs(rot[1][2]);
+
+			const PxReal a20 = PxAbs(rot[2][0]);
+			const PxReal a21 = PxAbs(rot[2][1]);
+			const PxReal a22 = PxAbs(rot[2][2]);
+
+			const PxReal ex = extents.x;
+			const PxReal ey = extents.y;
+			const PxReal ez = extents.z;
+
+			return PxVec3(	a00 * ex + a10 * ey + a20 * ez,
+							a01 * ex + a11 * ey + a21 * ez,
+							a02 * ex + a12 * ey + a22 * ez);
+		}
+
+		/**
+		Computes the obb points.
+		\param		pts	[out] 8 box points
+		*/
+		PX_FORCE_INLINE void computeBoxPoints(PxVec3* PX_RESTRICT pts) const
+		{
+			Gu::computeOBBPoints(pts, center, extents, rot.column0, rot.column1, rot.column2);
+		}
+
+		void create(const Gu::Capsule& capsule);
+
+		PxMat33	rot;
+		PxVec3	center;
+		PxVec3	extents;
+	};
+	PX_COMPILE_TIME_ASSERT(sizeof(Gu::Box) == 60);
+
+	//! A padded version of Gu::Box, to safely load its data using SIMD
+	class BoxPadded : public Box
+	{
+	public:
+		PX_FORCE_INLINE BoxPadded()		{}
+		PX_FORCE_INLINE ~BoxPadded()	{}
+		PxU32	padding;
+	};
+	PX_COMPILE_TIME_ASSERT(sizeof(Gu::BoxPadded) == 64);
+
+	//! Transforms a shape space AABB to a vertex space AABB (conservative).
+	PX_FORCE_INLINE void computeVertexSpaceAABB(Gu::Box& vertexSpaceOBB, const PxBounds3& worldBounds, const PxTransform& world2Shape, const Cm::FastVertex2ShapeScaling& scaling, bool idtScaleMesh)
+	{
+		PX_ASSERT(!worldBounds.isEmpty());
+		const PxBounds3 boundsInMesh = PxBounds3::transformFast(world2Shape, worldBounds);	// transform bounds from world to shape (excluding mesh scale)
+
+		vertexSpaceOBB.rot = PxMat33(PxIdentity);
+		if(idtScaleMesh)
+		{
+			vertexSpaceOBB.center = boundsInMesh.getCenter();
+			vertexSpaceOBB.extents = boundsInMesh.getExtents();
+		}
+		else
+		{
+			const PxBounds3 bounds = PxBounds3::basisExtent(scaling.getShape2VertexSkew() * boundsInMesh.getCenter(), scaling.getShape2VertexSkew(), boundsInMesh.getExtents());
+			vertexSpaceOBB.center = bounds.getCenter();
+			vertexSpaceOBB.extents = bounds.getExtents();
+		}
+	}
+
+#if PX_VC 
+     #pragma warning(pop) 
+#endif
+
+}
+
+}
+
+/** @} */
+#endif

physx/source/geomutils/include/GuDistanceSegmentBox.h

@@ -0,0 +1,57 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_DISTANCE_SEGMENT_BOX_H
+#define GU_DISTANCE_SEGMENT_BOX_H
+
+#include "foundation/PxMat33.h"
+#include "GuSegment.h"
+#include "GuBox.h"
+
+namespace physx
+{
+namespace Gu
+{
+
+	//! Compute the smallest distance from the (finite) line segment to the box.
+	PX_PHYSX_COMMON_API PxReal distanceSegmentBoxSquared(	const PxVec3& segmentPoint0, const PxVec3& segmentPoint1,
+															const PxVec3& boxOrigin, const PxVec3& boxExtent, const PxMat33& boxBase,
+															PxReal* segmentParam = NULL,
+															PxVec3* boxParam = NULL);
+
+	PX_FORCE_INLINE PxReal distanceSegmentBoxSquared(const Gu::Segment& segment, const Gu::Box& box, PxReal* t = NULL, PxVec3* p = NULL)
+	{
+		return distanceSegmentBoxSquared(segment.p0, segment.p1, box.center, box.extents, box.rot, t, p);
+	}
+
+} // namespace Gu
+
+}
+
+#endif

physx/source/geomutils/include/GuDistanceSegmentSegment.h

@@ -0,0 +1,65 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_DISTANCE_SEGMENT_SEGMENT_H
+#define GU_DISTANCE_SEGMENT_SEGMENT_H
+
+#include "common/PxPhysXCommonConfig.h"
+#include "GuSegment.h"
+
+namespace physx
+{
+namespace Gu
+{
+	// This version fixes accuracy issues (e.g.  TTP 4617), but needs to do 2 square roots in order
+	// to find the normalized direction and length of the segments, and then
+	// a division in order to renormalize the output
+
+	PX_PHYSX_COMMON_API	PxReal	distanceSegmentSegmentSquared(	const PxVec3& origin0, const PxVec3& dir0, PxReal extent0,
+																const PxVec3& origin1, const PxVec3& dir1, PxReal extent1,
+																PxReal* s=NULL, PxReal* t=NULL);
+
+	PX_PHYSX_COMMON_API PxReal	distanceSegmentSegmentSquared(	const PxVec3& origin0, const PxVec3& extent0,
+																const PxVec3& origin1, const PxVec3& extent1,
+																PxReal* s=NULL, PxReal* t=NULL);
+
+	PX_FORCE_INLINE	PxReal	distanceSegmentSegmentSquared(	const Gu::Segment& segment0,
+															const Gu::Segment& segment1,
+															PxReal* s=NULL, PxReal* t=NULL)
+	{
+		return distanceSegmentSegmentSquared(	segment0.p0, segment0.computeDirection(),
+												segment1.p0, segment1.computeDirection(),
+												s, t);
+	}
+
+} // namespace Gu
+
+}
+
+#endif

physx/source/geomutils/include/GuIntersectionBoxBox.h

@@ -0,0 +1,54 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_INTERSECTION_BOX_BOX_H
+#define GU_INTERSECTION_BOX_BOX_H
+
+#include "foundation/PxMat33.h"
+#include "foundation/PxBounds3.h"
+#include "GuBox.h"
+
+namespace physx
+{
+namespace Gu
+{
+	PX_PHYSX_COMMON_API bool intersectOBBOBB(const PxVec3& e0, const PxVec3& c0, const PxMat33& r0, const PxVec3& e1, const PxVec3& c1, const PxMat33& r1, bool full_test);
+
+	PX_FORCE_INLINE bool intersectOBBAABB(const Gu::Box& obb, const PxBounds3& aabb)
+	{
+		PxVec3 center = aabb.getCenter();
+		PxVec3 extents = aabb.getExtents();
+		return intersectOBBOBB(obb.extents, obb.center, obb.rot, extents, center, PxMat33(PxIdentity), true);
+	}
+
+} // namespace Gu
+
+}
+
+#endif

physx/source/geomutils/include/GuIntersectionTriangleBox.h

@@ -0,0 +1,91 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_INTERSECTION_TRIANGLE_BOX_H
+#define GU_INTERSECTION_TRIANGLE_BOX_H
+
+#include "foundation/PxMat33.h"
+#include "common/PxPhysXCommonConfig.h"
+#include "CmPhysXCommon.h"
+
+namespace physx
+{
+namespace Gu
+{
+	class Box;
+	class BoxPadded;
+
+	/**
+	Tests if a triangle overlaps a box (AABB). This is the reference non-SIMD code.
+
+	\param center	[in] the box center
+	\param extents	[in] the box extents
+	\param p0		[in] triangle's first point
+	\param p1		[in] triangle's second point
+	\param p2		[in] triangle's third point
+	\return	true if triangle overlaps box
+	*/
+	PX_PHYSX_COMMON_API Ps::IntBool intersectTriangleBox_ReferenceCode(const PxVec3& center, const PxVec3& extents, const PxVec3& p0, const PxVec3& p1, const PxVec3& p2);
+
+	/**
+	Tests if a triangle overlaps a box (AABB). This is the optimized SIMD code.
+
+	WARNING: the function has various SIMD requirements, left to the calling code:
+	- function will load 4 bytes after 'center'. Make sure it's safe to load from there.
+	- function will load 4 bytes after 'extents'. Make sure it's safe to load from there.
+	- function will load 4 bytes after 'p0'. Make sure it's safe to load from there.
+	- function will load 4 bytes after 'p1'. Make sure it's safe to load from there.
+	- function will load 4 bytes after 'p2'. Make sure it's safe to load from there.
+	If you can't guarantee these requirements, please use the non-SIMD reference code instead.
+
+	\param center	[in] the box center. 
+	\param extents	[in] the box extents
+	\param p0		[in] triangle's first point
+	\param p1		[in] triangle's second point
+	\param p2		[in] triangle's third point
+	\return	true if triangle overlaps box
+	*/
+	PX_PHYSX_COMMON_API Ps::IntBool intersectTriangleBox_Unsafe(const PxVec3& center, const PxVec3& extents, const PxVec3& p0, const PxVec3& p1, const PxVec3& p2);
+
+	/**
+	Tests if a triangle overlaps a box (OBB).
+
+	There are currently no SIMD-related requirements for p0, p1, p2.
+
+	\param box		[in] the box
+	\param p0		[in] triangle's first point
+	\param p1		[in] triangle's second point
+	\param p2		[in] triangle's third point
+	\return	true if triangle overlaps box
+	*/
+	PX_PHYSX_COMMON_API Ps::IntBool intersectTriangleBox(const BoxPadded& box, const PxVec3& p0, const PxVec3& p1, const PxVec3& p2);
+} // namespace Gu
+}
+
+#endif

physx/source/geomutils/include/GuIntersectionTriangleBoxRef.h

@@ -0,0 +1,251 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_INTERSECTION_TRIANGLE_BOX_REF_H
+#define GU_INTERSECTION_TRIANGLE_BOX_REF_H
+
+#include "CmPhysXCommon.h"
+#include "foundation/PxVec3.h"
+
+
+/********************************************************/
+/* AABB-triangle overlap test code                      */
+/* by Tomas Akenine-M?r									*/
+/* Function: int triBoxOverlap(float boxcenter[3],      */
+/*          float boxhalfsize[3],float triverts[3][3]); */
+/* History:                                             */
+/*   2001-03-05: released the code in its first version */
+/*   2001-06-18: changed the order of the tests, faster */
+/*                                                      */
+/* Acknowledgement: Many thanks to Pierre Terdiman for  */
+/* suggestions and discussions on how to optimize code. */
+/* Thanks to David Hunt for finding a ">="-bug!         */
+/********************************************************/
+
+
+namespace physx
+{
+
+#define CROSS(dest,v1,v2)		\
+	dest.x=v1.y*v2.z-v1.z*v2.y;	\
+	dest.y=v1.z*v2.x-v1.x*v2.z;	\
+	dest.z=v1.x*v2.y-v1.y*v2.x; 
+
+#define DOT(v1,v2) (v1.x*v2.x+v1.y*v2.y+v1.z*v2.z)
+
+#define FINDMINMAX(x0, x1, x2, minimum, maximum)			\
+	minimum = physx::intrinsics::selectMin(x0, x1);			\
+	maximum = physx::intrinsics::selectMax(x0, x1);			\
+	minimum = physx::intrinsics::selectMin(minimum, x2);	\
+	maximum = physx::intrinsics::selectMax(maximum, x2);
+
+	static PX_CUDA_CALLABLE PX_FORCE_INLINE Ps::IntBool planeBoxOverlap(const PxVec3& normal, PxReal d, const PxVec3& maxbox)
+	{
+		PxVec3 vmin, vmax;
+
+		if (normal.x>0.0f)
+		{
+			vmin.x = -maxbox.x;
+			vmax.x = maxbox.x;
+		}
+		else
+		{
+			vmin.x = maxbox.x;
+			vmax.x = -maxbox.x;
+		}
+
+		if (normal.y>0.0f)
+		{
+			vmin.y = -maxbox.y;
+			vmax.y = maxbox.y;
+		}
+		else
+		{
+			vmin.y = maxbox.y;
+			vmax.y = -maxbox.y;
+		}
+
+		if (normal.z>0.0f)
+		{
+			vmin.z = -maxbox.z;
+			vmax.z = maxbox.z;
+		}
+		else
+		{
+			vmin.z = maxbox.z;
+			vmax.z = -maxbox.z;
+		}
+
+		if (normal.dot(vmin) + d >  0.0f) return Ps::IntFalse;
+		if (normal.dot(vmax) + d >= 0.0f) return Ps::IntTrue;
+		return Ps::IntFalse;
+	}
+
+	/*======================== X-tests ========================*/
+#define AXISTEST_X01(a, b, fa, fb)							\
+	p0 = a*v0.y - b*v0.z;									\
+	p2 = a*v2.y - b*v2.z;									\
+	minimum = physx::intrinsics::selectMin(p0, p2);			\
+	maximum = physx::intrinsics::selectMax(p0, p2);			\
+	rad = fa * extents.y + fb * extents.z;					\
+	if(minimum>rad || maximum<-rad) return Ps::IntFalse;
+
+#define AXISTEST_X2(a, b, fa, fb)							\
+	p0 = a*v0.y - b*v0.z;									\
+	p1 = a*v1.y - b*v1.z;									\
+	minimum = physx::intrinsics::selectMin(p0, p1);			\
+	maximum = physx::intrinsics::selectMax(p0, p1);			\
+	rad = fa * extents.y + fb * extents.z;					\
+	if(minimum>rad || maximum<-rad) return Ps::IntFalse;
+
+	/*======================== Y-tests ========================*/
+#define AXISTEST_Y02(a, b, fa, fb)							\
+	p0 = -a*v0.x + b*v0.z;									\
+	p2 = -a*v2.x + b*v2.z;									\
+	minimum = physx::intrinsics::selectMin(p0, p2);			\
+	maximum = physx::intrinsics::selectMax(p0, p2);			\
+	rad = fa * extents.x + fb * extents.z;					\
+	if(minimum>rad || maximum<-rad) return Ps::IntFalse;
+
+#define AXISTEST_Y1(a, b, fa, fb)							\
+	p0 = -a*v0.x + b*v0.z;									\
+	p1 = -a*v1.x + b*v1.z;									\
+	minimum = physx::intrinsics::selectMin(p0, p1);			\
+	maximum = physx::intrinsics::selectMax(p0, p1);			\
+	rad = fa * extents.x + fb * extents.z;					\
+	if(minimum>rad || maximum<-rad) return Ps::IntFalse;
+
+	/*======================== Z-tests ========================*/
+#define AXISTEST_Z12(a, b, fa, fb)							\
+	p1 = a*v1.x - b*v1.y;									\
+	p2 = a*v2.x - b*v2.y;									\
+	minimum = physx::intrinsics::selectMin(p1, p2);			\
+	maximum = physx::intrinsics::selectMax(p1, p2);			\
+	rad = fa * extents.x + fb * extents.y;					\
+	if(minimum>rad || maximum<-rad) return Ps::IntFalse;
+
+#define AXISTEST_Z0(a, b, fa, fb)							\
+	p0 = a*v0.x - b*v0.y;									\
+	p1 = a*v1.x - b*v1.y;									\
+	minimum = physx::intrinsics::selectMin(p0, p1);			\
+	maximum = physx::intrinsics::selectMax(p0, p1);			\
+	rad = fa * extents.x + fb * extents.y;					\
+	if(minimum>rad || maximum<-rad) return Ps::IntFalse;
+
+	namespace Gu
+	{
+
+		static PX_CUDA_CALLABLE PX_FORCE_INLINE Ps::IntBool intersectTriangleBox_RefImpl(const PxVec3& boxcenter, const PxVec3& extents, const PxVec3& tp0, const PxVec3& tp1, const PxVec3& tp2)
+		{
+			/*    use separating axis theorem to test overlap between triangle and box */
+			/*    need to test for overlap in these directions: */
+			/*    1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */
+			/*       we do not even need to test these) */
+			/*    2) normal of the triangle */
+			/*    3) crossproduct(edge from tri, {x,y,z}-directin) */
+			/*       this gives 3x3=9 more tests */
+
+			// This is the fastest branch on Sun - move everything so that the boxcenter is in (0,0,0)
+			const PxVec3 v0 = tp0 - boxcenter;
+			const PxVec3 v1 = tp1 - boxcenter;
+			const PxVec3 v2 = tp2 - boxcenter;
+
+			// compute triangle edges
+			const PxVec3 e0 = v1 - v0;	// tri edge 0
+			const PxVec3 e1 = v2 - v1;	// tri edge 1
+			const PxVec3 e2 = v0 - v2;	// tri edge 2
+
+			float minimum, maximum, rad, p0, p1, p2;
+
+			// Bullet 3: test the 9 tests first (this was faster)
+			float fex = PxAbs(e0.x);
+			float fey = PxAbs(e0.y);
+			float fez = PxAbs(e0.z);
+			AXISTEST_X01(e0.z, e0.y, fez, fey);
+			AXISTEST_Y02(e0.z, e0.x, fez, fex);
+			AXISTEST_Z12(e0.y, e0.x, fey, fex);
+
+			fex = PxAbs(e1.x);
+			fey = PxAbs(e1.y);
+			fez = PxAbs(e1.z);
+			AXISTEST_X01(e1.z, e1.y, fez, fey);
+			AXISTEST_Y02(e1.z, e1.x, fez, fex);
+			AXISTEST_Z0(e1.y, e1.x, fey, fex);
+
+			fex = PxAbs(e2.x);
+			fey = PxAbs(e2.y);
+			fez = PxAbs(e2.z);
+			AXISTEST_X2(e2.z, e2.y, fez, fey);
+			AXISTEST_Y1(e2.z, e2.x, fez, fex);
+			AXISTEST_Z12(e2.y, e2.x, fey, fex);
+
+			// Bullet 1:
+			//  first test overlap in the {x,y,z}-directions
+			//  find minimum, maximum of the triangle each direction, and test for overlap in
+			//  that direction -- this is equivalent to testing a minimal AABB around
+			//  the triangle against the AABB
+
+			// test in X-direction
+			FINDMINMAX(v0.x, v1.x, v2.x, minimum, maximum);
+			if (minimum>extents.x || maximum<-extents.x) return Ps::IntFalse;
+
+			// test in Y-direction
+			FINDMINMAX(v0.y, v1.y, v2.y, minimum, maximum);
+			if (minimum>extents.y || maximum<-extents.y) return Ps::IntFalse;
+
+			// test in Z-direction
+			FINDMINMAX(v0.z, v1.z, v2.z, minimum, maximum);
+			if (minimum>extents.z || maximum<-extents.z) return Ps::IntFalse;
+
+			// Bullet 2:
+			//  test if the box intersects the plane of the triangle
+			//  compute plane equation of triangle: normal*x+d=0
+			PxVec3 normal;
+			CROSS(normal, e0, e1);
+			const float d = -DOT(normal, v0);	// plane eq: normal.x+d=0
+			if (!planeBoxOverlap(normal, d, extents)) return Ps::IntFalse;
+
+			return Ps::IntTrue;	// box and triangle overlaps
+		}
+	}
+
+#undef CROSS
+#undef DOT
+#undef FINDMINMAX
+#undef AXISTEST_X01
+#undef AXISTEST_X2
+#undef AXISTEST_Y02
+#undef AXISTEST_Y1
+#undef AXISTEST_Z12
+#undef AXISTEST_Z0
+
+}
+
+#endif
+

physx/source/geomutils/include/GuRaycastTests.h

@@ -0,0 +1,72 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_RAYCAST_TESTS_H
+#define GU_RAYCAST_TESTS_H
+
+#include "foundation/PxSimpleTypes.h"
+#include "geometry/PxGeometry.h"
+#include "PxQueryReport.h"
+#include "CmPhysXCommon.h"
+
+namespace physx
+{
+	// PT: TODO: why is PxHitFlag::eMESH_MULTIPLE used in the ray-vs-hf function, but not in the ray-vs-mesh function?
+
+	// PT: we use a define to be able to quickly change the signature of all raycast functions.
+	// (this also ensures they all use consistent names for passed parameters).
+	// \param[in]	geom		geometry object to raycast against
+	// \param[in]	pose		pose of geometry object
+	// \param[in]	rayOrigin	ray's origin
+	// \param[in]	rayDir		ray's unit dir
+	// \param[in]	maxDist		ray's length/max distance
+	// \param[in]	hitFlags	query behavior flags
+	// \param[in]	maxHits		max number of hits = size of 'hits' buffer
+	// \param[out]	hits		result buffer where to write raycast hits
+	#define GU_RAY_FUNC_PARAMS	const PxGeometry& geom, const PxTransform& pose,					\
+								const PxVec3& rayOrigin, const PxVec3& rayDir, PxReal maxDist,		\
+								PxHitFlags hitFlags, PxU32 maxHits, PxRaycastHit* PX_RESTRICT hits
+	namespace Gu
+	{
+		// PT: function pointer for Geom-indexed raycast functions
+		// See GU_RAY_FUNC_PARAMS for function parameters details.
+		// \return		number of hits written to 'hits' result buffer
+		// \note		there's no mechanism to report overflow. Returned number of hits is just clamped to maxHits.
+		typedef PxU32	(*RaycastFunc)		(GU_RAY_FUNC_PARAMS);
+
+		// PT: typedef for a bundle of all raycast functions, i.e. the function table itself (indexed by geom-type).
+		typedef RaycastFunc GeomRaycastTable[PxGeometryType::eGEOMETRY_COUNT];
+
+		// PT: retrieves the raycast function table (for access by external non-Gu modules)
+		PX_PHYSX_COMMON_API const GeomRaycastTable& getRaycastFuncTable();
+
+	}  // namespace Gu
+}
+
+#endif

physx/source/geomutils/include/GuSIMDHelpers.h

@@ -0,0 +1,98 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_SIMD_HELPERS_H
+#define GU_SIMD_HELPERS_H
+
+#include "foundation/PxMat33.h"
+#include "common/PxPhysXCommonConfig.h"
+#include "geometry/PxTriangle.h"
+#include "CmPhysXCommon.h"
+#include "PsVecMath.h"
+
+namespace physx
+{
+namespace Gu
+{
+	//! A padded version of PxTriangle, to safely load its data using SIMD
+	class TrianglePadded : public PxTriangle
+	{
+	public:
+		PX_FORCE_INLINE TrianglePadded()	{}
+		PX_FORCE_INLINE ~TrianglePadded()	{}
+		PxU32	padding;
+	};
+
+	// PT: wrapper helper class to make sure we can safely load a PxVec3 using SIMD loads
+	// PT: TODO: refactor with PxVec3Pad
+	class Vec3p : public PxVec3
+	{
+		public:
+		PX_FORCE_INLINE	Vec3p()								{}
+		PX_FORCE_INLINE	~Vec3p()							{}
+		PX_FORCE_INLINE	Vec3p(const PxVec3& p) : PxVec3(p)	{}
+		PX_FORCE_INLINE	Vec3p(float f) : PxVec3(f)			{}
+		PxU32	padding;
+	};
+	PX_COMPILE_TIME_ASSERT(sizeof(Vec3p) == 16);
+
+	//! A padded version of PxMat33, to safely load its data using SIMD
+	class PxMat33Padded : public PxMat33
+	{
+	public:
+		explicit PX_FORCE_INLINE PxMat33Padded(const PxQuat& q)
+		{
+			using namespace Ps::aos;
+			const QuatV qV = V4LoadU(&q.x);
+			Vec3V column0V, column1V, column2V;
+			QuatGetMat33V(qV, column0V, column1V, column2V);
+#if defined(PX_SIMD_DISABLED) || PX_ANDROID || (PX_LINUX && (PX_ARM || PX_A64)) || (PX_UWP && (PX_ARM || PX_A64))
+			V3StoreU(column0V, column0);
+			V3StoreU(column1V, column1);
+			V3StoreU(column2V, column2);
+#else
+			V4StoreU(column0V, &column0.x);
+			V4StoreU(column1V, &column1.x);
+			V4StoreU(column2V, &column2.x);
+#endif
+		}
+		PX_FORCE_INLINE ~PxMat33Padded()				{}
+		PX_FORCE_INLINE void operator=(const PxMat33& other)
+		{
+			column0 = other.column0;
+			column1 = other.column1;
+			column2 = other.column2;
+		}
+		PxU32	padding;
+	};
+
+} // namespace Gu
+}
+
+#endif

physx/source/geomutils/include/GuSegment.h

@@ -0,0 +1,182 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_SEGMENT_H
+#define GU_SEGMENT_H
+/** \addtogroup geomutils
+@{
+*/
+
+#include "foundation/PxVec3.h"
+#include "Ps.h"
+#include "CmPhysXCommon.h"
+
+namespace physx
+{
+namespace Gu
+{
+
+	/**
+	\brief Represents a line segment.
+
+	Line segment geometry
+	In some cases this structure will be used to represent the infinite line that passes point0 and point1.
+	*/
+	class Segment
+	{
+	public:
+		/**
+		\brief Constructor
+		*/
+		PX_INLINE Segment()
+		{
+		}
+
+		/**
+		\brief Constructor
+		*/
+		PX_INLINE Segment(const PxVec3& _p0, const PxVec3& _p1) : p0(_p0), p1(_p1)
+		{
+		}
+
+		/**
+		\brief Copy constructor
+		*/
+		PX_INLINE Segment(const Segment& seg) : p0(seg.p0), p1(seg.p1)
+		{
+		}
+
+		/**
+		\brief Destructor
+		*/
+		PX_INLINE ~Segment()
+		{
+		}
+
+		//! Assignment operator
+		PX_INLINE Segment& operator=(const Segment& other)
+		{
+			p0 = other.p0; 
+			p1 = other.p1; 
+			return *this;
+		}
+
+		//! Equality operator
+		PX_INLINE bool operator==(const Segment& other) const
+		{
+			return (p0==other.p0 && p1==other.p1);
+		}
+
+		//! Inequality operator
+		PX_INLINE bool operator!=(const Segment& other) const
+		{
+			return (p0!=other.p0 || p1!=other.p1);
+		}
+
+		PX_INLINE const PxVec3& getOrigin() const
+		{
+			return p0;
+		}
+
+		//! Return the vector from point0 to point1
+		PX_INLINE PxVec3 computeDirection() const
+		{
+			return p1 - p0;
+		}
+
+		//! Return the vector from point0 to point1
+		PX_INLINE void computeDirection(PxVec3& dir) const
+		{
+			dir = p1 - p0;
+		}
+
+		//! Return the center of the segment segment
+		PX_INLINE PxVec3 computeCenter() const
+		{
+			return (p0 + p1)*0.5f;
+		}
+
+		PX_INLINE PxF32 computeLength() const
+		{
+			return (p1-p0).magnitude();
+		}
+
+		PX_INLINE PxF32 computeSquareLength() const
+		{
+			return (p1-p0).magnitudeSquared();
+		}
+
+		// PT: TODO: remove this one
+		//! Return the square of the length of vector from point0 to point1
+		PX_INLINE PxReal lengthSquared() const
+		{
+			return ((p1 - p0).magnitudeSquared());
+		}
+
+		// PT: TODO: remove this one
+		//! Return the length of vector from point0 to point1
+		PX_INLINE PxReal length() const
+		{
+			return ((p1 - p0).magnitude());
+		}
+
+		/*		PX_INLINE void setOriginDirection(const PxVec3& origin, const PxVec3& direction)
+		{
+		p0 = p1 = origin;
+		p1 += direction;
+		}*/
+
+		/**
+		\brief Computes a point on the segment
+
+		\param[out] pt point on segment
+		\param[in] t point's parameter [t=0 => pt = mP0, t=1 => pt = mP1]
+		*/
+		PX_INLINE void computePoint(PxVec3& pt, PxF32 t) const
+		{
+			pt = p0 + t * (p1 - p0);
+		}
+
+		// PT: TODO: remove this one
+		//! Return the point at parameter t along the line: point0 + t*(point1-point0)
+		PX_INLINE PxVec3 getPointAt(PxReal t) const
+		{
+			return (p1 - p0)*t + p0;
+		}
+
+		PxVec3	p0;		//!< Start of segment
+		PxVec3	p1;		//!< End of segment
+	};
+	PX_COMPILE_TIME_ASSERT(sizeof(Gu::Segment) == 24);
+}
+
+}
+
+/** @} */
+#endif

physx/source/geomutils/src/GuAABBTreeBuild.cpp

@@ -0,0 +1,307 @@
+//
+// 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-2021 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 "GuAABBTreeBuild.h"
+
+#include "PsMathUtils.h"
+#include "PsFoundation.h"
+#include "GuInternal.h"
+
+using namespace physx;
+using namespace Gu;
+
+NodeAllocator::NodeAllocator() : mPool(NULL), mCurrentSlabIndex(0), mTotalNbNodes(0)
+{
+}
+
+NodeAllocator::~NodeAllocator()
+{
+	release();
+}
+
+void NodeAllocator::release()
+{
+	const PxU32 nbSlabs = mSlabs.size();
+	for (PxU32 i = 0; i<nbSlabs; i++)
+	{
+		Slab& s = mSlabs[i];
+		PX_DELETE_ARRAY(s.mPool);
+	}
+
+	mSlabs.reset();
+	mCurrentSlabIndex = 0;
+	mTotalNbNodes = 0;
+}
+
+void NodeAllocator::init(PxU32 nbPrimitives, PxU32 limit)
+{
+	const PxU32 maxSize = nbPrimitives * 2 - 1;	// PT: max possible #nodes for a complete tree
+	const PxU32 estimatedFinalSize = maxSize <= 1024 ? maxSize : maxSize / limit;
+	mPool = PX_NEW(AABBTreeBuildNode)[estimatedFinalSize];
+	PxMemZero(mPool, sizeof(AABBTreeBuildNode)*estimatedFinalSize);
+
+	// Setup initial node. Here we have a complete permutation of the app's primitives.
+	mPool->mNodeIndex = 0;
+	mPool->mNbPrimitives = nbPrimitives;
+
+	mSlabs.pushBack(Slab(mPool, 1, estimatedFinalSize));
+	mCurrentSlabIndex = 0;
+	mTotalNbNodes = 1;
+}
+
+// PT: TODO: inline this?
+AABBTreeBuildNode* NodeAllocator::getBiNode()
+{
+	mTotalNbNodes += 2;
+	Slab& currentSlab = mSlabs[mCurrentSlabIndex];
+	if (currentSlab.mNbUsedNodes + 2 <= currentSlab.mMaxNbNodes)
+	{
+		AABBTreeBuildNode* biNode = currentSlab.mPool + currentSlab.mNbUsedNodes;
+		currentSlab.mNbUsedNodes += 2;
+		return biNode;
+	}
+	else
+	{
+		// Allocate new slab
+		const PxU32 size = 1024;
+		AABBTreeBuildNode* pool = PX_NEW(AABBTreeBuildNode)[size];
+		PxMemZero(pool, sizeof(AABBTreeBuildNode)*size);
+
+		mSlabs.pushBack(Slab(pool, 2, size));
+		mCurrentSlabIndex++;
+		return pool;
+	}
+}
+
+static PX_FORCE_INLINE float getSplittingValue(const PxBounds3& global_box, PxU32 axis)
+{
+	// Default split value = middle of the axis (using only the box)
+	return global_box.getCenter(axis);
+}
+
+static PxU32 split(const PxBounds3& box, PxU32 nb, PxU32* const PX_RESTRICT prims, PxU32 axis, const AABBTreeBuildParams& params)
+{
+	// Get node split value
+	const float splitValue = getSplittingValue(box, axis);
+
+	PxU32 nbPos = 0;
+	// Loop through all node-related primitives. Their indices range from "mNodePrimitives[0]" to "mNodePrimitives[mNbPrimitives-1]",
+	// with mNodePrimitives = mIndices + mNodeIndex (i.e. those indices map the global list in the tree params).
+
+	// PT: to avoid calling the unsafe [] operator
+	const size_t ptrValue = size_t(params.mCache) + axis * sizeof(float);
+	const PxVec3* /*PX_RESTRICT*/ cache = reinterpret_cast<const PxVec3*>(ptrValue);
+
+	for (PxU32 i = 0; i<nb; i++)
+	{
+		// Get index in global list
+		const PxU32 index = prims[i];
+
+		// Test against the splitting value. The primitive value is tested against the enclosing-box center.
+		// [We only need an approximate partition of the enclosing box here.]
+		const float primitiveValue = cache[index].x;
+		PX_ASSERT(primitiveValue == params.mCache[index][axis]);
+
+		// Reorganize the list of indices in this order: positive - negative.
+		if (primitiveValue > splitValue)
+		{
+			// Swap entries
+			prims[i] = prims[nbPos];
+			prims[nbPos] = index;
+			// Count primitives assigned to positive space
+			nbPos++;
+		}
+	}
+	return nbPos;
+}
+
+void AABBTreeBuildNode::subdivide(const AABBTreeBuildParams& params, BuildStats& stats, NodeAllocator& allocator, PxU32* const indices)
+{
+	PxU32* const PX_RESTRICT primitives = indices + mNodeIndex;
+	const PxU32 nbPrims = mNbPrimitives;
+
+	// Compute global box & means for current node. The box is stored in mBV.
+	Vec4V meansV;
+	{
+		const PxBounds3* PX_RESTRICT boxes = params.mAABBArray;
+		PX_ASSERT(boxes);
+		PX_ASSERT(primitives);
+		PX_ASSERT(nbPrims);
+
+		Vec4V minV = V4LoadU(&boxes[primitives[0]].minimum.x);
+		Vec4V maxV = V4LoadU(&boxes[primitives[0]].maximum.x);
+
+		meansV = V4LoadU(&params.mCache[primitives[0]].x);
+
+		for (PxU32 i = 1; i<nbPrims; i++)
+		{
+			const PxU32 index = primitives[i];
+			const Vec4V curMinV = V4LoadU(&boxes[index].minimum.x);
+			const Vec4V curMaxV = V4LoadU(&boxes[index].maximum.x);
+			meansV = V4Add(meansV, V4LoadU(&params.mCache[index].x));
+			minV = V4Min(minV, curMinV);
+			maxV = V4Max(maxV, curMaxV);
+		}
+
+		StoreBounds(mBV, minV, maxV);
+
+		const float coeff = 1.0f / float(nbPrims);
+		meansV = V4Scale(meansV, FLoad(coeff));
+	}
+
+	// Check the user-defined limit. Also ensures we stop subdividing if we reach a leaf node.
+	if (nbPrims <= params.mLimit)
+		return;
+
+	bool validSplit = true;
+	PxU32 nbPos;
+	{
+		// Compute variances
+		Vec4V varsV = V4Zero();
+		for (PxU32 i = 0; i<nbPrims; i++)
+		{
+			const PxU32 index = primitives[i];
+			Vec4V centerV = V4LoadU(&params.mCache[index].x);
+			centerV = V4Sub(centerV, meansV);
+			centerV = V4Mul(centerV, centerV);
+			varsV = V4Add(varsV, centerV);
+		}
+		const float coeffNb1 = 1.0f / float(nbPrims - 1);
+		varsV = V4Scale(varsV, FLoad(coeffNb1));
+		PX_ALIGN(16, PxVec4) vars;
+		V4StoreA(varsV, &vars.x);
+
+		// Choose axis with greatest variance
+		const PxU32 axis = Ps::largestAxis(PxVec3(vars.x, vars.y, vars.z));
+
+		// Split along the axis
+		nbPos = split(mBV, nbPrims, primitives, axis, params);
+
+		// Check split validity
+		if (!nbPos || nbPos == nbPrims)
+			validSplit = false;
+	}
+
+	// Check the subdivision has been successful
+	if (!validSplit)
+	{
+		// Here, all boxes lie in the same sub-space. Two strategies:
+		// - if we are over the split limit, make an arbitrary 50-50 split
+		// - else stop subdividing
+		if (nbPrims>params.mLimit)
+		{
+			nbPos = nbPrims >> 1;
+		}
+		else return;
+	}
+
+	// Now create children and assign their pointers.
+	mPos = allocator.getBiNode();
+
+	stats.increaseCount(2);
+
+	// Assign children
+	PX_ASSERT(!isLeaf());
+	AABBTreeBuildNode* Pos = const_cast<AABBTreeBuildNode*>(mPos);
+	AABBTreeBuildNode* Neg = Pos + 1;
+	Pos->mNodeIndex = mNodeIndex;
+	Pos->mNbPrimitives = nbPos;
+	Neg->mNodeIndex = mNodeIndex + nbPos;
+	Neg->mNbPrimitives = mNbPrimitives - nbPos;
+}
+
+void AABBTreeBuildNode::_buildHierarchy(AABBTreeBuildParams& params, BuildStats& stats, NodeAllocator& nodeBase, PxU32* const indices)
+{
+	// Subdivide current node
+	subdivide(params, stats, nodeBase, indices);
+
+	// Recurse
+	if (!isLeaf())
+	{
+		AABBTreeBuildNode* Pos = const_cast<AABBTreeBuildNode*>(getPos());
+		PX_ASSERT(Pos);
+		AABBTreeBuildNode* Neg = Pos + 1;
+		Pos->_buildHierarchy(params, stats, nodeBase, indices);
+		Neg->_buildHierarchy(params, stats, nodeBase, indices);
+	}
+
+	stats.mTotalPrims += mNbPrimitives;
+}
+
+bool Gu::initAABBTreeBuild(AABBTreeBuildParams& params, NodeAllocator& nodeAllocator, BuildStats& stats, PxU32*&  indices)
+{
+	const PxU32 numPrimitives = params.mNbPrimitives;
+
+	if(numPrimitives == 0)
+		return false;	
+
+	// indices already initialized!
+	if(indices)
+		return false;
+
+	// Init stats
+	stats.setCount(1);
+
+	// Initialize indices. This list will be modified during build.
+	indices = reinterpret_cast<PxU32*>(PX_ALLOC(sizeof(PxU32)*numPrimitives, "AABB tree indices"));
+	// Identity permutation
+	for(PxU32 i=0;i<numPrimitives;i++)
+		indices[i] = i;
+
+	// Allocate a pool of nodes
+	nodeAllocator.init(numPrimitives, params.mLimit);
+
+	// Compute box centers only once and cache them
+	params.mCache = reinterpret_cast<PxVec3*>(PX_ALLOC(sizeof(PxVec3)*(numPrimitives+1), "cache"));
+	const float half = 0.5f;
+	const FloatV halfV = FLoad(half);
+	for(PxU32 i=0;i<numPrimitives;i++)
+	{
+		const Vec4V curMinV = V4LoadU(&params.mAABBArray[i].minimum.x);
+		const Vec4V curMaxV = V4LoadU(&params.mAABBArray[i].maximum.x);
+		const Vec4V centerV = V4Scale(V4Add(curMaxV, curMinV), halfV);
+		V4StoreU(centerV, &params.mCache[i].x);
+	}
+
+	return true;
+}
+
+bool Gu::buildAABBTree(AABBTreeBuildParams& params, NodeAllocator& nodeAllocator, BuildStats& stats, PxU32*&  indices)
+{
+	// initialize the build first
+	if(!initAABBTreeBuild(params, nodeAllocator, stats, indices))
+		return false;
+
+	// Build the hierarchy
+	nodeAllocator.mPool->_buildHierarchy(params, stats, nodeAllocator, indices);
+
+	return true;
+}
+

physx/source/geomutils/src/GuAABBTreeBuild.h

@@ -0,0 +1,185 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
+
+
+#ifndef SQ_AABBTREE_BUILD_H
+#define SQ_AABBTREE_BUILD_H
+
+#include "foundation/PxMemory.h"
+#include "foundation/PxBounds3.h"
+#include "common/PxPhysXCommonConfig.h"
+#include "CmPhysXCommon.h"
+#include "PsUserAllocated.h"
+#include "PsVecMath.h"
+#include "PsArray.h"
+
+namespace physx
+{
+
+	using namespace shdfnd::aos;
+
+	namespace Gu
+	{
+#if PX_VC 
+    #pragma warning(push)
+	#pragma warning( disable : 4251 ) // class needs to have dll-interface to be used by clients of class
+#endif
+		//! Contains AABB-tree build statistics
+		struct PX_PHYSX_COMMON_API BuildStats
+		{
+			BuildStats() : mCount(0), mTotalPrims(0) {}
+
+			PxU32	mCount;			//!< Number of nodes created
+			PxU32	mTotalPrims;	//!< Total accumulated number of primitives. Should be much higher than the source
+									//!< number of prims, since it accumulates all prims covered by each node (i.e. internal
+									//!< nodes too, not just leaf ones)
+
+			PX_FORCE_INLINE	void	reset() { mCount = mTotalPrims = 0; }
+
+			PX_FORCE_INLINE	void	setCount(PxU32 nb) { mCount = nb; }
+			PX_FORCE_INLINE	void	increaseCount(PxU32 nb) { mCount += nb; }
+			PX_FORCE_INLINE	PxU32	getCount()				const { return mCount; }
+		};
+
+		//! Contains AABB-tree build parameters
+		class PX_PHYSX_COMMON_API AABBTreeBuildParams : public Ps::UserAllocated
+		{
+		public:
+			AABBTreeBuildParams(PxU32 limit = 1, PxU32 nb_prims = 0, const PxBounds3* boxes = NULL) :
+				mLimit(limit), mNbPrimitives(nb_prims), mAABBArray(boxes), mCache(NULL) {}
+			~AABBTreeBuildParams()
+			{
+				reset();
+			}
+
+			PX_FORCE_INLINE	void	reset()
+			{
+				mLimit = mNbPrimitives = 0;
+				mAABBArray = NULL;
+				PX_FREE_AND_RESET(mCache);
+			}
+
+			PxU32			mLimit;			//!< Limit number of primitives / node. If limit is 1, build a complete tree (2*N-1 nodes)
+			PxU32			mNbPrimitives;	//!< Number of (source) primitives.
+			const	PxBounds3*		mAABBArray;		//!< Shortcut to an app-controlled array of AABBs.
+			PxVec3*			mCache;			//!< Cache for AABB centers - managed by build code.
+		};
+
+		class NodeAllocator;
+
+		//! AABB tree node used for building
+		class PX_PHYSX_COMMON_API AABBTreeBuildNode : public Ps::UserAllocated
+		{
+		public:
+			PX_FORCE_INLINE								AABBTreeBuildNode() {}
+			PX_FORCE_INLINE								~AABBTreeBuildNode() {}
+
+			PX_FORCE_INLINE	const PxBounds3&			getAABB()							const { return mBV; }
+			PX_FORCE_INLINE	const AABBTreeBuildNode*	getPos()							const { return mPos; }
+			PX_FORCE_INLINE	const AABBTreeBuildNode*	getNeg()							const { const AABBTreeBuildNode* P = mPos; return P ? P + 1 : NULL; }
+
+			PX_FORCE_INLINE	bool						isLeaf()							const { return !getPos(); }
+
+			PxBounds3					mBV;	//!< Global bounding-volume enclosing all the node-related primitives
+			const AABBTreeBuildNode*	mPos;	//!< "Positive" & "Negative" children
+
+			PxU32						mNodeIndex;			//!< Index of node-related primitives (in the tree's mIndices array)
+			PxU32						mNbPrimitives;		//!< Number of primitives for this node
+
+															// Data access
+			PX_FORCE_INLINE	PxU32						getNbPrimitives()					const { return mNbPrimitives; }
+
+			PX_FORCE_INLINE	PxU32						getNbRuntimePrimitives()			const { return mNbPrimitives; }
+			PX_FORCE_INLINE void						setNbRunTimePrimitives(PxU32 val) { mNbPrimitives = val; }
+			PX_FORCE_INLINE	const PxU32*				getPrimitives(const PxU32* base)	const { return base + mNodeIndex; }
+			PX_FORCE_INLINE	PxU32*						getPrimitives(PxU32* base) { return base + mNodeIndex; }
+
+			// Internal methods
+			void						subdivide(const AABBTreeBuildParams& params, BuildStats& stats, NodeAllocator& allocator, PxU32* const indices);
+			void						_buildHierarchy(AABBTreeBuildParams& params, BuildStats& stats, NodeAllocator& allocator, PxU32* const indices);
+		};
+
+		//! For complete trees we can predict the final number of nodes and preallocate them. For incomplete trees we can't.
+		//! But we don't want to allocate nodes one by one (which would be quite slow), so we use this helper class to
+		//! allocate N nodes at once, while minimizing the amount of nodes allocated for nothing. An initial amount of
+		//! nodes is estimated using the max number for a complete tree, and the user-defined number of primitives per leaf.
+		//! In ideal cases this estimated number will be quite close to the final number of nodes. When that number is not
+		//! enough though, slabs of N=1024 extra nodes are allocated until the build is complete.
+		class PX_PHYSX_COMMON_API NodeAllocator : public Ps::UserAllocated
+		{
+		public:
+			NodeAllocator();
+			~NodeAllocator();
+
+			void						release();
+			void						init(PxU32 nbPrimitives, PxU32 limit);
+			AABBTreeBuildNode*			getBiNode();
+
+			AABBTreeBuildNode*			mPool;
+
+			struct Slab
+			{
+				PX_FORCE_INLINE	Slab() {}
+				PX_FORCE_INLINE	Slab(AABBTreeBuildNode* pool, PxU32 nbUsedNodes, PxU32 maxNbNodes) : mPool(pool), mNbUsedNodes(nbUsedNodes), mMaxNbNodes(maxNbNodes) {}
+				AABBTreeBuildNode*		mPool;
+				PxU32					mNbUsedNodes;
+				PxU32					mMaxNbNodes;
+			};
+			Ps::Array<Slab>				mSlabs;
+			PxU32						mCurrentSlabIndex;
+			PxU32						mTotalNbNodes;
+		};
+
+		/* 
+		*	\brief		Initialize AABBtree build from given parameters.
+		*	\param		params				[in]	AABBTree build params
+		*	\param		nodeAllocator		[in]	Node allocator
+		*	\param		stats				[out]	Statistics
+		*	\param		indices				[out]	Indices buffer allocated during build
+		*/
+		bool PX_PHYSX_COMMON_API		initAABBTreeBuild(AABBTreeBuildParams& params, NodeAllocator& nodeAllocator, BuildStats& stats, PxU32*&  indices);
+
+		/* 
+		*	\brief		Builds AABBtree from given parameters.
+		*   \note		Initialize will be called!
+		*	\param		params				[in]	AABBTree build params
+		*	\param		nodeAllocator		[in]	Node allocator
+		*	\param		stats				[out]	Statistics
+		*	\param		indices				[out]	Indices buffer allocated during build
+		*/
+		bool PX_PHYSX_COMMON_API		buildAABBTree(AABBTreeBuildParams& params, NodeAllocator& nodeAllocator, BuildStats& stats, PxU32*&  indices);
+#if PX_VC 
+     #pragma warning(pop) 
+#endif
+
+	} // namespace Sq
+
+}
+
+#endif // SQ_AABBTREE_H

physx/source/geomutils/src/GuAABBTreeQuery.h

@@ -0,0 +1,239 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
+
+
+#ifndef GU_AABBTREEQUERY_H
+#define GU_AABBTREEQUERY_H
+
+#include "GuBVHTestsSIMD.h"
+#include "PsInlineArray.h"
+
+namespace physx
+{
+	namespace Gu
+	{
+		#define RAW_TRAVERSAL_STACK_SIZE 256
+
+		//////////////////////////////////////////////////////////////////////////
+
+		static PX_FORCE_INLINE void getBoundsTimesTwo(Vec4V& center, Vec4V& extents, const PxBounds3* boxes, PxU32 poolIndex)
+		{
+			const PxBounds3* objectBounds = boxes + poolIndex;
+
+			const Vec4V minV = V4LoadU(&objectBounds->minimum.x);
+			const Vec4V maxV = V4LoadU(&objectBounds->maximum.x);
+
+			center = V4Add(maxV, minV);
+			extents = V4Sub(maxV, minV);
+		}
+
+		//////////////////////////////////////////////////////////////////////////
+
+		template<typename Test, typename Tree, typename Node, typename Payload, typename QueryCallback>
+		class AABBTreeOverlap
+		{
+		public:
+			bool operator()(const Payload* objects, const PxBounds3* boxes, const Tree& tree, const Test& test, QueryCallback& visitor)
+			{
+				using namespace Cm;
+
+				Ps::InlineArray<const Node*, RAW_TRAVERSAL_STACK_SIZE> stack;
+				stack.forceSize_Unsafe(RAW_TRAVERSAL_STACK_SIZE);
+				const Node* const nodeBase = tree.getNodes();
+				stack[0] = nodeBase;
+				PxU32 stackIndex = 1;
+
+				while (stackIndex > 0)
+				{
+					const Node* node = stack[--stackIndex];
+					Vec3V center, extents;
+					node->getAABBCenterExtentsV(&center, &extents);
+					while (test(center, extents))
+					{
+						if (node->isLeaf())
+						{
+							PxU32 nbPrims = node->getNbPrimitives();
+							const bool doBoxTest = nbPrims > 1;
+							const PxU32* prims = node->getPrimitives(tree.getIndices());
+							while (nbPrims--)
+							{
+								const PxU32* prunableIndex = prims;
+								prims++;
+
+								const PxU32 poolIndex = *prunableIndex;
+								if (doBoxTest)
+								{
+									Vec4V center2, extents2;
+									getBoundsTimesTwo(center2, extents2, boxes, poolIndex);
+
+									const float half = 0.5f;
+									const FloatV halfV = FLoad(half);
+
+									const Vec4V extents_ = V4Scale(extents2, halfV);
+									const Vec4V center_ = V4Scale(center2, halfV);
+
+									if (!test(Vec3V_From_Vec4V(center_), Vec3V_From_Vec4V(extents_)))
+										continue;
+								}
+
+								PxReal unusedDistance;
+								if (!visitor.invoke(unusedDistance, objects[poolIndex]))
+									return false;
+							}
+							break;
+						}
+
+						const Node* children = node->getPos(nodeBase);
+
+						node = children;
+						stack[stackIndex++] = children + 1;
+						if(stackIndex == stack.capacity())
+							stack.resizeUninitialized(stack.capacity() * 2);
+						node->getAABBCenterExtentsV(&center, &extents);
+					}
+				}
+				return true;
+			}
+		};
+
+		//////////////////////////////////////////////////////////////////////////
+
+		template <bool tInflate, typename Tree, typename Node, typename Payload, typename QueryCallback> // use inflate=true for sweeps, inflate=false for raycasts
+		static PX_FORCE_INLINE bool doLeafTest(const Node* node, Gu::RayAABBTest& test, PxReal& md, PxReal oldMaxDist,
+			const Payload* objects, const PxBounds3* boxes, const Tree& tree,
+			PxReal& maxDist, QueryCallback& pcb)
+		{
+			PxU32 nbPrims = node->getNbPrimitives();
+			const bool doBoxTest = nbPrims > 1;
+			const PxU32* prims = node->getPrimitives(tree.getIndices());
+			while (nbPrims--)
+			{
+				const PxU32* prunableIndex = prims;
+				prims++;
+
+				const PxU32 poolIndex = *prunableIndex;
+				if (doBoxTest)
+				{
+					Vec4V center_, extents_;
+					getBoundsTimesTwo(center_, extents_, boxes, poolIndex);
+
+					if (!test.check<tInflate>(Vec3V_From_Vec4V(center_), Vec3V_From_Vec4V(extents_)))
+						continue;
+				}
+
+				if (!pcb.invoke(md, objects[poolIndex]))
+					return false;
+
+				if (md < oldMaxDist)
+				{
+					maxDist = md;
+					test.setDistance(md);
+				}
+			}
+			return true;
+		}
+
+		//////////////////////////////////////////////////////////////////////////
+
+		template <bool tInflate, typename Tree, typename Node, typename Payload, typename QueryCallback> // use inflate=true for sweeps, inflate=false for raycasts
+		class AABBTreeRaycast
+		{
+		public:
+			bool operator()(
+				const Payload* objects, const PxBounds3* boxes, const Tree& tree,
+				const PxVec3& origin, const PxVec3& unitDir, PxReal& maxDist, const PxVec3& inflation,
+				QueryCallback& pcb)
+			{
+				using namespace Cm;
+
+				// PT: we will pass center*2 and extents*2 to the ray-box code, to save some work per-box
+				// So we initialize the test with values multiplied by 2 as well, to get correct results
+				Gu::RayAABBTest test(origin*2.0f, unitDir*2.0f, maxDist, inflation*2.0f);
+
+				Ps::InlineArray<const Node*, RAW_TRAVERSAL_STACK_SIZE> stack;
+				stack.forceSize_Unsafe(RAW_TRAVERSAL_STACK_SIZE);
+				const Node* const nodeBase = tree.getNodes();
+				stack[0] = nodeBase;
+				PxU32 stackIndex = 1;
+
+				PxReal oldMaxDist;
+				while (stackIndex--)
+				{
+					const Node* node = stack[stackIndex];
+					Vec3V center, extents;
+					node->getAABBCenterExtentsV2(&center, &extents);
+					if (test.check<tInflate>(center, extents))	// TODO: try timestamp ray shortening to skip this
+					{
+						PxReal md = maxDist; // has to be before the goto below to avoid compile error
+						while (!node->isLeaf())
+						{
+							const Node* children = node->getPos(nodeBase);
+
+							Vec3V c0, e0;
+							children[0].getAABBCenterExtentsV2(&c0, &e0);
+							const PxU32 b0 = test.check<tInflate>(c0, e0);
+
+							Vec3V c1, e1;
+							children[1].getAABBCenterExtentsV2(&c1, &e1);
+							const PxU32 b1 = test.check<tInflate>(c1, e1);
+
+							if (b0 && b1)	// if both intersect, push the one with the further center on the stack for later
+							{
+								// & 1 because FAllGrtr behavior differs across platforms
+								const PxU32 bit = FAllGrtr(V3Dot(V3Sub(c1, c0), test.mDir), FZero()) & 1;
+								stack[stackIndex++] = children + bit;
+								node = children + (1 - bit);
+								if (stackIndex == stack.capacity())
+									stack.resizeUninitialized(stack.capacity() * 2);
+							}
+							else if (b0)
+								node = children;
+							else if (b1)
+								node = children + 1;
+							else
+								goto skip_leaf_code;
+						}
+
+						oldMaxDist = maxDist; // we copy since maxDist can be updated in the callback and md<maxDist test below can fail
+
+						if (!doLeafTest<tInflate, Tree, Node>(node, test, md, oldMaxDist,
+							objects, boxes, tree,
+							maxDist,
+							pcb))
+							return false;
+					skip_leaf_code:;
+					}
+				}
+				return true;
+			}
+		};
+	}
+}
+
+#endif   // SQ_AABBTREEQUERY_H

physx/source/geomutils/src/GuBVHStructure.cpp

@@ -0,0 +1,186 @@
+//
+// 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-2021 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 "GuBVHStructure.h"
+#include "GuAABBTreeBuild.h"
+#include "GuAABBTreeQuery.h"
+#include "GuSerialize.h"
+#include "GuBounds.h"
+#include "PsFoundation.h"
+#include "CmUtils.h"
+
+using namespace physx;
+using namespace Gu;
+
+BVHStructure::BVHStructure(GuMeshFactory* factory):	
+	PxBVHStructure(PxType(PxConcreteType::eBVH_STRUCTURE), PxBaseFlag::eOWNS_MEMORY | PxBaseFlag::eIS_RELEASABLE),
+	mMeshFactory(factory),
+	mNumVolumes(0),
+	mNumNodes(0),
+	mBounds(NULL),
+	mIndices(NULL),
+	mVolumes(NULL),
+	mNodes(NULL)
+{
+}
+
+BVHStructure::BVHStructure(GuMeshFactory* factory, BVHStructureData& bvhData):
+	PxBVHStructure(PxType(PxConcreteType::eBVH_STRUCTURE), PxBaseFlag::eOWNS_MEMORY | PxBaseFlag::eIS_RELEASABLE),
+	mMeshFactory(factory),
+	mNumVolumes(bvhData.mNumVolumes),
+	mNumNodes(bvhData.mNumNodes),
+	mBounds(bvhData.mBounds),
+	mIndices(bvhData.mIndices),
+	mVolumes(NULL),
+	mNodes(bvhData.mNodes)
+{
+}
+
+BVHStructure::~BVHStructure()
+{	
+}
+
+bool BVHStructure::load(PxInputStream& stream)
+{
+	// Import header
+	PxU32 version;
+	bool mismatch;
+	if(!readHeader('B', 'V', 'H', 'S', version, mismatch, stream))
+		return false;
+
+	// read numVolumes, numNodes together 
+	ReadDwordBuffer(&mNumVolumes, 2, mismatch, stream);	
+
+	// read indices
+	mIndices = reinterpret_cast<PxU32*>(PX_ALLOC(sizeof(PxU32)*mNumVolumes, "BVH indices"));
+	ReadDwordBuffer(mIndices, mNumVolumes, mismatch, stream);
+
+	// read bounds
+	mBounds = reinterpret_cast<PxBounds3*>(PX_ALLOC(sizeof(PxBounds3)*(mNumVolumes + 1), "BVH bounds"));
+	readFloatBuffer(&mBounds[0].minimum.x, mNumVolumes*(3 + 3), mismatch, stream);
+
+	// read nodes
+	mNodes = reinterpret_cast<BVHNode*>(PX_ALLOC(sizeof(BVHNode)*mNumNodes, "BVH nodes"));
+	for(PxU32 i = 0; i < mNumNodes; i++)
+	{
+		ReadDwordBuffer(&mNodes[i].mData, 1, mismatch, stream);
+
+		readFloatBuffer(&mNodes[i].mBV.minimum.x, 3 + 3, mismatch, stream);		
+	}
+	return true;
+}
+
+void BVHStructure::release()
+{
+	decRefCount();
+}
+
+void BVHStructure::onRefCountZero()
+{
+	PX_FREE_AND_RESET(mBounds);
+	PX_FREE_AND_RESET(mIndices);
+	PX_FREE_AND_RESET(mNodes);
+	PX_FREE_AND_RESET(mVolumes);
+
+	mNumNodes = 0;
+	mNumVolumes = 0;
+
+	if(mMeshFactory->removeBVHStructure(*this))
+	{
+		const PxType type = getConcreteType();
+		GuMeshFactory* mf = mMeshFactory;
+		Cm::deletePxBase(this);
+		mf->notifyFactoryListener(this, type);
+		return;
+	}
+
+	// PT: if we reach this point, we didn't find the mesh in the Physics object => don't delete!
+	// This prevents deleting the object twice.
+	Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, "Gu::BVHStructure::release: double deletion detected!");
+}
+
+void BVHStructure::createVolumes() const
+{
+	if(!mVolumes)
+	{
+		mVolumes = reinterpret_cast<PxU32*>(PX_ALLOC(sizeof(PxU32)*mNumVolumes, "BVH volume list"));
+		for(PxU32 i = 0; i < mNumVolumes; i++)
+		{
+			mVolumes[i] = i;
+		}
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Query Implementation
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+PxU32 BVHStructure::raycast(const PxVec3& origin, const PxVec3& unitDir, PxReal maxDist, PxU32 maxHits, PxU32* PX_RESTRICT rayHits) const
+{
+	createVolumes();
+
+	BVHCallback cbk(rayHits, maxHits);
+	BVHTree tree(mNodes, mIndices);
+	AABBTreeRaycast<false, BVHTree, BVHNode, PxU32, BVHCallback>()(mVolumes, mBounds, tree, origin, unitDir, maxDist, PxVec3(0.0f), cbk);
+
+	return cbk.mCurrentHitsCount;
+}
+
+PxU32 BVHStructure::sweep(const PxBounds3& aabb, const PxVec3& unitDir, PxReal maxDist, PxU32 maxHits, PxU32* PX_RESTRICT sweepHits) const
+{
+	createVolumes();
+
+	const PxVec3 extents = aabb.getExtents();
+
+	BVHCallback cbk(sweepHits, maxHits);
+	BVHTree tree(mNodes, mIndices);
+
+	AABBTreeRaycast<true, BVHTree, BVHNode, PxU32, BVHCallback>()(mVolumes, mBounds, tree, aabb.getCenter(), unitDir, maxDist, extents, cbk);
+
+	return cbk.mCurrentHitsCount;
+}
+
+
+PxU32 BVHStructure::overlap(const PxBounds3& aabb, PxU32 maxHits, PxU32* PX_RESTRICT overlapHits) const
+{
+	createVolumes();
+	
+	BVHCallback cbk(overlapHits, maxHits);
+	BVHTree tree(mNodes, mIndices);
+
+	const Gu::AABBAABBTest test(aabb);
+	AABBTreeOverlap<Gu::AABBAABBTest, BVHTree, BVHNode, PxU32, BVHCallback>()(mVolumes, mBounds, tree, test, cbk);
+
+	return cbk.mCurrentHitsCount;
+}
+

physx/source/geomutils/src/GuBVHStructure.h

@@ -0,0 +1,201 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
+
+
+#ifndef GU_BVH_STRUCTURE_H
+#define GU_BVH_STRUCTURE_H
+
+/** \addtogroup geomutils
+@{
+*/
+
+#include "geometry/PxBVHStructure.h"
+
+#include "CmRefCountable.h"
+#include "CmPhysXCommon.h"
+#include "GuMeshFactory.h"
+#include "PsVecMath.h"
+#include "PsUserAllocated.h"
+
+namespace physx
+{
+namespace Gu
+{
+	struct BVHNode
+	{
+		public:
+		PX_FORCE_INLINE	PxU32						isLeaf()								const	{ return mData&1;			}
+
+		PX_FORCE_INLINE	const PxU32*				getPrimitives(const PxU32* base)		const	{ return base + (mData>>5);	}
+		PX_FORCE_INLINE	PxU32*						getPrimitives(PxU32* base)						{ return base + (mData>>5);	}
+		PX_FORCE_INLINE	PxU32						getNbPrimitives()						const	{ return (mData>>1)&15;		}
+
+		PX_FORCE_INLINE	PxU32						getPosIndex()							const	{ return mData>>1;			}
+		PX_FORCE_INLINE	PxU32						getNegIndex()							const	{ return (mData>>1) + 1;			}
+		PX_FORCE_INLINE	const BVHNode*				getPos(const BVHNode* base)	const	{ return base + (mData>>1);	}
+		PX_FORCE_INLINE	const BVHNode*				getNeg(const BVHNode* base)	const	{ const BVHNode* P = getPos(base); return P ? P+1 : NULL;}
+
+		PX_FORCE_INLINE	BVHNode*					getPos(BVHNode* base)				{ return base + (mData >> 1); }
+		PX_FORCE_INLINE	BVHNode*					getNeg(BVHNode* base)				{ BVHNode* P = getPos(base); return P ? P + 1 : NULL; }
+
+
+		PX_FORCE_INLINE	void						getAABBCenterExtentsV(shdfnd::aos::Vec3V* center, shdfnd::aos::Vec3V* extents) const
+													{
+														const shdfnd::aos::Vec4V minV = shdfnd::aos::V4LoadU(&mBV.minimum.x);
+														const shdfnd::aos::Vec4V maxV = shdfnd::aos::V4LoadU(&mBV.maximum.x);
+
+														const float half = 0.5f;
+														const shdfnd::aos::FloatV halfV = shdfnd::aos::FLoad(half);
+
+														*extents = shdfnd::aos::Vec3V_From_Vec4V(shdfnd::aos::V4Scale(shdfnd::aos::V4Sub(maxV, minV), halfV));
+														*center = shdfnd::aos::Vec3V_From_Vec4V(shdfnd::aos::V4Scale(shdfnd::aos::V4Add(maxV, minV), halfV));
+													}
+
+		PX_FORCE_INLINE	void						getAABBCenterExtentsV2(shdfnd::aos::Vec3V* center, shdfnd::aos::Vec3V* extents) const
+													{
+														const shdfnd::aos::Vec4V minV = shdfnd::aos::V4LoadU(&mBV.minimum.x);
+														const shdfnd::aos::Vec4V maxV = shdfnd::aos::V4LoadU(&mBV.maximum.x);
+
+														*extents = shdfnd::aos::Vec3V_From_Vec4V(shdfnd::aos::V4Sub(maxV, minV));
+														*center = shdfnd::aos::Vec3V_From_Vec4V(shdfnd::aos::V4Add(maxV, minV));
+													}
+
+		PX_FORCE_INLINE	void						getAABBMinMaxV(shdfnd::aos::Vec4V* minV, shdfnd::aos::Vec4V* maxV) const
+													{
+														*minV = shdfnd::aos::V4LoadU(&mBV.minimum.x);
+														*maxV = shdfnd::aos::V4LoadU(&mBV.maximum.x);
+													}
+
+						PxBounds3					mBV;	// Global bounding-volume enclosing all the node-related primitives
+						PxU32						mData;	// 27 bits node or prim index|4 bits #prims|1 bit leaf
+	};
+
+	struct BVHTree
+	{
+		BVHTree(const BVHNode* node, const PxU32* indices):
+			mRootNode(node),
+			mIndices(indices)
+		{
+		}
+
+
+		const BVHNode*	getNodes() const { return mRootNode; }
+		const PxU32*	getIndices() const { return mIndices; }
+
+		const BVHNode*	mRootNode;
+		const PxU32*	mIndices;
+	};
+
+	struct BVHCallback
+	{
+		BVHCallback(PxU32* hits, PxU32 numMaxHits):
+			mHits(hits),
+			mNbMaxHits(numMaxHits),
+			mCurrentHitsCount(0)
+		{
+		}
+
+		bool invoke(PxReal& , PxU32 payload)
+		{
+			mHits[mCurrentHitsCount++] = payload;
+			if(mCurrentHitsCount == mNbMaxHits)
+				return false;
+
+			return true;
+		}
+
+		PxU32*		mHits;
+		PxU32		mNbMaxHits;
+		PxU32		mCurrentHitsCount;
+	};
+
+	struct BVHStructureData
+	{
+		PxU32				mNumVolumes;
+		PxU32				mNumNodes;
+		PxBounds3*			mBounds;
+		PxU32*				mIndices;		
+		BVHNode*			mNodes;
+	};
+/**
+\brief Represents a BVH.
+*/
+	class BVHStructure: public PxBVHStructure, public Ps::UserAllocated, public Cm::RefCountable
+	{
+	public:
+		/**
+		\brief Constructor
+		*/
+		BVHStructure(GuMeshFactory* factory);
+		BVHStructure(GuMeshFactory* factory, BVHStructureData& data);
+
+		/**
+		\brief Destructor
+		*/
+		~BVHStructure();
+
+
+		bool							load(PxInputStream& desc);
+
+		void							release();
+
+//		PxBVHStructure
+		PxU32							getNbBounds() const { return mNumVolumes; }
+		const PxBounds3*				getBounds() const { return mBounds; }
+		PxU32							raycast(const PxVec3& origin, const PxVec3& unitDir, PxReal maxDist, PxU32 maxHits, PxU32* PX_RESTRICT rayHits) const;
+		PxU32							sweep(const PxBounds3& aabb, const PxVec3& unitDir, PxReal maxDist, PxU32 maxHits, PxU32* PX_RESTRICT sweepHits) const;
+		PxU32							overlap(const PxBounds3& aabb, PxU32 maxHits, PxU32* PX_RESTRICT overlapHits) const;
+//		~PxBVHStructure
+
+// Cm::RefCountable
+		virtual	void					onRefCountZero();
+//~Cm::RefCountable
+
+		const BVHNode*					getNodes() const { return mNodes; }
+		const PxU32*					getIndices() const { return mIndices; }
+
+	private:
+		void							createVolumes() const;
+
+	private:
+		GuMeshFactory*					mMeshFactory;
+
+		PxU32							mNumVolumes;
+		PxU32							mNumNodes;
+		PxBounds3*						mBounds;
+		PxU32*							mIndices;
+		mutable PxU32*					mVolumes;			// used just for queries
+		BVHNode*						mNodes;
+	};
+}
+
+}
+
+/** @} */
+#endif

physx/source/geomutils/src/GuBVHTestsSIMD.h

@@ -0,0 +1,259 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
+
+
+#ifndef GU_RAWQUERY_TESTS_SIMD_H
+#define GU_RAWQUERY_TESTS_SIMD_H
+
+#include "foundation/PxTransform.h"
+#include "foundation/PxBounds3.h"
+#include "geometry/PxBoxGeometry.h"
+#include "geometry/PxSphereGeometry.h"
+#include "geometry/PxCapsuleGeometry.h"
+#include "CmPhysXCommon.h"
+#include "PsVecMath.h"
+
+namespace physx
+{
+namespace Gu
+{
+
+struct RayAABBTest
+{
+	PX_FORCE_INLINE RayAABBTest(const PxVec3& origin_, const PxVec3& unitDir_, const PxReal maxDist, const PxVec3& inflation_)
+	: mOrigin(V3LoadU(origin_))
+	, mDir(V3LoadU(unitDir_))
+	, mDirYZX(V3PermYZX(mDir))
+	, mInflation(V3LoadU(inflation_))
+	, mAbsDir(V3Abs(mDir))
+	, mAbsDirYZX(V3PermYZX(mAbsDir))
+	{
+		const PxVec3 ext = maxDist >= PX_MAX_F32 ? PxVec3(	unitDir_.x == 0 ? origin_.x : PxSign(unitDir_.x)*PX_MAX_F32,
+															unitDir_.y == 0 ? origin_.y : PxSign(unitDir_.y)*PX_MAX_F32,
+															unitDir_.z == 0 ? origin_.z : PxSign(unitDir_.z)*PX_MAX_F32)
+										   : origin_ + unitDir_ * maxDist;
+		mRayMin = V3Min(mOrigin, V3LoadU(ext));
+		mRayMax = V3Max(mOrigin, V3LoadU(ext));
+	}
+
+	PX_FORCE_INLINE void setDistance(PxReal distance)
+	{
+		const Vec3V ext = V3ScaleAdd(mDir, FLoad(distance), mOrigin);
+		mRayMin = V3Min(mOrigin, ext);
+		mRayMax = V3Max(mOrigin, ext);
+	}
+
+	template<bool TInflate>
+	PX_FORCE_INLINE PxU32 check(const Vec3V center, const Vec3V extents) const
+	{
+		const Vec3V iExt = TInflate ? V3Add(extents, mInflation) : extents;
+
+		// coordinate axes
+		const Vec3V nodeMax = V3Add(center, iExt);
+		const Vec3V nodeMin = V3Sub(center, iExt);
+
+		// cross axes
+		const Vec3V offset = V3Sub(mOrigin, center);
+		const Vec3V offsetYZX = V3PermYZX(offset);
+		const Vec3V iExtYZX = V3PermYZX(iExt);
+	
+		const Vec3V f = V3NegMulSub(mDirYZX, offset, V3Mul(mDir, offsetYZX));
+		const Vec3V g = V3MulAdd(iExt, mAbsDirYZX, V3Mul(iExtYZX, mAbsDir));
+
+		const BoolV
+			maskA = V3IsGrtrOrEq(nodeMax, mRayMin),
+			maskB = V3IsGrtrOrEq(mRayMax, nodeMin),
+			maskC = V3IsGrtrOrEq(g, V3Abs(f));
+		const BoolV andABCMasks = BAnd(BAnd(maskA, maskB), maskC);
+
+		return BAllEqTTTT(andABCMasks);
+	}
+
+	const Vec3V mOrigin, mDir, mDirYZX, mInflation, mAbsDir, mAbsDirYZX;
+	Vec3V mRayMin, mRayMax;
+protected:
+	RayAABBTest& operator=(const RayAABBTest&);
+};
+
+// probably not worth having a SIMD version of this unless the traversal passes Vec3Vs
+struct AABBAABBTest
+{
+	PX_FORCE_INLINE AABBAABBTest(const PxTransform&t, const PxBoxGeometry&b)
+	: mCenter(V3LoadU(t.p))
+	, mExtents(V3LoadU(b.halfExtents))
+	{ }
+
+	PX_FORCE_INLINE AABBAABBTest(const PxBounds3& b)
+	: mCenter(V3LoadU(b.getCenter()))
+	, mExtents(V3LoadU(b.getExtents()))
+	{ }
+
+	PX_FORCE_INLINE Ps::IntBool operator()(const Vec3V center, const Vec3V extents) const		
+	{	
+		//PxVec3 c; PxVec3_From_Vec3V(center, c);
+		//PxVec3 e; PxVec3_From_Vec3V(extents, e);
+		//if(PxAbs(c.x - mCenter.x) > mExtents.x + e.x) return Ps::IntFalse;
+		//if(PxAbs(c.y - mCenter.y) > mExtents.y + e.y) return Ps::IntFalse;
+		//if(PxAbs(c.z - mCenter.z) > mExtents.z + e.z) return Ps::IntFalse;
+		//return Ps::IntTrue;
+		return Ps::IntBool(V3AllGrtrOrEq(V3Add(mExtents, extents), V3Abs(V3Sub(center, mCenter))));
+	}
+
+private:
+	AABBAABBTest& operator=(const AABBAABBTest&);
+	const Vec3V mCenter, mExtents;
+};
+
+struct SphereAABBTest
+{
+	PX_FORCE_INLINE SphereAABBTest(const PxTransform& t, const PxSphereGeometry& s)
+	: mCenter(V3LoadU(t.p))
+	, mRadius2(FLoad(s.radius * s.radius))
+	{}
+
+	PX_FORCE_INLINE SphereAABBTest(const PxVec3& center, PxF32 radius)
+	: mCenter(V3LoadU(center))
+	, mRadius2(FLoad(radius * radius))
+	{}
+	
+	PX_FORCE_INLINE Ps::IntBool operator()(const Vec3V boxCenter, const Vec3V boxExtents) const		
+	{	
+		const Vec3V offset = V3Sub(mCenter, boxCenter);
+		const Vec3V closest = V3Clamp(offset, V3Neg(boxExtents), boxExtents);
+		const Vec3V d = V3Sub(offset, closest);
+		return Ps::IntBool(BAllEqTTTT(FIsGrtrOrEq(mRadius2, V3Dot(d, d))));
+	}
+
+private:
+	SphereAABBTest& operator=(const SphereAABBTest&);
+	const Vec3V mCenter;
+	const FloatV mRadius2;
+};
+
+// The Opcode capsule-AABB traversal test seems to be *exactly* the same as the ray-box test inflated by the capsule radius (so not a true capsule/box test)
+// and the code for the ray-box test is better. TODO: check the zero length case and use the sphere traversal if this one fails.
+// (OTOH it's not that hard to adapt the Ray-AABB test to a capsule test)
+
+struct CapsuleAABBTest: private RayAABBTest
+{
+	PX_FORCE_INLINE CapsuleAABBTest(const PxVec3& origin, const PxVec3& unitDir, const PxReal length, const PxVec3& inflation)
+		: RayAABBTest(origin, unitDir, length, inflation)
+	{}
+
+	PX_FORCE_INLINE Ps::IntBool operator()(const Vec3VArg center, const Vec3VArg extents) const
+	{
+		return Ps::IntBool(RayAABBTest::check<true>(center, extents));
+	}
+};
+
+template<bool fullTest>
+struct OBBAABBTests
+{
+	OBBAABBTests(const PxVec3& pos, const PxMat33& rot, const PxVec3& halfExtentsInflated)
+	{
+		const Vec3V eps = V3Load(1e-6f);
+
+		mT = V3LoadU(pos);
+		mExtents = V3LoadU(halfExtentsInflated);
+
+		// storing the transpose matrices yields a simpler SIMD test
+		mRT = Mat33V_From_PxMat33(rot.getTranspose());
+		mART = Mat33V(V3Add(V3Abs(mRT.col0), eps), V3Add(V3Abs(mRT.col1), eps), V3Add(V3Abs(mRT.col2), eps));
+		mBB_xyz = M33TrnspsMulV3(mART, mExtents);
+
+		if(fullTest)
+		{
+			const Vec3V eYZX = V3PermYZX(mExtents), eZXY = V3PermZXY(mExtents);
+
+			mBB_123 = V3MulAdd(eYZX, V3PermZXY(mART.col0), V3Mul(eZXY, V3PermYZX(mART.col0)));
+			mBB_456 = V3MulAdd(eYZX, V3PermZXY(mART.col1), V3Mul(eZXY, V3PermYZX(mART.col1)));
+			mBB_789 = V3MulAdd(eYZX, V3PermZXY(mART.col2), V3Mul(eZXY, V3PermYZX(mART.col2)));
+		}
+	}
+
+	// TODO: force inline it?
+	Ps::IntBool operator()(const Vec3V center, const Vec3V extents) const
+	{	
+		const Vec3V t = V3Sub(mT, center);
+
+		// class I - axes of AABB
+		if(V3OutOfBounds(t, V3Add(extents, mBB_xyz)))
+			return Ps::IntFalse;
+
+		const Vec3V rX = mRT.col0, rY = mRT.col1, rZ = mRT.col2;
+		const Vec3V arX = mART.col0, arY = mART.col1, arZ = mART.col2;
+
+		const FloatV eX = V3GetX(extents), eY = V3GetY(extents), eZ = V3GetZ(extents);
+		const FloatV tX = V3GetX(t), tY = V3GetY(t), tZ = V3GetZ(t);
+
+		// class II - axes of OBB
+		{
+			const Vec3V v = V3ScaleAdd(rZ, tZ, V3ScaleAdd(rY, tY, V3Scale(rX, tX)));
+			const Vec3V v2 = V3ScaleAdd(arZ, eZ, V3ScaleAdd(arY, eY, V3ScaleAdd(arX, eX, mExtents)));
+			if(V3OutOfBounds(v, v2))
+				return Ps::IntFalse;
+		}
+
+		if(!fullTest)
+			return Ps::IntTrue;
+
+		// class III - edge cross products. Almost all OBB tests early-out with type I or type II,
+		// so early-outs here probably aren't useful (TODO: profile)
+
+		const Vec3V va = V3NegScaleSub(rZ, tY, V3Scale(rY, tZ));
+		const Vec3V va2 = V3ScaleAdd(arY, eZ, V3ScaleAdd(arZ, eY, mBB_123));
+		const BoolV ba = BOr(V3IsGrtr(va, va2), V3IsGrtr(V3Neg(va2), va));
+	
+		const Vec3V vb = V3NegScaleSub(rX, tZ, V3Scale(rZ, tX));
+		const Vec3V vb2 = V3ScaleAdd(arX, eZ, V3ScaleAdd(arZ, eX, mBB_456));
+		const BoolV bb = BOr(V3IsGrtr(vb, vb2), V3IsGrtr(V3Neg(vb2), vb));
+		
+		const Vec3V vc = V3NegScaleSub(rY, tX, V3Scale(rX, tY));
+		const Vec3V vc2 = V3ScaleAdd(arX, eY, V3ScaleAdd(arY, eX, mBB_789));
+		const BoolV bc = BOr(V3IsGrtr(vc, vc2), V3IsGrtr(V3Neg(vc2), vc));
+
+		return Ps::IntBool(BAllEqFFFF(BOr(ba, BOr(bb,bc))));
+	}
+
+	Vec3V		mExtents;	// extents of OBB
+	Vec3V		mT;			// translation of OBB
+	Mat33V		mRT;		// transpose of rotation matrix of OBB
+	Mat33V		mART;		// transpose of mRT, padded by epsilon
+
+	Vec3V		mBB_xyz;	// extents of OBB along coordinate axes
+	Vec3V		mBB_123;	// projections of extents onto edge-cross axes
+	Vec3V		mBB_456;
+	Vec3V		mBB_789;
+};
+
+typedef OBBAABBTests<true> OBBAABBTest;
+
+}
+}
+#endif

physx/source/geomutils/src/GuBounds.cpp

@@ -0,0 +1,587 @@
+//
+// 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-2021 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/PxBoxGeometry.h"
+#include "geometry/PxSphereGeometry.h"
+#include "geometry/PxCapsuleGeometry.h"
+#include "geometry/PxPlaneGeometry.h"
+#include "geometry/PxConvexMeshGeometry.h"
+#include "geometry/PxTriangleMeshGeometry.h"
+#include "geometry/PxHeightFieldGeometry.h"
+#include "GuBounds.h"
+#include "GuInternal.h"
+#include "CmUtils.h"
+#include "GuConvexMesh.h"
+#include "GuConvexMeshData.h"
+#include "GuTriangleMesh.h"
+#include "GuHeightFieldData.h"
+#include "GuHeightField.h"
+#include "PsFoundation.h"
+#include "GuConvexUtilsInternal.h"
+#include "GuBoxConversion.h"
+
+using namespace physx;
+using namespace Gu;
+using namespace Ps::aos;
+
+static PX_FORCE_INLINE void transformNoEmptyTest(Vec3p& c, Vec3p& ext, const PxMat33& rot, const PxVec3& pos, const CenterExtentsPadded& bounds)
+{
+	c = rot.transform(bounds.mCenter) + pos;
+	ext = Cm::basisExtent(rot.column0, rot.column1, rot.column2, bounds.mExtents);
+}
+
+// PT: this one may have duplicates in GuBV4_BoxSweep_Internal.h & GuBV4_Raycast.cpp
+static PX_FORCE_INLINE Vec4V multiply3x3V(const Vec4V p, const PxMat33Padded& mat_Padded)
+{
+	Vec4V ResV = V4Scale(V4LoadU(&mat_Padded.column0.x), V4GetX(p));
+	ResV = V4Add(ResV, V4Scale(V4LoadU(&mat_Padded.column1.x), V4GetY(p)));
+	ResV = V4Add(ResV, V4Scale(V4LoadU(&mat_Padded.column2.x), V4GetZ(p)));
+	return ResV;
+}
+
+static PX_FORCE_INLINE void transformNoEmptyTestV(Vec3p& c, Vec3p& ext, const PxMat33Padded& rot, const PxVec3& pos, const CenterExtentsPadded& bounds)
+{
+	const Vec4V boundsCenterV = V4LoadU(&bounds.mCenter.x);	// PT: this load is safe since extents follow center in the class
+
+	// PT: unfortunately we can't V4LoadU 'pos' directly (it can come directly from users!). So we have to live with this for now:
+	const Vec4V posV = Vec4V_From_Vec3V(V3LoadU(&pos.x));
+	// PT: but eventually we'd like to use the "unsafe" version (e.g. by switching p&q in PxTransform), which would save 6 instructions on Win32
+	const Vec4V cV = V4Add(multiply3x3V(boundsCenterV, rot), posV);
+//	const Vec4V cV = V4Add(multiply3x3V(boundsCenterV, rot), V4LoadU(&pos.x));	// ### unsafe
+	V4StoreU(cV, &c.x);
+
+	// extended basis vectors
+	const Vec4V boundsExtentsV = V4LoadU(&bounds.mExtents.x);	// PT: this load is safe since bounds are padded
+	const Vec4V c0V = V4Scale(V4LoadU(&rot.column0.x), V4GetX(boundsExtentsV));
+	const Vec4V c1V = V4Scale(V4LoadU(&rot.column1.x), V4GetY(boundsExtentsV));
+	const Vec4V c2V = V4Scale(V4LoadU(&rot.column2.x), V4GetZ(boundsExtentsV));
+
+	// find combination of base vectors that produces max. distance for each component = sum of abs()
+	Vec4V extentsV = V4Add(V4Abs(c0V), V4Abs(c1V));
+	extentsV = V4Add(extentsV, V4Abs(c2V));
+	V4StoreU(extentsV, &ext.x);
+}
+
+static PX_FORCE_INLINE PxU32 isNonIdentity(const PxVec3& scale)
+{
+	#define IEEE_1_0	0x3f800000	//!< integer representation of 1.0
+	const PxU32* binary = reinterpret_cast<const PxU32*>(&scale.x);
+	return (binary[0] - IEEE_1_0)|(binary[1] - IEEE_1_0)|(binary[2] - IEEE_1_0);
+}
+
+// PT: please don't inline this one - 300+ lines of rarely used code
+static void computeScaledMatrix(PxMat33Padded& rot, const PxMeshScale& scale)
+{
+	rot = rot * scale.toMat33();
+}
+
+static PX_FORCE_INLINE void transformNoEmptyTest(Vec3p& c, Vec3p& ext, const PxTransform& transform, const PxMeshScale& scale, const CenterExtentsPadded& bounds)
+{
+	PxMat33Padded rot(transform.q);
+
+	if(isNonIdentity(scale.scale))
+		computeScaledMatrix(rot, scale);
+
+	transformNoEmptyTestV(c, ext, rot, transform.p, bounds);
+}
+
+static PX_FORCE_INLINE void transformNoEmptyTest(Vec3p& c, Vec3p& ext, const PxVec3& pos, const PxMat33Padded& rot, const PxMeshScale& scale, const CenterExtentsPadded& bounds)
+{
+	if(scale.isIdentity())
+		transformNoEmptyTest(c, ext, rot, pos, bounds);
+	else
+		transformNoEmptyTest(c, ext, rot * scale.toMat33(), pos, bounds);
+}
+
+static void computeMeshBounds(const PxTransform& pose, const CenterExtentsPadded* PX_RESTRICT localSpaceBounds, const PxMeshScale& meshScale, Vec3p& origin, Vec3p& extent)
+{
+	transformNoEmptyTest(origin, extent, pose, meshScale, *localSpaceBounds);
+}
+
+static void computePlaneBounds(PxBounds3& bounds, const PxTransform& pose, float contactOffset, float inflation)
+{
+	// PT: A plane is infinite, so usually the bounding box covers the whole world.
+	// Now, in particular cases when the plane is axis-aligned, we can take
+	// advantage of this to compute a smaller bounding box anyway.
+
+	// PT: we use PX_MAX_BOUNDS_EXTENTS to be compatible with PxBounds3::setMaximal,
+	// and to make sure that the value doesn't collide with the BP's sentinels.
+	const PxF32 bigValue = PX_MAX_BOUNDS_EXTENTS;
+//	const PxF32 bigValue = 1000000.0f;
+	PxVec3 minPt = PxVec3(-bigValue, -bigValue, -bigValue);
+	PxVec3 maxPt = PxVec3(bigValue, bigValue, bigValue);
+
+	const PxVec3 planeNormal = pose.q.getBasisVector0();
+	const PxPlane plane(pose.p, planeNormal);
+
+	const float nx = PxAbs(planeNormal.x);
+	const float ny = PxAbs(planeNormal.y);
+	const float nz = PxAbs(planeNormal.z);
+	const float epsilon = 1e-6f;
+	const float oneMinusEpsilon = 1.0f - epsilon;
+	if(nx>oneMinusEpsilon && ny<epsilon && nz<epsilon)
+	{
+		if(planeNormal.x>0.0f)	maxPt.x = -plane.d + contactOffset;
+		else					minPt.x = plane.d - contactOffset;
+	}
+	else if(nx<epsilon && ny>oneMinusEpsilon && nz<epsilon)
+	{
+		if(planeNormal.y>0.0f)	maxPt.y = -plane.d + contactOffset;
+		else					minPt.y = plane.d - contactOffset;
+	}
+	else if(nx<epsilon && ny<epsilon && nz>oneMinusEpsilon)
+	{
+		if(planeNormal.z>0.0f)	maxPt.z = -plane.d + contactOffset;
+		else					minPt.z = plane.d - contactOffset;
+	}
+
+	// PT: it is important to compute the min/max form directly without going through the
+	// center/extents intermediate form. With PX_MAX_BOUNDS_EXTENTS, those back-and-forth
+	// computations destroy accuracy.
+
+	// PT: inflation actually destroys the bounds really. We keep it to please UTs but this is broken (DE10595).
+	// (e.g. for SQ 1% of PX_MAX_BOUNDS_EXTENTS is still a huge number, effectively making the AABB infinite and defeating the point of the above computation)
+	if(inflation!=1.0f)
+	{
+		const PxVec3 c = (maxPt + minPt)*0.5f;
+		const PxVec3 e = (maxPt - minPt)*0.5f*inflation;
+		minPt = c - e;
+		maxPt = c + e;
+	}
+
+	bounds.minimum = minPt;
+	bounds.maximum = maxPt;
+}
+
+static PX_FORCE_INLINE void inflateBounds(PxBounds3& bounds, const Vec3p& origin, const Vec3p& extents, float contactOffset, float inflation)
+{
+	Vec4V extentsV = V4LoadU(&extents.x);
+	extentsV = V4Add(extentsV, V4Load(contactOffset));
+	extentsV = V4Scale(extentsV, FLoad(inflation));
+
+	const Vec4V originV = V4LoadU(&origin.x);
+	const Vec4V minV = V4Sub(originV, extentsV);
+	const Vec4V maxV = V4Add(originV, extentsV);
+
+	StoreBounds(bounds, minV, maxV);
+}
+
+static PX_FORCE_INLINE Vec4V basisExtentV(const PxMat33Padded& basis, const PxVec3& extent, float offset, float inflation)
+{
+	// extended basis vectors
+	const Vec4V c0V = V4Scale(V4LoadU(&basis.column0.x), FLoad(extent.x));
+	const Vec4V c1V = V4Scale(V4LoadU(&basis.column1.x), FLoad(extent.y));
+	const Vec4V c2V = V4Scale(V4LoadU(&basis.column2.x), FLoad(extent.z));
+
+	// find combination of base vectors that produces max. distance for each component = sum of abs()
+	Vec4V extentsV = V4Add(V4Abs(c0V), V4Abs(c1V));
+	extentsV = V4Add(extentsV, V4Abs(c2V));
+	extentsV = V4Add(extentsV, V4Load(offset));
+	extentsV = V4Scale(extentsV, FLoad(inflation));
+	return extentsV;
+}
+
+void Gu::computeBounds(PxBounds3& bounds, const PxGeometry& geometry, const PxTransform& pose, float contactOffset, const CenterExtentsPadded* PX_RESTRICT localSpaceBounds, float inflation)
+{
+	PX_ASSERT(contactOffset==0.0f || inflation==1.0f);
+
+	// Box, Convex, Mesh and HeightField will compute local bounds and pose to world space.
+	// Sphere, Capsule & Plane will compute world space bounds directly.
+
+	switch(geometry.getType())
+	{
+		case PxGeometryType::eSPHERE:
+		{
+			PX_ASSERT(!localSpaceBounds);
+
+			const PxSphereGeometry& shape = static_cast<const PxSphereGeometry&>(geometry);
+			const PxVec3 extents((shape.radius+contactOffset)*inflation);
+			bounds.minimum = pose.p - extents;
+			bounds.maximum = pose.p + extents;
+		}
+		break;
+
+		case PxGeometryType::ePLANE:
+		{
+			PX_ASSERT(!localSpaceBounds);
+
+			computePlaneBounds(bounds, pose, contactOffset, inflation);
+		}
+		break;
+
+		case PxGeometryType::eCAPSULE:
+		{
+			PX_ASSERT(!localSpaceBounds);
+
+			const PxCapsuleGeometry& shape = static_cast<const PxCapsuleGeometry& >(geometry);
+			const PxVec3 d = pose.q.getBasisVector0();
+			PxVec3 extents;
+			for(PxU32 ax = 0; ax<3; ax++)
+				extents[ax] = (PxAbs(d[ax]) * shape.halfHeight + shape.radius + contactOffset)*inflation;
+			bounds.minimum = pose.p - extents;
+			bounds.maximum = pose.p + extents;
+		}
+		break;
+
+		case PxGeometryType::eBOX:
+		{
+			PX_ASSERT(!localSpaceBounds);
+
+			const PxBoxGeometry& shape = static_cast<const PxBoxGeometry& >(geometry);
+
+			const Vec3p origin(pose.p);
+
+			const PxMat33Padded basis(pose.q);
+
+			const Vec4V extentsV = basisExtentV(basis, shape.halfExtents, contactOffset, inflation);
+
+			const Vec4V originV = V4LoadU(&origin.x);
+			const Vec4V minV = V4Sub(originV, extentsV);
+			const Vec4V maxV = V4Add(originV, extentsV);
+
+			StoreBounds(bounds, minV, maxV);
+		}
+		break;
+
+		case PxGeometryType::eCONVEXMESH:
+		{
+			const PxConvexMeshGeometry& shape = static_cast<const PxConvexMeshGeometry& >(geometry);
+			const Gu::ConvexHullData& hullData = static_cast<const Gu::ConvexMesh*>(shape.convexMesh)->getHull();
+
+			const bool useTightBounds = shape.meshFlags & PxConvexMeshGeometryFlag::eTIGHT_BOUNDS;
+			if(useTightBounds)
+			{
+				PxMat33Padded rot(pose.q);
+
+				if(isNonIdentity(shape.scale.scale))
+					computeScaledMatrix(rot, shape.scale);
+
+				PxU32 nb = hullData.mNbHullVertices;
+				const PxVec3* v = hullData.getHullVertices();
+				Vec4V minV;
+				Vec4V maxV;
+
+				{
+					const Vec4V vertexV = multiply3x3V(V4LoadU(&v->x), rot);
+					v++;
+
+					minV = vertexV;
+					maxV = vertexV;
+					nb--;
+				}
+
+				while(nb--)
+				{
+					const Vec4V vertexV = multiply3x3V(V4LoadU(&v->x), rot);
+					v++;
+
+					minV = V4Min(minV, vertexV);
+					maxV = V4Max(maxV, vertexV);
+				}
+
+				const Vec4V offsetV = V4Load(contactOffset);
+				minV = V4Sub(minV, offsetV);
+				maxV = V4Add(maxV, offsetV);
+
+				const Vec4V posV = Vec4V_From_Vec3V(V3LoadU(&pose.p.x));
+				maxV = V4Add(maxV, posV);
+				minV = V4Add(minV, posV);
+
+				// Inflation
+				{
+					const Vec4V centerV = V4Scale(V4Add(maxV, minV), FLoad(0.5f));
+					const Vec4V extentsV = V4Scale(V4Sub(maxV, minV), FLoad(0.5f*inflation));
+					maxV = V4Add(centerV, extentsV);
+					minV = V4Sub(centerV, extentsV);
+				}
+
+				StoreBounds(bounds, minV, maxV);
+			}
+			else
+			{
+				Vec3p origin, extents;
+				computeMeshBounds(pose, localSpaceBounds ? localSpaceBounds : &hullData.getPaddedBounds(), shape.scale, origin, extents);
+
+				inflateBounds(bounds, origin, extents, contactOffset, inflation);
+			}
+		}
+		break;
+
+		case PxGeometryType::eTRIANGLEMESH:
+		{
+			Vec3p origin, extents;
+			const PxTriangleMeshGeometry& shape = static_cast<const PxTriangleMeshGeometry& >(geometry);
+			computeMeshBounds(pose, localSpaceBounds ? localSpaceBounds : &static_cast<const Gu::TriangleMesh*>(shape.triangleMesh)->getPaddedBounds(), shape.scale, origin, extents);
+
+			inflateBounds(bounds, origin, extents, contactOffset, inflation);
+		}
+		break;
+
+		case PxGeometryType::eHEIGHTFIELD:
+		{
+			const PxHeightFieldGeometry& shape = static_cast<const PxHeightFieldGeometry& >(geometry);
+			const PxMeshScale scale(PxVec3(shape.rowScale, shape.heightScale, shape.columnScale), PxQuat(PxIdentity));
+			
+			if(!localSpaceBounds)
+				localSpaceBounds = &static_cast<const Gu::HeightField*>(shape.heightField)->getData().getPaddedBounds();
+
+			//Compute and inflate the bounds from the pose, scale and center/extents.
+			Vec3p origin, extents;
+			computeMeshBounds(pose, localSpaceBounds, scale, origin, extents);
+			inflateBounds(bounds, origin, extents, contactOffset, inflation);
+		}
+		break;
+		case PxGeometryType::eGEOMETRY_COUNT:
+		case PxGeometryType::eINVALID:
+		{
+			PX_ASSERT(0);		
+			Ps::getFoundation().error(PxErrorCode::eINTERNAL_ERROR, __FILE__, __LINE__, "Gu::GeometryUnion::computeBounds: Unknown shape type.");
+		}
+	}
+}
+
+// PT: TODO: refactor this with regular function
+PxF32 Gu::computeBoundsWithCCDThreshold(Vec3p& origin, Vec3p& extent, const PxGeometry& geometry, const PxTransform& pose, const CenterExtentsPadded* PX_RESTRICT localSpaceBounds)
+{
+	// Box, Convex, Mesh and HeightField will compute local bounds and pose to world space.
+	// Sphere, Capsule & Plane will compute world space bounds directly.
+
+	const PxReal inSphereRatio = 0.75f;
+
+	//The CCD thresholds are as follows:
+	//(1) sphere = inSphereRatio * radius
+	//(2) plane = inf (we never need CCD against this shape)
+	//(3) capsule = inSphereRatio * radius
+	//(4) box = inSphereRatio * (box minimum extent axis)
+	//(5) convex = inSphereRatio * convex in-sphere * min scale
+	//(6) triangle mesh = 0.f (polygons have 0 thickness)
+	//(7) heightfields = 0.f (polygons have 0 thickness)
+
+	//The decision to enter CCD depends on the sum of the shapes' CCD thresholds. One of the 2 shapes must be a 
+	//sphere/capsule/box/convex so the sum of the CCD thresholds will be non-zero.
+
+	PxBounds3 bounds;
+
+	computeBounds(bounds, geometry, pose, 0.f, localSpaceBounds, 1.f);
+
+	origin = bounds.getCenter();
+	extent = bounds.getExtents();
+
+	switch (geometry.getType())
+	{
+		case PxGeometryType::eSPHERE:
+		{
+			const PxSphereGeometry& shape = static_cast<const PxSphereGeometry&>(geometry);
+			return shape.radius*inSphereRatio;
+		}
+		case PxGeometryType::ePLANE:
+		{
+			return PX_MAX_REAL;
+		}
+		case PxGeometryType::eCAPSULE:
+		{
+			const PxCapsuleGeometry& shape = static_cast<const PxCapsuleGeometry&>(geometry);
+			return shape.radius * inSphereRatio;
+		}
+
+		case PxGeometryType::eBOX:
+		{
+			const PxBoxGeometry& shape = static_cast<const PxBoxGeometry&>(geometry);
+			return PxMin(PxMin(shape.halfExtents.x, shape.halfExtents.y), shape.halfExtents.z)*inSphereRatio;
+		}
+
+		case PxGeometryType::eCONVEXMESH:
+		{
+			const PxConvexMeshGeometry& shape = static_cast<const PxConvexMeshGeometry&>(geometry);
+			const Gu::ConvexHullData& hullData = static_cast<const Gu::ConvexMesh*>(shape.convexMesh)->getHull();
+			return PxMin(shape.scale.scale.z, PxMin(shape.scale.scale.x, shape.scale.scale.y)) * hullData.mInternal.mRadius * inSphereRatio;
+		}
+
+		case PxGeometryType::eTRIANGLEMESH:
+		{
+			return 0.0f;
+		}
+
+		case PxGeometryType::eHEIGHTFIELD:
+		{
+			return 0.f;
+		}
+
+		case PxGeometryType::eGEOMETRY_COUNT:
+		case PxGeometryType::eINVALID:
+		{
+			PX_ASSERT(0);		
+			Ps::getFoundation().error(PxErrorCode::eINTERNAL_ERROR, __FILE__, __LINE__, "Gu::GeometryUnion::computeBounds: Unknown shape type.");
+		}
+	}
+	return PX_MAX_REAL;
+}
+
+
+static PX_FORCE_INLINE void computeBoxExtentsAroundCapsule(PxVec3& extents, const PxCapsuleGeometry& capsuleGeom, float inflation)
+{
+	extents.x = (capsuleGeom.radius + capsuleGeom.halfHeight) * inflation;
+	extents.y = capsuleGeom.radius * inflation;
+	extents.z = capsuleGeom.radius * inflation;
+}
+
+static const PxReal SQ_PRUNER_INFLATION = 1.01f; // pruner test shape inflation (not narrow phase shape)
+
+static void computeMeshBounds(const PxVec3& pos, const PxMat33Padded& rot, const CenterExtentsPadded* PX_RESTRICT localSpaceBounds, const PxMeshScale& meshScale, Vec3p& origin, Vec3p& extent)
+{
+	Ps::prefetchLine(localSpaceBounds);	// PT: this one helps reducing L2 misses in transformNoEmptyTest
+	transformNoEmptyTest(origin, extent, pos, rot, meshScale, *localSpaceBounds);
+}
+
+// PT: warning: this writes 4 bytes after the end of 'bounds'. Calling code must ensure it is safe to do so.
+static PX_FORCE_INLINE void computeMinMaxBounds(PxBounds3* PX_RESTRICT bounds, const Vec3p& c, const Vec3p& e, float prunerInflation, float offset)
+{
+	const Vec4V extentsV = V4Scale(V4Add(V4LoadU(&e.x), V4Load(offset)), FLoad(prunerInflation));
+	const Vec4V centerV = V4LoadU(&c.x);
+	const Vec4V minV = V4Sub(centerV, extentsV);
+	const Vec4V maxV = V4Add(centerV, extentsV);
+	V4StoreU(minV, &bounds->minimum.x);
+	V4StoreU(maxV, &bounds->maximum.x);
+}
+
+ShapeData::ShapeData(const PxGeometry& g, const PxTransform& t, PxReal inflation)
+{
+	using namespace physx::shdfnd::aos;
+
+	// PT: this cast to matrix is already done in GeometryUnion::computeBounds (e.g. for boxes). So we do it first,
+	// then we'll pass the matrix directly to computeBoundsShapeData, to avoid the double conversion.
+	const bool isOBB = PxAbs(t.q.w) < 0.999999f;
+	if(isOBB)
+	{
+		// PT: writes 4 bytes after 'rot' but it's safe since we then write 'center' just afterwards
+		buildFrom(mGuBox, t.q);
+	}
+	else
+	{
+		mGuBox.rot = PxMat33(PxIdentity);
+	}
+
+	// PT: can't use V4Load here since there's no guarantee on 't.p'
+	// PT: must store 'center'  after 'rot' now
+	mGuBox.center = t.p;
+
+	// Compute AABB, used by the BucketPruner as cullBox
+	switch(g.getType())
+	{
+		case PxGeometryType::eSPHERE:
+		{
+			const PxSphereGeometry& shape = static_cast<const PxSphereGeometry&>(g);
+			computeMinMaxBounds(&mPrunerInflatedAABB, mGuBox.center, PxVec3(0.0f), SQ_PRUNER_INFLATION, shape.radius+inflation);
+
+			//
+
+			reinterpret_cast<Sphere&>(mGuSphere) = Sphere(t.p, shape.radius);
+		}
+		break;
+
+		case PxGeometryType::eCAPSULE:
+		{
+			const PxCapsuleGeometry& shape = static_cast<const PxCapsuleGeometry&>(g);
+			const Vec3p extents = mGuBox.rot.column0.abs() * shape.halfHeight;
+			computeMinMaxBounds(&mPrunerInflatedAABB, mGuBox.center, extents, SQ_PRUNER_INFLATION, shape.radius+inflation);
+
+			//
+
+			Capsule& dstWorldCapsule = reinterpret_cast<Capsule&>(mGuCapsule); // store a narrow phase version copy
+			getCapsule(dstWorldCapsule, shape, t);
+
+			mGuBox.extents.x = shape.halfHeight;
+
+			// compute PxBoxGeometry pruner geom around input capsule geom; transform remains unchanged
+
+			computeBoxExtentsAroundCapsule(mPrunerBoxGeomExtents, shape, SQ_PRUNER_INFLATION);
+		}
+		break;
+
+		case PxGeometryType::eBOX:
+		{
+			const PxBoxGeometry& shape = static_cast<const PxBoxGeometry&>(g);
+			// PT: cast is safe because 'rot' followed by other members
+			Vec4V extentsV = basisExtentV(static_cast<const PxMat33Padded&>(mGuBox.rot), shape.halfExtents, inflation, SQ_PRUNER_INFLATION);
+
+			// PT: c/e-to-m/M conversion
+			const Vec4V centerV = V4LoadU(&mGuBox.center.x);
+			const Vec4V minV = V4Sub(centerV, extentsV);
+			const Vec4V maxV = V4Add(centerV, extentsV);
+			V4StoreU(minV, &mPrunerInflatedAABB.minimum.x);
+			V4StoreU(maxV, &mPrunerInflatedAABB.maximum.x);	// PT: WARNING: writes past end of class
+
+			//
+
+			mGuBox.extents	= shape.halfExtents;	// PT: TODO: use SIMD
+			mPrunerBoxGeomExtents = shape.halfExtents*SQ_PRUNER_INFLATION;
+		}
+		break;
+
+		case PxGeometryType::eCONVEXMESH:
+		{
+			const PxConvexMeshGeometry& shape = static_cast<const PxConvexMeshGeometry&>(g);
+
+			const ConvexMesh* cm = static_cast<const ConvexMesh*>(shape.convexMesh);
+			const ConvexHullData* hullData = &cm->getHull();
+
+			// PT: cast is safe since 'rot' is followed by other members of the box
+			Vec3p center, extents;
+			computeMeshBounds(mGuBox.center, static_cast<const PxMat33Padded&>(mGuBox.rot), &hullData->getPaddedBounds(), shape.scale, center, extents);
+
+			computeMinMaxBounds(&mPrunerInflatedAABB, center, extents, SQ_PRUNER_INFLATION, inflation);
+
+			//
+
+			Box prunerBox;
+			computeOBBAroundConvex(prunerBox, shape, cm, t);
+			mGuBox.rot = prunerBox.rot;	// PT: TODO: optimize this copy
+
+			// AP: pruners are now responsible for growing the OBB by 1% for overlap/sweep/GJK accuracy
+			mPrunerBoxGeomExtents = prunerBox.extents*SQ_PRUNER_INFLATION;
+			mGuBox.center = prunerBox.center;
+		}
+		break;
+
+		case PxGeometryType::ePLANE:
+		case PxGeometryType::eTRIANGLEMESH:
+		case PxGeometryType::eHEIGHTFIELD:
+		case PxGeometryType::eGEOMETRY_COUNT:
+		case PxGeometryType::eINVALID:
+			PX_ALWAYS_ASSERT_MESSAGE("PhysX internal error: Invalid shape in ShapeData contructor.");
+	}
+
+	// PT: WARNING: these writes must stay after the above code
+	mIsOBB = PxU32(isOBB);
+	mType = PxU16(g.getType());
+}
+
+
+
+

physx/source/geomutils/src/GuBounds.h

@@ -0,0 +1,157 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_BOUNDS_H
+#define GU_BOUNDS_H
+
+#include "foundation/PxBounds3.h"
+#include "foundation/PxFlags.h"
+#include "geometry/PxGeometry.h"
+#include "GuSIMDHelpers.h"
+#include <stddef.h>
+#include "GuBox.h"
+#include "GuCenterExtents.h"
+#include "GuSphere.h"
+#include "GuCapsule.h"
+
+namespace physx
+{
+class PxGeometry;
+
+namespace Gu
+{
+
+//For spheres, planes, capsules and boxes just set localSpaceBounds to NULL.
+//For convex meshes, triangle meshes, and heightfields set localSpaceBounds to the relevant pointer if it has already been pre-fetched.  
+//For convex meshes, triangle meshes, and heightfields set localSpaceBounds to NULL if it has not already been pre-fetched.   computeBounds will synchronously 
+//prefetch the local space bounds if localSpaceBounds is NULL.
+//'contactOffset' and 'inflation' should not be used at the same time, i.e. either contactOffset==0.0f, or inflation==1.0f
+PX_PHYSX_COMMON_API void computeBounds(PxBounds3& bounds, const PxGeometry& geometry, const PxTransform& transform, float contactOffset, const CenterExtentsPadded* PX_RESTRICT localSpaceBounds, float inflation);	//AABB in world space.
+
+//For spheres, planes, capsules and boxes just set localSpaceBounds to NULL.
+//For convex meshes, triangle meshes, and heightfields set localSpaceBounds to the relevant pointer if it has not already been pre-fetched.  
+//For convex meshes, triangle meshes, and heightfields set localSpaceBounds to NULL if it has not already been pre-fetched.   computeBounds will synchronously 
+//prefetch the local space bounds if localSpaceBounds is NULL.
+PX_PHYSX_COMMON_API PxF32 computeBoundsWithCCDThreshold(Vec3p& origin, Vec3p& extent, const PxGeometry& geometry, const PxTransform& transform, const CenterExtentsPadded* PX_RESTRICT localSpaceBounds);	//AABB in world space.
+
+
+PX_FORCE_INLINE PxBounds3 computeBounds(const PxGeometry& geometry, const PxTransform& pose)
+{
+	PxBounds3 bounds;
+	computeBounds(bounds, geometry, pose, 0.0f, NULL, 1.0f);
+	return bounds;
+}
+
+class ShapeData
+{
+public:
+
+	PX_PHYSX_COMMON_API						ShapeData(const PxGeometry& g, const PxTransform& t, PxReal inflation);	
+
+	// PT: used by overlaps (box, capsule, convex)
+	PX_FORCE_INLINE const PxVec3&			getPrunerBoxGeomExtentsInflated()	const	{ return mPrunerBoxGeomExtents; }
+
+	// PT: used by overlaps (box, capsule, convex)
+	PX_FORCE_INLINE const PxVec3&			getPrunerWorldPos()					const	{ return mGuBox.center;			}
+
+	PX_FORCE_INLINE const PxBounds3&		getPrunerInflatedWorldAABB()		const	{ return mPrunerInflatedAABB;	}
+
+	// PT: used by overlaps (box, capsule, convex)
+	PX_FORCE_INLINE const PxMat33&			getPrunerWorldRot33()				const	{ return mGuBox.rot;			}
+
+	// PT: this one only used by overlaps so far (for sphere shape, pruner level)
+	PX_FORCE_INLINE const Gu::Sphere&		getGuSphere() const
+	{
+		PX_ASSERT(mType == PxGeometryType::eSPHERE);
+		return reinterpret_cast<const Gu::Sphere&>(mGuSphere);
+	}
+
+	// PT: this one only used by sweeps so far (for box shape, NP level)
+	PX_FORCE_INLINE const Gu::Box&			getGuBox() const
+	{
+		PX_ASSERT(mType == PxGeometryType::eBOX);
+		return mGuBox;
+	}
+
+	// PT: this one used by sweeps (NP level) and overlaps (pruner level) - for capsule shape
+	PX_FORCE_INLINE const Gu::Capsule&		getGuCapsule() const
+	{
+		PX_ASSERT(mType == PxGeometryType::eCAPSULE);
+		return reinterpret_cast<const Gu::Capsule&>(mGuCapsule);
+	}
+
+	PX_FORCE_INLINE float					getCapsuleHalfHeight() const
+	{
+		PX_ASSERT(mType == PxGeometryType::eCAPSULE);
+		return mGuBox.extents.x;
+	}
+
+	PX_FORCE_INLINE	PxU32					isOBB()		const { return PxU32(mIsOBB);				}
+	PX_FORCE_INLINE	PxGeometryType::Enum	getType()	const { return PxGeometryType::Enum(mType);	}
+
+	PX_NOCOPY(ShapeData)
+private:
+
+	// PT: box: pre-inflated box extents
+	//     capsule: pre-inflated extents of box-around-capsule
+	//     convex: pre-inflated extents of box-around-convex
+	//     sphere: not used
+	PxVec3				mPrunerBoxGeomExtents;	// used for pruners. This volume encloses but can differ from the original shape
+
+	// PT:
+	//
+	// box center = unchanged copy of initial shape's position, except for convex (position of box around convex)
+	// SIMD code will load it as a V4 (safe because member is not last of Gu structure)
+	//
+	// box rot = precomputed PxMat33 version of initial shape's rotation, except for convex (rotation of box around convex)
+	// SIMD code will load it as V4s (safe because member is not last of Gu structure)
+	//
+	// box extents = non-inflated initial box extents for box shape, half-height for capsule, otherwise not used
+	Gu::Box				mGuBox;
+
+	PxBounds3			mPrunerInflatedAABB;	// precomputed AABB for the pruner shape
+	PxU16				mIsOBB;					// true for OBB, false for AABB. Also used as padding for mPrunerInflatedAABB, don't move.
+	PxU16				mType;					// shape's type
+
+	// these union Gu shapes are only precomputed for narrow phase (not pruners), can be different from mPrunerVolume
+	// so need separate storage
+	union
+	{
+		PxU8 mGuCapsule[sizeof(Gu::Capsule)];	// 28
+		PxU8 mGuSphere[sizeof(Gu::Sphere)];		// 16
+	};
+};
+
+// PT: please make sure it fits in "one" cache line
+PX_COMPILE_TIME_ASSERT(sizeof(ShapeData)==128);
+
+}  // namespace Gu
+
+}
+#endif

physx/source/geomutils/src/GuBox.cpp

@@ -0,0 +1,132 @@
+//
+// 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-2021 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 "PsIntrinsics.h"
+#include "GuBoxConversion.h"
+#include "GuCapsule.h"
+#include "GuInternal.h"
+#include "CmMatrix34.h"
+#include "PsMathUtils.h"
+
+using namespace physx;
+
+void Gu::Box::create(const Gu::Capsule& capsule)
+{
+	// Box center = center of the two LSS's endpoints
+	center = capsule.computeCenter();
+
+	// Box orientation
+	const PxVec3 dir = capsule.p1 - capsule.p0;
+	const float d = dir.magnitude();
+	if(d!=0.0f)
+	{
+		rot.column0 = dir / d;
+		Ps::computeBasis(rot.column0, rot.column1, rot.column2);
+	}
+	else
+		rot = PxMat33(PxIdentity);
+
+	// Box extents
+	extents.x = capsule.radius + (d * 0.5f);
+	extents.y = capsule.radius;
+	extents.z = capsule.radius;
+}
+
+
+/**
+Returns edges.
+\return		24 indices (12 edges) indexing the list returned by ComputePoints()
+*/
+const PxU8* Gu::getBoxEdges()
+{
+	//     7+------+6			0 = ---
+	//     /|     /|			1 = +--
+	//    / |    / |			2 = ++-
+	//   / 4+---/--+5			3 = -+-
+	// 3+------+2 /    y   z	4 = --+
+	//  | /    | /     |  /		5 = +-+
+	//  |/     |/      |/		6 = +++
+	// 0+------+1      *---x	7 = -++
+
+	static PxU8 Indices[] = {
+		0, 1,	1, 2,	2, 3,	3, 0,
+		7, 6,	6, 5,	5, 4,	4, 7,
+		1, 5,	6, 2,
+		3, 7,	4, 0
+	};
+	return Indices;
+}
+
+
+void Gu::computeOBBPoints(PxVec3* PX_RESTRICT pts, const PxVec3& center, const PxVec3& extents, const PxVec3& base0, const PxVec3& base1, const PxVec3& base2)
+{
+	PX_ASSERT(pts);
+
+	// "Rotated extents"
+	const PxVec3 axis0 = base0 * extents.x;
+	const PxVec3 axis1 = base1 * extents.y;
+	const PxVec3 axis2 = base2 * extents.z;
+
+	//     7+------+6			0 = ---
+	//     /|     /|			1 = +--
+	//    / |    / |			2 = ++-
+	//   / 4+---/--+5			3 = -+-
+	// 3+------+2 /    y   z	4 = --+
+	//  | /    | /     |  /		5 = +-+
+	//  |/     |/      |/		6 = +++
+	// 0+------+1      *---x	7 = -++
+
+	// Original code: 24 vector ops
+	/*	pts[0] = box.center - Axis0 - Axis1 - Axis2;
+	pts[1] = box.center + Axis0 - Axis1 - Axis2;
+	pts[2] = box.center + Axis0 + Axis1 - Axis2;
+	pts[3] = box.center - Axis0 + Axis1 - Axis2;
+	pts[4] = box.center - Axis0 - Axis1 + Axis2;
+	pts[5] = box.center + Axis0 - Axis1 + Axis2;
+	pts[6] = box.center + Axis0 + Axis1 + Axis2;
+	pts[7] = box.center - Axis0 + Axis1 + Axis2;*/
+
+	// Rewritten: 12 vector ops
+	pts[0] = pts[3] = pts[4] = pts[7] = center - axis0;
+	pts[1] = pts[2] = pts[5] = pts[6] = center + axis0;
+
+	PxVec3 tmp = axis1 + axis2;
+	pts[0] -= tmp;
+	pts[1] -= tmp;
+	pts[6] += tmp;
+	pts[7] += tmp;
+
+	tmp = axis1 - axis2;
+	pts[2] += tmp;
+	pts[3] += tmp;
+	pts[4] -= tmp;
+	pts[5] -= tmp;
+}
+

physx/source/geomutils/src/GuCCTSweepTests.cpp

@@ -0,0 +1,440 @@
+//
+// 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-2021 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/PxSphereGeometry.h"
+#include "GuSweepTests.h"
+#include "GuHeightFieldUtil.h"
+#include "GuEntityReport.h"
+#include "GuDistanceSegmentBox.h"
+#include "GuDistancePointBox.h"
+#include "GuSweepBoxSphere.h"
+#include "GuSweepCapsuleBox.h"
+#include "GuSweepBoxBox.h"
+#include "GuSweepBoxTriangle_SAT.h"
+#include "GuSweepTriangleUtils.h"
+#include "GuInternal.h"
+#include "PsVecMath.h"
+
+using namespace physx;
+using namespace Gu;
+using namespace Cm;
+using namespace Ps::aos;
+
+static const bool gValidateBoxRadiusComputation = false;
+
+///////////////////////////////////////////
+
+bool sweepCapsule_BoxGeom_Precise(GU_CAPSULE_SWEEP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom.getType() == PxGeometryType::eBOX);
+	PX_UNUSED(inflation);
+	PX_UNUSED(capsulePose_);
+	PX_UNUSED(capsuleGeom_);
+
+	const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom);
+
+	if (lss.p0 == lss.p1)  // The capsule is actually a sphere
+	{
+		//TODO: Check if this is really faster than using a "sphere-aware" version of sweepCapsuleBox
+
+		Box box;	buildFrom(box, pose.p, boxGeom.halfExtents, pose.q);
+		if(!sweepBoxSphere(box, lss.radius, lss.p0, unitDir, distance, sweepHit.distance, sweepHit.normal, hitFlags))
+			return false;
+
+		sweepHit.normal = -sweepHit.normal;
+		sweepHit.flags = PxHitFlag::eNORMAL;
+		
+		if(hitFlags & PxHitFlag::ePOSITION && sweepHit.distance!=0.0f)
+		{
+			// The sweep test doesn't compute the impact point automatically, so we have to do it here.
+			const PxVec3 newSphereCenter = lss.p0 + unitDir * sweepHit.distance;
+			PxVec3 closest;
+			const PxReal d = distancePointBoxSquared(newSphereCenter, box.center, box.extents, box.rot, &closest);
+			PX_UNUSED(d);
+			// Compute point on the box, after sweep
+			closest = box.rotate(closest);
+			sweepHit.position = closest + box.center;
+			sweepHit.flags |= PxHitFlag::ePOSITION;
+		}
+	}
+	else
+	{
+		if(!sweepCapsuleBox(lss, pose, boxGeom.halfExtents, unitDir, distance, sweepHit.position, sweepHit.distance, sweepHit.normal, hitFlags))
+			return false;
+
+		sweepHit.flags = PxHitFlag::eNORMAL;
+		
+		if((hitFlags & PxHitFlag::ePOSITION) && sweepHit.distance!=0.0f)
+		{
+			// The sweep test doesn't compute the impact point automatically, so we have to do it here.
+			Capsule movedCaps = lss;
+			movedCaps.p0 += unitDir * sweepHit.distance;
+			movedCaps.p1 += unitDir * sweepHit.distance;
+
+			Box box;
+			buildFrom(box, pose.p, boxGeom.halfExtents, pose.q);
+
+			PxVec3 closest;
+			const PxReal d = distanceSegmentBoxSquared(movedCaps, box, NULL, &closest);
+			PX_UNUSED(d);
+			// Compute point on the box, after sweep
+			closest = pose.q.rotate(closest);
+			sweepHit.position = closest + pose.p;
+			sweepHit.flags |= PxHitFlag::ePOSITION;
+		}
+	}
+	return true;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+bool sweepBox_SphereGeom_Precise(GU_BOX_SWEEP_FUNC_PARAMS)
+{
+	PX_UNUSED(boxPose_);
+	PX_UNUSED(boxGeom_);
+
+	PX_ASSERT(geom.getType() == PxGeometryType::eSPHERE);
+	const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom);
+
+	// PT: move to relative space
+	const Box relBox(box.center - pose.p, box.extents, box.rot);
+
+	const PxReal sphereRadius = sphereGeom.radius + inflation;
+
+	if(!sweepBoxSphere(relBox, sphereRadius, PxVec3(0), -unitDir, distance, sweepHit.distance, sweepHit.normal, hitFlags))
+		return false;
+
+	sweepHit.flags = PxHitFlag::eNORMAL;
+
+	if((hitFlags & PxHitFlag::ePOSITION) && sweepHit.distance!=0.0f)
+	{
+		// The sweep test doesn't compute the impact point automatically, so we have to do it here.
+		const PxVec3 motion = sweepHit.distance * unitDir;
+		const PxVec3 newSphereCenter = - motion;
+		PxVec3 closest;
+		const PxReal d = distancePointBoxSquared(newSphereCenter, relBox.center, relBox.extents, relBox.rot, &closest);
+		PX_UNUSED(d);
+		// Compute point on the box, after sweep
+		sweepHit.position = relBox.rotate(closest) + box.center + motion;	// PT: undo move to local space here
+		sweepHit.flags |= PxHitFlag::ePOSITION;
+	}
+	return true;
+}
+
+bool sweepBox_CapsuleGeom_Precise(GU_BOX_SWEEP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom.getType() == PxGeometryType::eCAPSULE);
+	PX_UNUSED(inflation);
+	PX_UNUSED(boxGeom_);
+
+	const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom);
+
+	// PT: move to relative space
+	const PxVec3 delta = box.center - pose.p;
+	Box relBox(delta, box.extents, box.rot);
+
+	Capsule capsule;
+	const PxVec3 halfHeightVector = getCapsuleHalfHeightVector(pose, capsuleGeom);
+	capsule.p0 = halfHeightVector;
+	capsule.p1 = -halfHeightVector;
+	capsule.radius = capsuleGeom.radius;
+
+	// PT: TODO: remove this. We convert to PxTansform here but inside sweepCapsuleBox we convert back to a matrix.
+	const PxTransform boxWorldPose(delta, boxPose_.q);
+
+	PxVec3 n;
+	if(!sweepCapsuleBox(capsule, boxWorldPose, relBox.extents, -unitDir, distance, sweepHit.position, sweepHit.distance, n, hitFlags))
+		return false;
+
+	sweepHit.normal = -n;
+	sweepHit.flags = PxHitFlag::eNORMAL;
+
+	if((hitFlags & PxHitFlag::ePOSITION) && sweepHit.distance!=0.0f)
+	{
+		// The sweep test doesn't compute the impact point automatically, so we have to do it here.
+		relBox.center += (unitDir * sweepHit.distance);
+		PxVec3 closest;
+		const PxReal d = distanceSegmentBoxSquared(capsule, relBox, NULL, &closest);
+		PX_UNUSED(d);
+		// Compute point on the box, after sweep
+		sweepHit.position = relBox.transform(closest) + pose.p;	// PT: undo move to local space here
+		sweepHit.flags |= PxHitFlag::ePOSITION;
+	}
+	return true;
+}
+
+bool sweepBox_BoxGeom_Precise(GU_BOX_SWEEP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom.getType() == PxGeometryType::eBOX);
+	PX_UNUSED(inflation);
+	PX_UNUSED(boxPose_);
+	PX_UNUSED(boxGeom_);
+
+	const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom);
+
+	// PT: move to local space
+	const Box relBox(box.center - pose.p, box.extents, box.rot);
+	Box staticBox;	buildFrom(staticBox, PxVec3(0), boxGeom.halfExtents, pose.q);
+
+	if(!sweepBoxBox(relBox, staticBox, unitDir, distance, hitFlags, sweepHit))
+		return false;
+
+	if(sweepHit.distance!=0.0f)
+		sweepHit.position += pose.p;	// PT: undo move to local space
+	return true;
+}
+
+// PT: test: new version for CCT, based on code for general sweeps. Just to check it works or not with rotations
+// TODO: refactor this and the similar code in sweptBox for box-vs-mesh. Not so easy though.
+static bool sweepBoxVsTriangles(PxU32 nbTris, const PxTriangle* triangles, const Box& box, const PxVec3& unitDir, const PxReal distance, PxSweepHit& sweepHit,
+								PxHitFlags hitFlags, bool isDoubleSided, const PxU32* cachedIndex)
+{
+	if(!nbTris)
+		return false;
+
+	const bool meshBothSides = hitFlags & PxHitFlag::eMESH_BOTH_SIDES;
+	const bool doBackfaceCulling = !isDoubleSided && !meshBothSides;
+
+	// Move to AABB space
+	Matrix34 worldToBox;
+	computeWorldToBoxMatrix(worldToBox, box);
+
+	const PxVec3 localDir = worldToBox.rotate(unitDir);
+	const PxVec3 localMotion = localDir * distance;
+
+	bool status = false;
+	sweepHit.distance = distance;	//was PX_MAX_F32, but that may trigger an assert in the caller!
+
+	const PxVec3 oneOverMotion(
+		localDir.x!=0.0f ? 1.0f/localMotion.x : 0.0f,
+		localDir.y!=0.0f ? 1.0f/localMotion.y : 0.0f,
+		localDir.z!=0.0f ? 1.0f/localMotion.z : 0.0f);
+
+// PT: experimental code, don't clean up before I test it more and validate it
+
+// Project box
+/*float boxRadius0 =
+			PxAbs(dir.x) * box.extents.x
+		+	PxAbs(dir.y) * box.extents.y
+		+	PxAbs(dir.z) * box.extents.z;*/
+
+float boxRadius =
+			PxAbs(localDir.x) * box.extents.x
+		+	PxAbs(localDir.y) * box.extents.y
+		+	PxAbs(localDir.z) * box.extents.z;
+
+if(gValidateBoxRadiusComputation)	// PT: run this to check the box radius is correctly computed
+{
+	PxVec3 boxVertices2[8];
+	box.computeBoxPoints(boxVertices2);
+	float dpmin = FLT_MAX;
+	float dpmax = -FLT_MAX;
+	for(int i=0;i<8;i++)
+	{
+		const float dp = boxVertices2[i].dot(unitDir);
+		if(dp<dpmin)	dpmin = dp;
+		if(dp>dpmax)	dpmax = dp;
+	}
+	const float goodRadius = (dpmax-dpmin)/2.0f;
+	PX_UNUSED(goodRadius);
+}
+
+const float dpc0 = box.center.dot(unitDir);
+float localMinDist = 1.0f;
+#if PX_DEBUG
+	PxU32 totalTestsExpected = nbTris;
+	PxU32 totalTestsReal = 0;
+	PX_UNUSED(totalTestsExpected);
+	PX_UNUSED(totalTestsReal);
+#endif
+
+	const PxU32 idx = cachedIndex ? *cachedIndex : 0;
+
+	PxVec3 bestTriNormal(0.0f);
+
+	for(PxU32 ii=0;ii<nbTris;ii++)
+	{
+		const PxU32 triangleIndex = getTriangleIndex(ii, idx);
+
+		const PxTriangle& tri = triangles[triangleIndex];
+
+		if(!cullTriangle(tri.verts, unitDir, boxRadius, localMinDist*distance, dpc0))
+			continue;
+
+#if PX_DEBUG
+		totalTestsReal++;
+#endif
+		// Move to box space
+		const PxTriangle currentTriangle(
+			worldToBox.transform(tri.verts[0]),
+			worldToBox.transform(tri.verts[1]),
+			worldToBox.transform(tri.verts[2]));
+
+		PxF32 t = PX_MAX_F32;		// could be better!
+		if(triBoxSweepTestBoxSpace(currentTriangle, box.extents, localMotion, oneOverMotion, localMinDist, t, doBackfaceCulling))
+		{
+			if(t < localMinDist)
+			{
+				// PT: test if shapes initially overlap
+				if(t==0.0f)
+					return setInitialOverlapResults(sweepHit, unitDir, triangleIndex);
+
+				localMinDist		= t;
+				sweepHit.distance	= t * distance;
+				sweepHit.faceIndex	= triangleIndex;
+				status				= true;
+
+				// PT: TODO: optimize this.... already computed in triBoxSweepTestBoxSpace...
+				currentTriangle.denormalizedNormal(bestTriNormal);
+
+				if(hitFlags & PxHitFlag::eMESH_ANY)
+					break;
+			}
+		}
+	}
+
+	if(status)
+	{
+		sweepHit.flags = PxHitFlags(0);
+
+		// PT: TODO: refactor with computeBoxLocalImpact (TA34704)
+		if(hitFlags & (PxHitFlag::eNORMAL|PxHitFlag::ePOSITION))
+		{			
+			const PxTriangle& tri = triangles[sweepHit.faceIndex];
+
+			// Move to box space
+			const PxTriangle currentTriangle(
+				worldToBox.transform(tri.verts[0]),
+				worldToBox.transform(tri.verts[1]),
+				worldToBox.transform(tri.verts[2]));
+
+			computeBoxTriImpactData(sweepHit.position, sweepHit.normal, box.extents, localDir, currentTriangle, sweepHit.distance);
+
+			if(hitFlags & PxHitFlag::eNORMAL)
+			{
+				PxVec3 localNormal = sweepHit.normal;	// PT: both local space & local variable
+				localNormal.normalize();
+
+				if(shouldFlipNormal(localNormal, meshBothSides, isDoubleSided, bestTriNormal, localDir))
+					localNormal = -localNormal;
+
+				sweepHit.normal = box.rotate(localNormal);
+				sweepHit.flags |= PxHitFlag::eNORMAL;
+			}
+
+			if(hitFlags & PxHitFlag::ePOSITION)
+			{
+				sweepHit.position = box.rotate(sweepHit.position) + box.center;
+				sweepHit.flags |= PxHitFlag::ePOSITION;
+			}
+		}
+	}
+	return status;
+}
+
+bool sweepBox_HeightFieldGeom_Precise(GU_BOX_SWEEP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom.getType() == PxGeometryType::eHEIGHTFIELD);
+	PX_UNUSED(inflation);
+	PX_UNUSED(boxPose_);
+	PX_UNUSED(boxGeom_);
+
+	const PxHeightFieldGeometry& heightFieldGeom = static_cast<const PxHeightFieldGeometry&>(geom);
+
+	// Compute swept box
+	Box sweptBox;
+	computeSweptBox(sweptBox, box.extents, box.center, box.rot, unitDir, distance);
+
+	//### Temp hack until we can directly collide the OBB against the HF
+	const PxTransform sweptBoxTR = sweptBox.getTransform();
+	const PxBounds3 bounds = PxBounds3::poseExtent(sweptBoxTR, sweptBox.extents);
+
+	sweepHit.distance = PX_MAX_F32;
+
+	struct LocalReport : EntityReport<PxU32>
+	{
+		virtual bool onEvent(PxU32 nb, PxU32* indices)
+		{
+			for(PxU32 i=0; i<nb; i++)
+			{
+				const PxU32 triangleIndex = indices[i];
+
+				PxTriangle currentTriangle;	// in world space
+				mHFUtil->getTriangle(*mPose, currentTriangle, NULL, NULL, triangleIndex, true, true);
+
+				PxSweepHit sweepHit_;
+				const bool b = sweepBoxVsTriangles(1, &currentTriangle, mBox, mDir, mDist, sweepHit_, mHitFlags, mIsDoubleSided, NULL);
+				if(b && sweepHit_.distance<mHit->distance)
+				{
+					*mHit = sweepHit_;
+					mHit->faceIndex	= triangleIndex;
+					mStatus			= true;
+				}
+			}
+			return true;
+		}
+
+		const HeightFieldUtil*	mHFUtil;
+		const PxTransform*		mPose;
+		PxSweepHit*				mHit;
+		bool					mStatus;
+		Box						mBox;
+		PxVec3					mDir;
+		float					mDist;
+		PxHitFlags				mHitFlags;
+		bool					mIsDoubleSided;
+	} myReport;
+
+	HeightFieldUtil hfUtil(heightFieldGeom);
+
+	myReport.mBox		= box;
+	myReport.mDir		= unitDir;
+	myReport.mDist		= distance;
+	myReport.mHitFlags	= hitFlags;
+	myReport.mHFUtil	= &hfUtil;
+	myReport.mStatus	= false;
+	myReport.mPose		= &pose;
+	myReport.mHit		= &sweepHit;
+	const PxU32 meshBothSides = hitFlags & PxHitFlag::eMESH_BOTH_SIDES;
+	myReport.mIsDoubleSided = (heightFieldGeom.heightFieldFlags & PxMeshGeometryFlag::eDOUBLE_SIDED) || meshBothSides;
+
+	hfUtil.overlapAABBTriangles(pose, bounds, GuHfQueryFlags::eWORLD_SPACE, &myReport);
+
+	return myReport.mStatus;
+}
+
+bool Gu::sweepBoxTriangles_Precise(GU_SWEEP_TRIANGLES_FUNC_PARAMS(PxBoxGeometry))
+{
+	PX_UNUSED(inflation);
+
+	Box box;
+	buildFrom(box, pose.p, geom.halfExtents, pose.q);
+
+	return sweepBoxVsTriangles(nbTris, triangles, box, unitDir, distance, hit, hitFlags, doubleSided, cachedIndex);
+}

physx/source/geomutils/src/GuCapsule.cpp

@@ -0,0 +1,64 @@
+//
+// 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-2021 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 "PsIntrinsics.h"
+#include "PsMathUtils.h"
+#include "GuInternal.h"
+#include "GuBox.h"
+#include "GuCapsule.h"
+
+using namespace physx;
+
+/**
+*	Computes an OBB surrounding the capsule.
+*	\param		box		[out] the OBB
+*/
+void Gu::computeBoxAroundCapsule(const Gu::Capsule& capsule, Gu::Box& box)
+{
+	// Box center = center of the two capsule's endpoints
+	box.center = capsule.computeCenter();
+
+	// Box extents
+	const PxF32 d = (capsule.p0 - capsule.p1).magnitude();
+	box.extents.x = capsule.radius + (d * 0.5f);
+	box.extents.y = capsule.radius;
+	box.extents.z = capsule.radius;
+
+	// Box orientation
+	if(d==0.0f)
+	{
+		box.rot = PxMat33(PxIdentity);
+	}
+	else
+	{
+		PxVec3 dir, right, up;
+		Ps::computeBasis(capsule.p0, capsule.p1, dir, right, up);
+		box.setAxes(dir, right, up);
+	}
+}

physx/source/geomutils/src/GuCapsule.h

@@ -0,0 +1,92 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_CAPSULE_H
+#define GU_CAPSULE_H
+
+/** \addtogroup geomutils
+@{
+*/
+
+#include "GuSegment.h"
+
+namespace physx
+{
+namespace Gu
+{
+
+/**
+\brief Represents a capsule.
+*/
+	class Capsule : public Segment
+	{
+	public:
+		/**
+		\brief Constructor
+		*/
+		PX_INLINE Capsule()
+		{
+		}
+
+		/**
+		\brief Constructor
+
+		\param seg Line segment to create capsule from.
+		\param _radius Radius of the capsule.
+		*/
+		PX_INLINE Capsule(const Segment& seg, PxF32 _radius) : Segment(seg), radius(_radius)
+		{
+		}
+
+		/**
+		\brief Constructor
+
+		\param _p0 First segment point
+		\param _p1 Second segment point
+		\param _radius Radius of the capsule.
+		*/
+		PX_INLINE Capsule(const PxVec3& _p0, const PxVec3& _p1, PxF32 _radius) : Segment(_p0, _p1), radius(_radius)
+		{           
+		}
+
+		/**
+		\brief Destructor
+		*/
+		PX_INLINE ~Capsule()
+		{
+		}
+
+		PxF32	radius;
+	};
+}
+
+}
+
+/** @} */
+#endif

physx/source/geomutils/src/GuCenterExtents.h

@@ -0,0 +1,131 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_CENTER_EXTENTS_H
+#define GU_CENTER_EXTENTS_H
+
+/** \addtogroup geomutils
+@{
+*/
+
+#include "CmMatrix34.h"
+#include "CmUtils.h"
+#include "PsUserAllocated.h"
+
+namespace physx
+{
+namespace Gu
+{
+	class CenterExtents : public physx::shdfnd::UserAllocated
+	{
+		public:
+		PX_FORCE_INLINE				CenterExtents()						{}
+		PX_FORCE_INLINE				CenterExtents(const PxBounds3& b)	{ mCenter = b.getCenter();	mExtents = b.getExtents();	}
+		PX_FORCE_INLINE				~CenterExtents()					{}
+
+		PX_FORCE_INLINE	void		getMin(PxVec3& min)		const		{ min = mCenter - mExtents;					}
+		PX_FORCE_INLINE	void		getMax(PxVec3& max)		const		{ max = mCenter + mExtents;					}
+
+		PX_FORCE_INLINE	float		getMin(PxU32 axis)		const		{ return mCenter[axis] - mExtents[axis];	}
+		PX_FORCE_INLINE	float		getMax(PxU32 axis)		const		{ return mCenter[axis] + mExtents[axis];	}
+
+		PX_FORCE_INLINE	PxVec3		getMin()				const		{ return mCenter - mExtents;				}
+		PX_FORCE_INLINE	PxVec3		getMax()				const		{ return mCenter + mExtents;				}
+
+		PX_FORCE_INLINE	void		setMinMax(const PxVec3& min, const PxVec3& max)
+									{
+										mCenter = (max + min)*0.5f;
+										mExtents = (max - min)*0.5f;
+									}
+
+		PX_FORCE_INLINE	PxU32		isInside(const CenterExtents& box)	const
+									{
+										if(box.getMin(0)>getMin(0))	return 0;
+										if(box.getMin(1)>getMin(1))	return 0;
+										if(box.getMin(2)>getMin(2))	return 0;
+										if(box.getMax(0)<getMax(0))	return 0;
+										if(box.getMax(1)<getMax(1))	return 0;
+										if(box.getMax(2)<getMax(2))	return 0;
+										return 1;
+									}
+
+		PX_FORCE_INLINE	void		setEmpty()
+									{
+										mExtents = PxVec3(-PX_MAX_BOUNDS_EXTENTS);
+									}
+
+		PX_FORCE_INLINE	bool		isEmpty()	const
+									{
+										return Cm::isEmpty(mCenter, mExtents);
+									}
+
+		PX_FORCE_INLINE	bool		isFinite()	const
+									{
+										return mCenter.isFinite() && mExtents.isFinite();
+									}
+
+		PX_FORCE_INLINE	bool		isValid()	const
+									{
+										return Cm::isValid(mCenter, mExtents);
+									}
+
+		PX_FORCE_INLINE	PxBounds3	transformFast(const PxMat33& matrix)	const
+									{
+										PX_ASSERT(isValid());
+										return PxBounds3::basisExtent(matrix * mCenter, matrix, mExtents);
+									}
+
+		PX_INLINE		PxBounds3	transformSafe(const Cm::Matrix34& matrix)	const
+									{
+										if(isEmpty())
+											return PxBounds3::centerExtents(mCenter, mExtents);
+										else
+											return Cm::basisExtent(matrix.transform(mCenter), matrix.m.column0, matrix.m.column1, matrix.m.column2, mExtents);
+									}
+
+						PxVec3		mCenter;
+						PxVec3		mExtents;
+	};
+
+	//! A padded version of CenterExtents, to safely load its data using SIMD
+	class CenterExtentsPadded : public CenterExtents
+	{
+	public:
+		PX_FORCE_INLINE CenterExtentsPadded()	{}
+		PX_FORCE_INLINE ~CenterExtentsPadded()	{}
+		PxU32	padding;
+	};
+	PX_COMPILE_TIME_ASSERT(sizeof(CenterExtentsPadded) == 7*4);
+
+}
+
+}
+
+/** @} */
+#endif

physx/source/geomutils/src/GuGeometryQuery.cpp

@@ -0,0 +1,352 @@
+//
+// 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-2021 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 "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 "PsFPU.h"
+
+using namespace physx;
+using namespace Gu;
+
+extern GeomSweepFuncs gGeomSweepFuncs;
+extern GeomOverlapTable gGeomOverlapMethodTable[];
+extern RaycastFunc gRaycastMap[PxGeometryType::eGEOMETRY_COUNT];
+
+bool PxGeometryQuery::isValid(const PxGeometry& geom)
+{
+	switch(geom.getType())
+	{
+		case PxGeometryType::eSPHERE:
+		{
+			const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom);
+			if(!sphereGeom.isValid())
+				return false;
+			break;
+		}
+		case PxGeometryType::eCAPSULE:
+		{
+			const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom);
+			if(!capsuleGeom.isValid())
+				return false;
+			break;
+		}
+		case PxGeometryType::eBOX:
+		{
+			const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom);
+			if(!boxGeom.isValid())
+				return false;
+			break;
+		}
+		case PxGeometryType::eCONVEXMESH:
+		{
+			const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom);
+			if(!convexGeom.isValid())
+				return false;
+			break;
+		}
+		case PxGeometryType::ePLANE:
+		case PxGeometryType::eTRIANGLEMESH:
+		case PxGeometryType::eHEIGHTFIELD:
+		case PxGeometryType::eGEOMETRY_COUNT:
+		case PxGeometryType::eINVALID:
+			break;
+	}	
+	return true;
+}
+
+bool PxGeometryQuery::sweep(const PxVec3& unitDir, const PxReal distance,
+							const PxGeometry& geom0, const PxTransform& pose0,
+							const PxGeometry& geom1, const PxTransform& pose1,
+							PxSweepHit& sweepHit, PxHitFlags hitFlags,
+							const PxReal inflation)
+{
+	PX_SIMD_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))
+	{
+		Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, "Provided geometry 0 is not valid");
+		return false;
+	}
+	if(!PxGeometryQuery::isValid(geom1))
+	{
+		Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, "Provided geometry 1 is not valid");
+		return false;
+	}
+#endif // PX_CHECKED
+
+	const GeomSweepFuncs& sf = gGeomSweepFuncs;
+
+	switch(geom0.getType())
+	{
+		case PxGeometryType::eSPHERE:
+		{
+			const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom0);
+
+			// PT: TODO: technically this capsule with 0.0 half-height is invalid ("isValid" returns false)
+			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);
+		}
+
+		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);
+		}
+
+		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);
+		}
+
+		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);
+		}
+		case PxGeometryType::ePLANE:
+		case PxGeometryType::eTRIANGLEMESH:
+		case PxGeometryType::eHEIGHTFIELD:
+		case PxGeometryType::eGEOMETRY_COUNT:
+		case PxGeometryType::eINVALID:
+			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)
+{
+	PX_SIMD_GUARD;
+	return Gu::overlap(geom0, pose0, geom1, pose1, gGeomOverlapMethodTable);
+}
+
+///////////////////////////////////////////////////////////////////////////////
+PxU32 PxGeometryQuery::raycast(	const PxVec3& rayOrigin, const PxVec3& rayDir,
+								const PxGeometry& geom, const PxTransform& pose,
+								PxReal maxDist, PxHitFlags hitFlags,
+								PxU32 maxHits, PxRaycastHit* PX_RESTRICT rayHits)
+{
+	PX_SIMD_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);
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+bool pointConvexDistance(PxVec3& normal_, PxVec3& closestPoint_, PxReal& sqDistance, const PxVec3& pt, const ConvexMesh* convexMesh, const PxMeshScale& meshScale, const PxTransform& convexPose);
+
+PxReal PxGeometryQuery::pointDistance(const PxVec3& point, const PxGeometry& geom, const PxTransform& pose, PxVec3* closestPoint)
+{
+	PX_SIMD_GUARD;
+	PX_CHECK_AND_RETURN_VAL(pose.isValid(), "PxGeometryQuery::pointDistance(): pose is not valid.", false);
+
+	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);
+
+			PxVec3 normal, cp;
+			PxReal sqDistance;
+			const bool intersect = pointConvexDistance(normal, cp, sqDistance, point, static_cast<ConvexMesh*>(convexGeom.convexMesh), convexGeom.scale, pose);
+			if(!intersect && closestPoint)
+				*closestPoint = cp;
+			return sqDistance;
+		}
+		case PxGeometryType::ePLANE:
+		case PxGeometryType::eHEIGHTFIELD:
+		case PxGeometryType::eTRIANGLEMESH:
+		case PxGeometryType::eGEOMETRY_COUNT:
+		case PxGeometryType::eINVALID:
+			PX_CHECK_MSG(false, "PxGeometryQuery::pointDistance(): geometry object parameter must be sphere, capsule box or convex geometry.");
+			break;
+	}
+	return -1.0f;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+PxBounds3 PxGeometryQuery::getWorldBounds(const PxGeometry& geom, const PxTransform& pose, float inflation)
+{
+	PX_SIMD_GUARD;
+	PX_CHECK_AND_RETURN_VAL(pose.isValid(), "PxGeometryQuery::getWorldBounds(): pose is not valid.", PxBounds3::empty());
+
+	PxBounds3 bounds;
+	Gu::computeBounds(bounds, geom, pose, 0.0f, NULL, inflation);
+	PX_ASSERT(bounds.isValid());
+	return bounds;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+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)
+{
+	PX_SIMD_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);
+
+	if(geom0.getType() > geom1.getType())
+	{
+		GeomMTDFunc mtdFunc = gGeomMTDMethodTable[geom1.getType()][geom0.getType()];
+		PX_ASSERT(mtdFunc);
+		if(!mtdFunc(mtd, depth, geom1, pose1, geom0, pose0))
+			return false;
+		mtd = -mtd;
+		return true;
+	}
+	else
+	{
+		GeomMTDFunc mtdFunc = gGeomMTDMethodTable[geom0.getType()][geom1.getType()];
+		PX_ASSERT(mtdFunc);
+		return mtdFunc(mtd, depth, geom0, pose0, geom1, pose1);
+	}
+}

physx/source/geomutils/src/GuGeometryUnion.cpp

@@ -0,0 +1,120 @@
+//
+// 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-2021 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 "GuGeometryUnion.h"
+
+#include "GuConvexMesh.h"
+#include "GuTriangleMesh.h"
+#include "GuHeightField.h"
+#include "PsFoundation.h"
+
+using namespace physx;
+using namespace Gu;
+using namespace Cm;
+
+static PX_FORCE_INLINE Gu::ConvexMesh& getConvexMesh(PxConvexMesh* pxcm)
+{ 
+	return *static_cast<Gu::ConvexMesh*>(pxcm);
+}
+
+static PX_FORCE_INLINE Gu::TriangleMesh& getTriangleMesh(PxTriangleMesh* pxtm)
+{
+	return *static_cast<Gu::TriangleMesh*>(pxtm);
+}
+
+static PX_FORCE_INLINE Gu::HeightField& getHeightField(PxHeightField* pxhf)
+{
+	return *static_cast<Gu::HeightField*>(pxhf);
+}
+
+// PT: TODO: optimize all these data copies
+void Gu::GeometryUnion::set(const PxGeometry& g)
+{
+	switch(g.getType())
+	{
+		case PxGeometryType::eBOX:
+		{
+			reinterpret_cast<PxBoxGeometry&>(mGeometry) = static_cast<const PxBoxGeometry&>(g);
+		}
+		break;
+
+		case PxGeometryType::eCAPSULE:
+		{
+			reinterpret_cast<PxCapsuleGeometry&>(mGeometry) = static_cast<const PxCapsuleGeometry&>(g);
+		}
+		break;
+
+		case PxGeometryType::eSPHERE:
+		{
+			reinterpret_cast<PxSphereGeometry&>(mGeometry) = static_cast<const PxSphereGeometry&>(g);
+			reinterpret_cast<PxCapsuleGeometry&>(mGeometry).halfHeight = 0.0f;		//AM: make sphere geometry also castable as a zero height capsule.
+		}
+		break;
+
+		case PxGeometryType::ePLANE:
+		{
+			reinterpret_cast<PxPlaneGeometry&>(mGeometry) = static_cast<const PxPlaneGeometry&>(g);
+		}
+		break;
+
+		case PxGeometryType::eCONVEXMESH:
+		{
+			reinterpret_cast<PxConvexMeshGeometry&>(mGeometry) = static_cast<const PxConvexMeshGeometry&>(g);
+			reinterpret_cast<PxConvexMeshGeometryLL&>(mGeometry).hullData = &(::getConvexMesh(get<PxConvexMeshGeometryLL>().convexMesh).getHull());
+			reinterpret_cast<PxConvexMeshGeometryLL&>(mGeometry).gpuCompatible = ::getConvexMesh(get<PxConvexMeshGeometryLL>().convexMesh).isGpuCompatible();
+		}
+		break;
+
+		case PxGeometryType::eTRIANGLEMESH:
+		{
+			reinterpret_cast<PxTriangleMeshGeometry&>(mGeometry) = static_cast<const PxTriangleMeshGeometry&>(g);
+			reinterpret_cast<PxTriangleMeshGeometryLL&>(mGeometry).meshData = &(::getTriangleMesh(get<PxTriangleMeshGeometryLL>().triangleMesh));
+			reinterpret_cast<PxTriangleMeshGeometryLL&>(mGeometry).materialIndices = (::getTriangleMesh(get<PxTriangleMeshGeometryLL>().triangleMesh).getMaterials());
+			reinterpret_cast<PxTriangleMeshGeometryLL&>(mGeometry).materials = MaterialIndicesStruct();
+		}
+		break;
+
+		case PxGeometryType::eHEIGHTFIELD:
+		{
+			reinterpret_cast<PxHeightFieldGeometry&>(mGeometry) = static_cast<const PxHeightFieldGeometry&>(g);
+			reinterpret_cast<PxHeightFieldGeometryLL&>(mGeometry).heightFieldData = &::getHeightField(get<PxHeightFieldGeometryLL>().heightField).getData();
+			reinterpret_cast<PxHeightFieldGeometryLL&>(mGeometry).materials = MaterialIndicesStruct();
+		}
+		break;
+
+		case PxGeometryType::eGEOMETRY_COUNT:
+		case PxGeometryType::eINVALID:
+			PX_ALWAYS_ASSERT_MESSAGE("geometry type not handled");
+		break;
+	}
+}
+
+
+
+

physx/source/geomutils/src/GuGeometryUnion.h

@@ -0,0 +1,252 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_GEOMETRY_UNION_H
+#define GU_GEOMETRY_UNION_H
+
+#include "foundation/PxBounds3.h"
+#include "geometry/PxBoxGeometry.h"
+#include "geometry/PxSphereGeometry.h"
+#include "geometry/PxCapsuleGeometry.h"
+#include "geometry/PxPlaneGeometry.h"
+#include "geometry/PxConvexMeshGeometry.h"
+#include "geometry/PxTriangleMeshGeometry.h"
+#include "geometry/PxHeightFieldGeometry.h"
+#include "GuSIMDHelpers.h"
+#include <stddef.h>
+#include "PsAllocator.h"
+#include "GuBox.h"
+#include "GuCenterExtents.h"
+#include "GuSphere.h"
+#include "GuCapsule.h"
+
+namespace physx
+{
+
+namespace Gu
+{
+	struct ConvexHullData;
+	class TriangleMesh;
+	struct HeightFieldData;
+	class GeometryUnion;
+}
+
+
+//
+// Summary of our material approach:
+//
+// On the API level, materials are accessed via pointer. Internally we store indices into the material table.
+// The material table is stored in the SDK and the materials are shared among scenes. To make this threadsafe,
+// we have the following approach:
+//
+// - Every scene has a copy of the SDK master material table
+// - At the beginning of a simulation step, the scene material table gets synced to the master material table.
+// - While the simulation is running, the scene table does not get touched.
+// - Each shape stores the indices of its material(s). When the simulation is not running and a user requests the
+//   materials of the shape, the indices are used to fetch the material from the master material table. When the
+//   the simulation is running then the same indices are used internally to fetch the materials from the scene
+//   material table. If a user changes the materials of a shape while the simulation is running, the index list
+//   will not get touched, instead the new materials get buffered and synced at the end of the simulation.
+// - This whole scheme only works as long as the position of a material in the material table does not change
+//   when other materials get deleted/inserted. The data structure of the material table makes sure that is the case.
+//
+
+struct PX_PHYSX_COMMON_API MaterialIndicesStruct
+{
+//= ATTENTION! =====================================================================================
+// Changing the data layout of this class breaks the binary serialization format.  See comments for 
+// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
+// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
+// accordingly.
+//==================================================================================================
+// PX_SERIALIZATION
+	MaterialIndicesStruct(const PxEMPTY)	{}
+	static void getBinaryMetaData(PxOutputStream& stream);
+//~PX_SERIALIZATION
+
+	MaterialIndicesStruct()
+	:	indices(NULL)
+	,	numIndices(0)
+	,	pad(PX_PADDING_16)
+#if PX_P64_FAMILY
+	,	pad64(PX_PADDING_32)
+#endif
+	{
+	}
+
+	~MaterialIndicesStruct()
+	{
+	}
+
+	void allocate(PxU16 size)
+	{
+		indices = reinterpret_cast<PxU16*>(PX_ALLOC(sizeof(PxU16) * size, "MaterialIndicesStruct::allocate"));
+		numIndices = size;
+	}
+
+	void deallocate()
+	{
+		PX_FREE(indices);
+		numIndices = 0;
+	}
+	PxU16*	indices; // the remap table for material index
+	PxU16	numIndices; // the size of the remap table
+	PxU16	pad; // pad for serialization
+#if PX_P64_FAMILY
+	PxU32	pad64; // pad for serialization
+#endif
+};
+
+struct PxConvexMeshGeometryLL: public PxConvexMeshGeometry
+{
+	const Gu::ConvexHullData*			hullData;
+	bool								gpuCompatible;
+};
+
+struct PxTriangleMeshGeometryLL: public PxTriangleMeshGeometry
+{
+	const Gu::TriangleMesh*				meshData;
+	const PxU16*						materialIndices;
+	MaterialIndicesStruct				materials;	
+};
+
+struct PxHeightFieldGeometryLL : public PxHeightFieldGeometry
+{
+	const Gu::HeightFieldData*			heightFieldData;			
+	MaterialIndicesStruct				materials;
+};
+
+// We sometimes overload capsule code for spheres, so every sphere should have 
+// valid capsule data (height = 0). This is preferable to a typedef so that we
+// can maintain traits separately for a sphere, but some care is required to deal
+// with the fact that when a reference to a capsule is extracted, it may have its
+// type field set to eSPHERE
+
+template <typename T>
+struct PxcGeometryTraits
+{
+	enum {TypeID = PxGeometryType::eINVALID };
+};
+template <typename T> struct PxcGeometryTraits<const T> { enum { TypeID = PxcGeometryTraits<T>::TypeID }; };
+
+template <> struct PxcGeometryTraits<PxBoxGeometry>				{ enum { TypeID = PxGeometryType::eBOX }; };
+template <> struct PxcGeometryTraits<PxSphereGeometry>			{ enum { TypeID = PxGeometryType::eSPHERE }; };
+template <> struct PxcGeometryTraits<PxCapsuleGeometry>			{ enum { TypeID = PxGeometryType::eCAPSULE }; };
+template <> struct PxcGeometryTraits<PxPlaneGeometry>			{ enum { TypeID = PxGeometryType::ePLANE }; };
+template <> struct PxcGeometryTraits<PxConvexMeshGeometryLL>	{ enum { TypeID = PxGeometryType::eCONVEXMESH }; };
+template <> struct PxcGeometryTraits<PxTriangleMeshGeometryLL>	{ enum { TypeID = PxGeometryType::eTRIANGLEMESH }; };
+template <> struct PxcGeometryTraits<PxHeightFieldGeometryLL> 	{ enum { TypeID = PxGeometryType::eHEIGHTFIELD }; };
+template<class T> PX_CUDA_CALLABLE PX_FORCE_INLINE void checkType(const Gu::GeometryUnion& geometry);
+template<> PX_CUDA_CALLABLE PX_INLINE void checkType<PxCapsuleGeometry>(const Gu::GeometryUnion& geometry);
+template<> PX_CUDA_CALLABLE PX_INLINE void checkType<const PxCapsuleGeometry>(const Gu::GeometryUnion& geometry);
+
+
+namespace Gu
+{
+
+class InvalidGeometry : public PxGeometry
+{
+public:
+	PX_CUDA_CALLABLE PX_FORCE_INLINE InvalidGeometry() : PxGeometry(PxGeometryType::eINVALID) {}
+};
+
+class PX_PHYSX_COMMON_API GeometryUnion
+{
+//= ATTENTION! =====================================================================================
+// Changing the data layout of this class breaks the binary serialization format.  See comments for 
+// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
+// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
+// accordingly.
+//==================================================================================================
+public:
+// PX_SERIALIZATION
+	GeometryUnion(const PxEMPTY)	{}
+	static	void	getBinaryMetaData(PxOutputStream& stream);
+//~PX_SERIALIZATION
+
+	PX_CUDA_CALLABLE PX_FORCE_INLINE						GeometryUnion()						{ reinterpret_cast<InvalidGeometry&>(mGeometry) = InvalidGeometry(); }
+	PX_CUDA_CALLABLE PX_FORCE_INLINE						GeometryUnion(const PxGeometry& g)	{ set(g);	}
+
+	PX_CUDA_CALLABLE PX_FORCE_INLINE const PxGeometry&		getGeometry()				const	{ return reinterpret_cast<const PxGeometry&>(mGeometry);			}
+	PX_CUDA_CALLABLE PX_FORCE_INLINE PxGeometryType::Enum	getType()					const	{ return reinterpret_cast<const PxGeometry&>(mGeometry).getType();	}
+
+	PX_CUDA_CALLABLE void	set(const PxGeometry& g);
+
+	template<class Geom> PX_CUDA_CALLABLE PX_FORCE_INLINE Geom& get()
+	{
+		checkType<Geom>(*this);
+		return reinterpret_cast<Geom&>(mGeometry);
+	}
+
+	template<class Geom> PX_CUDA_CALLABLE PX_FORCE_INLINE const Geom& get() const
+	{
+		checkType<Geom>(*this);
+		return reinterpret_cast<const Geom&>(mGeometry);
+	}
+
+private:
+
+	union {
+		void*	alignment;	// PT: Makes sure the class is at least aligned to pointer size. See DE6803. 
+		PxU8	box[sizeof(PxBoxGeometry)];
+		PxU8	sphere[sizeof(PxSphereGeometry)];
+		PxU8	capsule[sizeof(PxCapsuleGeometry)];
+		PxU8	plane[sizeof(PxPlaneGeometry)];
+		PxU8	convex[sizeof(PxConvexMeshGeometryLL)];
+		PxU8	mesh[sizeof(PxTriangleMeshGeometryLL)];
+		PxU8	heightfield[sizeof(PxHeightFieldGeometryLL)];
+		PxU8	invalid[sizeof(InvalidGeometry)];
+	} mGeometry;
+};
+}
+
+
+template<class T> PX_CUDA_CALLABLE PX_FORCE_INLINE void checkType(const Gu::GeometryUnion& geometry)
+{
+	PX_ASSERT(PxU32(geometry.getType()) == PxU32(PxcGeometryTraits<T>::TypeID));
+	PX_UNUSED(geometry);
+}
+
+template<> PX_CUDA_CALLABLE PX_FORCE_INLINE void checkType<PxCapsuleGeometry>(const Gu::GeometryUnion& geometry)
+{
+	PX_ASSERT(geometry.getType() == PxGeometryType::eCAPSULE || geometry.getType() == PxGeometryType::eSPHERE);
+	PX_UNUSED(geometry);
+}
+
+template<> PX_CUDA_CALLABLE PX_FORCE_INLINE void checkType<const PxCapsuleGeometry>(const Gu::GeometryUnion& geometry)
+{
+	PX_ASSERT(geometry.getType()== PxGeometryType::eCAPSULE || geometry.getType() == PxGeometryType::eSPHERE);
+	PX_UNUSED(geometry);
+}
+
+// the shape structure relies on punning capsules and spheres 
+PX_COMPILE_TIME_ASSERT(PX_OFFSET_OF(PxCapsuleGeometry, radius) == PX_OFFSET_OF(PxSphereGeometry, radius));
+}
+
+#endif

physx/source/geomutils/src/GuInternal.cpp

@@ -0,0 +1,159 @@
+//
+// 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-2021 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 "foundation/PxBounds3.h"
+#include "geometry/PxCapsuleGeometry.h"
+#include "PsIntrinsics.h"
+#include "GuInternal.h"
+#include "GuBox.h"
+#include "GuVecPlane.h"
+#include "PsMathUtils.h"
+#include "PsVecMath.h"
+using namespace physx::shdfnd::aos;
+
+using namespace physx;
+
+/**
+Computes the aabb points.
+\param		pts	[out] 8 box points
+*/
+void Gu::computeBoxPoints(const PxBounds3& bounds, PxVec3* PX_RESTRICT pts)
+{
+	PX_ASSERT(pts);
+
+	// Get box corners
+	const PxVec3& minimum = bounds.minimum;
+	const PxVec3& maximum = bounds.maximum;
+
+	//     7+------+6			0 = ---
+	//     /|     /|			1 = +--
+	//    / |    / |			2 = ++-
+	//   / 4+---/--+5			3 = -+-
+	// 3+------+2 /    y   z	4 = --+
+	//  | /    | /     |  /		5 = +-+
+	//  |/     |/      |/		6 = +++
+	// 0+------+1      *---x	7 = -++
+
+	// Generate 8 corners of the bbox
+	pts[0] = PxVec3(minimum.x, minimum.y, minimum.z);
+	pts[1] = PxVec3(maximum.x, minimum.y, minimum.z);
+	pts[2] = PxVec3(maximum.x, maximum.y, minimum.z);
+	pts[3] = PxVec3(minimum.x, maximum.y, minimum.z);
+	pts[4] = PxVec3(minimum.x, minimum.y, maximum.z);
+	pts[5] = PxVec3(maximum.x, minimum.y, maximum.z);
+	pts[6] = PxVec3(maximum.x, maximum.y, maximum.z);
+	pts[7] = PxVec3(minimum.x, maximum.y, maximum.z);
+}
+
+PxPlane Gu::getPlane(const PxTransform& pose)
+{ 
+	const PxVec3 n = pose.q.getBasisVector0();
+	return PxPlane(n, -pose.p.dot(n)); 
+}
+
+void Gu::computeBoundsAroundVertices(PxBounds3& bounds, PxU32 nbVerts, const PxVec3* PX_RESTRICT verts)
+{
+	// PT: we can safely V4LoadU the first N-1 vertices. We must V3LoadU the last vertex, to make sure we don't read
+	// invalid memory. Since we have to special-case that last vertex anyway, we reuse that code to also initialize
+	// the minV/maxV values (bypassing the need for a 'setEmpty()' initialization).
+
+	if(!nbVerts)
+	{
+		bounds.setEmpty();
+		return;
+	}
+
+	PxU32 nbSafe = nbVerts-1;
+
+	// PT: read last (unsafe) vertex using V3LoadU, initialize minV/maxV
+	const Vec4V lastVertexV = Vec4V_From_Vec3V(V3LoadU(&verts[nbSafe].x));
+	Vec4V minV = lastVertexV;
+	Vec4V maxV = lastVertexV;
+
+	// PT: read N-1 first (safe) vertices using V4LoadU
+	while(nbSafe--)
+	{
+		const Vec4V vertexV = V4LoadU(&verts->x);
+		verts++;
+
+		minV = V4Min(minV, vertexV);
+		maxV = V4Max(maxV, vertexV);
+	}
+
+	StoreBounds(bounds, minV, maxV);
+}
+
+void Gu::computeSweptBox(Gu::Box& dest, const PxVec3& extents, const PxVec3& center, const PxMat33& rot, const PxVec3& unitDir, const PxReal distance)
+{
+	PxVec3 R1, R2;
+	Ps::computeBasis(unitDir, R1, R2);
+
+	PxReal dd[3];
+	dd[0] = PxAbs(rot.column0.dot(unitDir));
+	dd[1] = PxAbs(rot.column1.dot(unitDir));
+	dd[2] = PxAbs(rot.column2.dot(unitDir));
+	PxReal dmax = dd[0];
+	PxU32 ax0=1;
+	PxU32 ax1=2;
+	if(dd[1]>dmax)
+	{
+		dmax=dd[1];
+		ax0=0;
+		ax1=2;
+	}
+	if(dd[2]>dmax)
+	{
+		dmax=dd[2];
+		ax0=0;
+		ax1=1;
+	}
+	if(dd[ax1]<dd[ax0])
+		Ps::swap(ax0, ax1);
+
+	R1 = rot[ax0];
+	R1 -= (R1.dot(unitDir))*unitDir;	// Project to plane whose normal is dir
+	R1.normalize();
+	R2 = unitDir.cross(R1);
+
+	dest.setAxes(unitDir, R1, R2);
+
+	PxReal offset[3];
+	offset[0] = distance;
+	offset[1] = distance*(unitDir.dot(R1));
+	offset[2] = distance*(unitDir.dot(R2));
+
+	for(PxU32 r=0; r<3; r++)
+	{
+		const PxVec3& R = dest.rot[r];
+		dest.extents[r] = offset[r]*0.5f + PxAbs(rot.column0.dot(R))*extents.x + PxAbs(rot.column1.dot(R))*extents.y + PxAbs(rot.column2.dot(R))*extents.z;
+	}
+
+	dest.center = center + unitDir*distance*0.5f;
+}

physx/source/geomutils/src/GuInternal.h

@@ -0,0 +1,151 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_GEOM_UTILS_INTERNAL_H
+#define GU_GEOM_UTILS_INTERNAL_H
+
+#include "geometry/PxCapsuleGeometry.h"
+#include "geometry/PxBoxGeometry.h"
+#include "CmPhysXCommon.h"
+#include "GuCapsule.h"
+#include "PsMathUtils.h"
+#include "PsUtilities.h"
+
+#define GU_EPSILON_SAME_DISTANCE 1e-3f
+
+namespace physx
+{
+namespace Gu
+{
+	class Box;
+
+	// PT: TODO: now that the Gu files are not exposed to users anymore, we should move back capsule-related functions
+	// to GuCapsule.h, etc
+
+	PX_PHYSX_COMMON_API const PxU8*	getBoxEdges();
+
+	PX_PHYSX_COMMON_API void		computeBoxPoints(const PxBounds3& bounds, PxVec3* PX_RESTRICT pts);
+	PX_PHYSX_COMMON_API void		computeBoundsAroundVertices(PxBounds3& bounds, PxU32 nbVerts, const PxVec3* PX_RESTRICT verts);
+
+						void		computeBoxAroundCapsule(const Capsule& capsule, Box& box);
+
+						PxPlane		getPlane(const PxTransform& pose);
+
+	PX_FORCE_INLINE		PxVec3		getCapsuleHalfHeightVector(const PxTransform& transform, const PxCapsuleGeometry& capsuleGeom)
+									{
+										return transform.q.getBasisVector0() * capsuleGeom.halfHeight;
+									}
+
+	PX_FORCE_INLINE		void		getCapsuleSegment(const PxTransform& transform, const PxCapsuleGeometry& capsuleGeom, Gu::Segment& segment)
+									{
+										const PxVec3 tmp = getCapsuleHalfHeightVector(transform, capsuleGeom);
+										segment.p0 = transform.p + tmp;
+										segment.p1 = transform.p - tmp;
+									}
+
+	PX_FORCE_INLINE		void		getCapsule(Gu::Capsule& capsule, const PxCapsuleGeometry& capsuleGeom, const PxTransform& pose)
+									{
+										getCapsuleSegment(pose, capsuleGeom, capsule);
+										capsule.radius = capsuleGeom.radius;
+									}
+
+						void		computeSweptBox(Gu::Box& box, const PxVec3& extents, const PxVec3& center, const PxMat33& rot, const PxVec3& unitDir, const PxReal distance);
+
+	/**
+	*	PT: computes "alignment value" used to select the "best" triangle in case of identical impact distances (for sweeps).
+	*	This simply computes how much a triangle is aligned with a given sweep direction.
+	*	Captured in a function to make sure it is always computed correctly, i.e. working for double-sided triangles.
+	*
+	*	\param		triNormal	[in] triangle's normal
+	*	\param		unitDir		[in] sweep direction (normalized)
+	*	\return		alignment value in [-1.0f, 0.0f]. -1.0f for fully aligned, 0.0f for fully orthogonal.
+	*/
+	PX_FORCE_INLINE		PxReal		computeAlignmentValue(const PxVec3& triNormal, const PxVec3& unitDir)
+	{
+		// PT: initial dot product gives the angle between the two, with "best" triangles getting a +1 or -1 score
+		// depending on their winding. We take the absolute value to ignore the impact of winding. We negate the result
+		// to make the function compatible with the initial code, which assumed single-sided triangles and expected -1
+		// for best triangles.
+		return -PxAbs(triNormal.dot(unitDir));
+	}
+
+	/**
+	*	PT: sweeps: determines if a newly touched triangle is "better" than best one so far.
+	*	In this context "better" means either clearly smaller impact distance, or a similar impact
+	*	distance but a normal more aligned with the sweep direction.
+	*
+	*	\param		triImpactDistance	[in] new triangle's impact distance
+	*	\param		triAlignmentValue	[in] new triangle's alignment value (as computed by computeAlignmentValue)
+	*	\param		bestImpactDistance	[in] current best triangle's impact distance
+	*	\param		bestAlignmentValue	[in] current best triangle's alignment value (as computed by computeAlignmentValue)
+	*   \param		maxDistance			[in] maximum distance of the query, hit cannot be longer than this maxDistance
+	*	\param		distEpsilon			[in] tris have "similar" impact distances if the difference is smaller than 2*distEpsilon
+	*	\return		true if new triangle is better
+	*/
+	PX_FORCE_INLINE		bool		keepTriangle(	float triImpactDistance, float triAlignmentValue,
+										float bestImpactDistance, float bestAlignmentValue, float maxDistance,
+										float distEpsilon)
+	{
+		// Reject triangle if further than the maxDistance
+		if(triImpactDistance > maxDistance)
+			return false;
+
+		// PT: make it a relative epsilon to make sure it still works with large distances
+		distEpsilon *= PxMax(1.0f, PxMax(triImpactDistance, bestImpactDistance));
+
+		// If new distance is more than epsilon closer than old distance
+		if(triImpactDistance < bestImpactDistance - distEpsilon)
+			return true;
+
+		// If new distance is no more than epsilon farther than oldDistance and "face is more opposing than previous"
+		if(triImpactDistance < bestImpactDistance+distEpsilon && triAlignmentValue < bestAlignmentValue)
+			return true;
+
+		// If alignment value is the same, but the new triangle is closer than the best distance
+		if(triAlignmentValue == bestAlignmentValue && triImpactDistance < bestImpactDistance)
+			return true;
+
+		// If initial overlap happens, keep the triangle
+		if(triImpactDistance == 0.0f)
+			return true;
+
+		return false;
+	}
+
+	#define StoreBounds(bounds, minV, maxV)	\
+		V4StoreU(minV, &bounds.minimum.x);	\
+		PX_ALIGN(16, PxVec4) max4;			\
+		V4StoreA(maxV, &max4.x);			\
+		bounds.maximum = PxVec3(max4.x, max4.y, max4.z);
+
+}  // namespace Gu
+
+}
+
+#endif

physx/source/geomutils/src/GuMTD.h

@@ -0,0 +1,60 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_MTD_H
+#define GU_MTD_H
+
+#include "geometry/PxGeometry.h"
+
+namespace physx
+{
+namespace Gu
+{
+	// PT: we use a define to be able to quickly change the signature of all MTD functions.
+	// (this also ensures they all use consistent names for passed parameters).
+	// \param[out]	mtd		computed depenetration dir
+	// \param[out]	depth	computed depenetration depth
+	// \param[in]	geom0	first geometry object
+	// \param[in]	pose0	pose of first geometry object
+	// \param[in]	geom1	second geometry object
+	// \param[in]	pose1	pose of second geometry object
+	// \param[in]	cache	optional cached data for triggers
+	#define GU_MTD_FUNC_PARAMS	PxVec3& mtd, PxF32& depth,							\
+								const PxGeometry& geom0, const PxTransform& pose0,	\
+								const PxGeometry& geom1, const PxTransform& pose1
+
+	// PT: function pointer for Geom-indexed MTD functions
+	// See GU_MTD_FUNC_PARAMS for function parameters details.
+	// \return		true if an overlap was found, false otherwise
+	// \note		depenetration vector D is equal to mtd * depth. It should be applied to the 1st object, to get out of the 2nd object.
+	typedef bool (*GeomMTDFunc)	(GU_MTD_FUNC_PARAMS);
+}
+}
+
+#endif

physx/source/geomutils/src/GuMeshFactory.cpp

@@ -0,0 +1,703 @@
+//
+// 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-2021 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/PxHeightFieldDesc.h"
+
+#include "GuMeshFactory.h"
+#include "GuTriangleMesh.h"
+#include "GuTriangleMeshBV4.h"
+#include "GuTriangleMeshRTree.h"
+#include "GuConvexMesh.h"
+#include "GuBVHStructure.h"
+#include "GuHeightField.h"
+#include "GuConvexMeshData.h"
+#include "GuMeshData.h"
+#include "CmUtils.h"
+#include "PsIntrinsics.h"
+#include "PsFoundation.h"
+
+using namespace physx;
+using namespace Gu;
+
+// PT: TODO: refactor all this with a dedicated container
+
+GuMeshFactory::GuMeshFactory() :
+	mTriangleMeshes		(PX_DEBUG_EXP("mesh factory triangle mesh hash")),
+	mConvexMeshes		(PX_DEBUG_EXP("mesh factory convex mesh hash")),
+	mHeightFields		(PX_DEBUG_EXP("mesh factory height field hash")),
+    mBVHStructures 		(PX_DEBUG_EXP("BVH structure factory hash")),
+	mFactoryListeners	(PX_DEBUG_EXP("FactoryListeners"))
+{
+}
+
+GuMeshFactory::~GuMeshFactory()
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+template<class T>
+static void releaseObjects(Ps::CoalescedHashSet<T*>& objects)
+{
+	while(objects.size())
+	{
+		T* object = objects.getEntries()[0];
+		PX_ASSERT(object->getRefCount()==1);
+		object->release();
+	}
+}
+
+void GuMeshFactory::release()
+{
+	// Release all objects in case the user didn't do it
+	releaseObjects(mTriangleMeshes);
+	releaseObjects(mConvexMeshes);
+	releaseObjects(mHeightFields);
+	releaseObjects(mBVHStructures);
+
+	PX_DELETE(this);
+}
+
+template <typename T>
+static void addToHash(Ps::CoalescedHashSet<T*>& hash, T* element, Ps::Mutex* mutex)
+{
+	if(!element)
+		return;
+
+	if(mutex)
+		mutex->lock();
+
+	hash.insert(element);
+
+	if(mutex)
+		mutex->unlock();
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void GuMeshFactory::addTriangleMesh(TriangleMesh* np, bool lock)
+{
+	addToHash(mTriangleMeshes, np, lock ? &mTrackingMutex : NULL);
+}
+
+PxTriangleMesh* GuMeshFactory::createTriangleMesh(TriangleMeshData& data)
+{	
+	TriangleMesh* np;
+
+	if(data.mType==PxMeshMidPhase::eBVH33)
+	{
+		PX_NEW_SERIALIZED(np, RTreeTriangleMesh)(*this, data);
+	}
+	else if(data.mType==PxMeshMidPhase::eBVH34)
+	{
+		PX_NEW_SERIALIZED(np, BV4TriangleMesh)(*this, data);
+	}
+	else return NULL;
+
+	if(np)
+		addTriangleMesh(np);
+
+	return np;
+}
+
+// data injected by cooking lib for runtime cooking
+PxTriangleMesh* GuMeshFactory::createTriangleMesh(void* data)
+{	
+	return createTriangleMesh(*reinterpret_cast<TriangleMeshData*>(data));
+}
+
+static TriangleMeshData* loadMeshData(PxInputStream& stream)
+{
+	// Import header
+	PxU32 version;
+	bool mismatch;
+	if(!readHeader('M', 'E', 'S', 'H', version, mismatch, stream))
+		return NULL;
+
+	PxU32 midphaseID = PxMeshMidPhase::eBVH33;	// Default before version 14
+	if(version>=14)	// this refers to PX_MESH_VERSION
+	{
+		midphaseID = readDword(mismatch, stream);
+	}
+
+	// Check if old (incompatible) mesh format is loaded
+	if (version <= 9) // this refers to PX_MESH_VERSION
+	{
+		Ps::getFoundation().error(PxErrorCode::eINTERNAL_ERROR, __FILE__, __LINE__, "Loading triangle mesh failed: "
+			"Deprecated mesh cooking format. Please recook your mesh in a new cooking format.");
+		PX_ALWAYS_ASSERT_MESSAGE("Obsolete cooked mesh found. Mesh version has been updated, please recook your meshes.");
+		return NULL;
+	}
+
+	// Import serialization flags
+	const PxU32 serialFlags = readDword(mismatch, stream);
+
+	// Import misc values	
+	if (version <= 12) // this refers to PX_MESH_VERSION
+	{
+		// convexEdgeThreshold was removed in 3.4.0
+		readFloat(mismatch, stream);		
+	}
+
+	TriangleMeshData* data;
+	if(midphaseID==PxMeshMidPhase::eBVH33)
+		data = PX_NEW(RTreeTriangleData);
+	else if(midphaseID==PxMeshMidPhase::eBVH34)
+		data = PX_NEW(BV4TriangleData);
+	else return NULL;
+
+	// Import mesh
+	PxVec3* verts = data->allocateVertices(readDword(mismatch, stream));
+	const PxU32 nbTris = readDword(mismatch, stream);
+	bool force32 = (serialFlags & (IMSF_8BIT_INDICES|IMSF_16BIT_INDICES)) == 0;
+
+	//ML: this will allocate CPU triangle indices and GPU triangle indices if we have GRB data built
+	void* tris = data->allocateTriangles(nbTris, force32, serialFlags & IMSF_GRB_DATA);
+
+	stream.read(verts, sizeof(PxVec3)*data->mNbVertices);
+	if(mismatch)
+	{
+		for(PxU32 i=0;i<data->mNbVertices;i++)
+		{
+			flip(verts[i].x);
+			flip(verts[i].y);
+			flip(verts[i].z);
+		}
+	}
+	//TODO: stop support for format conversion on load!!
+	const PxU32 nbIndices = 3*data->mNbTriangles;
+	if(serialFlags & IMSF_8BIT_INDICES)
+	{
+		PxU8 x;
+		if(data->has16BitIndices())
+		{
+			PxU16* tris16 = reinterpret_cast<PxU16*>(tris);
+			for(PxU32 i=0;i<nbIndices;i++)
+			{
+				stream.read(&x, sizeof(PxU8));
+				*tris16++ = x;
+			}
+		}
+		else
+		{
+			PxU32* tris32 = reinterpret_cast<PxU32*>(tris);
+			for(PxU32 i=0;i<nbIndices;i++)
+			{
+				stream.read(&x, sizeof(PxU8));
+				*tris32++ = x;
+			}
+		}
+	}
+	else if(serialFlags & IMSF_16BIT_INDICES)
+	{
+		if(data->has16BitIndices())
+		{
+			PxU16* tris16 = reinterpret_cast<PxU16*>(tris);
+			stream.read(tris16, nbIndices*sizeof(PxU16));
+			if(mismatch)
+			{
+				for(PxU32 i=0;i<nbIndices;i++)
+					flip(tris16[i]);
+			}
+		}
+		else
+		{
+			PxU32* tris32 = reinterpret_cast<PxU32*>(tris);
+			PxU16 x;
+			for(PxU32 i=0;i<nbIndices;i++)
+			{
+				stream.read(&x, sizeof(PxU16));
+				if(mismatch)
+					flip(x);
+
+				*tris32++ = x;
+			}
+		}
+
+	}
+	else
+	{
+		if(data->has16BitIndices())
+		{
+			PxU32 x;
+			PxU16* tris16 = reinterpret_cast<PxU16*>(tris);
+			for(PxU32 i=0;i<nbIndices;i++)
+			{
+				stream.read(&x, sizeof(PxU32));
+				if(mismatch)
+					flip(x);
+				*tris16++ = Ps::to16(x);
+			}
+		}
+		else
+		{
+			PxU32* tris32 = reinterpret_cast<PxU32*>(tris);
+			stream.read(tris32, nbIndices*sizeof(PxU32));
+
+			if(mismatch)
+			{
+				for(PxU32 i=0;i<nbIndices;i++)
+					 flip(tris32[i]);
+			}
+		}
+	}
+
+	if(serialFlags & IMSF_MATERIALS)
+	{
+		PxU16* materials = data->allocateMaterials();
+		stream.read(materials, sizeof(PxU16)*data->mNbTriangles);
+		if(mismatch)
+		{
+			for(PxU32 i=0;i<data->mNbTriangles;i++)
+				flip(materials[i]);
+		}
+	}
+	if(serialFlags & IMSF_FACE_REMAP)
+	{
+		PxU32* remap = data->allocateFaceRemap();
+		readIndices(readDword(mismatch, stream), data->mNbTriangles, remap, stream, mismatch);
+	}
+
+	if(serialFlags & IMSF_ADJACENCIES)
+	{
+		PxU32* adj = data->allocateAdjacencies();
+		stream.read(adj, sizeof(PxU32)*data->mNbTriangles*3);
+		if(mismatch)
+		{
+			for(PxU32 i=0;i<data->mNbTriangles*3;i++)
+				flip(adj[i]);
+		}		
+	}
+
+	// PT: TODO better
+	if(midphaseID==PxMeshMidPhase::eBVH33)
+	{
+		if(!static_cast<RTreeTriangleData*>(data)->mRTree.load(stream, version, mismatch))
+		{
+			Ps::getFoundation().error(PxErrorCode::eINTERNAL_ERROR, __FILE__, __LINE__, "RTree binary image load error.");
+			PX_DELETE(data);
+			return NULL;
+		}
+	}
+	else if(midphaseID==PxMeshMidPhase::eBVH34)
+	{
+		BV4TriangleData* bv4data = static_cast<BV4TriangleData*>(data);
+		if(!bv4data->mBV4Tree.load(stream, mismatch))
+		{
+			Ps::getFoundation().error(PxErrorCode::eINTERNAL_ERROR, __FILE__, __LINE__, "BV4 binary image load error.");
+			PX_DELETE(data);
+			return NULL;
+		}
+
+		bv4data->mMeshInterface.setNbTriangles(nbTris);
+		bv4data->mMeshInterface.setNbVertices(data->mNbVertices);
+		if(data->has16BitIndices())
+			bv4data->mMeshInterface.setPointers(NULL, reinterpret_cast<IndTri16*>(tris), verts);
+		else
+			bv4data->mMeshInterface.setPointers(reinterpret_cast<IndTri32*>(tris), NULL, verts);
+		bv4data->mBV4Tree.mMeshInterface = &bv4data->mMeshInterface;
+	}
+	else PX_ASSERT(0);
+
+	// Import local bounds
+	data->mGeomEpsilon = readFloat(mismatch, stream);
+	readFloatBuffer(&data->mAABB.minimum.x, 6, mismatch, stream);
+
+	PxU32 nb = readDword(mismatch, stream);
+	if(nb)
+	{
+		PX_ASSERT(nb==data->mNbTriangles);
+		data->allocateExtraTrigData();
+		// No need to convert those bytes
+		stream.read(data->mExtraTrigData, nb*sizeof(PxU8));
+	}
+
+	if(serialFlags & IMSF_GRB_DATA)
+	{
+		PxU32 GRB_meshAdjVerticiesTotal = 0;
+		if(version < 15)
+			GRB_meshAdjVerticiesTotal = readDword(mismatch, stream);
+
+		//read grb triangle indices
+		PX_ASSERT(data->mGRB_primIndices);
+
+		if (serialFlags & IMSF_8BIT_INDICES)
+		{
+			PxU8 x;
+			if (data->has16BitIndices())
+			{
+				PxU16* tris16 = reinterpret_cast<PxU16*>(data->mGRB_primIndices);
+				for (PxU32 i = 0; i<nbIndices; i++)
+				{
+					stream.read(&x, sizeof(PxU8));
+					*tris16++ = x;
+				}
+			}
+			else
+			{
+				PxU32* tris32 = reinterpret_cast<PxU32*>(data->mGRB_primIndices);
+				for (PxU32 i = 0; i<nbIndices; i++)
+				{
+					stream.read(&x, sizeof(PxU8));
+					*tris32++ = x;
+				}
+			}
+		}
+		else if (serialFlags & IMSF_16BIT_INDICES)
+		{
+			if (data->has16BitIndices())
+			{
+				PxU16* tris16 = reinterpret_cast<PxU16*>(data->mGRB_primIndices);
+				stream.read(tris16, nbIndices*sizeof(PxU16));
+				if (mismatch)
+				{
+					for (PxU32 i = 0; i<nbIndices; i++)
+						flip(tris16[i]);
+				}
+			}
+			else
+			{
+				PxU32* tris32 = reinterpret_cast<PxU32*>(data->mGRB_primIndices);
+				PxU16 x;
+				for (PxU32 i = 0; i<nbIndices; i++)
+				{
+					stream.read(&x, sizeof(PxU16));
+					if (mismatch)
+						flip(x);
+
+					*tris32++ = x;
+				}
+			}
+
+		}
+		else
+		{
+			if (data->has16BitIndices())
+			{
+				PxU32 x;
+				PxU16* tris16 = reinterpret_cast<PxU16*>(data->mGRB_primIndices);
+				for (PxU32 i = 0; i<nbIndices; i++)
+				{
+					stream.read(&x, sizeof(PxU32));
+					if (mismatch)
+						flip(x);
+					*tris16++ = Ps::to16(x);
+				}
+			}
+			else
+			{
+				PxU32* tris32 = reinterpret_cast<PxU32*>(data->mGRB_primIndices);
+				stream.read(tris32, nbIndices*sizeof(PxU32));
+
+				if (mismatch)
+				{
+					for (PxU32 i = 0; i<nbIndices; i++)
+						flip(tris32[i]);
+				}
+			}
+		}
+		
+
+		data->mGRB_primAdjacencies = static_cast<void *>(PX_NEW(PxU32)[data->mNbTriangles*4]);
+		data->mGRB_faceRemap = PX_NEW(PxU32)[data->mNbTriangles];
+
+		stream.read(data->mGRB_primAdjacencies, sizeof(PxU32)*data->mNbTriangles*4);
+		if (version < 15)
+		{
+			//stream.read(data->mGRB_vertValency, sizeof(PxU32)*data->mNbVertices);
+			for (PxU32 i = 0; i < data->mNbVertices; ++i)
+				readDword(mismatch, stream);
+			//stream.read(data->mGRB_adjVertStart, sizeof(PxU32)*data->mNbVertices);
+			for (PxU32 i = 0; i < data->mNbVertices; ++i)
+				readDword(mismatch, stream);
+			//stream.read(data->mGRB_adjVertices, sizeof(PxU32)*GRB_meshAdjVerticiesTotal);
+			for (PxU32 i = 0; i < GRB_meshAdjVerticiesTotal; ++i)
+				readDword(mismatch, stream);
+		}
+		stream.read(data->mGRB_faceRemap, sizeof(PxU32)*data->mNbTriangles);
+
+		if(mismatch)
+		{
+			for(PxU32 i=0;i<data->mNbTriangles*4;i++)
+				flip(reinterpret_cast<PxU32 *>(data->mGRB_primIndices)[i]);
+
+			for(PxU32 i=0;i<data->mNbTriangles*4;i++)
+				flip(reinterpret_cast<PxU32 *>(data->mGRB_primAdjacencies)[i]);
+		}
+
+		//read BV32
+		data->mGRB_BV32Tree = PX_NEW(BV32Tree);
+		BV32Tree* bv32Tree = static_cast<BV32Tree*>(data->mGRB_BV32Tree);
+		if (!bv32Tree->load(stream, mismatch))
+		{
+			Ps::getFoundation().error(PxErrorCode::eINTERNAL_ERROR, __FILE__, __LINE__, "BV32 binary image load error.");
+			PX_DELETE(data);
+			return NULL;
+		}
+	}
+
+	return data;
+}
+
+PxTriangleMesh* GuMeshFactory::createTriangleMesh(PxInputStream& desc)
+{	
+	TriangleMeshData* data = ::loadMeshData(desc);
+	if(!data)
+		return NULL;
+	PxTriangleMesh* m = createTriangleMesh(*data);
+	PX_DELETE(data);
+	return m;
+}
+
+bool GuMeshFactory::removeTriangleMesh(PxTriangleMesh& m)
+{
+	Ps::Mutex::ScopedLock lock(mTrackingMutex);
+	TriangleMesh* gu = static_cast<TriangleMesh*>(&m);
+	bool found = mTriangleMeshes.erase(gu);
+	return found;
+}
+
+PxU32 GuMeshFactory::getNbTriangleMeshes() const
+{
+	Ps::Mutex::ScopedLock lock(mTrackingMutex);
+	return mTriangleMeshes.size();
+}
+
+PxU32 GuMeshFactory::getTriangleMeshes(PxTriangleMesh** userBuffer, PxU32 bufferSize, PxU32 startIndex) const
+{
+	Ps::Mutex::ScopedLock lock(mTrackingMutex);
+	return Cm::getArrayOfPointers(userBuffer, bufferSize, startIndex, mTriangleMeshes.getEntries(), mTriangleMeshes.size());
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void GuMeshFactory::addConvexMesh(ConvexMesh* np, bool lock)
+{
+	addToHash(mConvexMeshes, np, lock ? &mTrackingMutex : NULL);
+}
+
+// data injected by cooking lib for runtime cooking
+PxConvexMesh* GuMeshFactory::createConvexMesh(void* data)
+{
+	return createConvexMesh(*reinterpret_cast<Gu::ConvexHullInitData*>(data));
+}
+
+PxConvexMesh* GuMeshFactory::createConvexMesh(Gu::ConvexHullInitData& data)
+{
+	Gu::ConvexMesh *np;
+	PX_NEW_SERIALIZED(np, Gu::ConvexMesh)(*this, data);
+	if (np)
+		addConvexMesh(np);
+
+	return np;
+}
+
+PxConvexMesh* GuMeshFactory::createConvexMesh(PxInputStream& desc)
+{
+	ConvexMesh* np;
+	PX_NEW_SERIALIZED(np, ConvexMesh);
+	if(!np)
+		return NULL;
+
+	np->setMeshFactory(this);
+
+	if(!np->load(desc))
+	{
+		np->decRefCount();
+		return NULL;
+	}
+
+	addConvexMesh(np);
+	return np;
+}
+
+bool GuMeshFactory::removeConvexMesh(PxConvexMesh& m)
+{
+	Ps::Mutex::ScopedLock lock(mTrackingMutex);
+	ConvexMesh* gu = static_cast<ConvexMesh*>(&m);
+	bool found = mConvexMeshes.erase(gu);
+	return found;
+}
+
+PxU32 GuMeshFactory::getNbConvexMeshes() const
+{
+	Ps::Mutex::ScopedLock lock(mTrackingMutex);
+	return mConvexMeshes.size();
+}
+
+PxU32 GuMeshFactory::getConvexMeshes(PxConvexMesh** userBuffer, PxU32 bufferSize, PxU32 startIndex) const
+{
+	Ps::Mutex::ScopedLock lock(mTrackingMutex);
+	return Cm::getArrayOfPointers(userBuffer, bufferSize, startIndex, mConvexMeshes.getEntries(), mConvexMeshes.size());
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void GuMeshFactory::addHeightField(HeightField* np, bool lock)
+{
+	addToHash(mHeightFields, np, lock ? &mTrackingMutex : NULL);
+}
+
+PxHeightField* GuMeshFactory::createHeightField(void* heightFieldMeshData)
+{
+	HeightField* np;
+	PX_NEW_SERIALIZED(np, HeightField)(*this, *reinterpret_cast<Gu::HeightFieldData*>(heightFieldMeshData));
+	if(np)
+		addHeightField(np);
+	
+	return np;
+}
+
+PxHeightField* GuMeshFactory::createHeightField(PxInputStream& stream)
+{
+	HeightField* np;
+	PX_NEW_SERIALIZED(np, HeightField)(this);
+	if(!np)
+		return NULL;
+
+	if(!np->load(stream))
+	{
+		np->decRefCount();
+		return NULL;
+	}
+
+	addHeightField(np);
+	return np;
+}
+
+bool GuMeshFactory::removeHeightField(PxHeightField& hf)
+{
+	Ps::Mutex::ScopedLock lock(mTrackingMutex);
+	HeightField* gu = static_cast<HeightField*>(&hf);
+	bool found = mHeightFields.erase(gu);
+	return found;
+}
+
+PxU32 GuMeshFactory::getNbHeightFields() const
+{
+	Ps::Mutex::ScopedLock lock(mTrackingMutex);
+	return mHeightFields.size();
+}
+
+PxU32 GuMeshFactory::getHeightFields(PxHeightField** userBuffer, PxU32 bufferSize, PxU32 startIndex) const
+{
+	Ps::Mutex::ScopedLock lock(mTrackingMutex);
+	return Cm::getArrayOfPointers(userBuffer, bufferSize, startIndex, mHeightFields.getEntries(), mHeightFields.size());
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void GuMeshFactory::addFactoryListener( GuMeshFactoryListener& listener )
+{
+	Ps::Mutex::ScopedLock lock(mTrackingMutex);
+	mFactoryListeners.pushBack( &listener );
+}
+
+void GuMeshFactory::removeFactoryListener( GuMeshFactoryListener& listener )
+{
+	Ps::Mutex::ScopedLock lock(mTrackingMutex);
+	for ( PxU32 idx = 0; idx < mFactoryListeners.size(); ++idx )
+	{
+		if ( mFactoryListeners[idx] == &listener )
+		{
+			mFactoryListeners.replaceWithLast( idx );
+			--idx;
+		}
+	}
+}
+
+void GuMeshFactory::notifyFactoryListener(const PxBase* base, PxType typeID)
+{
+	const PxU32 nbListeners = mFactoryListeners.size();
+	for(PxU32 i=0; i<nbListeners; i++)
+		mFactoryListeners[i]->onGuMeshFactoryBufferRelease(base, typeID);
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void GuMeshFactory::addBVHStructure(BVHStructure* np, bool lock)
+{
+	addToHash(mBVHStructures, np, lock ? &mTrackingMutex : NULL);
+}
+
+// data injected by cooking lib for runtime cooking
+PxBVHStructure* GuMeshFactory::createBVHStructure(void* data)
+{
+	return createBVHStructure(*reinterpret_cast<Gu::BVHStructureData*>(data));
+}
+
+PxBVHStructure* GuMeshFactory::createBVHStructure(Gu::BVHStructureData& data)
+{
+	Gu::BVHStructure *np;
+	PX_NEW_SERIALIZED(np, Gu::BVHStructure)(this, data);
+	if (np)
+		addBVHStructure(np);
+
+	return np;
+}
+
+PxBVHStructure* GuMeshFactory::createBVHStructure(PxInputStream& desc)
+{
+	BVHStructure* np;
+	PX_NEW_SERIALIZED(np, BVHStructure)(this);
+	if(!np)
+		return NULL;
+
+	if(!np->load(desc))
+	{
+		np->decRefCount();
+		return NULL;
+	}
+
+	addBVHStructure(np);
+	return np;
+}
+
+bool GuMeshFactory::removeBVHStructure(PxBVHStructure& m)
+{
+	Ps::Mutex::ScopedLock lock(mTrackingMutex);
+	BVHStructure* gu = static_cast<BVHStructure*>(&m);
+	bool found = mBVHStructures.erase(gu);
+	return found;
+}
+
+PxU32 GuMeshFactory::getNbBVHStructures() const
+{
+	Ps::Mutex::ScopedLock lock(mTrackingMutex);
+	return mBVHStructures.size();
+}
+
+PxU32 GuMeshFactory::getBVHStructures(PxBVHStructure** userBuffer, PxU32 bufferSize, PxU32 startIndex) const
+{
+	Ps::Mutex::ScopedLock lock(mTrackingMutex);
+	return Cm::getArrayOfPointers(userBuffer, bufferSize, startIndex, mBVHStructures.getEntries(), mBVHStructures.size());
+}
+
+///////////////////////////////////////////////////////////////////////////////
+

physx/source/geomutils/src/GuMeshFactory.h

@@ -0,0 +1,141 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_MESH_FACTORY_H
+#define GU_MESH_FACTORY_H
+
+#include "foundation/PxIO.h"
+#include "geometry/PxTriangleMesh.h"
+#include "geometry/PxConvexMesh.h"
+#include "geometry/PxHeightField.h"
+#include "geometry/PxBVHStructure.h"
+#include "PxPhysXConfig.h"
+
+#include "CmPhysXCommon.h"
+#include "PsMutex.h"
+#include "PsArray.h"
+#include "PsUserAllocated.h"
+#include "PsHashSet.h"
+
+namespace physx
+{
+
+class PxHeightFieldDesc;
+
+namespace Gu
+{
+	class ConvexMesh;
+	class HeightField;
+	class TriangleMesh;
+	class TriangleMeshData;
+	class BVHStructure;
+	struct ConvexHullInitData;
+	struct BVHStructureData;
+}
+
+class GuMeshFactoryListener
+{
+protected:
+	virtual ~GuMeshFactoryListener(){}
+public:
+	virtual void onGuMeshFactoryBufferRelease(const PxBase* object, PxType type) = 0;
+};
+
+#if PX_VC 
+    #pragma warning(push)
+	#pragma warning( disable : 4251 ) // class needs to have dll-interface to be used by clients of class
+#endif
+class PX_PHYSX_COMMON_API GuMeshFactory : public Ps::UserAllocated
+{
+	PX_NOCOPY(GuMeshFactory)
+public:
+									GuMeshFactory();
+protected:
+	virtual							~GuMeshFactory();
+
+public:
+	void							release();
+
+	// Triangle meshes
+	void							addTriangleMesh(Gu::TriangleMesh* np, bool lock=true);
+	PxTriangleMesh*					createTriangleMesh(PxInputStream& stream);
+	PxTriangleMesh*					createTriangleMesh(void* triangleMeshData);
+	bool							removeTriangleMesh(PxTriangleMesh&);
+	PxU32							getNbTriangleMeshes()	const;
+	PxU32							getTriangleMeshes(PxTriangleMesh** userBuffer, PxU32 bufferSize, PxU32 startIndex)	const;
+
+	// Convexes
+	void							addConvexMesh(Gu::ConvexMesh* np, bool lock=true);
+	PxConvexMesh*					createConvexMesh(PxInputStream&);
+	PxConvexMesh*					createConvexMesh(void* convexMeshData);
+	bool							removeConvexMesh(PxConvexMesh&);
+	PxU32							getNbConvexMeshes() const;
+	PxU32							getConvexMeshes(PxConvexMesh** userBuffer, PxU32 bufferSize, PxU32 startIndex)	const;
+
+	// Heightfields
+	void							addHeightField(Gu::HeightField* np, bool lock=true);
+	PxHeightField*					createHeightField(void* heightFieldMeshData);
+	PxHeightField*					createHeightField(PxInputStream&);
+	bool							removeHeightField(PxHeightField&);
+	PxU32							getNbHeightFields()	const;
+	PxU32							getHeightFields(PxHeightField** userBuffer, PxU32 bufferSize, PxU32 startIndex)	const;
+
+	// BVHStructure
+	void							addBVHStructure(Gu::BVHStructure* np, bool lock=true);
+	PxBVHStructure*					createBVHStructure(PxInputStream&);
+	PxBVHStructure*					createBVHStructure(void* bvhData);
+	bool							removeBVHStructure(PxBVHStructure&);
+	PxU32							getNbBVHStructures() const;
+	PxU32							getBVHStructures(PxBVHStructure** userBuffer, PxU32 bufferSize, PxU32 startIndex)	const;
+
+	void							addFactoryListener( GuMeshFactoryListener& listener );
+	void							removeFactoryListener( GuMeshFactoryListener& listener );
+	void							notifyFactoryListener(const PxBase*, PxType typeID);
+
+protected:
+
+	PxTriangleMesh*					createTriangleMesh(Gu::TriangleMeshData& data);
+	PxConvexMesh*					createConvexMesh(Gu::ConvexHullInitData& data);
+	PxBVHStructure*					createBVHStructure(Gu::BVHStructureData& data);
+
+	mutable Ps::Mutex				mTrackingMutex;
+private:
+	Ps::CoalescedHashSet<Gu::TriangleMesh*>		mTriangleMeshes;
+	Ps::CoalescedHashSet<Gu::ConvexMesh*>		mConvexMeshes;
+	Ps::CoalescedHashSet<Gu::HeightField*>		mHeightFields;
+	Ps::CoalescedHashSet<Gu::BVHStructure*>		mBVHStructures;
+
+	Ps::Array<GuMeshFactoryListener*>		mFactoryListeners;
+};
+#if PX_VC 
+     #pragma warning(pop) 
+#endif
+}
+
+#endif

physx/source/geomutils/src/GuMetaData.cpp

@@ -0,0 +1,586 @@
+//
+// 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-2021 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 "foundation/PxIO.h"
+#include "common/PxMetaData.h"
+#include "GuHeightField.h"
+#include "GuConvexMeshData.h"
+#include "GuBigConvexData2.h"
+#include "GuConvexMesh.h"
+#include "GuTriangleMesh.h"
+#include "GuTriangleMeshBV4.h"
+#include "GuTriangleMeshRTree.h"
+#include "GuGeometryUnion.h"
+#include "PsIntrinsics.h"
+#include "CmPhysXCommon.h"
+
+using namespace physx;
+using namespace Ps;
+using namespace Cm;
+using namespace Gu;
+
+///////////////////////////////////////////////////////////////////////////////
+
+static void getBinaryMetaData_Valency(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_CLASS(stream,	Valency)
+	PX_DEF_BIN_METADATA_ITEM(stream,	Valency, PxU16,	mCount,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	Valency, PxU16,	mOffset,	0)
+}
+
+static void getBinaryMetaData_BigConvexRawData(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_CLASS(stream,	BigConvexRawData)
+	PX_DEF_BIN_METADATA_ITEM(stream,	BigConvexRawData, PxU16,	mSubdiv,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	BigConvexRawData, PxU16,	mNbSamples,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	BigConvexRawData, PxU8,		mSamples,		PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	BigConvexRawData, PxU32,	mNbVerts,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	BigConvexRawData, PxU32,	mNbAdjVerts,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	BigConvexRawData, Valency,	mValencies,		PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	BigConvexRawData, PxU8,		mAdjacentVerts,	PxMetaDataFlag::ePTR)
+}
+
+void BigConvexData::getBinaryMetaData(PxOutputStream& stream)
+{
+	getBinaryMetaData_Valency(stream);
+	getBinaryMetaData_BigConvexRawData(stream);
+
+	PX_DEF_BIN_METADATA_CLASS(stream,	BigConvexData)
+	PX_DEF_BIN_METADATA_ITEM(stream,	BigConvexData, BigConvexRawData,	mData,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	BigConvexData, void,				mVBuffer,	PxMetaDataFlag::ePTR)
+
+	//------ Extra-data ------
+
+	// mData.mSamples
+	// PT: can't use one array of PxU16 since we don't want to flip those bytes during conversion.
+	// PT: We only align the first array for DE1340, but the second one shouldn't be aligned since
+	// both are written as one unique block of memory.
+	PX_DEF_BIN_METADATA_EXTRA_ARRAY(stream,	BigConvexData, PxU8, mData.mNbSamples, PX_SERIAL_ALIGN, 0)
+	PX_DEF_BIN_METADATA_EXTRA_ARRAY(stream,	BigConvexData, PxU8, mData.mNbSamples, 0, 0)
+
+	// mData.mValencies
+	// PT: same here, we must only align the first array
+	PX_DEF_BIN_METADATA_EXTRA_ARRAY(stream,	BigConvexData, Valency,	mData.mNbVerts, PX_SERIAL_ALIGN, 0)
+	PX_DEF_BIN_METADATA_EXTRA_ALIGN(stream,	BigConvexData, PX_SERIAL_ALIGN)
+	PX_DEF_BIN_METADATA_EXTRA_ARRAY(stream,	BigConvexData, PxU8,	mData.mNbAdjVerts, 0, 0)
+}
+
+static void getBinaryMetaData_InternalObjectsData(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_CLASS(stream,		InternalObjectsData)
+	PX_DEF_BIN_METADATA_ITEM(stream,		InternalObjectsData,	PxReal,	mRadius,	0)
+	PX_DEF_BIN_METADATA_ITEMS_AUTO(stream,	InternalObjectsData,	PxReal,	mExtents,	0)
+}
+
+static void getBinaryMetaData_HullPolygonData(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_CLASS(stream,		HullPolygonData)
+	PX_DEF_BIN_METADATA_ITEMS_AUTO(stream,	HullPolygonData,	PxReal,	mPlane,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		HullPolygonData,	PxU16,	mVRef8,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		HullPolygonData,	PxU8,	mNbVerts,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		HullPolygonData,	PxU8,	mMinIndex,	0)
+}
+
+static void getBinaryMetaData_ConvexHullData(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_CLASS(stream,	ConvexHullData)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConvexHullData, PxBounds3,				mAABB,				0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConvexHullData, PxVec3,					mCenterOfMass,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConvexHullData, HullPolygonData,		mPolygons,			PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConvexHullData, BigConvexRawData,		mBigConvexRawData,	PxMetaDataFlag::ePTR)
+	//ML: the most significant bit of mNbEdges is used to indicate whether we have grb data or not. However, we don't support grb data
+	//in serialization so we have to mask the most significant bit and force the contact gen run on CPU code path
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConvexHullData, PxU16,					mNbEdges,			PxMetaDataFlag::eCOUNT_MASK_MSB)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConvexHullData, PxU8,					mNbHullVertices,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConvexHullData, PxU8,					mNbPolygons,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConvexHullData, InternalObjectsData,	mInternal,			0)
+}
+
+void Gu::ConvexMesh::getBinaryMetaData(PxOutputStream& stream)
+{
+	getBinaryMetaData_InternalObjectsData(stream);
+	getBinaryMetaData_HullPolygonData(stream);
+	getBinaryMetaData_ConvexHullData(stream);
+	BigConvexData::getBinaryMetaData(stream);
+
+	PX_DEF_BIN_METADATA_VCLASS(stream,ConvexMesh)
+	PX_DEF_BIN_METADATA_BASE_CLASS(stream,ConvexMesh, PxBase)
+	PX_DEF_BIN_METADATA_BASE_CLASS(stream,ConvexMesh, RefCountable)
+
+	//
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConvexMesh, ConvexHullData,	mHullData,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConvexMesh, PxU32,			mNb,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConvexMesh, BigConvexData,	mBigConvexData,	PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConvexMesh, PxReal,			mMass,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConvexMesh, PxMat33,		mInertia,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConvexMesh, GuMeshFactory,	mMeshFactory,	PxMetaDataFlag::ePTR)
+
+	//------ Extra-data ------
+
+	// mHullData.mPolygons (Gu::HullPolygonData, PxVec3, PxU8*2, PxU8)
+	// PT: we only align the first array since the other ones are contained within it
+
+	PX_DEF_BIN_METADATA_EXTRA_ARRAY(stream,	Gu::ConvexMesh, HullPolygonData,	mHullData.mNbPolygons,		PX_SERIAL_ALIGN, 0)
+	PX_DEF_BIN_METADATA_EXTRA_ARRAY(stream,	Gu::ConvexMesh, PxVec3,				mHullData.mNbHullVertices,	0, 0)
+	PX_DEF_BIN_METADATA_EXTRA_ARRAY(stream, Gu::ConvexMesh, PxU8,				mHullData.mNbEdges,			0, PxMetaDataFlag::eCOUNT_MASK_MSB)
+	PX_DEF_BIN_METADATA_EXTRA_ARRAY(stream, Gu::ConvexMesh, PxU8,				mHullData.mNbEdges,			0, PxMetaDataFlag::eCOUNT_MASK_MSB)
+	PX_DEF_BIN_METADATA_EXTRA_ARRAY(stream,	Gu::ConvexMesh, PxU8,			    mHullData.mNbHullVertices,	0, 0)
+	PX_DEF_BIN_METADATA_EXTRA_ARRAY(stream,	Gu::ConvexMesh, PxU8,			    mHullData.mNbHullVertices,	0, 0)
+	PX_DEF_BIN_METADATA_EXTRA_ARRAY(stream,	Gu::ConvexMesh, PxU8,			    mHullData.mNbHullVertices,	0, 0)
+
+	PX_DEF_BIN_METADATA_EXTRA_ARRAY(stream,	Gu::ConvexMesh, PxU8,			    mNb,						0, PxMetaDataFlag::eCOUNT_MASK_MSB)
+	PX_DEF_BIN_METADATA_EXTRA_ALIGN(stream,	ConvexMesh, 4)
+	// mBigConvexData
+	PX_DEF_BIN_METADATA_EXTRA_ITEM(stream, Gu::ConvexMesh, BigConvexData, mBigConvexData, PX_SERIAL_ALIGN)
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+static void getBinaryMetaData_PxHeightFieldSample(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_CLASS(stream,	PxHeightFieldSample)
+	PX_DEF_BIN_METADATA_ITEM(stream,	PxHeightFieldSample,	PxI16,		    height,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	PxHeightFieldSample,	PxBitAndByte,	materialIndex0,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	PxHeightFieldSample,	PxBitAndByte,	materialIndex1,	0)
+	PX_DEF_BIN_METADATA_TYPEDEF(stream,	PxBitAndByte,			PxU8)
+}
+
+static void getBinaryMetaData_HeightFieldData(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_TYPEDEF(stream,	PxHeightFieldFlags, PxU16)
+	PX_DEF_BIN_METADATA_TYPEDEF(stream,	PxHeightFieldFormat::Enum, PxU32)
+
+	PX_DEF_BIN_METADATA_CLASS(stream,	HeightFieldData)
+	PX_DEF_BIN_METADATA_ITEM(stream,	HeightFieldData, PxBounds3,				mAABB,					0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	HeightFieldData, PxU32,					rows,					0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	HeightFieldData, PxU32,					columns,				0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	HeightFieldData, PxReal,				rowLimit,				0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	HeightFieldData, PxReal,				colLimit,				0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	HeightFieldData, PxReal,				nbColumns,				0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	HeightFieldData, PxHeightFieldSample,	samples,				PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	HeightFieldData, PxReal,				convexEdgeThreshold,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	HeightFieldData, PxHeightFieldFlags,	flags,					0)
+#ifdef EXPLICIT_PADDING_METADATA
+	PX_DEF_BIN_METADATA_ITEM(stream,	HeightFieldData, PxU16,					paddAfterFlags,			PxMetaDataFlag::ePADDING)
+#endif
+	PX_DEF_BIN_METADATA_ITEM(stream,	HeightFieldData, PxHeightFieldFormat::Enum,	format,					0)
+}
+
+void Gu::HeightField::getBinaryMetaData(PxOutputStream& stream)
+{
+	getBinaryMetaData_PxHeightFieldSample(stream);
+	getBinaryMetaData_HeightFieldData(stream);
+
+	PX_DEF_BIN_METADATA_TYPEDEF(stream, PxMaterialTableIndex, PxU16)
+
+	PX_DEF_BIN_METADATA_VCLASS(stream,		HeightField)
+	PX_DEF_BIN_METADATA_BASE_CLASS(stream,	HeightField, PxBase)
+	PX_DEF_BIN_METADATA_BASE_CLASS(stream,	HeightField, RefCountable)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,	HeightField, HeightFieldData,	mData,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	HeightField, PxU32,				mSampleStride,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	HeightField, PxU32,				mNbSamples,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	HeightField, PxReal,			mMinHeight,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	HeightField, PxReal,			mMaxHeight,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	HeightField, PxU32,				mModifyCount,	0)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,	HeightField, GuMeshFactory,		mMeshFactory,	PxMetaDataFlag::ePTR)
+
+	//------ Extra-data ------
+
+	// mData.samples
+	PX_DEF_BIN_METADATA_EXTRA_ARRAY(stream,	HeightField, PxHeightFieldSample, mNbSamples, PX_SERIAL_ALIGN, 0)	// PT: ### try to remove mNbSamples later
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+static void getBinaryMetaData_RTreePage(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_CLASS(stream,	RTreePage)
+	PX_DEF_BIN_METADATA_ITEMS(stream,	RTreePage, PxReal,	minx,	0, RTREE_N)
+	PX_DEF_BIN_METADATA_ITEMS(stream,	RTreePage, PxReal,	miny,	0, RTREE_N)
+	PX_DEF_BIN_METADATA_ITEMS(stream,	RTreePage, PxReal,	minz,	0, RTREE_N)
+	PX_DEF_BIN_METADATA_ITEMS(stream,	RTreePage, PxReal,	maxx,	0, RTREE_N)
+	PX_DEF_BIN_METADATA_ITEMS(stream,	RTreePage, PxReal,	maxy,	0, RTREE_N)
+	PX_DEF_BIN_METADATA_ITEMS(stream,	RTreePage, PxReal,	maxz,	0, RTREE_N)
+	PX_DEF_BIN_METADATA_ITEMS(stream,	RTreePage, PxU32,	ptrs,	0, RTREE_N)
+}
+
+void RTree::getBinaryMetaData(PxOutputStream& stream)
+{
+	getBinaryMetaData_RTreePage(stream);
+
+	PX_DEF_BIN_METADATA_CLASS(stream, RTree)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,	RTree, PxVec4,		mBoundsMin,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	RTree, PxVec4,		mBoundsMax,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	RTree, PxVec4,		mInvDiagonal,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	RTree, PxVec4,		mDiagonalScaler,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	RTree, PxU32,		mPageSize,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	RTree, PxU32,		mNumRootPages,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	RTree, PxU32,		mNumLevels,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	RTree, PxU32,		mTotalNodes,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	RTree, PxU32,		mTotalPages,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	RTree, PxU32,		mFlags,				0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	RTree, RTreePage,	mPages,				PxMetaDataFlag::ePTR)
+
+	//------ Extra-data ------
+
+	// mPages
+	PX_DEF_BIN_METADATA_EXTRA_ARRAY(stream,RTree, RTreePage, mTotalPages, 128, 0)
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void SourceMesh::getBinaryMetaData(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_CLASS(stream,	SourceMesh)
+	PX_DEF_BIN_METADATA_ITEM(stream,	SourceMesh, PxU32,	mNbVerts,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	SourceMesh, PxVec3,	mVerts,			PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	SourceMesh, PxU32,	mNbTris,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	SourceMesh, void,	mTriangles32,	PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	SourceMesh, void,	mTriangles16,	PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	SourceMesh, PxU32,	mRemap,			PxMetaDataFlag::ePTR)
+}
+
+static void getBinaryMetaData_BVDataPackedQ(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_CLASS(stream,	QuantizedAABB)
+	PX_DEF_BIN_METADATA_ITEM(stream,	QuantizedAABB, PxU16,	mData[0].mExtents,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	QuantizedAABB, PxI16,	mData[0].mCenter,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	QuantizedAABB, PxU16,	mData[1].mExtents,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	QuantizedAABB, PxI16,	mData[1].mCenter,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	QuantizedAABB, PxU16,	mData[2].mExtents,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	QuantizedAABB, PxI16,	mData[2].mCenter,	0)
+
+	PX_DEF_BIN_METADATA_CLASS(stream,	BVDataPackedQ)
+	PX_DEF_BIN_METADATA_ITEM(stream,	BVDataPackedQ, QuantizedAABB,	mAABB,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	BVDataPackedQ, PxU32,			mData,	0)
+}
+
+#ifdef GU_BV4_COMPILE_NON_QUANTIZED_TREE
+static void getBinaryMetaData_BVDataPackedNQ(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_CLASS(stream,	CenterExtents)
+	PX_DEF_BIN_METADATA_ITEM(stream,	CenterExtents, PxVec3,	mCenter,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	CenterExtents, PxVec3,	mExtents,	0)
+
+	PX_DEF_BIN_METADATA_CLASS(stream,	BVDataPackedNQ)
+	PX_DEF_BIN_METADATA_ITEM(stream,	BVDataPackedNQ, CenterExtents,	mAABB,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	BVDataPackedNQ, PxU32,			mData,	0)
+}
+#endif
+
+void BV4Tree::getBinaryMetaData(PxOutputStream& stream)
+{
+	getBinaryMetaData_BVDataPackedQ(stream);
+#ifdef GU_BV4_COMPILE_NON_QUANTIZED_TREE
+	getBinaryMetaData_BVDataPackedNQ(stream);
+#endif
+	PX_DEF_BIN_METADATA_CLASS(stream,	LocalBounds)
+	PX_DEF_BIN_METADATA_ITEM(stream,	LocalBounds, PxVec3,	mCenter,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	LocalBounds, float,		mExtentsMagnitude,	0)
+
+	PX_DEF_BIN_METADATA_CLASS(stream, BV4Tree)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,	BV4Tree, void,			mMeshInterface,		PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	BV4Tree, LocalBounds,	mLocalBounds,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	BV4Tree, PxU32,			mNbNodes,			0)
+	//PX_DEF_BIN_METADATA_ITEM(stream,	BV4Tree, void,			mNodes,				PxMetaDataFlag::eEXTRA_DATA)
+	PX_DEF_BIN_METADATA_ITEM(stream,	BV4Tree, PxU32,			mInitData,			0)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,	BV4Tree, PxVec3,		mCenterOrMinCoeff,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	BV4Tree, PxVec3,		mExtentsOrMaxCoeff,	0)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,	BV4Tree, bool,			mUserAllocated,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	BV4Tree, bool,			mQuantized,			0)
+	PX_DEF_BIN_METADATA_ITEMS(stream,	BV4Tree, bool,			mPadding,			PxMetaDataFlag::ePADDING, 2)
+
+	//------ Extra-data ------
+
+	PX_DEF_BIN_METADATA_EXTRA_ARRAY(stream,	BV4Tree, BVDataPackedQ, mNbNodes, 16, 0)
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void Gu::TriangleMesh::getBinaryMetaData(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_VCLASS(stream,		TriangleMesh)
+	PX_DEF_BIN_METADATA_BASE_CLASS(stream,	TriangleMesh, PxBase)
+	PX_DEF_BIN_METADATA_BASE_CLASS(stream,	TriangleMesh, RefCountable)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,	TriangleMesh, PxU32,			mNbVertices,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	TriangleMesh, PxU32,			mNbTriangles,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	TriangleMesh, PxVec3,			mVertices,				PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	TriangleMesh, void,				mTriangles,				PxMetaDataFlag::ePTR)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,	TriangleMesh, PxBounds3,		mAABB,					0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	TriangleMesh, PxU8,				mExtraTrigData,			PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	TriangleMesh, PxReal,			mGeomEpsilon,			0)	
+
+	PX_DEF_BIN_METADATA_ITEM(stream,	TriangleMesh, PxU8,				mFlags,					0)	
+	PX_DEF_BIN_METADATA_ITEM(stream,	TriangleMesh, PxU16,			mMaterialIndices,		PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	TriangleMesh, PxU32,			mFaceRemap,				PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	TriangleMesh, PxU32,			mAdjacencies,			PxMetaDataFlag::ePTR)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,	TriangleMesh, void,				mGRB_triIndices,		PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	TriangleMesh, void,				mGRB_triAdjacencies,	PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	TriangleMesh, PxU32,			mGRB_faceRemap,			PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	TriangleMesh, void,				mGRB_BV32Tree,			PxMetaDataFlag::ePTR)
+
+
+	//------ Extra-data ------
+
+	// mVertices
+	PX_DEF_BIN_METADATA_EXTRA_ITEMS(stream, TriangleMesh, PxVec3, mVertices, mNbVertices, 0, PX_SERIAL_ALIGN)
+
+	// mTriangles
+	// PT: quite tricky here: we exported either an array of PxU16s or an array of PxU32s. We trick the converter by
+	// pretending we exported both, with the same control variable (m16BitIndices) but opposed control flags. Also there's
+	// no way to capture "mNumTriangles*3" using the macros, so we just pretend we exported 3 buffers instead of 1.
+	// But since in reality it's all the same buffer, only the first one is declared as aligned.
+
+	PX_DEF_BIN_METADATA_EXTRA_ITEMS_MASKED_CONTROL(stream,	TriangleMesh, PxU16, mFlags, PxTriangleMeshFlag::e16_BIT_INDICES, mNbTriangles, 0, PX_SERIAL_ALIGN)
+	PX_DEF_BIN_METADATA_EXTRA_ITEMS_MASKED_CONTROL(stream,	TriangleMesh, PxU16, mFlags, PxTriangleMeshFlag::e16_BIT_INDICES, mNbTriangles, 0, 0)
+	PX_DEF_BIN_METADATA_EXTRA_ITEMS_MASKED_CONTROL(stream,	TriangleMesh, PxU16, mFlags, PxTriangleMeshFlag::e16_BIT_INDICES, mNbTriangles, 0, 0)
+	PX_DEF_BIN_METADATA_EXTRA_ITEMS_MASKED_CONTROL(stream,	TriangleMesh, PxU32, mFlags, PxTriangleMeshFlag::e16_BIT_INDICES, mNbTriangles, PxMetaDataFlag::eCONTROL_FLIP, PX_SERIAL_ALIGN)
+	PX_DEF_BIN_METADATA_EXTRA_ITEMS_MASKED_CONTROL(stream,	TriangleMesh, PxU32, mFlags, PxTriangleMeshFlag::e16_BIT_INDICES, mNbTriangles, PxMetaDataFlag::eCONTROL_FLIP, 0)
+	PX_DEF_BIN_METADATA_EXTRA_ITEMS_MASKED_CONTROL(stream,	TriangleMesh, PxU32, mFlags, PxTriangleMeshFlag::e16_BIT_INDICES, mNbTriangles, PxMetaDataFlag::eCONTROL_FLIP, 0)
+
+	// mExtraTrigData
+	PX_DEF_BIN_METADATA_EXTRA_ITEMS(stream, TriangleMesh, PxU8, mExtraTrigData, mNbTriangles, 0, PX_SERIAL_ALIGN)
+
+	// mMaterialIndices
+	PX_DEF_BIN_METADATA_EXTRA_ITEMS(stream, TriangleMesh, PxU16, mMaterialIndices, mNbTriangles, 0, PX_SERIAL_ALIGN)
+
+	// mFaceRemap
+	PX_DEF_BIN_METADATA_EXTRA_ITEMS(stream, TriangleMesh, PxU32, mFaceRemap, mNbTriangles, 0, PX_SERIAL_ALIGN)
+
+	// mAdjacencies
+	PX_DEF_BIN_METADATA_EXTRA_ITEMS(stream, TriangleMesh, PxU32, mAdjacencies, mNbTriangles, 0, PX_SERIAL_ALIGN)
+	PX_DEF_BIN_METADATA_EXTRA_ITEMS(stream, TriangleMesh, PxU32, mAdjacencies, mNbTriangles, 0, 0)
+	PX_DEF_BIN_METADATA_EXTRA_ITEMS(stream, TriangleMesh, PxU32, mAdjacencies, mNbTriangles, 0, 0)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,		TriangleMesh, GuMeshFactory,		mMeshFactory,				PxMetaDataFlag::ePTR)
+
+
+#ifdef EXPLICIT_PADDING_METADATA
+	PX_DEF_BIN_METADATA_ITEMS_AUTO(stream,	TriangleMesh, PxU32,				mPaddingFromInternalMesh,	PxMetaDataFlag::ePADDING)
+#endif
+}
+
+void Gu::RTreeTriangleMesh::getBinaryMetaData(PxOutputStream& stream)
+{
+	RTree::getBinaryMetaData(stream);
+
+	PX_DEF_BIN_METADATA_VCLASS(stream,		RTreeTriangleMesh)
+	PX_DEF_BIN_METADATA_BASE_CLASS(stream,	RTreeTriangleMesh, TriangleMesh)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,		RTreeTriangleMesh, RTree,			mRTree,					0)	
+}
+
+void Gu::BV4TriangleMesh::getBinaryMetaData(PxOutputStream& stream)
+{
+	SourceMesh::getBinaryMetaData(stream);
+	BV4Tree::getBinaryMetaData(stream);
+
+	PX_DEF_BIN_METADATA_VCLASS(stream,		BV4TriangleMesh)
+	PX_DEF_BIN_METADATA_BASE_CLASS(stream,	BV4TriangleMesh, TriangleMesh)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,		BV4TriangleMesh, SourceMesh,		mMeshInterface,			0)	
+	PX_DEF_BIN_METADATA_ITEM(stream,		BV4TriangleMesh, BV4Tree,			mBV4Tree,				0)	
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void MaterialIndicesStruct::getBinaryMetaData(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_CLASS(stream,	MaterialIndicesStruct)
+	PX_DEF_BIN_METADATA_ITEM(stream,	MaterialIndicesStruct, PxU16,	indices,	PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	MaterialIndicesStruct, PxU16,	numIndices,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	MaterialIndicesStruct, PxU16,	pad,		PxMetaDataFlag::ePADDING)
+#if PX_P64_FAMILY
+	PX_DEF_BIN_METADATA_ITEM(stream,	MaterialIndicesStruct, PxU32,	pad64,		PxMetaDataFlag::ePADDING)
+#endif
+
+	//------ Extra-data ------
+	// indices
+	PX_DEF_BIN_METADATA_EXTRA_ITEMS(stream, MaterialIndicesStruct, PxU16, indices, numIndices, PxMetaDataFlag::eHANDLE, PX_SERIAL_ALIGN)
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void Gu::GeometryUnion::getBinaryMetaData(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_TYPEDEF(stream, PxGeometryType::Enum, PxU32)
+
+	// The various PxGeometry classes are all public, so I can't really put the meta-data function in there. And then
+	// I can't access their protected members. So we use the same trick as for the ShapeContainer
+	class ShadowConvexMeshGeometry : public PxConvexMeshGeometryLL
+	{
+	public:
+		static void getBinaryMetaData(PxOutputStream& stream_)
+		{
+			PX_DEF_BIN_METADATA_TYPEDEF(stream_, PxConvexMeshGeometryFlags, PxU8)
+
+			PX_DEF_BIN_METADATA_CLASS(stream_,	ShadowConvexMeshGeometry)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowConvexMeshGeometry, PxGeometryType::Enum,			mType,				0)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowConvexMeshGeometry, PxMeshScale,					scale,				0)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowConvexMeshGeometry, PxConvexMesh,					convexMesh,			PxMetaDataFlag::ePTR)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowConvexMeshGeometry, PxConvexMeshGeometryFlags,	meshFlags,			0)
+			PX_DEF_BIN_METADATA_ITEMS(stream_,	ShadowConvexMeshGeometry, PxU8,							paddingFromFlags,	PxMetaDataFlag::ePADDING, 3)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowConvexMeshGeometry, ConvexHullData,				hullData,			PxMetaDataFlag::ePTR)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowConvexMeshGeometry, bool,							gpuCompatible,		0)
+		}
+	};
+	ShadowConvexMeshGeometry::getBinaryMetaData(stream);
+	PX_DEF_BIN_METADATA_TYPEDEF(stream, PxConvexMeshGeometryLL, ShadowConvexMeshGeometry)
+
+	/////////////////
+
+	class ShadowTriangleMeshGeometry : public PxTriangleMeshGeometryLL
+	{
+	public:
+		static void getBinaryMetaData(PxOutputStream& stream_)
+		{
+			PX_DEF_BIN_METADATA_TYPEDEF(stream_, PxMeshGeometryFlags, PxU8)
+
+			PX_DEF_BIN_METADATA_CLASS(stream_,	ShadowTriangleMeshGeometry)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowTriangleMeshGeometry, PxGeometryType::Enum,		mType,				0)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowTriangleMeshGeometry, PxMeshScale,				scale,				0)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowTriangleMeshGeometry, PxMeshGeometryFlags,		meshFlags,			0)
+			PX_DEF_BIN_METADATA_ITEMS(stream_,	ShadowTriangleMeshGeometry,	PxU8,						paddingFromFlags,	PxMetaDataFlag::ePADDING, 3)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowTriangleMeshGeometry, PxTriangleMesh,				triangleMesh,		PxMetaDataFlag::ePTR)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowTriangleMeshGeometry, TriangleMesh,				meshData,			PxMetaDataFlag::ePTR)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowTriangleMeshGeometry, PxU16,						materialIndices,	PxMetaDataFlag::ePTR)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowTriangleMeshGeometry, MaterialIndicesStruct,		materials,			0)
+		}
+	};
+	ShadowTriangleMeshGeometry::getBinaryMetaData(stream);
+	PX_DEF_BIN_METADATA_TYPEDEF(stream,PxTriangleMeshGeometryLL, ShadowTriangleMeshGeometry)
+
+	/////////////////
+
+	class ShadowHeightFieldGeometry : public PxHeightFieldGeometryLL
+	{
+	public:
+		static void getBinaryMetaData(PxOutputStream& stream_)
+		{
+			PX_DEF_BIN_METADATA_CLASS(stream_,		ShadowHeightFieldGeometry)
+			PX_DEF_BIN_METADATA_ITEM(stream_,		ShadowHeightFieldGeometry, PxGeometryType::Enum,	mType,					0)
+			PX_DEF_BIN_METADATA_ITEM(stream_,		ShadowHeightFieldGeometry, PxHeightField,		    heightField,			PxMetaDataFlag::ePTR)
+			PX_DEF_BIN_METADATA_ITEM(stream_,		ShadowHeightFieldGeometry, PxReal,					heightScale,			0)
+			PX_DEF_BIN_METADATA_ITEM(stream_,		ShadowHeightFieldGeometry, PxReal,					rowScale,				0)
+			PX_DEF_BIN_METADATA_ITEM(stream_,		ShadowHeightFieldGeometry, PxReal,					columnScale,			0)
+			PX_DEF_BIN_METADATA_ITEM(stream_,		ShadowHeightFieldGeometry, PxMeshGeometryFlags,		heightFieldFlags,		0)
+			PX_DEF_BIN_METADATA_ITEMS_AUTO(stream_,	ShadowHeightFieldGeometry, PxU8,					paddingFromFlags,		PxMetaDataFlag::ePADDING)
+			PX_DEF_BIN_METADATA_ITEM(stream_,		ShadowHeightFieldGeometry, HeightField,				heightFieldData,		PxMetaDataFlag::ePTR)
+			PX_DEF_BIN_METADATA_ITEM(stream_,		ShadowHeightFieldGeometry, MaterialIndicesStruct,	materials,				0)
+		}
+	};
+	ShadowHeightFieldGeometry::getBinaryMetaData(stream);
+	PX_DEF_BIN_METADATA_TYPEDEF(stream,PxHeightFieldGeometryLL, ShadowHeightFieldGeometry)
+
+	/////////////////
+
+	class ShadowPlaneGeometry : public PxPlaneGeometry
+	{
+	public:
+		static void getBinaryMetaData(PxOutputStream& stream_)
+		{
+			PX_DEF_BIN_METADATA_CLASS(stream_,	ShadowPlaneGeometry)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowPlaneGeometry, PxGeometryType::Enum,	mType,		0)
+		}
+	};
+	ShadowPlaneGeometry::getBinaryMetaData(stream);
+	PX_DEF_BIN_METADATA_TYPEDEF(stream,PxPlaneGeometry, ShadowPlaneGeometry)
+
+	/////////////////
+
+	class ShadowSphereGeometry : public PxSphereGeometry
+	{
+	public:
+		static void getBinaryMetaData(PxOutputStream& stream_)
+		{
+			PX_DEF_BIN_METADATA_CLASS(stream_,	ShadowSphereGeometry)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowSphereGeometry, PxGeometryType::Enum,		mType,		0)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowSphereGeometry, PxReal,				    radius,		0)
+		}
+	};
+	ShadowSphereGeometry::getBinaryMetaData(stream);
+	PX_DEF_BIN_METADATA_TYPEDEF(stream, PxSphereGeometry, ShadowSphereGeometry)
+
+	/////////////////
+
+	class ShadowCapsuleGeometry : public PxCapsuleGeometry
+	{
+	public:
+		static void getBinaryMetaData(PxOutputStream& stream_)
+		{
+			PX_DEF_BIN_METADATA_CLASS(stream_,	ShadowCapsuleGeometry)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowCapsuleGeometry, PxGeometryType::Enum,	mType,		0)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowCapsuleGeometry, PxReal,					radius,		0)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowCapsuleGeometry, PxReal,					halfHeight,	0)
+		}
+	};
+	ShadowCapsuleGeometry::getBinaryMetaData(stream);
+	PX_DEF_BIN_METADATA_TYPEDEF(stream, PxCapsuleGeometry, ShadowCapsuleGeometry)
+
+	/////////////////
+
+	class ShadowBoxGeometry : public PxBoxGeometry
+	{
+	public:
+		static void getBinaryMetaData(PxOutputStream& stream_)
+		{
+			PX_DEF_BIN_METADATA_CLASS(stream_,	ShadowBoxGeometry)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowBoxGeometry, PxGeometryType::Enum,	mType,		0)
+			PX_DEF_BIN_METADATA_ITEM(stream_,	ShadowBoxGeometry, PxVec3,					halfExtents,0)
+		}
+	};
+	ShadowBoxGeometry::getBinaryMetaData(stream);
+	PX_DEF_BIN_METADATA_TYPEDEF(stream, PxBoxGeometry, ShadowBoxGeometry)
+
+	/*
+	- geom union offset & size
+	- control type offset & size
+	- type-to-class mapping
+	*/
+
+	PX_DEF_BIN_METADATA_CLASS(stream, Gu::GeometryUnion)
+
+	PX_DEF_BIN_METADATA_UNION(stream,		Gu::GeometryUnion, mGeometry)
+	PX_DEF_BIN_METADATA_UNION_TYPE(stream,	Gu::GeometryUnion, PxSphereGeometry,		PxGeometryType::eSPHERE)
+	PX_DEF_BIN_METADATA_UNION_TYPE(stream, 	Gu::GeometryUnion, PxPlaneGeometry,			PxGeometryType::ePLANE)
+	PX_DEF_BIN_METADATA_UNION_TYPE(stream, 	Gu::GeometryUnion, PxCapsuleGeometry,		PxGeometryType::eCAPSULE)
+	PX_DEF_BIN_METADATA_UNION_TYPE(stream, 	Gu::GeometryUnion, PxBoxGeometry,			PxGeometryType::eBOX)
+	PX_DEF_BIN_METADATA_UNION_TYPE(stream, 	Gu::GeometryUnion, PxConvexMeshGeometryLL,	PxGeometryType::eCONVEXMESH)
+	PX_DEF_BIN_METADATA_UNION_TYPE(stream, 	Gu::GeometryUnion, PxTriangleMeshGeometryLL,PxGeometryType::eTRIANGLEMESH)
+	PX_DEF_BIN_METADATA_UNION_TYPE(stream, 	Gu::GeometryUnion, PxHeightFieldGeometryLL,	PxGeometryType::eHEIGHTFIELD)
+}

physx/source/geomutils/src/GuOverlapTests.cpp

@@ -0,0 +1,695 @@
+//
+// 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-2021 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 "GuOverlapTests.h"
+#include "GuIntersectionBoxBox.h"
+#include "GuIntersectionSphereBox.h"
+#include "GuDistancePointSegment.h"
+#include "GuDistanceSegmentBox.h"
+#include "GuDistanceSegmentSegment.h"
+#include "GuSphere.h"
+#include "GuBoxConversion.h"
+#include "GuInternal.h"
+#include "GuVecCapsule.h"
+#include "GuVecConvexHull.h"
+#include "GuVecBox.h"
+#include "GuConvexMesh.h"
+#include "GuHillClimbing.h"
+#include "GuGJK.h"
+
+using namespace physx;
+using namespace Cm;
+using namespace Gu;
+
+// PT: TODO: why don't we use ShapeData for overlaps?
+
+//returns the maximal vertex in shape space
+// PT: this function should be removed. We already have 2 different project hull functions in PxcShapeConvex & GuGJKObjectSupport, this one looks like a weird mix of both!
+static PxVec3 projectHull_(	const ConvexHullData& hull,
+							float& minimum, float& maximum,
+							const PxVec3& localDir, // expected to be normalized
+							const PxMat33& vert2ShapeSkew)
+{
+	PX_ASSERT(localDir.isNormalized());
+
+	//use property that x|My == Mx|y for symmetric M to avoid having to transform vertices.
+	const PxVec3 vertexSpaceDir = vert2ShapeSkew * localDir;
+
+	const PxVec3* Verts = hull.getHullVertices();
+	const PxVec3* bestVert = NULL;
+
+	if(!hull.mBigConvexRawData)	// Brute-force, local space. Experiments show break-even point is around 32 verts.
+	{
+		PxU32 NbVerts = hull.mNbHullVertices;
+		float min_ = PX_MAX_F32;
+		float max_ = -PX_MAX_F32;
+		while(NbVerts--)
+		{
+			const float dp = (*Verts).dot(vertexSpaceDir);
+			min_ = physx::intrinsics::selectMin(min_, dp);
+			if(dp > max_)	{ max_ = dp; bestVert = Verts; }
+
+			Verts++;
+		}
+		minimum = min_;
+		maximum = max_;
+
+		PX_ASSERT(bestVert != NULL);
+
+		return vert2ShapeSkew * *bestVert;
+	}
+	else //*/if(1)	// This version is better for objects with a lot of vertices
+	{
+		const PxU32 Offset = ComputeCubemapNearestOffset(vertexSpaceDir, hull.mBigConvexRawData->mSubdiv);
+		PxU32 MinID = hull.mBigConvexRawData->mSamples[Offset];
+		PxU32 MaxID = hull.mBigConvexRawData->getSamples2()[Offset];
+
+		localSearch(MinID, -vertexSpaceDir, Verts, hull.mBigConvexRawData);
+		localSearch(MaxID, vertexSpaceDir, Verts, hull.mBigConvexRawData);
+
+		minimum = (Verts[MinID].dot(vertexSpaceDir));
+		maximum = (Verts[MaxID].dot(vertexSpaceDir));
+
+		PX_ASSERT(maximum >= minimum);
+
+		return vert2ShapeSkew * Verts[MaxID];
+	}
+}
+
+static bool intersectSphereConvex(const PxTransform& sphereTransform, float radius, const ConvexMesh& mesh, const PxMeshScale& meshScale, const PxTransform& convexGlobalPose,
+						   PxVec3*)
+{
+	using namespace Ps::aos;
+	const Vec3V zeroV = V3Zero();
+	const ConvexHullData* hullData = &mesh.getHull();
+	const FloatV sphereRadius = FLoad(radius);
+	const Vec3V vScale	= V3LoadU_SafeReadW(meshScale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
+	const QuatV vQuat = QuatVLoadU(&meshScale.rotation.x);
+
+	const PsMatTransformV aToB(convexGlobalPose.transformInv(sphereTransform));
+	ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, meshScale.isIdentity());
+	CapsuleV capsule(aToB.p, sphereRadius);
+
+	Vec3V contactA, contactB, normal;
+	FloatV dist;
+	LocalConvex<CapsuleV> convexA(capsule);
+	LocalConvex<ConvexHullV> convexB(convexHull);
+	const Vec3V initialSearchDir = V3Sub(capsule.getCenter(), convexHull.getCenter());
+
+	GjkStatus status = gjk(convexA, convexB, initialSearchDir, FZero(), contactA, contactB, normal, dist);
+
+	return status == GJK_CONTACT;
+}
+
+static bool intersectCapsuleConvex(	const PxCapsuleGeometry& capsGeom, const PxTransform& capsGlobalPose,
+									const ConvexMesh& mesh, const PxMeshScale& meshScale, const PxTransform& convexGlobalPose,
+									PxVec3*)
+{
+	using namespace Ps::aos;
+
+	const Vec3V zeroV = V3Zero();
+	const ConvexHullData* hull = &mesh.getHull();
+
+	const FloatV capsuleHalfHeight = FLoad(capsGeom.halfHeight);
+	const FloatV capsuleRadius = FLoad(capsGeom.radius);
+
+	const Vec3V vScale	= V3LoadU_SafeReadW(meshScale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
+	const QuatV vQuat = QuatVLoadU(&meshScale.rotation.x);
+
+	const PsMatTransformV aToB(convexGlobalPose.transformInv(capsGlobalPose));
+
+	ConvexHullV convexHull(hull, zeroV, vScale, vQuat, meshScale.isIdentity());
+	CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius);
+
+	Vec3V contactA, contactB, normal;
+	FloatV dist;
+	LocalConvex<CapsuleV> convexA(capsule);
+	LocalConvex<ConvexHullV> convexB(convexHull);
+	const Vec3V initialSearchDir = V3Sub(capsule.getCenter(), convexHull.getCenter());
+	
+	GjkStatus  status = gjk(convexA, convexB, initialSearchDir, FZero(), contactA, contactB, normal, dist);
+
+	return status == GJK_CONTACT;
+}
+
+static bool intersectBoxConvex(const PxBoxGeometry& boxGeom, const PxTransform& boxGlobalPose,
+								const ConvexMesh& mesh, const PxMeshScale& meshScale, const PxTransform& convexGlobalPose,
+								PxVec3*)
+{
+	// AP: see archived non-GJK version in //sw/physx/dev/pterdiman/graveyard/contactConvexBox.cpp
+	using namespace Ps::aos;
+	const Vec3V zeroV = V3Zero();
+	const ConvexHullData* hull = &mesh.getHull();
+
+	const Vec3V vScale	= V3LoadU_SafeReadW(meshScale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
+	const QuatV vQuat = QuatVLoadU(&meshScale.rotation.x);
+	const Vec3V boxExtents = V3LoadU(boxGeom.halfExtents);
+	const PsMatTransformV aToB(convexGlobalPose.transformInv(boxGlobalPose));
+
+	ConvexHullV convexHull(hull, zeroV, vScale, vQuat, meshScale.isIdentity());
+	BoxV box(zeroV, boxExtents);
+
+	Vec3V contactA, contactB, normal;
+	FloatV dist;
+	RelativeConvex<BoxV> convexA(box, aToB);
+	LocalConvex<ConvexHullV> convexB(convexHull);
+
+	GjkStatus status = gjk(convexA, convexB, aToB.p, FZero(), contactA, contactB, normal, dist);
+
+	//PX_PRINTF("BOX status = %i, overlap = %i, PxVec3(%f, %f, %f)\n", status, overlap, boxGlobalPose.p.x, boxGlobalPose.p.y, boxGlobalPose.p.z);
+
+	return status == GJK_CONTACT;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+static PX_FORCE_INLINE PxVec3* getCachedAxis(TriggerCache* cache)
+{
+	if(cache && cache->state==TRIGGER_OVERLAP)
+		return &cache->dir;
+	else
+		return NULL;
+}
+
+static PX_FORCE_INLINE bool updateTriggerCache(bool overlap, TriggerCache* cache)
+{
+	if(cache)
+	{
+		if(overlap)
+			cache->state = TRIGGER_OVERLAP;
+		else
+			cache->state = TRIGGER_DISJOINT;
+	}
+	return overlap;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Sphere-vs-shape
+
+static bool GeomOverlapCallback_SphereSphere(GU_OVERLAP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom0.getType()==PxGeometryType::eSPHERE);
+	PX_ASSERT(geom1.getType()==PxGeometryType::eSPHERE);
+	PX_UNUSED(cache);
+
+	const PxSphereGeometry& sphereGeom0 = static_cast<const PxSphereGeometry&>(geom0);
+	const PxSphereGeometry& sphereGeom1 = static_cast<const PxSphereGeometry&>(geom1);
+
+	const PxVec3 delta = pose1.p - pose0.p;
+	const PxReal r = sphereGeom0.radius + sphereGeom1.radius;
+	return delta.magnitudeSquared() <= r*r;	// PT: objects are defined as closed, so we return 'true' in case of equality
+}
+
+static bool GeomOverlapCallback_SpherePlane(GU_OVERLAP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom0.getType()==PxGeometryType::eSPHERE);
+	PX_ASSERT(geom1.getType()==PxGeometryType::ePLANE);
+	PX_UNUSED(cache);
+	PX_UNUSED(geom1);
+
+	const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom0);
+
+	return getPlane(pose1).distance(pose0.p) <= sphereGeom.radius;	// PT: objects are defined as closed, so we return 'true' in case of equality
+}
+
+static bool GeomOverlapCallback_SphereCapsule(GU_OVERLAP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom0.getType()==PxGeometryType::eSPHERE);
+	PX_ASSERT(geom1.getType()==PxGeometryType::eCAPSULE);
+	PX_UNUSED(cache);
+
+	const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom0);
+	const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom1);
+
+	// PT: TODO: remove this useless conversion
+	const PxVec3 capsuleHalfHeightVector = getCapsuleHalfHeightVector(pose1, capsuleGeom);
+	const PxReal r = sphereGeom.radius + capsuleGeom.radius;
+
+	return distancePointSegmentSquared(capsuleHalfHeightVector, -capsuleHalfHeightVector, pose0.p - pose1.p) <= r*r;	// PT: objects are defined as closed, so we return 'true' in case of equality
+}
+
+static bool GeomOverlapCallback_SphereBox(GU_OVERLAP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom0.getType()==PxGeometryType::eSPHERE);
+	PX_ASSERT(geom1.getType()==PxGeometryType::eBOX);
+	PX_UNUSED(cache);
+
+	const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom0);
+	const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom1);
+
+	// PT: TODO: remove this useless conversion
+	Box obb;
+	buildFrom(obb, pose1.p, boxGeom.halfExtents, pose1.q);
+
+	return intersectSphereBox(Sphere(pose0.p, sphereGeom.radius), obb);
+}
+
+static bool GeomOverlapCallback_SphereConvex(GU_OVERLAP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom0.getType()==PxGeometryType::eSPHERE);
+	PX_ASSERT(geom1.getType()==PxGeometryType::eCONVEXMESH);
+
+	const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom0);
+	const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom1);
+
+	ConvexMesh* cm = static_cast<ConvexMesh*>(convexGeom.convexMesh);
+
+	PxVec3 cachedSepAxis;
+	PxVec3* tmp = getCachedAxis(cache);
+	if(tmp)
+		cachedSepAxis = *tmp;
+	else
+		cachedSepAxis = PxVec3(0,0,1.f);
+
+	const bool overlap = intersectSphereConvex(pose0, sphereGeom.radius,
+		*cm,
+		convexGeom.scale, pose1,
+		&cachedSepAxis);
+
+	if(cache && overlap)
+		cache->dir = cachedSepAxis;
+
+	return updateTriggerCache(overlap, cache);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Plane-vs-shape
+
+static bool GeomOverlapCallback_PlaneCapsule(GU_OVERLAP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom0.getType()==PxGeometryType::ePLANE);
+	PX_ASSERT(geom1.getType()==PxGeometryType::eCAPSULE);
+	PX_UNUSED(cache);
+	PX_UNUSED(geom0);
+
+//	const PxPlaneGeometry& planeGeom = static_cast<const PxPlaneGeometry&>(geom0);
+	const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom1);
+
+	// PT: TODO: remove this useless conversion
+	Capsule capsule;
+	getCapsule(capsule, capsuleGeom, pose1);
+
+	const PxPlane plane = getPlane(pose0);
+
+	// We handle the capsule-plane collision with 2 sphere-plane collisions.
+	// Seems ok so far, since plane is infinite.
+
+	if(plane.distance(capsule.p0) <= capsule.radius)	// PT: objects are defined as closed, so we return 'true' in case of equality
+		return true;
+
+	if(plane.distance(capsule.p1) <= capsule.radius)	// PT: objects are defined as closed, so we return 'true' in case of equality
+		return true;
+
+	return false;
+}
+
+/*static bool intersectPlaneBox(const PxPlane& plane, const Box& box)
+{
+	PxVec3 pts[8];
+	box.computeBoxPoints(pts);
+
+	for(PxU32 i=0;i<8;i++)
+	{
+		if(plane.distance(pts[i]) <= 0.0f)	// PT: objects are defined as closed, so we return 'true' in case of equality
+			return true;
+	}
+	return false;
+}*/
+
+static bool GeomOverlapCallback_PlaneBox(GU_OVERLAP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom0.getType()==PxGeometryType::ePLANE);
+	PX_ASSERT(geom1.getType()==PxGeometryType::eBOX);
+	PX_UNUSED(cache);
+	PX_UNUSED(geom0);
+
+//	const PxPlaneGeometry& planeGeom = static_cast<const PxPlaneGeometry&>(geom0);
+	const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom1);
+
+	// I currently use the same code as for contact generation but maybe we could do something faster (in theory testing
+	// only 2 pts is enough).
+
+	const Matrix34 absPose(pose1);
+	const PxPlane worldPlane = getPlane(pose0);
+
+	for(int vx=-1; vx<=1; vx+=2)
+		for(int vy=-1; vy<=1; vy+=2)
+			for(int vz=-1; vz<=1; vz+=2)
+			{
+				const PxVec3 v = absPose.transform(PxVec3(PxReal(vx),PxReal(vy),PxReal(vz)).multiply(boxGeom.halfExtents));
+
+				if(worldPlane.distance(v) <= 0.0f)	// PT: objects are defined as closed, so we return 'true' in case of equality
+					return true;
+			}
+	return false;
+}
+
+static bool GeomOverlapCallback_PlaneConvex(GU_OVERLAP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom0.getType()==PxGeometryType::ePLANE);
+	PX_ASSERT(geom1.getType()==PxGeometryType::eCONVEXMESH);
+	PX_UNUSED(cache);
+	PX_UNUSED(geom0);
+
+//	const PxPlaneGeometry& planeGeom = static_cast<const PxPlaneGeometry&>(geom0);
+	const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom1);
+
+	ConvexMesh* cm = static_cast<ConvexMesh*>(convexGeom.convexMesh);
+	
+	//find plane normal in shape space of convex:
+	const PxTransform plane2convex = pose1.getInverse().transform(pose0);
+
+	const PxPlane shapeSpacePlane = getPlane(plane2convex);
+
+	PxReal minimum, maximum;
+	projectHull_(cm->getHull(), minimum, maximum, shapeSpacePlane.n, convexGeom.scale.toMat33());
+
+	return (minimum <= -shapeSpacePlane.d);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Capsule-vs-shape
+
+static bool GeomOverlapCallback_CapsuleCapsule(GU_OVERLAP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom0.getType()==PxGeometryType::eCAPSULE);
+	PX_ASSERT(geom1.getType()==PxGeometryType::eCAPSULE);
+	PX_UNUSED(cache);
+
+	const PxCapsuleGeometry& capsuleGeom0 = static_cast<const PxCapsuleGeometry&>(geom0);
+	const PxCapsuleGeometry& capsuleGeom1 = static_cast<const PxCapsuleGeometry&>(geom1);
+
+	// PT: move computation to local space for improved accuracy
+	const PxVec3 delta = pose1.p - pose0.p;
+
+	// PT: TODO: remove this useless conversion
+	const PxVec3 capsuleHalfHeightVector0 = getCapsuleHalfHeightVector(pose0, capsuleGeom0);
+	const PxVec3 capsuleHalfHeightVector1 = getCapsuleHalfHeightVector(pose1, capsuleGeom1);
+
+	const PxReal squareDist = distanceSegmentSegmentSquared(-capsuleHalfHeightVector0, capsuleHalfHeightVector0*2.0f,
+															delta-capsuleHalfHeightVector1, capsuleHalfHeightVector1*2.0f);
+	const PxReal r = capsuleGeom0.radius + capsuleGeom1.radius;
+	return squareDist <= r*r;	// PT: objects are defined as closed, so we return 'true' in case of equality
+}
+
+static bool GeomOverlapCallback_CapsuleBox(GU_OVERLAP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom0.getType()==PxGeometryType::eCAPSULE);
+	PX_ASSERT(geom1.getType()==PxGeometryType::eBOX);
+	PX_UNUSED(cache);
+
+	const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom0);
+	const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom1);
+
+	// PT: move computation to local space for improved accuracy
+	const PxVec3 delta = pose1.p - pose0.p;
+
+	// PT: TODO: remove this useless conversion
+	const PxVec3 capsuleHalfHeightVector = getCapsuleHalfHeightVector(pose0, capsuleGeom);
+
+	// PT: TODO: remove this useless conversion
+	const PxMat33 obbRot(pose1.q);
+
+	// PT: objects are defined as closed, so we return 'true' in case of equality
+	return distanceSegmentBoxSquared(capsuleHalfHeightVector, -capsuleHalfHeightVector, delta, boxGeom.halfExtents, obbRot) <= capsuleGeom.radius*capsuleGeom.radius;
+}
+
+static bool GeomOverlapCallback_CapsuleConvex(GU_OVERLAP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom0.getType()==PxGeometryType::eCAPSULE);
+	PX_ASSERT(geom1.getType()==PxGeometryType::eCONVEXMESH);
+
+	const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom0);
+	const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom1);
+
+	ConvexMesh* cm = static_cast<ConvexMesh*>(convexGeom.convexMesh);
+
+	
+	PxVec3 cachedSepAxis;
+	PxVec3* tmp = getCachedAxis(cache);
+	if(tmp)
+		cachedSepAxis = *tmp;
+	else
+		cachedSepAxis = PxVec3(0,0,1.0f);
+
+	const bool overlap = intersectCapsuleConvex(capsuleGeom, pose0, *cm, convexGeom.scale, pose1, &cachedSepAxis);
+
+	if(cache && overlap)
+		cache->dir = cachedSepAxis;
+
+	return updateTriggerCache(overlap, cache);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Box-vs-shape
+
+static bool GeomOverlapCallback_BoxBox(GU_OVERLAP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom0.getType()==PxGeometryType::eBOX);
+	PX_ASSERT(geom1.getType()==PxGeometryType::eBOX);
+	PX_UNUSED(cache);
+
+	const PxBoxGeometry& boxGeom0 = static_cast<const PxBoxGeometry&>(geom0);
+	const PxBoxGeometry& boxGeom1 = static_cast<const PxBoxGeometry&>(geom1);
+
+	// PT: TODO: remove this useless conversion
+	return intersectOBBOBB(	boxGeom0.halfExtents, pose0.p, PxMat33Padded(pose0.q),
+							boxGeom1.halfExtents, pose1.p, PxMat33Padded(pose1.q), true);
+}
+
+static bool GeomOverlapCallback_BoxConvex(GU_OVERLAP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom0.getType()==PxGeometryType::eBOX);
+	PX_ASSERT(geom1.getType()==PxGeometryType::eCONVEXMESH);
+
+	const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom0);
+	const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom1);
+
+	ConvexMesh* cm = static_cast<ConvexMesh*>(convexGeom.convexMesh);
+
+	PxVec3 cachedSepAxis;
+	PxVec3* tmp = getCachedAxis(cache);
+	if(tmp)
+		cachedSepAxis = *tmp;
+	else
+		cachedSepAxis = PxVec3(0.0f, 0.0f, 1.0f);
+
+	const bool overlap = intersectBoxConvex(boxGeom, pose0, *cm, convexGeom.scale, pose1, &cachedSepAxis);
+
+	if(cache && overlap)
+		cache->dir = cachedSepAxis;
+
+	return updateTriggerCache(overlap, cache);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Convex-vs-shape
+static bool GeomOverlapCallback_ConvexConvex(GU_OVERLAP_FUNC_PARAMS)
+{
+	using namespace Ps::aos;
+	PX_ASSERT(geom0.getType()==PxGeometryType::eCONVEXMESH);
+	PX_ASSERT(geom1.getType()==PxGeometryType::eCONVEXMESH);
+
+	const Vec3V zeroV = V3Zero();
+	const PxConvexMeshGeometry& convexGeom0 = static_cast<const PxConvexMeshGeometry&>(geom0);
+	const PxConvexMeshGeometry& convexGeom1 = static_cast<const PxConvexMeshGeometry&>(geom1);
+	const ConvexMesh* cm0 = static_cast<ConvexMesh*>(convexGeom0.convexMesh);	
+	const ConvexMesh* cm1 = static_cast<ConvexMesh*>(convexGeom1.convexMesh);	
+
+	bool overlap;
+	{
+		const ConvexHullData* hullData0 = &cm0->getHull();
+		const ConvexHullData* hullData1 = &cm1->getHull();
+
+		const Vec3V vScale0 = V3LoadU_SafeReadW(convexGeom0.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
+		const QuatV vQuat0 = QuatVLoadU(&convexGeom0.scale.rotation.x);
+		const Vec3V vScale1 = V3LoadU_SafeReadW(convexGeom1.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
+		const QuatV vQuat1 = QuatVLoadU(&convexGeom1.scale.rotation.x);
+
+		const QuatV q0 = QuatVLoadU(&pose0.q.x);
+		const Vec3V p0 = V3LoadU(&pose0.p.x);
+
+		const QuatV q1 = QuatVLoadU(&pose1.q.x);
+		const Vec3V p1 = V3LoadU(&pose1.p.x);
+
+		const PsTransformV transf0(p0, q0);
+		const PsTransformV transf1(p1, q1);
+
+		const PsMatTransformV aToB(transf1.transformInv(transf0));
+
+		ConvexHullV convexHull0(hullData0, zeroV, vScale0, vQuat0, convexGeom0.scale.isIdentity());
+		ConvexHullV convexHull1(hullData1, zeroV, vScale1, vQuat1, convexGeom1.scale.isIdentity());
+
+		Vec3V contactA, contactB, normal;
+		FloatV dist;
+		RelativeConvex<ConvexHullV> convexA(convexHull0, aToB);
+		LocalConvex<ConvexHullV> convexB(convexHull1);
+		
+		GjkStatus status = gjk(convexA, convexB, aToB.p,  FZero(), contactA, contactB, normal, dist);
+		overlap = (status == GJK_CONTACT);
+	}
+
+	return updateTriggerCache(overlap, cache);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+static bool GeomOverlapCallback_NotSupported(GU_OVERLAP_FUNC_PARAMS)
+{
+	PX_ALWAYS_ASSERT_MESSAGE("NOT SUPPORTED");
+	PX_UNUSED(cache);
+	PX_UNUSED(pose0);
+	PX_UNUSED(pose1);
+	PX_UNUSED(geom0);
+	PX_UNUSED(geom1);
+	return false;
+}
+
+static bool GeomOverlapCallback_HeightfieldUnregistered(GU_OVERLAP_FUNC_PARAMS)
+{
+	PX_UNUSED(cache);
+	PX_UNUSED(geom0);
+	PX_UNUSED(geom1);
+	PX_UNUSED(pose0);
+	PX_UNUSED(pose1);
+	Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, "Height Field Overlap test called with height fields unregistered ");
+	return false;
+}
+
+bool GeomOverlapCallback_SphereMesh			(GU_OVERLAP_FUNC_PARAMS);
+bool GeomOverlapCallback_CapsuleMesh		(GU_OVERLAP_FUNC_PARAMS);
+bool GeomOverlapCallback_BoxMesh			(GU_OVERLAP_FUNC_PARAMS);
+bool GeomOverlapCallback_ConvexMesh			(GU_OVERLAP_FUNC_PARAMS);
+bool GeomOverlapCallback_SphereHeightfield	(GU_OVERLAP_FUNC_PARAMS);
+bool GeomOverlapCallback_CapsuleHeightfield	(GU_OVERLAP_FUNC_PARAMS);
+bool GeomOverlapCallback_BoxHeightfield		(GU_OVERLAP_FUNC_PARAMS);
+bool GeomOverlapCallback_ConvexHeightfield	(GU_OVERLAP_FUNC_PARAMS);
+
+GeomOverlapTable gGeomOverlapMethodTable[] = 
+{
+	//PxGeometryType::eSPHERE
+	{
+		GeomOverlapCallback_SphereSphere,		//PxGeometryType::eSPHERE
+		GeomOverlapCallback_SpherePlane,		//PxGeometryType::ePLANE
+		GeomOverlapCallback_SphereCapsule,		//PxGeometryType::eCAPSULE
+		GeomOverlapCallback_SphereBox,			//PxGeometryType::eBOX
+		GeomOverlapCallback_SphereConvex,		//PxGeometryType::eCONVEXMESH
+		GeomOverlapCallback_SphereMesh,			//PxGeometryType::eTRIANGLEMESH
+		GeomOverlapCallback_HeightfieldUnregistered,	//PxGeometryType::eHEIGHTFIELD
+	},
+
+	//PxGeometryType::ePLANE
+	{
+		0,										//PxGeometryType::eSPHERE
+		GeomOverlapCallback_NotSupported,		//PxGeometryType::ePLANE
+		GeomOverlapCallback_PlaneCapsule,		//PxGeometryType::eCAPSULE
+		GeomOverlapCallback_PlaneBox,			//PxGeometryType::eBOX
+		GeomOverlapCallback_PlaneConvex,		//PxGeometryType::eCONVEXMESH
+		GeomOverlapCallback_NotSupported,		//PxGeometryType::eTRIANGLEMESH
+		GeomOverlapCallback_NotSupported,		//PxGeometryType::eHEIGHTFIELD
+	},
+
+	//PxGeometryType::eCAPSULE
+	{
+		0,										//PxGeometryType::eSPHERE
+		0,										//PxGeometryType::ePLANE
+		GeomOverlapCallback_CapsuleCapsule,		//PxGeometryType::eCAPSULE
+		GeomOverlapCallback_CapsuleBox,			//PxGeometryType::eBOX
+		GeomOverlapCallback_CapsuleConvex,		//PxGeometryType::eCONVEXMESH
+		GeomOverlapCallback_CapsuleMesh,		//PxGeometryType::eTRIANGLEMESH
+		GeomOverlapCallback_HeightfieldUnregistered,	//PxGeometryType::eHEIGHTFIELD
+	},
+
+	//PxGeometryType::eBOX
+	{
+		0,										//PxGeometryType::eSPHERE
+		0,										//PxGeometryType::ePLANE
+		0,										//PxGeometryType::eCAPSULE
+		GeomOverlapCallback_BoxBox,				//PxGeometryType::eBOX
+		GeomOverlapCallback_BoxConvex,			//PxGeometryType::eCONVEXMESH
+		GeomOverlapCallback_BoxMesh,			//PxGeometryType::eTRIANGLEMESH
+		GeomOverlapCallback_HeightfieldUnregistered,		//PxGeometryType::eHEIGHTFIELD
+	},
+
+	//PxGeometryType::eCONVEXMESH
+	{
+		0,										//PxGeometryType::eSPHERE
+		0,										//PxGeometryType::ePLANE
+		0,										//PxGeometryType::eCAPSULE
+		0,										//PxGeometryType::eBOX
+		GeomOverlapCallback_ConvexConvex,		//PxGeometryType::eCONVEXMESH
+		GeomOverlapCallback_ConvexMesh,			//PxGeometryType::eTRIANGLEMESH		//not used: mesh always uses swept method for midphase.
+		GeomOverlapCallback_HeightfieldUnregistered,	//PxGeometryType::eHEIGHTFIELD		//TODO: make HF midphase that will mask this
+	},
+
+	//PxGeometryType::eTRIANGLEMESH
+	{
+		0,										//PxGeometryType::eSPHERE
+		0,										//PxGeometryType::ePLANE
+		0,										//PxGeometryType::eCAPSULE
+		0,										//PxGeometryType::eBOX
+		0,										//PxGeometryType::eCONVEXMESH
+		GeomOverlapCallback_NotSupported,		//PxGeometryType::eTRIANGLEMESH
+		GeomOverlapCallback_NotSupported,		//PxGeometryType::eHEIGHTFIELD
+	},
+
+	//PxGeometryType::eHEIGHTFIELD
+	{
+		0,										//PxGeometryType::eSPHERE
+		0,										//PxGeometryType::ePLANE
+		0,										//PxGeometryType::eCAPSULE
+		0,										//PxGeometryType::eBOX
+		0,										//PxGeometryType::eCONVEXMESH
+		0,										//PxGeometryType::eTRIANGLEMESH
+		GeomOverlapCallback_NotSupported,		//PxGeometryType::eHEIGHTFIELD
+	},
+};
+
+const GeomOverlapTable* Gu::getOverlapFuncTable()
+{
+	return gGeomOverlapMethodTable;
+}
+
+void registerHeightFields_Raycasts();
+void registerHeightFields_Sweeps();
+void Gu::registerHeightFields()
+{
+	registerHeightFields_Raycasts();
+	registerHeightFields_Sweeps();
+
+	gGeomOverlapMethodTable[PxGeometryType::eSPHERE][PxGeometryType::eHEIGHTFIELD] = GeomOverlapCallback_SphereHeightfield;
+	gGeomOverlapMethodTable[PxGeometryType::eCAPSULE][PxGeometryType::eHEIGHTFIELD] = GeomOverlapCallback_CapsuleHeightfield;
+	gGeomOverlapMethodTable[PxGeometryType::eBOX][PxGeometryType::eHEIGHTFIELD] = GeomOverlapCallback_BoxHeightfield;
+	gGeomOverlapMethodTable[PxGeometryType::eCONVEXMESH][PxGeometryType::eHEIGHTFIELD] = GeomOverlapCallback_ConvexHeightfield;
+}

physx/source/geomutils/src/GuOverlapTests.h

@@ -0,0 +1,117 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_OVERLAP_TESTS_H
+#define GU_OVERLAP_TESTS_H
+
+#include "foundation/PxVec3.h"
+#include "foundation/PxTransform.h"
+#include "foundation/PxAssert.h"
+#include "geometry/PxGeometry.h"
+#include "CmPhysXCommon.h"
+#include "PsFoundation.h"
+
+namespace physx
+{
+namespace Gu
+{
+	class Capsule;
+	class Sphere;
+
+	PX_PHYSX_COMMON_API bool checkOverlapAABB_triangleGeom		(const PxGeometry& triGeom,	const PxTransform& pose, const PxBounds3& box);
+	PX_PHYSX_COMMON_API bool checkOverlapAABB_heightFieldGeom	(const PxGeometry& hfGeom,	const PxTransform& pose, const PxBounds3& box);
+
+	// PT: this is just a shadow of what it used to be. We currently don't use TRIGGER_INSIDE anymore, but I leave it for now,
+	// since I really want to put this back the way it was before.
+	enum TriggerStatus
+	{
+		TRIGGER_DISJOINT,
+		TRIGGER_INSIDE,
+		TRIGGER_OVERLAP
+	};
+
+	// PT: currently only used for convex triggers
+	struct TriggerCache
+	{
+		PxVec3	dir;
+		PxU16	state;
+		PxU16	gjkState; //gjk succeed or fail
+	};
+
+	// PT: we use a define to be able to quickly change the signature of all overlap functions.
+	// (this also ensures they all use consistent names for passed parameters).
+	// \param[in]	geom0	first geometry object
+	// \param[in]	pose0	pose of first geometry object
+	// \param[in]	geom1	second geometry object
+	// \param[in]	pose1	pose of second geometry object
+	// \param[in]	cache	optional cached data for triggers
+	#define GU_OVERLAP_FUNC_PARAMS	const PxGeometry& geom0, const PxTransform& pose0,	\
+									const PxGeometry& geom1, const PxTransform& pose1,	\
+									Gu::TriggerCache* cache
+
+	// PT: function pointer for Geom-indexed overlap functions
+	// See GU_OVERLAP_FUNC_PARAMS for function parameters details.
+	// \return		true if an overlap was found, false otherwise
+	typedef bool (*GeomOverlapFunc)	(GU_OVERLAP_FUNC_PARAMS);
+
+	// PT: typedef for a bundle of all overlap functions, i.e. the function table itself (indexed by geom-type).
+	typedef GeomOverlapFunc GeomOverlapTable[PxGeometryType::eGEOMETRY_COUNT];
+
+	// PT: retrieves the overlap function table (for access by external non-Gu modules)
+	PX_PHYSX_COMMON_API const GeomOverlapTable* getOverlapFuncTable();
+
+	// dynamic registration of height fields
+	PX_PHYSX_COMMON_API void registerHeightFields();
+
+	PX_FORCE_INLINE bool overlap(	const PxGeometry& geom0, const PxTransform& pose0,
+									const PxGeometry& geom1, const PxTransform& pose1,
+									const GeomOverlapTable* PX_RESTRICT overlapFuncs)
+	{
+		PX_CHECK_AND_RETURN_VAL(pose0.isValid(), "Gu::overlap(): pose0 is not valid.", false);
+		PX_CHECK_AND_RETURN_VAL(pose1.isValid(), "Gu::overlap(): pose1 is not valid.", false);
+		
+		if(geom0.getType() > geom1.getType())
+		{
+			GeomOverlapFunc overlapFunc = overlapFuncs[geom1.getType()][geom0.getType()];
+			PX_ASSERT(overlapFunc);
+			return overlapFunc(geom1, pose1, geom0, pose0, NULL);
+		}
+		else
+		{
+			GeomOverlapFunc overlapFunc = overlapFuncs[geom0.getType()][geom1.getType()];
+			PX_ASSERT(overlapFunc);
+			return overlapFunc(geom0, pose0, geom1, pose1, NULL);
+		}
+	}
+
+}  // namespace Gu
+
+}
+
+#endif

physx/source/geomutils/src/GuRaycastTests.cpp

@@ -0,0 +1,564 @@
+//
+// 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-2021 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/PxSphereGeometry.h"
+#include "geometry/PxConvexMeshGeometry.h"
+#include "GuMidphaseInterface.h"
+#include "GuInternal.h"
+#include "GuIntersectionRayCapsule.h"
+#include "GuIntersectionRaySphere.h"
+#include "GuIntersectionRayPlane.h"
+#include "GuHeightFieldUtil.h"
+#include "GuDistancePointSegment.h"
+#include "GuConvexMesh.h"
+#include "CmScaling.h"
+
+using namespace physx;
+using namespace Gu;
+
+////////////////////////////////////////////////// raycasts //////////////////////////////////////////////////////////////////
+PxU32 raycast_box(GU_RAY_FUNC_PARAMS)
+{
+	PX_UNUSED(maxHits);
+	PX_ASSERT(geom.getType() == PxGeometryType::eBOX);
+	PX_ASSERT(maxHits && hits);
+	const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom);
+
+	const PxTransform& absPose = pose;
+
+	PxVec3 localOrigin = rayOrigin - absPose.p;
+	localOrigin = absPose.q.rotateInv(localOrigin);
+
+	const PxVec3 localDir = absPose.q.rotateInv(rayDir);
+
+	PxVec3 localImpact;
+	PxReal t;
+	PxU32 rval = rayAABBIntersect2(-boxGeom.halfExtents, boxGeom.halfExtents, localOrigin, localDir, localImpact, t);
+	if(!rval)
+		return 0;
+
+	if(t>maxDist)
+		return 0;
+
+	hits->distance	= t; //worldRay.orig.distance(hit.worldImpact);	//should be the same, assuming ray dir was normalized!!
+	hits->faceIndex	= 0xffffffff;
+	hits->u			= 0.0f;
+	hits->v			= 0.0f;
+
+	PxHitFlags outFlags = PxHitFlags(0);
+	if((hitFlags & PxHitFlag::ePOSITION))
+	{
+		outFlags |= PxHitFlag::ePOSITION;
+		if(t!=0.0f)
+			hits->position = absPose.transform(localImpact);
+		else
+			hits->position = rayOrigin;
+	}
+
+	// Compute additional information if needed
+	if(hitFlags & PxHitFlag::eNORMAL)
+	{
+		outFlags |= PxHitFlag::eNORMAL;
+
+		//Because rayAABBIntersect2 set t = 0 if start point inside shape
+		if(t == 0)
+		{
+			hits->normal = -rayDir;
+		}
+		else
+		{
+			//local space normal is:
+			rval--;
+			PxVec3 n(0.0f);
+			n[rval] = PxReal((localImpact[rval] > 0.0f) ? 1.0f : -1.0f);
+			hits->normal = absPose.q.rotate(n);
+		}
+	}
+	else
+	{
+		hits->normal = PxVec3(0.0f);
+	}
+	hits->flags	= outFlags;
+	return 1;
+}
+
+PxU32 raycast_sphere(GU_RAY_FUNC_PARAMS)
+{
+	PX_UNUSED(maxHits);
+	PX_ASSERT(geom.getType() == PxGeometryType::eSPHERE);
+	PX_ASSERT(maxHits && hits);
+
+	const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom);
+
+	if(!intersectRaySphere(rayOrigin, rayDir, maxDist, pose.p, sphereGeom.radius, hits->distance, &hits->position))
+		return 0;
+
+	/*	// PT: should be useless now
+	hit.distance	= worldRay.orig.distance(hit.worldImpact);
+	if(hit.distance>maxDist)
+	return false;
+	*/
+	// PT: we can't avoid computing the position here since it's needed to compute the normal anyway
+	hits->faceIndex	= 0xffffffff;
+	hits->u			= 0.0f;
+	hits->v			= 0.0f;
+
+	// Compute additional information if needed
+	PxHitFlags outFlags = PxHitFlag::ePOSITION;
+	if(hitFlags & PxHitFlag::eNORMAL)
+	{
+		// User requested impact normal
+		//Because intersectRaySphere set distance = 0 if start point inside shape
+		if(hits->distance == 0.0f)
+		{
+			hits->normal = -rayDir;
+		}
+		else
+		{
+			hits->normal = hits->position - pose.p;
+			hits->normal.normalize();
+		}
+		outFlags |= PxHitFlag::eNORMAL;
+	}
+	else
+	{
+		hits->normal = PxVec3(0.0f);
+	}
+	hits->flags = outFlags;
+
+	return 1;
+}
+
+PxU32 raycast_capsule(GU_RAY_FUNC_PARAMS)
+{
+	PX_UNUSED(maxHits);
+	PX_ASSERT(geom.getType() == PxGeometryType::eCAPSULE);
+	PX_ASSERT(maxHits && hits);
+
+	const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom);
+
+	// TODO: PT: could we simplify this ?
+	Capsule capsule;
+	getCapsuleSegment(pose, capsuleGeom, capsule);
+	capsule.radius = capsuleGeom.radius;
+
+	PxReal t = 0.0f;
+	if(!intersectRayCapsule(rayOrigin, rayDir, capsule, t))
+		return 0;
+
+	if(t<0.0f || t>maxDist)
+		return 0;
+
+	// PT: we can't avoid computing the position here since it's needed to compute the normal anyway
+	hits->position	= rayOrigin + rayDir*t;	// PT: will be rayOrigin for t=0.0f (i.e. what the spec wants)
+	hits->distance	= t;
+	hits->faceIndex	= 0xffffffff;
+	hits->u			= 0.0f;
+	hits->v			= 0.0f;
+
+	// Compute additional information if needed
+	PxHitFlags outFlags = PxHitFlag::ePOSITION;
+	if(hitFlags & PxHitFlag::eNORMAL)
+	{
+		outFlags |= PxHitFlag::eNORMAL;
+
+		if(t==0.0f)
+		{
+			hits->normal = -rayDir;
+		}
+		else
+		{
+			PxReal capsuleT;
+			distancePointSegmentSquared(capsule, hits->position, &capsuleT);
+			capsule.computePoint(hits->normal, capsuleT);
+			hits->normal = hits->position - hits->normal;	 //this should never be zero. It should have a magnitude of the capsule radius.
+			hits->normal.normalize();
+		}
+	}
+	else
+	{
+		hits->normal = PxVec3(0.0f);
+	}
+	hits->flags = outFlags;
+
+	return 1;
+}
+
+PxU32 raycast_plane(GU_RAY_FUNC_PARAMS)
+{
+	PX_UNUSED(hitFlags);
+	PX_UNUSED(maxHits);
+	PX_ASSERT(geom.getType() == PxGeometryType::ePLANE);
+	PX_ASSERT(maxHits && hits);
+	PX_UNUSED(geom);
+//	const PxPlaneGeometry& planeGeom = static_cast<const PxPlaneGeometry&>(geom);
+
+	// Perform backface culling so that we can pick objects beyond planes
+	const PxPlane plane = getPlane(pose);
+	if(rayDir.dot(plane.n)>=0.0f)
+		return false;
+
+	PxReal distanceAlongLine;
+	if(!intersectRayPlane(rayOrigin, rayDir, plane, distanceAlongLine, &hits->position))
+		return 0;
+
+	/*
+	PxReal test = worldRay.orig.distance(hit.worldImpact);
+
+	PxReal dd;
+	PxVec3 pp;
+	PxSegmentPlaneIntersect(worldRay.orig, worldRay.orig+worldRay.dir*1000.0f, plane, dd, pp);
+	*/
+
+	if(distanceAlongLine<0.0f)
+		return 0;
+
+	if(distanceAlongLine>maxDist)
+		return 0;
+
+	hits->distance	= distanceAlongLine;
+	hits->faceIndex	= 0xffffffff;
+	hits->u			= 0.0f;
+	hits->v			= 0.0f;
+	hits->flags		= PxHitFlag::ePOSITION|PxHitFlag::eNORMAL;
+	hits->normal	= plane.n;
+	return 1;
+}
+
+PxU32 raycast_convexMesh(GU_RAY_FUNC_PARAMS)
+{ 
+	PX_UNUSED(maxHits);
+	PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);
+	PX_ASSERT(maxHits && hits);
+	PX_ASSERT(PxAbs(rayDir.magnitudeSquared()-1)<1e-4f);
+
+	const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom);
+
+	ConvexMesh* convexMesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
+
+	PxRaycastHit& hit = *hits;
+	
+	//scaling: transform the ray to vertex space
+	const Cm::Matrix34 world2vertexSkew = convexGeom.scale.getInverse() * pose.getInverse();	
+
+	//ConvexMesh* cmesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
+	const PxU32 nPolys = convexMesh->getNbPolygonsFast();
+	const HullPolygonData* PX_RESTRICT polysEA = convexMesh->getPolygons();
+	const HullPolygonData* polys = polysEA;
+
+	const PxVec3 vrayOrig = world2vertexSkew.transform(rayOrigin);
+	const PxVec3 vrayDir = world2vertexSkew.rotate(rayDir);
+
+	/*
+	Purely convex planes based algorithm
+	Iterate all planes of convex, with following rules:
+	* determine of ray origin is inside them all or not.  
+	* planes parallel to ray direction are immediate early out if we're on the outside side (plane normal is sep axis)
+	* else 
+		- for all planes the ray direction "enters" from the front side, track the one furthest along the ray direction (A)
+		- for all planes the ray direction "exits" from the back side, track the one furthest along the negative ray direction (B)
+	if the ray origin is outside the convex and if along the ray, A comes before B, the directed line stabs the convex at A
+	*/
+	bool originInsideAllPlanes = true;
+	PxReal latestEntry = -FLT_MAX;
+	PxReal earliestExit = FLT_MAX;
+//	PxU32 bestPolygonIndex = 0;
+	hit.faceIndex	= 0xffffffff;
+
+	for(PxU32 i=0;i<nPolys;i++)
+	{
+		const HullPolygonData& poly = polys[i];
+		const PxPlane& vertSpacePlane = poly.mPlane;
+
+		const PxReal distToPlane = vertSpacePlane.distance(vrayOrig);
+		const PxReal dn = vertSpacePlane.n.dot(vrayDir);
+		const PxReal distAlongRay = -distToPlane/dn;	// PT: TODO: potential divide by zero here!
+
+		// PT: TODO: this is computed again in the last branch!
+		if(distToPlane > 0.0f)
+			originInsideAllPlanes = false;	//origin not behind plane == ray starts outside the convex.
+
+		if(dn > 1E-7f)	//the ray direction "exits" from the back side
+		{
+			earliestExit = physx::intrinsics::selectMin(earliestExit, distAlongRay);
+		}
+		else if(dn < -1E-7f)	//the ray direction "enters" from the front side
+		{
+			if(distAlongRay > latestEntry)
+			{
+				latestEntry = distAlongRay;
+				hit.faceIndex = i;
+			}
+		}
+		else
+		{
+			//plane normal and ray dir are orthogonal
+			if(distToPlane > 0.0f)	
+				return 0;	//a plane is parallel with ray -- and we're outside the ray -- we definitely miss the entire convex!
+		}
+	}
+
+	if(originInsideAllPlanes)	//ray starts inside convex
+	{
+		hit.distance	= 0.0f;
+		hit.faceIndex	= 0xffffffff;
+		hit.u			= 0.0f;
+		hit.v			= 0.0f;
+		hit.position	= rayOrigin;
+		hit.normal		= -rayDir;
+		hit.flags		= PxHitFlag::eNORMAL|PxHitFlag::ePOSITION;
+		return 1;
+	}
+
+	// AP: changed to latestEntry < maxDist-1e-5f so that we have a conservatively negative result near end of ray
+	if(latestEntry < earliestExit && latestEntry > 0.0f && latestEntry < maxDist-1e-5f)
+	{
+		PxHitFlags outFlags = PxHitFlag::eFACE_INDEX;
+		if(hitFlags & PxHitFlag::ePOSITION)
+		{
+			outFlags |= PxHitFlag::ePOSITION;
+			const PxVec3 pointOnPlane = vrayOrig + latestEntry * vrayDir;
+			hit.position = pose.transform(convexGeom.scale.toMat33() * pointOnPlane);
+		}
+		hit.distance	= latestEntry;
+		hit.u			= 0.0f;
+		hit.v			= 0.0f;
+		hit.normal		= PxVec3(0.0f);
+
+		// Compute additional information if needed
+		if(hitFlags & PxHitFlag::eNORMAL)
+		{
+			outFlags |= PxHitFlag::eNORMAL;
+			//when we have nonuniform scaling we actually have to transform by the transpose of the inverse of vertex2worldSkew.M == transpose of world2vertexSkew:
+			hit.normal = world2vertexSkew.rotateTranspose(polys[hit.faceIndex].mPlane.n);
+			hit.normal.normalize();
+		}
+		hit.flags = outFlags;
+		return 1;
+	}
+	return 0;
+}
+
+PxU32 raycast_triangleMesh(GU_RAY_FUNC_PARAMS) 
+{
+	PX_ASSERT(geom.getType() == PxGeometryType::eTRIANGLEMESH);
+	PX_ASSERT(PxAbs(rayDir.magnitudeSquared()-1)<1e-4f);
+
+	const PxTriangleMeshGeometry& meshGeom = static_cast<const PxTriangleMeshGeometry&>(geom);
+
+	TriangleMesh* meshData = static_cast<TriangleMesh*>(meshGeom.triangleMesh);
+
+	return Midphase::raycastTriangleMesh(meshData, meshGeom, pose, rayOrigin, rayDir, maxDist, hitFlags, maxHits, hits);
+}
+
+namespace
+{
+	struct HFTraceSegmentCallback 
+	{
+		PX_NOCOPY(HFTraceSegmentCallback)
+	public:
+		PxRaycastHit*			mHits;
+		const PxU32				mMaxHits;
+		PxU32					mNbHits;
+		const HeightFieldUtil&	mUtil;
+		const PxTransform&		mPose;
+		const PxVec3&			mRayDir;
+		const PxVec3&			mLocalRayDir;
+		const PxVec3&			mLocalRayOrig;		
+		const PxHitFlags		mHitFlags;
+		const bool				mIsDoubleSided;
+
+		HFTraceSegmentCallback(	PxRaycastHit* hits, PxU32 maxHits, const PxHitFlags hitFlags, const HeightFieldUtil& hfUtil, const PxTransform& pose,
+								const PxVec3& rayDir, const PxVec3& localRayDir, const PxVec3& localRayOrig,
+								bool isDoubleSided) :
+			mHits			(hits),
+			mMaxHits		(maxHits),
+			mNbHits			(0),
+			mUtil			(hfUtil),
+			mPose			(pose),
+			mRayDir			(rayDir),
+			mLocalRayDir	(localRayDir),
+			mLocalRayOrig	(localRayOrig),
+			mHitFlags		(hitFlags),
+			mIsDoubleSided	(isDoubleSided)
+		{
+			PX_ASSERT(maxHits > 0);
+		}
+
+		PX_FORCE_INLINE bool onEvent(PxU32 , PxU32*)
+		{
+			return true;
+		}
+
+		PX_FORCE_INLINE bool underFaceHit(const HeightFieldUtil&, const PxVec3&, const PxVec3&, PxF32, PxF32, PxF32, PxU32)
+		{
+			return true;	// true means continue traversal
+		}
+
+		PxAgain faceHit(const HeightFieldUtil&, const PxVec3& aHitPoint, PxU32 aTriangleIndex, PxReal u, PxReal v)
+		{
+			// traversal is strictly sorted so there's no need to sort hits
+			if(mNbHits >= mMaxHits)
+				return false; // false = stop traversal
+
+			PxRaycastHit& hit = mHits[mNbHits++];
+			hit.position	= aHitPoint;
+			hit.faceIndex	= aTriangleIndex;
+			hit.u			= u;
+			hit.v			= v;
+			hit.flags		= PxHitFlag::eUV | PxHitFlag::eFACE_INDEX; // UVs and face index are always set
+
+			if(mHitFlags & PxHitFlag::eNORMAL)
+			{
+				// We need the normal for the dot product.
+				PxVec3 normal = mPose.q.rotate(mUtil.getNormalAtShapePoint(hit.position.x, hit.position.z)); 
+				normal.normalize();
+				if(mIsDoubleSided && normal.dot(mRayDir) > 0.0f) // comply with normal spec for double sided (should always face opposite rayDir)
+					hit.normal = -normal;
+				else
+					hit.normal = normal;
+				hit.flags |= PxHitFlag::eNORMAL;
+			}
+
+			hit.distance = physx::intrinsics::selectMax(0.f, (hit.position - mLocalRayOrig).dot(mLocalRayDir));
+
+			if(mHitFlags & PxHitFlag::ePOSITION)
+			{
+				hit.position = mPose.transform(hit.position);
+				hit.flags |= PxHitFlag::ePOSITION;
+			}
+			return (mNbHits < mMaxHits); // true = continue traversal, false = stop traversal
+		}
+	};   
+}
+
+PxU32 raycast_heightField(GU_RAY_FUNC_PARAMS)
+{
+	PX_ASSERT(geom.getType() == PxGeometryType::eHEIGHTFIELD);
+	PX_ASSERT(maxHits && hits);
+	PX_UNUSED(maxHits);
+
+	const PxHeightFieldGeometry& hfGeom = static_cast<const PxHeightFieldGeometry&>(geom);
+
+	const PxTransform invAbsPose = pose.getInverse();
+	const PxVec3 localRayOrig = invAbsPose.transform(rayOrigin);
+	const PxVec3 localRayDir = invAbsPose.rotate(rayDir);
+
+	const bool isDoubleSided = hfGeom.heightFieldFlags.isSet(PxMeshGeometryFlag::eDOUBLE_SIDED);
+	const bool bothSides = isDoubleSided || (hitFlags & PxHitFlag::eMESH_BOTH_SIDES);
+
+	const HeightFieldTraceUtil hfUtil(hfGeom);
+
+	PxVec3 normRayDir = localRayDir;
+	normRayDir.normalizeSafe(); // nothing will happen if length is < PX_NORMALIZATION_EPSILON
+	
+	// pretest if we intersect HF bounds. If no early exit, if yes move the origin and shorten the maxDist
+	// to deal with precision issues with large maxDist
+	PxBounds3 hfLocalBounds;
+	hfUtil.computeLocalBounds(hfLocalBounds);
+
+	// PT: inflate the bounds like we do in the scene-tree (see PX-1179)
+	const PxVec3 center = hfLocalBounds.getCenter();
+	const PxVec3 extents = hfLocalBounds.getExtents() * 1.01f;	//SQ_PRUNER_INFLATION;
+	hfLocalBounds.minimum = center - extents;
+	hfLocalBounds.maximum = center + extents;
+
+	PxVec3 localImpact;
+	PxReal t;	// closest intersection, t==0 hit inside
+	PxU32 rval = rayAABBIntersect2(hfLocalBounds.minimum, hfLocalBounds.maximum, localRayOrig, localRayDir, localImpact, t);
+	// early exit we miss the AABB
+	if (!rval)
+		return 0;
+	if (t > maxDist)
+		return 0;
+
+	// PT: if eMESH_ANY is used then eMESH_MULTIPLE won't be, and we'll stop the query after 1 hit is found. There is no difference
+	// between 'any hit' and 'closest hit' for HFs since hits are reported in order.
+	HFTraceSegmentCallback callback(hits, hitFlags.isSet(PxHitFlag::eMESH_MULTIPLE) ? maxHits : 1, hitFlags, hfUtil, pose,
+									rayDir, localRayDir, localRayOrig,
+									isDoubleSided); // make sure we return only 1 hit without eMESH_MULTIPLE
+
+	PxReal offset = 0.0f;
+	PxReal maxDistOffset = maxDist;
+	PxVec3 localRayOrigOffset = localRayOrig;
+
+	// if we don't start inside the AABB box, offset the start pos, because of precision issues with large maxDist
+	if(t > 0.0f)
+	{
+		offset = t - GU_RAY_SURFACE_OFFSET;
+		// move the rayOrig to offset start pos
+		localRayOrigOffset = localRayOrig + normRayDir*offset;
+	}
+
+	// shorten the maxDist of the offset that was cut off and clip it
+	// we pick either the original maxDist, if maxDist is huge we clip it
+	maxDistOffset = PxMin(maxDist - offset, GU_RAY_SURFACE_OFFSET + 2.0f * PxMax(hfLocalBounds.maximum.x - hfLocalBounds.minimum.x, PxMax(hfLocalBounds.maximum.y - hfLocalBounds.minimum.y, hfLocalBounds.maximum.z - hfLocalBounds.minimum.z)));
+
+	hfUtil.traceSegment<HFTraceSegmentCallback, false, false>(localRayOrigOffset, normRayDir, maxDistOffset, 
+		&callback, hfLocalBounds, !bothSides);
+	return callback.mNbHits;
+}
+
+static PxU32 raycast_heightField_unregistered(GU_RAY_FUNC_PARAMS)
+{
+	PX_UNUSED(geom);
+	PX_UNUSED(pose);
+	PX_UNUSED(rayOrigin);
+	PX_UNUSED(rayDir);
+	PX_UNUSED(maxDist);
+	PX_UNUSED(hitFlags);
+	PX_UNUSED(maxHits);
+	PX_UNUSED(hits);
+	Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, "Height Field Raycast test called with height fields unregistered ");
+	return 0;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// PT: table is not static because it's accessed as 'extern' within Gu (bypassing the function call).
+RaycastFunc gRaycastMap[PxGeometryType::eGEOMETRY_COUNT] =
+{
+	raycast_sphere,
+	raycast_plane,
+	raycast_capsule,
+	raycast_box,
+	raycast_convexMesh,
+	raycast_triangleMesh,	
+	raycast_heightField_unregistered
+};
+
+// PT: the function is used by external modules (Np, CCT, Sq)
+const Gu::GeomRaycastTable& Gu::getRaycastFuncTable()
+{
+	return gRaycastMap;
+}
+
+void registerHeightFields_Raycasts()
+{
+	gRaycastMap[PxGeometryType::eHEIGHTFIELD] = raycast_heightField;
+}

physx/source/geomutils/src/GuSerialize.cpp

@@ -0,0 +1,381 @@
+//
+// 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-2021 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 "PsIntrinsics.h"
+#include "PsUtilities.h"
+#include "GuSerialize.h"
+#include "PsUserAllocated.h"
+#include "PsAllocator.h"
+#include "PsFPU.h"
+
+using namespace physx;
+using namespace Gu;
+
+void physx::readChunk(PxI8& a, PxI8& b, PxI8& c, PxI8& d, PxInputStream& stream)
+{
+	stream.read(&a, sizeof(PxI8));
+	stream.read(&b, sizeof(PxI8));
+	stream.read(&c, sizeof(PxI8));
+	stream.read(&d, sizeof(PxI8));	
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+PxU16 physx::readWord(bool mismatch, PxInputStream& stream)
+{
+	PxU16 d;
+	stream.read(&d, sizeof(PxU16));
+	
+	if(mismatch)
+		flip(d);
+	return d;
+}
+
+PxU32 physx::readDword(bool mismatch, PxInputStream& stream)
+{
+	PxU32 d;
+	stream.read(&d, sizeof(PxU32));
+
+	if(mismatch)
+		flip(d);
+	return d;
+}
+
+PxF32 physx::readFloat(bool mismatch, PxInputStream& stream)
+{
+	union
+	{
+		PxU32 d;
+		PxF32 f;
+	} u;
+
+	stream.read(&u.d, sizeof(PxU32));
+
+	if(mismatch)
+		flip(u.d);
+	return u.f;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void physx::writeWord(PxU16 value, bool mismatch, PxOutputStream& stream)
+{
+	if(mismatch)
+		flip(value);
+	stream.write(&value, sizeof(PxU16));
+}
+
+void physx::writeDword(PxU32 value, bool mismatch, PxOutputStream& stream)
+{
+	if(mismatch)
+		flip(value);
+	stream.write(&value, sizeof(PxU32));
+}
+
+void physx::writeFloat(PxF32 value, bool mismatch, PxOutputStream& stream)
+{
+	if(mismatch)
+		flip(value);
+	stream.write(&value, sizeof(PxF32));
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+bool physx::readFloatBuffer(PxF32* dest, PxU32 nbFloats, bool mismatch, PxInputStream& stream)
+{
+	stream.read(dest, sizeof(PxF32)*nbFloats);
+	if(mismatch)
+	{
+		for(PxU32 i=0;i<nbFloats;i++)
+			flip(dest[i]);
+	}
+	return true;
+}
+
+void physx::writeFloatBuffer(const PxF32* src, PxU32 nb, bool mismatch, PxOutputStream& stream)
+{
+	if(mismatch)
+	{
+		while(nb--)
+		{
+			PxF32 f = *src++;
+			flip(f);
+			stream.write(&f, sizeof(PxF32));
+		}
+	}
+	else
+		stream.write(src, sizeof(PxF32) * nb);
+}
+
+void physx::writeWordBuffer(const PxU16* src, PxU32 nb, bool mismatch, PxOutputStream& stream)
+{
+	if(mismatch)
+	{
+		while(nb--)
+		{
+			PxU16 w = *src++;
+			flip(w);
+			stream.write(&w, sizeof(PxU16));
+		}
+	}
+	else
+		stream.write(src, sizeof(PxU16) * nb);
+}
+
+void physx::readWordBuffer(PxU16* dest, PxU32 nb, bool mismatch, PxInputStream& stream)
+{
+	stream.read(dest, sizeof(PxU16)*nb);
+	if(mismatch)
+	{
+		for(PxU32 i=0;i<nb;i++)
+		{
+			flip(dest[i]);
+		}
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+bool physx::writeHeader(PxI8 a, PxI8 b, PxI8 c, PxI8 d, PxU32 version, bool mismatch, PxOutputStream& stream)
+{
+	// Store endianness
+	PxI8 streamFlags = Ps::littleEndian();
+	if(mismatch)
+		streamFlags^=1;
+
+	// Export header
+	writeChunk('N', 'X', 'S', streamFlags, stream);	// "Novodex stream" identifier
+	writeChunk(a, b, c, d, stream);					// Chunk identifier
+	writeDword(version, mismatch, stream);
+	return true;
+}
+
+bool Gu::WriteHeader(PxU8 a, PxU8 b, PxU8 c, PxU8 d, PxU32 version, bool mismatch, PxOutputStream& stream)
+{
+	// Store endianness
+	PxU8 streamFlags = PxU8(Ps::littleEndian());
+	if(mismatch)
+		streamFlags^=1;
+
+	// Export header
+	writeChunk('I', 'C', 'E', PxI8(streamFlags), stream);	// ICE identifier
+	writeChunk(PxI8(a), PxI8(b), PxI8(c), PxI8(d), stream);	// Chunk identifier
+	writeDword(version, mismatch, stream);
+	return true;
+}
+
+bool physx::readHeader(PxI8 a_, PxI8 b_, PxI8 c_, PxI8 d_, PxU32& version, bool& mismatch, PxInputStream& stream)
+{
+	// Import header
+	PxI8 a, b, c, d;
+	readChunk(a, b, c, d, stream);
+	if(a!='N' || b!='X' || c!='S')
+		return false;
+
+	const PxI8 fileLittleEndian = d&1;
+	mismatch = fileLittleEndian!=Ps::littleEndian();
+
+	readChunk(a, b, c, d, stream);
+	if(a!=a_ || b!=b_ || c!=c_ || d!=d_)
+		return false;
+
+	version = readDword(mismatch, stream);
+	return true;
+}
+
+bool Gu::ReadHeader(PxU8 a_, PxU8 b_, PxU8 c_, PxU8 d_, PxU32& version, bool& mismatch, PxInputStream& stream)
+{
+	// Import header
+	PxI8 a, b, c, d;
+	readChunk(a, b, c, d, stream);
+	if(a!='I' || b!='C' || c!='E')
+		return false;
+
+	const PxU8 FileLittleEndian = PxU8(d&1);
+	mismatch = FileLittleEndian!=Ps::littleEndian();
+
+	readChunk(a, b, c, d, stream);
+	if(a!=a_ || b!=b_ || c!=c_ || d!=d_)
+		return false;
+
+	version = readDword(mismatch, stream);
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+PxU32 physx::computeMaxIndex(const PxU32* indices, PxU32 nbIndices)
+{
+	PxU32 maxIndex=0;
+	while(nbIndices--)
+	{
+		PxU32 currentIndex = *indices++;
+		if(currentIndex>maxIndex)
+			maxIndex = currentIndex;
+	}
+	return maxIndex;
+}
+PxU16 physx::computeMaxIndex(const PxU16* indices, PxU32 nbIndices)
+{
+	PxU16 maxIndex=0;
+	while(nbIndices--)
+	{
+		PxU16 currentIndex = *indices++;
+		if(currentIndex>maxIndex)
+			maxIndex = currentIndex;
+	}
+	return maxIndex;
+}
+
+void physx::storeIndices(PxU32 maxIndex, PxU32 nbIndices, const PxU32* indices, PxOutputStream& stream, bool platformMismatch)
+{
+	if(maxIndex<=0xff)
+	{
+		for(PxU32 i=0;i<nbIndices;i++)
+		{
+			PxU8 data = PxU8(indices[i]);
+			stream.write(&data, sizeof(PxU8));	
+		}
+	}
+	else if(maxIndex<=0xffff)
+	{
+		for(PxU32 i=0;i<nbIndices;i++)
+			writeWord(Ps::to16(indices[i]), platformMismatch, stream);
+	}
+	else
+	{
+		writeIntBuffer(indices, nbIndices, platformMismatch, stream);
+	}
+}
+
+void physx::readIndices(PxU32 maxIndex, PxU32 nbIndices, PxU32* indices, PxInputStream& stream, bool platformMismatch)
+{
+	if(maxIndex<=0xff)
+	{
+		PxU8 data;
+		for(PxU32 i=0;i<nbIndices;i++)
+		{
+			stream.read(&data, sizeof(PxU8));
+			indices[i] = data;
+		}
+	}
+	else if(maxIndex<=0xffff)
+	{
+		for(PxU32 i=0;i<nbIndices;i++)
+			indices[i] = readWord(platformMismatch, stream);
+	}
+	else
+	{
+		readIntBuffer(indices, nbIndices, platformMismatch, stream);
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void Gu::StoreIndices(PxU32 maxIndex, PxU32 nbIndices, const PxU32* indices, PxOutputStream& stream, bool platformMismatch)
+{
+	if(maxIndex<=0xff)
+	{
+		for(PxU32 i=0;i<nbIndices;i++)
+		{
+			PxU8 data = PxU8(indices[i]);
+			stream.write(&data, sizeof(PxU8));	
+		}
+	}
+	else if(maxIndex<=0xffff)
+	{
+		for(PxU32 i=0;i<nbIndices;i++)
+			writeWord(Ps::to16(indices[i]), platformMismatch, stream);
+	}
+	else
+	{
+//		WriteDwordBuffer(indices, nbIndices, platformMismatch, stream);
+		for(PxU32 i=0;i<nbIndices;i++)
+			writeDword(indices[i], platformMismatch, stream);
+	}
+}
+
+void Gu::ReadIndices(PxU32 maxIndex, PxU32 nbIndices, PxU32* indices, PxInputStream& stream, bool platformMismatch)
+{
+	if(maxIndex<=0xff)
+	{
+		PxU8* tmp = reinterpret_cast<PxU8*>(PxAlloca(nbIndices*sizeof(PxU8)));
+		stream.read(tmp, nbIndices*sizeof(PxU8));
+		for(PxU32 i=0;i<nbIndices;i++)
+			indices[i] = tmp[i];
+//		for(PxU32 i=0;i<nbIndices;i++)
+//			indices[i] = stream.ReadByte();
+	}
+	else if(maxIndex<=0xffff)
+	{
+		PxU16* tmp = reinterpret_cast<PxU16*>(PxAlloca(nbIndices*sizeof(PxU16)));
+		readWordBuffer(tmp, nbIndices, platformMismatch, stream);
+		for(PxU32 i=0;i<nbIndices;i++)
+			indices[i] = tmp[i];
+//		for(PxU32 i=0;i<nbIndices;i++)
+//			indices[i] = ReadWord(platformMismatch, stream);
+	}
+	else
+	{
+		ReadDwordBuffer(indices, nbIndices, platformMismatch, stream);
+	}
+}
+
+void Gu::StoreIndices(PxU16 maxIndex, PxU32 nbIndices, const PxU16* indices, PxOutputStream& stream, bool platformMismatch)
+{
+	if(maxIndex<=0xff)
+	{
+		for(PxU32 i=0;i<nbIndices;i++)
+		{
+			PxU8 data = PxU8(indices[i]);
+			stream.write(&data, sizeof(PxU8));	
+		}
+	}
+	else
+	{
+		for(PxU32 i=0;i<nbIndices;i++)
+			writeWord(indices[i], platformMismatch, stream);
+	}
+}
+
+void Gu::ReadIndices(PxU16 maxIndex, PxU32 nbIndices, PxU16* indices, PxInputStream& stream, bool platformMismatch)
+{
+	if(maxIndex<=0xff)
+	{
+		PxU8* tmp = reinterpret_cast<PxU8*>(PxAlloca(nbIndices*sizeof(PxU8)));
+		stream.read(tmp, nbIndices*sizeof(PxU8));
+		for(PxU32 i=0;i<nbIndices;i++)
+			indices[i] = tmp[i];
+	}
+	else
+	{
+		readWordBuffer(indices, nbIndices, platformMismatch, stream);
+	}
+}

physx/source/geomutils/src/GuSerialize.h

@@ -0,0 +1,196 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_SERIALIZE_H
+#define GU_SERIALIZE_H
+
+#include "foundation/PxSimpleTypes.h"
+#include "foundation/PxIO.h"
+#include "common/PxPhysXCommonConfig.h"
+#include "CmPhysXCommon.h"
+#include "PsUtilities.h"
+
+namespace physx
+{
+	PX_INLINE void flip(PxU16& v)
+	{
+		PxU8* b = reinterpret_cast<PxU8*>(&v);
+		PxU8 temp = b[0];
+		b[0] = b[1];
+		b[1] = temp;
+	}
+
+	PX_INLINE void flip(PxI16& v)
+	{
+		PxI8* b = reinterpret_cast<PxI8*>(&v);
+		PxI8 temp = b[0];
+		b[0] = b[1];
+		b[1] = temp;
+	}
+
+	PX_INLINE void flip(PxU32& v)
+	{
+		PxU8* b = reinterpret_cast<PxU8*>(&v);
+
+        PxU8 temp = b[0];
+		b[0] = b[3];
+		b[3] = temp;
+		temp = b[1];
+		b[1] = b[2];
+		b[2] = temp;
+	}
+
+	// MS: It is important to modify the value directly and not use a temporary variable or a return
+	//     value. The reason for this is that a flipped float might have a bit pattern which indicates
+	//     an invalid float. If such a float is assigned to another float, the bit pattern
+	//     can change again (maybe to map invalid floats to a common invalid pattern?).
+	//     When reading the float and flipping again, the changed bit pattern will result in a different
+	//     float than the original one.
+	PX_INLINE void flip(PxF32& v)
+	{
+		PxU8* b = reinterpret_cast<PxU8*>(&v);
+
+        PxU8 temp = b[0];
+		b[0] = b[3];
+		b[3] = temp;
+		temp = b[1];
+		b[1] = b[2];
+		b[2] = temp;
+	}
+
+	PX_INLINE void 	writeChunk(PxI8 a, PxI8 b, PxI8 c, PxI8 d, PxOutputStream& stream)
+	{
+		stream.write(&a, sizeof(PxI8));
+		stream.write(&b, sizeof(PxI8));
+		stream.write(&c, sizeof(PxI8));
+		stream.write(&d, sizeof(PxI8));
+	}
+
+					void 	readChunk(PxI8& a, PxI8& b, PxI8& c, PxI8& d, PxInputStream& stream);
+
+					PxU16 	readWord(bool mismatch, PxInputStream& stream);
+PX_PHYSX_COMMON_API	PxU32 	readDword(bool mismatch, PxInputStream& stream);
+					PxF32 	readFloat(bool mismatch, PxInputStream& stream);
+
+PX_PHYSX_COMMON_API	void 	writeWord(PxU16 value, bool mismatch, PxOutputStream& stream);
+PX_PHYSX_COMMON_API	void 	writeDword(PxU32 value, bool mismatch, PxOutputStream& stream);
+PX_PHYSX_COMMON_API	void 	writeFloat(PxF32 value, bool mismatch, PxOutputStream& stream);
+
+					bool 	readFloatBuffer(PxF32* dest, PxU32 nbFloats, bool mismatch, PxInputStream& stream);
+PX_PHYSX_COMMON_API	void 	writeFloatBuffer(const PxF32* src, PxU32 nb, bool mismatch, PxOutputStream& stream);
+PX_PHYSX_COMMON_API	void 	writeWordBuffer(const PxU16* src, PxU32 nb, bool mismatch, PxOutputStream& stream);
+					void	readWordBuffer(PxU16* dest, PxU32 nb, bool mismatch, PxInputStream& stream);
+
+PX_PHYSX_COMMON_API	bool 	writeHeader(PxI8 a, PxI8 b, PxI8 c, PxI8 d, PxU32 version, bool mismatch, PxOutputStream& stream);
+					bool 	readHeader(PxI8 a, PxI8 b, PxI8 c, PxI8 d, PxU32& version, bool& mismatch, PxInputStream& stream);
+
+PX_INLINE	bool	readIntBuffer(PxU32* dest, PxU32 nbInts, bool mismatch, PxInputStream& stream)
+{
+	return readFloatBuffer(reinterpret_cast<PxF32*>(dest), nbInts, mismatch, stream);
+}
+
+PX_INLINE	void	writeIntBuffer(const PxU32* src, PxU32 nb, bool mismatch, PxOutputStream& stream)
+{
+	writeFloatBuffer(reinterpret_cast<const PxF32*>(src), nb, mismatch, stream);
+}
+
+PX_INLINE	bool	ReadDwordBuffer(PxU32* dest, PxU32 nb, bool mismatch, PxInputStream& stream)
+{
+	return readFloatBuffer(reinterpret_cast<float*>(dest), nb, mismatch, stream);
+}
+
+PX_INLINE	void	WriteDwordBuffer(const PxU32* src, PxU32 nb, bool mismatch, PxOutputStream& stream)
+{
+	writeFloatBuffer(reinterpret_cast<const float*>(src), nb, mismatch, stream);
+}
+
+PX_PHYSX_COMMON_API PxU32 	computeMaxIndex(const PxU32* indices, PxU32 nbIndices);
+PX_PHYSX_COMMON_API PxU16 	computeMaxIndex(const PxU16* indices, PxU32 nbIndices);
+PX_PHYSX_COMMON_API void 	storeIndices(PxU32 maxIndex, PxU32 nbIndices, const PxU32* indices, PxOutputStream& stream, bool platformMismatch);
+PX_PHYSX_COMMON_API void 	readIndices(PxU32 maxIndex, PxU32 nbIndices, PxU32* indices, PxInputStream& stream, bool platformMismatch);
+
+	// PT: see PX-1163
+	PX_FORCE_INLINE bool readBigEndianVersionNumber(PxInputStream& stream, bool mismatch_, PxU32& fileVersion, bool& mismatch)
+	{
+		// PT: allright this is going to be subtle:
+		// - in version 1 the data was always saved in big-endian format
+		// - *including the version number*!
+		// - so we cannot just read the version "as usual" using the passed mismatch param
+
+		// PT: mismatch value for version 1
+		mismatch = (shdfnd::littleEndian() == 1);
+
+		const PxU32 rawFileVersion = readDword(false, stream);
+		if(rawFileVersion==1)
+		{
+			// PT: this is a version-1 file with no flip
+			fileVersion = 1;
+			PX_ASSERT(!mismatch);
+		}
+		else
+		{
+			PxU32 fileVersionFlipped = rawFileVersion;
+			flip(fileVersionFlipped);
+			if(fileVersionFlipped==1)
+			{
+				// PT: this is a version-1 file with flip
+				fileVersion = 1;
+				PX_ASSERT(mismatch);
+			}
+			else
+			{
+				// PT: this is at least version 2 so we can process it "as usual"
+				mismatch = mismatch_;
+				fileVersion = mismatch_ ? fileVersionFlipped : rawFileVersion;
+			}
+		}
+
+		PX_ASSERT(fileVersion<=3);
+		if(fileVersion>3)
+			return false;
+		return true;
+	}
+
+// PT: TODO: copied from IceSerialize.h, still needs to be refactored/cleaned up.
+namespace Gu
+{
+	PX_PHYSX_COMMON_API bool WriteHeader(PxU8 a, PxU8 b, PxU8 c, PxU8 d, PxU32 version, bool mismatch, PxOutputStream& stream);
+	PX_PHYSX_COMMON_API bool ReadHeader(PxU8 a_, PxU8 b_, PxU8 c_, PxU8 d_, PxU32& version, bool& mismatch, PxInputStream& stream);
+
+	PX_PHYSX_COMMON_API void StoreIndices(PxU32 maxIndex, PxU32 nbIndices, const PxU32* indices, PxOutputStream& stream, bool platformMismatch);
+						void ReadIndices(PxU32 maxIndex, PxU32 nbIndices, PxU32* indices, PxInputStream& stream, bool platformMismatch);
+
+	PX_PHYSX_COMMON_API void StoreIndices(PxU16 maxIndex, PxU32 nbIndices, const PxU16* indices, PxOutputStream& stream, bool platformMismatch);
+						void ReadIndices(PxU16 maxIndex, PxU32 nbIndices, PxU16* indices, PxInputStream& stream, bool platformMismatch);
+}
+
+
+}
+
+#endif

physx/source/geomutils/src/GuSphere.h

@@ -0,0 +1,108 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_SPHERE_H
+#define GU_SPHERE_H
+/** \addtogroup geomutils
+@{
+*/
+
+#include "foundation/PxVec3.h"
+#include "CmPhysXCommon.h"
+
+namespace physx
+{
+
+/**
+\brief Represents a sphere defined by its center point and radius.
+*/
+namespace Gu
+{
+	class Sphere
+	{
+	public:
+		/**
+		\brief Constructor
+		*/
+		PX_INLINE Sphere()
+		{
+		}
+
+		/**
+		\brief Constructor
+		*/
+		PX_INLINE Sphere(const PxVec3& _center, PxF32 _radius) : center(_center), radius(_radius)
+		{
+		}
+		/**
+		\brief Copy constructor
+		*/
+		PX_INLINE Sphere(const Sphere& sphere) : center(sphere.center), radius(sphere.radius)
+		{
+		}
+		/**
+		\brief Destructor
+		*/
+		PX_INLINE ~Sphere()
+		{
+		}
+
+		PX_INLINE	void	set(const PxVec3& _center, float _radius)		{ center = _center; radius = _radius;	}
+
+		/**
+		\brief Checks the sphere is valid.
+
+		\return		true if the sphere is valid
+		*/
+		PX_INLINE bool isValid() const
+		{
+			// Consistency condition for spheres: Radius >= 0.0f
+			return radius >= 0.0f;
+		}
+
+		/**
+		\brief Tests if a point is contained within the sphere.
+
+		\param[in] p the point to test
+		\return	true if inside the sphere
+		*/
+		PX_INLINE bool contains(const PxVec3& p) const
+		{
+			return (center-p).magnitudeSquared() <= radius*radius;
+		}
+
+		PxVec3	center;		//!< Sphere's center
+		PxF32	radius;		//!< Sphere's radius
+	};
+}
+
+}
+
+/** @} */
+#endif

physx/source/geomutils/src/GuSweepMTD.h

@@ -0,0 +1,95 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_SWEEP_MTD_H
+#define GU_SWEEP_MTD_H
+
+namespace physx
+{
+	class PxConvexMeshGeometry;
+	class PxTriangleMeshGeometry;
+	class PxGeometry;
+	class PxHeightFieldGeometry;
+
+namespace Gu
+{
+	class Sphere;
+	class Capsule;
+
+	bool computeCapsule_TriangleMeshMTD(const PxTriangleMeshGeometry& triMeshGeom, const PxTransform& pose, Gu::CapsuleV& capsuleV, PxReal inflatedRadius, bool isDoubleSided, PxSweepHit& hit);
+
+	bool computeCapsule_HeightFieldMTD(const PxHeightFieldGeometry& heightFieldGeom, const PxTransform& pose, Gu::CapsuleV& capsuleV, PxReal inflatedRadius, bool isDoubleSided, const PxU32 flags, PxSweepHit& hit);
+
+	bool computeBox_TriangleMeshMTD(const PxTriangleMeshGeometry& triMeshGeom, const PxTransform& pose, const Gu::Box& box, const PxTransform& boxTransform, PxReal inflation,
+									bool isDoubleSided,  PxSweepHit& hit);    
+
+	bool computeBox_HeightFieldMTD(	const PxHeightFieldGeometry& heightFieldGeom, const PxTransform& pose, const Gu::Box& box, const PxTransform& boxTransform, PxReal inflation, 
+									bool isDoubleSided, const PxU32 flags, PxSweepHit& hit);
+
+	bool computeConvex_TriangleMeshMTD(	const PxTriangleMeshGeometry& triMeshGeom, const PxTransform& pose, const PxConvexMeshGeometry& convexGeom, const PxTransform& convexTransform, PxReal inflation,
+										bool isDoubleSided, PxSweepHit& hit);
+
+	bool computeConvex_HeightFieldMTD(	const PxHeightFieldGeometry& heightFieldGeom, const PxTransform& pose, const PxConvexMeshGeometry& convexGeom, const PxTransform& convexTransform, PxReal inflation,
+										bool isDoubleSided, const PxU32 flags, PxSweepHit& hit);
+
+	bool computeSphere_SphereMTD(const Sphere& sphere0, const Sphere& sphere1, PxSweepHit& hit);
+	bool computeSphere_CapsuleMTD(const Sphere& sphere, const Capsule& capsule, PxSweepHit& hit);
+
+	bool computeCapsule_CapsuleMTD(const Capsule& capsule0, const Capsule& capsule1, PxSweepHit& hit);
+
+	bool computePlane_CapsuleMTD(const PxPlane& plane, const Capsule& capsule, PxSweepHit& hit);
+	bool computePlane_BoxMTD(const PxPlane& plane, const Box& box, PxSweepHit& hit);
+	bool computePlane_ConvexMTD(const PxPlane& plane, const PxConvexMeshGeometry& convexGeom, const PxTransform& convexPose, PxSweepHit& hit);
+
+	// PT: wrapper just to avoid duplicating these lines.
+	PX_FORCE_INLINE void setupSweepHitForMTD(PxSweepHit& sweepHit, bool hasContacts, const PxVec3& unitDir)
+	{
+		sweepHit.flags = PxHitFlag::eNORMAL | PxHitFlag::eFACE_INDEX;
+		if(!hasContacts)
+		{
+			sweepHit.distance	= 0.0f;
+			sweepHit.normal		= -unitDir;
+		}
+		else
+		{
+			//ML: touching contact. We need to overwrite the normal to the negative of sweep direction
+			if (sweepHit.distance == 0.0f)
+			{
+				sweepHit.normal = -unitDir;
+			}
+			sweepHit.flags |= PxHitFlag::ePOSITION;
+			
+		}
+	}
+}
+
+}
+
+
+#endif

physx/source/geomutils/src/GuSweepSharedTests.cpp

@@ -0,0 +1,725 @@
+//
+// 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-2021 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/PxConvexMeshGeometry.h"
+#include "geometry/PxSphereGeometry.h"
+#include "GuSweepTests.h"
+#include "GuHeightFieldUtil.h"
+#include "CmScaling.h"
+#include "GuConvexMesh.h"
+#include "GuIntersectionRayPlane.h"
+#include "GuVecBox.h"
+#include "GuVecCapsule.h"
+#include "GuVecConvexHull.h"
+#include "GuSweepMTD.h"
+#include "GuSweepSphereCapsule.h"
+#include "GuSweepCapsuleCapsule.h"
+#include "GuSweepTriangleUtils.h"
+#include "GuSweepCapsuleTriangle.h"
+#include "GuInternal.h"
+#include "GuGJKRaycast.h"
+
+using namespace physx;
+using namespace Gu;
+using namespace Cm;
+using namespace physx::shdfnd::aos;
+
+static const PxReal gEpsilon = .01f;
+
+static PxU32 computeSweepConvexPlane(
+	const PxConvexMeshGeometry& convexGeom, ConvexHullData* hullData, const PxU32& nbPolys, const PxTransform& pose,
+	const PxVec3& impact_, const PxVec3& unitDir)
+{
+	PX_ASSERT(nbPolys);
+
+	const PxVec3 impact = impact_ - unitDir * gEpsilon;
+
+	const PxVec3 localPoint = pose.transformInv(impact);
+	const PxVec3 localDir = pose.rotateInv(unitDir);
+
+	const FastVertex2ShapeScaling scaling(convexGeom.scale);
+
+	PxU32 minIndex = 0;
+	PxReal minD = PX_MAX_REAL;
+	for(PxU32 j=0; j<nbPolys; j++)
+	{
+		const PxPlane& pl = hullData->mPolygons[j].mPlane;
+
+		PxPlane plane;
+		scaling.transformPlaneToShapeSpace(pl.n, pl.d, plane.n, plane.d);
+
+		PxReal d = plane.distance(localPoint);
+		if(d<0.0f)
+			continue;
+
+		const PxReal tweak = plane.n.dot(localDir) * gEpsilon;
+		d += tweak;
+
+		if(d<minD)
+		{
+			minIndex = j;
+			minD = d;
+		}
+	}
+	return minIndex;
+}
+
+static PX_FORCE_INLINE bool computeFaceIndex(PxSweepHit& sweepHit, const PxHitFlags hitFlags, const PxConvexMeshGeometry& convexGeom, ConvexHullData* hullData, const PxTransform& pose, const PxVec3& unitDir)
+{
+	if(hitFlags & PxHitFlag::eFACE_INDEX)
+	{
+		// PT: compute closest polygon using the same tweak as in swept-capsule-vs-mesh
+		sweepHit.faceIndex = computeSweepConvexPlane(convexGeom, hullData, hullData->mNbPolygons, pose, sweepHit.position, unitDir);
+		sweepHit.flags |= PxHitFlag::eFACE_INDEX;
+	}
+	return true;
+}
+
+static PX_FORCE_INLINE bool hasInitialOverlap(PxSweepHit& sweepHit, const PxVec3& unitDir,
+											  const FloatVArg toi,
+											  const Vec3VArg normal, const Vec3VArg closestA,
+											  const PsTransformV& convexPose,
+											  const bool isMtd, const bool impactPointOnTheOtherShape)
+{
+	sweepHit.flags = PxHitFlag::eNORMAL;
+
+	const FloatV zero = FZero();
+	if(FAllGrtrOrEq(zero, toi))
+	{
+		//ML: initial overlap
+		if(isMtd)
+		{
+			sweepHit.flags |= PxHitFlag::ePOSITION;
+			const FloatV length = toi;
+			const Vec3V worldPointA = convexPose.transform(closestA);
+			const Vec3V worldNormal = V3Normalize(convexPose.rotate(normal));
+			if(impactPointOnTheOtherShape)
+			{
+				const Vec3V destWorldPointA = V3NegScaleSub(worldNormal, length, worldPointA);
+				V3StoreU(worldNormal, sweepHit.normal);
+				V3StoreU(destWorldPointA, sweepHit.position);
+			}
+			else
+			{
+				const Vec3V destNormal = V3Neg(worldNormal);
+				V3StoreU(destNormal, sweepHit.normal);
+				V3StoreU(worldPointA, sweepHit.position);
+			}
+			FStore(length, &sweepHit.distance);
+		}
+		else
+		{
+			sweepHit.distance	= 0.0f;
+			sweepHit.normal		= -unitDir;
+		}
+		sweepHit.faceIndex = 0xffffffff;
+		return true;
+	}
+	return false;
+}
+
+/////////////////////////////////////////////////  sweepCapsule/Sphere  //////////////////////////////////////////////////////
+bool sweepCapsule_SphereGeom(GU_CAPSULE_SWEEP_FUNC_PARAMS)
+{
+	PX_UNUSED(capsuleGeom_);
+	PX_UNUSED(capsulePose_);
+
+	PX_ASSERT(geom.getType() == PxGeometryType::eSPHERE);
+	const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom);
+
+	const Sphere sphere(pose.p, sphereGeom.radius+inflation);
+
+	if(!sweepSphereCapsule(sphere, lss, -unitDir, distance, sweepHit.distance, sweepHit.position, sweepHit.normal, hitFlags))
+		return false;
+
+	const bool isMtd = hitFlags & PxHitFlag::eMTD;
+
+	if(isMtd)
+	{
+		sweepHit.flags = PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
+
+		if(sweepHit.distance == 0.f)
+		{
+			//intialOverlap
+			if(lss.p0 == lss.p1)
+			{
+				//sphere
+				return computeSphere_SphereMTD(sphere, Sphere(lss.p0, lss.radius), sweepHit);
+			}
+			else
+			{
+				//capsule
+				return computeSphere_CapsuleMTD(sphere, lss, sweepHit);
+			}
+		}
+	}
+	else
+	{
+		if(sweepHit.distance!=0.0f)
+			sweepHit.flags = PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
+		else
+			sweepHit.flags = PxHitFlag::eNORMAL;
+	}
+	return true;
+}
+
+bool sweepCapsule_PlaneGeom(GU_CAPSULE_SWEEP_FUNC_PARAMS)
+{
+	PX_UNUSED(capsuleGeom_);
+	PX_UNUSED(capsulePose_);
+
+	PX_ASSERT(geom.getType() == PxGeometryType::ePLANE);
+	PX_UNUSED(geom);
+//	const PxPlaneGeometry& planeGeom = static_cast<const PxPlaneGeometry&>(geom);
+
+	const PxPlane& worldPlane = getPlane(pose);
+
+	const PxF32 capsuleRadius = lss.radius + inflation;
+
+	PxU32 index = 0;
+	PxVec3 pts[2];
+
+	PxReal minDp = PX_MAX_REAL;
+
+	sweepHit.faceIndex	= 0xFFFFffff; // spec says face index is undefined for planes
+
+	// Find extreme point on the capsule
+	// AP: removed if (lss.p0 == lss.p1 clause because it wasn't properly computing minDp)
+	pts[0] = lss.p0;
+	pts[1] = lss.p1;
+	for(PxU32 i=0; i<2; i++)
+	{
+		const PxReal dp = pts[i].dot(worldPlane.n);
+		if(dp<minDp)
+		{
+			minDp = dp;
+			index = i;
+		}
+	}
+
+	const bool isMtd = hitFlags & PxHitFlag::eMTD;
+
+	if(isMtd)
+	{
+		//initial overlap with the plane
+		if(minDp <= capsuleRadius - worldPlane.d)
+		{
+			sweepHit.flags = PxHitFlag::eNORMAL| PxHitFlag::ePOSITION;
+			return computePlane_CapsuleMTD(worldPlane, lss, sweepHit);
+		}
+	}
+	else
+	{
+		if(!(hitFlags & PxHitFlag::eASSUME_NO_INITIAL_OVERLAP))
+		{
+			// test if the capsule initially overlaps with plane
+			if(minDp <= capsuleRadius - worldPlane.d)
+			{
+				sweepHit.flags		= PxHitFlag::eNORMAL;
+				sweepHit.distance	= 0.0f;
+				sweepHit.normal		= -unitDir;
+				return true;
+			}
+		}
+	}
+
+	const PxVec3 ptOnCapsule = pts[index] - worldPlane.n*capsuleRadius;
+
+	// Raycast extreme vertex against plane
+	bool hitPlane = intersectRayPlane(ptOnCapsule, unitDir, worldPlane, sweepHit.distance, &sweepHit.position);
+	if(hitPlane && sweepHit.distance > 0 && sweepHit.distance <= distance)
+	{
+		sweepHit.normal = worldPlane.n;
+		sweepHit.flags = PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
+		return true;
+	}
+	return false;
+}
+
+bool sweepCapsule_CapsuleGeom(GU_CAPSULE_SWEEP_FUNC_PARAMS)
+{
+	PX_UNUSED(capsuleGeom_);
+	PX_UNUSED(capsulePose_);
+
+	PX_ASSERT(geom.getType() == PxGeometryType::eCAPSULE);
+	const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom);
+
+	Capsule staticCapsule;
+	getCapsule(staticCapsule, capsuleGeom, pose);
+	staticCapsule.radius +=inflation;
+
+	const bool isMtd = hitFlags & PxHitFlag::eMTD;
+
+	PxU16 outFlags;
+	if(!sweepCapsuleCapsule(lss, staticCapsule, -unitDir, distance, sweepHit.distance, sweepHit.position, sweepHit.normal, hitFlags, outFlags))
+		return false;
+
+	sweepHit.flags = PxHitFlags(outFlags);
+	if(sweepHit.distance == 0.0f)
+	{
+		//initial overlap
+		if(isMtd)
+		{
+			sweepHit.flags |= PxHitFlag::ePOSITION;
+			return computeCapsule_CapsuleMTD(lss, staticCapsule, sweepHit);
+		}
+	}
+	return true;
+}
+
+bool sweepCapsule_ConvexGeom(GU_CAPSULE_SWEEP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);
+
+	using namespace Ps::aos;
+	
+	PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);
+	const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom);
+
+	ConvexMesh* convexMesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
+	ConvexHullData* hullData = &convexMesh->getHull();
+
+	const Vec3V zeroV = V3Zero();
+	const FloatV zero = FZero();
+	const FloatV dist = FLoad(distance);
+	const Vec3V worldDir = V3LoadU(unitDir);
+
+	const PsTransformV capPose = loadTransformU(capsulePose_);
+	const PsTransformV convexPose = loadTransformU(pose);
+
+	const PsMatTransformV aToB(convexPose.transformInv(capPose));
+
+	const FloatV capsuleHalfHeight = FLoad(capsuleGeom_.halfHeight);
+	const FloatV capsuleRadius = FLoad(lss.radius);
+
+	const Vec3V vScale = V3LoadU_SafeReadW(convexGeom.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
+	const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x);
+
+	CapsuleV capsule(aToB.p, aToB.rotate( V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius);
+	ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, convexGeom.scale.isIdentity());
+
+	const Vec3V dir = convexPose.rotateInv(V3Neg(V3Scale(worldDir, dist)));
+
+	bool isMtd = hitFlags & PxHitFlag::eMTD;
+
+	FloatV toi;
+	Vec3V closestA, normal;//closestA and normal is in the local space of convex hull
+	LocalConvex<CapsuleV> convexA(capsule);
+	LocalConvex<ConvexHullV> convexB(convexHull);
+	const Vec3V initialSearchDir = V3Sub(capsule.getCenter(), convexHull.getCenter());
+	if(!gjkRaycastPenetration< LocalConvex<CapsuleV>, LocalConvex<ConvexHullV> >(convexA, convexB, initialSearchDir, zero, zeroV, dir, toi, normal, closestA, lss.radius + inflation, isMtd))
+		return false;
+
+	if(hasInitialOverlap(sweepHit, unitDir, toi, normal, closestA, convexPose, isMtd, true))
+		return true;
+
+	sweepHit.flags |= PxHitFlag::ePOSITION;
+	const Vec3V worldPointA = convexPose.transform(closestA);
+	const FloatV length = FMul(dist, toi);
+	const Vec3V destNormal = V3Normalize(convexPose.rotate(normal));
+	const Vec3V destWorldPointA = V3ScaleAdd(worldDir, length, worldPointA);
+	V3StoreU(destNormal, sweepHit.normal);
+	V3StoreU(destWorldPointA, sweepHit.position);
+	FStore(length, &sweepHit.distance);
+
+	return computeFaceIndex(sweepHit, hitFlags, convexGeom, hullData, pose, unitDir);
+}
+
+/////////////////////////////////////////////////  sweepBox  //////////////////////////////////////////////////////
+
+bool sweepBox_PlaneGeom(GU_BOX_SWEEP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom.getType() == PxGeometryType::ePLANE);
+	PX_UNUSED(geom);
+	PX_UNUSED(boxPose_);
+	PX_UNUSED(boxGeom_);
+
+//	const PxPlaneGeometry& planeGeom = static_cast<const PxPlaneGeometry&>(geom);
+
+	sweepHit.faceIndex	= 0xFFFFffff; // spec says face index is undefined for planes
+
+	PxPlane worldPlane = getPlane(pose);
+	worldPlane.d -=inflation;
+
+	// Find extreme point on the box
+	PxVec3 boxPts[8];
+	box.computeBoxPoints(boxPts);
+	PxU32 index = 0;
+	PxReal minDp = PX_MAX_REAL;
+	for(PxU32 i=0;i<8;i++)
+	{
+		const PxReal dp = boxPts[i].dot(worldPlane.n);
+	
+		if(dp<minDp)
+		{
+			minDp = dp;
+			index = i;
+		}
+	}
+
+	bool isMtd = hitFlags & PxHitFlag::eMTD;
+
+	if(isMtd)
+	{
+		// test if box initially overlap with plane
+		if(minDp <= -worldPlane.d)
+		{
+			sweepHit.flags = PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
+			//compute Mtd;
+			return computePlane_BoxMTD(worldPlane, box, sweepHit);
+		}
+	}
+	else
+	{
+		if(!(hitFlags & PxHitFlag::eASSUME_NO_INITIAL_OVERLAP))
+		{
+			// test if box initially overlap with plane
+			if(minDp <= -worldPlane.d)
+			{
+				sweepHit.flags		= PxHitFlag::eNORMAL;
+				sweepHit.distance	= 0.0f;
+				sweepHit.normal		= -unitDir;
+				return true;
+			}
+		}
+	}
+
+	// Raycast extreme vertex against plane
+	bool hitPlane = intersectRayPlane(boxPts[index], unitDir, worldPlane, sweepHit.distance, &sweepHit.position);
+	if(hitPlane && sweepHit.distance > 0 && sweepHit.distance <= distance)
+	{
+		sweepHit.normal = worldPlane.n;
+		sweepHit.flags = PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
+		return true;
+	}
+	return false;
+}
+
+bool sweepBox_ConvexGeom(GU_BOX_SWEEP_FUNC_PARAMS)
+{
+	PX_UNUSED(boxGeom_);
+
+	using namespace Ps::aos;
+	PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);
+	const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom);
+
+	ConvexMesh* convexMesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
+	ConvexHullData* hullData = &convexMesh->getHull();
+
+	const Vec3V zeroV = V3Zero();
+	const FloatV zero = FZero();
+
+	const PsTransformV boxPose = loadTransformU(boxPose_);
+	const PsTransformV convexPose = loadTransformU(pose);
+
+	const PsMatTransformV aToB(convexPose.transformInv(boxPose));
+
+	const Vec3V boxExtents = V3LoadU(box.extents);
+
+	const Vec3V vScale = V3LoadU_SafeReadW(convexGeom.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
+	const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x);
+	
+	BoxV boxV(zeroV, boxExtents);
+	ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, convexGeom.scale.isIdentity());
+
+	const Vec3V worldDir = V3LoadU(unitDir);
+	const FloatV dist = FLoad(distance);
+	const Vec3V dir = convexPose.rotateInv(V3Neg(V3Scale(worldDir, dist)));
+
+	bool isMtd = hitFlags & PxHitFlag::eMTD;
+
+	FloatV toi;
+	Vec3V closestA, normal;
+	RelativeConvex<BoxV> convexA(boxV, aToB);
+	LocalConvex<ConvexHullV> convexB(convexHull);
+	if(!gjkRaycastPenetration< RelativeConvex<BoxV>,LocalConvex<ConvexHullV> >(convexA, convexB, aToB.p, zero, zeroV, dir, toi, normal, closestA, inflation, isMtd))
+		return false;
+
+	if(hasInitialOverlap(sweepHit, unitDir, toi, normal, closestA, convexPose, isMtd, true))
+		return true;
+
+	sweepHit.flags |= PxHitFlag::ePOSITION;
+	const Vec3V destNormal = V3Normalize(convexPose.rotate(normal));
+	const FloatV length = FMul(dist, toi);
+	const Vec3V worldPointA = convexPose.transform(closestA);
+	const Vec3V destWorldPointA = V3ScaleAdd(worldDir, length, worldPointA);
+	V3StoreU(destNormal, sweepHit.normal);
+	V3StoreU(destWorldPointA, sweepHit.position);
+	FStore(length, &sweepHit.distance);
+
+	return computeFaceIndex(sweepHit, hitFlags, convexGeom, hullData, pose, unitDir);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+bool Gu::sweepCapsuleTriangles(GU_SWEEP_TRIANGLES_FUNC_PARAMS(PxCapsuleGeometry))
+{
+	Capsule capsule;
+	getCapsule(capsule, geom, pose);
+	capsule.radius +=inflation;
+
+	// Compute swept box
+	Box capsuleBox;
+	computeBoxAroundCapsule(capsule, capsuleBox);
+
+	BoxPadded sweptBounds;
+	computeSweptBox(sweptBounds, capsuleBox.extents, capsuleBox.center, capsuleBox.rot, unitDir, distance);
+
+	PxVec3 triNormal;
+	return sweepCapsuleTriangles_Precise(nbTris, triangles, capsule, unitDir, distance, cachedIndex, hit, triNormal, hitFlags, doubleSided, &sweptBounds);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+bool sweepConvex_SphereGeom(GU_CONVEX_SWEEP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom.getType() == PxGeometryType::eSPHERE);
+	const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom);
+
+	ConvexMesh* convexMesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
+	ConvexHullData* hullData = &convexMesh->getHull();
+
+	const Vec3V zeroV = V3Zero();
+	const FloatV zero= FZero();
+
+	const Vec3V vScale = V3LoadU_SafeReadW(convexGeom.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
+	const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x);
+
+	const FloatV sphereRadius = FLoad(sphereGeom.radius);
+
+	const PsTransformV sphereTransf = loadTransformU(pose);
+	const PsTransformV convexTransf = loadTransformU(convexPose);
+
+	const PsMatTransformV aToB(convexTransf.transformInv(sphereTransf));
+
+	const Vec3V worldDir = V3LoadU(unitDir);
+	const FloatV dist = FLoad(distance);
+	const Vec3V dir = convexTransf.rotateInv(V3Scale(worldDir, dist));
+
+	ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, convexGeom.scale.isIdentity());
+	//CapsuleV capsule(zeroV, sphereRadius);
+	CapsuleV capsule(aToB.p, sphereRadius);
+
+	const bool isMtd = hitFlags & PxHitFlag::eMTD;
+
+	FloatV toi;
+	Vec3V closestA, normal;
+	LocalConvex<CapsuleV> convexA(capsule);
+	LocalConvex<ConvexHullV> convexB(convexHull);
+	const Vec3V initialSearchDir = V3Sub(capsule.getCenter(), convexHull.getCenter());
+	if(!gjkRaycastPenetration< LocalConvex<CapsuleV>, LocalConvex<ConvexHullV> >(convexA, convexB, initialSearchDir, zero, zeroV, dir, toi, normal, closestA, sphereGeom.radius+inflation, isMtd))
+		return false;
+
+	if(hasInitialOverlap(sweepHit, unitDir, toi, normal, closestA, convexPose, isMtd, false))
+		return true;
+
+	sweepHit.flags |= PxHitFlag::ePOSITION;
+	const Vec3V destNormal = V3Neg(V3Normalize(convexTransf.rotate(normal)));
+	const FloatV length = FMul(dist, toi);
+	const Vec3V destWorldPointA = convexTransf.transform(closestA);
+	V3StoreU(destNormal, sweepHit.normal);
+	V3StoreU(destWorldPointA, sweepHit.position);
+	FStore(length, &sweepHit.distance);
+	sweepHit.faceIndex = 0xffffffff;
+	return true;
+}
+
+bool sweepConvex_PlaneGeom(GU_CONVEX_SWEEP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom.getType() == PxGeometryType::ePLANE);
+	PX_UNUSED(hitFlags);
+	PX_UNUSED(geom);
+
+	ConvexMesh* convexMesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
+	ConvexHullData* hullData = &convexMesh->getHull();
+
+	sweepHit.faceIndex	= 0xFFFFffff; // spec says face index is undefined for planes
+
+	const PxVec3* PX_RESTRICT hullVertices = hullData->getHullVertices();
+	PxU32 numHullVertices = hullData->mNbHullVertices;
+
+	const bool isMtd = hitFlags & PxHitFlag::eMTD;
+
+	const FastVertex2ShapeScaling convexScaling(convexGeom.scale);
+
+	PxPlane plane = getPlane(pose);
+	plane.d -=inflation;
+
+	sweepHit.distance	= distance;
+	bool status = false;
+	bool initialOverlap = false;
+	while(numHullVertices--)
+	{
+		const PxVec3& vertex = *hullVertices++;
+		const PxVec3 worldPt = convexPose.transform(convexScaling * vertex);
+		float t;
+		PxVec3 pointOnPlane;
+		if(intersectRayPlane(worldPt, unitDir, plane, t, &pointOnPlane))
+		{	
+			if(plane.distance(worldPt) <= 0.0f)
+			{
+				initialOverlap = true;
+				break;
+				//// Convex touches plane
+				//sweepHit.distance		= 0.0f;
+				//sweepHit.flags			= PxHitFlag::eNORMAL;
+				//sweepHit.normal			= -unitDir;
+				//return true;
+			}
+
+			if(t > 0.0f && t <= sweepHit.distance)
+			{
+				sweepHit.distance	= t;
+				sweepHit.flags		= PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
+				sweepHit.position	= pointOnPlane;
+				sweepHit.normal		= plane.n;
+				status				= true;
+			}
+		}
+	}
+
+	if(initialOverlap)
+	{
+		if(isMtd)
+		{
+			sweepHit.flags		= PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
+			return computePlane_ConvexMTD(plane, convexGeom, convexPose, sweepHit);
+		}
+		else
+		{
+			sweepHit.distance	= 0.0f;
+			sweepHit.flags		= PxHitFlag::eNORMAL;
+			sweepHit.normal		= -unitDir;
+			return true;
+		}
+	}
+	return status;
+}
+
+bool sweepConvex_CapsuleGeom(GU_CONVEX_SWEEP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom.getType() == PxGeometryType::eCAPSULE);
+	const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom);
+
+	Capsule capsule;
+	getCapsule(capsule, capsuleGeom, pose);
+
+	// remove PxHitFlag::eFACE_INDEX, not neeeded to compute.
+	PxHitFlags tempHitFlags = hitFlags;
+	tempHitFlags &= ~PxHitFlag::eFACE_INDEX;
+
+	if(!sweepCapsule_ConvexGeom(convexGeom, convexPose, capsuleGeom, pose, capsule, -unitDir, distance, sweepHit, tempHitFlags, inflation))
+		return false;
+
+	if(sweepHit.flags & PxHitFlag::ePOSITION)
+		sweepHit.position += unitDir * sweepHit.distance;
+
+	sweepHit.normal = -sweepHit.normal;
+	sweepHit.faceIndex = 0xffffffff;
+	return true;
+}
+
+bool sweepConvex_BoxGeom(GU_CONVEX_SWEEP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom.getType() == PxGeometryType::eBOX);
+	const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom);
+
+	Box box;
+	buildFrom(box, pose.p, boxGeom.halfExtents, pose.q);
+
+	// remove PxHitFlag::eFACE_INDEX, not neeeded to compute.
+	PxHitFlags tempHitFlags = hitFlags;
+	tempHitFlags &= ~PxHitFlag::eFACE_INDEX;
+
+	if(!sweepBox_ConvexGeom(convexGeom, convexPose, boxGeom, pose, box, -unitDir, distance, sweepHit, tempHitFlags, inflation))
+		return false;
+
+	if(sweepHit.flags & PxHitFlag::ePOSITION)
+		sweepHit.position += unitDir * sweepHit.distance;
+
+	sweepHit.normal = -sweepHit.normal;
+	sweepHit.faceIndex = 0xffffffff;
+	return true;
+}
+
+bool sweepConvex_ConvexGeom(GU_CONVEX_SWEEP_FUNC_PARAMS)
+{
+	using namespace Ps::aos;
+	PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);
+	const PxConvexMeshGeometry& otherConvexGeom = static_cast<const PxConvexMeshGeometry&>(geom);
+	ConvexMesh& otherConvexMesh = *static_cast<ConvexMesh*>(otherConvexGeom.convexMesh);
+
+	ConvexMesh* convexMesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
+	ConvexHullData* hullData = &convexMesh->getHull();
+
+	ConvexHullData* otherHullData = &otherConvexMesh.getHull();
+	
+	const Vec3V zeroV = V3Zero();
+	const FloatV zero = FZero();
+
+	const Vec3V otherVScale = V3LoadU_SafeReadW(otherConvexGeom.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
+	const QuatV otherVQuat = QuatVLoadU(&otherConvexGeom.scale.rotation.x);
+
+	const Vec3V vScale = V3LoadU_SafeReadW(convexGeom.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
+	const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x);
+
+	const PsTransformV otherTransf = loadTransformU(pose);
+	const PsTransformV convexTransf = loadTransformU(convexPose);
+
+	const Vec3V worldDir = V3LoadU(unitDir);
+	const FloatV dist = FLoad(distance);
+	const Vec3V dir = convexTransf.rotateInv(V3Scale(worldDir, dist));
+
+	const PsMatTransformV aToB(convexTransf.transformInv(otherTransf));
+	
+	ConvexHullV otherConvexHull(otherHullData, zeroV, otherVScale, otherVQuat, otherConvexGeom.scale.isIdentity());
+	ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, convexGeom.scale.isIdentity());
+
+	const bool isMtd = hitFlags & PxHitFlag::eMTD;
+
+	FloatV toi;
+	Vec3V closestA, normal;
+	RelativeConvex<ConvexHullV> convexA(otherConvexHull, aToB);
+	LocalConvex<ConvexHullV> convexB(convexHull);
+	if(!gjkRaycastPenetration< RelativeConvex<ConvexHullV>, LocalConvex<ConvexHullV> >(convexA, convexB, aToB.p, zero, zeroV, dir, toi, normal, closestA, inflation, isMtd))
+		return false;
+
+	if(hasInitialOverlap(sweepHit, unitDir, toi, normal, closestA, convexPose, isMtd, false))
+		return true;
+
+	sweepHit.flags |= PxHitFlag::ePOSITION;
+	const Vec3V worldPointA = convexTransf.transform(closestA);
+	const Vec3V destNormal = V3Neg(V3Normalize(convexTransf.rotate(normal)));
+	const FloatV length = FMul(dist, toi);
+	V3StoreU(destNormal, sweepHit.normal);
+	V3StoreU(worldPointA, sweepHit.position);
+	FStore(length, &sweepHit.distance);
+
+	return computeFaceIndex(sweepHit, hitFlags, otherConvexGeom, otherHullData, pose, unitDir);
+}

physx/source/geomutils/src/GuSweepSharedTests.h

@@ -0,0 +1,56 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_SWEEP_SHARED_TESTS_H
+#define GU_SWEEP_SHARED_TESTS_H
+
+#include "CmPhysXCommon.h"
+#include "GuBoxConversion.h"
+
+namespace physx
+{
+PX_FORCE_INLINE void computeWorldToBoxMatrix(physx::Cm::Matrix34& worldToBox, const physx::Gu::Box& box)
+{
+	physx::Cm::Matrix34 boxToWorld;
+	physx::buildMatrixFromBox(boxToWorld, box);
+	worldToBox = boxToWorld.getInverseRT();
+}
+
+PX_FORCE_INLINE PxU32 getTriangleIndex(PxU32 i, PxU32 cachedIndex)
+{
+	PxU32 triangleIndex;
+	if(i==0)				triangleIndex = cachedIndex;
+	else if(i==cachedIndex)	triangleIndex = 0;
+	else					triangleIndex = i;
+	return triangleIndex;
+}
+}
+
+
+#endif

physx/source/geomutils/src/GuSweepTests.cpp

@@ -0,0 +1,604 @@
+//
+// 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-2021 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/PxSphereGeometry.h"
+#include "GuSweepTests.h"
+#include "GuVecCapsule.h"
+#include "GuVecBox.h"
+#include "GuVecTriangle.h"
+#include "GuSweepTriangleUtils.h"
+#include "GuInternal.h"
+#include "PsFoundation.h"
+#include "GuGJKRaycast.h"
+
+using namespace physx;
+using namespace Gu;
+using namespace Cm;
+using namespace physx::shdfnd::aos;  
+
+bool sweepCapsule_BoxGeom(GU_CAPSULE_SWEEP_FUNC_PARAMS)
+{
+	PX_UNUSED(hitFlags);
+
+	using namespace Ps::aos;
+	PX_ASSERT(geom.getType() == PxGeometryType::eBOX);
+	const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom);
+
+	const FloatV zero = FZero();
+	const Vec3V zeroV = V3Zero();
+	const Vec3V boxExtents0 = V3LoadU(boxGeom.halfExtents);
+	const FloatV dist = FLoad(distance);
+	const Vec3V worldDir = V3LoadU(unitDir);
+
+	const PsTransformV capPos = loadTransformU(capsulePose_);
+	const PsTransformV boxPos = loadTransformU(pose);
+
+	const PsMatTransformV aToB(boxPos.transformInv(capPos));
+
+	const FloatV capsuleHalfHeight = FLoad(capsuleGeom_.halfHeight);
+	const FloatV capsuleRadius = FLoad(lss.radius);
+
+	BoxV box(zeroV, boxExtents0);
+	CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius);
+
+	const Vec3V dir = boxPos.rotateInv(V3Neg(V3Scale(worldDir, dist)));
+
+	const bool isMtd = hitFlags & PxHitFlag::eMTD;
+	FloatV toi = FMax();
+	Vec3V closestA, normal;//closestA and normal is in the local space of box
+	LocalConvex<CapsuleV> convexA(capsule);
+	LocalConvex<BoxV> convexB(box);
+	const Vec3V initialSearchDir = V3Sub(capsule.getCenter(), box.getCenter());
+	if(!gjkRaycastPenetration<LocalConvex<CapsuleV>, LocalConvex<BoxV> >(convexA, convexB, initialSearchDir, zero, zeroV, dir, toi, normal, 
+		closestA, lss.radius + inflation, isMtd))
+		return false;
+
+	sweepHit.flags = PxHitFlag::eNORMAL;
+	if(FAllGrtrOrEq(zero, toi))
+	{
+		//initial overlap
+		if(isMtd)
+		{
+			sweepHit.flags |= PxHitFlag::ePOSITION;
+			const Vec3V worldPointA = boxPos.transform(closestA);
+			const Vec3V destNormal = boxPos.rotate(normal);
+			const FloatV length = toi;
+			const Vec3V destWorldPointA = V3NegScaleSub(destNormal, length, worldPointA);
+			V3StoreU(destWorldPointA, sweepHit.position);
+			V3StoreU(destNormal, sweepHit.normal);
+			FStore(length, &sweepHit.distance);
+		}
+		else
+		{
+			sweepHit.distance	= 0.0f;
+			sweepHit.normal		= -unitDir;
+		}
+	}
+	else
+	{
+		sweepHit.flags |= PxHitFlag::ePOSITION;
+		const Vec3V worldPointA = boxPos.transform(closestA);
+		const Vec3V destNormal = boxPos.rotate(normal);
+		const FloatV length = FMul(dist, toi);
+		const Vec3V destWorldPointA = V3ScaleAdd(worldDir, length, worldPointA);
+		V3StoreU(destNormal, sweepHit.normal);
+		V3StoreU(destWorldPointA, sweepHit.position);
+		FStore(length, &sweepHit.distance);
+	}
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+bool sweepBox_SphereGeom(GU_BOX_SWEEP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom.getType() == PxGeometryType::eSPHERE);
+	PX_UNUSED(hitFlags);
+	PX_UNUSED(boxGeom_);
+
+	const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom);
+
+	const FloatV zero = FZero();
+	const Vec3V zeroV = V3Zero();
+	const Vec3V boxExtents = V3LoadU(box.extents);
+	const FloatV worldDist = FLoad(distance);
+	const Vec3V  unitDirV = V3LoadU(unitDir);
+
+	const FloatV sphereRadius = FLoad(sphereGeom.radius);
+
+	const PsTransformV spherePos = loadTransformU(pose);
+	const PsTransformV boxPos = loadTransformU(boxPose_);
+
+	const PsMatTransformV aToB(boxPos.transformInv(spherePos));
+
+	BoxV boxV(zeroV, boxExtents);
+	CapsuleV capsuleV(aToB.p, sphereRadius);
+
+	//transform into b space
+	const Vec3V dir = boxPos.rotateInv(V3Scale(unitDirV, worldDist));
+
+	bool isMtd = hitFlags & PxHitFlag::eMTD;
+	FloatV toi;
+	Vec3V closestA, normal;//closestA and normal is in the local space of box
+	const Vec3V initialSearchDir = V3Sub(capsuleV.getCenter(), boxV.getCenter());
+	LocalConvex<CapsuleV> convexA(capsuleV);
+	LocalConvex<BoxV> convexB(boxV);
+	if(!gjkRaycastPenetration< LocalConvex<CapsuleV>, LocalConvex<BoxV> >(convexA, convexB, initialSearchDir, zero, zeroV, dir, toi, normal, closestA, sphereGeom.radius+inflation, isMtd))
+		return false;
+
+	sweepHit.flags = PxHitFlag::eNORMAL;
+
+	//initial overlap
+	if(FAllGrtrOrEq(zero, toi))
+	{
+		if(isMtd)
+		{
+			sweepHit.flags |= PxHitFlag::ePOSITION;
+			const Vec3V destWorldPointA = boxPos.transform(closestA);
+			const Vec3V destNormal = V3Neg(boxPos.rotate(normal));
+			const FloatV length = toi;
+			V3StoreU(destNormal, sweepHit.normal);
+			V3StoreU(destWorldPointA, sweepHit.position);
+			FStore(length, &sweepHit.distance);
+		}
+		else
+		{
+			sweepHit.distance	= 0.0f;
+			sweepHit.normal		= -unitDir;
+		}
+	}
+	else
+	{
+		sweepHit.flags |= PxHitFlag::ePOSITION;
+		const Vec3V destWorldPointA = boxPos.transform(closestA);
+		const Vec3V destNormal = V3Neg(boxPos.rotate(normal));
+		const FloatV length = FMul(worldDist, toi);
+		V3StoreU(destNormal, sweepHit.normal);
+		V3StoreU(destWorldPointA, sweepHit.position);
+		FStore(length, &sweepHit.distance);
+	}
+	return true;
+}
+
+bool sweepBox_CapsuleGeom(GU_BOX_SWEEP_FUNC_PARAMS)
+{
+	using namespace Ps::aos;
+	PX_ASSERT(geom.getType() == PxGeometryType::eCAPSULE);
+	PX_UNUSED(hitFlags);
+	PX_UNUSED(boxGeom_);
+
+	const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom);
+
+	const FloatV capsuleHalfHeight = FLoad(capsuleGeom.halfHeight);
+	const FloatV capsuleRadius = FLoad(capsuleGeom.radius);
+
+	const FloatV zero = FZero();
+	const Vec3V zeroV = V3Zero();
+	const Vec3V boxExtents = V3LoadU(box.extents);
+	const FloatV worldDist = FLoad(distance);
+	const Vec3V  unitDirV = V3LoadU(unitDir);
+
+	const PsTransformV capPos = loadTransformU(pose);
+	const PsTransformV boxPos = loadTransformU(boxPose_);
+
+	const PsMatTransformV aToB(boxPos.transformInv(capPos));
+
+	BoxV boxV(zeroV, boxExtents);
+	CapsuleV capsuleV(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius);
+
+	//transform into b space
+	const Vec3V dir = boxPos.rotateInv(V3Scale(unitDirV, worldDist));
+
+	const bool isMtd = hitFlags & PxHitFlag::eMTD;
+	FloatV toi;
+	Vec3V closestA, normal;//closestA and normal is in the local space of box
+	const Vec3V initialSearchDir = V3Sub(capsuleV.getCenter(), boxV.getCenter());
+	LocalConvex<CapsuleV> convexA(capsuleV);
+	LocalConvex<BoxV> convexB(boxV);
+	if(!gjkRaycastPenetration< LocalConvex<CapsuleV>, LocalConvex<BoxV> >(convexA, convexB, initialSearchDir, zero, zeroV, dir, toi, normal, closestA, capsuleGeom.radius+inflation, isMtd))
+		return false;
+
+	sweepHit.flags = PxHitFlag::eNORMAL;
+
+	//initial overlap
+	if(FAllGrtrOrEq(zero, toi))
+	{
+		if(isMtd)
+		{
+			sweepHit.flags |= PxHitFlag::ePOSITION;
+			//initial overlap is toi < 0 
+			const FloatV length = toi;
+			const Vec3V destWorldPointA = boxPos.transform(closestA);
+			const Vec3V destNormal = boxPos.rotate(normal);
+			V3StoreU(V3Neg(destNormal), sweepHit.normal);
+			V3StoreU(destWorldPointA, sweepHit.position);
+			FStore(length, &sweepHit.distance);
+		}
+		else
+		{
+			sweepHit.distance	= 0.0f;
+			sweepHit.normal		= -unitDir;
+		}
+		return true;
+	}
+	else
+	{
+		sweepHit.flags |= PxHitFlag::ePOSITION;
+		const Vec3V destWorldPointA = boxPos.transform(closestA);
+		const Vec3V destNormal = boxPos.rotate(normal);
+		const FloatV length = FMul(worldDist, toi);
+		V3StoreU(V3Neg(destNormal), sweepHit.normal);
+		V3StoreU(destWorldPointA, sweepHit.position);
+		FStore(length, &sweepHit.distance);
+	}
+	return true;	
+}
+
+bool sweepBox_BoxGeom(GU_BOX_SWEEP_FUNC_PARAMS)
+{
+	PX_ASSERT(geom.getType() == PxGeometryType::eBOX);
+	PX_UNUSED(boxGeom_);
+
+	const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom);
+
+	const FloatV zero = FZero();
+	const Vec3V zeroV = V3Zero();
+	const Vec3V boxExtents0 = V3LoadU(boxGeom.halfExtents);
+	const Vec3V boxExtents1 = V3LoadU(box.extents);
+	const FloatV worldDist = FLoad(distance);
+	const Vec3V  unitDirV = V3LoadU(unitDir);
+
+	const PsTransformV boxTrans0 = loadTransformU(pose);
+	const PsTransformV boxTrans1 = loadTransformU(boxPose_);
+
+	const PsMatTransformV aToB(boxTrans1.transformInv(boxTrans0));
+
+	BoxV box0(zeroV, boxExtents0);
+	BoxV box1(zeroV, boxExtents1);
+
+	//transform into b space
+	const Vec3V dir = boxTrans1.rotateInv(V3Scale(unitDirV, worldDist));
+	const bool isMtd = hitFlags & PxHitFlag::eMTD;
+	FloatV toi;
+	Vec3V closestA, normal;//closestA and normal is in the local space of box
+	RelativeConvex<BoxV> convexA(box0, aToB);
+	LocalConvex<BoxV> convexB(box1);
+	if(!gjkRaycastPenetration<RelativeConvex<BoxV>, LocalConvex<BoxV> >(convexA, convexB, aToB.p, zero, zeroV, dir, toi, normal, closestA, inflation, isMtd))
+		return false;
+	
+	sweepHit.flags = PxHitFlag::eNORMAL;
+	if(FAllGrtrOrEq(zero, toi))
+	{
+		if(isMtd)
+		{
+			sweepHit.flags |= PxHitFlag::ePOSITION;
+			const FloatV length = toi;
+			const Vec3V destWorldPointA = boxTrans1.transform(closestA);
+			const Vec3V destNormal = V3Normalize(boxTrans1.rotate(normal));
+			V3StoreU(V3Neg(destNormal), sweepHit.normal);
+			V3StoreU(destWorldPointA, sweepHit.position);
+			FStore(length, &sweepHit.distance);
+		}
+		else
+		{
+			sweepHit.distance	= 0.0f;
+			sweepHit.normal		= -unitDir;
+		}
+	}
+	else
+	{
+		sweepHit.flags |= PxHitFlag::ePOSITION;
+		const Vec3V destWorldPointA = boxTrans1.transform(closestA);
+		const Vec3V destNormal = V3Normalize(boxTrans1.rotate(normal));
+		const FloatV length = FMul(worldDist, toi);
+		V3StoreU(V3Neg(destNormal), sweepHit.normal);
+		V3StoreU(destWorldPointA, sweepHit.position);
+		FStore(length, &sweepHit.distance);
+	}
+	return true;
+}
+
+bool Gu::sweepBoxTriangles(GU_SWEEP_TRIANGLES_FUNC_PARAMS(PxBoxGeometry))
+{
+	PX_UNUSED(hitFlags);
+
+	if(!nbTris)
+		return false;
+
+	const bool meshBothSides = hitFlags & PxHitFlag::eMESH_BOTH_SIDES;
+	const bool doBackfaceCulling = !doubleSided && !meshBothSides;
+
+	Box box;
+	buildFrom(box, pose.p, geom.halfExtents, pose.q);
+
+	PxSweepHit sweepHit;
+	// Move to AABB space
+	Matrix34 worldToBox;
+	computeWorldToBoxMatrix(worldToBox, box);
+
+	const PxVec3 localDir = worldToBox.rotate(unitDir);
+	const PxVec3 localMotion = localDir * distance;
+
+	const Vec3V base0 = V3LoadU(worldToBox.m.column0);
+	const Vec3V base1 = V3LoadU(worldToBox.m.column1);
+	const Vec3V base2 = V3LoadU(worldToBox.m.column2);
+	const Mat33V matV(base0, base1, base2);
+	const Vec3V p	  = V3LoadU(worldToBox.p);
+	const PsMatTransformV worldToBoxV(p, matV);
+
+	const FloatV zero = FZero();
+	const Vec3V zeroV = V3Zero();
+	const Vec3V boxExtents = V3LoadU(box.extents);
+	const Vec3V boxDir = V3LoadU(localDir);
+	const FloatV inflationV = FLoad(inflation);
+	const Vec3V absBoxDir = V3Abs(boxDir);
+	const FloatV boxRadiusV = FAdd(V3Dot(absBoxDir, boxExtents), inflationV);
+	BoxV boxV(zeroV, boxExtents);
+
+#if PX_DEBUG
+	PxU32 totalTestsExpected = nbTris;
+	PxU32 totalTestsReal = 0;
+	PX_UNUSED(totalTestsExpected);
+	PX_UNUSED(totalTestsReal);
+#endif
+
+	Vec3V boxLocalMotion = V3LoadU(localMotion);
+	Vec3V minClosestA = zeroV, minNormal = zeroV;
+	PxU32 minTriangleIndex = 0;
+	PxVec3 bestTriNormal(0.0f);
+	FloatV dist = FLoad(distance);
+
+	const PsTransformV boxPos = loadTransformU(pose);
+
+	bool status = false;
+
+	const PxU32 idx = cachedIndex ? *cachedIndex : 0;
+
+	for(PxU32 ii=0;ii<nbTris;ii++)
+	{
+		const PxU32 triangleIndex = getTriangleIndex(ii, idx);
+
+		const Vec3V localV0 =  V3LoadU(triangles[triangleIndex].verts[0]);
+		const Vec3V localV1 =  V3LoadU(triangles[triangleIndex].verts[1]);
+		const Vec3V localV2 =  V3LoadU(triangles[triangleIndex].verts[2]);
+
+		const Vec3V triV0 = worldToBoxV.transform(localV0);
+		const Vec3V triV1 = worldToBoxV.transform(localV1);
+		const Vec3V triV2 = worldToBoxV.transform(localV2);
+
+		const Vec3V triNormal = V3Cross(V3Sub(triV2, triV1),V3Sub(triV0, triV1)); 
+
+		if(doBackfaceCulling && FAllGrtrOrEq(V3Dot(triNormal, boxLocalMotion), zero)) // backface culling
+			continue;
+
+		const FloatV dp0 = V3Dot(triV0, boxDir);
+		const FloatV dp1 = V3Dot(triV1, boxDir);
+		const FloatV dp2 = V3Dot(triV2, boxDir);
+		
+		const FloatV dp = FMin(dp0, FMin(dp1, dp2));
+
+		const Vec3V dpV = V3Merge(dp0, dp1, dp2);
+
+		const FloatV temp1 = FAdd(boxRadiusV, dist);
+		const BoolV con0 = FIsGrtr(dp, temp1);
+		const BoolV con1 = V3IsGrtr(zeroV, dpV);
+
+		if(BAllEqTTTT(BOr(con0, con1)))
+			continue;
+
+#if PX_DEBUG
+		totalTestsReal++;
+#endif
+
+		TriangleV triangleV(triV0, triV1, triV2);
+		
+		FloatV lambda;   
+		Vec3V closestA, normal;//closestA and normal is in the local space of convex hull
+		LocalConvex<TriangleV> convexA(triangleV);
+		LocalConvex<BoxV> convexB(boxV);
+		const Vec3V initialSearchDir = V3Sub(triangleV.getCenter(), boxV.getCenter());
+		if(gjkRaycastPenetration<LocalConvex<TriangleV>, LocalConvex<BoxV> >(convexA, convexB, initialSearchDir, zero, zeroV, boxLocalMotion, lambda, normal, closestA, inflation, false))
+		{
+			//hitCount++;
+		
+			if(FAllGrtrOrEq(zero, lambda))
+			{
+				hit.distance	= 0.0f;
+				hit.faceIndex	= triangleIndex;
+				hit.normal		= -unitDir;
+				hit.flags		= PxHitFlag::eNORMAL;
+				return true;
+			}
+
+			dist = FMul(dist, lambda);
+			boxLocalMotion = V3Scale(boxDir, dist);  
+			minClosestA = closestA;
+			minNormal = normal;
+			minTriangleIndex = triangleIndex;
+			V3StoreU(triNormal, bestTriNormal);
+			status = true;
+			if(hitFlags & PxHitFlag::eMESH_ANY)
+				break;
+		}
+	}
+
+	if(!status)
+		return false;
+
+	hit.faceIndex	= minTriangleIndex;
+	const Vec3V destNormal = V3Neg(V3Normalize(boxPos.rotate(minNormal)));
+	const Vec3V destWorldPointA = boxPos.transform(minClosestA);
+	V3StoreU(destNormal, hit.normal);
+	V3StoreU(destWorldPointA, hit.position);
+	FStore(dist, &hit.distance);
+
+	// PT: by design, returned normal is opposed to the sweep direction.
+	if(shouldFlipNormal(hit.normal, meshBothSides, doubleSided, bestTriNormal, unitDir))
+		hit.normal = -hit.normal;
+
+	hit.flags = PxHitFlag::ePOSITION|PxHitFlag::eNORMAL;
+	return true;
+}
+
+bool sweepCapsule_SphereGeom		(GU_CAPSULE_SWEEP_FUNC_PARAMS);
+bool sweepCapsule_PlaneGeom			(GU_CAPSULE_SWEEP_FUNC_PARAMS);
+bool sweepCapsule_CapsuleGeom		(GU_CAPSULE_SWEEP_FUNC_PARAMS);
+bool sweepCapsule_BoxGeom			(GU_CAPSULE_SWEEP_FUNC_PARAMS);
+bool sweepCapsule_BoxGeom_Precise	(GU_CAPSULE_SWEEP_FUNC_PARAMS);
+bool sweepCapsule_ConvexGeom		(GU_CAPSULE_SWEEP_FUNC_PARAMS);
+bool sweepCapsule_MeshGeom			(GU_CAPSULE_SWEEP_FUNC_PARAMS);
+bool sweepCapsule_HeightFieldGeom	(GU_CAPSULE_SWEEP_FUNC_PARAMS);
+
+bool sweepBox_SphereGeom			(GU_BOX_SWEEP_FUNC_PARAMS);
+bool sweepBox_SphereGeom_Precise	(GU_BOX_SWEEP_FUNC_PARAMS);
+bool sweepBox_PlaneGeom				(GU_BOX_SWEEP_FUNC_PARAMS);
+bool sweepBox_CapsuleGeom			(GU_BOX_SWEEP_FUNC_PARAMS);
+bool sweepBox_CapsuleGeom_Precise	(GU_BOX_SWEEP_FUNC_PARAMS);
+bool sweepBox_BoxGeom				(GU_BOX_SWEEP_FUNC_PARAMS);
+bool sweepBox_BoxGeom_Precise		(GU_BOX_SWEEP_FUNC_PARAMS);
+bool sweepBox_ConvexGeom			(GU_BOX_SWEEP_FUNC_PARAMS);
+bool sweepBox_MeshGeom				(GU_BOX_SWEEP_FUNC_PARAMS);
+bool sweepBox_HeightFieldGeom		(GU_BOX_SWEEP_FUNC_PARAMS);
+bool sweepBox_HeightFieldGeom_Precise(GU_BOX_SWEEP_FUNC_PARAMS);
+
+bool sweepConvex_SphereGeom			(GU_CONVEX_SWEEP_FUNC_PARAMS);
+bool sweepConvex_PlaneGeom			(GU_CONVEX_SWEEP_FUNC_PARAMS);
+bool sweepConvex_CapsuleGeom		(GU_CONVEX_SWEEP_FUNC_PARAMS);
+bool sweepConvex_BoxGeom			(GU_CONVEX_SWEEP_FUNC_PARAMS);
+bool sweepConvex_ConvexGeom			(GU_CONVEX_SWEEP_FUNC_PARAMS);
+bool sweepConvex_MeshGeom			(GU_CONVEX_SWEEP_FUNC_PARAMS);
+bool sweepConvex_HeightFieldGeom	(GU_CONVEX_SWEEP_FUNC_PARAMS);
+
+static bool sweepCapsule_HeightfieldUnregistered(GU_CAPSULE_SWEEP_FUNC_PARAMS)
+{
+	PX_UNUSED(capsuleGeom_);
+	PX_UNUSED(capsulePose_);
+	PX_UNUSED(geom);
+	PX_UNUSED(pose);
+	PX_UNUSED(lss);
+	PX_UNUSED(unitDir);
+	PX_UNUSED(distance);
+	PX_UNUSED(sweepHit);
+	PX_UNUSED(hitFlags);
+	PX_UNUSED(inflation);
+	Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, "Height Field Sweep test called with height fields unregistered ");
+	return false;
+}
+static bool sweepBox_HeightfieldUnregistered(GU_BOX_SWEEP_FUNC_PARAMS)
+{
+	PX_UNUSED(boxPose_);
+	PX_UNUSED(boxGeom_);
+	PX_UNUSED(geom);
+	PX_UNUSED(pose);
+	PX_UNUSED(box);
+	PX_UNUSED(unitDir);
+	PX_UNUSED(distance);
+	PX_UNUSED(sweepHit);
+	PX_UNUSED(hitFlags);
+	PX_UNUSED(inflation);
+	Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, "Height Field Sweep test called with height fields unregistered ");
+	return false;
+}
+static bool sweepConvex_HeightfieldUnregistered(GU_CONVEX_SWEEP_FUNC_PARAMS)
+{
+	PX_UNUSED(geom);
+	PX_UNUSED(pose);
+	PX_UNUSED(convexGeom);
+	PX_UNUSED(convexPose);
+	PX_UNUSED(unitDir);
+	PX_UNUSED(distance);
+	PX_UNUSED(sweepHit);
+	PX_UNUSED(hitFlags);
+	PX_UNUSED(inflation);
+	Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, "Height Field Sweep test called with height fields unregistered ");
+	return false;
+}
+
+Gu::GeomSweepFuncs gGeomSweepFuncs =
+{
+	{
+		sweepCapsule_SphereGeom,
+		sweepCapsule_PlaneGeom,
+		sweepCapsule_CapsuleGeom,
+		sweepCapsule_BoxGeom,
+		sweepCapsule_ConvexGeom,
+		sweepCapsule_MeshGeom,
+		sweepCapsule_HeightfieldUnregistered
+	},
+	{
+		sweepCapsule_SphereGeom,
+		sweepCapsule_PlaneGeom,
+		sweepCapsule_CapsuleGeom,
+		sweepCapsule_BoxGeom_Precise,
+		sweepCapsule_ConvexGeom,
+		sweepCapsule_MeshGeom ,
+		sweepCapsule_HeightfieldUnregistered
+	},
+	{
+		sweepBox_SphereGeom,
+		sweepBox_PlaneGeom,
+		sweepBox_CapsuleGeom,
+		sweepBox_BoxGeom,
+		sweepBox_ConvexGeom,
+		sweepBox_MeshGeom,		
+		sweepBox_HeightfieldUnregistered
+	},
+	{
+		sweepBox_SphereGeom_Precise,
+		sweepBox_PlaneGeom,
+		sweepBox_CapsuleGeom_Precise,
+		sweepBox_BoxGeom_Precise,
+		sweepBox_ConvexGeom,
+		sweepBox_MeshGeom,		
+		sweepBox_HeightfieldUnregistered
+	},
+	{
+		sweepConvex_SphereGeom,		// 0
+		sweepConvex_PlaneGeom,		// 1
+		sweepConvex_CapsuleGeom,	// 2
+		sweepConvex_BoxGeom,		// 3
+		sweepConvex_ConvexGeom,		// 4
+		sweepConvex_MeshGeom,		// 5			
+		sweepConvex_HeightfieldUnregistered	// 6
+	}
+};
+
+PX_PHYSX_COMMON_API const GeomSweepFuncs& Gu::getSweepFuncTable()
+{
+	return gGeomSweepFuncs;
+}
+
+void registerHeightFields_Sweeps()
+{
+	gGeomSweepFuncs.capsuleMap[PxGeometryType::eHEIGHTFIELD] = sweepCapsule_HeightFieldGeom;
+	gGeomSweepFuncs.preciseCapsuleMap[PxGeometryType::eHEIGHTFIELD] = sweepCapsule_HeightFieldGeom;
+	gGeomSweepFuncs.boxMap[PxGeometryType::eHEIGHTFIELD] = sweepBox_HeightFieldGeom;
+	gGeomSweepFuncs.preciseBoxMap[PxGeometryType::eHEIGHTFIELD] = sweepBox_HeightFieldGeom_Precise;
+	gGeomSweepFuncs.convexMap[PxGeometryType::eHEIGHTFIELD] = sweepConvex_HeightFieldGeom;
+}

physx/source/geomutils/src/GuSweepTests.h

@@ -0,0 +1,136 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_SWEEP_TESTS_H
+#define GU_SWEEP_TESTS_H
+
+#include "PxQueryReport.h"
+#include "geometry/PxGeometry.h"
+#include "CmPhysXCommon.h"
+
+namespace physx
+{
+	class PxConvexMeshGeometry;
+	class PxCapsuleGeometry;
+	class PxTriangle;
+	class PxBoxGeometry;
+
+	// PT: TODO: unify this with raycast calls (names and order of params)
+
+	// PT: we use defines to be able to quickly change the signature of all sweep functions.
+	// (this also ensures they all use consistent names for passed parameters).
+	// \param[in]	geom		geometry object to sweep against
+	// \param[in]	pose		pose of geometry object
+	// \param[in]	unitDir		sweep's unit dir
+	// \param[in]	distance	sweep's length/max distance
+	// \param[out]	sweepHit	hit result
+	// \param[in]	hitFlags	query behavior flags
+	// \param[in]	inflation	optional inflation value for swept shape
+
+	// PT: sweep parameters for capsule
+	#define GU_CAPSULE_SWEEP_FUNC_PARAMS	const PxGeometry& geom, const PxTransform& pose,												\
+											const PxCapsuleGeometry& capsuleGeom_, const PxTransform& capsulePose_, const Gu::Capsule& lss,	\
+											const PxVec3& unitDir, PxReal distance,															\
+											PxSweepHit& sweepHit, const PxHitFlags hitFlags, PxReal inflation 
+
+	// PT: sweep parameters for box
+	#define GU_BOX_SWEEP_FUNC_PARAMS	const PxGeometry& geom, const PxTransform& pose,								\
+										const PxBoxGeometry& boxGeom_, const PxTransform& boxPose_, const Gu::Box& box,	\
+										const PxVec3& unitDir, PxReal distance,											\
+										PxSweepHit& sweepHit, const PxHitFlags hitFlags, PxReal inflation 
+
+	// PT: sweep parameters for convex
+	#define GU_CONVEX_SWEEP_FUNC_PARAMS		const PxGeometry& geom, const PxTransform& pose,						\
+											const PxConvexMeshGeometry& convexGeom, const PxTransform& convexPose,	\
+											const PxVec3& unitDir, PxReal distance,									\
+											PxSweepHit& sweepHit, const PxHitFlags hitFlags, PxReal inflation
+	namespace Gu
+	{
+		class Capsule;
+		class Box;
+
+		// PT: function pointer for Geom-indexed capsule sweep functions
+		// See GU_CAPSULE_SWEEP_FUNC_PARAMS for function parameters details.
+		// \return		true if a hit was found, false otherwise
+		typedef bool (*SweepCapsuleFunc)	(GU_CAPSULE_SWEEP_FUNC_PARAMS);
+
+		// PT: function pointer for Geom-indexed box sweep functions
+		// See GU_BOX_SWEEP_FUNC_PARAMS for function parameters details.
+		// \return		true if a hit was found, false otherwise
+		typedef bool (*SweepBoxFunc)		(GU_BOX_SWEEP_FUNC_PARAMS);
+
+		// PT: function pointer for Geom-indexed box sweep functions
+		// See GU_CONVEX_SWEEP_FUNC_PARAMS for function parameters details.
+		// \return		true if a hit was found, false otherwise
+		typedef bool (*SweepConvexFunc)		(GU_CONVEX_SWEEP_FUNC_PARAMS);
+
+		// PT: typedef for bundles of all sweep functions, i.e. the function tables themselves (indexed by geom-type).
+		typedef SweepCapsuleFunc	GeomSweepCapsuleTable	[PxGeometryType::eGEOMETRY_COUNT];
+		typedef SweepBoxFunc		GeomSweepBoxTable		[PxGeometryType::eGEOMETRY_COUNT];
+		typedef SweepConvexFunc		GeomSweepConvexTable	[PxGeometryType::eGEOMETRY_COUNT];
+
+		struct GeomSweepFuncs
+		{
+			GeomSweepCapsuleTable	capsuleMap;
+			GeomSweepCapsuleTable	preciseCapsuleMap;
+			GeomSweepBoxTable		boxMap;
+			GeomSweepBoxTable		preciseBoxMap;
+			GeomSweepConvexTable	convexMap;
+		};
+		// PT: grabs all sweep function tables at once (for access by external non-Gu modules)
+		PX_PHYSX_COMMON_API const GeomSweepFuncs& getSweepFuncTable();
+
+		// PT: signature for sweep-vs-triangles functions.
+		// We use defines to be able to quickly change the signature of all sweep functions.
+		// (this also ensures they all use consistent names for passed parameters).
+		// \param[in]	nbTris			number of triangles in input array
+		// \param[in]	triangles		array of triangles to sweep the shape against
+		// \param[in]	doubleSided		true if input triangles are double-sided
+		// \param[in]	x				geom to sweep against input triangles
+		// \param[in]	pose			pose of geom x
+		// \param[in]	unitDir			sweep's unit dir
+		// \param[in]	distance		sweep's length/max distance
+		// \param[out]	hit				hit result
+		// \param[in]	cachedIndex		optional initial triangle index (must be <nbTris)
+		// \param[in]	inflation		optional inflation value for swept shape
+		// \param[in]	hitFlags		query behavior flags
+		#define GU_SWEEP_TRIANGLES_FUNC_PARAMS(x)	PxU32 nbTris, const PxTriangle* triangles, bool doubleSided,	\
+													const x& geom, const PxTransform& pose,							\
+													const PxVec3& unitDir, const PxReal distance,					\
+													PxSweepHit& hit, const PxU32* cachedIndex,						\
+													const PxReal inflation, PxHitFlags hitFlags
+
+		bool sweepCapsuleTriangles		(GU_SWEEP_TRIANGLES_FUNC_PARAMS(PxCapsuleGeometry));
+		bool sweepBoxTriangles			(GU_SWEEP_TRIANGLES_FUNC_PARAMS(PxBoxGeometry));
+		bool sweepBoxTriangles_Precise	(GU_SWEEP_TRIANGLES_FUNC_PARAMS(PxBoxGeometry));
+
+	}  // namespace Gu
+}
+
+#endif

physx/source/geomutils/src/ccd/GuCCDSweepConvexMesh.cpp

@@ -0,0 +1,728 @@
+//
+// 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-2021 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 "Ps.h"
+#include "GuVecCapsule.h"
+#include "GuVecBox.h"
+#include "GuVecConvexHull.h"
+#include "GuVecTriangle.h"
+#include "GuGJKRaycast.h"
+#include "GuCCDSweepConvexMesh.h"
+#include "GuHeightFieldUtil.h"
+#include "PsInlineArray.h"
+#include "GuEntityReport.h"
+#include "PxContact.h"
+#include "GuDistancePointTriangle.h"
+#include "GuBox.h"
+#include "GuInternal.h"
+#include "GuBoxConversion.h"
+#include "GuConvexUtilsInternal.h"
+#include "GuMidphaseInterface.h"
+#include "GuTriangleVertexPointers.h"
+
+
+
+namespace physx
+{
+namespace Gu
+{
+
+PxReal SweepShapeTriangle(GU_TRIANGLE_SWEEP_METHOD_ARGS);
+
+using namespace Ps::aos;
+
+namespace 
+{
+struct AccumCallback: public MeshHitCallback<PxRaycastHit>
+{
+	PX_NOCOPY(AccumCallback)
+	public:
+
+	Ps::InlineArray<PxU32, 64>& mResult;
+	AccumCallback(Ps::InlineArray<PxU32, 64>& result)
+		:	MeshHitCallback<PxRaycastHit>(CallbackMode::eMULTIPLE),
+			mResult(result)
+	{
+	}
+
+	virtual PxAgain processHit( // all reported coords are in mesh local space including hit.position
+		const PxRaycastHit& hit, const PxVec3&, const PxVec3&, const PxVec3&, PxReal&, const PxU32*)
+	{
+		mResult.pushBack(hit.faceIndex);
+		return true;
+	}
+};
+
+// PT: TODO: refactor with MidPhaseQueryLocalReport
+struct EntityReportContainerCallback : public EntityReport<PxU32>
+{
+	Ps::InlineArray<PxU32, 64>& container;
+	EntityReportContainerCallback(Ps::InlineArray<PxU32,64>& container_) : container(container_)
+	{
+		container.forceSize_Unsafe(0);
+	}
+	virtual ~EntityReportContainerCallback() {}
+
+	virtual bool onEvent(PxU32 nb, PxU32* indices)
+	{
+		for(PxU32 i=0; i<nb; i++)
+			container.pushBack(indices[i]);
+		return true;
+	}
+private:
+	EntityReportContainerCallback& operator=(const EntityReportContainerCallback&);
+};
+
+class ConvexTriangles
+{
+public:
+										ConvexTriangles(const PxTriangleMeshGeometryLL& shapeMesh, 
+														   const Cm::FastVertex2ShapeScaling& skew, // object is not copied, beware!
+														   const PxU32* trigsInGroup, 
+														   PxU32 numTrigsInGroup, 
+														   PxU32* trigIndexDestBuffer);//trigIndexDestBuffer should be at least  numTrigsInGroup long.
+
+	void						getBounds(PxBounds3& bounds, const physx::PxTransform& transform)							const;
+
+	//non-virtuals:
+	PX_FORCE_INLINE	const TriangleMesh*					getMeshData() const			{ return shapeMesh.meshData; }
+
+	PxVec3 getPolygonNormal(PxU32 index) const;
+	
+
+private:
+	ConvexTriangles& operator=(const ConvexTriangles&);
+	void calcCenterAndBounds(const physx::PxTransform& transform) const;
+
+	const PxTriangleMeshGeometryLL&		shapeMesh;
+	const Cm::FastVertex2ShapeScaling&	mVertex2ShapeSkew;
+	const PxU32*						trigsInGroup;
+	PxU32								numTrigsInGroup;
+	PxU32*								trigIndexDestBuffer;
+	mutable HullPolygonData				selectedPolygon;
+
+	mutable PxBounds3					bounds;
+	mutable PxVec3						mCenter;			//average of vertices rather than center of bounds!
+	mutable bool						haveCenterAndBounds;
+};
+
+ConvexTriangles::ConvexTriangles(const PxTriangleMeshGeometryLL& md, 
+									   const Cm::FastVertex2ShapeScaling& skew,
+									   const PxU32* tg, PxU32 ntg, PxU32 * tb) 
+: shapeMesh(md), mVertex2ShapeSkew(skew), trigsInGroup(tg), numTrigsInGroup(ntg), trigIndexDestBuffer(tb), bounds(PxBounds3::empty()), mCenter(0.0f), haveCenterAndBounds(false)
+{
+}
+
+
+void ConvexTriangles::getBounds(PxBounds3& b, const physx::PxTransform& transform) const
+{
+	calcCenterAndBounds(transform);
+	b = bounds;
+}
+
+void ConvexTriangles::calcCenterAndBounds(const physx::PxTransform& transform) const	//computes bounds in shape space
+{
+	//NOTE: we have code that does this in a loop inside PxcContactHullMeshPenetrationFallback --  a relatively expensive weighted average of the faces.
+	//see if we really need to be that expensive!
+
+	//shound be done in ctor:
+	PX_ASSERT(bounds.isEmpty());
+	PX_ASSERT(mCenter.isZero());
+
+	for (PxU32 i = 0; i < numTrigsInGroup; i++)
+	{
+		const PxU32 triangleIndex = trigsInGroup[i];
+		PxVec3 v0l, v1l, v2l;
+		TriangleVertexPointers::getTriangleVerts(getMeshData(), triangleIndex, v0l, v1l, v2l);
+
+		//TODO: this does a lot of redundant work because shared vertices get tested multiple times.
+		//Still, its not a lot of work so any overhead of optimized data access may not be worth it.
+
+		//gotta take bounds in shape space because building it in vertex space and transforming it out would skew it.
+		
+		//unrolled loop of 3
+		const PxVec3 v0 = transform.transform(mVertex2ShapeSkew * v0l);
+		mCenter += v0;
+		bounds.include(v0);
+
+		const PxVec3 v1 = transform.transform(mVertex2ShapeSkew * v1l);
+		mCenter += v1;
+		bounds.include(v1);
+
+		const PxVec3 v2 = transform.transform(mVertex2ShapeSkew * v2l);
+		mCenter += v2;
+		bounds.include(v2);
+	}
+
+	mCenter *= 1.0f / (numTrigsInGroup * 3);
+	haveCenterAndBounds = true;
+}
+
+PxVec3 ConvexTriangles::getPolygonNormal(PxU32 index) const
+{
+	PX_ASSERT(index < numTrigsInGroup);
+	const PxU32 triangleIndex = trigsInGroup[index];
+	PxVec3 v0l, v1l, v2l;
+	TriangleVertexPointers::getTriangleVerts(getMeshData(), triangleIndex, v0l, v1l, v2l);
+	const bool flipNormal = mVertex2ShapeSkew.flipsNormal();
+	const PxVec3 t0 = mVertex2ShapeSkew * v0l;
+	const PxVec3 t1 = mVertex2ShapeSkew * (flipNormal ?  v2l : v1l);
+	const PxVec3 t2 = mVertex2ShapeSkew * (flipNormal ?  v1l : v2l);
+	const PxVec3 v0 = t0 - t1;
+	const PxVec3 v1 = t0 - t2;
+	const PxVec3 nor = v0.cross(v1);
+	return nor.getNormalized();
+}
+
+}
+
+
+
+PxReal SweepAnyShapeHeightfield(GU_SWEEP_METHOD_ARGS)
+{
+	PX_UNUSED(toiEstimate);
+	HeightFieldUtil hfUtil(shape1.mGeometry->get<const physx::PxHeightFieldGeometryLL>());
+
+	Ps::InlineArray<PxU32,64> tempContainer;
+
+	EntityReportContainerCallback callback(tempContainer);
+
+	PxVec3 trA = transform0.p - lastTm0.p;
+	PxVec3 trB = transform1.p - lastTm1.p;
+
+	PxVec3 relTr = trA - trB;
+	PxVec3 halfRelTr = relTr * 0.5f;
+
+	const PxVec3 ext = shape0.mExtents + halfRelTr.abs() + PxVec3(restDistance);
+	const PxVec3 cent = shape0.mCenter + halfRelTr;
+
+	PxBounds3 bounds0(cent - ext, cent + ext);
+
+	hfUtil.overlapAABBTriangles(transform1, bounds0, GuHfQueryFlags::eWORLD_SPACE, &callback);
+
+	Ps::Array<PxU32> orderedContainer(tempContainer.size());
+	
+	Ps::Array<PxU32> distanceEntries(tempContainer.size());
+
+	PxU32* orderedList = orderedContainer.begin();
+	PxF32* distances = reinterpret_cast<PxF32*>(distanceEntries.begin());
+	
+	PxVec3 origin = shape0.mCenter;
+	PxVec3 extent = shape0.mExtents + PxVec3(restDistance);
+
+	PxReal minTOI = PX_MAX_REAL;
+
+	PxU32 numTrigs = tempContainer.size();
+	PxU32* trianglesIndices = tempContainer.begin();
+
+	PxU32 count = 0;
+	for(PxU32 a = 0; a < numTrigs; ++a)
+	{
+		PxTriangle tri;
+		hfUtil.getTriangle(shape1.mPrevTransform, tri, 0, 0, trianglesIndices[a], true, true);
+
+		PxVec3 resultNormal = -(tri.verts[1]-tri.verts[0]).cross(tri.verts[2]-tri.verts[0]);
+		resultNormal.normalize();
+
+		if(relTr.dot(resultNormal) >= fastMovingThreshold)
+		{
+
+			PxBounds3 bounds;
+			bounds.setEmpty();
+			bounds.include(tri.verts[0]);
+			bounds.include(tri.verts[1]);
+			bounds.include(tri.verts[2]);
+
+			PxF32 toi = sweepAABBAABB(origin, extent * 1.1f, bounds.getCenter(), (bounds.getExtents() + PxVec3(0.01f, 0.01f, 0.01f)) * 1.1f, trA, trB);
+
+			PxU32 index = 0;
+			if(toi <= 1.f)
+			{
+				for(PxU32 b = count; b > 0; --b)
+				{
+					if(distances[b-1] <= toi)
+					{
+						//shuffle down and swap
+						index = b;
+						break;
+					}
+					PX_ASSERT(b > 0);
+					PX_ASSERT(b < numTrigs);
+					distances[b] = distances[b-1];
+					orderedList[b] = orderedList[b-1];
+				}
+				PX_ASSERT(index < numTrigs);
+				orderedList[index] = trianglesIndices[a];
+				distances[index] = toi;
+				count++;
+			}
+		}
+	}
+
+
+
+	worldNormal = PxVec3(PxReal(0));
+	worldPoint = PxVec3(PxReal(0));
+	Cm::FastVertex2ShapeScaling idScale;
+	PxU32 ccdFaceIndex = PXC_CONTACT_NO_FACE_INDEX;
+
+	PxVec3 sphereCenter(shape0.mPrevTransform.p);
+	PxF32 inSphereRadius = shape0.mFastMovingThreshold;
+	PxF32 inRadSq = inSphereRadius * inSphereRadius;
+
+
+	PxVec3 sphereCenterInTr1 = transform1.transformInv(sphereCenter);
+	PxVec3 sphereCenterInTr1T0 = transform1.transformInv(lastTm0.p);
+
+	PxVec3 tempWorldNormal(0.f), tempWorldPoint(0.f);
+
+	for (PxU32 ti = 0; ti < count; ti++)
+	{
+		PxTriangle tri;
+		hfUtil.getTriangle(lastTm1, tri, 0, 0, orderedList[ti], false, false);
+
+		PxVec3 resultNormal, resultPoint;
+
+		TriangleV triangle(V3LoadU(tri.verts[0]), V3LoadU(tri.verts[1]), V3LoadU(tri.verts[2]));
+
+		//do sweep
+
+		PxReal res = SweepShapeTriangle(
+			*shape0.mGeometry, *shape1.mGeometry, transform0, transform1, lastTm0, lastTm1, restDistance,
+			resultNormal, resultPoint, Cm::FastVertex2ShapeScaling(), triangle,
+			0.f);
+
+		if(res <= 0.f)
+		{
+			res = 0.f;
+
+			const PxVec3 v0 = tri.verts[1] - tri.verts[0] ;
+			const PxVec3 v1 = tri.verts[2] - tri.verts[0];
+
+			//Now we have a 0 TOI, lets see if the in-sphere hit it!
+
+			PxF32 distanceSq = distancePointTriangleSquared( sphereCenterInTr1, tri.verts[0], v0, v1);
+
+			if(distanceSq < inRadSq)
+			{
+				const PxVec3 nor = v0.cross(v1);
+				const PxF32 distance = PxSqrt(distanceSq);
+				res = distance - inSphereRadius;
+				const PxF32 d = nor.dot(tri.verts[0]);
+				const PxF32 dd = nor.dot(sphereCenterInTr1T0);
+				if((dd - d) > 0.f)
+				{
+					//back side, penetration 
+					res = -(2.f * inSphereRadius - distance);
+				}
+			}			
+		}
+
+		if (res < minTOI)
+		{
+			const PxVec3 v0 = tri.verts[1] - tri.verts[0] ;
+			const PxVec3 v1 = tri.verts[2] - tri.verts[0];
+
+			PxVec3 resultNormal1 = v0.cross(v1);
+			resultNormal1.normalize();
+			//if(norDotRel > 1e-6f)
+			{
+				tempWorldNormal = resultNormal1;
+				tempWorldPoint = resultPoint;
+				minTOI = res;
+				ccdFaceIndex = orderedList[ti];
+			}
+		}
+		
+	}
+
+	worldNormal = transform1.rotate(tempWorldNormal);
+	worldPoint = tempWorldPoint;
+
+	outCCDFaceIndex = ccdFaceIndex;
+
+	return minTOI;
+}
+
+
+PxReal SweepEstimateAnyShapeHeightfield(GU_SWEEP_ESTIMATE_ARGS)
+{
+	HeightFieldUtil hfUtil(shape1.mGeometry->get<const physx::PxHeightFieldGeometryLL>());
+
+	Ps::InlineArray<PxU32,64> tempContainer;
+	
+	EntityReportContainerCallback callback(tempContainer);
+
+	PxVec3 trA = transform0.p - lastTr0.p;
+	PxVec3 trB = transform1.p - lastTr1.p;
+
+	PxVec3 relTr = trA - trB;
+	PxVec3 halfRelTr = relTr * 0.5f;
+
+	const PxVec3 extents = shape0.mExtents + halfRelTr.abs() + PxVec3(restDistance);
+	const PxVec3 center = shape0.mCenter + halfRelTr;
+
+
+	PxBounds3 bounds0(center - extents, center + extents);
+
+
+	hfUtil.overlapAABBTriangles(transform1, bounds0, GuHfQueryFlags::eWORLD_SPACE, &callback);
+	
+	PxVec3 origin = shape0.mCenter;
+	PxVec3 extent = shape0.mExtents;
+
+	PxReal minTOI = PX_MAX_REAL;
+
+	PxU32 numTrigs = tempContainer.size();
+	PxU32* trianglesIndices = tempContainer.begin();
+
+	for(PxU32 a = 0; a < numTrigs; ++a)
+	{
+
+		PxTriangle tri;
+		hfUtil.getTriangle(shape1.mPrevTransform, tri, 0, 0, trianglesIndices[a], true, true);
+
+
+
+		PxVec3 resultNormal = -(tri.verts[1]-tri.verts[0]).cross(tri.verts[2]-tri.verts[0]);
+		resultNormal.normalize();
+
+		if(relTr.dot(resultNormal) >= fastMovingThreshold)
+		{
+
+			PxBounds3 bounds;
+			bounds.setEmpty();
+			bounds.include(tri.verts[0]);
+			bounds.include(tri.verts[1]);
+			bounds.include(tri.verts[2]);
+
+			PxF32 toi = sweepAABBAABB(origin, extent * 1.1f, bounds.getCenter(), (bounds.getExtents() + PxVec3(0.01f, 0.01f, 0.01f)) * 1.1f, trA, trB);
+
+			minTOI = PxMin(minTOI, toi);
+		}
+	}
+
+	return minTOI;
+}
+
+
+
+PxReal SweepAnyShapeMesh(GU_SWEEP_METHOD_ARGS)
+{
+	PX_UNUSED(toiEstimate);
+	// this is the trimesh midphase for convex vs mesh sweep. shape0 is the convex shape.
+
+	// Get actual shape data
+	const PxTriangleMeshGeometryLL& shapeMesh = shape1.mGeometry->get<const PxTriangleMeshGeometryLL>();
+
+	const Cm::FastVertex2ShapeScaling meshScaling(shapeMesh.scale);
+
+	/*---------------------------------------------------*\
+	|
+	| STEP1: OPCODE Geometry collection
+	|
+	\*---------------------------------------------------*/
+
+	PxVec3 trA = transform0.p - lastTm0.p;
+	PxVec3 trB = transform1.p - lastTm1.p;
+
+	PxVec3 relTr = trA - trB;
+	PxVec3 unitDir = relTr;
+	PxReal length = unitDir.normalize();
+
+	PxMat33 matRot(PxIdentity);
+
+
+	//1) Compute the swept bounds
+	Box sweptBox;
+	computeSweptBox(sweptBox, shape0.mExtents, shape0.mCenter, matRot, unitDir, length);
+
+	Box vertexSpaceBox;
+	if (shapeMesh.scale.isIdentity())
+		vertexSpaceBox = transformBoxOrthonormal(sweptBox, transform1.getInverse());
+	else
+		computeVertexSpaceOBB(vertexSpaceBox, sweptBox, transform1, shapeMesh.scale);
+
+
+	vertexSpaceBox.extents += PxVec3(restDistance);
+
+	Ps::InlineArray<PxU32, 64> tempContainer;
+
+	AccumCallback callback(tempContainer);
+
+	// AP scaffold: early out opportunities, should probably use fat raycast
+	Midphase::intersectOBB(shapeMesh.meshData, vertexSpaceBox, callback, true);
+
+	if (tempContainer.size() == 0)
+		return PX_MAX_REAL;
+
+	// Intersection found, fetch triangles
+	PxU32 numTrigs = tempContainer.size();
+	const PxU32* triangleIndices = tempContainer.begin();
+
+	PxVec3 origin = shape0.mCenter;
+	PxVec3 extent = shape0.mExtents + PxVec3(restDistance);
+	
+	Ps::InlineArray<PxU32, 64> orderedContainer;
+	orderedContainer.resize(tempContainer.size());
+	
+	Ps::InlineArray<PxU32, 64> distanceEntries;
+	distanceEntries.resize(tempContainer.size());
+	
+	PxU32* orderedList = orderedContainer.begin();
+	PxF32* distances = reinterpret_cast<PxF32*>(distanceEntries.begin());
+
+	PxReal minTOI = PX_MAX_REAL;
+
+
+	PxU32 count = 0;
+	for(PxU32 a = 0; a < numTrigs; ++a)
+	{
+		PxU32 unused;
+		ConvexTriangles convexPartOfMesh1(shapeMesh, meshScaling, &triangleIndices[a], 1, &unused);
+
+		PxVec3 resultNormal = -transform1.rotate(convexPartOfMesh1.getPolygonNormal(0));
+
+		if(relTr.dot(resultNormal) >= fastMovingThreshold)
+		{
+			PxBounds3 bounds;
+			convexPartOfMesh1.getBounds(bounds, lastTm1);
+			//OK, we have all 3 vertices, now calculate bounds...
+
+			PxF32 toi = sweepAABBAABB(origin, extent, bounds.getCenter(), bounds.getExtents() + PxVec3(0.02f, 0.02f, 0.02f), trA, trB);
+
+			PxU32 index = 0;
+			if(toi <= 1.f)
+			{
+				for(PxU32 b = count; b > 0; --b)
+				{
+					if(distances[b-1] <= toi)
+					{
+						//shuffle down and swap
+						index = b;
+						break;
+					}
+					PX_ASSERT(b > 0);
+					PX_ASSERT(b < numTrigs);
+					distances[b] = distances[b-1];
+					orderedList[b] = orderedList[b-1];
+				}
+				PX_ASSERT(index < numTrigs);
+				orderedList[index] = triangleIndices[a];
+				distances[index] = toi;
+				count++;
+			}
+		}
+	}
+
+
+
+	PxVec3 tempWorldNormal(0.f), tempWorldPoint(0.f);
+
+	Cm::FastVertex2ShapeScaling idScale;
+	PxU32 ccdFaceIndex = PXC_CONTACT_NO_FACE_INDEX;
+
+	PxVec3 sphereCenter(lastTm1.p);
+	PxF32 inSphereRadius = shape0.mFastMovingThreshold;
+	//PxF32 inRadSq = inSphereRadius * inSphereRadius;
+
+	PxVec3 sphereCenterInTransform1 = transform1.transformInv(sphereCenter);
+
+	PxVec3 sphereCenterInTransform0p = transform1.transformInv(lastTm0.p);
+
+
+	for (PxU32 ti = 0; ti < count /*&& PxMax(minTOI, 0.f) >= distances[ti]*/; ti++)
+	{
+		PxU32 unused;
+		ConvexTriangles convexPartOfMesh1(shapeMesh, meshScaling, &orderedList[ti], 1, &unused);
+
+		PxVec3 resultNormal, resultPoint, v0l, v1l, v2l;
+		TriangleVertexPointers::getTriangleVerts(shapeMesh.meshData, orderedList[ti], v0l, v1l, v2l);
+		const bool flipNormal = meshScaling.flipsNormal();
+
+		const PxVec3 v0 = meshScaling * v0l;
+		const PxVec3 v1 = meshScaling * (flipNormal ? v2l : v1l);
+		const PxVec3 v2 = meshScaling * (flipNormal ? v1l : v2l);
+
+		TriangleV triangle(V3LoadU(v0), V3LoadU(v1), V3LoadU(v2));
+
+		//do sweep
+		PxReal res = SweepShapeTriangle(
+			*shape0.mGeometry, *shape1.mGeometry, transform0, transform1, lastTm0, lastTm1, restDistance,
+			resultNormal, resultPoint, Cm::FastVertex2ShapeScaling(), triangle,
+			0.f);
+
+		resultNormal = -resultNormal;
+
+		if(res <= 0.f)
+		{
+			res = 0.f;
+
+			PxF32 inRad = inSphereRadius + restDistance;
+			PxF32 inRadSq = inRad*inRad;
+
+			const PxVec3 vv0 = v1 - v0 ;
+			const PxVec3 vv1 = v2 - v0;
+			const PxVec3 nor = vv0.cross(vv1);
+
+			//Now we have a 0 TOI, lets see if the in-sphere hit it!
+
+			PxF32 distanceSq = distancePointTriangleSquared( sphereCenterInTransform1, v0, vv0, vv1);
+
+			if(distanceSq < inRadSq)
+			{
+				const PxF32 distance = PxSqrt(distanceSq);
+				res = distance - inRad;
+				const PxF32 d = nor.dot(v0);
+				const PxF32 dd = nor.dot(sphereCenterInTransform0p);
+				if((dd - d) < 0.f)
+				{
+					//back side, penetration 
+					res = -(2.f * inRad - distance);
+				}
+			}	
+			PX_ASSERT(PxIsFinite(res));
+			resultNormal = transform1.rotate(convexPartOfMesh1.getPolygonNormal(0));			
+		}
+
+		if (res < minTOI)
+		{
+			tempWorldNormal = resultNormal;//convexPartOfMesh1.getPolygonNormal(0);//transform1.rotate(convexPartOfMesh1.getPolygonNormal(0));
+			tempWorldPoint = resultPoint;
+			minTOI = res;
+			ccdFaceIndex = orderedList[ti];
+		}
+		
+	}
+
+	worldNormal = tempWorldNormal;//transform1.rotate(tempWorldNormal);
+	worldPoint = tempWorldPoint;
+	outCCDFaceIndex = ccdFaceIndex;
+	return minTOI;
+}
+
+
+/**
+\brief This code performs a conservative estimate of the TOI of a shape v mesh.
+*/
+PxReal SweepEstimateAnyShapeMesh(GU_SWEEP_ESTIMATE_ARGS)
+{
+	// this is the trimesh midphase for convex vs mesh sweep. shape0 is the convex shape.
+	// Get actual shape data
+	const PxTriangleMeshGeometryLL& shapeMesh = shape1.mGeometry->get<const PxTriangleMeshGeometryLL>();
+
+	const Cm::FastVertex2ShapeScaling meshScaling(shapeMesh.scale);
+
+
+	/*---------------------------------------------------*\
+	|
+	| STEP1: OPCODE Geometry collection
+	|
+	\*---------------------------------------------------*/
+
+	PxVec3 trA = transform0.p - lastTr0.p;
+	PxVec3 trB = transform1.p - lastTr1.p;
+
+	PxVec3 relTr = trA - trB;
+	PxVec3 unitDir = relTr;
+	PxReal length = unitDir.normalize();
+
+	PxMat33 matRot(PxIdentity);
+
+	//1) Compute the swept bounds
+	Box sweptBox;
+	computeSweptBox(sweptBox, shape0.mExtents, shape0.mCenter, matRot, unitDir, length);
+
+	Box vertexSpaceBox;
+	computeVertexSpaceOBB(vertexSpaceBox, sweptBox, transform1, shapeMesh.scale);
+
+	vertexSpaceBox.extents += PxVec3(restDistance);
+
+	// TODO: implement a cached mode that fetches the trigs from a cache rather than per opcode if there is little motion.
+
+	struct CB : MeshHitCallback<PxRaycastHit>
+	{
+		PxReal minTOI;
+		PxReal sumFastMovingThresh;
+		const PxTriangleMeshGeometryLL& shapeMesh;
+		const Cm::FastVertex2ShapeScaling& meshScaling;
+		const PxVec3& relTr;
+		const PxVec3& trA;
+		const PxVec3& trB;
+		const PxTransform& transform1;
+		const PxVec3& origin;
+		const PxVec3& extent;
+
+		CB(PxReal aSumFast, const PxTriangleMeshGeometryLL& aShapeMesh, const Cm::FastVertex2ShapeScaling& aMeshScaling,
+			const PxVec3& aRelTr, const PxVec3& atrA, const PxVec3& atrB, const PxTransform& aTransform1, const PxVec3& aOrigin, const PxVec3& aExtent)
+			:	MeshHitCallback<PxRaycastHit>(CallbackMode::eMULTIPLE),
+				sumFastMovingThresh(aSumFast), shapeMesh(aShapeMesh), meshScaling(aMeshScaling), relTr(aRelTr), trA(atrA), trB(atrB),
+				transform1(aTransform1), origin(aOrigin), extent(aExtent)
+		{
+			minTOI = PX_MAX_REAL;
+		}
+
+		virtual PxAgain processHit( // all reported coords are in mesh local space including hit.position
+			const PxRaycastHit& hit, const PxVec3&, const PxVec3&, const PxVec3&, PxReal& shrunkMaxT, const PxU32*)
+		{
+			PxU32 unused;
+			ConvexTriangles convexPartOfMesh1(shapeMesh, meshScaling, &hit.faceIndex, 1, &unused);
+			PxVec3 resultNormal = -transform1.rotate(convexPartOfMesh1.getPolygonNormal(0));
+			if(relTr.dot(resultNormal) >= sumFastMovingThresh)
+			{
+				PxBounds3 bounds;
+				convexPartOfMesh1.getBounds(bounds, transform1);
+				//OK, we have all 3 vertices, now calculate bounds...
+
+				PxF32 toi = sweepAABBAABB(
+					origin, extent * 1.1f, bounds.getCenter(), (bounds.getExtents() + PxVec3(0.01f, 0.01f, 0.01f)) * 1.1f, trA, trB);
+
+				minTOI = PxMin(minTOI, toi);
+				shrunkMaxT = minTOI;
+			}
+
+			return (minTOI > 0.0f); // stop traversal if minTOI == 0.0f
+		}
+
+		void operator=(const CB&) {}
+	};
+
+	PxVec3 origin = shape0.mCenter;
+	PxVec3 extent = shape0.mExtents + PxVec3(restDistance);
+
+	CB callback(fastMovingThreshold, shapeMesh, meshScaling, relTr, trA, trB, transform1, origin, extent);
+	Midphase::intersectOBB(shapeMesh.meshData, vertexSpaceBox, callback, true);
+
+	return callback.minTOI;
+}
+
+	
+}
+}
+

physx/source/geomutils/src/ccd/GuCCDSweepConvexMesh.h

@@ -0,0 +1,178 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef GU_CCD_SWEEP_H
+#define GU_CCD_SWEEP_H
+
+#include "common/PxPhysXCommonConfig.h"
+#include "CmPhysXCommon.h"
+#include "PsVecTransform.h"
+#include "GuGeometryUnion.h"
+#include "CmScaling.h"
+
+#define GU_TRIANGLE_SWEEP_METHOD_ARGS				\
+	const Gu::GeometryUnion& shape0,				\
+	const Gu::GeometryUnion& shape1,				\
+	const PxTransform& transform0,					\
+	const PxTransform& transform1,					\
+	const PxTransform& lastTm0,						\
+	const PxTransform& lastTm1,						\
+	PxReal restDistance,							\
+	PxVec3& worldNormal,							\
+	PxVec3& worldPoint,								\
+	const Cm::FastVertex2ShapeScaling& meshScaling,	\
+	Gu::TriangleV& triangle,						\
+	const PxF32 toiEstimate
+
+
+#define GU_SWEEP_METHOD_ARGS			\
+	const Gu::CCDShape& shape0,			\
+	const Gu::CCDShape& shape1,			\
+	const PxTransform& transform0,		\
+	const PxTransform& transform1,		\
+	const PxTransform& lastTm0,			\
+	const PxTransform& lastTm1,			\
+	PxReal restDistance,				\
+	PxVec3& worldNormal,				\
+	PxVec3& worldPoint,					\
+	const PxF32 toiEstimate,			\
+	PxU32& outCCDFaceIndex,				\
+	const PxReal fastMovingThreshold
+
+
+#define GU_SWEEP_ESTIMATE_ARGS			\
+	const CCDShape& shape0,				\
+	const CCDShape& shape1,				\
+	const PxTransform& transform0,		\
+	const PxTransform& transform1,		\
+	const PxTransform& lastTr0,			\
+	const PxTransform& lastTr1,			\
+	const PxReal restDistance,			\
+	const PxReal fastMovingThreshold
+
+
+#define GU_SWEEP_METHOD_ARGS_UNUSED		\
+	const Gu::CCDShape& /*shape0*/,		\
+	const Gu::CCDShape& /*shape1*/,		\
+	const PxTransform& /*transform0*/,	\
+	const PxTransform& /*transform1*/,	\
+	const PxTransform& /*lastTm0*/,		\
+	const PxTransform& /*lastTm1*/,		\
+	PxReal /*restDistance*/,			\
+	PxVec3& /*worldNormal*/,			\
+	PxVec3& /*worldPoint*/,				\
+	const PxF32 /*toiEstimate*/,		\
+	PxU32& /*outCCDFaceIndex*/,			\
+	const PxReal /*fastMovingThreshold*/
+
+namespace physx
+{
+namespace Gu
+{
+	struct CCDShape
+	{
+		const Gu::GeometryUnion*	mGeometry;
+		PxReal						mFastMovingThreshold;		//The CCD threshold for this shape
+		PxTransform					mPrevTransform;				//This shape's previous transform
+		PxTransform					mCurrentTransform;			//This shape's current transform
+		PxVec3						mExtents;					//The extents of this shape's AABB
+		PxVec3						mCenter;					//The center of this shape's AABB
+		PxU32						mUpdateCount;				//How many times this shape has been updated in the CCD. This is correlated with the CCD body's update count.
+	};
+
+	PX_FORCE_INLINE PxF32 sweepAABBAABB(const PxVec3& centerA, const PxVec3& extentsA, const PxVec3& centerB, const PxVec3& extentsB, const PxVec3& trA, const PxVec3& trB)
+	{
+		//Sweep 2 AABBs against each other, return the TOI when they hit else PX_MAX_REAL if they don't hit
+		const PxVec3 cAcB = centerA - centerB;
+		const PxVec3 sumExtents = extentsA + extentsB;
+
+		//Initial hit
+		if(PxAbs(cAcB.x) <= sumExtents.x && 
+			PxAbs(cAcB.y) <= sumExtents.y && 
+			PxAbs(cAcB.z) <= sumExtents.z)
+			return 0.f;
+
+		//No initial hit - perform the sweep
+		const PxVec3 relTr = trB - trA;
+		PxF32 tfirst = 0.f;
+		PxF32 tlast = 1.f;
+
+		const PxVec3 aMax = centerA + extentsA;
+		const PxVec3 aMin = centerA - extentsA;
+		const PxVec3 bMax = centerB + extentsB;
+		const PxVec3 bMin = centerB - extentsB;
+
+		const PxF32 eps = 1e-6f;
+
+		for(PxU32 a = 0; a < 3; ++a)
+		{
+			if(relTr[a] < -eps)
+			{
+				if(bMax[a] < aMin[a]) 
+					return PX_MAX_REAL;
+				if(aMax[a] < bMin[a]) 
+					tfirst = PxMax((aMax[a] - bMin[a])/relTr[a], tfirst);
+				if(bMax[a] > aMin[a]) 
+					tlast = PxMin((aMin[a] - bMax[a])/relTr[a], tlast);
+			}
+			else if(relTr[a] > eps)
+			{
+				if(bMin[a] > aMax[a]) 
+					return PX_MAX_REAL;
+				if(bMax[a] < aMin[a]) 
+					tfirst = PxMax((aMin[a] - bMax[a])/relTr[a], tfirst);
+				if(aMax[a] > bMin[a]) 
+					tlast = PxMin((aMax[a] - bMin[a])/relTr[a], tlast);
+			}
+			else
+			{
+				if(bMax[a] < aMin[a] || bMin[a] > aMax[a])
+					return PX_MAX_REAL;
+			}
+
+			//No hit
+			if(tfirst > tlast)
+				return PX_MAX_REAL;
+		}
+		//There was a hit so return the TOI
+		return tfirst; 
+	}
+
+	PX_PHYSX_COMMON_API PxReal SweepShapeShape(GU_SWEEP_METHOD_ARGS);
+
+	PX_PHYSX_COMMON_API PxReal SweepEstimateAnyShapeHeightfield(GU_SWEEP_ESTIMATE_ARGS);
+
+	PX_PHYSX_COMMON_API PxReal SweepEstimateAnyShapeMesh(GU_SWEEP_ESTIMATE_ARGS);
+
+
+}
+}
+#endif
+

physx/source/geomutils/src/ccd/GuCCDSweepPrimitives.cpp

@@ -0,0 +1,286 @@
+//
+// 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-2021 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 "Ps.h"
+#include "GuVecCapsule.h"
+#include "GuVecBox.h"
+#include "GuVecConvexHull.h"
+#include "GuVecTriangle.h"
+#include "GuGJKRaycast.h"
+#include "GuCCDSweepConvexMesh.h"
+#include "GuGJKType.h"
+
+namespace physx
+{
+namespace Gu
+{
+
+using namespace Ps::aos;
+
+template<typename Geom> PX_FORCE_INLINE  PxReal getRadius(const PxGeometry&) 
+{	
+	return 0;	
+}
+
+template<> PX_FORCE_INLINE PxReal getRadius<CapsuleV>(const PxGeometry& g) 
+{ 
+	PX_ASSERT(g.getType() == PxGeometryType::eCAPSULE || g.getType() == PxGeometryType::eSPHERE);
+	PX_COMPILE_TIME_ASSERT(PX_OFFSET_OF(PxSphereGeometry, radius) == PX_OFFSET_OF(PxCapsuleGeometry, radius));
+	return static_cast<const PxSphereGeometry&>(g).radius;
+}
+
+
+
+template<class ConvexA, class ConvexB>
+static PxReal CCDSweep(ConvexA& a, ConvexB& b,  const PxTransform& transform0, const PxTransform& transform1, const PxTransform& lastTm0, const PxTransform& lastTm1,
+					 const Ps::aos::FloatV& toiEstimate, PxVec3& worldPoint, PxVec3& worldNormal, PxReal inflation = 0.f)
+{
+	PX_UNUSED(toiEstimate); //KS - TODO - can we use this again?
+	using namespace Ps::aos;
+
+	const Vec3V zero = V3Zero();
+
+	const QuatV q0 = QuatVLoadA(&transform0.q.x);
+	const Vec3V p0 = V3LoadA(&lastTm0.p.x);
+
+	const QuatV q1 = QuatVLoadA(&transform1.q.x);
+	const Vec3V p1 = V3LoadA(&lastTm1.p.x);
+
+	const PsTransformV tr0(p0, q0);
+	const PsTransformV tr1(p1, q1);
+
+	const PsMatTransformV aToB(tr1.transformInv(tr0));
+
+	const Vec3V trans0p = V3LoadU(transform0.p);
+	const Vec3V trans1p = V3LoadU(transform1.p);
+	const Vec3V trA = V3Sub(trans0p, p0);
+	const Vec3V trB = V3Sub(trans1p, p1);
+	const Vec3V relTr = tr1.rotateInv(V3Sub(trB, trA));
+
+	FloatV lambda;
+	Vec3V closestA, normal;
+	const FloatV initialLambda = FZero();
+	const RelativeConvex<ConvexA> convexA(a, aToB);
+	const LocalConvex<ConvexB> convexB(b);
+	if(gjkRaycastPenetration<RelativeConvex<ConvexA>, LocalConvex<ConvexB> >(convexA, convexB, aToB.p, initialLambda, zero, relTr, lambda, normal, closestA, inflation, true))
+	{
+		//Adjust closestA because it will be on the surface of convex a in its initial position (s). If the TOI > 0, we need to move 
+		//the point along the sweep direction to get the world-space hit position.
+		PxF32 res;
+		FStore(lambda, &res);
+		closestA = V3ScaleAdd(trA, FMax(lambda, FZero()), tr1.transform(closestA));
+		normal = tr1.rotate(normal);
+
+		V3StoreU(normal, worldNormal);
+		V3StoreU(closestA, worldPoint);
+		return res;
+	}
+	return PX_MAX_REAL;
+}
+
+
+
+//
+// lookup table for geometry-vs-geometry sweeps
+//
+
+
+PxReal UnimplementedSweep (GU_SWEEP_METHOD_ARGS_UNUSED)
+{
+	return PX_MAX_REAL;	//no impact
+}
+
+template<typename Geom0, typename Geom1>	
+PxReal SweepGeomGeom(GU_SWEEP_METHOD_ARGS)
+{
+	PX_UNUSED(outCCDFaceIndex);
+	PX_UNUSED(fastMovingThreshold);
+
+	const PxGeometry& g0 = shape0.mGeometry->getGeometry();
+	const PxGeometry& g1 = shape1.mGeometry->getGeometry();
+
+	typename ConvexGeom<Geom0>::Type geom0(g0);
+	typename ConvexGeom<Geom1>::Type geom1(g1);
+
+	return CCDSweep(geom0, geom1, transform0, transform1, lastTm0, lastTm1, FLoad(toiEstimate), worldPoint, worldNormal, restDistance+getRadius<Geom0>(g0)+getRadius<Geom1>(g1) );
+}
+
+typedef PxReal (*SweepMethod) (GU_SWEEP_METHOD_ARGS);
+
+PxReal SweepAnyShapeHeightfield(GU_SWEEP_METHOD_ARGS);
+PxReal SweepAnyShapeMesh(GU_SWEEP_METHOD_ARGS);
+
+SweepMethod g_SweepMethodTable[PxGeometryType::eGEOMETRY_COUNT][PxGeometryType::eGEOMETRY_COUNT] = 
+{
+	//PxGeometryType::eSPHERE
+	{
+		SweepGeomGeom<CapsuleV, CapsuleV>,				//PxGeometryType::eSPHERE
+		UnimplementedSweep,								//PxGeometryType::ePLANE
+		SweepGeomGeom<CapsuleV, CapsuleV>,				//PxGeometryType::eCAPSULE
+		SweepGeomGeom<CapsuleV, BoxV>,					//PxGeometryType::eBOX
+		SweepGeomGeom<CapsuleV, ConvexHullV>,			//PxGeometryType::eCONVEXMESH
+		SweepAnyShapeMesh,								//PxGeometryType::eTRIANGLEMESH
+		SweepAnyShapeHeightfield,						//PxGeometryType::eHEIGHTFIELD		//TODO		
+	},
+
+	//PxGeometryType::ePLANE
+	{
+		0,												//PxGeometryType::eSPHERE
+		UnimplementedSweep,								//PxGeometryType::ePLANE
+		UnimplementedSweep,								//PxGeometryType::eCAPSULE
+		UnimplementedSweep,								//PxGeometryType::eBOX
+		UnimplementedSweep,								//PxGeometryType::eCONVEXMESH
+		UnimplementedSweep,								//PxGeometryType::eTRIANGLEMESH
+		UnimplementedSweep,								//PxGeometryType::eHEIGHTFIELD
+	},
+
+	//PxGeometryType::eCAPSULE
+	{
+		0,												//PxGeometryType::eSPHERE
+		0,												//PxGeometryType::ePLANE
+		SweepGeomGeom<CapsuleV, CapsuleV>,				//PxGeometryType::eCAPSULE
+		SweepGeomGeom<CapsuleV, BoxV>,					//PxGeometryType::eBOX
+		SweepGeomGeom<CapsuleV, ConvexHullV>,			//PxGeometryType::eCONVEXMESH
+		SweepAnyShapeMesh,								//PxGeometryType::eTRIANGLEMESH
+		SweepAnyShapeHeightfield,						//PxGeometryType::eHEIGHTFIELD
+	},
+
+	//PxGeometryType::eBOX
+	{
+		0,											//PxGeometryType::eSPHERE
+		0,											//PxGeometryType::ePLANE
+		0,											//PxGeometryType::eCAPSULE
+		SweepGeomGeom<BoxV, BoxV>,					//PxGeometryType::eBOX
+		SweepGeomGeom<BoxV, ConvexHullV>,			//PxGeometryType::eCONVEXMESH
+		SweepAnyShapeMesh,							//PxGeometryType::eTRIANGLEMESH
+		SweepAnyShapeHeightfield,					//PxGeometryType::eHEIGHTFIELD
+	},
+
+	//PxGeometryType::eCONVEXMESH
+	{
+		0,											//PxGeometryType::eSPHERE
+		0,											//PxGeometryType::ePLANE
+		0,											//PxGeometryType::eCAPSULE
+		0,											//PxGeometryType::eBOX
+		SweepGeomGeom<ConvexHullV, ConvexHullV>,	//PxGeometryType::eCONVEXMESH
+		SweepAnyShapeMesh,							//PxGeometryType::eTRIANGLEMESH
+		SweepAnyShapeHeightfield,					//PxGeometryType::eHEIGHTFIELD
+	},
+
+	//PxGeometryType::eTRIANGLEMESH
+	{
+		0,											//PxGeometryType::eSPHERE
+		0,											//PxGeometryType::ePLANE
+		0,											//PxGeometryType::eCAPSULE
+		0,											//PxGeometryType::eBOX
+		0,											//PxGeometryType::eCONVEXMESH
+		UnimplementedSweep,							//PxGeometryType::eTRIANGLEMESH
+		UnimplementedSweep,							//PxGeometryType::eHEIGHTFIELD
+	},
+
+	//PxGeometryType::eHEIGHTFIELD
+	{
+		0,											//PxGeometryType::eSPHERE
+		0,											//PxGeometryType::ePLANE
+		0,											//PxGeometryType::eCAPSULE
+		0,											//PxGeometryType::eBOX
+		0,											//PxGeometryType::eCONVEXMESH
+		0,											//PxGeometryType::eTRIANGLEMESH
+		UnimplementedSweep,							//PxGeometryType::eHEIGHTFIELD
+	},
+};
+
+
+PxReal SweepShapeShape(GU_SWEEP_METHOD_ARGS)
+{
+	PxGeometryType::Enum type0 = shape0.mGeometry->getType();
+	PxGeometryType::Enum type1 = shape1.mGeometry->getType();
+
+	return g_SweepMethodTable[type0][type1](shape0, shape1, transform0, transform1, lastTm0, lastTm1,
+		restDistance, worldNormal, worldPoint, toiEstimate, outCCDFaceIndex, fastMovingThreshold);
+		
+}
+
+//
+// lookup table for sweeps agains triangles
+//
+
+PxReal UnimplementedTriangleSweep(GU_TRIANGLE_SWEEP_METHOD_ARGS)
+{
+	PX_UNUSED(shape0);
+	PX_UNUSED(shape1);
+	PX_UNUSED(transform0);
+	PX_UNUSED(transform1);
+	PX_UNUSED(lastTm0);
+	PX_UNUSED(lastTm1);
+	PX_UNUSED(restDistance);
+	PX_UNUSED(worldNormal);
+	PX_UNUSED(worldPoint);
+	PX_UNUSED(meshScaling);
+	PX_UNUSED(triangle);
+	PX_UNUSED(toiEstimate);
+
+	return 1e10f;	//no impact
+}
+
+template<typename Geom>	
+PxReal SweepGeomTriangles(GU_TRIANGLE_SWEEP_METHOD_ARGS)
+{
+	PX_UNUSED(meshScaling);
+	PX_UNUSED(shape1);
+
+	const PxGeometry& g = shape0.getGeometry();
+	//Geom geom(g);
+	typename ConvexGeom<Geom>::Type geom(g);
+
+	return CCDSweep<TriangleV, Geom>(triangle, geom, transform1, transform0, lastTm1, lastTm0, FLoad(toiEstimate), worldPoint, worldNormal, restDistance+getRadius<Geom>(g) );
+}
+
+typedef PxReal (*TriangleSweepMethod) (GU_TRIANGLE_SWEEP_METHOD_ARGS);
+TriangleSweepMethod g_TriangleSweepMethodTable[PxGeometryType::eGEOMETRY_COUNT] = 
+{
+	SweepGeomTriangles<CapsuleV>,		//PxGeometryType::eSPHERE
+	UnimplementedTriangleSweep,			//PxGeometryType::ePLANE
+	SweepGeomTriangles<CapsuleV>,		//PxGeometryType::eCAPSULE
+	SweepGeomTriangles<BoxV>,			//PxGeometryType::eBOX
+	SweepGeomTriangles<ConvexHullV>,	//PxGeometryType::eCONVEXMESH
+	UnimplementedTriangleSweep,			//PxGeometryType::eTRIANGLEMESH
+	UnimplementedTriangleSweep,			//PxGeometryType::eHEIGHTFIELD
+};
+
+PxReal SweepShapeTriangle(GU_TRIANGLE_SWEEP_METHOD_ARGS)
+{
+	const PxGeometryType::Enum type0 = shape0.getType();
+	TriangleSweepMethod method = g_TriangleSweepMethodTable[type0];
+	return method(shape0, shape1, transform0, transform1, lastTm0, lastTm1, restDistance, worldNormal, worldPoint, meshScaling, triangle, toiEstimate);
+}
+
+}
+}
+

physx/source/geomutils/src/common/GuBarycentricCoordinates.cpp

@@ -0,0 +1,89 @@
+//
+// 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-2021 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 "GuBarycentricCoordinates.h"
+
+using namespace physx;
+using namespace Ps::aos;
+
+void Gu::barycentricCoordinates(const Vec3VArg p, const Vec3VArg a, const Vec3VArg b, FloatV& v)
+{
+	const Vec3V v0 = V3Sub(a, p);
+	const Vec3V v1 = V3Sub(b, p);
+	const Vec3V d = V3Sub(v1, v0);
+	const FloatV denominator = V3Dot(d, d);
+	const FloatV numerator = V3Dot(V3Neg(v0), d);
+	const FloatV zero = FZero();
+	const FloatV denom = FSel(FIsGrtr(denominator, zero), FRecip(denominator), zero);
+	v = FMul(numerator, denom);
+}
+
+void Gu::barycentricCoordinates(const Ps::aos::Vec3VArg p, const Ps::aos::Vec3VArg a, const Ps::aos::Vec3VArg b, const Ps::aos::Vec3VArg c, Ps::aos::FloatV& v, Ps::aos::FloatV& w)
+{
+	const Vec3V ab = V3Sub(b, a);
+	const Vec3V ac = V3Sub(c, a);
+
+	const Vec3V n = V3Cross(ab, ac);
+
+	const VecCrossV crossA = V3PrepareCross(V3Sub(a, p));
+	const VecCrossV crossB = V3PrepareCross(V3Sub(b, p));
+	const VecCrossV crossC = V3PrepareCross(V3Sub(c, p));
+	const Vec3V bCrossC = V3Cross(crossB, crossC);
+	const Vec3V cCrossA = V3Cross(crossC, crossA);
+	const Vec3V aCrossB = V3Cross(crossA, crossB);
+
+	const FloatV va = V3Dot(n, bCrossC);//edge region of BC, signed area rbc, u = S(rbc)/S(abc) for a
+	const FloatV vb = V3Dot(n, cCrossA);//edge region of AC, signed area rac, v = S(rca)/S(abc) for b
+	const FloatV vc = V3Dot(n, aCrossB);//edge region of AB, signed area rab, w = S(rab)/S(abc) for c
+	const FloatV totalArea =FAdd(va, FAdd(vb, vc));
+	const FloatV zero = FZero();
+	const FloatV denom = FSel(FIsEq(totalArea, zero), zero, FRecip(totalArea));
+	v = FMul(vb, denom);
+	w = FMul(vc, denom);
+	
+}
+
+/*
+	v0 = b - a;
+	v1 = c - a;
+	v2 = p - a;
+*/
+void Gu::barycentricCoordinates(const Vec3VArg v0, const Vec3VArg v1, const Vec3VArg v2, FloatV& v, FloatV& w)
+{
+	const FloatV d00 = V3Dot(v0, v0);
+	const FloatV d01 = V3Dot(v0, v1);
+	const FloatV d11 = V3Dot(v1, v1);
+	const FloatV d20 = V3Dot(v2, v0);
+	const FloatV d21 = V3Dot(v2, v1);
+	const FloatV denom = FRecip(FSub(FMul(d00,d11), FMul(d01, d01)));
+	v = FMul(FSub(FMul(d11, d20), FMul(d01, d21)), denom);
+	w = FMul(FSub(FMul(d00, d21), FMul(d01, d20)), denom);
+}
+

physx/source/geomutils/src/common/GuBarycentricCoordinates.h

@@ -0,0 +1,93 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_BARYCENTRIC_COORDINATES_H
+#define GU_BARYCENTRIC_COORDINATES_H
+
+#include "common/PxPhysXCommonConfig.h"
+#include "CmPhysXCommon.h"
+#include "PsVecMath.h"
+
+namespace physx
+{
+namespace Gu
+{
+	//calculate the barycentric coorinates for a point in a segment
+	void barycentricCoordinates(const Ps::aos::Vec3VArg p, 
+		const Ps::aos::Vec3VArg a, 
+		const Ps::aos::Vec3VArg b, 
+		Ps::aos::FloatV& v);
+
+	//calculate the barycentric coorinates for a point in a triangle
+	void barycentricCoordinates(const Ps::aos::Vec3VArg p, 
+		const Ps::aos::Vec3VArg a, 
+		const Ps::aos::Vec3VArg b, 
+		const Ps::aos::Vec3VArg c, 
+		Ps::aos::FloatV& v, 
+		Ps::aos::FloatV& w);
+
+	void barycentricCoordinates(const Ps::aos::Vec3VArg v0, 
+		const Ps::aos::Vec3VArg v1, 
+		const Ps::aos::Vec3VArg v2,  
+		Ps::aos::FloatV& v, 
+		Ps::aos::FloatV& w);
+
+	PX_INLINE Ps::aos::BoolV isValidTriangleBarycentricCoord(const Ps::aos::FloatVArg v, const Ps::aos::FloatVArg w)
+	{
+		using namespace Ps::aos;
+		const FloatV zero = FNeg(FEps());
+		const FloatV one = FAdd(FOne(), FEps());
+
+		const BoolV con0 = BAnd(FIsGrtrOrEq(v, zero), FIsGrtrOrEq(one, v));
+		const BoolV con1 = BAnd(FIsGrtrOrEq(w, zero), FIsGrtrOrEq(one, w));
+		const BoolV con2 = FIsGrtr(one, FAdd(v, w));
+		return BAnd(con0, BAnd(con1, con2));
+	}
+
+	PX_INLINE Ps::aos::BoolV isValidTriangleBarycentricCoord2(const Ps::aos::Vec4VArg vwvw)
+	{
+		using namespace Ps::aos;
+		const Vec4V eps = V4Splat(FEps());
+		const Vec4V zero =V4Neg(eps);
+		const Vec4V one = V4Add(V4One(), eps);
+
+		const Vec4V v0v1v0v1 = V4PermXZXZ(vwvw);
+		const Vec4V w0w1w0w1 = V4PermYWYW(vwvw);
+
+		const BoolV con0 = BAnd(V4IsGrtrOrEq(v0v1v0v1, zero), V4IsGrtrOrEq(one, v0v1v0v1));
+		const BoolV con1 = BAnd(V4IsGrtrOrEq(w0w1w0w1, zero), V4IsGrtrOrEq(one, w0w1w0w1));
+		const BoolV con2 = V4IsGrtr(one, V4Add(v0v1v0v1, w0w1w0w1));
+		return BAnd(con0, BAnd(con1, con2));
+	}
+
+} // namespace Gu
+
+}
+
+#endif

physx/source/geomutils/src/common/GuBoxConversion.h

@@ -0,0 +1,121 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_BOX_CONVERSION_H
+#define GU_BOX_CONVERSION_H
+
+#include "GuBox.h"
+#include "PsMathUtils.h"
+#include "CmMatrix34.h"
+#include "PsVecMath.h"
+
+namespace physx
+{
+	// PT: builds rot from quat. WARNING: writes 4 bytes after 'dst.rot'.
+	PX_FORCE_INLINE void buildFrom(Gu::Box& dst, const PxQuat& q)
+	{
+		using namespace Ps::aos;
+		const QuatV qV = V4LoadU(&q.x);
+		Vec3V column0, column1, column2;
+		QuatGetMat33V(qV, column0, column1, column2);
+		// PT: TODO: investigate if these overlapping stores are a problem
+		V4StoreU(Vec4V_From_Vec3V(column0), &dst.rot.column0.x);
+		V4StoreU(Vec4V_From_Vec3V(column1), &dst.rot.column1.x);
+		V4StoreU(Vec4V_From_Vec3V(column2), &dst.rot.column2.x);
+	}
+
+	PX_FORCE_INLINE void buildFrom(Gu::Box& dst, const PxVec3& center, const PxVec3& extents, const PxQuat& q)
+	{
+		using namespace Ps::aos;
+		// PT: writes 4 bytes after 'rot' but it's safe since we then write 'center' just afterwards
+		buildFrom(dst, q);
+		dst.center	= center;
+		dst.extents	= extents;
+	}
+
+	PX_FORCE_INLINE void buildMatrixFromBox(Cm::Matrix34& mat34, const Gu::Box& box)
+	{
+		mat34.m	= box.rot;
+		mat34.p	= box.center;
+	}
+
+	// SD: function is now the same as FastVertex2ShapeScaling::transformQueryBounds
+	// PT: lots of LHS in that one. TODO: revisit...
+	PX_INLINE Gu::Box transform(const Cm::Matrix34& transfo, const Gu::Box& box)
+	{
+		Gu::Box ret;
+		PxMat33& obbBasis = ret.rot;
+
+		obbBasis.column0 = transfo.rotate(box.rot.column0 * box.extents.x);
+		obbBasis.column1 = transfo.rotate(box.rot.column1 * box.extents.y);
+		obbBasis.column2 = transfo.rotate(box.rot.column2 * box.extents.z);
+
+		ret.center = transfo.transform(box.center);
+		ret.extents = Ps::optimizeBoundingBox(obbBasis);
+		return ret;
+	}
+
+	PX_INLINE Gu::Box transformBoxOrthonormal(const Gu::Box& box, const PxTransform& t)
+	{
+		Gu::Box ret;
+		PxMat33& obbBasis = ret.rot;
+		obbBasis.column0 = t.rotate(box.rot.column0);
+		obbBasis.column1 = t.rotate(box.rot.column1);
+		obbBasis.column2 = t.rotate(box.rot.column2);
+		ret.center = t.transform(box.center);
+		ret.extents = box.extents;
+		return ret;
+	}
+
+	/**
+	\brief recomputes the OBB after an arbitrary transform by a 4x4 matrix.
+	\param	mtx		[in] the transform matrix
+	\param	obb		[out] the transformed OBB
+	*/
+	PX_INLINE	void rotate(const Gu::Box& src, const Cm::Matrix34& mtx, Gu::Box& obb)
+	{
+		// The extents remain constant
+		obb.extents = src.extents;
+		// The center gets x-formed
+		obb.center = mtx.transform(src.center);
+		// Combine rotations
+		obb.rot = mtx.m * src.rot;
+	}
+
+// PT: TODO: move this to a better place
+	PX_FORCE_INLINE void getInverse(PxMat33& dstRot, PxVec3& dstTrans, const PxMat33& srcRot, const PxVec3& srcTrans)
+	{
+		const PxMat33 invRot = srcRot.getInverse();
+		dstTrans = invRot.transform(-srcTrans);
+		dstRot = invRot;
+	}
+
+}
+
+#endif

physx/source/geomutils/src/common/GuEdgeCache.h

@@ -0,0 +1,85 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_EDGECACHE_H
+#define GU_EDGECACHE_H
+
+#include "foundation/PxMemory.h"
+#include "CmPhysXCommon.h"
+#include "PsHash.h"
+
+namespace physx
+{
+namespace Gu
+{
+	class EdgeCache
+	{
+#define NUM_EDGES_IN_CACHE 64		//must be power of 2.	32 lines result in 10% extra work (due to cache misses), 64 lines in 6% extra work, 128 lines in 4%.
+	public:
+		EdgeCache()
+		{
+			PxMemZero(cacheLines, NUM_EDGES_IN_CACHE*sizeof(CacheLine));
+		}
+
+		PxU32 hash(PxU32 key)	const
+		{
+			return (NUM_EDGES_IN_CACHE - 1) & Ps::hash(key);		//Only a 16 bit hash would be needed here.
+		}
+
+		bool isInCache(PxU8 vertex0, PxU8 vertex1)
+		{
+			PX_ASSERT(vertex1 >= vertex0);
+			PxU16 key = PxU16((vertex0 << 8) | vertex1);
+			PxU32 h = hash(key);
+			CacheLine& cl = cacheLines[h];
+			if (cl.fullKey == key)
+			{
+				return true;
+			}
+			else	//cache the line now as it's about to be processed
+			{
+				cl.fullKey = key;
+				return false;
+			}
+		}
+
+	private:
+		struct CacheLine
+		{
+			PxU16 fullKey;
+		};
+		CacheLine cacheLines[NUM_EDGES_IN_CACHE];
+#undef NUM_EDGES_IN_CACHE
+	};
+}
+
+}
+
+#endif
+

physx/source/geomutils/src/common/GuEdgeListData.h

@@ -0,0 +1,153 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_EDGE_LIST_DATA_H
+#define GU_EDGE_LIST_DATA_H
+
+#include "foundation/PxSimpleTypes.h"
+#include "CmPhysXCommon.h"
+
+namespace physx
+{
+namespace Gu
+{
+
+/*!
+NOTICE!
+
+This is a data-code separated version of PxPhysics::EdgeList.
+
+It is to be shared between high and low level code, so make sure both are recompiled
+if any change is done here.
+*/
+
+// Flags
+enum EdgeType
+{
+	PX_EDGE_UNDEFINED,
+
+	PX_EDGE_BOUNDARY,		//!< Edge belongs to a single triangle
+	PX_EDGE_INTERNAL,		//!< Edge belongs to exactly two triangles
+	PX_EDGE_SINGULAR,		//!< Edge belongs to three or more triangles
+
+	PX_EDGE_FORCE_DWORD	= 0x7fffffff
+};
+
+enum EdgeFlag
+{
+	PX_EDGE_ACTIVE	= (1<<0)
+};
+
+
+// Data
+
+
+//! Basic edge-data
+struct EdgeData
+{
+	PxU32	Ref0;	//!< First vertex reference	
+	PxU32	Ref1;	//!< Second vertex reference
+};
+PX_COMPILE_TIME_ASSERT(sizeof(Gu::EdgeData) == 8);
+
+
+//! Basic edge-data using 8-bit references
+struct Edge8Data
+{
+	PxU8	Ref0;	//!< First vertex reference
+	PxU8	Ref1;	//!< Second vertex reference
+};
+PX_COMPILE_TIME_ASSERT(sizeof(Gu::Edge8Data) == 2);
+
+
+//! A count/offset pair = an edge descriptor
+struct EdgeDescData
+{
+	PxU16	Flags;
+	PxU16	Count;
+	PxU32	Offset;
+};
+PX_COMPILE_TIME_ASSERT(sizeof(Gu::EdgeDescData) == 8);
+
+
+//! Edge<->triangle mapping
+struct EdgeTriangleData
+{
+	PxU32	mLink[3];
+};
+PX_COMPILE_TIME_ASSERT(sizeof(Gu::EdgeTriangleData) == 12);
+
+
+struct EdgeListData
+{
+	// The edge list
+	PxU32					mNbEdges;			//!< Number of edges in the list
+	Gu::EdgeData*			mEdges;				//!< List of edges
+	// Faces to edges
+	PxU32					mNbFaces;			//!< Number of faces for which we have data
+	Gu::EdgeTriangleData*	mEdgeFaces;			//!< Array of edge-triangles referencing mEdges
+	// Edges to faces
+	Gu::EdgeDescData*		mEdgeToTriangles;	//!< An EdgeDesc structure for each edge
+	PxU32*					mFacesByEdges;		//!< A pool of face indices
+};
+#if PX_P64_FAMILY
+PX_COMPILE_TIME_ASSERT(sizeof(Gu::EdgeListData) == 48);
+#else
+PX_COMPILE_TIME_ASSERT(sizeof(Gu::EdgeListData) == 24);
+#endif
+
+
+// Accessors
+
+enum
+{
+	MSH_EDGE_LINK_MASK		= 0x0fffffff,
+	MSH_ACTIVE_EDGE_MASK	= 0x80000000,
+	MSH_ACTIVE_VERTEX_MASK	= 0x40000000
+};
+
+class EdgeTriangleAC
+{
+public:
+	PX_INLINE static PxU32			GetEdge01(const Gu::EdgeTriangleData& data)					{ return data.mLink[0] & MSH_EDGE_LINK_MASK;	}
+	PX_INLINE static PxU32			GetEdge12(const Gu::EdgeTriangleData& data)					{ return data.mLink[1] & MSH_EDGE_LINK_MASK;	}
+	PX_INLINE static PxU32			GetEdge20(const Gu::EdgeTriangleData& data)					{ return data.mLink[2] & MSH_EDGE_LINK_MASK;	}
+	PX_INLINE static PxU32			GetEdge(const Gu::EdgeTriangleData& data, PxU32 i)			{ return data.mLink[i] & MSH_EDGE_LINK_MASK;	}
+
+	PX_INLINE static Ps::IntBool	HasActiveEdge01(const Gu::EdgeTriangleData& data)			{ return Ps::IntBool(data.mLink[0] & MSH_ACTIVE_EDGE_MASK);	}
+	PX_INLINE static Ps::IntBool	HasActiveEdge12(const Gu::EdgeTriangleData& data)			{ return Ps::IntBool(data.mLink[1] & MSH_ACTIVE_EDGE_MASK);	}
+	PX_INLINE static Ps::IntBool	HasActiveEdge20(const Gu::EdgeTriangleData& data)			{ return Ps::IntBool(data.mLink[2] & MSH_ACTIVE_EDGE_MASK);	}
+	PX_INLINE static Ps::IntBool	HasActiveEdge(const Gu::EdgeTriangleData& data, PxU32 i)	{ return Ps::IntBool(data.mLink[i] & MSH_ACTIVE_EDGE_MASK);	}
+};
+
+} // namespace Gu
+
+}
+
+#endif

physx/source/geomutils/src/common/GuSeparatingAxes.cpp

@@ -0,0 +1,64 @@
+//
+// 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-2021 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 "GuSeparatingAxes.h"
+
+using namespace physx;
+
+union FloatInt
+{
+	float	f;
+	PxU32	i;
+};
+
+bool Gu::SeparatingAxes::addAxis(const PxVec3& axis)
+{
+	PxU32 numAxes = getNumAxes();
+	const PxVec3* PX_RESTRICT axes = getAxes();
+	const PxVec3* PX_RESTRICT axes_end = axes + numAxes;
+	while(axes<axes_end)
+	{
+		if(PxAbs(axis.dot(*axes))>0.9999f)
+			return false;
+		axes++;
+	}
+
+#ifdef SEP_AXIS_FIXED_MEMORY
+	if(mNbAxes<SEP_AXIS_FIXED_MEMORY)
+	{
+		mAxes[mNbAxes++] = axis;
+		return true;
+	}
+
+	return false;
+#else
+	mAxes.pushBack(axis);
+	return true;
+#endif
+}

physx/source/geomutils/src/common/GuSeparatingAxes.h

@@ -0,0 +1,91 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_SEPARATINGAXES_H
+#define GU_SEPARATINGAXES_H
+
+#include "foundation/PxVec3.h"
+#include "common/PxPhysXCommonConfig.h"
+
+namespace physx
+{
+namespace Gu
+{
+	// PT: this is a number of axes. Multiply by sizeof(PxVec3) for size in bytes.
+	#define SEP_AXIS_FIXED_MEMORY	256
+
+	// This class holds a list of potential separating axes.
+	// - the orientation is irrelevant so V and -V should be the same vector
+	// - the scale is irrelevant so V and n*V should be the same vector
+	// - a given separating axis should appear only once in the class
+#if PX_VC 
+    #pragma warning(push)
+	#pragma warning( disable : 4251 ) // class needs to have dll-interface to be used by clients of class
+#endif
+	class PX_PHYSX_COMMON_API SeparatingAxes
+	{
+	public:
+		PX_INLINE SeparatingAxes() : mNbAxes(0)	{}
+
+		bool addAxis(const PxVec3& axis);
+
+		PX_FORCE_INLINE const PxVec3* getAxes() const
+		{
+			return mAxes;
+		}
+
+		PX_FORCE_INLINE PxU32 getNumAxes() const
+		{
+			return mNbAxes;
+		}
+
+		PX_FORCE_INLINE void reset()
+		{
+			mNbAxes = 0;
+		}
+
+	private:
+		PxU32	mNbAxes;
+		PxVec3	mAxes[SEP_AXIS_FIXED_MEMORY];
+	};
+#if PX_VC 
+     #pragma warning(pop) 
+#endif
+
+	enum PxcSepAxisType
+	{
+		SA_NORMAL0,		// Normal of object 0
+		SA_NORMAL1,		// Normal of object 1
+		SA_EE			// Cross product of edges
+	};
+
+}
+}
+
+#endif

physx/source/geomutils/src/contact/GuContactBoxBox.cpp

@@ -0,0 +1,705 @@
+//
+// 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-2021 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 "geomutils/GuContactBuffer.h"
+
+#include "GuContactMethodImpl.h"
+#include "GuGeometryUnion.h"
+
+#include "CmMatrix34.h"
+#include "PsUtilities.h"
+
+using namespace physx;
+using namespace Gu;
+
+#define MAX_NB_CTCS	8 + 12*5 + 6*4
+#define ABS_GREATER(x, y)			(PxAbs(x) > (y))
+#define ABS_SMALLER_EQUAL(x, y)		(PxAbs(x) <= (y))
+//#define AIR(x)					((PxU32&)(x)&SIGN_BITMASK)
+//#define ABS_GREATER(x, y)			(AIR(x) > IR(y))
+//#define ABS_SMALLER_EQUAL(x, y)		(AIR(x) <= IR(y))
+
+#if PX_X86 && !PX_OSX
+
+	// Some float optimizations ported over from novodex.
+
+	//returns non zero if the value is negative.
+	#define PXC_IS_NEGATIVE(x) (((PxU32&)(x)) & 0x80000000)
+
+#else
+
+	//On most platforms using the integer rep is worse(produces LHSs) since the CPU has more registers.
+
+	//returns non zero if the value is negative.
+	#define PXC_IS_NEGATIVE(x) ((x) < 0.0f)
+
+#endif
+
+
+enum
+{
+	AXIS_A0, AXIS_A1, AXIS_A2,
+	AXIS_B0, AXIS_B1, AXIS_B2
+};
+
+struct VertexInfo
+{
+	PxVec3	pos;
+	bool	penetrate;
+	bool	area;
+};
+
+
+/*static PxI32 doBoxBoxContactGeneration(PxVec3 ctcPts[MAX_NB_CTCS], PxReal depths[MAX_NB_CTCS], PxVec3* ctcNrm,
+									 const PxVec3& extents0, const PxVec3& extents1,
+									 PxU32& collisionData,
+									 const Cm::Matrix34& transform0, const Cm::Matrix34& transform1, PxReal contactDistance);*/
+
+static PxI32 doBoxBoxContactGeneration(ContactBuffer& contactBuffer,
+									 const PxVec3& extents0, const PxVec3& extents1,
+									 PxU32& collisionData,
+									 const Cm::Matrix34& transform0, const Cm::Matrix34& transform1, PxReal contactDistance);
+
+namespace physx
+{
+
+namespace Gu
+{
+bool contactBoxBox(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(renderOutput);
+
+	// Get actual shape data
+	const PxBoxGeometry& shapeBox0 = shape0.get<const PxBoxGeometry>();
+	const PxBoxGeometry& shapeBox1 = shape1.get<const PxBoxGeometry>();
+
+	PxU32 pd = PxU32(cache.mPairData);
+	PxI32 Nb = doBoxBoxContactGeneration(contactBuffer,
+					shapeBox0.halfExtents, shapeBox1.halfExtents,
+					pd,
+					Cm::Matrix34(transform0), Cm::Matrix34(transform1),
+					params.mContactDistance);
+
+	cache.mPairData = Ps::to8(pd);
+
+	if(!Nb)
+	{
+		cache.mPairData = 0;	// Mark as separated for temporal coherence
+		return false;			// WARNING: the contact stream code below used to output stuff even for 0 contacts (!). Now we just return here.
+	}
+	return true;
+}
+}//Gu
+}//physx
+
+// face => 4 vertices of a face of the cube (i.e. a quad)
+static PX_FORCE_INLINE PxReal IsInYZ(const PxReal y, const PxReal z, const VertexInfo** PX_RESTRICT face)
+{
+	// Warning, indices have been remapped. We're now actually like this:
+	//
+	// 3+------+2
+	//  |   |  |
+	//  |   *--|
+	//  | (y,z)|
+	// 0+------+1
+	PxReal PreviousY = face[3]->pos.y;
+	PxReal PreviousZ = face[3]->pos.z;
+
+	// Loop through quad vertices
+	for(PxI32 i=0; i<4; i++)
+	{
+		const PxReal CurrentY = face[i]->pos.y;
+		const PxReal CurrentZ = face[i]->pos.z;
+
+		// |CurrentY - PreviousY      y - PreviousY|
+		// |CurrentZ - PreviousZ      z - PreviousZ|
+		// => similar to backface culling, check each one of the 4 triangles are consistent, in which case
+		// the point is within the parallelogram.
+		if((CurrentY - PreviousY)*(z - PreviousZ) - (CurrentZ - PreviousZ)*(y - PreviousY) >= 0.0f)	return -1.0f;
+
+		PreviousY = CurrentY;
+		PreviousZ = CurrentZ;
+	}
+
+	PxReal x = face[0]->pos.x;
+	{
+		const PxReal ay = y - face[0]->pos.y;
+		const PxReal az = z - face[0]->pos.z;
+
+		PxVec3 b = face[1]->pos - face[0]->pos;	// ### could be precomputed ?
+		x += b.x * (ay*b.y + az*b.z) / b.magnitudeSquared();		// ### could be precomputed ?
+
+		b = face[3]->pos - face[0]->pos;		// ### could be precomputed ?
+		x += b.x * (ay*b.y + az*b.z) / b.magnitudeSquared();		// ### could be precomputed ?
+	}
+
+	return x;
+}
+
+// Test with respect to the quad defined by (0,-y1,-z1) and (0,y1,z1)
+//  +------+ y1    y
+//  |      |       |
+//  |  *   |       |
+//  |      |       |
+//  +------+ -y1   *-----z
+static PxI32 generateContacts(//PxVec3 ctcPts[], PxReal depths[], 
+								ContactBuffer& contactBuffer, const PxVec3& contactNormal,
+								PxReal y1, PxReal z1, const PxVec3& box2,
+								const Cm::Matrix34& transform0, const Cm::Matrix34& transform1, PxReal contactDistance)
+{
+//	PxI32 NbContacts=0;
+	contactBuffer.reset();
+	y1 += contactDistance;
+	z1 += contactDistance;
+
+	const Cm::Matrix34 trans1to0 = transform0.getInverseRT() * transform1;
+
+	VertexInfo vtx[8];	// The 8 cube vertices
+//	PxI32 i;
+
+	//     6+------+7
+	//     /|     /|
+	//    / |    / |
+	//   / 4+---/--+5
+	// 2+------+3 /    y   z
+	//  | /    | /     |  /
+	//  |/     |/      |/
+	// 0+------+1      *---x
+
+	{
+		const PxVec3 ex = trans1to0.m.column0 * box2.x;
+		const PxVec3 ey = trans1to0.m.column1 * box2.y;
+		const PxVec3 ez = trans1to0.m.column2 * box2.z;
+		
+		/*		
+		vtx[0].pos = mat.pos - ex - ey - ez;
+		vtx[1].pos = mat.pos + ex - ey - ez;
+		vtx[2].pos = mat.pos - ex + ey - ez;
+		vtx[3].pos = mat.pos + ex + ey - ez;
+		vtx[4].pos = mat.pos - ex - ey + ez;
+		vtx[5].pos = mat.pos + ex - ey + ez;
+		vtx[6].pos = mat.pos - ex + ey + ez;
+		vtx[7].pos = mat.pos + ex + ey + ez;
+		*/
+
+		// 12 vector ops = 12*3 = 36 FPU ops
+		vtx[0].pos = vtx[2].pos = vtx[4].pos = vtx[6].pos = trans1to0.p - ex;
+		vtx[1].pos = vtx[3].pos = vtx[5].pos = vtx[7].pos = trans1to0.p + ex;
+
+		PxVec3 e = ey+ez;
+		vtx[0].pos -= e;
+		vtx[1].pos -= e;
+		vtx[6].pos += e;
+		vtx[7].pos += e;
+		
+		e = ey-ez;
+		vtx[2].pos += e;
+		vtx[3].pos += e;
+		vtx[4].pos -= e;
+		vtx[5].pos -= e;
+	}
+
+	// Create vertex info for 8 vertices
+	for(PxU32 i=0; i<8; i++)
+	{
+		// Vertex suivant
+		VertexInfo& p = vtx[i];
+		// test the point with respect to the x = 0 plane
+		//		if(p.pos.x < 0)
+		if(p.pos.x < -contactDistance)	//if(PXC_IS_NEGATIVE(p.pos.x))
+		{
+			p.area		= false;
+			p.penetrate	= false;
+			continue;
+		}
+
+		{
+			// we penetrated the quad plane
+			p.penetrate = true;
+			// test to see if we are in the quad
+			// PxAbs => thus we test Y with respect to -Y1 and +Y1 (same for Z)
+			//			if(PxAbs(p->pos.y) <= y1 && PxAbs(p->pos.z) <= z1)
+			if(ABS_SMALLER_EQUAL(p.pos.y, y1) && ABS_SMALLER_EQUAL(p.pos.z, z1))
+			{
+				// the point is inside the quad
+				p.area=true;
+				// Since we are testing with respect to x = 0, the penetration is directly the x coordinate.
+//				depths[NbContacts] = p.pos.x;
+
+				// We take the vertex as the impact point
+//				ctcPts[NbContacts++] = p.pos;
+				contactBuffer.contact(p.pos, contactNormal, -p.pos.x);
+			}
+			else
+			{
+				p.area=false;
+			}
+		}
+	}
+
+	// Teste 12 edges on the quad
+	static const PxI32 indices[]={ 0,1, 1,3, 3,2, 2,0, 4,5, 5,7, 7,6, 6,4, 0,4, 1,5, 2,6, 3,7, };
+	const PxI32* runningLine = indices;
+	const PxI32* endLine = runningLine+24;
+	while(runningLine!=endLine)
+	{
+		// The two vertices of the current edge
+		const VertexInfo* p1 = &vtx[*runningLine++];
+		const VertexInfo* p2 = &vtx[*runningLine++];
+
+		// Penetrate|Area|Penetrate|Area => 16 cases
+
+		// We only take the edges that at least penetrated the quad's plane into account.
+		if(p1->penetrate || p2->penetrate)
+			//		if(p1->penetrate + p2->penetrate)	// One branch only
+		{
+			// If at least one of the two vertices is not in the quad...
+			if(!p1->area || !p2->area)
+				//			if(!p1->area + !p2->area)	// One branch only
+			{
+				// Test y
+				if(p1->pos.y > p2->pos.y)		{ const VertexInfo* tmp=p1; p1=p2; p2=tmp; }
+				// Impact on the +Y1 edge of the quad
+				if(p1->pos.y < +y1 && p2->pos.y >= +y1)	
+					// => a point under Y1, the other above
+				{
+					// Case 1
+					PxReal a = (+y1 - p1->pos.y)/(p2->pos.y - p1->pos.y);
+					PxReal z = p1->pos.z + (p2->pos.z - p1->pos.z)*a;
+					if(PxAbs(z) <= z1)
+					{
+						PxReal x = p1->pos.x + (p2->pos.x - p1->pos.x)*a;
+						if(x+contactDistance>=0.0f)
+						{
+//							depths[NbContacts] = x;
+//							ctcPts[NbContacts++] = PxVec3(x, y1, z);
+							contactBuffer.contact(PxVec3(x, y1, z), contactNormal, -x);
+						}
+					}
+				}
+				// Impact on the edge -Y1 of the quad
+				if(p1->pos.y < -y1 && p2->pos.y >= -y1)
+				{
+					// Case 2
+					PxReal a = (-y1 - p1->pos.y)/(p2->pos.y - p1->pos.y);
+					PxReal z = p1->pos.z + (p2->pos.z - p1->pos.z)*a;
+					if(PxAbs(z) <= z1)
+					{
+						PxReal x = p1->pos.x + (p2->pos.x - p1->pos.x)*a;
+						if(x+contactDistance>=0.0f)
+						{
+//							depths[NbContacts] = x;
+//							ctcPts[NbContacts++] = PxVec3(x, -y1, z);
+							contactBuffer.contact(PxVec3(x, -y1, z), contactNormal, -x);
+						}
+					}
+				}
+
+				// Test z
+				if(p1->pos.z > p2->pos.z)		{ const VertexInfo* tmp=p1; p1=p2; p2=tmp; }
+				// Impact on the edge +Z1 of the quad
+				if(p1->pos.z < +z1 && p2->pos.z >= +z1)
+				{
+					// Case 3
+					PxReal a = (+z1 - p1->pos.z)/(p2->pos.z - p1->pos.z);
+					PxReal y = p1->pos.y + (p2->pos.y - p1->pos.y)*a;
+					if(PxAbs(y) <= y1)
+					{
+						PxReal x = p1->pos.x + (p2->pos.x - p1->pos.x)*a;
+						if(x+contactDistance>=0.0f)
+						{
+//							depths[NbContacts] = x;
+//							ctcPts[NbContacts++] = PxVec3(x, y, z1);
+							contactBuffer.contact(PxVec3(x, y, z1), contactNormal, -x);
+						}
+					}
+				}
+				// Impact on the edge -Z1 of the quad
+				if(p1->pos.z < -z1 && p2->pos.z >= -z1)
+				{
+					// Case 4
+					PxReal a = (-z1 - p1->pos.z)/(p2->pos.z - p1->pos.z);
+					PxReal y = p1->pos.y + (p2->pos.y - p1->pos.y)*a;
+					if(PxAbs(y) <= y1)
+					{
+						PxReal x = p1->pos.x + (p2->pos.x - p1->pos.x)*a;
+						if(x+contactDistance>=0.0f)
+						{
+//							depths[NbContacts] = x;
+//							ctcPts[NbContacts++] = PxVec3(x, y, -z1);
+							contactBuffer.contact(PxVec3(x, y, -z1), contactNormal, -x);
+						}
+					}
+				}
+			}
+
+			// The case where one point penetrates the plane, and the other is not in the quad.
+			if((!p1->penetrate && !p2->area) || (!p2->penetrate && !p1->area))
+			{
+				// Case 5
+				PxReal a = (-p1->pos.x)/(p2->pos.x - p1->pos.x);
+				PxReal y = p1->pos.y + (p2->pos.y - p1->pos.y)*a;
+				if(PxAbs(y) <= y1)
+				{
+					PxReal z = p1->pos.z + (p2->pos.z - p1->pos.z)*a;
+					if(PxAbs(z) <= z1)
+					{
+//						depths[NbContacts] = 0;
+//						ctcPts[NbContacts++] = PxVec3(0, y, z);
+						contactBuffer.contact(PxVec3(0, y, z), contactNormal, 0);
+					}
+				}
+			}
+
+		}
+	}
+
+	{
+		// 6 quads => 6 faces of the cube
+		static const PxI32 face[][4]={ {0,1,3,2}, {1,5,7,3}, {5,4,6,7}, {4,0,2,6}, {2,3,7,6}, {0,4,5,1} };
+		PxI32 addflg=0;
+		for(PxU32 i=0; i<6 && addflg!=0x0f; i++)
+		{
+			const PxI32* p = face[i];
+			const VertexInfo* q[4];
+			if((q[0]=&vtx[p[0]])->penetrate && (q[1]=&vtx[p[1]])->penetrate && (q[2]=&vtx[p[2]])->penetrate && (q[3]=&vtx[p[3]])->penetrate)
+			{
+				if(!q[0]->area || !q[1]->area || !q[2]->area || !q[3]->area)
+				{
+					if(!(addflg&1))	{ PxReal x = IsInYZ(-y1, -z1, q); if(x>=0.0f)	{ addflg|=1; contactBuffer.contact(PxVec3(x, -y1, -z1), contactNormal, -x); /*depths[NbContacts]=x; ctcPts[NbContacts++] = PxVec3(x, -y1, -z1);*/ } }
+					if(!(addflg&2))	{ PxReal x = IsInYZ(+y1, -z1, q); if(x>=0.0f)	{ addflg|=2; contactBuffer.contact(PxVec3(x, +y1, -z1), contactNormal, -x); /*depths[NbContacts]=x; ctcPts[NbContacts++] = PxVec3(x, +y1, -z1);*/ } }
+					if(!(addflg&4))	{ PxReal x = IsInYZ(-y1, +z1, q); if(x>=0.0f)	{ addflg|=4; contactBuffer.contact(PxVec3(x, -y1, +z1), contactNormal, -x); /*depths[NbContacts]=x; ctcPts[NbContacts++] = PxVec3(x, -y1, +z1);*/ } }
+					if(!(addflg&8))	{ PxReal x = IsInYZ(+y1, +z1, q); if(x>=0.0f)	{ addflg|=8; contactBuffer.contact(PxVec3(x, +y1, +z1), contactNormal, -x); /*depths[NbContacts]=x; ctcPts[NbContacts++] = PxVec3(x, +y1, +z1);*/ } }
+				}
+			}
+		}
+	}
+
+//	for(i=0; i<NbContacts; i++)
+	for(PxU32 i=0; i<contactBuffer.count; i++)
+//		ctcPts[i] = transform0.transform(ctcPts[i]);	// local to world
+		contactBuffer.contacts[i].point = transform0.transform(contactBuffer.contacts[i].point);	// local to world
+
+	//PX_ASSERT(NbContacts);	//if this did not make contacts then something went wrong in theory, but even the old code without distances had this flaw!
+//	return NbContacts;
+	return PxI32(contactBuffer.count);
+}
+
+//static PxI32 doBoxBoxContactGeneration(PxVec3 ctcPts[MAX_NB_CTCS], PxReal depths[MAX_NB_CTCS], PxVec3* ctcNrm,
+static PxI32 doBoxBoxContactGeneration(ContactBuffer& contactBuffer,
+									 const PxVec3& extents0, const PxVec3& extents1,
+									 PxU32& collisionData,
+									 const Cm::Matrix34& transform0, const Cm::Matrix34& transform1, PxReal contactDistance)
+{
+	PxReal aafC[3][3];			// matrix C = A^T B, c_{ij} = Dot(A_i,B_j)
+	PxReal aafAbsC[3][3];		// |c_{ij}|
+	PxReal afAD[3];			// Dot(A_i,D)
+
+	PxReal d1[6];
+	PxReal overlap[6];
+
+	PxVec3 kD = transform1.p - transform0.p;
+
+	const PxVec3& axis00 = transform0.m.column0;
+	const PxVec3& axis01 = transform0.m.column1;
+	const PxVec3& axis02 = transform0.m.column2;
+	const PxVec3& axis10 = transform1.m.column0;
+	const PxVec3& axis11 = transform1.m.column1;
+	const PxVec3& axis12 = transform1.m.column2;
+
+	// Perform Class I tests
+
+	aafC[0][0]	= axis00.dot(axis10);
+	aafC[0][1]	= axis00.dot(axis11);
+	aafC[0][2]	= axis00.dot(axis12);
+	afAD[0]		= axis00.dot(kD);
+	aafAbsC[0][0] = 1e-6f + PxAbs(aafC[0][0]);
+	aafAbsC[0][1] = 1e-6f + PxAbs(aafC[0][1]);
+	aafAbsC[0][2] = 1e-6f + PxAbs(aafC[0][2]);
+	d1[AXIS_A0] = afAD[0];
+	PxReal d0 = extents0.x + extents1.x*aafAbsC[0][0] + extents1.y*aafAbsC[0][1] + extents1.z*aafAbsC[0][2];
+	overlap[AXIS_A0] = d0 - PxAbs(d1[AXIS_A0]) + contactDistance;
+	if(PXC_IS_NEGATIVE(overlap[AXIS_A0]))		return 0;
+
+	aafC[1][0]	= axis01.dot(axis10);
+	aafC[1][1]	= axis01.dot(axis11);
+	aafC[1][2]	= axis01.dot(axis12);
+	afAD[1]		= axis01.dot(kD);
+	aafAbsC[1][0] = 1e-6f + PxAbs(aafC[1][0]);
+	aafAbsC[1][1] = 1e-6f + PxAbs(aafC[1][1]);
+	aafAbsC[1][2] = 1e-6f + PxAbs(aafC[1][2]);
+	d1[AXIS_A1] = afAD[1];
+	d0 = extents0.y + extents1.x*aafAbsC[1][0] + extents1.y*aafAbsC[1][1] + extents1.z*aafAbsC[1][2];
+	overlap[AXIS_A1] = d0 - PxAbs(d1[AXIS_A1]) + contactDistance;
+	if(PXC_IS_NEGATIVE(overlap[AXIS_A1]))		return 0;
+
+	aafC[2][0]	= axis02.dot(axis10);
+	aafC[2][1]	= axis02.dot(axis11);
+	aafC[2][2]	= axis02.dot(axis12);
+	afAD[2]		= axis02.dot(kD);
+	aafAbsC[2][0] = 1e-6f + PxAbs(aafC[2][0]);
+	aafAbsC[2][1] = 1e-6f + PxAbs(aafC[2][1]);
+	aafAbsC[2][2] = 1e-6f + PxAbs(aafC[2][2]);
+	d1[AXIS_A2] = afAD[2];
+	d0 = extents0.z + extents1.x*aafAbsC[2][0] + extents1.y*aafAbsC[2][1] + extents1.z*aafAbsC[2][2];
+	overlap[AXIS_A2] = d0 - PxAbs(d1[AXIS_A2]) + contactDistance;
+	if(PXC_IS_NEGATIVE(overlap[AXIS_A2]))		return 0;
+
+	// Perform Class II tests
+
+	d1[AXIS_B0] = axis10.dot(kD);
+	d0 = extents1.x + extents0.x*aafAbsC[0][0] + extents0.y*aafAbsC[1][0] + extents0.z*aafAbsC[2][0];
+	overlap[AXIS_B0] = d0 - PxAbs(d1[AXIS_B0]) + contactDistance;
+	if(PXC_IS_NEGATIVE(overlap[AXIS_B0]))		return 0;
+
+	d1[AXIS_B1] = axis11.dot(kD);
+	d0 = extents1.y + extents0.x*aafAbsC[0][1] + extents0.y*aafAbsC[1][1] + extents0.z*aafAbsC[2][1];
+	overlap[AXIS_B1] = d0 - PxAbs(d1[AXIS_B1]) + contactDistance;
+	if(PXC_IS_NEGATIVE(overlap[AXIS_B1]))		return 0;
+
+	d1[AXIS_B2] = axis12.dot(kD);
+	d0 = extents1.z + extents0.x*aafAbsC[0][2] + extents0.y*aafAbsC[1][2] + extents0.z*aafAbsC[2][2];
+	overlap[AXIS_B2] = d0 - PxAbs(d1[AXIS_B2]) + contactDistance;
+	if(PXC_IS_NEGATIVE(overlap[AXIS_B2]))		return 0;
+
+	// Perform Class III tests - we don't need to store distances for those ones.
+	// We only test those axes when objects are likely to be separated, i.e. when they where previously non-colliding. For stacks, we'll have
+	// to do full contact generation anyway, and those tests are useless - so we skip them. This is similar to what I did in Opcode.
+	if(!collisionData)	// separated or first run
+	{
+		PxReal d = afAD[2]*aafC[1][0] - afAD[1]*aafC[2][0];
+		d0 = contactDistance + extents0.y*aafAbsC[2][0] + extents0.z*aafAbsC[1][0] + extents1.y*aafAbsC[0][2] + extents1.z*aafAbsC[0][1];
+		if(ABS_GREATER(d, d0))	return 0;
+
+		d = afAD[2]*aafC[1][1] - afAD[1]*aafC[2][1];
+		d0 = contactDistance + extents0.y*aafAbsC[2][1] + extents0.z*aafAbsC[1][1] + extents1.x*aafAbsC[0][2] + extents1.z*aafAbsC[0][0];
+		if(ABS_GREATER(d, d0))	return 0;
+
+		d = afAD[2]*aafC[1][2] - afAD[1]*aafC[2][2];
+		d0 = contactDistance + extents0.y*aafAbsC[2][2] + extents0.z*aafAbsC[1][2] + extents1.x*aafAbsC[0][1] + extents1.y*aafAbsC[0][0];
+		if(ABS_GREATER(d, d0))	return 0;
+
+		d = afAD[0]*aafC[2][0] - afAD[2]*aafC[0][0];
+		d0 = contactDistance + extents0.x*aafAbsC[2][0] + extents0.z*aafAbsC[0][0] + extents1.y*aafAbsC[1][2] + extents1.z*aafAbsC[1][1];
+		if(ABS_GREATER(d, d0))	return 0;
+
+		d = afAD[0]*aafC[2][1] - afAD[2]*aafC[0][1];
+		d0 = contactDistance + extents0.x*aafAbsC[2][1] + extents0.z*aafAbsC[0][1] + extents1.x*aafAbsC[1][2] + extents1.z*aafAbsC[1][0];
+		if(ABS_GREATER(d, d0))	return 0;
+
+		d = afAD[0]*aafC[2][2] - afAD[2]*aafC[0][2];
+		d0 = contactDistance + extents0.x*aafAbsC[2][2] + extents0.z*aafAbsC[0][2] + extents1.x*aafAbsC[1][1] + extents1.y*aafAbsC[1][0];
+		if(ABS_GREATER(d, d0))	return 0;
+
+		d = afAD[1]*aafC[0][0] - afAD[0]*aafC[1][0];
+		d0 = contactDistance + extents0.x*aafAbsC[1][0] + extents0.y*aafAbsC[0][0] + extents1.y*aafAbsC[2][2] + extents1.z*aafAbsC[2][1];
+		if(ABS_GREATER(d, d0))	return 0;
+
+		d = afAD[1]*aafC[0][1] - afAD[0]*aafC[1][1];
+		d0 = contactDistance + extents0.x*aafAbsC[1][1] + extents0.y*aafAbsC[0][1] + extents1.x*aafAbsC[2][2] + extents1.z*aafAbsC[2][0];
+		if(ABS_GREATER(d, d0))	return 0;
+
+		d = afAD[1]*aafC[0][2] - afAD[0]*aafC[1][2];
+		d0 = contactDistance + extents0.x*aafAbsC[1][2] + extents0.y*aafAbsC[0][2] + extents1.x*aafAbsC[2][1] + extents1.y*aafAbsC[2][0];
+		if(ABS_GREATER(d, d0))	return 0;
+	}
+
+	/* djs - tempUserData can be zero when it gets here 
+	- maybe if there was no previous axis? 
+	- which causes stack corruption, and thence a crash, in .NET
+
+	PT:	right! At first tempUserData wasn't ever supposed to be zero, but then I used that
+	value to mark separation of boxes, and forgot to update the code below. Now I think
+	the test is redundant with the one performed above, and the line could eventually
+	be merged in the previous block. I'll do that later when removing all the #defines.
+	*/
+	// NB: the "16" here has nothing to do with MAX_NB_CTCS. Don't touch.
+	if(collisionData) // if initialized & not previously separated
+		overlap[collisionData-1] *= 0.999f;	 // Favorise previous axis  .999 is too little.
+
+	PxReal	minimum = PX_MAX_REAL;
+	PxI32	minIndex = 0;
+
+	for(PxU32 i=AXIS_A0; i<6; i++)
+	{
+		PxReal d = overlap[i];
+
+		if(d>=0.0f && d<minimum)	{ minimum=d; minIndex=PxI32(i); }		// >=0 !!  otherwise bug at sep = 0
+	}
+
+	collisionData = PxU32(minIndex + 1);	// Leave "0" for separation
+
+#if PX_X86
+	const PxU32 sign = PXC_IS_NEGATIVE(d1[minIndex]);
+#else
+	const PxU32 sign = PxU32(PXC_IS_NEGATIVE(d1[minIndex]));
+#endif
+	Cm::Matrix34 trs;
+	PxVec3 ctcNrm;
+
+	switch(minIndex)
+	{
+	default:
+		return 0;
+
+	case AXIS_A0:
+//		*ctcNrm = axis00;
+		if(sign)	
+		{
+			ctcNrm = axis00;
+			trs.m = transform0.m;
+			trs.p = transform0.p - extents0.x*axis00;
+		}
+		else		
+		{
+//			*ctcNrm = -*ctcNrm;
+			ctcNrm = -axis00;
+
+			trs.m.column0 = -axis00;
+			trs.m.column1 = -axis01;
+			trs.m.column2 = axis02;
+			trs.p = transform0.p + extents0.x*axis00;
+		}
+//		return generateContacts(ctcPts, depths, extents0.y, extents0.z, extents1, trs, transform1, contactDistance);
+		return generateContacts(contactBuffer, ctcNrm, extents0.y, extents0.z, extents1, trs, transform1, contactDistance);
+
+	case AXIS_A1:
+//		*ctcNrm = axis01;
+		trs.m.column2 = axis00;	// Factored out
+		if(sign)	
+		{
+			ctcNrm = axis01;
+			trs.m.column0 = axis01;
+			trs.m.column1 = axis02;
+			trs.p = transform0.p - extents0.y*axis01;
+		}
+		else		
+		{
+//			*ctcNrm = -*ctcNrm;	
+			ctcNrm = -axis01;
+			
+			trs.m.column0 = -axis01;
+			trs.m.column1 = -axis02;
+			trs.p = transform0.p + extents0.y*axis01;
+		}
+//		return generateContacts(ctcPts, depths, extents0.z, extents0.x, extents1, trs, transform1, contactDistance);
+		return generateContacts(contactBuffer, ctcNrm, extents0.z, extents0.x, extents1, trs, transform1, contactDistance);
+
+	case AXIS_A2:
+//		*ctcNrm = axis02;
+		trs.m.column2 = axis01;	// Factored out
+
+		if(sign)	
+		{
+			ctcNrm = axis02;
+			trs.m.column0 = axis02;
+			trs.m.column1 = axis00;
+			trs.p = transform0.p - extents0.z*axis02;
+		}
+		else		
+		{
+//			*ctcNrm = -*ctcNrm;	
+			ctcNrm = -axis02;
+			
+			trs.m.column0 = -axis02;
+			trs.m.column1 = -axis00;
+			trs.p = transform0.p + extents0.z*axis02;
+		}
+//		return generateContacts(ctcPts, depths, extents0.x, extents0.y, extents1, trs, transform1, contactDistance);
+		return generateContacts(contactBuffer, ctcNrm, extents0.x, extents0.y, extents1, trs, transform1, contactDistance);
+
+	case AXIS_B0:
+//		*ctcNrm = axis10;
+		if(sign)	
+		{
+			ctcNrm = axis10;
+			trs.m.column0 = -axis10;
+			trs.m.column1 = -axis11;
+			trs.m.column2 = axis12;
+			trs.p = transform1.p + extents1.x*axis10;
+		}
+		else		
+		{
+//			*ctcNrm = -*ctcNrm;	
+			ctcNrm = -axis10;
+			trs.m = transform1.m;
+			trs.p = transform1.p - extents1.x*axis10;
+		
+		}
+//		return generateContacts(ctcPts, depths, extents1.y, extents1.z, extents0, trs, transform0, contactDistance);
+		return generateContacts(contactBuffer, ctcNrm, extents1.y, extents1.z, extents0, trs, transform0, contactDistance);
+
+	case AXIS_B1:
+//		*ctcNrm = axis11;
+		trs.m.column2 = axis10;	// Factored out
+		if(sign)	
+		{
+			ctcNrm = axis11;
+			trs.m.column0 = -axis11;
+			trs.m.column1 = -axis12;
+			trs.p = transform1.p + extents1.y*axis11;
+		}
+		else		
+		{ 
+//			*ctcNrm = -*ctcNrm;	
+			ctcNrm = -axis11;
+			
+			trs.m.column0 = axis11;
+			trs.m.column1 = axis12;
+			trs.m.column2 = axis10;
+			trs.p = transform1.p - extents1.y*axis11;
+		}
+//		return generateContacts(ctcPts, depths, extents1.z, extents1.x, extents0, trs, transform0, contactDistance);
+		return generateContacts(contactBuffer, ctcNrm, extents1.z, extents1.x, extents0, trs, transform0, contactDistance);
+
+	case AXIS_B2:
+//		*ctcNrm = axis12;
+		trs.m.column2 = axis11;	// Factored out
+
+		if(sign)	
+		{
+			ctcNrm = axis12;
+			trs.m.column0 = -axis12;
+			trs.m.column1 = -axis10;
+			trs.p = transform1.p + extents1.z*axis12;
+		}
+		else		
+		{ 
+//			*ctcNrm = -*ctcNrm;	
+			ctcNrm = -axis12;
+			
+			trs.m.column0 = axis12;
+			trs.m.column1 = axis10;
+			trs.p = transform1.p - extents1.z*axis12;
+		}
+
+//		return generateContacts(ctcPts, depths, extents1.x, extents1.y, extents0, trs, transform0, contactDistance);
+		return generateContacts(contactBuffer, ctcNrm, extents1.x, extents1.y, extents0, trs, transform0, contactDistance);
+	}
+}

physx/source/geomutils/src/contact/GuContactCapsuleBox.cpp

@@ -0,0 +1,459 @@
+//
+// 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-2021 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 "geomutils/GuContactBuffer.h"
+
+#include "GuIntersectionRayBox.h"
+#include "GuDistanceSegmentBox.h"
+#include "GuInternal.h"
+#include "GuContactMethodImpl.h"
+#include "GuGeometryUnion.h"
+#include "GuBoxConversion.h"
+
+#include "PsMathUtils.h"
+#include "PsUtilities.h"
+
+using namespace physx;
+using namespace Gu;
+
+/*namespace Gu
+{
+const PxU8* getBoxEdges();
+}*/
+
+/////////
+	/*#include "CmRenderOutput.h"
+	#include "PxsContext.h"
+	static void gVisualizeBox(const Box& box, PxcNpThreadContext& context, PxU32 color=0xffffff)
+	{
+		PxMat33 rot(box.base.column0, box.base.column1, box.base.column2);
+		PxMat44 m(rot, box.origin);
+
+		DebugBox db(box.extent);
+
+		Cm::RenderOutput& out = context.mRenderOutput;
+		out << color << m;
+		out << db;
+	}
+	static void gVisualizeLine(const PxVec3& a, const PxVec3& b, PxcNpThreadContext& context, PxU32 color=0xffffff)
+	{
+		PxMat44 m = PxMat44::identity();
+
+		Cm::RenderOutput& out = context.mRenderOutput;
+		out << color << m << Cm::RenderOutput::LINES << a << b;
+	}*/
+/////////
+
+
+static const PxReal fatBoxEdgeCoeff = 0.01f;
+
+static bool intersectEdgeEdgePreca(const PxVec3& p1, const PxVec3& p2, const PxVec3& v1, const PxPlane& plane, PxU32 i, PxU32 j, float coeff, const PxVec3& dir, const PxVec3& p3, const PxVec3& p4, PxReal& dist, PxVec3& ip)
+{
+	// if colliding edge (p3,p4) does not cross plane return no collision
+	// same as if p3 and p4 on same side of plane return 0
+	//
+	// Derivation:
+	// d3 = d(p3, P) = (p3 | plane.n) - plane.d;		Reversed sign compared to Plane::Distance() because plane.d is negated.
+	// d4 = d(p4, P) = (p4 | plane.n) - plane.d;		Reversed sign compared to Plane::Distance() because plane.d is negated.
+	// if d3 and d4 have the same sign, they're on the same side of the plane => no collision
+	// We test both sides at the same time by only testing Sign(d3 * d4).
+	// ### put that in the Plane class
+	// ### also check that code in the triangle class that might be similar
+	const PxReal d3 = plane.distance(p3);
+	PxReal temp = d3 * plane.distance(p4);
+	if(temp>0.0f)	return false;
+
+	// if colliding edge (p3,p4) and plane are parallel return no collision
+	PxVec3 v2 = p4 - p3;
+
+	temp = plane.n.dot(v2);
+	if(temp==0.0f)	return false;	// ### epsilon would be better
+
+	// compute intersection point of plane and colliding edge (p3,p4)
+	ip = p3-v2*(d3/temp);
+
+	// compute distance of intersection from line (ip, -dir) to line (p1,p2)
+	dist =	(v1[i]*(ip[j]-p1[j])-v1[j]*(ip[i]-p1[i]))*coeff;
+	if(dist<0.0f)	return false;
+
+	// compute intersection point on edge (p1,p2) line
+	ip -= dist*dir;
+
+	// check if intersection point (ip) is between edge (p1,p2) vertices
+	temp = (p1.x-ip.x)*(p2.x-ip.x)+(p1.y-ip.y)*(p2.y-ip.y)+(p1.z-ip.z)*(p2.z-ip.z);
+	if(temp<0.0f)	return true;	// collision found
+
+	return false;	// no collision
+}
+
+
+static bool GuTestAxis(const PxVec3& axis, const Segment& segment, PxReal radius, const Box& box, PxReal& depth)
+{
+	// Project capsule
+	PxReal min0 = segment.p0.dot(axis);
+	PxReal max0 = segment.p1.dot(axis);
+	if(min0>max0) Ps::swap(min0, max0);
+	min0 -= radius;
+	max0 += radius;
+
+	// Project box
+	PxReal Min1, Max1;
+	{
+		const PxReal BoxCen = box.center.dot(axis);
+		const PxReal BoxExt = 
+				PxAbs(box.rot.column0.dot(axis)) * box.extents.x
+			+	PxAbs(box.rot.column1.dot(axis)) * box.extents.y
+			+	PxAbs(box.rot.column2.dot(axis)) * box.extents.z;
+
+		Min1 = BoxCen - BoxExt; 
+		Max1 = BoxCen + BoxExt; 
+	}
+
+	// Test projections
+	if(max0<Min1 || Max1<min0)
+		return false;
+
+	const PxReal d0 = max0 - Min1;
+	PX_ASSERT(d0>=0.0f);
+	const PxReal d1 = Max1 - min0;
+	PX_ASSERT(d1>=0.0f);
+	depth = physx::intrinsics::selectMin(d0, d1);
+	return true;
+}
+
+
+static bool GuCapsuleOBBOverlap3(const Segment& segment, PxReal radius, const Box& box, PxReal* t=NULL, PxVec3* pp=NULL)
+{
+	PxVec3 Sep(PxReal(0));
+	PxReal PenDepth = PX_MAX_REAL;
+
+	// Test normals
+	for(PxU32 i=0;i<3;i++)
+	{
+		PxReal d;
+		if(!GuTestAxis(box.rot[i], segment, radius, box, d))
+			return false;
+
+		if(d<PenDepth)
+		{
+			PenDepth = d;
+			Sep = box.rot[i];
+		}
+	}
+
+	// Test edges
+	PxVec3 CapsuleAxis(segment.p1 - segment.p0);
+	CapsuleAxis = CapsuleAxis.getNormalized();
+	for(PxU32 i=0;i<3;i++)
+	{
+		PxVec3 Cross = CapsuleAxis.cross(box.rot[i]);
+		if(!Ps::isAlmostZero(Cross))
+		{
+			Cross = Cross.getNormalized();
+			PxReal d;
+			if(!GuTestAxis(Cross, segment, radius, box, d))
+				return false;
+
+			if(d<PenDepth)
+			{
+				PenDepth = d;
+				Sep = Cross;
+			}
+		}
+	}
+
+	const PxVec3 Witness = segment.computeCenter() - box.center;
+
+	if(Sep.dot(Witness) < 0.0f)
+		Sep = -Sep;
+
+	if(t)
+		*t = PenDepth;
+	if(pp)
+		*pp = Sep;
+
+	return true;
+}
+
+
+static void GuGenerateVFContacts(	ContactBuffer& contactBuffer,
+									//
+									const Segment& segment,
+									const PxReal radius,
+									//
+									const Box& worldBox,
+									//
+									const PxVec3& normal,
+									const PxReal contactDistance)
+{
+	const PxVec3 Max = worldBox.extents;
+	const PxVec3 Min = -worldBox.extents;
+
+	const PxVec3 tmp2 = - worldBox.rot.transformTranspose(normal);
+
+	const PxVec3* PX_RESTRICT Ptr = &segment.p0;
+	for(PxU32 i=0;i<2;i++)
+	{
+		const PxVec3& Pos = Ptr[i];
+
+		const PxVec3 tmp = worldBox.rot.transformTranspose(Pos - worldBox.center);
+		PxReal tnear, tfar;
+		int Res = intersectRayAABB(Min, Max, tmp, tmp2, tnear, tfar);
+
+		if(Res!=-1 && tnear < radius + contactDistance)
+		{
+			contactBuffer.contact(Pos - tnear * normal, normal, tnear - radius);
+		}
+	}
+}
+
+
+// PT: this looks similar to PxcGenerateEEContacts2 but it is mandatory to properly handle thin capsules.
+static void GuGenerateEEContacts(	ContactBuffer& contactBuffer,
+									//
+									const Segment& segment,
+									const PxReal radius,
+									//
+									const Box& worldBox,
+									//
+									const PxVec3& normal)
+{
+	const PxU8* PX_RESTRICT Indices = getBoxEdges();
+
+	PxVec3 Pts[8];
+	worldBox.computeBoxPoints(Pts);
+
+	PxVec3 s0 = segment.p0;
+	PxVec3 s1 = segment.p1;
+	Ps::makeFatEdge(s0, s1, fatBoxEdgeCoeff);
+
+	// PT: precomputed part of edge-edge intersection test
+//	const PxVec3 v1 = segment.p1 - segment.p0;
+	const PxVec3 v1 = s1 - s0;
+	PxPlane plane;
+	plane.n = v1.cross(normal);
+//		plane.d = -(plane.normal|segment.p0);
+	plane.d = -(plane.n.dot(s0));
+
+	PxU32 ii,jj;
+	Ps::closestAxis(plane.n, ii, jj);
+
+	const float coeff = 1.0f /(v1[ii]*normal[jj]-v1[jj]*normal[ii]);
+
+
+	for(PxU32 i=0;i<12;i++)
+	{
+//		PxVec3 p1 = Pts[*Indices++];
+//		PxVec3 p2 = Pts[*Indices++];
+//		Ps::makeFatEdge(p1, p2, fatBoxEdgeCoeff);	// PT: TODO: make fat segment instead
+		const PxVec3& p1 = Pts[*Indices++];
+		const PxVec3& p2 = Pts[*Indices++];
+
+//		PT: keep original code in case something goes wrong
+//		PxReal dist;
+//		PxVec3 ip;
+//		if(intersectEdgeEdge(p1, p2, -normal, segment.p0, segment.p1, dist, ip))
+//			contactBuffer.contact(ip, normal, - (radius + dist));
+
+		PxReal dist;
+		PxVec3 ip;
+
+		
+		if(intersectEdgeEdgePreca(s0, s1, v1, plane, ii, jj, coeff, normal, p1, p2, dist, ip))
+//		if(intersectEdgeEdgePreca(segment.p0, segment.p1, v1, plane, ii, jj, coeff, normal, p1, p2, dist, ip))
+		{
+			contactBuffer.contact(ip-normal*dist, normal, - (radius + dist));
+//			if(contactBuffer.count==2)	// PT: we only need 2 contacts to be stable
+//				return;
+		}
+	}
+}
+
+
+static void GuGenerateEEContacts2(	ContactBuffer& contactBuffer,
+									//
+									const Segment& segment,
+									const PxReal radius,
+									//
+									const Box& worldBox,
+									//
+									const PxVec3& normal,
+									const PxReal contactDistance)
+{
+	const PxU8* PX_RESTRICT Indices = getBoxEdges();
+
+	PxVec3 Pts[8];
+	worldBox.computeBoxPoints(Pts);
+
+	PxVec3 s0 = segment.p0;
+	PxVec3 s1 = segment.p1;
+	Ps::makeFatEdge(s0, s1, fatBoxEdgeCoeff);
+
+	// PT: precomputed part of edge-edge intersection test
+//		const PxVec3 v1 = segment.p1 - segment.p0;
+		const PxVec3 v1 = s1 - s0;
+		PxPlane plane;
+		plane.n = -(v1.cross(normal));
+//		plane.d = -(plane.normal|segment.p0);
+		plane.d = -(plane.n.dot(s0));
+
+		PxU32 ii,jj;
+		Ps::closestAxis(plane.n, ii, jj);
+
+		const float coeff = 1.0f /(v1[jj]*normal[ii]-v1[ii]*normal[jj]);
+
+	for(PxU32 i=0;i<12;i++)
+	{
+//		PxVec3 p1 = Pts[*Indices++];
+//		PxVec3 p2 = Pts[*Indices++];
+//		Ps::makeFatEdge(p1, p2, fatBoxEdgeCoeff);	// PT: TODO: make fat segment instead
+		const PxVec3& p1 = Pts[*Indices++];
+		const PxVec3& p2 = Pts[*Indices++];
+
+//		PT: keep original code in case something goes wrong
+//		PxReal dist;
+//		PxVec3 ip;
+//		bool contact = intersectEdgeEdge(p1, p2, normal, segment.p0, segment.p1, dist, ip);
+//		if(contact && dist < radius + contactDistance)
+//			contactBuffer.contact(ip, normal, dist - radius);
+
+		PxReal dist;
+		PxVec3 ip;
+//		bool contact = intersectEdgeEdgePreca(segment.p0, segment.p1, v1, plane, ii, jj, coeff, -normal, p1, p2, dist, ip);
+		bool contact = intersectEdgeEdgePreca(s0, s1, v1, plane, ii, jj, coeff, -normal, p1, p2, dist, ip);
+		if(contact && dist < radius + contactDistance)
+		{
+			contactBuffer.contact(ip-normal*dist, normal, dist - radius);
+//			if(contactBuffer.count==2)	// PT: we only need 2 contacts to be stable
+//				return;
+		}
+	}
+}
+
+namespace physx
+{
+namespace Gu
+{
+bool contactCapsuleBox(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(renderOutput);
+	PX_UNUSED(cache);
+
+	// Get actual shape data
+	const PxCapsuleGeometry& shapeCapsule = shape0.get<const PxCapsuleGeometry>();
+	const PxBoxGeometry& shapeBox = shape1.get<const PxBoxGeometry>();
+
+	// PT: TODO: move computations to local space
+
+	// Capsule data
+	Segment worldSegment;
+	getCapsuleSegment(transform0, shapeCapsule, worldSegment);
+	const PxReal inflatedRadius = shapeCapsule.radius + params.mContactDistance;
+
+	// Box data
+	Box worldBox;
+	buildFrom(worldBox, transform1.p, shapeBox.halfExtents, transform1.q);
+
+	// Collision detection
+	PxReal t;
+	PxVec3 onBox;
+	const PxReal squareDist = distanceSegmentBoxSquared(worldSegment.p0, worldSegment.p1, worldBox.center, worldBox.extents, worldBox.rot, &t, &onBox);
+	
+	if(squareDist >= inflatedRadius*inflatedRadius)
+		return false;
+
+	PX_ASSERT(contactBuffer.count==0);
+
+	if(squareDist != 0.0f)
+	{
+		// PT: the capsule segment doesn't intersect the box => distance-based version
+		const PxVec3 onSegment = worldSegment.getPointAt(t);
+		onBox = worldBox.center + worldBox.rot.transform(onBox);
+
+		PxVec3 normal = onSegment - onBox;
+		PxReal normalLen = normal.magnitude();
+
+		if(normalLen > 0.0f)
+		{
+			normal *= 1.0f/normalLen;
+
+			// PT: generate VF contacts for segment's vertices vs box
+			GuGenerateVFContacts(contactBuffer, worldSegment, shapeCapsule.radius, worldBox, normal, params.mContactDistance);
+
+			// PT: early exit if we already have 2 stable contacts
+			if(contactBuffer.count==2)
+				return true;
+
+			// PT: else generate slower EE contacts
+			GuGenerateEEContacts2(contactBuffer, worldSegment, shapeCapsule.radius, worldBox, normal, params.mContactDistance);
+
+			// PT: run VF case for box-vertex-vs-capsule only if we don't have any contact yet
+			if(!contactBuffer.count)
+				contactBuffer.contact(onBox, normal, sqrtf(squareDist) - shapeCapsule.radius);
+		}
+		else
+		{
+			// On linux we encountered the following:
+			// For a case where a segment endpoint lies on the surface of a box, the squared distance between segment and box was tiny but still larger than 0.
+			// However, the computation of the normal length was exactly 0. In that case we should have switched to the penetration based version so we do it now
+			// instead.
+			goto PenetrationBasedCode;
+		}
+	}
+	else
+	{
+		PenetrationBasedCode:
+
+		// PT: the capsule segment intersects the box => penetration-based version
+
+		// PT: compute penetration vector (MTD)
+		PxVec3 sepAxis;
+		PxReal depth;
+		if(!GuCapsuleOBBOverlap3(worldSegment, shapeCapsule.radius, worldBox, &depth, &sepAxis)) return false;
+
+		// PT: generate VF contacts for segment's vertices vs box
+		GuGenerateVFContacts(contactBuffer, worldSegment, shapeCapsule.radius, worldBox, sepAxis, params.mContactDistance);
+
+		// PT: early exit if we already have 2 stable contacts
+		if(contactBuffer.count==2)
+			return true;
+
+		// PT: else generate slower EE contacts
+		GuGenerateEEContacts(contactBuffer, worldSegment, shapeCapsule.radius, worldBox, sepAxis);
+
+		if(!contactBuffer.count)
+		{
+			contactBuffer.contact(worldSegment.computeCenter(), sepAxis, -(shapeCapsule.radius + depth));
+			return true;
+		}
+	}
+	return true;
+}
+}//Gu
+}//physx

physx/source/geomutils/src/contact/GuContactCapsuleCapsule.cpp

@@ -0,0 +1,156 @@
+//
+// 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-2021 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 "geomutils/GuContactBuffer.h"
+
+#include "GuDistanceSegmentSegment.h"
+#include "GuContactMethodImpl.h"
+#include "GuInternal.h"
+#include "GuGeometryUnion.h"
+
+using namespace physx;
+
+namespace physx
+{
+namespace Gu
+{
+bool contactCapsuleCapsule(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(renderOutput);
+	PX_UNUSED(cache);
+
+	const PxCapsuleGeometry& capsuleGeom0 = shape0.get<const PxCapsuleGeometry>();
+	const PxCapsuleGeometry& capsuleGeom1 = shape1.get<const PxCapsuleGeometry>();
+
+	// PT: get capsules in local space
+	PxVec3 dir[2];
+	Segment segment[2];
+	{
+		const PxVec3 capsuleLocalSegment0 = getCapsuleHalfHeightVector(transform0, capsuleGeom0);
+		const PxVec3 capsuleLocalSegment1 = getCapsuleHalfHeightVector(transform1, capsuleGeom1);
+
+		const PxVec3 delta = transform1.p - transform0.p;
+		segment[0].p0 = capsuleLocalSegment0;
+		segment[0].p1 = -capsuleLocalSegment0;
+		dir[0] = -capsuleLocalSegment0*2.0f;
+		segment[1].p0 = capsuleLocalSegment1 + delta;
+		segment[1].p1 = -capsuleLocalSegment1 + delta;
+		dir[1] = -capsuleLocalSegment1*2.0f;
+	}
+
+	// PT: compute distance between capsules' segments
+	PxReal s,t;
+	const PxReal squareDist = distanceSegmentSegmentSquared(segment[0], segment[1], &s, &t);
+	const PxReal radiusSum = capsuleGeom0.radius + capsuleGeom1.radius;
+	const PxReal inflatedSum = radiusSum + params.mContactDistance;
+	const PxReal inflatedSumSquared = inflatedSum*inflatedSum;
+
+	if(squareDist >= inflatedSumSquared)
+		return false;
+
+	// PT: TODO: optimize this away
+	PxReal segLen[2];
+	segLen[0] = dir[0].magnitude();
+	segLen[1] = dir[1].magnitude();
+
+	if (segLen[0]) dir[0] *= 1.0f / segLen[0];
+	if (segLen[1]) dir[1] *= 1.0f / segLen[1];
+
+	if (PxAbs(dir[0].dot(dir[1])) > 0.9998f)	//almost parallel, ca. 1 degree difference --> generate two contact points at ends
+	{
+		PxU32 numCons = 0;
+
+		PxReal segLenEps[2];
+		segLenEps[0] = segLen[0] * 0.001f;//0.1% error is ok.
+		segLenEps[1] = segLen[1] * 0.001f;
+			
+		//project the two end points of each onto the axis of the other and take those 4 points.
+		//we could also generate a single normal at the single closest point, but this would be 'unstable'.
+
+		for (PxU32 destShapeIndex = 0; destShapeIndex < 2; destShapeIndex ++)
+		{
+			for (PxU32 startEnd = 0; startEnd < 2; startEnd ++)
+			{
+				const PxU32 srcShapeIndex = 1-destShapeIndex;
+				//project start/end of srcShapeIndex onto destShapeIndex.
+				PxVec3 pos[2];
+				pos[destShapeIndex] = startEnd ? segment[srcShapeIndex].p1 : segment[srcShapeIndex].p0;
+				const PxReal p = dir[destShapeIndex].dot(pos[destShapeIndex] - segment[destShapeIndex].p0);
+				if (p >= -segLenEps[destShapeIndex] && p <= (segLen[destShapeIndex] + segLenEps[destShapeIndex]))
+				{
+					pos[srcShapeIndex] = p * dir[destShapeIndex] + segment[destShapeIndex].p0;
+
+					PxVec3 normal = pos[1] - pos[0];
+
+					const PxReal normalLenSq = normal.magnitudeSquared();
+					if (normalLenSq > 1e-6f && normalLenSq < inflatedSumSquared)
+					{
+						const PxReal distance = PxSqrt(normalLenSq);
+						normal *= 1.0f/distance;
+						PxVec3 point = pos[1] - normal * (srcShapeIndex ? capsuleGeom1 : capsuleGeom0).radius;
+						point += transform0.p;
+						contactBuffer.contact(point, normal, distance - radiusSum);
+						numCons++;
+					}					
+				}
+			}
+		}
+
+		if (numCons)	//if we did not have contacts, then we may have the case where they are parallel, but are stacked end to end, in which case the old code will generate good contacts.
+			return true;
+	}
+
+	// Collision response
+	PxVec3 pos1 = segment[0].getPointAt(s);
+	PxVec3 pos2 = segment[1].getPointAt(t);
+
+	PxVec3 normal = pos1 - pos2;
+
+	const PxReal normalLenSq = normal.magnitudeSquared();
+	if (normalLenSq < 1e-6f)
+	{
+		// PT: TODO: revisit this. "FW" sounds old.
+		// Zero normal -> pick the direction of segment 0.
+		// Not always accurate but consistent with FW.
+		if (segLen[0] > 1e-6f)
+			normal = dir[0];
+		else 
+			normal = PxVec3(1.0f, 0.0f, 0.0f);
+	}
+	else
+	{
+		normal *= PxRecipSqrt(normalLenSq);
+	}
+	
+	pos1 += transform0.p;
+	contactBuffer.contact(pos1 - normal * capsuleGeom0.radius, normal, PxSqrt(squareDist) - radiusSum);
+	return true;
+}
+}//Gu
+}//physx

physx/source/geomutils/src/contact/GuContactCapsuleConvex.cpp

@@ -0,0 +1,588 @@
+//
+// 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-2021 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 "geomutils/GuContactBuffer.h"
+
+#include "GuConvexMesh.h"
+#include "GuConvexHelper.h"
+#include "GuContactMethodImpl.h"
+#include "GuVecConvexHull.h"
+#include "GuVecCapsule.h"
+#include "GuInternal.h"
+#include "GuGJK.h"
+#include "GuGeometryUnion.h"
+
+using namespace physx;
+using namespace Gu;
+
+///////////
+//	#include "CmRenderOutput.h"
+//	#include "PxsContext.h"
+//	static void gVisualizeLine(const PxVec3& a, const PxVec3& b, PxcNpThreadContext& context, PxU32 color=0xffffff)
+//	{
+//		PxMat44 m = PxMat44::identity();
+//
+//		Cm::RenderOutput& out = context.mRenderOutput;
+//		out << color << m << Cm::RenderOutput::LINES << a << b;
+//	}
+///////////
+
+static const PxReal fatConvexEdgeCoeff = 0.01f;
+
+static bool intersectEdgeEdgePreca(const PxVec3& p1, const PxVec3& p2, const PxVec3& v1, const PxPlane& plane, PxU32 i, PxU32 j, float coeff, const PxVec3& dir, const PxVec3& p3, const PxVec3& p4, PxReal& dist, PxVec3& ip, float limit)
+{
+	// if colliding edge (p3,p4) does not cross plane return no collision
+	// same as if p3 and p4 on same side of plane return 0
+	//
+	// Derivation:
+	// d3 = d(p3, P) = (p3 | plane.n) - plane.d;		Reversed sign compared to Plane::Distance() because plane.d is negated.
+	// d4 = d(p4, P) = (p4 | plane.n) - plane.d;		Reversed sign compared to Plane::Distance() because plane.d is negated.
+	// if d3 and d4 have the same sign, they're on the same side of the plane => no collision
+	// We test both sides at the same time by only testing Sign(d3 * d4).
+	// ### put that in the Plane class
+	// ### also check that code in the triangle class that might be similar
+	const PxReal d3 = plane.distance(p3);
+	PxReal temp = d3 * plane.distance(p4);
+	if(temp>0.0f)
+		return false;
+
+	// if colliding edge (p3,p4) and plane are parallel return no collision
+	PxVec3 v2 = p4 - p3;
+
+	temp = plane.n.dot(v2);
+	if(temp==0.0f)
+		return false;	// ### epsilon would be better
+
+	// compute intersection point of plane and colliding edge (p3,p4)
+	ip = p3-v2*(d3/temp);
+
+	// compute distance of intersection from line (ip, -dir) to line (p1,p2)
+	dist =	(v1[i]*(ip[j]-p1[j])-v1[j]*(ip[i]-p1[i]))*coeff;
+	if(dist<limit)
+		return false;
+
+	// compute intersection point on edge (p1,p2) line
+	ip -= dist*dir;
+
+	// check if intersection point (ip) is between edge (p1,p2) vertices
+	temp = (p1.x-ip.x)*(p2.x-ip.x)+(p1.y-ip.y)*(p2.y-ip.y)+(p1.z-ip.z)*(p2.z-ip.z);
+	if(temp<0.0f)
+		return true;	// collision found
+
+	return false;	// no collision
+}
+
+static bool GuTestAxis(const PxVec3& axis, const Gu::Segment& segment, PxReal radius,
+						const PolygonalData& polyData, const Cm::FastVertex2ShapeScaling& scaling,
+						const Cm::Matrix34& worldTM,
+						PxReal& depth)
+{
+	// Project capsule
+	PxReal min0 = segment.p0.dot(axis);
+	PxReal max0 = segment.p1.dot(axis);
+	if(min0>max0) Ps::swap(min0, max0);
+	min0 -= radius;
+	max0 += radius;
+
+	// Project convex
+	PxReal Min1, Max1;
+	(polyData.mProjectHull)(polyData, axis, worldTM, scaling, Min1, Max1);
+
+	// Test projections
+	if(max0<Min1 || Max1<min0)
+		return false;
+
+	const PxReal d0 = max0 - Min1;
+	PX_ASSERT(d0>=0.0f);
+	const PxReal d1 = Max1 - min0;
+	PX_ASSERT(d1>=0.0f);
+	depth = physx::intrinsics::selectMin(d0, d1);
+	return true;
+}
+
+static bool GuCapsuleConvexOverlap(const Gu::Segment& segment, PxReal radius,
+									const PolygonalData& polyData,
+									const Cm::FastVertex2ShapeScaling& scaling,
+									const PxTransform& transform,
+									PxReal* t, PxVec3* pp, bool isSphere)
+{
+	// TODO:
+	// - test normal & edge in same loop
+	// - local space
+	// - use precomputed face value
+	// - optimize projection
+
+	PxVec3 Sep(0,0,0);
+	PxReal PenDepth = PX_MAX_REAL;
+
+	PxU32 nbPolys = polyData.mNbPolygons;
+	const Gu::HullPolygonData* polys = polyData.mPolygons;
+
+	const Cm::Matrix34 worldTM(transform);
+
+	// Test normals
+	for(PxU32 i=0;i<nbPolys;i++)
+	{
+		const Gu::HullPolygonData& poly = polys[i];
+		const PxPlane& vertSpacePlane = poly.mPlane;
+
+		const PxVec3 worldNormal = worldTM.rotate(vertSpacePlane.n);
+
+		PxReal d;
+		if(!GuTestAxis(worldNormal, segment, radius, polyData, scaling, worldTM, d))
+			return false;
+
+		if(d<PenDepth)
+		{
+			PenDepth = d;
+			Sep = worldNormal;
+		}
+	}
+
+	// Test edges
+	if(!isSphere)
+	{
+		PxVec3 CapsuleAxis(segment.p1 - segment.p0);
+		CapsuleAxis = CapsuleAxis.getNormalized();
+		for(PxU32 i=0;i<nbPolys;i++)
+		{
+			const Gu::HullPolygonData& poly = polys[i];
+			const PxPlane& vertSpacePlane = poly.mPlane;
+
+			const PxVec3 worldNormal = worldTM.rotate(vertSpacePlane.n);
+
+			PxVec3 Cross = CapsuleAxis.cross(worldNormal);
+			if(!Ps::isAlmostZero(Cross))
+			{
+				Cross = Cross.getNormalized();
+				PxReal d;
+				if(!GuTestAxis(Cross, segment, radius, polyData, scaling, worldTM, d))
+					return false;
+
+				if(d<PenDepth)
+				{
+					PenDepth = d;
+					Sep = Cross;
+				}
+			}
+		}
+	}
+
+	const PxVec3 Witness = segment.computeCenter() - transform.transform(polyData.mCenter);
+
+	if(Sep.dot(Witness) < 0.0f)
+		Sep = -Sep;
+
+	if(t)	*t = PenDepth;
+	if(pp)	*pp = Sep;
+
+	return true;
+}
+
+
+
+
+
+static bool raycast_convexMesh2(	const PolygonalData& polyData,
+									const PxVec3& vrayOrig, const PxVec3& vrayDir,
+									PxReal maxDist, PxF32& t)
+{ 
+	PxU32 nPolys = polyData.mNbPolygons;
+	const Gu::HullPolygonData* PX_RESTRICT polys = polyData.mPolygons;
+
+	/*
+	Purely convex planes based algorithm
+	Iterate all planes of convex, with following rules:
+	* determine of ray origin is inside them all or not.  
+	* planes parallel to ray direction are immediate early out if we're on the outside side (plane normal is sep axis)
+	* else 
+		- for all planes the ray direction "enters" from the front side, track the one furthest along the ray direction (A)
+		- for all planes the ray direction "exits" from the back side, track the one furthest along the negative ray direction (B)
+	if the ray origin is outside the convex and if along the ray, A comes before B, the directed line stabs the convex at A
+	*/
+	PxReal latestEntry = -FLT_MAX;
+	PxReal earliestExit = FLT_MAX;
+
+	while(nPolys--)
+	{
+		const Gu::HullPolygonData& poly = *polys++;
+		const PxPlane& vertSpacePlane = poly.mPlane;
+
+		const PxReal distToPlane = vertSpacePlane.distance(vrayOrig);
+		const PxReal dn = vertSpacePlane.n.dot(vrayDir);
+		const PxReal distAlongRay = -distToPlane/dn;
+
+		if (dn > 1E-7f)	//the ray direction "exits" from the back side
+		{
+			earliestExit = physx::intrinsics::selectMin(earliestExit, distAlongRay);
+		}
+		else if (dn < -1E-7f)	//the ray direction "enters" from the front side
+		{
+/*			if (distAlongRay > latestEntry)
+			{
+				latestEntry = distAlongRay;
+			}*/
+			latestEntry = physx::intrinsics::selectMax(latestEntry, distAlongRay);
+		}
+		else
+		{
+			//plane normal and ray dir are orthogonal
+			if(distToPlane > 0.0f)	
+				return false;	//a plane is parallel with ray -- and we're outside the ray -- we definitely miss the entire convex!
+		}
+	}
+
+	if(latestEntry < earliestExit && latestEntry != -FLT_MAX && latestEntry < maxDist-1e-5f)
+	{
+		t = latestEntry;
+		return true;
+	}
+	return false;
+}
+
+// PT: version based on Gu::raycast_convexMesh to handle scaling, but modified to make sure it works when ray starts inside the convex
+static void GuGenerateVFContacts2(ContactBuffer& contactBuffer,
+									//
+									const PxTransform& convexPose,
+									const PolygonalData& polyData,	// Convex data
+									const PxMeshScale& scale,
+									//
+									PxU32 nbPts,
+									const PxVec3* PX_RESTRICT points,
+									const PxReal radius,		// Capsule's radius
+									//
+									const PxVec3& normal,
+									const PxReal contactDistance)
+{
+	PX_ASSERT(PxAbs(normal.magnitudeSquared()-1)<1e-4f);
+
+	//scaling: transform the ray to vertex space
+	const Cm::Matrix34 world2vertexSkew = scale.getInverse() * convexPose.getInverse();	
+
+	const PxVec3 vrayDir = world2vertexSkew.rotate( -normal );	
+
+	const PxReal maxDist = contactDistance + radius;
+
+	for(PxU32 i=0;i<nbPts;i++)
+	{
+		const PxVec3& rayOrigin = points[i];
+
+		const PxVec3 vrayOrig = world2vertexSkew.transform( rayOrigin );
+		PxF32 t;
+		if(raycast_convexMesh2(polyData, vrayOrig, vrayDir, maxDist, t))
+		{
+			contactBuffer.contact(rayOrigin - t * normal, normal, t - radius);
+		}
+	}
+}
+
+static void GuGenerateEEContacts(	ContactBuffer& contactBuffer,
+									//
+									const Gu::Segment& segment,
+									const PxReal radius,
+									const PxReal contactDistance,
+									//
+									const PolygonalData& polyData,
+									const PxTransform& transform,
+									const Cm::FastVertex2ShapeScaling& scaling,
+									//
+									const PxVec3& normal)
+{
+	PxU32 numPolygons = polyData.mNbPolygons;
+	const Gu::HullPolygonData* PX_RESTRICT polygons = polyData.mPolygons;
+	const PxU8* PX_RESTRICT vertexData = polyData.mPolygonVertexRefs;
+
+	ConvexEdge edges[512];
+	PxU32 nbEdges = findUniqueConvexEdges(512, edges, numPolygons, polygons, vertexData);
+
+	//
+	PxVec3 s0 = segment.p0;
+	PxVec3 s1 = segment.p1;
+	Ps::makeFatEdge(s0, s1, fatConvexEdgeCoeff);
+
+	// PT: precomputed part of edge-edge intersection test
+//		const PxVec3 v1 = segment.p1 - segment.p0;
+		const PxVec3 v1 = s1 - s0;
+		PxPlane plane;
+		plane.n = v1.cross(normal);
+//		plane.d = -(plane.normal|segment.p0);
+		plane.d = -(plane.n.dot(s0));
+
+		PxU32 ii,jj;
+		Ps::closestAxis(plane.n, ii, jj);
+
+		const float coeff = 1.0f /(v1[ii]*normal[jj]-v1[jj]*normal[ii]);
+
+	//
+
+	const PxVec3* PX_RESTRICT verts = polyData.mVerts;
+	for(PxU32 i=0;i<nbEdges;i++)
+	{
+		const PxU8 vi0 = edges[i].vref0;
+		const PxU8 vi1 = edges[i].vref1;
+
+//		PxVec3 p1 = transform.transform(verts[vi0]);
+//		PxVec3 p2 = transform.transform(verts[vi1]);
+//		Ps::makeFatEdge(p1, p2, fatConvexEdgeCoeff);	// PT: TODO: make fat segment instead
+		const PxVec3 p1 = transform.transform(scaling * verts[vi0]);
+		const PxVec3 p2 = transform.transform(scaling * verts[vi1]);
+
+		PxReal dist;
+		PxVec3 ip;
+//		if(intersectEdgeEdgePreca(segment.p0, segment.p1, v1, plane, ii, jj, coeff, normal, p1, p2, dist, ip))
+//		if(intersectEdgeEdgePreca(s0, s1, v1, plane, ii, jj, coeff, normal, p1, p2, dist, ip, -FLT_MAX))
+		if(intersectEdgeEdgePreca(s0, s1, v1, plane, ii, jj, coeff, normal, p1, p2, dist, ip, -radius-contactDistance))
+//		if(intersectEdgeEdgePreca(s0, s1, v1, plane, ii, jj, coeff, normal, p1, p2, dist, ip, 0))
+		{
+			contactBuffer.contact(ip-normal*dist, normal, - (radius + dist));
+//			if(contactBuffer.count==2)	// PT: we only need 2 contacts to be stable
+//				return;
+		}
+	}
+}
+
+static void GuGenerateEEContacts2b(ContactBuffer& contactBuffer,
+									//
+									const Gu::Segment& segment, 
+									const PxReal radius, 
+									//
+									const Cm::Matrix34& transform,
+									const PolygonalData& polyData,
+									const Cm::FastVertex2ShapeScaling& scaling,
+									//
+									const PxVec3& normal,
+									const PxReal contactDistance)
+{
+	// TODO:
+	// - local space
+
+	const PxVec3 localDir = transform.rotateTranspose(normal);
+	PxU32 polyIndex = (polyData.mSelectClosestEdgeCB)(polyData, scaling, localDir);
+
+	PxVec3 s0 = segment.p0;
+	PxVec3 s1 = segment.p1;
+	Ps::makeFatEdge(s0, s1, fatConvexEdgeCoeff);
+
+	// PT: precomputed part of edge-edge intersection test
+//		const PxVec3 v1 = segment.p1 - segment.p0;
+		const PxVec3 v1 = s1 - s0;
+		PxPlane plane;
+		plane.n = -(v1.cross(normal));
+//		plane.d = -(plane.normal|segment.p0);
+		plane.d = -(plane.n.dot(s0));
+
+		PxU32 ii,jj;
+		Ps::closestAxis(plane.n, ii, jj);
+
+		const float coeff = 1.0f /(v1[jj]*normal[ii]-v1[ii]*normal[jj]);
+	//
+
+	const PxVec3* PX_RESTRICT verts = polyData.mVerts;
+
+	const Gu::HullPolygonData& polygon = polyData.mPolygons[polyIndex];
+	const PxU8* PX_RESTRICT vRefBase = polyData.mPolygonVertexRefs + polygon.mVRef8;
+	PxU32 numEdges = polygon.mNbVerts;
+
+	PxU32 a = numEdges - 1;
+	PxU32 b = 0;
+	while(numEdges--)
+	{
+//		const PxVec3 p1 = transform.transform(verts[vRefBase[a]]);
+//		const PxVec3 p2 = transform.transform(verts[vRefBase[b]]);
+		const PxVec3 p1 = transform.transform(scaling * verts[vRefBase[a]]);
+		const PxVec3 p2 = transform.transform(scaling * verts[vRefBase[b]]);
+
+		PxReal dist;
+		PxVec3 ip;
+//		bool contact = intersectEdgeEdgePreca(segment.p0, segment.p1, v1, plane, ii, jj, coeff, -normal, p1, p2, dist, ip);
+		bool contact = intersectEdgeEdgePreca(s0, s1, v1, plane, ii, jj, coeff, -normal, p1, p2, dist, ip, 0.0f);
+		if(contact && dist < radius + contactDistance)
+		{
+			contactBuffer.contact(ip-normal*dist, normal, dist - radius);
+//			if(contactBuffer.count==2)	// PT: we only need 2 contacts to be stable
+//				return;
+		}
+
+		a = b;
+		b++;
+	}
+}
+
+namespace physx
+{
+namespace Gu
+{
+bool contactCapsuleConvex(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(renderOutput);
+	PX_UNUSED(cache);
+
+	// Get actual shape data
+	const PxCapsuleGeometry& shapeCapsule = shape0.get<const PxCapsuleGeometry>();
+	const PxConvexMeshGeometryLL& shapeConvex = shape1.get<const PxConvexMeshGeometryLL>();
+
+	PxVec3 onSegment, onConvex;
+	PxReal distance;
+	PxVec3 normal_;
+	{
+		Gu::ConvexMesh* cm = static_cast<Gu::ConvexMesh*>(shapeConvex.convexMesh);
+
+		using namespace Ps::aos;
+		Vec3V closA, closB, normalV;
+		GjkStatus status;
+		FloatV dist;
+		{
+	
+			const Vec3V zeroV = V3Zero();
+			const Gu::ConvexHullData* hullData = &cm->getHull();
+
+			const FloatV capsuleHalfHeight = FLoad(shapeCapsule.halfHeight);
+
+			const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
+			const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);
+
+			const PsMatTransformV aToB(transform1.transformInv(transform0));
+
+			Gu::ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, shapeConvex.scale.isIdentity());
+
+			//transform capsule(a) into the local space of convexHull(b), treat capsule as segment
+			Gu::CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), FZero());
+
+
+			LocalConvex<CapsuleV> convexA(capsule);
+			LocalConvex<ConvexHullV> convexB(convexHull);
+			const Vec3V initialSearchDir = V3Sub(convexA.getCenter(), convexB.getCenter());
+			
+			status = gjk<LocalConvex<CapsuleV>, LocalConvex<ConvexHullV> >(convexA, convexB, initialSearchDir, FMax(),closA, closB, normalV, dist);
+		}
+	
+
+		if(status == GJK_CONTACT)
+			distance = 0.f;
+		else
+		{
+			//const FloatV sqDist = FMul(dist, dist);
+			V3StoreU(closB, onConvex);
+			FStore(dist, &distance);
+			V3StoreU(normalV, normal_);
+			onConvex = transform1.transform(onConvex);
+			normal_ = transform1.rotate(normal_);
+		}
+	}
+
+	const PxReal inflatedRadius = shapeCapsule.radius + params.mContactDistance;
+
+	if(distance >= inflatedRadius)  
+		return false;
+
+	Gu::Segment worldSegment;
+	getCapsuleSegment(transform0, shapeCapsule, worldSegment);
+
+	const bool isSphere = worldSegment.p0 == worldSegment.p1;
+	const PxU32 nbPts = PxU32(isSphere ? 1 : 2);
+
+	PX_ASSERT(contactBuffer.count==0);
+
+	Cm::FastVertex2ShapeScaling convexScaling;
+	const bool idtConvexScale = shapeConvex.scale.isIdentity();
+	if(!idtConvexScale)
+		convexScaling.init(shapeConvex.scale);
+
+	PolygonalData polyData;
+	getPolygonalData_Convex(&polyData, shapeConvex.hullData, convexScaling);
+
+//	if(0)
+	if(distance > 0.f)
+	{
+		// PT: the capsule segment doesn't intersect the convex => distance-based version
+		PxVec3 normal = -normal_;
+
+		// PT: generate VF contacts for segment's vertices vs convex
+		GuGenerateVFContacts2(	contactBuffer,
+								transform1, polyData, shapeConvex.scale,
+								nbPts, &worldSegment.p0, shapeCapsule.radius,
+								normal, params.mContactDistance);
+
+		// PT: early exit if we already have 2 stable contacts
+		if(contactBuffer.count==2)
+			return true;
+
+		// PT: else generate slower EE contacts
+		if(!isSphere)
+		{
+			const Cm::Matrix34 worldTM(transform1);
+			GuGenerateEEContacts2b(contactBuffer, worldSegment, shapeCapsule.radius,
+									worldTM, polyData, convexScaling,
+									normal, params.mContactDistance);
+		}
+
+		// PT: run VF case for convex-vertex-vs-capsule only if we don't have any contact yet
+		if(!contactBuffer.count)
+		{
+//			gVisualizeLine(onConvex, onConvex + normal, context, PxDebugColor::eARGB_RED);
+			//PxReal distance = PxSqrt(sqDistance);
+			contactBuffer.contact(onConvex, normal, distance - shapeCapsule.radius);
+		}
+	}
+	else
+	{
+		// PT: the capsule segment intersects the convex => penetration-based version
+//printf("Penetration-based:\n");
+
+		// PT: compute penetration vector (MTD)
+		PxVec3 SepAxis;
+		if(!GuCapsuleConvexOverlap(worldSegment, shapeCapsule.radius, polyData, convexScaling, transform1, NULL, &SepAxis, isSphere))
+		{
+//printf("- no overlap\n");
+			return false;
+		}
+
+		// PT: generate VF contacts for segment's vertices vs convex
+		GuGenerateVFContacts2(	contactBuffer,
+								transform1, polyData, shapeConvex.scale,
+								nbPts, &worldSegment.p0, shapeCapsule.radius,
+								SepAxis, params.mContactDistance);
+
+		// PT: early exit if we already have 2 stable contacts
+//printf("- %d VF contacts\n", contactBuffer.count);
+		if(contactBuffer.count==2)
+			return true;
+
+		// PT: else generate slower EE contacts
+		if(!isSphere)
+		{
+			GuGenerateEEContacts(contactBuffer, worldSegment, shapeCapsule.radius, params.mContactDistance, polyData, transform1, convexScaling, SepAxis);
+//printf("- %d total contacts\n", contactBuffer.count);
+		}
+	}
+	return true;
+}
+
+}
+}

physx/source/geomutils/src/contact/GuContactCapsuleMesh.cpp

@@ -0,0 +1,636 @@
+//
+// 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-2021 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 "geomutils/GuContactBuffer.h"
+
+#include "GuIntersectionEdgeEdge.h"
+#include "GuDistanceSegmentTriangle.h"
+#include "GuIntersectionRayTriangle.h"
+#include "GuIntersectionTriangleBox.h"
+#include "GuInternal.h"
+#include "GuContactMethodImpl.h"
+#include "GuFeatureCode.h"
+#include "GuMidphaseInterface.h"
+#include "GuEntityReport.h"
+#include "GuHeightFieldUtil.h"
+#include "GuConvexEdgeFlags.h"
+#include "GuGeometryUnion.h"
+#include "GuSIMDHelpers.h"
+#include "GuBox.h"
+
+using namespace physx;
+using namespace Gu;
+
+#define DEBUG_RENDER_MESHCONTACTS 0
+
+#if DEBUG_RENDER_MESHCONTACTS
+#include "PxPhysics.h"
+#include "PxScene.h"
+#endif
+
+#define USE_AABB_TRI_CULLING
+
+//#define USE_CAPSULE_TRI_PROJ_CULLING
+//#define USE_CAPSULE_TRI_SAT_CULLING
+#define VISUALIZE_TOUCHED_TRIS	0
+#define VISUALIZE_CULLING_BOX	0
+
+#if VISUALIZE_TOUCHED_TRIS
+#include "CmRenderOutput.h"
+#include "PxsContactManager.h"
+#include "PxsContext.h"
+static void gVisualizeLine(const PxVec3& a, const PxVec3& b, PxcNpThreadContext& context, PxU32 color=0xffffff)
+{
+	PxMat44 m = PxMat44::identity();
+
+	Cm::RenderOutput& out = context.mRenderOutput;
+	out << color << m << Cm::RenderOutput::LINES << a << b;
+}
+static void gVisualizeTri(const PxVec3& a, const PxVec3& b, const PxVec3& c, PxcNpThreadContext& context, PxU32 color=0xffffff)
+{
+	PxMat44 m = PxMat44::identity();
+
+	Cm::RenderOutput& out = context.mRenderOutput;
+	out << color << m << Cm::RenderOutput::TRIANGLES << a << b << c;
+}
+
+static PxU32 gColors[8] = { 0xff0000ff, 0xff00ff00, 0xffff0000,
+							0xff00ffff, 0xffff00ff, 0xffffff00,
+							0xff000080, 0xff008000};
+#endif
+
+static const float fatBoxEdgeCoeff = 0.01f;
+
+static bool PxcTestAxis(const PxVec3& axis, const Segment& segment, PxReal radius, 
+						const PxVec3* PX_RESTRICT triVerts, PxReal& depth)
+{
+	// Project capsule
+	PxReal min0 = segment.p0.dot(axis);
+	PxReal max0 = segment.p1.dot(axis);
+	if(min0>max0)	Ps::swap(min0, max0);
+	min0 -= radius;
+	max0 += radius;
+
+	// Project triangle
+	float Min1, Max1;
+	{
+		Min1 = Max1 = triVerts[0].dot(axis);
+		const PxReal dp1 = triVerts[1].dot(axis);
+		Min1 = physx::intrinsics::selectMin(Min1, dp1);
+		Max1 = physx::intrinsics::selectMax(Max1, dp1);
+		const PxReal dp2 = triVerts[2].dot(axis);
+		Min1 = physx::intrinsics::selectMin(Min1, dp2);
+		Max1 = physx::intrinsics::selectMax(Max1, dp2);
+	}
+
+	// Test projections
+	if(max0<Min1 || Max1<min0)
+		return false;
+
+	const PxReal d0 = max0 - Min1;
+	PX_ASSERT(d0>=0.0f);
+	const PxReal d1 = Max1 - min0;
+	PX_ASSERT(d1>=0.0f);
+	depth = physx::intrinsics::selectMin(d0, d1);
+	return true;
+}
+
+PX_FORCE_INLINE static PxVec3 PxcComputeTriangleNormal(const PxVec3* PX_RESTRICT triVerts)
+{
+	return ((triVerts[0]-triVerts[1]).cross(triVerts[0]-triVerts[2])).getNormalized();
+}
+
+PX_FORCE_INLINE static PxVec3 PxcComputeTriangleCenter(const PxVec3* PX_RESTRICT triVerts)
+{
+	static const PxReal inv3 = 1.0f / 3.0f;
+	return (triVerts[0] + triVerts[1] + triVerts[2]) * inv3;
+}
+
+static bool PxcCapsuleTriOverlap3(PxU8 edgeFlags, const Segment& segment, PxReal radius, const PxVec3* PX_RESTRICT triVerts,
+								  PxReal* PX_RESTRICT t=NULL, PxVec3* PX_RESTRICT pp=NULL)
+{
+	PxReal penDepth = PX_MAX_REAL;
+
+	// Test normal
+	PxVec3 sep = PxcComputeTriangleNormal(triVerts);
+	if(!PxcTestAxis(sep, segment, radius, triVerts, penDepth))
+		return false;
+
+	// Test edges
+	// ML:: use the active edge flag instead of the concave flag
+	const PxU32 activeEdgeFlag[] = {ETD_CONVEX_EDGE_01, ETD_CONVEX_EDGE_12, ETD_CONVEX_EDGE_20};
+	const PxVec3 capsuleAxis = (segment.p1 - segment.p0).getNormalized();
+	for(PxU32 i=0;i<3;i++)
+	{
+		//bool active =((edgeFlags & ignoreEdgeFlag[i]) == 0);
+		
+		if(edgeFlags & activeEdgeFlag[i])
+		{
+		
+			const PxVec3 e0 = triVerts[i];
+//			const PxVec3 e1 = triVerts[(i+1)%3];
+			const PxVec3 e1 = triVerts[Ps::getNextIndex3(i)];
+			const PxVec3 edge = e0 - e1;
+
+			PxVec3 cross = capsuleAxis.cross(edge);
+			if(!Ps::isAlmostZero(cross))
+			{
+				cross = cross.getNormalized();
+				PxReal d;
+				if(!PxcTestAxis(cross, segment, radius, triVerts, d))
+					return false;
+				if(d<penDepth)
+				{
+					penDepth = d;
+					sep = cross;
+				}
+			}
+		}
+	}
+
+	const PxVec3 capsuleCenter = segment.computeCenter();
+	const PxVec3 triCenter = PxcComputeTriangleCenter(triVerts);
+	const PxVec3 witness = capsuleCenter - triCenter;
+
+	if(sep.dot(witness) < 0.0f)
+		sep = -sep;
+
+	if(t)	*t = penDepth;
+	if(pp)	*pp = sep;
+
+	return true;
+}
+
+static void PxcGenerateVFContacts(	const Cm::Matrix34& meshAbsPose, ContactBuffer& contactBuffer, const Segment& segment,
+									const PxReal radius, const PxVec3* PX_RESTRICT triVerts, const PxVec3& normal, 
+									PxU32 triangleIndex, PxReal contactDistance)
+{
+	const PxVec3* PX_RESTRICT Ptr = &segment.p0;
+	for(PxU32 i=0;i<2;i++)
+	{
+		const PxVec3& Pos = Ptr[i];
+		PxReal t,u,v;
+		if(intersectRayTriangleCulling(Pos, -normal, triVerts[0], triVerts[1], triVerts[2], t, u, v, 1e-3f) && t < radius + contactDistance)
+		{
+			const PxVec3 Hit = meshAbsPose.transform(Pos - t * normal);
+			const PxVec3 wn = meshAbsPose.rotate(normal);
+			
+			contactBuffer.contact(Hit, wn, t-radius, triangleIndex);
+			#if DEBUG_RENDER_MESHCONTACTS
+			PxScene *s; PxGetPhysics().getScenes(&s, 1, 0);
+			Cm::RenderOutput((Cm::RenderBuffer&)s->getRenderBuffer()) << Cm::RenderOutput::LINES << PxDebugColor::eARGB_BLUE // red
+				<< Hit << (Hit + wn * 10.0f);
+			#endif
+		}
+	}
+}
+
+// PT: PxcGenerateEEContacts2 uses a segment-triangle distance function, which breaks when the segment
+// intersects the triangle, in which case you need to switch to a penetration-depth computation.
+// If you don't do this thin capsules don't work.
+static void PxcGenerateEEContacts(	const Cm::Matrix34& meshAbsPose, ContactBuffer& contactBuffer, const Segment& segment, const PxReal radius,
+									const PxVec3* PX_RESTRICT triVerts, const PxVec3& normal, PxU32 triangleIndex)
+{
+	PxVec3 s0 = segment.p0;
+	PxVec3 s1 = segment.p1;
+	Ps::makeFatEdge(s0, s1, fatBoxEdgeCoeff);
+
+	for(PxU32 i=0;i<3;i++)
+	{
+		PxReal dist;
+		PxVec3 ip;
+		if(intersectEdgeEdge(triVerts[i], triVerts[Ps::getNextIndex3(i)], -normal, s0, s1, dist, ip))
+		{
+			ip = meshAbsPose.transform(ip);
+			const PxVec3 wn = meshAbsPose.rotate(normal);
+
+			contactBuffer.contact(ip, wn, - (radius + dist), triangleIndex);
+			#if DEBUG_RENDER_MESHCONTACTS
+			PxScene *s; PxGetPhysics().getScenes(&s, 1, 0);
+			Cm::RenderOutput((Cm::RenderBuffer&)s->getRenderBuffer()) << Cm::RenderOutput::LINES << PxDebugColor::eARGB_BLUE // red
+				<< ip << (ip + wn * 10.0f);
+			#endif
+		}
+	}
+}
+
+static void PxcGenerateEEContacts2(	const Cm::Matrix34& meshAbsPose, ContactBuffer& contactBuffer, const Segment& segment, const PxReal radius,
+									const PxVec3* PX_RESTRICT triVerts, const PxVec3& normal, PxU32 triangleIndex, PxReal contactDistance)
+{
+	PxVec3 s0 = segment.p0;
+	PxVec3 s1 = segment.p1;
+	Ps::makeFatEdge(s0, s1, fatBoxEdgeCoeff);
+
+	for(PxU32 i=0;i<3;i++)
+	{
+		PxReal dist;
+		PxVec3 ip;
+		if(intersectEdgeEdge(triVerts[i], triVerts[Ps::getNextIndex3(i)], normal, s0, s1, dist, ip) && dist < radius+contactDistance)
+		{
+			ip = meshAbsPose.transform(ip);
+			const PxVec3 wn = meshAbsPose.rotate(normal);
+
+			contactBuffer.contact(ip, wn, dist - radius, triangleIndex);
+			#if DEBUG_RENDER_MESHCONTACTS
+			PxScene *s; PxGetPhysics().getScenes(&s, 1, 0);
+			Cm::RenderOutput((Cm::RenderBuffer&)s->getRenderBuffer()) << Cm::RenderOutput::LINES << PxDebugColor::eARGB_BLUE // red
+				<< ip << (ip + wn * 10.0f);
+			#endif
+		}
+	}
+}
+
+namespace
+{
+struct CapsuleMeshContactGeneration
+{
+	ContactBuffer&		mContactBuffer;
+	const Cm::Matrix34	mMeshAbsPose;
+	const Segment&		mMeshCapsule;
+#ifdef USE_AABB_TRI_CULLING
+	Vec3p				mBC;
+	Vec3p				mBE;
+#endif
+	PxReal				mInflatedRadius;
+	PxReal				mContactDistance;
+	PxReal				mShapeCapsuleRadius;
+
+	CapsuleMeshContactGeneration(ContactBuffer& contactBuffer, const PxTransform& transform1, const Segment& meshCapsule, PxReal inflatedRadius, PxReal contactDistance, PxReal shapeCapsuleRadius) :
+		mContactBuffer		(contactBuffer),
+		mMeshAbsPose		(Cm::Matrix34(transform1)),
+		mMeshCapsule		(meshCapsule),
+		mInflatedRadius		(inflatedRadius),
+		mContactDistance	(contactDistance),
+		mShapeCapsuleRadius	(shapeCapsuleRadius)
+	{
+		PX_ASSERT(contactBuffer.count==0);
+#ifdef USE_AABB_TRI_CULLING
+		mBC = (meshCapsule.p0 + meshCapsule.p1)*0.5f;
+		const Vec3p be = (meshCapsule.p0 - meshCapsule.p1)*0.5f;
+		mBE.x = fabsf(be.x) + inflatedRadius;
+		mBE.y = fabsf(be.y) + inflatedRadius;
+		mBE.z = fabsf(be.z) + inflatedRadius;
+#endif
+	}
+
+	void processTriangle(PxU32 triangleIndex, const TrianglePadded& tri, PxU8 extraData/*, const PxU32* vertInds*/)
+	{
+#ifdef USE_AABB_TRI_CULLING
+	#if VISUALIZE_CULLING_BOX
+		{
+			Cm::RenderOutput& out = context.mRenderOutput;
+			PxTransform idt = PxTransform(PxIdentity);
+			out << idt;
+			out << 0xffffffff;
+			out << Cm::DebugBox(mBC, mBE, true);
+		}
+	#endif
+#endif
+		const PxVec3& p0 = tri.verts[0];
+		const PxVec3& p1 = tri.verts[1];
+		const PxVec3& p2 = tri.verts[2];
+
+#ifdef USE_AABB_TRI_CULLING
+		// PT: this one is safe because triangle class is padded
+		// PT: TODO: is this test really needed? Not done in midphase already?
+		if(!intersectTriangleBox_Unsafe(mBC, mBE, p0, p1, p2))
+			return;
+#endif
+
+#ifdef USE_CAPSULE_TRI_PROJ_CULLING
+		PxVec3 triCenter = (p0 + p1 + p2)*0.33333333f;
+		PxVec3 delta = mBC - triCenter;
+
+		PxReal depth;
+		if(!PxcTestAxis(delta, mMeshCapsule, mInflatedRadius, tri.verts, depth))
+			return;
+#endif
+
+#if VISUALIZE_TOUCHED_TRIS
+		gVisualizeTri(p0, p1, p2, context, PxDebugColor::eARGB_RED);
+#endif
+
+#ifdef USE_CAPSULE_TRI_SAT_CULLING
+		PxVec3 SepAxis;
+		if(!PxcCapsuleTriOverlap3(extraData, mMeshCapsule, mInflatedRadius, tri.verts, NULL, &SepAxis)) 
+			return;
+#endif
+
+		PxReal t,u,v;
+		const PxVec3 p1_p0 = p1 - p0;
+		const PxVec3 p2_p0 = p2 - p0;
+		const PxReal squareDist = distanceSegmentTriangleSquared(mMeshCapsule, p0, p1_p0, p2_p0, &t, &u, &v);
+
+		// PT: do cheaper test first!
+		if(squareDist >= mInflatedRadius*mInflatedRadius) 
+			return;
+		
+		// PT: backface culling without the normalize
+		// PT: TODO: consider doing before the segment-triangle distance test if it's cheaper
+		const PxVec3 planeNormal = p1_p0.cross(p2_p0);
+		const PxF32 planeD = planeNormal.dot(p0);	// PT: actually -d compared to PxcPlane
+		if(planeNormal.dot(mBC) < planeD)
+			return;
+
+		if(squareDist > 0.001f*0.001f)
+		{
+			// Contact information
+			PxVec3 normal;
+			if(selectNormal(extraData, u, v))
+			{
+				normal = planeNormal.getNormalized();
+			}
+			else
+			{
+				const PxVec3 pointOnTriangle = Ps::computeBarycentricPoint(p0, p1, p2, u, v);
+
+				const PxVec3 pointOnSegment = mMeshCapsule.getPointAt(t);
+				normal = pointOnSegment - pointOnTriangle;
+				const PxReal l = normal.magnitude();
+				if(l == 0.0f)
+					return;
+				normal = normal / l;
+			}
+
+			PxcGenerateEEContacts2(mMeshAbsPose, mContactBuffer, mMeshCapsule, mShapeCapsuleRadius, tri.verts, normal, triangleIndex, mContactDistance);
+			PxcGenerateVFContacts(mMeshAbsPose, mContactBuffer, mMeshCapsule, mShapeCapsuleRadius, tri.verts, normal, triangleIndex, mContactDistance);
+		}
+		else
+		{
+			PxVec3 SepAxis;
+			if(!PxcCapsuleTriOverlap3(extraData, mMeshCapsule, mInflatedRadius, tri.verts, NULL, &SepAxis)) 
+				return;
+
+			PxcGenerateEEContacts(mMeshAbsPose, mContactBuffer, mMeshCapsule, mShapeCapsuleRadius, tri.verts, SepAxis, triangleIndex);
+			PxcGenerateVFContacts(mMeshAbsPose, mContactBuffer, mMeshCapsule, mShapeCapsuleRadius, tri.verts, SepAxis, triangleIndex, mContactDistance);
+		}
+	}
+
+private:
+	CapsuleMeshContactGeneration& operator=(const CapsuleMeshContactGeneration&);
+};
+
+struct CapsuleMeshContactGenerationCallback_NoScale : MeshHitCallback<PxRaycastHit>
+{
+	CapsuleMeshContactGeneration		mGeneration;
+	const TriangleMesh*					mMeshData;
+
+	CapsuleMeshContactGenerationCallback_NoScale(
+		ContactBuffer& contactBuffer,
+		const PxTransform& transform1, const Segment& meshCapsule,
+		PxReal inflatedRadius, PxReal contactDistance,
+		PxReal shapeCapsuleRadius, const TriangleMesh* meshData
+	)	:
+		MeshHitCallback<PxRaycastHit>	(CallbackMode::eMULTIPLE),
+		mGeneration						(contactBuffer, transform1, meshCapsule, inflatedRadius, contactDistance, shapeCapsuleRadius),
+		mMeshData						(meshData)
+	{
+		PX_ASSERT(contactBuffer.count==0);
+	}
+
+	PX_FORCE_INLINE PxAgain processTriangle(const PxRaycastHit& hit, const TrianglePadded& tri)
+	{
+		const PxU32 triangleIndex = hit.faceIndex;
+
+		//ML::set all the edges to be active, if the mExtraTrigData exist, we overwrite this flag
+		const PxU8 extraData = getConvexEdgeFlags(mMeshData->getExtraTrigData(), triangleIndex);
+		mGeneration.processTriangle(triangleIndex, tri, extraData);
+		return true;
+	}
+
+	virtual PxAgain processHit(
+		const PxRaycastHit& hit, const PxVec3& v0, const PxVec3& v1, const PxVec3& v2, PxReal&, const PxU32* /*vInds*/)
+	{
+		TrianglePadded tri;
+		// PT: TODO: revisit this, avoid the copy
+		tri.verts[0] = v0;
+		tri.verts[1] = v1;
+		tri.verts[2] = v2;
+		return processTriangle(hit, tri);
+	}
+
+private:
+	CapsuleMeshContactGenerationCallback_NoScale& operator=(const CapsuleMeshContactGenerationCallback_NoScale&);
+};
+
+struct CapsuleMeshContactGenerationCallback_Scale : CapsuleMeshContactGenerationCallback_NoScale
+{
+	const Cm::FastVertex2ShapeScaling&	mScaling;
+
+	CapsuleMeshContactGenerationCallback_Scale(
+		ContactBuffer& contactBuffer,
+		const PxTransform& transform1, const Segment& meshCapsule,
+		PxReal inflatedRadius, const Cm::FastVertex2ShapeScaling& scaling, PxReal contactDistance,
+		PxReal shapeCapsuleRadius, const TriangleMesh* meshData
+	)	:
+		CapsuleMeshContactGenerationCallback_NoScale(contactBuffer, transform1, meshCapsule, inflatedRadius, contactDistance, shapeCapsuleRadius, meshData),
+		mScaling									(scaling)
+	{
+	}
+
+	virtual PxAgain processHit(
+		const PxRaycastHit& hit, const PxVec3& v0, const PxVec3& v1, const PxVec3& v2, PxReal&, const PxU32* /*vInds*/)
+	{
+		TrianglePadded tri;
+		getScaledVertices(tri.verts, v0, v1, v2, false, mScaling);
+		return processTriangle(hit, tri);
+	}
+
+private:
+	CapsuleMeshContactGenerationCallback_Scale& operator=(const CapsuleMeshContactGenerationCallback_Scale&);
+};
+
+}
+
+// PT: computes local capsule without going to world-space
+static PX_FORCE_INLINE Segment computeLocalCapsule(const PxTransform& transform0, const PxTransform& transform1, const PxCapsuleGeometry& shapeCapsule)
+{
+	const PxVec3 halfHeight = getCapsuleHalfHeightVector(transform0, shapeCapsule);
+	const PxVec3 delta = transform1.p - transform0.p;
+	return Segment(
+		transform1.rotateInv(halfHeight - delta),
+		transform1.rotateInv(-halfHeight - delta));
+}
+
+bool Gu::contactCapsuleMesh(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(cache);
+	PX_UNUSED(renderOutput);
+
+	const PxCapsuleGeometry& shapeCapsule = shape0.get<const PxCapsuleGeometry>();
+	const PxTriangleMeshGeometryLL& shapeMesh = shape1.get<const PxTriangleMeshGeometryLL>();
+
+	const PxReal inflatedRadius = shapeCapsule.radius + params.mContactDistance;		//AM: inflate!
+	const Segment meshCapsule = computeLocalCapsule(transform0, transform1, shapeCapsule);
+
+	const TriangleMesh* meshData = shapeMesh.meshData;
+
+	//bound the capsule in shape space by an OBB:
+	Box queryBox;
+	{
+		const Capsule queryCapsule(meshCapsule, inflatedRadius);
+		queryBox.create(queryCapsule);
+	}
+
+	if(shapeMesh.scale.isIdentity())
+	{
+		CapsuleMeshContactGenerationCallback_NoScale callback(contactBuffer, transform1, meshCapsule,
+			inflatedRadius, params.mContactDistance, shapeCapsule.radius, meshData);
+
+		// PT: TODO: switch to capsule query here
+		Midphase::intersectOBB(meshData, queryBox, callback, true);
+	}
+	else
+	{
+		const Cm::FastVertex2ShapeScaling meshScaling(shapeMesh.scale);
+
+		CapsuleMeshContactGenerationCallback_Scale callback(contactBuffer, transform1, meshCapsule,
+			inflatedRadius, meshScaling, params.mContactDistance, shapeCapsule.radius, meshData);
+
+		//switched from capsuleCollider to boxCollider so we can support nonuniformly scaled meshes by scaling the query region:
+
+		//apply the skew transform to the box:
+		meshScaling.transformQueryBounds(queryBox.center, queryBox.extents, queryBox.rot);
+
+		Midphase::intersectOBB(meshData, queryBox, callback, true);
+	}
+	return contactBuffer.count > 0;
+}
+
+namespace
+{
+struct CapsuleHeightfieldContactGenerationCallback : EntityReport<PxU32>
+{
+	CapsuleMeshContactGeneration	mGeneration;
+	HeightFieldUtil&				mHfUtil;
+	const PxTransform&				mTransform1;
+
+	CapsuleHeightfieldContactGenerationCallback(
+		ContactBuffer& contactBuffer,
+		const PxTransform& transform1, HeightFieldUtil& hfUtil, const Segment& meshCapsule,
+		PxReal inflatedRadius, PxReal contactDistance, PxReal shapeCapsuleRadius
+	) : 
+		mGeneration	(contactBuffer, transform1, meshCapsule, inflatedRadius, contactDistance, shapeCapsuleRadius),
+		mHfUtil		(hfUtil),
+		mTransform1	(transform1)
+	{
+		PX_ASSERT(contactBuffer.count==0);
+	}
+
+	// PT: TODO: refactor/unify with similar code in other places
+	virtual bool onEvent(PxU32 nb, PxU32* indices)
+	{
+		const PxU8 nextInd[] = {2,0,1};
+
+		while(nb--)
+		{
+			const PxU32 triangleIndex = *indices++;
+
+			PxU32 vertIndices[3];
+			TrianglePadded currentTriangle;	// in world space
+			PxU32 adjInds[3];
+			mHfUtil.getTriangle(mTransform1, currentTriangle, vertIndices, adjInds, triangleIndex, false, false);
+
+			PxVec3 normal;
+			currentTriangle.normal(normal);
+
+			PxU8 triFlags = 0; //KS - temporary until we can calculate triFlags for HF
+
+			for(PxU32 a = 0; a < 3; ++a)
+			{
+				if(adjInds[a] != 0xFFFFFFFF)
+				{
+					PxTriangle adjTri;
+					mHfUtil.getTriangle(mTransform1, adjTri, NULL, NULL, adjInds[a], false, false);
+					//We now compare the triangles to see if this edge is active
+
+					PxVec3 adjNormal;
+					adjTri.denormalizedNormal(adjNormal);
+					PxU32 otherIndex = nextInd[a];
+					PxF32 projD = adjNormal.dot(currentTriangle.verts[otherIndex] - adjTri.verts[0]);
+					if(projD < 0.f)
+					{
+						adjNormal.normalize();
+
+						PxF32 proj = adjNormal.dot(normal);
+
+						if(proj < 0.999f)
+						{
+							triFlags |= 1 << (a+3);
+						}
+					}
+				}
+				else
+				{
+					triFlags |= 1 << (a+3);
+				}
+			}
+
+			mGeneration.processTriangle(triangleIndex, currentTriangle, triFlags);
+		}
+		return true;
+	}
+
+private:
+	CapsuleHeightfieldContactGenerationCallback& operator=(const CapsuleHeightfieldContactGenerationCallback&);
+};
+}
+
+bool Gu::contactCapsuleHeightfield(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(cache);
+	PX_UNUSED(renderOutput);
+
+	const PxCapsuleGeometry& shapeCapsule = shape0.get<const PxCapsuleGeometry>();
+	const PxHeightFieldGeometryLL& shapeMesh = shape1.get<const PxHeightFieldGeometryLL>();
+
+	const PxReal inflatedRadius = shapeCapsule.radius + params.mContactDistance;		//AM: inflate!
+	const Segment meshCapsule = computeLocalCapsule(transform0, transform1, shapeCapsule);
+
+	// We must be in local space to use the cache
+
+	HeightFieldUtil hfUtil(shapeMesh);
+
+	CapsuleHeightfieldContactGenerationCallback callback(
+		contactBuffer, transform1, hfUtil, meshCapsule, inflatedRadius, params.mContactDistance, shapeCapsule.radius);
+
+	//switched from capsuleCollider to boxCollider so we can support nonuniformly scaled meshes by scaling the query region:
+
+	//bound the capsule in shape space by an OBB:
+
+	PxBounds3 bounds;
+	bounds.maximum = PxVec3(shapeCapsule.halfHeight + inflatedRadius, inflatedRadius, inflatedRadius);
+	bounds.minimum = -bounds.maximum;
+
+	bounds = PxBounds3::transformFast(transform1.transformInv(transform0), bounds);
+
+	hfUtil.overlapAABBTriangles(transform1, bounds, 0, &callback);
+
+	return contactBuffer.count > 0;
+}

physx/source/geomutils/src/contact/GuContactMethodImpl.h

@@ -0,0 +1,170 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_CONTACTMETHODIMPL_H
+#define GU_CONTACTMETHODIMPL_H
+
+#include "foundation/PxAssert.h"
+#include "common/PxPhysXCommonConfig.h"
+#include "collision/PxCollisionDefs.h"
+#include "CmPhysXCommon.h"
+
+namespace physx
+{
+namespace Cm
+{
+	class RenderOutput;
+}
+
+namespace Gu
+{
+	class GeometryUnion;
+	class ContactBuffer;
+	struct NarrowPhaseParams;
+	class PersistentContactManifold;
+	class MultiplePersistentContactManifold;
+
+	enum ManifoldFlags
+	{
+		IS_MANIFOLD			= (1<<0),
+		IS_MULTI_MANIFOLD	= (1<<1)
+	};
+
+	struct Cache : public PxCache
+	{
+		Cache()
+		{
+		}
+
+		PX_FORCE_INLINE void setManifold(void* manifold)
+		{
+			PX_ASSERT((size_t(manifold) & 0xF) == 0);
+			mCachedData = reinterpret_cast<PxU8*>(manifold);
+			mManifoldFlags |= IS_MANIFOLD;
+		}
+
+		PX_FORCE_INLINE void setMultiManifold(void* manifold)
+		{
+			PX_ASSERT((size_t(manifold) & 0xF) == 0);
+			mCachedData = reinterpret_cast<PxU8*>(manifold);
+			mManifoldFlags |= IS_MANIFOLD|IS_MULTI_MANIFOLD;
+		}
+
+		PX_FORCE_INLINE PxU8 isManifold()	const
+		{
+			return PxU8(mManifoldFlags & IS_MANIFOLD);
+		}
+
+		PX_FORCE_INLINE PxU8 isMultiManifold()	const
+		{
+			return PxU8(mManifoldFlags & IS_MULTI_MANIFOLD);
+		}
+
+		PX_FORCE_INLINE PersistentContactManifold& getManifold()
+		{
+			PX_ASSERT(isManifold());
+			PX_ASSERT(!isMultiManifold());
+			PX_ASSERT((uintptr_t(mCachedData) & 0xf) == 0);
+			return *reinterpret_cast<PersistentContactManifold*>(mCachedData);
+		}
+
+		PX_FORCE_INLINE MultiplePersistentContactManifold& getMultipleManifold()
+		{
+			PX_ASSERT(isManifold());
+			PX_ASSERT(isMultiManifold());
+			PX_ASSERT((uintptr_t(mCachedData) & 0xf) == 0);
+			return *reinterpret_cast<MultiplePersistentContactManifold*>(mCachedData);
+		}
+	};
+}
+
+#define GU_CONTACT_METHOD_ARGS				\
+	const Gu::GeometryUnion& shape0,		\
+	const Gu::GeometryUnion& shape1,		\
+	const PxTransform& transform0,			\
+	const PxTransform& transform1,			\
+	const Gu::NarrowPhaseParams& params,	\
+	Gu::Cache& cache,						\
+	Gu::ContactBuffer& contactBuffer,		\
+	Cm::RenderOutput* renderOutput
+
+namespace Gu
+{
+	PX_PHYSX_COMMON_API bool contactSphereSphere(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool contactSphereCapsule(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool contactSphereBox(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool contactCapsuleCapsule(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool contactCapsuleBox(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool contactCapsuleConvex(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool contactBoxBox(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool contactBoxConvex(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool contactConvexConvex(GU_CONTACT_METHOD_ARGS);
+	
+	PX_PHYSX_COMMON_API bool contactSphereMesh(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool contactCapsuleMesh(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool contactBoxMesh(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool contactConvexMesh(GU_CONTACT_METHOD_ARGS);
+
+	PX_PHYSX_COMMON_API bool contactSphereHeightfield(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool contactCapsuleHeightfield(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool contactBoxHeightfield(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool contactConvexHeightfield(GU_CONTACT_METHOD_ARGS);
+	
+	PX_PHYSX_COMMON_API bool contactSpherePlane(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool contactPlaneBox(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool contactPlaneCapsule(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool contactPlaneConvex(GU_CONTACT_METHOD_ARGS);
+
+	PX_PHYSX_COMMON_API bool pcmContactSphereMesh(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactCapsuleMesh(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactBoxMesh(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactConvexMesh(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactSphereHeightField(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactCapsuleHeightField(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactBoxHeightField(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactConvexHeightField(GU_CONTACT_METHOD_ARGS);
+
+	PX_PHYSX_COMMON_API bool pcmContactPlaneCapsule(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactPlaneBox(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactPlaneConvex(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactSphereSphere(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactSpherePlane(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactSphereCapsule(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactSphereBox(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactSphereConvex(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactCapsuleCapsule(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactCapsuleBox(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactCapsuleConvex(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactBoxBox(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactBoxConvex(GU_CONTACT_METHOD_ARGS);
+	PX_PHYSX_COMMON_API bool pcmContactConvexConvex(GU_CONTACT_METHOD_ARGS);
+}
+}
+
+#endif

physx/source/geomutils/src/contact/GuContactPlaneBox.cpp

@@ -0,0 +1,129 @@
+//
+// 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-2021 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 "foundation/PxUnionCast.h"
+#include "geomutils/GuContactBuffer.h"
+
+#include "GuContactMethodImpl.h"
+#include "GuGeometryUnion.h"
+
+#include "CmMatrix34.h"
+#include "PsUtilities.h"
+
+namespace physx
+{
+namespace Gu
+{
+bool contactPlaneBox(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(renderOutput);
+	PX_UNUSED(cache);
+	PX_UNUSED(shape0);
+
+	// Get actual shape data      
+	//const PxPlaneGeometry& shapePlane = shape.get<const PxPlaneGeometry>();
+	const PxBoxGeometry& shapeBox = shape1.get<const PxBoxGeometry>();
+	
+	const PxVec3 negPlaneNormal = -transform0.q.getBasisVector0();
+	
+	//Make sure we have a normalized plane
+	//PX_ASSERT(PxAbs(shape0.mNormal.magnitudeSquared() - 1.0f) < 0.000001f);
+
+	Cm::Matrix34 boxMatrix(transform1);
+	Cm::Matrix34 boxToPlane(transform0.transformInv(transform1));
+
+	PxVec3 point;
+
+	PX_ASSERT(contactBuffer.count==0);
+
+/*	for(int vx=-1; vx<=1; vx+=2)
+		for(int vy=-1; vy<=1; vy+=2)
+			for(int vz=-1; vz<=1; vz+=2)
+			{				
+				//point = boxToPlane.transform(PxVec3(shapeBox.halfExtents.x*vx, shapeBox.halfExtents.y*vy, shapeBox.halfExtents.z*vz));	
+				//PxReal planeEq = point.x;
+				//Optimized a bit
+				point.set(shapeBox.halfExtents.x*vx, shapeBox.halfExtents.y*vy, shapeBox.halfExtents.z*vz);
+				const PxReal planeEq = boxToPlane.m.column0.x*point.x + boxToPlane.m.column1.x*point.y + boxToPlane.m.column2.x*point.z + boxToPlane.p.x;
+
+				if(planeEq <= contactDistance)
+				{
+					contactBuffer.contact(boxMatrix.transform(point), negPlaneNormal, planeEq);
+					
+					//no point in making more than 4 contacts.
+					if (contactBuffer.count >= 6) //was: 4)	actually, with strong interpenetration more than just the bottom surface goes through,
+						//and we want to find the *deepest* 4 vertices, really.
+						return true;
+				}
+			}*/
+
+	// PT: the above code is shock full of LHS/FCMPs. And there's no point in limiting the number of contacts to 6 when the max possible is 8.
+
+	const PxReal limit = params.mContactDistance - boxToPlane.p.x;
+	const PxReal dx = shapeBox.halfExtents.x;
+	const PxReal dy = shapeBox.halfExtents.y;
+	const PxReal dz = shapeBox.halfExtents.z;
+	const PxReal bxdx = boxToPlane.m.column0.x * dx;
+	const PxReal bxdy = boxToPlane.m.column1.x * dy;
+	const PxReal bxdz = boxToPlane.m.column2.x * dz;
+
+	PxReal depths[8];
+	depths[0] =   bxdx + bxdy + bxdz - limit;
+	depths[1] =   bxdx + bxdy - bxdz - limit;
+	depths[2] =   bxdx - bxdy + bxdz - limit;
+	depths[3] =   bxdx - bxdy - bxdz - limit;
+	depths[4] = - bxdx + bxdy + bxdz - limit;
+	depths[5] = - bxdx + bxdy - bxdz - limit;
+	depths[6] = - bxdx - bxdy + bxdz - limit;
+	depths[7] = - bxdx - bxdy - bxdz - limit;
+
+	//const PxU32* binary = reinterpret_cast<const PxU32*>(depths);
+	const PxU32* binary = PxUnionCast<PxU32*, PxF32*>(depths);
+
+	if(binary[0] & PX_SIGN_BITMASK)
+		contactBuffer.contact(boxMatrix.transform(PxVec3(dx, dy, dz)), negPlaneNormal, depths[0] + params.mContactDistance);
+	if(binary[1] & PX_SIGN_BITMASK)
+		contactBuffer.contact(boxMatrix.transform(PxVec3(dx, dy, -dz)), negPlaneNormal, depths[1] + params.mContactDistance);
+	if(binary[2] & PX_SIGN_BITMASK)
+		contactBuffer.contact(boxMatrix.transform(PxVec3(dx, -dy, dz)), negPlaneNormal, depths[2] + params.mContactDistance);
+	if(binary[3] & PX_SIGN_BITMASK)
+		contactBuffer.contact(boxMatrix.transform(PxVec3(dx, -dy, -dz)), negPlaneNormal, depths[3] + params.mContactDistance);
+	if(binary[4] & PX_SIGN_BITMASK)
+		contactBuffer.contact(boxMatrix.transform(PxVec3(-dx, dy, dz)), negPlaneNormal, depths[4] + params.mContactDistance);
+	if(binary[5] & PX_SIGN_BITMASK)
+		contactBuffer.contact(boxMatrix.transform(PxVec3(-dx, dy, -dz)), negPlaneNormal, depths[5] + params.mContactDistance);
+	if(binary[6] & PX_SIGN_BITMASK)
+		contactBuffer.contact(boxMatrix.transform(PxVec3(-dx, -dy, dz)), negPlaneNormal, depths[6] + params.mContactDistance);
+	if(binary[7] & PX_SIGN_BITMASK)
+		contactBuffer.contact(boxMatrix.transform(PxVec3(-dx, -dy, -dz)), negPlaneNormal, depths[7] + params.mContactDistance);
+
+	return contactBuffer.count > 0;
+}
+}//Gu
+}//physx

physx/source/geomutils/src/contact/GuContactPlaneCapsule.cpp

@@ -0,0 +1,81 @@
+//
+// 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-2021 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 "geomutils/GuContactBuffer.h"
+
+#include "GuContactMethodImpl.h"
+#include "GuInternal.h"
+#include "GuSegment.h"
+#include "GuGeometryUnion.h"
+
+namespace physx
+{
+namespace Gu
+{
+bool contactPlaneCapsule(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(renderOutput);
+	PX_UNUSED(cache);
+	PX_UNUSED(shape0);
+
+	// Get actual shape data
+	//const PxPlaneGeometry& shapePlane = shape.get<const PxPlaneGeometry>();
+	const PxCapsuleGeometry& shapeCapsule = shape1.get<const PxCapsuleGeometry>();
+
+	const PxTransform capsuleToPlane = transform0.transformInv(transform1);
+
+	//Capsule in plane space
+	Segment segment;
+	getCapsuleSegment(capsuleToPlane, shapeCapsule, segment);
+	
+	const PxVec3 negPlaneNormal = transform0.q.getBasisVector0();
+	
+	bool contact = false;
+
+	const PxReal separation0 = segment.p0.x - shapeCapsule.radius;
+	const PxReal separation1 = segment.p1.x - shapeCapsule.radius;
+	if(separation0 <= params.mContactDistance)
+	{
+		const PxVec3 temp(segment.p0.x - shapeCapsule.radius, segment.p0.y, segment.p0.z);
+		const PxVec3 point = transform0.transform(temp);
+		contactBuffer.contact(point, -negPlaneNormal, separation0);
+		contact = true;
+	}
+
+	if(separation1 <= params.mContactDistance)
+	{
+		const PxVec3 temp(segment.p1.x - shapeCapsule.radius, segment.p1.y, segment.p1.z);
+		const PxVec3 point = transform0.transform(temp);
+		contactBuffer.contact(point, -negPlaneNormal, separation1);
+		contact = true;
+	}
+	return contact;
+}
+}//Gu
+}//physx

physx/source/geomutils/src/contact/GuContactPlaneConvex.cpp

@@ -0,0 +1,102 @@
+//
+// 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-2021 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 "geomutils/GuContactBuffer.h"
+
+#include "GuConvexMeshData.h"
+#include "GuContactMethodImpl.h"
+#include "GuGeometryUnion.h"
+#include "CmScaling.h"
+
+
+namespace physx
+{
+namespace Gu
+{
+bool contactPlaneConvex(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(renderOutput);
+	PX_UNUSED(cache);
+	PX_UNUSED(shape0);
+
+	// Get actual shape data
+	//const PxPlaneGeometry& shapePlane = shape.get<const PxPlaneGeometry>();
+	const PxConvexMeshGeometryLL& shapeConvex = shape1.get<const PxConvexMeshGeometryLL>();
+
+	const PxVec3* PX_RESTRICT hullVertices = shapeConvex.hullData->getHullVertices();
+	PxU32 numHullVertices = shapeConvex.hullData->mNbHullVertices;
+//	Ps::prefetch128(hullVertices);
+
+	// Plane is implicitly <1,0,0> 0 in localspace
+	Cm::Matrix34 convexToPlane (transform0.transformInv(transform1));
+	PxMat33 convexToPlane_rot(convexToPlane[0], convexToPlane[1], convexToPlane[2] );
+
+	bool idtScale = shapeConvex.scale.isIdentity();
+	Cm::FastVertex2ShapeScaling convexScaling;	// PT: TODO: remove default ctor
+	if(!idtScale)
+		convexScaling.init(shapeConvex.scale);
+
+	convexToPlane = Cm::Matrix34( convexToPlane_rot * convexScaling.getVertex2ShapeSkew(), convexToPlane[3] );
+
+	//convexToPlane = context.mVertex2ShapeSkew[1].getVertex2WorldSkew(convexToPlane);
+
+	const Cm::Matrix34 planeToW (transform0);
+
+	// This is rather brute-force
+	
+	bool status = false;
+
+	const PxVec3 contactNormal = -planeToW.m.column0;
+
+	while(numHullVertices--)
+	{
+		const PxVec3& vertex = *hullVertices++;
+//		if(numHullVertices)
+//			Ps::prefetch128(hullVertices);
+
+		const PxVec3 pointInPlane = convexToPlane.transform(vertex);		//TODO: this multiply could be factored out!
+		if(pointInPlane.x <= params.mContactDistance)
+		{
+//			const PxVec3 pointInW = planeToW.transform(pointInPlane);
+//			contactBuffer.contact(pointInW, -planeToW.m.column0, pointInPlane.x);
+			status = true;
+			Gu::ContactPoint* PX_RESTRICT pt = contactBuffer.contact();
+			if(pt)
+			{
+				pt->normal				= contactNormal;
+				pt->point				= planeToW.transform(pointInPlane);
+				pt->separation			= pointInPlane.x;
+				pt->internalFaceIndex1	= PXC_CONTACT_NO_FACE_INDEX;
+			}
+		}
+	}
+	return status;
+}
+}//Gu
+}//physx

physx/source/geomutils/src/contact/GuContactPolygonPolygon.cpp

@@ -0,0 +1,862 @@
+//
+// 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-2021 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 "foundation/PxMemory.h"
+#include "geomutils/GuContactBuffer.h"
+
+#include "GuContactPolygonPolygon.h"
+#include "GuShapeConvex.h"
+#include "PsMathUtils.h"
+#include "PsAllocator.h"
+#include "PsFPU.h"
+
+using namespace physx;
+using namespace Gu;
+
+#define CONTACT_REDUCTION
+
+/*
+void gVisualizeLocalLine(const PxVec3& a, const PxVec3& b, const Cm::Matrix34& m, PxsContactManager& manager)	//temp debug
+{
+	Cm::RenderOutput out = manager.getContext()->getRenderOutput();
+	out << 0xffffff << m << Cm::RenderOutput::LINES << a << b;
+}
+*/
+
+#ifdef CONTACT_REDUCTION
+static PX_FORCE_INLINE PxReal dot2D(const PxVec3& v0, const PxVec3& v1)
+{	
+	return v0.x * v1.x + v0.y * v1.y;
+}
+
+static void ContactReductionAllIn(	ContactBuffer& contactBuffer, PxU32 nbExistingContacts, PxU32 numIn,
+									const PxMat33& rotT,
+									const PxVec3* PX_RESTRICT vertices, const PxU8* PX_RESTRICT indices)
+{
+	// Number of contacts created by current call
+	const PxU32 nbNewContacts = contactBuffer.count - nbExistingContacts;
+
+	if(nbNewContacts<=4)
+		return;	// no reduction for less than 4 verts
+
+	// We have 3 different numbers here:
+	// - numVerts = number of vertices in the convex polygon we're dealing with
+	// - numIn = number of those that were "inside" the other convex polygon (should be <= numVerts)
+	// - contactBuffer.count = total number of contacts *from both polygons* (that's the catch here)
+	// The fast path can only be chosen when the contact buffer contains all the verts from current polygon,
+	// i.e. when contactBuffer.count == numIn == numVerts
+
+	Gu::ContactPoint* PX_RESTRICT ctcs = contactBuffer.contacts + nbExistingContacts;
+
+	if(numIn == nbNewContacts)
+	{
+		// Codepath 1: all vertices generated a contact
+
+		PxReal deepestSeparation = ctcs[0].separation;
+		PxU32 deepestIndex = 0;
+		for(PxU32 i=1; i<nbNewContacts; ++i)
+		{
+			if(deepestSeparation > ctcs[i].separation)
+			{
+				deepestSeparation = ctcs[i].separation;
+				deepestIndex = i;
+			}
+		}
+
+		PxU32 index = 0;
+		const PxU32 step = (numIn<<16)>>2;	// Fixed point math, don't use floats here please
+		bool needsExtraPoint = true;
+		for(PxU32 i=0;i<4;i++)
+		{
+			const PxU32 contactIndex = index>>16;
+			ctcs[i] = ctcs[contactIndex];
+			if(contactIndex==deepestIndex)
+				needsExtraPoint = false;
+			index += step;
+		}
+
+		if(needsExtraPoint)
+		{
+			ctcs[4] = ctcs[deepestIndex];
+			contactBuffer.count = nbExistingContacts + 5;
+		}
+		else
+		{
+			contactBuffer.count = nbExistingContacts + 4;
+		}
+
+/*	PT: TODO: investigate why this one does not work
+		PxU32 index = deepestIndex<<16;
+		const PxU32 step = (numIn<<16)>>2;	// Fixed point math, don't use floats here please
+		for(PxU32 i=0;i<4;i++)
+		{
+			PxU32 contactIndex = index>>16;
+			if(contactIndex>=numIn)
+				contactIndex -= numIn;
+			ctcs[i] = ctcs[contactIndex];
+			index += step;
+		}
+		contactBuffer.count = nbExistingContacts + 4;*/
+	}
+	else
+	{
+		// Codepath 2: all vertices are "in" but only some of them generated a contact
+
+		// WARNING: this path doesn't work when the buffer contains vertices from both polys.
+
+		// TODO: precompute those axes
+		const PxU32 nbAxes = 8;
+		PxVec3 dirs[nbAxes];
+		float angle = 0.0f;
+		const float angleStep = Ps::degToRad(180.0f/float(nbAxes));
+		for(PxU32 i=0;i<nbAxes;i++)
+		{
+			dirs[i] = PxVec3(cosf(angle), sinf(angle), 0.0f);
+			angle += angleStep;
+		}
+
+		float dpmin[nbAxes];
+		float dpmax[nbAxes];
+		for(PxU32 i=0;i<nbAxes;i++)
+		{
+			dpmin[i] = PX_MAX_F32;
+			dpmax[i] = -PX_MAX_F32;
+		}
+
+		for(PxU32 i=0;i<nbNewContacts;i++)
+		{
+			const PxVec3& p = vertices[indices[i]];
+
+			// Transform to 2D
+			const PxVec3 p2d = rotT.transform(p);
+
+			for(PxU32 j=0;j<nbAxes;j++)
+			{
+				const float dp = dot2D(dirs[j], p2d);
+				dpmin[j] = physx::intrinsics::selectMin(dpmin[j], dp);
+				dpmax[j] = physx::intrinsics::selectMax(dpmax[j], dp);
+			}
+		}
+
+		PxU32 bestAxis = 0;
+		float maxVariance = dpmax[0] - dpmin[0];
+		for(PxU32 i=1;i<nbAxes;i++)
+		{
+			const float variance = dpmax[i] - dpmin[i];
+			if(variance>maxVariance)
+			{
+				maxVariance = variance;
+				bestAxis = i;
+			}
+		}
+
+		const PxVec3 u = dirs[bestAxis];
+		const PxVec3 v = PxVec3(-u.y, u.x, 0.0f);
+		// PxVec3(1.0f, 0.0f, 0.0f)		=> PxVec3(0.0f, 1.0f, 0.0f)
+		// PxVec3(0.0f, 1.0f, 0.0f)		=> PxVec3(-1.0f, 0.0f, 0.0f)
+		// PxVec3(-1.0f, 1.0f, 0.0f)	=> PxVec3(-1.0f, -1.0f, 0.0f)
+		// PxVec3(1.0f, 1.0f, 0.0f)		=> PxVec3(-1.0f, 1.0f, 0.0f)
+
+		float dpminu = PX_MAX_F32;
+		float dpmaxu = -PX_MAX_F32;
+		float dpminv = PX_MAX_F32;
+		float dpmaxv = -PX_MAX_F32;
+		PxU32 indexMinU = 0;
+		PxU32 indexMaxU = 0;
+		PxU32 indexMinV = 0;
+		PxU32 indexMaxV = 0;
+
+		for(PxU32 i=0;i<nbNewContacts;i++)
+		{
+			const PxVec3& p = vertices[indices[i]];
+
+			// Transform to 2D
+			const PxVec3 p2d = rotT.transform(p);
+
+			const float dpu = dot2D(u, p2d);
+			const float dpv = dot2D(v, p2d);
+
+			if(dpu<dpminu)
+			{
+				dpminu=dpu;
+				indexMinU = i;
+			}
+			if(dpu>dpmaxu)
+			{
+				dpmaxu=dpu;
+				indexMaxU = i;
+			}
+
+			if(dpv<dpminv)
+			{
+				dpminv=dpv;
+				indexMinV = i;
+			}
+			if(dpv>dpmaxv)
+			{
+				dpmaxv=dpv;
+				indexMaxV = i;
+			}
+		}
+
+		if(indexMaxU == indexMinU)
+			indexMaxU = 0xffffffff;
+		if(indexMinV == indexMinU || indexMinV == indexMaxU)
+			indexMinV = 0xffffffff;
+		if(indexMaxV == indexMinU || indexMaxV == indexMaxU || indexMaxV == indexMinV)
+			indexMaxV = 0xffffffff;
+		
+		PxU32 newCount = 0;
+		for(PxU32 i=0;i<nbNewContacts;i++)
+		{
+			if(		i==indexMinU
+				||	i==indexMaxU
+				||	i==indexMinV
+				||	i==indexMaxV)
+			{
+				ctcs[newCount++] = ctcs[i];
+			}
+		}
+		contactBuffer.count = nbExistingContacts + newCount;
+	}
+}
+#endif
+
+// PT: please leave that function in the same translation unit as the calling code
+/*static*/ PxMat33 Gu::findRotationMatrixFromZ(const PxVec3& to)
+{
+	PxMat33 result;
+
+	const PxReal e = to.z;
+	const PxReal f = PxAbs(e);
+
+	if(f <= 0.9999f)
+	{
+		// PT: please keep the normal case first for PS3 branch prediction
+
+		// Normal case, to and from are not parallel or anti-parallel
+		const PxVec3 v = Ps::cross001(to);
+		const PxReal h = 1.0f/(1.0f + e); /* optimization by Gottfried Chen */
+		const PxReal hvx = h * v.x;
+		const PxReal hvz = h * v.z;
+		const PxReal hvxy = hvx * v.y;
+		const PxReal hvxz = hvx * v.z;
+		const PxReal hvyz = hvz * v.y;
+
+		result(0,0) = e + hvx*v.x;
+		result(0,1) = hvxy - v.z;
+		result(0,2) = hvxz + v.y;
+
+		result(1,0) = hvxy + v.z;
+		result(1,1) = e + h*v.y*v.y;
+		result(1,2) = hvyz - v.x;
+
+		result(2,0) = hvxz - v.y;
+		result(2,1) = hvyz + v.x;
+		result(2,2) = e + hvz*v.z;
+	}
+	else
+	{
+		//Vectors almost parallel
+		// PT: TODO: simplify code below
+		PxVec3 from(0.0f, 0.0f, 1.0f);
+		PxVec3 absFrom(0.0f, 0.0f, 1.0f);
+
+		if(absFrom.x < absFrom.y)
+		{
+			if(absFrom.x < absFrom.z)
+				absFrom = PxVec3(1.0f, 0.0f, 0.0f);
+			else
+				absFrom = PxVec3(0.0f, 0.0f, 1.0f);
+		}
+		else
+		{
+			if(absFrom.y < absFrom.z)
+				absFrom = PxVec3(0.0f, 1.0f, 0.0f);
+			else
+				absFrom = PxVec3(0.0f, 0.0f, 1.0f);
+		}
+
+		PxVec3 u, v;
+		u.x = absFrom.x - from.x; u.y = absFrom.y - from.y; u.z = absFrom.z - from.z;
+		v.x = absFrom.x - to.x; v.y = absFrom.y - to.y; v.z = absFrom.z - to.z;
+
+		const PxReal c1 = 2.0f / u.dot(u);
+		const PxReal c2 = 2.0f / v.dot(v);
+		const PxReal c3 = c1 * c2 * u.dot(v);
+
+		for(unsigned int i = 0; i < 3; i++)
+		{
+			for(unsigned int j = 0; j < 3; j++)
+			{
+				result(i,j) = - c1*u[i]*u[j] - c2*v[i]*v[j] + c3*v[i]*u[j];
+			}
+			result(i,i) += 1.0f;
+		}
+	}
+	return result;
+}
+
+// PT: using this specialized version avoids doing an explicit transpose, which reduces LHS
+PX_FORCE_INLINE Cm::Matrix34 transformTranspose(const PxMat33& a, const Cm::Matrix34& b)
+{
+	return Cm::Matrix34(a.transformTranspose(b.m.column0), a.transformTranspose(b.m.column1), a.transformTranspose(b.m.column2), a.transformTranspose(b.p));
+}
+
+// Helper function to transform x/y coordinate of point. 
+PX_FORCE_INLINE void transform2D(float& x, float& y, const PxVec3& src, const Cm::Matrix34& mat)
+{
+	x = src.x * mat.m.column0.x + src.y * mat.m.column1.x + src.z * mat.m.column2.x + mat.p.x;
+	y = src.x * mat.m.column0.y + src.y * mat.m.column1.y + src.z * mat.m.column2.y + mat.p.y;
+}
+
+// Helper function to transform x/y coordinate of point. Use transposed matrix
+PX_FORCE_INLINE void transform2DT(float& x, float& y, const PxVec3& src, const PxMat33& mat)
+{
+	x = mat.column0.dot(src);
+	y = mat.column1.dot(src);
+}
+
+// Helper function to transform z coordinate of point.
+PX_FORCE_INLINE PxReal transformZ(const PxVec3& src, const Cm::Matrix34& mat)
+{
+	return src.x * mat.m.column0.z + src.y * mat.m.column1.z + src.z * mat.m.column2.z + mat.p.z;
+}
+
+static void transformVertices(	float& minX, float& minY,
+								float& maxX, float& maxY,
+								float* PX_RESTRICT verts2D,
+								PxU32 nb, const PxVec3* PX_RESTRICT vertices, const PxU8* PX_RESTRICT indices, const PxMat33& RotT)
+{
+	// PT: using local variables is important to reduce LHS.
+	float lminX = FLT_MAX;
+	float lminY = FLT_MAX;
+	float lmaxX = -FLT_MAX;
+	float lmaxY = -FLT_MAX;
+
+	// PT: project points, compute min & max at the same time
+	for(PxU32 i=0; i<nb; i++)
+	{
+		float x,y;
+		transform2DT(x, y, vertices[indices[i]], RotT);
+		lminX = physx::intrinsics::selectMin(lminX, x);
+		lminY = physx::intrinsics::selectMin(lminY, y);
+		lmaxX = physx::intrinsics::selectMax(lmaxX, x);
+		lmaxY = physx::intrinsics::selectMax(lmaxY, y);
+		verts2D[i*2+0] = x;
+		verts2D[i*2+1] = y;
+	}
+
+	// DE702
+		// Compute center of polygon
+		const float cx = (lminX + lmaxX)*0.5f;
+		const float cy = (lminY + lmaxY)*0.5f;
+		// We'll scale the polygon by epsilon
+		const float epsilon = 1.e-6f;
+		// Adjust bounds to take care of scaling
+		lminX -= epsilon;
+		lminY -= epsilon;
+		lmaxX += epsilon;
+		lmaxY += epsilon;
+	//~DE702
+
+	// PT: relocate polygon to positive quadrant
+	for(PxU32 i=0; i<nb; i++)
+	{
+		const float x = verts2D[i*2+0];
+		const float y = verts2D[i*2+1];
+
+		// PT: original code suffering from DE702 (relocation)
+//		verts2D[i*2+0] = x - lminX;
+//		verts2D[i*2+1] = y - lminY;
+
+		// PT: theoretically proper DE702 fix (relocation + scaling)
+		const float dx = x - cx;
+		const float dy = y - cy;
+//		const float coeff = epsilon * physx::intrinsics::recipSqrt(dx*dx+dy*dy);
+//		verts2D[i*2+0] = x - lminX + dx * coeff;
+//		verts2D[i*2+1] = y - lminY + dy * coeff;
+
+		// PT: approximate but faster DE702 fix. We multiply by epsilon so this is good enough.
+		verts2D[i*2+0] = x - lminX + physx::intrinsics::fsel(dx, epsilon, -epsilon);
+		verts2D[i*2+1] = y - lminY + physx::intrinsics::fsel(dy, epsilon, -epsilon);
+	}
+	lmaxX -= lminX;
+	lmaxY -= lminY;
+
+	minX = lminX;
+	minY = lminY;
+	maxX = lmaxX;
+	maxY = lmaxY;
+}
+
+//! Dedicated triangle version
+PX_FORCE_INLINE bool pointInTriangle2D(	float px, float pz,
+										float p0x, float p0z,
+										float e10x, float e10z,
+										float e20x, float e20z)
+{ 
+	const float a = e10x*e10x + e10z*e10z;
+	const float b = e10x*e20x + e10z*e20z;
+	const float c = e20x*e20x + e20z*e20z;
+	const float ac_bb = (a*c)-(b*b);
+
+	const float vpx = px - p0x;
+	const float vpz = pz - p0z;
+
+	const float d = vpx*e10x + vpz*e10z;
+	const float e = vpx*e20x + vpz*e20z;
+
+	const float x = (d*c) - (e*b);
+	const float y = (e*a) - (d*b);
+	const float z = x + y - ac_bb;
+
+	// Same as: if(x>0.0f && y>0.0f && z<0.0f)	return TRUE;
+	//			else							return FALSE;
+//		return (( IR(z) & ~(IR(x)|IR(y)) ) & SIGN_BITMASK) != 0;
+	if(x>0.0f && y>0.0f && z<0.0f)	return true;
+	else							return false;
+}
+
+
+	enum OutCode
+	{
+		OUT_XP	= (1<<0),
+		OUT_XN	= (1<<1),
+		OUT_YP	= (1<<2),
+		OUT_YN	= (1<<3)
+	};
+
+static
+//PX_FORCE_INLINE
+bool PointInConvexPolygon2D_OutCodes(const float* PX_RESTRICT pgon2D, PxU32 numVerts, const PxReal tx, const PxReal ty, const PxReal maxX, const PxReal maxY, PxU8& outCodes)
+{
+	PxU32 out = 0;
+	if(tx<0.0f)	out |= OUT_XN;
+	if(ty<0.0f)	out |= OUT_YN;
+	if(tx>maxX)	out |= OUT_XP;
+	if(ty>maxY)	out |= OUT_YP;
+	outCodes = PxU8(out);
+	if(out)
+		return false;
+
+	if(numVerts==3)
+		return pointInTriangle2D(	tx, ty,
+									pgon2D[0], pgon2D[1],
+									pgon2D[2] - pgon2D[0],
+									pgon2D[3] - pgon2D[1],
+									pgon2D[4] - pgon2D[0],
+									pgon2D[5] - pgon2D[1]);
+
+#define X 0
+#define Y 1
+
+	const PxReal* PX_RESTRICT vtx0_ = pgon2D + (numVerts-1)*2;
+	const PxReal* PX_RESTRICT vtx1_ = pgon2D;
+
+	const int* PX_RESTRICT ivtx0 = reinterpret_cast<const int*>(vtx0_);
+	const int* PX_RESTRICT ivtx1 = reinterpret_cast<const int*>(vtx1_);
+	//const int itx = (int&)tx;
+	//const int ity = (int&)ty;
+//	const int ity = PX_SIR(ty);
+		const int* tmp = reinterpret_cast<const int*>(&ty);
+		const int ity = *tmp;
+
+	// get test bit for above/below X axis
+	int yflag0 = ivtx0[Y] >= ity;
+
+	int InsideFlag = 0;
+
+	while(numVerts--)
+	{
+		const int yflag1 = ivtx1[Y] >= ity;
+		if(yflag0 != yflag1)
+		{
+			const PxReal* PX_RESTRICT vtx0 = reinterpret_cast<const PxReal*>(ivtx0);
+			const PxReal* PX_RESTRICT vtx1 = reinterpret_cast<const PxReal*>(ivtx1);
+			if( ((vtx1[Y]-ty) * (vtx0[X]-vtx1[X]) > (vtx1[X]-tx) * (vtx0[Y]-vtx1[Y])) == yflag1 )
+			{
+				if(InsideFlag == 1) return false;
+
+				InsideFlag++;
+			}
+		}
+		yflag0 = yflag1;
+		ivtx0 = ivtx1;
+		ivtx1 += 2;
+	}
+#undef X
+#undef Y
+
+	return InsideFlag & 1;
+}
+
+// Helper function to detect contact between two edges
+PX_FORCE_INLINE bool EdgeEdgeContactSpecial(const PxVec3& v1, const PxPlane& plane, 
+	const PxVec3& p1, const PxVec3& p2, const PxVec3& dir, const PxVec3& p3, const PxVec3& p4,
+	PxReal& dist, PxVec3& ip, unsigned int i, unsigned int j, float coeff)
+{
+	const PxReal d3 = plane.distance(p3);
+	PxReal temp = d3 * plane.distance(p4);
+	if(temp > 0.0f)
+		return false;
+
+	// if colliding edge (p3,p4) and plane are parallel return no collision
+	const PxVec3 v2 = (p4-p3);
+	temp = plane.n.dot(v2);
+	if(temp == 0.0f) // ### epsilon would be better
+		return false;
+
+	// compute intersection point of plane and colliding edge (p3,p4)
+	ip = p3-v2*(d3/temp);
+
+	// compute distance of intersection from line (ip, -dir) to line (p1,p2)
+	dist =	(v1[i]*(ip[j]-p1[j])-v1[j]*(ip[i]-p1[i])) * coeff;
+	if(dist < 0.0f)
+		return false;
+
+	// compute intersection point on edge (p1,p2) line
+	ip -= dist*dir;
+
+	// check if intersection point (ip) is between edge (p1,p2) vertices
+	temp = (p1.x-ip.x)*(p2.x-ip.x)+(p1.y-ip.y)*(p2.y-ip.y)+(p1.z-ip.z)*(p2.z-ip.z);
+	if(temp<0.0f)	
+		return true;	// collision found
+
+	return false; //no collision
+}
+
+//This one can also handle 2 vertex 'polygons' (useful for capsule surface segments) and can shift the results before contact generation.
+bool Gu::contactPolygonPolygonExt(	PxU32 numVerts0, const PxVec3* vertices0, const PxU8* indices0,	//polygon 0
+									const Cm::Matrix34& world0, const PxPlane& localPlane0,	//xform of polygon 0, plane of polygon
+									const PxMat33& rotT0,
+									//
+									PxU32 numVerts1, const PxVec3* PX_RESTRICT vertices1, const PxU8* PX_RESTRICT indices1,	//polygon 1
+									const Cm::Matrix34& world1, const PxPlane& localPlane1,	//xform of polygon 1, plane of polygon
+									const PxMat33& rotT1,
+								//
+									const PxVec3& worldSepAxis,	//world normal of separating plane - this is the world space normal of polygon0!!
+									const Cm::Matrix34& transform0to1, const Cm::Matrix34& transform1to0,	//transforms between polygons
+									PxU32 /*polyIndex0*/, PxU32 polyIndex1,	//feature indices for contact callback
+									ContactBuffer& contactBuffer,
+									bool flipNormal, const PxVec3& posShift, PxReal sepShift)	// shape order, result shift
+{
+	const PxVec3 n = flipNormal ? -worldSepAxis : worldSepAxis;
+
+	PX_ASSERT(indices0 != NULL && indices1 != NULL);
+
+	// - optimize "from to" computation
+	// - do the raycast case && EE tests in same space as 2D case...
+	// - project all edges at the same time ?
+	PxU32 NumIn = 0;
+	bool status = false;
+
+	void* PX_RESTRICT stackMemory;
+	{
+		const PxU32 maxNumVert = PxMax(numVerts0, numVerts1);
+		stackMemory = PxAlloca(maxNumVert * sizeof(PxVec3));
+	}
+
+	const PxU32 size0 = numVerts0 * sizeof(bool);
+	bool* PX_RESTRICT flags0 = reinterpret_cast<bool*>(PxAlloca(size0));
+	PxU8* PX_RESTRICT outCodes0 = reinterpret_cast<PxU8*>(PxAlloca(size0));
+//	Ps::memZero(flags0, size0);
+//	Ps::memZero(outCodes0, size0);
+
+	const PxU32 size1 = numVerts1 * sizeof(bool);
+	bool* PX_RESTRICT flags1 = reinterpret_cast<bool*>(PxAlloca(size1));
+	PxU8* PX_RESTRICT outCodes1 = reinterpret_cast<PxU8*>(PxAlloca(size1));
+//	Ps::memZero(flags1, size1);
+//	Ps::memZero(outCodes1, size1);
+
+#ifdef CONTACT_REDUCTION
+	// We want to do contact reduction on newly created contacts, not on all the already existing ones...
+	PxU32 nbExistingContacts = contactBuffer.count;
+	PxU32 nbCurrentContacts=0;
+	PxU8 indices[ContactBuffer::MAX_CONTACTS];
+#endif
+
+	{
+		//polygon 1
+		float* PX_RESTRICT verts2D = NULL;
+		float minX=0, minY=0;
+		float maxX=0, maxY=0;
+
+		const PxVec3 localDir = -world1.rotateTranspose(worldSepAxis);		//contactNormal in hull1 space
+																		//that's redundant, its equal to -localPlane1.d
+		const Cm::Matrix34 t0to2D = transformTranspose(rotT1, transform0to1);	//transform from hull0 to RotT
+
+		PxReal dn = localDir.dot(localPlane1.n);					//if the contactNormal == +-(normal of poly0) is NOT orthogonal to poly1 ...this is just to protect the division below.
+
+		// PT: TODO: if "numVerts1>2" we may skip more
+		if (numVerts1 > 2											//no need to test whether we're 'inside' ignore capsule segments and points
+//		if(!(-1E-7 < dn && dn < 1E-7))
+		&& dn >= 1E-7f)	// PT: it should never be negative so this unique test is enough
+		{
+			dn = 1.0f / dn;
+			const float ld1 = -localPlane1.d;						// PT: unavoidable "int-to-float" LHS here, so we only want to read it once!
+
+			// Lazy-transform vertices
+			if(!verts2D)
+			{
+				verts2D = reinterpret_cast<float*>(stackMemory);
+				//Project points
+				transformVertices(
+					minX, minY,
+					maxX, maxY,
+					verts2D, numVerts1, vertices1, indices1, rotT1);
+			}
+
+			for(PxU32 i=0; i < numVerts0; i++)							//for all vertices of poly0
+			{
+				const PxVec3& p = vertices0[indices0[i]];
+				const float p0_z = transformZ(p, t0to2D);							//transform ith vertex of poly0 to RotT
+
+				const PxVec3 pIn1 = transform0to1.transform(p);		//transform vertex to hull1 space, in which we have the poly1 vertices.
+
+				const PxReal dd = (p0_z - ld1) * dn;			//(p0_z + localPlane1.d) is the depth of the vertex behind the triangle measured along the triangle's normal.
+																	//we convert this to being measured along the 'contact normal' using the division.
+
+//				if(dd < 0.0f)										//if the penetrating vertex will have a penetration along the contact normal: 
+//				PX_ASSERT(dd <= 0.0f);		// PT: dn is always positive, so dd is always negative
+				{
+					float px, py;
+					transform2DT(px, py, pIn1 - dd*localDir, rotT1);	//project vertex into poly1 plane along CONTACT NORMAL - not the polygon's normal. 
+
+					const bool res = PointInConvexPolygon2D_OutCodes(verts2D, numVerts1, px-minX, py-minY, maxX, maxY, outCodes0[i]);
+					flags0[i] = res;
+					if(res)
+					{
+						NumIn++;
+
+						if(p0_z < ld1)
+						{
+							status = true;	// PT: keep this first to avoid an LHS when leaving the function
+
+							Gu::ContactPoint* PX_RESTRICT ctc = contactBuffer.contact();
+							if(ctc)
+							{
+#ifdef CONTACT_REDUCTION
+								indices[nbCurrentContacts++] = indices0[i];
+#endif
+								ctc->normal				= n;
+								ctc->point				= world0.transform(p) + (flipNormal ? posShift : PxVec3(0.0f));
+								ctc->separation			= dd + sepShift;
+								ctc->internalFaceIndex1	= polyIndex1;
+							}
+						}
+					}
+				}
+			}
+		}
+		else
+		{
+			PxMemZero(flags0, size0);
+			PxMemZero(outCodes0, size0);
+		}
+
+		if(NumIn == numVerts0)
+		{
+			//All vertices0 are inside polygon 1
+#ifdef CONTACT_REDUCTION
+			ContactReductionAllIn(contactBuffer, nbExistingContacts, NumIn, rotT0, vertices0, indices);
+#endif
+			return status;
+		}
+
+#ifdef CONTACT_REDUCTION
+		ContactReductionAllIn(contactBuffer, nbExistingContacts, NumIn, rotT0, vertices0, indices);
+#endif
+
+#ifdef CONTACT_REDUCTION
+		nbExistingContacts = contactBuffer.count;
+		nbCurrentContacts = 0;
+#endif
+		NumIn = 0;
+		verts2D = NULL;
+
+		//Polygon 0
+		const Cm::Matrix34 t1to2D = transformTranspose(rotT0, transform1to0);
+
+		if (numVerts0 > 2)											//no need to test whether we're 'inside' ignore capsule segments and points
+		{
+		const float ld0 = -localPlane0.d;						// PT: unavoidable "int-to-float" LHS here, so we only want to read it once!
+
+		// Lazy-transform vertices
+		if(!verts2D)
+		{
+			verts2D = reinterpret_cast<float*>(stackMemory);
+			//Project vertices
+			transformVertices(
+				minX, minY,
+				maxX, maxY,
+				verts2D, numVerts0, vertices0, indices0, rotT0);
+		}
+
+		for(PxU32 i=0; i < numVerts1; i++)
+		{
+			const PxVec3& p = vertices1[indices1[i]];
+
+			float px, py;
+			transform2D(px, py, p, t1to2D);
+
+			const bool res = PointInConvexPolygon2D_OutCodes(verts2D, numVerts0, px-minX, py-minY, maxX, maxY, outCodes1[i]);
+			flags1[i] = res;
+			if(res)
+			{
+				NumIn++;
+
+				const float pz = transformZ(p, t1to2D);
+				if(pz < ld0)
+				{
+					status = true;	// PT: keep this first to avoid an LHS when leaving the function
+
+					// PT: in theory, with this contact point we should use "worldSepAxis" as a contact normal.
+					// However we want to output the same normal for all contact points not to break friction
+					// patches!!! In theory again, it should be exactly the same since the contact point at
+					// time of impact is supposed to be the same on both bodies. In practice however, and with
+					// a depth-based engine, this is not the case. So the contact point here is not exactly
+					// right, but preserving the friction patch seems more important.
+
+					Gu::ContactPoint* PX_RESTRICT ctc = contactBuffer.contact();
+					if(ctc)
+					{
+#ifdef CONTACT_REDUCTION
+						indices[nbCurrentContacts++] = indices1[i];
+#endif
+						ctc->normal				= n;
+						ctc->point				= world1.transform(p) + (flipNormal ? PxVec3(0.0f) : posShift);
+						ctc->separation			= (pz - ld0) + sepShift;
+						ctc->internalFaceIndex1	= polyIndex1;
+					}
+				}
+			}
+		}
+
+		if(NumIn == numVerts1)
+		{
+			//all vertices 1 are inside polygon 0
+#ifdef CONTACT_REDUCTION
+			ContactReductionAllIn(contactBuffer, nbExistingContacts, NumIn, rotT1, vertices1, indices);
+#endif
+			return status;
+		}
+#ifdef CONTACT_REDUCTION
+		ContactReductionAllIn(contactBuffer, nbExistingContacts, NumIn, rotT1, vertices1, indices);
+#endif
+		}
+		else
+		{
+			PxMemZero(flags1, size1);
+			PxMemZero(outCodes1, size1);
+		}
+	}
+
+	//Edge/edge case
+	//Calculation done in space 0
+	PxVec3* PX_RESTRICT verts1in0 = reinterpret_cast<PxVec3*>(stackMemory);
+	for(PxU32 i=0; i<numVerts1; i++)
+	{
+		verts1in0[i] = transform1to0.transform(vertices1[indices1[i]]);
+	}
+
+	if (numVerts0 >= 2 && numVerts1 >= 2)//useless if one of them is degenerate.
+	for(PxU32 j=0; j<numVerts1; j++)
+	{
+		PxU32 j1 = j+1;
+		if(j1 >= numVerts1) j1 = 0;
+
+//		if(!(flags1[j] ^ flags1[j1]))
+//			continue;
+		if(flags1[j] && flags1[j1])
+			continue;
+		if(outCodes1[j]&outCodes1[j1])
+			continue;
+
+		const PxVec3& p0 = verts1in0[j];
+		const PxVec3& p1 = verts1in0[j1];
+
+//		gVisualizeLocalLine(vertices1[indices1[j]], vertices1[indices1[j1]], world1, callback.getManager());
+
+		const PxVec3 v1 = p1-p0;
+		const PxVec3 planeNormal = v1.cross(localPlane0.n);
+		const PxPlane plane(planeNormal, -(planeNormal.dot(p0)));
+
+		// find largest 2D plane projection
+		PxU32 _i, _j;
+		Ps::closestAxis(planeNormal, _i, _j);
+
+		const PxReal coeff = 1.0f / (v1[_i]*localPlane0.n[_j]-v1[_j]*localPlane0.n[_i]);
+
+		for(PxU32 i=0; i<numVerts0; i++)
+		{
+			PxU32 i1 = i+1;
+			if(i1 >= numVerts0) i1 = 0;
+
+//			if(!(flags0[i] ^ flags0[i1]))
+//				continue;
+			if(flags0[i] && flags0[i1])
+				continue;
+			if(outCodes0[i]&outCodes0[i1])
+				continue;
+
+			const PxVec3& p0b = vertices0[indices0[i]];
+			const PxVec3& p1b = vertices0[indices0[i1]];
+
+//			gVisualizeLocalLine(p0b, p1b, world0, callback.getManager());
+
+			PxReal dist;
+			PxVec3 p;
+
+			if(EdgeEdgeContactSpecial(v1, plane, p0, p1, localPlane0.n, p0b, p1b, dist, p, _i, _j, coeff))
+			{
+				status = true;	// PT: keep this first to avoid an LHS when leaving the function
+/*				p = world0.transform(p);
+
+				//contacts are generated on the edges of polygon 1
+				//we only have to shift the position of polygon 1 if flipNormal is false, because
+				//in this case convex 0 gets passed as polygon 1, and it is convex 0 that was shifted.
+				if (!flipNormal)
+					p += posShift;
+
+				contactBuffer.contact(p, n, -dist + sepShift, polyIndex0, polyIndex1, convexID);*/
+
+				Gu::ContactPoint* PX_RESTRICT ctc = contactBuffer.contact();
+				if(ctc)
+				{
+					ctc->normal				= n;
+					ctc->point				= world0.transform(p) + (flipNormal ? PxVec3(0.0f) : posShift);
+					ctc->separation			= -dist + sepShift;
+					ctc->internalFaceIndex1	= polyIndex1;
+				}
+			}
+		}
+	}
+	return status;
+}

physx/source/geomutils/src/contact/GuContactPolygonPolygon.h

@@ -0,0 +1,69 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_CONTACTPOLYGONPOLYGON_H
+#define GU_CONTACTPOLYGONPOLYGON_H
+
+#include "foundation/PxVec3.h"
+#include "common/PxPhysXCommonConfig.h"
+#include "CmPhysXCommon.h"
+
+namespace physx
+{
+
+namespace Cm
+{
+	class Matrix34;
+	class FastVertex2ShapeScaling;
+}
+
+namespace Gu
+{
+class ContactBuffer;
+
+PX_PHYSX_COMMON_API PxMat33 findRotationMatrixFromZ(const PxVec3& to);
+
+PX_PHYSX_COMMON_API bool contactPolygonPolygonExt(	PxU32 numVerts0, const PxVec3* vertices0, const PxU8* indices0,//polygon 0
+													const Cm::Matrix34& world0, const PxPlane& localPlane0,			//xform of polygon 0, plane of polygon
+													const PxMat33& RotT0,
+
+													PxU32 numVerts1, const PxVec3* vertices1, const PxU8* indices1,//polygon 1
+													const Cm::Matrix34& world1, const PxPlane& localPlane1,			//xform of polygon 1, plane of polygon
+													const PxMat33& RotT1,
+
+													const PxVec3& worldSepAxis,									//world normal of separating plane - this is the world space normal of polygon0!!
+													const Cm::Matrix34& transform0to1, const Cm::Matrix34& transform1to0,//transforms between polygons
+													PxU32 polyIndex0, PxU32 polyIndex1,	//face indices for contact callback,
+													ContactBuffer& contactBuffer,
+													bool flipNormal, const PxVec3& posShift, float sepShift
+													);	// shape order, post gen shift.
+}
+}
+
+#endif

physx/source/geomutils/src/contact/GuContactSphereBox.cpp

@@ -0,0 +1,181 @@
+//
+// 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-2021 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 "geomutils/GuContactBuffer.h"
+#include "GuContactMethodImpl.h"
+#include "GuGeometryUnion.h"
+
+using namespace physx;
+
+//This version is ported 1:1 from novodex
+static PX_FORCE_INLINE bool ContactSphereBox(const PxVec3& sphereOrigin, 
+							 PxReal sphereRadius,
+							 const PxVec3& boxExtents,
+//							 const PxcCachedTransforms& boxCacheTransform, 
+							 const PxTransform& boxTransform, 
+							 PxVec3& point, 
+							 PxVec3& normal, 
+							 PxReal& separation, 
+							 PxReal contactDistance)
+{
+//	const PxTransform& boxTransform = boxCacheTransform.getShapeToWorld();
+
+	//returns true on contact
+	const PxVec3 delta = sphereOrigin - boxTransform.p; // s1.center - s2.center;
+	PxVec3 dRot = boxTransform.rotateInv(delta); //transform delta into OBB body coords.
+
+	//check if delta is outside ABB - and clip the vector to the ABB.
+	bool outside = false;
+
+	if (dRot.x < -boxExtents.x)
+	{ 
+		outside = true; 
+		dRot.x = -boxExtents.x;
+	}
+	else if (dRot.x >  boxExtents.x)
+	{ 
+		outside = true; 
+		dRot.x = boxExtents.x;
+	}
+
+	if (dRot.y < -boxExtents.y)
+	{ 
+		outside = true; 
+		dRot.y = -boxExtents.y;
+	}
+	else if (dRot.y >  boxExtents.y)
+	{ 
+		outside = true; 
+		dRot.y = boxExtents.y;
+	}
+
+	if (dRot.z < -boxExtents.z)
+	{ 
+		outside = true; 
+		dRot.z =-boxExtents.z;
+	}
+	else if (dRot.z >  boxExtents.z)
+	{ 
+		outside = true; 
+		dRot.z = boxExtents.z;
+	}
+
+	if (outside) //if clipping was done, sphere center is outside of box.
+	{
+		point = boxTransform.rotate(dRot); //get clipped delta back in world coords.
+		normal = delta - point; //what we clipped away.	
+		const PxReal lenSquared = normal.magnitudeSquared();
+		const PxReal inflatedDist = sphereRadius + contactDistance;
+		if (lenSquared > inflatedDist * inflatedDist) 
+			return false;	//disjoint
+
+		//normalize to make it into the normal:
+		separation = PxRecipSqrt(lenSquared);
+		normal *= separation;	
+		separation *= lenSquared;
+		//any plane that touches the sphere is tangential, so a vector from contact point to sphere center defines normal.
+		//we could also use point here, which has same direction.
+		//this is either a faceFace or a vertexFace contact depending on whether the box's face or vertex collides, but we did not distinguish. 
+		//We'll just use vertex face for now, this info isn't really being used anyway.
+		//contact point is point on surface of cube closest to sphere center.
+		point += boxTransform.p;
+		separation -= sphereRadius;
+		return true;
+	}
+	else
+	{
+		//center is in box, we definitely have a contact.
+		PxVec3 locNorm;	//local coords contact normal
+
+		/*const*/ PxVec3 absdRot;
+		absdRot = PxVec3(PxAbs(dRot.x), PxAbs(dRot.y), PxAbs(dRot.z));
+		/*const*/ PxVec3 distToSurface = boxExtents - absdRot;	//dist from embedded center to box surface along 3 dimensions.
+
+		//find smallest element of distToSurface
+		if (distToSurface.y < distToSurface.x)
+		{
+			if (distToSurface.y < distToSurface.z)
+			{
+				//y
+				locNorm = PxVec3(0.0f, dRot.y > 0.0f ? 1.0f : -1.0f, 0.0f);
+				separation = -distToSurface.y;
+			}
+			else
+			{
+				//z
+				locNorm = PxVec3(0.0f,0.0f, dRot.z > 0.0f ? 1.0f : -1.0f);
+				separation = -distToSurface.z;
+			}
+		}
+		else
+		{
+			if (distToSurface.x < distToSurface.z)
+			{
+				//x
+				locNorm = PxVec3(dRot.x > 0.0f ? 1.0f : -1.0f, 0.0f, 0.0f);
+				separation = -distToSurface.x;
+			}
+			else
+			{
+				//z
+				locNorm = PxVec3(0.0f,0.0f, dRot.z > 0.0f ? 1.0f : -1.0f);
+				separation = -distToSurface.z;
+			}
+		}
+		//separation so far is just the embedding of the center point; we still have to push out all of the radius.
+		point = sphereOrigin;
+		normal = boxTransform.rotate(locNorm);
+		separation -= sphereRadius;
+		return true;
+	}
+}
+
+namespace physx
+{
+namespace Gu
+{
+bool contactSphereBox(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(renderOutput);
+	PX_UNUSED(cache);
+
+	const PxSphereGeometry& sphereGeom = shape0.get<const PxSphereGeometry>();
+	const PxBoxGeometry& boxGeom = shape1.get<const PxBoxGeometry>();
+
+	PxVec3 normal;
+	PxVec3 point;
+	PxReal separation;
+	if(!ContactSphereBox(transform0.p, sphereGeom.radius, boxGeom.halfExtents, transform1, point, normal, separation, params.mContactDistance))
+		return false;
+
+	contactBuffer.contact(point, normal, separation);
+	return true;
+}
+}//Gu
+}//physx

physx/source/geomutils/src/contact/GuContactSphereCapsule.cpp

@@ -0,0 +1,83 @@
+//
+// 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-2021 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 "geomutils/GuContactBuffer.h"
+
+#include "GuDistancePointSegment.h"
+#include "GuContactMethodImpl.h"
+#include "GuInternal.h"
+#include "GuGeometryUnion.h"
+
+namespace physx
+{
+namespace Gu
+{
+bool contactSphereCapsule(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(renderOutput);
+	PX_UNUSED(cache);
+
+	const PxSphereGeometry& sphereGeom = shape0.get<const PxSphereGeometry>();
+	const PxCapsuleGeometry& capsuleGeom = shape1.get<const PxCapsuleGeometry>();
+
+	// PT: get capsule in local space
+	const PxVec3 capsuleLocalSegment = getCapsuleHalfHeightVector(transform1, capsuleGeom);
+	const Segment localSegment(capsuleLocalSegment, -capsuleLocalSegment);
+
+	// PT: get sphere in capsule space
+	const PxVec3 sphereCenterInCapsuleSpace = transform0.p - transform1.p;
+
+	const PxReal radiusSum = sphereGeom.radius + capsuleGeom.radius;
+	const PxReal inflatedSum = radiusSum + params.mContactDistance;
+
+	// PT: compute distance between sphere center & capsule's segment
+	PxReal u;
+	const PxReal squareDist = distancePointSegmentSquared(localSegment, sphereCenterInCapsuleSpace, &u);
+	if(squareDist >= inflatedSum*inflatedSum)
+		return false;
+
+	// PT: compute contact normal
+	PxVec3 normal = sphereCenterInCapsuleSpace - localSegment.getPointAt(u);
+		
+	// We do a *manual* normalization to check for singularity condition
+	const PxReal lenSq = normal.magnitudeSquared();
+	if(lenSq==0.0f) 
+		normal = PxVec3(1.0f, 0.0f, 0.0f);	// PT: zero normal => pick up random one
+	else
+		normal *= PxRecipSqrt(lenSq);
+
+	// PT: compute contact point
+	const PxVec3 point = sphereCenterInCapsuleSpace + transform1.p - normal * sphereGeom.radius;
+
+	// PT: output unique contact
+	contactBuffer.contact(point, normal, PxSqrt(squareDist) - radiusSum);
+	return true;
+}
+}//Gu
+}//physx

physx/source/geomutils/src/contact/GuContactSphereMesh.cpp

@@ -0,0 +1,615 @@
+//
+// 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-2021 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 "geomutils/GuContactBuffer.h"
+
+#include "GuDistancePointTriangle.h"
+#include "GuContactMethodImpl.h"
+#include "GuGeometryUnion.h"
+#include "GuFeatureCode.h"
+#include "GuMidphaseInterface.h"
+#include "GuEntityReport.h"
+#include "GuHeightFieldUtil.h"
+#include "GuBox.h"
+
+#include "PsSort.h"
+#include "CmRenderOutput.h"
+
+#define DEBUG_RENDER_MESHCONTACTS	0
+
+using namespace physx;
+using namespace Gu;
+
+static const bool gDrawTouchedTriangles = false;
+
+static void outputErrorMessage()
+{
+#if PX_CHECKED
+	Ps::getFoundation().error(PxErrorCode::eINTERNAL_ERROR, __FILE__, __LINE__, "Dropping contacts in sphere vs mesh: exceeded limit of 64 ");
+#endif
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+// PT: a customized version that also returns the feature code
+static PxVec3 closestPtPointTriangle(const PxVec3& p, const PxVec3& a, const PxVec3& b, const PxVec3& c, float& s, float& t, FeatureCode& fc)
+{
+	// Check if P in vertex region outside A
+	const PxVec3 ab = b - a;
+	const PxVec3 ac = c - a;
+	const PxVec3 ap = p - a;
+	const float d1 = ab.dot(ap);
+	const float d2 = ac.dot(ap);
+	if(d1<=0.0f && d2<=0.0f)
+	{
+		s = 0.0f;
+		t = 0.0f;
+		fc = FC_VERTEX0;
+		return a;	// Barycentric coords 1,0,0
+	}
+
+	// Check if P in vertex region outside B
+	const PxVec3 bp = p - b;
+	const float d3 = ab.dot(bp);
+	const float d4 = ac.dot(bp);
+	if(d3>=0.0f && d4<=d3)
+	{
+		s = 1.0f;
+		t = 0.0f;
+		fc = FC_VERTEX1;
+		return b;	// Barycentric coords 0,1,0
+	}
+
+	// Check if P in edge region of AB, if so return projection of P onto AB
+	const float vc = d1*d4 - d3*d2;
+	if(vc<=0.0f && d1>=0.0f && d3<=0.0f)
+	{
+		const float v = d1 / (d1 - d3);
+		s = v;
+		t = 0.0f;
+		fc = FC_EDGE01;
+		return a + v * ab;	// barycentric coords (1-v, v, 0)
+	}
+
+	// Check if P in vertex region outside C
+	const PxVec3 cp = p - c;
+	const float d5 = ab.dot(cp);
+	const float d6 = ac.dot(cp);
+	if(d6>=0.0f && d5<=d6)
+	{
+		s = 0.0f;
+		t = 1.0f;
+		fc = FC_VERTEX2;
+		return c;	// Barycentric coords 0,0,1
+	}
+
+	// Check if P in edge region of AC, if so return projection of P onto AC
+	const float vb = d5*d2 - d1*d6;
+	if(vb<=0.0f && d2>=0.0f && d6<=0.0f)
+	{
+		const float w = d2 / (d2 - d6);
+		s = 0.0f;
+		t = w;
+		fc = FC_EDGE20;
+		return a + w * ac;	// barycentric coords (1-w, 0, w)
+	}
+
+	// Check if P in edge region of BC, if so return projection of P onto BC
+	const float va = d3*d6 - d5*d4;
+	if(va<=0.0f && (d4-d3)>=0.0f && (d5-d6)>=0.0f)
+	{
+		const float w = (d4-d3) / ((d4 - d3) + (d5-d6));
+		s = 1.0f-w;
+		t = w;
+		fc = FC_EDGE12;
+		return b + w * (c-b);	// barycentric coords (0, 1-w, w)
+	}
+
+	// P inside face region. Compute Q through its barycentric coords (u,v,w)
+	const float denom = 1.0f / (va + vb + vc);
+	const float v = vb * denom;
+	const float w = vc * denom;
+	s = v;
+	t = w;
+	fc = FC_FACE;
+	return a + ab*v + ac*w;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+// PT: we use a separate structure to make sorting faster
+struct SortKey
+{
+	float		mSquareDist;
+	PxU32		mIndex;
+
+	PX_FORCE_INLINE bool operator < (const SortKey& data) const
+	{
+		return mSquareDist < data.mSquareDist;
+	}		
+};
+
+struct TriangleData
+{
+	PxVec3		mDelta;
+	FeatureCode	mFC;
+	PxU32		mTriangleIndex;
+	PxU32		mVRef[3];
+};
+
+struct CachedTriangleIndices
+{
+	PxU32		mVRef[3];
+};
+
+static PX_FORCE_INLINE bool validateSquareDist(PxReal squareDist)
+{
+	return squareDist>0.0001f;
+}
+
+static bool validateEdge(PxU32 vref0, PxU32 vref1, const CachedTriangleIndices* cachedTris, PxU32 nbCachedTris)
+{
+	while(nbCachedTris--)
+	{
+		const CachedTriangleIndices& inds = *cachedTris++;
+		const PxU32 vi0 = inds.mVRef[0];
+		const PxU32 vi1 = inds.mVRef[1];
+		const PxU32 vi2 = inds.mVRef[2];
+
+		if(vi0==vref0)
+		{
+			if(vi1==vref1 || vi2==vref1)
+				return false;
+		}
+		else if(vi1==vref0)
+		{
+			if(vi0==vref1 || vi2==vref1)
+				return false;
+		}
+		else if(vi2==vref0)
+		{
+			if(vi1==vref1 || vi0==vref1)
+				return false;
+		}
+	}
+	return true;
+}
+
+static bool validateVertex(PxU32 vref, const CachedTriangleIndices* cachedTris, PxU32 nbCachedTris)
+{
+	while(nbCachedTris--)
+	{
+		const CachedTriangleIndices& inds = *cachedTris++;
+		if(inds.mVRef[0]==vref || inds.mVRef[1]==vref || inds.mVRef[2]==vref)
+			return false;
+	}
+	return true;
+}
+
+namespace
+{
+	class NullAllocator
+	{
+	public:
+		PX_FORCE_INLINE NullAllocator()								{				}
+		PX_FORCE_INLINE	void* allocate(size_t, const char*, int) 	{ return NULL;	}
+		PX_FORCE_INLINE	void deallocate(void*)						{				}
+	};
+
+struct SphereMeshContactGeneration
+{
+	const PxSphereGeometry&	mShapeSphere;
+	const PxTransform&		mTransform0;
+	const PxTransform&		mTransform1;
+	ContactBuffer&			mContactBuffer;
+	const PxVec3&			mSphereCenterShape1Space;
+	PxF32					mInflatedRadius2;
+	PxU32					mNbDelayed;
+	TriangleData			mSavedData[ContactBuffer::MAX_CONTACTS];
+	SortKey					mSortKey[ContactBuffer::MAX_CONTACTS];
+	PxU32					mNbCachedTris;
+	CachedTriangleIndices	mCachedTris[ContactBuffer::MAX_CONTACTS];
+	Cm::RenderOutput*		mRenderOutput;
+
+	SphereMeshContactGeneration(const PxSphereGeometry& shapeSphere, const PxTransform& transform0, const PxTransform& transform1,
+		ContactBuffer& contactBuffer, const PxVec3& sphereCenterShape1Space, PxF32 inflatedRadius,
+		Cm::RenderOutput* renderOutput) :
+		mShapeSphere				(shapeSphere),
+		mTransform0					(transform0),
+		mTransform1					(transform1),
+		mContactBuffer				(contactBuffer),
+		mSphereCenterShape1Space	(sphereCenterShape1Space),
+		mInflatedRadius2			(inflatedRadius*inflatedRadius),
+		mNbDelayed					(0),
+		mNbCachedTris				(0),
+		mRenderOutput				(renderOutput)
+	{
+	}
+
+	PX_FORCE_INLINE void cacheTriangle(PxU32 ref0, PxU32 ref1, PxU32 ref2)
+	{
+		const PxU32 nb = mNbCachedTris++;
+		mCachedTris[nb].mVRef[0] = ref0;
+		mCachedTris[nb].mVRef[1] = ref1;
+		mCachedTris[nb].mVRef[2] = ref2;
+	}
+
+	PX_FORCE_INLINE void addContact(const PxVec3& d, PxReal squareDist, PxU32 triangleIndex)
+	{
+		float dist;
+		PxVec3 delta;
+		if(validateSquareDist(squareDist))
+		{
+			// PT: regular contact. Normalize 'delta'.
+			dist = PxSqrt(squareDist);
+			delta = d / dist;
+		}
+		else
+		{
+			// PT: singular contact: 'd' is the non-unit triangle's normal in this case.
+			dist = 0.0f;
+			delta = -d.getNormalized();
+		}
+
+		const PxVec3 worldNormal = -mTransform1.rotate(delta);
+
+		const PxVec3 localHit = mSphereCenterShape1Space + mShapeSphere.radius*delta;
+		const PxVec3 hit = mTransform1.transform(localHit);
+
+		if(!mContactBuffer.contact(hit, worldNormal, dist - mShapeSphere.radius, triangleIndex))
+			outputErrorMessage();
+	}
+
+	void processTriangle(PxU32 triangleIndex, const PxVec3& v0, const PxVec3& v1, const PxVec3& v2, const PxU32* vertInds)
+	{
+		// PT: compute closest point between sphere center and triangle
+		PxReal u, v;
+		FeatureCode fc;
+		const PxVec3 cp = closestPtPointTriangle(mSphereCenterShape1Space, v0, v1, v2, u, v, fc);
+
+		// PT: compute 'delta' vector between closest point and sphere center
+		const PxVec3 delta = cp - mSphereCenterShape1Space;
+		const PxReal squareDist = delta.magnitudeSquared();
+		if(squareDist >= mInflatedRadius2)
+			return;
+
+		// PT: backface culling without the normalize
+		// PT: TODO: consider doing before the pt-triangle distance test if it's cheaper
+		// PT: TODO: e0/e1 already computed in closestPtPointTriangle
+		const PxVec3 e0 = v1 - v0;
+		const PxVec3 e1 = v2 - v0;
+		const PxVec3 planeNormal = e0.cross(e1);
+		const PxF32 planeD = planeNormal.dot(v0);	// PT: actually -d compared to PxcPlane
+		if(planeNormal.dot(mSphereCenterShape1Space) < planeD)
+			return;
+
+		// PT: for a regular contact, 'delta' is non-zero (and so is 'squareDist'). However when the sphere's center exactly touches
+		// the triangle, then both 'delta' and 'squareDist' become zero. This needs to be handled as a special case to avoid dividing
+		// by zero. We will use the triangle's normal as a contact normal in this special case.
+		//
+		// 'validateSquareDist' is called twice because there are conflicting goals here. We could call it once now and already
+		// compute the proper data for generating the contact. But this would mean doing a square-root and a division right here,
+		// even when the contact is not actually needed in the end. We could also call it only once in "addContact', but the plane's
+		// normal would not always be available (in case of delayed contacts), and thus it would need to be either recomputed (slower)
+		// or stored within 'TriangleData' (using more memory). Calling 'validateSquareDist' twice is a better option overall.
+		PxVec3 d;
+		if(validateSquareDist(squareDist))
+			d = delta;
+		else
+			d = planeNormal;
+
+		if(fc==FC_FACE)
+		{
+			addContact(d, squareDist, triangleIndex);
+
+			if(mNbCachedTris<ContactBuffer::MAX_CONTACTS)
+				cacheTriangle(vertInds[0], vertInds[1], vertInds[2]);
+		}
+		else
+		{
+			if(mNbDelayed<ContactBuffer::MAX_CONTACTS)
+			{
+				const PxU32 index = mNbDelayed++;
+				mSortKey[index].mSquareDist = squareDist;
+				mSortKey[index].mIndex = index;
+
+				TriangleData* saved = mSavedData + index;
+				saved->mDelta			= d;
+				saved->mVRef[0]			= vertInds[0];
+				saved->mVRef[1]			= vertInds[1];
+				saved->mVRef[2]			= vertInds[2];
+				saved->mFC				= fc;
+				saved->mTriangleIndex	= triangleIndex;
+			}
+			else outputErrorMessage();
+		}
+	}
+
+	void generateLastContacts()
+	{
+		const PxU32 count = mNbDelayed;
+		if(!count)
+			return;
+
+		Ps::sort(mSortKey, count, Ps::Less<SortKey>(), NullAllocator(), ContactBuffer::MAX_CONTACTS);
+
+		TriangleData* touchedTris = mSavedData;
+		for(PxU32 i=0;i<count;i++)
+		{
+			const TriangleData& data = touchedTris[mSortKey[i].mIndex];
+
+			const PxU32 ref0 = data.mVRef[0];
+			const PxU32 ref1 = data.mVRef[1];
+			const PxU32 ref2 = data.mVRef[2];
+
+			bool generateContact = false;
+
+			switch(data.mFC)
+			{
+				case FC_VERTEX0:
+					generateContact = ::validateVertex(ref0, mCachedTris, mNbCachedTris);
+					break;
+
+				case FC_VERTEX1:
+					generateContact = ::validateVertex(ref1, mCachedTris, mNbCachedTris);
+					break;
+
+				case FC_VERTEX2:
+					generateContact = ::validateVertex(ref2, mCachedTris, mNbCachedTris);
+					break;
+
+				case FC_EDGE01:
+					generateContact = ::validateEdge(ref0, ref1, mCachedTris, mNbCachedTris);
+					break;
+
+				case FC_EDGE12:
+					generateContact = ::validateEdge(ref1, ref2, mCachedTris, mNbCachedTris);
+					break;
+
+				case FC_EDGE20:
+					generateContact = ::validateEdge(ref0, ref2, mCachedTris, mNbCachedTris);
+					break;
+
+				case FC_FACE:
+				case FC_UNDEFINED:
+					PX_ASSERT(0);	// PT: should not be possible
+					break;
+			};
+	
+			if(generateContact)
+				addContact(data.mDelta, mSortKey[i].mSquareDist, data.mTriangleIndex);
+
+			if(mNbCachedTris<ContactBuffer::MAX_CONTACTS)
+				cacheTriangle(ref0, ref1, ref2);
+			else
+				outputErrorMessage();
+		}
+	}
+
+private:
+	SphereMeshContactGeneration& operator=(const SphereMeshContactGeneration&);
+};
+
+struct SphereMeshContactGenerationCallback_NoScale : MeshHitCallback<PxRaycastHit>
+{
+	SphereMeshContactGeneration			mGeneration;
+	const TriangleMesh&					mMeshData;
+
+	SphereMeshContactGenerationCallback_NoScale(const TriangleMesh& meshData, const PxSphereGeometry& shapeSphere,
+		const PxTransform& transform0, const PxTransform& transform1, ContactBuffer& contactBuffer,
+		const PxVec3& sphereCenterShape1Space, PxF32 inflatedRadius, Cm::RenderOutput* renderOutput
+	) : MeshHitCallback<PxRaycastHit>	(CallbackMode::eMULTIPLE),
+		mGeneration						(shapeSphere, transform0, transform1, contactBuffer, sphereCenterShape1Space, inflatedRadius, renderOutput),
+		mMeshData						(meshData)
+	{
+	}
+
+	virtual ~SphereMeshContactGenerationCallback_NoScale()
+	{
+		mGeneration.generateLastContacts();
+	}
+
+	virtual PxAgain processHit(
+		const PxRaycastHit& hit, const PxVec3& v0, const PxVec3& v1, const PxVec3& v2, PxReal&, const PxU32* vinds)
+	{
+		if(gDrawTouchedTriangles)
+		{
+			(*mGeneration.mRenderOutput) << 0xffffffff;
+			(*mGeneration.mRenderOutput) << PxMat44(PxIdentity);
+			const PxVec3 wp0 = mGeneration.mTransform1.transform(v0);
+			const PxVec3 wp1 = mGeneration.mTransform1.transform(v1);
+			const PxVec3 wp2 = mGeneration.mTransform1.transform(v2);
+			mGeneration.mRenderOutput->outputSegment(wp0, wp1);
+			mGeneration.mRenderOutput->outputSegment(wp1, wp2);
+			mGeneration.mRenderOutput->outputSegment(wp2, wp0);
+		}
+
+		mGeneration.processTriangle(hit.faceIndex, v0, v1, v2, vinds);
+		return true;
+	}
+
+protected:
+	SphereMeshContactGenerationCallback_NoScale &operator=(const SphereMeshContactGenerationCallback_NoScale &);
+};
+
+struct SphereMeshContactGenerationCallback_Scale : SphereMeshContactGenerationCallback_NoScale
+{
+	const Cm::FastVertex2ShapeScaling&	mMeshScaling;
+
+	SphereMeshContactGenerationCallback_Scale(const TriangleMesh& meshData, const PxSphereGeometry& shapeSphere,
+		const PxTransform& transform0, const PxTransform& transform1, const Cm::FastVertex2ShapeScaling& meshScaling,
+		ContactBuffer& contactBuffer, const PxVec3& sphereCenterShape1Space, PxF32 inflatedRadius, Cm::RenderOutput* renderOutput
+	) : SphereMeshContactGenerationCallback_NoScale(meshData, shapeSphere,
+		transform0, transform1, contactBuffer, sphereCenterShape1Space, inflatedRadius, renderOutput),
+		mMeshScaling	(meshScaling)
+	{
+	}
+
+	virtual ~SphereMeshContactGenerationCallback_Scale() {}
+
+	virtual PxAgain processHit(const PxRaycastHit& hit, const PxVec3& v0, const PxVec3& v1, const PxVec3& v2, PxReal&, const PxU32* vinds)
+	{
+		PxVec3 verts[3];
+		getScaledVertices(verts, v0, v1, v2, false, mMeshScaling);
+
+		if(gDrawTouchedTriangles)
+		{
+			(*mGeneration.mRenderOutput) << 0xffffffff;
+			(*mGeneration.mRenderOutput) << PxMat44(PxIdentity);
+			const PxVec3 wp0 = mGeneration.mTransform1.transform(verts[0]);
+			const PxVec3 wp1 = mGeneration.mTransform1.transform(verts[1]);
+			const PxVec3 wp2 = mGeneration.mTransform1.transform(verts[2]);
+			mGeneration.mRenderOutput->outputSegment(wp0, wp1);
+			mGeneration.mRenderOutput->outputSegment(wp1, wp2);
+			mGeneration.mRenderOutput->outputSegment(wp2, wp0);
+		}
+
+		mGeneration.processTriangle(hit.faceIndex, verts[0], verts[1], verts[2], vinds);
+		return true;
+	}
+protected:
+	SphereMeshContactGenerationCallback_Scale &operator=(const SphereMeshContactGenerationCallback_Scale &);
+};
+
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+bool Gu::contactSphereMesh(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(cache);
+
+	const PxSphereGeometry& shapeSphere = shape0.get<const PxSphereGeometry>();
+	const PxTriangleMeshGeometryLL& shapeMesh = shape1.get<const PxTriangleMeshGeometryLL>();
+
+	// We must be in local space to use the cache
+	const PxVec3 sphereCenterInMeshSpace = transform1.transformInv(transform0.p);
+	const PxReal inflatedRadius = shapeSphere.radius + params.mContactDistance;
+	const TriangleMesh* meshData = shapeMesh.meshData;
+
+	// mesh scale is not baked into cached verts
+	if(shapeMesh.scale.isIdentity())
+	{
+		SphereMeshContactGenerationCallback_NoScale callback(
+			*meshData, shapeSphere, transform0, transform1,
+			contactBuffer, sphereCenterInMeshSpace, inflatedRadius, renderOutput);
+
+		// PT: TODO: switch to sphere query here
+		const Box obb(sphereCenterInMeshSpace, PxVec3(inflatedRadius), PxMat33(PxIdentity));
+		Midphase::intersectOBB(meshData, obb, callback, true);
+	}
+	else
+	{
+		const Cm::FastVertex2ShapeScaling meshScaling(shapeMesh.scale);
+
+		SphereMeshContactGenerationCallback_Scale callback(
+			*meshData, shapeSphere, transform0, transform1,
+			meshScaling, contactBuffer, sphereCenterInMeshSpace, inflatedRadius, renderOutput);
+
+		PxVec3 obbCenter = sphereCenterInMeshSpace;
+		PxVec3 obbExtents = PxVec3(inflatedRadius);
+		PxMat33 obbRot(PxIdentity);
+		meshScaling.transformQueryBounds(obbCenter, obbExtents, obbRot);
+
+		const Box obb(obbCenter, obbExtents, obbRot);
+
+		Midphase::intersectOBB(meshData, obb, callback, true);
+	}
+	return contactBuffer.count > 0;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+namespace
+{
+struct SphereHeightfieldContactGenerationCallback : EntityReport<PxU32>
+{
+	SphereMeshContactGeneration mGeneration;
+	HeightFieldUtil&			mHfUtil;
+
+	SphereHeightfieldContactGenerationCallback(
+		HeightFieldUtil&		hfUtil,
+		const PxSphereGeometry&	shapeSphere,
+		const PxTransform&		transform0,
+		const PxTransform&		transform1,
+		ContactBuffer&			contactBuffer,
+		const PxVec3&			sphereCenterInMeshSpace,
+		PxF32					inflatedRadius,
+		Cm::RenderOutput*		renderOutput
+	) :
+		mGeneration				(shapeSphere, transform0, transform1, contactBuffer, sphereCenterInMeshSpace, inflatedRadius, renderOutput),
+		mHfUtil					(hfUtil)
+	{
+	}
+
+	// PT: TODO: refactor/unify with similar code in other places
+	virtual bool onEvent(PxU32 nb, PxU32* indices)
+	{
+		while(nb--)
+		{
+			const PxU32 triangleIndex = *indices++;
+			PxU32 vertIndices[3];
+			PxTriangle currentTriangle;
+			mHfUtil.getTriangle(mGeneration.mTransform1, currentTriangle, vertIndices, NULL, triangleIndex, false, false);
+
+			mGeneration.processTriangle(triangleIndex, currentTriangle.verts[0], currentTriangle.verts[1], currentTriangle.verts[2], vertIndices);
+		}
+		return true;
+	}
+protected:
+	SphereHeightfieldContactGenerationCallback &operator=(const SphereHeightfieldContactGenerationCallback &);
+};
+}
+
+bool Gu::contactSphereHeightfield(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(cache);
+	PX_UNUSED(renderOutput);
+
+	const PxSphereGeometry& shapeSphere = shape0.get<const PxSphereGeometry>();
+	const PxHeightFieldGeometryLL& shapeMesh = shape1.get<const PxHeightFieldGeometryLL>();
+
+	HeightFieldUtil hfUtil(shapeMesh);
+
+	const PxVec3 sphereCenterInMeshSpace = transform1.transformInv(transform0.p);
+	const PxReal inflatedRadius = shapeSphere.radius + params.mContactDistance;
+	const PxVec3 inflatedRV3(inflatedRadius);
+
+	const PxBounds3 bounds(sphereCenterInMeshSpace - inflatedRV3, sphereCenterInMeshSpace + inflatedRV3);
+
+	SphereHeightfieldContactGenerationCallback blockCallback(hfUtil, shapeSphere, transform0, transform1, contactBuffer, sphereCenterInMeshSpace, inflatedRadius, renderOutput);
+
+	hfUtil.overlapAABBTriangles(transform1, bounds, 0, &blockCallback);
+
+	blockCallback.mGeneration.generateLastContacts();
+
+	return contactBuffer.count > 0;
+}
+
+///////////////////////////////////////////////////////////////////////////////

physx/source/geomutils/src/contact/GuContactSpherePlane.cpp

@@ -0,0 +1,68 @@
+//
+// 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-2021 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 "geomutils/GuContactBuffer.h"
+#include "GuContactMethodImpl.h"
+#include "GuGeometryUnion.h"
+
+namespace physx
+{
+namespace Gu
+{
+bool contactSpherePlane(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(renderOutput);
+	PX_UNUSED(cache);
+	PX_UNUSED(shape1);
+
+	// Get actual shape data
+	const PxSphereGeometry& shapeSphere = shape0.get<const PxSphereGeometry>();
+	//const PxPlaneGeometry& shapePlane = shape1.get<const PxPlaneGeometry>();
+
+	//Sphere in plane space
+	const PxVec3 sphere = transform1.transformInv(transform0.p);
+	
+	//Make sure we have a normalized plane
+	//The plane is implicitly n=<1,0,0> d=0 (in plane-space)
+	//PX_ASSERT(PxAbs(shape1.mNormal.magnitudeSquared() - 1.0f) < 0.000001f);
+
+	//Separation
+	const PxReal separation = sphere.x - shapeSphere.radius;
+
+	if(separation<=params.mContactDistance)
+	{
+		const PxVec3 normal = transform1.q.getBasisVector0();
+		const PxVec3 point  = transform0.p - normal * shapeSphere.radius;
+		contactBuffer.contact(point, normal, separation);
+		return true;
+	}
+	return false;
+}
+}//Gu
+}//physx

physx/source/geomutils/src/contact/GuContactSphereSphere.cpp

@@ -0,0 +1,68 @@
+//
+// 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-2021 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 "geomutils/GuContactBuffer.h"
+#include "GuContactMethodImpl.h"
+#include "GuGeometryUnion.h"
+
+namespace physx
+{
+namespace Gu
+{
+bool contactSphereSphere(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(renderOutput);
+	PX_UNUSED(cache);
+
+	const PxSphereGeometry& sphereGeom0 = shape0.get<const PxSphereGeometry>();
+	const PxSphereGeometry& sphereGeom1 = shape1.get<const PxSphereGeometry>();
+
+	PxVec3 delta = transform0.p - transform1.p;
+
+	const PxReal distanceSq = delta.magnitudeSquared();
+	const PxReal radiusSum = sphereGeom0.radius + sphereGeom1.radius;
+	const PxReal inflatedSum = radiusSum + params.mContactDistance;
+	if(distanceSq >= inflatedSum*inflatedSum)
+		return false;
+
+	// We do a *manual* normalization to check for singularity condition
+	const PxReal magn = PxSqrt(distanceSq);
+	if(magn<=0.00001f)
+		delta = PxVec3(1.0f, 0.0f, 0.0f);	// PT: spheres are exactly overlapping => can't create normal => pick up random one
+	else
+		delta *= 1.0f/magn;
+
+	// PT: TODO: why is this formula different from the original code?
+	const PxVec3 contact = delta * ((sphereGeom0.radius + magn - sphereGeom1.radius)*-0.5f) + transform0.p;
+		
+	contactBuffer.contact(contact, delta, magn - radiusSum);
+	return true;
+}
+}//Gu
+}//physx

physx/source/geomutils/src/contact/GuFeatureCode.cpp

@@ -0,0 +1,128 @@
+//
+// 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-2021 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 "GuConvexEdgeFlags.h"
+#include "GuFeatureCode.h"
+
+using namespace physx;
+using namespace Gu;
+
+static FeatureCode computeFeatureCode(PxReal u, PxReal v)
+{
+	// Analysis
+	if(u==0.0f)
+	{
+		if(v==0.0f)
+		{
+			// Vertex 0
+			return FC_VERTEX0;
+		}
+		else if(v==1.0f)
+		{
+			// Vertex 2
+			return FC_VERTEX2;
+		}
+		else
+		{
+			// Edge 0-2
+			return FC_EDGE20;
+		}
+	}
+	else if(u==1.0f)
+	{
+		if(v==0.0f)
+		{
+			// Vertex 1
+			return FC_VERTEX1;
+		}
+	}
+	else
+	{
+		if(v==0.0f)
+		{
+			// Edge 0-1
+			return FC_EDGE01;
+		}
+		else
+		{
+			if((u+v)>=0.9999f)
+			{
+				// Edge 1-2
+				return FC_EDGE12;
+			}
+			else
+			{
+				// Face
+				return FC_FACE;
+			}
+		}
+	}
+	return FC_UNDEFINED;
+}
+
+
+bool Gu::selectNormal(PxU8 data, PxReal u, PxReal v)
+{
+	bool useFaceNormal = false;
+	const FeatureCode FC = computeFeatureCode(u, v);
+	switch(FC)
+	{
+		case FC_VERTEX0:
+			if(!(data & (Gu::ETD_CONVEX_EDGE_01|Gu::ETD_CONVEX_EDGE_20)))
+				useFaceNormal = true;
+			break;
+		case FC_VERTEX1:
+			if(!(data & (Gu::ETD_CONVEX_EDGE_01|Gu::ETD_CONVEX_EDGE_12)))
+				useFaceNormal = true;
+			break;
+		case FC_VERTEX2:
+			if(!(data & (Gu::ETD_CONVEX_EDGE_12|Gu::ETD_CONVEX_EDGE_20)))
+				useFaceNormal = true;
+			break;
+		case FC_EDGE01:
+			if(!(data & Gu::ETD_CONVEX_EDGE_01))
+				useFaceNormal = true;
+			break;
+		case FC_EDGE12:
+			if(!(data & Gu::ETD_CONVEX_EDGE_12))
+				useFaceNormal = true;
+			break;
+		case FC_EDGE20:
+			if(!(data & Gu::ETD_CONVEX_EDGE_20))
+				useFaceNormal = true;
+			break;
+		case FC_FACE:
+			useFaceNormal = true;
+			break;
+		case FC_UNDEFINED:
+			break;
+	};
+	return useFaceNormal;
+}
+

physx/source/geomutils/src/contact/GuFeatureCode.h

@@ -0,0 +1,56 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_FEATURE_CODE_H
+#define GU_FEATURE_CODE_H
+
+#include "CmPhysXCommon.h"
+
+namespace physx
+{
+namespace Gu
+{
+	enum FeatureCode
+	{
+		FC_VERTEX0,
+		FC_VERTEX1,
+		FC_VERTEX2,
+		FC_EDGE01,
+		FC_EDGE12,
+		FC_EDGE20,
+		FC_FACE,
+
+		FC_UNDEFINED
+	};
+
+	bool		selectNormal(PxU8 data, PxReal u, PxReal v);
+}
+}
+
+#endif

physx/source/geomutils/src/convex/GuBigConvexData.cpp

@@ -0,0 +1,203 @@
+//
+// 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-2021 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 "PsIntrinsics.h"
+#include "PsUserAllocated.h"
+#include "GuSerialize.h"
+#include "GuBigConvexData2.h"
+#include "GuCubeIndex.h"
+#include "PsIntrinsics.h"
+#include "CmUtils.h"
+#include "PsUtilities.h"
+#include "PsAllocator.h"
+
+using namespace physx;
+using namespace Gu;
+
+BigConvexData::BigConvexData() : mVBuffer(NULL)
+{
+	mData.mSubdiv			= 0;
+	mData.mNbSamples		= 0;
+	mData.mSamples			= NULL;
+
+	//////
+
+	mData.mNbVerts			= 0;
+	mData.mNbAdjVerts		= 0;
+	mData.mValencies		= NULL;
+	mData.mAdjacentVerts	= NULL;
+}
+
+BigConvexData::~BigConvexData()
+{
+	PX_FREE(mData.mSamples);
+
+	///////////
+
+	if(mVBuffer)
+	{
+		PX_FREE(mVBuffer);
+	}
+	else
+	{
+		// Allocated from somewhere else!!
+		PX_FREE(mData.mValencies);
+		PX_FREE(mData.mAdjacentVerts);
+	}
+}
+
+void BigConvexData::CreateOffsets()
+{
+	// Create offsets (radix style)
+	mData.mValencies[0].mOffset = 0;
+	for(PxU32 i=1;i<mData.mNbVerts;i++)
+		mData.mValencies[i].mOffset = PxU16(mData.mValencies[i-1].mOffset + mData.mValencies[i-1].mCount);
+}
+
+bool BigConvexData::VLoad(PxInputStream& stream)
+{
+	// Import header
+	PxU32 Version;
+	bool Mismatch;
+	if(!ReadHeader('V', 'A', 'L', 'E', Version, Mismatch, stream))
+		return false;
+
+	mData.mNbVerts		= readDword(Mismatch, stream);
+	mData.mNbAdjVerts	= readDword(Mismatch, stream);
+
+	PX_FREE(mVBuffer);
+
+	// PT: align Gu::Valency?
+	const PxU32 numVerts = (mData.mNbVerts+3)&~3;
+	const PxU32 TotalSize = sizeof(Gu::Valency)*numVerts + sizeof(PxU8)*mData.mNbAdjVerts;
+	mVBuffer = PX_ALLOC(TotalSize, "BigConvexData data");
+	mData.mValencies		= reinterpret_cast<Gu::Valency*>(mVBuffer);
+	mData.mAdjacentVerts	= (reinterpret_cast<PxU8*>(mVBuffer)) + sizeof(Gu::Valency)*numVerts;
+
+	PX_ASSERT(0 == (size_t(mData.mAdjacentVerts) & 0xf));
+	PX_ASSERT(Version==2);
+
+	{
+		PxU16* temp = reinterpret_cast<PxU16*>(mData.mValencies);
+
+		PxU32 MaxIndex = readDword(Mismatch, stream);
+		ReadIndices(Ps::to16(MaxIndex), mData.mNbVerts, temp, stream, Mismatch);
+
+		// We transform from:
+		//
+		// |5555|4444|3333|2222|1111|----|----|----|----|----|
+		//
+		// to:
+		//
+		// |5555|4444|4444|2222|3333|----|2222|----|1111|----|
+		//
+		for(PxU32 i=0;i<mData.mNbVerts;i++)
+			mData.mValencies[mData.mNbVerts-i-1].mCount = temp[mData.mNbVerts-i-1];
+	}
+	stream.read(mData.mAdjacentVerts, mData.mNbAdjVerts);
+
+	// Recreate offsets
+	CreateOffsets();
+
+	return true;
+}
+
+PxU32 BigConvexData::ComputeOffset(const PxVec3& dir) const
+{
+	return ComputeCubemapOffset(dir, mData.mSubdiv);
+}
+
+PxU32 BigConvexData::ComputeNearestOffset(const PxVec3& dir) const
+{
+	return ComputeCubemapNearestOffset(dir, mData.mSubdiv);
+}
+
+bool BigConvexData::Load(PxInputStream& stream)
+{
+	// Import header
+	PxU32 Version;
+	bool Mismatch;
+	if(!ReadHeader('S', 'U', 'P', 'M', Version, Mismatch, stream))
+		return false;
+
+	// Load base gaussmap
+//	if(!GaussMap::Load(stream))	return false;
+
+	// Import header
+	if(!ReadHeader('G', 'A', 'U', 'S', Version, Mismatch, stream))
+		return false;
+
+	// Import basic info
+	mData.mSubdiv		= Ps::to16(readDword(Mismatch, stream));
+	mData.mNbSamples	= Ps::to16(readDword(Mismatch, stream));
+
+	// Load map data
+	mData.mSamples = reinterpret_cast<PxU8*>(PX_ALLOC(sizeof(PxU8)*mData.mNbSamples*2, "BigConvex Samples Data"));
+
+	// These byte buffers shouldn't need converting
+	stream.read(mData.mSamples, sizeof(PxU8)*mData.mNbSamples*2);
+
+	//load the valencies
+	return VLoad(stream);
+}
+
+// PX_SERIALIZATION
+void BigConvexData::exportExtraData(PxSerializationContext& stream)
+{
+	if(mData.mSamples)
+	{
+		stream.alignData(PX_SERIAL_ALIGN);
+		stream.writeData(mData.mSamples, sizeof(PxU8)*mData.mNbSamples*2);
+	}
+
+	if(mData.mValencies)
+	{
+		stream.alignData(PX_SERIAL_ALIGN);
+		PxU32 numVerts = (mData.mNbVerts+3)&~3;
+		const PxU32 TotalSize = sizeof(Gu::Valency)*numVerts + sizeof(PxU8)*mData.mNbAdjVerts;
+		stream.writeData(mData.mValencies, TotalSize);
+	}
+}
+
+void BigConvexData::importExtraData(PxDeserializationContext& context)
+{
+	if(mData.mSamples)
+		mData.mSamples = context.readExtraData<PxU8, PX_SERIAL_ALIGN>(PxU32(mData.mNbSamples*2));
+
+	if(mData.mValencies)
+	{
+		context.alignExtraData();
+		PxU32 numVerts = (mData.mNbVerts+3)&~3;
+		mData.mValencies		= context.readExtraData<Gu::Valency>(numVerts);
+		mData.mAdjacentVerts	= context.readExtraData<PxU8>(mData.mNbAdjVerts);
+
+	}
+}
+//~PX_SERIALIZATION
+

physx/source/geomutils/src/convex/GuBigConvexData.h

@@ -0,0 +1,98 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_BIG_CONVEX_DATA_H
+#define GU_BIG_CONVEX_DATA_H
+
+#include "foundation/PxSimpleTypes.h"
+
+namespace physx
+{
+
+class BigConvexDataBuilder;
+class PxcHillClimb;
+class BigConvexData;
+
+// Data
+
+namespace Gu
+{
+
+struct Valency
+{
+//= ATTENTION! =====================================================================================
+// Changing the data layout of this class breaks the binary serialization format.  See comments for 
+// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
+// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
+// accordingly.
+//==================================================================================================
+
+	PxU16		mCount;
+	PxU16		mOffset;
+};
+PX_COMPILE_TIME_ASSERT(sizeof(Gu::Valency) == 4);
+
+struct BigConvexRawData
+{
+//= ATTENTION! =====================================================================================
+// Changing the data layout of this class breaks the binary serialization format.  See comments for 
+// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
+// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
+// accordingly.
+//==================================================================================================
+
+	// Support vertex map
+	PxU16		mSubdiv;		// "Gaussmap" subdivision
+	PxU16		mNbSamples;		// Total #samples in gaussmap PT: this is not even needed at runtime!
+
+	PxU8*		mSamples;
+	PX_FORCE_INLINE const PxU8*	getSamples2()	const
+	{
+		return mSamples + mNbSamples;
+	}
+	//~Support vertex map
+
+	// Valencies data
+	PxU32			mNbVerts;		//!< Number of vertices
+	PxU32			mNbAdjVerts;	//!< Total number of adjacent vertices  ### PT: this is useless at runtime and should not be stored here
+	Gu::Valency*	mValencies;		//!< A list of mNbVerts valencies (= number of neighbors)
+	PxU8*			mAdjacentVerts;	//!< List of adjacent vertices
+	//~Valencies data
+};
+#if PX_P64_FAMILY
+PX_COMPILE_TIME_ASSERT(sizeof(Gu::BigConvexRawData) == 40);
+#else
+PX_COMPILE_TIME_ASSERT(sizeof(Gu::BigConvexRawData) == 24);
+#endif
+
+} // namespace Gu
+
+}
+
+#endif

physx/source/geomutils/src/convex/GuBigConvexData2.h

@@ -0,0 +1,95 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_BIG_CONVEX_DATA2_H
+#define GU_BIG_CONVEX_DATA2_H
+
+#include "common/PxMetaData.h"
+#include "GuBigConvexData.h"
+
+namespace physx
+{
+	class PxSerializationContext;
+	class PxDeserializationContext;
+
+	class PX_PHYSX_COMMON_API BigConvexData : public Ps::UserAllocated
+	{
+	//= ATTENTION! =====================================================================================
+	// Changing the data layout of this class breaks the binary serialization format.  See comments for 
+	// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
+	// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
+	// accordingly.
+	//==================================================================================================
+		public:
+// PX_SERIALIZATION
+											BigConvexData(const PxEMPTY)	{}
+		static		void					getBinaryMetaData(PxOutputStream& stream);
+//~PX_SERIALIZATION
+											BigConvexData();
+											~BigConvexData();
+		// Support vertex map
+					bool					Load(PxInputStream& stream);
+
+					PxU32					ComputeOffset(const PxVec3& dir)		const;
+					PxU32					ComputeNearestOffset(const PxVec3& dir)	const;
+
+		// Data access
+		PX_INLINE	PxU32					GetSubdiv()								const	{ return mData.mSubdiv;											}
+		PX_INLINE	PxU32					GetNbSamples()							const	{ return mData.mNbSamples;										}
+		//~Support vertex map
+
+		// Valencies
+		// Data access
+		PX_INLINE	PxU32					GetNbVerts()							const	{ return mData.mNbVerts;										}
+		PX_INLINE	const Gu::Valency*		GetValencies()							const	{ return mData.mValencies;										}
+		PX_INLINE	PxU16					GetValency(PxU32 i)						const	{ return mData.mValencies[i].mCount;							}
+		PX_INLINE	PxU16					GetOffset(PxU32 i)						const	{ return mData.mValencies[i].mOffset;							}
+		PX_INLINE	const PxU8*				GetAdjacentVerts()						const	{ return mData.mAdjacentVerts;									}
+
+		PX_INLINE	PxU16					GetNbNeighbors(PxU32 i)					const	{ return mData.mValencies[i].mCount;							}
+		PX_INLINE	const PxU8*				GetNeighbors(PxU32 i)					const	{ return &mData.mAdjacentVerts[mData.mValencies[i].mOffset];	}
+
+// PX_SERIALIZATION
+					void					exportExtraData(PxSerializationContext& stream);
+					void					importExtraData(PxDeserializationContext& context);
+//~PX_SERIALIZATION
+					Gu::BigConvexRawData	mData;
+		protected:
+					void*					mVBuffer;
+		// Internal methods
+					void					CreateOffsets();
+					bool					VLoad(PxInputStream& stream);
+		//~Valencies
+		friend class BigConvexDataBuilder;
+	};
+
+}
+
+#endif // BIG_CONVEX_DATA_H
+

physx/source/geomutils/src/convex/GuConvexEdgeFlags.h

@@ -0,0 +1,59 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_CONVEX_EDGE_FLAGS_H
+#define GU_CONVEX_EDGE_FLAGS_H
+
+#include "CmPhysXCommon.h"
+
+namespace physx
+{
+namespace Gu
+{
+	enum ExtraTrigDataFlag
+	{
+		ETD_SILHOUETTE_EDGE_01 = (1 << 0),	//First edge is a silhouette edge
+		ETD_SILHOUETTE_EDGE_12 = (1 << 1),	//Second edge is a silhouette edge
+		ETD_SILHOUETTE_EDGE_20 = (1 << 2),	//Third edge is a silhouette edge
+		ETD_CONVEX_EDGE_01	= (1<<3),	// PT: important value, don't change
+		ETD_CONVEX_EDGE_12	= (1<<4),	// PT: important value, don't change
+		ETD_CONVEX_EDGE_20	= (1<<5),	// PT: important value, don't change
+
+		ETD_CONVEX_EDGE_ALL	= ETD_CONVEX_EDGE_01|ETD_CONVEX_EDGE_12|ETD_CONVEX_EDGE_20
+	};
+
+	// PT: helper function to make sure we use the proper default flags everywhere
+	PX_FORCE_INLINE PxU8 getConvexEdgeFlags(const PxU8* extraTrigData, PxU32 triangleIndex)
+	{
+		return extraTrigData ? extraTrigData[triangleIndex] : PxU8(ETD_CONVEX_EDGE_ALL);
+	}
+}
+}
+
+#endif

physx/source/geomutils/src/convex/GuConvexHelper.cpp

@@ -0,0 +1,137 @@
+//
+// 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-2021 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 "GuConvexHelper.h"
+#include "GuGeometryUnion.h"
+#include "GuInternal.h"
+#include "PsUtilities.h"
+
+using namespace physx;
+using namespace Gu;
+
+// PT: we can't call alloca in a function and we want to avoid defines or duplicating the code. This makes it a bit tricky to write.
+void Gu::getScaledConvex(	PxVec3*& scaledVertices, PxU8*& scaledIndices, PxVec3* dstVertices, PxU8* dstIndices,
+							bool idtConvexScale, const PxVec3* srcVerts, const PxU8* srcIndices, PxU32 nbVerts, const Cm::FastVertex2ShapeScaling& convexScaling)
+{
+	//pretransform convex polygon if we have scaling!
+	if(idtConvexScale)	// PT: the scale is always 1 for boxes so no need to test the type
+	{
+		scaledVertices = const_cast<PxVec3*>(srcVerts);
+		scaledIndices = const_cast<PxU8*>(srcIndices);
+	}
+	else
+	{
+		scaledIndices = dstIndices;
+		scaledVertices = dstVertices;
+		for(PxU32 i=0; i<nbVerts; i++)
+		{
+			scaledIndices[i] = Ps::to8(i);	//generate trivial indexing.
+			scaledVertices[i] = convexScaling * srcVerts[srcIndices[i]];
+		}
+	}
+}
+
+bool Gu::getConvexData(const Gu::GeometryUnion& shape, Cm::FastVertex2ShapeScaling& scaling, PxBounds3& bounds, PolygonalData& polyData)
+{
+	const PxConvexMeshGeometryLL& shapeConvex = shape.get<const PxConvexMeshGeometryLL>();
+
+	const bool idtScale = shapeConvex.scale.isIdentity();
+	if(!idtScale)
+		scaling.init(shapeConvex.scale);
+
+	// PT: this version removes all the FCMPs and almost all LHS. This is temporary until
+	// the legacy 3x3 matrix totally vanishes but meanwhile do NOT do useless matrix conversions,
+	// it's a perfect recipe for LHS.
+	PX_ASSERT(!shapeConvex.hullData->mAABB.isEmpty());
+	bounds = shapeConvex.hullData->mAABB.transformFast(scaling.getVertex2ShapeSkew());
+
+	getPolygonalData_Convex(&polyData, shapeConvex.hullData, scaling);
+
+	// PT: non-uniform scaling invalidates the "internal objects" optimization, since our internal sphere
+	// might become an ellipsoid or something. Just disable the optimization if scaling is used...
+	if(!idtScale)
+		polyData.mInternal.reset();
+
+	return idtScale;
+}
+
+PxU32 Gu::findUniqueConvexEdges(PxU32 maxNbEdges, ConvexEdge* PX_RESTRICT edges, PxU32 numPolygons, const Gu::HullPolygonData* PX_RESTRICT polygons, const PxU8* PX_RESTRICT vertexData)
+{
+	PxU32 nbEdges = 0;
+
+	while(numPolygons--)
+	{
+		const HullPolygonData& polygon = *polygons++;
+		const PxU8* vRefBase = vertexData + polygon.mVRef8;
+		PxU32 numEdges = polygon.mNbVerts;
+
+		PxU32 a = numEdges - 1;
+		PxU32 b = 0;
+		while(numEdges--)
+		{
+			PxU8 vi0 =  vRefBase[a];
+			PxU8 vi1 =	vRefBase[b];
+
+			if(vi1 < vi0)
+			{
+				PxU8 tmp = vi0;
+				vi0 = vi1;
+				vi1 = tmp;
+			}
+
+			bool found=false;
+			for(PxU32 i=0;i<nbEdges;i++)
+			{
+				if(edges[i].vref0==vi0 && edges[i].vref1==vi1)
+				{
+					found = true;
+					edges[i].normal += polygon.mPlane.n;
+					break;
+				}
+			}
+			if(!found)
+			{
+				if(nbEdges==maxNbEdges)
+				{
+					PX_ALWAYS_ASSERT_MESSAGE("Internal error: max nb edges reached. This shouldn't be possible...");
+					return nbEdges;
+				}
+
+				edges[nbEdges].vref0	= vi0;
+				edges[nbEdges].vref1	= vi1;
+				edges[nbEdges].normal	= polygon.mPlane.n;
+				nbEdges++;
+			}
+
+			a = b;
+			b++;
+		}
+	}
+	return nbEdges;
+}

physx/source/geomutils/src/convex/GuConvexHelper.h

@@ -0,0 +1,66 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_CONVEXHELPER_H
+#define GU_CONVEXHELPER_H
+
+#include "GuShapeConvex.h"
+
+namespace physx
+{
+namespace Gu
+{
+	class GeometryUnion;
+
+	///////////////////////////////////////////////////////////////////////////
+
+	PX_PHYSX_COMMON_API	void getScaledConvex(	PxVec3*& scaledVertices, PxU8*& scaledIndices, PxVec3* dstVertices, PxU8* dstIndices,
+												bool idtConvexScale, const PxVec3* srcVerts, const PxU8* srcIndices, PxU32 nbVerts, const Cm::FastVertex2ShapeScaling& convexScaling);
+
+	// PT: calling this correctly isn't trivial so let's macroize it. At least we limit the damage since it immediately calls a real function.
+	#define GET_SCALEX_CONVEX(scaledVertices, stackIndices, idtScaling, nbVerts, scaling, srcVerts, srcIndices)	\
+	getScaledConvex(scaledVertices, stackIndices,																\
+					idtScaling ? NULL : reinterpret_cast<PxVec3*>(PxAlloca(nbVerts * sizeof(PxVec3))),			\
+					idtScaling ? NULL : reinterpret_cast<PxU8*>(PxAlloca(nbVerts * sizeof(PxU8))),				\
+					idtScaling, srcVerts, srcIndices, nbVerts, scaling);
+
+	PX_PHYSX_COMMON_API bool getConvexData(const Gu::GeometryUnion& shape, Cm::FastVertex2ShapeScaling& scaling, PxBounds3& bounds, PolygonalData& polyData);
+
+	struct ConvexEdge
+	{
+		PxU8	vref0;
+		PxU8	vref1;
+		PxVec3	normal;	// warning: non-unit vector!
+	};
+
+	PX_PHYSX_COMMON_API PxU32 findUniqueConvexEdges(PxU32 maxNbEdges, ConvexEdge* PX_RESTRICT edges, PxU32 numPolygons, const Gu::HullPolygonData* PX_RESTRICT polygons, const PxU8* PX_RESTRICT vertexData);
+}
+}
+
+#endif

physx/source/geomutils/src/convex/GuConvexMesh.cpp

@@ -0,0 +1,420 @@
+//
+// 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-2021 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/PxMeshScale.h"
+#include "PxVisualizationParameter.h"
+
+#include "PsIntrinsics.h"
+#include "PsMathUtils.h"
+#include "PsAllocator.h"
+#include "PsFoundation.h"
+#include "GuConvexMesh.h"
+#include "GuTriangle32.h"
+#include "GuBigConvexData2.h"
+#include "GuSerialize.h"
+#include "GuMeshFactory.h"
+#include "CmPhysXCommon.h"
+#include "CmRenderOutput.h"
+#include "CmUtils.h"
+
+using namespace physx;
+using namespace Gu;
+
+// PX_SERIALIZATION
+#include "PsIntrinsics.h"
+//~PX_SERIALIZATION
+
+bool Gu::ConvexMesh::getPolygonData(PxU32 i, PxHullPolygon& data) const
+{
+	if(i>=mHullData.mNbPolygons)
+		return false;
+
+	const HullPolygonData& poly = mHullData.mPolygons[i];
+	data.mPlane[0]	= poly.mPlane.n.x;
+	data.mPlane[1]	= poly.mPlane.n.y;
+	data.mPlane[2]	= poly.mPlane.n.z;
+	data.mPlane[3]	= poly.mPlane.d;
+	data.mNbVerts	= poly.mNbVerts;
+	data.mIndexBase	= poly.mVRef8;
+	return true;
+}
+
+/// ======================================
+
+static void initConvexHullData(Gu::ConvexHullData& data)
+{
+	data.mAABB.setEmpty();
+	data.mCenterOfMass = PxVec3(0);
+	data.mNbEdges = PxBitAndWord();
+	data.mNbHullVertices = 0;
+	data.mNbPolygons = 0;
+	data.mPolygons = NULL;
+	data.mBigConvexRawData = NULL;
+	data.mInternal.mRadius = 0.0f;
+	data.mInternal.mExtents[0] = data.mInternal.mExtents[1] = data.mInternal.mExtents[2] = 0.0f;
+}
+
+Gu::ConvexMesh::ConvexMesh()
+: PxConvexMesh(PxConcreteType::eCONVEX_MESH, PxBaseFlag::eOWNS_MEMORY | PxBaseFlag::eIS_RELEASABLE)
+, mNb				(0)
+, mBigConvexData	(NULL)
+, mMass				(0)
+, mInertia			(PxMat33(PxIdentity))
+{
+	initConvexHullData(mHullData);
+}
+
+Gu::ConvexMesh::ConvexMesh(GuMeshFactory& factory, ConvexHullInitData& data)
+: PxConvexMesh(PxConcreteType::eCONVEX_MESH, PxBaseFlag::eOWNS_MEMORY | PxBaseFlag::eIS_RELEASABLE)
+, mNb(data.mNb)
+, mBigConvexData(data.mBigConvexData)
+, mMass(data.mMass)
+, mInertia(data.mInertia)
+, mMeshFactory(&factory)
+{
+	mHullData = data.mHullData;	
+}
+
+Gu::ConvexMesh::~ConvexMesh()
+{
+// PX_SERIALIZATION
+	if(getBaseFlags()&PxBaseFlag::eOWNS_MEMORY)
+//~PX_SERIALIZATION
+	{
+		PX_DELETE_POD(mHullData.mPolygons);
+		PX_DELETE_AND_RESET(mBigConvexData);
+	}
+}
+
+bool Gu::ConvexMesh::isGpuCompatible() const
+{
+	return mHullData.mNbHullVertices <= 64 &&
+		mHullData.mPolygons[0].mNbVerts <= 32 &&
+		mHullData.mNbEdges.isBitSet();
+}
+
+// PX_SERIALIZATION
+
+void Gu::ConvexMesh::exportExtraData(PxSerializationContext& context)
+{
+	context.alignData(PX_SERIAL_ALIGN);
+	const PxU32 bufferSize = computeBufferSize(mHullData, getNb());
+	context.writeData(mHullData.mPolygons, bufferSize);
+
+	if(mBigConvexData)
+	{
+		context.alignData(PX_SERIAL_ALIGN);
+		context.writeData(mBigConvexData, sizeof(BigConvexData));
+
+		mBigConvexData->exportExtraData(context);
+	}
+}
+
+void Gu::ConvexMesh::importExtraData(PxDeserializationContext& context)
+{
+	const PxU32 bufferSize = computeBufferSize(mHullData, getNb());
+	mHullData.mPolygons = reinterpret_cast<Gu::HullPolygonData*>(context.readExtraData<PxU8, PX_SERIAL_ALIGN>(bufferSize));
+
+	if(mBigConvexData)
+	{
+		mBigConvexData = context.readExtraData<BigConvexData, PX_SERIAL_ALIGN>();
+		new(mBigConvexData)BigConvexData(PxEmpty);
+		mBigConvexData->importExtraData(context);
+		mHullData.mBigConvexRawData = &mBigConvexData->mData;
+	}
+}
+
+Gu::ConvexMesh* Gu::ConvexMesh::createObject(PxU8*& address, PxDeserializationContext& context)
+{
+	ConvexMesh* obj = new (address) ConvexMesh(PxBaseFlag::eIS_RELEASABLE);
+	address += sizeof(ConvexMesh);	
+	obj->importExtraData(context);
+	obj->resolveReferences(context);
+	return obj;
+}
+
+static bool convexHullLoad(Gu::ConvexHullData& data, PxInputStream& stream, PxBitAndDword& bufferSize)
+{
+	PxU32 version;
+	bool Mismatch;
+	if(!ReadHeader('C', 'L', 'H', 'L', version, Mismatch, stream))
+		return false;
+
+	if(version<=8)
+	{
+		if(!ReadHeader('C', 'V', 'H', 'L', version, Mismatch, stream))
+			return false;
+	}
+
+	PxU32 Nb;
+
+	// Import figures
+	{
+		PxU32 tmp[4];
+		ReadDwordBuffer(tmp, 4, Mismatch, stream);
+		data.mNbHullVertices	= Ps::to8(tmp[0]);
+		data.mNbEdges			= Ps::to16(tmp[1]);
+		data.mNbPolygons		= Ps::to8(tmp[2]);
+		Nb						= tmp[3];
+	}
+
+	//AM: In practice the old aligner approach wastes 20 bytes and there is no reason to 20 byte align this data.
+	//I changed the code to just 4 align for the time being.  
+	//On consoles if anything we will need to make this stuff 16 byte align vectors to have any sense, which will have to be done by padding data structures.
+	PX_ASSERT(sizeof(Gu::HullPolygonData) % sizeof(PxReal) == 0);	//otherwise please pad it.
+	PX_ASSERT(sizeof(PxVec3) % sizeof(PxReal) == 0);
+
+	PxU32 bytesNeeded = computeBufferSize(data, Nb);
+
+	PX_FREE(data.mPolygons);	// Load() can be called for an existing convex mesh. In that case we need to free
+	// the memory first.
+
+	bufferSize = Nb;
+	void* mDataMemory = PX_ALLOC(bytesNeeded, "ConvexHullData data");
+
+	PxU8* address = reinterpret_cast<PxU8*>(mDataMemory);
+
+	data.mPolygons				= reinterpret_cast<Gu::HullPolygonData*>(address);	address += sizeof(Gu::HullPolygonData) * data.mNbPolygons;
+	PxVec3* mDataHullVertices	= reinterpret_cast<PxVec3*>(address);				address += sizeof(PxVec3) * data.mNbHullVertices;
+	PxU8* mDataFacesByEdges8	= address;											address += sizeof(PxU8) * data.mNbEdges * 2;
+	PxU8* mDataFacesByVertices8 = address;											address += sizeof(PxU8) * data.mNbHullVertices * 3;
+	PxU16* mEdges				= reinterpret_cast<PxU16*>(address);				address += data.mNbEdges.isBitSet() ? (sizeof(PxU16) * data.mNbEdges * 2) : 0;
+	PxU8* mDataVertexData8		= address;											address += sizeof(PxU8) * Nb;	// PT: leave that one last, so that we don't need to serialize "Nb"
+
+	PX_ASSERT(!(size_t(mDataHullVertices) % sizeof(PxReal)));
+	PX_ASSERT(!(size_t(data.mPolygons) % sizeof(PxReal)));
+	PX_ASSERT(size_t(address)<=size_t(mDataMemory)+bytesNeeded);
+
+	// Import vertices
+	readFloatBuffer(&mDataHullVertices->x, PxU32(3*data.mNbHullVertices), Mismatch, stream);
+
+	if(version<=6)
+	{
+		PxU16 useUnquantizedNormals = readWord(Mismatch, stream);
+		PX_UNUSED(useUnquantizedNormals);
+	}
+
+	// Import polygons
+	stream.read(data.mPolygons, data.mNbPolygons*sizeof(Gu::HullPolygonData));
+
+	if(Mismatch)
+	{
+		for(PxU32 i=0;i<data.mNbPolygons;i++)
+			flipData(data.mPolygons[i]);
+	}
+
+	stream.read(mDataVertexData8, Nb);
+	stream.read(mDataFacesByEdges8, PxU32(data.mNbEdges*2));
+	if(version <= 5)
+	{
+		//KS - we need to compute faces-by-vertices here
+
+		bool noPlaneShift = false;
+		for(PxU32 i=0; i< data.mNbHullVertices; ++i)
+		{
+			PxU32 count = 0;
+			PxU8 inds[3];
+			for(PxU32 j=0; j<data.mNbPolygons; ++j)
+			{
+				Gu::HullPolygonData& polygon = data.mPolygons[j];
+				for(PxU32 k=0; k< polygon.mNbVerts; ++k)
+				{
+					PxU8 index = mDataVertexData8[polygon.mVRef8 + k];
+					if(i == index)
+					{
+						//Found a polygon
+						inds[count++] = Ps::to8(j);
+						break;
+					}
+				}
+				if(count == 3)
+					break;
+			}
+			//We have 3 indices
+			//PX_ASSERT(count == 3);
+			//Do something here
+			if(count == 3)
+			{
+				mDataFacesByVertices8[i*3+0] = inds[0];
+				mDataFacesByVertices8[i*3+1] = inds[1];
+				mDataFacesByVertices8[i*3+2] = inds[2];
+			}
+			else
+			{
+				noPlaneShift = true;
+				break;
+			}
+		}
+
+
+		if(noPlaneShift)
+		{
+			for(PxU32 a = 0; a < data.mNbHullVertices; ++a)
+			{
+				mDataFacesByVertices8[a*3] = 0xFF;
+				mDataFacesByVertices8[a*3+1] = 0xFF;
+				mDataFacesByVertices8[a*3+2] = 0xFF;
+			}
+		}
+
+	}
+	else
+		stream.read(mDataFacesByVertices8, PxU32(data.mNbHullVertices * 3)); 
+
+	if (data.mNbEdges.isBitSet())
+	{
+		if (version <= 7)
+		{
+			for (PxU32 a = 0; a < PxU32(data.mNbEdges * 2); ++a)
+			{
+				mEdges[a] = 0xFFFF;
+			}
+		}
+		else
+		{
+			readWordBuffer(mEdges, PxU32(data.mNbEdges * 2), Mismatch, stream);
+		}
+	}
+	return true;
+}
+
+bool Gu::ConvexMesh::load(PxInputStream& stream)
+{
+	// Import header
+	PxU32 version;
+	bool mismatch;
+	if(!readHeader('C', 'V', 'X', 'M', version, mismatch, stream))
+		return false;
+
+	// Check if old (incompatible) mesh format is loaded
+	if (version < PX_CONVEX_VERSION)
+	{
+		Ps::getFoundation().error(PxErrorCode::eINTERNAL_ERROR, __FILE__, __LINE__, "Loading convex mesh failed: "
+			"Deprecated mesh cooking format.");
+		return false;
+	}
+
+	// Import serialization flags
+	PxU32 serialFlags	= readDword(mismatch, stream);
+	PX_UNUSED(serialFlags);
+
+	if(!convexHullLoad(mHullData, stream, mNb))
+		return false;
+
+	// Import local bounds
+	float tmp[8];
+	readFloatBuffer(tmp, 8, mismatch, stream);
+//	geomEpsilon				= tmp[0];
+	mHullData.mAABB = PxBounds3(PxVec3(tmp[1], tmp[2], tmp[3]), PxVec3(tmp[4],tmp[5],tmp[6]));
+
+	// Import mass info
+	mMass = tmp[7];
+	if(mMass!=-1.0f)
+	{
+		readFloatBuffer(&mInertia(0,0), 9, mismatch, stream);
+		readFloatBuffer(&mHullData.mCenterOfMass.x, 3, mismatch, stream);
+	}
+
+	// Import gaussmaps
+	PxF32 gaussMapFlag = readFloat(mismatch, stream);
+	if(gaussMapFlag != -1.0f)
+	{
+		PX_ASSERT(gaussMapFlag == 1.0f);	//otherwise file is corrupt
+
+		PX_DELETE_AND_RESET(mBigConvexData);
+		PX_NEW_SERIALIZED(mBigConvexData,BigConvexData);	
+      
+		if(mBigConvexData)	
+		{
+			mBigConvexData->Load(stream);
+			mHullData.mBigConvexRawData = &mBigConvexData->mData;
+		}
+	}
+	
+/*
+	printf("\n\n");
+	printf("COM: %f %f %f\n", massInfo.centerOfMass.x, massInfo.centerOfMass.y, massInfo.centerOfMass.z);
+	printf("BND: %f %f %f\n", mHullData.aabb.getCenter().x, mHullData.aabb.getCenter().y, mHullData.aabb.getCenter().z);
+	printf("CNT: %f %f %f\n", mHullData.mCenterxx.x, mHullData.mCenterxx.y, mHullData.mCenterxx.z);
+	printf("COM-BND: %f BND-CNT: %f, CNT-COM: %f\n", (massInfo.centerOfMass - mHullData.aabb.getCenter()).magnitude(), (mHullData.aabb.getCenter() - mHullData.mCenterxx).magnitude(), (mHullData.mCenterxx - massInfo.centerOfMass).magnitude());
+*/
+
+// TEST_INTERNAL_OBJECTS
+	readFloatBuffer(&mHullData.mInternal.mRadius, 4, mismatch, stream);
+
+	PX_ASSERT(PxVec3(mHullData.mInternal.mExtents[0], mHullData.mInternal.mExtents[1], mHullData.mInternal.mExtents[2]).isFinite());
+	PX_ASSERT(mHullData.mInternal.mExtents[0] != 0.0f);
+	PX_ASSERT(mHullData.mInternal.mExtents[1] != 0.0f);
+	PX_ASSERT(mHullData.mInternal.mExtents[2] != 0.0f);
+//~TEST_INTERNAL_OBJECTS
+	return true;
+}
+
+void Gu::ConvexMesh::release()
+{
+	decRefCount();
+}
+
+void Gu::ConvexMesh::onRefCountZero()
+{
+	if ((!getBufferSize()) || mMeshFactory->removeConvexMesh(*this))  // when the mesh failed to load properly, it will not have been added to the convex array
+	{
+		GuMeshFactory* mf = mMeshFactory;
+		Cm::deletePxBase(this);
+		mf->notifyFactoryListener(this, PxConcreteType::eCONVEX_MESH);
+		return;
+	}
+
+	// PT: if we reach this point, we didn't find the mesh in the Physics object => don't delete!
+	// This prevents deleting the object twice.
+	Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, "Gu::ConvexMesh::release: double deletion detected!");
+}
+
+void Gu::ConvexMesh::acquireReference()
+{
+	incRefCount();
+}
+
+PxU32 Gu::ConvexMesh::getReferenceCount() const
+{
+	return getRefCount();
+}
+
+void Gu::ConvexMesh::getMassInformation(PxReal& mass, PxMat33& localInertia, PxVec3& localCenterOfMass) const
+{
+	mass = Gu::ConvexMesh::getMass();
+	localInertia = Gu::ConvexMesh::getInertia();
+	localCenterOfMass = Gu::ConvexMesh::getHull().mCenterOfMass;
+}
+
+PxBounds3 Gu::ConvexMesh::getLocalBounds() const
+{
+	PX_ASSERT(mHullData.mAABB.isValid());
+	return PxBounds3::centerExtents(mHullData.mAABB.mCenter, mHullData.mAABB.mExtents);
+}

physx/source/geomutils/src/convex/GuConvexMesh.h

@@ -0,0 +1,187 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_COLLISION_CONVEXMESH_H
+#define GU_COLLISION_CONVEXMESH_H
+
+#include "foundation/PxBitAndData.h"
+#include "common/PxMetaData.h"
+#include "geometry/PxConvexMesh.h"
+#include "PsUserAllocated.h"
+#include "CmPhysXCommon.h"
+#include "CmRefCountable.h"
+#include "CmRenderOutput.h"
+#include "GuConvexMeshData.h"
+
+
+namespace physx
+{
+
+class BigConvexData;
+class GuMeshFactory;
+class PxMeshScale;
+
+
+namespace Gu
+{
+	struct HullPolygonData;
+
+	PX_INLINE PxU32 computeBufferSize(const Gu::ConvexHullData& data, PxU32 nb)
+	{
+		PxU32 bytesNeeded = sizeof(Gu::HullPolygonData) * data.mNbPolygons;
+		bytesNeeded += sizeof(PxVec3) * data.mNbHullVertices;
+		bytesNeeded += sizeof(PxU8) * data.mNbEdges * 2;		// mFacesByEdges8
+		bytesNeeded += sizeof(PxU8) * data.mNbHullVertices * 3; // mFacesByVertices8;
+		bytesNeeded += data.mNbEdges.isBitSet() ? (sizeof(PxU16) * data.mNbEdges * 2) : 0;		 // mEdges;
+		bytesNeeded += sizeof(PxU8) * nb;						// mVertexData8
+		
+		//4 align the whole thing!
+		const PxU32 mod = bytesNeeded % sizeof(PxReal);
+		if (mod)
+			bytesNeeded += sizeof(PxReal) - mod;
+		return bytesNeeded;
+	}
+
+	struct ConvexHullInitData
+	{
+		ConvexHullData	mHullData;
+		PxU32			mNb;
+		PxReal			mMass;		
+		PxMat33			mInertia;
+		BigConvexData*	mBigConvexData;
+	};
+
+
+	// 0: includes raycast map
+	// 1: discarded raycast map
+	// 2: support map not always there
+	// 3: support stackless trees for non-recursive collision queries
+	// 4: no more opcode model
+	// 5: valencies table and gauss map combined, only exported over a vertex count treshold that depends on the platform cooked for.
+	// 6: removed support for edgeData16.
+	// 7: removed support for edge8Data.
+	// 8: removed support for triangles.
+
+	// 9: removed local sphere.
+	//10: removed geometric center.
+	//11: removed mFlags, and mERef16 from Poly; nbVerts is just a byte.
+	//12: removed explicit minimum, maximum from Poly
+	//13: internal objects
+    #define  PX_CONVEX_VERSION 13
+  
+	class ConvexMesh : public PxConvexMesh, public Ps::UserAllocated, public Cm::RefCountable
+	{
+	//= ATTENTION! =====================================================================================
+	// Changing the data layout of this class breaks the binary serialization format.  See comments for 
+	// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
+	// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
+	// accordingly.
+	//==================================================================================================
+	public:
+	// PX_SERIALIZATION
+							 						ConvexMesh(PxBaseFlags baseFlags) : PxConvexMesh(baseFlags), Cm::RefCountable(PxEmpty), mHullData(PxEmpty), mNb(PxEmpty) 
+													{
+														mNb.setBit();
+													}									
+
+		PX_PHYSX_COMMON_API			void			preExportDataReset() { Cm::RefCountable::preExportDataReset(); }
+		PX_PHYSX_COMMON_API virtual	void			exportExtraData(PxSerializationContext& stream);
+		PX_PHYSX_COMMON_API			void			importExtraData(PxDeserializationContext& context);
+		PX_PHYSX_COMMON_API virtual	void			onRefCountZero();
+		PX_PHYSX_COMMON_API static	ConvexMesh*		createObject(PxU8*& address, PxDeserializationContext& context);
+		PX_PHYSX_COMMON_API static	void			getBinaryMetaData(PxOutputStream& stream);
+									void			resolveReferences(PxDeserializationContext&)				{}
+		virtual						void			requiresObjects(PxProcessPxBaseCallback&){}
+	//~PX_SERIALIZATION
+		PX_PHYSX_COMMON_API 						ConvexMesh();
+
+		PX_PHYSX_COMMON_API							ConvexMesh(GuMeshFactory& factory, ConvexHullInitData& data);
+
+		PX_PHYSX_COMMON_API bool					load(PxInputStream& stream);
+
+		// PxConvexMesh										
+		PX_PHYSX_COMMON_API virtual	void			release();
+		PX_PHYSX_COMMON_API virtual	PxU32			getNbVertices()									const	{ return mHullData.mNbHullVertices;		}
+		PX_PHYSX_COMMON_API virtual	const PxVec3*	getVertices()									const	{ return mHullData.getHullVertices();	}
+		PX_PHYSX_COMMON_API virtual	const PxU8*		getIndexBuffer()								const	{ return mHullData.getVertexData8();	}
+		PX_PHYSX_COMMON_API virtual	PxU32			getNbPolygons()									const	{ return mHullData.mNbPolygons;			}
+		PX_PHYSX_COMMON_API virtual	bool			getPolygonData(PxU32 i, PxHullPolygon& data)	const;
+		PX_PHYSX_COMMON_API virtual bool			isGpuCompatible()								const;						
+		PX_PHYSX_COMMON_API virtual	PxU32			getReferenceCount()								const;
+		PX_PHYSX_COMMON_API virtual	void			acquireReference();
+
+		PX_PHYSX_COMMON_API virtual	void			getMassInformation(PxReal& mass, PxMat33& localInertia, PxVec3& localCenterOfMass)	const;
+		PX_PHYSX_COMMON_API virtual	PxBounds3		getLocalBounds()								const;
+		//~PxConvexMesh
+
+		PX_FORCE_INLINE	PxU32					getNbVerts()										const	{ return mHullData.mNbHullVertices;		}
+		PX_FORCE_INLINE	const PxVec3*			getVerts()											const	{ return mHullData.getHullVertices();	}
+		PX_FORCE_INLINE	PxU32					getNbPolygonsFast()									const	{ return mHullData.mNbPolygons;			}
+		PX_FORCE_INLINE	const HullPolygonData&	getPolygon(PxU32 i)									const	{ return mHullData.mPolygons[i];		}
+		PX_FORCE_INLINE	const HullPolygonData*	getPolygons()										const	{ return mHullData.mPolygons;			}
+		PX_FORCE_INLINE	PxU32					getNbEdges()										const	{ return mHullData.mNbEdges;			}
+
+		PX_FORCE_INLINE	const ConvexHullData&	getHull()											const	{ return mHullData;						}
+		PX_FORCE_INLINE	ConvexHullData&			getHull()													{ return mHullData;						}
+		PX_FORCE_INLINE	const CenterExtents&	getLocalBoundsFast()								const	{ return mHullData.mAABB;				}
+		PX_FORCE_INLINE	PxReal					getMass()											const	{ return mMass;							}
+		PX_FORCE_INLINE void					setMass(PxReal mass)										{ mMass = mass;							}		
+		PX_FORCE_INLINE	const PxMat33&			getInertia()										const	{ return mInertia;						}
+		PX_FORCE_INLINE void					setInertia(const PxMat33& inertia)							{ mInertia = inertia;					}
+
+		PX_FORCE_INLINE BigConvexData*			getBigConvexData()									const	{ return mBigConvexData;				}
+		PX_FORCE_INLINE void					setBigConvexData(BigConvexData* bcd)						{ mBigConvexData = bcd;					}
+
+		PX_FORCE_INLINE	PxU32					getBufferSize()										const	{ return computeBufferSize(mHullData, getNb());	}
+
+		PX_PHYSX_COMMON_API virtual				~ConvexMesh();
+
+		PX_FORCE_INLINE	void					setMeshFactory(GuMeshFactory* f)							{ mMeshFactory = f;						}
+
+		PX_FORCE_INLINE void					setNb(PxU32 nb)												{ mNb = nb; }
+
+	protected:
+						ConvexHullData			mHullData;
+						PxBitAndDword			mNb;	// ### PT: added for serialization. Try to remove later?
+
+						BigConvexData*			mBigConvexData;		//!< optional, only for large meshes! PT: redundant with ptr in chull data? Could also be end of other buffer
+						PxReal					mMass;				//this is mass assuming a unit density that can be scaled by instances!
+						PxMat33					mInertia;			//in local space of mesh!
+private:
+						GuMeshFactory*			mMeshFactory;	// PT: changed to pointer for serialization
+
+		PX_FORCE_INLINE	PxU32					getNb()												const	{ return mNb;						}
+		PX_FORCE_INLINE	PxU32					ownsMemory()										const	{ return PxU32(!mNb.isBitSet());	}
+	};
+
+} // namespace Gu
+
+}
+
+#endif

physx/source/geomutils/src/convex/GuConvexMeshData.h

@@ -0,0 +1,216 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_CONVEX_MESH_DATA_H
+#define GU_CONVEX_MESH_DATA_H
+
+#include "foundation/PxPlane.h"
+#include "foundation/PxBitAndData.h"
+
+#include "PsIntrinsics.h"
+#include "GuSerialize.h"
+#include "GuCenterExtents.h"
+
+// Data definition
+
+namespace physx
+{
+namespace Gu
+{
+	struct BigConvexRawData;
+
+	struct HullPolygonData
+	{
+	//= ATTENTION! =====================================================================================
+	// Changing the data layout of this class breaks the binary serialization format.  See comments for 
+	// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
+	// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
+	// accordingly.
+	//==================================================================================================
+
+		// PT: this structure won't be allocated with PX_NEW because polygons aren't allocated alone (with a dedicated alloc).
+		// Instead they are part of a unique allocation/buffer containing all data for the ConvexHullData class (polygons, followed by
+		// hull vertices, edge data, etc). As a result, ctors for embedded classes like PxPlane won't be called.
+
+		PxPlane	mPlane;			//!< Plane equation for this polygon	//Could drop 4th elem as it can be computed from any vertex as: d = - p.dot(n);
+		PxU16	mVRef8;			//!< Offset of vertex references in hull vertex data (CS: can we assume indices are tightly packed and offsets are ascending?? DrawObjects makes and uses this assumption)
+		PxU8	mNbVerts;		//!< Number of vertices/edges in the polygon
+		PxU8	mMinIndex;		//!< Index of the polygon vertex that has minimal projection along this plane's normal.
+
+		PX_FORCE_INLINE	PxReal getMin(const PxVec3* PX_RESTRICT hullVertices) const	//minimum of projection of the hull along this plane normal
+		{ 
+			return mPlane.n.dot(hullVertices[mMinIndex]);
+		}
+
+		PX_FORCE_INLINE	PxReal getMax() const		{ return -mPlane.d; }	//maximum of projection of the hull along this plane normal
+	};
+
+	PX_FORCE_INLINE void flipData(Gu::HullPolygonData& data)
+	{
+		flip(data.mPlane.n.x);
+		flip(data.mPlane.n.y);
+		flip(data.mPlane.n.z);
+		flip(data.mPlane.d);
+		flip(data.mVRef8);
+	}
+	// PT: if this one breaks, please make sure the 'flipData' function is properly updated.
+	PX_COMPILE_TIME_ASSERT(sizeof(Gu::HullPolygonData) == 20);
+
+// TEST_INTERNAL_OBJECTS
+	struct InternalObjectsData
+	{
+	//= ATTENTION! =====================================================================================
+	// Changing the data layout of this class breaks the binary serialization format.  See comments for 
+	// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
+	// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
+	// accordingly.
+	//==================================================================================================
+		PxReal	mRadius;
+		PxReal	mExtents[3];
+
+		PX_FORCE_INLINE	void reset()
+		{
+			mRadius = 0.0f;
+			mExtents[0] = 0.0f;
+			mExtents[1] = 0.0f;
+			mExtents[2] = 0.0f;
+		}
+	};
+	PX_COMPILE_TIME_ASSERT(sizeof(Gu::InternalObjectsData) == 16);
+//~TEST_INTERNAL_OBJECTS
+
+	struct ConvexHullData
+	{
+	//= ATTENTION! =====================================================================================
+	// Changing the data layout of this class breaks the binary serialization format.  See comments for 
+	// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
+	// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
+	// accordingly.
+	//==================================================================================================
+
+		// PT: WARNING: bounds must be followed by at least 32bits of data for safe SIMD loading
+		CenterExtents		mAABB;				//!< bounds TODO: compute this on the fly from first 6 vertices in the vertex array.  We'll of course need to sort the most extreme ones to the front.
+		PxVec3				mCenterOfMass;		//in local space of mesh!
+
+		// PT: WARNING: mNbHullVertices *must* appear before mBigConvexRawData for ConvX to be able to do "big raw data" surgery
+	
+		PxBitAndWord		mNbEdges;			//!<the highest bit indicate whether we have grb data, the other 15 bits indicate the number of edges in this convex hull
+
+		PxU8				mNbHullVertices;	//!< Number of vertices in the convex hull
+		PxU8				mNbPolygons;		//!< Number of planar polygons composing the hull
+
+		HullPolygonData*	mPolygons;			//!< Array of mNbPolygons structures
+		BigConvexRawData*	mBigConvexRawData;	//!< Hill climbing data, only for large convexes! else NULL.
+
+// TEST_INTERNAL_OBJECTS
+		InternalObjectsData	mInternal;
+//~TEST_INTERNAL_OBJECTS
+
+		PX_FORCE_INLINE ConvexHullData(const PxEMPTY) : mNbEdges(PxEmpty)
+		{
+		}
+
+		PX_FORCE_INLINE ConvexHullData()
+		{
+		}
+
+		PX_FORCE_INLINE	const CenterExtentsPadded& getPaddedBounds() const
+		{
+			// PT: see compile-time assert at the end of file
+			return static_cast<const CenterExtentsPadded&>(mAABB);
+		}
+
+		PX_FORCE_INLINE	const PxVec3* getHullVertices()	const	//!< Convex hull vertices
+		{
+			const char* tmp = reinterpret_cast<const char*>(mPolygons);
+			tmp += sizeof(Gu::HullPolygonData) * mNbPolygons;
+			return reinterpret_cast<const PxVec3*>(tmp);
+		}
+
+		PX_FORCE_INLINE	const PxU8* getFacesByEdges8()	const	//!< for each edge, gives 2 adjacent polygons; used by convex-convex code to come up with all the convex' edge normals.  
+		{
+			const char* tmp = reinterpret_cast<const char*>(mPolygons);
+			tmp += sizeof(Gu::HullPolygonData) * mNbPolygons;
+			tmp += sizeof(PxVec3) * mNbHullVertices; 
+			return reinterpret_cast<const PxU8*>(tmp);
+		}
+
+		PX_FORCE_INLINE	const PxU8* getFacesByVertices8()	const	//!< for each edge, gives 2 adjacent polygons; used by convex-convex code to come up with all the convex' edge normals.  
+		{
+			const char* tmp = reinterpret_cast<const char*>(mPolygons);
+			tmp += sizeof(Gu::HullPolygonData) * mNbPolygons;
+			tmp += sizeof(PxVec3) * mNbHullVertices;
+			tmp += sizeof(PxU8) * mNbEdges * 2;
+			return reinterpret_cast<const PxU8*>(tmp);
+		}
+
+		//If we don't build the convex hull with grb data, we will return NULL pointer
+		PX_FORCE_INLINE	const PxU16*	getVerticesByEdges16()	const	//!< Vertex indices indexed by unique edges
+		{
+			if (mNbEdges.isBitSet())
+			{
+				const char* tmp = reinterpret_cast<const char*>(mPolygons);
+				tmp += sizeof(Gu::HullPolygonData) * mNbPolygons;
+				tmp += sizeof(PxVec3) * mNbHullVertices;
+				tmp += sizeof(PxU8) * mNbEdges * 2;
+				tmp += sizeof(PxU8) * mNbHullVertices * 3;
+				return reinterpret_cast<const PxU16*>(tmp);
+			}
+			return NULL;
+		}
+
+		PX_FORCE_INLINE	const PxU8*	getVertexData8()	const	//!< Vertex indices indexed by hull polygons
+		{
+			const char* tmp = reinterpret_cast<const char*>(mPolygons);
+			tmp += sizeof(Gu::HullPolygonData) * mNbPolygons;
+			tmp += sizeof(PxVec3) * mNbHullVertices;
+			tmp += sizeof(PxU8) * mNbEdges * 2;
+			tmp += sizeof(PxU8) * mNbHullVertices * 3;
+			if (mNbEdges.isBitSet())
+				tmp += sizeof(PxU16) * mNbEdges * 2;
+			return reinterpret_cast<const PxU8*>(tmp);
+		}
+
+	};
+	#if PX_P64_FAMILY
+	PX_COMPILE_TIME_ASSERT(sizeof(Gu::ConvexHullData) == 72);
+	#else
+	PX_COMPILE_TIME_ASSERT(sizeof(Gu::ConvexHullData) == 64);
+	#endif
+
+	// PT: 'getPaddedBounds()' is only safe if we make sure the bounds member is followed by at least 32bits of data
+	PX_COMPILE_TIME_ASSERT(PX_OFFSET_OF(Gu::ConvexHullData, mCenterOfMass)>=PX_OFFSET_OF(Gu::ConvexHullData, mAABB)+4);
+
+} // namespace Gu
+
+}
+
+//#pragma PX_POP_PACK
+
+#endif

physx/source/geomutils/src/convex/GuConvexSupportTable.cpp

@@ -0,0 +1,44 @@
+//
+// 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-2021 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 "GuVecBox.h"
+
+namespace physx
+{
+	const Ps::aos::BoolV boxVertexTable[8] = {
+										Ps::aos::BFFFF(),//---
+										Ps::aos::BTFFF(),//+--
+										Ps::aos::BFTFF(),//-+-
+										Ps::aos::BTTFF(),//++-
+										Ps::aos::BFFTF(),//--+
+										Ps::aos::BTFTF(),//+-+
+										Ps::aos::BFTTF(),//-++
+										Ps::aos::BTTTF(),//+++
+									}; 
+}

physx/source/geomutils/src/convex/GuConvexSupportTable.h

@@ -0,0 +1,117 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_CONVEX_SUPPORT_TABLE_H
+#define GU_CONVEX_SUPPORT_TABLE_H
+
+#include "common/PxPhysXCommonConfig.h"
+#include "GuVecConvex.h"
+#include "PsVecTransform.h"
+
+namespace physx
+{
+namespace Gu  
+{
+
+	class TriangleV; 
+	class CapsuleV;
+	class BoxV;
+	class ConvexHullV;
+	class ConvexHullNoScaleV;
+
+#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
+	class SupportLocal
+	{
+	public:		
+		Ps::aos::Vec3V shapeSpaceCenterOfMass;
+		const Ps::aos::PsTransformV& transform;
+		const Ps::aos::Mat33V& vertex2Shape;
+		const Ps::aos::Mat33V& shape2Vertex;
+		const bool isIdentityScale;	
+
+		SupportLocal(const Ps::aos::PsTransformV& _transform, const Ps::aos::Mat33V& _vertex2Shape, const Ps::aos::Mat33V& _shape2Vertex, const bool _isIdentityScale = true): transform(_transform), 
+			vertex2Shape(_vertex2Shape), shape2Vertex(_shape2Vertex), isIdentityScale(_isIdentityScale)
+		{
+		}
+
+		PX_FORCE_INLINE void setShapeSpaceCenterofMass(const Ps::aos::Vec3VArg _shapeSpaceCenterOfMass)
+		{
+			shapeSpaceCenterOfMass = _shapeSpaceCenterOfMass;
+		}
+        virtual ~SupportLocal() {}
+		virtual Ps::aos::Vec3V doSupport(const Ps::aos::Vec3VArg dir) const = 0;
+		virtual void doSupport(const Ps::aos::Vec3VArg dir, Ps::aos::FloatV& min, Ps::aos::FloatV& max) const = 0;
+		virtual void populateVerts(const PxU8* inds, PxU32 numInds, const PxVec3* originalVerts, Ps::aos::Vec3V* verts)const = 0;
+	
+	protected:
+		SupportLocal& operator=(const SupportLocal&);
+	};
+#if PX_VC 
+     #pragma warning(pop) 
+#endif
+
+	template <typename Convex>
+	class SupportLocalImpl : public SupportLocal
+	{
+		
+	public:
+		const Convex& conv;
+		SupportLocalImpl(const Convex& _conv, const Ps::aos::PsTransformV& _transform, const Ps::aos::Mat33V& _vertex2Shape, const Ps::aos::Mat33V& _shape2Vertex, const bool _isIdentityScale = true) : 
+		SupportLocal(_transform, _vertex2Shape, _shape2Vertex, _isIdentityScale), conv(_conv)
+		{
+		}
+
+		Ps::aos::Vec3V doSupport(const Ps::aos::Vec3VArg dir) const
+		{
+			//return conv.supportVertsLocal(dir);
+			return conv.supportLocal(dir);
+		}
+
+		void doSupport(const Ps::aos::Vec3VArg dir, Ps::aos::FloatV& min, Ps::aos::FloatV& max) const
+		{
+			return conv.supportLocal(dir, min, max);
+		}
+
+		void populateVerts(const PxU8* inds, PxU32 numInds, const PxVec3* originalVerts, Ps::aos::Vec3V* verts) const 
+		{
+			conv.populateVerts(inds, numInds, originalVerts, verts);
+		}
+
+	protected:
+		SupportLocalImpl& operator=(const SupportLocalImpl&);
+
+	};
+}
+
+}
+
+#endif

physx/source/geomutils/src/convex/GuConvexUtilsInternal.cpp

@@ -0,0 +1,77 @@
+//
+// 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-2021 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 "foundation/PxBounds3.h"
+#include "geometry/PxConvexMeshGeometry.h"
+#include "GuConvexUtilsInternal.h"
+#include "GuBoxConversion.h"
+#include "GuConvexMesh.h"
+#include "CmScaling.h"
+
+using namespace physx;
+using namespace Gu;
+
+void Gu::computeHullOBB(Box& hullOBB, const PxBounds3& hullAABB, float offset, 
+						const Cm::Matrix34& convexPose, 
+						const Cm::Matrix34& meshPose, const Cm::FastVertex2ShapeScaling& meshScaling, bool idtScaleMesh)
+{
+	// transform bounds = mesh space
+	Cm::Matrix34 m0to1 = meshPose.transformTranspose(convexPose);
+
+	hullOBB.extents = hullAABB.getExtents() + PxVec3(offset);
+	hullOBB.center = m0to1.transform(hullAABB.getCenter());
+	hullOBB.rot = m0to1.m;
+	
+	if(!idtScaleMesh)
+		meshScaling.transformQueryBounds(hullOBB.center, hullOBB.extents, hullOBB.rot);
+}
+
+void Gu::computeVertexSpaceOBB(Box& dst, const Box& src, const PxTransform& meshPose, const PxMeshScale& meshScale)
+{
+	// AP scaffold failure in x64 debug in GuConvexUtilsInternal.cpp
+	//PX_ASSERT("Performance warning - this path shouldn't execute for identity mesh scale." && !meshScale.isIdentity());
+
+	dst = transform(meshScale.getInverse() * Cm::Matrix34(meshPose.getInverse()), src);
+}
+
+void Gu::computeOBBAroundConvex(
+	Box& obb, const PxConvexMeshGeometry& convexGeom, const PxConvexMesh* cm, const PxTransform& convexPose)
+{
+	const CenterExtents& aabb = static_cast<const Gu::ConvexMesh*>(cm)->getLocalBoundsFast();
+
+	if(convexGeom.scale.isIdentity())
+	{
+		const PxMat33 m(convexPose.q);
+		obb = Gu::Box(m.transform(aabb.mCenter) + convexPose.p, aabb.mExtents, m);
+	}
+	else
+	{
+		obb = transform(Cm::Matrix34(convexPose) * convexGeom.scale.toMat33(), Box(aabb.mCenter, aabb.mExtents, PxMat33(PxIdentity)));
+	}
+}

physx/source/geomutils/src/convex/GuConvexUtilsInternal.h

@@ -0,0 +1,67 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_CONVEX_UTILS_INTERNALS_H
+#define GU_CONVEX_UTILS_INTERNALS_H
+
+#include "common/PxPhysXCommonConfig.h"
+#include "CmPhysXCommon.h"
+
+namespace physx
+{
+class PxMeshScale;
+class PxConvexMeshGeometry;
+class PxConvexMesh;
+
+namespace Cm
+{
+	class Matrix34;
+	class FastVertex2ShapeScaling;
+}
+
+namespace Gu
+{
+	class Box;
+
+	void computeHullOBB(
+		Gu::Box& hullOBB, const PxBounds3& hullAABB, float offset, const Cm::Matrix34& world0,
+		const Cm::Matrix34& world1, const Cm::FastVertex2ShapeScaling& meshScaling, bool idtScaleMesh);
+
+	// src = input
+	// computes a box in vertex space (including skewed scale) from src world box
+	void computeVertexSpaceOBB(Gu::Box& dst, const Gu::Box& src, const PxTransform& meshPose, const PxMeshScale& meshScale);
+
+	PX_PHYSX_COMMON_API void computeOBBAroundConvex(
+		Gu::Box& obb, const PxConvexMeshGeometry& convexGeom, const PxConvexMesh* cm, const PxTransform& convexPose);
+
+}  // namespace Gu
+
+}
+
+#endif

physx/source/geomutils/src/convex/GuCubeIndex.h

@@ -0,0 +1,155 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_CUBE_INDEX_H
+#define GU_CUBE_INDEX_H
+
+#include "foundation/PxVec3.h"
+#include "CmPhysXCommon.h"
+#include "PsFPU.h"
+
+namespace physx
+{
+
+	enum CubeIndex
+	{
+		CUBE_RIGHT,
+		CUBE_LEFT,
+		CUBE_TOP,
+		CUBE_BOTTOM,
+		CUBE_FRONT,
+		CUBE_BACK,
+
+		CUBE_FORCE_DWORD	= 0x7fffffff
+	};
+
+	/*
+		It's pretty straightforwards in concept (though the execution in hardware is
+		a bit crufty and complex). You use a 3D texture coord to look up a texel in
+		a cube map. First you find which of the axis has the largest value (i.e.
+		X,Y,Z), and then the sign of that axis decides which face you are going to
+		use. Which is why the faces are called +X, -X, +Y, -Y, +Z, -Z - after their
+		principle axis. Then you scale the vector so that the largest value is +/-1.
+		Then use the other two as 2D coords to look up your texel (with a 0.5 scale
+		& offset).
+
+		For example, vector (0.4, -0.2, -0.5). Largest value is the Z axis, and it's
+		-ve, so we're reading from the -Z map. Scale so that this Z axis is +/-1,
+		and you get the vector (0.8, -0.4, -1.0). So now use the other two values to
+		look up your texel. So we look up texel (0.8, -0.4). The scale & offset move
+		the -1->+1 range into the usual 0->1 UV range, so we actually look up texel
+		(0.9, 0.3). The filtering is extremely complex, especially where three maps
+		meet, but that's a hardware problem :-)
+	*/
+
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	/**
+	 *	Cubemap lookup function.
+	 *
+	 *	To transform returned uvs into mapping coordinates :
+	 *	u += 1.0f;	u *= 0.5f;
+	 *	v += 1.0f;	v *= 0.5f;
+	 *
+	 *	\fn			CubemapLookup(const PxVec3& direction, float& u, float& v)
+	 *	\param		direction	[in] a direction vector
+	 *	\param		u			[out] impact coordinate on the unit cube, in [-1,1]
+	 *	\param		v			[out] impact coordinate on the unit cube, in [-1,1]
+	 *	\return		cubemap texture index
+	 */
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	PX_INLINE CubeIndex		CubemapLookup(const PxVec3& direction, float& u, float& v);
+
+	PX_INLINE PxU32 ComputeCubemapOffset(const PxVec3& dir, PxU32 subdiv)
+	{
+		float u,v;
+		const CubeIndex CI = CubemapLookup(dir, u, v);
+
+		// Remap to [0, subdiv[
+		const float Coeff = 0.5f * float(subdiv-1);
+		u += 1.0f;	u *= Coeff;
+		v += 1.0f;	v *= Coeff;
+
+		// Compute offset
+		return PxU32(CI)*(subdiv*subdiv) + PxU32(u)*subdiv + PxU32(v);
+	}
+
+
+	PX_INLINE PxU32 ComputeCubemapNearestOffset(const PxVec3& dir, PxU32 subdiv)
+	{
+		float u,v;
+		const CubeIndex CI = CubemapLookup(dir, u, v);
+
+		// Remap to [0, subdiv]
+		const float Coeff = 0.5f * float(subdiv-1);
+		u += 1.0f;	u *= Coeff;
+		v += 1.0f;	v *= Coeff;
+
+		// Compute offset
+		return PxU32(CI)*(subdiv*subdiv) + PxU32(u + 0.5f)*subdiv + PxU32(v + 0.5f);
+	}
+
+
+	PX_INLINE CubeIndex CubemapLookup(const PxVec3& direction, float& u, float& v)
+	{
+		const PxU32* binary = reinterpret_cast<const PxU32*>(&direction.x);
+
+		const PxU32 absPx = binary[0] & ~PX_SIGN_BITMASK;
+		const PxU32 absNy = binary[1] & ~PX_SIGN_BITMASK;
+		const PxU32 absNz = binary[2] & ~PX_SIGN_BITMASK;
+
+		PxU32 Index1 = 0;	//x biggest axis
+		PxU32 Index2 = 1;
+		PxU32 Index3 = 2;
+		if( (absNy > absPx) & (absNy > absNz))
+		{
+			//y biggest
+			Index2 = 2;
+			Index3 = 0;
+			Index1 = 1;
+		}
+		else if(absNz > absPx)
+		{
+			//z biggest
+			Index2 = 0;
+			Index3 = 1;
+			Index1 = 2;
+		}
+
+		const PxF32* data = &direction.x;
+		const float Coeff = 1.0f / fabsf(data[Index1]);
+		u = data[Index2] * Coeff;
+		v = data[Index3] * Coeff;
+
+		const PxU32 Sign = binary[Index1]>>31;
+		return CubeIndex(Sign|(Index1+Index1));
+	}
+
+}
+
+#endif

physx/source/geomutils/src/convex/GuHillClimbing.cpp

@@ -0,0 +1,96 @@
+//
+// 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-2021 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 "foundation/PxVec3.h"
+#include "foundation/PxAssert.h"
+#include "PsUserAllocated.h"
+#include "CmPhysXCommon.h"
+#include "GuHillClimbing.h"
+#include "GuBigConvexData2.h"
+
+namespace physx
+{
+
+void localSearch(PxU32& id, const PxVec3& dir, const PxVec3* verts, const Gu::BigConvexRawData* val)
+{
+	// WARNING: there is a problem on x86 with a naive version of this code, where truncation
+	// of values from 80 bits to 32 bits as they're stored in memory means that iteratively moving to 
+	// an adjacent vertex of greater support can go into an infinite loop. So we use a version which 
+	// never visits a vertex twice. Note - this might not be enough for GJK, since local
+	// termination of the support function might not be enough to ensure convergence of GJK itself.
+
+	// if we got here, we'd better have vertices and valencies
+	PX_ASSERT(verts && val);
+
+	class TinyBitMap
+	{
+	public:
+		PxU32 m[8];
+		PX_FORCE_INLINE TinyBitMap()			{ m[0] = m[1] = m[2] = m[3] = m[4] = m[5] = m[6] = m[7] = 0;	}
+		PX_FORCE_INLINE void set(PxU8 v)		{ m[v>>5] |= 1<<(v&31);											}
+		PX_FORCE_INLINE bool get(PxU8 v) const	{ return (m[v>>5] & 1<<(v&31)) != 0;							}
+	};
+
+	TinyBitMap visited;
+
+	const Gu::Valency* Valencies	= val->mValencies;
+	const PxU8* Adj					= val->mAdjacentVerts;
+
+	PX_ASSERT(Valencies && Adj);
+
+	// Get the initial value and the initial vertex
+	float MaxVal	= dir.dot(verts[id]);
+	PxU32 NextVtx	= id;
+
+	do
+	{
+		PxU16 NbNeighbors = Valencies[NextVtx].mCount;
+		const PxU8* Run	= Adj + Valencies[NextVtx].mOffset;
+		id = NextVtx;
+		while(NbNeighbors--)
+		{
+			const PxU8 Neighbor = *Run++;
+
+			if(!visited.get(Neighbor))
+			{
+				visited.set(Neighbor);
+
+				const float CurVal = dir.dot(verts[Neighbor]);
+
+				if(CurVal>MaxVal)
+				{
+					MaxVal	= CurVal;
+					NextVtx	= Neighbor;
+				}
+			}
+		}
+	} while(NextVtx!=id);
+}
+
+}

physx/source/geomutils/src/convex/GuHillClimbing.h

@@ -0,0 +1,46 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_HILL_CLIMBING_H
+#define GU_HILL_CLIMBING_H
+
+#include "common/PxPhysXCommonConfig.h"
+#include "Ps.h"
+
+namespace physx
+{
+	namespace Gu
+	{
+		struct BigConvexRawData;
+	}
+
+	void localSearch(PxU32& id, const PxVec3& dir, const PxVec3* verts, const Gu::BigConvexRawData* val);
+}
+
+#endif

physx/source/geomutils/src/convex/GuShapeConvex.cpp

@@ -0,0 +1,516 @@
+//
+// 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-2021 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 "GuShapeConvex.h"
+#include "GuBigConvexData.h"
+#include "GuEdgeListData.h"
+#include "GuInternal.h"
+
+#include "CmMatrix34.h"
+#include "GuHillClimbing.h"
+#include "PsFPU.h"
+
+using namespace physx;
+using namespace Gu;
+
+static PX_FORCE_INLINE PxU32 selectClosestPolygon(PxReal& maxDp_, PxU32 numPolygons, const Gu::HullPolygonData* polys, const PxVec3& axis)
+{
+	float maxDp = polys[0].mPlane.n.dot(axis);
+	PxU32 closest = 0;
+
+	// Loop through polygons
+	for(PxU32 i=1; i <numPolygons; i++)
+	{
+		// Catch current polygon and test its plane
+		const PxReal dp = polys[i].mPlane.n.dot(axis);
+		if(dp>maxDp)
+		{
+			maxDp = dp;
+			closest = i;
+		}
+	}
+	maxDp_ = maxDp;
+	return closest;
+}
+
+static PxU32 SelectClosestEdgeCB_Convex(const PolygonalData& data, const Cm::FastVertex2ShapeScaling& scaling, const PxVec3& localSpaceDirection)
+{
+	//vertex1TOShape1Skew is a symmetric matrix.  
+	//it has the property that (vertex1TOShape1Skew * v)|localSpaceDirection == (vertex1TOShape1Skew * localSpaceDirection)|v 
+	const PxVec3 vertexSpaceDirection = scaling * localSpaceDirection;
+
+	const Gu::HullPolygonData* PX_RESTRICT polys = data.mPolygons;
+
+	PxReal maxDp;
+	// ##might not be needed
+	PxU32 closest = ::selectClosestPolygon(maxDp, data.mNbPolygons, polys, vertexSpaceDirection);
+
+	// Since the convex is closed, at least some poly must satisfy this
+	PX_ASSERT(maxDp>=0);
+
+	const PxU32 numEdges = data.mNbEdges;
+	const PxU8* const edgeToFace = data.mFacesByEdges;
+
+	//Loop through edges
+	PxU32 closestEdge = 0xffffffff;
+	PxReal maxDpSq = maxDp * maxDp;
+	for(PxU32 i=0; i < numEdges; i++)
+	{
+		const PxU8 f0 = edgeToFace[i*2];
+		const PxU8 f1 = edgeToFace[i*2+1];
+
+		// unnormalized edge normal
+		const PxVec3 edgeNormal = polys[f0].mPlane.n + polys[f1].mPlane.n;
+		const PxReal enMagSq = edgeNormal.magnitudeSquared();
+		//Test normal of current edge - squared test is valid if dp and maxDp both >= 0
+		const float dp = edgeNormal.dot(vertexSpaceDirection);
+		if(dp>=0.0f && dp*dp>maxDpSq*enMagSq)
+		{
+			maxDpSq = dp*dp/enMagSq;
+			closestEdge = i;
+		}
+	}
+
+	if(closestEdge!=0xffffffff)
+	{
+		const PxU8* FBE = edgeToFace;
+
+		const PxU32 f0 = FBE[closestEdge*2];
+		const PxU32 f1 = FBE[closestEdge*2+1];
+
+		const PxReal dp0 = polys[f0].mPlane.n.dot(vertexSpaceDirection);
+		const PxReal dp1 = polys[f1].mPlane.n.dot(vertexSpaceDirection);
+		if(dp0>dp1)
+			closest = f0;
+		else
+			closest = f1;
+	}
+	return closest;
+}
+ 
+// Hull projection callback for "small" hulls
+static void HullProjectionCB_SmallConvex(const PolygonalData& data, const PxVec3& dir,
+										 const Cm::Matrix34& world,
+										 const Cm::FastVertex2ShapeScaling& scaling,
+										 PxReal& min, PxReal& max)
+{
+	const PxVec3 localSpaceDirection = world.rotateTranspose(dir);
+	//vertex1TOShape1Skew is a symmetric matrix.  
+	//it has the property that (vertex1TOShape1Skew * v)|localSpaceDirection == (vertex1TOShape1Skew * localSpaceDirection)|v 
+	const PxVec3 vertexSpaceDirection = scaling * localSpaceDirection;
+
+	// PT: prevents aliasing
+	PxReal minimum = PX_MAX_REAL;
+	PxReal maximum = -PX_MAX_REAL;
+
+	//brute-force, localspace
+	{
+		const PxVec3* PX_RESTRICT verts = data.mVerts;
+		PxU32 numVerts = data.mNbVerts;
+		while(numVerts--)
+		{
+			const PxReal dp = (*verts++).dot(vertexSpaceDirection);
+			minimum = physx::intrinsics::selectMin(minimum, dp);
+			maximum = physx::intrinsics::selectMax(maximum, dp);
+		}
+
+	}
+
+	const PxReal offset = world.p.dot(dir);
+	min = minimum + offset;
+	max = maximum + offset;
+}
+
+static PxU32 computeNearestOffset(const PxU32 subdiv, const PxVec3& dir)
+{
+	// ComputeCubemapNearestOffset(const Point& dir, udword subdiv)
+
+	// PT: ok so why exactly was the code duplicated here?
+	// PxU32 CI = CubemapLookup(dir,u,v)
+
+	PxU32 index;
+	PxReal coeff;
+	// find largest axis
+	PxReal absNx = PxAbs(dir.x);
+	PxReal absNy = PxAbs(dir.y);
+	PxReal absNz = PxAbs(dir.z);
+
+	if( absNy > absNx &&
+		absNy > absNz)
+	{
+		//y biggest
+		index = 1;
+		coeff = 1.0f/absNy;
+	}
+	else if(absNz > absNx)
+	{
+		index = 2;
+		coeff = 1.0f/absNz;
+	}
+	else
+	{
+		index = 0;
+		coeff = 1.0f/absNx;
+	}
+
+	union
+	{
+		PxU32 aU32;
+		PxReal aFloat;
+	} conv;
+
+	conv.aFloat = dir[index];
+	PxU32 sign = conv.aU32>>31;
+	
+	const PxU32 index2 = Ps::getNextIndex3(index);
+	const PxU32 index3 = Ps::getNextIndex3(index2);
+	PxReal u = dir[index2] * coeff;
+	PxReal v = dir[index3] * coeff;
+	
+	PxU32 CI = (sign | (index+index));
+
+	//Remap to [0, subdiv[
+	coeff = 0.5f * PxReal(subdiv-1);
+	u += 1.0f; u *= coeff;
+	v += 1.0f; v *= coeff;
+
+	//Round to nearest
+	PxU32 ui = PxU32(u);
+	PxU32 vi = PxU32(v);
+
+	PxReal du = u - PxReal(ui);
+	PxReal dv = v - PxReal(vi);
+	if(du>0.5f) ui++;
+	if(dv>0.5f) vi++;
+
+	//Compute offset
+	return CI*(subdiv*subdiv) + ui*subdiv + vi;
+}
+
+// Hull projection callback for "big" hulls
+static void HullProjectionCB_BigConvex(const PolygonalData& data, const PxVec3& dir, const Cm::Matrix34& world, const Cm::FastVertex2ShapeScaling& scaling, PxReal& minimum, PxReal& maximum)
+{
+	const PxVec3* PX_RESTRICT verts = data.mVerts;
+
+	const PxVec3 localSpaceDirection = world.rotateTranspose(dir);
+	//vertex1TOShape1Skew is a symmetric matrix.  
+	//it has the property that (vertex1TOShape1Skew * v)|localSpaceDirection == (vertex1TOShape1Skew * localSpaceDirection)|v 
+	const PxVec3 vertexSpaceDirection = scaling * localSpaceDirection;	//NB: triangles are always shape 1! eek!
+
+	// This version is better for objects with a lot of vertices
+	const Gu::BigConvexRawData* bigData = data.mBigData;
+	PxU32 minID = 0, maxID = 0;
+	{
+		const PxU32 offset = computeNearestOffset(bigData->mSubdiv, -vertexSpaceDirection);
+		minID = bigData->mSamples[offset];
+		maxID = bigData->getSamples2()[offset];
+	}
+	
+	// Do hillclimbing!
+	localSearch(minID, -vertexSpaceDirection, verts, bigData);
+	localSearch(maxID, vertexSpaceDirection, verts, bigData);
+
+	const PxReal offset = world.p.dot(dir);
+	minimum = offset + verts[minID].dot(vertexSpaceDirection);
+	maximum = offset + verts[maxID].dot(vertexSpaceDirection);
+	PX_ASSERT(maximum >= minimum);
+}
+
+void Gu::getPolygonalData_Convex(PolygonalData* PX_RESTRICT dst, const Gu::ConvexHullData* PX_RESTRICT src, const Cm::FastVertex2ShapeScaling& scaling)
+{
+	dst->mCenter			= scaling * src->mCenterOfMass;
+	dst->mNbVerts			= src->mNbHullVertices;
+	dst->mNbPolygons		= src->mNbPolygons;
+	dst->mNbEdges			= src->mNbEdges;
+	dst->mPolygons			= src->mPolygons;
+	dst->mVerts				= src->getHullVertices();
+	dst->mPolygonVertexRefs	= src->getVertexData8();
+	dst->mFacesByEdges		= src->getFacesByEdges8();
+
+// TEST_INTERNAL_OBJECTS
+	dst->mInternal			= src->mInternal;
+//~TEST_INTERNAL_OBJECTS
+
+	dst->mBigData			= src->mBigConvexRawData;
+
+	// This threshold test doesnt cost much and many customers cook on PC and use this on 360.
+	// 360 has a much higher threshold than PC(and it makes a big difference)
+	// PT: the cool thing is that this test is now done once by contact generation call, not once by hull projection
+	if(!src->mBigConvexRawData)
+		dst->mProjectHull = HullProjectionCB_SmallConvex;
+	else
+		dst->mProjectHull = HullProjectionCB_BigConvex;
+	dst->mSelectClosestEdgeCB = SelectClosestEdgeCB_Convex;
+}
+
+// Box emulating convex mesh
+
+// Face0: 0-1-2-3
+// Face1: 1-5-6-2
+// Face2: 5-4-7-6
+// Face3: 4-0-3-7
+// Face4; 3-2-6-7
+// Face5: 4-5-1-0
+
+//     7+------+6			0 = ---
+//     /|     /|			1 = +--
+//    / |    / |			2 = ++-
+//   / 4+---/--+5			3 = -+-
+// 3+------+2 /    y   z	4 = --+
+//  | /    | /     |  /		5 = +-+
+//  |/     |/      |/		6 = +++
+// 0+------+1      *---x	7 = -++
+
+static const PxU8 gPxcBoxPolygonData[] = {
+	0, 1, 2, 3,
+	1, 5, 6, 2,
+	5, 4, 7, 6,
+	4, 0, 3, 7,
+	3, 2, 6, 7,
+	4, 5, 1, 0,
+};
+
+#define	INVSQRT2	0.707106781188f				//!< 1 / sqrt(2)
+static PxVec3 gPxcBoxEdgeNormals[] = 
+{
+	PxVec3(0,			-INVSQRT2,	-INVSQRT2),	// 0-1
+	PxVec3(INVSQRT2,	0,			-INVSQRT2),	// 1-2
+	PxVec3(0,			INVSQRT2,	-INVSQRT2),	// 2-3
+	PxVec3(-INVSQRT2,	0,			-INVSQRT2),	// 3-0
+
+	PxVec3(0,			INVSQRT2,	INVSQRT2),	// 7-6
+	PxVec3(INVSQRT2,	0,			INVSQRT2),	// 6-5
+	PxVec3(0,			-INVSQRT2,	INVSQRT2),	// 5-4
+	PxVec3(-INVSQRT2,	0,			INVSQRT2),	// 4-7
+
+	PxVec3(INVSQRT2,	-INVSQRT2,	0),			// 1-5
+	PxVec3(INVSQRT2,	INVSQRT2,	0),			// 6-2
+	PxVec3(-INVSQRT2,	INVSQRT2,	0),			// 3-7
+	PxVec3(-INVSQRT2,	-INVSQRT2,	0)			// 4-0
+};
+#undef INVSQRT2
+
+// ### needs serious checkings
+	// Flags(16), Count(16), Offset(32);
+static Gu::EdgeDescData gPxcBoxEdgeDesc[] = {
+	{Gu::PX_EDGE_ACTIVE, 2, 0},
+	{Gu::PX_EDGE_ACTIVE, 2, 2},
+	{Gu::PX_EDGE_ACTIVE, 2, 4},
+	{Gu::PX_EDGE_ACTIVE, 2, 6},
+	{Gu::PX_EDGE_ACTIVE, 2, 8},
+	{Gu::PX_EDGE_ACTIVE, 2, 10},
+	{Gu::PX_EDGE_ACTIVE, 2, 12},
+	{Gu::PX_EDGE_ACTIVE, 2, 14},
+	{Gu::PX_EDGE_ACTIVE, 2, 16},
+	{Gu::PX_EDGE_ACTIVE, 2, 18},
+	{Gu::PX_EDGE_ACTIVE, 2, 20},
+	{Gu::PX_EDGE_ACTIVE, 2, 22},
+};
+
+// ### needs serious checkings
+static PxU8 gPxcBoxFaceByEdge[] = {
+	0,5, 	// Edge 0-1
+	0,1, 	// Edge 1-2
+	0,4, 	// Edge 2-3
+	0,3, 	// Edge 3-0
+	2,4, 	// Edge 7-6
+	1,2, 	// Edge 6-5
+	2,5, 	// Edge 5-4
+	2,3, 	// Edge 4-7
+	1,5, 	// Edge 1-5
+	1,4, 	// Edge 6-2
+	3,4, 	// Edge 3-7
+	3,5, 	// Edge 4-0
+};
+
+static PxU32 SelectClosestEdgeCB_Box(const PolygonalData& data, const Cm::FastVertex2ShapeScaling& scaling, const PxVec3& localDirection)
+{
+	PX_UNUSED(scaling);
+
+	PxReal maxDp;
+	// ##might not be needed
+	PxU32 closest = ::selectClosestPolygon(maxDp, 6, data.mPolygons, localDirection);
+
+	PxU32 numEdges = 12;
+	const PxVec3* PX_RESTRICT edgeNormals = gPxcBoxEdgeNormals;
+
+	//Loop through edges
+	PxU32 closestEdge = 0xffffffff;
+	for(PxU32 i=0; i < numEdges; i++)
+	{
+		//Test normal of current edge
+		const float dp = edgeNormals[i].dot(localDirection);
+		if(dp>maxDp)
+		{
+			maxDp = dp;
+			closestEdge = i;
+		}
+	}
+
+	if(closestEdge!=0xffffffff)
+	{
+		const Gu::EdgeDescData* PX_RESTRICT ED = gPxcBoxEdgeDesc;
+		const PxU8* PX_RESTRICT FBE = gPxcBoxFaceByEdge;
+
+		PX_ASSERT(ED[closestEdge].Count==2);
+		const PxU32 f0 = FBE[ED[closestEdge].Offset];
+		const PxU32 f1 = FBE[ED[closestEdge].Offset+1];
+
+		const PxReal dp0 = data.mPolygons[f0].mPlane.n.dot(localDirection);
+		const PxReal dp1 = data.mPolygons[f1].mPlane.n.dot(localDirection);
+		if(dp0>dp1)
+			closest = f0;
+		else
+			closest = f1;
+	}
+
+	return closest;
+}
+
+static PX_FORCE_INLINE void projectBox(PxVec3& p, const PxVec3& localDir, const PxVec3& extents)
+{
+	// PT: the original code didn't have branches or FPU comparisons. Why rewrite it ?
+//	p.x = (localDir.x >= 0) ? extents.x : -extents.x;
+//	p.y = (localDir.y >= 0) ? extents.y : -extents.y;
+//	p.z = (localDir.z >= 0) ? extents.z : -extents.z;
+	p.x = physx::intrinsics::fsel(localDir.x, extents.x, -extents.x);
+	p.y = physx::intrinsics::fsel(localDir.y, extents.y, -extents.y);
+	p.z = physx::intrinsics::fsel(localDir.z, extents.z, -extents.z);
+}
+
+static void	HullProjectionCB_Box(const PolygonalData& data, const PxVec3& dir, const Cm::Matrix34& world, const Cm::FastVertex2ShapeScaling& scaling, PxReal& minimum, PxReal& maximum)
+{
+	PX_UNUSED(scaling);
+
+	const PxVec3 localDir = world.rotateTranspose(dir);
+
+	PxVec3 p;
+	projectBox(p, localDir, *data.mHalfSide);
+
+	const PxReal offset = world.p.dot(dir);
+	const PxReal tmp = p.dot(localDir);
+	maximum = offset + tmp;
+	minimum = offset - tmp;
+}
+
+PolygonalBox::PolygonalBox(const PxVec3& halfSide) : mHalfSide(halfSide)
+{
+	//Precompute the convex data
+	//     7+------+6			0 = ---
+	//     /|     /|			1 = +--
+	//    / |    / |			2 = ++-
+	//   / 4+---/--+5			3 = -+-
+	// 3+------+2 /    y   z	4 = --+
+	//  | /    | /     |  /		5 = +-+
+	//  |/     |/      |/		6 = +++
+	// 0+------+1      *---x	7 = -++
+
+	PxVec3 minimum = -mHalfSide;
+	PxVec3 maximum = mHalfSide;
+	// Generate 8 corners of the bbox
+	mVertices[0] = PxVec3(minimum.x, minimum.y, minimum.z);
+	mVertices[1] = PxVec3(maximum.x, minimum.y, minimum.z);
+	mVertices[2] = PxVec3(maximum.x, maximum.y, minimum.z);
+	mVertices[3] = PxVec3(minimum.x, maximum.y, minimum.z);
+	mVertices[4] = PxVec3(minimum.x, minimum.y, maximum.z);
+	mVertices[5] = PxVec3(maximum.x, minimum.y, maximum.z);
+	mVertices[6] = PxVec3(maximum.x, maximum.y, maximum.z);
+	mVertices[7] = PxVec3(minimum.x, maximum.y, maximum.z);
+
+	//Setup the polygons
+	for(PxU8 i=0; i < 6; i++)
+	{
+		mPolygons[i].mNbVerts = 4;
+		mPolygons[i].mVRef8 = PxU16(i*4);
+	}
+
+	// ### planes needs *very* careful checks
+	// X axis
+	mPolygons[1].mPlane.n = PxVec3(1.0f, 0.0f, 0.0f);
+	mPolygons[1].mPlane.d = -mHalfSide.x;
+	mPolygons[3].mPlane.n = PxVec3(-1.0f, 0.0f, 0.0f);
+	mPolygons[3].mPlane.d = -mHalfSide.x;
+	
+	mPolygons[1].mMinIndex = 0;
+	mPolygons[3].mMinIndex = 1;
+
+//	mPolygons[1].mMinObsolete = -mHalfSide.x;
+//	mPolygons[3].mMinObsolete = -mHalfSide.x;
+
+	PX_ASSERT(mPolygons[1].getMin(mVertices) == -mHalfSide.x); 
+	PX_ASSERT(mPolygons[3].getMin(mVertices) == -mHalfSide.x);
+
+
+	// Y axis
+	mPolygons[4].mPlane.n = PxVec3(0.f, 1.0f, 0.0f);
+	mPolygons[4].mPlane.d = -mHalfSide.y;
+	mPolygons[5].mPlane.n = PxVec3(0.0f, -1.0f, 0.0f);
+	mPolygons[5].mPlane.d = -mHalfSide.y;
+
+	mPolygons[4].mMinIndex = 0;
+	mPolygons[5].mMinIndex = 2;
+//	mPolygons[4].mMinObsolete = -mHalfSide.y;
+//	mPolygons[5].mMinObsolete = -mHalfSide.y;
+
+	PX_ASSERT(mPolygons[4].getMin(mVertices) == -mHalfSide.y); 
+	PX_ASSERT(mPolygons[5].getMin(mVertices) == -mHalfSide.y);
+
+	// Z axis
+	mPolygons[2].mPlane.n = PxVec3(0.f, 0.0f, 1.0f);
+	mPolygons[2].mPlane.d = -mHalfSide.z;
+	mPolygons[0].mPlane.n = PxVec3(0.0f, 0.0f, -1.0f);
+	mPolygons[0].mPlane.d = -mHalfSide.z;
+
+	mPolygons[2].mMinIndex = 0;
+	mPolygons[0].mMinIndex = 4;
+//	mPolygons[2].mMinObsolete = -mHalfSide.z;
+//	mPolygons[0].mMinObsolete = -mHalfSide.z;
+	PX_ASSERT(mPolygons[2].getMin(mVertices) == -mHalfSide.z); 
+	PX_ASSERT(mPolygons[0].getMin(mVertices) == -mHalfSide.z);
+}
+
+void PolygonalBox::getPolygonalData(PolygonalData* PX_RESTRICT dst) const
+{
+	dst->mCenter				= PxVec3(0.0f, 0.0f, 0.0f);
+	dst->mNbVerts				= 8;
+	dst->mNbPolygons			= 6;
+	dst->mPolygons				= mPolygons;
+	dst->mNbEdges				= 0;
+	dst->mVerts					= mVertices;
+	dst->mPolygonVertexRefs		= gPxcBoxPolygonData;
+	dst->mFacesByEdges			= NULL;
+	dst->mInternal.mRadius		= 0.0f;
+	dst->mInternal.mExtents[0]	= 0.0f;
+	dst->mInternal.mExtents[1]	= 0.0f;
+	dst->mInternal.mExtents[2]	= 0.0f;
+//	dst->mBigData				= NULL;
+	dst->mHalfSide				= &mHalfSide;
+	dst->mProjectHull			= HullProjectionCB_Box;
+	dst->mSelectClosestEdgeCB	= SelectClosestEdgeCB_Box;
+}

physx/source/geomutils/src/convex/GuShapeConvex.h

@@ -0,0 +1,100 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_SHAPECONVEX_H
+#define GU_SHAPECONVEX_H
+
+#include "GuConvexMeshData.h"
+#include "CmScaling.h"
+
+namespace physx
+{
+namespace Gu
+{
+	struct PolygonalData;
+	typedef void	(*HullPrefetchCB)		(PxU32 numVerts, const PxVec3* PX_RESTRICT verts);
+	typedef void	(*HullProjectionCB)		(const PolygonalData& data, const PxVec3& dir, const Cm::Matrix34& world2hull, const Cm::FastVertex2ShapeScaling& scaling, PxReal& minimum, PxReal& maximum);
+	typedef PxU32	(*SelectClosestEdgeCB)	(const PolygonalData& data, const Cm::FastVertex2ShapeScaling& scaling, const PxVec3& localDirection);
+
+	struct PolygonalData
+	{
+		// Data
+		PxVec3								mCenter;
+		PxU32								mNbVerts;
+		PxU32								mNbPolygons;
+		PxU32								mNbEdges;
+		const Gu::HullPolygonData*			mPolygons;
+		const PxVec3*						mVerts;
+		const PxU8*							mPolygonVertexRefs;
+		const PxU8*							mFacesByEdges;
+		const PxU16*						mVerticesByEdges;
+
+		Gu::InternalObjectsData				mInternal;
+		union
+		{
+			const Gu::BigConvexRawData*		mBigData;	// Only for big convexes
+			const PxVec3*					mHalfSide;	// Only for boxes
+		};
+
+		// Code
+		HullProjectionCB					mProjectHull;
+		SelectClosestEdgeCB					mSelectClosestEdgeCB;
+
+		PX_FORCE_INLINE const PxU8*	getPolygonVertexRefs(const Gu::HullPolygonData& poly)	const
+		{
+			return mPolygonVertexRefs + poly.mVRef8;
+		}
+	};
+
+#if PX_VC 
+    #pragma warning(push)
+	#pragma warning( disable : 4251 ) // class needs to have dll-interface to be used by clients of class
+#endif
+	class PX_PHYSX_COMMON_API PolygonalBox
+	{
+	public:
+									PolygonalBox(const PxVec3& halfSide);
+
+			void					getPolygonalData(PolygonalData* PX_RESTRICT dst)	const;
+
+			const PxVec3&			mHalfSide;
+			PxVec3					mVertices[8];
+			Gu::HullPolygonData		mPolygons[6];
+	private:
+			PolygonalBox& operator=(const PolygonalBox&);
+	};
+#if PX_VC 
+     #pragma warning(pop) 
+#endif
+
+	void getPolygonalData_Convex(PolygonalData* PX_RESTRICT dst, const Gu::ConvexHullData* PX_RESTRICT src, const Cm::FastVertex2ShapeScaling& scaling);
+}
+}
+
+#endif

physx/source/geomutils/src/distance/GuDistancePointBox.cpp

@@ -0,0 +1,66 @@
+//
+// 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-2021 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 "GuDistancePointBox.h"
+
+using namespace physx;
+
+PxReal Gu::distancePointBoxSquared(	const PxVec3& point, 
+									const PxVec3& boxOrigin, const PxVec3& boxExtent, const PxMat33& boxBase, 
+									PxVec3* boxParam)
+{
+	// Compute coordinates of point in box coordinate system
+	const PxVec3 diff = point - boxOrigin;
+
+	PxVec3 closest(	boxBase.column0.dot(diff),
+					boxBase.column1.dot(diff),
+					boxBase.column2.dot(diff));
+	
+	// Project test point onto box
+	PxReal sqrDistance = 0.0f;
+	for(PxU32 ax=0; ax<3; ax++) 
+	{
+		if(closest[ax] < -boxExtent[ax])
+		{
+			const PxReal delta = closest[ax] + boxExtent[ax];
+			sqrDistance += delta*delta;
+			closest[ax] = -boxExtent[ax];
+		}
+		else if(closest[ax] > boxExtent[ax])
+		{
+			const PxReal delta = closest[ax] - boxExtent[ax];
+			sqrDistance += delta*delta;
+			closest[ax] = boxExtent[ax];
+		}
+	}
+	
+	if(boxParam) *boxParam = closest;
+	
+	return sqrDistance;
+}

physx/source/geomutils/src/distance/GuDistancePointBox.h

@@ -0,0 +1,70 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_DISTANCE_POINT_BOX_H
+#define GU_DISTANCE_POINT_BOX_H
+
+#include "GuBox.h"
+
+namespace physx
+{
+namespace Gu
+{
+	/**
+	Return the square of the minimum distance from the surface of the box to the given point.
+	\param point The point
+	\param boxOrigin The origin of the box
+	\param boxExtent The extent of the box
+	\param boxBase The orientation of the box
+	\param boxParam Set to coordinates of the closest point on the box in its local space
+	*/
+	PxReal distancePointBoxSquared(	const PxVec3& point,
+									const PxVec3& boxOrigin, 
+									const PxVec3& boxExtent, 
+									const PxMat33& boxBase, 
+									PxVec3* boxParam=NULL);
+
+	/**
+	Return the square of the minimum distance from the surface of the box to the given point.
+	\param point The point
+	\param box The box
+	\param boxParam Set to coordinates of the closest point on the box in its local space
+	*/
+	PX_FORCE_INLINE PxReal distancePointBoxSquared(	const PxVec3& point, 
+													const Gu::Box& box, 
+													PxVec3* boxParam=NULL)
+	{
+		return distancePointBoxSquared(point, box.center, box.extents, box.rot, boxParam);
+	}
+
+} // namespace Gu
+
+}
+
+#endif

physx/source/geomutils/src/distance/GuDistancePointSegment.h

@@ -0,0 +1,90 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_DISTANCE_POINT_SEGMENT_H
+#define GU_DISTANCE_POINT_SEGMENT_H
+
+#include "common/PxPhysXCommonConfig.h"
+#include "GuSegment.h"
+
+namespace physx
+{
+namespace Gu
+{
+	// dir = p1 - p0
+	PX_FORCE_INLINE PxReal distancePointSegmentSquaredInternal(const PxVec3& p0, const PxVec3& dir, const PxVec3& point, PxReal* param=NULL)
+	{
+		PxVec3 diff = point - p0;
+		PxReal fT = diff.dot(dir);
+
+		if(fT<=0.0f)
+		{
+			fT = 0.0f;
+		}
+		else
+		{
+			const PxReal sqrLen = dir.magnitudeSquared();
+			if(fT>=sqrLen)
+			{
+				fT = 1.0f;
+				diff -= dir;
+			}
+			else
+			{
+				fT /= sqrLen;
+				diff -= fT*dir;
+			}
+		}
+
+		if(param)
+			*param = fT;
+
+		return diff.magnitudeSquared();
+	}
+
+	/**
+	A segment is defined by S(t) = mP0 * (1 - t) + mP1 * t, with 0 <= t <= 1
+	Alternatively, a segment is S(t) = Origin + t * Direction for 0 <= t <= 1.
+	Direction is not necessarily unit length. The end points are Origin = mP0 and Origin + Direction = mP1.
+	*/
+	PX_FORCE_INLINE PxReal distancePointSegmentSquared(const PxVec3& p0, const PxVec3& p1, const PxVec3& point, PxReal* param=NULL)
+	{
+		return distancePointSegmentSquaredInternal(p0, p1 - p0, point, param);
+	}
+
+	PX_INLINE PxReal distancePointSegmentSquared(const Gu::Segment& segment, const PxVec3& point, PxReal* param=NULL)
+	{
+		return distancePointSegmentSquared(segment.p0, segment.p1, point, param);
+	}
+
+} // namespace Gu
+
+}
+
+#endif

physx/source/geomutils/src/distance/GuDistancePointTriangle.cpp

@@ -0,0 +1,353 @@
+//
+// 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-2021 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 "foundation/PxVec3.h"
+#include "GuDistancePointTriangle.h"
+#include "GuDistancePointTriangleSIMD.h"
+
+using namespace physx;
+
+// Based on Christer Ericson's book
+PxVec3 Gu::closestPtPointTriangle(const PxVec3& p, const PxVec3& a, const PxVec3& b, const PxVec3& c, float& s, float& t)
+{
+	// Check if P in vertex region outside A
+	const PxVec3 ab = b - a;
+	const PxVec3 ac = c - a;
+	const PxVec3 ap = p - a;
+	const float d1 = ab.dot(ap);
+	const float d2 = ac.dot(ap);
+	if(d1<=0.0f && d2<=0.0f)
+	{
+		s = 0.0f;
+		t = 0.0f;
+		return a;	// Barycentric coords 1,0,0
+	}
+
+	// Check if P in vertex region outside B
+	const PxVec3 bp = p - b;
+	const float d3 = ab.dot(bp);
+	const float d4 = ac.dot(bp);
+	if(d3>=0.0f && d4<=d3)
+	{
+		s = 1.0f;
+		t = 0.0f;
+		return b;	// Barycentric coords 0,1,0
+	}
+
+	// Check if P in edge region of AB, if so return projection of P onto AB
+	const float vc = d1*d4 - d3*d2;
+	if(vc<=0.0f && d1>=0.0f && d3<=0.0f)
+	{
+		const float v = d1 / (d1 - d3);
+		s = v;
+		t = 0.0f;
+		return a + v * ab;	// barycentric coords (1-v, v, 0)
+	}
+
+	// Check if P in vertex region outside C
+	const PxVec3 cp = p - c;
+	const float d5 = ab.dot(cp);
+	const float d6 = ac.dot(cp);
+	if(d6>=0.0f && d5<=d6)
+	{
+		s = 0.0f;
+		t = 1.0f;
+		return c;	// Barycentric coords 0,0,1
+	}
+
+	// Check if P in edge region of AC, if so return projection of P onto AC
+	const float vb = d5*d2 - d1*d6;
+	if(vb<=0.0f && d2>=0.0f && d6<=0.0f)
+	{
+		const float w = d2 / (d2 - d6);
+		s = 0.0f;
+		t = w;
+		return a + w * ac;	// barycentric coords (1-w, 0, w)
+	}
+
+	// Check if P in edge region of BC, if so return projection of P onto BC
+	const float va = d3*d6 - d5*d4;
+	if(va<=0.0f && (d4-d3)>=0.0f && (d5-d6)>=0.0f)
+	{
+		const float w = (d4-d3) / ((d4 - d3) + (d5-d6));
+		s = 1.0f-w;
+		t = w;
+		return b + w * (c-b);	// barycentric coords (0, 1-w, w)
+	}
+
+	// P inside face region. Compute Q through its barycentric coords (u,v,w)
+	const float denom = 1.0f / (va + vb + vc);
+	const float v = vb * denom;
+	const float w = vc * denom;
+	s = v;
+	t = w;
+	return a + ab*v + ac*w;
+}
+
+//Ps::aos::FloatV Gu::distancePointTriangleSquared(	const Ps::aos::Vec3VArg p, 
+//													const Ps::aos::Vec3VArg a, 
+//													const Ps::aos::Vec3VArg b, 
+//													const Ps::aos::Vec3VArg c,
+//													Ps::aos::FloatV& u,
+//													Ps::aos::FloatV& v,
+//													Ps::aos::Vec3V& closestP)
+//{
+//	using namespace Ps::aos;
+//
+//	const FloatV zero = FZero();
+//	const FloatV one = FOne();
+//	//const Vec3V zero = V3Zero();
+//	const Vec3V ab = V3Sub(b, a);
+//	const Vec3V ac = V3Sub(c, a);
+//	const Vec3V bc = V3Sub(c, b);
+//	const Vec3V ap = V3Sub(p, a);
+//	const Vec3V bp = V3Sub(p, b);
+//	const Vec3V cp = V3Sub(p, c);
+//
+//	const FloatV d1 = V3Dot(ab, ap); //  snom
+//	const FloatV d2 = V3Dot(ac, ap); //  tnom
+//	const FloatV d3 = V3Dot(ab, bp); // -sdenom
+//	const FloatV d4 = V3Dot(ac, bp); //  unom = d4 - d3
+//	const FloatV d5 = V3Dot(ab, cp); //  udenom = d5 - d6
+//	const FloatV d6 = V3Dot(ac, cp); // -tdenom
+//	const FloatV unom = FSub(d4, d3);
+//	const FloatV udenom = FSub(d5, d6);
+//	
+//	//check if p in vertex region outside a
+//	const BoolV con00 = FIsGrtr(zero, d1); // snom <= 0
+//	const BoolV con01 = FIsGrtr(zero, d2); // tnom <= 0
+//	const BoolV con0 = BAnd(con00, con01); // vertex region a
+//	const FloatV u0 = zero;
+//	const FloatV v0 = zero;
+//
+//	//check if p in vertex region outside b
+//	const BoolV con10 = FIsGrtrOrEq(d3, zero);
+//	const BoolV con11 = FIsGrtrOrEq(d3, d4);
+//	const BoolV con1 = BAnd(con10, con11); // vertex region b
+//	const FloatV u1 = one;
+//	const FloatV v1 = zero;
+//
+//	//check if p in vertex region outside c
+//	const BoolV con20 = FIsGrtrOrEq(d6, zero);
+//	const BoolV con21 = FIsGrtrOrEq(d6, d5); 
+//	const BoolV con2 = BAnd(con20, con21); // vertex region c
+//	const FloatV u2 = zero;
+//	const FloatV v2 = one;
+//
+//	//check if p in edge region of AB
+//	const FloatV vc = FSub(FMul(d1, d4), FMul(d3, d2));
+//	
+//	const BoolV con30 = FIsGrtr(zero, vc);
+//	const BoolV con31 = FIsGrtrOrEq(d1, zero);
+//	const BoolV con32 = FIsGrtr(zero, d3);
+//	const BoolV con3 = BAnd(con30, BAnd(con31, con32));
+//	const FloatV sScale = FDiv(d1, FSub(d1, d3));
+//	const Vec3V closest3 = V3Add(a, V3Scale(ab, sScale));
+//	const FloatV u3 = sScale;
+//	const FloatV v3 = zero;
+//
+//	//check if p in edge region of BC
+//	const FloatV va = FSub(FMul(d3, d6),FMul(d5, d4));
+//	const BoolV con40 = FIsGrtr(zero, va);
+//	const BoolV con41 = FIsGrtrOrEq(d4, d3);
+//	const BoolV con42 = FIsGrtrOrEq(d5, d6);
+//	const BoolV con4 = BAnd(con40, BAnd(con41, con42)); 
+//	const FloatV uScale = FDiv(unom, FAdd(unom, udenom));
+//	const Vec3V closest4 = V3Add(b, V3Scale(bc, uScale));
+//	const FloatV u4 = FSub(one, uScale);
+//	const FloatV v4 = uScale;
+//
+//	//check if p in edge region of AC
+//	const FloatV vb = FSub(FMul(d5, d2), FMul(d1, d6));
+//	const BoolV con50 = FIsGrtr(zero, vb);
+//	const BoolV con51 = FIsGrtrOrEq(d2, zero);
+//	const BoolV con52 = FIsGrtr(zero, d6);
+//	const BoolV con5 = BAnd(con50, BAnd(con51, con52));
+//	const FloatV tScale = FDiv(d2, FSub(d2, d6));
+//	const Vec3V closest5 = V3Add(a, V3Scale(ac, tScale));
+//	const FloatV u5 = zero;
+//	const FloatV v5 = tScale;
+//
+//	//P must project inside face region. Compute Q using Barycentric coordinates
+//	const FloatV denom = FRecip(FAdd(va, FAdd(vb, vc)));
+//	const FloatV t = FMul(vb, denom);
+//	const FloatV w = FMul(vc, denom);
+//	const Vec3V bCom = V3Scale(ab, t);
+//	const Vec3V cCom = V3Scale(ac, w);
+//	const Vec3V closest6 = V3Add(a, V3Add(bCom, cCom));
+//	const FloatV u6 = t;
+//	const FloatV v6 = w;
+//	
+//	const Vec3V closest= V3Sel(con0, a, V3Sel(con1, b, V3Sel(con2, c, V3Sel(con3, closest3, V3Sel(con4, closest4, V3Sel(con5, closest5, closest6))))));
+//	u = FSel(con0, u0, FSel(con1, u1, FSel(con2, u2, FSel(con3, u3, FSel(con4, u4, FSel(con5, u5, u6))))));
+//	v = FSel(con0, v0, FSel(con1, v1, FSel(con2, v2, FSel(con3, v3, FSel(con4, v4, FSel(con5, v5, v6))))));
+//	closestP = closest;
+//
+//	const Vec3V vv = V3Sub(p, closest);
+//
+//	return V3Dot(vv, vv);
+//}
+
+
+Ps::aos::FloatV Gu::distancePointTriangleSquared(	const Ps::aos::Vec3VArg p, 
+													const Ps::aos::Vec3VArg a, 
+													const Ps::aos::Vec3VArg b, 
+													const Ps::aos::Vec3VArg c,
+													Ps::aos::FloatV& u,
+													Ps::aos::FloatV& v,
+													Ps::aos::Vec3V& closestP)
+{
+	using namespace Ps::aos;
+
+	const FloatV zero = FZero();
+	const FloatV one = FOne();
+	//const Vec3V zero = V3Zero();
+	const Vec3V ab = V3Sub(b, a);
+	const Vec3V ac = V3Sub(c, a);
+	const Vec3V bc = V3Sub(c, b);
+	const Vec3V ap = V3Sub(p, a);
+	const Vec3V bp = V3Sub(p, b);
+	const Vec3V cp = V3Sub(p, c);
+
+	const FloatV d1 = V3Dot(ab, ap); //  snom
+	const FloatV d2 = V3Dot(ac, ap); //  tnom
+	const FloatV d3 = V3Dot(ab, bp); // -sdenom
+	const FloatV d4 = V3Dot(ac, bp); //  unom = d4 - d3
+	const FloatV d5 = V3Dot(ab, cp); //  udenom = d5 - d6
+	const FloatV d6 = V3Dot(ac, cp); // -tdenom
+	const FloatV unom = FSub(d4, d3);
+	const FloatV udenom = FSub(d5, d6);
+	
+	//check if p in vertex region outside a
+	const BoolV con00 = FIsGrtr(zero, d1); // snom <= 0
+	const BoolV con01 = FIsGrtr(zero, d2); // tnom <= 0
+	const BoolV con0 = BAnd(con00, con01); // vertex region a
+
+	if(BAllEqTTTT(con0))
+	{
+		u = zero;
+		v = zero;
+		const Vec3V vv = V3Sub(p, a);
+		closestP = a;
+		return V3Dot(vv, vv);
+	}
+
+	//check if p in vertex region outside b
+	const BoolV con10 = FIsGrtrOrEq(d3, zero);
+	const BoolV con11 = FIsGrtrOrEq(d3, d4);
+	const BoolV con1 = BAnd(con10, con11); // vertex region b
+	if(BAllEqTTTT(con1))
+	{
+		u = one;
+		v = zero;
+		const Vec3V vv = V3Sub(p, b);
+		closestP = b;
+		return V3Dot(vv, vv);
+	}
+
+	//check if p in vertex region outside c
+	const BoolV con20 = FIsGrtrOrEq(d6, zero);
+	const BoolV con21 = FIsGrtrOrEq(d6, d5); 
+	const BoolV con2 = BAnd(con20, con21); // vertex region c
+	if(BAllEqTTTT(con2))
+	{
+		u = zero;
+		v = one;
+		const Vec3V vv = V3Sub(p, c);
+		closestP = c;
+		return V3Dot(vv, vv);
+	}
+
+	//check if p in edge region of AB
+	const FloatV vc = FSub(FMul(d1, d4), FMul(d3, d2));
+	
+	const BoolV con30 = FIsGrtr(zero, vc);
+	const BoolV con31 = FIsGrtrOrEq(d1, zero);
+	const BoolV con32 = FIsGrtr(zero, d3);
+	const BoolV con3 = BAnd(con30, BAnd(con31, con32));
+	if(BAllEqTTTT(con3))
+	{
+		const FloatV sScale = FDiv(d1, FSub(d1, d3));
+		const Vec3V closest3 = V3Add(a, V3Scale(ab, sScale));
+		u = sScale;
+		v = zero;
+		const Vec3V vv = V3Sub(p, closest3);
+		closestP = closest3;
+		return V3Dot(vv, vv);
+	}
+
+	//check if p in edge region of BC
+	const FloatV va = FSub(FMul(d3, d6),FMul(d5, d4));
+	const BoolV con40 = FIsGrtr(zero, va);
+	const BoolV con41 = FIsGrtrOrEq(d4, d3);
+	const BoolV con42 = FIsGrtrOrEq(d5, d6);
+	const BoolV con4 = BAnd(con40, BAnd(con41, con42)); 
+	if(BAllEqTTTT(con4))
+	{
+		const FloatV uScale = FDiv(unom, FAdd(unom, udenom));
+		const Vec3V closest4 = V3Add(b, V3Scale(bc, uScale));
+		u = FSub(one, uScale);
+		v = uScale;
+		const Vec3V vv = V3Sub(p, closest4);
+		closestP = closest4;
+		return V3Dot(vv, vv);
+	}
+
+	//check if p in edge region of AC
+	const FloatV vb = FSub(FMul(d5, d2), FMul(d1, d6));
+	const BoolV con50 = FIsGrtr(zero, vb);
+	const BoolV con51 = FIsGrtrOrEq(d2, zero);
+	const BoolV con52 = FIsGrtr(zero, d6);
+	const BoolV con5 = BAnd(con50, BAnd(con51, con52));
+	if(BAllEqTTTT(con5))
+	{
+		const FloatV tScale = FDiv(d2, FSub(d2, d6));
+		const Vec3V closest5 = V3Add(a, V3Scale(ac, tScale));
+		u = zero;
+		v = tScale;
+		const Vec3V vv = V3Sub(p, closest5);
+		closestP = closest5;
+		return V3Dot(vv, vv);
+	}
+
+	//P must project inside face region. Compute Q using Barycentric coordinates
+	const FloatV denom = FRecip(FAdd(va, FAdd(vb, vc)));
+	const FloatV t = FMul(vb, denom);
+	const FloatV w = FMul(vc, denom);
+	const Vec3V bCom = V3Scale(ab, t);
+	const Vec3V cCom = V3Scale(ac, w);
+	const Vec3V closest6 = V3Add(a, V3Add(bCom, cCom));
+	u = t;
+	v = w;
+	closestP = closest6;
+
+	const Vec3V vv = V3Sub(p, closest6);
+
+	return V3Dot(vv, vv);
+}

physx/source/geomutils/src/distance/GuDistancePointTriangle.h

@@ -0,0 +1,125 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_DISTANCE_POINT_TRIANGLE_H
+#define GU_DISTANCE_POINT_TRIANGLE_H
+
+#include "foundation/PxVec3.h"
+#include "common/PxPhysXCommonConfig.h"
+#include "CmPhysXCommon.h"
+
+namespace physx
+{
+namespace Gu
+{
+	// PT: special version:
+	// - inlined
+	// - doesn't compute (s,t) output params
+	// - expects precomputed edges in input
+	PX_FORCE_INLINE PxVec3 closestPtPointTriangle2(const PxVec3& p, const PxVec3& a, const PxVec3& b, const PxVec3& c, const PxVec3& ab, const PxVec3& ac)
+	{
+		// Check if P in vertex region outside A
+		//const PxVec3 ab = b - a;
+		//const PxVec3 ac = c - a;
+		const PxVec3 ap = p - a;
+		const float d1 = ab.dot(ap);
+		const float d2 = ac.dot(ap);
+		if(d1<=0.0f && d2<=0.0f)
+			return a;	// Barycentric coords 1,0,0
+
+		// Check if P in vertex region outside B
+		const PxVec3 bp = p - b;
+		const float d3 = ab.dot(bp);
+		const float d4 = ac.dot(bp);
+		if(d3>=0.0f && d4<=d3)
+			return b;	// Barycentric coords 0,1,0
+
+		// Check if P in edge region of AB, if so return projection of P onto AB
+		const float vc = d1*d4 - d3*d2;
+		if(vc<=0.0f && d1>=0.0f && d3<=0.0f)
+		{
+			const float v = d1 / (d1 - d3);
+			return a + v * ab;	// barycentric coords (1-v, v, 0)
+		}
+
+		// Check if P in vertex region outside C
+		const PxVec3 cp = p - c;
+		const float d5 = ab.dot(cp);
+		const float d6 = ac.dot(cp);
+		if(d6>=0.0f && d5<=d6)
+			return c;	// Barycentric coords 0,0,1
+
+		// Check if P in edge region of AC, if so return projection of P onto AC
+		const float vb = d5*d2 - d1*d6;
+		if(vb<=0.0f && d2>=0.0f && d6<=0.0f)
+		{
+			const float w = d2 / (d2 - d6);
+			return a + w * ac;	// barycentric coords (1-w, 0, w)
+		}
+
+		// Check if P in edge region of BC, if so return projection of P onto BC
+		const float va = d3*d6 - d5*d4;
+		if(va<=0.0f && (d4-d3)>=0.0f && (d5-d6)>=0.0f)
+		{
+			const float w = (d4-d3) / ((d4 - d3) + (d5-d6));
+			return b + w * (c-b);	// barycentric coords (0, 1-w, w)
+		}
+
+		// P inside face region. Compute Q through its barycentric coords (u,v,w)
+		const float denom = 1.0f / (va + vb + vc);
+		const float v = vb * denom;
+		const float w = vc * denom;
+		return a + ab*v + ac*w;
+	}
+
+	PX_PHYSX_COMMON_API PxVec3 closestPtPointTriangle(const PxVec3& p, const PxVec3& a, const PxVec3& b, const PxVec3& c, float& s, float& t);
+
+	PX_FORCE_INLINE PxReal distancePointTriangleSquared(const PxVec3& point, 
+														const PxVec3& triangleOrigin, 
+														const PxVec3& triangleEdge0, 
+														const PxVec3& triangleEdge1,
+														PxReal* param0=NULL, 
+														PxReal* param1=NULL)
+	{
+		const PxVec3 pt0 = triangleEdge0 + triangleOrigin;
+		const PxVec3 pt1 = triangleEdge1 + triangleOrigin;
+		float s,t;
+		const PxVec3 cp = closestPtPointTriangle(point, triangleOrigin, pt0, pt1, s, t);
+		if(param0)
+			*param0 = s;
+		if(param1)
+			*param1 = t;
+		return (cp - point).magnitudeSquared();
+	}
+
+} // namespace Gu
+
+}
+
+#endif

physx/source/geomutils/src/distance/GuDistancePointTriangleSIMD.h

@@ -0,0 +1,54 @@
+//
+// 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-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef GU_DISTANCE_POINT_TRIANGLE_SIMD_H
+#define GU_DISTANCE_POINT_TRIANGLE_SIMD_H
+
+#include "common/PxPhysXCommonConfig.h"
+#include "CmPhysXCommon.h"
+#include "PsVecMath.h"
+
+namespace physx
+{
+namespace Gu
+{
+
+	PX_PHYSX_COMMON_API Ps::aos::FloatV distancePointTriangleSquared(	const Ps::aos::Vec3VArg point, 
+																		const Ps::aos::Vec3VArg a, 
+																		const Ps::aos::Vec3VArg b, 
+																		const Ps::aos::Vec3VArg c,
+																		Ps::aos::FloatV& u,
+																		Ps::aos::FloatV& v,
+																		Ps::aos::Vec3V& closestP); 
+
+} // namespace Gu
+
+}
+
+#endif

physx/source/geomutils/src/distance/GuDistanceSegmentBox.cpp

@@ -0,0 +1,549 @@
+//
+// 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-2021 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 "GuDistanceSegmentBox.h"
+#include "GuDistancePointBox.h"
+#include "GuDistanceSegmentSegment.h"
+#include "GuDistancePointSegment.h"
+#include "GuIntersectionRayBox.h"
+
+using namespace physx;
+
+static void face(unsigned int i0, unsigned int i1, unsigned int i2, PxVec3& rkPnt, const PxVec3& rkDir, const PxVec3& extents, const PxVec3& rkPmE, PxReal* pfLParam, PxReal& rfSqrDistance)
+{
+	PxVec3 kPpE;
+	PxReal fLSqr, fInv, fTmp, fParam, fT, fDelta;
+
+	kPpE[i1] = rkPnt[i1] + extents[i1];
+	kPpE[i2] = rkPnt[i2] + extents[i2];
+	if(rkDir[i0]*kPpE[i1] >= rkDir[i1]*rkPmE[i0])
+	{
+		if(rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0])
+		{
+			// v[i1] >= -e[i1], v[i2] >= -e[i2] (distance = 0)
+			if(pfLParam)
+			{
+				rkPnt[i0] = extents[i0];
+				fInv = 1.0f/rkDir[i0];
+				rkPnt[i1] -= rkDir[i1]*rkPmE[i0]*fInv;
+				rkPnt[i2] -= rkDir[i2]*rkPmE[i0]*fInv;
+				*pfLParam = -rkPmE[i0]*fInv;
+			}
+		}
+		else
+		{
+			// v[i1] >= -e[i1], v[i2] < -e[i2]
+			fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i2]*rkDir[i2];
+			fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] + rkDir[i2]*kPpE[i2]);
+			if(fTmp <= 2.0f*fLSqr*extents[i1])
+			{
+				fT = fTmp/fLSqr;
+				fLSqr += rkDir[i1]*rkDir[i1];
+				fTmp = kPpE[i1] - fT;
+				fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp + rkDir[i2]*kPpE[i2];
+				fParam = -fDelta/fLSqr;
+				rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp + kPpE[i2]*kPpE[i2] + fDelta*fParam;
+
+				if(pfLParam)
+				{
+					*pfLParam = fParam;
+					rkPnt[i0] = extents[i0];
+					rkPnt[i1] = fT - extents[i1];
+					rkPnt[i2] = -extents[i2];
+				}
+			}
+			else
+			{
+				fLSqr += rkDir[i1]*rkDir[i1];
+				fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] + rkDir[i2]*kPpE[i2];
+				fParam = -fDelta/fLSqr;
+				rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam;
+
+				if(pfLParam)
+				{
+					*pfLParam = fParam;
+					rkPnt[i0] = extents[i0];
+					rkPnt[i1] = extents[i1];
+					rkPnt[i2] = -extents[i2];
+				}
+			}
+		}
+	}
+	else
+	{
+		if ( rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0] )
+		{
+			// v[i1] < -e[i1], v[i2] >= -e[i2]
+			fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1];
+			fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1]);
+			if(fTmp <= 2.0f*fLSqr*extents[i2])
+			{
+				fT = fTmp/fLSqr;
+				fLSqr += rkDir[i2]*rkDir[i2];
+				fTmp = kPpE[i2] - fT;
+				fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*fTmp;
+				fParam = -fDelta/fLSqr;
+				rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + fTmp*fTmp + fDelta*fParam;
+
+				if(pfLParam)
+				{
+					*pfLParam = fParam;
+					rkPnt[i0] = extents[i0];
+					rkPnt[i1] = -extents[i1];
+					rkPnt[i2] = fT - extents[i2];
+				}
+			}
+			else
+			{
+				fLSqr += rkDir[i2]*rkDir[i2];
+				fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*rkPmE[i2];
+				fParam = -fDelta/fLSqr;
+				rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + rkPmE[i2]*rkPmE[i2] + fDelta*fParam;
+
+				if(pfLParam)
+				{
+					*pfLParam = fParam;
+					rkPnt[i0] = extents[i0];
+					rkPnt[i1] = -extents[i1];
+					rkPnt[i2] = extents[i2];
+				}
+			}
+		}
+		else
+		{
+			// v[i1] < -e[i1], v[i2] < -e[i2]
+			fLSqr = rkDir[i0]*rkDir[i0]+rkDir[i2]*rkDir[i2];
+			fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] + rkDir[i2]*kPpE[i2]);
+			if(fTmp >= 0.0f)
+			{
+				// v[i1]-edge is closest
+				if ( fTmp <= 2.0f*fLSqr*extents[i1] )
+				{
+					fT = fTmp/fLSqr;
+					fLSqr += rkDir[i1]*rkDir[i1];
+					fTmp = kPpE[i1] - fT;
+					fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp + rkDir[i2]*kPpE[i2];
+					fParam = -fDelta/fLSqr;
+					rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp + kPpE[i2]*kPpE[i2] + fDelta*fParam;
+
+					if(pfLParam)
+					{
+						*pfLParam = fParam;
+						rkPnt[i0] = extents[i0];
+						rkPnt[i1] = fT - extents[i1];
+						rkPnt[i2] = -extents[i2];
+					}
+				}
+				else
+				{
+					fLSqr += rkDir[i1]*rkDir[i1];
+					fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] + rkDir[i2]*kPpE[i2];
+					fParam = -fDelta/fLSqr;
+					rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam;
+
+					if(pfLParam)
+					{
+						*pfLParam = fParam;
+						rkPnt[i0] = extents[i0];
+						rkPnt[i1] = extents[i1];
+						rkPnt[i2] = -extents[i2];
+					}
+				}
+				return;
+			}
+
+			fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1];
+			fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1]);
+			if(fTmp >= 0.0f)
+			{
+				// v[i2]-edge is closest
+				if(fTmp <= 2.0f*fLSqr*extents[i2])
+				{
+					fT = fTmp/fLSqr;
+					fLSqr += rkDir[i2]*rkDir[i2];
+					fTmp = kPpE[i2] - fT;
+					fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*fTmp;
+					fParam = -fDelta/fLSqr;
+					rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + fTmp*fTmp + fDelta*fParam;
+
+					if(pfLParam)
+					{
+						*pfLParam = fParam;
+						rkPnt[i0] = extents[i0];
+						rkPnt[i1] = -extents[i1];
+						rkPnt[i2] = fT - extents[i2];
+					}
+				}
+				else
+				{
+					fLSqr += rkDir[i2]*rkDir[i2];
+					fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*rkPmE[i2];
+					fParam = -fDelta/fLSqr;
+					rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + rkPmE[i2]*rkPmE[i2] + fDelta*fParam;
+
+					if(pfLParam)
+					{
+						*pfLParam = fParam;
+						rkPnt[i0] = extents[i0];
+						rkPnt[i1] = -extents[i1];
+						rkPnt[i2] = extents[i2];
+					}
+				}
+				return;
+			}
+
+			// (v[i1],v[i2])-corner is closest
+			fLSqr += rkDir[i2]*rkDir[i2];
+			fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*kPpE[i2];
+			fParam = -fDelta/fLSqr;
+			rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam;
+
+			if(pfLParam)
+			{
+				*pfLParam = fParam;
+				rkPnt[i0] = extents[i0];
+				rkPnt[i1] = -extents[i1];
+				rkPnt[i2] = -extents[i2];
+			}
+		}
+	}
+}
+
+static void caseNoZeros(PxVec3& rkPnt, const PxVec3& rkDir, const PxVec3& extents, PxReal* pfLParam, PxReal& rfSqrDistance)
+{
+	PxVec3 kPmE(rkPnt.x - extents.x, rkPnt.y - extents.y, rkPnt.z - extents.z);
+
+	PxReal fProdDxPy, fProdDyPx, fProdDzPx, fProdDxPz, fProdDzPy, fProdDyPz;
+
+	fProdDxPy = rkDir.x*kPmE.y;
+	fProdDyPx = rkDir.y*kPmE.x;
+	if(fProdDyPx >= fProdDxPy)
+	{
+		fProdDzPx = rkDir.z*kPmE.x;
+		fProdDxPz = rkDir.x*kPmE.z;
+		if(fProdDzPx >= fProdDxPz)
+		{
+			// line intersects x = e0
+			face(0, 1, 2, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
+		}
+		else
+		{
+			// line intersects z = e2
+			face(2, 0, 1, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
+		}
+	}
+	else
+	{
+		fProdDzPy = rkDir.z*kPmE.y;
+		fProdDyPz = rkDir.y*kPmE.z;
+		if(fProdDzPy >= fProdDyPz)
+		{
+			// line intersects y = e1
+			face(1, 2, 0, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
+		}
+		else
+		{
+			// line intersects z = e2
+			face(2, 0, 1, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
+		}
+	}
+}
+
+static void case0(unsigned int i0, unsigned int i1, unsigned int i2, PxVec3& rkPnt, const PxVec3& rkDir, const PxVec3& extents, PxReal* pfLParam, PxReal& rfSqrDistance)
+{
+	PxReal fPmE0 = rkPnt[i0] - extents[i0];
+	PxReal fPmE1 = rkPnt[i1] - extents[i1];
+	PxReal fProd0 = rkDir[i1]*fPmE0;
+	PxReal fProd1 = rkDir[i0]*fPmE1;
+	PxReal fDelta, fInvLSqr, fInv;
+
+	if(fProd0 >= fProd1)
+	{
+		// line intersects P[i0] = e[i0]
+		rkPnt[i0] = extents[i0];
+
+		PxReal fPpE1 = rkPnt[i1] + extents[i1];
+		fDelta = fProd0 - rkDir[i0]*fPpE1;
+		if(fDelta >= 0.0f)
+		{
+			fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]);
+			rfSqrDistance += fDelta*fDelta*fInvLSqr;
+			if(pfLParam)
+			{
+				rkPnt[i1] = -extents[i1];
+				*pfLParam = -(rkDir[i0]*fPmE0+rkDir[i1]*fPpE1)*fInvLSqr;
+			}
+		}
+		else
+		{
+			if(pfLParam)
+			{
+				fInv = 1.0f/rkDir[i0];
+				rkPnt[i1] -= fProd0*fInv;
+				*pfLParam = -fPmE0*fInv;
+			}
+		}
+	}
+	else
+	{
+		// line intersects P[i1] = e[i1]
+		rkPnt[i1] = extents[i1];
+
+		PxReal fPpE0 = rkPnt[i0] + extents[i0];
+		fDelta = fProd1 - rkDir[i1]*fPpE0;
+		if(fDelta >= 0.0f)
+		{
+			fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]);
+			rfSqrDistance += fDelta*fDelta*fInvLSqr;
+			if(pfLParam)
+			{
+				rkPnt[i0] = -extents[i0];
+				*pfLParam = -(rkDir[i0]*fPpE0+rkDir[i1]*fPmE1)*fInvLSqr;
+			}
+		}
+		else
+		{
+			if(pfLParam)
+			{
+				fInv = 1.0f/rkDir[i1];
+				rkPnt[i0] -= fProd1*fInv;
+				*pfLParam = -fPmE1*fInv;
+			}
+		}
+	}
+
+	if(rkPnt[i2] < -extents[i2])
+	{
+		fDelta = rkPnt[i2] + extents[i2];
+		rfSqrDistance += fDelta*fDelta;
+		rkPnt[i2] = -extents[i2];
+	}
+	else if ( rkPnt[i2] > extents[i2] )
+	{
+		fDelta = rkPnt[i2] - extents[i2];
+		rfSqrDistance += fDelta*fDelta;
+		rkPnt[i2] = extents[i2];
+	}
+}
+
+static void case00(unsigned int i0, unsigned int i1, unsigned int i2, PxVec3& rkPnt, const PxVec3& rkDir, const PxVec3& extents, PxReal* pfLParam, PxReal& rfSqrDistance)
+{
+	PxReal fDelta;
+
+	if(pfLParam)
+		*pfLParam = (extents[i0] - rkPnt[i0])/rkDir[i0];
+
+	rkPnt[i0] = extents[i0];
+
+	if(rkPnt[i1] < -extents[i1])
+	{
+		fDelta = rkPnt[i1] + extents[i1];
+		rfSqrDistance += fDelta*fDelta;
+		rkPnt[i1] = -extents[i1];
+	}
+	else if(rkPnt[i1] > extents[i1])
+	{
+		fDelta = rkPnt[i1] - extents[i1];
+		rfSqrDistance += fDelta*fDelta;
+		rkPnt[i1] = extents[i1];
+	}
+
+	if(rkPnt[i2] < -extents[i2])
+	{
+		fDelta = rkPnt[i2] + extents[i2];
+		rfSqrDistance += fDelta*fDelta;
+		rkPnt[i2] = -extents[i2];
+	}
+	else if(rkPnt[i2] > extents[i2])
+	{
+		fDelta = rkPnt[i2] - extents[i2];
+		rfSqrDistance += fDelta*fDelta;
+		rkPnt[i2] = extents[i2];
+	}
+}
+
+static void case000(PxVec3& rkPnt, const PxVec3& extents, PxReal& rfSqrDistance)
+{
+	PxReal fDelta;
+
+	if(rkPnt.x < -extents.x)
+	{
+		fDelta = rkPnt.x + extents.x;
+		rfSqrDistance += fDelta*fDelta;
+		rkPnt.x = -extents.x;
+	}
+	else if(rkPnt.x > extents.x)
+	{
+		fDelta = rkPnt.x - extents.x;
+		rfSqrDistance += fDelta*fDelta;
+		rkPnt.x = extents.x;
+	}
+
+	if(rkPnt.y < -extents.y)
+	{
+		fDelta = rkPnt.y + extents.y;
+		rfSqrDistance += fDelta*fDelta;
+		rkPnt.y = -extents.y;
+	}
+	else if(rkPnt.y > extents.y)
+	{
+		fDelta = rkPnt.y - extents.y;
+		rfSqrDistance += fDelta*fDelta;
+		rkPnt.y = extents.y;
+	}
+
+	if(rkPnt.z < -extents.z)
+	{
+		fDelta = rkPnt.z + extents.z;
+		rfSqrDistance += fDelta*fDelta;
+		rkPnt.z = -extents.z;
+	}
+	else if(rkPnt.z > extents.z)
+	{
+		fDelta = rkPnt.z - extents.z;
+		rfSqrDistance += fDelta*fDelta;
+		rkPnt.z = extents.z;
+	}
+}
+
+//! Compute the smallest distance from the (infinite) line to the box.
+static PxReal distanceLineBoxSquared(const PxVec3& lineOrigin, const PxVec3& lineDirection,
+								  const PxVec3& boxOrigin, const PxVec3& boxExtent, const PxMat33& boxBase,
+								  PxReal* lineParam,
+								  PxVec3* boxParam)
+{
+	const PxVec3& axis0 = boxBase.column0;
+	const PxVec3& axis1 = boxBase.column1;
+	const PxVec3& axis2 = boxBase.column2;
+	
+	// compute coordinates of line in box coordinate system
+	const PxVec3 diff = lineOrigin - boxOrigin;
+	PxVec3 pnt(diff.dot(axis0), diff.dot(axis1), diff.dot(axis2));
+	PxVec3 dir(lineDirection.dot(axis0), lineDirection.dot(axis1), lineDirection.dot(axis2));
+
+	// Apply reflections so that direction vector has nonnegative components.
+	bool reflect[3];
+	for(unsigned int i=0;i<3;i++)
+	{
+		if(dir[i]<0.0f)
+		{
+			pnt[i] = -pnt[i];
+			dir[i] = -dir[i];
+			reflect[i] = true;
+		}
+		else
+		{
+			reflect[i] = false;
+		}
+	}
+
+	PxReal sqrDistance = 0.0f;
+
+	if(dir.x>0.0f)
+	{
+		if(dir.y>0.0f)
+		{
+			if(dir.z>0.0f)	caseNoZeros(pnt, dir, boxExtent, lineParam, sqrDistance);		// (+,+,+)
+			else			case0(0, 1, 2, pnt, dir, boxExtent, lineParam, sqrDistance);	// (+,+,0)
+		}
+		else
+		{
+			if(dir.z>0.0f)	case0(0, 2, 1, pnt, dir, boxExtent, lineParam, sqrDistance);	// (+,0,+)
+			else			case00(0, 1, 2, pnt, dir, boxExtent, lineParam, sqrDistance);	// (+,0,0)
+		}
+	}
+	else
+	{
+		if(dir.y>0.0f)
+		{
+			if(dir.z>0.0f)	case0(1, 2, 0, pnt, dir, boxExtent, lineParam, sqrDistance);	// (0,+,+)
+			else			case00(1, 0, 2, pnt, dir, boxExtent, lineParam, sqrDistance);	// (0,+,0)
+		}
+		else
+		{
+			if(dir.z>0.0f)	case00(2, 0, 1, pnt, dir, boxExtent, lineParam, sqrDistance);	// (0,0,+)
+			else
+			{
+				case000(pnt, boxExtent, sqrDistance);										// (0,0,0)
+				if(lineParam)
+					*lineParam = 0.0f;
+			}
+		}
+	}
+
+	if(boxParam)
+	{
+		// undo reflections
+		for(unsigned int i=0;i<3;i++)
+		{
+			if(reflect[i])
+				pnt[i] = -pnt[i];
+		}
+
+		*boxParam = pnt;
+	}
+
+	return sqrDistance;
+}
+
+//! Compute the smallest distance from the (finite) line segment to the box.
+PxReal Gu::distanceSegmentBoxSquared(	const PxVec3& segmentPoint0, const PxVec3& segmentPoint1,
+										const PxVec3& boxOrigin, const PxVec3& boxExtent, const PxMat33& boxBase,
+										PxReal* segmentParam,
+										PxVec3* boxParam)
+{
+	// compute coordinates of line in box coordinate system
+
+	PxReal lp;
+	PxVec3 bp;
+	PxReal sqrDistance = distanceLineBoxSquared(segmentPoint0, segmentPoint1 - segmentPoint0, boxOrigin, boxExtent, boxBase, &lp, &bp);
+	if(lp>=0.0f)
+	{
+		if(lp<=1.0f)
+		{
+			if(segmentParam)
+				*segmentParam = lp;
+			if(boxParam)
+				*boxParam = bp;
+			return sqrDistance;
+		}
+		else
+		{
+			if(segmentParam)
+				*segmentParam = 1.0f;
+			return Gu::distancePointBoxSquared(segmentPoint1, boxOrigin, boxExtent, boxBase, boxParam);
+		}
+	}
+	else
+	{
+		if(segmentParam)
+			*segmentParam = 0.0f;
+		return Gu::distancePointBoxSquared(segmentPoint0, boxOrigin, boxExtent, boxBase, boxParam);
+	}
+}

physx/source/geomutils/src/distance/GuDistanceSegmentSegment.cpp

@@ -0,0 +1,576 @@
+//
+// 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-2021 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 "GuDistanceSegmentSegment.h"
+#include "GuDistanceSegmentSegmentSIMD.h"
+
+using namespace physx;
+using namespace Ps;
+using namespace aos;
+
+static const float ZERO_TOLERANCE = 1e-06f;
+
+// S0 = origin + extent * dir;
+// S1 = origin - extent * dir;
+PxReal Gu::distanceSegmentSegmentSquared(	const PxVec3& origin0, const PxVec3& dir0, PxReal extent0,
+											const PxVec3& origin1, const PxVec3& dir1, PxReal extent1,
+											PxReal* param0,  PxReal* param1)
+{
+    const PxVec3 kDiff	= origin0 - origin1;
+    const PxReal fA01	= -dir0.dot(dir1);
+    const PxReal fB0	= kDiff.dot(dir0);
+    const PxReal fB1	= -kDiff.dot(dir1);
+	const PxReal fC		= kDiff.magnitudeSquared();
+	const PxReal fDet	= PxAbs(1.0f - fA01*fA01);
+    PxReal fS0, fS1, fSqrDist, fExtDet0, fExtDet1, fTmpS0, fTmpS1;
+
+    if (fDet >= ZERO_TOLERANCE)
+    {
+        // segments are not parallel
+        fS0 = fA01*fB1-fB0;
+        fS1 = fA01*fB0-fB1;
+        fExtDet0 = extent0*fDet;
+        fExtDet1 = extent1*fDet;
+
+        if (fS0 >= -fExtDet0)
+        {
+            if (fS0 <= fExtDet0)
+            {
+                if (fS1 >= -fExtDet1)
+                {
+                    if (fS1 <= fExtDet1)  // region 0 (interior)
+                    {
+                        // minimum at two interior points of 3D lines
+                        PxReal fInvDet = 1.0f/fDet;
+                        fS0 *= fInvDet;
+                        fS1 *= fInvDet;
+                        fSqrDist = fS0*(fS0+fA01*fS1+2.0f*fB0) + fS1*(fA01*fS0+fS1+2.0f*fB1)+fC;
+                    }
+                    else  // region 3 (side)
+                    {
+                        fS1 = extent1;
+                        fTmpS0 = -(fA01*fS1+fB0);
+                        if (fTmpS0 < -extent0)
+                        {
+                            fS0 = -extent0;
+                            fSqrDist = fS0*(fS0-2.0f*fTmpS0) + fS1*(fS1+2.0f*fB1)+fC;
+                        }
+                        else if (fTmpS0 <= extent0)
+                        {
+                            fS0 = fTmpS0;
+                            fSqrDist = -fS0*fS0+fS1*(fS1+2.0f*fB1)+fC;
+                        }
+                        else
+                        {
+                            fS0 = extent0;
+                            fSqrDist = fS0*(fS0-2.0f*fTmpS0) + fS1*(fS1+2.0f*fB1)+fC;
+                        }
+                    }
+                }
+                else  // region 7 (side)
+                {
+                    fS1 = -extent1;
+                    fTmpS0 = -(fA01*fS1+fB0);
+                    if (fTmpS0 < -extent0)
+                    {
+                        fS0 = -extent0;
+                        fSqrDist = fS0*(fS0-2.0f*fTmpS0) + fS1*(fS1+2.0f*fB1)+fC;
+                    }
+                    else if (fTmpS0 <= extent0)
+                    {
+                        fS0 = fTmpS0;
+                        fSqrDist = -fS0*fS0+fS1*(fS1+2.0f*fB1)+fC;
+                    }
+                    else
+                    {
+                        fS0 = extent0;
+                        fSqrDist = fS0*(fS0-2.0f*fTmpS0) + fS1*(fS1+2.0f*fB1)+fC;
+                    }
+                }
+            }
+            else
+            {
+                if (fS1 >= -fExtDet1)
+                {
+                    if (fS1 <= fExtDet1)  // region 1 (side)
+                    {
+                        fS0 = extent0;
+                        fTmpS1 = -(fA01*fS0+fB1);
+                        if (fTmpS1 < -extent1)
+                        {
+                            fS1 = -extent1;
+                            fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
+                        }
+                        else if (fTmpS1 <= extent1)
+                        {
+                            fS1 = fTmpS1;
+                            fSqrDist = -fS1*fS1+fS0*(fS0+2.0f*fB0)+fC;
+                        }
+                        else
+                        {
+                            fS1 = extent1;
+                            fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
+                        }
+                    }
+                    else  // region 2 (corner)
+                    {
+                        fS1 = extent1;
+                        fTmpS0 = -(fA01*fS1+fB0);
+                        if (fTmpS0 < -extent0)
+                        {
+                            fS0 = -extent0;
+                            fSqrDist = fS0*(fS0-2.0f*fTmpS0) + fS1*(fS1+2.0f*fB1)+fC;
+                        }
+                        else if (fTmpS0 <= extent0)
+                        {
+                            fS0 = fTmpS0;
+                            fSqrDist = -fS0*fS0+fS1*(fS1+2.0f*fB1)+fC;
+                        }
+                        else
+                        {
+                            fS0 = extent0;
+                            fTmpS1 = -(fA01*fS0+fB1);
+                            if (fTmpS1 < -extent1)
+                            {
+                                fS1 = -extent1;
+                                fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
+                            }
+                            else if (fTmpS1 <= extent1)
+                            {
+                                fS1 = fTmpS1;
+                                fSqrDist = -fS1*fS1+fS0*(fS0+2.0f*fB0) + fC;
+                            }
+                            else
+                            {
+                                fS1 = extent1;
+                                fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
+                            }
+                        }
+                    }
+                }
+                else  // region 8 (corner)
+                {
+                    fS1 = -extent1;
+                    fTmpS0 = -(fA01*fS1+fB0);
+                    if (fTmpS0 < -extent0)
+                    {
+                        fS0 = -extent0;
+                        fSqrDist = fS0*(fS0-2.0f*fTmpS0) + fS1*(fS1+2.0f*fB1)+fC;
+                    }
+                    else if (fTmpS0 <= extent0)
+                    {
+                        fS0 = fTmpS0;
+                        fSqrDist = -fS0*fS0+fS1*(fS1+2.0f*fB1)+fC;
+                    }
+                    else
+                    {
+                        fS0 = extent0;
+                        fTmpS1 = -(fA01*fS0+fB1);
+                        if (fTmpS1 > extent1)
+                        {
+                            fS1 = extent1;
+                            fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
+                        }
+                        else if (fTmpS1 >= -extent1)
+                        {
+                            fS1 = fTmpS1;
+                            fSqrDist = -fS1*fS1+fS0*(fS0+2.0f*fB0) + fC;
+                        }
+                        else
+                        {
+                            fS1 = -extent1;
+                            fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
+                        }
+                    }
+                }
+            }
+        }
+        else 
+        {
+            if (fS1 >= -fExtDet1)
+            {
+                if (fS1 <= fExtDet1)  // region 5 (side)
+                {
+                    fS0 = -extent0;
+                    fTmpS1 = -(fA01*fS0+fB1);
+                    if (fTmpS1 < -extent1)
+                    {
+                        fS1 = -extent1;
+                        fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
+                    }
+                    else if (fTmpS1 <= extent1)
+                    {
+                        fS1 = fTmpS1;
+                        fSqrDist = -fS1*fS1+fS0*(fS0+2.0f*fB0)+fC;
+                    }
+                    else
+                    {
+                        fS1 = extent1;
+                        fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
+                    }
+                }
+                else  // region 4 (corner)
+                {
+                    fS1 = extent1;
+                    fTmpS0 = -(fA01*fS1+fB0);
+                    if (fTmpS0 > extent0)
+                    {
+                        fS0 = extent0;
+                        fSqrDist = fS0*(fS0-2.0f*fTmpS0) + fS1*(fS1+2.0f*fB1)+fC;
+                    }
+                    else if (fTmpS0 >= -extent0)
+                    {
+                        fS0 = fTmpS0;
+                        fSqrDist = -fS0*fS0+fS1*(fS1+2.0f*fB1)+fC;
+                    }
+                    else
+                    {
+                        fS0 = -extent0;
+                        fTmpS1 = -(fA01*fS0+fB1);
+                        if (fTmpS1 < -extent1)
+                        {
+                            fS1 = -extent1;
+                            fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
+                        }
+                        else if (fTmpS1 <= extent1)
+                        {
+                            fS1 = fTmpS1;
+                            fSqrDist = -fS1*fS1+fS0*(fS0+2.0f*fB0) + fC;
+                        }
+                        else
+                        {
+                            fS1 = extent1;
+                            fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
+                        }
+                    }
+                }
+            }
+            else   // region 6 (corner)
+            {
+                fS1 = -extent1;
+                fTmpS0 = -(fA01*fS1+fB0);
+                if (fTmpS0 > extent0)
+                {
+                    fS0 = extent0;
+                    fSqrDist = fS0*(fS0-2.0f*fTmpS0) + fS1*(fS1+2.0f*fB1)+fC;
+                }
+                else if (fTmpS0 >= -extent0)
+                {
+                    fS0 = fTmpS0;
+                    fSqrDist = -fS0*fS0+fS1*(fS1+2.0f*fB1)+fC;
+                }
+                else
+                {
+                    fS0 = -extent0;
+                    fTmpS1 = -(fA01*fS0+fB1);
+                    if (fTmpS1 < -extent1)
+                    {
+                        fS1 = -extent1;
+                        fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
+                    }
+                    else if (fTmpS1 <= extent1)
+                    {
+                        fS1 = fTmpS1;
+                        fSqrDist = -fS1*fS1+fS0*(fS0+2.0f*fB0) + fC;
+                    }
+                    else
+                    {
+                        fS1 = extent1;
+                        fSqrDist = fS1*(fS1-2.0f*fTmpS1) + fS0*(fS0+2.0f*fB0)+fC;
+                    }
+                }
+            }
+        }
+    }
+    else
+    {
+		// The segments are parallel.
+		PxReal fE0pE1 = extent0 + extent1;
+		PxReal fSign = (fA01 > 0.0f ? -1.0f : 1.0f);
+		PxReal b0Avr = 0.5f*(fB0 - fSign*fB1);
+		PxReal fLambda = -b0Avr;
+		if(fLambda < -fE0pE1)
+		{
+			fLambda = -fE0pE1;
+		}
+		else if(fLambda > fE0pE1)
+		{
+			fLambda = fE0pE1;
+		}
+
+		fS1 = -fSign*fLambda*extent1/fE0pE1;
+		fS0 = fLambda + fSign*fS1;
+		fSqrDist = fLambda*(fLambda + 2.0f*b0Avr) + fC;
+	}
+
+	if(param0)
+		*param0 = fS0;
+	if(param1)
+		*param1 = fS1;
+
+	// account for numerical round-off error
+	return physx::intrinsics::selectMax(0.0f, fSqrDist);
+}
+
+PxReal Gu::distanceSegmentSegmentSquared(	const PxVec3& origin0, const PxVec3& extent0,
+											const PxVec3& origin1, const PxVec3& extent1,
+											PxReal* param0, 
+											PxReal* param1)
+{
+	// Some conversion is needed between the old & new code
+	// Old:
+	// segment (s0, s1)
+	// origin = s0
+	// extent = s1 - s0
+	//
+	// New:
+	// s0 = origin + extent * dir;
+	// s1 = origin - extent * dir;
+
+	// dsequeira: is this really sensible? We use a highly optimized Wild Magic routine, 
+	// then use a segment representation that requires an expensive conversion to/from...
+
+	PxVec3 dir0 = extent0;
+	const PxVec3 center0 = origin0 + extent0*0.5f;
+	PxReal length0 = extent0.magnitude();	//AM: change to make it work for degenerate (zero length) segments.
+	const bool b0 = length0 != 0.0f;
+	PxReal oneOverLength0 = 0.0f;
+	if(b0)
+	{
+		oneOverLength0 = 1.0f / length0;
+		dir0 *= oneOverLength0;
+		length0 *= 0.5f;
+	}
+
+	PxVec3 dir1 = extent1;
+	const PxVec3 center1 = origin1 + extent1*0.5f;
+	PxReal length1 = extent1.magnitude();
+	const bool b1 = length1 != 0.0f;
+	PxReal oneOverLength1 = 0.0f;
+	if(b1)
+	{
+		oneOverLength1 = 1.0f / length1;
+		dir1 *= oneOverLength1;
+		length1 *= 0.5f;
+	}
+
+	// the return param vals have -extent = s0, extent = s1
+
+	const PxReal d2 = distanceSegmentSegmentSquared(center0, dir0, length0,
+													center1, dir1, length1,
+													param0, param1);
+
+	//ML : This is wrong for some reason, I guess it has precision issue
+	//// renormalize into the 0 = s0, 1 = s1 range
+	//if (param0)
+	//	*param0 = b0 ? ((*param0) * oneOverLength0 * 0.5f + 0.5f) : 0.0f;
+	//if (param1)
+	//	*param1 = b1 ? ((*param1) * oneOverLength1 * 0.5f + 0.5f) : 0.0f;
+
+	if(param0)
+		*param0 = b0 ? ((length0 + (*param0))*oneOverLength0) : 0.0f;
+	if(param1)
+		*param1 = b1 ? ((length1 + (*param1))*oneOverLength1) : 0.0f;
+
+	return d2;
+}
+
+/*
+	S0 = origin + extent * dir;
+	S1 = origin + extent * dir;
+	dir is the vector from start to end point
+	p1 is the start point of segment1
+	d1 is the direction vector(q1 - p1)
+	p2 is the start point of segment2
+	d2 is the direction vector(q2 - p2) 
+*/
+
+FloatV Gu::distanceSegmentSegmentSquared(	const Vec3VArg p1, 
+											const Vec3VArg d1,
+											const Vec3VArg p2, 
+											const Vec3VArg d2,
+											FloatV& s, 
+											FloatV& t)
+{
+	const FloatV zero = FZero();
+	const FloatV one = FOne();
+	const FloatV eps = FEps();
+
+	const Vec3V r = V3Sub(p1, p2);
+	const Vec4V combinedDot = V3Dot4(d1, d1, d2, d2, d1, d2, d1, r);
+	const Vec4V combinedRecip = V4Sel(V4IsGrtr(combinedDot, V4Splat(eps)), V4Recip(combinedDot), V4Splat(zero));
+	const FloatV a = V4GetX(combinedDot);
+	const FloatV e = V4GetY(combinedDot);
+	const FloatV b = V4GetZ(combinedDot);
+	const FloatV c = V4GetW(combinedDot);
+	const FloatV aRecip = V4GetX(combinedRecip);//FSel(FIsGrtr(a, eps), FRecip(a), zero);
+	const FloatV eRecip = V4GetY(combinedRecip);//FSel(FIsGrtr(e, eps), FRecip(e), zero);
+
+	const FloatV f = V3Dot(d2, r);
+
+	/*
+		s = (b*f - c*e)/(a*e - b*b);
+		t = (a*f - b*c)/(a*e - b*b);
+
+		s = (b*t - c)/a;
+		t = (b*s + f)/e;
+	*/
+	
+	//if segments not parallel, the general non-degenerated case, compute closest point on two segments and clamp to segment1
+	const FloatV denom = FSub(FMul(a, e), FMul(b, b));
+	const FloatV temp = FSub(FMul(b, f), FMul(c, e));
+	const FloatV s0 = FClamp(FDiv(temp, denom), zero, one);
+	
+	//if segment is parallel, demon < eps
+	const BoolV con2 = FIsGrtr(eps, denom);//FIsEq(denom, zero);
+	const FloatV sTmp = FSel(con2, FHalf(), s0);
+	
+	//compute point on segment2 closest to segment1
+	//const FloatV tTmp = FMul(FAdd(FMul(b, sTmp), f), eRecip);
+	const FloatV tTmp = FMul(FScaleAdd(b, sTmp, f), eRecip);
+
+	//if t is in [zero, one], done. otherwise clamp t
+	const FloatV t2 = FClamp(tTmp, zero, one);
+
+	//recompute s for the new value
+	const FloatV comp = FMul(FSub(FMul(b,t2), c), aRecip);
+	const FloatV s2 = FClamp(comp, zero, one);
+
+	s = s2;
+	t = t2;
+
+	const Vec3V closest1 = V3ScaleAdd(d1, s2, p1);//V3Add(p1, V3Scale(d1, tempS));
+	const Vec3V closest2 = V3ScaleAdd(d2, t2, p2);//V3Add(p2, V3Scale(d2, tempT));
+	const Vec3V vv = V3Sub(closest1, closest2);
+	return V3Dot(vv, vv);
+}
+
+
+
+
+/*
+	segment (p, d) and segment (p02, d02)
+	segment (p, d) and segment (p12, d12)
+	segment (p, d) and segment (p22, d22)
+	segment (p, d) and segment (p32, d32)
+*/
+Vec4V Gu::distanceSegmentSegmentSquared4(	const Vec3VArg p, const Vec3VArg d0, 
+											const Vec3VArg p02, const Vec3VArg d02, 
+											const Vec3VArg p12, const Vec3VArg d12, 
+											const Vec3VArg p22, const Vec3VArg d22,
+											const Vec3VArg p32, const Vec3VArg d32,
+											Vec4V& s, Vec4V& t)
+{
+	const Vec4V zero = V4Zero();
+	const Vec4V one = V4One();
+	const Vec4V eps = V4Eps();
+	const Vec4V half = V4Splat(FHalf());
+
+	const Vec4V d0X = V4Splat(V3GetX(d0));
+	const Vec4V d0Y = V4Splat(V3GetY(d0));
+	const Vec4V d0Z = V4Splat(V3GetZ(d0));
+	const Vec4V pX  = V4Splat(V3GetX(p));
+	const Vec4V pY  = V4Splat(V3GetY(p));
+	const Vec4V pZ  = V4Splat(V3GetZ(p));
+
+	Vec4V d024 = Vec4V_From_Vec3V(d02);
+	Vec4V d124 = Vec4V_From_Vec3V(d12);
+	Vec4V d224 = Vec4V_From_Vec3V(d22);
+	Vec4V d324 = Vec4V_From_Vec3V(d32);
+
+	Vec4V p024 = Vec4V_From_Vec3V(p02);
+	Vec4V p124 = Vec4V_From_Vec3V(p12);
+	Vec4V p224 = Vec4V_From_Vec3V(p22);
+	Vec4V p324 = Vec4V_From_Vec3V(p32);
+
+	Vec4V d0123X, d0123Y, d0123Z;
+	Vec4V p0123X, p0123Y, p0123Z;
+
+	PX_TRANSPOSE_44_34(d024, d124, d224, d324, d0123X, d0123Y, d0123Z);
+	PX_TRANSPOSE_44_34(p024, p124, p224, p324, p0123X, p0123Y, p0123Z);
+
+	const Vec4V rX = V4Sub(pX, p0123X);
+	const Vec4V rY = V4Sub(pY, p0123Y);
+	const Vec4V rZ = V4Sub(pZ, p0123Z);
+
+	//TODO - store this in a transposed state and avoid so many dot products?
+
+	const FloatV dd = V3Dot(d0, d0);
+
+	const Vec4V e = V4MulAdd(d0123Z, d0123Z, V4MulAdd(d0123X, d0123X, V4Mul(d0123Y, d0123Y)));
+	const Vec4V b = V4MulAdd(d0Z, d0123Z, V4MulAdd(d0X, d0123X, V4Mul(d0Y, d0123Y)));
+	const Vec4V c = V4MulAdd(d0Z, rZ, V4MulAdd(d0X, rX, V4Mul(d0Y, rY)));
+	const Vec4V f = V4MulAdd(d0123Z, rZ, V4MulAdd(d0123X, rX, V4Mul(d0123Y, rY))); 
+
+	const Vec4V a(V4Splat(dd));
+
+	const Vec4V aRecip(V4Recip(a));
+	const Vec4V eRecip(V4Recip(e));
+
+	//if segments not parallell, compute closest point on two segments and clamp to segment1
+	const Vec4V denom = V4Sub(V4Mul(a, e), V4Mul(b, b));
+	const Vec4V temp = V4Sub(V4Mul(b, f), V4Mul(c, e));
+	const Vec4V s0 = V4Clamp(V4Div(temp, denom), zero, one);
+
+	//test whether segments are parallel
+	const BoolV con2 = V4IsGrtrOrEq(eps, denom);     
+	const Vec4V sTmp = V4Sel(con2, half, s0);
+
+	//compute point on segment2 closest to segment1
+	const Vec4V tTmp = V4Mul(V4Add(V4Mul(b, sTmp), f), eRecip);
+
+	//if t is in [zero, one], done. otherwise clamp t
+	const Vec4V t2 = V4Clamp(tTmp, zero, one);
+
+	//recompute s for the new value
+	const Vec4V comp = V4Mul(V4Sub(V4Mul(b,t2), c), aRecip);
+	const BoolV aaNearZero = V4IsGrtrOrEq(eps, a); // check if aRecip is valid (aa>eps)
+	const Vec4V s2 = V4Sel(aaNearZero, V4Zero(), V4Clamp(comp, zero, one));
+
+	/*  s = V4Sel(con0, zero, V4Sel(con1, cd, s2));
+	t = V4Sel(con1, zero, V4Sel(con0, cg, t2));  */
+	s = s2;
+	t = t2;
+
+	const Vec4V closest1X = V4MulAdd(d0X, s2, pX);
+	const Vec4V closest1Y = V4MulAdd(d0Y, s2, pY);
+	const Vec4V closest1Z = V4MulAdd(d0Z, s2, pZ);
+
+	const Vec4V closest2X = V4MulAdd(d0123X, t2, p0123X);
+	const Vec4V closest2Y = V4MulAdd(d0123Y, t2, p0123Y);
+	const Vec4V closest2Z = V4MulAdd(d0123Z, t2, p0123Z);
+
+	const Vec4V vvX = V4Sub(closest1X, closest2X);
+	const Vec4V vvY = V4Sub(closest1Y, closest2Y);
+	const Vec4V vvZ = V4Sub(closest1Z, closest2Z);
+
+	const Vec4V vd = V4MulAdd(vvX, vvX, V4MulAdd(vvY, vvY, V4Mul(vvZ, vvZ)));
+
+	return vd;
+}