Added optimal code to the 3DPathfinder, enabling it looking for a better path by inverse the start and end point.
Made the navigation plan to use optimal pathfinder.
Added an interface to get the block from a given location.
Fixed a bug when there is only one numeric variable candidate , didn't check its fintess.
Fixed should not stand on water.
Merge pull request #456 from shujingke/planner

Planner