Ejemplo n.º 1
0
        internal static PointSampleGlobal SampleCenter(EcsRoadData road, SamplingStateEcsRoad state, float sRoad)
        {
            var offset         = state.GetLaneOffset(road, sRoad);
            var geometrySample = state.GetGeometrySample(road, sRoad);
            var sampleLocal    = new PointSampleLocal(0, offset, 0);

            return(FromLocalToGlobal(geometrySample.pose, sampleLocal));
        }
Ejemplo n.º 2
0
        internal static void BuildSamplesInsideLaneSection(
            EcsRoadData road, int laneId, int numSamplesSection, int outputIdx, TraversalDirection samplingDirection,
            ref SamplingStateEcsRoad samplingState, ref NativeArray <PointSampleGlobal> samplesOut)
        {
            var numSamplesOut    = samplesOut.Length;
            var numEdgesToSample = math.abs(laneId);
            var numSamplesEdge   = numEdgesToSample == 0 ? numSamplesOut : numSamplesOut * numEdgesToSample;
            // We will need to sample the center line as well, if we are not between two lane edges
            var shouldSampleCenter = numEdgesToSample <= 1;
            var samplesEdges       = new NativeArray <PointSampleGlobal>(numSamplesEdge, Allocator.Temp);
            var side         = ToSide(laneId);
            var sectionStart = samplingDirection == TraversalDirection.Forward ? outputIdx : numSamplesSection + outputIdx - 1;

            for (var sampleNum = 0; sampleNum < numSamplesSection; sampleNum++)
            {
                var sampleIdx = sectionStart + sampleNum * (int)samplingDirection;
                var sRoad     = samplingState.sRoadLastComputed;
                SampleLanesOneSide(road, samplingState, side, sRoad, sampleIdx, ref samplesEdges,
                                   numEdgesToSample);
                // Lane index is lane ID - 1 because ID's start at 1, not 0
                var            laneSampleIdx = ComputeLaneSampleIdx(numEdgesToSample - 1, numSamplesOut, sampleIdx);
                var            poseOuterEdge = samplesEdges[laneSampleIdx].pose;
                RigidTransform poseInnerEdge;
                if (shouldSampleCenter)
                {
                    poseInnerEdge = SampleCenter(road, samplingState, sRoad).pose;
                }
                else
                {
                    var innerEdgeIdx = ComputeLaneSampleIdx(numEdgesToSample - 2, numSamplesOut, sampleIdx);
                    poseInnerEdge = samplesEdges[innerEdgeIdx].pose;
                }
                var positionMean = math.lerp(poseInnerEdge.pos, poseOuterEdge.pos, 0.5f);
                var rotationMean = math.nlerp(poseInnerEdge.rot, poseOuterEdge.rot, 0.5f);
                samplesOut[sampleIdx] = new PointSampleGlobal(rotationMean, positionMean);

                samplingState.Step(road);
            }

            samplesEdges.Dispose();
        }
Ejemplo n.º 3
0
        internal static void SampleLanesOneSide(EcsRoadData road, SamplingStateEcsRoad state, Side side, float sRoad,
                                                int outputIdx, ref NativeArray <PointSampleGlobal> samples, int stopBeforeIdx = int.MaxValue)
        {
            var laneEdgeOffsetLocal = state.GetLaneOffset(road, sRoad);

            // TODO: Direct access to the Idx should be revoked and replaced with Getter methods
            var laneSection = state.GetLaneSection(road, sRoad);
            var lanes       = GetLaneSectionSide(road, laneSection, side);

            if (lanes.Length == 0)
            {
                return;
            }

            var numLanes          = lanes.Length;
            var laneIter          = lanes.GetEnumerator();
            var numSamplesPerLane = samples.Length / math.min(numLanes, stopBeforeIdx);
            var sLaneSection      = sRoad - laneSection.sRoad;
            var geometrySample    = state.GetGeometrySample(road, sRoad);

            for (var laneIdx = 0; laneIdx < numLanes && laneIdx < stopBeforeIdx; ++laneIdx)
            {
                // For some reason, in IEnumerables, the 'current' field actually returns the value at index-1 rather
                // than the actual current value, so we need to move the iterator first to set 'current' to the value
                // we'd expect
                laneIter.MoveNext();
                var lane        = laneIter.Current;
                var sampleLocal = BuildLaneSample(road, lane, laneEdgeOffsetLocal, sLaneSection);
                laneEdgeOffsetLocal = sampleLocal.position.y;

                var currentIdx = ComputeLaneSampleIdx(laneIdx, numSamplesPerLane, outputIdx);
                if (currentIdx >= samples.Length)
                {
                    throw new Exception("Computed an out of range index.");
                }

                // The offset for the lane edge is measured along the normal of the reference line
                samples[currentIdx] =
                    FromLocalToGlobal(geometrySample.pose, sampleLocal);
            }
        }
