massProperties.h

//
// Copyright 2021 Pixar
//
// Licensed under the Apache License, Version 2.0 (the "Apache License")
// with the following modification; you may not use this file except in
// compliance with the Apache License and the following modification to it:
// Section 6. Trademarks. is deleted and replaced with:
//
// 6. Trademarks. This License does not grant permission to use the trade
//    names, trademarks, service marks, or product names of the Licensor
//    and its affiliates, except as required to comply with Section 4(c) of
//    the License and to reproduce the content of the NOTICE file.
//
// You may obtain a copy of the Apache License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the Apache License with the above modification is
// distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied. See the Apache License for the specific
// language governing permissions and limitations under the Apache 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