From da645c32ffc6fbffc1f5f3c70f3ed0ae7e4cb491 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 17 Jun 2019 13:29:10 +0900 Subject: [PATCH] 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 | 18 ++++++++++-------- libraries/AC_Avoidance/AP_OADijkstra.h | 3 --- libraries/AC_Avoidance/AP_OAVisGraph.cpp | 20 ++++++-------------- libraries/AC_Avoidance/AP_OAVisGraph.h | 4 ---- 4 files changed, 16 insertions(+), 29 deletions(-) diff --git a/libraries/AC_Avoidance/AP_OADijkstra.cpp b/libraries/AC_Avoidance/AP_OADijkstra.cpp index 77c987264f..3c08e87484 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.cpp +++ b/libraries/AC_Avoidance/AP_OADijkstra.cpp @@ -16,14 +16,14 @@ #include "AP_OADijkstra.h" #include -#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; diff --git a/libraries/AC_Avoidance/AP_OADijkstra.h b/libraries/AC_Avoidance/AP_OADijkstra.h index 38227d46dc..1efc615140 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.h +++ b/libraries/AC_Avoidance/AP_OADijkstra.h @@ -6,9 +6,6 @@ #include #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 */ diff --git a/libraries/AC_Avoidance/AP_OAVisGraph.cpp b/libraries/AC_Avoidance/AP_OAVisGraph.cpp index a9dcce7df7..d4ca32d158 100644 --- a/libraries/AC_Avoidance/AP_OAVisGraph.cpp +++ b/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 diff --git a/libraries/AC_Avoidance/AP_OAVisGraph.h b/libraries/AC_Avoidance/AP_OAVisGraph.h index 4f54770d59..91309aaca5 100644 --- a/libraries/AC_Avoidance/AP_OAVisGraph.h +++ b/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; }