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 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);
}