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);
}
/// <summary>
/// 로봇이 처음 실행될 때 라인의 전체 경로를 파악하기 위해 DFS 탐색을 함.
/// </summary>
static MapNode ExploreMap(Junction currJunc, MapNode prev, double errorAdded, Direction?cameFrom)
{
Console.WriteLine("ExploreMap");
Point currPoint = new Point {
X = lowRobot.X, Y = lowRobot.Y
};
Console.WriteLine($"{currPoint}, {(prev == null ? "(Null)" : (prev.Error + errorAdded).ToString())}, {errorAdded}");
if (prev != null)
{
List <MapNode> nearList = new List <MapNode>();
foreach (MapNode node in nodeList)
{
if (IsSameNode(currPoint, node.Position, currJunc, node.Junction, prev.Error + errorAdded))
{
nearList.Add(node);
}
}
// 현재 위치한 정점이 이미 방문한 정점일 때, 로봇의 위치 정보와 오차를 조정하고 return.
if (nearList.Count > 0)
{
MapNode sameNode = nearList[0];
foreach (MapNode near in nearList)
{
if (sameNode.Position - currPoint > near.Position - currPoint)
{
sameNode = near;
}
}
Console.WriteLine($"FoundSame: [{sameNode}]");
Console.WriteLine($"(nearList: {string.Join("/", nearList)})");
Point newError = sameNode.Error;
double maxError = Math.Max(Math.Abs(prev.Position.X - sameNode.Position.X), Math.Abs(prev.Position.Y - sameNode.Position.Y));
newError.X += maxError;
newError.Y += maxError;
if (newError.X < prev.Error.X || newError.Y < prev.Error.Y)
{
prev.Error = newError;
}
return(sameNode);
}
}
MapNode newNode = new MapNode();
newNode.Position = currPoint;
newNode.NodeNumber = nodeList.Count;
newNode.Junction = currJunc;
newNode.Error = prev == null ? new Point(errorAdded, errorAdded) : prev.Error + errorAdded;
nodeList.Add(newNode);
nodeConnection.Add(new List <NodeDistPair>());
Console.WriteLine("Node Added: " + newNode);
List <Direction> visitLater = new List <Direction>();
foreach (Direction dirToExplore in DIRS_FOR_DFS)
{
if (currJunc[dirToExplore] && !(cameFrom != null && cameFrom.Value == dirToExplore) && newNode[dirToExplore] == null)
{
if (usedDir[(int)dirToExplore] != 0)
{
visitLater.Add(dirToExplore);
}
else
{
++usedDir[(int)dirToExplore];
ExploreDir(dirToExplore, newNode);
--usedDir[(int)dirToExplore];
}
}
}
foreach (Direction dir in visitLater.OrderBy(x => usedDir[(int)x]))
{
++usedDir[(int)dir];
ExploreDir(dir, newNode);
--usedDir[(int)dir];
}
return(newNode);
}
public NodeDistPair(MapNode node, double dist)
{
Node = node ?? throw new ArgumentNullException(nameof(node));
Dist = dist;
}