From d5a84e0c7fc1034ba2b06f0dd536a7c133c89453 Mon Sep 17 00:00:00 2001
From: in0finite <leshow816@gmail.com>
Date: Sun, 27 Mar 2022 03:43:24 +0200
Subject: [PATCH] add option to calculate full nav mesh path in map window

---
 .../Scripts/Behaviours/PathfindingManager.cs  | 32 +++++++++++++++++
 Assets/Scripts/UI/MapWindow.cs                | 34 ++++++++++++++-----
 2 files changed, 58 insertions(+), 8 deletions(-)

diff --git a/Assets/Scripts/Behaviours/PathfindingManager.cs b/Assets/Scripts/Behaviours/PathfindingManager.cs
index c8f1b0c3..65a2cc10 100644
--- a/Assets/Scripts/Behaviours/PathfindingManager.cs
+++ b/Assets/Scripts/Behaviours/PathfindingManager.cs
@@ -360,5 +360,37 @@ namespace SanAndreasUnity.Behaviours
             if (!sortedSet.Remove(element))
                 throw new InvalidOperationException("Failed to remove element from SortedSet");
         }
+
+        public static List<Vector3> CalculateFullNavMeshPath(
+            Vector3 source, Vector3 dest, int areaMask)
+        {
+            var allCorners = new List<Vector3>();
+            var path = new NavMeshPath();
+            Vector3 nextSource = source;
+            int i = 0;
+
+            while (true)
+            {
+                if (!NavMesh.CalculatePath(nextSource, dest, areaMask, path))
+                    break;
+
+                Vector3[] pathCorners = path.corners;
+                allCorners.AddRange(pathCorners);
+
+                if (path.status == NavMeshPathStatus.PathComplete)
+                    break;
+
+                if (pathCorners.Length <= 1) // partial path that leads nowhere ?
+                    break;
+
+                nextSource = pathCorners.Last();
+
+                i++;
+                if (i >= 50)
+                    break;
+            }
+
+            return allCorners;
+        }
     }
 }
diff --git a/Assets/Scripts/UI/MapWindow.cs b/Assets/Scripts/UI/MapWindow.cs
index 536eb766..69cfb240 100644
--- a/Assets/Scripts/UI/MapWindow.cs
+++ b/Assets/Scripts/UI/MapWindow.cs
@@ -32,11 +32,12 @@ namespace SanAndreasUnity.UI {
 		private Vector3[] m_navMeshPathToWaypoint = null;
 		private enum PathType
         {
+			NavMeshFull,
 			NavMesh,
 			Ped,
         }
-		private PathType m_pathType = PathType.NavMesh;
-		private static readonly string[] s_pathTypeTexts = new string[] { "NavMesh", "Ped" };
+		private PathType m_pathType = PathType.NavMeshFull;
+		private static readonly string[] s_pathTypeTexts = new string[] { "NavMeshFull", "NavMesh", "Ped" };
 		private int m_selectedPathType = 0;
 
 		private	Vector2	m_lastMousePosition = Vector2.zero;
@@ -291,6 +292,17 @@ namespace SanAndreasUnity.UI {
 					targetPos,
 					OnPathToWaypointFound);
 			}
+			else if (m_pathType == PathType.NavMeshFull)
+            {
+				var sw = System.Diagnostics.Stopwatch.StartNew();
+
+				if (NavMesh.SamplePosition(targetPos, out var hit, 300f, -1))
+					targetPos = hit.position;
+
+				m_navMeshPathToWaypoint = PathfindingManager.CalculateFullNavMeshPath(sourcePos, targetPos, -1).ToArray();
+
+				LogPathCalculationInfo(m_navMeshPathToWaypoint, null, sw.Elapsed.TotalMilliseconds);
+            }
 			else if (m_pathType == PathType.NavMesh)
             {
 				var sw = System.Diagnostics.Stopwatch.StartNew();
@@ -302,12 +314,8 @@ namespace SanAndreasUnity.UI {
 				if (NavMesh.CalculatePath(sourcePos, targetPos, -1, path))
 					m_navMeshPathToWaypoint = path.corners;
 
-				Debug.Log($"Nav mesh path calculation done - " +
-					$"status {path.status}, " +
-					$"num corners {m_navMeshPathToWaypoint?.Length ?? 0}, " +
-					$"total distance {m_navMeshPathToWaypoint?.Skip(1).Select((corner, i) => Vector3.Distance(corner, m_navMeshPathToWaypoint[i])).Sum()}, " +
-					$"time {sw.Elapsed.TotalMilliseconds} ms");
-            }
+				LogPathCalculationInfo(m_navMeshPathToWaypoint, path.status, sw.Elapsed.TotalMilliseconds);
+			}
 		}
 
 		private void OnPathToWaypointFound(PathfindingManager.PathResult pathResult)
@@ -323,6 +331,16 @@ namespace SanAndreasUnity.UI {
 				m_pathToWaypoint = null;
         }
 
+		private static void LogPathCalculationInfo(
+			Vector3[] corners, NavMeshPathStatus? pathStatus, double timeElapsedMs)
+        {
+			Debug.Log($"Nav mesh path calculation done - " +
+					(pathStatus.HasValue ? $"status {pathStatus.Value}, " : "") +
+					$"num corners {corners?.Length ?? 0}, " +
+					$"total distance {corners?.Skip(1).Select((corner, i) => Vector3.Distance(corner, corners[i])).Sum()}, " +
+					$"time {timeElapsedMs} ms");
+		}
+
 
 		void Update() {