Program Listing for File trajectory.hpp

Return to documentation for file (cosm/nav/trajectory.hpp)

#pragma once

/*******************************************************************************
 * Includes
 ******************************************************************************/
#include <vector>
#include <string>

#include "rcppsw/math/vector2.hpp"
#include "rcppsw/patterns/fsm/event.hpp"

#include "cosm/cosm.hpp"
#include "cosm/nav/config/trajectory_config.hpp"

/*******************************************************************************
 * Namespaces/Decls
 ******************************************************************************/
namespace cosm::nav {

/*******************************************************************************
 * Class Definitions
 ******************************************************************************/
class trajectory : public rpfsm::event_data,
                   public rer::stringizable {
 public:
  explicit trajectory(const config::trajectory_config* config)
      : m_loop(config->loop),
        m_path(config->path) {}

  const path3D& path(void) const { return m_path; }
  size_t size(void) const { return m_path.size(); }

  rmath::vector3d next_point(void) const { return m_path[m_index]; }

  void mark_progress(size_t amount) {
    m_index += amount;
    if (m_index >= size() && m_loop) {
      m_index = 0;
    }
  }

  int index_of(const rmath::vector3d& point) const;


  bool is_complete(void) const {
    return  !m_loop && (-1 == index_of(next_point()));
  }

  std::string to_str(void) const override;

 private:
  /* clang-format off */
  bool   m_loop;
  path3D m_path;

  size_t m_index{0};
    /* clang-format on */
};

} /* namespace cosm::nav */