126 lines
4.5 KiB
C#
126 lines
4.5 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Drawing;
|
|
using AGVNavigationCore.Models;
|
|
using AGVNavigationCore.PathFinding.Planning;
|
|
|
|
namespace AGVNavigationCore.Utils
|
|
{
|
|
/// <summary>
|
|
/// AGV 방향 기반 다음 노드 계산기
|
|
/// VirtualAGV 또는 실제 AGV 시스템에서 현재 방향을 알 때, 다음 목적지 노드를 결정
|
|
/// </summary>
|
|
public class AGVDirectionCalculator
|
|
{
|
|
private DirectionalPathfinder _pathfinder;
|
|
|
|
public AGVDirectionCalculator(DirectionalPathfinder.DirectionWeights weights = null)
|
|
{
|
|
_pathfinder = new DirectionalPathfinder(weights);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 이전 RFID 위치 + 현재 위치 + 현재 방향을 기반으로 다음 노드 ID를 반환
|
|
///
|
|
/// 사용 예시:
|
|
/// - 001에서 002로 이동 후 GetNextNodeId(001_pos, 002_node, 002_pos, Forward) → 003
|
|
/// - 003에서 004로 이동 후, Left 선택 → 030
|
|
/// - 004에서 003으로 이동(Backward) 후, GetNextNodeId(..., Backward) → 002
|
|
/// </summary>
|
|
/// <param name="previousRfidPos">이전 RFID 감지 위치</param>
|
|
/// <param name="currentNode">현재 RFID 노드</param>
|
|
/// <param name="currentRfidPos">현재 RFID 감지 위치</param>
|
|
/// <param name="direction">이동 방향</param>
|
|
/// <param name="allNodes">맵의 모든 노드</param>
|
|
/// <returns>다음 노드 ID (실패 시 null)</returns>
|
|
public string GetNextNodeId(
|
|
Point previousRfidPos,
|
|
MapNode currentNode,
|
|
Point currentRfidPos,
|
|
AgvDirection direction,
|
|
List<MapNode> allNodes)
|
|
{
|
|
// 유효성 검사
|
|
if (previousRfidPos == Point.Empty)
|
|
{
|
|
throw new ArgumentException("previousRfidPos는 빈 값일 수 없습니다. 최소 2개의 위치 히스토리가 필요합니다.");
|
|
}
|
|
|
|
if (currentNode == null)
|
|
{
|
|
throw new ArgumentNullException(nameof(currentNode), "currentNode는 null일 수 없습니다.");
|
|
}
|
|
|
|
if (allNodes == null || allNodes.Count == 0)
|
|
{
|
|
throw new ArgumentException("allNodes는 비어있을 수 없습니다.");
|
|
}
|
|
|
|
return _pathfinder.GetNextNodeId(
|
|
previousRfidPos,
|
|
currentNode,
|
|
currentRfidPos,
|
|
direction,
|
|
allNodes
|
|
);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 현재 모터 상태를 기반으로 실제 선택된 방향을 분석
|
|
/// VirtualAGV의 현재/이전 상태로부터 선택된 방향을 역추적
|
|
/// </summary>
|
|
public AgvDirection AnalyzeSelectedDirection(
|
|
Point previousPos,
|
|
Point currentPos,
|
|
MapNode selectedNextNode,
|
|
List<MapNode> connectedNodes)
|
|
{
|
|
if (previousPos == Point.Empty || currentPos == Point.Empty || selectedNextNode == null)
|
|
{
|
|
return AgvDirection.Forward;
|
|
}
|
|
|
|
// 이동 벡터
|
|
var movementVector = new PointF(
|
|
currentPos.X - previousPos.X,
|
|
currentPos.Y - previousPos.Y
|
|
);
|
|
|
|
// 다음 노드 벡터
|
|
var nextVector = new PointF(
|
|
selectedNextNode.Position.X - currentPos.X,
|
|
selectedNextNode.Position.Y - currentPos.Y
|
|
);
|
|
|
|
// 내적 계산 (유사도)
|
|
float dotProduct = (movementVector.X * nextVector.X) +
|
|
(movementVector.Y * nextVector.Y);
|
|
|
|
// 외적 계산 (좌우 판별)
|
|
float crossProduct = (movementVector.X * nextVector.Y) -
|
|
(movementVector.Y * nextVector.X);
|
|
|
|
// 진행 방향 판별
|
|
if (dotProduct > 0) // 같은 방향으로 진행
|
|
{
|
|
if (Math.Abs(crossProduct) < 0.1f) // 거의 직진
|
|
{
|
|
return AgvDirection.Forward;
|
|
}
|
|
else if (crossProduct > 0) // 좌측으로 회전
|
|
{
|
|
return AgvDirection.Left;
|
|
}
|
|
else // 우측으로 회전
|
|
{
|
|
return AgvDirection.Right;
|
|
}
|
|
}
|
|
else // 반대 방향으로 진행 (후진)
|
|
{
|
|
return AgvDirection.Backward;
|
|
}
|
|
}
|
|
}
|
|
}
|