first attempt at pathfinding

Assets/Prefabs/GameManager.prefab

@@ -38,6 +38,7 @@ GameObject:
   - component: {fileID: 7374219779517658196}
   - component: {fileID: 2483599330734961439}
   - component: {fileID: 6746758691818800092}
+  - component: {fileID: 6423383668577662412}
   m_Layer: 0
   m_Name: GameManager
   m_TagString: Untagged
@@ -535,3 +536,16 @@ MonoBehaviour:
   m_Script: {fileID: 11500000, guid: 81bcbcc6c7d0163408eea6ffa3732506, type: 3}
   m_Name: 
   m_EditorClassIdentifier: 
+--- !u!114 &6423383668577662412
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 1297494511425690}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: 575d2f8acc87f5f4fbe2ee0f8d1476e4, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
+  m_maxTimePerFrameMs: 0

Assets/Scripts/Behaviours/PathfindingManager.cs

@@ -0,0 +1,257 @@
+using SanAndreasUnity.Importing.Paths;
+using SanAndreasUnity.Utilities;
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Linq;
+using UnityEngine;
+
+namespace SanAndreasUnity.Behaviours
+{
+    public class PathfindingManager : StartupSingleton<PathfindingManager>
+    {
+        public class PathResult
+        {
+            public bool IsSuccess { get; set; }
+
+            public List<PathNodeId> Nodes { get; set; }
+
+            public float TimeElapsed { get; set; }
+        }
+
+        public class NodeComparer : IComparer<PathNodeId>
+        {
+            private readonly NodePathfindingData[][] m_nodePathfindingDatas;
+
+            public NodeComparer(NodePathfindingData[][] nodePathfindingDatas)
+            {
+                m_nodePathfindingDatas = nodePathfindingDatas;
+            }
+
+            int IComparer<PathNodeId>.Compare(PathNodeId a, PathNodeId b)
+            {
+                return m_nodePathfindingDatas[a.AreaID][a.NodeID].f.CompareTo(
+                    m_nodePathfindingDatas[b.AreaID][b.NodeID].f);
+            }
+        }
+
+        public BackgroundJobRunner BackgroundJobRunner { get; } = new BackgroundJobRunner();
+
+        [SerializeField] private ushort m_maxTimePerFrameMs = 0;
+
+        private NodePathfindingData[][] m_nodePathfindingDatas = null;
+
+        private readonly List<PathNodeId> m_modifiedDatas = new List<PathNodeId>();
+
+
+
+        protected override void OnSingletonStart()
+        {
+            this.BackgroundJobRunner.EnsureBackgroundThreadStarted();
+        }
+
+        protected override void OnSingletonDisable()
+        {
+            this.BackgroundJobRunner.ShutDown();
+        }
+
+        void OnLoaderFinished()
+        {
+            m_nodePathfindingDatas = new NodePathfindingData[NodeReader.NodeFiles.Count][];
+            for (int i = 0; i < m_nodePathfindingDatas.Length; i++)
+            {
+                m_nodePathfindingDatas[i] = new NodePathfindingData[NodeReader.NodeFiles[i].NumOfPedNodes + NodeReader.NodeFiles[i].NumOfVehNodes];
+            }
+        }
+
+        void Update()
+        {
+            this.BackgroundJobRunner.UpdateJobs(m_maxTimePerFrameMs);
+        }
+
+        public void FindPath(Vector3 source, Vector3 destination, Action<PathResult> callback)
+        {
+            this.BackgroundJobRunner.RegisterJob(new BackgroundJobRunner.Job<PathResult>()
+            {
+                action = () => FindPathInBackground(source, destination),
+                callbackFinish = callback,
+                priority = 1,
+            });
+        }
+
+        private PathResult FindPathInBackground(Vector3 sourcePos, Vector3 destinationPos)
+        {
+            var stopwatch = Stopwatch.StartNew();
+            PathResult pathResult = new PathResult { IsSuccess = false };
+
+            // find closest node of source position
+
+            /*var closestSourceEdge = NodeReader.GetAreasInRadius(sourcePos, 300f)
+                .SelectMany(_ => _.PedNodes)
+                .Where(_ => Vector3.Distance(_.Position, sourcePos) < 1000f)
+                .SelectMany(_ => NodeReader.GetAllLinkedNodes(_).Select(ln => (n1: _, n2: ln)))
+                .MinBy(_ => MathUtils.DistanceFromPointToLineSegment(sourcePos, _.n1.Position, _.n2.Position), default);*/
+
+            var closestSourceNode = NodeReader.GetAreasInRadius(sourcePos, 300f)
+                .SelectMany(_ => _.PedNodes)
+                .MinBy(_ => Vector3.Distance(_.Position, sourcePos), PathNode.InvalidNode);
+
+            if (closestSourceNode.Equals(PathNode.InvalidNode))
+                return pathResult;
+
+            // find closest node of destination position
+            var closestDestinationNode = NodeReader.GetAreasInRadius(destinationPos, 300f)
+                .SelectMany(_ => _.PedNodes)
+                .MinBy(_ => Vector3.Distance(_.Position, destinationPos), PathNode.InvalidNode);
+
+            if (closestDestinationNode.Equals(PathNode.InvalidNode))
+                return pathResult;
+
+
+
+
+            this.RestoreModifiedDatas();
+
+            var closedList = new HashSet<PathNodeId>();
+            var openList = new SortedSet<PathNodeId>(new NodeComparer(m_nodePathfindingDatas));
+
+            PathNodeId startId = closestSourceNode.Id;
+            PathNodeId targetId = closestDestinationNode.Id;
+
+            var startData = GetData(startId);
+            startData.f = startData.g + CalculateHeuristic(startId, targetId);
+            SetData(startId, startData);
+
+            openList.Add(startId);
+
+            while (openList.Count > 0)
+            {
+                PathNodeId idN = openList.Min; // TODO: optimize ; call Remove() ;
+                
+                if (idN.Equals(targetId))
+                {
+                    pathResult.Nodes = BuildPath(idN);
+                    pathResult.IsSuccess = true;
+                    break;
+                }
+
+                var nodeN = NodeReader.GetNodeById(idN);
+                var areaN = NodeReader.GetAreaById(idN.AreaID);
+                var dataN = GetData(idN);
+
+                for (int i = 0; i < nodeN.LinkCount; i++)
+                {
+                    NodeLink link = areaN.NodeLinks[nodeN.BaseLinkID + i];
+
+                    PathNodeId idM = link.PathNodeId;
+                    var dataM = GetData(idM);
+
+                    float totalWeight = dataN.g + link.Length;
+
+                    if (!openList.Contains(idM) && !closedList.Contains(idM))
+                    {
+                        dataM.parentId = idN;
+                        dataM.hasParent = true;
+                        dataM.g = totalWeight;
+                        dataM.f = dataM.g + CalculateHeuristic(idM, targetId);
+                        SetData(idM, dataM);
+
+                        openList.Add(idM);
+                    }
+                    else
+                    {
+                        if (totalWeight < dataM.g)
+                        {
+                            dataM.parentId = idN;
+                            dataM.hasParent = true;
+                            dataM.g = totalWeight;
+                            dataM.f = dataM.g + CalculateHeuristic(idM, targetId);
+                            SetData(idM, dataM);
+
+                            if (closedList.Contains(idM))
+                            {
+                                closedList.Remove(idM);
+                                openList.Add(idM);
+                            }
+                        }
+                    }
+                }
+
+                openList.Remove(idN);
+                closedList.Add(idN);
+            }
+
+            int numModifiedDatas = m_modifiedDatas.Count;
+            RestoreModifiedDatas();
+
+            pathResult.TimeElapsed = (float)stopwatch.Elapsed.TotalSeconds;
+
+            UnityEngine.Debug.Log($"Path finding finished: time {pathResult.TimeElapsed}, num nodes {pathResult.Nodes?.Count ?? 0}, numModifiedDatas {numModifiedDatas}");
+
+            return pathResult;
+        }
+
+        private NodePathfindingData GetData(PathNodeId id)
+        {
+            return m_nodePathfindingDatas[id.AreaID][id.NodeID];
+        }
+
+        private void SetData(PathNodeId id, NodePathfindingData data)
+        {
+            m_nodePathfindingDatas[id.AreaID][id.NodeID] = data;
+            m_modifiedDatas.Add(id);
+        }
+
+        private void SetDataNoRemember(PathNodeId id, NodePathfindingData data)
+        {
+            m_nodePathfindingDatas[id.AreaID][id.NodeID] = data;
+        }
+
+        private float CalculateHeuristic(PathNodeId source, PathNodeId destination)
+        {
+            return Vector3.Distance(
+                NodeReader.GetNodeById(source).Position,
+                NodeReader.GetNodeById(destination).Position);
+        }
+
+        private void RestoreModifiedDatas()
+        {
+            foreach (var id in m_modifiedDatas)
+            {
+                var data = GetResettedData();
+                SetDataNoRemember(id, data);
+            }
+
+            m_modifiedDatas.Clear();
+        }
+
+        private static NodePathfindingData GetResettedData()
+        {
+            var data = new NodePathfindingData();
+            data.parentId = PathNodeId.InvalidId;
+            return data;
+        }
+
+        private List<PathNodeId> BuildPath(PathNodeId root)
+        {
+            var list = new List<PathNodeId>();
+
+            PathNodeId n = root;
+            var data = GetData(n);
+
+            while (data.hasParent)
+            {
+                list.Add(n);
+
+                n = data.parentId;
+                data = GetData(n);
+            }
+
+            list.Add(n);
+
+            list.Reverse();
+
+            return list;
+        }
+    }
+}

