Framed.h

Go to the documentation of this file.

 
#include "ReferenceFrame.h"
 
#ifndef _GAMS_POSE_FRAMED_H_
#define _GAMS_POSE_FRAMED_H_
 
#include "gams/GamsExport.h"
#include <string>
#include <cfloat>
#include <utility>
#include "gams/CPP11_compat.h"
#include "madara/knowledge/containers/DoubleVector.h"
#include "madara/knowledge/containers/NativeDoubleVector.h"
#include "ReferenceFrameFwd.h"
#include "Coordinate.h"
 
namespace gams { namespace pose {
 
template<typename T>
inline auto try_transform_to(const T& v, const ReferenceFrame &frame) ->
  typename std::enable_if<supports_transform_to<T>::value, T>::type
{
  return v.transform_to(frame);
}
 
template<typename T>
inline auto try_transform_to(const T& v, const ReferenceFrame &) ->
  typename std::enable_if<!supports_transform_to<T>::value, const T &>::type
{
  return v;
}
 
inline ReferenceFrame try_get_frame()
{
  return default_frame();
}
 
template<typename T, typename... Args>
inline auto try_get_frame(const T& v, Args&&...) ->
  typename std::enable_if<supports_transform_to<T>::value, ReferenceFrame>::type
{
  return v.frame();
}
 
template<typename T, typename... Args>
inline auto try_get_frame(const T&, Args&&... args) ->
  typename std::enable_if<!supports_transform_to<T>::value, ReferenceFrame>::type
{
  return try_get_frame(std::forward<Args>(args)...);
}
 
template<typename Impl>
class Framed : public Impl
{
public:
  using Impl::self;
  using derived_type = typename Impl::derived_type;
 
  Framed() = default;
 
#ifdef DOXYGEN
  template<typename... Args>
  Framed(Args&&... args);
#else
  template<typename Arg, typename... Args,
    typename std::enable_if<!std::is_same<typename std::decay<Arg>::type,
      ReferenceFrame>::value && sizeof...(Args) >= 1, void*>::type = nullptr>
  Framed(Arg&& arg, Args&&... args)
    : Impl((Arg &)arg, ((Args &)args)...),
      frame_(try_get_frame((Arg &)arg, ((Args &)args)...)) {}
 
  template<typename Arg,
    typename std::enable_if<
      !std::is_same<typename std::decay<Arg>::type,
        ReferenceFrame>::value, void*>::type = nullptr>
  explicit Framed(Arg&& arg)
    : Impl((Arg &)arg), frame_(try_get_frame(arg)) {}
#endif
 
  explicit Framed(ReferenceFrame frame)
    : Impl(), frame_(frame) {}
 
#ifdef DOXYGEN
  template<typename... Args>
  Framed(ReferenceFrame frame, Args&&... args);
#else
  template<typename... Args,
    typename std::enable_if<(sizeof...(Args) >= 1), void*>::type = nullptr>
  Framed(ReferenceFrame frame, Args&&... args)
    : Impl(try_transform_to(std::forward<Args>(args), frame)...),
           frame_(frame) {}
#endif
 
  const ReferenceFrame &frame() const { return frame_; }
 
  ReferenceFrame frame(ReferenceFrame new_frame)
  {
    using std::swap;
    swap(frame_, new_frame);
    return new_frame;
  }
 
  template<typename Base2>
  bool approximately_equal(const Framed<Base2> &other,
      double epsilon) const
  {
    return std::fabs(self().distance_to(other.self())) < epsilon;
  }
 
  /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
   * The four methods below are defined in ReferenceFrame.inl,
   * due to circular dependencies
   * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 
  derived_type WARN_UNUSED
  transform_to(const ReferenceFrame &new_frame) const;
 
  void transform_this_to(const ReferenceFrame &new_frame);
 
  double distance_to(const derived_type &target) const;
 
  //using Base::distance_to;
 
  void normalize();
 
private:
  ReferenceFrame frame_ = default_frame();
};
 
} }
 
#endif