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;
  };
}
 
#include <knowCore/Formatter.h>
KNOWCORE_CORE_DECLARE_FORMATTER(knowGIS::Pose)
{
  return format_to(ctx.out(), "(pos: {} quat: {})", p.position(), p.orientation());
}
 
KNOWCORE_DECLARE_FULL_METATYPE(knowGIS, Pose)