refactor: instead of computing the distance between the nodes at each iteration,

precute them in advance. Separate vertices from start and destination
calculations.

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/lib/geofence/GeofenceUtilsTest.cpp

@@ -89,3 +89,18 @@ TEST(GeofenceUtilsTest, PolygonIsCCW)
 
 	EXPECT_TRUE(geofence_utils::isPolygonCCW(vertices, 4));
 }
+TEST(GeofenceUtilsTest, SymmetricPairIndex)
+{
+	EXPECT_EQ(geofence_utils::symmetricPairIndex(0, 1, 4), 0);
+	EXPECT_EQ(geofence_utils::symmetricPairIndex(0, 2, 4), 1);
+	EXPECT_EQ(geofence_utils::symmetricPairIndex(3, 2, 4), 5);
+	EXPECT_EQ(geofence_utils::symmetricPairIndex(1, 3, 4), 4);
+	EXPECT_EQ(geofence_utils::symmetricPairIndex(3, 1, 4), 4);
+
+
+	EXPECT_EQ(geofence_utils::symmetricPairIndex(0, 1, 5), 0);
+	EXPECT_EQ(geofence_utils::symmetricPairIndex(2, 4, 5), 8);
+	EXPECT_EQ(geofence_utils::symmetricPairIndex(3, 2, 5), 7);
+	EXPECT_EQ(geofence_utils::symmetricPairIndex(1, 3, 5), 5);
+	EXPECT_EQ(geofence_utils::symmetricPairIndex(3, 1, 5), 5);
+}

src/lib/geofence/geofence_utils.cpp

@@ -188,6 +188,18 @@ bool expandOrShrinkPolygon(const matrix::Vector2f *vertices_in, int num_vertices
 	return true;
 }
 
+size_t symmetricPairIndex(size_t i, size_t j, size_t num_nodes)
+{
+	// Pack the strict upper triangle (i < j) of an N x N symmetric matrix into a flat array.
+	// Row i contains (N - 1 - i) entries and starts at offset i*(2N - i - 1)/2.
+	// Within row i, column j sits at local offset (j - i - 1).
+	if (i > j) {
+		const size_t tmp = i;
+		i = j;
+		j = tmp;
+	}
 
+	return i * (2 * num_nodes - i - 1) / 2 + (j - i - 1);
+}
 
 } // namespace geofence_utils

src/lib/geofence/geofence_utils.h

@@ -139,4 +139,18 @@ bool expandOrShrinkPolygon(const matrix::Vector2f *vertices_in, int num_vertices
 			   float margin, bool expand,
 			   matrix::Vector2f *vertices_out);
 
+/**
+ * Map an unordered pair (i, j) of node indices to a flat array index for storing
+ * symmetric pairwise values (e.g. distances where d(i,j) == d(j,i)). Diagonal
+ * entries (i == j) are not stored; caller must ensure i != j.
+ *
+ * Required array size is num_nodes * (num_nodes - 1) / 2.
+ *
+ * @param i          first node index, in [0, num_nodes)
+ * @param j          second node index, in [0, num_nodes), must differ from i
+ * @param num_nodes  total number of nodes
+ * @return flat array index in [0, num_nodes * (num_nodes - 1) / 2)
+ */
+size_t symmetricPairIndex(size_t i, size_t j, size_t num_nodes);
+
 } // namespace geofence_utils

src/modules/navigator/GeofenceBreachAvoidance/GeofenceAvoidancePlannerTest.cpp

@@ -95,7 +95,11 @@ TEST_F(GeofenceAvoidancePlannerTest, StartEqualsDestination)
 	Vector2<double> point(47.3977, 8.5456);
 
 	FakeGeofence fake(nullptr, 0, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
-	PlannedPath path = _planner.planPath(point, point, &fake);
+	_planner.update_vertices(&fake);
+	_planner.update_start(point, &fake);
+	_planner.update_destination(point, &fake);
+
+	PlannedPath path = _planner.planPath();
 
 	ASSERT_EQ(path.num_points, 0);
 }
