void UpdateStopLines()
{
foreach (var entry in Id2StopLine)
{
var stopLine = entry.Value;
// Compute intersecting lanes
var intersectingLanes = GetOrderedIntersectingLanes(stopLine);
if (intersectingLanes.Count == 0)
{
continue; // No intersecting lanes found for this stop line.
}
var firstMapLanePositions = intersectingLanes[0].mapWorldPositions;
var laneDir = (firstMapLanePositions.Last() - firstMapLanePositions[firstMapLanePositions.Count - 2]).normalized;
// Compute points based on lane end points
var endPoints = intersectingLanes.Select(lane => lane.mapWorldPositions.Last()).ToList();
var newStopLinePositions = new List <Vector3>();
for (int i = 0; i < endPoints.Count; i++)
{
var endPoint = endPoints[i];
var normalDir = Vector3.Cross(laneDir, Vector3.up).normalized;
var halfLaneWidth = 2;
newStopLinePositions.Add(endPoint + normalDir * halfLaneWidth - laneDir * 0.5f);
if (i == endPoints.Count - 1)
{
newStopLinePositions.Add(endPoint - normalDir * halfLaneWidth - laneDir * 0.5f);
}
}
// Update stop line mapWorldPositions with new computed points
stopLine.mapWorldPositions = newStopLinePositions;
stopLine.transform.position = Lanelet2MapImporter.GetAverage(newStopLinePositions);
UpdateLocalPositions(stopLine);
}
}
void UpdateObjPosAndLocalPos(Transform objectTransform, MapDataPoints mapDataPoints)
{
var ObjPosition = Lanelet2MapImporter.GetAverage(mapDataPoints.mapWorldPositions);
objectTransform.position = ObjPosition;
// Update child local positions after parent position changed
UpdateLocalPositions(mapDataPoints);
}
void UpdateObjPosAndLocalPos(Transform objectTransform, MapDataPoints mapDataPoints)
{
var ObjPosition = Lanelet2MapImporter.GetAverage(mapDataPoints.mapWorldPositions);
objectTransform.position = ObjPosition;
// Update child local positions after parent position changed
mapDataPoints.mapLocalPositions.Clear();
mapDataPoints.mapLocalPositions = mapDataPoints.mapWorldPositions.Select(p => p - ObjPosition).ToList();
}
void MoveLaneLines(List <string> laneIds, GameObject mapLaneSection)
{
var lanePositions = new List <Vector3>();
foreach (var laneId in laneIds)
{
var lane = Id2Lane[laneId];
lane.transform.parent = mapLaneSection.transform;
UpdateObjPosAndLocalPos(lane.transform, lane);
lanePositions.Add(lane.transform.position);
var leftLine = Id2LeftLineBoundary[laneId];
leftLine.transform.parent = mapLaneSection.transform;
UpdateObjPosAndLocalPos(leftLine.transform, leftLine);
var rightLine = Id2RightLineBoundary[laneId];
rightLine.transform.parent = mapLaneSection.transform;
UpdateObjPosAndLocalPos(rightLine.transform, rightLine);
}
// Update mapLaneSection transform based on all lanes and update all lane's positions
mapLaneSection.transform.position = Lanelet2MapImporter.GetAverage(lanePositions);
UpdateChildrenPositions(mapLaneSection);
}
