Added KDTree, basic implementation