A* algorithm A star algorithm (matlab)