Initial commit:

PhysX 3.4.0 Update @ 21294896
APEX 1.4.0 Update @ 21275617

[CL 21300167]

1 files changed, 404 insertions, 0 deletions

diff --git a/PhysX_3.4/Source/PhysXExtensions/src/ExtInertiaTensor.h b/PhysX_3.4/Source/PhysXExtensions/src/ExtInertiaTensor.h
new file mode 100644
index 00000000..510e46b7
--- /dev/null
+++ b/PhysX_3.4/Source/PhysXExtensions/src/ExtInertiaTensor.h
@@ -0,0 +1,404 @@
+// 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.  
+
+
+#ifndef PX_PHYSICS_EXTENSIONS_INERTIATENSOR_H
+#define PX_PHYSICS_EXTENSIONS_INERTIATENSOR_H
+
+#include "foundation/PxMat33.h"
+#include "foundation/PxMathUtils.h"
+#include "CmPhysXCommon.h"
+#include "PsMathUtils.h"
+
+namespace physx
+{
+namespace Ext
+{
+	class InertiaTensorComputer
+	{
+		public:
+									InertiaTensorComputer(bool initTozero = true);
+									InertiaTensorComputer(const PxMat33& inertia, const PxVec3& com, PxReal mass);
+									~InertiaTensorComputer();
+
+		PX_INLINE	void			zero();																//sets to zero mass
+		PX_INLINE	void			setDiagonal(PxReal mass, const PxVec3& diagonal);					//sets as a diagonal tensor
+		PX_INLINE	void			rotate(const PxMat33& rot);											//rotates the mass
+					void			translate(const PxVec3& t);											//translates the mass
+		PX_INLINE	void			transform(const PxTransform& transform);							//transforms the mass
+		PX_INLINE	void			scaleDensity(PxReal densityScale);									//scales by a density factor
+		PX_INLINE	void			add(const InertiaTensorComputer& it);								//adds a mass
+		PX_INLINE	void			center();															//recenters inertia around center of mass
+
+					void			setBox(const PxVec3& halfWidths);									//sets as an axis aligned box
+		PX_INLINE	void			setBox(const PxVec3& halfWidths, const PxTransform* pose);			//sets as an oriented box
+
+					void			setSphere(PxReal radius);
+		PX_INLINE	void			setSphere(PxReal radius, const PxTransform* pose);
+
+					void			setCylinder(int dir, PxReal r, PxReal l);
+		PX_INLINE	void			setCylinder(int dir, PxReal r, PxReal l, const PxTransform* pose);
+
+					void			setCapsule(int dir, PxReal r, PxReal l);
+		PX_INLINE	void			setCapsule(int dir, PxReal r, PxReal l, const PxTransform* pose);
+					void			addCapsule(PxReal density, int dir, PxReal r, PxReal l, const PxTransform* pose = 0);
+
+					void			setEllipsoid(PxReal rx, PxReal ry, PxReal rz);
+		PX_INLINE	void			setEllipsoid(PxReal rx, PxReal ry, PxReal rz, const PxTransform* pose);
+
+		PX_INLINE	PxVec3			getCenterOfMass()				const	{ return mG;	}
+		PX_INLINE	PxReal			getMass()						const	{ return mMass;	}
+		PX_INLINE	PxMat33			getInertia()					const	{ return mI;	}
+
+		private:
+					PxMat33				mI;
+					PxVec3				mG;
+					PxReal				mMass;
+	};
+
+
+	//--------------------------------------------------------------
+	//
+	// Helper routines
+	//
+	//--------------------------------------------------------------
+
+	// Special version allowing 2D quads
+	PX_INLINE PxReal volume(const PxVec3& extents)
+	{
+		PxReal v = 1.0f;
+		if(extents.x != 0.0f)	v*=extents.x;
+		if(extents.y != 0.0f)	v*=extents.y;
+		if(extents.z != 0.0f)	v*=extents.z;
+		return v;
+	}
+
+	// Sphere
+	PX_INLINE PxReal computeSphereRatio(PxReal radius)						{ return (4.0f/3.0f) * PxPi * radius * radius * radius;	}
+	PxReal computeSphereMass(PxReal radius, PxReal density)					{ return density * computeSphereRatio(radius);				}
+	PxReal computeSphereDensity(PxReal radius, PxReal mass)					{ return mass / computeSphereRatio(radius);					}
+
+	// Box
+	PX_INLINE PxReal computeBoxRatio(const PxVec3& extents)					{ return volume(extents);									}
+	PxReal computeBoxMass(const PxVec3& extents, PxReal density)			{ return density * computeBoxRatio(extents);				}
+	PxReal computeBoxDensity(const PxVec3& extents, PxReal mass)			{ return mass / computeBoxRatio(extents);					}
+
+	// Ellipsoid
+	PX_INLINE PxReal computeEllipsoidRatio(const PxVec3& extents)			{ return (4.0f/3.0f) * PxPi * volume(extents);		}
+	PxReal computeEllipsoidMass(const PxVec3& extents, PxReal density)		{ return density * computeEllipsoidRatio(extents);			}
+	PxReal computeEllipsoidDensity(const PxVec3& extents, PxReal mass)		{ return mass / computeEllipsoidRatio(extents);				}
+
+	// Cylinder
+	PX_INLINE PxReal computeCylinderRatio(PxReal r, PxReal l)				{ return PxPi * r  * r * (2.0f*l);					}
+	PxReal computeCylinderMass(PxReal r, PxReal l, PxReal density)			{ return density * computeCylinderRatio(r, l);				}
+	PxReal computeCylinderDensity(PxReal r, PxReal l, PxReal mass)			{ return mass / computeCylinderRatio(r, l);					}
+
+	// Capsule
+	PX_INLINE PxReal computeCapsuleRatio(PxReal r, PxReal l)				{ return computeSphereRatio(r) + computeCylinderRatio(r, l);}
+	PxReal computeCapsuleMass(PxReal r, PxReal l, PxReal density)			{ return density * computeCapsuleRatio(r, l);				}
+	PxReal computeCapsuleDensity(PxReal r, PxReal l, PxReal mass)			{ return mass / computeCapsuleRatio(r, l);					}
+
+	// Cone
+	PX_INLINE PxReal computeConeRatio(PxReal r, PxReal l)					{ return PxPi * r * r * PxAbs(l)/3.0f;			}
+	PxReal computeConeMass(PxReal r, PxReal l, PxReal density)				{ return density * computeConeRatio(r, l);					}
+	PxReal computeConeDensity(PxReal r, PxReal l, PxReal mass)				{ return mass / computeConeRatio(r, l);						}
+
+	void computeBoxInertiaTensor(PxVec3& inertia, PxReal mass, PxReal xlength, PxReal ylength, PxReal zlength);
+	void computeSphereInertiaTensor(PxVec3& inertia, PxReal mass, PxReal radius, bool hollow);
+	bool jacobiTransform(PxI32 n, PxF64 a[], PxF64 w[]);
+	bool diagonalizeInertiaTensor(const PxMat33& denseInertia, PxVec3& diagonalInertia, PxMat33& rotation);
+
+} // namespace Ext
+
+void Ext::computeBoxInertiaTensor(PxVec3& inertia, PxReal mass, PxReal xlength, PxReal ylength, PxReal zlength)
+{
+	//to model a hollow block, one would have to multiply coeff by up to two.
+	const PxReal coeff = mass/12;
+	inertia.x = coeff * (ylength*ylength + zlength*zlength);
+	inertia.y = coeff * (xlength*xlength + zlength*zlength);
+	inertia.z = coeff * (xlength*xlength + ylength*ylength);
+
+	PX_ASSERT(inertia.x != 0.0f);
+	PX_ASSERT(inertia.y != 0.0f);
+	PX_ASSERT(inertia.z != 0.0f);
+	PX_ASSERT(inertia.isFinite());
+}
+
+
+void Ext::computeSphereInertiaTensor(PxVec3& inertia, PxReal mass, PxReal radius, bool hollow)
+{
+	inertia.x = mass * radius * radius;
+	if (hollow) 
+		inertia.x *= PxReal(2 / 3.0);
+	else
+		inertia.x *= PxReal(2 / 5.0);
+
+	inertia.z = inertia.y = inertia.x;
+	PX_ASSERT(inertia.isFinite());
+}
+
+//--------------------------------------------------------------
+//
+// InertiaTensorComputer implementation
+//
+//--------------------------------------------------------------
+
+Ext::InertiaTensorComputer::InertiaTensorComputer(bool initTozero)
+{
+	if (initTozero)
+		zero();
+}
+
+
+Ext::InertiaTensorComputer::InertiaTensorComputer(const PxMat33& inertia, const PxVec3& com, PxReal mass) :
+	mI(inertia),
+	mG(com),
+	mMass(mass)
+{
+}
+
+
+Ext::InertiaTensorComputer::~InertiaTensorComputer()
+{
+	//nothing
+}
+
+
+PX_INLINE void Ext::InertiaTensorComputer::zero()
+{
+	mMass = 0.0f;
+	mI = PxMat33(PxZero);
+	mG = PxVec3(0);
+}
+
+
+PX_INLINE void Ext::InertiaTensorComputer::setDiagonal(PxReal mass, const PxVec3& diag)
+{
+	mMass = mass;
+	mI = PxMat33::createDiagonal(diag);
+	mG = PxVec3(0);
+	PX_ASSERT(mI.column0.isFinite() && mI.column1.isFinite() && mI.column2.isFinite());
+	PX_ASSERT(PxIsFinite(mMass));
+}
+
+
+void Ext::InertiaTensorComputer::setBox(const PxVec3& halfWidths)
+{
+	// Setup inertia tensor for a cube with unit density
+	const PxReal mass = 8.0f * computeBoxRatio(halfWidths);
+	const PxReal s =(1.0f/3.0f) * mass;
+
+	const PxReal x = halfWidths.x*halfWidths.x;
+	const PxReal y = halfWidths.y*halfWidths.y;
+	const PxReal z = halfWidths.z*halfWidths.z;
+
+	setDiagonal(mass, PxVec3(y+z, z+x, x+y) * s);
+}
+
+
+PX_INLINE void Ext::InertiaTensorComputer::rotate(const PxMat33& rot)
+{
+	//well known inertia tensor rotation expression is: RIR' -- this could be optimized due to symmetry, see code to do that in Body::updateGlobalInverseInertia
+	mI = rot * mI * rot.getTranspose();
+	PX_ASSERT(mI.column0.isFinite() && mI.column1.isFinite() && mI.column2.isFinite());
+	//com also needs to be rotated
+	mG = rot * mG;
+	PX_ASSERT(mG.isFinite());
+}
+
+
+void Ext::InertiaTensorComputer::translate(const PxVec3& t)
+{
+	if (!t.isZero())	//its common for this to be zero
+	{
+		PxMat33 t1, t2;
+
+		t1.column0 = PxVec3(0, mG.z, -mG.y);
+		t1.column1 = PxVec3(-mG.z, 0, mG.x);
+		t1.column2 = PxVec3(mG.y, -mG.x, 0);
+
+		PxVec3 sum = mG + t;
+		if (sum.isZero())
+		{
+			mI += (t1 * t1)*mMass;
+		}
+		else
+		{			
+			t2.column0 = PxVec3(0, sum.z, -sum.y);
+			t2.column1 = PxVec3(-sum.z, 0, sum.x);
+			t2.column2 = PxVec3(sum.y, -sum.x, 0);
+			mI += (t1 * t1 - t2 * t2)*mMass;
+		}
+
+		//move center of mass
+		mG += t;
+
+		PX_ASSERT(mI.column0.isFinite() && mI.column1.isFinite() && mI.column2.isFinite());
+		PX_ASSERT(mG.isFinite());
+	}
+}
+
+
+PX_INLINE void Ext::InertiaTensorComputer::transform(const PxTransform& transform)
+{
+	rotate(PxMat33(transform.q));
+	translate(transform.p);
+}
+
+
+PX_INLINE void Ext::InertiaTensorComputer::setBox(const PxVec3& halfWidths, const PxTransform* pose)
+{
+	setBox(halfWidths);
+	if (pose)
+		transform(*pose);
+
+}
+
+
+PX_INLINE void Ext::InertiaTensorComputer::scaleDensity(PxReal densityScale)
+{
+	mI *= densityScale;
+	mMass *= densityScale;
+	PX_ASSERT(mI.column0.isFinite() && mI.column1.isFinite() && mI.column2.isFinite());
+	PX_ASSERT(PxIsFinite(mMass));
+}
+
+
+PX_INLINE void Ext::InertiaTensorComputer::add(const InertiaTensorComputer& it)
+{
+	const PxReal TotalMass = mMass + it.mMass;
+	mG = (mG * mMass + it.mG * it.mMass) / TotalMass;
+
+	mMass = TotalMass;
+	mI += it.mI;
+	PX_ASSERT(mI.column0.isFinite() && mI.column1.isFinite() && mI.column2.isFinite());
+	PX_ASSERT(mG.isFinite());
+	PX_ASSERT(PxIsFinite(mMass));
+}
+
+
+PX_INLINE void Ext::InertiaTensorComputer::center()
+{
+	PxVec3 center = -mG;
+	translate(center);
+}
+
+
+void Ext::InertiaTensorComputer::setSphere(PxReal radius)
+{
+	// Compute mass of the sphere
+	const PxReal m = computeSphereRatio(radius);
+	// Compute moment of inertia
+	const PxReal s = m * radius * radius * (2.0f/5.0f);
+	setDiagonal(m,PxVec3(s,s,s));
+}
+
+
+PX_INLINE void Ext::InertiaTensorComputer::setSphere(PxReal radius, const PxTransform* pose)
+{
+	setSphere(radius);
+	if (pose)
+		transform(*pose);
+}
+
+
+void Ext::InertiaTensorComputer::setCylinder(int dir, PxReal r, PxReal l)
+{
+	// Compute mass of cylinder
+	const PxReal m = computeCylinderRatio(r, l);
+
+	const PxReal i1 = r*r*m/2.0f;
+	const PxReal i2 = (3.0f*r*r+4.0f*l*l)*m/12.0f;
+
+	switch(dir)
+	{
+	case 0:		setDiagonal(m,PxVec3(i1,i2,i2));	break;
+	case 1:		setDiagonal(m,PxVec3(i2,i1,i2));	break;
+	default:	setDiagonal(m,PxVec3(i2,i2,i1));	break;
+	}
+}
+
+
+PX_INLINE void Ext::InertiaTensorComputer::setCylinder(int dir, PxReal r, PxReal l, const PxTransform* pose)
+{
+	setCylinder(dir, r, l);
+	if (pose)
+		transform(*pose);
+}
+
+
+void Ext::InertiaTensorComputer::setCapsule(int dir, PxReal r, PxReal l)
+{
+	// Compute mass of capsule
+	const PxReal m = computeCapsuleRatio(r, l);
+
+	const PxReal t  = PxPi * r * r;
+	const PxReal i1 = t * ((r*r*r * 8.0f/15.0f) + (l*r*r));
+	const PxReal i2 = t * ((r*r*r * 8.0f/15.0f) + (l*r*r * 3.0f/2.0f) + (l*l*r * 4.0f/3.0f) + (l*l*l * 2.0f/3.0f));
+
+	switch(dir)
+	{
+	case 0:		setDiagonal(m,PxVec3(i1,i2,i2));	break;
+	case 1:		setDiagonal(m,PxVec3(i2,i1,i2));	break;
+	default:	setDiagonal(m,PxVec3(i2,i2,i1));	break;
+	}
+}
+
+
+PX_INLINE void Ext::InertiaTensorComputer::setCapsule(int dir, PxReal r, PxReal l, const PxTransform* pose)
+{
+	setCapsule(dir, r, l);
+	if (pose)
+		transform(*pose);
+}
+
+
+void Ext::InertiaTensorComputer::setEllipsoid(PxReal rx, PxReal ry, PxReal rz)
+{
+	// Compute mass of ellipsoid
+	const PxReal m = computeEllipsoidRatio(PxVec3(rx, ry, rz));
+
+	// Compute moment of inertia
+	const PxReal s = m * (2.0f/5.0f);
+
+	// Setup inertia tensor for an ellipsoid centered at the origin
+	setDiagonal(m,PxVec3(ry*rz,rz*rx,rx*ry)*s);
+}
+
+
+PX_INLINE void Ext::InertiaTensorComputer::setEllipsoid(PxReal rx, PxReal ry, PxReal rz, const PxTransform* pose)
+{
+	setEllipsoid(rx,ry,rz);
+	if (pose)
+		transform(*pose);
+}
+
+}
+
+#endif