public void RemoveArc(PathNode node1, PathNode node2) { PathArc arc = GetArc(node1, node2); node1.arcs.Remove(arc); node2.arcs.Remove(arc); }
public PathArc GetArc(PathNode node1, PathNode node2) { for (int i = 0; i < node1.arcs.Count; i++) { PathArc arc = node1.arcs[i]; if (arc.node1 == node2 || arc.node2 == node2) { return(arc); } } return(InvalidArc); }
public void AddArc(PathNode node1, PathNode node2) { PathArc arc = new PathArc(node1, node2); node1.arcs.Add(arc); node2.arcs.Add(arc); // Compute the arc's traversal time for each MobilityType foreach (MobilityType mobility in MobilityType.MobilityTypes) { arc.time[mobility.Index] = Pathfinder.FindLocalPath( this, Position(node1), Position(node2), mobility, 0f); } }
private void BuildGraph() { graph.Clear(); Vector3 size = terrain.terrainData.size; // TODO: Add nodes for terrain features // Add nodes for roads ERModularRoad[] roads = (ERModularRoad[])GameObject.FindObjectsOfType(typeof(ERModularRoad)); foreach (ERModularRoad road in roads) { for (int i = 0; i < road.middleIndentVecs.Count; i++) { Vector3 roadVert = road.middleIndentVecs[i]; if (Mathf.Abs(roadVert.x) < size.x / 2 && Mathf.Abs(roadVert.z) < size.z / 2) { graph.Add(new PathNode(roadVert, true)); } if (i < road.middleIndentVecs.Count - 1) { Vector3 nextRoadVert = road.middleIndentVecs[i + 1]; Vector3 stretch = nextRoadVert - roadVert; int nIntermediate = (int)(stretch.magnitude / RoadGridSpacing); stretch /= 1 + nIntermediate; Vector3 intermediatePosition = roadVert; for (int j = 0; j < nIntermediate; j++) { intermediatePosition += stretch; if (Mathf.Abs(intermediatePosition.x) < size.x / 2 && Mathf.Abs(intermediatePosition.z) < size.z / 2) { graph.Add(new PathNode(intermediatePosition, true)); } } } } } /*// Fill in any big open spaces with a sparse grid in case the above missed anything important * Vector3 newPos = new Vector3(0f, 0f, 0f); * for (float x = -size.x/2; x < size.x/2; x += SparseGridSpacing / 10) { * for (float z = -size.z/2; z < size.z/2; z += SparseGridSpacing / 10) { * newPos.Set(x, 0f, z); * * float minDist = float.MaxValue; * foreach (PathNode node in graph) * minDist = Mathf.Min(minDist, Vector3.Distance(newPos, node.position)); * * if (minDist > SparseGridSpacing) * graph.Add(new PathNode(newPos)); * } * }*/ // Remove nodes that are right on top of each other for (int i = 0; i < graph.Count; i++) { for (int j = i + 1; j < graph.Count; j++) { if ((Position(graph[i]) - Position(graph[j])).magnitude < NodePruneDistThresh) { graph.RemoveAt(j); } } } openSet = new FastPriorityQueue <PathNode>(graph.Count + 1); // Compute arcs for all pairs of nodes within cutoff distance for (int i = 0; i < graph.Count; i++) { for (int j = i + 1; j < graph.Count; j++) { if ((Position(graph[i]) - Position(graph[j])).magnitude < ArcMaxDist) { AddArc(graph[i], graph[j]); } } } // Remove unnecessary arcs // An arc in necessary if for any MobilityType, the direct path between the nodes is at least // as good as the optimal global path. This is a brute force approach and it might be too slow List <PathNode> path = new List <PathNode>(); for (int i = 0; i < graph.Count; i++) { for (int j = i + 1; j < graph.Count; j++) { PathArc arc = GetArc(graph[i], graph[j]); if (arc.Equals(InvalidArc)) { continue; } bool necessary = false; foreach (MobilityType mobility in MobilityType.MobilityTypes) { if (arc.time[mobility.Index] == Pathfinder.Forever) { continue; } float time = FindPath(path, Position(graph[i]), Position(graph[j]), mobility, 0f, MoveCommandType.Fast); if (arc.time[mobility.Index] < 1.5 * time) { necessary = true; break; } } if (!necessary) { RemoveArc(graph[i], graph[j]); } } } }