/// <summary>
/// 加入一个节点, 并调整堆结构.
/// </summary>
public void Push(PathfindingNode node)
{
node.Flag = container.Count;
container.Add(node);
AdjustHeap(node);
Utility.Assert(IsHeap());
}
/// <summary>
/// 关闭一个节点.
/// </summary>
public void Close(PathfindingNode node)
{
close.Add(node);
node.Flag = kNodeStateClosed;
}
/// <summary>
/// 调整堆.
/// </summary>
void AdjustHeap(PathfindingNode node)
{
int parent = Parent(node.Flag);
for (; parent >= 0 && F(node) < F(container[parent]); parent = Parent(parent))
{
Swap(node.Flag, parent);
}
}
float F(PathfindingNode node) { return node.G + node.H; }
/// <summary>
/// 节点是否在此次的A*中访问过.
/// </summary>
public bool IsVisited(PathfindingNode node)
{
return node.Flag >= 0;
}
/// <summary>
/// 节点是否已经关闭.
/// </summary>
/// <param name="node"></param>
/// <returns></returns>
public bool IsClosed(PathfindingNode node)
{
return node.Flag == kNodeStateClosed;
}
/// <summary>
/// 调整G和H.
/// <para>新的G+H必须比之前小.</para>
/// </summary>
public void DecreaseGH(PathfindingNode node, float newG, float newH)
{
Utility.Assert(newG + newH < node.G + node.H);
node.G = newG;
node.H = newH;
AdjustHeap(node);
Utility.Assert(IsHeap());
}
/// <summary>
/// 计算半径为radius的物体, 从节点startNode, 位置startPosition到节点destNode, 位置destPosition的移动, 经过的边.
/// </summary>
public static List<HalfEdge> FindPath(PathfindingNode startNode, Vector3 startPosition, PathfindingNode destNode, Vector3 destPosition, float radius)
{
AStarNodeContainer container = new AStarNodeContainer();
startNode.G = 0f;
startNode.H = 0f;
container.Push(startNode);
PathfindingNode currentNode = null;
for (; container.Count != 0 && currentNode != destNode; )
{
currentNode = container.Pop();
foreach (HalfEdge portal in currentNode.AdjacencyPortals)
{
// 忽略不可行走的三角形已经被关闭的三角形.
if (!portal.Face.Walkable || container.IsClosed(portal.Face))
{
continue;
}
// 可否进入节点.
if (!CheckEntryAndExitWidthLimit(currentNode, destNode, portal, radius))
{
continue;
}
// 可否进入节点, 如果非终点的话, 可否有边供离开.
if (!CheckCorridorWidthLimit(portal.Pair.Face.Portal, portal, radius))
{
continue;
}
if (!container.IsVisited(portal.Face))
{
container.Push(portal.Face);
}
Utility.Verify(currentNode.G == 0 || currentNode.Portal != null);
// https://raygun.com/blog/2015/01/game-development-triangulated-spaces-part-2/
float newH = MathUtility.MinDistance2Segment(destPosition, portal.Src.Position, portal.Dest.Position);
float newG = MathUtility.MinDistance2Segment(startPosition, portal.Src.Position, portal.Dest.Position);
if (currentNode.Portal != null)
{
newG = Mathf.Max(newG, (currentNode.H - newH) + currentNode.G);
newG = Mathf.Max(newG, CalculateArcLengthBetweenPortals(currentNode.Portal, portal, radius));
}
// 更新G和H, 以及入口.
if (newG + newH < portal.Face.G + portal.Face.H)
{
container.DecreaseGH(portal.Face, newG, newH);
portal.Face.Portal = portal;
}
}
container.Close(currentNode);
}
List<HalfEdge> path = CreatePath(currentNode);
// Create truncated path if currentNode != destNode.
//if (currentNode == destNode) { path = CreatePath(destNode); }
container.Dispose();
return path;
}
/// <summary>
/// 计算半径为radius的物体, 从节点startNode, 位置startPosition到节点destNode, 位置destPosition的移动的路径.
/// </summary>
public static List<Vector3> FindPath(PathfindingNode startNode, Vector3 startPosition, PathfindingNode destNode, Vector3 destPosition, float radius)
{
List<HalfEdge> portals = AStarPathfinding.FindPath(startNode, startPosition, destNode, destPosition, radius);
return PathSmoother.Smooth(startPosition, destPosition, portals, radius);
}
/// <summary>
/// 创建到dest的经过的边的集合.
/// </summary>
static List<HalfEdge> CreatePath(PathfindingNode dest)
{
List<HalfEdge> result = new List<HalfEdge>();
for (HalfEdge entry; (entry = dest.Portal) != null; dest = entry.Pair.Face)
{
Utility.Verify(result.Count < 1024, "Too many waypoints");
result.Add(entry);
}
// 创建的集合为从dest到start, 所以将它反序.
result.Reverse();
return result;
}
/// <summary>
/// 判断半径为radius的物体, 可否通过portal进入currentNode.
/// </summary>
static bool CheckEntryAndExitWidthLimit(PathfindingNode currentNode, PathfindingNode destNode, HalfEdge portal, float radius)
{
// 如果已经到达最终节点, 只判断可否经由此边进入.
if (currentNode == destNode)
{
return (portal.Dest.Position - portal.Src.Position).magnitude2() >= radius * 2;
}
// 否则, 除了判断, 是否可以经由次边进入外, 仍需判断该节点是否存在其他的边, 供离开.
HalfEdge other1 = portal.Face.AB, other2 = portal.Face.BC;
if (portal == portal.Face.AB)
{
other1 = portal.Face.BC;
other2 = portal.Face.CA;
}
else if (portal == portal.Face.BC)
{
other1 = portal.Face.AB;
other2 = portal.Face.CA;
}
// 其他两条边是否可供离开.
float diameter = radius * 2f;
if (portal.Face.GetWidth(portal, other1) >= diameter
|| portal.Face.GetWidth(portal, other2) >= diameter)
{
return true;
}
return false;
}
