Unity realizes a * pathfinding algorithm learning 1.0

Time:2022-5-7

1、 Principle of a * routing algorithm

If there are two points a and B on the map, set a as the starting point and B as the target point (end point)

Here, three values are defined for each map node
Gcost: cost from the starting point (distance)
Hcost: cost (distance) from the target point
Fcost: the sum of gcost and gcost.
The cost here can adopt straight-line distance or Manhattan distance, as long as it is suitable
Then first calculate the three values of all nodes around the starting point
Here, let the distance between each two adjacent nodes be 10, then the diagonal distance is 14

Then, it is calculated that the box in the upper left corner of point a has the smallest F value. Put the node into the list (array is also OK), set a as the parent node of the node, and then calculate the distance between the surrounding boxes

Because it moves from point a, it will not compare point a in the next comparison
Calculate again that the node with the smallest F value is still the node in the upper left corner

In this way, the shortest path from point a to point B is found

What if there are obstacles between a and B?

Also calculate the minimum F value

But there are three identical F values

Then, the path with the smallest H value is preferred, that is, the path closest to the target point

But after the movement, the F value becomes larger
Then turn to find the path with the lowest F value before, but then the F value is higher

Then the path with the lowest F value is still selected

Then there is the next path with the lowest F value

Then next
Until the hcost from the destination is 0

Another example is how to find the shortest path. The arrow indicates the parent node


After calculating the F value of the nodes around point a for the first time, find the smallest one and set the parent node of the node as point a
Calculate again, set the surrounding nodes as child nodes, and then find that there are two 58 points around. Select gcost smaller, that is, the 58 point next to a below
Recalculate

After calculating the following 58 nodes, it is found that the cost required for the next node to pass through here is smaller, so reset the parent node

Again

If the path passes through the yellow line, the cost of the node in the lower right corner will reach 66

If you arrive from below, the cost is 58, which will be smaller and the parent node will be reset

Here, the recalculated fcost is a-58-58, and fcost is 58 smaller, indicating that the new path is smaller and the parent node is reset

Follow this method to cycle until the target point is found

Because the parent node is set (indicated by the arrow in the figure), you only need to obtain the parent node from the target point. Store all the obtained nodes in the list or array, and then flip it to obtain the shortest path of a-b

2、 Set the path point in unity


Then add the cable as an obstacle

Set the level of the cube to unwalkable

Then copy a few

New scriptNodeAt present, the node only contains the coordinate position and whether it can walk

public class Node
{
    public bool walkable; 		// Can nodes move
    public Vector3 worldPos; 	// Spatial coordinates of nodes
    public Node(bool _walkable, Vector3 _worldPos) 	// constructor 
    {
        walkable = _walkable;
        worldPos = _worldPos;
    }
}

New scriptMyGrid, add to new empty objectA*upper

public class MyGrid : MonoBehaviour
{
    public LayerMask unwalkableMask; 	// Can nodes move
    public Vector2 gridWorldSize; 	// The scope of the map. Nodes are created within the map
    public float nodeRadius; 	// Node size
    Node[,] grid; 	// Node array
    
    private void OnDrawGizmos()
    {
        //First draw the scope of the map 								// Width thickness length
        Gizmos.DrawWireCube(transform.position, new Vector3(gridWorldSize.x, 1, gridWorldSize.y));
    }
}

Then set the node map size



Continue to modifyMyGrid

public class MyGrid : MonoBehaviour
{
    public LayerMask unwalkableMask;    // Can you walk
    public Vector2 gridWorldSize;   // The size of the map that needs to be searched
    public float nodeRadius;    // Node radius
    Node[,] grid;               // node

    float nodeDiameter;         // Diameter of node
    int gridSizeX, gridSizeY;

    void Start()
    {
        nodeDiameter = nodeRadius * 2;
        gridSizeX = Mathf. RoundToInt(gridWorldSize.x / nodeDiameter); // Calculate the number of nodes in the x-axis direction
        gridSizeY = Mathf. RoundToInt(gridWorldSize.y / nodeDiameter); // Calculate the number of nodes in the z-axis direction
        CreateGrid();
    }
    void CreateGrid()
    {
        grid = new Node[gridSizeX, gridSizeY]; 	// Initialize node array
        //The starting point (origin) of the calculation grid
        Vector3 worldButtonLeft = transform.position 
            					- Vector3.right * gridWorldSize.x / 2 
            					- Vector3.forward * gridWorldSize.y / 2;
        for (int x = 0; x < gridSizeX; x++)
        {
            for (int y = 0; y < gridSizeY; y++)
            {
                //Calculate the spatial coordinates of nodes
                Vector3 worldPoint = worldButtonLeft 
                    				+ Vector3.right * (x * nodeDiameter + nodeRadius) 
                    				+ Vector3.forward * (y * nodeDiameter + nodeRadius);
                
                //Judge whether the node can walk and whether it collides with obstacles according to the range of the node
                bool walkable = !(Physics.CheckSphere(worldPoint, nodeRadius, unwalkableMask)); 

                grid[x, y] = new Node(walkable, worldPoint);    // Adds the data of the node to the binary array
            }
        }
    }
    private void OnDrawGizmos()
    {
        Gizmos.DrawWireCube(transform.position, new Vector3(gridWorldSize.x, 1, gridWorldSize.y));
        if (grid != null)
        {
            foreach (Node node in grid)
            {	
                //Draw all nodes, which can walk in white and can't walk in red
                Gizmos.color = (node.walkable) ? Color.white : Color.red;
                Gizmos. DrawCube(node.worldPos, Vector3.one * (nodeDiameter - 0.1f));// Reduce the size of gizmos squares for easy observation
            }
        }
    }
}