@@ -107,7 +111,12 @@ TEST_F(GeofenceAvoidancePlannerTest, DirectPathNoFence)
 	Vector2<double> destination(47.3984, 8.5470);
 
 	FakeGeofence fake(nullptr, 0, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
-	PlannedPath path = _planner.planPath(start, destination, &fake);
+	_planner.update_vertices(&fake);
+	_planner.update_start(start, &fake);
+	_planner.update_destination(destination, &fake);
+
+	PlannedPath path = _planner.planPath();
+
 	ASSERT_EQ(path.num_points, 0);
 }
 
@@ -127,7 +136,12 @@ TEST_F(GeofenceAvoidancePlannerTest, PathAroundExclusionZone)
 	};
 
 	FakeGeofence fake(vertices, 4, NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION);
-	PlannedPath path = _planner.planPath(start, destination, &fake);
+	_planner.update_vertices(&fake);
+	_planner.update_start(start, &fake);
+	_planner.update_destination(destination, &fake);
+
+	PlannedPath path = _planner.planPath();
+
 	ASSERT_EQ(path.num_points, 2);
 	// The optimal path is via vertex 1 and 2
 	EXPECT_NEAR(path.points[0](0), vertices[1](0), 1e-3);
@@ -153,7 +167,12 @@ TEST_F(GeofenceAvoidancePlannerTest, PathInsideInclusionZone)
 	};
 
 	FakeGeofence fake(vertices, 6, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
-	PlannedPath path = _planner.planPath(start, destination, &fake);
+	_planner.update_vertices(&fake);
+	_planner.update_start(start, &fake);
+	_planner.update_destination(destination, &fake);
+
+	PlannedPath path = _planner.planPath();
+
 	ASSERT_EQ(path.num_points, 1);
 	ASSERT_NEAR(path.points[0](0), vertices[5](0), 1e-3);
 	ASSERT_NEAR(path.points[0](1), vertices[5](1), 1e-3);
@@ -175,7 +194,12 @@ TEST_F(GeofenceAvoidancePlannerTest, DestinationOutsideInclusion)
 	};
 
 	FakeGeofence fake(vertices, 6, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
-	PlannedPath path = _planner.planPath(start, destination, &fake);
+	_planner.update_vertices(&fake);
+	_planner.update_start(start, &fake);
+	_planner.update_destination(destination, &fake);
+
+	PlannedPath path = _planner.planPath();
+
 	ASSERT_EQ(path.num_points, 0);
 }
 TEST_F(GeofenceAvoidancePlannerTest, DestinationInsideExclusion)
@@ -194,7 +218,12 @@ TEST_F(GeofenceAvoidancePlannerTest, DestinationInsideExclusion)
 	};
 
 	FakeGeofence fake(vertices, 4, NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION);
-	PlannedPath path = _planner.planPath(start, destination, &fake);
+	_planner.update_vertices(&fake);
+	_planner.update_start(start, &fake);
+	_planner.update_destination(destination, &fake);
+
+	PlannedPath path = _planner.planPath();
+
 	ASSERT_EQ(path.num_points, 0);
 }
 TEST_F(GeofenceAvoidancePlannerTest, StartInsideExclusion)
@@ -214,7 +243,12 @@ TEST_F(GeofenceAvoidancePlannerTest, StartInsideExclusion)
 	};
 
 	FakeGeofence fake(vertices, 4, NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION);
-	PlannedPath path = _planner.planPath(start, destination, &fake);
+	_planner.update_vertices(&fake);
+	_planner.update_start(start, &fake);
+	_planner.update_destination(destination, &fake);
+
+	PlannedPath path = _planner.planPath();
+
 	ASSERT_EQ(path.num_points, 0);
 }
 TEST_F(GeofenceAvoidancePlannerTest, NanStartOrDestination)
@@ -225,11 +259,18 @@ TEST_F(GeofenceAvoidancePlannerTest, NanStartOrDestination)
 	Vector2<double> nan_lon(47.3977, NAN);
 
 	FakeGeofence fake(nullptr, 0, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
+	_planner.update_vertices(&fake);
 
-	EXPECT_EQ(_planner.planPath(nan_lat, valid, &fake).num_points, 0);
+	auto plan = [&](const Vector2<double> &s, const Vector2<double> &d) {
-	EXPECT_EQ(_planner.planPath(nan_lon, valid, &fake).num_points, 0);
+		_planner.update_start(s, &fake);
-	EXPECT_EQ(_planner.planPath(valid, nan_lat, &fake).num_points, 0);
+		_planner.update_destination(d, &fake);
-	EXPECT_EQ(_planner.planPath(valid, nan_lon, &fake).num_points, 0);
+		return _planner.planPath();
+	};
+
+	EXPECT_EQ(plan(nan_lat, valid).num_points, 0);
+	EXPECT_EQ(plan(nan_lon, valid).num_points, 0);
+	EXPECT_EQ(plan(valid, nan_lat).num_points, 0);
+	EXPECT_EQ(plan(valid, nan_lon).num_points, 0);
 }
 
 TEST_F(GeofenceAvoidancePlannerTest, LatLonOutOfBounds)
