static bool TryAdvanceThroughJunction(Junction junction, RoadNetworkDescription network,
ref TraversalState state, bool allowLooping = false)
{
var roadId = state.RoadId.ToString();
// TODO: This search currently terminates on the first found linked road/lane, but we may want to find
// all of them first and then select one based on some criteria
foreach (var connection in junction.connections)
{
if (roadId == connection.incomingRoadId)
{
if (!allowLooping && state.AllRoadIds.HasBeenTraversed(new NativeString64(connection.incomingRoadId)))
{
return(false);
}
foreach (var link in connection.laneLinks)
{
if (link.laneIdFrom != state.LaneId)
{
continue;
}
var direction = DetermineNewDirection(connection.contactPoint);
var idx = direction == TraversalDirection.Forward
? 0
: network.GetRoadById(connection.connectingRoadId).laneSections.Count - 1;
state.SetNewLocation(new NativeString64(connection.connectingRoadId), idx, link.laneIdTo, direction);
return(true);
}
}
else if (roadId == connection.connectingRoadId)
{
if (!allowLooping && state.AllRoadIds.HasBeenTraversed(new NativeString64(connection.connectingRoadId)))
{
return(false);
}
foreach (var link in connection.laneLinks)
{
if (link.laneIdTo != state.LaneId)
{
continue;
}
var nextRoad = network.GetRoadById(connection.incomingRoadId);
var sectionIdx = nextRoad.laneSections.Count - 1;
state.SetNewLocation(new NativeString64(connection.incomingRoadId), sectionIdx, link.laneIdFrom, TraversalDirection.Backward);
return(true);
}
}
}
return(false);
}
// Each non-junction road is effectively a segment of a graph edge - this function identifies which roads
// belong on the same edge as the input graph edge and return the information necessary to query this edge
internal static RoadGroup IdentifyGraphEdgeGroup(RoadNetworkDescription roadNetwork,
TraversalState state, NativeString64 roadId)
{
if (roadNetwork.GetRoadById(roadId).junction != "-1")
{
throw new ArgumentException(
"Cannot collect graph edge group for a road inside a junction - roads inside junctions" +
"are part of a graph node.");
}
// Because we want to limit traversal to this graph edge, we will always stop at junctions (nodes)
const bool stopAtJunctions = true;
var param = new TraversalParameters(roadNetwork, stopAtJunctions);
// Count how many Road elements are in this group, including the starting road
var numRoads = 1;
state.SetNewLocation(roadId, 0, 0, TraversalDirection.Backward);
while (TryAdvanceOneRoad(param, state))
{
numRoads++;
}
// We've moved all the way to the "front" of this collection of roads - store it as the starting location
var startingRoadId = state.RoadId;
// To get from the first Road to the second, we need to travel in the opposite direction we traversed to
// reach the first Road
var startDirection = (TraversalDirection)(-(int)state.Direction);
// Traverse forward to ensure we mark all the roads in this group as traversed
state.SetNewLocation(roadId, 0, 0, TraversalDirection.Forward);
while (TryAdvanceOneRoad(param, state))
{
numRoads++;
}
return(new RoadGroup(startDirection, startingRoadId, numRoads));
}
// A Road shares a graph edge with another linked element if that element is a Road which is not part of
// a set of junction roads
private static bool IsLinkOnGraphEdge(RoadNetworkDescription roadNetwork, RoadLink link)
{
return(!(link.linkType == RoadLinkType.None || link.linkType == RoadLinkType.Junction ||
roadNetwork.GetRoadById(link.nodeId).junction != "-1"));
}
// TODO: This should be modified to take a TraversalParams struct instead of taking the network directly
internal static bool TryAdvanceOneLaneSection(RoadNetworkDescription network, ref TraversalState state,
bool allowLooping = false)
{
var roadCurrent = network.GetRoadById(state.RoadId);
var laneCurrent = roadCurrent.laneSections[state.LaneSectionIdx].GetLane(state.LaneId);
var laneIdNext = GetNextLaneId(laneCurrent, state.Direction);
// If there are more lane sections, we haven't reached the end of the road
if (HasMoreLaneSectionsInDirection(roadCurrent, state))
{
// Check that the current lane exists in this next road section
if (laneIdNext == 0)
{
Debug.Log($"Lane {state.LaneId} on Road {state.RoadId} ends after section {state.LaneSectionIdx}");
return(false);
}
var sectionIdxNext = state.LaneSectionIdx + (int)state.Direction;
// Debug.Log($"Moving to section {sectionIdxNext} lane {laneIdNext} in road {state.RoadId}");
// Since we're staying in the same road, we preserve the traverse travelDirection
state = new TraversalState(network, state.RoadId, sectionIdxNext, laneIdNext, state.Direction);
if (!roadCurrent.laneSections[state.LaneSectionIdx].HasLane(state.LaneId))
{
throw new Exception($"Expected road {state.RoadId} to have lane {state.LaneId}.");
}
return(true);
}
var roadLink = GetLinkToNextRoad(roadCurrent, state.Direction);
// Check to see if road terminates
if (roadLink.linkType == RoadLinkType.None)
{
Debug.Log($"Road {state.RoadId} ends in travelDirection {state.Direction}");
return(false);
}
if (roadLink.linkType == RoadLinkType.Road)
{
// If the road has a link, it should be guaranteed to have a valid LinkContactPoint as well
Debug.Assert(roadLink.contactPoint == LinkContactPoint.Start ||
roadLink.contactPoint == LinkContactPoint.End,
$"Road {state.RoadId} road link contact point was not set correctly.'");
if (laneIdNext == 0)
{
Debug.Log($"Lane {state.LaneId} on Road {state.RoadId} ends in travelDirection {state.Direction}");
return(false);
}
var roadIdNext = roadLink.nodeId;
var roadNext = network.GetRoadById(roadIdNext);
var directionNext = roadLink.contactPoint == LinkContactPoint.Start ? TraversalDirection.Forward : TraversalDirection.Backward;
var sectionIdxNext = directionNext == TraversalDirection.Forward ? 0 : roadNext.laneSections.Count - 1;
var laneSectionNext = roadNext.laneSections[sectionIdxNext];
if (!laneSectionNext.HasLane(laneIdNext))
{
throw new Exception($"Expected {roadNext.roadId} to have lane {laneIdNext}.");
}
if (state.AllRoadIds.HasBeenTraversed(new NativeString64(roadIdNext)) && !allowLooping)
{
return(false);
}
// Debug.Log($"Moving to road {roadIdNext} section {sectionIdxNext} lane {laneIdNext}");
state.SetNewLocation(new NativeString64(roadIdNext), sectionIdxNext, laneIdNext, directionNext);
}
else if (roadLink.linkType == RoadLinkType.Junction)
{
var junction = network.GetJunctionById(roadLink.nodeId);
if (!TryAdvanceThroughJunction(junction, network, ref state, allowLooping))
{
Debug.Log($"Found no connected lanes for lane {state.LaneId} on road {state.RoadId} in junction {junction.junctionId}");
return(false);
}
}
return(true);
}
