Pose.h

#pragma once
 
#include <knowGIS/Point.h>
 
#include "Quaternion.h"
 
namespace knowGIS
{
  class Pose
  {
  public:
    Pose() {}
    Pose(const knowGIS::Point& _position, const Quaternion& _orientation)
        : m_position(_position), m_orientation(_orientation)
    {
    }
    ~Pose() {}
    Pose transform(const Cartography::CoordinateSystem& _coordinateSystem) const;
    knowGIS::Point position() const { return m_position; }
    Quaternion orientation() const { return m_orientation; }
    bool operator==(const Pose& _rhs) const
    {
      return m_position == _rhs.m_position and m_orientation == _rhs.m_orientation;
    }
  private:
    knowGIS::Point m_position;
    Quaternion m_orientation;
  };
} // namespace knowGIS
 
#include <knowCore/Formatter.h>
clog_format_declare_formatter(knowGIS::Pose)
{
  return format_to(ctx.out(), "(pos: {} quat: {})", p.position(), p.orientation());
}
 
KNOWCORE_DECLARE_FULL_METATYPE(knowGIS, Pose);