rotation3_ref.h

Go to the documentation of this file.

#ifndef OPEN_WALKER_CORE_ROTATION3_REF_H
#define OPEN_WALKER_CORE_ROTATION3_REF_H

#include <ow_core/conversions.h>
#include <ow_core/matrix_ref.h>

namespace ow_core
{
template <typename _Derived>
class Rotation3Ref : public MatrixRef<_Derived, 3, 3>
{
public:
  typedef _Derived Derived;
  typedef MatrixRef<Derived, 3, 3> Base;

public:
  explicit Rotation3Ref(Derived& ref, int startRow = 0, int startCol = 0) : 
    Base(ref, startRow, startCol)
  {
  }

  using Base::operator=;

  using Base::operator*;

  template <typename OtherDerived>
  Rotation3Ref& operator=(const Eigen::RotationBase<OtherDerived, 3>& r)
  {
    *this = r.toRotationMatrix();
    return *this;
  }

  void operator=(const tf::Quaternion& q)
  {
    quaternionTFToEigenMatrix(q, *this);
  }

  void operator=(const geometry_msgs::Quaternion& q)
  {
    quaternionMsgToEigenMatrix(q, *this);
  }

  void operator=(const tf::Matrix3x3& R)
  {
    matrixTFToEigenMatrix(R, *this);
  }

  operator tf::Quaternion() const
  {
    tf::Quaternion q;
    matrixEigenToQuaternionTF(*this, q);
    return q;
  }

  operator geometry_msgs::Quaternion() const
  {
    geometry_msgs::Quaternion q;
    matrixEigenToQuaternionMsg(*this, q);
    return q;
  }

  operator tf::Matrix3x3() const
  {
    tf::Matrix3x3 m;
    matrixEigenToMatrixTF(*this, m);
    return m;
  }

  tf::Quaternion toQuaternionTF()
  {
    return static_cast<tf::Quaternion>(*this);
  }

  geometry_msgs::Quaternion toQuaternionMsg()
  {
    return static_cast<geometry_msgs::Quaternion>(*this);
  }

  tf::Matrix3x3 toMatrixTF()
  {
    return static_cast<tf::Matrix3x3>(*this);
  }

private:
  Rotation3Ref();
};

}  // namespace ow_core

#endif  // OPEN_WALKER_CORE_ROTATION3_REF_H