KerelOlivier
3 years ago
10 changed files with 745 additions and 15 deletions
@ -0,0 +1,231 @@
@@ -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<Vector2Int> 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<Vector2Int> getNeighbours(Vector2Int pos) |
||||
{ |
||||
List<Vector2Int> neighbours = new List<Vector2Int>(); |
||||
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<Vector2> 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<Vector2Int, Vector2Int> cameFrom = new Dictionary<Vector2Int, Vector2Int>(); |
||||
|
||||
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<Vector2Int> path = Backtrack(cameFrom, current.Item1); |
||||
p = path; |
||||
return ConvertPath(path); |
||||
} |
||||
|
||||
inset[current.Item1.x, current.Item1.y] = true; |
||||
List<Vector2Int> 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<Vector2Int> Backtrack(Dictionary<Vector2Int, Vector2Int> cameFrom, Vector2Int current) |
||||
{ |
||||
LinkedList<Vector2Int> path = new LinkedList<Vector2Int>(); |
||||
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<Vector2> ConvertPath(LinkedList<Vector2Int> path) |
||||
{ |
||||
LinkedList<Vector2> converted = new LinkedList<Vector2>(); |
||||
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; |
||||
} |
||||
} |
||||
} |
@ -0,0 +1,11 @@
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2 |
||||
guid: a0ee750a6dc3f9e97b39747ae230044c |
||||
MonoImporter: |
||||
externalObjects: {} |
||||
serializedVersion: 2 |
||||
defaultReferences: [] |
||||
executionOrder: 0 |
||||
icon: {instanceID: 0} |
||||
userData: |
||||
assetBundleName: |
||||
assetBundleVariant: |
@ -0,0 +1,85 @@
@@ -0,0 +1,85 @@
|
||||
using System; |
||||
using System.Collections.Generic; |
||||
|
||||
public class PriorityQueue<T> |
||||
{ |
||||
private Func<T, T, int> comparer; |
||||
|
||||
public PriorityQueue(Func<T, T, int> comparer = null) |
||||
{ |
||||
if (comparer == null) |
||||
{ |
||||
comparer = (x, y) => Comparer<T>.Default.Compare(x, y); |
||||
} |
||||
this.comparer = comparer; |
||||
} |
||||
|
||||
private readonly List<T> queue = new List<T>(); |
||||
|
||||
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; |
||||
} |
||||
|
||||
|
||||
} |
||||
|
@ -0,0 +1,3 @@
@@ -0,0 +1,3 @@
|
||||
fileFormatVersion: 2 |
||||
guid: 7551f13ccb9d4bfab6c3c7c57aa77505 |
||||
timeCreated: 1648930596 |
After Width: | Height: | Size: 996 B |
@ -0,0 +1,240 @@
@@ -0,0 +1,240 @@
|
||||
fileFormatVersion: 2 |
||||
guid: 26aed7a8e043773feb7cc6bb749fdc3c |
||||
TextureImporter: |
||||
internalIDToNameTable: |
||||
- first: |
||||
213: 7788020138054186290 |
||||
second: Obstacles_1 |
||||
- first: |
||||
213: 3770364138508046682 |
||||
second: Obstacles_8 |
||||
- first: |
||||
213: 5054341098094839364 |
||||
second: Obstacles_10 |
||||
- first: |
||||
213: -4040938002640087940 |
||||
second: Obstacles_11 |
||||
- first: |
||||
213: 3023420621403531877 |
||||
second: Obstacles_12 |
||||
externalObjects: {} |
||||
serializedVersion: 11 |
||||
mipmaps: |
||||
mipMapMode: 0 |
||||
enableMipMap: 0 |
||||
sRGBTexture: 1 |
||||
linearTexture: 0 |
||||
fadeOut: 0 |
||||
borderMipMap: 0 |
||||
mipMapsPreserveCoverage: 0 |
||||
alphaTestReferenceValue: 0.5 |
||||
mipMapFadeDistanceStart: 1 |
||||
mipMapFadeDistanceEnd: 3 |
||||
bumpmap: |
||||
convertToNormalMap: 0 |
||||
externalNormalMap: 0 |
||||
heightScale: 0.25 |
||||
normalMapFilter: 0 |
||||
isReadable: 0 |
||||
streamingMipmaps: 0 |
||||
streamingMipmapsPriority: 0 |
||||
vTOnly: 0 |
||||
grayScaleToAlpha: 0 |
||||
generateCubemap: 6 |
||||
cubemapConvolution: 0 |
||||
seamlessCubemap: 0 |
||||
textureFormat: 1 |
||||
maxTextureSize: 2048 |
||||
textureSettings: |
||||
serializedVersion: 2 |
||||
filterMode: 0 |
||||
aniso: 1 |
||||
mipBias: 0 |
||||
wrapU: 1 |
||||
wrapV: 1 |
||||
wrapW: 1 |
||||
nPOTScale: 0 |
||||
lightmap: 0 |
||||
compressionQuality: 50 |
||||
spriteMode: 2 |
||||
spriteExtrude: 1 |
||||
spriteMeshType: 1 |
||||
alignment: 0 |
||||
spritePivot: {x: 0.5, y: 0.5} |
||||
spritePixelsToUnits: 64 |
||||
spriteBorder: {x: 0, y: 0, z: 0, w: 0} |
||||
spriteGenerateFallbackPhysicsShape: 1 |
||||
alphaUsage: 1 |
||||
alphaIsTransparency: 1 |
||||
spriteTessellationDetail: -1 |
||||
textureType: 8 |
||||
textureShape: 1 |
||||
singleChannelComponent: 0 |
||||
flipbookRows: 1 |
||||
flipbookColumns: 1 |
||||
maxTextureSizeSet: 0 |
||||
compressionQualitySet: 0 |
||||
textureFormatSet: 0 |
||||
ignorePngGamma: 0 |
||||
applyGammaDecoding: 0 |
||||
platformSettings: |
||||
- serializedVersion: 3 |
||||
buildTarget: DefaultTexturePlatform |
||||
maxTextureSize: 2048 |
||||
resizeAlgorithm: 0 |
||||
textureFormat: -1 |
||||
textureCompression: 1 |
||||
compressionQuality: 50 |
||||
crunchedCompression: 0 |
||||
allowsAlphaSplitting: 0 |
||||
overridden: 0 |
||||
androidETC2FallbackOverride: 0 |
||||
forceMaximumCompressionQuality_BC6H_BC7: 0 |
||||
- serializedVersion: 3 |
||||
buildTarget: Standalone |
||||
maxTextureSize: 2048 |
||||
resizeAlgorithm: 0 |
||||
textureFormat: -1 |
||||
textureCompression: 1 |
||||
compressionQuality: 50 |
||||
crunchedCompression: 0 |
||||
allowsAlphaSplitting: 0 |
||||
overridden: 0 |
||||
androidETC2FallbackOverride: 0 |
||||
forceMaximumCompressionQuality_BC6H_BC7: 0 |
||||
- serializedVersion: 3 |
||||
buildTarget: WebGL |
||||
maxTextureSize: 2048 |
||||
resizeAlgorithm: 0 |
||||
textureFormat: -1 |
||||
textureCompression: 1 |
||||
compressionQuality: 50 |
||||
crunchedCompression: 0 |
||||
allowsAlphaSplitting: 0 |
||||
overridden: 0 |
||||
androidETC2FallbackOverride: 0 |
||||
forceMaximumCompressionQuality_BC6H_BC7: 0 |
||||
spriteSheet: |
||||
serializedVersion: 2 |
||||
sprites: |
||||
- serializedVersion: 2 |
||||
name: Obstacles_1 |
||||
rect: |
||||
serializedVersion: 2 |
||||
x: 0 |
||||
y: 192 |
||||
width: 128 |
||||
height: 64 |
||||
alignment: 0 |
||||
pivot: {x: 0, y: 0} |
||||
border: {x: 0, y: 0, z: 0, w: 0} |
||||
outline: [] |
||||
physicsShape: [] |
||||
tessellationDetail: 0 |
||||
bones: [] |
||||
spriteID: 235fe488d0b941c60800000000000000 |
||||
internalID: 7788020138054186290 |
||||
vertices: [] |
||||
indices: |
||||
edges: [] |
||||
weights: [] |
||||
- serializedVersion: 2 |
||||
name: Obstacles_8 |
||||
rect: |
||||
serializedVersion: 2 |
||||
x: 0 |
||||
y: 64 |
||||
width: 64 |
||||
height: 128 |
||||
alignment: 0 |
||||
pivot: {x: 0, y: 0} |
||||
border: {x: 0, y: 0, z: 0, w: 0} |
||||
outline: [] |
||||
physicsShape: [] |
||||
tessellationDetail: 0 |
||||
bones: [] |
||||
spriteID: a51d4ad2536035430800000000000000 |
||||
internalID: 3770364138508046682 |
||||
vertices: [] |
||||
indices: |
||||
edges: [] |
||||
weights: [] |
||||
- serializedVersion: 2 |
||||
name: Obstacles_10 |
||||
rect: |
||||
serializedVersion: 2 |
||||
x: 64 |
||||
y: 64 |
||||
width: 128 |
||||
height: 128 |
||||
alignment: 0 |
||||
pivot: {x: 0, y: 0} |
||||
border: {x: 0, y: 0, z: 0, w: 0} |
||||
outline: [] |
||||
physicsShape: [] |
||||
tessellationDetail: 0 |
||||
bones: [] |
||||
spriteID: 446c99d1370a42640800000000000000 |
||||
internalID: 5054341098094839364 |
||||
vertices: [] |
||||
indices: |
||||
edges: [] |
||||
weights: [] |
||||
- serializedVersion: 2 |
||||
name: Obstacles_11 |
||||
rect: |
||||
serializedVersion: 2 |
||||
x: 192 |
||||
y: 64 |
||||
width: 64 |
||||
height: 64 |
||||
alignment: 0 |
||||
pivot: {x: 0, y: 0} |
||||
border: {x: 0, y: 0, z: 0, w: 0} |
||||
outline: [] |
||||
physicsShape: [] |
||||
tessellationDetail: 0 |
||||
bones: [] |
||||
spriteID: c74d241eb44bbe7c0800000000000000 |
||||
internalID: -4040938002640087940 |
||||
vertices: [] |
||||
indices: |
||||
edges: [] |
||||
weights: [] |
||||
- serializedVersion: 2 |
||||
name: Obstacles_12 |
||||
rect: |
||||
serializedVersion: 2 |
||||
x: 0 |
||||
y: 0 |
||||
width: 256 |
||||
height: 64 |
||||
alignment: 0 |
||||
pivot: {x: 0, y: 0} |
||||
border: {x: 0, y: 0, z: 0, w: 0} |
||||
outline: [] |
||||
physicsShape: [] |
||||
tessellationDetail: 0 |
||||
bones: [] |
||||
spriteID: 56a2951590955f920800000000000000 |
||||
internalID: 3023420621403531877 |
||||
vertices: [] |
||||
indices: |
||||
edges: [] |
||||
weights: [] |
||||
outline: [] |
||||
physicsShape: [] |
||||
bones: [] |
||||
spriteID: 5e97eb03825dee720800000000000000 |
||||
internalID: 0 |
||||
vertices: [] |
||||
indices: |
||||
edges: [] |
||||
weights: [] |
||||
secondaryTextures: [] |
||||
spritePackingTag: |
||||
pSDRemoveMatte: 0 |
||||
pSDShowRemoveMatteOption: 0 |
||||
userData: |
||||
assetBundleName: |
||||
assetBundleVariant: |
Loading…
Reference in new issue