Added code for KDTree