Added A KDTree test. Tests are not rigorous and require work