public void TestFromGlobalToLocal(RigidTransform geometryPose, PointSampleLocal pointExpected,
PointSampleGlobal pointIn)
{
var pointActual = GeometrySampling.FromGlobalToLocal(geometryPose, pointIn);
AssertSamplesEqual(pointExpected, pointActual);
}
static void AssertLaneSamplesEqual(PointSampleLocal sampleExpected, PointSampleLocal sampleActual)
{
Assert.IsTrue(Mathf.Approximately(0, sampleActual.position.x),
$"Lane samples should always have an x-value of zero, but got {sampleActual.position.x}");
Assert.IsTrue(Mathf.Approximately(sampleExpected.position.y, sampleActual.position.y) &&
Mathf.Approximately(sampleExpected.slope, sampleActual.slope),
$"Samples are different. Expected {sampleExpected}, got {sampleActual}");
}
static void AssertSamplesEqual(PointSampleLocal sampleExpected, PointSampleLocal sampleActual,
float positionTolerance = k_Delta, float rotationTolerance = k_Delta)
{
var posDelta = sampleExpected.position - sampleActual.position;
var posDeltaMagnitude = math.sqrt(math.dot(posDelta, posDelta));
Assert.AreEqual(0f, posDeltaMagnitude, positionTolerance,
"Sample positions are different. Expected " +
$"{sampleExpected.position.x:E}, {sampleExpected.position.y:E}, " +
$"was {sampleActual.position.x:E}, {sampleActual.position.y:E}");
Assert.AreEqual(sampleExpected.headingRadians, sampleActual.headingRadians, rotationTolerance);
}
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 TestComputeCircleSample(float s, float curvature, PointSampleLocal pointExpected)
{
var pointActual = GeometrySampling.BuildCircleSample(s, curvature);
AssertSamplesEqual(pointExpected, pointActual);
}
