void Calculate()
{
exportLists.ForEach(e => e.List.Clear()); //clear all vector map data before calculate
//list of target transforms
var targetList = new List <Transform>();
var noTarget = true;
foreach (var t in targets)
{
if (t != null)
{
noTarget = false;
targetList.Add(t);
}
}
if (noTarget)
{
targetList.Add(transform);
}
//initial collection
var segBldrs = new List <VectorMapSegmentBuilder>();
var signalLightPoles = new List <VectorMapPole>();
foreach (var t in targetList)
{
if (t == null)
{
continue;
}
var vmsb = t.GetComponentsInChildren <VectorMapSegmentBuilder>();
var vmp = t.GetComponentsInChildren <VectorMapPole>();
segBldrs.AddRange(vmsb);
signalLightPoles.AddRange(vmp);
}
bool missingPoints = false;
var allSegs = new HashSet <VectorMapSegment>(); //All segments regardless of segment actual type
//connect builder reference for each segment
foreach (var segBldr in segBldrs)
{
segBldr.segment.builder = segBldr;
allSegs.Add(segBldr.segment);
}
//Link before and after segment for each segment
foreach (var segment in allSegs)
{
//Make sure clear everything that might have data left over by previous generation
segment.befores.Clear();
segment.afters.Clear();
segment.targetWorldPositions.Clear();
//this is to avoid accidentally connect two nearby stoplines
if ((segment.builder as VectorMapStopLineSegmentBuilder) != null)
{
continue;
}
//each segment must have at least 2 waypoints for calculation so complement to 2 waypoints as needed
while (segment.targetLocalPositions.Count < 2)
{
segment.targetLocalPositions.Add(Vector3.zero);
missingPoints = true;
}
var firstPt = segment.builder.transform.TransformPoint(segment.targetLocalPositions[0]);
var lastPt = segment.builder.transform.TransformPoint(segment.targetLocalPositions[segment.targetLocalPositions.Count - 1]);
foreach (var segment_cmp in allSegs)
{
if (segment_cmp.builder.GetType() != segment.builder.GetType())
{
continue;
}
var firstPt_cmp = segment_cmp.builder.transform.TransformPoint(segment_cmp.targetLocalPositions[0]);
var lastPt_cmp = segment_cmp.builder.transform.TransformPoint(segment_cmp.targetLocalPositions[segment_cmp.targetLocalPositions.Count - 1]);
if ((firstPt - lastPt_cmp).magnitude < PROXIMITY)
{
segment.befores.Add(segment_cmp);
}
if ((lastPt - firstPt_cmp).magnitude < PROXIMITY)
{
segment.afters.Add(segment_cmp);
}
}
}
if (missingPoints)
{
Debug.Log("Some segment has less than 2 waypoints, complement it to 2");
}
var allLnSegs = new HashSet <VectorMapSegment>();
var allLinSegs = new HashSet <VectorMapSegment>();
foreach (var segment in allSegs)
{
if (segment.builder.GetType() == typeof(VectorMapLaneSegmentBuilder))
{
allLnSegs.Add(segment);
}
if (segment.builder.GetType() == typeof(VectorMapStopLineSegmentBuilder))
{
allLinSegs.Add(segment);
}
if (segment.builder.GetType() == typeof(VectorMapWhiteLineSegmentBuilder))
{
allLinSegs.Add(segment);
}
}
//New sets for newly converted(to world space) segments
var allConvertedLnSeg = new HashSet <VectorMapSegment>(); //for lane segments
var allConvertedLinSeg = new HashSet <VectorMapSegment>(); //for line segments
//Filter and convert all lane segments
if (allLnSegs.Count > 0)
{
var startLnSegs = new HashSet <VectorMapSegment>(); //The lane segments that are at merging or forking or starting position
var visitedLnSegs = new HashSet <VectorMapSegment>(); //tracking for record
foreach (var lnSeg in allLnSegs)
{
if (lnSeg.befores.Count != 1 || (lnSeg.befores.Count == 1 && lnSeg.befores[0].afters.Count > 1)) //no any before segments
{
startLnSegs.Add(lnSeg);
}
}
foreach (var startLnSeg in startLnSegs)
{
ConvertAndJointSegmentSet(startLnSeg, allLnSegs, allConvertedLnSeg, visitedLnSegs);
}
while (allLnSegs.Count > 0)//Remaining should be isolated loops
{
VectorMapSegment pickedSeg = null;
foreach (var lnSeg in allLnSegs)
{
pickedSeg = lnSeg;
break;
}
if (pickedSeg != null)
{
ConvertAndJointSegmentSet(pickedSeg, allLnSegs, allConvertedLnSeg, visitedLnSegs);
}
}
}
//convert all line segments
if (allLinSegs.Count > 0)
{
//setup world positions and their proper befores and afters for all line segments
foreach (var linSeg in allLinSegs)
{
var convertedSeg = new VectorMapSegment();
convertedSeg.builder = linSeg.builder;
foreach (var localPos in linSeg.targetLocalPositions)
{
convertedSeg.targetWorldPositions.Add(linSeg.builder.transform.TransformPoint(localPos)); //Convert to world position
}
convertedSeg.befores = linSeg.befores;
convertedSeg.afters = linSeg.afters;
foreach (var beforeSeg in convertedSeg.befores)
{
for (int i = 0; i < beforeSeg.afters.Count; i++)
{
if (beforeSeg.afters[i] == linSeg)
{
beforeSeg.afters[i] = convertedSeg;
}
}
}
foreach (var afterSeg in convertedSeg.afters)
{
for (int i = 0; i < afterSeg.befores.Count; i++)
{
if (afterSeg.befores[i] == linSeg)
{
afterSeg.befores[i] = convertedSeg;
}
}
}
allConvertedLinSeg.Add(convertedSeg);
}
}
//set up all lanes vector map data
var lnSegTerminalIDsMapping = new Dictionary <VectorMapSegment, int[]>(); //tracking for record
if (allConvertedLnSeg.Count > 0)
{
foreach (var lnSeg in allConvertedLnSeg)
{
List <Vector3> positions;
List <LaneInfo> laneInfos;
var cast = (lnSeg as VectorMapLaneSegment);
List <LaneInfo> waypointLaneInfos = (cast == null ? new List <LaneInfo>(new LaneInfo[lnSeg.targetWorldPositions.Count]) : cast.laneInfos);
//Interpolate based on waypoint world positions
VectorMapUtility.Interpolate(lnSeg.targetWorldPositions, waypointLaneInfos, out positions, out laneInfos, 1.0f / exportScaleFactor, true); //interpolate and divide to ensure 1 meter apart
lnSegTerminalIDsMapping.Add(lnSeg, new int[2]);
for (int i = 0; i < positions.Count; i++)
{
//Point
var pos = positions[i];
var vectorMapPos = GetVectorMapPosition(pos);
var vmPoint = Point.MakePoint(points.Count + 1, vectorMapPos.Bx, vectorMapPos.Ly, vectorMapPos.H);
points.Add(vmPoint);
//DtLane
var deltaDir = Vector3.zero;
if (i < positions.Count - 1)
{
deltaDir = positions[i + 1] - pos;
}
else
{
if (lnSeg.afters.Count > 0)
{
deltaDir = lnSeg.afters[0].targetWorldPositions[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;
var beforeLane = lanes[beforeLaneID - 1];
beforeLane.FLID = laneId;
lanes[beforeLaneID - 1] = beforeLane; //This is needed for struct copy to be applied back
}
var vmLane = Lane.MakeLane(laneId, vmDTLane.DID, beforeLaneID, 0, laneInfos[i].laneCount, laneInfos[i].laneNumber);
lanes.Add(vmLane);
if (i == 0)
{
lnSegTerminalIDsMapping[lnSeg][(int)TerminalType.START] = vmLane.LnID;
}
if (i == positions.Count - 1)
{
lnSegTerminalIDsMapping[lnSeg][(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 lnSeg in allConvertedLnSeg)
{
var terminalIdPair = lnSegTerminalIDsMapping[lnSeg];
if (lnSeg.befores.Count > 0)
{
var segStartId = terminalIdPair[(int)TerminalType.START];
var ln = lanes[segStartId - 1];
for (int i = 0; i < lnSeg.befores.Count; i++)
{
int beforeSegAfterLn = lnSegTerminalIDsMapping[lnSeg.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 (lnSeg.afters.Count > 0)
{
var segEndId = terminalIdPair[(int)TerminalType.END];
var ln = lanes[segEndId - 1];
int afterSegStartLn = lnSegTerminalIDsMapping[lnSeg.afters[0]][(int)TerminalType.START];
for (int i = 0; i < lnSeg.afters.Count; i++)
{
afterSegStartLn = lnSegTerminalIDsMapping[lnSeg.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 = GetUnityPosition(new VectorMapPosition()
{
Bx = points[endDtLn.PID - 1].Bx, Ly = points[endDtLn.PID - 1].Ly, H = points[endDtLn.PID - 1].H
});
var pointAfterPos = GetUnityPosition(new VectorMapPosition()
{
Bx = points[DtLnAfter.PID - 1].Bx, Ly = points[DtLnAfter.PID - 1].Ly, H = points[DtLnAfter.PID - 1].H
});
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 linSegTerminalIDsMapping = new Dictionary <VectorMapSegment, KeyValuePair <int, LineType>[]>(); //tracking for record
var stopLinkIDMapping = new Dictionary <VectorMapStopLineSegmentBuilder, List <int> >();
if (allConvertedLinSeg.Count > 0)
{
foreach (var linSeg in allConvertedLinSeg)
{
var linType = LineType.NONE;
if (linSeg.builder.GetType() == typeof(VectorMapStopLineSegmentBuilder)) //If it is stopline
{
linType = LineType.STOP;
}
else if (linSeg.builder.GetType() == typeof(VectorMapWhiteLineSegmentBuilder)) //if it is whiteline
{
var whiteLineBuilder = linSeg.builder as VectorMapWhiteLineSegmentBuilder;
if (whiteLineBuilder.lineColor == LineColor.WHITE)
{
linType = LineType.WHITE;
}
else if (whiteLineBuilder.lineColor == LineColor.YELLOW)
{
linType = LineType.YELLOW;
}
}
var positions = linSeg.targetWorldPositions;
linSegTerminalIDsMapping.Add(linSeg, 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 = GetVectorMapPosition(startPos);
var vmStartPoint = Point.MakePoint(points.Count + 1, vectorMapPos.Bx, vectorMapPos.Ly, vectorMapPos.H);
points.Add(vmStartPoint);
vectorMapPos = GetVectorMapPosition(endPos);
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)
{
linSegTerminalIDsMapping[linSeg][(int)TerminalType.START] = new KeyValuePair <int, LineType>(vmLine.LID, linType);
}
if (i == positions.Count - 2)
{
linSegTerminalIDsMapping[linSeg][(int)TerminalType.END] = new KeyValuePair <int, LineType>(vmLine.LID, linType);
}
int LinkID;
MakeVisualLine(vmLine, linType, out LinkID);
if (linType == LineType.STOP)
{
var builder = (VectorMapStopLineSegmentBuilder)linSeg.builder;
if (stopLinkIDMapping.ContainsKey(builder))
{
stopLinkIDMapping[builder].Add(LinkID);
}
else
{
stopLinkIDMapping.Add((VectorMapStopLineSegmentBuilder)linSeg.builder, 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 linSeg in allConvertedLinSeg)
{
var terminalIdPairs = linSegTerminalIDsMapping[linSeg];
if (linSeg.befores.Count > 0 && linSegTerminalIDsMapping.ContainsKey(linSeg.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 = linSegTerminalIDsMapping[linSeg.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 (linSeg.afters.Count > 0 && linSegTerminalIDsMapping.ContainsKey(linSeg.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 = linSegTerminalIDsMapping[linSeg.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 tempMapping = new Dictionary <VectorMapPole, int>();
foreach (var pole in signalLightPoles)
{
//Vector
var pos = pole.transform.position;
var vectorMapPos = GetVectorMapPosition(pos);
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);
tempMapping.Add(pole, PLID);
}
foreach (var pole in signalLightPoles)
{
var PLID = tempMapping[pole];
foreach (var signalLight in pole.signalLights)
{
var trafficLightAim = signalLight.transform.forward;
foreach (var lightData in signalLight.signalDatas)
{
//Vector
var trafficLightPos = signalLight.transform.TransformPoint(lightData.localPosition);
var vectorMapPos = GetVectorMapPosition(trafficLightPos);
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 = signalDatas.Count + 1;
int Type = (int)lightData.type;
int LinkID = -1;
if (signalLight.hintStopline != null)
{
LinkID = PickAimingLinkID(signalLight.transform, stopLinkIDMapping[signalLight.hintStopline]);
}
else
{
LinkID = FindProperStoplineLinkID(trafficLightPos, trafficLightAim);
}
var vmSignalData = SignalData.MakeSignalData(ID, VID, PLID, Type, LinkID);
signalDatas.Add(vmSignalData);
}
}
}
}
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);
}
