fix: Add motor direction parameter to magnet direction calculation in pathfinding
- Fixed critical issue in ConvertToDetailedPath where motor direction was not passed to GetRequiredMagnetDirection - Motor direction is essential for backward movement as Left/Right directions must be inverted - Modified AGVPathfinder.cs line 280 to pass currentDirection parameter - Ensures backward motor direction properly inverts magnet sensor directions feat: Add waypoint support to pathfinding system - Added FindPath overload with params string[] waypointNodeIds in AStarPathfinder - Supports sequential traversal through multiple intermediate nodes - Validates waypoints and prevents duplicates in sequence - Returns combined path result with aggregated metrics feat: Implement path result merging with DetailedPath preservation - Added CombineResults method in AStarPathfinder for intelligent path merging - Automatically deduplicates nodes when last of previous path equals first of current - Preserves DetailedPath information including motor and magnet directions - Essential for multi-segment path operations feat: Integrate magnet direction with motor direction awareness - Modified JunctionAnalyzer.GetRequiredMagnetDirection to accept AgvDirection parameter - Inverts Left/Right magnet directions when moving Backward - Properly handles motor direction context throughout pathfinding feat: Add automatic start node selection in simulator - Added SetStartNodeToCombo method to SimulatorForm - Automatically selects start node combo box when AGV position is set via RFID - Improves UI usability and workflow efficiency 🤖 Generated with [Claude Code](https://claude.com/claude-code) Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
backuppc
@@ -206,9 +206,14 @@ namespace AGVNavigationCore.PathFinding.Analysis
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 특정 경로에서 요구되는 마그넷 방향 계산 (전진 방향 기준)
|
||||
/// 특정 경로에서 요구되는 마그넷 방향 계산
|
||||
/// </summary>
|
||||
public MagnetDirection GetRequiredMagnetDirection(string fromNodeId, string currentNodeId, string toNodeId)
|
||||
/// <param name="fromNodeId">이전 노드 ID</param>
|
||||
/// <param name="currentNodeId">현재 노드 ID</param>
|
||||
/// <param name="toNodeId">목표 노드 ID</param>
|
||||
/// <param name="motorDirection">AGV 모터 방향 (Forward/Backward)</param>
|
||||
/// <returns>마그넷 방향 (모터 방향 고려)</returns>
|
||||
public MagnetDirection GetRequiredMagnetDirection(string fromNodeId, string currentNodeId, string toNodeId, AgvDirection motorDirection )
|
||||
{
|
||||
if (!_junctions.ContainsKey(currentNodeId))
|
||||
return MagnetDirection.Straight;
|
||||
@@ -240,12 +245,26 @@ namespace AGVNavigationCore.PathFinding.Analysis
|
||||
|
||||
// 전진 방향 기준으로 마그넷 방향 결정
|
||||
// 각도 차이가 작으면 직진, 음수면 왼쪽, 양수면 오른쪽
|
||||
MagnetDirection baseMagnetDirection;
|
||||
if (Math.Abs(angleDiff) < Math.PI / 6) // 30도 이내는 직진
|
||||
return MagnetDirection.Straight;
|
||||
baseMagnetDirection = MagnetDirection.Straight;
|
||||
else if (angleDiff < 0) // 음수면 왼쪽 회전
|
||||
return MagnetDirection.Left;
|
||||
baseMagnetDirection = MagnetDirection.Left;
|
||||
else // 양수면 오른쪽 회전
|
||||
return MagnetDirection.Right;
|
||||
baseMagnetDirection = MagnetDirection.Right;
|
||||
|
||||
// 후진 모터 방향일 경우 마그넷 방향 반대로 설정
|
||||
// Forward: Left/Right 그대로 사용
|
||||
// Backward: Left ↔ Right 반대로 사용
|
||||
if (motorDirection == AgvDirection.Backward)
|
||||
{
|
||||
if (baseMagnetDirection == MagnetDirection.Left)
|
||||
return MagnetDirection.Right;
|
||||
else if (baseMagnetDirection == MagnetDirection.Right)
|
||||
return MagnetDirection.Left;
|
||||
}
|
||||
|
||||
return baseMagnetDirection;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@@ -27,11 +27,6 @@ namespace AGVNavigationCore.PathFinding.Core
|
||||
/// </summary>
|
||||
public List<AgvDirection> Commands { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 노드별 모터방향 정보 목록
|
||||
/// </summary>
|
||||
public List<NodeMotorInfo> NodeMotorInfos { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 총 거리
|
||||
/// </summary>
|
||||
@@ -104,7 +99,6 @@ namespace AGVNavigationCore.PathFinding.Core
|
||||
Success = false;
|
||||
Path = new List<string>();
|
||||
Commands = new List<AgvDirection>();
|
||||
NodeMotorInfos = new List<NodeMotorInfo>();
|
||||
DetailedPath = new List<NodeMotorInfo>();
|
||||
TotalDistance = 0;
|
||||
CalculationTimeMs = 0;
|
||||
@@ -157,7 +151,6 @@ namespace AGVNavigationCore.PathFinding.Core
|
||||
Success = true,
|
||||
Path = new List<string>(path),
|
||||
Commands = new List<AgvDirection>(commands),
|
||||
NodeMotorInfos = new List<NodeMotorInfo>(nodeMotorInfos),
|
||||
TotalDistance = totalDistance,
|
||||
CalculationTimeMs = calculationTimeMs
|
||||
};
|
||||
|
||||
@@ -3,6 +3,7 @@ using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using AGVNavigationCore.Models;
|
||||
using AGVNavigationCore.PathFinding.Planning;
|
||||
|
||||
namespace AGVNavigationCore.PathFinding.Core
|
||||
{
|
||||
@@ -166,6 +167,247 @@ namespace AGVNavigationCore.PathFinding.Core
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 경유지를 거쳐 경로 찾기 (오버로드)
|
||||
/// 여러 경유지를 순차적으로 거쳐서 최종 목적지까지의 경로를 계산합니다.
|
||||
/// 기존 FindPath를 여러 번 호출하여 각 구간의 경로를 합칩니다.
|
||||
/// </summary>
|
||||
/// <param name="startNodeId">시작 노드 ID</param>
|
||||
/// <param name="endNodeId">최종 목적지 노드 ID</param>
|
||||
/// <param name="waypointNodeIds">경유지 노드 ID 배열 (선택사항)</param>
|
||||
/// <returns>경로 계산 결과 (모든 경유지를 거친 전체 경로)</returns>
|
||||
public AGVPathResult FindPath(string startNodeId, string endNodeId, params string[] waypointNodeIds)
|
||||
{
|
||||
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
|
||||
|
||||
try
|
||||
{
|
||||
// 경유지가 없으면 기본 FindPath 호출
|
||||
if (waypointNodeIds == null || waypointNodeIds.Length == 0)
|
||||
{
|
||||
return FindPath(startNodeId, endNodeId);
|
||||
}
|
||||
|
||||
// 경유지 유효성 검증
|
||||
var validWaypoints = new List<string>();
|
||||
foreach (var waypointId in waypointNodeIds)
|
||||
{
|
||||
if (string.IsNullOrEmpty(waypointId))
|
||||
continue;
|
||||
|
||||
if (!_nodeMap.ContainsKey(waypointId))
|
||||
{
|
||||
return AGVPathResult.CreateFailure($"경유지 노드를 찾을 수 없습니다: {waypointId}", stopwatch.ElapsedMilliseconds, 0);
|
||||
}
|
||||
|
||||
validWaypoints.Add(waypointId);
|
||||
}
|
||||
|
||||
// 경유지가 없으면 기본 경로 계산
|
||||
if (validWaypoints.Count == 0)
|
||||
{
|
||||
return FindPath(startNodeId, endNodeId);
|
||||
}
|
||||
|
||||
// 첫 번째 경유지가 시작노드와 같은지 검사
|
||||
if (validWaypoints[0] == startNodeId)
|
||||
{
|
||||
return AGVPathResult.CreateFailure(
|
||||
$"첫 번째 경유지({validWaypoints[0]})가 시작 노드({startNodeId})와 동일합니다. 경유지는 시작노드와 달라야 합니다.",
|
||||
stopwatch.ElapsedMilliseconds, 0);
|
||||
}
|
||||
|
||||
// 마지막 경유지가 목적지노드와 같은지 검사
|
||||
if (validWaypoints[validWaypoints.Count - 1] == endNodeId)
|
||||
{
|
||||
return AGVPathResult.CreateFailure(
|
||||
$"마지막 경유지({validWaypoints[validWaypoints.Count - 1]})가 목적지 노드({endNodeId})와 동일합니다. 경유지는 목적지노드와 달라야 합니다.",
|
||||
stopwatch.ElapsedMilliseconds, 0);
|
||||
}
|
||||
|
||||
// 연속된 중복만 제거 (순서 유지)
|
||||
// 예: [1, 2, 2, 3, 2] -> [1, 2, 3, 2] (연속 중복만 제거)
|
||||
var deduplicatedWaypoints = new List<string>();
|
||||
string lastWaypoint = null;
|
||||
foreach (var waypoint in validWaypoints)
|
||||
{
|
||||
if (waypoint != lastWaypoint)
|
||||
{
|
||||
deduplicatedWaypoints.Add(waypoint);
|
||||
lastWaypoint = waypoint;
|
||||
}
|
||||
}
|
||||
validWaypoints = deduplicatedWaypoints;
|
||||
|
||||
// 최종 경로 리스트와 누적 값
|
||||
var combinedPath = new List<string>();
|
||||
float totalDistance = 0;
|
||||
long totalCalculationTime = 0;
|
||||
|
||||
// 현재 시작점
|
||||
string currentStart = startNodeId;
|
||||
|
||||
// 1단계: 각 경유지까지의 경로 계산
|
||||
for (int i = 0; i < validWaypoints.Count; i++)
|
||||
{
|
||||
string waypoint = validWaypoints[i];
|
||||
|
||||
// 현재 위치에서 경유지까지의 경로 계산
|
||||
var segmentResult = FindPath(currentStart, waypoint);
|
||||
|
||||
if (!segmentResult.Success)
|
||||
{
|
||||
return AGVPathResult.CreateFailure(
|
||||
$"경유지 {i + 1}({waypoint})까지의 경로 계산 실패: {segmentResult.ErrorMessage}",
|
||||
stopwatch.ElapsedMilliseconds, 0);
|
||||
}
|
||||
|
||||
// 경로 합치기 (첫 번째 구간이 아니면 시작점 제거하여 중복 방지)
|
||||
if (combinedPath.Count > 0 && segmentResult.Path.Count > 0)
|
||||
{
|
||||
// 시작 노드 제거 (이전 경로의 마지막 노드와 동일)
|
||||
combinedPath.AddRange(segmentResult.Path.Skip(1));
|
||||
}
|
||||
else
|
||||
{
|
||||
combinedPath.AddRange(segmentResult.Path);
|
||||
}
|
||||
|
||||
totalDistance += segmentResult.TotalDistance;
|
||||
totalCalculationTime += segmentResult.CalculationTimeMs;
|
||||
|
||||
// 다음 경유지의 시작점은 현재 경유지
|
||||
currentStart = waypoint;
|
||||
}
|
||||
|
||||
// 2단계: 마지막 경유지에서 최종 목적지까지의 경로 계산
|
||||
var finalSegmentResult = FindPath(currentStart, endNodeId);
|
||||
|
||||
if (!finalSegmentResult.Success)
|
||||
{
|
||||
return AGVPathResult.CreateFailure(
|
||||
$"최종 목적지까지의 경로 계산 실패: {finalSegmentResult.ErrorMessage}",
|
||||
stopwatch.ElapsedMilliseconds, 0);
|
||||
}
|
||||
|
||||
// 최종 경로 합치기 (시작점 제거)
|
||||
if (combinedPath.Count > 0 && finalSegmentResult.Path.Count > 0)
|
||||
{
|
||||
combinedPath.AddRange(finalSegmentResult.Path.Skip(1));
|
||||
}
|
||||
else
|
||||
{
|
||||
combinedPath.AddRange(finalSegmentResult.Path);
|
||||
}
|
||||
|
||||
totalDistance += finalSegmentResult.TotalDistance;
|
||||
totalCalculationTime += finalSegmentResult.CalculationTimeMs;
|
||||
|
||||
stopwatch.Stop();
|
||||
|
||||
// 결과 생성
|
||||
return AGVPathResult.CreateSuccess(
|
||||
combinedPath,
|
||||
new List<AgvDirection>(),
|
||||
totalDistance,
|
||||
totalCalculationTime
|
||||
);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return AGVPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 두 경로 결과를 합치기
|
||||
/// 이전 경로의 마지막 노드와 현재 경로의 시작 노드가 같으면 시작 노드를 제거하고 합침
|
||||
/// </summary>
|
||||
/// <param name="previousResult">이전 경로 결과</param>
|
||||
/// <param name="currentResult">현재 경로 결과</param>
|
||||
/// <returns>합쳐진 경로 결과</returns>
|
||||
public AGVPathResult CombineResults( AGVPathResult previousResult, AGVPathResult currentResult)
|
||||
{
|
||||
// 입력 검증
|
||||
if (previousResult == null)
|
||||
return currentResult;
|
||||
|
||||
if (currentResult == null)
|
||||
return previousResult;
|
||||
|
||||
if (!previousResult.Success)
|
||||
return AGVPathResult.CreateFailure(
|
||||
$"이전 경로 결과 실패: {previousResult.ErrorMessage}",
|
||||
previousResult.CalculationTimeMs);
|
||||
|
||||
if (!currentResult.Success)
|
||||
return AGVPathResult.CreateFailure(
|
||||
$"현재 경로 결과 실패: {currentResult.ErrorMessage}",
|
||||
currentResult.CalculationTimeMs);
|
||||
|
||||
// 경로가 비어있는 경우 처리
|
||||
if (previousResult.Path == null || previousResult.Path.Count == 0)
|
||||
return currentResult;
|
||||
|
||||
if (currentResult.Path == null || currentResult.Path.Count == 0)
|
||||
return previousResult;
|
||||
|
||||
// 합친 경로 생성
|
||||
var combinedPath = new List<string>(previousResult.Path);
|
||||
var combinedCommands = new List<AgvDirection>(previousResult.Commands);
|
||||
var combinedDetailedPath = new List<NodeMotorInfo>(previousResult.DetailedPath ?? new List<NodeMotorInfo>());
|
||||
|
||||
// 이전 경로의 마지막 노드와 현재 경로의 시작 노드 비교
|
||||
string lastNodeOfPrevious = previousResult.Path[previousResult.Path.Count - 1];
|
||||
string firstNodeOfCurrent = currentResult.Path[0];
|
||||
|
||||
if (lastNodeOfPrevious == firstNodeOfCurrent)
|
||||
{
|
||||
// 첫 번째 노드 제거 (중복 제거)
|
||||
combinedPath.AddRange(currentResult.Path.Skip(1));
|
||||
|
||||
// DetailedPath도 첫 번째 노드 제거
|
||||
if (currentResult.DetailedPath != null && currentResult.DetailedPath.Count > 0)
|
||||
{
|
||||
combinedDetailedPath.AddRange(currentResult.DetailedPath.Skip(1));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 그대로 붙임
|
||||
combinedPath.AddRange(currentResult.Path);
|
||||
|
||||
// DetailedPath도 그대로 붙임
|
||||
if (currentResult.DetailedPath != null && currentResult.DetailedPath.Count > 0)
|
||||
{
|
||||
combinedDetailedPath.AddRange(currentResult.DetailedPath);
|
||||
}
|
||||
}
|
||||
|
||||
// 명령어 합치기
|
||||
combinedCommands.AddRange(currentResult.Commands);
|
||||
|
||||
// 총 거리 합산
|
||||
float combinedDistance = previousResult.TotalDistance + currentResult.TotalDistance;
|
||||
|
||||
// 계산 시간 합산
|
||||
long combinedCalculationTime = previousResult.CalculationTimeMs + currentResult.CalculationTimeMs;
|
||||
|
||||
// 합쳐진 결과 생성
|
||||
var result = AGVPathResult.CreateSuccess(
|
||||
combinedPath,
|
||||
combinedCommands,
|
||||
combinedDistance,
|
||||
combinedCalculationTime
|
||||
);
|
||||
|
||||
// DetailedPath 설정
|
||||
result.DetailedPath = combinedDetailedPath;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 여러 목적지 중 가장 가까운 노드로의 경로 찾기
|
||||
/// </summary>
|
||||
@@ -268,6 +510,7 @@ namespace AGVNavigationCore.PathFinding.Core
|
||||
return _nodeMap[nodeId1].ConnectedNodes.Contains(nodeId2);
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 네비게이션 가능한 노드 목록 반환
|
||||
/// </summary>
|
||||
@@ -286,5 +529,69 @@ namespace AGVNavigationCore.PathFinding.Core
|
||||
{
|
||||
return _nodeMap.ContainsKey(nodeId) ? _nodeMap[nodeId] : null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 방향 전환을 위한 대체 노드 찾기
|
||||
/// 교차로에 연결된 노드 중에서 왔던 길과 갈 길이 아닌 다른 노드를 찾음
|
||||
/// 방향 전환 시 왕복 경로에 사용될 노드
|
||||
/// </summary>
|
||||
/// <param name="junctionNodeId">교차로 노드 ID (B)</param>
|
||||
/// <param name="previousNodeId">이전 노드 ID (A - 왔던 길)</param>
|
||||
/// <param name="targetNodeId">목표 노드 ID (C - 갈 길)</param>
|
||||
/// <param name="mapNodes">전체 맵 노드 목록</param>
|
||||
/// <returns>방향 전환에 사용할 대체 노드, 없으면 null</returns>
|
||||
public MapNode FindAlternateNodeForDirectionChange(
|
||||
string junctionNodeId,
|
||||
string previousNodeId,
|
||||
string targetNodeId)
|
||||
{
|
||||
// 입력 검증
|
||||
if (string.IsNullOrEmpty(junctionNodeId) || string.IsNullOrEmpty(previousNodeId) || string.IsNullOrEmpty(targetNodeId))
|
||||
return null;
|
||||
|
||||
if (_mapNodes == null || _mapNodes.Count == 0)
|
||||
return null;
|
||||
|
||||
// 교차로 노드 찾기
|
||||
var junctionNode = _mapNodes.FirstOrDefault(n => n.NodeId == junctionNodeId);
|
||||
if (junctionNode == null || junctionNode.ConnectedNodes == null || junctionNode.ConnectedNodes.Count == 0)
|
||||
return null;
|
||||
|
||||
// 교차로에 연결된 모든 노드 중에서 조건을 만족하는 노드 찾기
|
||||
// 조건:
|
||||
// 1. 이전 노드(왔던 길)가 아님
|
||||
// 2. 목표 노드(갈 길)가 아님
|
||||
// 3. 실제로 존재하는 노드
|
||||
// 4. 활성 상태인 노드
|
||||
// 5. 네비게이션 가능한 노드
|
||||
|
||||
var alternateNodes = new List<MapNode>();
|
||||
|
||||
foreach (var connectedNodeId in junctionNode.ConnectedNodes)
|
||||
{
|
||||
// 조건 1: 왔던 길이 아님
|
||||
if (connectedNodeId == previousNodeId)
|
||||
continue;
|
||||
|
||||
// 조건 2: 갈 길이 아님
|
||||
if (connectedNodeId == targetNodeId)
|
||||
continue;
|
||||
|
||||
// 조건 3, 4, 5: 존재하고, 활성 상태이고, 네비게이션 가능
|
||||
var connectedNode = _mapNodes.FirstOrDefault(n => n.NodeId == connectedNodeId);
|
||||
if (connectedNode != null && connectedNode.IsActive && connectedNode.IsNavigationNode())
|
||||
{
|
||||
alternateNodes.Add(connectedNode);
|
||||
}
|
||||
}
|
||||
|
||||
// 찾은 노드가 없으면 null 반환
|
||||
if (alternateNodes.Count == 0)
|
||||
return null;
|
||||
|
||||
// 여러 개 찾았으면 첫 번째 노드 반환
|
||||
// (필요시 거리 기반으로 가장 가까운 노드를 선택할 수도 있음)
|
||||
return alternateNodes[0];
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -30,130 +30,236 @@ namespace AGVNavigationCore.PathFinding.Planning
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// AGV 경로 계산
|
||||
/// 지정한 노드에서 가장 가까운 교차로(3개 이상 연결된 노드)를 찾는다.
|
||||
/// </summary>
|
||||
public AGVPathResult FindPath(MapNode startNode, MapNode targetNode,
|
||||
MapNode prevNode, AgvDirection currentDirection = AgvDirection.Forward)
|
||||
/// <param name="startNode">기준이 되는 노드</param>
|
||||
/// <returns>가장 가까운 교차로 노드 (또는 null)</returns>
|
||||
public MapNode FindNearestJunction(MapNode startNode)
|
||||
{
|
||||
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
|
||||
if (startNode == null || _mapNodes == null || _mapNodes.Count == 0)
|
||||
return null;
|
||||
|
||||
try
|
||||
// 교차로: 3개 이상의 노드가 연결된 노드
|
||||
var junctions = _mapNodes.Where(n =>
|
||||
n.IsActive &&
|
||||
n.IsNavigationNode() &&
|
||||
n.ConnectedNodes != null &&
|
||||
n.ConnectedNodes.Count >= 3 &&
|
||||
n.NodeId != startNode.NodeId
|
||||
).ToList();
|
||||
|
||||
if (junctions.Count == 0)
|
||||
return null;
|
||||
|
||||
// 직선 거리 기반으로 가장 가까운 교차로 찾기
|
||||
MapNode nearestJunction = null;
|
||||
float minDistance = float.MaxValue;
|
||||
|
||||
foreach (var junction in junctions)
|
||||
{
|
||||
// 입력 검증
|
||||
if (startNode == null)
|
||||
return AGVPathResult.CreateFailure("시작 노드가 null입니다.", 0, 0);
|
||||
if (targetNode == null)
|
||||
return AGVPathResult.CreateFailure("목적지 노드가 null입니다.", 0, 0);
|
||||
if (prevNode == null)
|
||||
return AGVPathResult.CreateFailure("이전위치 노드가 null입니다.", 0, 0);
|
||||
float dx = junction.Position.X - startNode.Position.X;
|
||||
float dy = junction.Position.Y - startNode.Position.Y;
|
||||
float distance = (float)Math.Sqrt(dx * dx + dy * dy);
|
||||
|
||||
// 1. 목적지 도킹 방향 요구사항 확인 (노드의 도킹방향 속성에서 확인)
|
||||
var requiredDirection = GetRequiredDockingDirection(targetNode.DockDirection);
|
||||
|
||||
|
||||
// 통합된 경로 계획 함수 사용
|
||||
AGVPathResult result = PlanPath(startNode, targetNode, prevNode, requiredDirection, currentDirection);
|
||||
|
||||
result.CalculationTimeMs = stopwatch.ElapsedMilliseconds;
|
||||
|
||||
// 도킹 검증 수행
|
||||
if (result.Success && _mapNodes != null)
|
||||
if (distance < minDistance)
|
||||
{
|
||||
result.DockingValidation = DockingValidator.ValidateDockingDirection(result, _mapNodes, currentDirection);
|
||||
minDistance = distance;
|
||||
nearestJunction = junction;
|
||||
}
|
||||
}
|
||||
|
||||
return nearestJunction;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 지정한 노드에서 경로상 가장 가까운 교차로를 찾는다.
|
||||
/// (최단 경로 내에서 3개 이상 연결된 교차로를 찾음)
|
||||
/// </summary>
|
||||
/// <param name="startNode">시작 노드</param>
|
||||
/// <param name="targetNode">목적지 노드</param>
|
||||
/// <returns>경로상의 가장 가까운 교차로 노드 (또는 null)</returns>
|
||||
public MapNode FindNearestJunctionOnPath(AGVPathResult pathResult)
|
||||
{
|
||||
if (pathResult == null || !pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0)
|
||||
return null;
|
||||
|
||||
// 경로상의 모든 노드 중 교차로(3개 이상 연결) 찾기
|
||||
var StartNode = pathResult.Path.First();
|
||||
foreach (var nodeId in pathResult.Path)
|
||||
{
|
||||
var node = _mapNodes.FirstOrDefault(n => n.NodeId == nodeId);
|
||||
if (node != null &&
|
||||
node.IsActive &&
|
||||
node.IsNavigationNode() &&
|
||||
node.ConnectedNodes != null &&
|
||||
node.ConnectedNodes.Count >= 3)
|
||||
{
|
||||
if (node.NodeId.Equals(StartNode) == false)
|
||||
return node;
|
||||
}
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
|
||||
public AGVPathResult FindPath_test(MapNode startNode, MapNode targetNode,
|
||||
MapNode prevNode, AgvDirection currentDirection)
|
||||
{
|
||||
// 입력 검증
|
||||
if (startNode == null)
|
||||
return AGVPathResult.CreateFailure("시작 노드가 null입니다.", 0, 0);
|
||||
if (targetNode == null)
|
||||
return AGVPathResult.CreateFailure("목적지 노드가 null입니다.", 0, 0);
|
||||
if (prevNode == null)
|
||||
return AGVPathResult.CreateFailure("이전위치 노드가 null입니다.", 0, 0);
|
||||
if (startNode == targetNode)
|
||||
return AGVPathResult.CreateFailure("목적지와 현재위치가 동일합니다.", 0, 0);
|
||||
|
||||
var ReverseDirection = (currentDirection == AgvDirection.Forward ? AgvDirection.Backward : AgvDirection.Forward);
|
||||
|
||||
//1.목적지까지의 최단거리 경로를 찾는다.
|
||||
var pathResult = _basicPathfinder.FindPath(startNode.NodeId, targetNode.NodeId);
|
||||
if (!pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0)
|
||||
return AGVPathResult.CreateFailure("각 노드간 최단 경로 계산이 실패되었습니다", 0, 0);
|
||||
|
||||
//2.AGV방향과 목적지에 설정된 방향이 일치하면 그대로 진행하면된다.(목적지에 방향이 없는 경우에도 그대로 진행)
|
||||
if (targetNode.DockDirection == DockingDirection.DontCare ||
|
||||
(targetNode.DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Forward) ||
|
||||
(targetNode.DockDirection == DockingDirection.Backward && currentDirection == AgvDirection.Backward))
|
||||
{
|
||||
MakeDetailData(pathResult, currentDirection);
|
||||
MakeMagnetDirection(pathResult);
|
||||
return pathResult;
|
||||
}
|
||||
|
||||
//3. 도킹방향이 일치하지 않으니 교차로에서 방향을 회전시켜야 한다
|
||||
//최단거리(=minpath)경로에 속하는 교차로가 있다면 그것을 사용하고 없다면 가장 가까운 교차로를 찾는다.
|
||||
var JunctionInPath = FindNearestJunctionOnPath(pathResult);
|
||||
if (JunctionInPath == null)
|
||||
{
|
||||
//시작노드로부터 가까운 교차로 검색
|
||||
JunctionInPath = FindNearestJunction(startNode);
|
||||
|
||||
//종료노드로부터 가까운 교차로 검색
|
||||
if (JunctionInPath == null) JunctionInPath = FindNearestJunction(targetNode);
|
||||
}
|
||||
if (JunctionInPath == null)
|
||||
return AGVPathResult.CreateFailure("교차로가 없어 경로계산을 할 수 없습니다", 0, 0);
|
||||
|
||||
//경유지를 포함하여 경로를 다시 계산한다.
|
||||
|
||||
//1.시작위치 - 교차로(여기까지는 현재 방향으로 그대로 이동을 한다)
|
||||
var path1 = _basicPathfinder.FindPath(startNode.NodeId, JunctionInPath.NodeId);
|
||||
|
||||
// path1의 상세 경로 정보 채우기 (모터 방향 설정)
|
||||
MakeDetailData(path1, currentDirection);
|
||||
|
||||
//2.교차로 - 종료위치
|
||||
var path2 = _basicPathfinder.FindPath(JunctionInPath.NodeId, targetNode.NodeId);
|
||||
MakeDetailData(path2, ReverseDirection);
|
||||
|
||||
//3.방향전환을 위환 대체 노드찾기
|
||||
var tempNode = _basicPathfinder.FindAlternateNodeForDirectionChange(JunctionInPath.NodeId,
|
||||
path1.Path[path1.Path.Count - 2],
|
||||
path2.Path[1]);
|
||||
|
||||
//4. path1 + tempnode + path2 가 최종 위치가 된다.
|
||||
if (tempNode == null)
|
||||
return AGVPathResult.CreateFailure("방향 전환을 위한 대체 노드를 찾을 수 없습니다.", 0, 0);
|
||||
|
||||
// path1 (시작 → 교차로)
|
||||
var combinedResult = path1;
|
||||
|
||||
// 교차로 → 대체노드 경로 계산
|
||||
var pathToTemp = _basicPathfinder.FindPath(JunctionInPath.NodeId, tempNode.NodeId);
|
||||
if (!pathToTemp.Success)
|
||||
return AGVPathResult.CreateFailure("교차로에서 대체 노드까지의 경로를 찾을 수 없습니다.", 0, 0);
|
||||
MakeDetailData(pathToTemp, currentDirection);
|
||||
if (pathToTemp.DetailedPath.Count > 1)
|
||||
pathToTemp.DetailedPath[pathToTemp.DetailedPath.Count - 1].MotorDirection = ReverseDirection;
|
||||
|
||||
// path1 + pathToTemp 합치기
|
||||
combinedResult = _basicPathfinder.CombineResults(combinedResult, pathToTemp);
|
||||
|
||||
// 대체노드 → 교차로 경로 계산 (역방향)
|
||||
var pathFromTemp = _basicPathfinder.FindPath(tempNode.NodeId, JunctionInPath.NodeId);
|
||||
if (!pathFromTemp.Success)
|
||||
return AGVPathResult.CreateFailure("대체 노드에서 교차로까지의 경로를 찾을 수 없습니다.", 0, 0);
|
||||
MakeDetailData(pathFromTemp, ReverseDirection);
|
||||
|
||||
// (path1 + pathToTemp) + pathFromTemp 합치기
|
||||
combinedResult = _basicPathfinder.CombineResults(combinedResult, pathFromTemp);
|
||||
|
||||
// (path1 + pathToTemp + pathFromTemp) + path2 합치기
|
||||
combinedResult = _basicPathfinder.CombineResults(combinedResult, path2);
|
||||
|
||||
MakeMagnetDirection(combinedResult);
|
||||
|
||||
return combinedResult;
|
||||
|
||||
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 이 작업후에 MakeMagnetDirection 를 추가로 실행 하세요
|
||||
/// </summary>
|
||||
/// <param name="path1"></param>
|
||||
/// <param name="currentDirection"></param>
|
||||
private void MakeDetailData(AGVPathResult path1, AgvDirection currentDirection)
|
||||
{
|
||||
if (path1.Success && path1.Path != null && path1.Path.Count > 0)
|
||||
{
|
||||
var detailedPath1 = new List<NodeMotorInfo>();
|
||||
for (int i = 0; i < path1.Path.Count; i++)
|
||||
{
|
||||
string nodeId = path1.Path[i];
|
||||
string nextNodeId = (i + 1 < path1.Path.Count) ? path1.Path[i + 1] : null;
|
||||
|
||||
// 노드 정보 생성 (현재 방향 유지)
|
||||
var nodeInfo = new NodeMotorInfo(
|
||||
nodeId,
|
||||
currentDirection,
|
||||
nextNodeId,
|
||||
MagnetDirection.Straight
|
||||
);
|
||||
|
||||
detailedPath1.Add(nodeInfo);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return AGVPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
|
||||
// path1에 상세 경로 정보 설정
|
||||
path1.DetailedPath = detailedPath1;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 노드 도킹 방향에 따른 필요한 AGV 방향 반환
|
||||
/// Path에 등록된 방향을 확인하여 마그넷정보를 업데이트 합니다
|
||||
/// </summary>
|
||||
private AgvDirection? GetRequiredDockingDirection(DockingDirection dockDirection)
|
||||
/// <param name="path1"></param>
|
||||
private void MakeMagnetDirection(AGVPathResult path1)
|
||||
{
|
||||
switch (dockDirection)
|
||||
if (path1.Success && path1.DetailedPath != null && path1.DetailedPath.Count > 0)
|
||||
{
|
||||
case DockingDirection.Forward:
|
||||
return AgvDirection.Forward; // 전진 도킹
|
||||
case DockingDirection.Backward:
|
||||
return AgvDirection.Backward; // 후진 도킹
|
||||
case DockingDirection.DontCare:
|
||||
default:
|
||||
return null; // 도킹 방향 상관없음
|
||||
for (int i = 0; i < path1.DetailedPath.Count; i++)
|
||||
{
|
||||
var detailPath = path1.DetailedPath[i];
|
||||
string nodeId = path1.Path[i];
|
||||
string nextNodeId = (i + 1 < path1.Path.Count) ? path1.Path[i + 1] : null;
|
||||
|
||||
// 마그넷 방향 계산 (3개 이상 연결된 교차로에서만 좌/우 가중치 적용)
|
||||
if (i > 0 && nextNodeId != null)
|
||||
{
|
||||
string prevNodeId = path1.Path[i - 1];
|
||||
if (path1.DetailedPath[i - 1].MotorDirection != detailPath.MotorDirection)
|
||||
detailPath.MagnetDirection = MagnetDirection.Straight;
|
||||
else
|
||||
detailPath.MagnetDirection = _junctionAnalyzer.GetRequiredMagnetDirection(prevNodeId, nodeId, nextNodeId, detailPath.MotorDirection);
|
||||
}
|
||||
else detailPath.MagnetDirection = MagnetDirection.Straight;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 통합 경로 계획 (직접 경로 또는 방향 전환 경로)
|
||||
/// </summary>
|
||||
private AGVPathResult PlanPath(MapNode startNode, MapNode targetNode, MapNode prevNode, AgvDirection? requiredDirection = null, AgvDirection currentDirection = AgvDirection.Forward)
|
||||
{
|
||||
|
||||
bool needDirectionChange = requiredDirection.HasValue && (currentDirection != requiredDirection.Value);
|
||||
|
||||
//현재 위치에서 목적지까지의 최단 거리 모록을 찾는다.
|
||||
var DirectPathResult = _basicPathfinder.FindPath(startNode.NodeId, targetNode.NodeId);
|
||||
|
||||
//이전 위치에서 목적지까지의 최단 거리를 모록을 찾는다.
|
||||
var DirectPathResultP = _basicPathfinder.FindPath(prevNode.NodeId, targetNode.NodeId);
|
||||
|
||||
//
|
||||
if (DirectPathResultP.Path.Contains(startNode.NodeId))
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
if (needDirectionChange)
|
||||
{
|
||||
// 방향 전환 경로 계획
|
||||
var directionChangePlan = _directionChangePlanner.PlanDirectionChange(
|
||||
startNode.NodeId, targetNode.NodeId, currentDirection, requiredDirection.Value);
|
||||
|
||||
if (!directionChangePlan.Success)
|
||||
{
|
||||
return AGVPathResult.CreateFailure(directionChangePlan.ErrorMessage, 0, 0);
|
||||
}
|
||||
|
||||
var detailedPath = ConvertDirectionChangePath(directionChangePlan, currentDirection, requiredDirection.Value);
|
||||
float totalDistance = CalculatePathDistance(detailedPath);
|
||||
|
||||
return AGVPathResult.CreateSuccess(
|
||||
detailedPath,
|
||||
totalDistance,
|
||||
0,
|
||||
0,
|
||||
directionChangePlan.PlanDescription,
|
||||
true,
|
||||
directionChangePlan.DirectionChangeNode
|
||||
);
|
||||
}
|
||||
else
|
||||
{
|
||||
// 직접 경로 계획
|
||||
var basicResult = _basicPathfinder.FindPath(startNode.NodeId, targetNode.NodeId);
|
||||
|
||||
if (!basicResult.Success)
|
||||
{
|
||||
return AGVPathResult.CreateFailure(basicResult.ErrorMessage, basicResult.CalculationTimeMs, basicResult.ExploredNodeCount);
|
||||
}
|
||||
|
||||
var detailedPath = ConvertToDetailedPath(basicResult.Path, currentDirection);
|
||||
|
||||
return AGVPathResult.CreateSuccess(
|
||||
detailedPath,
|
||||
basicResult.TotalDistance,
|
||||
basicResult.CalculationTimeMs,
|
||||
basicResult.ExploredNodeCount,
|
||||
"직접 경로 - 방향 전환 불필요"
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
@@ -174,7 +280,7 @@ namespace AGVNavigationCore.PathFinding.Planning
|
||||
if (i > 0 && nextNodeId != null)
|
||||
{
|
||||
string prevNodeId = simplePath[i - 1];
|
||||
magnetDirection = _junctionAnalyzer.GetRequiredMagnetDirection(prevNodeId, currentNodeId, nextNodeId);
|
||||
magnetDirection = _junctionAnalyzer.GetRequiredMagnetDirection(prevNodeId, currentNodeId, nextNodeId, currentDirection);
|
||||
}
|
||||
|
||||
// 노드 정보 생성
|
||||
@@ -198,60 +304,6 @@ namespace AGVNavigationCore.PathFinding.Planning
|
||||
return detailedPath;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 방향 전환 경로를 상세 경로로 변환
|
||||
/// </summary>
|
||||
private List<NodeMotorInfo> ConvertDirectionChangePath(DirectionChangePlanner.DirectionChangePlan plan, AgvDirection startDirection, AgvDirection endDirection)
|
||||
{
|
||||
var detailedPath = new List<NodeMotorInfo>();
|
||||
var currentDirection = startDirection;
|
||||
|
||||
for (int i = 0; i < plan.DirectionChangePath.Count; i++)
|
||||
{
|
||||
string currentNodeId = plan.DirectionChangePath[i];
|
||||
string nextNodeId = (i + 1 < plan.DirectionChangePath.Count) ? plan.DirectionChangePath[i + 1] : null;
|
||||
|
||||
// 방향 전환 노드에서 방향 변경
|
||||
if (currentNodeId == plan.DirectionChangeNode && currentDirection != endDirection)
|
||||
{
|
||||
currentDirection = endDirection;
|
||||
}
|
||||
|
||||
// 마그넷 방향 계산
|
||||
MagnetDirection magnetDirection = MagnetDirection.Straight;
|
||||
if (i > 0 && nextNodeId != null)
|
||||
{
|
||||
string prevNodeId = plan.DirectionChangePath[i - 1];
|
||||
magnetDirection = _junctionAnalyzer.GetRequiredMagnetDirection(prevNodeId, currentNodeId, nextNodeId);
|
||||
}
|
||||
|
||||
// 특수 동작 확인
|
||||
bool requiresSpecialAction = false;
|
||||
string specialActionDescription = "";
|
||||
|
||||
if (currentNodeId == plan.DirectionChangeNode)
|
||||
{
|
||||
requiresSpecialAction = true;
|
||||
specialActionDescription = $"방향전환: {startDirection} → {endDirection}";
|
||||
}
|
||||
|
||||
// 노드 정보 생성
|
||||
var nodeMotorInfo = new NodeMotorInfo(
|
||||
currentNodeId,
|
||||
currentDirection,
|
||||
nextNodeId,
|
||||
true, // 방향 전환 경로의 경우 회전 가능으로 설정
|
||||
currentNodeId == plan.DirectionChangeNode,
|
||||
magnetDirection,
|
||||
requiresSpecialAction,
|
||||
specialActionDescription
|
||||
);
|
||||
|
||||
detailedPath.Add(nodeMotorInfo);
|
||||
}
|
||||
|
||||
return detailedPath;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 경로 총 거리 계산
|
||||
|
||||
@@ -0,0 +1,329 @@
|
||||
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.NodeId) && n.NodeId != currentNode.NodeId
|
||||
).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].NodeId; // 첫 번째 연결 노드 반환
|
||||
}
|
||||
|
||||
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.NodeId;
|
||||
}
|
||||
|
||||
/// <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);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user