ExtInertiaTensor.h

// 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-2022 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 EXT_INERTIA_TENSOR_H
#define EXT_INERTIA_TENSOR_H
 
#include "foundation/PxMat33.h"
#include "foundation/PxMathUtils.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        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()
{
}
 
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