Beispiel #1
0
    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);
                }
            }
        }
    }
Beispiel #2
0
        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);
        }