// Join and convert a set of singlely-connected segments and also setup world positions for all segments
static void ConvertAndJointSingleConnectedSegments(MapVectorLane curSeg, HashSet <MapVectorLane> laneSegmentsSet, ref HashSet <MapVectorLane> newLaneSegmentsSet, HashSet <MapVectorLane> visitedSegs)
{
var combSegs = new List <MapVectorLane>();
visitedSegs.Add(curSeg);
combSegs.Add(curSeg);
while (curSeg.Afters.Count == 1 && curSeg.Afters[0].Befores.Count == 1 && curSeg.Afters[0].Befores[0] == curSeg && !visitedSegs.Contains((MapVectorLane)(curSeg.Afters[0])))
{
visitedSegs.Add(curSeg.Afters[0]);
combSegs.Add(curSeg.Afters[0]);
curSeg = curSeg.Afters[0];
}
//construct new lane segments in with its wappoints in world positions and update related dependencies to relate to new segments
MapVectorLane combinedSeg = new MapVectorLane();
combinedSeg.LaneCount = curSeg.LaneCount;
combinedSeg.LaneNumber = curSeg.LaneNumber;
combinedSeg.SpeedLimit = curSeg.SpeedLimit;
combinedSeg.Befores = combSegs[0].Befores;
combinedSeg.Afters = combSegs[combSegs.Count - 1].Afters;
foreach (var beforeSeg in combinedSeg.Befores)
{
for (int i = 0; i < beforeSeg.Afters.Count; i++)
{
if (beforeSeg.Afters[i] == combSegs[0])
{
beforeSeg.Afters[i] = combinedSeg;
}
}
}
foreach (var afterSeg in combinedSeg.Afters)
{
for (int i = 0; i < afterSeg.Befores.Count; i++)
{
if (afterSeg.Befores[i] == combSegs[combSegs.Count - 1])
{
afterSeg.Befores[i] = combinedSeg;
}
}
}
// Note: no local positions for combinedSeg
foreach (var seg in combSegs)
{
foreach (var worldPos in seg.MapWorldPositions)
{
combinedSeg.MapWorldPositions.Add(worldPos); //Convert to world position
}
}
foreach (var seg in combSegs)
{
laneSegmentsSet.Remove(seg);
}
newLaneSegmentsSet.Add(combinedSeg);
}
bool Calculate()
{
// Initial collection
var laneSegments = new List <MapLane>();
var lineSegments = new List <MapLine>();
var signalLightPoles = new List <MapPole>();
laneSegments.AddRange(MapAnnotationData.GetData <MapLane>());
lineSegments.AddRange(MapAnnotationData.GetData <MapLine>());
signalLightPoles.AddRange(MapAnnotationData.GetData <MapPole>());
var allVectorLaneSegments = new HashSet <MapVectorLane>();
var allLineSegments = new HashSet <MapLine>();
foreach (var laneSegment in laneSegments)
{
var vectorLaneSegment = new MapVectorLane();
vectorLaneSegment.copyFromMapLane(laneSegment);
allVectorLaneSegments.Add(vectorLaneSegment);
}
// Link before and after segment for each lane segment
foreach (var laneSegment in allVectorLaneSegments)
{
// Each segment must have at least 2 waypoints for calculation, otherwise exit
while (laneSegment.MapWorldPositions.Count < 2)
{
Debug.LogError("Some segment has less than 2 waypoints. Cancelling map generation.");
return(false);
}
// Link lanes
var firstPt = laneSegment.MapWorldPositions[0];
var lastPt = laneSegment.MapWorldPositions[laneSegment.MapWorldPositions.Count - 1];
foreach (var laneSegmentCmp in allVectorLaneSegments)
{
if (laneSegment == laneSegmentCmp)
{
continue;
}
var firstPt_cmp = laneSegmentCmp.MapWorldPositions[0];
var lastPt_cmp = laneSegmentCmp.MapWorldPositions[laneSegmentCmp.MapWorldPositions.Count - 1];
if ((firstPt - lastPt_cmp).magnitude < MapAnnotationTool.PROXIMITY / MapAnnotationTool.EXPORT_SCALE_FACTOR)
{
laneSegment.Befores.Add(laneSegmentCmp);
}
if ((lastPt - firstPt_cmp).magnitude < MapAnnotationTool.PROXIMITY / MapAnnotationTool.EXPORT_SCALE_FACTOR)
{
laneSegment.Afters.Add(laneSegmentCmp);
}
}
}
foreach (var lineSegment in lineSegments)
{
if (lineSegment.lineType != MapData.LineType.VIRTUAL)
{
allLineSegments.Add(lineSegment);
}
}
// Link before and after segment for each line segment
foreach (var lineSegment in allLineSegments)
{
if (lineSegment.lineType != MapData.LineType.STOP)
{
continue; // Skip stop lines
}
// Each segment must have at least 2 waypoints for calculation, otherwise exit
while (lineSegment.mapLocalPositions.Count < 2)
{
Debug.LogError("Some segment has less than 2 waypoints. Cancelling map generation.");
return(false);
}
// Link lanes
var firstPt = lineSegment.transform.TransformPoint(lineSegment.mapLocalPositions[0]);
var lastPt = lineSegment.transform.TransformPoint(lineSegment.mapLocalPositions[lineSegment.mapLocalPositions.Count - 1]);
foreach (var lineSegmentCmp in allLineSegments)
{
if (lineSegment == lineSegmentCmp)
{
continue;
}
if (lineSegmentCmp.lineType != MapData.LineType.STOP)
{
continue; // Skip stop lines
}
var firstPt_cmp = lineSegmentCmp.transform.TransformPoint(lineSegmentCmp.mapLocalPositions[0]);
var lastPt_cmp = lineSegmentCmp.transform.TransformPoint(lineSegmentCmp.mapLocalPositions[lineSegmentCmp.mapLocalPositions.Count - 1]);
if ((firstPt - lastPt_cmp).magnitude < MapAnnotationTool.PROXIMITY / MapAnnotationTool.EXPORT_SCALE_FACTOR)
{
lineSegmentCmp.mapLocalPositions[lineSegmentCmp.mapLocalPositions.Count - 1] = lineSegmentCmp.transform.InverseTransformPoint(firstPt);
lineSegmentCmp.mapWorldPositions[lineSegmentCmp.mapWorldPositions.Count - 1] = firstPt;
}
if ((lastPt - firstPt_cmp).magnitude < MapAnnotationTool.PROXIMITY / MapAnnotationTool.EXPORT_SCALE_FACTOR)
{
lineSegmentCmp.mapLocalPositions[0] = lineSegmentCmp.transform.InverseTransformPoint(lastPt);
lineSegmentCmp.mapWorldPositions[0] = lastPt;
}
}
}
// New sets for newly converted(to world space) segments
var allConvertedLaneSegments = new HashSet <MapVectorLane>(); //for lane segments
// Filter and convert all lane segments
if (allVectorLaneSegments.Count > 0)
{
var startLaneSegs = new HashSet <MapVectorLane>(); // The lane segments that are at merging or forking or starting position
var visitedLaneSegs = new HashSet <MapVectorLane>(); // tracking for record
foreach (var laneSegment in allVectorLaneSegments)
{
if (laneSegment.Befores.Count != 1 || (laneSegment.Befores.Count == 1 && laneSegment.Befores[0].Afters.Count > 1)) //no any before segments
{
startLaneSegs.Add(laneSegment);
}
}
foreach (var startLaneSeg in startLaneSegs)
{
ConvertAndJointSingleConnectedSegments(startLaneSeg, allVectorLaneSegments, ref allConvertedLaneSegments, visitedLaneSegs);
}
while (allVectorLaneSegments.Count > 0) // Remaining should be isolated loops
{
MapVectorLane pickedLaneSeg = null;
foreach (var laneSegment in allVectorLaneSegments)
{
pickedLaneSeg = laneSegment;
break;
}
if (pickedLaneSeg != null)
{
ConvertAndJointSingleConnectedSegments(pickedLaneSeg, allVectorLaneSegments, ref allConvertedLaneSegments, visitedLaneSegs);
}
}
}
// Set up all lanes vector map data
var laneSegTerminalIDsMapping = new Dictionary <MapVectorLane, int[]>(); //tracking for record
if (allConvertedLaneSegments.Count > 0)
{
foreach (var laneSegment in allConvertedLaneSegments)
{
List <Vector3> positions;
var laneCount = laneSegment.LaneCount;
var laneNumber = laneSegment.LaneNumber;
//Interpolate based on waypoint world positions
Interpolate(laneSegment.MapWorldPositions, out positions, 1.0f / MapAnnotationTool.EXPORT_SCALE_FACTOR, true); //interpolate and divide to ensure 1 meter apart
laneSegTerminalIDsMapping.Add(laneSegment, new int[2]);
for (int i = 0; i < positions.Count; i++)
{
//Point
var pos = positions[i];
var vectorMapPos = VectorMapUtility.GetVectorMapPosition(pos, MapAnnotationTool.EXPORT_SCALE_FACTOR);
var vmPoint = Point.MakePoint(Points.Count + 1, vectorMapPos.Bx, vectorMapPos.Ly, vectorMapPos.H);
Points.Add(vmPoint);
//Node
var vmNode = Node.MakeNode(Nodes.Count, Points.Count + 1);
Nodes.Add(vmNode);
//DtLane
var deltaDir = Vector3.zero;
if (i < positions.Count - 1)
{
deltaDir = positions[i + 1] - pos;
}
else
{
if (laneSegment.Afters.Count > 0)
{
deltaDir = laneSegment.Afters[0].MapWorldPositions[0] - pos;
}
else
{
deltaDir = pos - positions[i - 1];
}
}
Vector3 eulerAngles = Quaternion.FromToRotation(Vector3.right, deltaDir).eulerAngles;
var convertedEulerAngles = VectorMapUtility.GetRvizCoordinates(eulerAngles);
var vmDTLane = DtLane.MakeDtLane(DtLanes.Count + 1, i, vmPoint.PID, -convertedEulerAngles.z * Mathf.Deg2Rad, .0, .1, .1);
DtLanes.Add(vmDTLane);
//Lane
int beforeLaneID = 0;
int laneId = Lanes.Count + 1;
if (i > 0)
{
beforeLaneID = Lanes.Count; // beforeLaneID won't be 0 since we add one vmLane to Lanes when i == 0
var beforeLane = Lanes[beforeLaneID - 1];
beforeLane.FLID = laneId;
Lanes[beforeLaneID - 1] = beforeLane; //This is needed for struct copy to be applied back
}
var speedLimit = (int)laneSegment.SpeedLimit;
var vmLane = Lane.MakeLane(laneId, vmDTLane.DID, beforeLaneID, 0, (Nodes.Count - 1), Nodes.Count, laneCount, laneNumber, speedLimit);
Lanes.Add(vmLane); // if positions.Count is n, then Lanes.Count is n-1.
if (i == 0)
{
laneSegTerminalIDsMapping[laneSegment][(int)TerminalType.START] = vmLane.LnID;
}
if (i == positions.Count - 1)
{
laneSegTerminalIDsMapping[laneSegment][(int)TerminalType.END] = vmLane.LnID;
}
}
}
//Assuming merging and diversing won't happen at the same spot
//correcting start and end lanes's BLID and FLID to the lane segment's before and after lane IDs in other lane segments
//each lane will have up to 4 before lane Ids and after lane Ids to set, later executed operation will be ignore if number exceeds 4
//correcting DtLanes last lane's dir value
foreach (var laneSegment in allConvertedLaneSegments)
{
var terminalIdPair = laneSegTerminalIDsMapping[laneSegment];
if (laneSegment.Befores.Count > 0)
{
var segStartId = terminalIdPair[(int)TerminalType.START];
var ln = Lanes[segStartId - 1];
for (int i = 0; i < laneSegment.Befores.Count; i++)
{
int beforeSegAfterLn = laneSegTerminalIDsMapping[laneSegment.Befores[i]][(int)TerminalType.END];
//chose the BLID that has not been set to meaningful lane id
if (ln.BLID == 0)
{
ln.BLID = beforeSegAfterLn;
}
else if (ln.BLID2 == 0)
{
ln.BLID2 = beforeSegAfterLn;
//ln.JCT = 3; //means merging
}
else if (ln.BLID3 == 0)
{
ln.BLID3 = beforeSegAfterLn;
}
else if (ln.BLID4 == 0)
{
ln.BLID4 = beforeSegAfterLn;
}
}
Lanes[segStartId - 1] = ln;
}
if (laneSegment.Afters.Count > 0)
{
var segEndId = terminalIdPair[(int)TerminalType.END];
var ln = Lanes[segEndId - 1];
int afterSegStartLn = laneSegTerminalIDsMapping[laneSegment.Afters[0]][(int)TerminalType.START];
for (int i = 0; i < laneSegment.Afters.Count; i++)
{
afterSegStartLn = laneSegTerminalIDsMapping[laneSegment.Afters[i]][(int)TerminalType.START];
//chose the FLID that has not been set to meaningful lane id
if (ln.FLID == 0)
{
ln.FLID = afterSegStartLn;
}
else if (ln.FLID2 == 0)
{
ln.FLID2 = afterSegStartLn;
ln.JCT = 1; //means branching
}
else if (ln.FLID3 == 0)
{
ln.FLID3 = afterSegStartLn;
}
else if (ln.FLID4 == 0)
{
ln.FLID4 = afterSegStartLn;
}
}
Lanes[segEndId - 1] = ln;
// Adjust last dtlane of each lane segment
var endDtLn = DtLanes[Lanes[segEndId - 1].DID - 1];
var DtLnAfter = DtLanes[Lanes[afterSegStartLn - 1].DID - 1];
var pointPos = VectorMapUtility.GetUnityPosition(new VectorMapPosition()
{
Bx = Points[endDtLn.PID - 1].Bx, Ly = Points[endDtLn.PID - 1].Ly, H = Points[endDtLn.PID - 1].H
}, MapAnnotationTool.EXPORT_SCALE_FACTOR);
var pointAfterPos = VectorMapUtility.GetUnityPosition(new VectorMapPosition()
{
Bx = Points[DtLnAfter.PID - 1].Bx, Ly = Points[DtLnAfter.PID - 1].Ly, H = Points[DtLnAfter.PID - 1].H
}, MapAnnotationTool.EXPORT_SCALE_FACTOR);
var deltaDir = pointAfterPos - pointPos;
Vector3 eulerAngles = Quaternion.FromToRotation(Vector3.right, deltaDir).eulerAngles;
var convertedEulerAngles = VectorMapUtility.GetRvizCoordinates(eulerAngles);
endDtLn.Dir = -convertedEulerAngles.z * Mathf.Deg2Rad;
DtLanes[endDtLn.DID - 1] = endDtLn;
}
}
}
// Set up all lines vector map data
var lineSegTerminalIDsMapping = new Dictionary <MapLine, KeyValuePair <int, LineType>[]>(); //tracking for record
var stoplineLinkIDMapping = new Dictionary <MapLine, List <int> >();
if (allLineSegments.Count > 0)
{
foreach (var lineSegment in allLineSegments)
{
var lineType = LineType.NONE;
if (lineSegment.lineType == MapData.LineType.STOP) //If it is stopline
{
lineType = LineType.STOP;
}
else if (lineSegment.lineType == MapData.LineType.SOLID_WHITE || lineSegment.lineType == MapData.LineType.DOUBLE_WHITE || lineSegment.lineType == MapData.LineType.DOTTED_WHITE)
{
lineType = LineType.WHITE;
}
else if (lineSegment.lineType == MapData.LineType.SOLID_YELLOW || lineSegment.lineType == MapData.LineType.DOUBLE_YELLOW || lineSegment.lineType == MapData.LineType.DOTTED_YELLOW)
{
lineType = LineType.YELLOW;
}
var positions = lineSegment.mapWorldPositions;
lineSegTerminalIDsMapping.Add(lineSegment, new KeyValuePair <int, LineType>[2] {
new KeyValuePair <int, LineType>(-1, LineType.NONE), new KeyValuePair <int, LineType>(-1, LineType.NONE)
});
for (int i = 0; i < positions.Count - 1; i++)
{
//Point
var startPos = positions[i];
var endPos = positions[i + 1];
var vectorMapPos = VectorMapUtility.GetVectorMapPosition(startPos, MapAnnotationTool.EXPORT_SCALE_FACTOR);
var vmStartPoint = Point.MakePoint(Points.Count + 1, vectorMapPos.Bx, vectorMapPos.Ly, vectorMapPos.H);
Points.Add(vmStartPoint);
vectorMapPos = VectorMapUtility.GetVectorMapPosition(endPos, MapAnnotationTool.EXPORT_SCALE_FACTOR);
var vmEndPoint = Point.MakePoint(Points.Count + 1, vectorMapPos.Bx, vectorMapPos.Ly, vectorMapPos.H);
Points.Add(vmEndPoint);
//Line
int beforeLineID = 0;
if (i > 0)
{
beforeLineID = Lines.Count;
var beforeLine = Lines[beforeLineID - 1];
beforeLine.FLID = Lines.Count + 1;
Lines[beforeLineID - 1] = beforeLine; //This is needed for struct copy to be applied back
}
var vmLine = Line.MakeLine(Lines.Count + 1, vmStartPoint.PID, vmEndPoint.PID, beforeLineID, 0);
Lines.Add(vmLine);
if (i == 0)
{
lineSegTerminalIDsMapping[lineSegment][(int)TerminalType.START] = new KeyValuePair <int, LineType>(vmLine.LID, lineType);
}
if (i == positions.Count - 2)
{
lineSegTerminalIDsMapping[lineSegment][(int)TerminalType.END] = new KeyValuePair <int, LineType>(vmLine.LID, lineType);
}
int LinkID;
MakeVisualLine(vmLine, lineType, out LinkID);
if (lineType == LineType.STOP)
{
if (LinkID < 1)
{
Debug.LogError("Some stopline can not find have a correct linkID that equals to a nearby lane's id", lineSegment.gameObject);
UnityEditor.Selection.activeGameObject = lineSegment.gameObject;
Debug.LogError($"Selected the problematic {nameof(lineSegment)}");
return(false);
}
if (stoplineLinkIDMapping.ContainsKey(lineSegment))
{
stoplineLinkIDMapping[lineSegment].Add(LinkID);
//Debug.Log("Extra IDs for same builder: " + stopLinkIDMapping[builder].Count);
}
else
{
stoplineLinkIDMapping.Add(lineSegment, new List <int>()
{
LinkID
});
}
}
}
}
//correcting each segment's start and end line segments BLID and FLID to new line segments that is between each segment and their adjacent segment
//only make BLID/FLID set to only one of the new line segments' last/first line ID because in the vector map format one line only has one BLID and one FLID
//Later executed operation will overrite previously configured BLID and/or FLID
foreach (var lineSegment in allLineSegments)
{
var terminalIdPairs = lineSegTerminalIDsMapping[lineSegment];
if (lineSegment.befores.Count > 0 && lineSegTerminalIDsMapping.ContainsKey(lineSegment.befores[0])) //if before line doesn't exist in the record set then no operation to avoid extra in between line
{
var tPairs = terminalIdPairs[(int)TerminalType.START];
var segStartId = tPairs.Key;
var segStartLin = Lines[segStartId - 1];
var beforeSegEndLinId = lineSegTerminalIDsMapping[lineSegment.befores[0]][(int)TerminalType.END].Key; //only make BLID set to only one of the line segments' last line ID
var newLin = Line.MakeLine(Lines.Count + 1, Lines[beforeSegEndLinId - 1].FPID, segStartLin.BPID, beforeSegEndLinId, segStartLin.LID);
Lines.Add(newLin);
MakeVisualLine(newLin, tPairs.Value);
segStartLin.BLID = beforeSegEndLinId;
Lines[segStartId - 1] = segStartLin;
}
if (lineSegment.afters.Count > 0 && lineSegTerminalIDsMapping.ContainsKey(lineSegment.afters[0])) //if before line doesn't exist in the record set then no operation to avoid extra in between line
{
var tPairs = terminalIdPairs[(int)TerminalType.END];
var segEndId = tPairs.Key;
var segEndLin = Lines[segEndId - 1];
var afterSegStartLinId = lineSegTerminalIDsMapping[lineSegment.afters[0]][(int)TerminalType.START].Key; //only make FLID set to only one of the line segments' first line ID
var newLin = Line.MakeLine(Lines.Count + 1, segEndLin.FPID, Lines[afterSegStartLinId - 1].BPID, segEndLin.LID, afterSegStartLinId);
Lines.Add(newLin);
MakeVisualLine(newLin, tPairs.Value);
segEndLin.FLID = newLin.LID;
Lines[segEndId - 1] = segEndLin;
}
}
}
//Setup all traffic light poles and their corrsponding traffic lights
var tempIDMapping = new Dictionary <MapPole, int>(); //builder pole id mapping
foreach (var pole in signalLightPoles)
{
//Vector
var pos = pole.transform.position;
var vectorMapPos = VectorMapUtility.GetVectorMapPosition(pos, MapAnnotationTool.EXPORT_SCALE_FACTOR);
var PID = Points.Count + 1;
var vmPoint = Point.MakePoint(PID, vectorMapPos.Bx, vectorMapPos.Ly, vectorMapPos.H);
Points.Add(vmPoint);
var VID = Vectors.Count + 1;
float Vang = Vector3.Angle(pole.transform.forward, Vector3.up);
float Hang = .0f;
if (Vang != .0f)
{
var projectedHorizonVec = Vector3.ProjectOnPlane(pole.transform.forward, Vector3.up);
Hang = Vector3.Angle(projectedHorizonVec, Vector3.forward) * (Vector3.Cross(Vector3.forward, projectedHorizonVec).y > 0 ? 1 : -1);
}
var vmVector = Vector.MakeVector(VID, PID, Hang, Vang);
Vectors.Add(vmVector);
float Length = pole.length;
float Dim = .4f;
var PLID = Poles.Count + 1;
var vmPole = Pole.MakePole(PLID, VID, Length, Dim);
Poles.Add(vmPole);
tempIDMapping.Add(pole, PLID);
}
foreach (var pole in signalLightPoles)
{
var PLID = tempIDMapping[pole];
foreach (var signalLight in pole.signalLights)
{
if (signalLight == null || !signalLight.gameObject.activeInHierarchy)
{
continue;
}
var trafficLightAim = signalLight.transform.forward;
foreach (var lightData in signalLight.signalData)
{
//Vector
var trafficLightPos = signalLight.transform.TransformPoint(lightData.localPosition);
var vectorMapPos = VectorMapUtility.GetVectorMapPosition(trafficLightPos, MapAnnotationTool.EXPORT_SCALE_FACTOR);
var PID = Points.Count + 1;
var vmPoint = Point.MakePoint(PID, vectorMapPos.Bx, vectorMapPos.Ly, vectorMapPos.H);
Points.Add(vmPoint);
var VID = Vectors.Count + 1;
float Vang = Vector3.Angle(trafficLightAim, Vector3.up);
float Hang = .0f;
if (Vang != .0f)
{
var projectedHorizonVec = Vector3.ProjectOnPlane(trafficLightAim, Vector3.up);
Hang = Vector3.Angle(projectedHorizonVec, Vector3.forward) * (Vector3.Cross(Vector3.forward, projectedHorizonVec).y > 0 ? 1 : -1);
}
var vmVector = Vector.MakeVector(VID, PID, Hang, Vang);
Vectors.Add(vmVector);
//Signaldata
int ID = SignalDataList.Count + 1;
int Type = (int)lightData.signalColor;
int LinkID = -1;
if (signalLight.stopLine != null)
{
if (!stoplineLinkIDMapping.ContainsKey(signalLight.stopLine))
{
Debug.LogError("Selected the related stopline that is not in the mapping");
UnityEditor.Selection.activeGameObject = signalLight.stopLine.gameObject;
return(false);
}
LinkID = PickAimingLinkID(signalLight.transform, stoplineLinkIDMapping[signalLight.stopLine]);
if (LinkID < 0)
{
Debug.LogError("Selected the related stopline that is related to the missing LinkID");
UnityEditor.Selection.activeGameObject = signalLight.stopLine.gameObject;
return(false);
}
}
else
{
Debug.LogError($"some signal light({nameof(MapSignal)}) have null stopline");
UnityEditor.Selection.activeGameObject = signalLight.gameObject;
Debug.LogError($"selected the problematic {nameof(MapSignal)}");
return(false);
#if THIS_IS_NOT_USED
LinkID = FindProperStoplineLinkID(trafficLightPos, trafficLightAim);
#endif
}
if (Type == 2)
{
Type = 3;
}
else if (Type == 3)
{
Type = 2;
}
var vmSignalData = SignalData.MakeSignalData(ID, VID, PLID, Type, LinkID);
SignalDataList.Add(vmSignalData);
}
}
}
return(true);
}
