GU_AgentXform.h

Go to the documentation of this file.

/*
 * PROPRIETARY INFORMATION.  This software is proprietary to
 * Side Effects Software Inc., and is not to be reproduced,
 * transmitted, or disclosed in any way without written permission.
 */

#ifndef __GU_AgentXform__
#define __GU_AgentXform__

#include <SYS/SYS_TypeDecorate.h>
#include <UT/UT_Matrix4.h>
#include <UT/UT_Quaternion.h>
#include <UT/UT_SymMatrix3.h>
#include <UT/UT_Vector3.h>
#include <UT/UT_VectorTypes.h>
#include <UT/UT_XformOrder.h>

/// A factored transform geared for animation blending.
template <typename T>
class GU_AgentXformT
{
public:
    typedef GU_AgentXformT<T> type;

    SYS_FORCE_INLINE
    GU_AgentXformT() = default;

    explicit GU_AgentXformT(T scale)
        : myStretch(scale)
        , myRotate(0, 0, 0, 0)
        , myTranslate(0, 0, 0)
    {
    }

    /// Convert from another floating point type.
    template <typename S>
    explicit GU_AgentXformT(const GU_AgentXformT<S> &other)
        : myStretch(UT_SymMatrix3T<T>(other.stretch()))
        , myRotate(UT_QuaternionT<T>(other.rotate()))
        , myTranslate(UT_Vector3T<T>(other.translate()))
    {
    }

    const UT_SymMatrix3T<T>&    stretch() const     { return myStretch; }
    const UT_QuaternionT<T>&    rotate() const      { return myRotate; }
    const UT_Vector3T<T>&       translate() const   { return myTranslate; }

    void zero()
    {
        myStretch.zero();
        myRotate.assign(0, 0, 0, 0);
        myTranslate.assign(0, 0, 0);
    }
    void identity()
    {
        myStretch.identity();
        myRotate.identity();
        myTranslate.assign(0, 0, 0);
    }

    /// Set the transform, in SRT/XYZ transform order.
    /// The rotations are given in radians.
    void setTransform(
            T tx, T ty, T tz,
            T rx, T ry, T rz,
            T sx, T sy, T sz)
    {
        const UT_XformOrder xord(UT_XformOrder::SRT, UT_XformOrder::XYZ);
        myStretch.setScale(sx, sy, sz);
        myRotate.updateFromEuler(UT_Vector3T<T>(rx, ry, rz), xord);
        myTranslate.assign(tx, ty, tz);
    }

    /// Set the transform, in SRT transform order.
    /// The rotations are specified by a quaternion.
    void setTransform(
        const UT_Vector3T<T> &t, const UT_QuaternionT<T> &r,
        const UT_Vector3T<T> &s)
    {
        myStretch.setScale(s);
        myRotate = r;
        myTranslate = t;
    }

    template <typename S>
    void getTransform(
            S& tx, S& ty, S& tz,
            S& rx, S& ry, S& rz,
            S& sx, S& sy, S& sz) const
    {
        tx = myTranslate[0];
        ty = myTranslate[1];
        tz = myTranslate[2];

        const UT_XformOrder xord(UT_XformOrder::SRT, UT_XformOrder::XYZ);
        UT_Vector3D r = myRotate.computeRotations(xord);
        rx = r[0];
        ry = r[1];
        rz = r[2];

        sx = myStretch(0, 0);
        sy = myStretch(1, 1);
        sz = myStretch(2, 2);
    }

    /// Set this to a linear interpolation of the two given transforms:
    /// *this = a + t*(b - a)
    void setLerp(const type& a, const type& b, T t)
    {
        myStretch.lerp(a.myStretch, b.myStretch, t);

        if (dot(a.myRotate, b.myRotate) >= 0)
            myRotate.lerp(a.myRotate, b.myRotate, t);
        else
            myRotate.lerp(a.myRotate, -b.myRotate, t);
        // NOTE: We don't normalize myRotate since this is already handled when
        // we call UT_QuaternionT::getRotationMatrix().

        myTranslate = SYSlerp(a.myTranslate, b.myTranslate, t);
    }

    /// Same as setLerp, but this assumes that a and b's rotation
    /// matrices are lined up.
    void setLerpAligned(const type& a, const type& b, T t)
    {
        myStretch.lerp(a.myStretch, b.myStretch, t);
        myRotate.lerp(a.myRotate, b.myRotate, t);
        myTranslate = SYSlerp(a.myTranslate, b.myTranslate, t);
    }

    void addScaled(T s, const type& xform)
    {
        myStretch += xform.myStretch * s;
        if (dot(myRotate, xform.myRotate) >= 0)
            myRotate += xform.myRotate * s;
        else
            myRotate += xform.myRotate * -s;
        myTranslate += xform.myTranslate * s;
    }

    /// Perform an additive blend with the specified percentage.
    void concat(T t, const type& xform)
    {
        myRotate.normalize();

        myTranslate += myRotate.rotate(xform.myTranslate * t);

        UT_QuaternionT<T> additive_r = xform.myRotate;
        additive_r.normalize();
        UT_QuaternionT<T> new_r = myRotate * additive_r;

        if (dot(myRotate, new_r) < 0)
            new_r.negate();

        myRotate.lerp(myRotate, new_r, t);
    }

    SYS_FORCE_INLINE
    void getMatrix4(UT_Matrix4T<T>& m) const
    {
        // Set m to the transform myStretch*myRotate*myTranslate
        if (!myStretch.isIdentity())
        {
            UT_Matrix3T<T> sr;
            myRotate.getRotationMatrix(sr);
            sr.leftMult(myStretch);
            m = sr;
        }
        else
            myRotate.getTransformMatrix(m);

        m.setTranslates(myTranslate);
    }

    void setMatrix4(const UT_Matrix4T<T>& m)
    {
        UT_Matrix3T<T> stretch;
        UT_Matrix4T<T> mat = m;

        bool success = mat.makeRigidMatrix(&stretch);
        if (success)
        {
            myStretch = stretch.averagedSymMatrix3();
            myRotate.updateFromRotationMatrix(UT_Matrix3T<T>(mat));
            mat.getTranslates(myTranslate);
        }
        else
        {
            const UT_XformOrder xord(UT_XformOrder::SRT, UT_XformOrder::XYZ);
            UT_Vector3T<T> s, r, t;
            if (!m.explode(xord, r, s, t))
            {
                setTransform(
                        t.x(), t.y(), t.z(), r.x(), r.y(), r.z(),
                        s.x(), s.y(), s.z());
            }
            else
            {
                UT_ASSERT_MSG(false, "Failed to crack matrix");
                identity();
            }
        }
    }
    // ensures that the dot product of ref and the rotation is non-negative
    void setMatrix4(const UT_Matrix4T<T>& m, const UT_QuaternionT<T>& ref)
    {
        setMatrix4(m);
        if (dot(ref, myRotate) <= 0)
        {
            myRotate = -myRotate;
        }
    }

private:
    UT_SymMatrix3T<T>   myStretch;
    UT_QuaternionT<T>   myRotate;
    UT_Vector3T<T>      myTranslate;
};

using GU_AgentXformF = GU_AgentXformT<float>;
using GU_AgentXformD = GU_AgentXformT<double>;
using GU_AgentXformR = GU_AgentXformT<fpreal>;

// Declare as POD for UT_Array optimizations.
SYS_DECLARE_IS_POD(GU_AgentXformF)
SYS_DECLARE_IS_POD(GU_AgentXformD)

#endif