//Add a neighbor to this node
public void AddNeighbor(FlowFieldNode neighbor)
{
if (neighborNodes == null)
{
neighborNodes = new List <FlowFieldNode>();
}
neighborNodes.Add(neighbor);
}
//Reset
public void ResetNode()
{
gCost = 0f;
hCost = 0f;
parent = null;
isClosed = false;
isInOpenSet = false;
}
//Generate the flow field showing how far to the closest obstacle from each cell
private void GenerateObstacleFlowField()
{
FlowField flowField = new FlowField();
int mapLength = PathfindingController.mapLength;
int mapWidth = PathfindingController.mapWidth;
//The flow field will be stored in this array
FlowFieldNode[,] gridArray = new FlowFieldNode[mapLength, mapWidth];
for (int x = 0; x < mapLength; x++)
{
for (int z = 0; z < mapWidth; z++)
{
bool isWalkable = true;
FlowFieldNode node = new FlowFieldNode(isWalkable);
node.cellPos = new IntVector2(x, z);
gridArray[x, z] = node;
}
}
//A flow field can have several start nodes
List <FlowFieldNode> startNodes = new List <FlowFieldNode>();
for (int x = 0; x < mapLength; x++)
{
for (int z = 0; z < mapWidth; z++)
{
//If this is an obstacle
if (isObstacleInCell[x, z])
{
startNodes.Add(gridArray[x, z]);
}
}
}
//Generate the flow field
flowField.FindPath(startNodes, gridArray);
//Add the values to the other array
for (int x = 0; x < mapLength; x++)
{
for (int z = 0; z < mapWidth; z++)
{
distanceToClosestObstacle[x, z] = gridArray[x, z].totalCostFlowField;
}
}
}
//Calculate the shortest path with obstacles from each square
//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 void DynamicProgramming(IntVector2 targetPos)
{
FlowField flowField = new FlowField();
int mapLength = PathfindingController.mapLength;
int mapWidth = PathfindingController.mapWidth;
//The final flow field will be stored here, so init it
FlowFieldNode[,] gridArray = new FlowFieldNode[mapLength, mapWidth];
for (int x = 0; x < mapLength; x++)
{
for (int z = 0; z < mapWidth; z++)
{
//bool isWalkable = ObstaclesController.isObstacleInCell[x, z] ? false : true; (MATT)
bool isWalkable = true;
FlowFieldNode node = new FlowFieldNode(isWalkable);
node.cellPos = new IntVector2(x, z);
gridArray[x, z] = node;
}
}
//A flow field can have several start nodes
List <FlowFieldNode> startNodes = new List <FlowFieldNode>();
startNodes.Add(gridArray[targetPos.x, targetPos.z]);
flowField.FindPath(startNodes, gridArray);
//Add the values to the other array
for (int x = 0; x < mapLength; x++)
{
for (int z = 0; z < mapWidth; z++)
{
flowFieldHeuristics[x, z] = gridArray[x, z].totalCostFlowField;
}
}
}
//Find the neighboring nodes to a node by checking all 4 nodes around it
private List <FlowFieldNode> FindNeighboringNodes(FlowFieldNode node, FlowFieldNode[,] gridArray)
{
List <FlowFieldNode> neighboringNodes = new List <FlowFieldNode>();
//Get the directions we can move in, which are up, left, right, down
IntVector2[] delta = DeltaMovements.delta;
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, gridArray))
{
neighboringNodes.Add(gridArray[cellPos.x, cellPos.z]);
}
}
return(neighboringNodes);
}
//Find the neighboring node with the least cost
private FlowFieldNode FindLowestCostNeighbor(FlowFieldNode node, FlowFieldNode[,] gridArray, bool includeCorners)
{
int lowestCost = System.Int32.MaxValue;
FlowFieldNode lowestCostNode = null;
//Get the directions we can move in
IntVector2[] delta = DeltaMovements.delta;
if (includeCorners)
{
delta = DeltaMovements.deltaWithCorners;
}
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, gridArray))
{
////Make sure we are not crossing obstacles diagonally
//bool topNode = gridArray[node.cellPos.x + 0, node.cellPos.z + 1].isWalkable;
//bool bottomNode = gridArray[node.cellPos.x + 0, node.cellPos.z - 1].isWalkable;
//bool leftNode = gridArray[node.cellPos.x - 1, node.cellPos.z + 0].isWalkable;
//bool rightNode = gridArray[node.cellPos.x + 1, node.cellPos.z + 0].isWalkable;
////TR
//if (delta[i].x == 1 && delta[i].z == 1)
//{
// if (!topNode || !rightNode)
// {
// continue;
// }
//}
////BL
//else if (delta[i].x == -1 && delta[i].z == -1)
//{
// if (!leftNode || !bottomNode)
// {
// continue;
// }
//}
////TL
//else if (delta[i].x == -1 && delta[i].z == 1)
//{
// if (!topNode || !leftNode)
// {
// continue;
// }
//}
////BR
//else if (delta[i].x == 1 && delta[i].z == -1)
//{
// if (!rightNode || !bottomNode)
// {
// continue;
// }
//}
int neighborCost = gridArray[cellPos.x, cellPos.z].totalCostFlowField;
if (neighborCost < lowestCost)
{
lowestCost = neighborCost;
lowestCostNode = gridArray[cellPos.x, cellPos.z];
}
}
}
if (lowestCost < System.Int32.MaxValue)
{
return(lowestCostNode);
}
else
{
return(null);
}
}
public void FindPath(List <FlowFieldNode> startNodes, FlowFieldNode[,] gridArray)
{
//Reset such as costs and parent nodes, etc
//Will set set costs to max value
Debug.Log("Length 0: " + gridArray.GetLength(0));
Debug.Log("Length 1: " + gridArray.GetLength(1));
for (int x = 0; x < gridArray.GetLength(0); x++)
{
for (int z = 0; z < gridArray.GetLength(1); z++)
{
gridArray[x, z].ResetNodeFlowField();
}
}
//The list with the open nodes
List <FlowFieldNode> openSet = new List <FlowFieldNode>();
//Add the start nodes to the list with open nodes
for (int i = 0; i < startNodes.Count; i++)
{
FlowFieldNode startNode = startNodes[i];
openSet.Add(startNode);
//Set the cost of the start node to 0
startNode.totalCostFlowField = 0;
startNode.isInOpenSet = true;
//Can have different start costs if we want one node to be better than another node
//if (i == 1)
//{
// startNode.costFlowField = 10;
//}
}
//To avoid infinite loop
int safety = 0;
//Stop the algorithm if open list is empty
while (openSet.Count > 0 && safety < 50000)
{
safety += 1;
//Pick the first node in the open set as the current node, no sorting is needed
FlowFieldNode currentNode = openSet[0];
//Remove it from the list of open nodes
openSet.RemoveAt(0);
currentNode.isInOpenSet = false;
//Explore the neighboring nodes
List <FlowFieldNode> neighbors = FindNeighboringNodes(currentNode, gridArray);
//Loop through all neighbors, which is 4 (up-down-left-right)
for (int i = 0; i < neighbors.Count; i++)
{
FlowFieldNode neighbor = neighbors[i];
//Ignore the neighbor if it's an obstacle
if (!neighbor.isWalkable)
{
continue;
}
//Cost calculations - The cost added can be different depending on the terrain
int newCost = currentNode.totalCostFlowField + neighbor.movementCostFlowField;
//Update the the cost if it's is less than the old cost
if (newCost < neighbor.totalCostFlowField)
{
neighbor.totalCostFlowField = newCost;
//Add it if it isnt already in the list of open nodes
if (!neighbor.isInOpenSet)
{
openSet.Add(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
}
}
//IS NOT NEEDED HERE
//Now we have the integration field, and we are now going to create the flow field
//which is the direction from a node to the node with the smallest cost
//for (int x = 0; x < gridArray.GetLength(0); x++)
//{
// for (int z = 0; z < gridArray.GetLength(0); z++)
// {
// FlowFieldNode thisNode = gridArray[x, z];
// if (thisNode.isWalkable && thisNode.totalCostFlowField < System.Int32.MaxValue && thisNode.totalCostFlowField != 0)
// {
// thisNode.parent = FindLowestCostNeighbor(thisNode, gridArray, true);
// //Find the direction between the nodes
// if (thisNode.parent != null)
// {
// thisNode.flowDirection = (thisNode.parent.worldPos - thisNode.worldPos).normalized;
// }
// }
// }
//}
}
