I. Introduction
In the urban game scene of our team's project, I implemented the code for vehicle pathfinding, and this blog will simply record the work content.
2. Code implementation
PathFinding:
using System.Collections;
using System.Collections.Generic;
using System;
using UnityEngine;
#if UNITY_EDITOR
using UnityEditor;
using UnityEditorInternal;
#endif
namespace PolyPerfect.City
{
public class PathFinding : MonoBehaviour
{
private BinaryHeap Open =new BinaryHeap(2048);
private Dictionary<Guid, int> OpenIDs = new Dictionary<Guid, int>();
private Dictionary<Guid, PathNode> Closed = new Dictionary<Guid, PathNode>();
[HideInInspector]
public List<Path> PathList = new List<Path>();
[HideInInspector]
public List<Path> wholePath;
private Vector3 startPoint;
private Vector3 endPoint;
private Tile endTile;
private Tile startTile;
private Guid endId;
public List<Path> GetPath(Vector3 start, Vector3 end, PathType pathType)
{
Open.Clear();
PathList = new List<Path>();
Closed.Clear();
OpenIDs.Clear();
startPoint = start;
endPoint = end;
startTile = FindClosestTile(startPoint, pathType);
endTile = FindClosestTile(endPoint, pathType);
List<Path> startPaths;
if(pathType == PathType.Sidewalk)
{
startPaths = startTile.sidewalkPaths;
}
else
startPaths = startTile.paths;
foreach (Path path in startPaths)
{
float h = CalculateHeuristic(path.pathPositions[path.pathPositions.Count - 1].position);
float g = Vector3.Distance(transform.position, path.pathPositions[0].position) + Vector3.Distance(transform.position, path.pathPositions[path.pathPositions.Count-1].position);
PathNode node = new PathNode() {
path = path, lastNode = null, currentScore = g, score = h +g};
Open.Insert(node);
OpenIDs.Add(node.path.Id, 0);
}
if(DoBfsSteps())
{
Closed[endId].path = FindClosestPath(endPoint, Closed[endId].lastNode.path.nextPaths);
GetPathList(Closed[endId]);
PathList.Reverse();
//PathList[0] = FindClosestPath(startPoint, startPaths);
return PathList;
}
else
{
return null;
}
}
public List<Path> GetPathWithCheckpoints(List<Vector3> checkpoints, PathType pathType)
{
wholePath = new List<Path>();
for (int i = 1; i < checkpoints.Count ;i++)
{
startPoint = checkpoints[i-1];
endPoint = checkpoints[i];
Open.Clear();
PathList = new List<Path>();
OpenIDs.Clear();
startTile = FindClosestTile(checkpoints[i-1], pathType);
endTile = FindClosestTile(checkpoints[i], pathType);
Closed.Clear();
List<Path> startPaths;
if (i == 1)
{
if (pathType == PathType.Sidewalk)
startPaths = startTile.sidewalkPaths;
else
startPaths = startTile.paths;
}
else
startPaths = wholePath[wholePath.Count - 2].nextPaths;
foreach (Path path in startPaths)
{
float h = CalculateHeuristic(path.pathPositions[path.pathPositions.Count - 1].position);
float g = Vector3.Distance(transform.position, path.pathPositions[0].position) + Vector3.Distance(transform.position, path.pathPositions[path.pathPositions.Count - 1].position);
PathNode node = new PathNode() {
path = path, lastNode = null, currentScore = g, score = h + g};
Open.Insert(node);
OpenIDs.Add(node.path.Id, 0);
}
if (DoBfsSteps())
{
if(i == checkpoints.Count-1)
Closed[endId].path = FindClosestPath(checkpoints[i], Closed[endId].lastNode.path.nextPaths);
GetPathList(Closed[endId]);
PathList.Reverse();
//PathList[0] = FindClosestPath(startPoint, startPaths);
if(i>1)
{
wholePath.RemoveAt(wholePath.Count - 1);
}
wholePath.AddRange(PathList);
}
else
{
return null;
}
}
return wholePath;
}
private bool DoBfsSteps()
{
//int i = 0;
while (Open.Count > 0)
{
PathNode node = GetBestNode();
//Debug.Log(i++ + " " + (node.score) + " "+ node.path.transform.parent.parent.name);
if (node.path.TileId == endTile.Id)
{
Closed.Add(node.path.Id, node);
endId = node.path.Id;
return true;
}
foreach (Path item in node.path.nextPaths)
{
if (item != null)
{
if(!Closed.ContainsKey(item.Id) && !OpenIDs.ContainsKey(item.Id))
{
Vector3 distance = item.pathPositions[0].position - item.pathPositions[item.pathPositions.Count-1].position;
float currentScore = node.currentScore + Math.Abs(distance.x) + Math.Abs(distance.y) + Math.Abs(distance.z) - ((item.speed /10) * item.transform.lossyScale.x);
Open.Insert(new PathNode() {
path = item, lastNode = node, currentScore = currentScore, score = CalculateHeuristic(item.pathPositions[item.pathPositions.Count - 1].position) + currentScore });
OpenIDs.Add(item.Id, 0);
}
}
}
Closed.Add(node.path.Id, node);
}
return false;
}
private PathNode GetBestNode()
{
PathNode pathNode = Open.PopTop();
OpenIDs.Remove(pathNode.path.Id);
return pathNode;
}
private float CalculateHeuristic(Vector3 currentTile)
{
Vector3 distance = endPoint - currentTile;
return Math.Abs(distance.x) + Math.Abs(distance.y) + Math.Abs(distance.z);
}
private void GetPathList(PathNode thisNode)
{
if (thisNode != null)
{
PathList.Add(thisNode.path);
GetPathList(thisNode.lastNode);
}
}
private Tile FindClosestTile(Vector3 point, PathType pathType)
{
Tile closestTile = null;
Collider[] coliders = Physics.OverlapBox(point, new Vector3(Mathf.Abs(2 * transform.lossyScale.x), Mathf.Abs(2 * transform.lossyScale.y), Mathf.Abs(2 * transform.lossyScale.z)));
foreach (Collider collider in coliders)
{
closestTile = collider.transform.GetComponent<Tile>();
if (closestTile != null)
{
if (pathType == PathType.Road)
{
if(closestTile.tileType == Tile.TileType.Road || closestTile.tileType == Tile.TileType.RoadAndRail)
{
return closestTile;
}
}
else if(pathType == PathType.Rail)
{
if (closestTile.tileType == Tile.TileType.Rail || closestTile.tileType == Tile.TileType.RoadAndRail)
{
return closestTile;
}
}
else if (pathType == PathType.Sidewalk)
{
if (closestTile.tileType == Tile.TileType.Road || closestTile.tileType == Tile.TileType.OnlyPathwalk)
{
return closestTile;
}
}
}
}
float minDistance = Mathf.Infinity;
foreach (Tile tile in Tile.tiles)
{
float distance = Vector3.Distance(tile.transform.position, point);
if (distance < minDistance)
{
if (pathType == PathType.Road)
{
if (tile.tileType == Tile.TileType.Road || tile.tileType == Tile.TileType.RoadAndRail)
{
minDistance = distance;
closestTile = tile;
}
}
else if (pathType == PathType.Rail)
{
if (tile.tileType == Tile.TileType.Rail || tile.tileType == Tile.TileType.RoadAndRail)
{
minDistance = distance;
closestTile = tile;
}
}
else if (pathType == PathType.Sidewalk)
{
if (tile.tileType == Tile.TileType.Road || tile.tileType == Tile.TileType.OnlyPathwalk)
{
minDistance = distance;
closestTile = tile;
}
}
}
}
return closestTile;
}
private Path FindClosestPath(Vector3 point, List<Path> paths)
{
Path closestPath = null;
float minDistance = Mathf.Infinity;
foreach (Path path in paths)
{
for (int i = 0; i < path.pathPositions.Count; i++)
{
float distance = Vector3.Distance(path.pathPositions[i].position, point);
if (distance < minDistance)
{
minDistance = distance;
closestPath = path;
}
}
}
return closestPath;
}
}
#region Editor
#if UNITY_EDITOR
[CustomEditor(typeof(PathFinding)), CanEditMultipleObjects]
public class CustomPathEditor : Editor
{
PathFinding navPath;
private void OnEnable()
{
navPath = target as PathFinding;
}
void OnSceneGUI()
{
if (navPath.wholePath.Count == 0)
{
if (navPath.PathList.Count != 0)
{
for (int i = 0; i < navPath.PathList.Count; i++)
{
if (navPath.PathList[i] != null)
{
if (i < navPath.PathList.Count - 1)
{
for (int j = 1; j < navPath.PathList[i].pathPositions.Count; j++)
{
Handles.color = Color.white;
Handles.DrawLine(navPath.PathList[i].pathPositions[j - 1].position, navPath.PathList[i].pathPositions[j].position);
Handles.color = Color.blue;
Handles.ArrowHandleCap(0, navPath.PathList[i].pathPositions[j - 1].position, Quaternion.LookRotation(navPath.PathList[i].pathPositions[j].position - navPath.PathList[i].pathPositions[j - 1].position), 3f, EventType.Repaint);
if (i == 0)
Handles.color = Color.blue;
else if (i == navPath.PathList.Count - 1)
Handles.color = Color.red;
else
Handles.color = Color.white;
Handles.SphereHandleCap(0, navPath.PathList[i].pathPositions[j].position, Quaternion.LookRotation(navPath.PathList[i].pathPositions[j].position), 0.2f, EventType.Repaint);
}
}
}
}
}
}
else
{
for (int i = 0; i < navPath.wholePath.Count; i++)
{
if (navPath.wholePath[i] != null)
{
if (i < navPath.wholePath.Count - 1)
{
for (int j = 1; j < navPath.wholePath[i].pathPositions.Count; j++)
{
Handles.color = Color.white;
Handles.DrawLine(navPath.wholePath[i].pathPositions[j - 1].position, navPath.wholePath[i].pathPositions[j].position);
Handles.color = Color.blue;
Handles.ArrowHandleCap(0, navPath.wholePath[i].pathPositions[j - 1].position, Quaternion.LookRotation(navPath.wholePath[i].pathPositions[j].position - navPath.wholePath[i].pathPositions[j - 1].position), 3f, EventType.Repaint);
if (i == 0)
Handles.color = Color.blue;
else if (i == navPath.wholePath.Count - 1)
Handles.color = Color.red;
else
Handles.color = Color.white;
Handles.SphereHandleCap(0, navPath.wholePath[i].pathPositions[j].position, Quaternion.LookRotation(navPath.wholePath[i].pathPositions[j].position), 0.2f, EventType.Repaint);
}
}
}
}
}
}
}
#endif
#endregion
}
3. Algorithm learning
A* algorithm
Although Unity provides us with Navigation as our pathfinding solution, in practice we also have to use our own solution to achieve it, and A is our most commonly used method.
A search algorithm is a computer algorithm to solve the problem of graph traversal, which is an algorithm for finding the lowest passing cost on a graph plane with paths of multiple nodes. The main application is to find the best route between two points on the map.
A's ability to change its own behavior is based on a heuristic cost function* that strikes a compromise between speed and accuracy to make the game run faster.
Before introducing the heuristic search algorithm, first introduce the state space search. State space search is the process of expressing the solution of a problem from the initial state to the target state. In this process, we can use a data structure such as a tree to express. Since there are many ways to solve (tree branches), there are many ways for us to reach the target state. This process is called state space search.
State space search is also divided into: depth-first and breadth-first.
Breadth-first is to search down from the initial state layer by layer until it is found.
Depth-first is to search for a branch in a certain order and then search for another branch until it is found.
Depth-first and breadth-first algorithms are good when the state space is small, but not if the state space is very large and unpredictable. It will be very inefficient, or even impossible to complete. This is where heuristic search is needed.
Heuristic search algorithm:
The heuristic search algorithm is to evaluate each search branch in the state space, get the best branch, and then search from this branch to reach the goal. This can omit a large number of fearless search paths and improve efficiency.
Among them, the evaluation of branches is very important, and different evaluations will have different effects. The following are some evaluation algorithms:
Local optimal search: During the search process, after selecting the "best node" in the branch of this layer, other sibling nodes and parent nodes are discarded, and the search continues. Since other nodes are discarded, it is also likely that the global best node is also discarded.
Best-first search: an optimized version for local selection. When searching, after finding the best node, it does not discard other nodes. In each evaluation, the current node is compared with the estimated value of the previous node to get a best node.
Algorithm A: The A ** algorithm is also a best-first algorithm, but it is not subject to some conditional constraints. Because when solving some problems, we hope to find the shortest path of state space search, that is, solve the problem with the fastest solution. Let's define the problem first. If an evaluation method can find the shortest path, we call it admissibility. Algorithm A is an admissible best-first algorithm.
Valuation Function
The valuation in the heuristic is expressed by the valuation function:
f(n)=g(n)+h(n)
where f(n) is the evaluation function of node n, g(n) is the actual cost from the initial node to node n in the state space, and h(n) is the estimated cost of the best path from n to the target node. Here mainly h(n) embodies the heuristic information of the search, because g(n) is known. g(n) represents the breadth-first trend of the search. But when h(n)>g(n), g(n) can be omitted to improve efficiency.
Pixelated map.
We want to control the AI path first we need to pixelate the map. What is a pixelated map? Just like the grid of unity. Gridding a map according to a certain scale is pixelation. Each of the grid units is called a node (it is not represented by a square because these grid units are not the same as rectangles or hexagons, circles, etc.). The node is called the unit of the pathfinding algorithm.
Example image:
Using a square as a pathfinding algorithm unit
Visual Navigation Points
Navigation mesh, the walkable area is divided into convex polygons.
The first step in pathfinding is to reduce it to a manageable search area.
Then we need to simulate and calculate the shortest path for pathfinding.
Thinking from our point of view, there are two shortest paths for cats to food.
1-2-3-4-5-6-food
1-2-3-4-5-7-food
So what does the computer do?
The most critical part when we calculate the shortest path is to calculate the distance! .
And this distance is not a straight-line distance, but the Manhattan distance.
Manhattan distance
Manhattan distance is a geometric term used in geometric metric spaces to indicate the sum of the absolute axis distances of two points on a standard coordinate system. The red line in the figure represents the Manhattan distance, the green represents the Euclidean distance, that is, the straight-line distance, and the blue and yellow represent the equivalent Manhattan distance.
Manhattan distance: the distance between two points in the X-axis direction plus the distance between two points in the Y-axis direction.
For example, on a plane, the Manhattan distance between point i of coordinates (x1, y1) and point j of coordinates (x2, y2) is:
d(i,j)=|X1-X2|+|Y1-Y2|
For the above map of cats:
If the square in the lower left corner is the point (0,0), then the coordinates of the cat are (1,2), and the coordinates of the food are (5,1), then their Manhattan distance is d(i,j)= |1-5|+|2-1|=5. The two red lines in the figure are the Manhattan distance. (without considering obstacles)
f-value, f(n), estimated value function. If you want to go from the starting point A to B, you may pass through n points in the middle. If you pass through point x, the estimated value calculated for point x is f(x), and the smaller the estimated value, the better.
g value: the actual cost to reach point x from starting point A. Taking the square grid as an example, the g value represents the number of grids from A to point x.
h value, the value that it may take to go from point X to end point B, the so-called estimated value. f(x)=g(x)+h(x), g(x) is fixed, if f(x) is smaller, h(x) must be smaller.
Pathfinding algorithm.
As shown in the figure, since there are obstacles under the cat, the cat can move in three directions. If the F values are 5, 7, and 7, then the path with the smaller F value, that is, 5, will be selected for movement.
After moving. Then there are two routes of 5 and 7, continue to choose the route of 5 to move
But when we find that we are continuing to move, the following problems will arise. We started to choose the best path, but at this time there are four paths with valuations of 7.
Let's continue walking along the path first.
In the end we have two shortest paths. (Paths we didn't consider with computers before)
If not move along the previous closest point. (Valuations of other routes tested by simulation will be larger)
Implemented using algorithms. (Open list and Close list)
First of all, we need a list to save and retrieve all walkable routes (Open list). When we pass a path, we need to know whether this path has been retrieved. (Close list)
Open List: Records all blocks considered to find the shortest path.
Close List: Keeps track of blocks that will no longer be considered.
Steps for cat pathfinding:
Add the block to the open list, which has the smallest F value, and mark this block as S.
Remove S from the open list and add it to the closed list.
Mark each passable block adjacent to S as T.
If T is in the closed list, it is not considered.
If T is not in the open list, add it and calculate the F value.
If T is already in open, check if F is smaller when getting there using the currently generated path. If yes, update its F value and move on.
When encountering such a bifurcated path with multiple identical F values, we need a parent node to record the path traveled and trace back the path.
pseudocode:
