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