ABInertia.h

Go to the documentation of this file.

/*
 * Copyright 2012-2020 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include "EigenTypedef.h"
#include "fwd.h"
 
namespace sva
{
 
template<typename T>
class ABInertia
{
public:
  typedef Eigen::Vector3<T> vector3_t;
  typedef Eigen::Matrix3<T> matrix3_t;
  typedef Eigen::Matrix6<T> matrix6_t;
 
public:
  ABInertia() : M_(), H_(), I_() {}
 
  ABInertia(const matrix3_t & M, const matrix3_t & H, const matrix3_t & I)
  : M_(matrix3_t::Zero()), H_(H), I_(matrix3_t::Zero())
  {
    M_.template triangularView<Eigen::Lower>() = M;
    I_.template triangularView<Eigen::Lower>() = I;
  }
 
  ABInertia(const Eigen::TriangularView<matrix3_t, Eigen::Lower> & ltM,
            const matrix3_t & H,
            const Eigen::TriangularView<matrix3_t, Eigen::Lower> & ltI)
  : M_(ltM), H_(H), I_(ltI)
  {
  }
 
  // Accessor
  const matrix3_t & lowerTriangularMassMatrix() const
  {
    return M_;
  }
 
  matrix3_t massMatrix() const
  {
    matrix3_t M;
    M.template triangularView<Eigen::Upper>() = M_.transpose();
    M.template triangularView<Eigen::StrictlyLower>() = M_;
    return M;
  }
 
  const matrix3_t & gInertia() const
  {
    return H_;
  }
 
  const matrix3_t & lowerTriangularInertia() const
  {
    return I_;
  }
 
  matrix3_t inertia() const
  {
    matrix3_t I;
    I.template triangularView<Eigen::Upper>() = I_.transpose();
    I.template triangularView<Eigen::StrictlyLower>() = I_;
    return I;
  }
 
  matrix6_t matrix() const
  {
    matrix6_t m;
    m << inertia(), H_, H_.transpose(), massMatrix();
    return m;
  }
 
  template<typename T2>
  ABInertia<T2> cast() const
  {
    return ABInertia<T2>(M_.template cast<T2>(), H_.template cast<T2>(), I_.template cast<T2>());
  }
 
  // Operators
  ABInertia<T> operator+(const ABInertia<T> & rbI) const
  {
    matrix3_t M, I;
    M.template triangularView<Eigen::Lower>() = M_ + rbI.M_;
    I.template triangularView<Eigen::Lower>() = I_ + rbI.I_;
    return ABInertia<T>(M, H_ + rbI.H_, I);
  }
 
  ABInertia<T> operator-(const ABInertia<T> & rbI) const
  {
    matrix3_t M, I;
    M.template triangularView<Eigen::Lower>() = M_ - rbI.M_;
    I.template triangularView<Eigen::Lower>() = I_ - rbI.I_;
    return ABInertia<T>(M, H_ - rbI.H_, I);
  }
 
  ABInertia<T> operator-() const
  {
    return ABInertia<T>(-M_, -H_, -I_);
  }
 
  ABInertia<T> & operator+=(const ABInertia<T> & rbI)
  {
    M_.template triangularView<Eigen::Lower>() += rbI.M_;
    H_ += rbI.H_;
    I_.template triangularView<Eigen::Lower>() += rbI.I_;
    return *this;
  }
 
  ABInertia<T> & operator-=(const ABInertia<T> & rbI)
  {
    M_.template triangularView<Eigen::Lower>() -= rbI.M_;
    H_ -= rbI.H_;
    I_.template triangularView<Eigen::Lower>() -= rbI.I_;
    return *this;
  }
 
  template<typename T2>
  ABInertia<T> operator*(T2 scalar) const
  {
    matrix3_t M, I;
    M.template triangularView<Eigen::Lower>() = scalar * M_;
    I.template triangularView<Eigen::Lower>() = scalar * I_;
    return ABInertia<T>(M, scalar * H_, I);
  }
 
  ABInertia<T> operator+(const RBInertia<T> & rbI) const;
 
  ForceVec<T> operator*(const MotionVec<T> & mv) const;
 
  template<typename Derived>
  void mul(const Eigen::MatrixBase<Derived> & mv, Eigen::MatrixBase<Derived> & result) const;
 
  bool operator==(const ABInertia<T> & abI) const
  {
    return M_ == abI.M_ && H_ == abI.H_ && I_ == abI.I_;
  }
 
  bool operator!=(const ABInertia<T> & abI) const
  {
    return M_ != abI.M_ || H_ != abI.H_ || I_ != abI.I_;
  }
 
private:
  matrix3_t M_;
  matrix3_t H_;
  matrix3_t I_;
};
 
template<typename T, typename T2>
inline ABInertia<T> operator*(T2 scalar, const ABInertia<T> & abI)
{
  return abI * scalar;
}
 
template<typename T>
inline std::ostream & operator<<(std::ostream & out, const ABInertia<T> & abI)
{
  out << abI.matrix();
  return out;
}
 
} // namespace sva