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
367
Cs_HMI/AGVLogic/GETNEXTNODEID_LOGIC_ANALYSIS.md
Normal file
367
Cs_HMI/AGVLogic/GETNEXTNODEID_LOGIC_ANALYSIS.md
Normal file
@@ -0,0 +1,367 @@
|
||||
# GetNextNodeId() 로직 분석 및 검증
|
||||
|
||||
## 🎯 검증 대상
|
||||
|
||||
사용자 요구사항:
|
||||
```
|
||||
001 (65, 229) → 002 (206, 244) → Forward → 003 ✓
|
||||
001 (65, 229) → 002 (206, 244) → Backward → 001 ✓
|
||||
|
||||
002 (206, 244) → 003 (278, 278) → Forward → 004 ✓
|
||||
002 (206, 244) → 003 (278, 278) → Backward → 002 ✓
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 📐 벡터 계산 논리
|
||||
|
||||
### 기본 개념
|
||||
|
||||
```
|
||||
이전 위치: prevPos = (x1, y1)
|
||||
현재 위치: currentPos = (x2, y2)
|
||||
|
||||
이동 벡터: v_movement = (x2-x1, y2-y1)
|
||||
→ 이 벡터의 방향이 "AGV가 이동한 방향"
|
||||
|
||||
다음 노드 위치: nextPos = (x3, y3)
|
||||
|
||||
다음 벡터: v_next = (x3-x2, y3-y2)
|
||||
→ 이 벡터의 방향이 "다음 노드로 가는 방향"
|
||||
```
|
||||
|
||||
### 내적 (Dot Product)
|
||||
```
|
||||
dot = v_movement · v_next = v_m.x * v_n.x + v_m.y * v_n.y
|
||||
|
||||
의미:
|
||||
dot ≈ 1 : 거의 같은 방향 (0°) → Forward에 적합
|
||||
dot ≈ 0 : 직각 (90°) → Left/Right
|
||||
dot ≈ -1 : 거의 반대 방향 (180°) → Backward에 적합
|
||||
```
|
||||
|
||||
### 외적 (Cross Product)
|
||||
```
|
||||
cross = v_movement × v_next (Z 성분) = v_m.x * v_n.y - v_m.y * v_n.x
|
||||
|
||||
의미:
|
||||
cross > 0 : 반시계 방향 (좌측)
|
||||
cross < 0 : 시계 방향 (우측)
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 🧪 실제 시나리오 계산
|
||||
|
||||
### 시나리오 1: 001 → 002 → Forward → ?
|
||||
|
||||
#### 초기 조건
|
||||
```
|
||||
001: (65, 229)
|
||||
002: (206, 244)
|
||||
003: (278, 278)
|
||||
|
||||
이동 벡터: v_m = (206-65, 244-229) = (141, 15)
|
||||
정규화: n_m = (141/142.79, 15/142.79) ≈ (0.987, 0.105)
|
||||
```
|
||||
|
||||
#### 002의 ConnectedNodes: [N001, N003]
|
||||
|
||||
**후보 1: N001**
|
||||
```
|
||||
다음 벡터: v_n = (65-206, 229-244) = (-141, -15)
|
||||
정규화: n_n = (-141/142.79, -15/142.79) ≈ (-0.987, -0.105)
|
||||
|
||||
내적: dot = 0.987*(-0.987) + 0.105*(-0.105)
|
||||
= -0.974 - 0.011
|
||||
≈ -0.985 (매우 반대 방향)
|
||||
|
||||
외적: cross = 0.987*(-0.105) - 0.105*(-0.987)
|
||||
= -0.104 + 0.104
|
||||
≈ 0
|
||||
|
||||
Forward 모드에서:
|
||||
dotProduct < -0.5 → baseScore = 20.0 (낮은 점수)
|
||||
```
|
||||
|
||||
**후보 2: N003** ⭐
|
||||
```
|
||||
다음 벡터: v_n = (278-206, 278-244) = (72, 34)
|
||||
정규화: n_n = (72/79.88, 34/79.88) ≈ (0.901, 0.426)
|
||||
|
||||
내적: dot = 0.987*0.901 + 0.105*0.426
|
||||
= 0.889 + 0.045
|
||||
≈ 0.934 (거의 같은 방향)
|
||||
|
||||
외적: cross = 0.987*0.426 - 0.105*0.901
|
||||
= 0.421 - 0.095
|
||||
≈ 0.326
|
||||
|
||||
Forward 모드에서:
|
||||
dotProduct > 0.9 → baseScore = 100.0 ✓ (최고 점수!)
|
||||
```
|
||||
|
||||
#### 결과: N003 선택 ✅
|
||||
|
||||
---
|
||||
|
||||
### 시나리오 2: 001 → 002 → Backward → ?
|
||||
|
||||
#### 초기 조건
|
||||
```
|
||||
001: (65, 229)
|
||||
002: (206, 244)
|
||||
|
||||
이동 벡터: v_m = (141, 15) (같음)
|
||||
정규화: n_m = (0.987, 0.105) (같음)
|
||||
```
|
||||
|
||||
#### 002의 ConnectedNodes: [N001, N003]
|
||||
|
||||
**후보 1: N001** ⭐
|
||||
```
|
||||
다음 벡터: v_n = (-141, -15) (같음)
|
||||
정규화: n_n = (-0.987, -0.105) (같음)
|
||||
|
||||
내적: dot ≈ -0.985 (매우 반대 방향)
|
||||
|
||||
Backward 모드에서:
|
||||
dotProduct < -0.9 → baseScore = 100.0 ✓ (최고 점수!)
|
||||
```
|
||||
|
||||
**후보 2: N003**
|
||||
```
|
||||
다음 벡터: v_n = (72, 34) (같음)
|
||||
정규화: n_n = (0.901, 0.426) (같음)
|
||||
|
||||
내적: dot ≈ 0.934 (거의 같은 방향)
|
||||
|
||||
Backward 모드에서:
|
||||
dotProduct > 0.5 → baseScore = 0 (점수 없음)
|
||||
```
|
||||
|
||||
#### 결과: N001 선택 ✅
|
||||
|
||||
---
|
||||
|
||||
### 시나리오 3: 002 → 003 → Forward → ?
|
||||
|
||||
#### 초기 조건
|
||||
```
|
||||
002: (206, 244)
|
||||
003: (278, 278)
|
||||
004: (380, 340)
|
||||
|
||||
이동 벡터: v_m = (278-206, 278-244) = (72, 34)
|
||||
정규화: n_m ≈ (0.901, 0.426)
|
||||
```
|
||||
|
||||
#### 003의 ConnectedNodes: [N002, N004]
|
||||
|
||||
**후보 1: N002**
|
||||
```
|
||||
다음 벡터: v_n = (206-278, 244-278) = (-72, -34)
|
||||
정규화: n_n ≈ (-0.901, -0.426)
|
||||
|
||||
내적: dot ≈ -0.934 (거의 반대)
|
||||
|
||||
Forward 모드에서:
|
||||
dotProduct < 0 → baseScore ≤ 50.0
|
||||
```
|
||||
|
||||
**후보 2: N004** ⭐
|
||||
```
|
||||
다음 벡터: v_n = (380-278, 340-278) = (102, 62)
|
||||
정규화: n_n = (102/119.54, 62/119.54) ≈ (0.853, 0.519)
|
||||
|
||||
내적: dot = 0.901*0.853 + 0.426*0.519
|
||||
= 0.768 + 0.221
|
||||
≈ 0.989 (거의 같은 방향)
|
||||
|
||||
Forward 모드에서:
|
||||
dotProduct > 0.9 → baseScore = 100.0 ✓
|
||||
```
|
||||
|
||||
#### 결과: N004 선택 ✅
|
||||
|
||||
---
|
||||
|
||||
### 시나리오 4: 002 → 003 → Backward → ?
|
||||
|
||||
#### 초기 조건
|
||||
```
|
||||
002: (206, 244)
|
||||
003: (278, 278)
|
||||
|
||||
이동 벡터: v_m = (72, 34)
|
||||
정규화: n_m = (0.901, 0.426)
|
||||
```
|
||||
|
||||
#### 003의 ConnectedNodes: [N002, N004]
|
||||
|
||||
**후보 1: N002** ⭐
|
||||
```
|
||||
다음 벡터: v_n = (-72, -34)
|
||||
정규화: n_n = (-0.901, -0.426)
|
||||
|
||||
내적: dot ≈ -0.934 (거의 반대)
|
||||
|
||||
Backward 모드에서:
|
||||
dotProduct < -0.9 → baseScore = 100.0 ✓
|
||||
```
|
||||
|
||||
**후보 2: N004**
|
||||
```
|
||||
다음 벡터: v_n = (102, 62)
|
||||
정규화: n_n = (0.853, 0.519)
|
||||
|
||||
내적: dot ≈ 0.989 (거의 같은)
|
||||
|
||||
Backward 모드에서:
|
||||
dotProduct > 0 → baseScore = 0 (점수 없음)
|
||||
```
|
||||
|
||||
#### 결과: N002 선택 ✅
|
||||
|
||||
---
|
||||
|
||||
## ✅ 검증 결과
|
||||
|
||||
| 시나리오 | 이전→현재 | 방향 | 예상 | 계산 결과 | 검증 |
|
||||
|---------|----------|------|------|----------|------|
|
||||
| 1 | 001→002 | Forward | 003 | 003 (100.0) | ✅ |
|
||||
| 2 | 001→002 | Backward | 001 | 001 (100.0) | ✅ |
|
||||
| 3 | 002→003 | Forward | 004 | 004 (100.0) | ✅ |
|
||||
| 4 | 002→003 | Backward | 002 | 002 (100.0) | ✅ |
|
||||
|
||||
---
|
||||
|
||||
## 🔍 핵심 로직 검토
|
||||
|
||||
### VirtualAGV.GetNextNodeId() - 라인 628-719
|
||||
|
||||
```csharp
|
||||
public string GetNextNodeId(AgvDirection direction, List<MapNode> allNodes)
|
||||
{
|
||||
// 1️⃣ 히스토리 검증
|
||||
if (_prevPosition == Point.Empty || _currentPosition == Point.Empty)
|
||||
return null; // ← 2개 위치 필수
|
||||
|
||||
// 2️⃣ 연결된 노드 필터링
|
||||
var candidateNodes = allNodes.Where(n =>
|
||||
_currentNode.ConnectedNodes.Contains(n.NodeId)
|
||||
).ToList();
|
||||
|
||||
// 3️⃣ 이동 벡터 계산
|
||||
var movementVector = new PointF(
|
||||
_currentPosition.X - _prevPosition.X,
|
||||
_currentPosition.Y - _prevPosition.Y
|
||||
);
|
||||
|
||||
// 4️⃣ 정규화
|
||||
var normalizedMovement = new PointF(
|
||||
movementVector.X / movementLength,
|
||||
movementVector.Y / movementLength
|
||||
);
|
||||
|
||||
// 5️⃣ 각 후보에 대해 점수 계산
|
||||
foreach (var candidate in candidateNodes)
|
||||
{
|
||||
float score = CalculateDirectionalScore(
|
||||
normalizedMovement,
|
||||
normalizedToNext,
|
||||
direction // ← Forward/Backward/Left/Right
|
||||
);
|
||||
|
||||
if (score > bestCandidate.score)
|
||||
bestCandidate = (candidate, score);
|
||||
}
|
||||
|
||||
return bestCandidate.node?.NodeId;
|
||||
}
|
||||
```
|
||||
|
||||
### CalculateDirectionalScore() - 라인 721-821
|
||||
|
||||
```csharp
|
||||
private float CalculateDirectionalScore(
|
||||
PointF movementDirection, // 정규화된 이동 벡터
|
||||
PointF nextDirection, // 정규화된 다음 벡터
|
||||
AgvDirection requestedDir) // 요청된 방향
|
||||
{
|
||||
// 내적: 유사도 계산
|
||||
float dotProduct = (movementDirection.X * nextDirection.X) +
|
||||
(movementDirection.Y * nextDirection.Y);
|
||||
|
||||
// 외적: 좌우 판별
|
||||
float crossProduct = (movementDirection.X * nextDirection.Y) -
|
||||
(movementDirection.Y * nextDirection.X);
|
||||
|
||||
// 방향에 따라 점수 계산
|
||||
switch (requestedDir)
|
||||
{
|
||||
case AgvDirection.Forward:
|
||||
// Forward: dotProduct > 0.9 → 100점 ✓
|
||||
break;
|
||||
|
||||
case AgvDirection.Backward:
|
||||
// Backward: dotProduct < -0.9 → 100점 ✓
|
||||
break;
|
||||
|
||||
case AgvDirection.Left:
|
||||
// Left: crossProduct 양수 선호 ✓
|
||||
break;
|
||||
|
||||
case AgvDirection.Right:
|
||||
// Right: crossProduct 음수 선호 ✓
|
||||
break;
|
||||
}
|
||||
|
||||
return baseScore;
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 📊 최종 결론
|
||||
|
||||
### ✅ 로직이 정확함
|
||||
|
||||
모든 시나리오에서:
|
||||
- **Forward 이동**: 이동 벡터와 방향이 거의 같은 노드 선택 (dotProduct > 0.9)
|
||||
- **Backward 이동**: 이동 벡터와 반대 방향인 노드 선택 (dotProduct < -0.9)
|
||||
|
||||
### 🎯 동작 원리
|
||||
|
||||
1. **이동 벡터**: "AGV가 이동한 방향"을 나타냄
|
||||
2. **Forward**: 같은 방향으로 계속 진행
|
||||
3. **Backward**: 반대 방향으로 돌아감
|
||||
|
||||
```
|
||||
Forward 논리:
|
||||
001→002 이동 벡터 방향으로
|
||||
→ 002에서 Forward 선택
|
||||
→ 같은 방향인 003 선택 ✓
|
||||
|
||||
Backward 논리:
|
||||
001→002 이동 벡터 방향으로
|
||||
→ 002에서 Backward 선택
|
||||
→ 반대 방향인 001 선택 ✓
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 🧪 테스트 클래스
|
||||
|
||||
**파일**: `GetNextNodeIdTest.cs`
|
||||
|
||||
실행하면:
|
||||
1. 각 시나리오별 벡터 계산 출력
|
||||
2. 내적/외적 값 표시
|
||||
3. 후보 노드별 점수 계산
|
||||
4. 선택된 노드 및 검증 결과
|
||||
|
||||
---
|
||||
|
||||
**분석 완료**: 2025-10-23
|
||||
**상태**: 🟢 로직 정확 검증 완료
|
||||
Reference in New Issue
Block a user