HungDK
424 lines
14 KiB
C#
424 lines
14 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using UnityEngine;
|
||
|
||
// Filename : CMoveAgent.cs
|
||
// Creator : ported/simplified from C++ (AutoPFImp/AutoMoveImp/MoveAgent.*)
|
||
// Date : 2026/01/09
|
||
|
||
namespace AutoMove
|
||
{
|
||
/// <summary>
|
||
/// Minimal MoveAgent: loads movemap and provides A* path on layer0 rmap.
|
||
/// 最小 MoveAgent：加载 movemap，并在第0层 rmap 上执行 A* 路径搜索。
|
||
/// </summary>
|
||
public class CMoveAgent
|
||
{
|
||
// Debug switch (enable temporarily while validating routes).
|
||
// 调试开关（验证寻路时可临时开启）。
|
||
// NOTE: must not be const, otherwise Unity compiler warns about unreachable code.
|
||
// 注意：不要用 const，否则 Unity 编译会报“不可达代码”警告。
|
||
private static bool DEBUG_AUTOPF = false;
|
||
public abstract class BrushTest
|
||
{
|
||
// from.y/to.y store DH in original engine.
|
||
// 原版中 from.y/to.y 存储 DH（相对地形高度差）。
|
||
public abstract bool Collide(Vector3 from, Vector3 to);
|
||
}
|
||
|
||
private readonly CMoveMap m_pMoveMap = new CMoveMap();
|
||
private Vector3 m_vOriginOverride;
|
||
|
||
private Vector2Int m_ptStart;
|
||
private Vector2Int m_ptGoal;
|
||
private int m_iLayerStart;
|
||
private int m_iLayerGoal;
|
||
|
||
private readonly List<Vector3> m_path3D = new List<Vector3>(1024);
|
||
|
||
public bool Load(string basePathNoExt, Func<string, byte[]> resolver, Vector3? originOverride)
|
||
{
|
||
// basePathNoExt corresponds to "maps\\<map>\\movemap\\r.._..-c.._..-l0" in C++.
|
||
// basePathNoExt 对应 C++ 的 "maps\\<map>\\movemap\\r.._..-c.._..-l0"。
|
||
string cfgName = basePathNoExt;
|
||
if (!cfgName.EndsWith(".cfg", StringComparison.OrdinalIgnoreCase))
|
||
{
|
||
cfgName += ".cfg";
|
||
}
|
||
|
||
byte[] cfgBytes = resolver(cfgName);
|
||
if (cfgBytes == null)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
// Resolver for referenced files: in cfg, references are baseName + ".prmap" etc.
|
||
// cfg 内引用：baseName + ".prmap" 等。
|
||
bool ok = m_pMoveMap.Load(cfgBytes, resolver, basePathNoExt);
|
||
if (!ok)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (originOverride.HasValue)
|
||
{
|
||
m_vOriginOverride = originOverride.Value;
|
||
m_pMoveMap.SetOrigin(m_vOriginOverride);
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
public bool IsReady()
|
||
{
|
||
// Original C++ requires MultiCluGraph; here we only require layer0 RMap.
|
||
// 原版 C++ 依赖 MultiCluGraph；这里最小实现只要求第0层 RMap。
|
||
var layer0 = m_pMoveMap.GetLayer(0);
|
||
return layer0 != null && layer0.GetRMap() != null;
|
||
}
|
||
|
||
public bool IsContain(Vector3 vWld)
|
||
{
|
||
var p = TransWld2Map(vWld);
|
||
int w = m_pMoveMap.GetMapWidth();
|
||
int h = m_pMoveMap.GetMapLength();
|
||
return p.x >= 0 && p.x < w && p.y >= 0 && p.y < h;
|
||
}
|
||
|
||
public Vector2Int TransWld2Map(Vector3 vWld)
|
||
{
|
||
return m_pMoveMap.TransWld2Map(vWld.x, vWld.z);
|
||
}
|
||
|
||
public Vector3 Map2Wld(Vector2Int ptMap)
|
||
{
|
||
Vector2 xz = m_pMoveMap.TransMap2Wld(ptMap.x, ptMap.y);
|
||
return new Vector3(xz.x, 0.0f, xz.y);
|
||
}
|
||
|
||
public int WhichLayer(Vector3 vWld, float dH, out float layerDist)
|
||
{
|
||
// Minimal: single layer 0 if passable.
|
||
// 最小实现：如果可通行则使用单层0。
|
||
layerDist = 0.0f;
|
||
var pt = TransWld2Map(vWld);
|
||
var layer0 = m_pMoveMap.GetLayer(0);
|
||
if (layer0 == null || layer0.GetRMap() == null) return -1;
|
||
return layer0.GetRMap().GetPixel(pt.x, pt.y) ? 0 : -1;
|
||
}
|
||
|
||
public bool SetStartEnd(Vector3 vStart, int iLayerStart, Vector3 vGoal, int iLayerGoal, BrushTest brushTest = null)
|
||
{
|
||
m_ptStart = TransWld2Map(vStart);
|
||
m_ptGoal = TransWld2Map(vGoal);
|
||
m_iLayerStart = iLayerStart;
|
||
m_iLayerGoal = iLayerGoal;
|
||
return true;
|
||
}
|
||
|
||
public int GetPathCount() => m_path3D.Count;
|
||
|
||
/// <summary>
|
||
/// Matches C++ CMoveAgent::GetOptimizeCatchCount — lookahead window for path following.
|
||
/// 对应 C++：无 PathOptimizer 时为 0，FindNearest/Farthest 仅在单步索引上工作。
|
||
/// </summary>
|
||
public int GetOptimizeCatchCount() => 0;
|
||
|
||
public Vector3 Get3DPathNode(int index)
|
||
{
|
||
if (index < 0 || index >= m_path3D.Count) return Vector3.zero;
|
||
return m_path3D[index];
|
||
}
|
||
|
||
public System.Collections.Generic.List<Vector3> GetFullPath()
|
||
{
|
||
return new System.Collections.Generic.List<Vector3>(m_path3D);
|
||
}
|
||
|
||
public bool Search(int nMaxExpand = -1)
|
||
{
|
||
// A* on layer0 rmap
|
||
// 在第0层 rmap 上执行 A*
|
||
m_path3D.Clear();
|
||
|
||
var layer0 = m_pMoveMap.GetLayer(0);
|
||
var rmap = layer0?.GetRMap();
|
||
if (rmap == null) return false;
|
||
|
||
rmap.GetImageSize(out int w, out int h);
|
||
if (w <= 0 || h <= 0) return false;
|
||
|
||
if (!InBounds(m_ptStart, w, h) || !InBounds(m_ptGoal, w, h))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
// If goal or start is not passable, try to find nearest passable (original MoveAgent does this).
|
||
// 如果起点或终点不可通行，尝试寻找最近可通行点（原版 MoveAgent 会这样做）。
|
||
if (!rmap.GetPixel(m_ptStart.x, m_ptStart.y))
|
||
{
|
||
if (!TryFindNearestPassable(rmap, m_ptStart, w, h, 64, out var newStart))
|
||
{
|
||
if (DEBUG_AUTOPF) Debug.LogWarning($"[CMoveAgent] start not passable and no nearest passable found. start={m_ptStart}");
|
||
return false;
|
||
}
|
||
if (DEBUG_AUTOPF) Debug.Log($"[CMoveAgent] Adjust start {m_ptStart} -> {newStart}");
|
||
m_ptStart = newStart;
|
||
}
|
||
if (!rmap.GetPixel(m_ptGoal.x, m_ptGoal.y))
|
||
{
|
||
if (!TryFindNearestPassable(rmap, m_ptGoal, w, h, 64, out var newGoal))
|
||
{
|
||
if (DEBUG_AUTOPF) Debug.LogWarning($"[CMoveAgent] goal not passable and no nearest passable found. goal={m_ptGoal}");
|
||
return false;
|
||
}
|
||
if (DEBUG_AUTOPF) Debug.Log($"[CMoveAgent] Adjust goal {m_ptGoal} -> {newGoal}");
|
||
m_ptGoal = newGoal;
|
||
}
|
||
|
||
var cameFrom = new Dictionary<Vector2Int, Vector2Int>(4096);
|
||
var gScore = new Dictionary<Vector2Int, int>(4096);
|
||
var open = new MinHeap();
|
||
|
||
gScore[m_ptStart] = 0;
|
||
open.Push(m_ptStart, Heuristic(m_ptStart, m_ptGoal));
|
||
|
||
int expands = 0;
|
||
//ToDo: need use another method to caculate the value of maxExpand
|
||
//800000 is a magic number, need to be optimized
|
||
int maxExpand = nMaxExpand > 0 ? nMaxExpand : 800000;
|
||
|
||
while (open.Count > 0 && expands < maxExpand)
|
||
{
|
||
var cur = open.Pop();
|
||
expands++;
|
||
|
||
if (cur == m_ptGoal)
|
||
{
|
||
ReconstructPath(cameFrom, cur);
|
||
return true;
|
||
}
|
||
|
||
int curG = gScore[cur];
|
||
foreach (var nb in Neighbors8(cur))
|
||
{
|
||
if (!InBounds(nb, w, h)) continue;
|
||
if (!rmap.GetPixel(nb.x, nb.y)) continue;
|
||
|
||
int tentative = curG + Cost(cur, nb);
|
||
if (!gScore.TryGetValue(nb, out int old) || tentative < old)
|
||
{
|
||
cameFrom[nb] = cur;
|
||
gScore[nb] = tentative;
|
||
int f = tentative + Heuristic(nb, m_ptGoal);
|
||
open.Push(nb, f);
|
||
}
|
||
}
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
private static bool TryFindNearestPassable(CBitImage rmap, Vector2Int origin, int w, int h, int maxRadius, out Vector2Int best)
|
||
{
|
||
best = origin;
|
||
int bestDist2 = int.MaxValue;
|
||
int bestX = origin.x;
|
||
int bestY = origin.y;
|
||
|
||
if (rmap == null) return false;
|
||
|
||
// Check origin first
|
||
// 先检查原点
|
||
if (origin.x >= 0 && origin.x < w && origin.y >= 0 && origin.y < h && rmap.GetPixel(origin.x, origin.y))
|
||
{
|
||
best = origin;
|
||
return true;
|
||
}
|
||
|
||
// Expand square rings
|
||
// 按方形“圈”扩展搜索
|
||
for (int r = 1; r <= maxRadius; r++)
|
||
{
|
||
int minX = Math.Max(0, origin.x - r);
|
||
int maxX = Math.Min(w - 1, origin.x + r);
|
||
int minY = Math.Max(0, origin.y - r);
|
||
int maxY = Math.Min(h - 1, origin.y + r);
|
||
|
||
// Top/bottom edges
|
||
for (int x = minX; x <= maxX; x++)
|
||
{
|
||
TryConsider(x, minY);
|
||
TryConsider(x, maxY);
|
||
}
|
||
|
||
// Left/right edges (excluding corners already checked)
|
||
for (int y = minY + 1; y <= maxY - 1; y++)
|
||
{
|
||
TryConsider(minX, y);
|
||
TryConsider(maxX, y);
|
||
}
|
||
|
||
if (bestDist2 != int.MaxValue)
|
||
{
|
||
best = new Vector2Int(bestX, bestY);
|
||
return true;
|
||
}
|
||
}
|
||
|
||
return false;
|
||
|
||
void TryConsider(int x, int y)
|
||
{
|
||
if (!rmap.GetPixel(x, y)) return;
|
||
int dx = x - origin.x;
|
||
int dy = y - origin.y;
|
||
int d2 = dx * dx + dy * dy;
|
||
if (d2 < bestDist2)
|
||
{
|
||
bestDist2 = d2;
|
||
bestX = x;
|
||
bestY = y;
|
||
}
|
||
}
|
||
}
|
||
|
||
private void ReconstructPath(Dictionary<Vector2Int, Vector2Int> cameFrom, Vector2Int cur)
|
||
{
|
||
List<Vector2Int> rev = new List<Vector2Int>(1024) { cur };
|
||
while (cameFrom.TryGetValue(cur, out var prev))
|
||
{
|
||
cur = prev;
|
||
rev.Add(cur);
|
||
}
|
||
|
||
rev.Reverse();
|
||
// Convert to world positions (y will be resolved by host terrain in movement).
|
||
// 转为世界坐标（y 由移动逻辑/地形解析）。
|
||
for (int i = 0; i < rev.Count; i++)
|
||
{
|
||
m_path3D.Add(Map2Wld(rev[i]));
|
||
}
|
||
}
|
||
|
||
private static bool InBounds(Vector2Int p, int w, int h) => p.x >= 0 && p.x < w && p.y >= 0 && p.y < h;
|
||
|
||
private static int Heuristic(Vector2Int a, Vector2Int b)
|
||
{
|
||
// Octile distance * 10
|
||
// 八方向启发式（octile）*10
|
||
int dx = Mathf.Abs(a.x - b.x);
|
||
int dy = Mathf.Abs(a.y - b.y);
|
||
int min = Math.Min(dx, dy);
|
||
int max = Math.Max(dx, dy);
|
||
return 14 * min + 10 * (max - min);
|
||
}
|
||
|
||
private static int Cost(Vector2Int a, Vector2Int b)
|
||
{
|
||
// Diagonal=14, straight=10
|
||
// 斜向=14，直向=10
|
||
int dx = Mathf.Abs(a.x - b.x);
|
||
int dy = Mathf.Abs(a.y - b.y);
|
||
return (dx + dy == 2) ? 14 : 10;
|
||
}
|
||
|
||
private static IEnumerable<Vector2Int> Neighbors8(Vector2Int p)
|
||
{
|
||
yield return new Vector2Int(p.x + 1, p.y);
|
||
yield return new Vector2Int(p.x - 1, p.y);
|
||
yield return new Vector2Int(p.x, p.y + 1);
|
||
yield return new Vector2Int(p.x, p.y - 1);
|
||
yield return new Vector2Int(p.x + 1, p.y + 1);
|
||
yield return new Vector2Int(p.x + 1, p.y - 1);
|
||
yield return new Vector2Int(p.x - 1, p.y + 1);
|
||
yield return new Vector2Int(p.x - 1, p.y - 1);
|
||
}
|
||
|
||
public void ResetSearch()
|
||
{
|
||
// Çå³ýµ±Ç°ËÑË÷״̬
|
||
//if (m_iStat == PF_STATE_UNKNOWN)
|
||
//{
|
||
// return;
|
||
//}
|
||
//if (m_pPfAlg)
|
||
//{
|
||
// m_pPfAlg->Reset();
|
||
//}
|
||
//if (m_pPathOptimizer)
|
||
//{
|
||
// m_pPathOptimizer.Reset();
|
||
//}
|
||
m_ptStart.x = m_ptStart.y = 0;
|
||
m_iLayerStart = -1;
|
||
m_ptGoal.x = m_ptGoal.y = 0;
|
||
m_iLayerGoal = -1;
|
||
//m_pBrushTest = null;
|
||
|
||
//m_iStat = PF_STATE_UNKNOWN;
|
||
m_path3D.Clear();
|
||
}
|
||
|
||
/// <summary>
|
||
/// Very small min-heap for A*.
|
||
/// A* 用的小型最小堆。
|
||
/// </summary>
|
||
private sealed class MinHeap
|
||
{
|
||
private struct Node
|
||
{
|
||
public Vector2Int P;
|
||
public int F;
|
||
}
|
||
|
||
private readonly List<Node> _a = new List<Node>(4096);
|
||
public int Count => _a.Count;
|
||
|
||
public void Push(Vector2Int p, int f)
|
||
{
|
||
_a.Add(new Node { P = p, F = f });
|
||
SiftUp(_a.Count - 1);
|
||
}
|
||
|
||
public Vector2Int Pop()
|
||
{
|
||
var root = _a[0].P;
|
||
int last = _a.Count - 1;
|
||
_a[0] = _a[last];
|
||
_a.RemoveAt(last);
|
||
if (_a.Count > 0) SiftDown(0);
|
||
return root;
|
||
}
|
||
|
||
private void SiftUp(int i)
|
||
{
|
||
while (i > 0)
|
||
{
|
||
int p = (i - 1) >> 1;
|
||
if (_a[p].F <= _a[i].F) break;
|
||
(_a[p], _a[i]) = (_a[i], _a[p]);
|
||
i = p;
|
||
}
|
||
}
|
||
|
||
private void SiftDown(int i)
|
||
{
|
||
int n = _a.Count;
|
||
while (true)
|
||
{
|
||
int l = (i << 1) + 1;
|
||
if (l >= n) break;
|
||
int r = l + 1;
|
||
int m = (r < n && _a[r].F < _a[l].F) ? r : l;
|
||
if (_a[i].F <= _a[m].F) break;
|
||
(_a[i], _a[m]) = (_a[m], _a[i]);
|
||
i = m;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|