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)
};
}
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)
};
}
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 void SetNextValues(SimpsonsRuleValues newValues)
{
m_values[m_currentIdx].SetAll(newValues.s, newValues.l, newValues.p);
m_currentIdx = 1 - m_currentIdx;
}
