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));
}
