HungDK
408 lines
13 KiB
C#
408 lines
13 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using UnityEngine;
|
|
|
|
// Filename : CMoveAgent.cs
|
|
// Creator : ported from C++ (AutoPFImp/AutoMoveImp/MoveAgent.*)
|
|
// Date : 2026/01/09
|
|
|
|
namespace AutoMove
|
|
{
|
|
/// <summary>
|
|
/// MoveAgent: A* on layer0 rmap + COptimizePath (matches C++ CMoveAgent flow).
|
|
/// </summary>
|
|
public class CMoveAgent
|
|
{
|
|
private static bool DEBUG_AUTOPF = false;
|
|
|
|
public abstract class BrushTest
|
|
{
|
|
public abstract bool Collide(Vector3 from, Vector3 to);
|
|
}
|
|
|
|
private readonly CMoveMap m_pMoveMap = new CMoveMap();
|
|
private COptimizePath m_pPathOptimizer;
|
|
|
|
private Vector2Int m_ptStart;
|
|
private Vector2Int m_ptGoal;
|
|
private int m_iLayerStart;
|
|
private int m_iLayerGoal;
|
|
|
|
public bool Load(string basePathNoExt, Func<string, byte[]> resolver, Vector3? originOverride)
|
|
{
|
|
string cfgName = basePathNoExt;
|
|
if (!cfgName.EndsWith(".cfg", StringComparison.OrdinalIgnoreCase))
|
|
cfgName += ".cfg";
|
|
|
|
byte[] cfgBytes = resolver(cfgName);
|
|
if (cfgBytes == null)
|
|
return false;
|
|
|
|
bool ok = m_pMoveMap.Load(cfgBytes, resolver, basePathNoExt);
|
|
if (!ok)
|
|
return false;
|
|
|
|
if (originOverride.HasValue)
|
|
m_pMoveMap.SetOrigin(originOverride.Value);
|
|
|
|
CreateOptimizer();
|
|
return true;
|
|
}
|
|
|
|
void CreateOptimizer()
|
|
{
|
|
m_pPathOptimizer = new COptimizePath();
|
|
}
|
|
|
|
public bool IsReady()
|
|
{
|
|
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)
|
|
{
|
|
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 List<PathNode> Get2DPath()
|
|
{
|
|
return m_pPathOptimizer?.GetPath();
|
|
}
|
|
|
|
public int GetPathCount()
|
|
{
|
|
if (m_pPathOptimizer != null)
|
|
{
|
|
var path = m_pPathOptimizer.GetPath();
|
|
if (path != null)
|
|
return path.Count;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
public int GetOptimizeCatchCount()
|
|
{
|
|
return m_pPathOptimizer != null ? m_pPathOptimizer.GetCatchCount() : 0;
|
|
}
|
|
|
|
public Vector3 Get2DPathNode(int index)
|
|
{
|
|
var path2d = Get2DPath();
|
|
if (path2d == null || index < 0 || index >= path2d.Count)
|
|
return Vector3.zero;
|
|
PathNode pathNode = path2d[index];
|
|
Vector2 ptWld = m_pMoveMap.TransMap2Wld(pathNode.ptMap.x, pathNode.ptMap.y);
|
|
return new Vector3(ptWld.x, 0f, ptWld.y);
|
|
}
|
|
|
|
public Vector3 Get3DPathNode(int index)
|
|
{
|
|
var path2d = Get2DPath();
|
|
if (path2d == null || index < 0 || index >= path2d.Count)
|
|
return Vector3.zero;
|
|
PathNode pathNode = path2d[index];
|
|
Vector2 ptWld = m_pMoveMap.TransMap2Wld(pathNode.ptMap.x, pathNode.ptMap.y);
|
|
float dh = m_pMoveMap.GetDH(pathNode.layer, (int)pathNode.ptMap.x, (int)pathNode.ptMap.y);
|
|
return new Vector3(ptWld.x, dh, ptWld.y);
|
|
}
|
|
|
|
/// <summary>Ported from C++ CMoveAgent::Optimize.</summary>
|
|
public bool Optimize(int moveIndex)
|
|
{
|
|
if (m_pPathOptimizer == null)
|
|
return false;
|
|
if (!m_pPathOptimizer.NeedOptimize(moveIndex))
|
|
return false;
|
|
m_pPathOptimizer.StepOptimize();
|
|
return true;
|
|
}
|
|
|
|
public List<Vector3> GetFullPath()
|
|
{
|
|
var path = new List<Vector3>();
|
|
var path2d = Get2DPath();
|
|
if (path2d == null)
|
|
return path;
|
|
for (int i = 0; i < path2d.Count; i++)
|
|
path.Add(Get2DPathNode(i));
|
|
return path;
|
|
}
|
|
|
|
public bool Search(int nMaxExpand = -1)
|
|
{
|
|
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 (!rmap.GetPixel(m_ptStart.x, m_ptStart.y))
|
|
{
|
|
if (!TryFindNearestPassable(rmap, m_ptStart, w, h, 64, out var newStart))
|
|
return false;
|
|
m_ptStart = newStart;
|
|
}
|
|
if (!rmap.GetPixel(m_ptGoal.x, m_ptGoal.y))
|
|
{
|
|
if (!TryFindNearestPassable(rmap, m_ptGoal, w, h, 64, out var newGoal))
|
|
return false;
|
|
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;
|
|
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;
|
|
|
|
if (origin.x >= 0 && origin.x < w && origin.y >= 0 && origin.y < h && rmap.GetPixel(origin.x, origin.y))
|
|
{
|
|
best = origin;
|
|
return true;
|
|
}
|
|
|
|
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);
|
|
|
|
for (int x = minX; x <= maxX; x++)
|
|
{
|
|
TryConsider(x, minY);
|
|
TryConsider(x, maxY);
|
|
}
|
|
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)
|
|
{
|
|
var rev = new List<Vector2Int>(1024) { cur };
|
|
while (cameFrom.TryGetValue(cur, out var prev))
|
|
{
|
|
cur = prev;
|
|
rev.Add(cur);
|
|
}
|
|
rev.Reverse();
|
|
|
|
var initPath = new List<PathNode>(rev.Count);
|
|
for (int i = 0; i < rev.Count; i++)
|
|
{
|
|
initPath.Add(new PathNode
|
|
{
|
|
ptMap = new Vector2(rev[i].x, rev[i].y),
|
|
layer = m_iLayerStart,
|
|
});
|
|
}
|
|
|
|
if (m_pPathOptimizer == null)
|
|
CreateOptimizer();
|
|
|
|
// C++: m_pPathOptimizer->SetupOptimize(GetMoveMap(), path);
|
|
m_pPathOptimizer.SetupOptimize(m_pMoveMap, initPath);
|
|
}
|
|
|
|
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)
|
|
{
|
|
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)
|
|
{
|
|
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()
|
|
{
|
|
m_ptStart = Vector2Int.zero;
|
|
m_iLayerStart = -1;
|
|
m_ptGoal = Vector2Int.zero;
|
|
m_iLayerGoal = -1;
|
|
m_pPathOptimizer?.Reset();
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|