private void FindNeighbors(TnsPathNode node) { m_Neighbors.Clear(); TriangleNode triNode = node.triangle_node; for (int i = 0; i < 3; ++i) { TriangleNode neighbor = triNode.Neighbors[i]; if (null != neighbor) { m_Neighbors.Add(neighbor); } } }
private void IdentifySuccessors(TnsPathNode node) { FindNeighbors(node); int ct = m_Neighbors.Count; for (int i = 0; i < ct; ++i) { TriangleNode triNode = m_Neighbors[i]; if (m_CloseSet.ContainsKey(triNode)) { continue; } Vector3 pos1 = node.triangle_node.Position; Vector3 pos2 = triNode.Position; float d = Geometry.Distance(pos1, pos2); float ng = node.from_start_cost + d; bool isOpened = true; TnsPathNode nextNode; if (!m_OpenNodes.TryGetValue(triNode, out nextNode)) { nextNode = new TnsPathNode(triNode); isOpened = false; } if (!isOpened || ng < nextNode.from_start_cost) { nextNode.from_start_cost = ng; nextNode.cost = nextNode.from_start_cost + CalcHeuristic(triNode); nextNode.prev = node; if (!isOpened) { m_OpenQueue.Push(nextNode); m_OpenNodes.Add(triNode, nextNode); } else { int index = m_OpenQueue.IndexOf(nextNode); m_OpenQueue.Update(index, nextNode); } } } }
//查找下一个拐点 private Vector3 GetNextWayPoint(Vector3 curPnt, Vector3 target, TnsPathNode curNode, out TnsPathNode nextNode) { TnsPathNode lastNode1, lastNode2, next; Vector3 lpt1, lpt2; int ix = curNode.new_point_index; if (curNode.edge_flags == 0x02) { lpt1 = curNode.triangle_node.Points[(ix + 2) % 3]; lpt2 = curNode.triangle_node.Points[ix]; } else if (curNode.edge_flags == 0x01) { lpt1 = curNode.triangle_node.Points[ix]; lpt2 = curNode.triangle_node.Points[(ix + 1) % 3]; } else { if (null != curNode.next) {//起点 int sideIx = GetSideIndex(curNode.triangle_node, curNode.next.triangle_node); lpt1 = curNode.triangle_node.Points[sideIx]; lpt2 = curNode.triangle_node.Points[(sideIx + 1) % 3]; } else if (null != curNode.prev) {//终点 int sideIx = GetSideIndex(curNode.triangle_node, curNode.prev.triangle_node); lpt1 = curNode.triangle_node.Points[sideIx]; lpt2 = curNode.triangle_node.Points[(sideIx + 1) % 3]; } else {//起点也是终点 lpt1 = curNode.triangle_node.Points[ix]; lpt2 = curNode.triangle_node.Points[(ix + 1) % 3]; } } Vector3 lastPnt1 = lpt1 * (1 - m_OffsetToEdge) + lpt2 * m_OffsetToEdge; Vector3 lastPnt2 = lpt2 * (1 - m_OffsetToEdge) + lpt1 * m_OffsetToEdge; lastNode1 = lastNode2 = curNode; curNode = curNode.next; while (null != curNode) { Vector3 testPnt1, testPnt2; next = curNode.next; ix = curNode.new_point_index; if ((curNode.edge_flags & 0x02) != 0) {//左边是边沿 if (curNode.triangle_node == m_EndNode) { testPnt1 = target; testPnt2 = target; } else { Vector3 tpt1 = curNode.triangle_node.Points[(ix + 2) % 3]; Vector3 tpt2 = curNode.triangle_node.Points[ix]; testPnt1 = tpt1 * (1 - m_OffsetToEdge) + tpt2 * m_OffsetToEdge; testPnt2 = tpt2 * (1 - m_OffsetToEdge) + tpt1 * m_OffsetToEdge; } if (Geometry.PointIsLeftOn(testPnt2, lastPnt2, curPnt)) { if (!Geometry.PointIsLeftOn(curPnt, lastPnt1, testPnt2)) { nextNode = lastNode1.next; return(lastPnt1); } lastPnt2 = testPnt2; lastNode2 = curNode; } } if ((curNode.edge_flags & 0x01) != 0) {//右边是边沿 if (curNode.triangle_node == m_EndNode) { testPnt1 = target; testPnt2 = target; } else { Vector3 tpt1 = curNode.triangle_node.Points[ix]; Vector3 tpt2 = curNode.triangle_node.Points[(ix + 1) % 3]; testPnt1 = tpt1 * (1 - m_OffsetToEdge) + tpt2 * m_OffsetToEdge; testPnt2 = tpt2 * (1 - m_OffsetToEdge) + tpt1 * m_OffsetToEdge; } if (Geometry.PointIsLeftOn(curPnt, lastPnt1, testPnt1)) { if (!Geometry.PointIsLeftOn(curPnt, testPnt1, lastPnt2)) { nextNode = lastNode2.next; return(lastPnt2); } lastPnt1 = testPnt1; lastNode1 = curNode; } } curNode = next; } nextNode = null; return(target); }
public List <Vector3> FindPath(Vector3 start, Vector3 target, Vector3[] addToFirst, Vector3[] addToLast) { m_OpenQueue.Clear(); m_OpenNodes.Clear(); m_CloseSet.Clear(); m_VisitedNodes.Clear(); TriangleNode stNode = null, edNode = null; m_CacheNodesForStart.Clear(); m_CacheNodesForEnd.Clear(); m_KdTree.Query(start, m_SparseDistance * 2, (float distSqr, TriangleNode node) => { if (0 <= Geometry.PointInTriangle(start, node.Points[0], node.Points[1], node.Points[2])) { stNode = node; return(false); } else { m_CacheNodesForStart.Add(node); } return(true); }); m_KdTree.Query(target, m_SparseDistance * 2, (float distSqr, TriangleNode node) => { if (0 <= Geometry.PointInTriangle(target, node.Points[0], node.Points[1], node.Points[2])) { edNode = node; return(false); } else { m_CacheNodesForEnd.Add(node); } return(true); }); Vector3 nearstStartPt = new Vector3(); bool useNearstStart = false; if (null == stNode) { TriangleNode nearstNode = null; float nearstDistSqr = float.MaxValue; for (int ix = 0; ix < m_CacheNodesForStart.Count; ++ix) { TriangleNode node = m_CacheNodesForStart[ix]; for (int i = 0; i < 3; ++i) { if (node.Neighbors[i] == null) { Vector3 pt; float dSqr = Geometry.PointToLineSegmentDistanceSquare(start, node.Points[i], node.Points[(i + 1) % 3], out pt); if (dSqr < nearstDistSqr) { nearstNode = node; nearstDistSqr = dSqr; nearstStartPt = pt; } } } } if (null != nearstNode) { stNode = nearstNode; useNearstStart = true; } } Vector3 nearstEndPt = new Vector3(); bool useNearstEnd = false; if (null == edNode) { TriangleNode nearstNode = null; float nearstDistSqr = float.MaxValue; for (int ix = 0; ix < m_CacheNodesForEnd.Count; ++ix) { TriangleNode node = m_CacheNodesForEnd[ix]; for (int i = 0; i < 3; ++i) { if (node.Neighbors[i] == null) { Vector3 pt; float dSqr = Geometry.PointToLineSegmentDistanceSquare(target, node.Points[i], node.Points[(i + 1) % 3], out pt); if (dSqr < nearstDistSqr) { nearstNode = node; nearstDistSqr = dSqr; nearstEndPt = pt; } } } } if (null != nearstNode) { edNode = nearstNode; useNearstEnd = true; } } if (null == stNode || null == edNode) { return(new List <Vector3>()); } m_StartNode = stNode; m_EndNode = edNode; TnsPathNode startNode = new TnsPathNode(stNode); startNode.cost = 0; startNode.from_start_cost = 0; m_OpenQueue.Push(startNode); m_OpenNodes.Add(startNode.triangle_node, startNode); TnsPathNode curNode = null; while (m_OpenQueue.Count > 0) { curNode = m_OpenQueue.Pop(); if (m_RecordVisitedNodes) { m_VisitedNodes.Add(curNode.triangle_node); //LogSystem.Debug("cost:{0} from_start_cost:{1}", curNode.cost, curNode.from_start_cost); } m_OpenNodes.Remove(curNode.triangle_node); m_CloseSet.Add(curNode.triangle_node, true); if (curNode.triangle_node == edNode) { break; } IdentifySuccessors(curNode); } List <Vector3> path = new List <Vector3>(); if (curNode.triangle_node == edNode) { //建立路径邻接信息与路点三角形的穿出边信息 TnsPathNode next = null; while (curNode != null) { TnsPathNode prev = curNode.prev; if (null != prev) { int ptIx = GetSideIndex(curNode.triangle_node, prev.triangle_node); if (ptIx >= 0) { ptIx = (ptIx + 2) % 3; curNode.new_point_index = ptIx; } if (null != next) { int sideIx = GetSideIndex(curNode.triangle_node, next.triangle_node); if (sideIx == ptIx) { curNode.edge_flags = 0x01; } else { curNode.edge_flags = 0x02; } } prev.next = curNode; } next = curNode; curNode = prev; } //拐点法化简与平滑路径 if (null != addToFirst && addToFirst.Length > 0) { path.AddRange(addToFirst); } path.Add(start); Vector3 curPnt; Vector3 targetPnt; if (useNearstStart) { path.Add(nearstStartPt); curPnt = nearstStartPt; } else { curPnt = start; } if (useNearstEnd) { targetPnt = nearstEndPt; } else { targetPnt = target; } curNode = startNode; while (null != curNode) { TnsPathNode nextNode; curPnt = GetNextWayPoint(curPnt, targetPnt, curNode, out nextNode); int ct = path.Count; if (Geometry.DistanceSquare(path[ct - 1], curPnt) > m_SparseDistance * m_SparseDistance) { path.Add(curPnt); } curNode = nextNode; } if (useNearstEnd) { path.Add(nearstEndPt); } /// path.Add(target); if (null != addToLast && addToLast.Length > 0) { path.AddRange(addToLast); } } return(path); }