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]);
}
}
}
}
