private void Awake()
{
openSet = new PathfindingHeap <PathfinderNode>(MAXPATHHEAPSIZE);
activeNodes = new Dictionary <Point, PathfinderNode>(1000);
closedSet = new HashSet <PathfinderNode>();
instance = this;
}
public void TestBiggerMap()
{
var map = new Map();
var chunk1 = CreateChunk(0, 0);
var chunk2 = CreateChunk(1, 0);
var chunk3 = CreateChunk(-1, 0);
//chunk3.SetTile(15, 0, 2);
var chunk4 = CreateChunk(0, 1);
var chunk5 = CreateChunk(0, -1);
var chunk6 = CreateChunk(1, 1);
var chunk7 = CreateChunk(-1, -1);
var chunk8 = CreateChunk(-1, 1);
var chunk9 = CreateChunk(1, -1);
map.AddChunk(chunk1);
map.AddChunk(chunk2);
map.AddChunk(chunk3);
map.AddChunk(chunk4);
map.AddChunk(chunk5);
map.AddChunk(chunk6);
map.AddChunk(chunk7);
map.AddChunk(chunk8);
map.AddChunk(chunk9);
var passableMapArray = PathfinderHelper.GetPassableByteArray(new Position(0, 0), new Position(-1, 0), map.Chunks);
var path = Map.FindPath(new Position(0, 0), new Position(-1, 0), map.Chunks);
Assert.That(path.Count == 2);
Assert.That(path[0].X == 0 && path[0].Y == 0);
Assert.That(path[1].X == -1 && path[1].Y == 0);
}
/*
* Cannot implement coroutined pathfinding
* unless pinning poolable objects is
* implemented, due to excess mapnodes being
* recycled every lateUpdate();
*
*/
public static PathResult FindPath(PathRequest request)
{
return
(PathfinderHelper.FindPath(
request
));
}
public static List <Position> FindPath(Position start, Position goal, Dictionary <string, Chunk> chunks)
{
var passableMapResult = PathfinderHelper.GetPassableByteArray(start, goal, chunks);
var pathfinder = new PathFinderFast(passableMapResult.PassableMap);
var offsetedStart = new Position(start.X + (passableMapResult.OffsetX * 16), start.Y + (passableMapResult.OffsetY * 16));
var offsetedGoal = new Position(goal.X + (passableMapResult.OffsetX * 16), goal.Y + (passableMapResult.OffsetY * 16));
var result = pathfinder.FindPath(offsetedStart, offsetedGoal);
if (result == null)
{
return(null);
}
result.Reverse();
return(result.Select(node => new Position(node.X - (passableMapResult.OffsetX * 16), node.Y - (passableMapResult.OffsetY * 16))).ToList());
}
public void TestCrossingUnecessaryChunkStillGeneratesPassableArrayForHim()
{
var chunk1 = CreateChunk(0, 0);
chunk1.SetTile(0, 1, BLOCK);
chunk1.SetTile(1, 0, BLOCK);
var chunk2 = CreateChunk(0, -1);
_map.AddChunk(chunk1);
_map.AddChunk(chunk2);
var grid = PathfinderHelper.GetPassableByteArray(new Position(0, 0), new Position(0, 2), _map.Chunks, _map.IsPassable);
Assert.AreEqual(48, grid.PassableMap.GetLength(0));
Assert.AreEqual(48, grid.PassableMap.GetLength(1));
}
public void Play()
{
//Sanitize the input
OptionsController.instance.MapSize = Mathf.Max(mapSize.text == "" ? OptionsController.instance.defaultMapSize : int.Parse(mapSize.text), 2);
int MapSizeLocal = OptionsController.instance.MapSize;
OptionsController.instance.XStart = Mathf.Clamp(startX.text == "" ? OptionsController.instance.defaultXStart : int.Parse(startX.text), 0, OptionsController.instance.MapSize - 1);
OptionsController.instance.YStart = Mathf.Clamp(startY.text == "" ? OptionsController.instance.defaultYStart : int.Parse(startY.text), 0, OptionsController.instance.MapSize - 1);
OptionsController.instance.XEnd = Mathf.Clamp(endX.text == "" ? OptionsController.instance.defaultXEnd : int.Parse(endX.text), 0, OptionsController.instance.MapSize - 1);
OptionsController.instance.YEnd = Mathf.Clamp(endY.text == "" ? OptionsController.instance.defaultYEnd : int.Parse(endY.text), 0, OptionsController.instance.MapSize - 1);
int distanceFromStartToEnd = PathfinderHelper.ManhattanDistance(OptionsController.instance.XStart, OptionsController.instance.YStart, OptionsController.instance.XEnd, OptionsController.instance.YEnd);
OptionsController.instance.NumberOfObstacles = Mathf.Clamp(numberOfObstacles.text == "" ? OptionsController.instance.defaultNumberOfObstacles : int.Parse(numberOfObstacles.text), 0, MapSizeLocal * MapSizeLocal - distanceFromStartToEnd + 1);
//load the simulation scene
SceneManager.LoadScene("SimulationScene");
}
/// <summary>
/// Search if the path can be reached without collision, if not launches the search for the Astar
/// </summary>
/// <param name="start"></param>
/// <param name="end"></param>
/// <param name="map"></param>
/// <returns></returns>
public static Position[] FindPath(Position start, Position end, IMap map)
{
Position[] path = new Position[Math.Max(Math.Abs(start.X - end.X), Math.Abs(start.Y - end.Y))];
Position node = start;
short i = 0;
while (!node.Equals(end))
{
short x = (short)PathfinderHelper.NextCoord(node.X, end.X),
y = (short)PathfinderHelper.NextCoord(node.Y, end.Y);
if (!map.IsWalkable(x, y))
{
return(AStar.FindPath(start, end, map));
}
node = new Position(x, y);
path[i++] = node;
}
return(path);
}
public List <Position> FindPath(Position start, Position goal)
{
var passableMapResult = PathfinderHelper.GetPassableByteArray(start, goal, this.Chunks, IsPassable);
var pathfinder = new PathFinder(passableMapResult.PassableMap);
var offsetedStart = new Position(start.X + (passableMapResult.OffsetX * 16), start.Y + (passableMapResult.OffsetY * 16));
var offsetedGoal = new Position(goal.X + (passableMapResult.OffsetX * 16), goal.Y + (passableMapResult.OffsetY * 16));
var result = pathfinder.FindPath(offsetedStart, offsetedGoal);
if (result == null)
{
return(null);
}
List <Position> returned = new List <Position>();
foreach (var node in result)
{
returned.Add(new Position(node.X - (passableMapResult.OffsetX * 16), node.Y - (passableMapResult.OffsetY * 16)));
}
return(returned);
}
public int Compare(PathfinderNode x, PathfinderNode y)
{
return(-(PathfinderHelper.GetDistance(x.GetGridCoord(), location) -
PathfinderHelper.GetDistance(y.GetGridCoord(), location)));
}
/// <summary> /// Returns an Array with the neighbors of the given position /// </summary> /// <param name="pos"></param> /// <param name="map"></param> /// <returns></returns> public Position <short>[] GetNeighbors(Position <short> pos, IMap map) => PathfinderHelper.GetNeighbors(pos, map);
/// <summary>
/// Find a path using the AStar algorithm
/// </summary>
/// <param name="start"></param>
/// <param name="end"></param>
/// <param name="map"></param>
/// <returns></returns>
public static Position <short>[] FindPath(Position <short> start, Position <short> end, IMap map)
{
MiniHeap <Node> delNodes = new MiniHeap <Node>();
MiniHeap <Node> nodes = new MiniHeap <Node>();
nodes.Add(new Node {
Position = start
}, false);
while (nodes.Count() > 0)
{
Node node = nodes.Pop();
if (node.Position.Equals(end))
{
int i = node.Distance;
Position <short>[] path = new Position <short> [i];
while (i >= 1)
{
path[--i] = node.Position;
node = node.Parent;
}
return(path);
}
node.Closed = true;
foreach (Position <short> n in PathfinderHelper.GetNeighbors(node.Position, map))
{
if (n == null)
{
break;
}
Node neighbor = nodes.Get(no => no.Position.Equals(n)) ?? delNodes.Get(no => no.Position.Equals(n));
if (neighbor == null)
{
neighbor = new Node
{
Position = n,
G = node.G + (n.X == node.Position.X || n.Y == node.Position.Y ? 1 : 1.4),
H = Heuristics.Manhattan(Math.Abs(n.X - end.Y), Math.Abs(n.Y - end.Y)),
Parent = node,
Distance = node.Distance + 1
};
neighbor.F = neighbor.G + neighbor.H;
nodes.Add(neighbor);
}
else if (!neighbor.Closed)
{
double gScore = node.G + (n.X == node.Position.X || n.Y == node.Position.Y ? 1 : 1.4);
if (gScore > neighbor.G)
{
continue;
}
neighbor.G = gScore;
neighbor.F = neighbor.G + neighbor.H;
neighbor.Parent = node;
neighbor.Distance = node.Distance + 1;
}
}
delNodes.Add(node, false);
}
return(null);
}
private void GenerateMap()
{
//Generate map filled with blockers
List <Vector2Int> blockers = new List <Vector2Int>();
for (int i = 0; i < n; i++)
{
for (int j = 0; j < n; j++)
{
map[i, j] = TileType.Blocked;
blockers.Add(new Vector2Int(i, j));
}
}
//Here we "dig" through blocked, generating a random path that is always guided towards the end.
//This effectivelly removes minimum number of blockers to ensure the path exists
int StartEndDistance = PathfinderHelper.ManhattanDistance(StartX, StartY, EndX, EndY);
int xOffset = EndX - StartX;
int yOffset = EndY - StartY;
int DigX = 0;
int DigY = 0;
Random.InitState(System.DateTime.Now.Second);
map[StartX, StartY] = TileType.Free;
blockers.Remove(new Vector2Int(StartX, StartY));
for (int i = 0; i < StartEndDistance; i++)
{
if (Random.value < 0.5)
{
if (xOffset == 0)
{
if (yOffset > 0)
{
DigY++;
yOffset--;
}
if (yOffset < 0)
{
DigY--;
yOffset++;
}
}
else
{
if (xOffset > 0)
{
DigX++;
xOffset--;
}
if (xOffset < 0)
{
DigX--;
xOffset++;
}
}
}
else
{
if (yOffset == 0)
{
if (xOffset > 0)
{
DigX++;
xOffset--;
}
if (xOffset < 0)
{
DigX--;
xOffset++;
}
}
else
{
if (yOffset > 0)
{
DigY++;
yOffset--;
}
if (yOffset < 0)
{
DigY--;
yOffset++;
}
}
}
map[StartX + DigX, StartY + DigY] = TileType.Free;
blockers.Remove(new Vector2Int(StartX + DigX, StartY + DigY));
}
//Randomly remove blockers so that only specified amount is left
int numBlockersToRemove = n * n - OptionsController.instance.NumberOfObstacles - StartEndDistance - 1;
for (int i = 0; i < numBlockersToRemove; i++)
{
int indexToRemove = Random.Range(0, blockers.Count - 1);
Vector2Int elementToRemove = blockers[indexToRemove];
map[elementToRemove.x, elementToRemove.y] = TileType.Free;
blockers.RemoveAt(indexToRemove);
}
}
public void GreedyManhattanAlgorithm(Vector2Int start, Vector2Int end)
{
Queue <Vector2Int> frontier = new Queue <Vector2Int>();
frontier.Enqueue(start);
Dictionary <Vector2Int, Vector2Int> cameFrom = new Dictionary <Vector2Int, Vector2Int>();
cameFrom.Add(start, start);
List <Vector2Int> visited = new List <Vector2Int>();
while (frontier.Count != 0)
{
Vector2Int current = frontier.Dequeue();
visited.Add(current);
pathFollowed.Add(current);
List <Vector2Int> neighbours = GetNeighbours(current);
Vector2Int cameFromLocal;
cameFrom.TryGetValue(current, out cameFromLocal);
neighbours.Remove(cameFromLocal);
//Remove visited neighbours
List <Vector2Int> neighboursToRemove = new List <Vector2Int>();
foreach (Vector2Int neighbour in neighbours)
{
if (visited.Contains(neighbour))
{
neighboursToRemove.Add(neighbour);
}
}
foreach (Vector2Int neighbour in neighboursToRemove)
{
neighbours.Remove(neighbour);
FoundPath.Remove(current);
}
//Sort neighbours so the element closest to the end (Using Manhattan distance) is first
neighbours.Sort(delegate(Vector2Int a, Vector2Int b)
{
int aManhattan = PathfinderHelper.ManhattanDistance(a.x, a.y, end.x, end.y);
int bManhattan = PathfinderHelper.ManhattanDistance(b.x, b.y, end.x, end.y);
if (aManhattan > bManhattan)
{
return(1);
}
if (aManhattan < bManhattan)
{
return(-1);
}
return(0);
});
//If there is a neighbour, check him. Otherwise, backtrack
if (neighbours.Count > 0)
{
Vector2Int nextNeighbour = neighbours[0];
FoundPath.Add(current);
if (nextNeighbour == end)
{
pathFollowed.Add(nextNeighbour);
return;
}
else
{
frontier.Enqueue(nextNeighbour);
cameFrom.Add(nextNeighbour, current);
visited.Add(current);
}
}
else
{
frontier.Enqueue(cameFromLocal);
FoundPath.Remove(current);
}
}
}
public override void ProcessNode(
PathfinderNode currentNode,
PathfinderNode startNode,
PathfinderNode targetNode,
PathfindingHeap <PathfinderNode> openSet,
HashSet <PathfinderNode> closedSet,
Dictionary <Point, PathfinderNode> activeNodes,
Grid grid,
int maxPathLength
)
{
Vector3 currentLocation = grid.NodeToWorldCoord(currentNode.GetGridCoord());
List <PathfinderNode> neighbors = PathfinderHelper.GetNeighbors(
currentNode,
activeNodes,
currentNodeCreator
);
foreach (PathfinderNode neighbour in neighbors)
{
Vector3 neighbourLocation = grid.NodeToWorldCoord(neighbour.GetGridCoord());
if (!neighbour.IsWalkable() ||
closedSet.Contains(neighbour))
{
continue;
}
CostResult newStrategyCost =
currentCostStrategy.GetAdditionalCostAt(
currentLocation,
neighbourLocation
);
//neighbour.UpdateAccumulatedStrategyCost(newStrategyCost);
int newPhysicalGCost =
currentNode.GetPhysicalGCost()
+ PathfinderHelper.GetDistance(currentNode, neighbour);
int newStrategyGCost =
currentNode.GetStrategyGCost()
+ neighbour.GetExtractor().Extract(newStrategyCost);
int newMovementCostToNeighbour =
newPhysicalGCost + newStrategyGCost;
// bool smaller = newStrategyGCost < neighbour.GetStrategyGCost();
//if (smaller)
// {
//DrawGizmo.AddGizmo(Color.green, newStrategyGCost + " " + neighbour.GetStrategyGCost(), neighbour.GetLocation());
//}
//Debug.Log(neighbour.GetGCost());
if (newMovementCostToNeighbour < neighbour.GetGCost() || !openSet.Contains(neighbour))
{
//Debug.Log(neighbour.GetGCost());
//DrawGizmo.AddGizmo(Color.green, "" + currentNode.GetExtractor().Extract(newStrategyCost), neighbour.GetLocation());
neighbour.SetStrategyCost(
newStrategyCost
);
neighbour.SetStrategyGCost(
newStrategyGCost
);
neighbour.SetPhysicalGCost(newPhysicalGCost);
neighbour.SetHCost(GetDistance(neighbour, targetNode));
neighbour.SetParent(currentNode);
if (!openSet.Contains(neighbour) &&
neighbour.WithInRangeOfStart(maxPathLength)
)
{
openSet.Add(neighbour);
PathfinderVisualizer.Visit(neighbour);
/*DrawGizmo.AddGizmo(Color.grey, "", grid.NodeToWorldCoord(
* neighbour.GetGridCoord())
* );*/
}
else
{
openSet.UpdateItem(neighbour);
}
}
else
{
}
}
}