Assets/Scripts/Behaviours/PathfindingManager.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 575d2f8acc87f5f4fbe2ee0f8d1476e4
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/Scripts/Importing/Paths/NodeFile.cs

@@ -34,12 +34,38 @@ namespace SanAndreasUnity.Importing.Paths
         }
     }
 
+    public struct PathNodeId : IEquatable<PathNodeId>
+    {
+        public int AreaID { get; set; }
+        public int NodeID { get; set; }
+
+        public bool Equals(PathNodeId other)
+        {
+            return AreaID == other.AreaID && NodeID == other.NodeID;
+        }
+
+        public override int GetHashCode()
+        {
+            return ((AreaID << 5) + AreaID) ^ NodeID;
+        }
+
+        public static PathNodeId InvalidId => new PathNodeId { AreaID = -1, NodeID = -1 };
+    }
+
+    public struct NodePathfindingData
+    {
+        public float f, g;
+        public PathNodeId parentId;
+        public bool hasParent;
+    }
+
     public struct PathNode : IEquatable<PathNode>
     {
         public UnityEngine.Vector3 Position { get; set; }
         public int BaseLinkID { get; set; }
         public int AreaID { get; set; }
         public int NodeID { get; set; }
+        public PathNodeId Id => new PathNodeId { AreaID = AreaID, NodeID = NodeID };
         public float PathWidth { get; set; }
         public int NodeType { get; set; } // enum
         public int LinkCount { get; set; }
@@ -51,6 +77,8 @@ namespace SanAndreasUnity.Importing.Paths
         {
             return AreaID == other.AreaID && NodeID == other.NodeID;
         }
+
+        public static PathNode InvalidNode => new PathNode { AreaID = -1, NodeID = -1 };
     }
 
     public struct NavNode
