RoadGraphEdge[] TraverseEdgeBlock(RoadGraphEdge startEdge, HashSet <int> edgeVisited)
{
var edges = new List <RoadGraphEdge>();
RoadGraphEdge edge = startEdge;
while (edge != null && !edgeVisited.Contains(edge.edgeId))
{
edges.Add(edge);
edgeVisited.Add(edge.edgeId);
// Move to the next edge
var owningNode = graphNodes[edge.ownerNodeId];
var otherNode = graphNodes[edge.otherNodeId];
// Find the opposite edge
RoadGraphEdge nextEdge = null;
int numOtherNodeEdges = otherNode.adjacentEdges.Length;
for (int i = 0; i < numOtherNodeEdges; i++)
{
var otherNodeEdge = otherNode.adjacentEdges[i];
if (otherNodeEdge.otherNodeId == owningNode.nodeId)
{
// This is the opposite edge. Grab the adjacent one
nextEdge = otherNode.adjacentEdges[(i + 1) % numOtherNodeEdges];
}
}
edge = nextEdge;
}
return(edges.ToArray());
}
private static int SortEdgesByAngle(RoadGraphEdge a, RoadGraphEdge b)
{
if (a.angleToXAxis == b.angleToXAxis)
{
return(0);
}
return(a.angleToXAxis < b.angleToXAxis ? -1 : 1);
}
public static RoadGraph BakeRoadGraph(IEnumerable <RoadGraphBuildNode> buildNodes)
{
var bakedNodes = new List <RoadGraphNode>();
int _edgeIdCounter = 0;
foreach (var node in buildNodes)
{
var bakedNode = new RoadGraphNode();
bakedNode.nodeId = node.nodeId;
bakedNode.position = node.position;
var edges = new List <RoadGraphEdge>();
for (int i = 0; i < node.connectedNodes.Count; i++)
{
var connectedNode = node.connectedNodes[i];
var edgeThickness = node.edgeThickness[i];
var edge = new RoadGraphEdge();
edge.edgeId = ++_edgeIdCounter;
edge.ownerNodeId = node.nodeId;
edge.otherNodeId = connectedNode.nodeId;
edge.thickness = edgeThickness;
// Find the dot product with the x axis
Vector3 direction = (connectedNode.position - node.position).normalized;
var dot = Vector3.Dot(new Vector3(1, 0, 0), direction);
float angle = Mathf.Acos(dot);
if (direction.z < 0)
{
angle = 2 * Mathf.PI - angle;
}
edge.angleToXAxis = angle;
edges.Add(edge);
}
bakedNode.adjacentEdges = edges.ToArray();
System.Array.Sort(bakedNode.adjacentEdges, SortEdgesByAngle);
bakedNodes.Add(bakedNode);
}
var graph = new RoadGraph();
graph.nodes = bakedNodes.ToArray();
return(graph);
}