Operation results

Next, add a start point and an end point

Create two new caps

So how do you know where the starting point is now?
Continue to modifyMyGrid

public class MyGrid : MonoBehaviour
{
    ......
    public Node NodeFromWorldPos(Vector3 worldPos) 	// Here is the location of the starting point
    {
        //Here, percentx and percenty calculate the proportion of the starting point position to the horizontal and vertical coordinates of the map area
        float percentX = (worldPos.x + gridWorldSize.x / 2) / gridWorldSize.x;
        float percentY = (worldPos.z + gridWorldSize.y / 2) / gridWorldSize.y;
        
        //Limit the location of the starting point within the scope of the map
        percentX = Mathf.Clamp01(percentX);
        percentY = Mathf.Clamp01(percentY);
		
        //Total number of nodes * area proportion = the number of nodes, - 1 is to start from 0, because 0 also has a node
        int x = Mathf.RoundToInt((gridSizeX - 1) * percentX);
        int y = Mathf.RoundToInt((gridSizeY - 1) * percentY);
        return grid[x, y];
    }
    private void OnDrawGizmos()
    {
        Gizmos.DrawWireCube(transform.position, new Vector3(gridWorldSize.x, 1, gridWorldSize.y));
        if (grid != null)
        {
            //Calculate the position of the starting point
            Node playerNode = NodeFromWorldPos(player.position);
            foreach (Node node in grid)
            {
                Gizmos.color = (node.walkable) ? Color.white : Color.red;
                if (playerNode == node) 	// Sets the color of the start position node
                {
                    Gizmos.color = Color.cyan;
                }
                Gizmos.DrawCube(node.worldPos, Vector3.one * (nodeDiameter - 0.1f));
            }
        }
    }
}

Operation results

3、 Implement routing algorithm

modifyNode

public class Node
{
    ......
    public int gridX; 	// Which node in the X direction in the map
    public int gridY; 	// Which node in the Y direction in the map
    
    public int gCost; 	// G value
    public int hCost; 	// H value
    public Node parent; 	 // The parent node is finally used to store the actual path
    										//Two parameters are added again to facilitate the calculation of adjacent nodes
    public Node(bool _walkable, Vector3 _worldPos,int _gridX, int _gridY)	
    {
        walkable = _walkable;
        worldPos = _worldPos;
        gridX = _gridX;
        gridY = _gridY;
    }
    public int FCost 	// Attribute, F value
    {
        get
        {
            return gCost + hCost;
        }
    }
}

New scriptPathFindingAnd added to object a *

