// 返回路格在边中的位置(数组下标) public int FindEdgeOfRoadCell(int cellIndex, out MapPathGraph.VertexNode vn1, out MapPathGraph.VertexNode vn2, out MapPathGraph.Edge e) { vn1 = vn2 = null; e = null; List <MapPathGraph.WeightVertexNode> listVertex = FindNearestVertex(cellIndex); foreach (MapPathGraph.WeightVertexNode vn in listVertex) { MapPathGraph.EdgeNode p = vn.FirstEdge; while (p != null) { MapPathGraph.Edge edge = p.EdgeLink; for (int i = 0; i < edge.Spots.Length; i++) { if (edge.Spots[i] == cellIndex) { vn1 = vn; vn2 = this.pathGraph.VertexList[p.AdjacentVertex]; e = edge; return(i); } } p = p.Next; } } return(-1); }
// e1,e2在屏幕坐标系统下,返回值小于0表示e1 -> e2为逆时针 private int CompareEdgeByRadian(MapPathGraph.Edge e1, MapPathGraph.Edge e2) { int v, v1, v2; if (e1.Cell1 == e2.Cell1) { v = e1.Cell1; v1 = e1.Cell2; v2 = e2.Cell2; } else if (e1.Cell1 == e2.Cell2) { v = e1.Cell1; v1 = e1.Cell2; v2 = e2.Cell1; } else if (e1.Cell2 == e2.Cell1) { v = e1.Cell2; v1 = e1.Cell1; v2 = e2.Cell2; } else if (e1.Cell2 == e2.Cell2) { v = e1.Cell2; v1 = e1.Cell1; v2 = e2.Cell1; } else { throw new Exception("无相同的顶点"); } int x = v % this.colNum; int y = v / this.colNum; int x1 = v1 % this.colNum; int y1 = v1 / this.colNum; int x2 = v2 % this.colNum; int y2 = v2 / this.colNum; Vector3 ve = new Vector3(x, y, 0); Vector3 ve1 = new Vector3(x1, y1, 0); Vector3 ve2 = new Vector3(x2, y2, 0); Vector3 vs = Vector3.Cross((ve1 - ve).normalized, (ve2 - ve).normalized); return(vs.z > 0 ? 1 : -1); }
// 输出的顶点数和边数相同 public bool FindCell2CrossPath(GpsMapCell from, GpsMapCell cross, out List <MapPathGraph.NavigateVertexNode> listNavigateVertexNode, out List <MapPathGraph.Edge> listEdge) { listNavigateVertexNode = null; listEdge = null; from = FindNearestCellOf(from, GpsMapCell.EType.EN_ROAD); if (from == null) { XxdwDebugger.Log("Road nearby not found. Strange!"); return(false); } int crossVertexIndex = this.pathGraph.FindVertex(cross.Index); if (crossVertexIndex < 0) { XxdwDebugger.Log("Target cross not found. Strange!"); return(false); } MapPathGraph.VertexNode vnTo = this.pathGraph.VertexList[crossVertexIndex]; MapPathGraph.VertexNode vn1, vn2; MapPathGraph.Edge e; int spotIndex = FindEdgeOfRoadCell(from.Index, out vn1, out vn2, out e); if (spotIndex < 0) { XxdwDebugger.Log("Cross of road not found. Strange!"); return(false); } float w1 = CalcDetailWeight(e.Spots, 0, spotIndex + 1); float w2 = CalcDetailWeight(e.Spots, spotIndex, e.Spots.Length); int[] spots1 = new int[spotIndex + 1]; Array.Copy(e.Spots, 0, spots1, 0, spotIndex + 1); int[] spots2 = new int[e.Spots.Length - spotIndex]; Array.Copy(e.Spots, spotIndex, spots2, 0, e.Spots.Length - spotIndex); MapPathGraph.Edge e1, e2; if (e.Spots[0] == vn1.Cell) { e1 = new MapPathGraph.Edge(vn1.Cell, e.Spots[spotIndex], w1, spots1, ""); e1.ReverseSpots(); e2 = new MapPathGraph.Edge(vn2.Cell, e.Spots[spotIndex], w2, spots2, ""); } else { e1 = new MapPathGraph.Edge(vn1.Cell, e.Spots[spotIndex], w2, spots2, ""); e2 = new MapPathGraph.Edge(vn2.Cell, e.Spots[spotIndex], w1, spots1, ""); e2.ReverseSpots(); } List <MapPathGraph.NavigateVertexNode> listNavigateVertexNode1 = new List <MapPathGraph.NavigateVertexNode>(); List <MapPathGraph.NavigateVertexNode> listNavigateVertexNode2 = new List <MapPathGraph.NavigateVertexNode>(); AStarHelper ash = new AStarHelper(this.CalcDistanceWeight); ash.Clear(); float weight1 = this.pathGraph.FindShortestPaths(ash, vn1, vnTo, listNavigateVertexNode1); weight1 += weight1 >= 0 ? w1 : Parameters.MAX_EDGE_WEIGHT; ash.Clear(); float weight2 = this.pathGraph.FindShortestPaths(ash, vn2, vnTo, listNavigateVertexNode2); weight2 += weight2 >= 0 ? w2 : Parameters.MAX_EDGE_WEIGHT; if (listNavigateVertexNode1.Count == 0 && listNavigateVertexNode2.Count == 0) { return(false); } listEdge = new List <MapPathGraph.Edge>(); if (weight1 < weight2) { listNavigateVertexNode1[0].SelectedEdgeIndex = this.pathGraph.GetEdgeNumber(vn1, e); listNavigateVertexNode = listNavigateVertexNode1; this.pathGraph.NormalizeVertexAndSpotsOrder(listNavigateVertexNode, listEdge); listEdge.Insert(0, e1); } else { listNavigateVertexNode2[0].SelectedEdgeIndex = this.pathGraph.GetEdgeNumber(vn2, e); listNavigateVertexNode = listNavigateVertexNode2; this.pathGraph.NormalizeVertexAndSpotsOrder(listNavigateVertexNode, listEdge); listEdge.Insert(0, e2); } return(true); }