From 3dfe2108cfab31ba3ee5527e217d0d8e99a51162 Mon Sep 17 00:00:00 2001
From: git perforce import user <a@b>
Date: Tue, 25 Oct 2016 12:29:14 -0600
Subject: Initial commit: PhysX 3.4.0 Update @ 21294896 APEX 1.4.0 Update @
 21275617

[CL 21300167]
---
 .../GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp | 725 +++++++++++++++++++++
 1 file changed, 725 insertions(+)
 create mode 100644 PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp

(limited to 'PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp')

diff --git a/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp
new file mode 100644
index 00000000..68354425
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/pcm/GuPCMContactGenBoxConvex.cpp
@@ -0,0 +1,725 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 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 "GuPCMContactGen.h"
+#include "GuPCMShapeConvex.h"
+#include "CmRenderOutput.h"
+#include "GuPCMContactGenUtil.h"
+#include "PsVecMath.h"
+#include "GuVecCapsule.h"
+#include "GuVecBox.h"
+
+#define PCM_USE_INTERNAL_OBJECT 1
+
+using namespace physx;
+using namespace Gu;
+using namespace Ps::aos;
+
+
+	//Precompute the convex data
+	//     7+------+6			0 = ---
+	//     /|     /|			1 = +--
+	//    / |    / |			2 = ++-
+	//   / 4+---/--+5			3 = -+-
+	// 3+------+2 /    y   z	4 = --+
+	//  | /    | /     |  /		5 = +-+
+	//  |/     |/      |/		6 = +++
+	// 0+------+1      *---x	7 = -++
+
+namespace physx
+{
+
+namespace Gu
+{
+	
+	static bool testFaceNormal(const PolygonalData& polyData0, const PolygonalData& polyData1, SupportLocal* map0, SupportLocal* map1, const PsMatTransformV& transform0To1, const PsMatTransformV& transform1To0, const FloatVArg contactDist, 
+		FloatV& minOverlap, PxU32& feature, Vec3V& faceNormal, const FeatureStatus faceStatus, FeatureStatus& status)
+	{
+		PX_UNUSED(polyData1);
+		
+		FloatV _minOverlap = FMax();//minOverlap;
+		PxU32  _feature = 0;
+		Vec3V  _faceNormal = faceNormal;
+		FloatV min0, max0;
+		FloatV min1, max1;
+	
+		const Vec3V center1To0 = transform1To0.p;
+		
+#if PCM_USE_INTERNAL_OBJECT
+		const Vec3V zeroV = V3Zero();
+		const Vec3V shapeSpaceCenter1 = V3LoadU(polyData1.mCenter);
+		const Vec3V internalCenter1In0 = transform1To0.transform(shapeSpaceCenter1);
+		const FloatV internalRadius1 = FLoad(polyData1.mInternal.mRadius);
+		const Vec3V internalExtents1 = V3LoadU(polyData1.mInternal.mExtents);
+		const Vec3V negInternalExtents1 = V3Neg(internalExtents1);
+
+#endif
+
+		//in the local space of polyData0
+		for(PxU32 i=0; i<polyData0.mNbPolygons; ++i)
+		{
+			const Gu::HullPolygonData& polygon = polyData0.mPolygons[i];
+
+			const Vec3V minVert = V3LoadU(polyData0.mVerts[polygon.mMinIndex]);
+			const FloatV planeDist = FLoad(polygon.mPlane.d);
+			const Vec3V vertexSpacePlaneNormal = V3LoadU(polygon.mPlane.n);
+
+			//transform plane n to shape space
+			const Vec3V shapeSpacePlaneNormal = M33TrnspsMulV3(map0->shape2Vertex, vertexSpacePlaneNormal);
+
+			const FloatV magnitude = FRecip(V3Length(shapeSpacePlaneNormal));
+			//ML::use this to avoid LHS
+			min0 = FMul(V3Dot(vertexSpacePlaneNormal, minVert), magnitude);
+			max0 = FMul(FNeg(planeDist), magnitude);
+
+			//normalize shape space normal
+			const Vec3V n0 = V3Scale(shapeSpacePlaneNormal, magnitude);
+
+			//calculate polyData1 projection
+			//rotate polygon's normal into the local space of polyData1
+			const Vec3V n1 = transform0To1.rotate(n0);
+
+
+#if PCM_USE_INTERNAL_OBJECT
+			
+			//test internal object	
+			//ML: we don't need to transform the normal into the vertex space. If polyData1 don't have scale,
+			//the vertex2Shape matrix will be identity, shape space normal will be the same as vertex space's normal.
+			//If polyData0 have scale, internalExtens1 will be 0.
+			const Vec3V proj = V3Sel(V3IsGrtr(n1, zeroV), internalExtents1, negInternalExtents1);
+			const FloatV radius = FMax(V3Dot(n1, proj), internalRadius1);
+			const FloatV internalTrans = V3Dot(internalCenter1In0, n0);
+
+			const FloatV _min1 = FSub(internalTrans, radius);
+			const FloatV _max1 = FAdd(internalTrans, radius);
+
+			const FloatV _min = FMax(min0, _min1);
+			const FloatV _max = FMin(max0, _max1);
+
+			const FloatV _tempOverlap = FSub(_max, _min);
+			//const FloatV _tempOverlap = FSub(max0, _min1);	
+
+			//Internal object overlaps more than current min, so can skip it
+			//because (a) it isn't a separating axis and (b) it isn't the smallest axis
+			if(FAllGrtr(_tempOverlap, _minOverlap))
+			{
+				continue;
+			}
+
+#endif
+
+			const FloatV translate = V3Dot(center1To0, n0);
+			map1->doSupport(n1, min1, max1);
+
+			min1 = FAdd(translate, min1);
+			max1 = FAdd(translate, max1);
+
+			const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
+
+			if(BAllEqTTTT(con))
+				return false;
+
+			const FloatV tempOverlap = FSub(max0, min1);
+
+			if(FAllGrtr(_minOverlap, tempOverlap))
+			{
+				_minOverlap = tempOverlap;
+				_feature = i;
+				_faceNormal = n0;
+			}
+		}   
+
+		if(FAllGrtr(minOverlap, _minOverlap))
+		{
+			faceNormal = _faceNormal; 
+			minOverlap = _minOverlap;
+			status = faceStatus;
+		}
+
+		
+
+		feature = _feature;
+
+		return true;
+
+	}
+
+
+	//plane is in the shape space of polyData
+	void buildPartialHull(const PolygonalData& polyData, SupportLocal* map, SeparatingAxes& validAxes, const Vec3VArg planeP, const Vec3VArg planeDir)
+	{		
+		const FloatV zero = FZero();
+		const Vec3V dir = V3Normalize(planeDir);
+		for(PxU32 i=0; i<polyData.mNbPolygons; ++i)
+		{
+			const Gu::HullPolygonData& polygon = polyData.mPolygons[i];
+			const PxU8* inds = polyData.mPolygonVertexRefs + polygon.mVRef8;
+			
+			Vec3V v0 = M33MulV3(map->vertex2Shape, V3LoadU_SafeReadW(polyData.mVerts[inds[0]]));	// PT: safe because of the way vertex memory is allocated in ConvexHullData
+		
+			FloatV dist0 = V3Dot(dir, V3Sub(v0, planeP));
+
+			for (PxU32 iStart = 0, iEnd = PxU32(polygon.mNbVerts - 1); iStart < polygon.mNbVerts; iEnd = iStart++)
+			{
+				const Vec3V v1 = M33MulV3(map->vertex2Shape, V3LoadU_SafeReadW(polyData.mVerts[inds[iEnd]]));	// PT: safe because of the way vertex memory is allocated in ConvexHullData
+				
+				const FloatV dist1 = V3Dot(dir, V3Sub(v1, planeP));
+
+				const BoolV con = BOr(FIsGrtr(dist0, zero), FIsGrtr(dist1, zero));
+
+				//cull edge if either of the vertex will on the positive size of the plane
+				if(BAllEqTTTT(con))
+				{
+					const Vec3V tempV = V3Sub(v0, v1); 
+					PxVec3 temp;
+					V3StoreU(tempV, temp);
+					validAxes.addAxis(temp.getNormalized());
+				}
+
+				v0 = v1;
+				dist0 = dist1;
+			}
+		}
+	}
+
+
+	static bool testEdgeNormal(const PolygonalData& polyData0, const PolygonalData& polyData1, SupportLocal* map0, SupportLocal* map1, const PsMatTransformV& transform0To1, const PsMatTransformV& transform1To0, const FloatVArg contactDist,
+		FloatV& minOverlap, Vec3V& edgeNormalIn0, const FeatureStatus edgeStatus, FeatureStatus& status)
+	{
+		
+		FloatV overlap = minOverlap;
+		FloatV min0, max0;
+		FloatV min1, max1;
+		const FloatV eps = FEps();
+
+		const Vec3V shapeSpaceCenter0 = V3LoadU(polyData0.mCenter);
+		const Vec3V shapeSpaceCenter1 = V3LoadU(polyData1.mCenter);
+
+#if PCM_USE_INTERNAL_OBJECT
+		const Vec3V zeroV = V3Zero();
+		const Vec3V internalCenter1In0 = V3Sub(transform1To0.transform(shapeSpaceCenter1), shapeSpaceCenter0);
+		const FloatV internalRadius1 = FLoad(polyData1.mInternal.mRadius);
+		const Vec3V internalExtents1 = V3LoadU(polyData1.mInternal.mExtents);
+		const Vec3V negInternalExtents1 = V3Neg(internalExtents1);
+
+		const FloatV internalRadius0 = FLoad(polyData0.mInternal.mRadius);
+		const Vec3V internalExtents0 = V3LoadU(polyData0.mInternal.mExtents);
+		const Vec3V negInternalExtents0 = V3Neg(internalExtents0);
+#endif
+
+		const Vec3V center1To0 = transform1To0.p;
+
+		//in polyData0 shape space
+		const Vec3V dir0 = V3Sub(transform1To0.transform(shapeSpaceCenter1), shapeSpaceCenter0);
+		const Vec3V support0 = map0->doSupport(dir0);
+
+		//in polyData1 shape space
+		const Vec3V dir1 = transform0To1.rotate(V3Neg(dir0));
+		const Vec3V support1 = map1->doSupport(dir1);
+		
+		const Vec3V support0In1 = transform0To1.transform(support0);
+		const Vec3V support1In0 = transform1To0.transform(support1);
+
+		SeparatingAxes mSA0;
+		SeparatingAxes mSA1;
+		mSA0.reset();
+		mSA1.reset();
+		
+		buildPartialHull(polyData0, map0, mSA0, support1In0, dir0);
+		buildPartialHull(polyData1, map1, mSA1, support0In1, dir1);
+
+		const PxVec3* axe0 = mSA0.getAxes();
+		const PxVec3* axe1 = mSA1.getAxes();
+		const PxU32 numAxe0 = mSA0.getNumAxes();
+		const PxU32 numAxe1 = mSA1.getNumAxes();
+		for(PxU32 i=0; i < numAxe0; ++i)
+		{
+			//axe0[i] is in the shape space of polyData0
+			const Vec3V v0 = V3LoadU(axe0[i]);
+
+			for(PxU32 j=0; j< numAxe1; ++j)
+			{
+				//axe1[j] is in the shape space of polyData1
+				const Vec3V v1 = V3LoadU(axe1[j]);
+
+				const Vec3V dir = V3Cross(v0, transform1To0.rotate(v1));
+				const FloatV lenSq = V3Dot(dir, dir);
+				if(FAllGrtr(eps, lenSq))
+					continue;
+
+				//n0 is in polyData0's local space
+				const Vec3V n0 = V3Scale(dir, FRsqrt(lenSq));
+				
+				//n1 is in polyData1's local space
+				const Vec3V n1 = transform0To1.rotate(n0);
+
+#if PCM_USE_INTERNAL_OBJECT
+
+				//ML: we don't need to transform the normal into the vertex space. If polyData1 don't have scale,
+				//the vertex2Shape matrix will be identity, shape space normal will be the same as vertex space's normal.
+				//If polyData0 have scale, internalExtens1 will be 0.
+				//vertex space n1
+				const Vec3V proj = V3Sel(V3IsGrtr(n1, zeroV), internalExtents1, negInternalExtents1);
+				const FloatV radius = FMax(V3Dot(n1, proj), internalRadius1);
+
+				const FloatV internalTrans = V3Dot(internalCenter1In0, n0);
+
+				const FloatV _min1 = FSub(internalTrans, radius);
+				const FloatV _max1 = FAdd(internalTrans, radius);
+
+				const Vec3V proj0 = V3Sel(V3IsGrtr(n0, zeroV), internalExtents0, negInternalExtents0);
+				const FloatV radius0 = FMax(V3Dot(n0, proj0), internalRadius0);
+
+				const FloatV _max0 = radius0;
+				const FloatV _min0 = FNeg(radius0);
+
+				PX_ASSERT(FAllGrtrOrEq(_max0, _min0));
+				PX_ASSERT(FAllGrtrOrEq(_max1, _min1));
+
+
+				const FloatV _min = FMax(_min0, _min1);
+				const FloatV _max = FMin(_max0, _max1);
+
+				const FloatV _tempOverlap = FSub(_max, _min);
+
+				//Internal object overlaps more than current min, so can skip it
+				//because (a) it isn't a separating axis and (b) it isn't the smallest axis
+				if(FAllGrtr(_tempOverlap, overlap))
+				{
+					continue;
+				}
+
+#endif
+				//get polyData0's projection
+				map0->doSupport(n0, min0, max0);
+
+				const FloatV translate = V3Dot(center1To0, n0);
+
+				//get polyData1's projection
+				map1->doSupport(n1, min1, max1);
+
+				min1 = FAdd(translate, min1);
+				max1 = FAdd(translate, max1);
+
+				const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
+				if(BAllEqTTTT(con))
+					return false;
+
+				const FloatV tempOverlap = FSub(max0, min1);
+
+#if PCM_USE_INTERNAL_OBJECT
+				PX_ASSERT(FAllGrtrOrEq(tempOverlap, _tempOverlap));
+#endif
+
+				if(FAllGrtr(overlap, tempOverlap))
+				{
+					overlap = tempOverlap;
+					edgeNormalIn0 = n0;
+					status = edgeStatus;
+				}
+			}
+		}
+
+		minOverlap = overlap;
+		
+		return true;
+
+	}
+
+
+	//contactNormal is in the space of polyData0
+	void generatedContacts(PolygonalData& polyData0, PolygonalData& polyData1, const HullPolygonData& referencePolygon, const HullPolygonData& incidentPolygon,  
+		SupportLocal* map0, SupportLocal* map1, const PsMatTransformV& transform0To1, PersistentContact* manifoldContacts, 
+		PxU32& numContacts, const FloatVArg contactDist, Cm::RenderOutput* renderOutput)
+	{
+
+		PX_UNUSED(renderOutput);
+		const FloatV zero = FZero();
+
+		const PxU8* inds0 = polyData0.mPolygonVertexRefs + referencePolygon.mVRef8;
+
+		//transform the plane normal to shape space
+		const Vec3V contactNormal = V3Normalize(M33TrnspsMulV3(map0->shape2Vertex, V3LoadU(referencePolygon.mPlane.n)));
+		
+		//this is the matrix transform all points to the 2d plane
+		const Mat33V rot = findRotationMatrixFromZAxis(contactNormal);
+
+		const PxU8* inds1 = polyData1.mPolygonVertexRefs + incidentPolygon.mVRef8;
+
+
+		Vec3V* points0In0 = reinterpret_cast<Vec3V*>(PxAllocaAligned(sizeof(Vec3V)*referencePolygon.mNbVerts, 16));
+		Vec3V* points1In0 = reinterpret_cast<Vec3V*>(PxAllocaAligned(sizeof(Vec3V)*incidentPolygon.mNbVerts, 16));
+		bool* points1In0Penetration = reinterpret_cast<bool*>(PxAlloca(sizeof(bool)*incidentPolygon.mNbVerts));
+		FloatV* points1In0TValue = reinterpret_cast<FloatV*>(PxAllocaAligned(sizeof(FloatV)*incidentPolygon.mNbVerts, 16));
+
+		//Transform all the verts from vertex space to shape space
+		map0->populateVerts(inds0, referencePolygon.mNbVerts, polyData0.mVerts, points0In0);
+		map1->populateVerts(inds1, incidentPolygon.mNbVerts, polyData1.mVerts, points1In0);
+
+#if PCM_LOW_LEVEL_DEBUG
+		Gu::PersistentContactManifold::drawPolygon(*renderOutput, map0->transform, points0In0, (PxU32)referencePolygon.mNbVerts, 0x00ff0000); 
+		Gu::PersistentContactManifold::drawPolygon(*renderOutput, map1->transform, points1In0, (PxU32)incidentPolygon.mNbVerts,  0x0000ff00); 
+#endif
+	
+		//This is used to calculate the project point when the 2D reference face points is inside the 2D incident face point
+		const Vec3V sPoint = points1In0[0]; 
+	
+		PX_ASSERT(incidentPolygon.mNbVerts <= 64);
+ 
+		Vec3V eps = Vec3V_From_FloatV(FEps());
+		Vec3V max = Vec3V_From_FloatV(FMax());
+		Vec3V nmax = V3Neg(max); 
+
+		//transform reference polygon to 2d, calculate min and max
+		Vec3V rPolygonMin= max;
+		Vec3V rPolygonMax = nmax;
+		for(PxU32 i=0; i<referencePolygon.mNbVerts; ++i)
+		{
+			points0In0[i] = M33MulV3(rot, points0In0[i]);
+			rPolygonMin = V3Min(rPolygonMin, points0In0[i]);
+			rPolygonMax = V3Max(rPolygonMax, points0In0[i]);
+		}
+		
+		rPolygonMin = V3Sub(rPolygonMin, eps);
+		rPolygonMax = V3Add(rPolygonMax, eps);
+
+		const FloatV d = V3GetZ(points0In0[0]);
+		const FloatV rd = FAdd(d, contactDist);
+
+		Vec3V iPolygonMin= max; 
+		Vec3V iPolygonMax = nmax;
+
+
+		PxU32 inside = 0;
+		for(PxU32 i=0; i<incidentPolygon.mNbVerts; ++i)
+		{
+			const Vec3V vert1 =points1In0[i]; //this still in polyData1's local space
+			const Vec3V a = transform0To1.transformInv(vert1);
+			points1In0[i] = M33MulV3(rot, a);
+			const FloatV z = V3GetZ(points1In0[i]);
+			points1In0TValue[i] = FSub(z, d);
+			points1In0[i] = V3SetZ(points1In0[i], d);
+			iPolygonMin = V3Min(iPolygonMin, points1In0[i]);
+			iPolygonMax = V3Max(iPolygonMax, points1In0[i]);
+			if(FAllGrtr(rd, z))
+			{
+				points1In0Penetration[i] = true;
+
+				if(contains(points0In0, referencePolygon.mNbVerts, points1In0[i], rPolygonMin, rPolygonMax))
+				{
+					inside++;
+
+					const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(contactNormal), points1In0TValue[i]);
+					manifoldContacts[numContacts].mLocalPointA = vert1;
+					manifoldContacts[numContacts].mLocalPointB = M33TrnspsMulV3(rot, points1In0[i]);
+					manifoldContacts[numContacts++].mLocalNormalPen = localNormalPen;
+					
+				}
+			}
+			else
+			{
+				points1In0Penetration[i] = false;
+			}
+			
+		}
+
+
+		if(inside == incidentPolygon.mNbVerts)
+		{
+			return;
+		}
+
+		inside = 0;
+		iPolygonMin = V3Sub(iPolygonMin, eps);
+		iPolygonMax = V3Add(iPolygonMax, eps);
+
+		const Vec3V incidentNormal = V3Normalize(M33TrnspsMulV3(map1->shape2Vertex, V3LoadU(incidentPolygon.mPlane.n)));
+	
+		const Vec3V contactNormalIn1 = transform0To1.rotate(contactNormal);
+
+		for(PxU32 i=0; i<referencePolygon.mNbVerts; ++i)
+		{
+			if(contains(points1In0, incidentPolygon.mNbVerts, points0In0[i], iPolygonMin, iPolygonMax))
+			{
+				//const Vec3V vert0=Vec3V_From_PxVec3(polyData0.mVerts[inds0[i]]);
+				const Vec3V vert0 = M33TrnspsMulV3(rot, points0In0[i]);
+				const Vec3V a = transform0To1.transform(vert0);
+
+				const FloatV nom = V3Dot(incidentNormal, V3Sub(sPoint, a)); 
+				const FloatV denom = V3Dot(incidentNormal, contactNormalIn1);
+				PX_ASSERT(FAllEq(denom, zero)==0);
+				const FloatV t = FDiv(nom, denom);
+
+				if(FAllGrtr(t, contactDist))
+					continue;
+
+
+				inside++;
+
+				const Vec3V projPoint = V3ScaleAdd(contactNormalIn1, t, a);
+				const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(contactNormal), t);
+		
+				manifoldContacts[numContacts].mLocalPointA = projPoint;
+				manifoldContacts[numContacts].mLocalPointB = vert0;
+				manifoldContacts[numContacts++].mLocalNormalPen = localNormalPen;
+
+			}
+		}
+
+		if(inside == referencePolygon.mNbVerts)
+			return;
+
+
+		//(2) segment intesection
+		for (PxU32 iStart = 0, iEnd = PxU32(incidentPolygon.mNbVerts - 1); iStart < incidentPolygon.mNbVerts; iEnd = iStart++)
+		{
+			if((!points1In0Penetration[iStart] && !points1In0Penetration[iEnd] ) )//|| (points1In0[i].status == POINT_OUTSIDE && points1In0[incidentIndex].status == POINT_OUTSIDE))
+				continue;
+
+	
+			const Vec3V ipA = points1In0[iStart];
+			const Vec3V ipB = points1In0[iEnd];
+
+			Vec3V ipAOri = V3SetZ(points1In0[iStart], FAdd(points1In0TValue[iStart], d));
+			Vec3V ipBOri = V3SetZ(points1In0[iEnd], FAdd(points1In0TValue[iEnd], d));
+
+			const Vec3V iMin = V3Min(ipA, ipB);
+			const Vec3V iMax = V3Max(ipA, ipB);
+			
+
+			for (PxU32 rStart = 0, rEnd = PxU32(referencePolygon.mNbVerts - 1); rStart < referencePolygon.mNbVerts; rEnd = rStart++) 
+			{
+	
+				const Vec3V rpA = points0In0[rStart];
+				const Vec3V rpB = points0In0[rEnd];
+
+				const Vec3V rMin = V3Min(rpA, rpB);
+				const Vec3V rMax = V3Max(rpA, rpB);
+				
+				const BoolV tempCon =BOr(V3IsGrtr(iMin, rMax), V3IsGrtr(rMin, iMax));
+				const BoolV con = BOr(BGetX(tempCon), BGetY(tempCon));
+		
+				if(BAllEqTTTT(con))
+					continue;
+			
+					
+				FloatV a1 = signed2DTriArea(rpA, rpB, ipA);
+				FloatV a2 = signed2DTriArea(rpA, rpB, ipB);
+
+
+				if(FAllGrtr(zero, FMul(a1, a2)))
+				{
+					FloatV a3 = signed2DTriArea(ipA, ipB, rpA);
+					FloatV a4 = signed2DTriArea(ipA, ipB, rpB);
+
+					if(FAllGrtr(zero, FMul(a3, a4)))
+					{
+					
+						//these two segment intersect
+
+						const FloatV t = FDiv(a1, FSub(a2, a1));
+
+						const Vec3V pBB = V3NegScaleSub(V3Sub(ipBOri, ipAOri), t, ipAOri); 
+						const Vec3V pAA = V3SetZ(pBB, d);
+						const Vec3V pA = M33TrnspsMulV3(rot, pAA);
+						const Vec3V pB = transform0To1.transform(M33TrnspsMulV3(rot, pBB));
+						const FloatV pen = FSub(V3GetZ(pBB), V3GetZ(pAA));
+						if(FAllGrtr(pen, contactDist))
+							continue;
+
+
+						const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(contactNormal), pen);
+						manifoldContacts[numContacts].mLocalPointA = pB;
+						manifoldContacts[numContacts].mLocalPointB = pA;
+						manifoldContacts[numContacts++].mLocalNormalPen = localNormalPen;
+					}
+				}
+			}
+
+		}
+	}
+
+
+	bool generateFullContactManifold(PolygonalData& polyData0, PolygonalData& polyData1, SupportLocal* map0, SupportLocal* map1,  PersistentContact* manifoldContacts, PxU32& numContacts,
+		const FloatVArg contactDist, const Vec3VArg normal, const Vec3VArg closestA, const Vec3VArg closestB, const FloatVArg marginA, const FloatVArg marginB, const bool doOverlapTest, 
+		Cm::RenderOutput* renderOutput, const Ps::aos::FloatVArg toleranceScale)
+	{
+	
+		const PsMatTransformV transform1To0V = map0->transform.transformInv(map1->transform);
+		const PsMatTransformV transform0To1V = map1->transform.transformInv(map0->transform);
+
+	
+		if(doOverlapTest)
+		{
+			//if gjk fail, SAT based yes/no test
+			FeatureStatus status = POLYDATA0;
+			FloatV minOverlap = FMax();
+			Vec3V minNormal = V3Zero();
+	
+			PxU32 feature0;
+			//in the local space of polyData0, minNormal is in polyData0 space
+			if(!testFaceNormal(polyData0, polyData1, map0, map1, transform0To1V, transform1To0V, contactDist, minOverlap, feature0, minNormal, POLYDATA0, status))
+				return false;
+			
+			PxU32 feature1;
+			//in the local space of polyData1, if minOverlap is overwrite inside this function, minNormal will be in polyData1 space
+			if(!testFaceNormal(polyData1, polyData0, map1, map0, transform1To0V, transform0To1V, contactDist, minOverlap, feature1, minNormal, POLYDATA1, status))
+				return false;
+
+		
+			bool doEdgeTest = false;
+			
+EdgeTest:
+			if(doEdgeTest)
+			{
+
+				if(!testEdgeNormal(polyData0, polyData1, map0, map1, transform0To1V, transform1To0V, contactDist, minOverlap, minNormal, EDGE, status))
+					return false;
+
+				if(status != EDGE)
+					return true;
+			}
+
+			
+			if(status == POLYDATA0)
+			{
+				//minNormal is in the local space of polydata0
+		
+				const HullPolygonData& referencePolygon = polyData0.mPolygons[feature0];
+				const Vec3V n = transform0To1V.rotate(minNormal);
+				const HullPolygonData& incidentPolygon = polyData1.mPolygons[getPolygonIndex(polyData1, map1, n)];
+				
+				generatedContacts(polyData0, polyData1, referencePolygon,  incidentPolygon, map0, map1, transform0To1V, manifoldContacts, numContacts, contactDist, renderOutput);
+				
+				if (numContacts > 0)
+				{
+					const Vec3V nn = V3Neg(n);
+					//flip the contacts
+					for(PxU32 i=0; i<numContacts; ++i)
+					{
+						const Vec3V localPointB = manifoldContacts[i].mLocalPointB;
+						manifoldContacts[i].mLocalPointB = manifoldContacts[i].mLocalPointA;
+						manifoldContacts[i].mLocalPointA = localPointB;
+						manifoldContacts[i].mLocalNormalPen = V4SetW(nn, V4GetW(manifoldContacts[i].mLocalNormalPen));
+					}
+				}
+			}
+			else if(status == POLYDATA1)
+			{
+				//minNormal is in the local space of polydata1
+				const HullPolygonData& referencePolygon = polyData1.mPolygons[feature1];
+				const HullPolygonData& incidentPolygon =  polyData0.mPolygons[getPolygonIndex(polyData0, map0, transform1To0V.rotate(minNormal))];
+				
+				//reference face is polyData1
+				generatedContacts(polyData1, polyData0, referencePolygon,  incidentPolygon, map1, map0, transform1To0V, manifoldContacts, numContacts, contactDist, renderOutput);
+
+			}
+			else //if(status == EDGE0)
+			{
+				//minNormal is in the local space of polydata0
+			
+				const HullPolygonData& incidentPolygon = polyData0.mPolygons[getPolygonIndex(polyData0, map0, V3Neg(minNormal))];
+				const HullPolygonData& referencePolygon = polyData1.mPolygons[getPolygonIndex(polyData1, map1, transform0To1V.rotate(minNormal))];
+				generatedContacts(polyData1, polyData0, referencePolygon,  incidentPolygon, map1, map0, transform1To0V, manifoldContacts, numContacts, contactDist, renderOutput);
+			}
+
+			if(numContacts == 0 && !doEdgeTest)
+			{
+				doEdgeTest = true;
+				goto EdgeTest;
+			}
+
+		}
+		else
+		{
+			const FloatV eps = FLoad(PCM_WITNESS_POINT_EPS);
+			const FloatV lowerEps = FMul(toleranceScale, FLoad(PCM_WITNESS_POINT_ABSOLUTE_EPS));
+			const FloatV toleranceA = FMax(FMul(marginA, eps), lowerEps);
+			
+			const FloatV toleranceB = FMax(FMul(marginB, eps), lowerEps);
+
+			//use gjk normal to get the faceIndex(status == GJK_CONTACT)
+			const PxU32 faceIndex1 = getWitnessPolygonIndex(polyData1, map1, V3Neg(normal), closestB, toleranceA);
+			const PxU32 faceIndex0 = getWitnessPolygonIndex(polyData0, map0, transform0To1V.rotateInv(normal), transform0To1V.transformInv(closestA), toleranceB);
+
+			const HullPolygonData& referencePolygon = polyData1.mPolygons[faceIndex1];
+			const HullPolygonData& incidentPolygon = polyData0.mPolygons[faceIndex0];
+			generatedContacts(polyData1, polyData0, referencePolygon, incidentPolygon, map1, map0, transform1To0V, manifoldContacts, numContacts, contactDist, renderOutput);
+			
+		}
+	
+		return true;
+	}
+
+
+	bool computeMTD(Gu::PolygonalData& polyData0, Gu::PolygonalData& polyData1,  SupportLocal* map0, SupportLocal* map1, Ps::aos::FloatV& penDepth, Ps::aos::Vec3V& normal)
+	{
+	
+		using namespace Ps::aos;
+
+		const PsMatTransformV transform1To0V = map0->transform.transformInv(map1->transform);
+		const PsMatTransformV transform0To1V = map1->transform.transformInv(map0->transform);
+	
+		FeatureStatus status = POLYDATA0;
+		FloatV minOverlap = FMax();
+		Vec3V minNormal = V3Zero();
+		const FloatV contactDist = FZero();
+
+		PxU32 feature0;
+		//in the local space of polyData0, minNormal is in polyData0 space
+		if(!testFaceNormal(polyData0, polyData1, map0, map1, transform0To1V, transform1To0V, contactDist, minOverlap, feature0, minNormal, POLYDATA0, status))
+			return false;
+		
+		PxU32 feature1;
+		//in the local space of polyData1, if minOverlap is overwrite inside this function, minNormal will be in polyData1 space
+		if(!testFaceNormal(polyData1, polyData0, map1, map0, transform1To0V, transform0To1V, contactDist, minOverlap, feature1, minNormal, POLYDATA1, status))
+			return false;
+	
+		if(!testEdgeNormal(polyData0, polyData1, map0, map1, transform0To1V, transform1To0V, contactDist, minOverlap, minNormal, EDGE, status))
+			return false;
+
+		penDepth = minOverlap;
+		if(status == POLYDATA1)
+		{
+			//minNormal is in the local space of polydata1
+			normal = map1->transform.rotate(minNormal);
+		}
+		else
+		{
+			PX_ASSERT(status == POLYDATA0 || status == EDGE);
+			//ML: status == POLYDATA0 or status == EDGE, minNormal is in the local space of polydata0
+			normal = V3Neg(map0->transform.rotate(minNormal));
+		}
+
+		return true;
+	}
+
+}//Gu
+}//physx
-- 
cgit v1.2.3