private static IEnumerable <IncomingRoadData> ComputeRoadAngles(RoadTopologyNode node, IEnumerable <RoadTopologyEdge> edges)
{
IncomingRoadData GetIncomingRoadData(RoadTopologyNode node, RoadTopologyEdge incomingEdge)
{
var targetNodePosition = incomingEdge.Start.Position == node.Position ? incomingEdge.End.Position : incomingEdge.Start.Position;
var roadVector = targetNodePosition - node.Position;
var direction = roadVector.LengthSquared() < 0.01f ? Vector3.UnitX : Vector3.Normalize(roadVector);
return(new IncomingRoadData(
incomingEdge,
targetNodePosition,
direction,
MathF.Atan2(direction.Y, direction.X)));
}
var incomingRoads = edges
.Select(e => GetIncomingRoadData(node, e))
.OrderBy(d => d.AngleToAxis)
.ToList();
for (var i = 1; i < incomingRoads.Count; ++i)
{
incomingRoads[i].Previous = incomingRoads[i - 1];
incomingRoads[i].AngleToPreviousEdge = incomingRoads[i].AngleToAxis - incomingRoads[i - 1].AngleToAxis;
}
incomingRoads[0].Previous = incomingRoads[incomingRoads.Count - 1];
incomingRoads[0].AngleToPreviousEdge = 2 * MathF.PI + incomingRoads[0].AngleToAxis - incomingRoads[incomingRoads.Count - 1].AngleToAxis;
return(incomingRoads);
}
private static IReadOnlyList <IncomingRoadData> ComputeRoadAngles(RoadTopologyNode node, IEnumerable <RoadTopologyEdge> edges, IReadOnlyDictionary <RoadTopologyEdge, StraightRoadSegment> edgeSegments)
{
var incomingRoads = edges
.Select(e => GetIncomingRoadData(node, e, edgeSegments[e]))
.GroupBy(d => d.AngleToAxis) // treat road segments coming in at the same angle as one
.Select(g => g.FirstOrDefault(d => d.TargetEndPoint.To != null) ?? g.First())
.OrderBy(d => d.AngleToAxis)
.ToList();
if (incomingRoads.Count < 2)
{
return(Array.Empty <IncomingRoadData>());
}
for (var i = 1; i < incomingRoads.Count; ++i)
{
incomingRoads[i].Previous = incomingRoads[i - 1];
incomingRoads[i].AngleToPreviousEdge = incomingRoads[i].AngleToAxis - incomingRoads[i - 1].AngleToAxis;
}
incomingRoads[0].Previous = incomingRoads[incomingRoads.Count - 1];
incomingRoads[0].AngleToPreviousEdge = 2 * MathF.PI + incomingRoads[0].AngleToAxis - incomingRoads[incomingRoads.Count - 1].AngleToAxis;
return(incomingRoads);
}
private RoadTopologyNode GetOrCreateNode(Vector3 position)
{
var node = Nodes.Find(x => x.Position == position);
if (node == null)
{
Nodes.Add(node = new RoadTopologyNode(position));
}
return(node);
}
private static IncomingRoadData GetIncomingRoadData(RoadTopologyNode node, RoadTopologyEdge incomingEdge)
{
var isStart = incomingEdge.Start.Position == node.Position;
var targetNodePosition = isStart ? incomingEdge.End.Position : incomingEdge.Start.Position;
var roadVector = targetNodePosition - node.Position;
var direction = roadVector.LengthSquared() < 0.01f ? Vector3.UnitX : Vector3.Normalize(roadVector);
return(new IncomingRoadData(
incomingEdge,
targetNodePosition,
direction,
MathF.Atan2(direction.Y, direction.X)));
}
private static IncomingRoadData GetIncomingRoadData(RoadTopologyNode node, RoadTopologyEdge incomingEdge, StraightRoadSegment edgeSegment)
{
var isStart = incomingEdge.Start.Position == node.Position;
var fromPosition = isStart ? edgeSegment.EndPosition : edgeSegment.StartPosition;
var segmentVector = (edgeSegment.EndPosition - edgeSegment.StartPosition) * (isStart ? 1 : -1);
var direction = segmentVector.LengthSquared() < 0.01f ? Vector3.UnitX : Vector3.Normalize(segmentVector);
return(new IncomingRoadData(
incomingEdge,
fromPosition,
direction,
MathF.Atan2(direction.Y, direction.X)));
}
private static IReadOnlyList <IncomingRoadData> ComputeRoadAngles(RoadTopologyNode node, IEnumerable <RoadTopologyEdge> edges)
{
if (edges.Count() < 2)
{
return(Array.Empty <IncomingRoadData>());
}
var incomingRoads = edges
.Select(e => GetIncomingRoadData(node, e))
.OrderBy(d => d.AngleToAxis)
.ToList();
for (var i = 1; i < incomingRoads.Count; ++i)
{
incomingRoads[i].Previous = incomingRoads[i - 1];
incomingRoads[i].AngleToPreviousEdge = incomingRoads[i].AngleToAxis - incomingRoads[i - 1].AngleToAxis;
}
incomingRoads[0].Previous = incomingRoads[incomingRoads.Count - 1];
incomingRoads[0].AngleToPreviousEdge = 2 * MathF.PI + incomingRoads[0].AngleToAxis - incomingRoads[incomingRoads.Count - 1].AngleToAxis;
return(incomingRoads);
}