waypoint_trajectory.h

Go to the documentation of this file.

#ifndef WAYPOINT_TRAJECTORY_H
#define WAYPOINT_TRAJECTORY_H
#include "bitrl/bitrl_types.h"
 
#include <limits>
#include <type_traits>
#include <utility>
#include <vector>
 
namespace bitrl
{
namespace utils
{
namespace trajectory
{
 
template <typename LinkType> class WaypointTrajectory
{
  public:
    static_assert(std::is_default_constructible<LinkType>::value,
                  "LinkType must have default constructor");
 
    typedef LinkType link_type;
 
    typedef typename link_type::w_point_type w_point_type;
 
    typedef typename std::vector<link_type>::iterator link_iterator;
    typedef typename std::vector<link_type>::const_iterator const_link_iterator;
 
    WaypointTrajectory();
 
    explicit WaypointTrajectory(uint_t n);
 
    std::pair<real_t, link_type> distance(const w_point_type &p) const;
 
    uint_t size() const { return links_.size(); }
 
    void reserve(uint_t n) { links_.reserve(n); }
 
    void clear() { links_.clear(); }
 
    bool empty() const { return links_.empty(); }
 
    void push(const link_type &link) { links_.push_back(link); }
 
    link_type &operator[](uint_t i) { return links_[i]; }
 
    const link_type &operator[](uint_t i) const { return links_[i]; }
 
    void resize(uint_t n) { links_.resize(n); }
 
    link_iterator begin() { return links_.begin(); }
    link_iterator end() { return links_.end(); }
 
    const_link_iterator begin() const { return links_.begin(); }
    const_link_iterator end() const { return links_.end(); }
 
  private:
    std::vector<link_type> links_;
};
 
template <typename LinkType> WaypointTrajectory<LinkType>::WaypointTrajectory() : links_() {}
 
template <typename LinkType> WaypointTrajectory<LinkType>::WaypointTrajectory(uint_t n) : links_()
{
    links_.resize(n);
}
 
template <typename LinkType>
std::pair<real_t, typename WaypointTrajectory<LinkType>::link_type>
WaypointTrajectory<LinkType>::distance(
    const typename WaypointTrajectory<LinkType>::w_point_type &p) const
{
 
    real_t dist_ = std::numeric_limits<real_t>::max();
    link_type link_;
 
    for (const auto &link : links_)
    {
 
        auto dist = link.distance(p);
 
        if (dist < dist_)
        {
            dist_ = dist;
            link_ = link;
        }
    }
 
    return std::make_pair(dist_, links_);
}
 
} // namespace trajectory
} // namespace utils
} // namespace bitrl
 
#endif