// 返回路格在边中的位置（数组下标）
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);
}