RotoTrans.h

#ifndef BIORBD_UTILS_ROTO_TRANS_H
#define BIORBD_UTILS_ROTO_TRANS_H
 
#include <vector>
#include <memory>
#include "rbdl_math.h"
#include "Utils/Scalar.h"
 
namespace RigidBodyDynamics { namespace Math {
struct SpatialTransform;
}}
 
#include "biorbdConfig.h"
 
namespace biorbd {
namespace rigidbody {
class NodeSegment;
}
 
namespace utils {
class String;
class Vector;
class Rotation;
class Vector3d;
 
#ifdef SWIG
class BIORBD_API RotoTrans
#else
class BIORBD_API RotoTrans : public RigidBodyDynamics::Math::Matrix4d
#endif
{
public:
    RotoTrans(
            const RigidBodyDynamics::Math::Matrix4d& matrix = RigidBodyDynamics::Math::Matrix4d::Identity());
 
    RotoTrans(
            const biorbd::utils::Scalar& v00, const biorbd::utils::Scalar& v01, const biorbd::utils::Scalar& v02, const biorbd::utils::Scalar& v03,
            const biorbd::utils::Scalar& v10, const biorbd::utils::Scalar& v11, const biorbd::utils::Scalar& v12, const biorbd::utils::Scalar& v13,
            const biorbd::utils::Scalar& v20, const biorbd::utils::Scalar& v21, const biorbd::utils::Scalar& v22, const biorbd::utils::Scalar& v23,
            const biorbd::utils::Scalar& v30, const biorbd::utils::Scalar& v31, const biorbd::utils::Scalar& v32, const biorbd::utils::Scalar& v33);
 
    RotoTrans(
            const biorbd::utils::Rotation& rot);
 
    RotoTrans(
            const biorbd::utils::Rotation& rot,
            const biorbd::utils::Vector3d& trans);
 
    RotoTrans(
            const biorbd::utils::Vector& rotation,
            const biorbd::utils::Vector3d& translation,
            const biorbd::utils::String &rotationSequence);
 
    RotoTrans(
            const RigidBodyDynamics::Math::SpatialTransform& st);
 
#ifdef BIORBD_USE_EIGEN3_MATH
 
    template<typename OtherDerived> RotoTrans(
            const Eigen::MatrixBase<OtherDerived>& other) :
        RigidBodyDynamics::Math::Matrix4d(other){
        checkUnitary();
    }
 
#endif
 
    static biorbd::utils::RotoTrans fromMarkers(
            const biorbd::rigidbody::NodeSegment& origin,
            const std::pair<biorbd::rigidbody::NodeSegment, biorbd::rigidbody::NodeSegment>& axis1markers,
            const std::pair<biorbd::rigidbody::NodeSegment, biorbd::rigidbody::NodeSegment>& axis2markers,
            const std::pair<biorbd::utils::String, biorbd::utils::String> &axesNames,
            const biorbd::utils::String& axisToRecalculate);
 
    biorbd::utils::Vector3d axe(
            unsigned int idx) const ;
 
    biorbd::utils::RotoTrans transpose() const;
 
    biorbd::utils::Vector3d trans() const;
 
    biorbd::utils::Rotation rot() const;
 
    static biorbd::utils::RotoTrans combineRotAndTrans(
            const biorbd::utils::Rotation& rot,
            const biorbd::utils::Vector3d& trans);
 
    static biorbd::utils::RotoTrans fromSpatialTransform(
            const RigidBodyDynamics::Math::SpatialTransform& st);
 
    static biorbd::utils::RotoTrans fromEulerAngles(
            const biorbd::utils::Vector &rot,
            const biorbd::utils::Vector3d& trans,
            const biorbd::utils::String& seq);
 
    static biorbd::utils::Vector toEulerAngles(
            const biorbd::utils::RotoTrans& rt,
            const biorbd::utils::String &seq);
 
#ifndef BIORBD_USE_CASADI_MATH
    static biorbd::utils::RotoTrans mean(
            const std::vector<biorbd::utils::RotoTrans>&rt);
#endif
 
#ifndef SWIG
#ifdef BIORBD_USE_EIGEN3_MATH
    template<typename OtherDerived>
        biorbd::utils::RotoTrans& operator=(const Eigen::MatrixBase <OtherDerived>& other){
            Eigen::Matrix4d::operator=(other);
            return *this;
        }
#endif
#endif
 
protected:
    RigidBodyDynamics::Math::Vector4d expand3dTo4d(const biorbd::utils::Vector3d& v1);
 
    void checkUnitary();
};
 
}}
 
#ifndef SWIG
std::ostream& operator<<(std::ostream& os, const biorbd::utils::RotoTrans &rt);
#endif
 
#endif // BIORBD_UTILS_ROTO_TRANS_H