ReferenceFrame.h

Go to the documentation of this file.

 
#ifndef _GAMS_POSE_REFERENCE_FRAME_H_
#define _GAMS_POSE_REFERENCE_FRAME_H_
 
#include "gams/GamsExport.h"
#include "gams/CPP11_compat.h"
#include <vector>
#include <string>
#include <cstring>
#include <sstream>
#include <utility>
#include <memory>
#include <stdexcept>
#include <mutex>
#include "ReferenceFrameFwd.h"
#include "CartesianFrame.h"
#include "Pose.h"
#include "Quaternion.h"
#include "madara/knowledge/EvalSettings.h"
#include "gams/exceptions/ReferenceFrameException.h"
 
namespace gams { namespace pose {
 
class ReferenceFrameVersion;
class ReferenceFrameIdentity;
 
class ReferenceFrameArena
{
private:
  std::map<std::string,
      std::weak_ptr<ReferenceFrameIdentity>> idents_;
 
public:
  std::shared_ptr<ReferenceFrameIdentity> lookup(std::string id);
 
  std::shared_ptr<ReferenceFrameIdentity> find(std::string id) const;
 
  std::shared_ptr<ReferenceFrameIdentity> make_guid();
 
  void gc();
};
 
class GAMS_EXPORT ReferenceFrameIdentity
{
public:
  static const uint64_t ETERNAL = ReferenceFrame::ETERNAL;
  static const uint64_t TEMP = ReferenceFrame::TEMP;
 
private:
  std::string id_;
 
  static ReferenceFrameArena arena_;
  //static std::map<std::string,
      //std::weak_ptr<ReferenceFrameIdentity>> idents_;
 
  static uint64_t default_expiry_;
 
  static std::recursive_mutex idents_lock_;
 
  mutable std::map<uint64_t, std::weak_ptr<ReferenceFrameVersion>>
    versions_;
 
  mutable uint64_t expiry_ = ETERNAL;
 
  mutable std::recursive_mutex versions_lock_;
 
public:
  ReferenceFrameIdentity(std::string id, uint64_t expiry)
    : id_(std::move(id)), expiry_(expiry) {}
 
  static std::shared_ptr<ReferenceFrameIdentity> lookup(std::string id)
  {
    std::lock_guard<std::recursive_mutex> guard(idents_lock_);
 
    return arena_.lookup(std::move(id));
  }
 
  static std::shared_ptr<ReferenceFrameIdentity> find(std::string id)
  {
    std::lock_guard<std::recursive_mutex> guard(idents_lock_);
 
    return arena_.find(std::move(id));
  }
 
  static std::shared_ptr<ReferenceFrameIdentity> make_guid()
  {
    std::lock_guard<std::recursive_mutex> guard(idents_lock_);
 
    return arena_.make_guid();
  }
 
  void register_version(uint64_t timestamp,
      std::shared_ptr<ReferenceFrameVersion> ver) const
  {
    std::lock_guard<std::recursive_mutex> guard(versions_lock_);
 
    std::weak_ptr<ReferenceFrameVersion> weak(std::move(ver));
    versions_[timestamp] = ver;
  }
 
  std::shared_ptr<ReferenceFrameVersion> get_version(uint64_t timestamp) const
  {
    std::lock_guard<std::recursive_mutex> guard(versions_lock_);
 
    auto find = versions_.find(timestamp);
 
    if (find == versions_.end()) {
      return nullptr;
    }
 
    return find->second.lock();
  }
 
  const std::string &id() const { return id_; }
 
  static uint64_t default_expiry(uint64_t age) {
    std::lock_guard<std::recursive_mutex> guard(idents_lock_);
 
    uint64_t ret = default_expiry_;
    default_expiry_ = age;
    return ret;
  }
 
  static uint64_t default_expiry() {
    std::lock_guard<std::recursive_mutex> guard(idents_lock_);
    return default_expiry_;
  }
 
  uint64_t expiry(uint64_t age) const {
    std::lock_guard<std::recursive_mutex> guard(versions_lock_);
 
    uint64_t ret = expiry_;
    expiry_ = age;
    return ret;
  }
 
