diff --git a/Cs_HMI/AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Events.cs b/Cs_HMI/AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Events.cs index 42b9d6b..c8d8fe4 100644 --- a/Cs_HMI/AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Events.cs +++ b/Cs_HMI/AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Events.cs @@ -154,39 +154,37 @@ namespace AGVNavigationCore.Controls private void DrawDirectionArrow(Graphics g, Point point, double angle, AgvDirection direction) { - var arrowSize = CONNECTION_ARROW_SIZE * 1.5; // 화살표 크기 증가 - var arrowAngle = Math.PI / 6; // 30도 + // 정삼각형 화살표 - 크기 축소 (8 픽셀) + var arrowSize = 8; - var cos = Math.Cos(angle); - var sin = Math.Sin(angle); - - // 화살표 끝점 (방향) + // 정삼각형의 3개 점 계산 + // 끝점 (방향 가르키는 점) var arrowTipPoint = new Point( (int)(point.X + arrowSize * Math.Cos(angle)), (int)(point.Y + arrowSize * Math.Sin(angle)) ); - // 화살표 좌측 점 + // 좌측 점 (120도 차이) var arrowPoint1 = new Point( - (int)(point.X - arrowSize * Math.Cos(angle - arrowAngle)), - (int)(point.Y - arrowSize * Math.Sin(angle - arrowAngle)) + (int)(point.X + arrowSize * Math.Cos(angle + 2 * Math.PI / 3)), + (int)(point.Y + arrowSize * Math.Sin(angle + 2 * Math.PI / 3)) ); - // 화살표 우측 점 + // 우측 점 (240도 차이) var arrowPoint2 = new Point( - (int)(point.X - arrowSize * Math.Cos(angle + arrowAngle)), - (int)(point.Y - arrowSize * Math.Sin(angle + arrowAngle)) + (int)(point.X + arrowSize * Math.Cos(angle + 4 * Math.PI / 3)), + (int)(point.Y + arrowSize * Math.Sin(angle + 4 * Math.PI / 3)) ); var arrowColor = direction == AgvDirection.Forward ? Color.Blue : Color.Red; var arrowBrush = new SolidBrush(arrowColor); - // 삼각형으로 화살표 그리기 (내부 채움) + // 정삼각형으로 화살표 그리기 (내부 채움) var trianglePoints = new Point[] { arrowTipPoint, arrowPoint1, arrowPoint2 }; g.FillPolygon(arrowBrush, trianglePoints); // 윤곽선 그리기 - var arrowPen = new Pen(arrowColor, 1.5f); + var arrowPen = new Pen(arrowColor, 1f); g.DrawPolygon(arrowPen, trianglePoints); arrowBrush.Dispose(); diff --git a/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs b/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs index 201ae08..0b63d8c 100644 --- a/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs +++ b/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs @@ -1,5 +1,6 @@ using System; using System.Collections.Generic; +using System.Drawing; using System.Linq; using AGVNavigationCore.Models; using AGVNavigationCore.Utils; @@ -17,6 +18,7 @@ namespace AGVNavigationCore.PathFinding.Planning private readonly List _mapNodes; private readonly AStarPathfinder _basicPathfinder; + private readonly DirectionalPathfinder _directionPathfinder; private readonly JunctionAnalyzer _junctionAnalyzer; private readonly DirectionChangePlanner _directionChangePlanner; @@ -27,6 +29,7 @@ namespace AGVNavigationCore.PathFinding.Planning _basicPathfinder.SetMapNodes(_mapNodes); _junctionAnalyzer = new JunctionAnalyzer(_mapNodes); _directionChangePlanner = new DirectionChangePlanner(_mapNodes); + _directionPathfinder = new DirectionalPathfinder(); } /// @@ -132,6 +135,37 @@ namespace AGVNavigationCore.PathFinding.Planning return pathResult; } + //2-1 현재위치의 반대방향과 대상의 방향이 맞는 경우에도 그대로 사용가능하다. + //if (targetNode.DockDirection == DockingDirection.DontCare || + // (targetNode.DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Backward) || + // (targetNode.DockDirection == DockingDirection.Backward && currentDirection == AgvDirection.Forward)) + //{ + // // 반대 방향으로 이동하여 목적지에 진입해야 함 + // // 현재 방향으로 가면서 역방향으로 도킹 + // MakeDetailData(pathResult, ReverseDirection); + // MakeMagnetDirection(pathResult); + // return pathResult; + //} + + //2-2 정방향/역방향 이동 시 다음 노드 확인 + var nextNodeForward = GetNextNodeByDirection(startNode, prevNode, currentDirection, _mapNodes); + var nextNodeBackward = GetNextNodeByDirection(startNode, prevNode, ReverseDirection, _mapNodes); + + //뒤로 이동시 경로상의 처음 만나는 노드가 같다면 그 방향으로 이동하면 된다. + if (nextNodeBackward.NodeId == pathResult.Path[1]) + { + MakeDetailData(pathResult, ReverseDirection); + MakeMagnetDirection(pathResult); + return pathResult; + } + + //if(nextNodeForward.NodeId == pathResult.Path[1]) + //{ + // MakeDetailData(pathResult, currentDirection); + // MakeMagnetDirection(pathResult); + // return pathResult; + //} + //3. 도킹방향이 일치하지 않으니 교차로에서 방향을 회전시켜야 한다 //최단거리(=minpath)경로에 속하는 교차로가 있다면 그것을 사용하고 없다면 가장 가까운 교차로를 찾는다. var JunctionInPath = FindNearestJunctionOnPath(pathResult); @@ -231,6 +265,101 @@ namespace AGVNavigationCore.PathFinding.Planning } } + /// + /// 현재 노드에서 주어진 방향(Forward/Backward)으로 이동할 때 다음 노드를 반환 + /// + /// 현재 노드 + /// 이전 노드 (진행 방향 기준점) + /// 이동 방향 (Forward 또는 Backward) + /// 모든 맵 노드 + /// 다음 노드 (또는 null) + private MapNode GetNextNodeByDirection(MapNode currentNode, MapNode prevNode, AgvDirection direction, List allNodes) + { + if (currentNode == null || prevNode == null || allNodes == null) + return null; + + // 현재 노드에 연결된 노드들 중 이전 노드가 아닌 노드들만 필터링 + var connectedNodeIds = currentNode.ConnectedNodes; + if (connectedNodeIds == null || connectedNodeIds.Count == 0) + return null; + + var candidateNodes = allNodes.Where(n => + connectedNodeIds.Contains(n.NodeId) + ).ToList(); + + if (candidateNodes.Count == 0) + return null; + + // Forward인 경우: 이전→현재 방향으로 계속 직진하는 노드 우선 + // Backward인 경우: 이전→현재 방향의 반대로 이동하는 노드 우선 + var movementVector = new PointF( + currentNode.Position.X - prevNode.Position.X, + currentNode.Position.Y - prevNode.Position.Y + ); + + var movementLength = (float)Math.Sqrt( + movementVector.X * movementVector.X + + movementVector.Y * movementVector.Y + ); + + if (movementLength < 0.001f) + return candidateNodes[0]; + + var normalizedMovement = new PointF( + movementVector.X / movementLength, + movementVector.Y / movementLength + ); + + // 각 후보 노드에 대해 점수 계산 + MapNode bestNode = null; + float bestScore = float.MinValue; + + foreach (var candidate in candidateNodes) + { + var toNextVector = new PointF( + candidate.Position.X - currentNode.Position.X, + candidate.Position.Y - currentNode.Position.Y + ); + + var toNextLength = (float)Math.Sqrt( + toNextVector.X * toNextVector.X + + toNextVector.Y * toNextVector.Y + ); + + if (toNextLength < 0.001f) + continue; + + var normalizedToNext = new PointF( + toNextVector.X / toNextLength, + toNextVector.Y / toNextLength + ); + + // 내적 계산 (유사도: -1 ~ 1) + float dotProduct = (normalizedMovement.X * normalizedToNext.X) + + (normalizedMovement.Y * normalizedToNext.Y); + + float score; + if (direction == AgvDirection.Forward) + { + // Forward: 진행 방향과 유사한 방향 선택 (높은 내적 = 좋음) + score = dotProduct; + } + else // Backward + { + // Backward: 진행 방향과 반대인 방향 선택 (낮은 내적 = 좋음) + score = -dotProduct; + } + + if (score > bestScore) + { + bestScore = score; + bestNode = candidate; + } + } + + return bestNode; + } + /// /// Path에 등록된 방향을 확인하여 마그넷정보를 업데이트 합니다 ///