rbd.h

Go to the documentation of this file.

/* CPYHDR { */
/*
 * This file is part of the 'iit-rbd' library.
 * Copyright © 2015 2016, Marco Frigerio (marco.frigerio@iit.it)
 *
 * See the LICENSE file for more information.
 */
/* } CPYHDR */
#ifndef IIT_RBD_H_
#define IIT_RBD_H_

#include <Eigen/Dense>
#include <Eigen/StdVector>
#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET 1
#include <Eigen/Sparse>


namespace iit {
namespace rbd {

// Use alias templates to limit the references to 'Eigen' to this header

template<typename Derived>
using MatrixBase = Eigen::MatrixBase< Derived >;

template<typename Scalar, int R, int C>
using PlainMatrix = Eigen::Matrix< Scalar, R, C >;

template<typename XprType, int R, int C>
using MatrixBlock = Eigen::Block< XprType, R, C >;

template<typename Scalar>
using SparseMatrix = Eigen::SparseMatrix<Scalar>;

template<typename Scalar>
using SparseVector = Eigen::SparseVector<Scalar>;

template<typename SCALAR>
struct Core
{
    typedef SCALAR Scalar;

    typedef PlainMatrix<Scalar,3,3> Matrix33;
    typedef PlainMatrix<Scalar,6,6> Matrix66;
    typedef PlainMatrix<Scalar,3,1> Vector3;
    typedef PlainMatrix<Scalar,6,1> Vector6;

    typedef Vector6  Vector6D;       // here the 'D' stands for Dimension, not double !
    typedef Vector6D Column6D;
    typedef Vector6D VelocityVector;
    typedef Vector6D ForceVector;

    typedef MatrixBlock<Vector6D,3,1>       Part3D;     
    typedef MatrixBlock<const Vector6D,3,1> Part3DConst;



    static inline Part3D angularPart(Vector6D& f) {
        return f.template topRows<3>();
    }
    static inline Part3D linearPart(Vector6D& f) {
        return f.template bottomRows<3>();
    }
    static inline Part3DConst angularPart(const Vector6D& f) {
        return f.template topRows<3>();
    }
    static inline Part3DConst linearPart(const Vector6D& f) {
        return f.template bottomRows<3>();
    }
};


typedef Core<double> Cored;

using Matrix33d = Cored::Matrix33;
using Vector3d  = Cored::Vector3;
using Matrix66d = Cored::Matrix66;
using Vector6d  = Cored::Vector6;

using Vector6D       = Cored::Vector6D;
using VelocityVector = Cored::VelocityVector;
using ForceVector    = Cored::ForceVector;

using Part3D      = Cored::Part3D;
using Part3DConst = Cored::Part3DConst;

using Column6d = Cored::Column6D;  // an alias

using SparseMatrixd = SparseMatrix<double>;
using SparseColumnd = SparseVector<double>;


inline Part3D angularPart(Vector6D& f) {
    return Cored::angularPart(f);
}
inline Part3D linearPart(Vector6D& f) {
    return Cored::linearPart(f);
}
inline Part3DConst angularPart(const Vector6D& f) {
    return Cored::angularPart(f);
}
inline Part3DConst linearPart(const Vector6D& f) {
    return Cored::linearPart(f);
}

enum Coords3D { X=0, Y, Z};
enum Coords6D { AX=0, AY, AZ, LX, LY, LZ };


static const double g = 9.81;

}
}


#endif /* IIT_RBD_H_ */