-
Notifications
You must be signed in to change notification settings - Fork 46
/
GridSearch.cs
88 lines (76 loc) · 2.9 KB
/
GridSearch.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;
/// <summary>
/// Source https://github.com/lordjesus/Packt-Introduction-to-graph-algorithms-for-game-developers
/// </summary>
public class GridSearch {
public struct SearchResult
{
public List<Point> Path { get; set; }
}
public static List<Point> AStarSearch(Grid grid, Point startPosition, Point endPosition, bool isAgent = false)
{
List<Point> path = new List<Point>();
List<Point> positionsTocheck = new List<Point>();
Dictionary<Point, float> costDictionary = new Dictionary<Point, float>();
Dictionary<Point, float> priorityDictionary = new Dictionary<Point, float>();
Dictionary<Point, Point> parentsDictionary = new Dictionary<Point, Point>();
positionsTocheck.Add(startPosition);
priorityDictionary.Add(startPosition, 0);
costDictionary.Add(startPosition, 0);
parentsDictionary.Add(startPosition, null);
while (positionsTocheck.Count > 0)
{
Point current = GetClosestVertex(positionsTocheck, priorityDictionary);
positionsTocheck.Remove(current);
if (current.Equals(endPosition))
{
path = GeneratePath(parentsDictionary, current);
return path;
}
foreach (Point neighbour in grid.GetAdjacentCells(current, isAgent))
{
float newCost = costDictionary[current] + grid.GetCostOfEnteringCell(neighbour);
if (!costDictionary.ContainsKey(neighbour) || newCost < costDictionary[neighbour])
{
costDictionary[neighbour] = newCost;
float priority = newCost + ManhattanDiscance(endPosition, neighbour);
positionsTocheck.Add(neighbour);
priorityDictionary[neighbour] = priority;
parentsDictionary[neighbour] = current;
}
}
}
return path;
}
private static Point GetClosestVertex(List<Point> list, Dictionary<Point, float> distanceMap)
{
Point candidate = list[0];
foreach (Point vertex in list)
{
if (distanceMap[vertex] < distanceMap[candidate])
{
candidate = vertex;
}
}
return candidate;
}
private static float ManhattanDiscance(Point endPos, Point point)
{
return Math.Abs(endPos.X - point.X) + Math.Abs(endPos.Y - point.Y);
}
public static List<Point> GeneratePath(Dictionary<Point, Point> parentMap, Point endState)
{
List<Point> path = new List<Point>();
Point parent = endState;
while (parent != null && parentMap.ContainsKey(parent))
{
path.Add(parent);
parent = parentMap[parent];
}
return path;
}
}