Rotation.h

#ifndef BIORBD_UTILS_ROTATION_H
#define BIORBD_UTILS_ROTATION_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 Vector3d;
class Matrix;
 
#ifdef SWIG
class BIORBD_API Rotation
#else
class BIORBD_API Rotation : public RigidBodyDynamics::Math::Matrix3d
#endif
{
public:
    Rotation(
            const RigidBodyDynamics::Math::Matrix3d& matrix = RigidBodyDynamics::Math::Matrix3d::Identity());
 
#ifdef BIORBD_USE_EIGEN3_MATH
    template<typename OtherDerived> Rotation(
            const Eigen::MatrixBase<OtherDerived>& other) :
        RigidBodyDynamics::Math::Matrix3d(other){
        checkUnitary();
    }
#endif
#ifdef BIORBD_USE_CASADI_MATH
    Rotation(
            const RigidBodyDynamics::Math::MatrixNd& m);
#endif
 
    Rotation(const biorbd::utils::Scalar& v00, const biorbd::utils::Scalar& v01, const biorbd::utils::Scalar& v02,
             const biorbd::utils::Scalar& v10, const biorbd::utils::Scalar& v11, const biorbd::utils::Scalar& v12,
             const biorbd::utils::Scalar& v20, const biorbd::utils::Scalar& v21, const biorbd::utils::Scalar& v22);
 
    Rotation(
            const biorbd::utils::Vector& rotation,
            const biorbd::utils::String& rotationSequence);
 
    Rotation(
            const RigidBodyDynamics::Math::SpatialTransform& st);
 
    biorbd::utils::Vector3d axe(
            unsigned int idx) const;
 
    static biorbd::utils::Rotation fromSpatialTransform(
            const RigidBodyDynamics::Math::SpatialTransform& st);
 
    static biorbd::utils::Rotation fromEulerAngles(
            const biorbd::utils::Vector& rot,
            const biorbd::utils::String& seq);
 
    static biorbd::utils::Matrix fromMarkersNonNormalized(
            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);
 
    static biorbd::utils::Rotation fromMarkers(
            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);
 
    static  biorbd::utils::Vector toEulerAngles(
            const biorbd::utils::Rotation& r,
            const biorbd::utils::String& seq);
 
#ifndef BIORBD_USE_CASADI_MATH
    static biorbd::utils::Rotation mean(
            const std::vector<biorbd::utils::Rotation>& mToMean);
#endif
 
#ifndef SWIG
#ifdef BIORBD_USE_EIGEN3_MATH
    template<typename OtherDerived>
        biorbd::utils::Rotation& operator=(
                const Eigen::MatrixBase <OtherDerived>& other){
            Eigen::Matrix3d::operator=(other);
            return *this;
        }
#endif
#endif
 
protected:
    void checkUnitary();
};
 
}}
 
#ifndef SWIG
std::ostream& operator<<(std::ostream& os, const biorbd::utils::Rotation &rt);
#endif
 
#endif // BIORBD_UTILS_ROTO_TRANS_H