From 51de6732c90b1ad7556aea1f2c26a7e206335940 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?R=C3=A9mi=20Verschelde?= <rverschelde@gmail.com>
Date: Tue, 9 Jun 2020 10:26:21 +0200
Subject: [PATCH] AStar: Make get_closest_point() deterministic for equidistant
 points

Closes godotengine/godot-docs#3667.
Supersedes #39405.

(cherry picked from commit 187ba4c5a884aaecd97febcdfaaa76466820be07)
---
 core/math/a_star.cpp    | 14 +++++++++-----
 doc/classes/AStar.xml   |  3 ++-
 doc/classes/AStar2D.xml |  3 ++-
 3 files changed, 13 insertions(+), 7 deletions(-)

diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp
index df77dd94948..21523963c15 100644
--- a/core/math/a_star.cpp
+++ b/core/math/a_star.cpp
@@ -292,10 +292,16 @@ int AStar::get_closest_point(const Vector3 &p_point, bool p_include_disabled) co
 
 		if (!p_include_disabled && !(*it.value)->enabled) continue; // Disabled points should not be considered.
 
+		// Keep the closest point's ID, and in case of multiple closest IDs,
+		// the smallest one (makes it deterministic).
 		real_t d = p_point.distance_squared_to((*it.value)->pos);
-		if (closest_id < 0 || d < closest_dist) {
+		int id = *(it.key);
+		if (d <= closest_dist) {
+			if (d == closest_dist && id > closest_id) { // Keep lowest ID.
+				continue;
+			}
 			closest_dist = d;
-			closest_id = *(it.key);
+			closest_id = id;
 		}
 	}
 
@@ -304,7 +310,6 @@ int AStar::get_closest_point(const Vector3 &p_point, bool p_include_disabled) co
 
 Vector3 AStar::get_closest_position_in_segment(const Vector3 &p_point) const {
 
-	bool found = false;
 	real_t closest_dist = 1e20;
 	Vector3 closest_point;
 
@@ -325,11 +330,10 @@ Vector3 AStar::get_closest_position_in_segment(const Vector3 &p_point) const {
 
 		Vector3 p = Geometry::get_closest_point_to_segment(p_point, segment);
 		real_t d = p_point.distance_squared_to(p);
-		if (!found || d < closest_dist) {
+		if (d < closest_dist) {
 
 			closest_point = p;
 			closest_dist = d;
-			found = true;
 		}
 	}
 
diff --git a/doc/classes/AStar.xml b/doc/classes/AStar.xml
index 22c22b0c167..03831964720 100644
--- a/doc/classes/AStar.xml
+++ b/doc/classes/AStar.xml
@@ -131,7 +131,8 @@
 			<argument index="1" name="include_disabled" type="bool" default="false">
 			</argument>
 			<description>
-				Returns the ID of the closest point to [code]to_position[/code], optionally taking disabled points into account. Returns -1 if there are no points in the points pool.
+				Returns the ID of the closest point to [code]to_position[/code], optionally taking disabled points into account. Returns [code]-1[/code] if there are no points in the points pool.
+				[b]Note:[/b] If several points are the closest to [code]to_position[/code], the one with the smallest ID will be returned, ensuring a deterministic result.
 			</description>
 		</method>
 		<method name="get_closest_position_in_segment" qualifiers="const">
diff --git a/doc/classes/AStar2D.xml b/doc/classes/AStar2D.xml
index a8a5b34f633..b313597d14e 100644
--- a/doc/classes/AStar2D.xml
+++ b/doc/classes/AStar2D.xml
@@ -114,7 +114,8 @@
 			<argument index="1" name="include_disabled" type="bool" default="false">
 			</argument>
 			<description>
-				Returns the ID of the closest point to [code]to_position[/code], optionally taking disabled points into account. Returns -1 if there are no points in the points pool.
+				Returns the ID of the closest point to [code]to_position[/code], optionally taking disabled points into account. Returns [code]-1[/code] if there are no points in the points pool.
+				[b]Note:[/b] If several points are the closest to [code]to_position[/code], the one with the smallest ID will be returned, ensuring a deterministic result.
 			</description>
 		</method>
 		<method name="get_closest_position_in_segment" qualifiers="const">