public void TestFromGlobalToLocal(RigidTransform geometryPose, PointSampleLocal pointExpected,
PointSampleGlobal pointIn)
{
var pointActual = GeometrySampling.FromGlobalToLocal(geometryPose, pointIn);
AssertSamplesEqual(pointExpected, pointActual);
}
static void AssertSamplesEqual(PointSampleGlobal sampleExpected, PointSampleGlobal sampleActual,
float positionTolerance = k_Delta, float rotationTolerance = k_Delta)
{
var posDelta = sampleExpected.pose.pos - sampleActual.pose.pos;
var posDeltaMagnitude = math.sqrt(math.dot(posDelta, posDelta));
Assert.AreEqual(0f, posDeltaMagnitude, positionTolerance,
"Sample positions are different. Expected " +
$"{sampleExpected.pose.pos.x:E}, {sampleExpected.pose.pos.y:E}, {sampleExpected.pose.pos.z:E}, " +
$"was {sampleActual.pose.pos.x:E}, {sampleActual.pose.pos.y:E}, {sampleActual.pose.pos.z:E}");
var rotProduct = math.dot(sampleExpected.pose.rot, sampleActual.pose.rot);
// NOTE: The closer the dot product is to 1, the more similar two quaternions are
Assert.AreEqual(1f, math.abs(rotProduct), rotationTolerance,
"Sample quaternions were insufficiently similar.");
}
public void SampleLine_WithHeading_ReturnsCorrectValues(
float headingOpenDriveDegrees, float s, float xExpected, float zExpected, float sOffset)
{
var geometry = ConstructGeometryFromOpenDriveCoordinates(
headingDegrees: headingOpenDriveDegrees,
length: 3,
sAbsolute: sOffset,
x: 0,
z: 0,
geometryKind: GeometryKind.Line
);
var sampleActual = geometry.Sample(s);
var sampleExpected = new PointSampleGlobal(new RigidTransform(
quaternion.RotateY(math.radians(-headingOpenDriveDegrees)),
new float3(xExpected, 0, zExpected)));
AssertSamplesEqual(sampleExpected, sampleActual);
// Since we start the segment at the origin, the length should be equal to the L2 norm for this point
var lineLength = math.length(sampleActual.pose.pos);
Assert.IsTrue(Mathf.Approximately(s - sOffset, lineLength),
$"Expected line length to equal s ({s}) but was {lineLength}");
}
protected override JobHandle OnUpdate(JobHandle inputDeps)
{
List <List <IntPoint> > polygons;
switch (Parameters.processingScheme)
{
case ProcessingScheme.JobWithCode:
polygons = PolygonsFromRoadOutlineJobWithCode(Parameters.extensionDistance, inputDeps);
break;
case ProcessingScheme.IJobParallelFor:
polygons = PolygonsFromRoadOutlineIJobParallelFor(Parameters.extensionDistance, inputDeps);
break;
case ProcessingScheme.EntityForEachTempDynamicArrays:
polygons = PolygonsFromRoadOutlineEntityForEach(Parameters.extensionDistance, inputDeps);
break;
case ProcessingScheme.EntityForEachSeparateBuffer:
polygons = PolygonsFromRoadOutlineEntityForEachSeparateBuffer(Parameters.extensionDistance, inputDeps);
break;
default:
throw new NotSupportedException();
}
Profiler.BeginSample("Clipper");
var clipper = new Clipper();
foreach (var polygon in polygons)
{
if (!Clipper.Orientation(polygon))
{
polygon.Reverse();
}
if (Clipper.Area(polygon) > 0)
{
clipper.AddPath(polygon, PolyType.ptSubject, true);
}
}
var solution = new List <List <IntPoint> >();
clipper.Execute(ClipType.ctUnion, solution, PolyFillType.pftNonZero, PolyFillType.pftNonZero);
solution.RemoveAll(IsSmallerThanMinimumArea);
Profiler.EndSample();
Profiler.BeginSample("Create sample entities");
foreach (var polygon in solution)
{
var entity = EntityManager.CreateEntity(m_SamplesArchetype);
EntityManager.SetComponentData(entity, new PolygonOrientationComponent
{
Orientation = Clipper.Orientation(polygon)
? PolygonOrientation.Outside
: PolygonOrientation.Inside
});
var buffer = EntityManager.GetBuffer <PointSampleGlobal>(entity);
var count = polygon.Count;
for (var i = 0; i < polygon.Count; i++)
{
var previousPoint = polygon[((i - 1) % count + count) % count];
var point = polygon[i];
var nextPoint = polygon[(i + 1) % polygon.Count];
var v1 = new float3(point.X - previousPoint.X, 0, point.Y - previousPoint.Y);
var v2 = new float3(nextPoint.X - point.X, 0, nextPoint.Y - point.Y);
var rotation = quaternion.LookRotation(v2 + v1, new float3(0, 1, 0));
var fromClipperToGlobal = new PointSampleGlobal(new RigidTransform
{
pos = new float3(
point.X * PlacementUtility.DownScaleFactor,
0,
point.Y * PlacementUtility.DownScaleFactor),
rot = rotation
});
buffer.Add(fromClipperToGlobal);
}
}
Profiler.EndSample();
return(inputDeps);
}
