public void TestFromGlobalToLocal(RigidTransform geometryPose, PointSampleLocal pointExpected,
PointSampleGlobal pointIn)
{
var pointActual = GeometrySampling.FromGlobalToLocal(geometryPose, pointIn);
AssertSamplesEqual(pointExpected, pointActual);
}
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);
}