Program Listing for File pose.hpp

↰ Return to documentation for file (main/include/geometry/pose.hpp)

#ifndef _GEOMETRY_POSE_HPP_
#define _GEOMETRY_POSE_HPP_

#include <cmath>
#include <string>

#include "math/utils.hpp"

namespace squiggles {
class Pose {
  public:
  Pose(double ix, double iy, double iyaw) : x(ix), y(iy), yaw(iyaw) {}

  Pose() = default;

  double dist(const Pose& other) const {
    return std::sqrt((x - other.x) * (x - other.x) +
                     (y - other.y) * (y - other.y));
  }

  std::string to_string() const {
    return "Pose: {x: " + std::to_string(x) + ", y: " + std::to_string(y) +
           ", yaw: " + std::to_string(yaw) + "}";
  }

  std::string to_csv() const {
    return std::to_string(x) + "," + std::to_string(y) + "," +
           std::to_string(yaw);
  }

  bool operator==(const Pose& other) const {
    return nearly_equal(x, other.x) && nearly_equal(y, other.y) &&
           nearly_equal(yaw, other.yaw);
  }

  double x;
  double y;
  double yaw;
};
} // namespace squiggles

#endif