Initial commit:

PhysX 3.4.0 Update @ 21294896
APEX 1.4.0 Update @ 21275617

[CL 21300167]

1 files changed, 476 insertions, 0 deletions

diff --git a/PhysX_3.4/Source/GeomUtils/src/contact/GuLegacyContactBoxHeightField.cpp b/PhysX_3.4/Source/GeomUtils/src/contact/GuLegacyContactBoxHeightField.cpp
new file mode 100644
index 00000000..8429fde3
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/contact/GuLegacyContactBoxHeightField.cpp
@@ -0,0 +1,476 @@
+// 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 "GuContactMethodImpl.h"
+#include "GuGeometryUnion.h"
+#include "GuHeightFieldUtil.h"
+#include "CmRenderBuffer.h"
+#include "GuContactBuffer.h"
+#include "GuContactMethodImpl.h"
+#include "GuLegacyTraceLineCallback.h"
+#include "PsFPU.h"
+
+#include "CmMatrix34.h"
+
+using namespace physx;
+
+#define DISTANCE_BASED_TEST
+
+/////////
+#if 0
+	#include "CmRenderOutput.h"
+	#include "PxsContext.h"
+	static void gVisualizeBox(const Gu::Box& box, PxcNpThreadContext& context, PxU32 color=0xffffff)
+	{
+		PxMat33 rot(box.rot.column0, box.rot.column1, box.rot.column2);
+		PxMat44 m(rot, box.center);
+
+		Cm::DebugBox db(box.extents);
+
+		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(PxIdentity);
+
+		Cm::RenderOutput& out = context.mRenderOutput;
+		out << color << m << Cm::RenderOutput::LINES << a << b;
+	}
+#endif
+/////////
+
+// ptchernev TODO: make sure these are ok before shipping
+static const bool gCompileBoxVertex         = true;
+static const bool gCompileEdgeEdge          = true;
+static const bool gCompileHeightFieldVertex = true;
+
+static const PxReal signs[24] = 
+{
+	-1,-1,-1,
+	-1,-1, 1,
+	-1, 1,-1,
+	-1, 1, 1,
+	1,-1,-1,
+	1,-1, 1,
+	1, 1,-1,
+	1, 1, 1,
+};
+
+static const PxU8 edges[24] = 
+{
+	0,1,
+	1,3,
+	3,2,
+	2,0,
+	4,5,
+	5,7,
+	7,6,
+	6,4,
+	0,4,
+	1,5,
+	2,6,
+	3,7,
+};
+
+static bool GuDepenetrateBox(	const PxVec3& point, 
+								const PxVec3& safeNormal, 
+								const PxVec3& dimensions, 
+								float /*contactDistance*/,
+								PxVec3& normal, 
+								PxReal& distance)
+{
+	PxVec3 faceNormal(PxReal(0));
+	PxReal distance1 = -PX_MAX_REAL; // cant be more
+	PxReal distance2 = -PX_MAX_REAL; // cant be more
+	PxI32 poly1 = -1;
+	PxI32 poly2 = -2;
+
+	for (PxU32 poly = 0; poly < 6; poly++)
+	{
+		PxU32 dim = poly % 3;
+
+		PxReal sign = (poly > 2) ? -PxReal(1) : PxReal(1);
+
+		PxVec3 n(PxVec3(0));
+
+		n[dim] = sign;
+		PxReal proj = n[dim] * safeNormal[dim];
+		PxReal d    = n[dim] * (point[dim] - sign * dimensions[dim]);
+
+#ifdef DISTANCE_BASED_TEST
+		// PT: I'm not really sure about contactDistance here
+		// AP: enabling this causes jitter in DE2740
+		//d -= contactDistance;
+#endif
+
+		if (d >= 0)
+			return false;
+
+		if (proj > 0)
+		{
+			if (d > distance1) // less penetration
+			{
+				distance1 = d;
+				faceNormal = n;
+				poly1 = PxI32(poly);
+			}
+
+			// distance2 / d = 1 / proj 
+			PxReal tmp = d / proj;
+			if (tmp > distance2)
+			{
+				distance2 = tmp;
+				poly2 = PxI32(poly);
+			}
+		}
+	}
+
+	if (poly1 == poly2)
+	{
+		PX_ASSERT(faceNormal.magnitudeSquared() != 0.0f);
+		normal = faceNormal;
+		distance = -distance1;
+	}
+	else
+	{
+		normal = safeNormal;
+		distance = -distance2;
+	}
+
+	return true;
+}
+
+//Box-Heightfield and Convex-Heightfield do not support positive values for contactDistance,
+//and if in this case we would emit contacts normally, we'd cause things to jitter.
+//as a workaround we add contactDistance to the distance values that we emit in contacts.
+//this has the effect that the biasing will work exactly as if we had specified a legacy skinWidth of (contactDistance - restDistance)
+
+namespace physx
+{
+namespace Gu
+{
+bool legacyContactBoxHeightfield(GU_CONTACT_METHOD_ARGS)
+{
+	PX_UNUSED(renderOutput);
+	PX_UNUSED(cache);
+	//PXC_WARN_ONCE(contactDistance > 0.0f, "PxcContactBoxHeightField: Box-Heightfield does not support distance based contact generation! Ignoring contactOffset > 0!");
+
+	// Get actual shape data
+	const PxBoxGeometry& shapeBox = shape0.get<const PxBoxGeometry>();
+	const PxHeightFieldGeometryLL& hfGeom = shape1.get<const PxHeightFieldGeometryLL>();
+
+	const Gu::HeightField& hf = *static_cast<Gu::HeightField*>(hfGeom.heightField);
+	const Gu::HeightFieldUtil hfUtil(hfGeom, hf);
+
+	PX_ASSERT(contactBuffer.count==0);
+
+	Cm::Matrix34 boxShapeAbsPose(transform0);
+	Cm::Matrix34 hfShapeAbsPose(transform1);
+
+	const PxMat33& left = hfShapeAbsPose.m;
+	const PxMat33& right = boxShapeAbsPose.m;
+
+	Cm::Matrix34 boxShape2HfShape(left.getInverse()* right, left.getInverse()*(boxShapeAbsPose.p - hfShapeAbsPose.p));
+
+	// Get box vertices.
+	PxVec3 boxVertices[8];
+	PxVec3 boxVertexNormals[8];
+	for(PxU32 i=0; i<8; i++) 
+	{
+		boxVertices[i] = PxVec3(shapeBox.halfExtents.x*signs[3*i], shapeBox.halfExtents.y*signs[3*i+1], shapeBox.halfExtents.z*signs[3*i+2]);
+
+		boxVertexNormals[i] = PxVec3(signs[3*i], signs[3*i+1], signs[3*i+2]);
+		boxVertexNormals[i].normalize();
+	}
+
+	// Transform box vertices to HeightFieldShape space.
+	PxVec3 boxVerticesInHfShape[8];
+	PxVec3 boxVertexNormalsInHfShape[8];
+	for(PxU32 i=0; i<8; i++) 
+	{
+		boxVerticesInHfShape[i] = boxShape2HfShape.transform(boxVertices[i]);
+		boxVertexNormalsInHfShape[i] = boxShape2HfShape.rotate(boxVertexNormals[i]);
+	}
+
+	// bounds of box on HeightField.
+	PxVec3 aabbMin(boxVerticesInHfShape[0]);
+	PxVec3 aabbMax(boxVerticesInHfShape[0]);
+	for(PxU32 i=1; i<8; i++) 
+	{
+		for(PxU32 dim = 0; dim < 3; dim++)
+		{
+			aabbMin[dim] = PxMin(aabbMin[dim], boxVerticesInHfShape[i][dim]);
+			aabbMax[dim] = PxMax(aabbMax[dim], boxVerticesInHfShape[i][dim]);
+		}
+	}
+
+	const bool thicknessNegOrNull = (hf.getThicknessFast() <= 0.0f);	// PT: don't do this each time! FCMPs are slow.
+
+	// Compute the height field extreme over the bounds area.
+//	PxReal hfExtreme = thicknessNegOrNull ? -PX_MAX_REAL : PX_MAX_REAL;
+	// PT: we already computed those!
+//	const PxReal oneOverRowScale = 1.0f / shapeHeightField.rowScale;
+//	const PxReal oneOverColumnScale = 1.0f / shapeHeightField.columnScale;
+	const PxReal oneOverRowScale = hfUtil.getOneOverRowScale();
+	const PxReal oneOverColumnScale = hfUtil.getOneOverColumnScale();
+
+	PxU32 minRow;
+	PxU32 maxRow;
+	PxU32 minColumn;
+	PxU32 maxColumn;
+
+	if (hfGeom.rowScale < 0)
+	{
+		minRow = hf.getMinRow(aabbMax.x * oneOverRowScale);
+		maxRow = hf.getMaxRow(aabbMin.x * oneOverRowScale);
+	}
+	else
+	{
+		minRow = hf.getMinRow(aabbMin.x * oneOverRowScale);
+		maxRow = hf.getMaxRow(aabbMax.x * oneOverRowScale);
+	}
+
+	if (hfGeom.columnScale < 0)
+	{
+		minColumn = hf.getMinColumn(aabbMax.z * oneOverColumnScale);
+		maxColumn = hf.getMaxColumn(aabbMin.z * oneOverColumnScale);
+	}
+	else
+	{
+		minColumn = hf.getMinColumn(aabbMin.z * oneOverColumnScale);
+		maxColumn = hf.getMaxColumn(aabbMax.z * oneOverColumnScale);
+	}
+
+	PxReal hfExtreme = hf.computeExtreme(minRow, maxRow, minColumn, maxColumn);
+
+	//hfExtreme *= hfShape.getHeightScale();
+	hfExtreme *= hfGeom.heightScale;
+
+	// Return if convex is on the wrong side of the extreme.
+	if (thicknessNegOrNull)
+	{
+		if (aabbMin.y > hfExtreme) return false;
+	}
+	else
+	{
+		if (aabbMax.y < hfExtreme) return false;
+	}
+
+	// Test box vertices.
+	if (gCompileBoxVertex)
+	{
+		for(PxU32 i=0; i<8; i++)
+		{
+			const int32_t* tmp = reinterpret_cast<const int32_t*>(&boxVertexNormalsInHfShape[i].y);
+			// PT: orientation culling
+			if(*tmp>0)
+//			if(PX_SIR(boxVertexNormalsInHfShape[i].y)>0)
+				continue;
+
+			const PxVec3& boxVertexInHfShape = boxVerticesInHfShape[i];
+#if 0
+	PxVec3 pt = boxShapeAbsPose.transform(boxVertices[i]);
+	PxVec3 worldNormal = boxShapeAbsPose.rotate(boxVertexNormals[i]);
+	//gVisualizeLine(pt, pt+PxVec3(1.0f,0.0f,0.0f), context, PxDebugColor::eARGB_RED);
+	//gVisualizeLine(pt, pt+PxVec3(0.0f,1.0f,0.0f), context, PxDebugColor::eARGB_GREEN);
+	//gVisualizeLine(pt, pt+PxVec3(0.0f,0.0f,1.0f), context, PxDebugColor::eARGB_BLUE);
+	gVisualizeLine(pt, pt+worldNormal, context, PxDebugColor::eARGB_MAGENTA);
+#endif
+
+//////// SAME CODE AS IN CONVEX-HF
+			const bool insideExtreme =
+				thicknessNegOrNull ? (boxVertexInHfShape.y < hfExtreme + params.mContactDistance) : (boxVertexInHfShape.y > hfExtreme-params.mContactDistance);
+
+			//if (insideExtreme && hfShape.isShapePointOnHeightField(boxVertexInHfShape.x, boxVertexInHfShape.z))
+			if (insideExtreme && hfUtil.isShapePointOnHeightField(boxVertexInHfShape.x, boxVertexInHfShape.z))
+			{
+				//PxReal y = hfShape.getHeightAtShapePoint(boxVertexInHfShape.x, boxVertexInHfShape.z);
+//				const PxReal y = hfUtil.getHeightAtShapePoint(boxVertexInHfShape.x, boxVertexInHfShape.z);
+
+				// PT: compute this once, reuse results (3 times!)
+				// PT: TODO: also reuse this in EE tests
+				PxReal fracX, fracZ;
+				const PxU32 vertexIndex = hfUtil.getHeightField().computeCellCoordinates(
+					boxVertexInHfShape.x * oneOverRowScale, boxVertexInHfShape.z * oneOverColumnScale, fracX, fracZ);
+
+				const PxReal y = hfUtil.getHeightAtShapePoint2(vertexIndex, fracX, fracZ);
+
+				const PxReal dy = boxVertexInHfShape.y - y;
+#ifdef DISTANCE_BASED_TEST
+				if (hf.isDeltaHeightInsideExtent(dy, params.mContactDistance))
+#else
+				if (hf.isDeltaHeightInsideExtent(dy))
+#endif
+				{
+					const PxU32 faceIndex = hfUtil.getFaceIndexAtShapePointNoTest2(vertexIndex, fracX, fracZ);
+					if (faceIndex != 0xffffffff)
+					{
+//						PxcMaterialIndex material = hfShape.getTriangleMaterial(feature);
+						PxVec3 n;
+						//n = hfShape.getNormalAtShapePoint(boxVertexInHfShape.x, boxVertexInHfShape.z);
+//						n = hfUtil.getNormalAtShapePoint(boxVertexInHfShape.x, boxVertexInHfShape.z);
+						n = hfUtil.getNormalAtShapePoint2(vertexIndex, fracX, fracZ);
+						n = n.getNormalized();
+
+						contactBuffer
+#ifdef DISTANCE_BASED_TEST
+							.contact(boxShapeAbsPose.transform(boxVertices[i]), hfShapeAbsPose.rotate(n), n.y*dy/* + contactDistance*/, faceIndex);
+#else
+							.contact(boxShapeAbsPose.transform(boxVertices[i]), hfShapeAbsPose.rotate(n), n.y*dy + contactDistance, feature);//add contactDistance to compensate for fact that we don't support dist based contacts! See comment at start of funct.
+#endif
+					}
+				}
+			}
+////////~SAME CODE AS IN CONVEX-HF
+		}
+	}
+
+	// Test box edges.
+	if (gCompileEdgeEdge)
+	{
+		// create helper class for the trace segment
+		GuContactHeightfieldTraceSegmentHelper traceSegmentHelper(hfUtil);		
+
+		for(PxU32 i=0; i<12; i++) 
+		{
+// PT: orientation culling
+//float worldNormalY = (boxVertexNormalsInHfShape[edges[2*i]].y + boxVertexNormalsInHfShape[edges[2*i+1]].y)*0.5f;
+float worldNormalY = boxVertexNormalsInHfShape[edges[2*i]].y + boxVertexNormalsInHfShape[edges[2*i+1]].y;
+if(worldNormalY>0.0f)
+	continue;
+
+
+			const PxVec3& v0 = boxVerticesInHfShape[edges[2*i]];
+			const PxVec3& v1 = boxVerticesInHfShape[edges[2*i+1]];
+
+#if 0
+	PxVec3 pt0 = boxShapeAbsPose.transform(boxVertices[edges[2*i]]);
+	PxVec3 pt1 = boxShapeAbsPose.transform(boxVertices[edges[2*i+1]]);
+
+	PxVec3 worldNormal0 = boxShapeAbsPose.rotate(boxVertexNormals[edges[2*i]]);
+	PxVec3 worldNormal1 = boxShapeAbsPose.rotate(boxVertexNormals[edges[2*i+1]]);
+
+	PxVec3 pt = (pt0 + pt1)*0.5f;
+	PxVec3 worldNormal = (worldNormal0 + worldNormal1)*0.5f;
+
+	gVisualizeLine(pt, pt+worldNormal, context, PxDebugColor::eARGB_CYAN);
+#endif
+
+
+
+			if (hf.getThicknessFast())	// PT: errr...? not the same test as in the convex code?
+			{
+				if ((v0.y > hfExtreme) && (v1.y > hfExtreme)) continue;
+			}
+			else
+			{
+				if ((v0.y < hfExtreme) && (v1.y < hfExtreme)) continue;
+			}
+			GuContactTraceSegmentCallback cb(v1 - v0,
+				contactBuffer,
+				hfShapeAbsPose, params.mContactDistance/*, context.mRenderOutput*/);
+
+			//context.mRenderOutput << PxVec3(1,0,0) << Gu::Debug::convertToPxMat44(transform1)
+			//	<< Cm::RenderOutput::LINES << v0+PxVec3(0.01f) << v1+PxVec3(0.01f);
+			//hfShape.traceSegment<PxcContactTraceSegmentCallback>(v0, v1, &cb);
+			traceSegmentHelper.traceSegment(v0, v1, &cb);
+		}
+	}
+
+	// Test HeightField vertices.
+	if (gCompileHeightFieldVertex)
+	{
+		// Iterate over all HeightField vertices inside the bounds.
+		for(PxU32 row = minRow; row <= maxRow; row++)
+		{
+			for(PxU32 column = minColumn; column <= maxColumn; column++)
+			{
+				const PxU32 vertexIndex = row * hf.getNbColumnsFast() + column;
+
+				//if (!hfShape.isCollisionVertex(vertexIndex)) continue;
+				if (!hfUtil.isCollisionVertex(vertexIndex, row, column)) continue;
+
+				// Check if hf vertex is inside the box.
+				//PxVec3 hfVertex;
+				//hfVertex.set(hfShape.getRowScale() * row, hfShape.getHeightScale() * hfShape.getHeight(vertexIndex), hfShape.getColumnScale() * column);
+//				const PxVec3 hfVertex(shapeHeightField.rowScale * row, shapeHeightField.columnScale * hfShape.getHeight(vertexIndex), shapeHeightField.columnScale * column);
+				const PxVec3 hfVertex(hfGeom.rowScale * row, hfGeom.heightScale * hf.getHeight(vertexIndex), hfGeom.columnScale * column);
+
+				const PxVec3 hfVertexInBoxShape = boxShape2HfShape.transformTranspose(hfVertex);
+				if ((PxAbs(hfVertexInBoxShape.x) - shapeBox.halfExtents.x - params.mContactDistance < 0)
+					&& (PxAbs(hfVertexInBoxShape.y) - shapeBox.halfExtents.y - params.mContactDistance < 0)
+					&& (PxAbs(hfVertexInBoxShape.z) - shapeBox.halfExtents.z - params.mContactDistance < 0))
+				{
+
+					// ptchernev: should have done this in HeightFieldShape
+					// check if this really is a collision vertex
+					//PxVec3 hfVertexNormal = thicknessNegOrNull ? hfShape.getVertexNormal(vertexIndex) : -hfShape.getVertexNormal(vertexIndex);
+//					PxVec3 hfVertexNormal = thicknessNegOrNull ? hfUtil.getVertexNormal(vertexIndex) : -hfUtil.getVertexNormal(vertexIndex);
+					const PxVec3 nrm = hfUtil.getVertexNormal(vertexIndex, row, column);
+					PxVec3 hfVertexNormal = thicknessNegOrNull ? nrm : -nrm;
+					hfVertexNormal = hfVertexNormal.getNormalized();
+					const PxVec3 hfVertexNormalInBoxShape = boxShape2HfShape.rotateTranspose(hfVertexNormal);
+					PxVec3 normal;
+					PxReal depth;
+					if (!GuDepenetrateBox(hfVertexInBoxShape, -hfVertexNormalInBoxShape, shapeBox.halfExtents, params.mContactDistance, normal, depth)) 
+					{
+						continue;
+					}
+					
+//					PxMat33 rot(boxShape2HfShape[0],boxShape2HfShape[1],boxShape2HfShape[2]);
+//					PxVec3 normalInHfShape = rot * (-normal);
+					PxVec3 normalInHfShape = boxShape2HfShape.rotate(-normal);
+					//hfShape.clipShapeNormalToVertexVoronoi(normalInHfShape, vertexIndex);
+					hfUtil.clipShapeNormalToVertexVoronoi(normalInHfShape, vertexIndex, row, column);
+					if (normalInHfShape.dot(hfVertexNormal) < PX_EPS_REAL) 
+					{
+						// hmm, I dont think this can happen
+						continue;
+					}
+					normalInHfShape = normalInHfShape.getNormalized();
+					const PxU32 faceIndex = hfUtil.getVertexFaceIndex(vertexIndex, row, column);
+					contactBuffer
+						.contact(hfShapeAbsPose.transform(hfVertex), hfShapeAbsPose.rotate(normalInHfShape),
+#ifdef DISTANCE_BASED_TEST
+						-depth,
+#else
+						-depth + contactDistance,	//add contactDistance to compensate for fact that we don't support dist based contacts! See comment at start of funct.
+#endif
+						faceIndex);
+				}
+			}
+		}
+	}
+
+	return contactBuffer.count > 0;
+}
+}//Gu
+}//physx