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