Binary Heaps Pathfinding

Hey all,

So my pathfinding code isn't fast enough and so I optimized as much as I could, and went to our trusty friend Google to see what else I could do when I happened upon Binary Heaps..... again. And so, I decided to give it another shot. First I tried creating a separate script just dedicated to binary heaps just to see if I could get it to work. And I did, which was great. So then I just translated the script over to my Pathfinding code and....

....didn't turn out so well. Now through debugging, I noticed that the binary heaps were working except for a little problem. In the function that adds the adjacent nodes to the open list, it adds all of the adjacent nodes at the exact same time, so since the binary heap requires that the added node be compared with the f-costs of the existing nodes, it couldn't possibly work since they all get inserted at the same time.

Well, why they're being inserted at the same time is beyond me since they're all sequential and come after one another so technically, some should be inserted first, before the others. But for whatever reason, they all get inserted at the same time (I used Debug.Break() to stop the code whenever ONE node was added to the open list and when I checked the open list, ALL of the nodes had been inserted, and thus, were out of order). So not sure what's going wrong here, but here's the basic of code:


Find Adjacent Nodes

Code (csharp):

1.     if(column > 0){
2.             if(row < nodes[column-1].nodeCount){
3.                 Node left = nodes[column-1].nodes[row];
4.                 if(left.node.gameObject.layer.Equals(0)  !closedTransforms.Contains(left.node)){
5.                     if(!open.Contains(left)){
6.                         BinaryInsertion(left); //Left
7.                         Debug.Break();
8.                         left.nodeParent = current;
9.                         left.g = left.nodeParent.g + 10;
10.                         left.h = Mathf.RoundToInt((end.position - left.node.position).sqrMagnitude);
11.                         left.f = left.g + left.h;
12.                     }else{
13.                         if(current.g + 10 < left.g){
14.                             left.g = current.g + 10;
15.                             left.f = left.g + left.h;
16.                             left.nodeParent = current;
17.                         }
18.                     }
19.                 }
20.             }

This code finds which nodes are adjacent to the current node. This code is duplicated a couple times so that it finds nodes on all sides of the node, not just on the left. So like this:

Code (csharp):

1.     if(column > 0){
2.             if(row < nodes[column-1].nodeCount){
3.                 Node left = nodes[column-1].nodes[row];
4.                 if(left.node.gameObject.layer.Equals(0)  !closedTransforms.Contains(left.node)){
5.                     if(!open.Contains(left)){
6.                         BinaryInsertion(left); //Left
7.                         Debug.Break();
8.                         left.nodeParent = current;
9.                         left.g = left.nodeParent.g + 10;
10.                         left.h = Mathf.RoundToInt((end.position - left.node.position).sqrMagnitude);
11.                         left.f = left.g + left.h;
12.                     }else{
13.                         if(current.g + 10 < left.g){
14.                             left.g = current.g + 10;
15.                             left.f = left.g + left.h;
16.                             left.nodeParent = current;
17.                         }
18.                     }
19.                 }
20.             }
21.             if(row > 0  row < nodes[column-1].nodeCount){
22.                 Node upLeft = nodes[column-1].nodes[row-1];
23.                 if(upLeft.node.gameObject.layer.Equals(0)  !closedTransforms.Contains(upLeft.node)){
24.                     if(!open.Contains(upLeft)){
25.                         BinaryInsertion(upLeft); //Up-Left
26.                         Debug.Break();
27.                         upLeft.nodeParent = current;
28.                         upLeft.g = upLeft.nodeParent.g + 14;
29.                         upLeft.h = Mathf.RoundToInt((end.position - upLeft.node.position).sqrMagnitude);
30.                         upLeft.f = upLeft.g + upLeft.h;
31.                     }else{
32.                         if(current.g + 10 < upLeft.g){
33.                             upLeft.g = current.g + 10;
34.                             upLeft.f = upLeft.g + upLeft.h;
35.                             upLeft.nodeParent = current;
36.                         }
37.                     }
38.                 }
39.             }
40.         }

.....but a few more. So as you see, the code breaks after each node insertion which means that if it found that the left node was adjacent, then the entire code should stop before the rest of the comparisons execute. But, when I check, all of the nodes have been inserted. So not sure what to do here.

I don't know if this will help at all but this is the BinaryInsertion() code:

Code (csharp):

1.     void BinaryInsertion (Node addedItem) {
2.                
3.         open.Add(addedItem);
4.         int aNodePos = open.IndexOf(addedItem);
5.  
6.         while(true){
7.             int comparer = Mathf.RoundToInt(aNodePos/2);
8.             if(open[aNodePos].f < open[comparer].f){
9.                 open = Swap(aNodePos, comparer, open);
10.                 aNodePos = comparer;
11.             }else{
12.                 break;
13.             }
14.         }
15.     }

Thanks in advance!

 

Only thing i can up with is step debugging and target the list that the items get added to.

http://docs.unity3d.com/Documentation/Manual/Debugger.html

And set a debugdot on the variable or such so every time it gets affected program will stop.

 

Tried that as well, get the same results.

 

Also, is the method I'm using to find the adjacent nodes efficient?

 

These are the lines i use to look through all adjacent nodes.

Code (csharp):

1.  
2.  for (int i = y - 1; i < y + 2; i++)
3.  {
4.         for (int j = x - 1; j < x + 2; j++)
5.         {
6.                //Do all checks you need in here
7.         }
8. }
9.  

 

And make sure the first check is for y<smallestY y>largestY ... continue, or you'll get out-of-bounds errors

 

Ofcause i do that. Thats why i wrote do all checks here.

 

What does the code in the Swap function look like?