massProperties.h

//
// Copyright 2021 Pixar
//
// Licensed under the terms set forth in the LICENSE.txt file available at
// https://openusd.org/license.
//
#ifndef PXR_USD_USD_MASS_PROPERTIES_H
#define PXR_USD_USD_MASS_PROPERTIES_H
 
#include "pxr/pxr.h"
#include "pxr/usd/usdPhysics/api.h"
#include "pxr/usd/usd/common.h"
 
#include "pxr/base/gf/vec3f.h"
#include "pxr/base/gf/quatf.h"
#include "pxr/base/gf/rotation.h"
#include "pxr/base/gf/matrix3f.h"
#include "pxr/base/gf/matrix4f.h"
 
PXR_NAMESPACE_OPEN_SCOPE
 
GfQuatf UsdPhysicsIndexedRotation(uint32_t axis, float s, float c)
{
    float v[3] = { 0, 0, 0 };
    v[axis] = s;
    return GfQuatf(c, v[0], v[1], v[2]);
}
 
uint32_t UsdPhysicsGetNextIndex3(uint32_t i)
{
    return (i + 1 + (i >> 1)) & 3;
}
 
 
GfVec3f UsdPhysicsDiagonalize(const GfMatrix3f& m, GfQuatf& massFrame)
{
    const uint32_t MAX_ITERS = 24;
 
    GfQuatf q = GfQuatf(1.0);
 
    GfMatrix3f d;
    for (uint32_t i = 0; i < MAX_ITERS; i++)
    {
        GfMatrix3f axes(q);
        d = axes * m * axes.GetTranspose();
 
        float d0 = fabs(d[1][2]), d1 = fabs(d[0][2]), d2 = fabs(d[0][1]);
        uint32_t a = uint32_t(d0 > d1 && d0 > d2 ? 0 : d1 > d2 ? 1 : 2); // rotation axis index, from largest
                                                                         // off-diagonal
                                                                         // element
 
        uint32_t a1 = UsdPhysicsGetNextIndex3(a), a2 = UsdPhysicsGetNextIndex3(a1);
        if (d[a1][a2] == 0.0f || fabs(d[a1][a1] - d[a2][a2]) > 2e6 * fabs(2.0 * d[a1][a2]))
            break;
 
        float w = (d[a1][a1] - d[a2][a2]) / (2.0f * d[a1][a2]); // cot(2 * phi), where phi is the rotation angle
        float absw = fabs(w);
 
        GfQuatf r;
        if (absw > 1000)
            r = UsdPhysicsIndexedRotation(a, 1 / (4 * w), 1.0f); // h will be very close to 1, so use small angle approx instead
        else
        {
            float t = 1 / (absw + sqrt(w * w + 1)); // absolute value of tan phi
            float h = 1 / sqrt(t * t + 1); // absolute value of cos phi
 
            r = UsdPhysicsIndexedRotation(a, sqrt((1 - h) / 2) * ((w >= 0.0f) ? 1.0f : -1.0f), sqrt((1 + h) / 2));
        }
 
        q = (q * r).GetNormalized();
    }
 
    massFrame = q;
    return GfVec3f(d.GetColumn(0)[0], d.GetColumn(1)[1], d.GetColumn(2)[2]);
}
 
// -------------------------------------------------------------------------- //
// MASSPROPERTIES                                                             //
// -------------------------------------------------------------------------- //
 
class UsdPhysicsMassProperties
{
public:
 
    USDPHYSICS_API UsdPhysicsMassProperties() : _inertiaTensor(0.0f), _centerOfMass(0.0f), _mass(1.0f)
    {
        _inertiaTensor[0][0] = 1.0;
        _inertiaTensor[1][1] = 1.0;
        _inertiaTensor[2][2] = 1.0;
    }
 
    USDPHYSICS_API UsdPhysicsMassProperties(const float m, const GfMatrix3f& inertiaT, const GfVec3f& com)
        : _inertiaTensor(inertiaT), _centerOfMass(com), _mass(m)
    {
    }
 
    USDPHYSICS_API UsdPhysicsMassProperties operator*(const float scale) const
    {
        return UsdPhysicsMassProperties(_mass * scale, _inertiaTensor * scale, _centerOfMass);
    }
 
    USDPHYSICS_API void Translate(const GfVec3f& t)
    {
        _inertiaTensor = TranslateInertia(_inertiaTensor, _mass, t);
        _centerOfMass += t;
    }
 
    USDPHYSICS_API static GfVec3f GetMassSpaceInertia(const GfMatrix3f& inertia, GfQuatf& massFrame)
    {
 
        GfVec3f diagT = UsdPhysicsDiagonalize(inertia, massFrame);
        return diagT;
    }
 
    USDPHYSICS_API static GfMatrix3f TranslateInertia(const GfMatrix3f& inertia, const float mass, const GfVec3f& t)
    {
        GfMatrix3f s;
        s.SetColumn(0, GfVec3f(0, t[2], -t[1]));
        s.SetColumn(1, GfVec3f(-t[2], 0, t[0]));
        s.SetColumn(2, GfVec3f(t[1], -t[0], 0));
 
        GfMatrix3f translatedIT = s * s.GetTranspose() * mass + inertia;
        return translatedIT;
    }
 
 
    USDPHYSICS_API static GfMatrix3f RotateInertia(const GfMatrix3f& inertia, const GfQuatf& q)
    {
        GfMatrix3f m(q);
        GfMatrix3f rotatedIT = m.GetTranspose() * inertia * m;
        return rotatedIT;
    }
 
    USDPHYSICS_API static UsdPhysicsMassProperties Sum(const UsdPhysicsMassProperties* props, const GfMatrix4f* transforms, const uint32_t count)
    {
        float combinedMass = 0.0f;
        GfVec3f combinedCoM(0.0f);
        GfMatrix3f combinedInertiaT = GfMatrix3f(0.0f);
 
        for (uint32_t i = 0; i < count; i++)
        {
            combinedMass += props[i]._mass;
            const GfVec3f comTm = transforms[i].Transform(props[i]._centerOfMass);
            combinedCoM += comTm * props[i]._mass;
        }
 
        if (combinedMass > 0.f)
            combinedCoM /= combinedMass;
 
        for (uint32_t i = 0; i < count; i++)
        {
            const GfVec3f comTm = transforms[i].Transform(props[i]._centerOfMass);
            combinedInertiaT += TranslateInertia(
                RotateInertia(props[i]._inertiaTensor, GfQuatf(transforms[i].ExtractRotation().GetQuat())),
                props[i]._mass, combinedCoM - comTm);
        }
 
        return UsdPhysicsMassProperties(combinedMass, combinedInertiaT, combinedCoM);
    }
 
    const GfMatrix3f& GetInertiaTensor() const
    {
        return _inertiaTensor;
    }
 
    void SetInertiaTensor(const GfMatrix3f& inTensor)
    {
        _inertiaTensor = inTensor;
    }
 
    const GfVec3f& GetCenterOfMass() const
    {
        return _centerOfMass;
    }
 
    float GetMass() const
    {
        return _mass;
    }
 
    void SetMass(float inMass)
    {
        _mass = inMass;
    }    
 
private:
    GfMatrix3f _inertiaTensor; 
    GfVec3f    _centerOfMass; 
    float           _mass; 
};
 
PXR_NAMESPACE_CLOSE_SCOPE
 
#endif