//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; } } } } }