405 lines
13 KiB
C++
405 lines
13 KiB
C++
//
|
|
// 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-2019 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
|