Temp changes do not PR
NearestNeighbor search on PlannerData class implemented