Esempio n. 1
0
        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);
        }
Esempio n. 2
0
        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);
        }