Rigid3d.h

// Rigid3d.h:
// ----------
//

#ifndef __IPSDKMATH_RIGID3D_H__
#define __IPSDKMATH_RIGID3D_H__

// suppression warnings
// warning C4251: 'ipsdk::math::transform::Rigid3d::g_registrator' : struct 'ipsdk::math::transform::Registrator<ipsdk::math::transform::Rigid3d>' needs to have dll-interface to be used by clients of class 'ipsdk::math::transform::Rigid3d'
#pragma warning (push)
#pragma warning (disable : 4251)

#include <IPSDKMath/Geometry/3d/Transform/BaseLinearGeometricTransform3d.h>
#include <IPSDKMath/Geometry/3d/Angles3d.h>

namespace ipsdk {
namespace math {
namespace transform {


class IPSDKMATH_API Rigid3d : public BaseLinearGeometricTransform3d
{
// predefined public type
public:
    IPSDK_ENUM(eTransformParams,

        (eTP_Chi)       
        (eTP_Beta)      
        (eTP_Alpha)     
        (eTP_Tx)        
        (eTP_Ty)        
        (eTP_Tz)        
    );

    // declare geometric transformation 3d
    IPSDK_DECLARE_TRANSFORM3D(Rigid3d);

public:
    Rigid3d()
    {
        setParams(0, 0, 0, 0, 0, 0);
    }
    Rigid3d(const ipReal64 chi, const ipReal64 beta, const ipReal64 alpha,
            const ipReal64 tx, const ipReal64 ty, const ipReal64 tz)
    {
        setParams(chi, beta, alpha, tx, ty, tz);
    }
    ~Rigid3d() {}

// methods
public:
    static void             transform(const ipReal64 chi, const ipReal64 beta, const ipReal64 alpha,
                                      const ipReal64 tx, const ipReal64 ty, const ipReal64 tz,
                                      ipReal64& x, ipReal64& y, ipReal64& z)
    {
        transform(std::cos(chi), std::sin(chi),
                  std::cos(beta), std::sin(beta),
                  std::cos(alpha), std::sin(alpha),
                  tx, ty, tz,
                  x, y, z);
    }
    static void             transform(const ipReal64 cosChi, const ipReal64 sinChi,
                                      const ipReal64 cosBeta, const ipReal64 sinBeta,
                                      const ipReal64 cosAlpha, const ipReal64 sinAlpha,
                                      const ipReal64 tx, const ipReal64 ty, const ipReal64 tz,
                                      ipReal64& x, ipReal64& y, ipReal64& z)
    {
        rotate3dInSitu(cosChi, sinChi, cosBeta, sinBeta, cosAlpha, sinAlpha, x, y, z);
        x += tx;
        y += ty;
        z += tz;
    }

    static Matrix4d         getHomogenousMatrix(const ipReal64 chi,
                                                const ipReal64 beta,
                                                const ipReal64 alpha,
                                                const ipReal64 tx,
                                                const ipReal64 ty,
                                                const ipReal64 tz)
    {
        return getHomogenousMatrix(std::cos(chi), std::sin(chi),
                                   std::cos(beta), std::sin(beta),
                                   std::cos(alpha), std::sin(alpha),
                                   tx, ty, tz);
    }
    static Matrix4d         getHomogenousMatrix(const ipReal64 cosChi, const ipReal64 sinChi,
                                                const ipReal64 cosBeta, const ipReal64 sinBeta,
                                                const ipReal64 cosAlpha, const ipReal64 sinAlpha,
                                                const ipReal64 tx, const ipReal64 ty, const ipReal64 tz)
    {
        Matrix4d transform;
        eulerToMatrix(cosChi, sinChi, cosBeta, sinBeta, cosAlpha, sinAlpha,
                      transform(0, 0), transform(0, 1), transform(0, 2),
                      transform(1, 0), transform(1, 1), transform(1, 2),
                      transform(2, 0), transform(2, 1), transform(2, 2));
        transform(0, 3) = tx;
        transform(1, 3) = ty;
        transform(2, 3) = tz;
        transform(3, 0) = 0; transform(3, 1) = 0; transform(3, 2) = 0; transform(3, 3) = 1;

        return transform;
    }