@@ -242,15 +283,22 @@ TEST_F(GeofenceAvoidancePlannerTest, LatLonOutOfBounds)
 	Vector2<double> lon_too_low(47.3977, -181.0);
 
 	FakeGeofence fake(nullptr, 0, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
+	_planner.update_vertices(&fake);
 
-	EXPECT_EQ(_planner.planPath(lat_too_high, valid, &fake).num_points, 0);
+	auto plan = [&](const Vector2<double> &s, const Vector2<double> &d) {
-	EXPECT_EQ(_planner.planPath(lat_too_low, valid, &fake).num_points, 0);
+		_planner.update_start(s, &fake);
-	EXPECT_EQ(_planner.planPath(lon_too_high, valid, &fake).num_points, 0);
+		_planner.update_destination(d, &fake);
-	EXPECT_EQ(_planner.planPath(lon_too_low, valid, &fake).num_points, 0);
+		return _planner.planPath();
-	EXPECT_EQ(_planner.planPath(valid, lat_too_high, &fake).num_points, 0);
+	};
-	EXPECT_EQ(_planner.planPath(valid, lat_too_low, &fake).num_points, 0);
+
-	EXPECT_EQ(_planner.planPath(valid, lon_too_high, &fake).num_points, 0);
+	EXPECT_EQ(plan(lat_too_high, valid).num_points, 0);
-	EXPECT_EQ(_planner.planPath(valid, lon_too_low, &fake).num_points, 0);
+	EXPECT_EQ(plan(lat_too_low, valid).num_points, 0);
+	EXPECT_EQ(plan(lon_too_high, valid).num_points, 0);
+	EXPECT_EQ(plan(lon_too_low, valid).num_points, 0);
+	EXPECT_EQ(plan(valid, lat_too_high).num_points, 0);
+	EXPECT_EQ(plan(valid, lat_too_low).num_points, 0);
+	EXPECT_EQ(plan(valid, lon_too_high).num_points, 0);
+	EXPECT_EQ(plan(valid, lon_too_low).num_points, 0);
 }
 
 TEST_F(GeofenceAvoidancePlannerTest, DuplicateNeighborVertex)
