281 lines
12 KiB
C#
281 lines
12 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Drawing;
|
|
using System.Linq;
|
|
using AGVNavigationCore.Models;
|
|
|
|
namespace AGVNavigationCore.Utils
|
|
{
|
|
/// <summary>
|
|
/// AGV 리프트 방향 계산 유틸리티 클래스
|
|
/// 모든 리프트 방향 계산 로직을 중앙화하여 일관성 보장
|
|
/// </summary>
|
|
public static class LiftCalculator
|
|
{
|
|
|
|
/// <summary>
|
|
/// 경로 예측 기반 리프트 방향 계산
|
|
/// 현재 노드에서 연결된 다음 노드들을 분석하여 리프트 방향 결정
|
|
/// </summary>
|
|
/// <param name="currentPos">현재 위치</param>
|
|
/// <param name="previousPos">이전 위치</param>
|
|
/// <param name="motorDirection">모터 방향</param>
|
|
/// <param name="mapNodes">맵 노드 리스트 (경로 예측용)</param>
|
|
/// <param name="tolerance">위치 허용 오차</param>
|
|
/// <returns>리프트 계산 결과</returns>
|
|
public static LiftCalculationResult CalculateLiftInfoWithPathPrediction(
|
|
Point currentPos, Point previousPos, AgvDirection motorDirection,
|
|
List<MapNode> mapNodes, int tolerance = 10)
|
|
{
|
|
if (mapNodes == null || mapNodes.Count == 0)
|
|
{
|
|
// 맵 노드 정보가 없으면 기존 방식 사용
|
|
return CalculateLiftInfo(previousPos, currentPos, motorDirection);
|
|
}
|
|
|
|
// 현재 위치에 해당하는 노드 찾기
|
|
var currentNode = FindNodeByPosition(mapNodes, currentPos, tolerance);
|
|
|
|
if (currentNode == null)
|
|
{
|
|
// 현재 노드를 찾을 수 없으면 기존 방식 사용
|
|
return CalculateLiftInfo(previousPos, currentPos, motorDirection);
|
|
}
|
|
|
|
// 이전 위치에 해당하는 노드 찾기
|
|
var previousNode = FindNodeByPosition(mapNodes, previousPos, tolerance);
|
|
|
|
Point targetPosition;
|
|
string calculationMethod;
|
|
|
|
// 모터 방향에 따른 예측 방향 결정
|
|
if (motorDirection == AgvDirection.Backward)
|
|
{
|
|
// 후진 모터: AGV가 리프트 쪽(목표 위치)으로 이동
|
|
// 경로 예측 없이 단순히 현재→목표 방향 사용
|
|
return CalculateLiftInfo(currentPos, previousPos, motorDirection);
|
|
}
|
|
else
|
|
{
|
|
// 전진 모터: 기존 로직 (다음 노드 예측)
|
|
var nextNodes = GetConnectedNodes(mapNodes, currentNode);
|
|
|
|
// 이전 노드 제외 (되돌아가는 방향 제외)
|
|
if (previousNode != null)
|
|
{
|
|
nextNodes = nextNodes.Where(n => n.Id != previousNode.Id).ToList();
|
|
}
|
|
|
|
if (nextNodes.Count == 1)
|
|
{
|
|
// 직선 경로: 다음 노드 방향으로 예측
|
|
targetPosition = nextNodes.First().Position;
|
|
calculationMethod = $"전진 경로 예측 ({currentNode.Id}→{nextNodes.First().Id})";
|
|
}
|
|
else if (nextNodes.Count > 1)
|
|
{
|
|
// 갈래길: 이전 위치 기반 계산 사용
|
|
var prevResult = CalculateLiftInfo(previousPos, currentPos, motorDirection);
|
|
prevResult.CalculationMethod += " (전진 갈래길)";
|
|
return prevResult;
|
|
}
|
|
else
|
|
{
|
|
// 연결된 노드가 없으면 기존 방식 사용
|
|
return CalculateLiftInfo(previousPos, currentPos, motorDirection);
|
|
}
|
|
}
|
|
|
|
// 리프트 각도 계산
|
|
var angleRadians = CalculateLiftAngleRadians(currentPos, targetPosition, motorDirection);
|
|
var angleDegrees = angleRadians * 180.0 / Math.PI;
|
|
|
|
// 0-360도 범위로 정규화
|
|
while (angleDegrees < 0) angleDegrees += 360;
|
|
while (angleDegrees >= 360) angleDegrees -= 360;
|
|
|
|
var directionString = AngleToDirectionString(angleDegrees);
|
|
|
|
return new LiftCalculationResult
|
|
{
|
|
AngleRadians = angleRadians,
|
|
AngleDegrees = angleDegrees,
|
|
DirectionString = directionString,
|
|
CalculationMethod = calculationMethod,
|
|
MotorDirection = motorDirection
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// AGV 이동 방향과 모터 방향을 기반으로 리프트 각도 계산
|
|
/// </summary>
|
|
/// <param name="currentPos">현재 위치</param>
|
|
/// <param name="targetPos">목표 위치</param>
|
|
/// <param name="motorDirection">모터 방향</param>
|
|
/// <returns>리프트 각도 (라디안)</returns>
|
|
public static double CalculateLiftAngleRadians(Point currentPos, Point targetPos, AgvDirection motorDirection)
|
|
{
|
|
// 모터 방향에 따른 리프트 위치 계산
|
|
if (motorDirection == AgvDirection.Forward)
|
|
{
|
|
// 전진 모터: AGV가 앞으로 가므로 리프트는 뒤쪽 (타겟 → 현재 방향)
|
|
var dx = currentPos.X - targetPos.X;
|
|
var dy = currentPos.Y - targetPos.Y;
|
|
return Math.Atan2(dy, dx);
|
|
}
|
|
else if (motorDirection == AgvDirection.Backward)
|
|
{
|
|
// 후진 모터: AGV가 리프트 쪽으로 이동하므로 리프트는 AGV 이동 방향에 위치
|
|
// 007→006 후진시: 리프트는 006방향(이동방향)을 향해야 함 (타겟→현재 반대방향)
|
|
var dx = currentPos.X - targetPos.X;
|
|
var dy = currentPos.Y - targetPos.Y;
|
|
return Math.Atan2(dy, dx);
|
|
}
|
|
else
|
|
{
|
|
// 기본값: 전진 모터와 동일
|
|
var dx = currentPos.X - targetPos.X;
|
|
var dy = currentPos.Y - targetPos.Y;
|
|
return Math.Atan2(dy, dx);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// AGV 이동 방향과 모터 방향을 기반으로 리프트 각도 계산 (도 단위)
|
|
/// </summary>
|
|
/// <param name="currentPos">현재 위치</param>
|
|
/// <param name="targetPos">목표 위치</param>
|
|
/// <param name="motorDirection">모터 방향</param>
|
|
/// <returns>리프트 각도 (도)</returns>
|
|
public static double CalculateLiftAngleDegrees(Point currentPos, Point targetPos, AgvDirection motorDirection)
|
|
{
|
|
var radians = CalculateLiftAngleRadians(currentPos, targetPos, motorDirection);
|
|
var degrees = radians * 180.0 / Math.PI;
|
|
|
|
// 0-360도 범위로 정규화
|
|
while (degrees < 0) degrees += 360;
|
|
while (degrees >= 360) degrees -= 360;
|
|
|
|
return degrees;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 각도를 8방향 문자열로 변환 (화면 좌표계 기준)
|
|
/// 화면 좌표계: 0°=동쪽, 90°=남쪽, 180°=서쪽, 270°=북쪽
|
|
/// </summary>
|
|
/// <param name="angleDegrees">각도 (도)</param>
|
|
/// <returns>방향 문자열</returns>
|
|
public static string AngleToDirectionString(double angleDegrees)
|
|
{
|
|
// 0-360도 범위로 정규화
|
|
while (angleDegrees < 0) angleDegrees += 360;
|
|
while (angleDegrees >= 360) angleDegrees -= 360;
|
|
|
|
// 8방향으로 분류 (화면 좌표계)
|
|
if (angleDegrees >= 337.5 || angleDegrees < 22.5)
|
|
return "동쪽(→)";
|
|
else if (angleDegrees >= 22.5 && angleDegrees < 67.5)
|
|
return "남동쪽(↘)";
|
|
else if (angleDegrees >= 67.5 && angleDegrees < 112.5)
|
|
return "남쪽(↓)";
|
|
else if (angleDegrees >= 112.5 && angleDegrees < 157.5)
|
|
return "남서쪽(↙)";
|
|
else if (angleDegrees >= 157.5 && angleDegrees < 202.5)
|
|
return "서쪽(←)";
|
|
else if (angleDegrees >= 202.5 && angleDegrees < 247.5)
|
|
return "북서쪽(↖)";
|
|
else if (angleDegrees >= 247.5 && angleDegrees < 292.5)
|
|
return "북쪽(↑)";
|
|
else if (angleDegrees >= 292.5 && angleDegrees < 337.5)
|
|
return "북동쪽(↗)";
|
|
else
|
|
return "알 수 없음";
|
|
}
|
|
|
|
/// <summary>
|
|
/// 리프트 계산 결과 정보
|
|
/// </summary>
|
|
public class LiftCalculationResult
|
|
{
|
|
public double AngleRadians { get; set; }
|
|
public double AngleDegrees { get; set; }
|
|
public string DirectionString { get; set; }
|
|
public string CalculationMethod { get; set; }
|
|
public AgvDirection MotorDirection { get; set; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// 종합적인 리프트 계산 (모든 정보 포함)
|
|
/// </summary>
|
|
/// <param name="currentPos">현재 위치</param>
|
|
/// <param name="targetPos">목표 위치</param>
|
|
/// <param name="motorDirection">모터 방향</param>
|
|
/// <returns>리프트 계산 결과</returns>
|
|
public static LiftCalculationResult CalculateLiftInfo(Point currentPos, Point targetPos, AgvDirection motorDirection)
|
|
{
|
|
var angleRadians = CalculateLiftAngleRadians(currentPos, targetPos, motorDirection);
|
|
var angleDegrees = angleRadians * 180.0 / Math.PI;
|
|
|
|
// 0-360도 범위로 정규화
|
|
while (angleDegrees < 0) angleDegrees += 360;
|
|
while (angleDegrees >= 360) angleDegrees -= 360;
|
|
|
|
var directionString = AngleToDirectionString(angleDegrees);
|
|
|
|
string calculationMethod;
|
|
if (motorDirection == AgvDirection.Forward)
|
|
calculationMethod = "이동방향 + 180도 (전진모터)";
|
|
else if (motorDirection == AgvDirection.Backward)
|
|
calculationMethod = "이동방향과 동일 (후진모터 - 리프트는 이동방향에 위치)";
|
|
else
|
|
calculationMethod = "기본값 (전진모터)";
|
|
|
|
return new LiftCalculationResult
|
|
{
|
|
AngleRadians = angleRadians,
|
|
AngleDegrees = angleDegrees,
|
|
DirectionString = directionString,
|
|
CalculationMethod = calculationMethod,
|
|
MotorDirection = motorDirection
|
|
};
|
|
}
|
|
|
|
|
|
|
|
/// <summary>
|
|
/// 위치 기반 노드 찾기
|
|
/// </summary>
|
|
/// <param name="mapNodes">맵 노드 리스트</param>
|
|
/// <param name="position">찾을 위치</param>
|
|
/// <param name="tolerance">허용 오차</param>
|
|
/// <returns>해당하는 노드 또는 null</returns>
|
|
private static MapNode FindNodeByPosition(List<MapNode> mapNodes, Point position, int tolerance)
|
|
{
|
|
return mapNodes.FirstOrDefault(node =>
|
|
Math.Abs(node.Position.X - position.X) <= tolerance &&
|
|
Math.Abs(node.Position.Y - position.Y) <= tolerance);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 노드에서 연결된 다른 노드들 찾기
|
|
/// </summary>
|
|
/// <param name="mapNodes">맵 노드 리스트</param>
|
|
/// <param name="currentNode">현재 노드</param>
|
|
/// <returns>연결된 노드 리스트</returns>
|
|
private static List<MapNode> GetConnectedNodes(List<MapNode> mapNodes, MapNode currentNode)
|
|
{
|
|
var connectedNodes = new List<MapNode>();
|
|
|
|
foreach (var nodeId in currentNode.ConnectedNodes)
|
|
{
|
|
var connectedNode = mapNodes.FirstOrDefault(n => n.Id == nodeId);
|
|
if (connectedNode != null)
|
|
{
|
|
connectedNodes.Add(connectedNode);
|
|
}
|
|
}
|
|
|
|
return connectedNodes;
|
|
}
|
|
}
|
|
} |