    static Matrix4d         getInvHomogenousMatrix(const ipReal64 chi, const ipReal64 beta, const ipReal64 alpha,
                                                   const ipReal64 tx, const ipReal64 ty, const ipReal64 tz)
    {
        // invert input parameters
        ipReal64 invChi= chi;
        ipReal64 invBeta = beta;
        ipReal64 invAlpha = alpha;
        ipReal64 invTx = tx;
        ipReal64 invTy = ty;
        ipReal64 invTz = tz;
        getInvParams(invChi, invBeta, invAlpha, invTx, invTy, invTz);

        return getHomogenousMatrix(invChi, invBeta, invAlpha, invTx, invTy, invTz);
    }

    static void             getInvParams(ipReal64& chi, ipReal64& beta, ipReal64& alpha,
                                         ipReal64& tx, ipReal64& ty, ipReal64& tz);

    static Vector           getIdentityParams();

    void                    setParams(const ipReal64 chi, const ipReal64 beta, const ipReal64 alpha,
                                      const ipReal64 tx, const ipReal64 ty, const ipReal64 tz)
    {
        _chi = chi;
        _cosChi = std::cos(_chi);
        _sinChi = std::sin(_chi);
        _beta = beta;
        _cosBeta = std::cos(_beta);
        _sinBeta = std::sin(_beta);
        _alpha = alpha;
        _cosAlpha = std::cos(_alpha);
        _sinAlpha = std::sin(_alpha);
        _tx = tx;
        _ty = ty;
        _tz = tz;
    }

    void                    setCenteredRotation(const ipReal64 chi, const ipReal64 beta, const ipReal64 alpha,
                                                const ipReal64 centerX, const ipReal64 centerY, const ipReal64 centerZ)
    {
        // store angle informations
        _chi = chi;
        _cosChi = std::cos(_chi);
        _sinChi = std::sin(_chi);
        _beta = beta;
        _cosBeta = std::cos(_beta);
        _sinBeta = std::sin(_beta);
        _alpha = alpha;
        _cosAlpha = std::cos(_alpha);
        _sinAlpha = std::sin(_alpha);

        // we compute translation such as center point is invariant
        // (ie C = RxC + T => T = C - RxC)
        ipReal64 rx, ry, rz;
        rotate3d<ipReal64>(centerX, centerY, centerZ,
                           _cosChi, _sinChi, _cosBeta, _sinBeta, _cosAlpha, _sinAlpha,
                           rx, ry, rz);
        _tx = centerX - rx;
        _ty = centerY - ry;
        _tz = centerZ - rz;
    }

    void                    setParams(const Vector& params);

    void                    setIdentity();

    void                    apply(ipReal64& x, ipReal64& y, ipReal64& z) const;

    Matrix4d                getHomogenousMatrix() const;

    Vector                  getParams() const;

    ipReal64                getChi() const;
    ipReal64                getBeta() const;
    ipReal64                getAlpha() const;
    ipReal64                getTx() const;
    ipReal64                getTy() const;
    ipReal64                getTz() const;

    Matrix4d                getInvHomogenousMatrix() const;

    Vector                  getInvParams() const;

// attributes
protected:
    ipReal64        _chi;
    ipReal64        _beta;
    ipReal64        _alpha;

    ipReal64        _cosChi;
    ipReal64        _sinChi;
    ipReal64        _cosBeta;
    ipReal64        _sinBeta;
    ipReal64        _cosAlpha;
    ipReal64        _sinAlpha;

    ipReal64        _tx;

    ipReal64        _ty;

    ipReal64        _tz;
};


inline ipReal64
Rigid3d::getChi() const
{
    return _chi;
}

inline ipReal64
Rigid3d::getBeta() const
{
    return _beta;
}

inline ipReal64
Rigid3d::getAlpha() const
{
    return _alpha;
}

inline ipReal64
Rigid3d::getTx() const
{
    return _tx;
}

inline ipReal64
Rigid3d::getTy() const
{
    return _ty;
}

inline ipReal64
Rigid3d::getTz() const
{
    return _tz;
}

inline void
Rigid3d::setIdentity()
{
    setParams(getIdentityParams());
}


} // end of namespace transform
} // end of namespace math
} // end of namespace ipsdk

#pragma warning (pop)

#endif // __IPSDKMATH_RIGID3D_H__