↓Boid系のプログラムを参考にするのはどうよ?
http://www.red3d.com/cwr/steer/CrowdPath.html
http://www.red3d.com/cwr/steer/Doorway.html