int
XSpline::linearCombinationFor(LinearCoefficient* coeffs, int segment, double t) const
{
assert(segment >= 0 && segment < (int)m_controlPoints.size() - 1);
assert(t >= 0 && t <= 1);
int idxs[4];
idxs[0] = std::max<int>(0, segment - 1);
idxs[1] = segment;
idxs[2] = segment + 1;
idxs[3] = std::min<int>(segment + 2, m_controlPoints.size() - 1);
ControlPoint pts[4];
for (int i = 0; i < 4; ++i) {
pts[i] = m_controlPoints[idxs[i]];
}
TensionDerivedParams const tdp(pts[1].tension, pts[2].tension);
Vec4d A;
if (t <= tdp.T0p) {
A[0] = GBlendFunc(tdp.q[0], tdp.p[0]).value((t - tdp.T0p) / (tdp.t0 - tdp.T0p));
} else {
A[0] = HBlendFunc(tdp.q[0]).value((t - tdp.T0p) / (tdp.t0 - tdp.T0p));
}
A[1] = GBlendFunc(tdp.q[1], tdp.p[1]).value((t - tdp.T1p) / (tdp.t1 - tdp.T1p));
A[2] = GBlendFunc(tdp.q[2], tdp.p[2]).value((t - tdp.T2m) / (tdp.t2 - tdp.T2m));
if (t >= tdp.T3m) {
A[3] = GBlendFunc(tdp.q[3], tdp.p[3]).value((t - tdp.T3m) / (tdp.t3 - tdp.T3m));
} else {
A[3] = HBlendFunc(tdp.q[3]).value((t - tdp.T3m) / (tdp.t3 - tdp.T3m));
}
A /= A.sum();
int out_idx = 0;
if (idxs[0] == idxs[1]) {
coeffs[out_idx++] = LinearCoefficient(idxs[0], A[0] + A[1]);
} else {
coeffs[out_idx++] = LinearCoefficient(idxs[0], A[0]);
coeffs[out_idx++] = LinearCoefficient(idxs[1], A[1]);
}
if (idxs[2] == idxs[3]) {
coeffs[out_idx++] = LinearCoefficient(idxs[2], A[2] + A[3]);
} else {
coeffs[out_idx++] = LinearCoefficient(idxs[2], A[2]);
coeffs[out_idx++] = LinearCoefficient(idxs[3], A[3]);
}
return out_idx;
}
XSpline::DecomposedDerivs
XSpline::decomposedDerivsImpl(int const segment, double const t) const
{
assert(segment >= 0 && segment < (int)m_controlPoints.size() - 1);
assert(t >= 0 && t <= 1);
DecomposedDerivs derivs;
derivs.numControlPoints = 4; // May be modified later in this function.
derivs.controlPoints[0] = std::max<int>(0, segment - 1);
derivs.controlPoints[1] = segment;
derivs.controlPoints[2] = segment + 1;
derivs.controlPoints[3] = std::min<int>(segment + 2, m_controlPoints.size() - 1);
ControlPoint pts[4];
for (int i = 0; i < 4; ++i) {
pts[i] = m_controlPoints[derivs.controlPoints[i]];
}
TensionDerivedParams const tdp(pts[1].tension, pts[2].tension);
// Note that we don't want the derivate with respect to t that's
// passed to us (ranging from 0 to 1 within a segment).
// Rather we want it with respect to the t that's passed to
// decomposedDerivs(), ranging from 0 to 1 across all segments.
// Let's call the latter capital T. Their relationship is:
// t = T*num_segments - C
// dt/dT = num_segments
double const dtdT = numSegments();
Vec4d A;
Vec4d dA; // First derivatives with respect to T.
Vec4d ddA; // Second derivatives with respect to T.
// Control point 0.
{
// u = (t - tdp.T0p) / (tdp.t0 - tdp.T0p)
double const ta = 1.0 / (tdp.t0 - tdp.T0p);
double const tb = -tdp.T0p * ta;
double const u = ta * t + tb;
if (t <= tdp.T0p) {
// u(t) = ta * tt + tb
// u'(t) = ta
// g(t) = g(u(t), <tension derived params>)
GBlendFunc g(tdp.q[0], tdp.p[0]);
A[0] = g.value(u);
// g'(u(t(T))) = g'(u)*u'(t)*t'(T)
dA[0] = g.firstDerivative(u) * (ta * dtdT);
// Note that u'(t) and t'(T) are constant functions.
// g"(u(t(T))) = g"(u)*u'(t)*t'(T)*u'(t)*t'(T)
ddA[0] = g.secondDerivative(u) * (ta * dtdT) * (ta * dtdT);
} else {
HBlendFunc h(tdp.q[0]);
A[0] = h.value(u);
dA[0] = h.firstDerivative(u) * (ta * dtdT);
ddA[0] = h.secondDerivative(u) * (ta * dtdT) * (ta * dtdT);
}
}
// Control point 1.
{
// u = (t - tdp.T1p) / (tdp.t1 - tdp.T1p)
double const ta = 1.0 / (tdp.t1 - tdp.T1p);
double const tb = -tdp.T1p * ta;
double const u = ta * t + tb;
GBlendFunc g(tdp.q[1], tdp.p[1]);
A[1] = g.value(u);
dA[1] = g.firstDerivative(u) * (ta * dtdT);
ddA[1] = g.secondDerivative(u) * (ta * dtdT) * (ta * dtdT);
}
// Control point 2.
{
// u = (t - tdp.T2m) / (tdp.t2 - tdp.T2m)
double const ta = 1.0 / (tdp.t2 - tdp.T2m);
double const tb = -tdp.T2m * ta;
double const u = ta * t + tb;
GBlendFunc g(tdp.q[2], tdp.p[2]);
A[2] = g.value(u);
dA[2] = g.firstDerivative(u) * (ta * dtdT);
ddA[2] = g.secondDerivative(u) * (ta * dtdT) * (ta * dtdT);
}
// Control point 3.
{
// u = (t - tdp.T3m) / (tdp.t3 - tdp.T3m)
double const ta = 1.0 / (tdp.t3 - tdp.T3m);
double const tb = -tdp.T3m * ta;
double const u = ta * t + tb;
if (t >= tdp.T3m) {
GBlendFunc g(tdp.q[3], tdp.p[3]);
A[3] = g.value(u);
dA[3] = g.firstDerivative(u) * (ta * dtdT);
ddA[3] = g.secondDerivative(u) * (ta * dtdT) * (ta * dtdT);
} else {
HBlendFunc h(tdp.q[3]);
A[3] = h.value(u);
dA[3] = h.firstDerivative(u) * (ta * dtdT);
ddA[3] = h.secondDerivative(u) * (ta * dtdT) * (ta * dtdT);
}
}
double const sum = A.sum();
double const sum2 = sum * sum;
double const sum4 = sum2 * sum2;
double const d_sum = dA.sum();
double const dd_sum = ddA.sum();
for (int i = 0; i < 4; ++i) {
derivs.zeroDerivCoeffs[i] = A[i] / sum;
double const d1 = dA[i] * sum - A[i] * d_sum;
derivs.firstDerivCoeffs[i] = d1 / sum2; // Derivative of: A[i] / sum
// Derivative of: dA[i] * sum
double const dd1 = ddA[i] * sum + dA[i] * d_sum;
// Derivative of: A[i] * d_sum
double const dd2 = dA[i] * d_sum + A[i] * dd_sum;
// Derivative of (dA[i] * sum - A[i] * d_sum) / sum2
double const dd3 = ((dd1 - dd2) * sum2 - d1 * (2 * sum * d_sum)) / sum4;
derivs.secondDerivCoeffs[i] = dd3;
}
// Merge / throw away some control points.
int write_idx = 0;
int merge_idx = 0;
int read_idx = 1;
int const end = 4;
for (;;) {
assert(merge_idx != read_idx);
for (; read_idx != end && derivs.controlPoints[read_idx] == derivs.controlPoints[merge_idx]; ++read_idx) {
// Merge
derivs.zeroDerivCoeffs[merge_idx] += derivs.zeroDerivCoeffs[read_idx];
derivs.firstDerivCoeffs[merge_idx] += derivs.firstDerivCoeffs[read_idx];
derivs.secondDerivCoeffs[merge_idx] += derivs.secondDerivCoeffs[read_idx];
}
if (derivs.hasNonZeroCoeffs(merge_idx)) {
// Copy
derivs.zeroDerivCoeffs[write_idx] = derivs.zeroDerivCoeffs[merge_idx];
derivs.firstDerivCoeffs[write_idx] = derivs.firstDerivCoeffs[merge_idx];
derivs.secondDerivCoeffs[write_idx] = derivs.secondDerivCoeffs[merge_idx];
derivs.controlPoints[write_idx] = derivs.controlPoints[merge_idx];
++write_idx;
}
if (read_idx == end) {
break;
}
merge_idx = read_idx;
++read_idx;
}
derivs.numControlPoints = write_idx;
return derivs;
}
