static void Main(string[] args) { GraphViewer gv; string graph1 = "...........\n" + ".OOOOOOOOO.\n" + ".O...O...O.\n" + ".OOOOOOOOO.\n" + ".O...O...O.\n" + ".OOOOOOOOO.\n" + "..........."; string graph2 = ".......\n" + ".O...O.\n" + ".O...O.\n" + ".OOOOO.\n" + "...O...\n" + "...O...\n" + ".......\n"; string graph3 = "...........\n" + ".OOOOOOOOO.\n" + ".O...O...O.\n" + ".O...O...O.\n" + ".O...O...O.\n" + ".....O.....\n" + ".....O.....\n" + ".....O.....\n" + ".OOOOOOOOO.\n" + "...........\n"; string graph4 = ".........................\n" + ".O...OOOOOOOOOOOOOOO...O.\n" + ".O.......O.....O.......O.\n" + ".O.......O.....O.......O.\n" + ".O.......O.....O.......O.\n" + ".O.......O.....O.......O.\n" + ".OOOOOOOOO.....OOOOOOOOO.\n" + ".....O.............O.....\n" + ".....O......O......O.....\n" + ".....O......O......O.....\n" + ".....O......O......O.....\n" + ".OOOOOOOOOOOOOOOOOOOOOOO.\n" + ".........................\n"; string graph5 = "...........................\n" + ".OOOOOOOOOOOOOOOOOOOOOOOOO.\n" + ".O.....O.....O.....O.....O.\n" + ".O.....O.....O.....O.....O.\n" + ".OOOOOOOOOOOOOOOOOOOOOOOOO.\n" + ".O.....O.....O.....O.....O.\n" + ".O.....O.....O.....O.....O.\n" + ".OOOOOOOOOOOOOOOOOOOOOOOOO.\n" + ".O.....O.....O.....O.....O.\n" + ".O.....O.....O.....O.....O.\n" + ".OOOOOOOOOOOOOOOOOOOOOOOOO.\n" + ".O.....O.....O.....O.....O.\n" + ".O.....O.....O.....O.....O.\n" + ".OOOOOOOOOOOOOOOOOOOOOOOOO.\n" + "...........................\n"; lowMap = new LowLevelMap('O', '.', graph4); lowRobot = new LowLevelRobot(lowMap, 1, 1); MapNode firstNode = ExploreMap(lowRobot.CheckNode(), null, 0, null); double distToSecond; Console.WriteLine("[DFS 결과]"); Console.WriteLine($"Node 수 : {nodeList.Count}"); foreach (var ele in nodeList) { Console.WriteLine($"{ele.NodeNumber} : ({ele.Position}), ({ele.Error})"); } gv = new GraphViewer(); gv.AddPoints(firstNode); gv.Show(); Direction secondDir = firstNode.FindDirOf(nodeList[1]).Value; TurnRobot(secondDir); distToSecond = firstNode.GetDist(secondDir); lowRobot.Move(1); robotPos.Node1 = firstNode; robotPos.Node2 = nodeList[1]; robotPos.DivRatio1 = 1; robotPos.DivRatio2 = distToSecond - 1; Console.Write("최단 거리를 탐색할 위치를 입력하세요. (입력형식: {node1} {node2} {ratio1} {ratio2}) : "); string[] token = Console.ReadLine().Split(); MapPosition dest = new MapPosition(); dest.Node1 = nodeList[int.Parse(token[0])]; dest.Node2 = nodeList[int.Parse(token[1])]; dest.DivRatio1 = double.Parse(token[2]); dest.DivRatio2 = double.Parse(token[3]); Queue <Command> commands = new Queue <Command>(); double dist; FindShortestRoute(dest, commands, out dist); Console.WriteLine($"최단 거리: {dist}"); while (commands.Count > 0) { Console.WriteLine(commands.Dequeue().ToString()); } Console.WriteLine("아무키나 누르면 디버그를 종료합니다..."); Console.ReadKey(true); }
static bool FindShortestRoute(MapPosition dest, Queue <Command> commQueue, out double resultDist) { if (dest.IsSameSection(robotPos)) { if ((dest.Node1.NodeNumber > dest.Node2.NodeNumber) != (robotPos.Node1.NodeNumber > robotPos.Node2.NodeNumber)) { MapNode temp = dest.Node2; dest.Node2 = dest.Node1; dest.Node1 = temp; } if (dest.Dist1 < robotPos.Dist1) { AddTurningToQueue(commQueue, lowRobot.Dir, dest.Node2.FindDirOf(dest.Node1).Value); commQueue.Enqueue(new Command() { Act = Action.GoStraight, Dist = robotPos.Dist1 - dest.Dist1 }); resultDist = robotPos.Dist1 - dest.Dist1; } else { AddTurningToQueue(commQueue, lowRobot.Dir, dest.Node1.FindDirOf(dest.Node2).Value); commQueue.Enqueue(new Command() { Act = Action.GoStraight, Dist = dest.Dist1 - robotPos.Dist1 }); resultDist = dest.Dist1 - robotPos.Dist1; } return(true); } int[] route; // 로봇이 v1과 v2 정점 사이에 있는데, v1과 v2의 거리 double dist = Dijkstra(robotPos.Node1.NodeNumber, robotPos.Dist1, robotPos.Node2.NodeNumber, robotPos.Dist2, dest, out route); if (dist == -1) { resultDist = -1; return(false); } Direction virtualRobotDir; // Node1에서 시작하는 경로일 때 if (route[0] == robotPos.Node1.NodeNumber) { Direction dirOfNode1 = robotPos.Node2.FindDirOf(robotPos.Node1).Value; AddTurningToQueue(commQueue, lowRobot.Dir, dirOfNode1); virtualRobotDir = dirOfNode1; } // Node2에서 시작하는 경로일 때 else { Direction dirOfNode2 = robotPos.Node1.FindDirOf(robotPos.Node2).Value; AddTurningToQueue(commQueue, lowRobot.Dir, dirOfNode2); virtualRobotDir = dirOfNode2; } commQueue.Enqueue(new Command() { Act = Action.GoToNextNode, NextExpect = nodeList[route[0]] }); // route[i]와 route[i + 1]의 위치관계를 파악하여 큐 작성 for (int i = 0; i < route.Length - 1; ++i) { Direction nextDir = nodeList[route[i]].FindDirOf(nodeList[route[i + 1]]).Value; AddTurningToQueue(commQueue, virtualRobotDir, nextDir); virtualRobotDir = nextDir; commQueue.Enqueue(new Command() { Act = Action.GoToNextNode, NextExpect = nodeList[route[i + 1]] }); } AddTurningToQueue(commQueue, virtualRobotDir, (route.Last() == dest.Node1.NodeNumber ? dest.Node1.FindDirOf(dest.Node2) : dest.Node2.FindDirOf(dest.Node1)).Value); commQueue.Enqueue(new Command() { Act = Action.GoStraight, Dist = (route.Last() == dest.Node1.NodeNumber ? dest.Dist1 : dest.Dist2) }); resultDist = dist; return(true); }
static double Dijkstra(int v1, double v1Init, int v2, double v2Init, MapPosition dest, out int[] route) { double[] dist = new double[nodeList.Count]; int[] prev = new int[nodeList.Count]; PriorityQueue <DijkElement> q = new PriorityQueue <DijkElement>(); for (int i = 0; i < nodeList.Count; ++i) { dist[i] = DIST_INF; prev[i] = -1; } dist[v1] = v1Init; dist[v2] = v2Init; q.Enqueue(new DijkElement(v1, v1Init)); q.Enqueue(new DijkElement(v2, v2Init)); while (q.Count > 0) { DijkElement now = q.Dequeue(); if (dist[now.Index] < now.Cost) { continue; } foreach (var next in nodeConnection[now.Index]) { int nextIndex = next.Node.NodeNumber; double nextDist = next.Dist + now.Cost; if (dist[nextIndex] > nextDist) { dist[nextIndex] = nextDist; prev[nextIndex] = now.Index; q.Enqueue(new DijkElement(nextIndex, nextDist)); } } } if (dist[dest.Node1.NodeNumber] == DIST_INF && dist[dest.Node2.NodeNumber] == DIST_INF) { route = null; return(-1); } Stack <int> routeTrace = new Stack <int>(); double result; double n1Dist = dest.Dist1; double n2Dist = dest.Dist2; int currIndex; if (n1Dist + dist[dest.Node1.NodeNumber] >= n2Dist + dist[dest.Node2.NodeNumber]) { currIndex = dest.Node2.NodeNumber; result = n2Dist + dist[dest.Node2.NodeNumber]; } else { currIndex = dest.Node1.NodeNumber; result = n1Dist + dist[dest.Node1.NodeNumber]; } while (currIndex != v1 && currIndex != v2) { routeTrace.Push(currIndex); currIndex = prev[currIndex]; } routeTrace.Push(currIndex); route = new int[routeTrace.Count]; for (int i = 0; i < route.Length; ++i) { route[i] = routeTrace.Pop(); } return(result); }
public bool IsSameSection(MapPosition mp) { return((Node1.NodeNumber == mp.Node1.NodeNumber && Node2.NodeNumber == mp.Node2.NodeNumber) || (Node1.NodeNumber == mp.Node2.NodeNumber && Node2.NodeNumber == mp.Node1.NodeNumber)); }