From e4f19fba87801cc2b40eb627e658b6f94d0ea36c Mon Sep 17 00:00:00 2001 From: KerelOlivier Date: Sun, 3 Apr 2022 00:36:04 +0200 Subject: [PATCH] add: fancy new pathfinding --- Assets/Scenes/test.unity | 171 ++++++++++++++- Assets/Scripts/Enemies/Enemy.cs | 1 + Assets/Scripts/Enemies/States/PathState.cs | 16 +- Assets/Scripts/PathFinding.cs | 231 ++++++++++++++++++++ Assets/Scripts/PathFinding.cs.meta | 11 + Assets/Scripts/Priorityqueue.cs | 85 ++++++++ Assets/Scripts/Priorityqueue.cs.meta | 3 + Assets/Sprites/Obstacles.png | Bin 0 -> 996 bytes Assets/Sprites/Obstacles.png.meta | 240 +++++++++++++++++++++ ProjectSettings/TagManager.asset | 2 +- 10 files changed, 745 insertions(+), 15 deletions(-) create mode 100644 Assets/Scripts/PathFinding.cs create mode 100644 Assets/Scripts/PathFinding.cs.meta create mode 100644 Assets/Scripts/Priorityqueue.cs create mode 100644 Assets/Scripts/Priorityqueue.cs.meta create mode 100644 Assets/Sprites/Obstacles.png create mode 100644 Assets/Sprites/Obstacles.png.meta diff --git a/Assets/Scenes/test.unity b/Assets/Scenes/test.unity index 02012e6..e66f1ea 100644 --- a/Assets/Scenes/test.unity +++ b/Assets/Scenes/test.unity @@ -151,7 +151,7 @@ Transform: m_LocalScale: {x: 1, y: 1, z: 1} m_Children: [] m_Father: {fileID: 0} - m_RootOrder: 5 + m_RootOrder: 6 m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} --- !u!1 &394882245 GameObject: @@ -187,7 +187,7 @@ Transform: m_Children: - {fileID: 1731621445} m_Father: {fileID: 0} - m_RootOrder: 2 + m_RootOrder: 3 m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} --- !u!212 &394882247 SpriteRenderer: @@ -456,8 +456,117 @@ Transform: m_LocalScale: {x: 1, y: 1, z: 1} m_Children: [] m_Father: {fileID: 0} - m_RootOrder: 0 + m_RootOrder: 1 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!1 &976255692 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 976255694} + - component: {fileID: 976255693} + - component: {fileID: 976255695} + m_Layer: 6 + m_Name: Obstacles_1 + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!212 &976255693 +SpriteRenderer: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 976255692} + m_Enabled: 1 + m_CastShadows: 0 + m_ReceiveShadows: 0 + m_DynamicOccludee: 1 + m_MotionVectors: 1 + m_LightProbeUsage: 1 + m_ReflectionProbeUsage: 1 + m_RayTracingMode: 0 + m_RayTraceProcedural: 0 + m_RenderingLayerMask: 1 + m_RendererPriority: 0 + m_Materials: + - {fileID: 2100000, guid: a97c105638bdf8b4a8650670310a4cd3, type: 2} + m_StaticBatchInfo: + firstSubMesh: 0 + subMeshCount: 0 + m_StaticBatchRoot: {fileID: 0} + m_ProbeAnchor: {fileID: 0} + m_LightProbeVolumeOverride: {fileID: 0} + m_ScaleInLightmap: 1 + m_ReceiveGI: 1 + m_PreserveUVs: 0 + m_IgnoreNormalsForChartDetection: 0 + m_ImportantGI: 0 + m_StitchLightmapSeams: 1 + m_SelectedEditorRenderState: 0 + m_MinimumChartSize: 4 + m_AutoUVMaxDistance: 0.5 + m_AutoUVMaxAngle: 89 + m_LightmapParameters: {fileID: 0} + m_SortingLayerID: 0 + m_SortingLayer: 0 + m_SortingOrder: 0 + m_Sprite: {fileID: 7788020138054186290, guid: 26aed7a8e043773feb7cc6bb749fdc3c, type: 3} + m_Color: {r: 1, g: 1, b: 1, a: 1} + m_FlipX: 0 + m_FlipY: 0 + m_DrawMode: 0 + m_Size: {x: 2, y: 1} + m_AdaptiveModeThreshold: 0.5 + m_SpriteTileMode: 0 + m_WasSpriteAssigned: 1 + m_MaskInteraction: 0 + m_SpriteSortPoint: 0 +--- !u!4 &976255694 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 976255692} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 0} + m_RootOrder: 7 m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!61 &976255695 +BoxCollider2D: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 976255692} + m_Enabled: 1 + m_Density: 1 + m_Material: {fileID: 0} + m_IsTrigger: 0 + m_UsedByEffector: 0 + m_UsedByComposite: 0 + m_Offset: {x: 0, y: 0} + m_SpriteTilingProperty: + border: {x: 0, y: 0, z: 0, w: 0} + pivot: {x: 0.5, y: 0.5} + oldSize: {x: 2, y: 1} + newSize: {x: 2, y: 1} + adaptiveTilingThreshold: 0.5 + drawMode: 0 + adaptiveTiling: 0 + m_AutoTiling: 0 + serializedVersion: 2 + m_Size: {x: 1.6, y: 0.8} + m_EdgeRadius: 0 --- !u!1 &1142655128 GameObject: m_ObjectHideFlags: 0 @@ -528,8 +637,56 @@ Transform: m_LocalScale: {x: 1, y: 1, z: 1} m_Children: [] m_Father: {fileID: 0} - m_RootOrder: 1 + m_RootOrder: 2 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!1 &1412150546 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 1412150547} + - component: {fileID: 1412150548} + m_Layer: 0 + m_Name: GameManager + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &1412150547 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1412150546} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 0} + m_RootOrder: 0 m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &1412150548 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1412150546} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: a0ee750a6dc3f9e97b39747ae230044c, type: 3} + m_Name: + m_EditorClassIdentifier: + a: -5 + b: -5 + width: 10 + height: 10 + obst: {fileID: 976255692} --- !u!1 &1455080910 GameObject: m_ObjectHideFlags: 0 @@ -572,7 +729,7 @@ Transform: m_Children: - {fileID: 2065747230} m_Father: {fileID: 0} - m_RootOrder: 3 + m_RootOrder: 4 m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} --- !u!1 &1731621444 GameObject: @@ -729,7 +886,7 @@ Transform: m_LocalScale: {x: 1, y: 1, z: 1} m_Children: [] m_Father: {fileID: 0} - m_RootOrder: 4 + m_RootOrder: 5 m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} --- !u!58 &1849325999 CircleCollider2D: @@ -767,7 +924,7 @@ Rigidbody2D: m_Interpolate: 0 m_SleepingMode: 1 m_CollisionDetection: 0 - m_Constraints: 7 + m_Constraints: 0 --- !u!114 &1849326001 MonoBehaviour: m_ObjectHideFlags: 0 diff --git a/Assets/Scripts/Enemies/Enemy.cs b/Assets/Scripts/Enemies/Enemy.cs index 94b22d6..feb6829 100644 --- a/Assets/Scripts/Enemies/Enemy.cs +++ b/Assets/Scripts/Enemies/Enemy.cs @@ -21,6 +21,7 @@ namespace Enemies public float damage = 1; public GameObject target { get; set; } + public GameObject GameManager; public float speed = 2f; public float viewRange = 5f; public float attackRange = 1f; diff --git a/Assets/Scripts/Enemies/States/PathState.cs b/Assets/Scripts/Enemies/States/PathState.cs index 0b89cff..50d8404 100644 --- a/Assets/Scripts/Enemies/States/PathState.cs +++ b/Assets/Scripts/Enemies/States/PathState.cs @@ -1,3 +1,4 @@ +using System.Collections.Generic; using Enemies.States; using UnityEngine; @@ -20,13 +21,14 @@ namespace Enemies public void Execute() { - //Determine direction to target - Vector2 direction = context.target.transform.position - context.transform.position; - - //Move in desired direction - context.GetComponent().MovePosition((Vector2)context.transform.position + direction.normalized * context.speed * Time.deltaTime); - float angle = Mathf.Atan2(direction.y, direction.x) * Mathf.Rad2Deg; - context.transform.rotation = Quaternion.Euler(0, 0, angle); + LinkedList path = context.GameManager.GetComponent() + .FindPath(context.transform.position, context.target.transform.position); + Vector2 target = path.First.Value; + Vector2 direction = target - (Vector2) context.transform.position; + context.transform.rotation = Quaternion.Euler(0, 0, Mathf.Atan2(direction.y, direction.x) * Mathf.Rad2Deg); + context.transform.position += (Vector3)(direction * context.speed * Time.deltaTime); + //Set looking direction + } public void Exit() diff --git a/Assets/Scripts/PathFinding.cs b/Assets/Scripts/PathFinding.cs new file mode 100644 index 0000000..b665ba7 --- /dev/null +++ b/Assets/Scripts/PathFinding.cs @@ -0,0 +1,231 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.Numerics; +using UnityEngine; +using Vector2 = UnityEngine.Vector2; +using Vector3 = UnityEngine.Vector3; + +public class PathFinding : MonoBehaviour +{ + private float unitSize = .5f; //unit size + + public int a = 0, b = 0; //Bot left corner of grid + public int width = 10, height = 10; //Grid size + + public bool[,] grid; //Valid path nodes + + public GameObject obst; //Obstacle prefab + + private LinkedList p; + + // Start is called before the first frame update + void Start() + { + Debug.Log("size: " + width + "," + height); + InitPathNodes(); + FindPath(new Vector2(-2, 0), new Vector2(2f, 0)); + } + + // Update is called once per frame + void Update() + { + } + + void InitPathNodes() + { + int xCount = (int) Mathf.Floor(width / unitSize); + int yCount = (int) Mathf.Floor(height / unitSize); + grid = new bool[xCount, yCount]; + for (int x = 0; x < xCount; x++) + { + for (int y = 0; y < yCount; y++) + { + bool walkable = isUnobstructed(a + x * unitSize, b + y * unitSize); + grid[x, y] = walkable; + } + } + + Debug.Log("fuck: " + (1 << obst.layer)); + Debug.Log("test: " + Physics2D.OverlapPoint(new Vector2(0, 0))); + } + + private bool isUnobstructed(float x, float y) + { + Vector2 p = new Vector2(x, y); + int mask = LayerMask.GetMask("Obstacle"); + Collider2D overlapPoint = Physics2D.OverlapPoint(p, mask); + bool res = overlapPoint == null; + return res; + } + + float Heuristic(Vector2Int node, Vector2Int goal) + { + //Return the distance between two points + int dx = node.x - goal.x; + int dy = node.y - goal.y; + return Mathf.Sqrt(dx * dx + dy * dy); + } + + List getNeighbours(Vector2Int pos) + { + List neighbours = new List(); + for (int x = 0; x < 3; x++) + { + for (int y = 0; y < 3; y++) + { + if (x == 1 && y == 1) + continue; + Vector2Int n = new Vector2Int(pos.x + x - 1, pos.y + y - 1); + if (n.x >= 0 && n.x < grid.GetLength(0) && n.y >= 0 && n.y < grid.GetLength(1)) + neighbours.Add(n); + } + } + + return neighbours; + } + + + //Find path with the A* algorithm + public LinkedList FindPath(Vector2 start_f, Vector2 goal_f) + { + //Check if in bounds + if (start_f.x - a < 0 || start_f.x - a > width || start_f.y - b < 0 || start_f.y - b > height) + { + Debug.LogError("Start out of bounds"); + return null; + } + + if (goal_f.x - a < 0 || goal_f.x - a > width || goal_f.y - b < 0 || goal_f.y - b > height) + { + Debug.LogError("Goal out of bounds"); + return null; + } + + bool[,] inset = new bool[grid.GetLength(0), grid.GetLength(1)]; + + //Calculate the grid positions of the two points + Vector2Int start = + new Vector2Int(Mathf.FloorToInt((start_f.x - a) / unitSize), Mathf.FloorToInt((start_f.y - b) / unitSize)); + Vector2Int goal = + new Vector2Int(Mathf.FloorToInt((goal_f.x - a) / unitSize), Mathf.FloorToInt((goal_f.y - b) / unitSize)); + + Debug.Log("start: " + start.x + "," + start.y); + Debug.Log("goal: " + goal.x + "," + goal.y); + + + PriorityQueue<(Vector2Int, float)> openset = + new PriorityQueue<(Vector2Int, float)>((a, b) => a.Item2.CompareTo(b.Item2)); + Dictionary cameFrom = new Dictionary(); + + openset.Insert((start, 0)); + float[,] gScore = new float[grid.GetLength(0), grid.GetLength(1)]; + float[,] fScore = new float[grid.GetLength(0), grid.GetLength(1)]; + for (int x = 0; x < grid.GetLength(0); x++) + { + for (int y = 0; y < grid.GetLength(1); y++) + { + gScore[x, y] = fScore[x, y] = Mathf.Infinity; + } + } + + gScore[start.x, start.y] = 0; + fScore[start.x, start.y] = Heuristic(start, goal); + + while (!openset.IsEmpty()) + { + (Vector2Int, float) current = openset.Extract(); + if (current.Item1 == goal) + { + Debug.Log("Found path"); + Debug.Log("Path length: " + gScore[current.Item1.x, current.Item1.y]); + Debug.Log("Path: " + cameFrom[current.Item1]); + LinkedList path = Backtrack(cameFrom, current.Item1); + p = path; + return ConvertPath(path); + } + + inset[current.Item1.x, current.Item1.y] = true; + List neighbours = getNeighbours(current.Item1); + foreach (Vector2Int neighbour in neighbours) + { + if (!grid[neighbour.x, neighbour.y]) + continue; + float tentativeGScore = gScore[current.Item1.x, current.Item1.y] + 1; + if (tentativeGScore >= gScore[neighbour.x, neighbour.y]) + continue; + cameFrom[neighbour] = current.Item1; + gScore[neighbour.x, neighbour.y] = tentativeGScore; + fScore[neighbour.x, neighbour.y] = gScore[neighbour.x, neighbour.y] + Heuristic(neighbour, goal); + if (!inset[neighbour.x, neighbour.y]) + { + openset.Insert((neighbour, fScore[neighbour.x, neighbour.y])); + inset[neighbour.x, neighbour.y] = true; + } + } + } + + return null; + } + + private LinkedList Backtrack(Dictionary cameFrom, Vector2Int current) + { + LinkedList path = new LinkedList(); + int count = 0; + Debug.Log("Current: " + current); + path.AddFirst(current); + while (cameFrom.ContainsKey(current)) + { + count++; + current = cameFrom[current]; + path.AddFirst(current); + Debug.Log("Came from: " + current); + } + + Debug.Log("Count: " + count); + return path; + } + + private LinkedList ConvertPath(LinkedList path) + { + LinkedList converted = new LinkedList(); + foreach (Vector2Int v in path) + { + converted.AddFirst(new Vector2(v.x * unitSize + a, v.y * unitSize + b)); + } + + return converted; + } + + private void OnDrawGizmos() + { + if (grid == null) return; + Gizmos.color = Color.yellow; + + int xCount = (int) Mathf.Floor(width / unitSize); + int yCount = (int) Mathf.Floor(height / unitSize); + + for (int x = 0; x < xCount; x++) + { + for (int y = 0; y < yCount; y++) + { + Gizmos.color = Color.yellow; + if (!grid[x, y]) + Gizmos.color = Color.red; + Vector3 pos = new Vector3(a + x * unitSize, b + y * unitSize, 0); + Gizmos.DrawSphere(pos, 0.05f); + } + } + + if (p == null) return; + Gizmos.color = Color.magenta; + var prev = p.First; + for (int i = 0; i < p.Count - 1; i++) + { + Vector3 pos = new Vector3(prev.Value.x * unitSize + a, prev.Value.y * unitSize + b, 0); + Vector3 target = new Vector3(prev.Next.Value.x * unitSize + a, prev.Next.Value.y * unitSize + b, 0); + Gizmos.DrawLine(pos, target); + prev = prev.Next; + } + } +} \ No newline at end of file diff --git a/Assets/Scripts/PathFinding.cs.meta b/Assets/Scripts/PathFinding.cs.meta new file mode 100644 index 0000000..c5d166a --- /dev/null +++ b/Assets/Scripts/PathFinding.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a0ee750a6dc3f9e97b39747ae230044c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/Priorityqueue.cs b/Assets/Scripts/Priorityqueue.cs new file mode 100644 index 0000000..808d5de --- /dev/null +++ b/Assets/Scripts/Priorityqueue.cs @@ -0,0 +1,85 @@ +using System; +using System.Collections.Generic; + +public class PriorityQueue +{ + private Func comparer; + + public PriorityQueue(Func comparer = null) + { + if (comparer == null) + { + comparer = (x, y) => Comparer.Default.Compare(x, y); + } + this.comparer = comparer; + } + + private readonly List queue = new List(); + + private void MinHeapify(int i) + { + int left = 2 * i + 1; + int right = 2 * i + 2; + int smallest = i; + if (left < queue.Count && comparer(queue[left], queue[smallest]) < 0) + { + smallest = left; + } + if (right < queue.Count && comparer(queue[right], queue[smallest]) < 0) + { + smallest = right; + } + if (smallest != i) + { + (queue[i], queue[smallest]) = (queue[smallest], queue[i]); + MinHeapify(smallest); + } + } + + public void Insert(T item) + { + queue.Add(item); + int ci = queue.Count - 1; // child index; start at end + while (ci > 0) + { + int pi = (ci - 1) / 2; // parent index + if (comparer(queue[ci], queue[pi]) >= 0) break; // child item is larger than (or equal) parent so we're done + (queue[ci], queue[pi]) = (queue[pi], queue[ci]); + ci = pi; + } + } + + public T Extract() + { + T item = queue[0]; + queue[0] = queue[queue.Count - 1]; + queue.RemoveAt(queue.Count - 1); + MinHeapify(0); + return item; + } + + public int Count => queue.Count; + + public T Peek() + { + return queue[0]; + } + + public bool Contains(T item) + { + return queue.Contains(item); + } + + public void Clear() + { + queue.Clear(); + } + + public bool IsEmpty() + { + return queue.Count == 0; + } + + +} + diff --git a/Assets/Scripts/Priorityqueue.cs.meta b/Assets/Scripts/Priorityqueue.cs.meta new file mode 100644 index 0000000..aa32bf9 --- /dev/null +++ b/Assets/Scripts/Priorityqueue.cs.meta @@ -0,0 +1,3 @@ +fileFormatVersion: 2 +guid: 7551f13ccb9d4bfab6c3c7c57aa77505 +timeCreated: 1648930596 \ No newline at end of file diff --git a/Assets/Sprites/Obstacles.png b/Assets/Sprites/Obstacles.png new file mode 100644 index 0000000000000000000000000000000000000000..7ee06cfa8b77417955ad1a1f7b2dde70a20d336f GIT binary patch literal 996 zcmeAS@N?(olHy`uVBq!ia0y~yU<5K5893O0R7}x|G!U;i$lZxy-8q?;3=GWkJY5_^ zD(1YsyD?KbQKBI+c0#oO0$rAOO(#|2lzbJIa5S1kI!%^%eC~m+O=Gn9bWWAV)}OWC z?fkU`zwl6MpX<6GK>udg8*%K3v%t@I`iIn4};3j>wkVBY1bvp&=On1RB`8y{=Rns&sZ1| znJ7T%+nMg)tBe2Q_ocnP-c0r};!tK{*yRsMYu3IW&06;IM!Fw^fEwMAS+88e%X@M8&n_BYiH)_d zz454%A=t(6-NNR1gCRdAj