KD Tree modified