HungDK
564 lines
19 KiB
C#
564 lines
19 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);
|
||
private readonly List<Vector2Int> m_pathMap = new List<Vector2Int>(1024);
|
||
private int m_optimizeCurIndex;
|
||
private const int DefaultOptimizeCatchCount = 10;
|
||
|
||
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 (COptimizePath::m_CatchCount, default 10).
|
||
/// </summary>
|
||
public int GetOptimizeCatchCount() => DefaultOptimizeCatchCount;
|
||
|
||
public Vector3 Get2DPathNode(int index)
|
||
{
|
||
if (index < 0 || index >= m_path3D.Count) return Vector3.zero;
|
||
Vector3 v = m_path3D[index];
|
||
return new Vector3(v.x, 0f, v.z);
|
||
}
|
||
|
||
public Vector3 Get3DPathNode(int index)
|
||
{
|
||
if (index < 0 || index >= m_path3D.Count) return Vector3.zero;
|
||
return m_path3D[index];
|
||
}
|
||
|
||
/// <summary>Ported from C++ CMoveAgent::Optimize — incremental path smoothing while walking.</summary>
|
||
public bool Optimize(int moveIndex)
|
||
{
|
||
if (moveIndex < 0 || moveIndex >= m_pathMap.Count - 1)
|
||
return false;
|
||
|
||
var layer0 = m_pMoveMap.GetLayer(0);
|
||
var rmap = layer0?.GetRMap();
|
||
if (rmap == null)
|
||
return false;
|
||
|
||
int from = Mathf.Max(moveIndex, m_optimizeCurIndex);
|
||
if (from >= m_pathMap.Count - 1)
|
||
return false;
|
||
|
||
int best = from + 1;
|
||
for (int j = m_pathMap.Count - 1; j > from; j--)
|
||
{
|
||
if (IsLinePassable(rmap, m_pathMap[from], m_pathMap[j]))
|
||
{
|
||
best = j;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (best <= from + 1)
|
||
return false;
|
||
|
||
m_pathMap.RemoveRange(from + 1, best - from - 1);
|
||
m_path3D.RemoveRange(from + 1, best - from - 1);
|
||
m_optimizeCurIndex = from + 1;
|
||
return true;
|
||
}
|
||
|
||
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();
|
||
var layer0 = m_pMoveMap.GetLayer(0);
|
||
var rmap = layer0?.GetRMap();
|
||
List<Vector2Int> smoothed = rmap != null ? SmoothPathMap(rev, rmap) : rev;
|
||
|
||
m_pathMap.Clear();
|
||
m_path3D.Clear();
|
||
m_optimizeCurIndex = 0;
|
||
for (int i = 0; i < smoothed.Count; i++)
|
||
{
|
||
m_pathMap.Add(smoothed[i]);
|
||
m_path3D.Add(Map2Wld(smoothed[i]));
|
||
}
|
||
}
|
||
|
||
/// <summary>String-pull on passable grid (same goal as C++ COptimizePath::SetupOptimize).</summary>
|
||
static List<Vector2Int> SmoothPathMap(List<Vector2Int> raw, CBitImage rmap)
|
||
{
|
||
if (raw == null || raw.Count <= 2)
|
||
return raw;
|
||
|
||
var result = new List<Vector2Int>(raw.Count) { raw[0] };
|
||
int i = 0;
|
||
while (i < raw.Count - 1)
|
||
{
|
||
int furthest = i + 1;
|
||
for (int j = raw.Count - 1; j > i; j--)
|
||
{
|
||
if (IsLinePassable(rmap, raw[i], raw[j]))
|
||
{
|
||
furthest = j;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (furthest != i)
|
||
{
|
||
if (result[result.Count - 1] != raw[furthest])
|
||
result.Add(raw[furthest]);
|
||
i = furthest;
|
||
}
|
||
else
|
||
{
|
||
i++;
|
||
if (result[result.Count - 1] != raw[i])
|
||
result.Add(raw[i]);
|
||
}
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
/// <summary>Matches C++ COptimizePath::_LineTo strict diagonal checks on rmap.</summary>
|
||
static bool IsLinePassable(CBitImage rmap, Vector2Int from, Vector2Int to)
|
||
{
|
||
if (from == to)
|
||
return true;
|
||
|
||
rmap.GetImageSize(out int w, out int h);
|
||
int x0 = from.x, y0 = from.y;
|
||
int x1 = to.x, y1 = to.y;
|
||
int dx = Mathf.Abs(x1 - x0);
|
||
int dy = Mathf.Abs(y1 - y0);
|
||
int sx = x0 < x1 ? 1 : -1;
|
||
int sy = y0 < y1 ? 1 : -1;
|
||
int err = dx - dy;
|
||
int lastX = x0, lastY = y0;
|
||
|
||
while (true)
|
||
{
|
||
if (x0 < 0 || x0 >= w || y0 < 0 || y0 >= h || !rmap.GetPixel(x0, y0))
|
||
return false;
|
||
|
||
if (x0 == x1 && y0 == y1)
|
||
break;
|
||
|
||
int e2 = err << 1;
|
||
int nextX = x0, nextY = y0;
|
||
if (e2 > -dy)
|
||
{
|
||
err -= dy;
|
||
nextX += sx;
|
||
}
|
||
if (e2 < dx)
|
||
{
|
||
err += dx;
|
||
nextY += sy;
|
||
}
|
||
|
||
if (nextX != lastX && nextY != lastY)
|
||
{
|
||
if (!rmap.GetPixel(lastX, nextY) || !rmap.GetPixel(nextX, lastY))
|
||
return false;
|
||
}
|
||
|
||
x0 = nextX;
|
||
y0 = nextY;
|
||
lastX = x0;
|
||
lastY = y0;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
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();
|
||
m_pathMap.Clear();
|
||
m_optimizeCurIndex = 0;
|
||
}
|
||
|
||
/// <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;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|