From 855dd77dd4388a630535e6acc6cac30c3c3244ab Mon Sep 17 00:00:00 2001 From: in0finite Date: Sat, 19 Feb 2022 03:25:43 +0100 Subject: [PATCH] first attempt at pathfinding --- Assets/Prefabs/GameManager.prefab | 14 + .../Scripts/Behaviours/PathfindingManager.cs | 257 ++++++++++++++++++ .../Behaviours/PathfindingManager.cs.meta | 11 + Assets/Scripts/Importing/Paths/NodeFile.cs | 34 +++ Assets/Scripts/Utilities/MathUtils.cs | 21 ++ Assets/Scripts/Utilities/MathUtils.cs.meta | 11 + 6 files changed, 348 insertions(+) create mode 100644 Assets/Scripts/Behaviours/PathfindingManager.cs create mode 100644 Assets/Scripts/Behaviours/PathfindingManager.cs.meta create mode 100644 Assets/Scripts/Utilities/MathUtils.cs create mode 100644 Assets/Scripts/Utilities/MathUtils.cs.meta diff --git a/Assets/Prefabs/GameManager.prefab b/Assets/Prefabs/GameManager.prefab index e15b5825..45801e02 100644 --- a/Assets/Prefabs/GameManager.prefab +++ b/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 diff --git a/Assets/Scripts/Behaviours/PathfindingManager.cs b/Assets/Scripts/Behaviours/PathfindingManager.cs new file mode 100644 index 00000000..e23a7dba --- /dev/null +++ b/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 + { + public class PathResult + { + public bool IsSuccess { get; set; } + + public List Nodes { get; set; } + + public float TimeElapsed { get; set; } + } + + public class NodeComparer : IComparer + { + private readonly NodePathfindingData[][] m_nodePathfindingDatas; + + public NodeComparer(NodePathfindingData[][] nodePathfindingDatas) + { + m_nodePathfindingDatas = nodePathfindingDatas; + } + + int IComparer.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 m_modifiedDatas = new List(); + + + + 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 callback) + { + this.BackgroundJobRunner.RegisterJob(new BackgroundJobRunner.Job() + { + 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(); + var openList = new SortedSet(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 BuildPath(PathNodeId root) + { + var list = new List(); + + 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; + } + } +} diff --git a/Assets/Scripts/Behaviours/PathfindingManager.cs.meta b/Assets/Scripts/Behaviours/PathfindingManager.cs.meta new file mode 100644 index 00000000..719140e9 --- /dev/null +++ b/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: diff --git a/Assets/Scripts/Importing/Paths/NodeFile.cs b/Assets/Scripts/Importing/Paths/NodeFile.cs index 482d4309..75f4b30d 100644 --- a/Assets/Scripts/Importing/Paths/NodeFile.cs +++ b/Assets/Scripts/Importing/Paths/NodeFile.cs @@ -34,12 +34,38 @@ namespace SanAndreasUnity.Importing.Paths } } + public struct PathNodeId : IEquatable + { + 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 { 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( diff --git a/Assets/Scripts/Utilities/MathUtils.cs b/Assets/Scripts/Utilities/MathUtils.cs new file mode 100644 index 00000000..d57695c3 --- /dev/null +++ b/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); + } + } +} diff --git a/Assets/Scripts/Utilities/MathUtils.cs.meta b/Assets/Scripts/Utilities/MathUtils.cs.meta new file mode 100644 index 00000000..f9d4fcc2 --- /dev/null +++ b/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: