Changed KD Tree algorithm. Still not working perfectly.