Basic graph search code

Includes basic graph search code including hash table and heap implementations, as well as environment and planner base classes. Also implements a 2d environment and generic dijkstra planner, and a basic 3d environment skeleton.