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
@@ -87,12 +87,12 @@ namespace AGVNavigationCore.Models
|
||||
/// <summary>
|
||||
/// 목표 위치
|
||||
/// </summary>
|
||||
Point? TargetPosition { get; }
|
||||
Point? PrevPosition { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 목표 노드 ID
|
||||
/// </summary>
|
||||
string TargetNodeId { get; }
|
||||
string PrevNodeId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 도킹 방향
|
||||
|
||||
@@ -81,6 +81,9 @@ namespace AGVNavigationCore.Models
|
||||
// 중복 연결 정리 (양방향 중복 제거)
|
||||
CleanupDuplicateConnections(result.Nodes);
|
||||
|
||||
// 양방향 연결 자동 설정 (A→B가 있으면 B→A도 설정)
|
||||
EnsureBidirectionalConnections(result.Nodes);
|
||||
|
||||
// 이미지 노드들의 이미지 로드
|
||||
LoadImageNodes(result.Nodes);
|
||||
|
||||
@@ -324,6 +327,67 @@ namespace AGVNavigationCore.Models
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 맵의 모든 연결을 양방향으로 만듭니다.
|
||||
/// A→B 연결이 있으면 B→A 연결도 자동으로 추가합니다.
|
||||
/// GetNextNodeId() 메서드에서 현재 노드의 ConnectedNodes만으로 다음 노드를 찾을 수 있도록 하기 위함.
|
||||
///
|
||||
/// 예시:
|
||||
/// - 맵 에디터에서 002→003 연결을 생성했다면
|
||||
/// - 자동으로 003→002 연결도 추가됨
|
||||
/// - 따라서 003의 ConnectedNodes에 002가 포함됨
|
||||
/// </summary>
|
||||
/// <param name="mapNodes">맵 노드 목록</param>
|
||||
private static void EnsureBidirectionalConnections(List<MapNode> mapNodes)
|
||||
{
|
||||
if (mapNodes == null || mapNodes.Count == 0) return;
|
||||
|
||||
// 모든 노드의 연결 정보를 수집
|
||||
var allConnections = new Dictionary<string, HashSet<string>>();
|
||||
|
||||
// 1단계: 모든 명시적 연결 수집
|
||||
foreach (var node in mapNodes)
|
||||
{
|
||||
if (!allConnections.ContainsKey(node.NodeId))
|
||||
{
|
||||
allConnections[node.NodeId] = new HashSet<string>();
|
||||
}
|
||||
|
||||
if (node.ConnectedNodes != null)
|
||||
{
|
||||
foreach (var connectedId in node.ConnectedNodes)
|
||||
{
|
||||
allConnections[node.NodeId].Add(connectedId);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 2단계: 역방향 연결 추가
|
||||
foreach (var node in mapNodes)
|
||||
{
|
||||
if (node.ConnectedNodes == null)
|
||||
{
|
||||
node.ConnectedNodes = new List<string>();
|
||||
}
|
||||
|
||||
// 이 노드를 연결하는 모든 노드 찾기
|
||||
foreach (var otherNodeId in allConnections.Keys)
|
||||
{
|
||||
if (otherNodeId == node.NodeId) continue;
|
||||
|
||||
// 다른 노드가 이 노드를 연결하고 있다면
|
||||
if (allConnections[otherNodeId].Contains(node.NodeId))
|
||||
{
|
||||
// 이 노드의 ConnectedNodes에 그 노드를 추가 (중복 방지)
|
||||
if (!node.ConnectedNodes.Contains(otherNodeId))
|
||||
{
|
||||
node.ConnectedNodes.Add(otherNodeId);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// MapNode 목록에서 RFID가 없는 노드들에 자동으로 RFID ID를 할당합니다.
|
||||
/// *** 에디터와 시뮬레이터 데이터 불일치 방지를 위해 비활성화됨 ***
|
||||
|
||||
@@ -2,6 +2,7 @@ using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Drawing.Drawing2D;
|
||||
using AGVNavigationCore.Utils;
|
||||
|
||||
namespace AGVNavigationCore.Models
|
||||
{
|
||||
@@ -132,10 +133,16 @@ namespace AGVNavigationCore.Models
|
||||
public bool ShowBackground { get; set; } = false;
|
||||
|
||||
/// <summary>
|
||||
/// 이미지 파일 경로 (NodeType.Image인 경우 사용)
|
||||
/// 이미지 파일 경로 (편집용, 저장시엔 사용되지 않음)
|
||||
/// </summary>
|
||||
[Newtonsoft.Json.JsonIgnore]
|
||||
public string ImagePath { get; set; } = string.Empty;
|
||||
|
||||
/// <summary>
|
||||
/// Base64 인코딩된 이미지 데이터 (JSON 저장용)
|
||||
/// </summary>
|
||||
public string ImageBase64 { get; set; } = string.Empty;
|
||||
|
||||
/// <summary>
|
||||
/// 이미지 크기 배율 (NodeType.Image인 경우 사용)
|
||||
/// </summary>
|
||||
@@ -331,6 +338,7 @@ namespace AGVNavigationCore.Models
|
||||
BackColor = BackColor,
|
||||
ShowBackground = ShowBackground,
|
||||
ImagePath = ImagePath,
|
||||
ImageBase64 = ImageBase64,
|
||||
Scale = Scale,
|
||||
Opacity = Opacity,
|
||||
Rotation = Rotation
|
||||
@@ -339,7 +347,7 @@ namespace AGVNavigationCore.Models
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 이미지 로드 (256x256 이상일 경우 자동 리사이즈)
|
||||
/// 이미지 로드 (Base64 또는 파일 경로에서, 256x256 이상일 경우 자동 리사이즈)
|
||||
/// </summary>
|
||||
/// <returns>로드 성공 여부</returns>
|
||||
public bool LoadImage()
|
||||
@@ -348,11 +356,23 @@ namespace AGVNavigationCore.Models
|
||||
|
||||
try
|
||||
{
|
||||
if (!string.IsNullOrEmpty(ImagePath) && System.IO.File.Exists(ImagePath))
|
||||
Image originalImage = null;
|
||||
|
||||
// 1. 먼저 Base64 데이터 시도
|
||||
if (!string.IsNullOrEmpty(ImageBase64))
|
||||
{
|
||||
originalImage = ImageConverterUtil.Base64ToImage(ImageBase64);
|
||||
}
|
||||
// 2. Base64가 없으면 파일 경로에서 로드
|
||||
else if (!string.IsNullOrEmpty(ImagePath) && System.IO.File.Exists(ImagePath))
|
||||
{
|
||||
originalImage = Image.FromFile(ImagePath);
|
||||
}
|
||||
|
||||
if (originalImage != null)
|
||||
{
|
||||
LoadedImage?.Dispose();
|
||||
var originalImage = Image.FromFile(ImagePath);
|
||||
|
||||
|
||||
// 이미지 크기 체크 및 리사이즈
|
||||
if (originalImage.Width > 256 || originalImage.Height > 256)
|
||||
{
|
||||
@@ -363,7 +383,7 @@ namespace AGVNavigationCore.Models
|
||||
{
|
||||
LoadedImage = originalImage;
|
||||
}
|
||||
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -419,6 +439,33 @@ namespace AGVNavigationCore.Models
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 파일 경로에서 이미지를 Base64로 변환하여 저장
|
||||
/// </summary>
|
||||
/// <param name="filePath">이미지 파일 경로</param>
|
||||
/// <returns>변환 성공 여부</returns>
|
||||
public bool ConvertImageToBase64(string filePath)
|
||||
{
|
||||
if (Type != NodeType.Image) return false;
|
||||
|
||||
try
|
||||
{
|
||||
if (!System.IO.File.Exists(filePath))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
ImageBase64 = ImageConverterUtil.FileToBase64(filePath, System.Drawing.Imaging.ImageFormat.Png);
|
||||
ImagePath = filePath; // 편집용으로 경로 유지
|
||||
|
||||
return !string.IsNullOrEmpty(ImageBase64);
|
||||
}
|
||||
catch (Exception)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 리소스 정리
|
||||
/// </summary>
|
||||
|
||||
@@ -49,11 +49,11 @@ namespace AGVNavigationCore.Models
|
||||
|
||||
private string _agvId;
|
||||
private Point _currentPosition;
|
||||
private Point _targetPosition;
|
||||
private Point _prevPosition;
|
||||
private string _targetId;
|
||||
private string _currentId;
|
||||
private AgvDirection _currentDirection;
|
||||
private AgvDirection _targetDirection;
|
||||
private AgvDirection _prevDirection;
|
||||
private AGVState _currentState;
|
||||
private float _currentSpeed;
|
||||
|
||||
@@ -62,7 +62,7 @@ namespace AGVNavigationCore.Models
|
||||
private List<string> _remainingNodes;
|
||||
private int _currentNodeIndex;
|
||||
private MapNode _currentNode;
|
||||
private MapNode _targetNode;
|
||||
private MapNode _prevNode;
|
||||
|
||||
// 이동 관련
|
||||
private DateTime _lastUpdateTime;
|
||||
@@ -82,6 +82,13 @@ namespace AGVNavigationCore.Models
|
||||
|
||||
#region Properties
|
||||
|
||||
/// <summary>
|
||||
/// 대상 이동시 모터 방향
|
||||
/// </summary>
|
||||
public AgvDirection PrevDirection => _prevDirection;
|
||||
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// AGV ID
|
||||
/// </summary>
|
||||
@@ -131,9 +138,9 @@ namespace AGVNavigationCore.Models
|
||||
public string CurrentNodeId => _currentNode?.NodeId;
|
||||
|
||||
/// <summary>
|
||||
/// 목표 위치
|
||||
/// 이전 위치
|
||||
/// </summary>
|
||||
public Point? TargetPosition => _targetPosition;
|
||||
public Point? PrevPosition => _prevPosition;
|
||||
|
||||
/// <summary>
|
||||
/// 배터리 레벨 (시뮬레이션)
|
||||
@@ -141,9 +148,14 @@ namespace AGVNavigationCore.Models
|
||||
public float BatteryLevel { get; set; } = 100.0f;
|
||||
|
||||
/// <summary>
|
||||
/// 목표 노드 ID
|
||||
/// 이전 노드 ID
|
||||
/// </summary>
|
||||
public string TargetNodeId => _targetNode?.NodeId;
|
||||
public string PrevNodeId => _prevNode?.NodeId;
|
||||
|
||||
/// <summary>
|
||||
/// 이전 노드
|
||||
/// </summary>
|
||||
public MapNode PrevNode => _prevNode;
|
||||
|
||||
/// <summary>
|
||||
/// 도킹 방향
|
||||
@@ -169,7 +181,7 @@ namespace AGVNavigationCore.Models
|
||||
_currentSpeed = 0;
|
||||
_dockingDirection = DockingDirection.Forward; // 기본값: 전진 도킹
|
||||
_currentNode = null;
|
||||
_targetNode = null;
|
||||
_prevNode = null;
|
||||
_isMoving = false;
|
||||
_lastUpdateTime = DateTime.Now;
|
||||
}
|
||||
@@ -284,7 +296,7 @@ namespace AGVNavigationCore.Models
|
||||
if (targetNode != null)
|
||||
{
|
||||
_dockingDirection = GetDockingDirection(targetNode.Type);
|
||||
_targetNode = targetNode;
|
||||
_prevNode = targetNode;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -356,7 +368,7 @@ namespace AGVNavigationCore.Models
|
||||
/// <param name="targetPosition">목표 위치</param>
|
||||
public void MoveTo(Point targetPosition)
|
||||
{
|
||||
_targetPosition = targetPosition;
|
||||
_prevPosition = targetPosition;
|
||||
_moveStartPosition = _currentPosition;
|
||||
_moveTargetPosition = targetPosition;
|
||||
_moveProgress = 0;
|
||||
@@ -407,23 +419,28 @@ namespace AGVNavigationCore.Models
|
||||
|
||||
/// <summary>
|
||||
/// AGV 위치 직접 설정
|
||||
/// TargetPosition을 이전 위치로 저장하여 리프트 방향 계산이 가능하도록 함
|
||||
/// PrevPosition을 이전 위치로 저장하여 리프트 방향 계산이 가능하도록 함
|
||||
/// </summary>
|
||||
/// <param name="node">현재 노드</param>
|
||||
/// <param name="newPosition">새로운 위치</param>
|
||||
/// <param name="motorDirection">모터이동방향</param>
|
||||
public void SetPosition(MapNode node, Point newPosition, AgvDirection motorDirection)
|
||||
public void SetPosition(MapNode node, AgvDirection motorDirection)
|
||||
{
|
||||
// 현재 위치를 이전 위치로 저장 (리프트 방향 계산용)
|
||||
if (_currentPosition != Point.Empty)
|
||||
if (_currentNode != null && _currentNode.NodeId != node.NodeId)
|
||||
{
|
||||
_targetPosition = _currentPosition; // 이전 위치
|
||||
_targetDirection = _currentDirection;
|
||||
_targetNode = node;
|
||||
_prevPosition = _currentPosition; // 이전 위치
|
||||
_prevNode = _currentNode;
|
||||
}
|
||||
|
||||
//모터방향이 다르다면 적용한다
|
||||
if (_currentDirection != motorDirection)
|
||||
{
|
||||
_prevDirection = _currentDirection;
|
||||
}
|
||||
|
||||
// 새로운 위치 설정
|
||||
_currentPosition = newPosition;
|
||||
_currentPosition = node.Position;
|
||||
_currentDirection = motorDirection;
|
||||
_currentNode = node;
|
||||
|
||||
@@ -598,6 +615,254 @@ namespace AGVNavigationCore.Models
|
||||
|
||||
#endregion
|
||||
|
||||
#region Directional Navigation
|
||||
|
||||
/// <summary>
|
||||
/// 현재 이전/현재 위치와 이동 방향을 기반으로 다음 노드 ID를 반환
|
||||
///
|
||||
/// 사용 예시:
|
||||
/// - 001에서 002로 이동 후, Forward 선택 → 003 반환
|
||||
/// - 003에서 004로 이동 후, Right 선택 → 030 반환
|
||||
/// - 004에서 003으로 Backward 이동 중, GetNextNodeId(Backward) → 002 반환
|
||||
///
|
||||
/// 전제조건: SetPosition이 최소 2번 이상 호출되어 _prevPosition이 설정되어야 함
|
||||
/// </summary>
|
||||
/// <param name="direction">이동 방향 (Forward/Backward/Left/Right)</param>
|
||||
/// <param name="allNodes">맵의 모든 노드</param>
|
||||
/// <returns>다음 노드 ID (또는 null)</returns>
|
||||
public MapNode GetNextNodeId(AgvDirection direction, List<MapNode> allNodes)
|
||||
{
|
||||
// 전제조건 검증: 2개 위치 히스토리 필요
|
||||
if ( _prevNode == null)
|
||||
{
|
||||
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(
|
||||
_currentPosition.X - _prevPosition.X,
|
||||
_currentPosition.Y - _prevPosition.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
|
||||
);
|
||||
|
||||
// 각 후보 노드에 대해 방향 점수 계산
|
||||
var bestCandidate = (node: (MapNode)null, score: 0.0f);
|
||||
|
||||
foreach (var candidate in candidateNodes)
|
||||
{
|
||||
var toNextVector = new PointF(
|
||||
candidate.Position.X - _currentPosition.X,
|
||||
candidate.Position.Y - _currentPosition.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
|
||||
);
|
||||
|
||||
if (score > bestCandidate.score)
|
||||
{
|
||||
bestCandidate = (candidate, score);
|
||||
}
|
||||
}
|
||||
|
||||
return bestCandidate.node;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 이동 방향을 기반으로 방향 점수를 계산
|
||||
/// 높은 점수 = 더 나은 선택지
|
||||
/// </summary>
|
||||
private float CalculateDirectionalScore(
|
||||
PointF movementDirection, // 정규화된 이전→현재 벡터
|
||||
PointF nextDirection, // 정규화된 현재→다음 벡터
|
||||
AgvDirection requestedDir) // 요청된 이동 방향
|
||||
{
|
||||
// 벡터 간 내적 계산 (유사도: -1 ~ 1)
|
||||
float dotProduct = (movementDirection.X * nextDirection.X) +
|
||||
(movementDirection.Y * nextDirection.Y);
|
||||
|
||||
// 벡터 간 외적 계산 (좌우 판별: Z 성분)
|
||||
// 양수 = 좌측, 음수 = 우측
|
||||
float crossProduct = (movementDirection.X * nextDirection.Y) -
|
||||
(movementDirection.Y * nextDirection.X);
|
||||
|
||||
float baseScore = 0;
|
||||
|
||||
switch (requestedDir)
|
||||
{
|
||||
case AgvDirection.Forward:
|
||||
// Forward: 현재 모터 상태에 따라 다름
|
||||
// 1) 현재 Forward 모터 상태라면 → 같은 경로 (계속 전진)
|
||||
// 2) 현재 Backward 모터 상태라면 → 반대 경로 (모터 방향 전환)
|
||||
if (_currentDirection == AgvDirection.Forward)
|
||||
{
|
||||
// 이미 Forward 상태, 계속 Forward → 같은 경로
|
||||
if (dotProduct > 0.9f)
|
||||
baseScore = 100.0f;
|
||||
else if (dotProduct > 0.5f)
|
||||
baseScore = 80.0f;
|
||||
else if (dotProduct > 0.0f)
|
||||
baseScore = 50.0f;
|
||||
else if (dotProduct > -0.5f)
|
||||
baseScore = 20.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Backward 상태에서 Forward로 → 반대 경로
|
||||
if (dotProduct < -0.9f)
|
||||
baseScore = 100.0f;
|
||||
else if (dotProduct < -0.5f)
|
||||
baseScore = 80.0f;
|
||||
else if (dotProduct < 0.0f)
|
||||
baseScore = 50.0f;
|
||||
else if (dotProduct < 0.5f)
|
||||
baseScore = 20.0f;
|
||||
}
|
||||
break;
|
||||
|
||||
case AgvDirection.Backward:
|
||||
// Backward: 현재 모터 상태에 따라 다름
|
||||
// 1) 현재 Backward 모터 상태라면 → 같은 경로 (Forward와 동일)
|
||||
// 2) 현재 Forward 모터 상태라면 → 반대 경로 (현재의 Backward와 반대)
|
||||
if (_currentDirection == AgvDirection.Backward)
|
||||
{
|
||||
// 이미 Backward 상태, 계속 Backward → 같은 경로
|
||||
if (dotProduct > 0.9f)
|
||||
baseScore = 100.0f;
|
||||
else if (dotProduct > 0.5f)
|
||||
baseScore = 80.0f;
|
||||
else if (dotProduct > 0.0f)
|
||||
baseScore = 50.0f;
|
||||
else if (dotProduct > -0.5f)
|
||||
baseScore = 20.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Forward 상태에서 Backward로 → 반대 경로
|
||||
if (dotProduct < -0.9f)
|
||||
baseScore = 100.0f;
|
||||
else if (dotProduct < -0.5f)
|
||||
baseScore = 80.0f;
|
||||
else if (dotProduct < 0.0f)
|
||||
baseScore = 50.0f;
|
||||
else if (dotProduct < 0.5f)
|
||||
baseScore = 20.0f;
|
||||
}
|
||||
break;
|
||||
|
||||
case AgvDirection.Left:
|
||||
// Left: 좌측 방향 선호
|
||||
if (dotProduct > 0.0f) // Forward 상태
|
||||
{
|
||||
if (crossProduct > 0.5f)
|
||||
baseScore = 100.0f;
|
||||
else if (crossProduct > 0.0f)
|
||||
baseScore = 70.0f;
|
||||
else if (crossProduct > -0.5f)
|
||||
baseScore = 50.0f;
|
||||
else
|
||||
baseScore = 30.0f;
|
||||
}
|
||||
else // Backward 상태 - 좌우 반전
|
||||
{
|
||||
if (crossProduct < -0.5f)
|
||||
baseScore = 100.0f;
|
||||
else if (crossProduct < 0.0f)
|
||||
baseScore = 70.0f;
|
||||
else if (crossProduct < 0.5f)
|
||||
baseScore = 50.0f;
|
||||
else
|
||||
baseScore = 30.0f;
|
||||
}
|
||||
break;
|
||||
|
||||
case AgvDirection.Right:
|
||||
// Right: 우측 방향 선호
|
||||
if (dotProduct > 0.0f) // Forward 상태
|
||||
{
|
||||
if (crossProduct < -0.5f)
|
||||
baseScore = 100.0f;
|
||||
else if (crossProduct < 0.0f)
|
||||
baseScore = 70.0f;
|
||||
else if (crossProduct < 0.5f)
|
||||
baseScore = 50.0f;
|
||||
else
|
||||
baseScore = 30.0f;
|
||||
}
|
||||
else // Backward 상태 - 좌우 반전
|
||||
{
|
||||
if (crossProduct > 0.5f)
|
||||
baseScore = 100.0f;
|
||||
else if (crossProduct > 0.0f)
|
||||
baseScore = 70.0f;
|
||||
else if (crossProduct > -0.5f)
|
||||
baseScore = 50.0f;
|
||||
else
|
||||
baseScore = 30.0f;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
return baseScore;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Cleanup
|
||||
|
||||
/// <summary>
|
||||
|
||||
Reference in New Issue
Block a user