A* Pathfinding is not working in a 3D Minecraft Environment

Question

Quite a long time ago I tried implementing a Minecraft Bot with pathfinding using a modified client and a simple breadth first search algorithm which worked, but was extremely imperformant. Nevertheless I actually got it as far as being able to decide whether it might even be more intelligent to break certain blocks or place them to get to its goal.

So I started implementing my own Minecraft Client, which works 'OK' I guess :D Also I wanted to use A* Pathfinding for better performance. Unfortunately after implementing it I started to see that it worked great in most cases, but as soon as the path got more complicated it would just calculate for minutes straight without finding a path to the goal. It seems like it either searches in the wrong direction or skips the goal entirely somehow. Either way, it doesn't find it. In addition to that after implementing jumping to be able to jump over gaps it would just keep jumping no matter if he had to or not. Looks like a happy Minecraft player jumping around, but doesn't make any sense. Even after setting the cost of jumping to an extremely large number, it would keep jumping everywhere instead of walking normally.

I've started thinking there must be a fundamental error in this approach but couldn't find out what it might be.

This is my pathfinding logic

class PathFinder
{
    private const int WalkCost = 10;
    private const int DiagonalWalkCost = 14;
    private const int JumpCost = 5;

    private const int MaxDistance = 100;

    private int GetDistance(Node a, Node b)
    {
        return Math.Abs(b.X - a.X) + Math.Abs(b.Y - a.Y) + Math.Abs(b.Z - a.Z);
    }

    public Path FindPath(Node start, Node goal)
    {
        start.H = GetDistance(start, goal) * WalkCost;
        var open = new List<Node>();
        open.Add(start);
        var closed = new List<Node>();
        var cameFrom = new Dictionary<Node, Node>();
        var action = new Dictionary<Node, Movement>();
        var done = false;
        var considered = 0;
        var world = Client.Instance.World;

        Node current;
        while (open.Count > 0)
        {
            current = open.OrderBy(n => n.F).First();

            open.Remove(current);
            closed.Add(current);

            if (current == goal)
            {
                done = true;
                break;
            }

            foreach (var neighbour in GetNeighbours(current))
            {
                if (GetDistance(neighbour, start) > MaxDistance)
                    continue;

                if (closed.Contains(neighbour))
                    continue;

                if (neighbour.Y > current.Y) //Jump up
                {
                    if (!IsWalkable(neighbour)
                        || world.IsSolid(current.Up().Up()))
                        continue;

                    neighbour.G = current.G + WalkCost + JumpCost;
                    action[neighbour] = Movement.JumpUp;
                }
                else if(neighbour.Y < current.Y) //Jump Down
                {
                    if (!IsWalkable(neighbour)
                        || world.IsSolid(neighbour.Up().Up()))
                        continue;

                    neighbour.G = current.G + WalkCost + JumpCost;
                    action[neighbour] = Movement.Fall;
                }
                else //Walk
                {
                    if (!IsWalkable(neighbour))
                        continue;

                    if (neighbour.X != current.X && neighbour.Z != current.Z) //Diagonal
                    {
                        if (world.IsSolid(new Node(neighbour.X, current.Y, current.Z))
                            || world.IsSolid(new Node(neighbour.X, current.Y, current.Z).Up())
                            || world.IsSolid(new Node(current.X, current.Y, neighbour.Z))
                            || world.IsSolid(new Node(current.X, current.Y, neighbour.Z).Up()))
                            continue;

                        neighbour.G = current.G + DiagonalWalkCost;
                        action[neighbour] = Movement.Walk;
                    }
                    else //Straight
                    {
                        if(GetDistance(current, neighbour) >= 2) //Straight Jump
                        {
                            var nodeBetween = new Node(current.X + (neighbour.X - current.X) / 2, current.Y, current.Z + (neighbour.Z - current.Z) / 2);
                            if (world.IsSolid(current.Up().Up())
                                || world.IsSolid(neighbour.Up().Up())
                                || world.IsSolid(nodeBetween)
                                || world.IsSolid(nodeBetween.Up())
                                || world.IsSolid(nodeBetween.Up().Up()))
                                continue;

                            neighbour.G = current.G + 2 * WalkCost + JumpCost;
                            action[neighbour] = Movement.JumpGap;
                        }
                        else //Straight Walk
                        {
                            neighbour.G = current.G + WalkCost;
                            action[neighbour] = Movement.Walk;
                        }
                    }
                }

                considered++;

                var previousEntry = open.FirstOrDefault(n => n == neighbour);
                if (previousEntry == null || neighbour.G < previousEntry.G)
                {
                    neighbour.H = GetDistance(neighbour, goal) * WalkCost;
                    cameFrom[neighbour] = current;
                    open.Add(neighbour);
                }
            }
        }

        Client.Instance.SendChatMessage(String.Format("{0} possible moves considered", considered));

        if (!done)
            return null;

        var currentNode = goal;
        var path = new List<Node>();
        var moves = new List<Movement>();
        while (currentNode != start)
        {
            path.Add(currentNode);
            moves.Add(action[action.Keys.First(k => k == currentNode)]);
            currentNode = cameFrom[cameFrom.Keys.First(k => k == currentNode)];
        }
        path.Add(start);
        path.Reverse();
        moves.Add(Movement.Walk);
        moves.Reverse();
        return new Path(path, moves);
    }

