Speed up function findShortestDistancesByRadius using set instead of vector