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