AC_Avoidance: Dijkstra's specifies expanding array chunk sizes

also add check that number of polygon points is less than 255

libraries/AC_Avoidance/AP_OADijkstra.cpp

@@ -16,14 +16,14 @@
 #include "AP_OADijkstra.h"
 #include <AC_Fence/AC_Fence.h>
 
-#define OA_DIJKSTRA_POLYGON_VISGRAPH_PTS         (OA_DIJKSTRA_POLYGON_FENCE_PTS * OA_DIJKSTRA_POLYGON_FENCE_PTS / 2)
-#define OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX 255     // index use to indicate we do not have a tentative short path for a node
+#define OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK  20      // expanding arrays for inner polygon fence and paths to destination will grow in increments of 20 elements
+#define OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX        255     // index use to indicate we do not have a tentative short path for a node
 
 /// Constructor
 AP_OADijkstra::AP_OADijkstra() :
-        _polyfence_visgraph(OA_DIJKSTRA_POLYGON_VISGRAPH_PTS),
-        _source_visgraph(OA_DIJKSTRA_POLYGON_FENCE_PTS),
-        _destination_visgraph(OA_DIJKSTRA_POLYGON_FENCE_PTS)
+        _polyfence_pts(OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK),
+        _short_path_data(OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK),
+        _path(OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK)
 {
 }
 
@@ -226,6 +226,11 @@ bool AP_OADijkstra::create_polygon_fence_visgraph()
         return false;
     }
 
+    // fail if more than number of polygon points algorithm can handle
+    if (num_points >= OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX) {
+        return false;
+    }
+
     // clear polygon visibility graph
     _polyfence_visgraph.clear();
 
@@ -413,9 +418,6 @@ bool AP_OADijkstra::calc_shortest_path(const Location &origin, const Location &d
     update_visgraph(_source_visgraph, {AP_OAVisGraph::OATYPE_SOURCE, 0}, origin_NE, true, destination_NE);
     update_visgraph(_destination_visgraph, {AP_OAVisGraph::OATYPE_DESTINATION, 0}, destination_NE);
 
-    // check OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX is defined correct
-    static_assert(OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX > OA_DIJKSTRA_POLYGON_FENCE_PTS, "check OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX > OA_DIJKSTRA_POLYGON_FENCE_PTS");
-
     // expand _short_path_data if necessary
     if (!_short_path_data.expand_to_hold(2 + _polyfence_numpoints)) {
         return false;

libraries/AC_Avoidance/AP_OADijkstra.h

@@ -6,9 +6,6 @@
 #include <AP_HAL/AP_HAL.h>
 #include "AP_OAVisGraph.h"
 
-#define OA_DIJKSTRA_POLYGON_FENCE_PTS       20      // algorithm can handle up to this many polygon fence points
-#define OA_DIJKSTRA_POLYGON_SHORTPATH_PTS   OA_DIJKSTRA_POLYGON_FENCE_PTS+1 // return path can have up to this many destinations
-
 /*
  * Dijkstra's algorithm for path planning around polygon fence
  */

libraries/AC_Avoidance/AP_OAVisGraph.cpp

@@ -15,26 +15,18 @@
 
 #include "AP_OAVisGraph.h"
 
-// constructor with size argument
-AP_OAVisGraph::AP_OAVisGraph(uint8_t size)
+// constructor initialises expanding array to use 20 elements per chunk
+AP_OAVisGraph::AP_OAVisGraph() :
+    _items(20)
 {
-    init(size);
-}
-
-// initialise array to given size
-bool AP_OAVisGraph::init(uint8_t size)
-{
-    return _items.expand_to_hold(size);
 }
 
 // add item to visiblity graph, returns true on success, false if graph is full
 bool AP_OAVisGraph::add_item(const OAItemID &id1, const OAItemID &id2, float distance_cm)
 {
-    // check there is space
-    if (_num_items >= _items.max_items()) {
-        if (!_items.expand(1)) {
-            return false;
-        }
+    // ensure there is space in the array
+    if (!_items.expand_to_hold(_num_items+1)) {
+        return false;
     }
 
     // add item

libraries/AC_Avoidance/AP_OAVisGraph.h

@@ -10,7 +10,6 @@
 class AP_OAVisGraph {
 public:
     AP_OAVisGraph();
-    AP_OAVisGraph(uint8_t size);
 
     /* Do not allow copies */
     AP_OAVisGraph(const AP_OAVisGraph &other) = delete;
@@ -40,9 +39,6 @@ public:
         float distance_cm;  // distance between the items
     };
 
-    // initialise array to given size
-    bool init(uint8_t size);
-
     // clear all elements from graph
     void clear() { _num_items = 0; }