Reimplementing the algorithm with critical thinking over every procedure seems to be working well. Agents already avoid and push each other out of the way. And the best part – I know why they do so now.
A little sneak peak into the internals:
On the picture rightmost agent was staying idle and 2 agents from the left are on their course to walk to the purple dot. Each agent samples a square area in front of him (1024 points, for better debug display). Red shows where agent should not go (due to being off-way or on a course to collide with another agent). Green is preferred. White dot is the best way agent chose to go.
Looks very interesting, hope to see more of it soon. 🙂