public class PathFinding : MonoBehaviour
{
    public Transform seeker, target; 	// Declare two coordinates, the starting point and the target point
    private MyGrid grid;
    ......
    private void Update()
    {
        FindPath(seeker.position, target.position); 	// Calculation path
    }
    private void FindPath(Vector3 startPos, Vector3 targetPos)
    {
        Node startNode = grid. NodeFromWorldPos(startPos);   // Enter the spatial coordinates and calculate the node position of the starting point
        Node targwtNode = grid. NodeFromWorldPos(targetPos); // Enter the spatial coordinates and calculate the node position of the target point

        List openSet = new List();          // Used to store nodes that need to be evaluated
        HashSet closedSet = new HashSet();  // Used to store nodes that have been evaluated

        openSet. Add(startNode); 	// Add the starting point to OpenSet for evaluation
        
        While (OpenSet. Count > 0) // if there are nodes to be evaluated
        {
            #Region // get the node with the lowest F value in the list to be evaluated
            Node currentNode = openSet[0];  // Get one of the nodes to be evaluated
            For (int i = 0; I < OpenSet. Count; I + +) // compare this node with all the nodes to be evaluated to find the node with the lowest F value, f
                								 //Nodes with the same value and smaller H value
            {
                if (openSet[i].FCost < currentNode.FCost 
                    || openSet[i].FCost == currentNode.FCost 
                    && openSet[i].hCost < currentNode.hCost)
                {
                    currentNode = openSet[i];
                }
            }
            #endregion

            openSet. Remove(currentNode);    // Remove the node with the lowest F value from the nodes to be evaluated
            closedSet. Add(currentNode);     // Add this node to the evaluated node and no longer participate in the evaluation
            
            If (currentnode = = targwtnode) // if the node is the destination, calculate the actual path and end the loop
            {
                RetracePath(startNode, targwtNode);
                return;
            }

            //If the node is not the target point, traverse all nodes around the point
            foreach (Node neighbor in grid.GetNeighbors(currentNode))
            {
                //If a surrounding node cannot walk or a surrounding node has been evaluated as the previous node, skip
                //This indicates that a node has been set as a parent node
                if (!neighbor.walkable || closedSet.Contains(neighbor))
                {
                    continue;
                }

                //Before calculation, the gcost value of the starting point to a node is 0  
                //After the loop, the g value of all surrounding nodes will be calculated here
                int newMovementCostToNeighbor = currentNode.gCost + GetDinstance(currentNode, neighbor);
                
                //If the gcost value of the new route is smaller (closer), or a node has not been evaluated (it is a new node)
                if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
                {
                                                                            
                    neighbor. gCost = newMovementCostToNeighbor;             // Calculate gcost of a node
                    neighbor. hCost = GetDinstance(neighbor, targwtNode);    // Calculate a node hcost
                    neighbor. parent = currentNode;                          // Make the intermediate node the parent of a node
                                                      //If there is a node with a smaller gcost, the intermediate node will be set as the parent node of a node again

                    If (! OpenSet. Contains (neighbor)) // if a node has not been evaluated
                    {
                        openSet. Add(neighbor);          // Add a node to the list to be evaluated and evaluate it in the next cycle,
                                                        //The next cycle will find out the nodes with the lowest F value of these surrounding nodes
                    }
                }
            }
        }
    }
    private void RetracePath(Node startNode, Node endNode) 	// Get actual path
    {
        List path = new List();
        Node currentNode = endNode;	
        while (currentNode != startNode) 	// If it is not currently the target point
        {
            path. Add(currentNode); 			// Add the current node to the path
            currentNode = currentNode. parent;// Get the next node (the parent node of the current node)
        }
        path. Reverse(); 		// Reverse the order of all elements
        grid. path = path; 	// Return actual path
    }
    private int GetDinstance(Node nodeA, Node nodeB) 	// Calculate the cost between two nodes
    {
        int dstX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
        int dstY = Mathf.Abs(nodeA.gridY - nodeB.gridY);
        if (dstX > dstY)
        {
            return 14 * dstY + 10 * (dstX - dstY);
        }
        return 14 * dstX + 10 * (dstY - dstX);
    }
}

Modify scriptMyGrid

public class MyGrid : MonoBehaviour
{
    ......

    public List path;
    ......
    void CreateGrid()
    {
        ......
        for (int x = 0; x < gridSizeX; x++)
        {
            for (int y = 0; y < gridSizeY; y++)
            {
                ...... 									// Two more parameters are added to facilitate the calculation of surrounding nodes
                grid[x, y] = new Node(walkable, worldPoint, x, y);    // Adds the data of the node to the binary array
            }
        }
    }
    ......
    public List GetNeighbors(Node node) 	// Get all nodes around the node
    {
        List neighbors = new List();
        //The relative coordinates of the node are X-1 on the left, x + 1 on the right, Y-1 below and y + 1 above
        for (int x = -1; x <= 1; x++)
        {
            for (int y = -1; y <= 1; y++)
            {
                if (x == 0 && y == 0) 	// Skip intermediate nodes
                {
                    continue;
                }
                //From the coordinates of X and Y relative to the middle node and the coordinates of the middle node in the map, the coordinates of the surrounding nodes in the map are obtained
                int checkX = node.gridX + x;
                int checkY = node.gridY + y;
                
                //Limit the range of nodes to prevent non-existent nodes outside the map
                if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY)
                {
                    neighbors. Add(grid[checkX, checkY]);// Add surrounding nodes
                }
            }
        }
        return neighbors;
    }

    private void OnDrawGizmos()
    {
        ......
                if (path != null)
                {
                    if (path.Contains(node)) 	// Add color to path
                    {
                        Gizmos.color = Color.yellow;
                    }
                }
                Gizmos.DrawCube(node.worldPos, Vector3.one * (nodeDiameter - 0.1f));
            }
        }
    }
}

Set the corresponding parameters in the inspector panel by yourself
Operation results


You can change the position of the start and end points at any time
Demo video:https://www.bilibili.com/video/BV14B4y127YN/
In the next a * routing algorithm 2.0, the array implementation heap will be used to replace the list storage node, and the time consumed by the algorithm will be reduced by about 60%