  uint64_t expiry() const {
    std::lock_guard<std::recursive_mutex> guard(versions_lock_);
    return expiry_;
  }
 
  void expire_older_than(madara::knowledge::KnowledgeBase &kb,
      uint64_t time, const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT) const;
 
  static const std::string &default_prefix() {
    std::lock_guard<std::recursive_mutex> guard(idents_lock_);
    return FrameEvalSettings::default_prefix();
  }
 
  static void gc()
  {
    std::lock_guard<std::recursive_mutex> guard(idents_lock_);
 
    return arena_.gc();
  }
 
  void gc_versions();
};
 
namespace impl {
  inline static std::string &make_kb_key(
      std::string &prefix,
      const std::string &id)
  {
    prefix.reserve(prefix.capacity() + 1 + id.size());
 
    prefix += ".";
    prefix += id;
 
    return prefix;
  }
 
  inline static std::string &make_kb_key(
      std::string &prefix, uint64_t timestamp)
  {
    if (timestamp == ReferenceFrameIdentity::ETERNAL) {
      prefix += ".inf";
    } else {
      prefix += ".";
 
      std::ostringstream oss;
      oss.fill('0');
      oss.width(16);
      oss << std::hex << timestamp;
 
      prefix += oss.str();
    }
 
    return prefix;
  }
 