Ejemplo n.º 4
0
        internal static NativeArray <PointSampleGlobal> BuildSamplesForLaneSectionSide(EcsRoadData roadData, SamplingStateEcsRoad state, Side side)
        {
            var laneSection = roadData.ecsLaneSections[state.laneSectionIdx];
            var sideLanes   = GetLaneSectionSide(roadData, laneSection, side);

            if (sideLanes.Length == 0)
            {
                // NOTE: This should be un-reachable from the function that calls this one
                Debug.LogWarning(String.Format("Can't sample {0} lanes for road {1} because there are none",
                                               side.ToString(), roadData.ecsRoad.name));
                return(new NativeArray <PointSampleGlobal>(0, Allocator.Temp));
            }

            var laneLength      = state.laneSectionEndS - laneSection.sRoad;
            var samplesPerMeter = state.samplesPerMeter;
            // Convert the outside points (throwing out y at this point)
            var sampleCount = 1 + (int)(laneLength * samplesPerMeter);

            //Add a sample at the end of the curve if the last
            var sampleAtEnd = !Mathf.Approximately(laneLength, (sampleCount - 1) / samplesPerMeter);

            var totalSampleCount = sampleAtEnd ? sampleCount + 1 : sampleCount;

            // samples for each lane are in contiguous blocks [s0l0, s1l0, s2l0, s0l1, s1l1, s1l2, s0l2...]
            var samples =
                new NativeArray <PointSampleGlobal>(totalSampleCount * sideLanes.Count(), Allocator.Temp);

            for (var sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++)
            {
                var sSampleAbsolute = sampleIdx / samplesPerMeter + laneSection.sRoad;
                SampleLanesOneSide(roadData, state, side, sSampleAbsolute, sampleIdx, ref samples);
            }

            if (sampleAtEnd)
            {
                var sEnd = laneSection.sRoad + laneLength;
                SampleLanesOneSide(roadData, state, side, sEnd, totalSampleCount - 1, ref samples);
            }

            return(samples);
        }
Ejemplo n.º 5
0
        internal static void BuildSamplesFromRoadOutline(EcsRoadData road, float samplesPerMeter, NativeSlice <PointSampleGlobal> allSamples)
        {
            if (road.ecsGeometries.Length == 0)
            {
                throw new Exception($"The road {road} has no reference line.");
            }

            var numTotalSamples = ComputeSampleCountForRoadOutline(road.ecsRoad, samplesPerMeter);

            if (numTotalSamples > allSamples.Length)
            {
                throw new ArgumentException("sample NativeSlice is not large enough to hold all the samples. Use ComputeSampleCountForRoadOutline to determine the proper number of samples.", nameof(allSamples));
            }

            var samplingState = new SamplingStateEcsRoad(road, samplesPerMeter);
            //var numSectionsLeft = Max(road.ecsLaneSections, s => s.LeftLaneCount);
            //var numSectionsRight = Max(road.ecsLaneSections, s => s.RightLaneCount);
            int maxValue = int.MinValue;

            foreach (var laneSection in road.ecsLaneSections)
            {
                maxValue = math.max(maxValue, laneSection.LeftLaneCount);
            }

            var numSectionsLeft = maxValue;
            int maxValue1       = int.MinValue;

            foreach (var laneSection1 in road.ecsLaneSections)
            {
                maxValue1 = math.max(maxValue1, laneSection1.RightLaneCount);
            }

            var numSectionsRight = maxValue1;
            var leftSamplesTemp  = new NativeArray <PointSampleGlobal>(numSectionsLeft, Allocator.Temp);
            var rightSamplesTemp = new NativeArray <PointSampleGlobal>(numSectionsRight, Allocator.Temp);

            for (var sampleIdx = 0; sampleIdx < numTotalSamples / 2; sampleIdx++)
            {
                var sRoad = samplingState.sRoadLastComputed;
                // We intentionally do not pass state as a reference here, as each sampling pass will cover the same
                // segment of road. We would need to do some refactoring to sample all lanes for a given s-coordinate
                // in one call in order to use sampling state more effectively
                SampleLanesOneSide(road, samplingState, Side.Left, sRoad, 0, ref leftSamplesTemp);
                SampleLanesOneSide(road, samplingState, Side.Right, sRoad, 0, ref rightSamplesTemp);
                var currentSection = road.ecsLaneSections[samplingState.laneSectionIdx];
                allSamples[sampleIdx] = currentSection.RightLaneCount > 0
                    ? rightSamplesTemp[currentSection.RightLaneCount - 1]
                    : SampleCenter(road, samplingState, sRoad);
                // allSamples must be ordered to form a counter-clockwise outline, so left samples get added
                // in reverse order to the end of the array
                allSamples[numTotalSamples - 1 - sampleIdx] = currentSection.LeftLaneCount > 0
                    ? leftSamplesTemp[currentSection.LeftLaneCount - 1]
                    : SampleCenter(road, samplingState, sRoad);
                var stepSucceeded = samplingState.Step(road);

                // We expect stepping to fail at the end of the current segment, but otherwise this would be a problem
                if (!stepSucceeded && sampleIdx * 2 + 2 != numTotalSamples)
                {
                    Debug.LogWarning($"Reached end of road {road.ecsRoad.name} early. Still have " +
                                     $"{numTotalSamples / 2 - sampleIdx - 1} samples to do.");
                }
            }

            leftSamplesTemp.Dispose();
            rightSamplesTemp.Dispose();
        }