(Issues solved) I did not multiple the heuristic by the correct cost. But... why would this have mattered in the way it could not find a path? It should visit each node anyway afaik. If I simply return 0 on the heuristic it should work like Dijkstra but it is not adding anything to the openList. Anyway, this obviously fixed the performance issue by a great deal since it was not sorting correctly and the closed list was getting huge.

Original post

I am trying to make my own Hierarchical pathfinding system and apart from the fact I still think it's slow I'm having a issue I cannot put my finger on. I want to solve this issue first before I continue. I will explain what I have and start with this picture to visualize it for you. It is the top right of the map, so no node connection on the far top and far right are needed since the map ends there.

I have divided the map into "sectors" and connect these sectors by nodes (white circles). In the sector with the blue circles I have selected the single white one and painting all connected nodes blue, so afaik this is correctly implemented. The red ones are the path I generated from the bottom left to the bottom right.

When I request a path I create a node at the start and destination and add all reachable nodes in that sector as connections with corresponding costs. Then I add the start node to the open list and perform A* search.

The first time this always finds a path if there is a path. But somehow if I try to find a path between topleft-bottomright and then topright-bottomleft it won't find a path for the seconds one. If I start my program and do that in reversed order it won't find a path for topleft-bottomright. On a smaller scale, so not traversing the complete map but just a couple of sectors there does not seem to be a problem.

public List<Node> findNodePath(Coordinates start, Coordinates destination)
    {
        long time = System.nanoTime();
        List<Node> path = new ArrayList<Node>();
        boolean pathFound = false;

        //create startnode and connect it to sector exits.
        Node startNode = findExitNodes(start); //Find exit nodes for the sector start is in
        startNode.setH(manhattanHeuristic(start, destination));
        startNode.setG(0);
        Node destinationNode = findExitNodes(destination); //Find exit nodes for the sector destination is in.

        List<Node> openList = new ArrayList<Node>(); //Deciding what datastructure is best
        CostComparator comparator = new CostComparator();
        TreeSet<Node> closedList = new TreeSet<Node>(comparator); //Treemap since closeList is more for looking up then inserting

        openList.add(startNode);
        while (openList.size() > 0)
        {
            //sort openList
            Collections.sort(openList, comparator);
            //pop first
            Node currentNode = openList.remove(0);
            closedList.add(currentNode);

            //Check if connection with the destination node is made.
            //Todo: add a more efficient way of finding destination
            for (Connection c : destinationNode.getConnections())
            {
                if (c.getTo().equals(currentNode)) pathFound = true;
            }
            if (!pathFound) {
                //Find all connections and add them to the openlist
                for (Connection c : currentNode.getConnections()) {
                    if (!closedList.contains(c.getTo())) {
                        if (!openList.contains(c.getTo())) {
                            //If it's not on the openList add it
                            c.getTo().setH(manhattanHeuristic(c.getTo().getPosition(), destination));
                            c.getTo().setG(currentNode.getG() + c.getCost());
                            c.getTo().setParent(currentNode);
                            openList.add(c.getTo());

                        } else {
                            //If it's already on the openList check if this is closer to start point and update it.
                            if (c.getTo().getG() > currentNode.getG() + c.getCost()) {
                                c.getTo().setG(currentNode.getG() + c.getCost());
                                c.getTo().setParent(currentNode);
                                //Collections.sort(openList, comparator);
                            }
                        }
                    }
                }
            }
            else //currentNode is connected to destination
            {
                System.out.print("Path found! ");
                while (currentNode.getParent() != null)
                {
                    path.add(currentNode);
                    currentNode = currentNode.getParent();
                }
                //Break out of while loop
                break;
            }
        }
        System.out.println((System.nanoTime() - time) / 1000000 + "ms");
        return path;
    }

Now I really hope someone can spot what I am doing wrong. The only thing I am altering in the main graph is the G & H and the parent. But this should not matter since if it's not on the openList yet I am setting these to the correct values. The cost comparator just sorts on highest F score (G + H).

@Override
    public int compare(Node n1, Node n2) {
        return n1.getF() < n2.getF() ? -1 : n1.getF() == n2.getF() ? 0 : 1;
    }

And the equals and hashcode of node are derived from just the coordinates.

Node.Java

@Override
    public boolean equals(Object o) {
        if (this == o) return true;
        if (o == null || getClass() != o.getClass()) return false;
        Node node = (Node) o;
        return position.equals(node.position);

    }

    @Override
    public int hashCode() {
        return position.hashCode();
    }

Coordinate.java

