4
\$\begingroup\$

I am fairly new to programming, unity, and game design in general.
I am following an old A* unity tutorial by Sebastian Lang and the algorithm he used doesn't seem to be completely accurate in terms of obstacles that aren't directly beside the path.

This is the current result of my code

enter image description here

So... not quite optimal. As you can see, going from bottom to top, it's trying to match the target's x-value, but the 1st block is in the way. As soon as the block ends, it successfully matches the target's x-value but then has to go around the 2nd block. Optimally, it would know that the only real obstacle is the 2nd block.

It's possible that he will come back later in another future video and fix this problem, as I am only 5/10 videos in; however, I'd rather practice thinking like a programmer instead of being told what to do exactly.

My guess for a fix would be to draw some sort of obstacle checking line from the start to the target and use the position data of the intersecting obstacles as a reference for the max distance the path needs to go in a specific axis. But even then, certain obstacle layouts wouldn't work, because an obstacle can obstruct the way without being exactly between the 2 points.

I'm not sure where to really go from here, so any advice would be helpful.

The main concepts are in pseudocode so those who don't know C# can also help.

Pathfinding is calculated with the following pseudocode...

h_cost = distance from start node
g_cost = distance from target node
f_cost = h_cost + g_cost

Get the start node position
Get the target node position

create an OPEN set for nodes that need to be evaluated
create a CLOSED set for nodes that have already been evaluated
add start node to OPEN

loop
     current = node with lowest f_cost
     move current from OPEN to CLOSED

     if current is the target node
          return

     for each neighbor of current (4 cardinal sides, and 4 diagonals)
          if unwalkable (an obstacle) OR in Closed
               continue

          if new path to neighbor is shorter OR neighbor is not in OPEN
               set f_cost of neighbor
               set parent of neighbor to current
               if neighbor is not in OPEN
                    add neighbor to OPEN

Distance is calculated with the following pseudocode

 Distance between start and target x-value = x
 Distance between start and target y-value = y

 If x > y, then 14 * y + 10 * (x - y)  
 If x < y, then 14 * x + 10 * (y - x)

Heap.cs

    using System.Collections;
    using System.Collections.Generic;
    using UnityEngine;
    using System;

    public class Heap<T> where T : IHeapItem<T> {

        T[] items;
        int currentItemCount;

        public Heap(int maxHeapSize) {
            items = new T[maxHeapSize];
        }

        public void Add(T item) {
            item.HeapIndex = currentItemCount;
            items[currentItemCount] = item;
            SortUp(item);
            currentItemCount++;
        }

        public T RemoveFirst() {
            T firstItem = items[0];
            currentItemCount--;
            items[0] = items[currentItemCount];
            items[0].HeapIndex = 0;
            SortDown(items[0]);
            return firstItem;
        }

        public void UpdateItem(T item) {
            SortUp(item);
        }

        public int Count {
            get {
                return currentItemCount;
            }
        }

        public bool Contains(T item) {
            return Equals(items[item.HeapIndex], item);
        }

        void SortDown(T item) {
            while (true) {
                int childIndexLeft = item.HeapIndex * 2 + 1;
                int childIndexRight = item.HeapIndex * 2 + 2;
                int swapIndex = 0;

                if (childIndexLeft < currentItemCount) {
                    swapIndex = childIndexLeft;

                    if (childIndexRight < currentItemCount) {
                        if (items[childIndexLeft].CompareTo(items[childIndexRight]) < 0) {
                            swapIndex = childIndexRight;
                        }
                    }

                    if (item.CompareTo(items[swapIndex]) < 0) {
                        Swap(item, items[swapIndex]);
                    }
                    else {
                        return;
                    }
                }
                else {
                    return;
                }
            }
        }

        void SortUp(T item) {
            int parentIndex = (item.HeapIndex - 1) / 2;

            while (true) {
                T parentItem = items[parentIndex];
                if (item.CompareTo(parentItem) > 0) {
                    Swap(item, parentItem);
                }
                else {
                    break;
                }

                parentIndex = (item.HeapIndex - 1) / 2;
            }
        }

        void Swap(T itemA, T itemB) {
            items[itemA.HeapIndex] = itemB;
            items[itemB.HeapIndex] = itemA;
            int itemAIndex = itemA.HeapIndex;
            itemA.HeapIndex = itemB.HeapIndex;
            itemB.HeapIndex = itemAIndex;
        }

    }


    public interface IHeapItem<T> : IComparable<T> { 
        int HeapIndex {
            get;
            set;
        }
    }

