481 lines
18 KiB
C#
481 lines
18 KiB
C#
|
using System.Collections.Generic;
|
|||
|
|
using UnityEngine;
|
|||
|
|
using Thousandto.Core.Base;
|
|||
|
|
using SimpleAI.Planning;
|
|||
|
|
using SceneEditor.Proxy.Plugin;
|
|||
|
|
using Thousandto.Plugins.Common.UniScene;
|
|||
|
|
|
|||
|
|
namespace Thousandto.Plugins.PathGrid
|
|||
|
|
{
|
|||
|
|
//陆地寻路系统
|
|||
|
|
public class PathLandSystem : PathGridSystem
|
|||
|
|
{
|
|||
|
|
#region 私有变量
|
|||
|
|
private List<Vector2> _pathPoints2d = new List<Vector2>();
|
|||
|
|
//A星寻路
|
|||
|
|
private AStarPlanner wayPointPlanner = null;
|
|||
|
|
//wayPoint数据
|
|||
|
|
private WayPointPlanData wayPointWorld = null;
|
|||
|
|
#endregion
|
|||
|
|
|
|||
|
|
#region 私有函数
|
|||
|
|
//在没有wayPoint情况下的纯A星寻路
|
|||
|
|
private List<Vector2> GetPathNoWayPoint(Vector2 startPosition, Vector2 endPosition)
|
|||
|
|
{
|
|||
|
|
_pathPoints2d.Clear();
|
|||
|
|
var grid = _pathWorld;
|
|||
|
|
int startNodeIndex = grid.GetPathNodeIndex(startPosition);
|
|||
|
|
if (grid.IsBlocked(startNodeIndex))
|
|||
|
|
{
|
|||
|
|
return null;
|
|||
|
|
}
|
|||
|
|
int endNodeIndex = grid.GetPathNodeIndex(endPosition);
|
|||
|
|
if (grid.IsBlocked(endNodeIndex))
|
|||
|
|
{
|
|||
|
|
return null;
|
|||
|
|
}
|
|||
|
|
float dx = MathLib.FastAbs(startPosition.x - endPosition.x);
|
|||
|
|
float dz = MathLib.FastAbs(startPosition.y - endPosition.y);
|
|||
|
|
float cdx = dx * grid.InvCellSize + 1;
|
|||
|
|
float cdz = dz * grid.InvCellSize + 1;
|
|||
|
|
float fdd = cdx * cdz;
|
|||
|
|
bool result = false;
|
|||
|
|
if (fdd <= PreSearchSize)
|
|||
|
|
{
|
|||
|
|
_pathPlannder.StartANewPlan(startNodeIndex, endNodeIndex, true);
|
|||
|
|
while (!_pathPlannder.HasPlanCompleted())
|
|||
|
|
{
|
|||
|
|
_pathPlannder.Update(50);
|
|||
|
|
}
|
|||
|
|
result = _pathPlannder.HasPlanSucceeded();
|
|||
|
|
if (!result)
|
|||
|
|
{
|
|||
|
|
if (_pathPlannder.GetAvailableSearchNodeCount() != 0)
|
|||
|
|
{
|
|||
|
|
// search node pool size is ok, add node in openTable has been fully tested.
|
|||
|
|
return null;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if (!result)
|
|||
|
|
{
|
|||
|
|
_pathPlannder.StartANewPlan(startNodeIndex, endNodeIndex);
|
|||
|
|
while (!_pathPlannder.HasPlanCompleted())
|
|||
|
|
{
|
|||
|
|
_pathPlannder.Update(50);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
result = _pathPlannder.HasPlanSucceeded();
|
|||
|
|
if (_pathPlannder.Solution.Count > 0)
|
|||
|
|
{
|
|||
|
|
Vector2 last = startPosition;
|
|||
|
|
_pathPoints2d.Add(startPosition);
|
|||
|
|
var solution = _pathPlannder.Solution;
|
|||
|
|
for (int i = solution.Count - 1; i >= 0; --i)
|
|||
|
|
{
|
|||
|
|
var pt = MathLib.ToVector2_XOZ(grid.GetPathNodePos(solution[i].Index));
|
|||
|
|
if ((pt - last).sqrMagnitude > Vector2.kEpsilon)
|
|||
|
|
{
|
|||
|
|
_pathPoints2d.Add(pt);
|
|||
|
|
last = pt;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if ((endPosition - last).sqrMagnitude > Vector2.kEpsilon)
|
|||
|
|
{
|
|||
|
|
_pathPoints2d.Add(endPosition);
|
|||
|
|
}
|
|||
|
|
var finalPath = SmoothPath(_pathPoints2d, endPosition);
|
|||
|
|
if (finalPath.Count >= 2)
|
|||
|
|
{
|
|||
|
|
return finalPath;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
return null;
|
|||
|
|
}
|
|||
|
|
//获取点到waypoint的寻路路径
|
|||
|
|
private bool GetGridPath(Vector2 startPosition, Vector2 endPosition, List<Vector2> pathPosition)
|
|||
|
|
{
|
|||
|
|
var grid = _pathWorld;
|
|||
|
|
int startNodeIndex = grid.GetPathNodeIndex(startPosition);
|
|||
|
|
int endNodeIndex = grid.GetPathNodeIndex(endPosition);
|
|||
|
|
float dx = MathLib.FastAbs(startPosition.x - endPosition.x);
|
|||
|
|
float dz = MathLib.FastAbs(startPosition.y - endPosition.y);
|
|||
|
|
float cdx = dx * grid.InvCellSize + 1;
|
|||
|
|
float cdz = dz * grid.InvCellSize + 1;
|
|||
|
|
float fdd = cdx * cdz;
|
|||
|
|
bool result = false;
|
|||
|
|
if (fdd <= PreSearchSize)
|
|||
|
|
{
|
|||
|
|
_pathPlannder.StartANewPlan(startNodeIndex, endNodeIndex, true);
|
|||
|
|
while (!_pathPlannder.HasPlanCompleted())
|
|||
|
|
{
|
|||
|
|
_pathPlannder.Update(50);
|
|||
|
|
}
|
|||
|
|
result = _pathPlannder.HasPlanSucceeded();
|
|||
|
|
if (!result)
|
|||
|
|
{
|
|||
|
|
if (_pathPlannder.GetAvailableSearchNodeCount() != 0)
|
|||
|
|
{
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if (!result)
|
|||
|
|
{
|
|||
|
|
_pathPlannder.StartANewPlan(startNodeIndex, endNodeIndex);
|
|||
|
|
while (!_pathPlannder.HasPlanCompleted())
|
|||
|
|
{
|
|||
|
|
_pathPlannder.Update(50);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
result = _pathPlannder.HasPlanSucceeded();
|
|||
|
|
if (_pathPlannder.Solution.Count > 0)
|
|||
|
|
{
|
|||
|
|
Vector2 last = startPosition;
|
|||
|
|
pathPosition.Add(startPosition);
|
|||
|
|
var solution = _pathPlannder.Solution;
|
|||
|
|
for (int i = solution.Count - 1; i >= 0; --i)
|
|||
|
|
{
|
|||
|
|
var pt = MathLib.ToVector2_XOZ(grid.GetPathNodePos(solution[i].Index));
|
|||
|
|
if ((pt - last).sqrMagnitude > Vector2.kEpsilon)
|
|||
|
|
{
|
|||
|
|
pathPosition.Add(pt);
|
|||
|
|
last = pt;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if ((endPosition - last).sqrMagnitude > Vector2.kEpsilon)
|
|||
|
|
{
|
|||
|
|
pathPosition.Add(endPosition);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
return result;
|
|||
|
|
}
|
|||
|
|
//寻出waypoint的路径
|
|||
|
|
public bool searchWayPointPath(int startIndex, int endIndex, List<Vector2> pathPosition)
|
|||
|
|
{
|
|||
|
|
if (wayPointWorld.IsNodeBlocked(startIndex))
|
|||
|
|
{
|
|||
|
|
WayPointNode node = wayPointWorld.GetNode(startIndex);
|
|||
|
|
//Debug.LogError(string.Format("{0}这个点放到了阻挡里面",node.name));
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
if (wayPointWorld.IsNodeBlocked(endIndex))
|
|||
|
|
{
|
|||
|
|
WayPointNode node = wayPointWorld.GetNode(endIndex);
|
|||
|
|
//Debug.LogError(string.Format("{0}这个点放到了阻挡里面", node.name));
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
FProfiler.Begin("PathLandSystem.searchWayPointPath");
|
|||
|
|
wayPointPlanner.StartANewPlan(startIndex, endIndex);
|
|||
|
|
while (!wayPointPlanner.HasPlanCompleted())
|
|||
|
|
wayPointPlanner.Update(100);
|
|||
|
|
FProfiler.End();
|
|||
|
|
if (!wayPointPlanner.HasPlanSucceeded())
|
|||
|
|
{
|
|||
|
|
//Debug.LogError(string.Format("MapId:{0} 路点编辑有问题,起始路点和终点路点不连通！！！", Owner.Cfg.MapId));
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
Vector2 tempPos = Vector2.zero;
|
|||
|
|
for (int i = wayPointPlanner.Solution.Count - 1; i >= 0; --i)
|
|||
|
|
{
|
|||
|
|
Vector3 pos = wayPointWorld.m_pointInfo[wayPointPlanner.Solution[i].Index].pos;
|
|||
|
|
tempPos.x = pos.x;
|
|||
|
|
tempPos.y = pos.z;
|
|||
|
|
pathPosition.Add(tempPos);
|
|||
|
|
}
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
private List<Vector2> GetPath(Vector2 startPosition, Vector2 endPosition)
|
|||
|
|
{
|
|||
|
|
if (wayPointWorld == null)
|
|||
|
|
{
|
|||
|
|
//Debug.LogError(string.Format("MapId:{0} 场景中找不到[WayPoint] WayPointWorld为空",Owner.Cfg.MapId));
|
|||
|
|
return GetPathNoWayPoint(startPosition, endPosition);
|
|||
|
|
}
|
|||
|
|
int startWayPoint = wayPointWorld.GetNearestIndexBypos(ref startPosition);
|
|||
|
|
int endWaypoint = wayPointWorld.GetNearestIndexBypos(ref endPosition);
|
|||
|
|
if (startWayPoint == -1 || endWaypoint == -1)
|
|||
|
|
{
|
|||
|
|
//Debug.LogError(string.Format("MapId:{0} 场景中找不到[WayPoint]",Owner.Cfg.MapId));
|
|||
|
|
return GetPathNoWayPoint(startPosition, endPosition);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
_pathPoints2d.Clear();
|
|||
|
|
int startNodeIndex = _pathWorld.GetPathNodeIndex(startPosition);
|
|||
|
|
int endNodeIndex = _pathWorld.GetPathNodeIndex(endPosition);
|
|||
|
|
Vector3 startWayPointPos = wayPointWorld.m_pointInfo[startWayPoint].pos;
|
|||
|
|
Vector3 endWayPointPos = wayPointWorld.m_pointInfo[endWaypoint].pos;
|
|||
|
|
Vector2 startWayPoint2d = new Vector2(startWayPointPos.x, startWayPointPos.z);
|
|||
|
|
Vector2 endWayPoint2d = new Vector2(endWayPointPos.x, endWayPointPos.z);
|
|||
|
|
|
|||
|
|
_pathPoints2d.Add(startPosition);
|
|||
|
|
if(!GetGridPath(startPosition, startWayPoint2d, _pathPoints2d))
|
|||
|
|
return null;
|
|||
|
|
if(!searchWayPointPath(startWayPoint, endWaypoint, _pathPoints2d))
|
|||
|
|
return null;
|
|||
|
|
if(!GetGridPath(endWayPoint2d, endPosition, _pathPoints2d))
|
|||
|
|
return null;
|
|||
|
|
var finalPath = SmoothPath(_pathPoints2d, endPosition);
|
|||
|
|
if (finalPath.Count >= 2)
|
|||
|
|
return finalPath;
|
|||
|
|
return null;
|
|||
|
|
}
|
|||
|
|
//优化寻路出来的寻路点,把可以去掉的寻路点去掉
|
|||
|
|
private List<Vector2> SmoothPath(List<Vector2> input, Vector2 endPosition)
|
|||
|
|
{
|
|||
|
|
var ret = Path2dPool.sharedInstance.Allocate(input.Count >> 1);
|
|||
|
|
int count = input.Count;
|
|||
|
|
if (count > 1)
|
|||
|
|
{
|
|||
|
|
ret.Add(input[0]); // add start point
|
|||
|
|
Vector2 s, e, hit;
|
|||
|
|
int curEndIndex = -1;
|
|||
|
|
for (int j = 0; j < count - 1;)
|
|||
|
|
{
|
|||
|
|
s = input[j];
|
|||
|
|
int i = j + 1;
|
|||
|
|
int si = j;
|
|||
|
|
int c = 0;
|
|||
|
|
for (; i < count && c < int.MaxValue; ++i)
|
|||
|
|
{
|
|||
|
|
e = input[i];
|
|||
|
|
if ((i - si) > 1 && Raycast2dSafe(s, e, out hit))
|
|||
|
|
{
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
curEndIndex = i;
|
|||
|
|
j = i;
|
|||
|
|
}
|
|||
|
|
++c;
|
|||
|
|
}
|
|||
|
|
ret.Add(input[curEndIndex]);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
return ret;
|
|||
|
|
}
|
|||
|
|
#endregion
|
|||
|
|
|
|||
|
|
#region 公共方法
|
|||
|
|
public override void Initialize(BaseScene scene)
|
|||
|
|
{
|
|||
|
|
base.Initialize(scene);
|
|||
|
|
}
|
|||
|
|
public override void Uninitialize()
|
|||
|
|
{
|
|||
|
|
base.Uninitialize();
|
|||
|
|
}
|
|||
|
|
public bool LoadWayPointData(BaseScene owner)
|
|||
|
|
{
|
|||
|
|
Transform trans = owner.SceneObjectsRoot.Find(BaseScene.WayPointRootName);
|
|||
|
|
if (trans != null)
|
|||
|
|
wayPointWorld = trans.GetComponent<WayPointPlanData>();
|
|||
|
|
if (wayPointWorld != null && wayPointWorld.m_pointInfo != null && wayPointWorld.m_maxNeighborNumber > 0)
|
|||
|
|
{
|
|||
|
|
wayPointPlanner = new AStarPlanner(wayPointWorld.m_pointInfo.Length + 1, 0);
|
|||
|
|
wayPointPlanner.Awake();
|
|||
|
|
wayPointPlanner.Start(wayPointWorld);
|
|||
|
|
}
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
//根据目标的半径范围,从起点到终点寻路获取寻路点
|
|||
|
|
public override List<Vector2> SearchTargetPath(Vector3 startPos, Vector3 endPos, float targetRadius)
|
|||
|
|
{
|
|||
|
|
var startIsBlock = IsBlocked(startPos);
|
|||
|
|
if(startIsBlock)
|
|||
|
|
{
|
|||
|
|
//Debug.Log("起点在阻挡里");
|
|||
|
|
return null;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
var endIsSafe = IsSafe(endPos);
|
|||
|
|
if (endIsSafe)
|
|||
|
|
{
|
|||
|
|
//Debug.Log("终点在传送点里");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
List<Vector2> outPath = Path2dPool.sharedInstance.Allocate(1);
|
|||
|
|
|
|||
|
|
//如果玩家处于跟目标位置一样的点
|
|||
|
|
if (startPos == endPos)
|
|||
|
|
{
|
|||
|
|
return outPath;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//如果寻路到的目标点有半径
|
|||
|
|
if (targetRadius > 0.0f)
|
|||
|
|
{
|
|||
|
|
if (Vector3.Distance(startPos, endPos) <= targetRadius)
|
|||
|
|
{
|
|||
|
|
return outPath;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Vector2 start2d = MathLib.ToVector2_XOZ(startPos);
|
|||
|
|
Vector2 end2d = MathLib.ToVector2_XOZ(endPos);
|
|||
|
|
|
|||
|
|
var blocked = IsBlocked(endPos);
|
|||
|
|
if (blocked)
|
|||
|
|
{
|
|||
|
|
//Debug.Log("目标点在阻挡里");
|
|||
|
|
Vector2 dir = (end2d - start2d).normalized;
|
|||
|
|
Vector2 newPos = end2d - dir * Mathf.Min(targetRadius, CellSize * 0.5f);
|
|||
|
|
if(IsBlocked(newPos))
|
|||
|
|
{
|
|||
|
|
//Debug.Log("在半径边缘的目标点还是在阻挡里");
|
|||
|
|
return null;
|
|||
|
|
}
|
|||
|
|
if (GetCellIndex(start2d) != GetCellIndex(newPos))
|
|||
|
|
{
|
|||
|
|
end2d = newPos;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
return outPath;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Math2d.Circle circle;
|
|||
|
|
circle.Center = end2d;
|
|||
|
|
circle.Radius = targetRadius;
|
|||
|
|
bool goDirect = false;
|
|||
|
|
if(endIsSafe)
|
|||
|
|
{
|
|||
|
|
goDirect = CanGoDirect(start2d, end2d);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
goDirect = CanGoDirectSafe(start2d, end2d);
|
|||
|
|
}
|
|||
|
|
if (goDirect)
|
|||
|
|
{
|
|||
|
|
if (targetRadius > 0)
|
|||
|
|
{
|
|||
|
|
Math2d.LineSegment line;
|
|||
|
|
line.Start = start2d;
|
|||
|
|
line.End = end2d;
|
|||
|
|
var solves = Math2d.Intersect_LineSegment_Circle(circle, line);
|
|||
|
|
if (solves != null && solves.Length > 0 && solves[0] != end2d)
|
|||
|
|
{
|
|||
|
|
end2d = solves[0];
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
return outPath;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if (start2d != end2d)
|
|||
|
|
{
|
|||
|
|
outPath = Path2dPool.sharedInstance.Allocate(2);
|
|||
|
|
outPath.Add(start2d);
|
|||
|
|
outPath.Add(end2d);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
return outPath;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
var ptlist = GetPath(start2d, end2d);
|
|||
|
|
if (ptlist == null || ptlist.Count < 2)
|
|||
|
|
{
|
|||
|
|
//Debug.Log("通过waypoint寻路没有寻到");
|
|||
|
|
return null;
|
|||
|
|
}
|
|||
|
|
Vector2 currPoint = ptlist.Count > 0 ? ptlist[0] : Vector2.zero;
|
|||
|
|
var _ptlist = Path2dPool.sharedInstance.Allocate(ptlist.Count);
|
|||
|
|
bool earlyBreak = false;
|
|||
|
|
_ptlist.Add(currPoint);
|
|||
|
|
for (int i = 1; i < ptlist.Count; ++i)
|
|||
|
|
{
|
|||
|
|
Vector2 nextPoint = ptlist[i];
|
|||
|
|
if (targetRadius > 0)
|
|||
|
|
{
|
|||
|
|
Math2d.LineSegment line;
|
|||
|
|
line.Start = currPoint;
|
|||
|
|
line.End = nextPoint;
|
|||
|
|
var solves = Math2d.Intersect_LineSegment_Circle(circle, line);
|
|||
|
|
if (solves != null && solves.Length > 0)
|
|||
|
|
{
|
|||
|
|
nextPoint = solves[0];
|
|||
|
|
earlyBreak = true;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
_ptlist.Add(nextPoint);
|
|||
|
|
if (earlyBreak)
|
|||
|
|
{
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
currPoint = nextPoint;
|
|||
|
|
}
|
|||
|
|
outPath = _ptlist;
|
|||
|
|
}
|
|||
|
|
return outPath;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public override List<Vector2> SearchPlayerPath(Vector3 startPosition, Vector3 endPosition)
|
|||
|
|
{
|
|||
|
|
if (startPosition == endPosition)
|
|||
|
|
{
|
|||
|
|
return null;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
Vector2 hit;
|
|||
|
|
if (!Raycast2d(startPosition, endPosition, out hit))
|
|||
|
|
{
|
|||
|
|
return GetPath(startPosition, endPosition);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
return null;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public override List<Vector2> SearchOtherPath(Vector3 startPosition, Vector3 endPosition)
|
|||
|
|
{
|
|||
|
|
if (startPosition == endPosition)
|
|||
|
|
{
|
|||
|
|
return null;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
List<Vector2> ret = Path2dPool.sharedInstance.Allocate(2);
|
|||
|
|
ret.Add(startPosition);
|
|||
|
|
ret.Add(endPosition);
|
|||
|
|
return ret;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//评价路径，返回-1表示无法寻路到达，返回距离表示到目标点的实际距离
|
|||
|
|
public override float EvaluatePath(Vector3 startPosition, Vector3 endPosition)
|
|||
|
|
{
|
|||
|
|
List<Vector2> outPath = null;
|
|||
|
|
outPath = SearchTargetPath(startPosition, endPosition, 0f);
|
|||
|
|
if (outPath == null || outPath.Count < 2)
|
|||
|
|
{
|
|||
|
|
//寻路失败，无法到达
|
|||
|
|
return -1f;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
var result = 0f;
|
|||
|
|
for(int i = 0; i < outPath.Count - 1; ++i)
|
|||
|
|
{
|
|||
|
|
if (IsNotBlockRayCast2d(outPath[i], outPath[i + 1]))
|
|||
|
|
{
|
|||
|
|
result += Vector2.Distance(outPath[i], outPath[i + 1]);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
//有阻断，无法到达
|
|||
|
|
return -1f;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
//寻路失败无法到达
|
|||
|
|
return result;
|
|||
|
|
}
|
|||
|
|
#endregion
|
|||
|
|
}
|
|||
|
|
}
|