Algorithm for finding the path A * in a voxel 3d game on Unity

Introduction

When developing my game, I got to the point of creation of the first NPCs. And the question was how to make the NPC go around the wall and not “go into it”.

Climbing on the Internet, I found these algorithms:

Search Wide (BFS, Breadth-First Search)
Dijkstra's Algorithm (Dijkstra)
And Star "A with an asterisk"
Search for the first best match (Best-First Search)
IDA (A with iterative deepening)
Jump Point Search

And I decided to try to implement my A * on the voxel 3d grid.

Sample card of my game

Algorithm Description

A * step through all the paths leading from the initial vertex to the final, until it finds the minimum. Like all informed search algorithms, he first looks at those routes that “appear” to lead to the goal. It differs from the greedy algorithm, which is also the search algorithm for the first best match, when choosing a vertex it takes into account, among other things, the entire path traveled to it.

Visualization of work A * from Wikipedia

Implementation

Since the algorithm needs "nodes" - points for defining the path, we write the class structure of the node:

Node code:

public enum EMoveAction { walk, jump, fall, swim }; public class PathPoint { //   public Vector3 point { get; set; } //    public float pathLenghtFromStart { get; set; } //     public float heuristicEstimatePathLenght { get; set; } //     public float estimateFullPathLenght { get { return this.heuristicEstimatePathLenght + this.pathLenghtFromStart; } } //   public EMoveAction moveAction = EMoveAction.walk; //      public PathPoint cameFrom; }

Small class constructors:

  private PathPoint NewPathPoint(Vector3 point, float pathLenghtFromStart, float heuristicEstimatePathLenght, EMoveAction moveAction) { PathPoint a = new PathPoint(); a.point = point; a.pathLenghtFromStart = pathLenghtFromStart; a.heuristicEstimatePathLenght = heuristicEstimatePathLenght; a.moveAction = moveAction; return a; } private PathPoint NewPathPoint(Vector3 point, float pathLenghtFromStart, float heuristicEstimatePathLenght, EMoveAction moveAction, PathPoint ppoint) { PathPoint a = new PathPoint(); a.point = point; a.pathLenghtFromStart = pathLenghtFromStart; a.heuristicEstimatePathLenght = heuristicEstimatePathLenght; a.moveAction = moveAction; a.cameFrom = ppoint; return a; }

Next, we need the structure of the path search settings:

Path Search Settings Code:

  public struct SPathFinderType { //   , , ,   public bool walk, jump, fall, swim; //   ,  public int maxFallDistance, jumpHeight, jumpDistance; //   public int characterHeight; //    public static SPathFinderType normal() { SPathFinderType n = new SPathFinderType(); n.walk = true; n.jump = true; n.fall = true; n.swim = false; n.maxFallDistance = 1; n.jumpHeight = 1; n.jumpDistance = 0; n.characterHeight = 1; return n; } }

Further, "World" is a class of a unique database for storing information about map blocks. You may have implemented differently.

The result of the pathfinding route retrieval function:

