private int GetPriority(RobotMap map, Point p, int rotation) { if (!map.Explored(p)) return 5; var t = map[p]; if (t == MapTileResult.Goal) return 10000000; if (t == MapTileResult.Wall || t == MapTileResult.Robot) return 0; if (t == MapTileResult.ClosedDoor) return 0; var count = 1; if (!map.Explored(p.Move(rotation + 3))) count++; if (!map.Explored(p.Move(rotation))) count++; if (!map.Explored(p.Move(rotation + 1))) count++; return count; }
private static Point FindFirstPoint(Dictionary<Point, int> d, Queue<Point> q, RobotMap map, Predicate<Point> goalPredicate) { while (q.Count > 0) { var e = q.Dequeue(); var nf = e.NearFour(); var i = d[e]; i++; foreach (var n in nf) { if (!d.ContainsKey(n) && map.Explored(n) && map[n].IsFree()) { q.Enqueue(n); d[n] = i; if (goalPredicate(e)) { return e; } } } } return new Point(int.MaxValue, int.MaxValue); }