public void TestFromGlobalToLocal(RigidTransform geometryPose, PointSampleLocal pointExpected,
PointSampleGlobal pointIn)
{
var pointActual = GeometrySampling.FromGlobalToLocal(geometryPose, pointIn);
AssertSamplesEqual(pointExpected, pointActual);
}
List <List <IntPoint> > PolygonsFromRoadOutline(float extensionDistance)
{
var polygons = new List <List <IntPoint> >();
var rnd = Parameters.roadNetworkDescription;
foreach (var road in rnd.AllRoads)
{
var points = GeometrySampling.BuildSamplesFromRoadOutlineWithExtensionDistance(
road, 0.5f, extensionDistance, Parameters.outermostLaneType);
var path = new List <IntPoint>(points.Length);
foreach (var point in points)
{
path.Add(new IntPoint(
point.pose.pos.x * PlacementUtility.UpScaleFactor,
point.pose.pos.z * PlacementUtility.UpScaleFactor));
}
if (!Clipper.Orientation(path))
{
path.Reverse();
}
polygons.Add(path);
points.Dispose();
}
return(polygons);
}
/// <summary>
/// This approach uses a single large NativeArray of samples filled in by each job instead of adding temporary DynamicBuffers to the entities.
/// This appears to be the best balance between speed and simplicity.
///
/// Test results:
/// Total: ~40ms
/// Setup: ~0ms
/// Combine: 2.27ms
/// </summary>
private List <List <IntPoint> > PolygonsFromRoadOutlineEntityForEachSeparateBuffer(float extensionDistance, JobHandle jobHandle)
{
Profiler.BeginSample("PolygonsFromRoadOutlineEntityForEachSeparateBuffer");
Profiler.BeginSample("PolygonsFromRoadOutlineEntityForEachSeparateBuffer_Setup");
var polygons = new List <List <IntPoint> >();
var sampleStartIndexMap = new NativeHashMap <Entity, RoadSampleSpan>(300, Allocator.TempJob);
var sampleCountTotal = 0;
Entities.ForEach((int entityInQueryIndex, Entity entity, ref EcsRoad road) =>
{
var sampleCount = GeometrySampling.ComputeSampleCountForRoadOutline(road, .5f);
sampleStartIndexMap[entity] = new RoadSampleSpan
{
startIndex = sampleCountTotal,
count = sampleCount
};
sampleCountTotal += sampleCount;
}).Run();
var samples = new NativeArray <PointSampleGlobal>(sampleCountTotal, Allocator.TempJob, NativeArrayOptions.UninitializedMemory);
var job = Entities.ForEach((Entity entity,
ref EcsRoad road,
in DynamicBuffer <Geometry> ecsGeometries,
in DynamicBuffer <EcsLane> ecsLanes,
in DynamicBuffer <EcsLaneSection> ecsLaneSections,
in DynamicBuffer <LaneOffset> laneOffsets,
in DynamicBuffer <LaneWidthRecord> laneWidthRecords) => //(Entity entity, ref EcsRoad road) =>
{
var ecsRoadData = new EcsRoadData()
{
ecsRoad = road,
ecsGeometries = ecsGeometries,
ecsLaneSections = ecsLaneSections,
ecsLanes = ecsLanes,
laneOffsets = laneOffsets,
laneWidthRecords = laneWidthRecords
};;
var roadSampleSpan = sampleStartIndexMap[entity];
GeometrySampling.BuildSamplesFromRoadOutlineWithExtensionDistance(
ecsRoadData, 0.5f, extensionDistance, new NativeSlice <PointSampleGlobal>(samples, roadSampleSpan.startIndex, roadSampleSpan.count));
}).WithReadOnly(sampleStartIndexMap).WithoutBurst().Schedule(jobHandle);
public void Execute()
{
for (int i = 0; i < outlineJobBatch.count; i++)
{
var outlineJobParam = outlineJobParams[outlineJobBatch.startIndex + i];
var entity = outlineJobParam.entity;
var ecsRoadData = new EcsRoadData()
{
ecsRoad = ecsRoadGetter[entity],
ecsGeometries = geometryGetter[entity],
ecsLaneSections = laneSectionGetter[entity],
ecsLanes = laneGetter[entity],
laneOffsets = laneOffsetGetter[entity],
laneWidthRecords = laneWidthRecordGetter[entity]
};
GeometrySampling.BuildSamplesFromRoadOutlineWithExtensionDistance(
ecsRoadData, 0.5f, extensionDistance, new NativeSlice <PointSampleGlobal>(samples, outlineJobParam.startIndex, outlineJobParam.count));
}
}
public void SampleParamPoly3_ReturnsCorrectStartAndEndPoints(
ParamPoly3Data poly3, PointSampleLocal startExpected, PointSampleLocal endExpected, float sEnd)
{
var samplingState = new SamplingStatePoly3(poly3);
var startActual = GeometrySampling.SamplePoly3(ref samplingState, 0);
AssertSamplesEqual(startExpected, startActual);
var endActual = GeometrySampling.SamplePoly3(ref samplingState, sEnd);
// Check state object first to see if traversal worked
Assert.IsTrue(samplingState.NextValues.s >= sEnd, "Sampling didn't traverse far enough along the line. " +
$"Expected to get to s={sEnd} but only got to s={samplingState.NextValues.s}");
Assert.IsTrue(samplingState.CurrentValues.s <= sEnd, "Sampling traversed past end of line. +" +
$"Should have stopped at s={sEnd} but went to s={samplingState.CurrentValues.s}");
// Because we're treating these polynomials as normalized, p will always be 1.0f when s = sEnd
Assert.IsTrue(samplingState.NextValues.p >= 1.0f, "Sampling didn't traverse far enough along the line. " +
$"Expected to get to p=1.0 but only got to p={samplingState.NextValues.p}");
Assert.IsTrue(samplingState.CurrentValues.p <= 1.0f, "Sampling traversed past end of line. +" +
$"Should have stopped at p=1.0 but went to p={samplingState.CurrentValues.p}");
AssertSamplesEqual(endExpected, endActual, positionTolerance: 0.001f);
}
public void Execute(int index)
{
var road = Roads[index];
Entity entity = default;
DynamicBuffer <LaneVertex> laneVertices = default;
if (SplitScheme == SplitScheme.Road)
{
entity = CommandBuffer.CreateEntity(index, RoadLaneArchetype);
CommandBuffer.SetComponent(index, entity, new RoadLane {
Name = road.Name
});
laneVertices = CommandBuffer.AddBuffer <LaneVertex>(index, entity);
}
for (var geometryIdx = road.GeometryStartIndex; geometryIdx < road.GeometryStartIndex + road.GeometryCount; geometryIdx++)
{
var geometry = Geometries[geometryIdx];
if (SplitScheme == SplitScheme.Geometry)
{
entity = CommandBuffer.CreateEntity(index, RoadLaneArchetype);
CommandBuffer.SetComponent(index, entity, new RoadLane {
Name = new NativeString512($"{road.Name} {geometry.geometryKind}")
});
laneVertices = CommandBuffer.AddBuffer <LaneVertex>(index, entity);
}
var samples = GeometrySampling.BuildSamplesFromGeometry(geometry, 3f);
foreach (var sample in samples)
{
// TODO: support elevation
//Samples use x/y coordinates. Translate to x/z planar Unity coordinates
laneVertices.Add(new LaneVertex
{
GeometryIndex = geometryIdx - road.GeometryStartIndex,
Position = sample.pose.pos,
Orientation = sample.pose.rot
});
}
}
}
public void SampleParamPoly3_ReturnsCorrectValuesForPoly3Inputs(
Poly3Data poly3Data, float s, Vector2 samplePositionExpected, float headingExpectedDegrees)
{
var pPoly3 = new ParamPoly3Data(poly3Data);
var initialPosition = new Vector2(12, -5);
var geometryHeading = 10;
var geometry = ConstructGeometryFromOpenDriveCoordinates(
headingDegrees: geometryHeading,
length: 2 + s,
sAbsolute: 0,
x: initialPosition.x,
z: initialPosition.y,
geometryKind: GeometryKind.ParamPoly3,
paramPoly3Data: pPoly3);
var positionExpected =
(Vector2)(Quaternion.AngleAxis(-geometryHeading, Vector3.back) * samplePositionExpected) + initialPosition;
var sampleExpectedGlobal = ConstructPointSampleFromOpenDriveCoordinates(
positionExpected.x, positionExpected.y, headingExpectedDegrees + geometryHeading);
var sampleExpected = GeometrySampling.FromGlobalToLocal(geometry.pose, sampleExpectedGlobal);
var poly3SampleState = new SamplingStatePoly3(geometry.paramPoly3Data, 0.01f);
var sampleActual = GeometrySampling.SamplePoly3(ref poly3SampleState, s);
AssertSamplesEqual(sampleExpected, sampleActual, positionTolerance: 0.01f, rotationTolerance: 0.01f);
}
public void TestComputeCircleSample(float s, float curvature, PointSampleLocal pointExpected)
{
var pointActual = GeometrySampling.BuildCircleSample(s, curvature);
AssertSamplesEqual(pointExpected, pointActual);
}
public void SetGeometrySampling(GeometrySampling sampling)
{
PrevalidateMaterial();
_currentMaterial = _currentMaterial.UpdateGeometrySampling(sampling);
}
/// <summary>
/// This approach extends the idea in Job.WithCode() by manually generating batches of work with one job per batch for better load balancing.
/// Setup is tedious, but is slightly faster (~1-2ms) than Entities.ForEach with a separate buffer. We should probably avoid this pattern due to the large overhead and brittle code.
///
/// Test results:
/// Total: 36ms
/// Setup: 1.12ms
/// Combine: 2.12ms
/// </summary>
private List <List <IntPoint> > PolygonsFromRoadOutlineIJobParallelFor(float extensionDistance, JobHandle jobHandle)
{
Profiler.BeginSample("PolygonsFromRoadOutlineIJobParallelFor");
Profiler.BeginSample("PolygonsFromRoadOutlineIJobParallelFor_Setup");
var polygons = new List <List <IntPoint> >();
var outlineJobParams = new NativeList <OutlineJobParams>(300, Allocator.TempJob);
var sampleCountTotal = 0;
Entities.ForEach((int entityInQueryIndex, Entity entity, ref EcsRoad road) =>
{
var sampleCount = GeometrySampling.ComputeSampleCountForRoadOutline(road, .5f);
outlineJobParams.Add(new OutlineJobParams
{
entity = entity,
startIndex = sampleCountTotal,
count = sampleCount
});
sampleCountTotal += sampleCount;
}).Run();
var samples = new NativeArray <PointSampleGlobal>(sampleCountTotal, Allocator.TempJob, NativeArrayOptions.UninitializedMemory);
var ecsRoadGetter = GetComponentDataFromEntity <EcsRoad>(true);
var geometryGetter = GetBufferFromEntity <Geometry>(true);
var laneSectionGetter = GetBufferFromEntity <EcsLaneSection>(true);
var laneGetter = GetBufferFromEntity <EcsLane>(true);
var laneOffsetGetter = GetBufferFromEntity <LaneOffset>(true);
var laneWidthRecordGetter = GetBufferFromEntity <LaneWidthRecord>(true);
var batchSize = samples.Length / 64;
var jobBatches = new NativeList <JobHandle>(Allocator.TempJob);
int currentBatchSize = 0;
int currentBatchStartIndex = 0;
for (int i = 0; i < outlineJobParams.Length; i++)
{
var currentJobParams = outlineJobParams[i];
currentBatchSize += currentJobParams.count;
if (currentBatchSize >= batchSize || i == outlineJobParams.Length - 1)
{
var job = new BuildSamplesJob()
{
extensionDistance = extensionDistance,
outlineJobBatch = new OutlineJobBatch
{
startIndex = currentBatchStartIndex,
count = i - currentBatchStartIndex + 1
},
outlineJobParams = outlineJobParams,
ecsRoadGetter = ecsRoadGetter,
geometryGetter = geometryGetter,
laneSectionGetter = laneSectionGetter,
laneGetter = laneGetter,
laneOffsetGetter = laneOffsetGetter,
laneWidthRecordGetter = laneWidthRecordGetter,
samples = samples
}.Schedule(jobHandle);
jobBatches.Add(job);
currentBatchSize = 0;
currentBatchStartIndex = i + 1;
}
}
Profiler.EndSample();
Profiler.BeginSample("PolygonsFromRoadOutlineIJobParallelFor_CompleteJob");
JobHandle.CombineDependencies(jobBatches).Complete();
Profiler.EndSample();
Profiler.BeginSample("PolygonsFromRoadOutlineIJobParallelFor_Combine");
foreach (var outlineJobParam in outlineJobParams)
{
var sampleSlice = new NativeSlice <PointSampleGlobal>(samples, outlineJobParam.startIndex, outlineJobParam.count);
var path = new List <IntPoint>(sampleSlice.Length);
foreach (var point in sampleSlice)
{
path.Add(new IntPoint(
point.pose.pos.x * PlacementUtility.UpScaleFactor,
point.pose.pos.z * PlacementUtility.UpScaleFactor));
}
if (!Clipper.Orientation(path))
{
path.Reverse();
}
polygons.Add(path);
}
outlineJobParams.Dispose();
jobBatches.Dispose();
samples.Dispose();
Profiler.EndSample();
Profiler.EndSample();
return(polygons);
}