  public List<PathPoint> GetPathToTarget(Vector3 beginPoint, Vector3 targetPoint, World worldData, SPathFinderType pfType) { List<PathPoint> path = new List<PathPoint>(); //     List<PathPoint> openPoints = new List<PathPoint>(); //    List<PathPoint> closedPoints = new List<PathPoint>(); //      openPoints.Add(NewPathPoint(beginPoint, 0, GameLogic.Distance(beginPoint, targetPoint), EMoveAction.walk)); //   closedPoints.Add(openPoints[0]); //       openPoints = ClosePoint(0, openPoints, closedPoints, worldData, pfType, targetPoint); // " "    bool stopFlag = true; //        float maxEstimatePath = 1500; int maxNodes = 6000; while (stopFlag) { //        int minIndex = GetMinEstimate(openPoints); if (openPoints.Count > 0) if (openPoints[minIndex].estimateFullPathLenght < maxEstimatePath) { //   closedPoints.Add(openPoints[minIndex]); //       minIndex openPoints = ClosePoint(minIndex, openPoints, closedPoints, worldData, pfType, targetPoint); } else { //      //       closedPoints.Add(openPoints[minIndex]); openPoints.RemoveAt(minIndex); } //      if (FinishFounded(closedPoints)) { Debug.Log(" !"); path = GetPathToTarget(closedPoints); stopFlag = false; //      } if (openPoints.Count <= 0) stopFlag = false; //       if ((openPoints.Count>= maxNodes) ||(closedPoints.Count>= maxNodes)) stopFlag = false; //      } Debug.Log("Nodes created "+ closedPoints.Count.ToString()); //    DrawPath(openPoints, Color.green, 6f); DrawPath(closedPoints, Color.blue, 6f); DrawPath(path, Color.red, 6f); return path; }

GetMinEstimate

  //        private int GetMinEstimate(List<PathPoint> points) { int min = 0; for (int i = 0; i < points.Count; i++) { if (points[i].estimateFullPathLenght < points[min].estimateFullPathLenght) min = i; } return min; }

Drawpath

  //      public void DrawPath(List<PathPoint> points, Color c, float time) { for (int i = 0; i < points.Count; i++) { if (points[i].cameFrom != null) Debug.DrawLine(points[i].point, points[i].cameFrom.point, c, time); } }

FinishFounded

  //     private bool FinishFounded(List<PathPoint> points) { for (int i = 0; i < points.Count; i++) { if (points[i].heuristicEstimatePathLenght <= 0) return true; } return false; }

GetPathToTarget

  //        private List<PathPoint> GetPathToTarget(List<PathPoint> points) { List<PathPoint> path = new List<PathPoint>(); int targetIndex = 0; for (int i = 0; i < points.Count; i++) { if (points[i].heuristicEstimatePathLenght <= 0) targetIndex = i; } PathPoint ppoint = new PathPoint(); ppoint = points[targetIndex]; while (ppoint.pathLenghtFromStart > 0) { path.Add(ppoint); ppoint = ppoint.cameFrom; } path.Reverse(); return path; }

Closepoint

The ClosePoint function depends entirely on the implementation of the World class; it adds all possible paths from it to the list of open points and removes the current point from this list (closes it). I will give an example of my "closing point" in the first four directions.

Attention big code jumble

  private List<PathPoint> ClosePoint(int index, List<PathPoint> openPoints, List<PathPoint> closedPoints, World worldData, SPathFinderType pfType, Vector3 targetPoint) { List<PathPoint> newOpenPoints = openPoints; PathPoint lastPoint = openPoints[index]; //     if (pfType.walk) //        if (CanStand(new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z), pfType.characterHeight, worldData)) { // --------------------------------------------------------------- //north // /|\ // | //      if (!InList(closedPoints, new Vector3(lastPoint.point.x + 1, lastPoint.point.y, lastPoint.point.z))) //     if (!InList(newOpenPoints, new Vector3(lastPoint.point.x + 1, lastPoint.point.y, lastPoint.point.z))) //     if (CanStand(new Vector3(lastPoint.point.x + 1, lastPoint.point.y, lastPoint.point.z), pfType.characterHeight, worldData)) { newOpenPoints.Add(NewPathPoint(new Vector3(lastPoint.point.x + 1, lastPoint.point.y, lastPoint.point.z) , lastPoint.pathLenghtFromStart + GetTravelCost(new Vector3(lastPoint.point.x + 1, lastPoint.point.y, lastPoint.point.z), worldData, pfType.characterHeight) , GameLogic.Distance(new Vector3(lastPoint.point.x + 1, lastPoint.point.y, lastPoint.point.z), targetPoint) , EMoveAction.walk , lastPoint)); } // south // | // \|/ //      if (!InList(closedPoints, new Vector3(lastPoint.point.x - 1, lastPoint.point.y, lastPoint.point.z))) //     if (!InList(newOpenPoints, new Vector3(lastPoint.point.x - 1, lastPoint.point.y, lastPoint.point.z))) //     if (CanStand(new Vector3(lastPoint.point.x - 1, lastPoint.point.y, lastPoint.point.z), pfType.characterHeight, worldData)) { newOpenPoints.Add(NewPathPoint(new Vector3(lastPoint.point.x - 1, lastPoint.point.y, lastPoint.point.z) , lastPoint.pathLenghtFromStart + GetTravelCost(new Vector3(lastPoint.point.x - 1, lastPoint.point.y, lastPoint.point.z), worldData, pfType.characterHeight) , GameLogic.Distance(new Vector3(lastPoint.point.x - 1, lastPoint.point.y, lastPoint.point.z), targetPoint) , EMoveAction.walk , lastPoint)); } // east // ----> // //      if (!InList(closedPoints, new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z + 1))) //     if (!InList(newOpenPoints, new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z + 1))) //     if (CanStand(new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z + 1), pfType.characterHeight, worldData)) { newOpenPoints.Add(NewPathPoint(new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z + 1) , lastPoint.pathLenghtFromStart + GetTravelCost(new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z + 1), worldData, pfType.characterHeight) , GameLogic.Distance(new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z + 1), targetPoint) , EMoveAction.walk , lastPoint)); } // west // <---- // //      if (!InList(closedPoints, new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z - 1))) //     if (!InList(newOpenPoints, new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z - 1))) //    if (CanStand(new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z - 1), pfType.characterHeight, worldData)) { newOpenPoints.Add(NewPathPoint(new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z - 1) , lastPoint.pathLenghtFromStart + GetTravelCost(new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z - 1), worldData, pfType.characterHeight) , GameLogic.Distance(new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z - 1), targetPoint) , EMoveAction.walk , lastPoint)); } } newOpenPoints.RemoveAt(index); return newOpenPoints; }

Optimization

By simply dividing the path from the start to the current point, we reduce the number of nodes many times and make it more "greedy".

 return this.heuristicEstimatePathLenght + this.pathLenghtFromStart /2;

Results

Pros:

Quick search in open spaces.
Universality of the algorithm

Minuses:

It requires a lot of memory to calculate the route.

Green is the open list of nodes, red is the way to the target, blue is closed nodes.

Received routes to optimization:

Received routes after optimization:

Literature

https://tproger.ru/articles/pathfindings/
https://ru.wikipedia.org/wiki/A*

Source: https://habr.com/ru/post/416737/

All Articles