HungDK
615 lines
24 KiB
C#
615 lines
24 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.IO;
|
||
using UnityEngine;
|
||
using UnityEngine.AddressableAssets;
|
||
using AutoMove;
|
||
using BrewMonster.Network;
|
||
using CSNetwork.GPDataType;
|
||
|
||
// Filename : CECIntelligentRoute.cs
|
||
// Creator : ported (simplified) from C++ EC_IntelligentRoute.*
|
||
// Date : 2026/01/09
|
||
|
||
namespace BrewMonster.Scripts
|
||
{
|
||
/// <summary>
|
||
/// Intelligent route (AutoPF) controller.
|
||
/// 智能寻路（AutoPF）控制器。
|
||
/// </summary>
|
||
public sealed class CECIntelligentRoute
|
||
{
|
||
private const bool DEBUG_AUTOPF = true;
|
||
/// <summary>cos(5°) for FindFarthestNode — same as C++ EC_IntelligentRoute.cpp.</summary>
|
||
private static readonly float s_cos5Deg = Mathf.Cos(5f * Mathf.PI / 180f);
|
||
public enum SearchResult
|
||
{
|
||
enumSearchSuccess, // 寻路成功 // Search success
|
||
enumSearchStartEndCoincide, // 起点终点相同 // Start==End
|
||
enumSearchUnInitialized, // 未初始化 // Uninitialized
|
||
enumSearchStartInvalid, // 起点无效 // Start invalid
|
||
enumSearchEndInvalid, // 终点无效 // End invalid
|
||
enumSearchStartEndInvalid, // 起点终点都无效 // Start & End invalid
|
||
enumSearchNoPath, // 未找到路径 // No path
|
||
enumSearchExceedMaxExpand, // 超过最大扩展 // Exceed max expand
|
||
}
|
||
|
||
public enum RouteState
|
||
{
|
||
enumRouteIdle, // 未进行寻路 // Idle
|
||
enumRouteMoving, // 路径行进中 // Moving
|
||
enumRoutePathFinished, // 到达终点 // Finished
|
||
}
|
||
|
||
public enum Usage
|
||
{
|
||
enumUsageNone,
|
||
enumUsageWorkMove,
|
||
enumUsageWorkTrace,
|
||
}
|
||
|
||
private struct RangedMoveAgent
|
||
{
|
||
public RectInt rect; // map rect (in map pixels, 1024 blocks) // 地图范围（按 map 像素/块）
|
||
public Vector3 origin; // origin override // 原点覆盖
|
||
public CMoveAgent agent; // agent // 寻路器
|
||
|
||
public bool Contain(Vector3 pos)
|
||
{
|
||
return agent != null && agent.IsContain(pos);
|
||
}
|
||
}
|
||
|
||
private static readonly CECIntelligentRoute s_inst = new CECIntelligentRoute();
|
||
public static CECIntelligentRoute Instance() => s_inst;
|
||
|
||
private CECInstance m_pInst;
|
||
private readonly List<RangedMoveAgent> m_moveAgents = new List<RangedMoveAgent>(8);
|
||
|
||
private RouteState m_state = RouteState.enumRouteIdle;
|
||
private A3DVECTOR3 m_start;
|
||
private A3DVECTOR3 m_end;
|
||
private int m_iCurMoveAgent = -1;
|
||
private int m_iCurDest = -1;
|
||
private Usage m_usage = Usage.enumUsageNone;
|
||
private A3DVECTOR3 m_lastPos; // ÉÏ´ÎÒƶ¯µ½µÄλÖ㬵±ÓÐÐÂλÖÃʱ£¬ÓÃÓÚ¼ì²âÊÇ·ñÓ¦ÒÆÍùÏÂÒ»²½
|
||
private float m_lastMove; // ÉÏ´ÎÒƶ¯¾àÀ룬ÓÃÓÚ¹À¼ÆÏÂÒ»²½½«Òƶ¯µÄ¾àÀ룬´Ó¶øÅжÏÊÇ·ñÓ¦ÒÆÍùÏÂÒ»²½
|
||
private float m_dist2CurDest; // µ½´ï m_iCurDest »¹ÐèÒƶ¯µÄ¾àÀ룬ÓÃÓÚÅжÏÊÇ·ñÓ¦ÒÆÍùÏÂÒ»²½
|
||
|
||
private CECIntelligentRoute() { }
|
||
|
||
public Usage GetUsage() => m_usage;
|
||
public void SetUsage(Usage usage) => m_usage = usage;
|
||
public bool IsUsageMove() => m_usage == Usage.enumUsageWorkMove;
|
||
public bool IsUsageTrace() => m_usage == Usage.enumUsageWorkTrace;
|
||
|
||
public RouteState GetState() => m_state;
|
||
public bool IsIdle() => m_state == RouteState.enumRouteIdle;
|
||
public bool IsMoveOn() => m_state == RouteState.enumRouteMoving;
|
||
public bool IsPathFinished() => m_state == RouteState.enumRoutePathFinished;
|
||
public A3DVECTOR3 GetDest() => m_end;
|
||
|
||
public void Release()
|
||
{
|
||
m_pInst = null;
|
||
m_moveAgents.Clear();
|
||
ResetSearch();
|
||
m_usage = Usage.enumUsageNone;
|
||
}
|
||
|
||
public void ResetSearch()
|
||
{
|
||
//m_state = RouteState.enumRouteIdle;
|
||
//m_start = new A3DVECTOR3(0);
|
||
//m_end = new A3DVECTOR3(0);
|
||
//m_iCurMoveAgent = -1;
|
||
//m_iCurDest = -1;
|
||
|
||
if (IsIdle())
|
||
{
|
||
// µ±Ç°²»ÔÚËÑË÷״̬
|
||
return;
|
||
}
|
||
RangedMoveAgent? pCurAgent = GetCurAgent();
|
||
if (pCurAgent != null)
|
||
{
|
||
pCurAgent.Value.agent.ResetSearch();
|
||
}
|
||
m_state = RouteState.enumRouteIdle;
|
||
m_start.Clear();
|
||
m_end.Clear();
|
||
m_iCurMoveAgent = -1;
|
||
m_iCurDest = -1;
|
||
m_lastPos.Clear();
|
||
m_lastMove = 0.0f;
|
||
m_dist2CurDest = 0.0f;
|
||
}
|
||
|
||
public void ChangeWorldInstance(int idInstance)
|
||
{
|
||
CECInstance pInst = EC_Game.GetGameRun()?.GetInstance(idInstance);
|
||
if (pInst == null)
|
||
{
|
||
Release();
|
||
return;
|
||
}
|
||
|
||
if (ReferenceEquals(pInst, m_pInst))
|
||
{
|
||
return;
|
||
}
|
||
|
||
Release();
|
||
m_pInst = pInst;
|
||
|
||
List<string> files = pInst.GetRouteFiles();
|
||
if (files == null || files.Count == 0)
|
||
{
|
||
return;
|
||
}
|
||
|
||
// Equivalent to C++ CECIntelligentRoute::ChangeWorldInstance file parsing.
|
||
// 等价于 C++ 的 CECIntelligentRoute::ChangeWorldInstance 文件解析。
|
||
foreach (var fileBase in files)
|
||
{
|
||
// parse "r{rowFrom}_{rowTo}-c{colFrom}_{colTo}"
|
||
// 解析 "r{rowFrom}_{rowTo}-c{colFrom}_{colTo}"
|
||
int rowFrom, rowTo, colFrom, colTo;
|
||
if (!TryParseRouteFileRange(fileBase, out rowFrom, out rowTo, out colFrom, out colTo))
|
||
{
|
||
continue;
|
||
}
|
||
|
||
if (rowFrom < 0 || rowTo >= pInst.GetRowNum() || colFrom < 0 || colTo >= pInst.GetColNum())
|
||
{
|
||
continue;
|
||
}
|
||
|
||
// Build rect/origin (1024 units per tile) like original.
|
||
// 按原版构建 rect/origin（每格 1024）。
|
||
var rect = new RectInt(
|
||
colFrom * 1024,
|
||
(pInst.GetRowNum() - rowTo - 1) * 1024,
|
||
(colTo - colFrom + 1) * 1024,
|
||
(rowTo - rowFrom + 1) * 1024
|
||
);
|
||
|
||
Vector3 origin = Vector3.zero;
|
||
origin.x = rect.xMin - pInst.GetColNum() * 1024 * 0.5f;
|
||
origin.z = rect.yMin - pInst.GetRowNum() * 1024 * 0.5f;
|
||
|
||
string basePath = $"maps/{pInst.GetPath()}/movemap/{fileBase}";
|
||
var agent = new CMoveAgent();
|
||
// Create resolver that captures the basePath context for relative file references
|
||
// 创建捕获 basePath 上下文的解析器，用于相对文件引用
|
||
Func<string, byte[]> resolver = (key) => ResolveAddressableBytes(key, basePath);
|
||
if (!agent.Load(basePath, resolver, origin))
|
||
{
|
||
continue;
|
||
}
|
||
|
||
if (!agent.IsReady())
|
||
{
|
||
continue;
|
||
}
|
||
|
||
m_moveAgents.Add(new RangedMoveAgent
|
||
{
|
||
rect = rect,
|
||
origin = origin,
|
||
agent = agent,
|
||
});
|
||
}
|
||
}
|
||
|
||
private static bool TryParseRouteFileRange(string fileBase, out int rowFrom, out int rowTo, out int colFrom, out int colTo)
|
||
{
|
||
rowFrom = rowTo = colFrom = colTo = -1;
|
||
if (string.IsNullOrEmpty(fileBase)) return false;
|
||
// Expected prefix: r{a}_{b}-c{c}_{d}
|
||
// 期望前缀：r{a}_{b}-c{c}_{d}
|
||
// We ignore "-l0" suffix if present.
|
||
// 忽略可能存在的 "-l0" 后缀。
|
||
string s = fileBase;
|
||
int l0 = s.IndexOf("-l", StringComparison.OrdinalIgnoreCase);
|
||
if (l0 >= 0) s = s.Substring(0, l0);
|
||
|
||
// Example: r1_1-c1_2
|
||
// 例：r1_1-c1_2
|
||
try
|
||
{
|
||
if (!s.StartsWith("r", StringComparison.OrdinalIgnoreCase)) return false;
|
||
int dash = s.IndexOf("-c", StringComparison.OrdinalIgnoreCase);
|
||
if (dash < 0) return false;
|
||
string rPart = s.Substring(1, dash - 1);
|
||
string cPart = s.Substring(dash + 2);
|
||
|
||
string[] r = rPart.Split('_');
|
||
string[] c = cPart.Split('_');
|
||
if (r.Length != 2 || c.Length != 2) return false;
|
||
rowFrom = int.Parse(r[0]);
|
||
rowTo = int.Parse(r[1]);
|
||
colFrom = int.Parse(c[0]);
|
||
colTo = int.Parse(c[1]);
|
||
return rowFrom <= rowTo && colFrom <= colTo;
|
||
}
|
||
catch
|
||
{
|
||
return false;
|
||
}
|
||
}
|
||
|
||
private static byte[] ResolveAddressableBytes(string key, string basePath)
|
||
{
|
||
// key is like "maps/a61/movemap/r1_1-c1_2-l0.cfg" or "r1_1-c1_2-l0.prmap"
|
||
// key 类似 "maps/a61/movemap/r1_1-c1_2-l0.cfg" 或 "r1_1-c1_2-l0.prmap"
|
||
// basePath is like "maps/a61/movemap/r1_1-c1_2-l0" (without extension)
|
||
// basePath 类似 "maps/a61/movemap/r1_1-c1_2-l0"（无扩展名）
|
||
try
|
||
{
|
||
// Normalize key to use forward slashes for Addressables
|
||
// 规范化 key 以使用 Addressables 的正斜杠
|
||
string addressableKey = key.Replace('\\', '/');
|
||
|
||
// If key doesn't start with "maps/", it's a relative reference from cfg file
|
||
// 如果 key 不以 "maps/" 开头，则是 cfg 文件中的相对引用
|
||
if (!addressableKey.StartsWith("maps/", StringComparison.OrdinalIgnoreCase))
|
||
{
|
||
// Construct full path from basePath directory + key
|
||
// 从 basePath 目录 + key 构建完整路径
|
||
// basePath is "maps/a61/movemap/r1_1-c1_2-l0", so directory is "maps/a61/movemap/"
|
||
// basePath 是 "maps/a61/movemap/r1_1-c1_2-l0"，所以目录是 "maps/a61/movemap/"
|
||
int lastSlash = basePath.LastIndexOf('/');
|
||
if (lastSlash >= 0)
|
||
{
|
||
string baseDir = basePath.Substring(0, lastSlash + 1);
|
||
addressableKey = baseDir + addressableKey;
|
||
}
|
||
else
|
||
{
|
||
// Fallback: assume maps/{mapName}/movemap/ structure
|
||
// 回退：假设 maps/{mapName}/movemap/ 结构
|
||
BMLogger.LogWarning($"[CECIntelligentRoute] ResolveAddressableBytes: cannot resolve relative key '{key}' with basePath '{basePath}'");
|
||
return null;
|
||
}
|
||
}
|
||
|
||
// Append .txt extension for Addressables (all map files are renamed to .txt)
|
||
// 为 Addressables 追加 .txt 扩展名（所有地图文件已重命名为 .txt）
|
||
if (!addressableKey.EndsWith(".txt", StringComparison.OrdinalIgnoreCase))
|
||
{
|
||
addressableKey += ".txt";
|
||
}
|
||
|
||
// Build Addressables path: Assets/Addressable/{key}
|
||
// 构建 Addressables 路径：Assets/Addressable/{key}
|
||
string address = $"Assets/Addressable/{addressableKey}";
|
||
|
||
// Load synchronously (similar to navigate.txt loading pattern)
|
||
// 同步加载（类似于 navigate.txt 的加载模式）
|
||
Addressables.InitializeAsync().WaitForCompletion();
|
||
|
||
try
|
||
{
|
||
var ta = Addressables.LoadAssetAsync<TextAsset>(address).WaitForCompletion();
|
||
if (ta != null && ta.bytes != null)
|
||
{
|
||
return ta.bytes;
|
||
}
|
||
}
|
||
catch (UnityEngine.AddressableAssets.InvalidKeyException ex)
|
||
{
|
||
// If the asset exists but is registered as DefaultAsset (folder) instead of TextAsset,
|
||
// it means the files need to be properly configured in Addressables.
|
||
// 如果资产存在但注册为 DefaultAsset（文件夹）而不是 TextAsset，
|
||
// 这意味着文件需要在 Addressables 中正确配置。
|
||
BMLogger.LogError($"[CECIntelligentRoute] ResolveAddressableBytes: Asset '{address}' is registered as DefaultAsset (folder) instead of TextAsset. " +
|
||
$"Please ensure the file is properly imported as a TextAsset in Unity and marked as Addressable. " +
|
||
$"You may need to: 1) Select the file in Unity, 2) Set Import Type to 'Text' in Inspector, 3) Mark as Addressable. Error: {ex.Message}");
|
||
return null;
|
||
}
|
||
|
||
BMLogger.LogWarning($"[CECIntelligentRoute] ResolveAddressableBytes: failed to load '{address}' (asset is null or has no bytes)");
|
||
return null;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
BMLogger.LogWarning($"[CECIntelligentRoute] ResolveAddressableBytes failed for '{key}': {ex.Message}");
|
||
return null;
|
||
}
|
||
}
|
||
|
||
private RangedMoveAgent? GetCurAgent()
|
||
{
|
||
if (m_iCurMoveAgent >= 0 && m_iCurMoveAgent < m_moveAgents.Count)
|
||
{
|
||
return m_moveAgents[m_iCurMoveAgent];
|
||
}
|
||
return null;
|
||
}
|
||
|
||
public SearchResult Search(A3DVECTOR3 start, A3DVECTOR3 end, CMoveAgent.BrushTest brushTest = null, int nMaxExpand = -1)
|
||
{
|
||
ResetSearch();
|
||
if (m_moveAgents.Count == 0)
|
||
{
|
||
if (DEBUG_AUTOPF) BMLogger.LogWarning("[CECIntelligentRoute] Search: no moveAgents (uninitialized)");
|
||
return SearchResult.enumSearchUnInitialized;
|
||
}
|
||
|
||
m_start = start;
|
||
m_end = end;
|
||
|
||
// Find an agent that contains both start & end.
|
||
// 找到同时包含起点和终点的 agent。
|
||
bool bStartContained = false, bEndContained = false;
|
||
int idx = -1;
|
||
for (int i = 0; i < m_moveAgents.Count; i++)
|
||
{
|
||
var a = m_moveAgents[i];
|
||
if (a.Contain(new Vector3(start.x, start.y, start.z)))
|
||
{
|
||
bStartContained = true;
|
||
if (a.Contain(new Vector3(end.x, end.y, end.z)))
|
||
{
|
||
bEndContained = true;
|
||
idx = i;
|
||
break;
|
||
}
|
||
}
|
||
else if (a.Contain(new Vector3(end.x, end.y, end.z)))
|
||
{
|
||
bEndContained = true;
|
||
}
|
||
}
|
||
|
||
if (idx < 0)
|
||
{
|
||
if (bStartContained) return SearchResult.enumSearchEndInvalid;
|
||
if (bEndContained) return SearchResult.enumSearchStartInvalid;
|
||
return SearchResult.enumSearchStartEndInvalid;
|
||
}
|
||
|
||
m_iCurMoveAgent = idx;
|
||
var agent = m_moveAgents[idx].agent;
|
||
if (agent == null) return SearchResult.enumSearchUnInitialized;
|
||
|
||
// Determine layers (minimal: layer0 or invalid).
|
||
// 确定层（最小实现：层0或无效）。
|
||
float dist;
|
||
int startLayer = agent.WhichLayer(new Vector3(start.x, start.y, start.z), 0.0f, out dist);
|
||
int endLayer = agent.WhichLayer(new Vector3(end.x, end.y, end.z), 0.0f, out dist);
|
||
if (startLayer < 0) startLayer = 0;
|
||
if (endLayer < 0) endLayer = 0;
|
||
|
||
agent.SetStartEnd(new Vector3(start.x, start.y, start.z), startLayer, new Vector3(end.x, end.y, end.z), endLayer, brushTest);
|
||
bool ok = agent.Search(nMaxExpand);
|
||
if (!ok)
|
||
{
|
||
if (DEBUG_AUTOPF)
|
||
{
|
||
BMLogger.Log($"[CECIntelligentRoute] Search: no path found, ignoring. End position=({end.x:F2},{end.y:F2},{end.z:F2})");
|
||
}
|
||
// Don't call ResetSearch() again - it was already called at the start of Search()
|
||
// This prevents the idle state deadlock that blocks all movement
|
||
// 不再调用 ResetSearch() - 已在 Search() 开始时调用
|
||
// 这防止了阻止所有移动的空闲状态死锁
|
||
return SearchResult.enumSearchNoPath;
|
||
}
|
||
|
||
// EC_IntelligentRoute.cpp: single-node 2D path → start/goal coincide.
|
||
if (agent.GetPathCount() == 1)
|
||
{
|
||
agent.ResetSearch();
|
||
if (DEBUG_AUTOPF)
|
||
BMLogger.Log("[CECIntelligentRoute] Search: path size 1 (start/end coincide).");
|
||
return SearchResult.enumSearchStartEndCoincide;
|
||
}
|
||
|
||
m_state = RouteState.enumRouteMoving;
|
||
m_lastPos = m_start;
|
||
m_lastMove = 0.0f;
|
||
m_iCurDest = FindNextNode(start, 0);
|
||
m_dist2CurDest = (GetCurDest() - m_start).MagnitudeH();
|
||
if (DEBUG_AUTOPF)
|
||
{
|
||
BMLogger.Log($"[CECIntelligentRoute] Search: success pathCount={agent.GetPathCount()} start=({start.x:F2},{start.z:F2}) end=({end.x:F2},{end.z:F2}) curDestIdx={m_iCurDest}");
|
||
}
|
||
return SearchResult.enumSearchSuccess;
|
||
}
|
||
|
||
public A3DVECTOR3 GetCurDest()
|
||
{
|
||
if (IsIdle() || m_iCurMoveAgent < 0)
|
||
return m_end;
|
||
return GetNodePos(m_iCurDest);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Get full path for debug visualization.
|
||
/// 获取完整路径用于调试可视化。
|
||
/// </summary>
|
||
public System.Collections.Generic.List<Vector3> GetFullPath()
|
||
{
|
||
var path = new System.Collections.Generic.List<Vector3>();
|
||
if (m_state == RouteState.enumRouteIdle || m_iCurMoveAgent < 0) return path;
|
||
var agent = m_moveAgents[m_iCurMoveAgent].agent;
|
||
if (agent == null) return path;
|
||
return agent.GetFullPath();
|
||
}
|
||
|
||
/// <summary>Ported from C++ CECIntelligentRoute::OnPlayerPosChange (EC_IntelligentRoute.cpp).</summary>
|
||
public void OnPlayerPosChange(A3DVECTOR3 pos)
|
||
{
|
||
if (!IsMoveOn())
|
||
return;
|
||
|
||
if (CanFinishPath(pos))
|
||
{
|
||
m_state = RouteState.enumRoutePathFinished;
|
||
return;
|
||
}
|
||
|
||
if (!CanMoveToNext(pos))
|
||
return;
|
||
|
||
RangedMoveAgent? pCur = GetCurAgent();
|
||
if (pCur == null || pCur.Value.agent == null)
|
||
return;
|
||
CMoveAgent agent = pCur.Value.agent;
|
||
int pathCount = agent.GetPathCount();
|
||
if (m_iCurDest == pathCount - 1)
|
||
{
|
||
m_state = RouteState.enumRoutePathFinished;
|
||
return;
|
||
}
|
||
|
||
m_iCurDest = FindNextNode(pos, m_iCurDest + 1);
|
||
m_dist2CurDest = (GetCurDest() - pos).MagnitudeH();
|
||
// C++ calls agent->Optimize(m_iCurDest) here; Unity CMoveAgent has no path optimizer yet.
|
||
}
|
||
|
||
A3DVECTOR3 GetNodePos(int iNode)
|
||
{
|
||
RangedMoveAgent? pCurAgent = GetCurAgent();
|
||
if (pCurAgent == null || pCurAgent.Value.agent == null)
|
||
return new A3DVECTOR3(0f, 0f, 0f);
|
||
CMoveAgent ag = pCurAgent.Value.agent;
|
||
int nPathCount = ag.GetPathCount();
|
||
if (iNode >= 0 && iNode < nPathCount)
|
||
return GetNodePosNoCheck(iNode);
|
||
return new A3DVECTOR3(0f, 0f, 0f);
|
||
}
|
||
|
||
A3DVECTOR3 GetNodePosNoCheck(int iNode)
|
||
{
|
||
if (m_iCurMoveAgent < 0 || m_iCurMoveAgent >= m_moveAgents.Count)
|
||
return new A3DVECTOR3(0f, 0f, 0f);
|
||
CMoveAgent agent = m_moveAgents[m_iCurMoveAgent].agent;
|
||
if (agent == null)
|
||
return new A3DVECTOR3(0f, 0f, 0f);
|
||
Vector3 v = agent.Get3DPathNode(iNode);
|
||
// C++ adds GetTerrainHeight to Y; Unity path nodes use movemap/world height from loader.
|
||
return new A3DVECTOR3(v.x, v.y, v.z);
|
||
}
|
||
|
||
A3DVECTOR3 GetNodePosXZ(int iNode)
|
||
{
|
||
if (IsIdle())
|
||
return new A3DVECTOR3(0f, 0f, 0f);
|
||
A3DVECTOR3 p = GetNodePosNoCheck(iNode);
|
||
p.y = 0f;
|
||
return p;
|
||
}
|
||
|
||
int FindNearestNode(A3DVECTOR3 curPos, int iNodeFrom)
|
||
{
|
||
if (!IsMoveOn())
|
||
return -1;
|
||
RangedMoveAgent? pCur = GetCurAgent();
|
||
if (pCur == null || pCur.Value.agent == null)
|
||
return -1;
|
||
CMoveAgent agent = pCur.Value.agent;
|
||
int pathCount = agent.GetPathCount();
|
||
if (iNodeFrom < 0 || iNodeFrom >= pathCount)
|
||
return -1;
|
||
if (iNodeFrom == pathCount - 1)
|
||
return iNodeFrom;
|
||
|
||
int maxCheckIndex = iNodeFrom + agent.GetOptimizeCatchCount();
|
||
maxCheckIndex = Mathf.Min(maxCheckIndex, pathCount - 1);
|
||
int bestIndex = -1;
|
||
double bestDist2 = double.MaxValue;
|
||
for (int i = iNodeFrom; i <= maxCheckIndex; ++i)
|
||
{
|
||
A3DVECTOR3 testPos = GetNodePosXZ(i);
|
||
testPos -= curPos;
|
||
testPos.y = 0f;
|
||
double dist2 = testPos.x * testPos.x + testPos.z * testPos.z;
|
||
if (dist2 < bestDist2)
|
||
{
|
||
bestIndex = i;
|
||
bestDist2 = dist2;
|
||
}
|
||
}
|
||
|
||
return bestIndex >= 0 ? bestIndex : -1;
|
||
}
|
||
|
||
int FindFarthestNode(A3DVECTOR3 curPos, int iNodeFrom)
|
||
{
|
||
if (iNodeFrom < 0)
|
||
return iNodeFrom;
|
||
RangedMoveAgent? pCur = GetCurAgent();
|
||
if (pCur == null || pCur.Value.agent == null)
|
||
return iNodeFrom;
|
||
CMoveAgent agent = pCur.Value.agent;
|
||
int pathCount = agent.GetPathCount();
|
||
if (iNodeFrom == pathCount - 1)
|
||
return iNodeFrom;
|
||
|
||
A3DVECTOR3 moveDir = GetNodePosXZ(iNodeFrom);
|
||
moveDir -= curPos;
|
||
moveDir.y = 0f;
|
||
if (moveDir.Normalize() < 1e-4f)
|
||
return iNodeFrom;
|
||
|
||
int maxCheckIndex = iNodeFrom + agent.GetOptimizeCatchCount();
|
||
maxCheckIndex = Mathf.Min(maxCheckIndex, pathCount - 1);
|
||
for (int i = iNodeFrom + 1; i <= maxCheckIndex; ++i)
|
||
{
|
||
A3DVECTOR3 testDir = GetNodePosXZ(i);
|
||
testDir -= curPos;
|
||
testDir.y = 0f;
|
||
if (testDir.Normalize() < 1e-4f)
|
||
break;
|
||
float dtp = A3DVECTOR3.DotProduct(moveDir, testDir);
|
||
if (dtp < s_cos5Deg)
|
||
break;
|
||
iNodeFrom = i;
|
||
}
|
||
|
||
return iNodeFrom;
|
||
}
|
||
|
||
int FindNextNode(A3DVECTOR3 curPos, int iNodeFrom)
|
||
{
|
||
int iCandidate = FindNearestNode(curPos, iNodeFrom);
|
||
if (iCandidate < 0)
|
||
return Mathf.Max(0, iNodeFrom);
|
||
return FindFarthestNode(curPos, iCandidate);
|
||
}
|
||
|
||
bool CanFinishPath(A3DVECTOR3 pos)
|
||
{
|
||
if (!IsMoveOn())
|
||
return false;
|
||
A3DVECTOR3 delta = m_end - pos;
|
||
return delta.Magnitude() <= 0.5f;
|
||
}
|
||
|
||
bool CanMoveToNext(A3DVECTOR3 pos)
|
||
{
|
||
float fMove = (pos - m_lastPos).MagnitudeH();
|
||
m_lastPos = pos;
|
||
|
||
float lastMove = m_lastMove;
|
||
m_lastMove = fMove;
|
||
|
||
float dist2CurDest = m_dist2CurDest;
|
||
A3DVECTOR3 vCurDest = GetCurDest();
|
||
m_dist2CurDest = (vCurDest - pos).MagnitudeH();
|
||
|
||
if (fMove >= dist2CurDest)
|
||
return true;
|
||
if (fMove + 0.1f >= dist2CurDest)
|
||
return true;
|
||
if (lastMove * 0.5f >= m_dist2CurDest)
|
||
return true;
|
||
return false;
|
||
}
|
||
}
|
||
}
|
||
|
||
|