ENIG/Cs_HMI/AGVLogic/IMPLEMENTATION_DirectionalPathfinder.md

방향 기반 경로 탐색 (DirectionalPathfinder) 구현 문서

📋 개요

이전 위치 + 현재 위치 + 진행 방향을 기반으로 다음 노드 ID를 반환하는 시스템 구현

핵심 요구사항

• ✅ VirtualAGV에 최소 2개 위치 히스토리 필요 (prev/current)
• ✅ 방향별 가중치 시스템 (Forward/Backward/Left/Right)
• ✅ Backward 시 좌/우 방향 반전 처리
• ✅ NewMap.agvmap 파일 기반 동작

---

🏗️ 구현 아키텍처

클래스 다이어그램

┌─────────────────────────────────────────┐
│   AGVDirectionCalculator                │
│   (메인 인터페이스)                      │
│                                          │
│ GetNextNodeId(                          │
│   prevPos, currentNode, currentPos,     │
│   direction, allNodes                   │
│ )                                        │
└──────────────┬──────────────────────────┘
               │ uses
               ▼
┌─────────────────────────────────────────┐
│   DirectionalPathfinder                 │
│   (핵심 알고리즘)                        │
│                                          │
│ - DirectionWeights 설정                 │
│ - 벡터 기반 방향 계산                    │
│ - 방향별 점수 계산                       │
└─────────────────────────────────────────┘

┌─────────────────────────────────────────┐
│   DirectionalPathfinderTest             │
│   (NewMap.agvmap 기반 테스트)           │
│                                          │
│ - 맵 파일 로드                           │
│ - 테스트 시나리오 실행                   │
│ - 결과 검증                              │
└─────────────────────────────────────────┘

---

🔧 구현 상세

1. DirectionalPathfinder.cs (PathFinding/Planning/)

목적: 벡터 기반 방향 계산 엔진

핵심 메서드: GetNextNodeId()

public string GetNextNodeId(
    Point previousPos,              // 이전 RFID 감지 위치
    MapNode currentNode,            // 현재 RFID 노드
    Point currentPos,               // 현재 위치
    AgvDirection direction,         // 요청된 이동 방향
    List<MapNode> allNodes          // 맵의 모든 노드
)

실행 순서

1️⃣ 입력 검증

if (previousPos == Point.Empty || currentPos == Point.Empty)
    return null;  // 2개 위치 히스토리 필수

2️⃣ 연결된 노드 필터링

var candidateNodes = allNodes.Where(n =>
    currentNode.ConnectedNodes.Contains(n.NodeId)
).ToList();

3️⃣ 이동 벡터 계산

var movementVector = new PointF(
    currentPos.X - previousPos.X,    // Δx
    currentPos.Y - previousPos.Y     // Δy
);

4️⃣ 벡터 정규화 (길이 1로 만듦)

float length = √(Δx² + Δy²);
normalizedMovement = (Δx/length, Δy/length);

5️⃣ 각 후보 노드에 대해 방향 점수 계산

for each candidate in candidateNodes:
    score = CalculateDirectionalScore(
        이동방향,
        현재→다음 벡터,
        요청된 방향
    )

6️⃣ 가장 높은 점수 선택

return scoredCandidates
    .OrderByDescending(x => x.score)
    .First()
    .node.NodeId;

방향 점수 계산 로직 (CalculateDirectionalScore)

사용하는 벡터 연산:

• 내적 (Dot Product): 두 벡터의 유사도 (-1 ~ 1)
• 외적 (Cross Product): 좌우 판별 (양수 = 좌, 음수 = 우)

내적 = v1.x * v2.x + v1.y * v2.y
외적 = v1.x * v2.y - v1.y * v2.x

🔄 Forward (전진) 모드

직진(dotProduct ≈ 1)     → 점수 100 * 1.0
비슷한 방향(0.5~0.9)    → 점수 80 * 1.0
약간 다른(0~0.5)        → 점수 50 * 1.0
거의 반대(-0.5~0)       → 점수 20 * 2.0 (후진 가중치)
완전 반대(< -0.5)       → 점수 0

↩️ Backward (후진) 모드

반대 방향(dotProduct < -0.9) → 점수 100 * 2.0
비슷하게 반대(-0.5~-0.9)    → 점수 80 * 2.0
약간 다른(-0~0.5)           → 점수 50 * 2.0
거의 같은(0~0.5)            → 점수 20 * 1.0
완전 같은(> 0.5)            → 점수 0

