//Add a neighbor to this node
public void AddNeighbor(FlowFieldNode neighbor)
{
if (neighborNodes == null)
{
neighborNodes = new HashSet <FlowFieldNode>();
}
neighborNodes.Add(neighbor);
}
public void Reset()
{
parent = null;
isInOpenSet = false;
//Reset cost of movement (total cost) to this node to something large
totalCostFlowField = float.MaxValue;
//The cost to move to this node
movementCostFlowField = 0f;
}
//Calculate the shortest path with obstacles from each cell to the target cell we want to reach
//Is called Dynamic Programming in "Programming self-driving car" but is the same as a flow map
//Is called holonomic-with-obstacles in the reports
public static void DynamicProgramming(Map map, IntVector2 targetPos)
{
int mapWidth = map.MapWidth;
//Debug.DrawLine(map.cellData[targetPos.x, targetPos.z].centerPos, Vector3.zero, Color.red, 15f);
//The final flow field will be stored here, so init it
FlowFieldNode[,] nodesArray = new FlowFieldNode[mapWidth, mapWidth];
for (int x = 0; x < mapWidth; x++)
{
for (int z = 0; z < mapWidth; z++)
{
bool isWalkable = map.cellData[x, z].isObstacleInCell ? false : true;
FlowFieldNode node = new FlowFieldNode(isWalkable, map.cellData[x, z].centerPos, new IntVector2(x, z));
nodesArray[x, z] = node;
}
}
//A flow field can have several start nodes, but in this case we have just one, which is the cell we want to reach
List <FlowFieldNode> startNodes = new List <FlowFieldNode>();
startNodes.Add(nodesArray[targetPos.x, targetPos.z]);
//Generate the flow field
FlowField.Generate(startNodes, nodesArray, includeCorners: true);
//Save the values
flowFieldHeuristics = new float[mapWidth, mapWidth];
for (int x = 0; x < mapWidth; x++)
{
for (int z = 0; z < mapWidth; z++)
{
//Save the flow field because we will use it to display it on a texture
map.cellData[x, z].distanceToTarget = nodesArray[x, z].totalCostFlowField;
//This heuristics has to be discounted by a value to be admissible to never overestimate the actual cost
flowFieldHeuristics[x, z] = nodesArray[x, z].totalCostFlowField * 0.92621f;
}
}
//Debug.Log("Distance flow field: " + nodesArray[targetPos.x, targetPos.z].totalCostFlowField);
//Debug.Log("Distance flow field: " + nodesArray[0, 0].totalCostFlowField);
}
//Generate the flow field showing distance to closest obstacle from each cell
private void GenerateObstacleFlowField(Map map, bool check8Cells)
{
int mapWidth = map.MapWidth;
//The flow field will be stored in this array
FlowFieldNode[,] flowField = new FlowFieldNode[mapWidth, mapWidth];
//Init
Cell[,] cellData = map.cellData;
for (int x = 0; x < mapWidth; x++)
{
for (int z = 0; z < mapWidth; z++)
{
//All nodes are walkable because we are generating the flow from each obstacle
bool isWalkable = true;
FlowFieldNode node = new FlowFieldNode(isWalkable, cellData[x, z].centerPos, new IntVector2(x, z));
flowField[x, z] = node;
}
}
//A flow field can have several start nodes, which are the obstacles in this case
List <FlowFieldNode> startNodes = new List <FlowFieldNode>();
for (int x = 0; x < mapWidth; x++)
{
for (int z = 0; z < mapWidth; z++)
{
//If this is an obstacle
if (cellData[x, z].isObstacleInCell)
{
startNodes.Add(flowField[x, z]);
}
}
}
//Generate the flow field
FlowField.Generate(startNodes, flowField, check8Cells);
//Add the values to the celldata that belongs to the map
for (int x = 0; x < mapWidth; x++)
{
for (int z = 0; z < mapWidth; z++)
{
cellData[x, z].distanceToClosestObstacle = flowField[x, z].totalCostFlowField;
}
}
}
//Find the neighboring nodes to a node by checking all nodes around it
private static HashSet <FlowFieldNode> FindNeighboringNodes(FlowFieldNode node, FlowFieldNode[,] nodeArray, int mapWidth, bool includeCorners)
{
HashSet <IntVector2> neighborCells = new HashSet <IntVector2>();
//Get the directions we can move in, which are up, left, right, down
IntVector2[] delta = HelpStuff.delta;
if (includeCorners)
{
delta = HelpStuff.deltaWithCorners;
}
//Will track if at least one neighbor is an obstacle, which may be useful to know later
bool isNeighborObstacle = false;
for (int i = 0; i < delta.Length; i++)
{
IntVector2 cellPos = new IntVector2(node.cellPos.x + delta[i].x, node.cellPos.z + delta[i].z);
//Is this cell position within the grid?
if (IsCellPosWithinGrid(cellPos, mapWidth))
{
//Is not a valid neighbor if its obstacle
if (!nodeArray[cellPos.x, cellPos.z].isWalkable)
{
isNeighborObstacle = true;
}
else
{
neighborCells.Add(cellPos);
}
}
}
//If we are checking 8 neighbors we have to be careful to not jump diagonally if one cell next to the diagonal is obstacle
//This is a costly operation (0.3 seconds if we do it for all cells) so only do it if at least one neighbor is obstacle
if (includeCorners && isNeighborObstacle)
{
HashSet <IntVector2> corners = new HashSet <IntVector2>(HelpStuff.deltaJustCorners);
//Loop through all 8 neighbors
for (int i = 0; i < delta.Length; i++)
{
//Is this neighbor a corner?
if (corners.Contains(delta[i]))
{
IntVector2 cellPos = new IntVector2(node.cellPos.x + delta[i].x, node.cellPos.z + delta[i].z);
//Have we added the corner to the list of neighbors
if (!neighborCells.Contains(cellPos))
{
continue;
}
//Check if neighbors to the corner are obstacles, if so we cant move to this corner
IntVector2 n1 = delta[HelpStuff.ClampListIndex(i + 1, delta.Length)];
IntVector2 n2 = delta[HelpStuff.ClampListIndex(i - 1, delta.Length)];
IntVector2 cellPos_n1 = new IntVector2(node.cellPos.x + n1.x, node.cellPos.z + n1.z);
IntVector2 cellPos_n2 = new IntVector2(node.cellPos.x + n2.x, node.cellPos.z + n2.z);
if (!nodeArray[cellPos_n1.x, cellPos_n1.z].isWalkable || !nodeArray[cellPos_n2.x, cellPos_n2.z].isWalkable)
{
//This is not a valid neighbor so remove it from neighbors
neighborCells.Remove(cellPos);
}
}
}
}
//From cell to node
HashSet <FlowFieldNode> neighborNodes = new HashSet <FlowFieldNode>();
foreach (IntVector2 cell in neighborCells)
{
neighborNodes.Add(nodeArray[cell.x, cell.z]);
}
return(neighborNodes);
}
//Include corners means we check 8 cells around each cell and not just 4
public static void Generate(List <FlowFieldNode> startNodes, FlowFieldNode[,] allNodes, bool includeCorners)
{
//Reset such as costs and parent nodes, etc
//Will set set costs to max value
int mapWidth = allNodes.GetLength(0);
for (int x = 0; x < mapWidth; x++)
{
for (int z = 0; z < mapWidth; z++)
{
allNodes[x, z].Reset();
//Find all valid neighbors to this node, so no obstacles are allowed
HashSet <FlowFieldNode> neighbors = FindNeighboringNodes(allNodes[x, z], allNodes, mapWidth, includeCorners);
allNodes[x, z].neighborNodes = neighbors;
}
}
//The queue with the open nodes
Queue <FlowFieldNode> openSet = new Queue <FlowFieldNode>();
//Add the start nodes to the list with open nodes
for (int i = 0; i < startNodes.Count; i++)
{
FlowFieldNode startNode = startNodes[i];
openSet.Enqueue(startNode);
//Set the cost of the start node to 0
startNode.totalCostFlowField = 0f;
startNode.isInOpenSet = true;
//The closest start cell to this cell is the cell itself
startNode.closestStartNodes.Add(startNode.cellPos);
}
//Generate the flow field
//To avoid infinite loop
int safety = 0;
//Stop the algorithm if open list is empty
while (openSet.Count > 0)
{
if (safety > 500000)
{
Debug.Log("Flow field stuck in infinite loop");
break;
}
safety += 1;
//Pick the first node in the open set as the current node, no sorting is needed
FlowFieldNode currentNode = openSet.Dequeue();
currentNode.isInOpenSet = false;
//Explore the neighboring nodes
HashSet <FlowFieldNode> neighbors = currentNode.neighborNodes;
foreach (FlowFieldNode neighbor in neighbors)
{
//Cost calculations - The cost added can be different depending on the terrain
//This is not a costly operation (doesnt affect time) so no need to precalculate
float newCost = currentNode.totalCostFlowField + (currentNode.worldPos - neighbor.worldPos).magnitude;
//Update the the cost if it's less than the old cost
if (newCost <= neighbor.totalCostFlowField)
{
neighbor.totalCostFlowField = newCost;
//Change to which region this node belongs
//Is not always needed but is a fast operation
neighbor.region = currentNode.region;
//The closest of the start nodes to this node
//If they are equally close we need to save both
if (newCost == neighbor.totalCostFlowField)
{
foreach (IntVector2 c in currentNode.closestStartNodes)
{
neighbor.closestStartNodes.Add(c);
}
}
else
{
neighbor.closestStartNodes.Clear();
foreach (IntVector2 c in currentNode.closestStartNodes)
{
neighbor.closestStartNodes.Add(c);
}
}
//Add it if it isnt already in the list of open nodes
if (!neighbor.isInOpenSet)
{
openSet.Enqueue(neighbor);
neighbor.isInOpenSet = true;
}
}
//Dont need to add the current node back to the open set. If we find a shorter path to it from
//another node, it will be added
}
}
}
//
// Find the distance from each cell to the nearest voronoi edge
//
//This can be done with a flow field from each edge
private static void FindDistanceFromEdgeToCell(VoronoiFieldCell[,] voronoiField)
{
int mapWidth = voronoiField.GetLength(0);
//The flow field will be stored in this array
FlowFieldNode[,] flowField = new FlowFieldNode[mapWidth, mapWidth];
//Init
for (int x = 0; x < mapWidth; x++)
{
for (int z = 0; z < mapWidth; z++)
{
//All nodes are walkable because we are generating the flow from each obstacle
bool isWalkable = voronoiField[x, z].isObstacle ? false : true;
FlowFieldNode node = new FlowFieldNode(isWalkable, voronoiField[x, z].worldPos, new IntVector2(x, z));
//node.region = voronoiField[x, z].region;
flowField[x, z] = node;
}
}
//A flow field can have several start nodes, which are the obstacles in this case
List <FlowFieldNode> startNodes = new List <FlowFieldNode>();
for (int x = 0; x < mapWidth; x++)
{
for (int z = 0; z < mapWidth; z++)
{
//If this is an edge
if (voronoiField[x, z].isVoronoiEdge)
{
startNodes.Add(flowField[x, z]);
}
}
}
//Generate the flow field
FlowField.Generate(startNodes, flowField, includeCorners: true);
//Add the values to the celldata that belongs to the map
for (int x = 0; x < mapWidth; x++)
{
for (int z = 0; z < mapWidth; z++)
{
voronoiField[x, z].distanceToClosestEdge = flowField[x, z].totalCostFlowField;
voronoiField[x, z].closestEdgeCells = flowField[x, z].closestStartNodes;
//Now we can calculate the euclidean distance to the closest obstacle, which is more accurate then the flow field distance
HashSet <IntVector2> closest = voronoiField[x, z].closestEdgeCells;
if (closest != null && closest.Count > 0)
{
foreach (IntVector2 c in closest)
{
float distance = (voronoiField[c.x, c.z].worldPos - voronoiField[x, z].worldPos).magnitude;
voronoiField[x, z].distanceToClosestEdge = distance;
//If we have multiple cells that are equally close, we only need to test one of them
break;
}
}
}
}
}
