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)); }
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); } }