GeoPose.h

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