KDRoadTree is working, need commenting and posibly ordering on length