330 lines
12 KiB
C#
330 lines
12 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Drawing;
|
|
using System.Linq;
|
|
using AGVNavigationCore.Models;
|
|
|
|
namespace AGVNavigationCore.PathFinding.Planning
|
|
{
|
|
/// <summary>
|
|
/// 방향 기반 경로 탐색기
|
|
/// 이전 위치 + 현재 위치 + 이동 방향을 기반으로 다음 노드를 결정
|
|
/// </summary>
|
|
public class DirectionalPathfinder
|
|
{
|
|
/// <summary>
|
|
/// 이동 방향별 가중치
|
|
/// </summary>
|
|
public class DirectionWeights
|
|
{
|
|
public float ForwardWeight { get; set; } = 1.0f; // 직진
|
|
public float LeftWeight { get; set; } = 1.5f; // 좌측
|
|
public float RightWeight { get; set; } = 1.5f; // 우측
|
|
public float BackwardWeight { get; set; } = 2.0f; // 후진
|
|
}
|
|
|
|
private readonly DirectionWeights _weights;
|
|
|
|
public DirectionalPathfinder(DirectionWeights weights = null)
|
|
{
|
|
_weights = weights ?? new DirectionWeights();
|
|
}
|
|
|
|
/// <summary>
|
|
/// 이전 위치와 현재 위치, 그리고 이동 방향을 기반으로 다음 노드 ID를 반환
|
|
/// </summary>
|
|
/// <param name="previousPos">이전 위치 (이전 RFID 감지 위치)</param>
|
|
/// <param name="currentNode">현재 노드 (현재 RFID 노드)</param>
|
|
/// <param name="currentPos">현재 위치</param>
|
|
/// <param name="direction">이동 방향 (Forward/Backward/Left/Right)</param>
|
|
/// <param name="allNodes">맵의 모든 노드</param>
|
|
/// <returns>다음 노드 ID (또는 null)</returns>
|
|
public string GetNextNodeId(
|
|
Point previousPos,
|
|
MapNode currentNode,
|
|
Point currentPos,
|
|
AgvDirection direction,
|
|
List<MapNode> allNodes)
|
|
{
|
|
// 전제조건: 최소 2개 위치 히스토리 필요
|
|
if (previousPos == Point.Empty || currentPos == Point.Empty)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
if (currentNode == null || allNodes == null || allNodes.Count == 0)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
// 현재 노드에 연결된 노드들 가져오기
|
|
var connectedNodeIds = currentNode.ConnectedNodes;
|
|
if (connectedNodeIds == null || connectedNodeIds.Count == 0)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
// 연결된 노드 중 현재 노드가 아닌 것들만 필터링
|
|
var candidateNodes = allNodes.Where(n =>
|
|
connectedNodeIds.Contains(n.Id) && n.Id != currentNode.Id
|
|
).ToList();
|
|
|
|
if (candidateNodes.Count == 0)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
// 이전→현재 벡터 계산 (진행 방향 벡터)
|
|
var movementVector = new PointF(
|
|
currentPos.X - previousPos.X,
|
|
currentPos.Y - previousPos.Y
|
|
);
|
|
|
|
// 벡터 정규화
|
|
var movementLength = (float)Math.Sqrt(
|
|
movementVector.X * movementVector.X +
|
|
movementVector.Y * movementVector.Y
|
|
);
|
|
|
|
if (movementLength < 0.001f) // 거의 이동하지 않음
|
|
{
|
|
return candidateNodes[0].Id; // 첫 번째 연결 노드 반환
|
|
}
|
|
|
|
var normalizedMovement = new PointF(
|
|
movementVector.X / movementLength,
|
|
movementVector.Y / movementLength
|
|
);
|
|
|
|
// 각 후보 노드에 대해 방향 점수 계산
|
|
var scoredCandidates = new List<(MapNode node, float score)>();
|
|
|
|
foreach (var candidate in candidateNodes)
|
|
{
|
|
var toNextVector = new PointF(
|
|
candidate.Position.X - currentPos.X,
|
|
candidate.Position.Y - currentPos.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 score = CalculateDirectionalScore(
|
|
normalizedMovement,
|
|
normalizedToNext,
|
|
direction
|
|
);
|
|
|
|
scoredCandidates.Add((candidate, score));
|
|
}
|
|
|
|
if (scoredCandidates.Count == 0)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
// 가장 높은 점수를 가진 노드 반환
|
|
var bestCandidate = scoredCandidates.OrderByDescending(x => x.score).First();
|
|
return bestCandidate.node.Id;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 이동 방향을 기반으로 방향 점수를 계산
|
|
/// 높은 점수 = 더 나은 선택지
|
|
/// </summary>
|
|
private float CalculateDirectionalScore(
|
|
PointF movementDirection, // 정규화된 이전→현재 벡터
|
|
PointF nextDirection, // 정규화된 현재→다음 벡터
|
|
AgvDirection requestedDir) // 요청된 이동 방향
|
|
{
|
|
float baseScore = 0;
|
|
|
|
// 벡터 간 각도 계산 (내적)
|
|
float dotProduct = (movementDirection.X * nextDirection.X) +
|
|
(movementDirection.Y * nextDirection.Y);
|
|
|
|
// 외적으로 좌우 판별 (Z 성분)
|
|
float crossProduct = (movementDirection.X * nextDirection.Y) -
|
|
(movementDirection.Y * nextDirection.X);
|
|
|
|
switch (requestedDir)
|
|
{
|
|
case AgvDirection.Forward:
|
|
// Forward: 직진 방향 선호 (dotProduct ≈ 1)
|
|
if (dotProduct > 0.9f) // 거의 같은 방향
|
|
{
|
|
baseScore = 100.0f * _weights.ForwardWeight;
|
|
}
|
|
else if (dotProduct > 0.5f) // 비슷한 방향
|
|
{
|
|
baseScore = 80.0f * _weights.ForwardWeight;
|
|
}
|
|
else if (dotProduct > 0.0f) // 약간 다른 방향
|
|
{
|
|
baseScore = 50.0f * _weights.ForwardWeight;
|
|
}
|
|
else if (dotProduct > -0.5f) // 거의 반대 방향 아님
|
|
{
|
|
baseScore = 20.0f * _weights.BackwardWeight;
|
|
}
|
|
else
|
|
{
|
|
baseScore = 0.0f; // 완전 반대
|
|
}
|
|
break;
|
|
|
|
case AgvDirection.Backward:
|
|
// Backward: 역진 방향 선호 (dotProduct ≈ -1)
|
|
if (dotProduct < -0.9f) // 거의 반대 방향
|
|
{
|
|
baseScore = 100.0f * _weights.BackwardWeight;
|
|
}
|
|
else if (dotProduct < -0.5f) // 비슷하게 반대
|
|
{
|
|
baseScore = 80.0f * _weights.BackwardWeight;
|
|
}
|
|
else if (dotProduct < 0.0f) // 약간 다른 방향
|
|
{
|
|
baseScore = 50.0f * _weights.BackwardWeight;
|
|
}
|
|
else if (dotProduct < 0.5f) // 거의 같은 방향 아님
|
|
{
|
|
baseScore = 20.0f * _weights.ForwardWeight;
|
|
}
|
|
else
|
|
{
|
|
baseScore = 0.0f; // 완전 같은 방향
|
|
}
|
|
break;
|
|
|
|
case AgvDirection.Left:
|
|
// Left: 좌측 방향 선호
|
|
// Forward 상태에서: crossProduct > 0 = 좌측
|
|
// Backward 상태에서: crossProduct < 0 = 좌측 (반대)
|
|
if (dotProduct > 0.0f) // Forward 상태
|
|
{
|
|
// crossProduct > 0이면 좌측
|
|
if (crossProduct > 0.5f)
|
|
{
|
|
baseScore = 100.0f * _weights.LeftWeight;
|
|
}
|
|
else if (crossProduct > 0.0f)
|
|
{
|
|
baseScore = 70.0f * _weights.LeftWeight;
|
|
}
|
|
else if (crossProduct > -0.5f)
|
|
{
|
|
baseScore = 50.0f * _weights.ForwardWeight;
|
|
}
|
|
else
|
|
{
|
|
baseScore = 30.0f * _weights.RightWeight;
|
|
}
|
|
}
|
|
else // Backward 상태 - 좌우 반전
|
|
{
|
|
// Backward에서 좌측 = crossProduct < 0
|
|
if (crossProduct < -0.5f)
|
|
{
|
|
baseScore = 100.0f * _weights.LeftWeight;
|
|
}
|
|
else if (crossProduct < 0.0f)
|
|
{
|
|
baseScore = 70.0f * _weights.LeftWeight;
|
|
}
|
|
else if (crossProduct < 0.5f)
|
|
{
|
|
baseScore = 50.0f * _weights.BackwardWeight;
|
|
}
|
|
else
|
|
{
|
|
baseScore = 30.0f * _weights.RightWeight;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case AgvDirection.Right:
|
|
// Right: 우측 방향 선호
|
|
// Forward 상태에서: crossProduct < 0 = 우측
|
|
// Backward 상태에서: crossProduct > 0 = 우측 (반대)
|
|
if (dotProduct > 0.0f) // Forward 상태
|
|
{
|
|
// crossProduct < 0이면 우측
|
|
if (crossProduct < -0.5f)
|
|
{
|
|
baseScore = 100.0f * _weights.RightWeight;
|
|
}
|
|
else if (crossProduct < 0.0f)
|
|
{
|
|
baseScore = 70.0f * _weights.RightWeight;
|
|
}
|
|
else if (crossProduct < 0.5f)
|
|
{
|
|
baseScore = 50.0f * _weights.ForwardWeight;
|
|
}
|
|
else
|
|
{
|
|
baseScore = 30.0f * _weights.LeftWeight;
|
|
}
|
|
}
|
|
else // Backward 상태 - 좌우 반전
|
|
{
|
|
// Backward에서 우측 = crossProduct > 0
|
|
if (crossProduct > 0.5f)
|
|
{
|
|
baseScore = 100.0f * _weights.RightWeight;
|
|
}
|
|
else if (crossProduct > 0.0f)
|
|
{
|
|
baseScore = 70.0f * _weights.RightWeight;
|
|
}
|
|
else if (crossProduct > -0.5f)
|
|
{
|
|
baseScore = 50.0f * _weights.BackwardWeight;
|
|
}
|
|
else
|
|
{
|
|
baseScore = 30.0f * _weights.LeftWeight;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
return baseScore;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 벡터 간 각도를 도 단위로 계산
|
|
/// </summary>
|
|
private float CalculateAngle(PointF vector1, PointF vector2)
|
|
{
|
|
float dotProduct = (vector1.X * vector2.X) + (vector1.Y * vector2.Y);
|
|
float magnitude1 = (float)Math.Sqrt(vector1.X * vector1.X + vector1.Y * vector1.Y);
|
|
float magnitude2 = (float)Math.Sqrt(vector2.X * vector2.X + vector2.Y * vector2.Y);
|
|
|
|
if (magnitude1 < 0.001f || magnitude2 < 0.001f)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
float cosAngle = dotProduct / (magnitude1 * magnitude2);
|
|
cosAngle = Math.Max(-1.0f, Math.Min(1.0f, cosAngle)); // 범위 제한
|
|
|
|
return (float)(Math.Acos(cosAngle) * 180.0 / Math.PI);
|
|
}
|
|
}
|
|
}
|