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