KDTree initial implementation, still need range and nearest point on kdtree