    private bool IsWalkable(Node node)
    {
        var world = Client.Instance.World;
        var block = world.GetBlock(node.X, node.Y, node.Z);
        return world.IsSolid(node.Down())
            && !world.IsSolid(node)
            && !world.IsSolid(node.Up())
            && !block.BlockName.Contains("water")
            && !block.BlockName.Contains("lava");
    }

    private Node[] GetNeighbours(Node node)
    {
        var neighbours = new List<Node>();

        //neighbours.Add(node.Up()); //pillar up
        //neighbours.Add(node.Down()); //dig down

        neighbours.Add(node.North()); //simple walk
        neighbours.Add(node.East());
        neighbours.Add(node.South());
        neighbours.Add(node.West());

        neighbours.Add(node.North().East()); //diagonal walk
        neighbours.Add(node.North().West());
        neighbours.Add(node.South().East());
        neighbours.Add(node.South().West());

        neighbours.Add(node.North().Up()); //jump up
        neighbours.Add(node.East().Up());
        neighbours.Add(node.South().Up());
        neighbours.Add(node.West().Up());

        neighbours.Add(node.North().Down()); //fall down
        neighbours.Add(node.East().Down());
        neighbours.Add(node.South().Down());
        neighbours.Add(node.West().Down());

        neighbours.Add(node.North().North()); //jump 1 wide gap
        neighbours.Add(node.East().East());
        neighbours.Add(node.South().South());
        neighbours.Add(node.West().West());

        return neighbours.ToArray();
    }
}

And this is my Node class

public class Node/* : IEquatable<Node>*/
{
    public int X { get; set; }
    public int Y { get; set; }
    public int Z { get; set; }

    public int G { get; set; }
    public int H { get; set; }

    public int F { get => G + H; }

    public Node CameFrom { get; set; }

    public Node(int x, int y, int z)
    {
        X = x;
        Y = y;
        Z = z;
    }

    public Node()
    {

    }

    public Node Down()
    {
        return new Node(X, Y - 1, Z);
    }

    public Node Up()
    {
        return new Node(X, Y + 1, Z);
    }

    public Node East()
    {
        return new Node(X + 1, Y, Z);
    }

    public Node West()
    {
        return new Node(X - 1, Y, Z);
    }

    public Node South()
    {
        return new Node(X, Y, Z + 1);
    }
    public Node North()
    {
        return new Node(X, Y, Z - 1);
    }

    //public bool Equals(Node other)
    //{
    //    return other.X == X && other.Y == Y && other.Z == Z;
    //}

    public override bool Equals(object obj)
    {
        if (!(obj is Node))
            return false;

        return this == (Node)obj;
    }

    public static bool operator ==(Node a, Node b)
    {
        if (a is null && b is object || b is null && a is object)
            return false;
        if (a is null && b is null)
            return true;
        return a.X == b.X && a.Y == b.Y && a.Z == b.Z;
    }

    public static bool operator !=(Node a, Node b)
    {
        if (a is null && b is object || b is null && a is object)
            return true;
        if (a is null && b is null)
            return false;

        return a.X != b.X || a.Y != b.Y || a.Z != b.Z;
    }

    //public static bool operator ==(Node a, Node b)
    //{
    //    return a.GetHashCode() == b.GetHashCode();
    //}

    //public static bool operator !=(Node a, Node b)
    //{
    //    return a.GetHashCode() != b.GetHashCode();
    //}

    public override int GetHashCode()
    {
        int hash = 17;
        hash = hash * 23 + X.GetHashCode();
        hash = hash * 23 + Y.GetHashCode();
        hash = hash * 23 + Z.GetHashCode();
        return hash;
    }
}

Ignore the chaotic equality checks. I was thinking it might have to do with the way I check equality in nodes but had no success with that either, besides my code being messier than it was before xD

Does anybody have any suggestions on what might be the problem here ? Any articles on something similar, that means pathfinding in a 3D environment with jumping over gaps, would be greatly appreciated aswell.

Addition

This is a visualization of what a run looks like that took 16.5sec, 62.700 possible neighbours looked at and eventually a path of 48 moves total.

The bot was trying to go from the orange block on the right, to the blue block at the end of this ladder kind of thing, which is in the air, which makes it harder for pathfinding to be fair:

This is the visualization:

Green: Path taken, Blue: Closed List, Light Blue: Open List

From what I can see, the pathfinding seems to be working, but my uneducated assumption would be, that it is quite ineffective as it had a lot of blocks in the closed list before finding the goal eventually.

This time seems to increase exponentially with longer paths, as it takes up to half an hour or so to find the goal sometimes, which I would consider as not working.

Answer 1

Another try: What you can try to improve performance:

• Since you already implented get HashCode, you can change the list in HastSet, especially closed because you are using contains on it and with List it's in O(n), with HashSet in O(1)
• The OrderBy.First() is not the best way to do it because it has to sort the list which also takes a lot of time. Maybe you can do the same thing with Min() and Select()

Can you do that and tell me if it works better ? I tried on my computer and it seems to work.

Answer 2

I haven't seen all the code but the GetDistance seems wrong. Let's say that we forget Y. (1,1) and (2,2) would have a distance of 2 with your method so the same distance as (1,1) and (1,3) which is wrong, you can make a draw to see that.

For a 3D distance the calcul is

float deltaX = b.x - a.x;
float deltaY = b.y - a.y;
float deltaZ = b.z - a.z;

float distance = (float) Math.Sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ);