/// <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());
		}
Beispiel #8
0
		/// <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;
		}
Beispiel #9
0
		/// <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);
		}
Beispiel #10
0
		/// <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;
		}
Beispiel #11
0
		/// <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;
		}