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
|
||
}
|
||
}
|