  inline static std::string &make_kb_key(
      std::string &prefix,
      const std::string &id, uint64_t timestamp)
  {
    make_kb_key(prefix, id);
    make_kb_key(prefix, timestamp);
    return prefix;
  }
}
 
MADARA_MAKE_VAL_SUPPORT_TEST(nano_timestamp, x,
    (x.nanos(), x.nanos(0UL)));
 
inline uint64_t try_get_nano_time(uint64_t def)
{
  return def;
}
 
template<typename T, typename... Args>
inline auto try_get_nano_time(uint64_t, const T& v, Args&&...) ->
  typename std::enable_if<supports_nano_timestamp<T>::value, uint64_t>::type
{
  return v.nanos();
}
 
template<typename T, typename... Args>
inline auto try_get_nano_time(uint64_t def, const T&, Args&&... args) ->
  typename std::enable_if<!supports_nano_timestamp<T>::value, uint64_t>::type
{
  return try_get_nano_time(def, std::forward<Args>(args)...);
}
 
class GAMS_EXPORT ReferenceFrameVersion :
  public std::enable_shared_from_this<ReferenceFrameVersion>
{
private:
  mutable std::shared_ptr<ReferenceFrameIdentity> ident_;
  const ReferenceFrameType *type_;
  uint64_t timestamp_ = ETERNAL;
  Pose origin_;
  mutable bool interpolated_ = false;
  bool temp_ = false;
 
private:
  template<typename T>
  static uint64_t init_timestamp(uint64_t given, const T &p)
  {
    if (given == ETERNAL) {
      return try_get_nano_time(ETERNAL, p);
    } else {
      return given;
    }
  }
 
public:
  static const uint64_t ETERNAL = ReferenceFrameIdentity::ETERNAL;
  static const uint64_t TEMP = ReferenceFrameIdentity::TEMP;
 
  template<typename P,
    typename std::enable_if<
      supports_transform_to<P>::value, void*>::type = nullptr>
  explicit ReferenceFrameVersion(
      P &&origin,
      uint64_t timestamp = ETERNAL,
      bool temp = false)
    : ReferenceFrameVersion({}, Cartesian,
        std::forward<P>(origin),
        timestamp, temp) {}
 
  template<typename P,
    typename std::enable_if<
      supports_transform_to<P>::value, void*>::type = nullptr>
  ReferenceFrameVersion(
      const ReferenceFrameType *type,
      P &&origin,
      uint64_t timestamp = ETERNAL,
      bool temp = false)
    : ReferenceFrameVersion({}, type,
        std::forward<P>(origin), timestamp, temp) {}
 
  template<typename P,
    typename std::enable_if<
      supports_transform_to<P>::value, void*>::type = nullptr>
  ReferenceFrameVersion(
      std::string name,
      P &&origin,
      uint64_t timestamp = ETERNAL,
      bool temp = false)
    : ReferenceFrameVersion(
        ReferenceFrameIdentity::lookup(std::move(name)), Cartesian,
        std::forward<P>(origin), timestamp, temp) {}
 
  template<typename P,
    typename std::enable_if<
      supports_transform_to<P>::value, void*>::type = nullptr>
  ReferenceFrameVersion(
      const ReferenceFrameType *type,
      std::string name,
      P &&origin,
      uint64_t timestamp = ETERNAL,
      bool temp = false)
    : ReferenceFrameVersion(
        ReferenceFrameIdentity::lookup(std::move(name)), type,
        std::forward<P>(origin), timestamp, temp) {}
 
  template<typename P,
    typename std::enable_if<
      supports_transform_to<P>::value, void*>::type = nullptr>
  ReferenceFrameVersion(
      std::shared_ptr<ReferenceFrameIdentity> ident,
      const ReferenceFrameType *type,
      P &&origin,
      uint64_t timestamp = ETERNAL,
      bool temp = false)
    : ident_(std::move(ident)),
      type_(type),
      timestamp_(init_timestamp(timestamp, origin)),
      origin_(std::forward<P>(origin)),
      temp_(temp) {}
 
  template<typename P,
    typename std::enable_if<
      supports_transform_to<P>::value, void*>::type = nullptr>
  ReferenceFrameVersion(
      std::shared_ptr<ReferenceFrameIdentity> ident,
      P &&origin,
      uint64_t timestamp = ETERNAL,
      bool temp = false)
    : ReferenceFrameVersion(std::move(ident), Cartesian,
        std::forward<P>(origin), timestamp, temp) {}
 
  const ReferenceFrameIdentity &ident() const {
    if (!ident_) {
      ident_ = ReferenceFrameIdentity::make_guid();
    }
    return *ident_;
  }
 
  const ReferenceFrameType *type() const { return type_; }
 
  const Pose &origin() const {
    return origin_;
  }
 
  Pose &mut_origin() {
    return origin_;
  }
 
  ReferenceFrame pose(Pose new_origin) const {
    return pose(new_origin, timestamp_);
  }
 
  ReferenceFrame move(Position new_origin) const {
    return move(new_origin, timestamp_);
  }
 
  ReferenceFrame orient(Orientation new_origin) const {
    return orient(new_origin, timestamp_);
  }
 
  ReferenceFrame pose(const Pose &new_origin, uint64_t timestamp) const {
    return ReferenceFrame(std::make_shared<ReferenceFrameVersion>(
          ident_, type(), new_origin, timestamp));
  }
 
  ReferenceFrame move(const Position &new_origin,
                      uint64_t timestamp) const {
    return pose(Pose(new_origin, Orientation(origin_)), timestamp);
  }
 
  ReferenceFrame orient(const Orientation &new_origin,
                        uint64_t timestamp) const {
    return pose(Pose(Position(origin_), new_origin), timestamp);
  }
 
  ReferenceFrame origin_frame() const {
    return origin_.frame();
  }
 
  bool operator==(const ReferenceFrame &other) const;
 
  bool operator!=(const ReferenceFrame &other) const;
 
  bool operator==(const ReferenceFrameVersion &other) const;
 
  bool operator!=(const ReferenceFrameVersion &other) const;
 
  const char *name() const {
    return type()->name;
  }
 
  const std::string &id() const {
    return ident().id();
  }
 
  bool has_id() const {
    return (bool)ident_;
  }
 
  const std::string *id_ptr() const {
    return has_id() ? nullptr : &id();
  }
 
  uint64_t timestamp() const {
    return timestamp_;
  }
 
  ReferenceFrame timestamp(uint64_t timestamp) const {
    return ReferenceFrame(std::make_shared<ReferenceFrameVersion>(
          ident_, type(), origin(), timestamp));
  }
 
  bool interpolated() const {
    return interpolated_;
  }
 
  bool temp() const {
    return temp_;
  }
 
  static const std::string &default_prefix() {
    return ReferenceFrameIdentity::default_prefix();
  }
 
  std::string key(const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT) const {
    std::string prefix = settings.prefix();
    impl::make_kb_key(prefix, id(), timestamp());
    return prefix;
  }
 
  void save(madara::knowledge::KnowledgeBase &kb,
            uint64_t expiry,
            const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT) const {
    std::string key = this->key(settings);
    save_as(kb, std::move(key), expiry, settings);
  }
 
  void save(madara::knowledge::KnowledgeBase &kb,
      const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT) const {
    std::string key = this->key(settings);
    save_as(kb, std::move(key), settings);
  }
 
  static ReferenceFrame load_exact(
          madara::knowledge::KnowledgeBase &kb,
          const std::string &id,
          uint64_t timestamp = ETERNAL,
          uint64_t parent_timestamp = ETERNAL,
          const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT,
          bool throw_on_errors = true,
          ReferenceFrameArena* arena = nullptr);
 
private:
  static ReferenceFrame load_exact_internal(
          madara::knowledge::KnowledgeBase &kb,
          const std::string &id,
          uint64_t timestamp = ETERNAL,
          uint64_t parent_timestamp = ETERNAL,
          const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT,
          bool throw_on_errors = true,
          ReferenceFrameArena* arena = nullptr);
 
public:
  static ReferenceFrame load(
          madara::knowledge::KnowledgeBase &kb,
          const std::string &id,
          uint64_t timestamp = ETERNAL,
          const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT,
          bool throw_on_errors = true,
          ReferenceFrameArena* arena = nullptr);
 
  static uint64_t latest_timestamp(
          madara::knowledge::KnowledgeBase &kb,
          const std::string &id,
          const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT);
 
private:
  using ancestor_elem = std::pair<std::string, uint64_t>;
  using ancestor_vec = std::vector<ancestor_elem>;
 
  static std::pair<uint64_t, uint64_t> find_nearest_neighbors(
      madara::knowledge::KnowledgeBase &kb, const std::string &id,
      uint64_t timestamp, const FrameEvalSettings &settings);
 
  static ancestor_vec get_ancestry(
      madara::knowledge::KnowledgeBase &kb,
      std::string name,
      const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT);
 
  static uint64_t find_common_timestamp_to_first_ancestor(
      const std::vector<ancestor_vec> &stamps);
 
public:
  template<typename InputIterator>
  static uint64_t latest_common_timestamp(
          madara::knowledge::KnowledgeBase &kb,
          InputIterator begin,
          InputIterator end,
          const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT)
  {
    if (begin == end) {
      return 0UL - 1;
    }
 
    std::vector<ancestor_vec> stamps;
 
    if (std::is_same<
        typename std::iterator_traits<InputIterator>::iterator_category,
        std::random_access_iterator_tag>::value) {
      size_t count = std::distance(begin, end);
 
      if (count == 1) {
        madara::knowledge::ContextGuard guard(kb);
 
        return find_nearest_neighbors(kb, *begin, -1, settings).first;
      }
 
      stamps.reserve(count);
    }
 
    {
      madara::knowledge::ContextGuard guard(kb);
 
      InputIterator cur = begin;
      while (cur != end) {
        stamps.emplace_back(get_ancestry(kb, *cur, settings));
        ++cur;
      }
    }
 
    return find_common_timestamp_to_first_ancestor(stamps);
  }
 
  template<typename Container>
  static uint64_t latest_common_timestamp(
          madara::knowledge::KnowledgeBase &kb,
          const Container &ids,
          const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT)
  {
    madara::knowledge::ContextGuard guard(kb);
 
    return latest_common_timestamp(kb, ids.cbegin(), ids.cend(),
        settings);
  }
 
  template<typename ForwardIterator>
  static std::vector<ReferenceFrame> load_tree(
          madara::knowledge::KnowledgeBase &kb,
          ForwardIterator begin,
          ForwardIterator end,
          uint64_t timestamp = ETERNAL,
          const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT,
          ReferenceFrameArena* arena = nullptr)
  {
    std::vector<ReferenceFrame> ret;
    if (std::is_same<
        typename std::iterator_traits<ForwardIterator>::iterator_category,
        std::random_access_iterator_tag>::value) {
      size_t count = std::distance(begin, end);
      ret.reserve(count);
    }
 
    madara::knowledge::ContextGuard guard(kb);
 
    if (timestamp == ETERNAL) {
      timestamp = latest_common_timestamp(kb, begin, end, settings);
    }
 
    ReferenceFrameArena local_arena;
    if (arena == nullptr) {
      arena = &local_arena;
    }
 
    while (begin != end) {
      ReferenceFrame frame = load(kb, *begin, timestamp, settings, arena);
      if (!frame.valid()) {
        std::stringstream msg;
        msg << "ReferenceFrame::load_tree: could not find frame \"" <<
            *begin << "\" at timestamp " << timestamp << std::endl;
        throw exceptions::ReferenceFrameException(msg.str());
      }
      ret.push_back(frame);
      ++begin;
    }
    throw_if_not_connected(ret);
    return ret;
  }
 
  static bool check_is_connected(const std::vector<ReferenceFrame> &frames);
  static void throw_if_not_connected(const std::vector<ReferenceFrame> &frames);
 
  template<typename Container>
  static std::vector<ReferenceFrame> load_tree(
      madara::knowledge::KnowledgeBase &kb, const Container &ids,
      uint64_t timestamp = ETERNAL,
      const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT,
      ReferenceFrameArena* arena = nullptr)
  {
    return load_tree(kb, ids.begin(), ids.end(),
        timestamp, std::move(settings), arena);
  }
 
  void save_as(madara::knowledge::KnowledgeBase &kb,
      std::string key, uint64_t expiry,
      const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT) const;
  void save_as(madara::knowledge::KnowledgeBase &kb,
      std::string key,
      const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT) const
  {
    save_as(kb, key, ident().expiry(), settings);
  }
 
  ReferenceFrame interpolate(const ReferenceFrame &other, ReferenceFrame parent, uint64_t time) const
  {
    //std::cerr << "Interpolate has_id: " << has_id() << std::endl;
    if (has_id()) {
      auto ret = ident().get_version(time);
      if (ret) {
        return ret;
      }
    }
    double fraction = (time - timestamp()) / (double)(other.timestamp() - timestamp());
 
    Pose interp = origin();
    const Pose &opose = other.origin();
 
    interp.frame(std::move(parent));
 
    interp.x((fraction * (opose.x() - interp.x())) + interp.x());
    interp.y((fraction * (opose.y() - interp.y())) + interp.y());
    interp.z((fraction * (opose.z() - interp.z())) + interp.z());
 
    Quaternion iq(origin().as_orientation_vec());
    Quaternion oq(opose.as_orientation_vec());
 
    iq.slerp_this(oq, fraction);
    iq.to_angular_vector(interp.as_orientation_vec());
 
    //std::cerr << "Interp " << fraction << " " << origin() << "  " << interp << "  " << opose << std::endl;
 
    auto ret = std::make_shared<ReferenceFrameVersion>(
        ident_, type(), std::move(interp), time);
 
    if (has_id()) {
      ident().register_version(time, ret);
    }
 
    ret->interpolated_ = true;
    return ret;
  }
 
  template<typename CoordType>
  friend class Coordinate;
 
private:
  bool check_consistent() const;
};
 
class GAMS_EXPORT FrameStore {
private:
  /*
   * KnowledgeBase uses const to indicate not changing the underlying
   * KnowledgeBaseImpl, as well as itself. We won't be changing the
   * KnowledgeBase object itself, and will only call thread-safe methods.
   */
  mutable madara::knowledge::KnowledgeBase kb_;
 
