Twist.h

Go to the documentation of this file.

// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009-2013 CEA LIST (DIASI/LSI) <xde-support@saxifrage.cea.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef EIGEN_LGSM_LIE_TWIST_H
#define EIGEN_LGSM_LIE_TWIST_H

/*******************************************************************************
* Definition/implementation of TwistBase
********************************************************************************/

namespace internal {
  template<class Derived>
    struct traits<TwistBase<Derived> > : traits<LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived> > {
    };
}

template<class Derived> 
class TwistBase : public LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived>{
protected:
  typedef LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived> Base;
public:
  // inherit MatrixBase interface
  EIGEN_DENSE_PUBLIC_INTERFACE(TwistBase)
  // inherit assignement operator
  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(TwistBase)

  typedef typename Base::BaseType BaseType;
  typedef typename internal::traits<Derived>::Coefficients Coefficients;

  inline Scalar rx() const { return this->getAngularVelocity().x(); }
  inline Scalar ry() const { return this->getAngularVelocity().y(); }
  inline Scalar rz() const { return this->getAngularVelocity().z(); }
  inline Scalar vx() const { return this->getLinearVelocity().x(); }
  inline Scalar vy() const { return this->getLinearVelocity().y(); }
  inline Scalar vz() const { return this->getLinearVelocity().z(); }

  inline Scalar& rx() { return this->getAngularVelocity().x(); }
  inline Scalar& ry() { return this->getAngularVelocity().y(); }
  inline Scalar& rz() { return this->getAngularVelocity().z(); }
  inline Scalar& vx() { return this->getLinearVelocity().x(); }
  inline Scalar& vy() { return this->getLinearVelocity().y(); }
  inline Scalar& vz() { return this->getLinearVelocity().z(); }

  typedef Matrix<Scalar, 3, 1> LinearVelocity;
  typedef LieAlgebra<Matrix<Scalar, 3, 1> > AngularVelocity;
  
  inline Map<AngularVelocity> getAngularVelocity(){ return Map<AngularVelocity>(this->derived().get().template head<3>().data()); }
  inline Map<const AngularVelocity> getAngularVelocity() const {return  Map<const AngularVelocity>(this->derived().get().template head<3>().data()); }
  inline Map<LinearVelocity> getLinearVelocity() { return  Map<LinearVelocity>(this->derived().get().template tail<3>().data()); }
  inline Map<const LinearVelocity> getLinearVelocity() const { return  Map<const LinearVelocity>(this->derived().get().template tail<3>().data()); }

  template<class RotationDerived> inline Twist<Scalar> changeFrame(const LieGroupBase<Quaternion<Scalar>, RotationDerived>&) const;

  template<class OtherDerived> inline Twist<Scalar> changePoint(const MatrixBase<OtherDerived>& point) const;
};


template<class Derived>
template<class RotationDerived> 
inline Twist<typename internal::traits<TwistBase<Derived> >::Scalar> TwistBase<Derived>::changeFrame(const LieGroupBase<Quaternion<Scalar>, RotationDerived>& rot) const {
  return Twist<Scalar>(rot*this->getAngularVelocity(),
                       rot*this->getLinearVelocity());
}

template<class Derived>
template<class OtherDerived> 
inline Twist<typename internal::traits<TwistBase<Derived> >::Scalar> TwistBase<Derived>::changePoint(const MatrixBase<OtherDerived>& point) const {
  return Twist<Scalar>(this->getAngularVelocity(),
                       this->getLinearVelocity() + this->getAngularVelocity().cross(point) );
}

/**************************
 * Implementation of Twist
 **************************/

namespace internal {  
  template<typename Scalar>
    struct traits<Twist<Scalar> > 
    : public traits<LieAlgebra<Matrix<Scalar, 6, 1> > > 
    {
      typedef Displacement<Scalar> Group;
    };

  template<typename Scalar, int Options>
    struct traits<Map<Twist<Scalar>, Options> > 
    : public traits<Map<LieAlgebra<Matrix<Scalar, 6, 1> >, Options > > 
    {
      typedef Displacement<Scalar> Group;
    };
}

template<typename _Scalar>
class Twist : public TwistBase<Twist<_Scalar> >{
protected:
  typedef TwistBase<Twist<_Scalar> > Base;
public:
  // inherit MatrixBase interface
  EIGEN_DENSE_PUBLIC_INTERFACE(Twist)
  // inherit assignement operator
  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Twist)

  typedef typename Base::BaseType BaseType;
  typedef typename internal::traits<Twist>::Coefficients Coefficients;


  inline Twist() : m_coeffs() {}
  inline Twist(const Twist& g) : m_coeffs(g.get() ) {}
  inline Twist(const BaseType& g) : m_coeffs(g) {}
  inline Twist(Scalar rx, Scalar ry, Scalar rz, Scalar vx, Scalar vy, Scalar vz) {
    m_coeffs[0] = rx;
    m_coeffs[1] = ry;
    m_coeffs[2] = rz;
    m_coeffs[3] = vx;
    m_coeffs[4] = vy;
    m_coeffs[5] = vz;
  }
  template<typename OtherDerived>
  EIGEN_STRONG_INLINE Twist(const MatrixBase<OtherDerived>& other)
    : m_coeffs(other)
  {
  }
  //template<typename OtherDerived>
  //EIGEN_STRONG_INLINE Twist& operator=(const MatrixBase<OtherDerived>& other) 
  //{
  //  this->m_coeffs = other;
  //  return *this;
  //}
  template<class LinearVelocityDerived, class AngularVelocityDerived>
  inline Twist(const LieAlgebraBase<Matrix<Scalar, 3, 1>, AngularVelocityDerived>& r, const MatrixBase<LinearVelocityDerived>& v) {
    this->getLinearVelocity() = v;
    this->getAngularVelocity() = r;
  }
  inline Twist(const typename Base::AngularVelocity& r, const typename Base::LinearVelocity& v) {
    this->getLinearVelocity() = v;
    this->getAngularVelocity() = r;
  }
  inline Coefficients& get() { return m_coeffs; }
  inline const Coefficients& get() const { return m_coeffs; }

protected:
  Coefficients m_coeffs;
};

typedef Twist<double> Twistd;
typedef Twist<float> Twistf;

/**************************************
 * Implementation of Map<Twist>
 **************************************/


template<typename _Scalar, int MapOptions, typename StrideType>
class Map<Twist<_Scalar>, MapOptions, StrideType> : public TwistBase<Map<Twist<_Scalar>, MapOptions, StrideType> >{
  protected:
    typedef TwistBase<Map<Twist<_Scalar> > > Base;
  public:
    EIGEN_DENSE_PUBLIC_INTERFACE(Map)
      EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
      typedef typename internal::traits<Map>::Coefficients Coefficients;

    inline Map(const Twist<Scalar>& d) : m_coeffs(d.get()) {};
    template<int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols> 
      inline Map(const Array<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& g) : m_coeffs(g.data()) {};

    inline Map(Scalar* data) : m_coeffs(data) {};
    inline Map(const Map& m) : m_coeffs(m.get()) {};

    inline Coefficients& get() { return m_coeffs; }
    inline const Coefficients& get() const { return m_coeffs; }

  protected:
    Coefficients m_coeffs;
};

#endif