Added functionality to KDTree structure
Fixed small typo
Added generic point location data structures to speed up nearest neighbor queries
Made many more options for testing optimal motion planners