I've attached a scene file if you want to play with it. I would also appreciate input on how to make it faster.
I'm using many find in array nodes currently, which I assume is quite slow. Therefore, I'm trying to implement binary heaps which should speed things up tremendously. The tree already contains subrees for binary heaps (see comment boxes), but I'm not using them yet for the actual pathfinding.
If you want to change anything (Start null position, End null position, ground geo, obstacle geo), you should first set the SearchDepth to a small value or you'll lose all interactivity. The compound should work with polygons of any shape, so they don't need to be all quads.
