fixed KD-Trees and added unit tests