polygon.h

// clang: TomasFormat
#ifndef MRS_LIB_POLYGON_H
#define MRS_LIB_POLYGON_H
 
#include <rclcpp/rclcpp.hpp>
#include <eigen3/Eigen/Eigen>
#include <geometry_msgs/msg/point.hpp>
#include <mrs_lib/safety_zone/line_operations.h>
 
namespace mrs_lib
{
 
namespace safety_zone
{
 
// exceptions
struct WrongNumberOfVertices : public std::exception
{
  const char* what() const throw() {
    return "Polygon: wrong number of vertices was supplied!";
  }
};
 
struct WrongNumberOfColumns : public std::exception
{
  const char* what() const throw() {
    return "Polygon: wrong number of colums, it should be =2!";
  }
};
 
struct ExtraVertices : public std::exception
{
  const char* what() const throw() {
    return "Polygon: useless vertices detected, polygon methods may break!";
  }
};
 
class Polygon {
public:
  Polygon(const Eigen::MatrixXd vertices);
 
  bool isPointInside(const double px, const double py);
  bool doesSectionIntersect(const double startX, const double startY, const double endX, const double endY);
  bool isClockwise();
  void inflateSelf(double amount);
 
  std::vector<geometry_msgs::msg::Point> getPointMessageVector(const double z);
 
private:
  Eigen::MatrixXd vertices;
};
 
}  // namespace safety_zone
 
}  // namespace mrs_lib
 
#endif  // MRS_LIB_POLYGON_H