  FrameEvalSettings settings_ = FrameEvalSettings::DEFAULT;
  uint64_t expiry_;
 
public:
  static const uint64_t ETERNAL = ReferenceFrameIdentity::ETERNAL;
  static const uint64_t TEMP = ReferenceFrameIdentity::TEMP;
 
  FrameStore(madara::knowledge::KnowledgeBase kb,
             FrameEvalSettings settings, uint64_t expiry)
    : kb_(std::move(kb)), settings_(std::move(settings)), expiry_(expiry) {}
 
  FrameStore(madara::knowledge::KnowledgeBase kb, FrameEvalSettings settings)
    : FrameStore(std::move(kb), std::move(settings),
        ReferenceFrame::default_expiry()) {}
 
  FrameStore(madara::knowledge::KnowledgeBase kb, uint64_t expiry)
    : FrameStore(std::move(kb), FrameEvalSettings::DEFAULT, expiry) {}
 
 
  FrameStore(madara::knowledge::KnowledgeBase kb)
    : FrameStore(std::move(kb), FrameEvalSettings::DEFAULT,
        ReferenceFrame::default_expiry()) {}
 
  uint64_t expiry() const { return expiry_; }
 
  const FrameEvalSettings &settings() const { return settings_; }
 
  const madara::knowledge::KnowledgeBase &kb() const { return kb_; }
 
