An Implementation of KDTree