コード例 #1
0
        internal static PointSampleGlobal SampleCenter(Road road, SamplingStateRoad 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));
        }
コード例 #2
0
        // TODO: Refactor to separate lane sampling into helper which is agnostic to Side or LaneSection
        //       Find all invocations of SampleLanes_____ and modify them to move the lanes array initialization
        //       into the calling function
        internal static void SampleLanesOneSide(Road road, SamplingStateRoad 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(laneSection, side);

            if (!lanes.Any())
            {
                return;
            }

            var numLanes          = lanes.Count();
            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(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);
            }
            laneIter.Dispose();
        }