  void save(const ReferenceFrame &frame) const {
    return frame.save(kb_, expiry_, settings_);
  }
 
   ReferenceFrame load(const std::string &id, uint64_t timestamp = ETERNAL) {
     return ReferenceFrame::load(kb_, id, timestamp, settings_);
   }
 
  template<typename InputIterator>
  std::vector<ReferenceFrame> load_tree(
        InputIterator begin,
        InputIterator end,
        uint64_t timestamp = ETERNAL) const {
   return ReferenceFrame::load_tree(kb_, begin, end, timestamp, settings_);
  }
 
  template<typename Container>
  std::vector<ReferenceFrame> load_tree(
        const Container &ids,
        uint64_t timestamp = ETERNAL) const {
   return ReferenceFrame::load_tree(kb_, ids, timestamp, settings_);
  }
};
 
GAMS_EXPORT const ReferenceFrame *find_common_frame(
    const ReferenceFrame *from,
    const ReferenceFrame *to,
    std::vector<const ReferenceFrame *> *to_stack = nullptr);
 
class unrelated_frames : public std::runtime_error
{
public:
  unrelated_frames(ReferenceFrame from_frame, ReferenceFrame to_frame);
  ~unrelated_frames() throw();
 
  ReferenceFrame from_frame;
  ReferenceFrame to_frame;
};
 
class undefined_transform : public std::runtime_error
{
public:
  undefined_transform(
    const ReferenceFrameType *parent_frame,
    const ReferenceFrameType *child_frame,
    bool is_child_to_parent,
    bool unsupported_angular = false);
  ~undefined_transform() throw();
 
