Ich empfehle zu dem Thema unbedingt A* Pathfinding for Beginners und die dort verlinkten Seiten. Für dein Problem mit der Diagonalbewegung könnte insbesondere Marco Pinter - Toward More Realistic Pathfinding interessant sein.