// Make sure stop line is intersecting with the correct lane, not an intersection lane.
void MoveBackIfOnIntersectionLane(string id, Tuple <string, double> overlapLaneIdStartS)
{
var laneId = overlapLaneIdStartS.Item1;
var startS = overlapLaneIdStartS.Item2;
if (LaneId2JunctionId.ContainsKey(laneId))
{
var stopLine = Id2StopLine[id];
var dir = -stopLine.transform.forward;
var befores = Id2Lane[laneId].befores;
// If multiple before lanes, hard to know where to move the stop line
if (befores.Count > 1)
{
Debug.LogWarning($"stopLine {id} might not in correct position, please check and move back yourself if necessary.");
}
// move 1 meter more over the last point of the predecessor lane
var sqrDist = Utility.SqrDistanceToSegment(stopLine.mapWorldPositions.First(),
stopLine.mapWorldPositions.Last(), befores[0].mapWorldPositions.Last());
var dist = math.sqrt(sqrDist) + 1;
stopLine.transform.position += dir * (float)dist;
stopLine.mapWorldPositions = stopLine.mapWorldPositions.Select(x => x + dir * (float)dist).ToList();
}
}
List <MapLane> GetOrderedIntersectingLanes(MapLine stopLine)
{
var stopLinePositions = stopLine.mapWorldPositions;
var intersectingLanes = new List <MapLane>();
foreach (var entry in Id2Lane)
{
var mapLane = entry.Value;
var positions = mapLane.mapWorldPositions;
// last two points of the lane
var p1 = positions[positions.Count - 2];
var p2 = positions.Last();
// check with every segment of the stop line
for (var i = 0; i < stopLinePositions.Count - 1; i++)
{
var p3 = stopLinePositions[i];
var p4 = stopLinePositions[i + 1];
var isIntersect = Utility.LineSegementsIntersect(ToVector2(p1), ToVector2(p2), ToVector2(p3), ToVector2(p4), out Vector2 intersection);
if (isIntersect)
{
intersectingLanes.Add(mapLane);
break;
}
}
}
if (intersectingLanes.Count == 0)
{
Debug.LogWarning($"stopLine {stopLine.name} have no intersecting lanes");
}
else if (intersectingLanes.Count == 1)
{
return(intersectingLanes);
}
else
{
intersectingLanes = OrderLanes(intersectingLanes);
}
return(intersectingLanes);
}