⬅️ Left (좌측) 모드

Forward 상태 (dotProduct > 0):

좌측(crossProduct > 0.5)     → 점수 100 * 1.5
약간 좌측(0~0.5)            → 점수 70 * 1.5
직진(-0.5~0)                → 점수 50 * 1.0
우측 방향(-0.5~-1)          → 점수 30 * 1.5

Backward 상태 (dotProduct < 0) - 좌우 반전:

좌측(crossProduct < -0.5)    → 점수 100 * 1.5
약간 좌측(-0.5~0)           → 점수 70 * 1.5
역진(0~0.5)                 → 점수 50 * 2.0
우측 방향(> 0.5)            → 점수 30 * 1.5

➡️ Right (우측) 모드

Forward 상태 (dotProduct > 0):

우측(crossProduct < -0.5)    → 점수 100 * 1.5
약간 우측(-0.5~0)           → 점수 70 * 1.5
직진(0~0.5)                 → 점수 50 * 1.0
좌측 방향(> 0.5)            → 점수 30 * 1.5

Backward 상태 (dotProduct < 0) - 좌우 반전:

우측(crossProduct > 0.5)     → 점수 100 * 1.5
약간 우측(0~0.5)            → 점수 70 * 1.5
역진(-0.5~0)                → 점수 50 * 2.0
좌측 방향(< -0.5)           → 점수 30 * 1.5

방향 가중치 (DirectionWeights)

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;  // 후진 (거리 페널티)
}

---

2. AGVDirectionCalculator.cs (Utils/)

목적: VirtualAGV 또는 실제 AGV와의 통합 인터페이스

public class AGVDirectionCalculator
{
    public string GetNextNodeId(
        Point previousRfidPos,    // 이전 RFID 위치
        MapNode currentNode,      // 현재 노드
        Point currentRfidPos,     // 현재 RFID 위치
        AgvDirection direction,   // 이동 방향
        List<MapNode> allNodes    // 모든 노드
    )
    {
        return _pathfinder.GetNextNodeId(
            previousRfidPos,
            currentNode,
            currentRfidPos,
            direction,
            allNodes
        );
    }

    // 추가: 선택된 방향 역추적
    public AgvDirection AnalyzeSelectedDirection(
        Point previousPos,
        Point currentPos,
        MapNode selectedNextNode,
        List<MapNode> connectedNodes
    )
    {
        // 벡터 비교로 실제 선택된 방향 분석
    }
}

---

3. DirectionalPathfinderTest.cs (Utils/)

목적: NewMap.agvmap 파일 기반 테스트

기능

public class DirectionalPathfinderTest
{
    // 맵 파일 로드
    public bool LoadMapFile(string filePath)

    // 테스트 실행
    public void TestDirectionalMovement(
        string previousRfidId,
        string currentRfidId,
        AgvDirection direction
    )

    // 정보 출력
    public void PrintAllNodes()
    public void PrintNodeInfo(string rfidId)
}

---

📊 테스트 시나리오

테스트 케이스 1: 직선 경로 전진

001 → 002 → Forward
예상: 003
이유: 직진 이동 (직진 가중치 1.0)

테스트 케이스 2: 역진

002 → 001 → Backward
예상: 이전 노드 또는 null (001이 002의 유일한 연결)
이유: 역진 방향 (후진 가중치 2.0)

테스트 케이스 3: 좌회전

002 → 003 → Forward
예상: 004
이유: 직진 계속

테스트 케이스 4: 분기점에서 우회전

003 → 004 → Right
예상: 030 (또는 N022)
이유: 우측 방향 가중치 1.5

테스트 케이스 5: Backward 시 좌우 반전

004 → 003 → Backward → Left
예상: 002
이유: Backward 상태에서 Left = 반전되어 원래는 우측 방향
      (Backward 기준 좌측 = Forward 기준 우측)

---

🔍 벡터 연산 예시

예시 1: 001 → 002 → Forward

이동 벡터: (206-65, 244-229) = (141, 15)
정규화: (141/142, 15/142) ≈ (0.993, 0.106)

003 위치: (278, 278)
002→003 벡터: (278-206, 278-244) = (72, 34)
정규화: (72/80, 34/80) ≈ (0.9, 0.425)

