prism.h

#ifndef MRS_PRISM_H
#define MRS_PRISM_H
 
#include <boost/geometry.hpp>
#include <boost/geometry/algorithms/centroid.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/polygon.hpp>
 
namespace mrs_lib
{
 
  namespace safety_zone
  {
 
    typedef boost::geometry::model::point<double, 2, boost::geometry::cs::cartesian> Point2d;
    typedef boost::geometry::model::point<double, 3, boost::geometry::cs::cartesian> Point3d;
    typedef boost::geometry::model::polygon<Point2d> Polygon2D;
 
    class Subscriber
    {
    protected:
      bool is_active_ = true;
 
    public:
      // Called every time a change has been made to the prism
      virtual void update() = 0;
 
      // Called once upon deleting the prism
      virtual void cleanup() = 0;
    };
 
    class Prism
    {
    private:
      Polygon2D polygon_;
      double min_z_;
      double max_z_;
      std::string horizontal_frame_; // frame in which the polygon is defined
      std::string vertical_frame_;   // frame in which the z limits are defined
 
      std::set<Subscriber*> subscribers_;
 
      void notifySubscribers();
      void cleanSubscribers();
 
    public:
      // Empty constructor
      Prism() : polygon_(), min_z_(0.0), max_z_(0.0), horizontal_frame_("world_origin"), vertical_frame_("world_origin")
      {
      }
      // If max_z < min_z, automatically swaps them
      // Throws std::invalid argument if polygon is invalid
      Prism(const std::vector<Point2d>& points, const double max_z, const double min_z);
 
      Prism(const std::vector<Point2d>& points, const double max_z, const double min_z, const std::string& horizontal_frame, const std::string& vertical_frame);
 
      // Copy assignment operator
      Prism& operator=(const Prism& other)
      {
        if (this != &other)
        {
          polygon_ = other.polygon_;
          min_z_ = other.min_z_;
          max_z_ = other.max_z_;
          horizontal_frame_ = other.horizontal_frame_;
          vertical_frame_ = other.vertical_frame_;
          // Keep existing subscribers, don't copy them
        }
        return *this;
      }
 
      // Copy constructor
      Prism(const Prism& other)
          : polygon_(other.polygon_),
            min_z_(other.min_z_),
            max_z_(other.max_z_),
            horizontal_frame_(other.horizontal_frame_),
            vertical_frame_(other.vertical_frame_),
            subscribers_() // Don't copy observers
      {
      }
 
      // Move constructor
      Prism(Prism&& other) noexcept
          : polygon_(std::move(other.polygon_)),
            min_z_(other.min_z_),
            max_z_(other.max_z_),
            horizontal_frame_(other.horizontal_frame_),
            vertical_frame_(other.vertical_frame_),
            subscribers_(std::move(other.subscribers_))
      {
      }
 
      // Move assignment operator
      Prism& operator=(Prism&& other) noexcept
      {
        if (this != &other)
        {
          polygon_ = std::move(other.polygon_);
          min_z_ = other.min_z_;
          max_z_ = other.max_z_;
          horizontal_frame_ = other.horizontal_frame_;
          vertical_frame_ = other.vertical_frame_;
          subscribers_ = std::move(other.subscribers_);
        }
        return *this;
      }
 
      ~Prism();
 
      void subscribe(Subscriber* entity);
      void unsubscribe(Subscriber* entity);
 
      std::vector<Point2d> getPoints() const;
      double getMaxZ() const;
      double getMinZ() const;
      std::string getHorizontalFrame() const;
      std::string getVerticalFrame() const;
 
      Polygon2D getPolygon() const;
 
      // Returns number of vertices in the polygon of the prism.
      unsigned int getNumVertices() const;
 
      // Returns the centroid of the polygon of the prism.
      Point2d getCenter() const;
 
      void setMaxZ(const double value);
 
      void setMinZ(const double value);
 
      // Tries to change the coordinates of given vertex.
      // returns true if succeeded
      // returns false otherwise
      bool setVertex(const Point2d& vertex, const unsigned int index);
 
      // Tries to change the coordinates of given vertices.
      // Only notifies subsribers once in case of success
      // returns true if succeeded
      // returns false otherwise
      bool setVertices(const std::vector<Point2d>& vertices, const std::vector<unsigned int>& indices);
 
      // Adds new vertex in the middle of the neighboring verge
      void addVertexClockwise(const unsigned int index);
 
      // Adds new vertex in the middle of the neighboring verge
      void addVertexCounterclockwise(const unsigned int index);
 
      void move(const Point3d& adjustment);
 
      // Takes angle in radians, rotates clockwise (counter-clockwise if alpha < 0)
      void rotate(const double alpha);
 
      // Deletes the vertex only if vertex_count > 3
      void deleteVertex(const unsigned int index);
 
      // Overload with Point3d
      bool isPointIn(const Point3d& point) const;
 
      // Overload with 3D point (double,double,double)
      bool isPointIn(const double x, const double y, const double z) const;
 
      // Overload with Point2d
      bool isPointIn(const Point2d& point) const;
 
      // Overload with 2D point (double,double)
      bool isPointIn(const double x, const double y) const;
 
      // Helper method for text representation
      // void accept(Visitor &visitor);
    };
 
  } // namespace safety_zone
} // namespace mrs_lib
 
#endif