feat: analytic bounds checking for environment

include/pathing/dubins.hpp

@@ -9,6 +9,8 @@
 #include "utilities/datatypes.hpp"
 
 struct DubinsPath {
+    // members left indeterminate; only needed so this can live in a std::array
+    DubinsPath() = default;
     DubinsPath(double beta_0, double beta_2, double straight_dist)
         : beta_0(beta_0), beta_2(beta_2), straight_dist(straight_dist) {}
 
@@ -21,6 +23,8 @@ struct DubinsPath {
 };
 
 struct RRTOption {
+    // members left indeterminate; only needed so this can live in a std::array
+    RRTOption() = default;
     RRTOption(double length, DubinsPath dubins_path, bool has_straight)
         : length(length), dubins_path(dubins_path), has_straight(has_straight) {}
 
@@ -29,6 +33,21 @@ struct RRTOption {
     bool has_straight;       // if this option has a straight path or not
 };
 
+/**
+ *  One leg of a flight: a dubins path, and the vector it lands on.
+ *
+ *  Every option is generated to reach a known vector, so that vector is carried
+ *  along with it instead of being recovered from the path parameters later on.
+ */
+struct PathSegment {
+    // members left indeterminate; only needed so this can live in a std::array
+    PathSegment() = default;
+    PathSegment(const RRTPoint& end, const RRTOption& option) : end(end), option(option) {}
+
+    RRTPoint end;       // the vector the leg ends on
+    RRTOption option;   // the dubins path flown to get there
+};
+
 /**
  *  Reproduction of np.sign() function as used in the older obc
  *  from
@@ -149,6 +168,20 @@ std::vector<XYZCoord> generatePointsCurve(const RRTPoint& start, const RRTPoint&
 std::vector<XYZCoord> generatePoints(const RRTPoint& start, const RRTPoint& end,
                                      const DubinsPath& path, bool has_straight);
 
+/**
+ *  Generates the points along a sequence of dubins paths
+ *
+ *  The segments are flown back to back, each one starting on the vector the
+ *  previous one ended on. The first point of every segment is dropped, as it is
+ *  either the last point of the previous segment or the point the plane is
+ *  already at.
+ *
+ *  @param start    ==> vector the path is flown from
+ *  @param segments ==> the dubins paths to fly, in order, with their end vectors
+ *  @return         ==> a list of points along the entire path
+ */
+std::vector<XYZCoord> generatePath(const RRTPoint& start, const std::vector<PathSegment>& segments);
+
 /**
  *  First, the straight distance (it turns out) is equal to the
  *  distance between the two centers

include/pathing/environment.hpp

@@ -74,6 +74,31 @@ namespace Environment {
      */
     bool isPathInBounds(const std::vector<XYZCoord>& path);
 
+    /**
+     * Check whether an entire Dubins path is in bounds, analytically.
+     *
+     * ONLY HANDLES CSC PATHS [LSL, RSR, LSR, RSL]
+     *
+     * @param[in] start     ==> the start vector of the path
+     * @param[in] end       ==> the end vector of the path
+     * @param[in] option    ==> the Dubins option connecting start to end
+     * @return  ==> true if every point of the path is in bounds, false otherwise
+     */
+    bool isDubinsPathInBounds(const RRTPoint& start, const RRTPoint& end, const RRTOption& option);
+
+    /**
+     * Check whether a circular arc is in bounds
+     *
+     * Checks for containment then edge intersections.
+     *
+     * @param[in] center        ==> the center of the arc's circle
+     * @param[in] radius        ==> the radius of the arc's circle
+     * @param[in] start_angle   ==> angle from the center to the arc's first point (0 is +x, CCW positive)
+     * @param[in] sweep         ==> signed angle swept, CCW positive (i.e. a DubinsPath beta)
+     * @return  ==> whether or not the arc is in bounds
+     */
+    bool isArcInBounds(const XYZCoord& center, double radius, double start_angle, double sweep);
+
     /**
      * Generate a random point in a valid region or mapping region
      *
@@ -99,6 +124,22 @@ namespace Environment {
      */
     bool isPointInPolygon(const Polygon& polygon, const XYZCoord& point);
 
+    /**
+     * Determines whether a polygon lies entirely inside another one
+     *
+     * Every corner of the inner polygon has to be inside the outer one, and no
+     * edge of it may cross the outer boundary -- corners alone are not enough,
+     * as an edge can bulge out between two corners that are both inside.
+     *
+     * Points on the edge of the outer polygon count as outside, the same way
+     * isPointInPolygon treats them.
+     *
+     * @param inner ==> the polygon that has to be contained
+     * @param outer ==> the polygon that has to contain it
+     * @return      ==> whether or not inner lies entirely inside outer
+     */
+    bool isPolygonInPolygon(const Polygon& inner, const Polygon& outer);
+
     /**
      * Checks wheter a line segment is in bounds or not, it must NOT intersect
      * either the valid region or the obstacles
@@ -121,6 +162,38 @@ namespace Environment {
     bool doesLineIntersectPolygon(const XYZCoord& start_point, const XYZCoord& end_point,
                                   const Polygon& polygon);
 
+    /**
+     * Determines whether a circular arc intersects any edge of the polygon
+     *
+     * @param[in] center        ==> the center of the arc's circle
+     * @param[in] radius        ==> the radius of the arc's circle
+     * @param[in] start_angle   ==> angle from the center to the arc's first
+     *                              point (0 is +x, CCW positive)
+     * @param[in] sweep         ==> signed angle swept, CCW positive
+     * @param[in] polygon       ==> polygon to check
+     * @return  ==> true if arc intersects edge
+     */
+    bool doesArcIntersectPolygon(const XYZCoord& center, double radius, double start_angle,
+                                 double sweep, const Polygon& polygon);
+
+    /**
+     * Determines whether a circular arc intersects a line segment
+     *
+     * Intersects the segment with the full circle then checks whether each hit
+     * lies within both the segment and the arc's angular range.
+     *
+     * @param[in] center        ==> the center of the arc's circle
+     * @param[in] radius        ==> the radius of the arc's circle
+     * @param[in] start_angle   ==> angle from the center to the arc's first
+     *                              point (0 is +x, CCW positive)
+     * @param[in] sweep         ==> signed angle swept, CCW positive
+     * @param[in] seg_start     ==> start point of the segment
+     * @param[in] seg_end       ==> end point of the segment
+     * @return if arc intersecs a line segement
+     */
+    bool doesArcIntersectSegment(const XYZCoord& center, double radius, double start_angle,
+                                 double sweep, const XYZCoord& seg_start, const XYZCoord& seg_end);
+
     /**
      * Given three colinear points p, q, r, the function checks if
      * point q lies on line segment 'pr'
@@ -229,15 +302,6 @@ namespace Environment {
                                                        const XYZCoord& start_point,
                                                        const XYZCoord& end_point);
 
-    /**
-     * Estimate the area and path length covered by the given goals
-     *
-     * @param goals the new goals
-     *
-     * @return a pair of the area covered and the path length
-     */
-    std::pair<double, double> estimateAreaCoveredAndPathLength(const std::vector<XYZCoord>& goals);
-
     /**
      * Returns a new polygon that is scaled by a given factor
      *