@@ -273,7 +321,11 @@ TEST_F(GeofenceAvoidancePlannerTest, DuplicateNeighborVertex)
 	};
 
 	FakeGeofence fake(vertices, 7, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
-	PlannedPath path = _planner.planPath(start, destination, &fake);
+	_planner.update_vertices(&fake);
+	_planner.update_start(start, &fake);
+	_planner.update_destination(destination, &fake);
+
+	PlannedPath path = _planner.planPath();
 
 	// THEN the pathplanner should fail
 	ASSERT_EQ(path.num_points, 0);

src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.cpp

@@ -46,53 +46,17 @@ static matrix::Vector2f get_vertex_local_position(int poly_index, int vertex_idx
 	return local;
 }
 
-PlannedPath GeofenceAvoidancePlanner::planPath(const matrix::Vector2<double> &start,
+PlannedPath GeofenceAvoidancePlanner::planPath()
-		const matrix::Vector2<double> &destination,
-		GeofenceInterface *geofence,
-		float margin)
 {
 	PlannedPath path;
 
-	margin = math::max(margin, 1.f); // margin should be non-zero as otherwhise the algorithm breaks down
+	if (!_polygons_healthy || !_start_healthy || !_destination_healthy) {
-
+		// we are not in a state where we can reliably plan a path, return an empty one
-	if (!start.isAllFinite() || !destination.isAllFinite()) {
 		path.num_points = 0;
 		return path;
 	}
 
-	if (!lat_lon_within_bounds(start) || !lat_lon_within_bounds(destination)) {
+	reset_graph_state();
-		path.num_points = 0;
-		return path;
-	}
-
-	if ((start - destination).norm() < 1e-10) {
-		path.num_points = 0;
-		return path;
-	}
-
-	const int num_polygons = geofence->getNumPolygons();
-	int num_vertices_total = 0;
-
-	for (int poly_index = 0; poly_index < num_polygons; poly_index++) {
-		const PolygonInfo info = geofence->getPolygonInfoByIndex(poly_index);
-
-		if (info.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION || info.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
-			num_vertices_total += kCircleApproxVertices;
-
-		} else {
-			num_vertices_total += info.vertex_count;
-		}
-	}
-
-	if (num_vertices_total + 2 > kMaxNodes) {
-		path.num_points = 0;
-		return path;
-	}
-
-	if (!calculate_graph_nodes(start, destination, geofence, margin)) {
-		path.num_points = 0;
-		return path;
-	}
 
 	// reset to the start location
 	int graph_index = 0;
@@ -104,24 +68,19 @@ PlannedPath GeofenceAvoidancePlanner::planPath(const matrix::Vector2<double> &st
 		VisibleVertices visible_vertices = {};
 		int visible_count = 0;
 
-		for (int i = 0; i < num_vertices_total + 2; i++) { // +2 accounts for start and destination node
+		for (int i = 0; i < _num_nodes; i++) {
 
 			if (i == graph_index) {
 				continue;
 			}
 
-			// check if the line from our current node to any other node is clear
+			const float distance = _distances[geofence_utils::symmetricPairIndex(graph_index, i, _num_nodes)];
-			const bool clear = !geofence->checkIfLineViolatesAnyFence(_graph_nodes[graph_index].position,
-					   _graph_nodes[i].position, _reference);
 
-			if (clear) {
+			if (distance < INFINITY) {
 				visible_vertices.items[visible_count].index = i;
-				visible_vertices.items[visible_count].distance =
+				visible_vertices.items[visible_count].distance = distance;
-					(_graph_nodes[graph_index].position - _graph_nodes[i].position).norm();
 				visible_count++;
 			}
-
-
 		}
 
 		visible_vertices.count = visible_count;
@@ -138,9 +97,10 @@ PlannedPath GeofenceAvoidancePlanner::planPath(const matrix::Vector2<double> &st
 		}
 
 		// find the unvisited node with the smallest best distance
+		last_graph_index = graph_index;
 		float min_dist = INFINITY;
 
-		for (int i = 0; i < num_vertices_total + 2; i++) {	// +2 accounts for start and destination
+		for (int i = 0; i < _num_nodes; i++) {
 			if (!_graph_nodes[i].visited && _graph_nodes[i].best_distance < min_dist) {
 				min_dist = _graph_nodes[i].best_distance;
 				graph_index = i;
@@ -197,42 +157,63 @@ PlannedPath GeofenceAvoidancePlanner::planPath(const matrix::Vector2<double> &st
 	return path;
 }
 
-bool GeofenceAvoidancePlanner::calculate_graph_nodes(const matrix::Vector2<double> &start,
+bool GeofenceAvoidancePlanner::update_vertices(GeofenceInterface *geofence, float margin)
-		const matrix::Vector2<double> &destination,
-		GeofenceInterface *geofence,
-		float margin)
 {
-	_reference = start;
+	_polygons_healthy = true;
-	MapProjection ref{start(0), start(1)};
+	margin = math::max(margin, 1.f); // margin should be non-zero otherwise polygon expansion breaks down
 
 	const int num_polygons = geofence->getNumPolygons();
+	int num_vertices{0};
 
-	int node_index = 0;
+	for (int poly_idx = 0; poly_idx < num_polygons; poly_idx++) {
-	// Node 0: START at local origin
+		PolygonInfo info = geofence->getPolygonInfoByIndex(poly_idx);
-	_graph_nodes[node_index].type = Node::Type::START;
-	_graph_nodes[node_index].position = {0.f, 0.f};
-	_graph_nodes[node_index].best_distance = 0.0f;
-	_graph_nodes[node_index].previous_index = 0;
-	_graph_nodes[node_index].visited = true;
 
-	node_index++;
+		if (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION || info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) {
+			num_vertices += info.vertex_count;
+
+		} else if (info.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION || info.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
+			num_vertices += kCircleApproxVertices;
+		}
+	}
+
+	if (num_vertices == 0 || num_vertices > kMaxNodes - 2) { // -2 to reserve start and destination slots
+		_polygons_healthy = false;
+		return false;
+	}
+
+	_reference = geofence->getPolygonVertexByIndex(0, 0);
+
+	if (!update_graph_nodes_without_start_and_destination(geofence, margin)) {
+		_polygons_healthy = false;
+		return false;
+	}
+
+	update_distances_between_vertices(geofence);
+	return true;
+}
+
+bool GeofenceAvoidancePlanner::update_graph_nodes_without_start_and_destination(
+	GeofenceInterface *geofence, float margin)
+{
+	// local frame is anchored at _reference (set by update_vertices); this can be computed
+	// once up-front; start and destination nodes are filled in later when they are known
+	const int num_polygons = geofence->getNumPolygons();
+
+	int node_index = 1; // reserve index 0 for the start
 
 	for (int poly_index = 0; poly_index < num_polygons; poly_index++) {
-		PolygonInfo info = geofence->getPolygonInfoByIndex(poly_index);
 
+		PolygonInfo info = geofence->getPolygonInfoByIndex(poly_index);
 
 		if (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION || info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) {
 
-			// Project vertices to local frame
 			matrix::Vector2f local_in[info.vertex_count];
 			matrix::Vector2f local_out[info.vertex_count];
 
 			for (int vertex_idx = 0; vertex_idx < info.vertex_count; vertex_idx++) {
-				local_in[vertex_idx] = get_vertex_local_position(poly_index, vertex_idx, geofence, start);
+				local_in[vertex_idx] = get_vertex_local_position(poly_index, vertex_idx, geofence, _reference);
 			}
 
-			// In order to avoid moving too close to the edges of the polygons, we either expand (inclusion polygon)
-			// or shrink (exclusion) the polygon by a margin and use the resulting vertices as graph nodes.
 			const bool should_expand = (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION);
 
 			if (!geofence_utils::expandOrShrinkPolygon(local_in, info.vertex_count, margin, should_expand,
@@ -250,7 +231,7 @@ bool GeofenceAvoidancePlanner::calculate_graph_nodes(const matrix::Vector2<doubl
 			}
 
 		} else if (info.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION || info.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
-			const matrix::Vector2f center = get_vertex_local_position(poly_index, 0, geofence, start);
+			const matrix::Vector2f center = get_vertex_local_position(poly_index, 0, geofence, _reference);
 			const float delta_angle = 2.f * M_PI_F / kCircleApproxVertices;
 
 			float adjusted_radius;
@@ -278,17 +259,129 @@ bool GeofenceAvoidancePlanner::calculate_graph_nodes(const matrix::Vector2<doubl
 		}
 	}
 
-	// Last node: DESTINATION in local frame
+	// node_index now points at the reserved destination slot; total count includes it
+	_num_vertices = node_index - 1;
+	_num_nodes = node_index + 1;	// +1 for the destination
+
+	return true;
+}
+
+void GeofenceAvoidancePlanner::update_distances_between_vertices(GeofenceInterface *geofence)
+{
+	// vertices occupy indices 1 .. _num_vertices; index 0 and the last slot hold the
+	// start and destination, which are handled by update_start / update_destination
+	const int last_vertex = _num_vertices;
+
+	// loop through all possible vertex to vertex combinations and store the distance between them
+	for (int i = 1; i <= last_vertex; i++) {
+		for (int j = i + 1; j <= last_vertex; j++) {
+			const size_t idx = geofence_utils::symmetricPairIndex(i, j, _num_nodes);
+
+			const bool clear = !geofence->checkIfLineViolatesAnyFence(_graph_nodes[i].position,
+					   _graph_nodes[j].position, _reference);
+
+			if (clear) {
+				_distances[idx] = (_graph_nodes[i].position - _graph_nodes[j].position).norm();
+
+			} else {
+				_distances[idx] = INFINITY;
+			}
+		}
+	}
+}
+
+void GeofenceAvoidancePlanner::update_start(const matrix::Vector2d &start, GeofenceInterface *geofence)
+{
+	if (!start.isAllFinite() || !lat_lon_within_bounds(start) || !_polygons_healthy) {
+		_start_healthy = false;
+		return;
+	}
+
+	MapProjection ref{_reference(0), _reference(1)};
+	matrix::Vector2f start_local;
+	ref.project(start(0), start(1), start_local(0), start_local(1));
+
+	_graph_nodes[0].position = start_local;
+
+	// distances from start to each vertex
+	for (int graph_idx = 1; graph_idx <= _num_vertices; graph_idx++) {
+		const size_t dist_idx = geofence_utils::symmetricPairIndex(0, graph_idx, _num_nodes);
+
+		const bool clear = !geofence->checkIfLineViolatesAnyFence(_graph_nodes[0].position,
+				   _graph_nodes[graph_idx].position, _reference);
+
+		if (clear) {
+			_distances[dist_idx] = (_graph_nodes[0].position - _graph_nodes[graph_idx].position).norm();
+
+		} else {
+			_distances[dist_idx] = INFINITY;
+		}
+	}
+
+	// refresh start <-> destination edge if destination is already known
+	if (_destination_healthy) {
+		const int dest_idx = _num_nodes - 1;
+		const size_t dist_idx = geofence_utils::symmetricPairIndex(0, dest_idx, _num_nodes);
+
+		const bool clear = !geofence->checkIfLineViolatesAnyFence(_graph_nodes[0].position,
+				   _graph_nodes[dest_idx].position, _reference);
+
+		if (clear) {
+			_distances[dist_idx] = (_graph_nodes[0].position - _graph_nodes[dest_idx].position).norm();
+
+		} else {
+			_distances[dist_idx] = INFINITY;
+		}
+	}
+
+	_start_healthy = true;
+}
+
+void GeofenceAvoidancePlanner::update_destination(const matrix::Vector2d &destination, GeofenceInterface *geofence)
+{
+	if (!destination.isAllFinite() || !lat_lon_within_bounds(destination) || !_polygons_healthy) {
+		_destination_healthy = false;
+		return;
+	}
+
+	MapProjection ref{_reference(0), _reference(1)};
 	matrix::Vector2f dest_local;
 	ref.project(destination(0), destination(1), dest_local(0), dest_local(1));
 
-	_graph_nodes[node_index].type = Node::Type::DESTINATION;
+	const int dest_idx = _num_nodes - 1;
-	_graph_nodes[node_index].position = dest_local;
+	_graph_nodes[dest_idx].position = dest_local;
-	_graph_nodes[node_index].best_distance = FLT_MAX;
-	_graph_nodes[node_index].previous_index = -1;
-	_graph_nodes[node_index].visited = false;
 
-	return true;
+	// distances from each vertex to destination
+	for (int graph_idx = 1; graph_idx <= _num_vertices; graph_idx++) {
+		const size_t dist_idx = geofence_utils::symmetricPairIndex(graph_idx, dest_idx, _num_nodes);
+
+		const bool clear = !geofence->checkIfLineViolatesAnyFence(_graph_nodes[graph_idx].position,
+				   _graph_nodes[dest_idx].position, _reference);
+
+		if (clear) {
+			_distances[dist_idx] = (_graph_nodes[graph_idx].position - _graph_nodes[dest_idx].position).norm();
+
+		} else {
+			_distances[dist_idx] = INFINITY;
+		}
+	}
+
+	// refresh start <-> destination edge if start is already known
+	if (_start_healthy) {
+		const size_t dist_idx = geofence_utils::symmetricPairIndex(0, dest_idx, _num_nodes);
+
+		const bool clear = !geofence->checkIfLineViolatesAnyFence(_graph_nodes[0].position,
+				   _graph_nodes[dest_idx].position, _reference);
+
+		if (clear) {
+			_distances[dist_idx] = (_graph_nodes[0].position - _graph_nodes[dest_idx].position).norm();
+
+		} else {
+			_distances[dist_idx] = INFINITY;
+		}
+	}
+
+	_destination_healthy = true;
 }
 
 bool GeofenceAvoidancePlanner::lat_lon_within_bounds(const matrix::Vector2<double> &lat_lon)
@@ -299,3 +392,23 @@ bool GeofenceAvoidancePlanner::lat_lon_within_bounds(const matrix::Vector2<doubl
 
 	return true;
 }
+
+void GeofenceAvoidancePlanner::reset_graph_state()
+{
+
+	_graph_nodes[0].best_distance = 0.0f;
+	_graph_nodes[0].previous_index = 0;
+	_graph_nodes[0].visited = true;
+
+	// reset vertex Dijkstra state
+	for (int i = 1; i <= _num_vertices; i++) {
+		_graph_nodes[i].best_distance = FLT_MAX;
+		_graph_nodes[i].previous_index = -1;
+		_graph_nodes[i].visited = false;
+	}
+
+	_graph_nodes[_num_nodes - 1].best_distance = FLT_MAX;
+	_graph_nodes[_num_nodes - 1].previous_index = -1;
+	_graph_nodes[_num_nodes - 1].visited = false;
+	_graph_nodes[_num_nodes - 1].type = Node::Type::DESTINATION;
+}

src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h

@@ -119,21 +119,30 @@ public:
 	GeofenceAvoidancePlanner() = default;
 	~GeofenceAvoidancePlanner() = default;
 
-	PlannedPath planPath(const matrix::Vector2<double> &start,
+	PlannedPath planPath();
-			     const matrix::Vector2<double> &destination,
-			     GeofenceInterface *geofence,
-			     float margin = 10.0f);
 
+	bool update_vertices(GeofenceInterface *geofence, float margin = 10.0f);
+	void update_start(const matrix::Vector2d &start, GeofenceInterface *geofence);
+	void update_destination(const matrix::Vector2d &destination, GeofenceInterface *geofence);
 
 private:
 
-	Node _graph_nodes[kMaxNodes];
+	static constexpr int num_distances_in_graph = (kMaxNodes) * (kMaxNodes - 1) / 2;
-	matrix::Vector2<double> _reference; // lat/lon of start, used as local frame origin
 
-	bool calculate_graph_nodes(const matrix::Vector2<double> &start,
+	Node _graph_nodes[kMaxNodes];
-				   const matrix::Vector2<double> &destination,
+	float _distances[num_distances_in_graph];
-				   GeofenceInterface *geofence,
+	int _num_nodes{0};
-				   float margin);
+	int _num_vertices{0};
+	matrix::Vector2<double> _reference; // lat/lon anchor of the local frame
+
+	bool _polygons_healthy{false};
+	bool _start_healthy{false};
+	bool _destination_healthy{false};
+
+	bool update_graph_nodes_without_start_and_destination(GeofenceInterface *geofence, float margin);
+	void update_distances_between_vertices(GeofenceInterface *geofence);
+
+	void reset_graph_state();
 
 	bool lat_lon_within_bounds(const matrix::Vector2<double> &lat_lon);
 

src/modules/navigator/geofence.cpp

@@ -157,6 +157,7 @@ void Geofence::run()
 			} else {
 				_dataman_state = DatamanState::UpdateRequestWait;
 				_fence_updated = true;
+				_avoidance_planner.update_vertices(this);
 
 				geofence_status_s status{};
 				status.timestamp = hrt_absolute_time();
@@ -184,6 +185,8 @@ void Geofence::run()
 			status.status = geofence_status_s::GF_STATUS_READY;
 
 			_geofence_status_pub.publish(status);
+
+			_avoidance_planner.update_vertices(this);
 		}
 
 		break;
@@ -725,13 +728,6 @@ void Geofence::printStatus()
 
 
 
-PlannedPath
-Geofence::planPathToDestination(const matrix::Vector2<double> &start, const matrix::Vector2<double> &destination,
-				float margin)
-{
-	return _avoidance_planner.planPath(start, destination, this, margin);
-}
-
 bool Geofence::checkIfLineViolatesAnyFence(const matrix::Vector2f &start_local, const matrix::Vector2f &end_local,
 		const matrix::Vector2<double> &reference)
 {

src/modules/navigator/geofence.h

@@ -144,9 +144,17 @@ public:
 
 	bool isHomeRequired();
 
-	PlannedPath planPathToDestination(const matrix::Vector2<double> &start,
+	void updateStartForRTLPathPlanner(const matrix::Vector2<double> &start)
-					  const matrix::Vector2<double> &destination,
+	{
-					  float margin);
+		_avoidance_planner.update_start(start, this);
+	}
+
+	void updateDestinationForRTLPathPlanner(const matrix::Vector2<double> &destination)
+	{
+		_avoidance_planner.update_destination(destination, this);
+	}
+
+	PlannedPath planPath() { return _avoidance_planner.planPath(); };
 
 	/**
 	 * print Geofence status to the console

src/modules/navigator/navigator.h

@@ -312,10 +312,19 @@ public:
 
 	void trigger_hagl_failsafe(uint8_t nav_state);
 
-	PlannedPath planPathToDestination(const matrix::Vector2<double> &start, const matrix::Vector2<double> &destination,
+	void updateStartOfRTLPathPlanner(const matrix::Vector2<double> &start)
-					  float margin)
 	{
-		return _geofence.planPathToDestination(start, destination, margin);
+		_geofence.updateStartForRTLPathPlanner(start);
+	}
+
+	void updateDestinationOfRTLPathPlanner(const matrix::Vector2<double> &destination)
+	{
+		_geofence.updateDestinationForRTLPathPlanner(destination);
+	}
+
+	PlannedPath planPath()
+	{
+		return _geofence.planPath();
 	}
 
 private:

src/modules/navigator/rtl.cpp

@@ -182,7 +182,6 @@ void RTL::on_inactive()
 	if ((now - _destination_check_time) > 2_s) {
 		_destination_check_time = now;
 		setRtlTypeAndDestination();
-		updateRtlPath();
 		publishRemainingTimeEstimate();
 	}
 
@@ -219,25 +218,6 @@ void RTL::publishRemainingTimeEstimate()
 	_rtl_time_estimate_pub.publish(estimated_time);
 }
 
-void RTL::updateRtlPath()
-{
-
-	const bool global_position_recently_updated = _global_pos_sub.get().timestamp > 0
-			&& hrt_elapsed_time(&_global_pos_sub.get().timestamp) < 10_s;
-
-	if (_navigator->home_global_position_valid() && global_position_recently_updated) {
-		switch (_rtl_type) {
-		case RtlType::RTL_DIRECT:
-			_rtl_direct.updateRtlPath();
-			break;
-
-
-		default:
-			break;
-		}
-	}
-}
-
 void RTL::on_activation()
 {
 	_global_pos_sub.update();

src/modules/navigator/rtl.h

@@ -126,8 +126,6 @@ private:
 	 */
 	void publishRemainingTimeEstimate();
 
-	void updateRtlPath();
-
 	/**
 	 * @brief Find RTL destination.
 	 *

src/modules/navigator/rtl_direct.cpp

@@ -73,6 +73,9 @@ void RtlDirect::on_activation()
 	_global_pos_sub.update();
 	_vehicle_status_sub.update();
 
+	_navigator->updateStartOfRTLPathPlanner(matrix::Vector2d{_global_pos_sub.get().lat, _global_pos_sub.get().lon});
+	_geofence_aware_return_path = _navigator->planPath();
+
 	parameters_update();
 
 	_rtl_state = getActivationState();
@@ -132,6 +135,8 @@ void RtlDirect::setRtlPosition(const PositionYawSetpoint &rtl_position, const lo
 		_destination = rtl_position;
 		_force_heading = false;
 
+		_navigator->updateDestinationOfRTLPathPlanner(matrix::Vector2d{_destination.lat, _destination.lon});
+
 		_land_approach = sanitizeLandApproach(loiter_pos);
 
 		const float dist_to_destination{get_distance_to_next_waypoint(_land_approach.lat, _land_approach.lon, _destination.lat, _destination.lon)};
@@ -660,10 +665,3 @@ void RtlDirect::publish_rtl_direct_navigator_mission_item()
 
 	_navigator_mission_item_pub.publish(navigator_mission_item);
 }
-
-void RtlDirect::updateRtlPath()
-{
-	const matrix::Vector2d current_position{_global_pos_sub.get().lat, _global_pos_sub.get().lon};
-	_geofence_aware_return_path = _navigator->planPathToDestination(current_position, matrix::Vector2d(_destination.lat, _destination.lon),
-				      2.0f * _navigator->get_acceptance_radius());
-}

src/modules/navigator/rtl_direct.h

@@ -112,8 +112,6 @@ public:
 
 	bool isLanding() { return (_rtl_state != RTLState::IDLE) && (_rtl_state >= RTLState::LOITER_DOWN);};
 
-	void updateRtlPath();
-
 private:
 	/**
 	 * @brief Return to launch state machine.