internal static float Poly3ComputeL(ParamPoly3Data poly3, float p) { //l = sqrt((du/dp)^2 + (dv/dp)^2) // dv/du is the slope var(dvDp, duDp) = Poly3ComputeDerivative(poly3, p); return(math.sqrt(duDp * duDp + dvDp * dvDp)); }
internal static SimpsonsRuleValues Poly3ComputeSimpsonStep(ParamPoly3Data poly3, SimpsonsRuleValues current, float samplingDelta) { //use the arc length rule (https://en.wikipedia.org/wiki/Arc_length#Finding_arc_lengths_by_integrating) and //Simpson's Rule (https://en.wikipedia.org/wiki/Simpson's_rule) to estimate the distance along the polynomial var(dvdp, dudp) = Poly3ComputeDerivative(poly3, current.p); var stepSize = samplingDelta / math.sqrt(dvdp * dvdp + dudp * dudp); var pMid = current.p + stepSize; var lMid = Poly3ComputeL(poly3, pMid); var pNext = current.p + stepSize * 2; var lNext = Poly3ComputeL(poly3, pNext); var ds = (current.l + 4 * lMid + lNext) * stepSize / 3; var sNext = current.s + ds; if (ds <= 0) { throw new Exception( $"ds was {ds:0.0000} but should always be positive and non-zero while stepping through a Poly3.\n" + $"Error caused by sampling with step size {samplingDelta:0.0000}: {poly3}"); } return(new SimpsonsRuleValues(sNext, lNext, pNext)); }
internal static (float, float) Poly3ComputeDerivative(ParamPoly3Data poly3, float t) { var ddt = Poly3FirstOrder(t); var dudt = math.dot(ddt, poly3.u); var dvdt = math.dot(ddt, poly3.v); return(dvdt, dudt); }
// Because all of these tests were written assuming open drive coordinates, but our geometry is in // Unity coordinates, rather than re-write all the test cases, we just convert from OpenDRIVE to Unity here static Geometry ConstructGeometryFromOpenDriveCoordinates( GeometryKind geometryKind, float headingDegrees, float length, float sAbsolute, float x, float z, ArcData arcData = default, SpiralData spiralData = default, Poly3Data poly3Data = default, ParamPoly3Data paramPoly3Data = default) { var pose = OpenDriveMapElementFactory.FromOpenDriveGlobalToUnity(math.radians(headingDegrees), x, z); return(ConstructGeometry(geometryKind, pose, length, sAbsolute, arcData, spiralData, poly3Data, paramPoly3Data)); }
internal static PointSampleLocal Poly3ConstructSample(ParamPoly3Data poly3, float sLocal, SimpsonsRuleValues current, SimpsonsRuleValues next) { var sPercent = (sLocal - current.s) / (next.s - current.s); var pFinal = current.p + (next.p - current.p) * sPercent; var uFinal = Poly3ComputeU(poly3, pFinal); var vFinal = Poly3ComputeV(poly3, pFinal); var headingLocal = Poly3ComputeHeading(poly3, pFinal); return(new PointSampleLocal(uFinal, vFinal, headingLocal)); }
internal SamplingStatePoly3(ParamPoly3Data poly3, float dsStepForApproximation = 0.1f) { m_poly3 = poly3; m_dsStepForApproximation = dsStepForApproximation; m_currentIdx = 0; var initialValues = new SimpsonsRuleValues(0, GeometrySampling.Poly3ComputeL(m_poly3, 0), 0); m_values = new[] { initialValues, GeometrySampling.Poly3ComputeSimpsonStep(m_poly3, initialValues, dsStepForApproximation) }; }
static Geometry ConstructGeometry( GeometryKind geometryKind, RigidTransform pose, float length, float sAbsolute, ArcData arcData = default, SpiralData spiralData = default, Poly3Data poly3Data = default, ParamPoly3Data paramPoly3Data = default) { return(new Geometry() { length = length, sRoad = sAbsolute, startPose = pose, geometryKind = geometryKind, arcData = arcData, spiralData = spiralData, poly3Data = poly3Data, paramPoly3Data = paramPoly3Data }); }
// Because all of these tests were written assuming open drive coordinates, but our geometry is in // Unity coordinates, rather than re-write all the test cases, we just convert from OpenDRIVE to Unity here static Geometry ConstructGeometryFromOpenDriveCoordinates( float headingDegrees, float length, float sAbsolute, float x, float z, GeometryKind geometryKind, ArcData arcData = default, SpiralData spiralData = default, Poly3Data poly3Data = default, ParamPoly3Data paramPoly3Data = default) { var pose = OpenDriveMapElementFactory.FromOpenDriveGlobalToUnity(math.radians(headingDegrees), x, z); return(new Geometry() { length = length, sRoad = sAbsolute, startPose = pose, geometryKind = geometryKind, arcData = arcData, spiralData = spiralData, poly3Data = poly3Data, paramPoly3Data = paramPoly3Data }); }
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); }
internal SamplingStatePoly3(Geometry geometry, float dsStepForApproximation = 0.1f) { if (geometry.geometryKind == GeometryKind.Poly3) { m_poly3 = new ParamPoly3Data(geometry.poly3Data); } else { m_poly3 = geometry.paramPoly3Data; } m_dsStepForApproximation = dsStepForApproximation; m_currentIdx = 0; var initialValues = new SimpsonsRuleValues(0, GeometrySampling.Poly3ComputeL(m_poly3, 0), 0); m_values = new[] { initialValues, GeometrySampling.Poly3ComputeSimpsonStep(m_poly3, initialValues, m_dsStepForApproximation) }; }
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); }
internal static float Poly3ComputeV(ParamPoly3Data poly3, float t) { var tValues = Poly3ZeroOrder(t); return(math.dot(tValues, poly3.v)); }
internal static float Poly3ComputeHeading(ParamPoly3Data poly3, float t) { var(dv, du) = Poly3ComputeDerivative(poly3, t); return(math.atan2(dv, du)); }