Adapt formation to walkable terrain

[system]

	Attention	Topic was automatically imported from the old Question2Answer platform.
	Asked By	bloqm

I have a game with units moving in formation. This formation is an array of position offsets, so when the player clicks each unit in the formation offset the mouse position to find their destination. This works fine for most cases… but not in tight spaces.
My problem is (A) preventing the units destinations from overlapping each other, while (B) keeping them inside the walkable area – I’m using navigation2d.get_closest_point() to keep the destinations inside valid pathfinding areas.

Any ideas?
Thanks!