Grid.cs

    using System.Collections;
    using System.Collections.Generic;
    using UnityEngine;

    public class Grid : MonoBehaviour {

        public bool onlyDisplayPathGizmos;
        public Transform player;
        public LayerMask unwalkableMask;
        public Vector2 gridWorldSize;
        public float nodeRadius;
        Node[,] grid;

        float nodeDiameter;
        int gridSizeX, gridSizeY;

        void Start() {
            nodeDiameter = nodeRadius * 2;
            gridSizeX = Mathf.RoundToInt(gridWorldSize.x / nodeDiameter);
            gridSizeY = Mathf.RoundToInt(gridWorldSize.y / nodeDiameter);
            CreateGrid();
        }

        public int MaxSize {
            get {
                return gridSizeX * gridSizeY;
            }
        }

        void CreateGrid() {
            grid = new Node[gridSizeX, gridSizeY];
            Vector3 worldBottomLeft = 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++) {
                    Vector3 worldPoint = worldBottomLeft + Vector3.right * (x * nodeDiameter + nodeRadius) + Vector3.forward * (y * nodeDiameter + nodeRadius);
                    bool walkable = !(Physics.CheckSphere(worldPoint, nodeRadius, unwalkableMask));
                    grid[x, y] = new Node(walkable, worldPoint, x, y);
                }
            }
        }

        public List<Node> GetNeighbors(Node node) {
            List<Node> neighbors = new List<Node>();

            for (int x = -1; x <= 1; x++) {
                for (int y = -1; y <= 1; y++) {
                    if (x == 0 && y == 0)
                        continue;

                    int checkX = node.gridX + x;
                    int checkY = node.gridY + y;

                    if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY) {
                        neighbors.Add(grid[checkX, checkY]);
                    }
                }
            }

            return neighbors;
        }
        public Node NodeFromWorldPoint(Vector3 worldPosition) {
            float percentX = (worldPosition.x + gridWorldSize.x / 2) / gridWorldSize.x;
            float percentY = (worldPosition.z + gridWorldSize.y / 2) / gridWorldSize.y;
            percentX = Mathf.Clamp01(percentX);
            percentY = Mathf.Clamp01(percentY);

            int x = Mathf.RoundToInt((gridSizeX - 1) * percentX);
            int y = Mathf.RoundToInt((gridSizeY - 1) * percentY);
            return grid[x, y];
        }

        public List<Node> path;
        void OnDrawGizmos() {
            Gizmos.DrawWireCube(transform.position, new Vector3(gridWorldSize.x, 1, gridWorldSize.y));

            if (onlyDisplayPathGizmos) {
                if (path != null) {
                    foreach (Node n in path) {
                        Gizmos.color = Color.black;
                        Gizmos.DrawCube(n.worldPosition, Vector3.one * (nodeDiameter - .1f));
                    }
                }
            }
            else {
                if (grid != null) {
                    Node playerNode = NodeFromWorldPoint(player.position);
                    foreach (Node n in grid) {
                        Gizmos.color = (n.walkable) ? Color.white : Color.red;
                        if (path != null)
                            if (path.Contains(n))
                                Gizmos.color = Color.black;
                        Gizmos.DrawCube(n.worldPosition, Vector3.one * (nodeDiameter - .1f));
                    }
                }
            }
        }
    }

Node.cs

    using System.Collections;
    using System.Collections.Generic;
    using UnityEngine;

    public class Node : IHeapItem<Node> {

        public bool walkable;
        public Vector3 worldPosition;
        public int gridX;
        public int gridY;

        public int gCost;
        public int hCost;
        public Node parent;
        int heapIndex;


        public Node(bool _walkable, Vector3 _worldPos, int _gridX, int _gridY) {
            walkable = _walkable;
            worldPosition = _worldPos;
            gridX = _gridX;
            gridY = _gridY;
        }

        public int fCost {
            get {
                return gCost = hCost;
            }
        }

        public int HeapIndex {
            get {
                return heapIndex;
            }
            set {
                heapIndex = value;
            }
        }

        public int CompareTo(Node nodeToCompare) {
            int compare = fCost.CompareTo(nodeToCompare.fCost);
            if (compare == 0) {
                compare = hCost.CompareTo(nodeToCompare.hCost);
            }
            return -compare;
        }

    }