  const ReferenceFrameType *parent_frame;
  const ReferenceFrameType *child_frame;
  bool is_child_to_parent;
  bool unsupported_angular;
};
 
namespace simple_rotate {
  void orient_linear_vec(
      double &x, double &y, double &z,
      double rx, double ry, double rz,
      bool reverse = false);
 
  void transform_angular_to_origin(
                  const ReferenceFrameType *origin,
                  const ReferenceFrameType *self,
                  double orx, double ory, double orz,
                  double &rx, double &ry, double &rz);
 
  void transform_angular_from_origin(
                  const ReferenceFrameType *origin,
                  const ReferenceFrameType *self,
                  double orx, double ory, double orz,
                  double &rx, double &ry, double &rz);
 
  void transform_pose_to_origin(
                  const ReferenceFrameType *origin,
                  const ReferenceFrameType *self,
                  double ox, double oy, double oz,
                  double orx, double ory, double orz,
                  double &x, double &y, double &z,
                  double &rx, double &ry, double &rz,
                  bool fixed);
 
  void transform_pose_from_origin(
                  const ReferenceFrameType *origin,
                  const ReferenceFrameType *self,
                  double ox, double oy, double oz,
                  double orx, double ory, double orz,
                  double &x, double &y, double &z,
                  double &rx, double &ry, double &rz,
                  bool fixed);
 
  double calc_angle(
                  const ReferenceFrameType *self,
                  double rx1, double ry1, double rz1,
                  double rx2, double ry2, double rz2);
}
 
inline void default_normalize_linear(
            const ReferenceFrameType *,
            double &, double &, double &) {}
 
inline void default_normalize_angular(
            const ReferenceFrameType *,
            double &, double &, double &) {}
 
inline void default_normalize_pose(
                const ReferenceFrameType *self,
                double &x, double &y, double &z,
                double &rx, double &ry, double &rz)
{
  self->normalize_linear(self, x, y, z);
  self->normalize_angular(self, rx, ry, rz);
}
 
} }
 
#include "ReferenceFrame.inl"
 
#endif