@Override
    public boolean equals(Object o) {
        if (this == o) return true;
        if (o == null || getClass() != o.getClass()) return false;
        Coordinates that = (Coordinates) o;
        if (x != that.x) return false;
        return y == that.y;
    }

    @Override
    public int hashCode() {
        int result = x;
        result = 31 * result + y;
        return result;
    }

Performance?

Now other then this I find this to be very slow and this is probably mostly because of the closedList lookups. I am aware that my map in it's current state pretty much represents a worse case of a map, it is generating a lot more nodes then a open/indoor/playerbuild type of map. Anyway, on smaller maps with regular A* I have been using a boolean[][] as the close list so I had direct access if a certain location was already closed. I was getting near the same lookup times as I have now with a hierarchical search. But for this project I want to have a much larger map and do not want a huge boolean[][] in memory. So can you see a major performance issue and how to speed it up? Any other way to speed it up? I have a private HashMap<Coordinates, Node>[][] sectorGraph; which could be useful for much quicker lookups if I know which sector to look for.

Your hierchical path finding looks very interesting. I'd love to hear the results.

In the mean time, I went though your code and found a couple of things you may want to check.

It's a bit long, but hopefully it's useful to you.

public List<Node> findNodePath(Coordinates start, Coordinates destination)
    {
        long time = System.nanoTime();
        List<Node> path = new ArrayList<Node>();
        boolean pathFound = false;

        //create startnode and connect it to sector exits.
        Node startNode = findExitNodes(start); //Find exit nodes for the sector start is in

// "exitNodes" is quite confusing as name, since you're find a start position here.
// Also, comment says "nodes" while you return only one node.

        startNode.setH(manhattanHeuristic(start, destination));
        startNode.setG(0);
        Node destinationNode = findExitNodes(destination); //Find exit nodes for the sector destination is in.

        List<Node> openList = new ArrayList<Node>(); //Deciding what datastructure is best

// Definitely "best" is not an arraylist. This is likely your performance killer.
// You generally want a priority queue here, ordered by cost. However that
// usually means you cannot modify the cost value of a node once you added it to
// the queue, which is what you seem to be doing.

        CostComparator comparator = new CostComparator();
        TreeSet<Node> closedList = new TreeSet<Node>(comparator); //Treemap since closeList is more for looking up then inserting

        openList.add(startNode);
        while (openList.size() > 0)

// While for an arraylist it likely doesn't matter, in general, you want to use
// "isEmpty" for comparing against 0. The difference is that counting the exact
// number of elements could be costly, while all you want to know "is there any
// element in there" ?

        {
            //sort openList
            Collections.sort(openList, comparator);
            //pop first
            Node currentNode = openList.remove(0);

// :O

// So you sort the entire list on each iteration, and then take 1 element from it?
// In addition, you take the **first** element, so all other elements have to shift 1
// position? This does not scream "Death to any kind of performance!!!" to you?

// Just a walk over all elements to find the smallest element, and then get the
// element and replace it with the last on the list (if the smallest element is
// not the last one) is already way faster, and that is O(n), which is still
// horribly slow.
//
// For improvement, see below in the discussion of inspecting closed and open
// lists.

            closedList.add(currentNode);

            //Check if connection with the destination node is made.
            //Todo: add a more efficient way of finding destination
            for (Connection c : destinationNode.getConnections())
            {
                if (c.getTo().equals(currentNode)) pathFound = true;
            }

// Technically, this is the wrong way to find a path. Consider the case of 3
// nodes "c" (the current node), "B" (an intermediate node), and "D" (the destination node):
// Edges:
//    c -> D (weight 10)
//    c -> B (weight 1)
//    B -> D (weight 1)
//
// Here, you look around "D", to find c. If you do that in my example, you will
// conclude that you have a path using the c -> D edge. While this is definitely
// true, it is not the shortest one, since c -> B -> D has total weight 2 rather
// than the 10 you found.
//
// "c" is always the open node with the shortest distance. In particular, that
// means that all other nodes in the open list are at least length_of_path(c) long.
//
// You can use this fact, by just checking whether "c.equals(destinationNode)".
// If it holds, you have the 'open node with the shortest distance that is also
// the destination', ie the path you are looking for!
// In other words, the end criterium is when you find the destination in the open list.
//
// I said "technically" above, since you seem to use a regular grid. In such a
// grid, cases like my counter example never happen, so your solution works due
// to a property you get from the graph you are searching.
//
// Finally, you can do { pathFound = true; break; } above, although it doesn't
// make any difference in performance at all in this case.

            if (!pathFound) {

// You have an "if" here, where the "!pathFound" branch loops back to the next
// node, ie a normal "while" loop iteration, and the "else" branch does some
// stuff, and then exits from the while loop.
// If you change the above "if" to "if (pathFound) break;", and move the "else"
// part to below the "while", the structure of the code becomes more clear.

                //Find all connections and add them to the openlist
                for (Connection c : currentNode.getConnections()) {

// Minor thing: If you add "Node newNode = c.getTo();" here, and use that
// everywhere, the code is likely more readable.

                    if (!closedList.contains(c.getTo())) {
                        if (!openList.contains(c.getTo())) {

// By modifying the data structures, you can speed up the code significantly.
//
// Let's keep the closedList as it is now. (There are ways to merge closed and
// open lists, but one step at a time is complicated enough.)

// First check thus stays

if (closedList.contains(c.getTo())) continue;

// (I don't like nested conditions, and prefer a list of simple cases. For this
// reason I write "continue" here.)


// This leaves the open list. There are two points where you access it. Once for
// getting a new node with the lowest cost (ie the sort thingie you do above),
// and once for finding if a node already exists in the list.
// The simplest solution here is to have a list sorted on total cost. However,
// there can be more than one node with the same total cost. There are two
// solutions for that:

SortedMap<Cost, Set<Node>>

// Keep sorted on costs, and with each cost value have a set of nodes.

// or

SortedSet<Node, distanceNodeComparator>

// Set of nodes, but the comparator first sorts on cost, and then on node.
// This ensures all nodes with the lowest cost are at the front of the set.
//
// I will use the second solution here.

SortedSet<Node, distanceNodeComparator> openList;

// This eliminates the need for the sort thingie above.
// Getting a (any) node with the smallest total cost is now a simple

Node currentNode = openList.pollFirst();

// Since you also sort on node, the algorithm is biased on that sorting order,
// but that is allowed within the algorithm. Possibly you can gain more if you
// make the comparator even smarter here.

// Testing if a node is in the open list, and whether or not it is a better path
// can stay the same:

if (!openList.contains(c.getTo())) {
// Add new node.
} else if (c.getTo().getG() > currentNode.getG() + c.getCost()) {

// And here you have to watch out what you're doing. The node is in the open
// list, and the comparator has put it at a point in the data structure based on
// the current value of its total cost.
// If you change the total cost while the node is in the open list, the
// comparator will fail to find it. You may even get crashes. So don't change
// the node while it is in the list, instead

openList.remove(c.getTo());
c.getTo().setG(currentNode.getG() + c.getCost());
c.getTo().setParent(currentNode);
openList.add(c.getTo());

// Remove the entry from the open list, modify it, and insert it again.

I see various places where you can simplify equality tests a bit:

Great stuff, never actually looked at the equals method that way. But I guess the @Override equals & hashcode are used most of the time by the maps and sets I use. But I'm surely going to try this and see if it can shave off a couple of ms.

Your hierchical path finding looks very interesting. I'd love to hear the results.

Well, the code I posted still had a couple of issues that I solved now. These are the numbers for it's current state on a 1408x1408 size tilemap.

First run (bottom left to top right)

Node path: 75.38ms (for finding the path between start node and end node)

Full path: 62.78ms (for finding the path between the nodes and restraining A* within these "sectors")

Total: 140.75 ms (A bit of extra work for looking up the sectors)

Second run (bottom right to top left)

Node path: 50,69ms

Full path: 30,78ms

Total: 81.77

Third run, same as second run quickly after it

Node path: 36.83ms

Full path: 25.02ms

Total: 62.03ms

Sometimes there is a hickup in the Nodepath resulting in a node path time between 200 and 300 ms.

To me this is very promising. This is just a prototype and I eventually want to use this on a isometric Tile[128][128][128] map where just a couple of z levels could contain such a "noisy" terrain. Most of the terrain will be solid (so no nodes needed) and a complete "air" sector probably just get a single node connecting to the one above. I expect performance to increase dramatically then.

But since such large maps take a ton of memory I need to work with chunks. The higher level node graphs will be kept in memory and looking up the final path within a single sector from node to node takes around 0.1ms including creating new nodes on the fly. I could save these nodes as chunks too and load them from disc but my guess is that it would be much slower and each time the map changes I have to update it. But when chunks are not loaded in memory and a unit needs to find a path I still have to read the tiles into memory when it is warmed up it takes about 0.5ms to read the sector from disc into memory. So I'm wondering if saving nodes to disc and just fetch those would be a better option. But well, that's what I am prototyping for ;).

Anyway, it's hard to recommend on your comments within my code so I go by them one by one.

exitNodes really are exit nodes, or edgeNodes. It returns all other nodes within a sector that can be eached by a specific Coordinate.

I still have to expiriment with the openList. But I am not sure if there is much performance to gain. The openList gets a ton of inserts and most of them will never be looked at. It would be great to know beforehand if a specific node would be silly to add and if it finally is really needed still add it. Anyway, if I would be using a PriorityQueue I am getting in trouble when I need to update a node. I can only get the first in a PriorityQueue and if I would change something in it it would not resort afaik. I have tried working with a TreeMap but this really did not do much.

.isEmpty seems to shave of 0.5ms on avarage, fastest path is now < 60ms

Changing the code for getting the current node to the following kills performance. Look up time is doubled.

            Node currentNode = openList.get(0);
            int listSize = openList.size();
            int currentIndex = 0;
            //find first
            for (int i = 1; i < openList.size(); i++)
            {
                if (openList.get(i).getF() < currentNode.getF()) {
                    currentNode = openList.get(i);
                    currentIndex = i;
                }
            }
            openList.set(currentIndex, openList.get(listSize - 1));
            openList.remove(listSize - 1);

Perhaps now because I am calling openList.size() a lot. But to get and remove the last element I need to know the index. I understand what you mean but sorting the list that many times really does not seem to be much work since it is almost completely sorted anyway.

The check for the found path is a remnant of my laziness. But I cannot add the end node as a connection for the nodes in the sector, or I will have to remove that node once a path is found. Otherwise this node remains in the complete graph while it belongs to a specific lookup. Since the end node is not connected to the main graph it can never find it from main graph. Anyway, I have noticed this does not always yield the best path, but it's just costing a couple of extra moves and I'm okay with that. If you are walking on the street you are not cutting corners on each block are you ;)? If the request is nearby I would try a lookup on tile level anyway.

Since this wall of text is getting close to the one in china I will follow your instructions on the open/closed map and report this later.

// This leaves the open list. There are two points where you access it. Once for
// getting a new node with the lowest cost (ie the sort thingie you do above),
// and once for finding if a node already exists in the list.
// The simplest solution here is to have a list sorted on total cost. However,
// there can be more than one node with the same total cost. There are two
// solutions for that:

I believe those people with a hearing disability living a couple of blocks away just heard me shout "sick!". Finding a node path is about 10 times faster now. I have tried this before but somehow could not get it right, I was aware of updating in a sorted datastructure is bad but somehow I could not get it to work in the short time I gave it. I am going to modify the pathfinder for finding a path within sectors (Yes I could use the same code for this, but at the moment of creating this I was still wondering how I should tackle finding the path within the sector and did not want to mess up the higher level node path finding method. I doubt this would get the same results since the openList cannot exceed a maximum of 16x16 (sector width). But I have goosebumps all over ,

-edit-

Somehow these sectors are still giving me problems. Like I said I am creating these on the fly since eventually I don't want them to remain in memory all the time. But having both the algorithms altered now the time to find the sector path is fluctuating between 30 and 400ms. Since reading in 16x16 tiles takes less then .5 ms I think I will just store a complete lower level graph on disc and read 16x16 pieces on demand. A node is a bit bigger but I'm not expecting it to be more the 1ms to retrieve. Yes, that is 1ms per sector to retrieve but I only need that when a entity reached a particular higher level node. Perhaps I can create a pool where frequently needed sectors can chill in memory. But first I need to cleanup everything.

I eventually want to use this on a isometric Tile[128][128][128] map where just a couple of z levels could contain such a "noisy" terrain. Most of the terrain will be solid (so no nodes needed) and a complete "air" sector probably just get a single node connecting to the one above.

Hmm, wouldn't 128x128x128 be a terrible waste of space then? You seem to use only something like a few levels z.

In my FreeRCT program, I use a 'voxelstack', basically

class Stack {
    List<Tile> z_stack; // Tiles that are actually used, starting from base level.
    int base_level;  // Level of z_stack.get(0) in the world.
};
List<List<Stack>> world; // 128x128 stacks, can also be Stack[128*128] I think.

My stacks can grow a lot, as people can build rides in the air, but in your case that may not be much of a problem.

If you can manage it, I would recommend trying to keep the world in memory. Not sure if 16x16 is worth the trouble (it sounds so small). Maybe you can reduce the amount of information for path finding? If a tile is really that big, I doubt you need all for path finding.

Hmm, wouldn't 128x128x128 be a terrible waste of space then? You seem to use only something like a few levels z.

Another interesting thing. But 90% of the tiles will actually hold important data when they are generated. Just a view layers on top will be empty initially but they player is able to build a couple of tiles "above ground". So I'm not sure if this would help out since I have to do lookups in those lists on what to draw. My current isometric engine draws only visible tiles with a cap of 20 z levels down and it never draws above the current z level. It also can be rotated 4 * 90 degrees and it's lightning fast.

A higher level graph of 16x16 currently gives me the best result. Of course I can build on the current pathfinding and have even higher graphs like 64x64 to cut the map in 4 per z layer but I don't think that is going to shave of much of the 2ms.

One other thing, If I have a unreachable path to time to find the nodepath is > 100ms. Would you have a solution for this? I was thinking of two things.

1. I could mark my tiles with a number and each separate area gets there own number. When two area's connect I have to floodfill these numbers and if a area gets split into two I have to give each there own number again. Now I just compare the numbers and if they are not the same don't even bother with requesting a path. But the game I want to use this for will often separate and connect large area's, a door could be locked for example so I'm not sure about this one.
2. I could try to find a path both ways somehow, this would double the cost but if the destination area is a lot smaller this could help a lot.

I was actually experimenting with running the pathfinding on a separate thread and while I got this working this can be a pain in the ass with a very dynamic map. With the help of you I can have every path request well within half the frame time so now I will just queue up path requests. So in the rare case that I need 100 path requests the last request just has to wait just 100 frames.

If I have a unreachable path to time to find the nodepath is > 100ms. Would you have a solution for this?

Hmm, yeah, it will start floodfilling the entire reachable area in an attempt to find a path. If you want truly optimal paths, this is what you want. Theoretically, you could make a very long zig-zag pattern with many branches, forcing the A* algorithm into exploring a lot of nodes.

Your closedlist is probably exploding, perhaps together with the JVM running into memory limits, which triggers the GC a lot. You could move the closed list to a bit in each node, but you'd need to clear those bits before each path find request, which probably defeats any profit you may have in not having a long closed list. Still, a closedlist only requires presence testing, so a bitset would be sufficient which is a lot smaller than a Set<Node>.

Another idea that pops up is to limit the real path length to eg 2 or 3 times the estimated cost. It would however mean you can have cases where the algorithm fails in finding a path even though one exists (ie anything longer than the upper limit you set). Depending on the map properties (ie if you have reasonably connected maps, you won't hit the limit) and the game, this may or may not be acceptable.

Theoretically, you could do off-line testing of a map beforehand, don't know how feasible that is though. Finding the longest shortest path is a new problem to me.

1. I could mark my tiles with a number and each separate area gets there own number. When two area's connect I have to floodfill these numbers and if a area gets split into two I have to give each there own number again. Now I just compare the numbers and if they are not the same don't even bother with requesting a path. But the game I want to use this for will often separate and connect large area's, a door could be locked for example so I'm not sure about this one.

Flood filling is quicker than path finding, as you don't care about traveled distance.

You can do without flood filling due to doors though, by adding another layer. You create "Area", which are all tiles with the same number after closing all doors. Areas are connected to each other by doors (boolean on/off switches). You make a number of sets "Connected". One Connected set contains all Areas that are connected with each other through at least one open door. If all doors are closed, you have a Connected set for each Area. If all doors are open, there is one Connected set containing all Areas.

Note that conceptually, it's the same idea as your number system, each Connected set gets a unique number, which is equivalent to giving all tiles in all Areas in that Connected that same number. The trick is however, that you can move an Area between Connected sets without ever touching all tiles of the Area.

Now, a path exists between area A and area B if and only if both areas are in the same Connected set.

If you open a door, find the Connected set at one side, and the Connected set at the other side, and merge them (if they are different).

If you close a door, find the Connected set that contains both areas (it was open, so both sides must be in the same Connected set). Build a new Connected set, starting at an area at one side of the now closed door, moving Areas from the original Connected set, moving all Areas that can be reached from the first moved Area directly of indirectly. If you move only some Areas, the Connected was indeed split, and you now have two Connected. If you move everything, the closed door was not critical.

Under the assumption that you generally have much less Areas than tiles, this should be faster.

This leaves the problem of deciding which area a tile belongs to. You can solve that quite easily by introducing Area numbers. Each tile has such a number. An area then reduces to just a single number, and Connected sets become sets of integers.