Pathfinding.cs

    using System.Collections;
    using System.Collections.Generic;
    using UnityEngine;
    using System.Diagnostics;
    public class Pathfinding : MonoBehaviour {

        public Transform seeker, target;

        Grid grid;

        void Awake() {
            grid = GetComponent<Grid>();
        }

        void Update() {
            FindPath(seeker.position, target.position);
        }

        void FindPath(Vector3 startPos, Vector3 targetPos) {

            Stopwatch sw = new Stopwatch();
            sw.Start();

            Node startNode = grid.NodeFromWorldPoint(startPos);
            Node targetNode = grid.NodeFromWorldPoint(targetPos);

            Heap<Node> openSet = new Heap<Node>(grid.MaxSize);
            HashSet<Node> closedSet = new HashSet<Node>();
            openSet.Add(startNode);

            while (openSet.Count > 0) {
                Node currentNode = openSet.RemoveFirst();
                closedSet.Add(currentNode);

                if (currentNode == targetNode) {
                    sw.Stop();
                    print("Path found: " + sw.ElapsedMilliseconds + " ms");
                    RetracePath(startNode, targetNode);
                    return;
                }

                foreach (Node neighbor in grid.GetNeighbors(currentNode)) {
                    if (!neighbor.walkable || closedSet.Contains(neighbor)) {
                        continue;
                    }

                    int newMovementCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor);
                    if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor)) {
                        neighbor.gCost = newMovementCostToNeighbor;
                        neighbor.hCost = GetDistance(neighbor, targetNode);
                        neighbor.parent = currentNode;

                        if (!openSet.Contains(neighbor))
                            openSet.Add(neighbor);
                    }
                }
            }
        }

        void RetracePath(Node startNode, Node endNode) {
            List<Node> path = new List<Node>();
            Node currentNode = endNode;

            while (currentNode != startNode) {
                path.Add(currentNode);
                currentNode = currentNode.parent;
            }
            path.Reverse();

            grid.path = path;


        }

        int GetDistance(Node nodeA, Node nodeB) {
            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);

        }

    }
\$\endgroup\$
  • \$\begingroup\$ I don't think your distance functions for g and h are correct. g is the actual cost of the path from the start to the node, which can't be determined solely from the node's position relative to start. h is the estimated cost from the node to the finish, which must not overestimate. You're using the same code for g and h, which can't be correct - it would mean that h is a perfect estimator (it actually calculates the path length to finish), in which case you wouldn't need A* to start with. \$\endgroup\$ – Nuclear Wang Sep 12 at 20:56
  • \$\begingroup\$ @NuclearWang I'm not quite sure where you are seeing gCost and hCost using the same code. neighbor.gCost = newMovementCostToNeighbor; Current gCost + Distance from Current to Neighbor. neighbor.hCost = GetDistance(neighbor, targetNode); Distance from Neighbor to Target \$\endgroup\$ – EspieBodespie Sep 12 at 21:51
  • \$\begingroup\$ @AlexandreVaillancourt You're a damned genius. I've been trying to figure this out for at least 5 or 6 hours. Thank you so much! Do you want to write an answer, or do you want me to? \$\endgroup\$ – EspieBodespie Sep 13 at 1:25
  • \$\begingroup\$ I don't know if you've seen the comment from DMGregory, he spotted the actual issue: return gCost = hCost; should most likely be return gCost + hCost;. My proposed solution "worked" but is not the solution. \$\endgroup\$ – Alexandre Vaillancourt Sep 13 at 2:04
  • \$\begingroup\$ @AlexandreVaillancourt That completely fixed it and made it so much better. Where is this comment from DMGregory? \$\endgroup\$ – EspieBodespie Sep 13 at 2:29
5
\$\begingroup\$

It looks to me like the problem is just a typo here in your node type:

public int fCost { 
    get {
         return gCost = hCost;
      }
 }

This should be a +, not an =

This typo would result in your heuristic cost h alone being used when comparing items in the heap, so the algorithm would prefer nodes close to the target even if they have a larger total path distance to reach them, leading it to swerve inward between obstacles to minimize the heuristic instead of taking the shorter straight path.

This typo also pollutes your g cost, so g costs of its neighbours will come out wrong too.

\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.