@@ -72,6 +100,7 @@ namespace SanAndreasUnity.Importing.Paths
     {
         public int AreaID { get; set; }
         public int NodeID { get; set; }
+        public PathNodeId PathNodeId => new PathNodeId { AreaID = AreaID, NodeID = NodeID };
         public int Length { get; set; }
     }
 
@@ -122,6 +151,11 @@ namespace SanAndreasUnity.Importing.Paths
             return NodeFiles[id];
         }
 
+        public static PathNode GetNodeById(PathNodeId id)
+        {
+            return NodeFiles[id.AreaID].GetNodeById(id.NodeID);
+        }
+
         public static Vector2Int GetAreaIndexesFromPosition(UnityEngine.Vector3 position, bool clamp)
         {
             return new Vector2Int(

Assets/Scripts/Utilities/MathUtils.cs

@@ -0,0 +1,21 @@
+using UnityEngine;
+
+namespace SanAndreasUnity.Utilities
+{
+    public static class MathUtils
+    {
+        public static float DistanceFromPointToLineSegment(Vector3 p, Vector3 v, Vector3 w)
+        {
+            // Return minimum distance between line segment vw and point p
+            float l2 = Vector3.SqrMagnitude(v - w);  // i.e. |w-v|^2 -  avoid a sqrt
+            if (l2 == 0.0f) return Vector3.Distance(p, v);   // v == w case
+                                                    // Consider the line extending the segment, parameterized as v + t (w - v).
+                                                    // We find projection of point p onto the line. 
+                                                    // It falls where t = [(p-v) . (w-v)] / |w-v|^2
+                                                    // We clamp t from [0,1] to handle points outside the segment vw.
+            float t = Mathf.Max(0, Mathf.Min(1, Vector3.Dot(p - v, w - v) / l2));
+            Vector3 projection = v + t * (w - v);  // Projection falls on the segment
+            return Vector3.Distance(p, projection);
+        }
+    }
+}

Assets/Scripts/Utilities/MathUtils.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 7988e4c48d3669647a4fdf11e186dc5c
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: