465 lines
20 KiB
C#
465 lines
20 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Drawing;
|
||
using System.Linq;
|
||
using AGVNavigationCore.Models;
|
||
using AGVNavigationCore.PathFinding.Analysis;
|
||
using AGVNavigationCore.PathFinding.Planning;
|
||
|
||
namespace AGVNavigationCore.Utils
|
||
{
|
||
/// <summary>
|
||
/// AGV 방향 계산 헬퍼 유틸리티
|
||
/// 현재 위치에서 주어진 모터 방향과 마그넷 방향으로 이동할 때 다음 노드를 계산
|
||
/// 이전 이동 방향과 마그넷 방향을 고려하여 더 정확한 경로 예측
|
||
/// </summary>
|
||
public static class DirectionalHelper
|
||
{
|
||
|
||
/// <summary>
|
||
/// AGV방향과 일치하는지 확인한다. 단 원본위치에서 dock 위치가 Don't Care 라면 true가 반환 됩니다.
|
||
/// </summary>
|
||
/// <param name="dock"></param>
|
||
/// <param name="agvdirection"></param>
|
||
/// <returns></returns>
|
||
public static bool MatchAGVDirection(this DockingDirection dock, AgvDirection agvdirection)
|
||
{
|
||
if (dock == DockingDirection.DontCare) return true;
|
||
if (dock == DockingDirection.Forward && agvdirection == AgvDirection.Forward) return true;
|
||
if (dock == DockingDirection.Backward && agvdirection == AgvDirection.Backward) return true;
|
||
return false;
|
||
}
|
||
|
||
private static JunctionAnalyzer _junctionAnalyzer;
|
||
|
||
/// <summary>
|
||
/// JunctionAnalyzer 초기화 (첫 호출 시)
|
||
/// </summary>
|
||
private static void InitializeJunctionAnalyzer(List<MapNode> allNodes)
|
||
{
|
||
if (_junctionAnalyzer == null && allNodes != null)
|
||
{
|
||
_junctionAnalyzer = new JunctionAnalyzer(allNodes);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 현재 노드에서 주어진 모터 방향과 마그넷 방향으로 이동할 때 다음 노드를 반환
|
||
/// 이전 모터 방향과 마그넷 방향을 고려하여 더 정확한 경로 예측
|
||
/// </summary>
|
||
/// <param name="currentNode">현재 노드</param>
|
||
/// <param name="prevNode">이전 노드 (진행 방향 기준점)</param>
|
||
/// <param name="prevDirection">이전 구간의 모터 방향</param>
|
||
/// <param name="direction">현재 모터 방향 (Forward 또는 Backward)</param>
|
||
/// <param name="magnetDirection">현재 마그넷 방향 (Straight/Left/Right)</param>
|
||
/// <param name="allNodes">모든 맵 노드</param>
|
||
/// <returns>다음 노드 (또는 null)</returns>
|
||
public static MapNode GetNextNodeByDirection(
|
||
MapNode currentNode,
|
||
MapNode prevNode,
|
||
AgvDirection prevDirection,
|
||
AgvDirection direction,
|
||
MagnetDirection magnetDirection,
|
||
List<MapNode> allNodes)
|
||
{
|
||
if (currentNode == null || prevNode == null || allNodes == null)
|
||
return null;
|
||
|
||
// JunctionAnalyzer 초기화
|
||
InitializeJunctionAnalyzer(allNodes);
|
||
|
||
// 현재 노드에 연결된 노드들 중 이전 노드가 아닌 노드들만 필터링
|
||
var connectedMapNodes = currentNode.ConnectedMapNodes;
|
||
if (connectedMapNodes == null || connectedMapNodes.Count == 0)
|
||
return null;
|
||
|
||
List<MapNode> candidateNodes = new List<MapNode>();
|
||
if (prevDirection == direction)
|
||
{
|
||
candidateNodes = connectedMapNodes.Where(n => n.Id != prevNode.Id).ToList();
|
||
}
|
||
else
|
||
{
|
||
candidateNodes = connectedMapNodes.ToList();
|
||
}
|
||
|
||
if (candidateNodes.Count == 0)
|
||
return null;
|
||
|
||
// 이전→현재 이동 벡터
|
||
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;
|
||
|
||
Console.WriteLine(
|
||
$"\n[GetNextNodeByDirection] ========== 다음 노드 선택 시작 ==========");
|
||
Console.WriteLine(
|
||
$" 현재노드: {currentNode.RfidId}[{currentNode.Id}]({currentNode.Position.X:F1}, {currentNode.Position.Y:F1})");
|
||
Console.WriteLine(
|
||
$" 이전노드: {prevNode.RfidId}[{prevNode.Id}]({prevNode.Position.X:F1}, {prevNode.Position.Y:F1})");
|
||
Console.WriteLine(
|
||
$" 이동벡터: ({movementVector.X:F2}, {movementVector.Y:F2}) → 정규화: ({normalizedMovement.X:F3}, {normalizedMovement.Y:F3})");
|
||
Console.WriteLine(
|
||
$" 현재방향: {direction}, 이전방향: {prevDirection}, 마그넷방향: {magnetDirection}");
|
||
Console.WriteLine(
|
||
$" 후보노드 개수: {candidateNodes.Count}");
|
||
|
||
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 == prevDirection)
|
||
{
|
||
// Forward: 진행 방향과 유사한 방향 선택 (높은 내적 = 좋음)
|
||
score = dotProduct;
|
||
}
|
||
else // Backward
|
||
{
|
||
// Backward: 진행 방향과 반대인 방향 선택 (낮은 내적 = 좋음)
|
||
score = -dotProduct;
|
||
}
|
||
|
||
Console.WriteLine(
|
||
$"\n [후보] {candidate.RfidId}[{candidate.Id}]({candidate.Position.X:F1}, {candidate.Position.Y:F1})");
|
||
Console.WriteLine(
|
||
$" 벡터: ({toNextVector.X:F2}, {toNextVector.Y:F2}), 길이: {toNextLength:F2}");
|
||
Console.WriteLine(
|
||
$" 정규화벡터: ({normalizedToNext.X:F3}, {normalizedToNext.Y:F3})");
|
||
Console.WriteLine(
|
||
$" 내적(dotProduct): {dotProduct:F4}");
|
||
Console.WriteLine(
|
||
$" 기본점수 ({(direction == prevDirection ? "방향유지" : "방향변경")}): {score:F4}");
|
||
|
||
// 이전 모터 방향이 제공된 경우: 방향 일관성 보너스 추가
|
||
var scoreBeforeMotor = score;
|
||
score = ApplyMotorDirectionConsistencyBonus(
|
||
score,
|
||
direction,
|
||
prevDirection,
|
||
dotProduct
|
||
);
|
||
Console.WriteLine(
|
||
$" 모터방향 적용 후: {scoreBeforeMotor:F4} → {score:F4}");
|
||
|
||
// 마그넷 방향을 고려한 점수 조정
|
||
var scoreBeforeMagnet = score;
|
||
score = ApplyMagnetDirectionBonus(
|
||
score,
|
||
magnetDirection,
|
||
normalizedMovement,
|
||
normalizedToNext,
|
||
currentNode,
|
||
candidate,
|
||
direction
|
||
);
|
||
Console.WriteLine(
|
||
$" 마그넷방향 적용 후: {scoreBeforeMagnet:F4} → {score:F4}");
|
||
|
||
if (score > bestScore)
|
||
{
|
||
bestScore = score;
|
||
bestNode = candidate;
|
||
Console.WriteLine(
|
||
$" ⭐ 현재 최고점수 선택됨!");
|
||
}
|
||
}
|
||
|
||
Console.WriteLine(
|
||
$"\n 최종선택: {bestNode?.RfidId ?? 0}[{bestNode?.Id ?? "null"}] (점수: {bestScore:F4})");
|
||
Console.WriteLine(
|
||
$"[GetNextNodeByDirection] ========== 다음 노드 선택 종료 ==========\n");
|
||
|
||
return bestNode;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 모터 방향 일관성을 고려한 점수 보정
|
||
/// 같은 방향으로 계속 이동하는 경우 보너스 점수 부여
|
||
/// </summary>
|
||
/// <param name="baseScore">기본 점수</param>
|
||
/// <param name="currentDirection">현재 모터 방향</param>
|
||
/// <param name="prevMotorDirection">이전 모터 방향</param>
|
||
/// <param name="dotProduct">벡터 내적값</param>
|
||
/// <returns>조정된 점수</returns>
|
||
private static float ApplyMotorDirectionConsistencyBonus(
|
||
float baseScore,
|
||
AgvDirection currentDirection,
|
||
AgvDirection prevMotorDirection,
|
||
float dotProduct)
|
||
{
|
||
float adjustedScore = baseScore;
|
||
|
||
// 모터 방향이 변경되지 않은 경우: 일관성 보너스
|
||
if (currentDirection == prevMotorDirection)
|
||
{
|
||
// Forward 지속: 직진 방향으로의 이동 선호
|
||
// Backward 지속: 반대 방향으로의 이동 선호
|
||
const float CONSISTENCY_BONUS = 0.2f;
|
||
adjustedScore += CONSISTENCY_BONUS;
|
||
|
||
System.Diagnostics.Debug.WriteLine(
|
||
$"[DirectionalHelper] 모터 방향 일관성 보너스: {currentDirection} → {currentDirection} " +
|
||
$"(점수: {baseScore:F3} → {adjustedScore:F3})");
|
||
}
|
||
else
|
||
{
|
||
// 모터 방향이 변경된 경우: 방향 변경 페널티
|
||
const float DIRECTION_CHANGE_PENALTY = 0.15f;
|
||
adjustedScore -= DIRECTION_CHANGE_PENALTY;
|
||
|
||
System.Diagnostics.Debug.WriteLine(
|
||
$"[DirectionalHelper] 모터 방향 변경 페널티: {prevMotorDirection} → {currentDirection} " +
|
||
$"(점수: {baseScore:F3} → {adjustedScore:F3})");
|
||
}
|
||
|
||
return adjustedScore;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 마그넷 방향을 고려한 점수 보정
|
||
/// Straight/Left/Right 마그넷 방향에 따라 후보 노드를 평가
|
||
/// </summary>
|
||
/// <param name="baseScore">기본 점수</param>
|
||
/// <param name="magnetDirection">마그넷 방향 (Straight/Left/Right)</param>
|
||
/// <param name="normalizedMovement">정규화된 이동 벡터</param>
|
||
/// <param name="normalizedToNext">정규화된 다음 이동 벡터</param>
|
||
/// <param name="currentNode">현재 노드</param>
|
||
/// <param name="candidate">후보 노드</param>
|
||
/// <returns>조정된 점수</returns>
|
||
private static float ApplyMagnetDirectionBonus(
|
||
float baseScore,
|
||
MagnetDirection magnetDirection,
|
||
PointF normalizedMovement,
|
||
PointF normalizedToNext,
|
||
MapNode currentNode,
|
||
MapNode candidate,
|
||
AgvDirection direction)
|
||
{
|
||
float adjustedScore = baseScore;
|
||
|
||
// Straight: 일직선 방향 (높은 내적 보너스)
|
||
if (magnetDirection == MagnetDirection.Straight)
|
||
{
|
||
const float STRAIGHT_BONUS = 0.5f;
|
||
adjustedScore += STRAIGHT_BONUS;
|
||
|
||
Console.WriteLine(
|
||
$" [마그넷 판정] Straight 보너스 +0.5: {baseScore:F4} → {adjustedScore:F4}");
|
||
}
|
||
// Left 또는 Right: 모터 위치에 따른 회전 방향 판단
|
||
else if (magnetDirection == MagnetDirection.Left || magnetDirection == MagnetDirection.Right)
|
||
{
|
||
// 2D 외적: movement × toNext = movement.X * toNext.Y - movement.Y * toNext.X
|
||
float crossProduct = (normalizedMovement.X * normalizedToNext.Y) -
|
||
(normalizedMovement.Y * normalizedToNext.X);
|
||
|
||
bool isLeftMotorMatch = false;
|
||
bool isRightMotorMatch = false;
|
||
|
||
// ===== 정방향(Forward) 이동 =====
|
||
if (direction == AgvDirection.Forward)
|
||
{
|
||
// Forward 이동 시 외적 판정:
|
||
// - 외적 < 0 (음수) = 반시계 회전 = Left 모터 멈춤
|
||
// - 외적 > 0 (양수) = 시계 회전 = Right 모터 멈춤
|
||
//
|
||
// 예: 004 → 012 → 016 (Left 모터)
|
||
// 외적 = -0.9407 (음수) → 반시계 → Left 일치 ✅
|
||
|
||
isLeftMotorMatch = crossProduct < 0; // 음수 = 반시계 = Left 멈춤
|
||
isRightMotorMatch = crossProduct > 0; // 양수 = 시계 = Right 멈춤
|
||
}
|
||
// ===== 역방향(Backward) 이동 =====
|
||
else // Backward
|
||
{
|
||
// Backward 이동 시 외적 판정:
|
||
// - 외적 < 0 (음수) = 시계 회전 = Left 모터 멈춤
|
||
// - 외적 > 0 (양수) = 반시계 회전 = Right 모터 멈춤
|
||
//
|
||
// 예: 012 → 004 → 003 (Left 모터)
|
||
// 외적 = 0.9334 (양수) → 반시계(역방향 기준 시계) → Left 일치 ✅
|
||
|
||
isLeftMotorMatch = crossProduct > 0; // 양수 = 시계(역) = Left 멈춤
|
||
isRightMotorMatch = crossProduct < 0; // 음수 = 반시계(역) = Right 멈춤
|
||
}
|
||
|
||
Console.WriteLine(
|
||
$" [마그넷 판정] 외적(Cross): {crossProduct:F4}, Left모터일치: {isLeftMotorMatch}, Right모터일치: {isRightMotorMatch} [{direction}]");
|
||
|
||
// 외적의 절대값으로 회전 강도 판단 (0에 가까우면 약함, 1에 가까우면 강함)
|
||
float rotationStrength = Math.Abs(crossProduct);
|
||
|
||
if ((magnetDirection == MagnetDirection.Left && isLeftMotorMatch) ||
|
||
(magnetDirection == MagnetDirection.Right && isRightMotorMatch))
|
||
{
|
||
// 올바른 모터 방향: 회전 강도에 비례한 보너스
|
||
// 강한 회전(|외적| ≈ 1): +2.0
|
||
// 약한 회전(|외적| ≈ 0.2): +0.4
|
||
float magnetBonus = rotationStrength * 2.0f;
|
||
adjustedScore += magnetBonus;
|
||
|
||
Console.WriteLine(
|
||
$" [마그넷 판정] ✅ {magnetDirection} 모터 일치 (회전강도: {rotationStrength:F4}, 보너스 +{magnetBonus:F4}): {baseScore:F4} → {adjustedScore:F4}");
|
||
}
|
||
else
|
||
{
|
||
// 잘못된 모터 방향: 회전 강도에 비례한 페널티
|
||
// 강한 회전(|외적| ≈ 1): -2.0
|
||
// 약한 회전(|외적| ≈ 0.2): -0.4
|
||
float magnetPenalty = rotationStrength * 2.0f;
|
||
adjustedScore -= magnetPenalty;
|
||
|
||
string actualMotor = crossProduct > 0 ? "Left" : "Right";
|
||
|
||
Console.WriteLine(
|
||
$" [마그넷 판정] ❌ {magnetDirection} 모터 불일치 (실제: {actualMotor}, 회전강도: {rotationStrength:F4}, 페널티 -{magnetPenalty:F4}): {baseScore:F4} → {adjustedScore:F4}");
|
||
}
|
||
}
|
||
|
||
return adjustedScore;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 모터 방향을 고려한 다음 노드 선택 (디버깅/분석용)
|
||
/// </summary>
|
||
public static (MapNode node, float score, string reason) GetNextNodeByDirectionWithDetails(
|
||
MapNode currentNode,
|
||
MapNode prevNode,
|
||
AgvDirection direction,
|
||
List<MapNode> allNodes,
|
||
AgvDirection? prevMotorDirection)
|
||
{
|
||
if (currentNode == null || prevNode == null || allNodes == null)
|
||
return (null, 0, "입력 파라미터가 null입니다");
|
||
|
||
var connectedMapNodes = currentNode.ConnectedMapNodes;
|
||
if (connectedMapNodes == null || connectedMapNodes.Count == 0)
|
||
return (null, 0, "연결된 노드가 없습니다");
|
||
|
||
var candidateNodes = connectedMapNodes.ToList();
|
||
|
||
if (candidateNodes.Count == 0)
|
||
return (null, 0, "후보 노드가 없습니다");
|
||
|
||
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], 1.0f, "움직임이 거의 없음");
|
||
|
||
var normalizedMovement = new PointF(
|
||
movementVector.X / movementLength,
|
||
movementVector.Y / movementLength
|
||
);
|
||
|
||
MapNode bestNode = null;
|
||
float bestScore = float.MinValue;
|
||
string reason = "";
|
||
|
||
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
|
||
);
|
||
|
||
float dotProduct = (normalizedMovement.X * normalizedToNext.X) +
|
||
(normalizedMovement.Y * normalizedToNext.Y);
|
||
|
||
float score = (direction == AgvDirection.Forward) ? dotProduct : -dotProduct;
|
||
|
||
if (prevMotorDirection.HasValue)
|
||
{
|
||
score = ApplyMotorDirectionConsistencyBonus(
|
||
score,
|
||
direction,
|
||
prevMotorDirection.Value,
|
||
dotProduct
|
||
);
|
||
}
|
||
|
||
if (score > bestScore)
|
||
{
|
||
bestScore = score;
|
||
bestNode = candidate;
|
||
|
||
// 선택 이유 생성
|
||
if (prevMotorDirection.HasValue && direction == prevMotorDirection)
|
||
{
|
||
reason = $"모터 방향 일관성 유지 ({direction}) → {candidate.Id}";
|
||
}
|
||
else if (prevMotorDirection.HasValue)
|
||
{
|
||
reason = $"모터 방향 변경 ({prevMotorDirection} → {direction}) → {candidate.Id}";
|
||
}
|
||
else
|
||
{
|
||
reason = $"방향 기반 선택 ({direction}) → {candidate.Id}";
|
||
}
|
||
}
|
||
}
|
||
|
||
return (bestNode, bestScore, reason);
|
||
}
|
||
}
|
||
}
|