Initial commit:

PhysX 3.4.0 Update @ 21294896
APEX 1.4.0 Update @ 21275617

[CL 21300167]

1 files changed, 549 insertions, 0 deletions

diff --git a/PhysX_3.4/Source/GeomUtils/src/distance/GuDistanceSegmentBox.cpp b/PhysX_3.4/Source/GeomUtils/src/distance/GuDistanceSegmentBox.cpp
new file mode 100644
index 00000000..71f0be67
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/distance/GuDistanceSegmentBox.cpp
@@ -0,0 +1,549 @@
+// 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 "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);
+	}
+}