Function TargetMover.OptimizeFormationDestinations

IEnumerator OptimizeFormationDestinations (

List<IAstarAI>	ais
List<Vector3>	destinations

)

Swap the destinations of pairs of agents if it reduces the total distance they need to travel.

This is a simple optimization algorithm to make group movement smoother and more efficient. It is not perfect and may not always find the optimal solution, but it is very fast and works well in practice. It will not work great for large groups of agents, as the optimization becomes too hard for this simple algorithm.