내적 = 0.993*0.9 + 0.106*0.425 ≈ 0.939 (매우 유사)
→ Forward 점수: 100 * 1.0 = 100.0 ✓ 최고 점수
→ 결과: 003 반환

예시 2: 003 → 004 → Right

이동 벡터: (380-278, 340-278) = (102, 62)
정규화: (102/119, 62/119) ≈ (0.857, 0.521)

N022 위치: (?, ?)
N031 위치: (?, ?)
(실제 맵 데이터에 따라 계산)

Right 선택 → crossProduct 음수 선호
→ N031 선택 가능성 높음

---

📁 파일 구조

AGVNavigationCore/
├── PathFinding/
│   └── Planning/
│       └── DirectionalPathfinder.cs          ← 핵심 알고리즘
├── Utils/
│   ├── AGVDirectionCalculator.cs             ← 통합 인터페이스
│   ├── DirectionalPathfinderTest.cs          ← 테스트 클래스
│   └── TestRunner.cs                         ← 실행 프로그램
└── AGVNavigationCore.csproj                  ← 프로젝트 파일 (수정됨)

---

🚀 사용 방법

기본 사용

// 1. 계산기 생성
var calculator = new AGVDirectionCalculator();

// 2. 맵 노드 로드
List<MapNode> allNodes = LoadMapFromFile("NewMap.agvmap");

// 3. 다음 노드 계산
string nextNodeId = calculator.GetNextNodeId(
    previousRfidPos: new Point(65, 229),      // 001 위치
    currentNode: node002,                      // 현재 노드
    currentRfidPos: new Point(206, 244),      // 002 위치
    direction: AgvDirection.Forward,           // 전진
    allNodes: allNodes
);

Console.WriteLine($"다음 노드: {nextNodeId}"); // 003

VirtualAGV 통합

public class VirtualAGV
{
    private AGVDirectionCalculator _directionCalc;

    public void OnPositionChanged()
    {
        // SetPosition() 호출 후
        string nextNodeId = _directionCalc.GetNextNodeId(
            _targetPosition,       // 이전 위치
            _currentNode,         // 현재 노드
            _currentPosition,     // 현재 위치
            _currentDirection,    // 현재 방향
            _allNodes
        );
    }
}

테스트 실행

var tester = new DirectionalPathfinderTest();
tester.LoadMapFile(@"C:\Data\...\NewMap.agvmap");
tester.TestDirectionalMovement("001", "002", AgvDirection.Forward);

---

✅ 검증 체크리스트

• 2개 위치 히스토리 검증 로직
• Forward/Backward/Left/Right 방향 처리
• Backward 시 좌우 반전 구현
• 방향별 가중치 시스템
• 벡터 기반 방향 계산
• NewMap.agvmap 파일 로드 지원
• 테스트 프레임워크

---

🔗 관련 클래스

클래스	파일	용도
DirectionalPathfinder	PathFinding/Planning/	핵심 알고리즘
AGVDirectionCalculator	Utils/	통합 인터페이스
DirectionalPathfinderTest	Utils/	테스트
TestRunner	Utils/	실행 프로그램
MapNode	Models/	노드 데이터
AgvDirection	Models/Enums.cs	방향 열거형

---

📝 주의사항

⚠️ 2개 위치 히스토리 필수

• previousPos가 Point.Empty이면 null 반환
• VirtualAGV.SetPosition() 호출 시 이전 위치를 _targetPosition에 저장

⚠️ 벡터 정규화

• 매우 작은 이동(< 0.001)은 거리 0으로 처리

⚠️ 방향 가중치

• 기본값: Forward=1.0, Left/Right=1.5, Backward=2.0
• 프로젝트별로 조정 가능

⚠️ 점수 시스템

• 100점 = 완벽한 방향
• 0점 = 방향 불가
• 낮은 점수도 반환됨 (대안 경로)

---

🎯 향후 개선사항

1. A 알고리즘 통합*

   • 현재는 직접 연결된 노드만 고려
   • A* 알고리즘으로 확장 가능

2. 경로 캐싱

   • 자주 이동하는 경로 캐시
   • 성능 향상

3. 동적 가중치 조정

   • AGV 상태(배터리, 속도)에 따라 가중치 변경

4. 3D 좌표 지원

   • 현재 2D Point만 지원
   • 3D 좌표 추가 가능

---

작성일: 2025-10-23 상태: 구현 완료, 테스트 대기