//joint and convert a set of singlely-connected segments and also setup world positions for all segments
private static void ConvertAndJointSegmentSet(VectorMapSegment curLnSeg, HashSet <VectorMapSegment> allLnSegs, HashSet <VectorMapSegment> newAllLnSegs, HashSet <VectorMapSegment> visitedLnSegs)
{
var combLnSegs = new List <VectorMapSegment>();
visitedLnSegs.Add(curLnSeg);
combLnSegs.Add(curLnSeg);
while (curLnSeg.afters.Count == 1 && curLnSeg.afters[0].befores.Count == 1 && curLnSeg.afters[0].befores[0] == curLnSeg && !visitedLnSegs.Contains((VectorMapSegment)curLnSeg.afters[0]))
{
visitedLnSegs.Add(curLnSeg.afters[0]);
combLnSegs.Add(curLnSeg.afters[0]);
curLnSeg = curLnSeg.afters[0];
}
//construct new lane segments in with its wappoints in world positions and update related dependencies to relate to new segments
var combinedLnSeg = new VectorMapLaneSegment();
combinedLnSeg.befores = combLnSegs[0].befores;
combinedLnSeg.afters = combLnSegs[combLnSegs.Count - 1].afters;
foreach (var beforeSeg in combinedLnSeg.befores)
{
for (int i = 0; i < beforeSeg.afters.Count; i++)
{
if (beforeSeg.afters[i] == combLnSegs[0])
{
beforeSeg.afters[i] = combinedLnSeg;
}
}
}
foreach (var afterSeg in combinedLnSeg.afters)
{
for (int i = 0; i < afterSeg.befores.Count; i++)
{
if (afterSeg.befores[i] == combLnSegs[combLnSegs.Count - 1])
{
afterSeg.befores[i] = combinedLnSeg;
}
}
}
foreach (var lnSeg in combLnSegs)
{
int laneCount = 0;
int laneNumber = 0;
var segBlder = lnSeg.builder as VectorMapLaneSegmentBuilder;
if (segBlder != null)
{
laneCount = segBlder.laneInfo.laneCount;
laneNumber = segBlder.laneInfo.laneNumber;
}
foreach (var localPos in lnSeg.targetLocalPositions)
{
combinedLnSeg.targetWorldPositions.Add(lnSeg.builder.transform.TransformPoint(localPos)); //Convert to world position
combinedLnSeg.laneInfos.Add(new LaneInfo()
{
laneCount = laneCount, laneNumber = laneNumber
});
}
}
foreach (var lnSeg in combLnSegs)
{
allLnSegs.Remove(lnSeg);
}
newAllLnSegs.Add(combinedLnSeg);
}
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);
}
}
}
}
