internal static NativeArray <PointSampleGlobal> BuildSamplesFromGeometry(
Geometry geometry,
float samplesPerMeter,
Allocator allocator = Allocator.Temp)
{
// Add one to count because we start sampling at zero
var sampleCount = 1 + (int)(geometry.length * samplesPerMeter);
//Add a sample at the end of the curve if the last
var sampleAtEnd = !Mathf.Approximately(geometry.length, sampleCount * samplesPerMeter);
var totalSampleCount = sampleAtEnd ? sampleCount + 1 : sampleCount;
var state = new SamplingStateGeometry(geometry);
NativeArray <PointSampleGlobal> samples = new NativeArray <PointSampleGlobal>(totalSampleCount, allocator);
for (int i = 0; i < sampleCount; i++)
{
var sample = geometry.Sample(i / samplesPerMeter + geometry.sRoad, ref state);
samples[i] = sample;
}
if (sampleAtEnd)
{
samples[totalSampleCount - 1] = geometry.Sample(geometry.length + geometry.sRoad, ref state);
}
return(samples);
}
public SamplingStateEcsRoad(EcsRoadData road, float samplesPerMeter)
{
canMoveToNextLaneSection = true;
geometryIdx = 0;
geometryEndS = road.ecsGeometries[0].length;
laneSectionIdx = 0;
laneSectionEndS = road.ecsLaneSections.Length > 1 ? road.ecsLaneSections[1].sRoad : road.ecsRoad.length;
if (road.laneOffsets.Length > 0)
{
laneOffsetIdx = 0;
laneOffsetEndS = road.laneOffsets.Length > 1 ? road.laneOffsets[1].sRoad : road.ecsRoad.length;
}
else
{
laneOffsetIdx = -1;
laneOffsetEndS = float.MaxValue;
}
sRoadCurrent = 0.0f;
sRoadStep = 1.0f / samplesPerMeter;
sRoadLastComputed = -1f;
geometrySample = default;
laneOffset = 0f;
geometryState = new SamplingStateGeometry(road.ecsGeometries[0]);
ComputeSamples(road);
}
private void CheckStateAndUpdate(EcsRoadData road)
{
if (!IsStale())
{
return;
}
if (!GeometrySampling.IsApproximatelyLessThan(sRoadCurrent, geometryEndS))
{
(geometryIdx, geometryEndS) =
FindNextAlongLength(road.ecsGeometries, sRoadCurrent, geometryIdx, road.ecsRoad.length);
geometryState = new SamplingStateGeometry(road.ecsGeometries[geometryIdx]);
}
if (!GeometrySampling.IsApproximatelyLessThan(sRoadCurrent, laneOffsetEndS))
{
(laneOffsetIdx, laneOffsetEndS) =
FindNextAlongLength(road.laneOffsets, sRoadCurrent, laneOffsetIdx, road.ecsRoad.length);
}
if (!GeometrySampling.IsApproximatelyLessThan(sRoadCurrent, laneSectionEndS))
{
if (canMoveToNextLaneSection)
{
(laneSectionIdx, laneSectionEndS) =
FindNextAlongLength(road.ecsLaneSections, sRoadCurrent, laneSectionIdx, road.ecsRoad.length);
}
else if (!Mathf.Approximately(sRoadCurrent, laneSectionEndS))
{
throw new Exception("Attempted to traverse past end of lane section, but " +
$"{nameof(canMoveToNextLaneSection)} is {canMoveToNextLaneSection}");
}
}
ComputeSamples(road);
}
internal static PointSampleGlobal Sample(this Geometry geometry, float sRoad, ref SamplingStateGeometry state)
{
switch (geometry.geometryKind)
{
case GeometryKind.Line:
return(SampleLine(geometry, sRoad));
case GeometryKind.Arc:
return(SampleArc(geometry, sRoad));
case GeometryKind.Spiral:
return(SampleSpiral(geometry, sRoad));
case GeometryKind.Poly3:
case GeometryKind.ParamPoly3:
var sLocal = sRoad - geometry.sRoad;
var sampleLocal = SamplePoly3(ref state.poly3, sLocal);
return(FromLocalToGlobal(geometry.startPose, sampleLocal));
case GeometryKind.Unknown:
default:
throw new NotImplementedException($"GeometryKind {geometry.geometryKind} not supported");
}
}
// TODO: Many of our internal sampling paths could be optimized by calling the stateful sample method instead of
// this one.
/// <summary>
/// Sample the geometry at the given s position without tracking progress along the sampled segment
/// </summary>
/// <param name="sRoad">S position to sample. Must be between s and s + length.</param>
public static PointSampleGlobal Sample(this Geometry geometry, float sRoad)
{
var transientState = new SamplingStateGeometry(geometry);
return(Sample(geometry, sRoad, ref transientState));
}
