KDRoadTree not working but closer to working, hopefully