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 CalculateFullNavMeshPath( + Vector3 source, Vector3 dest, int areaMask) + { + var allCorners = new List(); + 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() {