internal static void SampleLanesOneSideUpTo(Road road, SamplingStateRoad state, Side side, float sRoad,
int outputIdx, ref NativeArray <PointSampleGlobal> samples, LaneType stopAtLane)
{
var lanes = GetLaneSectionSideUpTo(state.GetLaneSection(road, sRoad), side, stopAtLane);
SampleLanesOneSide(road, state, side, sRoad, outputIdx, ref samples, lanes.Count());
}
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();
}
internal static void BuildSamplesInsideLaneSection(
Road road, int laneId, int numSamplesSection, int outputIdx, TraversalDirection samplingDirection,
ref SamplingStateRoad 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();
}
internal static NativeArray <PointSampleGlobal> BuildSamplesForLaneSectionSide(Road road, SamplingStateRoad state,
Side side, int numLanesToSample = int.MaxValue)
{
var laneSection = road.laneSections[state.laneSectionIdx];
var sideLanes = GetLaneSectionSide(laneSection, side);
if (sideLanes == null)
{
// 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(), road.roadId));
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;
var numLanes = math.min(numLanesToSample, sideLanes.Count());
// samples for each lane are in contiguous blocks [s0l0, s1l0, s2l0, s0l1, s1l1, s1l2, s0l2...]
var samples =
new NativeArray <PointSampleGlobal>(totalSampleCount * numLanes, Allocator.Temp);
for (var sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++)
{
var sSampleAbsolute = sampleIdx / samplesPerMeter + laneSection.sRoad;
SampleLanesOneSide(road, state, side, sSampleAbsolute, sampleIdx, ref samples, numLanesToSample);
}
if (sampleAtEnd)
{
var sEnd = laneSection.sRoad + laneLength;
SampleLanesOneSide(road, state, side, sEnd, totalSampleCount - 1, ref samples, numLanesToSample);
}
return(samples);
}
internal static NativeArray <PointSampleGlobal> BuildSamplesFromRoadOutline(
Road road, float samplesPerMeter, LaneType stopAtLane)
{
if (road.geometry.Count == 0)
{
throw new Exception($"The road {road} has no reference line.");
}
var numTotalSamples = ComputeNumSamplesForRoadLength(road.length, samplesPerMeter) * 2;
var samplingState = new SamplingStateRoad(road, samplesPerMeter);
var numSectionsLeft = road.laneSections.Max(s => s.leftLanes.Count());
var numSectionsRight = road.laneSections.Max(s => s.rightLanes.Count());
var leftSamplesTemp = new NativeArray <PointSampleGlobal>(numSectionsLeft, Allocator.Temp);
var rightSamplesTemp = new NativeArray <PointSampleGlobal>(numSectionsRight, Allocator.Temp);
var allSamples = new NativeArray <PointSampleGlobal>(numTotalSamples, 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
SampleLanesOneSideUpTo(
road, samplingState, Side.Left, sRoad, 0, ref leftSamplesTemp, stopAtLane);
SampleLanesOneSideUpTo(
road, samplingState, Side.Right, sRoad, 0, ref rightSamplesTemp, stopAtLane);
var currentSection = road.laneSections[samplingState.laneSectionIdx];
if (currentSection.rightLanes.Any())
{
var sampledLanes = GetLaneSectionSideUpTo(samplingState.GetLaneSection(road, sRoad), Side.Right,
stopAtLane);
allSamples[sampleIdx] = sampledLanes.Any()
? rightSamplesTemp[sampledLanes.Count() - 1]
: SampleCenter(road, samplingState, sRoad);
}
else
{
allSamples[sampleIdx] = 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
var leftIdx = numTotalSamples - 1 - sampleIdx;
if (currentSection.leftLanes.Any())
{
var sampledLanes = GetLaneSectionSideUpTo(samplingState.GetLaneSection(road, sRoad), Side.Left,
stopAtLane);
allSamples[leftIdx] = sampledLanes.Any()
? leftSamplesTemp[sampledLanes.Count() - 1]
: SampleCenter(road, samplingState, sRoad);
}
else
{
allSamples[leftIdx] = 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.roadId} early. Still have " +
$"{numTotalSamples / 2 - sampleIdx - 1} samples to do.");
}
}
leftSamplesTemp.Dispose();
rightSamplesTemp.Dispose();
return(allSamples);
}
