From 8eb29a4dce73467fd7c36b3b09b0b2561dfbdb18 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Manuel=20V=C3=B6gele?= <develop@manuel-voegele.de>
Date: Wed, 9 Feb 2022 21:08:28 +0100
Subject: [PATCH] Prevent creation of unnecessary waypoints

---
 rust/src/pathfinder.rs | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/rust/src/pathfinder.rs b/rust/src/pathfinder.rs
index 0b8a411..4e49db3 100644
--- a/rust/src/pathfinder.rs
+++ b/rust/src/pathfinder.rs
@@ -359,7 +359,8 @@ impl Pathfinder {
 					continue;
 				}
 				nodes.initialize_edges(neighbor, &self.walls);
-				let cost = current_node.borrow().cost + edge.cost;
+				// Add a flat 0.00001 cost per node to discurage creation of unnecessary waypoints
+				let cost = current_node.borrow().cost + edge.cost + 0.00001;
 				let discovered_neighbor = DiscoveredNode {
 					node: neighbor.clone(),
 					cost,