static bool IsSameNode(Point p1, Point p2, Junction j1, Junction j2, Point error)
{
return(Math.Abs(p1.X - p2.X) <= error.X * 2 &&
Math.Abs(p1.Y - p2.Y) <= error.Y * 2 &&
j1 == j2);
}
/// <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);
}