added kd_tree
modified smoothness tolerance
minor changes
changed astar heuristic
include kd_tree
minor changes
initial commit