public override void Generate()
{
pathfinderObject = new GameObject(GraphName);
pathfinderObject.transform.parent = GraphContainer;
pathfinderObject.transform.position = GraphPosition;
pathfinderObject.tag = GraphTag;
pathfinder = pathfinderObject.AddComponent<Pathfinder>();
pathfinder.ColorTraversable = ColorTraversable;
pathfinder.ColorOutOfReach = ColorOutOfReach;
pathfinder.ColorNeutral = ColorNeutral;
pathfindingNodes = new PathfindingNode[GraphWidth, GraphHeight];
for (int y = 0; y < GraphHeight; y++)
{
for (int x = 0; x < GraphWidth; x++)
{
pathfinderNodeObject = GameObject.Instantiate(NodePrefab) as GameObject;
pathfindingNode = pathfinderNodeObject.GetComponent<PathfindingNode>();
pathfindingNode.SetColor(pathfinder.ColorNeutral);
pathfinderNodeObject.name = "(" + x + ", " + y + ") " + NodePrefab.name;
pathfinderNodeObject.transform.parent = pathfinderObject.transform;
pathfinderNodeObject.transform.position = new Vector3(x, y, 0);
pathfindingNodes[x, y] = pathfindingNode;
}
}
pathfinder.SetNodes(pathfindingNodes);
}
void CreateGrid ()
{
grid = new PathfindingNode[gridSizeX, gridSizeY];
Vector3 worldBottomLeft = transform.position - (Vector3.right * gridWorldSize.x/2) - (Vector3.forward * gridWorldSize.y/2); // center - left - bottom [(0,0,0)-(-1,0,0)-(0,0,-1) = (-1,0,-1)]
for (int x = 0; x < gridSizeX; x++)
{
for (int y = 0; y < gridSizeY; y++)
{
//Vector3 worldPoint = worldBottomLeft + Vector3.right * (x * nodeDiameter /*+ nodeRadius*/) + Vector3.forward * (y * nodeDiameter /*+ nodeRadius*/); // bottome left + current x + current y, current x = x for iterator * node diameter for diatacne between+ node ra
Vector3 worldPoint = worldBottomLeft + new Vector3(x * nodeDiameter /*+ nodeRadius*/ , 0, y * nodeDiameter /*+ nodeRadius*/);
// allows for gridnodes to be based on unity centered coodinates <-- toggleable
if (gridOnNodeCenter)
{
//worldPoint = worldPoint + (Vector3.one * nodeRadius);
worldPoint.x += nodeRadius;
worldPoint.z += nodeRadius;
}
// may need something here to eliminate areas outside of grid from play (past walls
bool walkable = !(Physics.CheckSphere(worldPoint, nodeRadius, unwalkableMask));
grid[x,y] = new PathfindingNode(walkable, worldPoint, x, y);
}
}
}
public static List <Node> DijkstraNodes(Node startNode, Node endNode)
{
float starTime = Time.realtimeSinceStartup;
List <Node> waypoints = new List <Node>();
// open list, closed list
List <PathfindingNode> openList = new List <PathfindingNode>(), closedList = new List <PathfindingNode>();
Dictionary <Node, PathfindingNode> pathfindingNodes = new Dictionary <Node, PathfindingNode>();
pathfindingNodes.Add(startNode, new PathfindingNode(startNode));
openList.Add(pathfindingNodes[startNode]);
while (!DoesListContainNode(endNode, closedList) && openList.Count > 0)
{
openList.Sort();
PathfindingNode smallestCostSoFar = openList[0];
foreach (Node connectedNode in smallestCostSoFar.graphNode.connections.Keys)
{
if (smallestCostSoFar.graphNode.connections[connectedNode] != 0)
{
if (!DoesListContainNode(connectedNode, closedList))
{
if (!DoesListContainNode(connectedNode, openList))
{
float costToConnected = smallestCostSoFar.costSoFar + smallestCostSoFar.graphNode.connections[connectedNode] + Vector3.Distance(connectedNode.transform.position, endNode.transform.position);
PathfindingNode predecessor = smallestCostSoFar;
pathfindingNodes.Add(connectedNode, new PathfindingNode(connectedNode, costToConnected, predecessor));
openList.Add(pathfindingNodes[connectedNode]);
}
else
{
float currentCostToTarget = pathfindingNodes[connectedNode].costSoFar;
float costToTargetThroughCurrentNode = smallestCostSoFar.costSoFar + smallestCostSoFar.graphNode.connections[connectedNode];
if (costToTargetThroughCurrentNode < currentCostToTarget)
{
pathfindingNodes[connectedNode].costSoFar = costToTargetThroughCurrentNode;
pathfindingNodes[connectedNode].predecessor = smallestCostSoFar;
}
}
}
}
}
closedList.Add(smallestCostSoFar);
openList.Remove(smallestCostSoFar);
}
for (PathfindingNode waypoint = pathfindingNodes[endNode]; waypoint != null; waypoint = waypoint.predecessor)
{
waypoints.Add(waypoint.graphNode);
}
waypoints.Reverse();
//Debug.Log("Finding path took: " + (Time.realtimeSinceStartup - starTime) + "\n");
return(waypoints);
}
public static List <PathfindingNode> Find(PathfindingNode start, PathfindingNode end, PathfindingNode[,] map, int width, int height)
{
int x, y, cost = 0, step = 0;
map[end.x, end.y].cost = 0; // начало поиска с точки назначения
if (start.x - 1 >= 0)
{
if (map[start.x - 1, start.y].cost == -2)
{
step++;
}
}
else
{
step++;
}
if (start.y - 1 >= 0)
{
if (map[start.x, start.y - 1].cost == -2)
{
step++;
}
}
else
{
step++;
}
if (start.x + 1 < width)
{
if (map[start.x + 1, start.y].cost == -2)
{
step++;
}
}
else
{
step++;
}
if (start.y + 1 < height)
{
if (map[start.x, start.y + 1].cost == -2)
{
step++;
}
}
else
{
step++;
}
// проверка на доступность (например, юнит окружен)
if (step == 4)
{
return(null);
}
else
{
step = 0;
}
while (true) // цикл поиска
{
for (y = 0; y < height; y++)
{
for (x = 0; x < width; x++)
{
if (map[x, y].cost == step) // находим клетку, соответствующую текущему шагу
{
if (x - 1 >= 0) // если не выходим за границы массива
{
if (map[x - 1, y].cost == -1) // если клетка еще не проверялась
{
cost = step + 1; // сохраняем проходимость клетки
map[x - 1, y].cost = cost; // назначаем проходимость
}
}
if (y - 1 >= 0)
{
if (map[x, y - 1].cost == -1)
{
cost = step + 1;
map[x, y - 1].cost = cost;
}
}
if (x + 1 < width)
{
if (map[x + 1, y].cost == -1)
{
cost = step + 1;
map[x + 1, y].cost = cost;
}
}
if (y + 1 < height)
{
if (map[x, y + 1].cost == -1)
{
cost = step + 1;
map[x, y + 1].cost = cost;
}
}
}
}
}
step++; // следующий шаг/волна
if (map[start.x, start.y].cost != -1)
{
break; // если путь найден, выходим из цикла
}
if (step != cost || step > width * height)
{
return(null); // если путь найти невозможно, возврат
}
}
List <PathfindingNode> result = new List <PathfindingNode>(); // массив пути
// начало поиска со старта
x = start.x;
y = start.y;
step = map[x, y].cost; // определяем базовую проходимость
while (x != end.x || y != end.y) // прокладка пути
{
if (x - 1 >= 0 && y - 1 >= 0) // если не выходим за границы массива
{
if (map[x - 1, y - 1].cost >= 0) // если клетка проходима
{
if (map[x - 1, y - 1].cost < step) // если эта проходимость меньше, базовой проходимости
{
step = map[x - 1, y - 1].cost; // новая базовая проходимость
result.Add(map[x - 1, y - 1]); // добавляем клетку в массив пути
x--;
y--;
continue; // переходим на следующий цикл
}
}
}
if (y - 1 >= 0 && x + 1 < width)
{
if (map[x + 1, y - 1].cost >= 0)
{
if (map[x + 1, y - 1].cost < step)
{
step = map[x + 1, y - 1].cost;
result.Add(map[x + 1, y - 1]);
x++;
y--;
continue;
}
}
}
if (y + 1 < height && x + 1 < width)
{
if (map[x + 1, y + 1].cost >= 0)
{
if (map[x + 1, y + 1].cost < step)
{
step = map[x + 1, y + 1].cost;
result.Add(map[x + 1, y + 1]);
x++;
y++;
continue;
}
}
}
if (y + 1 < height && x - 1 >= 0)
{
if (map[x - 1, y + 1].cost >= 0)
{
if (map[x - 1, y + 1].cost < step)
{
step = map[x - 1, y + 1].cost;
result.Add(map[x - 1, y + 1]);
x--;
y++;
continue;
}
}
}
if (x - 1 >= 0)
{
if (map[x - 1, y].cost >= 0)
{
if (map[x - 1, y].cost < step)
{
step = map[x - 1, y].cost;
result.Add(map[x - 1, y]);
x--;
continue;
}
}
}
if (y - 1 >= 0)
{
if (map[x, y - 1].cost >= 0)
{
if (map[x, y - 1].cost < step)
{
step = map[x, y - 1].cost;
result.Add(map[x, y - 1]);
y--;
continue;
}
}
}
if (x + 1 < width)
{
if (map[x + 1, y].cost >= 0)
{
if (map[x + 1, y].cost < step)
{
step = map[x + 1, y].cost;
result.Add(map[x + 1, y]);
x++;
continue;
}
}
}
if (y + 1 < height)
{
if (map[x, y + 1].cost >= 0)
{
if (map[x, y + 1].cost < step)
{
step = map[x, y + 1].cost;
result.Add(map[x, y + 1]);
y++;
continue;
}
}
}
return(null); // текущая клетка не прошла проверку, ошибка пути, возврат
}
return(result); // возвращаем найденный маршрут
}
protected override async Task <Queue <TileRef> > Process()
{
// VERY similar to A*; main difference is with the neighbor tiles you look for jump nodes instead
if (_startNode == null ||
_endNode == null)
{
return(null);
}
// If we couldn't get a nearby node that's good enough
if (!PathfindingHelpers.TryEndNode(ref _endNode, _pathfindingArgs))
{
return(null);
}
var openTiles = new PriorityQueue <ValueTuple <float, PathfindingNode> >(new PathfindingComparer());
var gScores = new Dictionary <PathfindingNode, float>();
var cameFrom = new Dictionary <PathfindingNode, PathfindingNode>();
var closedTiles = new HashSet <PathfindingNode>();
#if DEBUG
var jumpNodes = new HashSet <PathfindingNode>();
#endif
PathfindingNode currentNode = null;
openTiles.Add((0, _startNode));
gScores[_startNode] = 0.0f;
var routeFound = false;
var count = 0;
while (openTiles.Count > 0)
{
count++;
// JPS probably getting a lot fewer nodes than A* is
if (count % 5 == 0 && count > 0)
{
await SuspendIfOutOfTime();
}
(_, currentNode) = openTiles.Take();
if (currentNode.Equals(_endNode))
{
routeFound = true;
break;
}
foreach (var node in currentNode.GetNeighbors())
{
var direction = PathfindingHelpers.RelativeDirection(node, currentNode);
var jumpNode = GetJumpPoint(currentNode, direction, _endNode);
if (jumpNode != null && !closedTiles.Contains(jumpNode))
{
closedTiles.Add(jumpNode);
#if DEBUG
jumpNodes.Add(jumpNode);
#endif
// GetJumpPoint should already check if we can traverse to the node
var tileCost = PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, jumpNode);
if (tileCost == null)
{
throw new InvalidOperationException();
}
var gScore = gScores[currentNode] + tileCost.Value;
if (gScores.TryGetValue(jumpNode, out var nextValue) && gScore >= nextValue)
{
continue;
}
cameFrom[jumpNode] = currentNode;
gScores[jumpNode] = gScore;
// pFactor is tie-breaker where the fscore is otherwise equal.
// See http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#breaking-ties
// There's other ways to do it but future consideration
var fScore = gScores[jumpNode] + PathfindingHelpers.OctileDistance(_endNode, jumpNode) * (1.0f + 1.0f / 1000.0f);
openTiles.Add((fScore, jumpNode));
}
}
}
if (!routeFound)
{
return(null);
}
var route = PathfindingHelpers.ReconstructJumpPath(cameFrom, currentNode);
if (route.Count == 1)
{
return(null);
}
#if DEBUG
// Need to get data into an easier format to send to the relevant clients
if (DebugRoute != null && route.Count > 0)
{
var debugJumpNodes = new HashSet <TileRef>(jumpNodes.Count);
foreach (var node in jumpNodes)
{
debugJumpNodes.Add(node.TileRef);
}
var debugRoute = new SharedAiDebug.JpsRouteDebug(
_pathfindingArgs.Uid,
route,
debugJumpNodes,
DebugTime);
DebugRoute.Invoke(debugRoute);
}
#endif
return(route);
}
private PathfindingNode GetJumpPoint(PathfindingNode currentNode, Direction direction, PathfindingNode endNode)
{
var count = 0;
while (count < 1000)
{
count++;
PathfindingNode nextNode = null;
foreach (var node in currentNode.GetNeighbors())
{
if (PathfindingHelpers.RelativeDirection(node, currentNode) == direction)
{
nextNode = node;
break;
}
}
// We'll do opposite DirectionTraversable just because of how the method's setup
// Nodes should be 2-way anyway.
if (nextNode == null ||
PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, nextNode) == null)
{
return(null);
}
if (nextNode == endNode)
{
return(endNode);
}
// Horizontal and vertical are treated the same i.e.
// They only check in their specific direction
// (So Going North means you check NorthWest and NorthEast to see if we're a jump point)
// Diagonals also check the cardinal directions at the same time at the same time
// See https://harablog.wordpress.com/2011/09/07/jump-point-search/ for original description
switch (direction)
{
case Direction.East:
if (IsCardinalJumpPoint(direction, nextNode))
{
return(nextNode);
}
break;
case Direction.NorthEast:
if (IsDiagonalJumpPoint(direction, nextNode))
{
return(nextNode);
}
if (GetJumpPoint(nextNode, Direction.North, endNode) != null || GetJumpPoint(nextNode, Direction.East, endNode) != null)
{
return(nextNode);
}
break;
case Direction.North:
if (IsCardinalJumpPoint(direction, nextNode))
{
return(nextNode);
}
break;
case Direction.NorthWest:
if (IsDiagonalJumpPoint(direction, nextNode))
{
return(nextNode);
}
if (GetJumpPoint(nextNode, Direction.North, endNode) != null || GetJumpPoint(nextNode, Direction.West, endNode) != null)
{
return(nextNode);
}
break;
case Direction.West:
if (IsCardinalJumpPoint(direction, nextNode))
{
return(nextNode);
}
break;
case Direction.SouthWest:
if (IsDiagonalJumpPoint(direction, nextNode))
{
return(nextNode);
}
if (GetJumpPoint(nextNode, Direction.South, endNode) != null || GetJumpPoint(nextNode, Direction.West, endNode) != null)
{
return(nextNode);
}
break;
case Direction.South:
if (IsCardinalJumpPoint(direction, nextNode))
{
return(nextNode);
}
break;
case Direction.SouthEast:
if (IsDiagonalJumpPoint(direction, nextNode))
{
return(nextNode);
}
if (GetJumpPoint(nextNode, Direction.South, endNode) != null || GetJumpPoint(nextNode, Direction.East, endNode) != null)
{
return(nextNode);
}
break;
default:
throw new ArgumentOutOfRangeException(nameof(direction), direction, null);
}
currentNode = nextNode;
}
Logger.WarningS("pathfinding", "Recursion found in JPS pathfinder");
return(null);
}
void GenerateLevel(int size)
{
size = Mathf.Clamp(size, minLevelSize, maxLevelSize);
if (!gameCamera)
{
gameCamera = Camera.main;
}
if (gameCamera)
{
gameCamera.orthographicSize = size / 2f * 1920 / 1080 / Screen.width * Screen.height;
}
startPoint = Vector2.one * levelSize / 2 * -1;
levelNodes = new PathfindingNode[size, size];
//create playing field
for (int vert = 0; vert < size; vert++)
{
for (int hor = 0; hor < size; hor++)
{
GameObject.Instantiate(groundTilePrefab, new Vector3(hor, vert, 0), new Quaternion(), groundParentTr);
levelNodes[hor, vert] = new PathfindingNode(true, hor, vert);
}
}
// create maze
for (int vert = -1; vert <= size; vert++)
{
// add left and right level border
GameObject.Instantiate(wallTilePrefab, new Vector3(-1, vert, 0), new Quaternion(), groundParentTr);
GameObject.Instantiate(wallTilePrefab, new Vector3(size, vert, 0), new Quaternion(), groundParentTr);
int predefinedHole = Random.Range(0, size - 1);
for (int hor = 0; hor < size; hor++)
{
// full bottom and top border
if (vert == -1 || vert == size)
{
GameObject.Instantiate(wallTilePrefab, new Vector3(hor, vert, 0), new Quaternion(), groundParentTr);
continue;
}
// every second empty
if (vert % 2 == 0)
{
continue;
}
// ensure there is a hole in every wall
if (predefinedHole == hor)
{
continue;
}
// generate random walls
//todo implement here random destructable walls
if (Random.Range(0, 100) < levelDensity)
{
GameObject.Instantiate(wallTilePrefab, new Vector3(hor, vert, 0), new Quaternion(), groundParentTr);
levelNodes[hor, vert].walkable = false;
}
}
}
// place base & player
int baseLocation = Random.Range(0, size - 1);
baseGO.transform.localPosition = new Vector3(baseLocation, 0, 0);
playerGO.transform.localPosition = new Vector3(baseLocation, 0, 0);
// place enemies
enemiesGos = new List <GameObject>();
int ver = size; // upper half of level only
for (int iter = 0; iter < enemyCount; iter++)
{
ver--;
int hor = Random.Range(0, size - 1);
int startHor = hor;
while (levelNodes[hor, ver].walkable == false)
{
// find empty space for enemy
hor++;
if (hor == size)
{
hor = 0;
}
if (hor == startHor)
{
continue;
}
}
bool randAlgorithm = Random.Range(0, 100) < 50;
// spawn enemy
enemiesGos.Add(GameObject.Instantiate(randAlgorithm ? enemyPrefabs[1] : enemyPrefabs[0],
new Vector3(hor, ver, 0), new Quaternion(), groundParentTr));
// start enemy pathfinding
EnemyAI tempAi = enemiesGos[iter].GetComponent <EnemyAI>();
tempAi.StartPathfinding(randAlgorithm, iter, levelNodes[hor, ver], GetNode(baseGO.transform));
}
// center playing field
groundParentTr.position = new Vector3(startPoint.x, startPoint.y, 0);
}
public static List <Vector3> Pathfind(TileGraph graph, Node fromNode, Node toNode)
{
List <Vector3> waypoints = new List <Vector3>();
//TODO : implement Dijkstra
List <PathfindingNode> openList = new List <PathfindingNode>();
List <PathfindingNode> closedList = new List <PathfindingNode>();
Dictionary <Node, PathfindingNode> pathfindingNodes = new Dictionary <Node, PathfindingNode>();
pathfindingNodes.Add(fromNode, new PathfindingNode(fromNode));
openList.Add(pathfindingNodes[fromNode]);
while (openList.Count > 0 && !DoesListContainNode(toNode, closedList))
{
//TODO find connections from the lowest cost so far point to all connected points
// How do I know what the lowest cost so far is?
openList.Sort();
PathfindingNode smallestCostSoFar = openList[0];
// How do we get connections?
foreach (Node connectedNode in smallestCostSoFar.graphNode.connections.Keys)
{
if (!DoesListContainNode(connectedNode, closedList))
{
if (!DoesListContainNode(connectedNode, openList))
{
float costToConnectedNode = smallestCostSoFar.costSoFar + smallestCostSoFar.graphNode.connections[connectedNode];
PathfindingNode predecessor = smallestCostSoFar;
pathfindingNodes.Add(connectedNode, new PathfindingNode(connectedNode, costToConnectedNode, predecessor));
openList.Add(pathfindingNodes[connectedNode]);
}
else
{
// Is my connection from the current processing node faster than the existing connectino to this node?
// If so, update it.
float currentCostToTarget = pathfindingNodes[connectedNode].costSoFar;
float costToTargetThroughCurrentNode = smallestCostSoFar.costSoFar + smallestCostSoFar.graphNode.connections[connectedNode];
if (costToTargetThroughCurrentNode < currentCostToTarget)
{
pathfindingNodes[connectedNode].costSoFar = costToTargetThroughCurrentNode;
pathfindingNodes[connectedNode].predecessor = smallestCostSoFar;
}
}
}
}
closedList.Add(smallestCostSoFar);
openList.Remove(smallestCostSoFar);
}//end of while loop - pathfinding complete
//TODO fill out waypoints
//Destination node is on closed list
//All of its predecessors are on the closed list
for (PathfindingNode waypoint = pathfindingNodes[toNode]; waypoint != null; waypoint = waypoint.predecessor)
{
waypoints.Add(waypoint.graphNode.pos);
Debug.Log(waypoint.graphNode.pos);
}
waypoints.Reverse();
return(waypoints);
}
// Gets tiles adjacent to another on this map.
// For now includes only the 4 tiles around it but could theoretically include portal jumps
public List <PathfindingNode> GetAdjacentTiles(PathfindingNode origin, bool[,] passabilityMask, bool[,] seenBefore)
{
List <PathfindingNode> ret = new List <PathfindingNode>();
if (origin.x - 1 >= 0)
{
int newX = origin.x - 1;
int newY = origin.y;
if (!seenBefore[newY, newX] && passabilityMask[newY, newX])
{
PathfindingNode neighbor = new PathfindingNode
{
x = newX,
y = newY,
origin = origin,
cost = origin.cost + 1, // Pathfinding cost is 1 due to the tiled nature of the space
heuristic = 0
};
ret.Add(neighbor);
}
}
if (origin.y - 1 >= 0)
{
int newX = origin.x;
int newY = origin.y - 1;
if (!seenBefore[newY, newX] && passabilityMask[newY, newX])
{
PathfindingNode neighbor = new PathfindingNode
{
x = newX,
y = newY,
origin = origin,
cost = origin.cost + 1, // Pathfinding cost is 1 due to the tiled nature of the space
heuristic = 0
};
ret.Add(neighbor);
}
}
if (origin.x + 1 < map.width)
{
int newX = origin.x + 1;
int newY = origin.y;
if (!seenBefore[newY, newX] && passabilityMask[newY, newX])
{
PathfindingNode neighbor = new PathfindingNode
{
x = newX,
y = newY,
origin = origin,
cost = origin.cost + 1, // Pathfinding cost is 1 due to the tiled nature of the space
heuristic = 0
};
ret.Add(neighbor);
}
}
if (origin.y + 1 < map.height)
{
int newX = origin.x;
int newY = origin.y + 1;
if (!seenBefore[newY, newX] && passabilityMask[newY, newX])
{
PathfindingNode neighbor = new PathfindingNode
{
x = newX,
y = newY,
origin = origin,
cost = origin.cost + 1, // Pathfinding cost is 1 due to the tiled nature of the space
heuristic = 0
};
ret.Add(neighbor);
}
}
return(ret);
}
// Main pathfinder. Uses A*. Could get faster but due to the small size of the maps, it is not a high priority.
public List <BattleMapTile> GetPath(int startX, int startY, int endX, int endY, bool[,] passabilityMask)
{
List <BattleMapTile> path = new List <BattleMapTile>();
// Mask to know which tiles we have already checked
bool[,] seenBefore = new bool[map.height, map.width];
for (int x = 0; x < map.width; x++)
{
for (int y = 0; y < map.height; y++)
{
seenBefore[y, x] = false;
}
}
// A* for getting a specific path
// Define the base node
PathfindingNode start = new PathfindingNode
{
x = startX,
y = startY,
origin = null,
cost = 0,
heuristic = 0
};
start.heuristic = CalculateHeuristic(start, endX, endY);
// Set the start position as seen
seenBefore[startY, startX] = true;
// TODO: This is a grossly inefficient way of handling this. Should use a proper min heap based priority queue instead
List <PathfindingNode> pool = new List <PathfindingNode>();
PathfindingNodeComparer comparer = new PathfindingNodeComparer();
pool.Add(start);
PathfindingNode current = null;
bool foundPath = false;
// Main loop
while (pool.Count > 0)
{
pool.Sort(comparer);
current = pool[0];
pool.RemoveAt(0);
List <PathfindingNode> adj = GetAdjacentTiles(current, passabilityMask, seenBefore);
foreach (PathfindingNode node in adj)
{
// If we found the path, termiate early
if (node.x == endX && node.y == endY)
{
foundPath = true;
current = node;
break;
}
else
{
node.heuristic = CalculateHeuristic(node, endX, endY);
pool.Add(node);
seenBefore[node.y, node.x] = true;
}
}
if (foundPath)
{
break;
}
}
if (foundPath)
{
while (current.origin != null)
{
path.Add(map.tiles[current.y][current.x]);
current = current.origin;
}
path.Reverse();
}
return(path);
}
private float CalculateHeuristic(PathfindingNode node, int endX, int endY)
{
return(node.cost + Mathf.Abs(endX - node.x) + Mathf.Abs(endY - node.y));
}
bool SetCoverTargetFromPlayerLastKnownPosition()
{
Vector2 bestCoverPoint;
List <PathfindingNode> potentialCoverNodes = new List <PathfindingNode>();
HashSet <Vector2> inProcessQueue = new HashSet <Vector2>();
Queue <PathfindingNode> nodesToProcess = new Queue <PathfindingNode>();
grid.Reset();
PathfindingNode node = grid.NodeAtWorldPosition(rigidbody.position);
node.Parent = null;
nodesToProcess.Enqueue(node);
inProcessQueue.Add(node.WorldPosition);
while (nodesToProcess.Count > 0 && Vector2.Distance(rigidbody.position, node.WorldPosition) < maxCoverSearchDistance)
{
node = nodesToProcess.Dequeue();
Quaternion left = Quaternion.AngleAxis(-7, Vector3.forward);
Quaternion right = Quaternion.AngleAxis(7, Vector3.forward);
RaycastHit2D leftRaycastHit = Physics2D.Linecast(playerLastKnownPosition, left * node.WorldPosition, sightBlockMask);
RaycastHit2D rightRaycastHit = Physics2D.Linecast(playerLastKnownPosition, right * node.WorldPosition, sightBlockMask);
if (leftRaycastHit && rightRaycastHit)
{
potentialCoverNodes.Add(node);
}
foreach (PathfindingNode neighbor in grid.GetNeighbors(node))
{
if (!inProcessQueue.Contains(neighbor.WorldPosition))
{
if (neighbor.IsWalkable)
{
neighbor.Parent = node;
nodesToProcess.Enqueue(neighbor);
inProcessQueue.Add(neighbor.WorldPosition);
}
}
}
}
float nodeDistance = 0;
float smallestNodeDistance = float.MaxValue;
PathfindingNode currNode;
bestCoverPoint = NullVector;
foreach (PathfindingNode coverNode in potentialCoverNodes)
{
currNode = coverNode;
nodeDistance = 0;
while (currNode != null)
{
if (currNode.Parent != null)
{
nodeDistance += Vector2.Distance(currNode.WorldPosition, currNode.Parent.WorldPosition);
}
currNode = currNode.Parent;
}
if (nodeDistance < 5)
{
//print("NodePos: " + coverNode.WorldPosition + ", NodeDist: " + nodeDistance);
}
if (nodeDistance < smallestNodeDistance)
{
smallestNodeDistance = nodeDistance;
bestCoverPoint = coverNode.WorldPosition;
}
}
if (bestCoverPoint != NullVector)
{
coverDistance = smallestNodeDistance;
coverTarget = bestCoverPoint;
//print("BEST NodePos: " + coverTarget + ", NodeDist: " + coverDistance);
Quaternion left = Quaternion.AngleAxis(-sightAngle, Vector3.forward);
Quaternion right = Quaternion.AngleAxis(sightAngle, Vector3.forward);
leftP = left * coverTarget;
rightP = right * coverTarget;
nodeP = coverTarget;
return(true);
}
return(false);
}
protected override async Task <Queue <TileRef> > Process()
{
if (_startNode == null ||
_endNode == null ||
Status == JobStatus.Finished)
{
return(null);
}
// If we couldn't get a nearby node that's good enough
if (!PathfindingHelpers.TryEndNode(ref _endNode, _pathfindingArgs))
{
return(null);
}
var frontier = new PriorityQueue <ValueTuple <float, PathfindingNode> >(new PathfindingComparer());
var costSoFar = new Dictionary <PathfindingNode, float>();
var cameFrom = new Dictionary <PathfindingNode, PathfindingNode>();
PathfindingNode currentNode = null;
frontier.Add((0.0f, _startNode));
costSoFar[_startNode] = 0.0f;
var routeFound = false;
var count = 0;
while (frontier.Count > 0)
{
// Handle whether we need to pause if we've taken too long
count++;
if (count % 20 == 0 && count > 0)
{
await SuspendIfOutOfTime();
if (_startNode == null || _endNode == null)
{
return(null);
}
}
// Actual pathfinding here
(_, currentNode) = frontier.Take();
if (currentNode.Equals(_endNode))
{
routeFound = true;
break;
}
foreach (var nextNode in currentNode.GetNeighbors())
{
// If tile is untraversable it'll be null
var tileCost = PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, nextNode);
if (tileCost == null)
{
continue;
}
// So if we're going NE then that means either N or E needs to be free to actually get there
var direction = PathfindingHelpers.RelativeDirection(nextNode, currentNode);
if (!PathfindingHelpers.DirectionTraversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, currentNode, direction))
{
continue;
}
// f = g + h
// gScore is distance to the start node
// hScore is distance to the end node
var gScore = costSoFar[currentNode] + tileCost.Value;
if (costSoFar.TryGetValue(nextNode, out var nextValue) && gScore >= nextValue)
{
continue;
}
cameFrom[nextNode] = currentNode;
costSoFar[nextNode] = gScore;
// pFactor is tie-breaker where the fscore is otherwise equal.
// See http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#breaking-ties
// There's other ways to do it but future consideration
// The closer the fScore is to the actual distance then the better the pathfinder will be
// (i.e. somewhere between 1 and infinite)
// Can use hierarchical pathfinder or whatever to improve the heuristic but this is fine for now.
var fScore = gScore + PathfindingHelpers.OctileDistance(_endNode, nextNode) * (1.0f + 1.0f / 1000.0f);
frontier.Add((fScore, nextNode));
}
}
if (!routeFound)
{
return(null);
}
var route = PathfindingHelpers.ReconstructPath(cameFrom, currentNode);
if (route.Count == 1)
{
return(null);
}
#if DEBUG
// Need to get data into an easier format to send to the relevant clients
if (DebugRoute != null && route.Count > 0)
{
var debugCameFrom = new Dictionary <TileRef, TileRef>(cameFrom.Count);
var debugGScores = new Dictionary <TileRef, float>(costSoFar.Count);
foreach (var(node, parent) in cameFrom)
{
debugCameFrom.Add(node.TileRef, parent.TileRef);
}
foreach (var(node, score) in costSoFar)
{
debugGScores.Add(node.TileRef, score);
}
var debugRoute = new SharedAiDebug.AStarRouteDebug(
_pathfindingArgs.Uid,
route,
debugCameFrom,
debugGScores,
DebugTime);
DebugRoute.Invoke(debugRoute);
}
#endif
return(route);
}
public override List <PathfindingNode> FindPath(LevelGenerator tempLevelNodes, PathfindingNode tempStartNode, PathfindingNode tempTargetNode)
{
StartPathfindingSteps(tempLevelNodes, tempStartNode, tempTargetNode);
while (openSet.Count > 0)
{
PathfindingNode node = openSet[0];
for (int i = 1; i < openSet.Count; i++)
{
if (openSet[i].fCost < node.fCost || openSet[i].fCost == node.fCost)
{
if (openSet[i].hCost < node.hCost)
{
node = openSet[i];
}
}
}
openSet.Remove(node);
closedSet.Add(node);
if (node == targetNode)
{
return(RetracePath(startNode, targetNode));
}
foreach (PathfindingNode neighbour in GetNeighbours(node))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
int newCostToNeighbour = node.gCost + GetDistance(node, neighbour);
if (newCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, targetNode);
neighbour.parent = node;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
}
}
}
return(null);
}
// this uses Dijkstra's Algorithm to get all the spaces a player can visit.
public static List <PathfindingNode> Dijkstras(SpaceModel startSpace, int maxCost, bool ignoreTerrain)
{
List <PathfindingNode> allNodes = new List <PathfindingNode>();
//List<SpaceModel> shortestPath = new List<SpaceModel>
//{
// startSpace
//};
PathfindingNode currentnode = new PathfindingNode(startSpace, null, 0, 0, true, null);
allNodes.Add(currentnode);
bool done = false;
while (!done)
{
foreach (SpaceModel adjacentSpace in currentnode.GetSpace().GetAdjacentSpaces())
{
if (adjacentSpace != null)
{
PathfindingNode nextNode = adjacentSpace.GetNode();
if (nextNode != null)
{
// not null
if (!nextNode.BeenSeen())
{
// Next node hasn't been visited yet
if (ignoreTerrain)
{
nextNode.Update(currentnode.GetCost() + 1, currentnode.GetCost() + 1, currentnode);
}
else
{
int adjacentSpaceCost = currentnode.GetCost() + adjacentSpace.GetMovementCost();
double adjacentSpacePathfindingCost = adjacentSpaceCost;
if (adjacentSpace.IsHazardous())
{
adjacentSpacePathfindingCost += 0.001;
}
nextNode.Update(adjacentSpaceCost, adjacentSpacePathfindingCost, currentnode);
}
}
}
else
{
// Is null, need new node
int newNodeCost;
double pathfindingCost;
if (ignoreTerrain)
{
newNodeCost = currentnode.GetCost() + 1;
pathfindingCost = newNodeCost;
}
else
{
newNodeCost = currentnode.GetCost() + adjacentSpace.GetMovementCost();
pathfindingCost = newNodeCost;
if (adjacentSpace.IsHazardous())
{
pathfindingCost = newNodeCost + 0.001;
}
if (adjacentSpace.Occupied())
{
pathfindingCost += 100;
}
}
if (newNodeCost <= maxCost)
{
PathfindingNode newNode = new PathfindingNode(adjacentSpace, currentnode, newNodeCost, pathfindingCost, false, null);
allNodes.Add(newNode);
adjacentSpace.SetNode(newNode);
}
}
}
}
PathfindingNode lowestNode = null;
foreach (PathfindingNode node in allNodes)
{
if (!node.BeenSeen())
{
if (lowestNode == null)
{
lowestNode = node;
}
else
{
if (node.GetPathfindingCost() < lowestNode.GetPathfindingCost())
{
lowestNode = node;
}
}
}
}
if (lowestNode == null)
{
done = true;
}
else
{
lowestNode.Seen();
currentnode = lowestNode;
}
}
foreach (PathfindingNode node in allNodes)
{
node.GetSpace().SetNode(null);
}
return(allNodes);
}
List<Vector3> RetracePath(PathfindingNode startNode, PathfindingNode endNode)
{
List<PathfindingNode> path = new List<PathfindingNode>();
PathfindingNode currentNode = endNode;
while (currentNode != startNode)
{
path.Add(currentNode);
currentNode = currentNode.parent;
}
List<Vector3> waypoints = SimplifyPath(path);
//List<Vector3> waypoints = FullPath(path);
//Array.Reverse(waypoints);
waypoints.Reverse();
return waypoints;
//path.Reverse();
//return path;
}
public abstract List <PathfindingNode> FindPath(LevelGenerator tempLevelNodes, PathfindingNode tempStartNode,
PathfindingNode tempTargetNode);
public Vector3 GetWorldPos(PathfindingNode node)
{
return(new Vector3(node.gridX, node.gridY, 0f));
}
public void StartPathfindingSteps(LevelGenerator tempLevelNodes, PathfindingNode tempStartNode, PathfindingNode tempTargetNode)
{
LevelPathfindingNodes = new PathfindingNode[tempLevelNodes.LevelSize, tempLevelNodes.LevelSize];
LevelPathfindingNodes = tempLevelNodes.CopyLevelNodes();
startNode = LevelPathfindingNodes[tempStartNode.gridX, tempStartNode.gridY];
targetNode = LevelPathfindingNodes[tempTargetNode.gridX, tempTargetNode.gridY];
openSet = new List <PathfindingNode>();
closedSet = new List <PathfindingNode>();
openSet.Add(startNode);
pathfindingActive = true;
}
public void MarkPassible(int x, int y)
{
nodes[x, y] = new PathfindingNode(cellSize, new Rect(x * cellSize.x, y * cellSize.y, cellSize.x, cellSize.y));
}
public bool IsNeighboor(PathfindingNode node)
{
return((Math.Abs(node.X - X) + Math.Abs(node.Y - Y)) == 1);
}
private bool IsDiagonalJumpPoint(Direction direction, PathfindingNode currentNode)
{
// If we're going diagonally need to check all cardinals.
// I tried just casting direction ints and offsets to make it smaller but brain no worky.
// From NorthEast we check (Closed / Open) S - SE, W - NW
PathfindingNode openNeighborOne = null;
PathfindingNode closedNeighborOne = null;
PathfindingNode openNeighborTwo = null;
PathfindingNode closedNeighborTwo = null;
switch (direction)
{
case Direction.NorthEast:
foreach (var neighbor in currentNode.GetNeighbors())
{
var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode);
switch (neighborDirection)
{
case Direction.SouthEast:
openNeighborOne = neighbor;
break;
case Direction.South:
closedNeighborOne = neighbor;
break;
case Direction.NorthWest:
openNeighborTwo = neighbor;
break;
case Direction.West:
closedNeighborTwo = neighbor;
break;
}
}
break;
case Direction.SouthEast:
foreach (var neighbor in currentNode.GetNeighbors())
{
var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode);
switch (neighborDirection)
{
case Direction.NorthEast:
openNeighborOne = neighbor;
break;
case Direction.North:
closedNeighborOne = neighbor;
break;
case Direction.SouthWest:
openNeighborTwo = neighbor;
break;
case Direction.West:
closedNeighborTwo = neighbor;
break;
}
}
break;
case Direction.SouthWest:
foreach (var neighbor in currentNode.GetNeighbors())
{
var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode);
switch (neighborDirection)
{
case Direction.NorthWest:
openNeighborOne = neighbor;
break;
case Direction.North:
closedNeighborOne = neighbor;
break;
case Direction.SouthEast:
openNeighborTwo = neighbor;
break;
case Direction.East:
closedNeighborTwo = neighbor;
break;
}
}
break;
case Direction.NorthWest:
foreach (var neighbor in currentNode.GetNeighbors())
{
var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode);
switch (neighborDirection)
{
case Direction.SouthWest:
openNeighborOne = neighbor;
break;
case Direction.South:
closedNeighborOne = neighbor;
break;
case Direction.NorthEast:
openNeighborTwo = neighbor;
break;
case Direction.East:
closedNeighborTwo = neighbor;
break;
}
}
break;
default:
throw new ArgumentOutOfRangeException();
}
if ((closedNeighborOne == null || PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, closedNeighborOne) == null) &&
openNeighborOne != null && PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, openNeighborOne) != null)
{
return(true);
}
if ((closedNeighborTwo == null || PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, closedNeighborTwo) == null) &&
openNeighborTwo != null && PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, openNeighborTwo) != null)
{
return(true);
}
return(false);
}
public Vector3Int To(PathfindingNode node)
{
return(new Vector3Int(node.X - X, node.Y - Y, 0));
}
/// <summary>
/// Check to see if the node is a jump point (only works for cardinal directions)
/// </summary>
private bool IsCardinalJumpPoint(Direction direction, PathfindingNode currentNode)
{
PathfindingNode openNeighborOne = null;
PathfindingNode closedNeighborOne = null;
PathfindingNode openNeighborTwo = null;
PathfindingNode closedNeighborTwo = null;
switch (direction)
{
case Direction.North:
foreach (var neighbor in currentNode.GetNeighbors())
{
var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode);
switch (neighborDirection)
{
case Direction.NorthEast:
openNeighborOne = neighbor;
break;
case Direction.East:
closedNeighborOne = neighbor;
break;
case Direction.NorthWest:
openNeighborTwo = neighbor;
break;
case Direction.West:
closedNeighborTwo = neighbor;
break;
}
}
break;
case Direction.East:
foreach (var neighbor in currentNode.GetNeighbors())
{
var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode);
switch (neighborDirection)
{
case Direction.NorthEast:
openNeighborOne = neighbor;
break;
case Direction.North:
closedNeighborOne = neighbor;
break;
case Direction.SouthEast:
openNeighborTwo = neighbor;
break;
case Direction.South:
closedNeighborTwo = neighbor;
break;
}
}
break;
case Direction.South:
foreach (var neighbor in currentNode.GetNeighbors())
{
var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode);
switch (neighborDirection)
{
case Direction.SouthEast:
openNeighborOne = neighbor;
break;
case Direction.East:
closedNeighborOne = neighbor;
break;
case Direction.SouthWest:
openNeighborTwo = neighbor;
break;
case Direction.West:
closedNeighborTwo = neighbor;
break;
}
}
break;
case Direction.West:
foreach (var neighbor in currentNode.GetNeighbors())
{
var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode);
switch (neighborDirection)
{
case Direction.NorthWest:
openNeighborOne = neighbor;
break;
case Direction.North:
closedNeighborOne = neighbor;
break;
case Direction.SouthWest:
openNeighborTwo = neighbor;
break;
case Direction.South:
closedNeighborTwo = neighbor;
break;
}
}
break;
default:
throw new ArgumentOutOfRangeException();
}
if ((closedNeighborOne == null || !PathfindingHelpers.Traversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, closedNeighborOne)) &&
openNeighborOne != null && PathfindingHelpers.Traversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, openNeighborOne))
{
return(true);
}
if ((closedNeighborTwo == null || !PathfindingHelpers.Traversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, closedNeighborTwo)) &&
openNeighborTwo != null && PathfindingHelpers.Traversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, openNeighborTwo))
{
return(true);
}
return(false);
}
public Path NavigateTo(Vector3Int from, Vector3Int to)
{
if (!HasNodeAt(from) || !HasNodeAt(to))
{
return(null);
}
List <PathfindingNode> nodes = new List <PathfindingNode>();
PathfindingNode current = new PathfindingNode(from);
PathfindingNode toNode = new PathfindingNode(to);
for (int x = 0; x < Width; x += 1)
{
for (int y = 0; y < Height; y += 1)
{
if (!_nodes[x, y])
{
continue;
}
PathfindingNode node = new PathfindingNode(x, y);
nodes.Add(node);
node.FScore = node.Heuristic(toNode);
if (node.Equals(from))
{
current = node;
}
if (node.Equals(to))
{
toNode = node;
}
}
}
current.GScore = 0;
List <PathfindingNode> closedSet = new List <PathfindingNode>();
List <PathfindingNode> openSet = new List <PathfindingNode> {
current
};
while (openSet.Count > 0)
{
current = openSet.OrderBy(n => n.FScore).First();
if (current.Equals(to))
{
return(current.ConstructPath());
}
openSet.Remove(current);
closedSet.Add(current);
foreach (PathfindingNode node in nodes.Where(n => n.IsNeighboor(current)))
{
if (closedSet.Contains(node))
{
continue;
}
if (!openSet.Contains(node))
{
openSet.Add(node);
}
int tentativeGScore = current.GScore + 1;
if (tentativeGScore > AStarLimit)
{
return(null);
}
if (tentativeGScore >= node.GScore)
{
continue;
}
node.GScore = tentativeGScore;
node.FScore = node.GScore + node.Heuristic(toNode);
if (current.From != null)
{
node.FScore += current.To(current.From).Equals(node.To(current)) ? 0 : 4;
}
node.From = current;
}
}
return(null);
}
public List <Tuple <int, int> > FindShortestPathBetween(int sx, int sy, int dx, int dy)
{
// Djikstras
var queue = new List <Tuple <int, int> >();
var visited = new HashSet <Tuple <int, int> >();
queue.Add(new Tuple <int, int>(sx, sy));
var pathMap = new PathfindingNode[MAP_WIDTH, MAP_HEIGHT];
for (int x = 0; x < MAP_WIDTH; x++)
{
for (int y = 0; y < MAP_HEIGHT; y++)
{
pathMap[x, y] = new PathfindingNode();
pathMap[x, y].distanceToGoal = float.MaxValue;
pathMap[x, y].distanceToStart = float.MaxValue;
}
}
GD.Print("pathMap initialized");
GD.Print($"Searching between: ({sx},{sy}) -> ({dx},{dy})");
pathMap[sx, sy].distanceToStart = 0;
pathMap[sx, sy].distanceToGoal = (dx - sx) * (dx - sx) + (dy - sy) * (dy - sy);
while (queue.Count > 0)
{
Tuple <int, int> current = null;
var min = float.MaxValue;
foreach (var i in queue)
{
if (pathMap[i.Item1, i.Item2].distanceToGoal < min)
{
current = i;
min = pathMap[i.Item1, i.Item2].distanceToGoal;
}
}
if (current.Item1 == dx && current.Item2 == dy)
{
break;
}
queue.Remove(current);
visited.Add(current);
var currentPathMap = pathMap[current.Item1, current.Item2];
var left = new Tuple <int, int>(current.Item1 - 1, current.Item2);
if (!visited.Contains(left) && IsWalkable(left.Item1, left.Item2))
{
var leftPathMap = pathMap[left.Item1, left.Item2];
var distToNeighbor = 1;
var tentativeScore = currentPathMap.distanceToStart + distToNeighbor;
if (!queue.Contains(left))
{
queue.Add(left);
}
else if (tentativeScore >= leftPathMap.distanceToStart)
{
continue;
}
leftPathMap.prevX = current.Item1;
leftPathMap.prevY = current.Item2;
leftPathMap.distanceToStart = tentativeScore;
var distToEnd = (dx - left.Item1) * (dx - left.Item1) + (dy - left.Item2) * (dy - left.Item2);
leftPathMap.distanceToGoal = leftPathMap.distanceToStart + distToEnd;
}
var right = new Tuple <int, int>(current.Item1 + 1, current.Item2);
if (!visited.Contains(right) && IsWalkable(right.Item1, right.Item2))
{
var rightPathMap = pathMap[right.Item1, right.Item2];
var distToNeighbor = 1;
var tentativeScore = currentPathMap.distanceToStart + distToNeighbor;
if (!queue.Contains(right))
{
queue.Add(right);
}
else if (tentativeScore >= rightPathMap.distanceToStart)
{
continue;
}
rightPathMap.prevX = current.Item1;
rightPathMap.prevY = current.Item2;
rightPathMap.distanceToStart = tentativeScore;
var distToEnd = (dx - left.Item1) * (dx - left.Item1) + (dy - right.Item2) * (dy - right.Item2);
rightPathMap.distanceToGoal = rightPathMap.distanceToStart + distToEnd;
}
var up = new Tuple <int, int>(current.Item1, current.Item2 - 1);
if (!visited.Contains(up) && IsWalkable(up.Item1, up.Item2))
{
var upPathMap = pathMap[up.Item1, up.Item2];
var distToNeighbor = 1;
var tentativeScore = currentPathMap.distanceToStart + distToNeighbor;
if (!queue.Contains(up))
{
queue.Add(up);
}
else if (tentativeScore >= upPathMap.distanceToStart)
{
continue;
}
upPathMap.prevX = current.Item1;
upPathMap.prevY = current.Item2;
upPathMap.distanceToStart = tentativeScore;
var distToEnd = (dx - left.Item1) * (dx - left.Item1) + (dy - left.Item2) * (dy - left.Item2);
upPathMap.distanceToGoal = upPathMap.distanceToStart + distToEnd;
}
var down = new Tuple <int, int>(current.Item1, current.Item2 + 1);
if (!visited.Contains(down) && IsWalkable(down.Item1, down.Item2))
{
var downPathMap = pathMap[down.Item1, down.Item2];
var distToNeighbor = 1;
var tentativeScore = currentPathMap.distanceToStart + distToNeighbor;
if (!queue.Contains(down))
{
queue.Add(down);
}
else if (tentativeScore >= downPathMap.distanceToStart)
{
continue;
}
downPathMap.prevX = current.Item1;
downPathMap.prevY = current.Item2;
downPathMap.distanceToStart = tentativeScore;
var distToEnd = (dx - left.Item1) * (dx - left.Item1) + (dy - down.Item2) * (dy - down.Item2);
downPathMap.distanceToGoal = downPathMap.distanceToStart + distToEnd;
}
}
// Now, starting from the destination, walk back a path
var path = new List <Tuple <int, int> >();
var cursor = new Tuple <int, int>(dx, dy);
path.Add(cursor);
while (cursor.Item1 != sx || cursor.Item2 != sy)
{
var pm = pathMap[cursor.Item1, cursor.Item2];
cursor = new Tuple <int, int>(pm.prevX, pm.prevY);
path.Add(cursor);
}
path.Reverse();
return(path);
}
protected override void OnUpdate()
{
ActiveMap activeMap = MapManager.ActiveMap;
if (activeMap == null)
{
return;
}
Dictionary <Entity, List <Hex> > Paths = new Dictionary <Entity, List <Hex> >();
//clean buffer and restart waypoint
Entities.WithAll <PathWaypoint, TriggerPathfinding>().ForEach((Entity entity, ref PathWaypointIndex waypointIndex) =>
{
DynamicBuffer <PathWaypoint> buffer = World.EntityManager.GetBuffer <PathWaypoint>(entity);
buffer.Clear();
waypointIndex = new PathWaypointIndex()
{
Value = 0
};
});
//shortestPath
Entities.ForEach((Entity entity, ref TriggerPathfinding pathSolicitude, ref HexPosition hexPosition) =>
{
var path = new List <Hex>();
Hex startHex = hexPosition.HexCoordinates.Round();
Hex destinationHex = pathSolicitude.Destination;
if (!activeMap.map.MovementMapValues.TryGetValue(destinationHex, out bool c))
{
destinationHex = MapUtilities.FindClosestOpenHex((FractionalHex)destinationHex, activeMap.map, true);
}
else if (!c)
{
destinationHex = MapUtilities.FindClosestOpenHex((FractionalHex)destinationHex, activeMap.map, true);
}
if (!activeMap.map.MovementMapValues.TryGetValue(startHex, out bool b))
{
startHex = MapUtilities.FindClosestOpenHex(hexPosition.HexCoordinates, activeMap.map, true);
}
else if (!b)
{
startHex = MapUtilities.FindClosestOpenHex(hexPosition.HexCoordinates, activeMap.map, true);
}
if (startHex == destinationHex)
{
path.Add(startHex);
Paths.Add(entity, path);
return;
}
var startNode = new PathfindingNode(startHex, 0, 0, startHex);
var openList = new NativeList <PathfindingNode>(Allocator.Temp);
var closedList = new NativeList <PathfindingNode>(Allocator.Temp);
openList.Add(startNode);
while (openList.Length > 0)
{
var currentNode = openList[0];
for (int i = 1; i < openList.Length; i++)
{
var scanNode = openList[i];
if (currentNode.fCost > scanNode.fCost || currentNode.fCost == scanNode.fCost && currentNode.hCost > scanNode.hCost)
{
currentNode = scanNode;
}
}
openList.RemoveAtSwapBack(openList.IndexOf(currentNode));
closedList.Add(currentNode);
//enters here when the path have been found
if (currentNode.Equals(destinationHex))
{
var reversePath = new List <Hex>();
//esto no incluye el inicial.
while (currentNode != startNode)
{
reversePath.Add(currentNode.hex);
currentNode = closedList[closedList.IndexOf(currentNode.parent)];
}
//add starting node.
reversePath.Add(currentNode.hex);
reversePath.Reverse();
path.AddRange(reversePath);
//Debug.Log($"Pathfinding called. start hex: {startHex} dest hex: {destinationHex}.");
//for (int i = 0; i < path.Count; i++)
//{
// Debug.Log($"waypoint {i}: {path[i]}");
//}
var simplifiedPath = new List <Hex>(HexFunnelAlgorithm(path, true));
Paths.Add(entity, simplifiedPath);
//var path0 = new List<Hex>(path.GetRange(1, path.Count - 1));
//Paths.Add(entity, path0);
openList.Dispose();
closedList.Dispose();
return;
}
for (int i = 0; i < 6; i++)
{
var neightbor = currentNode.hex.Neightbor(i);
if (activeMap.map.MovementMapValues.ContainsKey(neightbor))
{
//Este check debe ser actualizado!
if (!MapUtilities.IsTraversable(neightbor, currentNode.hex, MapUtilities.MapType.MOVEMENT) || closedList.Contains(neightbor))//!activeMap.map.MovementMapValues[neightbor]
{
continue;
}
if (!openList.Contains(neightbor))
{
var neightborNode = new PathfindingNode(neightbor, currentNode.gCost + 1, neightbor.Distance(destinationHex), currentNode.hex);
openList.Add(neightborNode);
}
else
{
int indexOfNeightbor = openList.IndexOf(neightbor);
var neightborNode = openList[indexOfNeightbor];
int newMovementCostToNeighbor = currentNode.gCost + 1;
if (newMovementCostToNeighbor < neightborNode.gCost)
{
openList[indexOfNeightbor] = new PathfindingNode(neightbor, newMovementCostToNeighbor, neightbor.Distance(destinationHex), currentNode.hex);
}
}
}
}
}
//we may end up here if the destinaation isn't recheable from the start
//a posible solution is to return the path to the closest reachable hex to the destination
Debug.LogError($"The pathfinding was a failure for of index:{entity.Index}. The start node is: {startHex} and is open:{activeMap.map.MovementMapValues[startHex]}. the end node is: {destinationHex} and is open:{activeMap.map.MovementMapValues[destinationHex]}");
openList.Dispose();
closedList.Dispose();
});
//clean the buffer and then add the path there
Entities.WithAll <PathWaypoint>().ForEach((Entity entity, ref TriggerPathfinding pathSolicitude) =>
{
DynamicBuffer <PathWaypoint> buffer = World.EntityManager.GetBuffer <PathWaypoint>(entity);
buffer.Clear();
List <Hex> path;
if (Paths.TryGetValue(entity, out path))
{
foreach (Hex waypoint in path)
{
buffer.Add(new PathWaypoint()
{
Value = waypoint
});
}
}
else
{
Debug.LogError("the entity doesn't have a path. this is because the pathfinding failed before");
}
});
//remove path solicitude
Entities.ForEach((Entity entity, ref TriggerPathfinding pathSolicitude) =>
{
PostUpdateCommands.RemoveComponent <TriggerPathfinding>(entity);
});
}
public Transform CreateAPath(Transform start, Transform end)
{
//Getting closest node to start transform
PathfindingNode startingNode = GetClosestNode(start);
//Getting closest node to the end transform
PathfindingNode endingNode = GetClosestNode(end);
//Open list
List <PathfindingNode> openList = new List <PathfindingNode>();
foreach (PathfindingNode node in map)
{
//Set the g cost to infinity
node.gCost = Mathf.Infinity;
//set the parent to null
node.parent = null;
//add all the nodes to the open list
openList.Add(node);
}
//Set the starting node g cost to 0
startingNode.gCost = 0;
while (openList.Count > 0)
{
//Get the node with the lowest g cost inside the open list
PathfindingNode currentNode = GiveMeAnImportantNode(openList);
//Remove it from the open list because it is visited
openList.Remove(currentNode);
//Check if the node is the ending node
if (currentNode == endingNode)
{
//create new list for the path
List <Transform> path = new List <Transform>();
//If the current node is not null
while (currentNode != null)
{
//Add the current node to the path
path.Add(currentNode.nodeTransform);
//Retrace by going back to the parent of the current node
currentNode = currentNode.parent;
}
//Reverse the list
path.Reverse();
//Add the target to the list
path.Add(end);
//Calculate the distance between the second node in the path, and the start position
float distance = Vector3.Distance(path[1].position, start.position);
//If the raycast returns false then remove the node from the path as it is not a viable node to be placed inside the path
if (!Physics.Raycast(start.position, Vector3.Normalize(path[1].position - start.position), distance, layer))
{
path.Remove(path[0]);
}
//Return the first one in the path
return(path[0]);
}
//Calculate the G costs
foreach (PathfindingNode v in currentNode.linkedNodes)
{
float alt = currentNode.gCost + (currentNode.nodeTransform.position - v.nodeTransform.position).magnitude;
if (alt < v.gCost)
{
v.gCost = alt;
v.parent = currentNode;
}
}
}
return(null);
}
public List <PathfindingNode> FindTarget(Vector2 startPos, Vector2 endPos)
{
if (grid.GetGridObject(endPos) == null)
{
return(null);
}
PathfindingNode startNode = grid.GetGridObject(startPos);
PathfindingNode endNode = grid.GetGridObject(endPos);
openNodes = new List <PathfindingNode> {
startNode
};
closedNodes = new List <PathfindingNode>();
for (int i = 0; i < grid.GetGridWidth(); i++)
{
for (int j = 0; j < grid.GetGridHeight(); j++)
{
PathfindingNode node = grid.GetGridObject(i, j);
node.gCost = int.MaxValue;
node.CalculateFCost();
node.parentNode = null;
}
}
startNode.gCost = 0;
startNode.hCost = GetDistanceValue(startNode, endNode);
startNode.CalculateFCost();
while (openNodes.Count > 0)
{
PathfindingNode currentNode = GetLowestValueNode(openNodes);
if (currentNode == endNode)
{
return(CalculateFinalPath(endNode));
}
openNodes.Remove(currentNode);
closedNodes.Add(currentNode);
foreach (PathfindingNode neighbor in FindNeighbor(currentNode))
{
if (closedNodes.Contains(neighbor))
{
continue;
}
if (!neighbor.isWalkable)
{
closedNodes.Add(neighbor);
/*
* for(int i = -2; i < 3; i++)
* {
* for(int j = -2; j < 3; j++)
* {
* closedNodes.Add(grid.GetGridObject(neighbor.GetX() + i, neighbor.GetY() + j));
* }
* }
*/
continue;
}
int newGCost = currentNode.gCost + GetDistanceValue(currentNode, neighbor);
if (newGCost < neighbor.gCost)
{
neighbor.parentNode = currentNode;
neighbor.gCost = newGCost;
neighbor.hCost = GetDistanceValue(currentNode, endNode);
neighbor.CalculateFCost();
}
if (!openNodes.Contains(neighbor))
{
openNodes.Add(neighbor);
}
}
}
return(null);
}
public List<PathfindingNode> GetNeighbors (PathfindingNode node)
{
List<PathfindingNode> neighbors = new List<PathfindingNode>();
for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
if (x ==0 && y == 0)
{
continue;
}
else
{
int checkX = node.gridX + x;
int checkY = node.gridY + y;
if (checkX >- 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY)
{
neighbors.Add(grid[checkX, checkY]);
}
}
}
}
return neighbors;
}
public List <Point> getPath(Point start, Point goal)
{
List <Point> path = new List <Point>();
PriorityQueue <PathfindingNode, float> open_set = new PriorityQueue <PathfindingNode, float>();
ResetPathfindingNodeData();
PathfindingNode starting_node = this.pathfindingNodes[pointToIndex(start)];
starting_node.pos = start;
starting_node.parent = null;
starting_node.givenCost = 0;
starting_node.finalCost = 0;
starting_node.listStatus = ListStatus.ON_OPEN;
open_set.push(starting_node, 0);
while (!open_set.isEmpty())
{
PathfindingNode curr_node = open_set.pop();
PathfindingNode parent = curr_node.parent;
Node jp_node = gridNodes[pointToIndex(curr_node.pos)]; // get jump point info
// Check if we've reached the goal
if (curr_node.pos.Equals(goal))
{
// end and return path
return(reconstructPath(curr_node, start));
}
// foreach direction from parent
foreach (eDirections dir in getAllValidDirections(curr_node))
{
PathfindingNode new_successor = null;
int given_cost = 0;
// goal is closer than wall distance or closer than or equal to jump point distnace
if (isCardinal(dir) &&
goalIsInExactDirection(curr_node.pos, dir, goal) &&
Point.diff(curr_node.pos, goal) <= Mathf.Abs(jp_node.jpDistances[(int)dir]))
{
new_successor = this.pathfindingNodes[pointToIndex(goal)];
given_cost = curr_node.givenCost + Point.diff(curr_node.pos, goal);
}
// Goal is closer or equal in either row or column than wall or jump point distance
else if (isDiagonal(dir) &&
goalIsInGeneralDirection(curr_node.pos, dir, goal) &&
(Mathf.Abs(goal.column - curr_node.pos.column) <= Mathf.Abs(jp_node.jpDistances[(int)dir]) ||
Mathf.Abs(goal.row - curr_node.pos.row) <= Mathf.Abs(jp_node.jpDistances[(int)dir])))
{
// Create a target jump point
// int minDiff = min(RowDiff(curNode, goalNode),
// ColDiff(curNode, goalNode));
int min_diff = Mathf.Min(Mathf.Abs(goal.column - curr_node.pos.column),
Mathf.Abs(goal.row - curr_node.pos.row));
// newSuccessor = GetNode (curNode, minDiff, direction);
new_successor = getNodeDist(
curr_node.pos.row,
curr_node.pos.column,
dir,
min_diff);
// givenCost = curNode->givenCost + (SQRT2 * DiffNodes(curNode, newSuccessor));
given_cost = curr_node.givenCost + (int)(SQRT_2 * Point.diff(curr_node.pos, new_successor.pos));
}
else if (jp_node.jpDistances[(int)dir] > 0)
{
// Jump Point in this direction
// newSuccessor = GetNode(curNode, direction);
new_successor = getNodeDist(
curr_node.pos.row,
curr_node.pos.column,
dir,
jp_node.jpDistances[(int)dir]);
// givenCost = DiffNodes(curNode, newSuccessor);
given_cost = Point.diff(curr_node.pos, new_successor.pos);
// if (diagonal direction) { givenCost *= SQRT2; }
if (isDiagonal(dir))
{
given_cost = (int)(given_cost * SQRT_2);
}
// givenCost += curNode->givenCost;
given_cost += curr_node.givenCost;
}
// Traditional A* from this point
if (new_successor != null)
{
// if (newSuccessor not on OpenList)
if (new_successor.listStatus != ListStatus.ON_OPEN)
{
// newSuccessor->parent = curNode;
new_successor.parent = curr_node;
// newSuccessor->givenCost = givenCost;
new_successor.givenCost = given_cost;
new_successor.directionFromParent = dir;
// newSuccessor->finalCost = givenCost +
// CalculateHeuristic(curNode, goalNode);
new_successor.finalCost = given_cost + octileHeuristic(new_successor.pos.column, new_successor.pos.row, goal.column, goal.row);
new_successor.listStatus = ListStatus.ON_OPEN;
// OpenList.Push(newSuccessor);
open_set.push(new_successor, new_successor.finalCost);
}
// else if(givenCost < newSuccessor->givenCost)
else if (given_cost < new_successor.givenCost)
{
// newSuccessor->parent = curNode;
new_successor.parent = curr_node;
// newSuccessor->givenCost = givenCost;
new_successor.givenCost = given_cost;
new_successor.directionFromParent = dir;
// newSuccessor->finalCost = givenCost +
// CalculateHeuristic(curNode, goalNode);
new_successor.finalCost = given_cost + octileHeuristic(new_successor.pos.column, new_successor.pos.row, goal.column, goal.row);
new_successor.listStatus = ListStatus.ON_OPEN;
// OpenList.Update(newSuccessor);
open_set.push(new_successor, new_successor.finalCost);
}
}
}
}
return(path);
}
public ConsideredNode(PathfindingNode nodeInfo, ConsideredNode parent, int directTravelCostToTarget)
{
this.Info = nodeInfo;
this.DirectTravelCostToTarget = directTravelCostToTarget;
this.SetParent(parent);
}
public void DrawPath(int enemyNum, List <PathfindingNode> myPath, PathfindingNode startNode)
{
StartCoroutine(DrawPathSequental(enemyNum, myPath, startNode));
}
public int GetDistance (PathfindingNode nodeA, PathfindingNode nodeB)
{
int distX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
int distY = Mathf.Abs(nodeA.gridY - nodeB.gridY);
int diagonal = 14; // make this more than double strait if you want to eliminate diagonals
int strait = 10;
int diagonalWeighting = diagonalWeight;
if (distX > distY)
{
return ((diagonal * distY * diagonalWeighting) + (strait * (distX - distY)));
}
else
{
return ((diagonal * distX * diagonalWeighting) + (strait * (distY - distX)));
}
}
public PathfindingConnection(PathfindingNode a, PathfindingNode b, double c)
{
From = a;
To = b;
Cost = c;
}
