// Inverse mapping of s = Length(t) given by t = Length^{-1}(s). The
// inverse length function generally cannot be written in closed form,
// in which case it is not directly computable. Instead, we can
// specify s and estimate the root t for F(t) = Length(t) - s. The
// derivative is F'(t) = Speed(t) >= 0, so F(t) is non decreasing. To
// be robust, we use bisection to locate the root, although it is
// possible to use a hybrid of Newton's method and bisection. For
// details, see the document
// https://www.geometrictools.com/Documentation/MovingAlongCurveSpecifiedSpeed.pdf
public double GetTime(double length)
{
if (length > 0)
{
if (length < this.GetTotalLength())
{
double F(double t)
{
double speed(double z)
{
return(this.GetSpeed(z));
}
return(Integration.Romberg(this.rombergOrder, this.times[0], t, speed) - length);
}
// We know that F(tmin) < 0 and F(tmax) > 0, which allows us to
// use bisection. Rather than bisect the entire interval, let's
// narrow it down with a reasonable initial guess.
double ratio = length / this.GetTotalLength();
double omratio = 1.0 - ratio;
double tmid = omratio * this.times[0] + ratio * this.times[this.times.Length - 1];
double fmid = F(tmid);
if (fmid > 0)
{
RootsBisection.Find(F, this.times[0], tmid, -1.0, 1.0, this.maxBisections, out tmid);
}
else if (fmid < 0)
{
RootsBisection.Find(F, tmid, this.times[this.times.Length - 1], -1.0, 1.0, this.maxBisections, out tmid);
}
return(tmid);
}
return(this.times[this.times.Length - 1]);
}
return(this.times[0]);
}
public BSplineReduction(Vector3[] inControls, int degree, double fraction)
: this()
{
int numSamples = inControls.Length;
this.sampleData = inControls;
Debug.Assert(numSamples >= 2 && 1 <= degree && degree < numSamples, "Invalid input.");
// Clamp the number of control points to [degree+1,quantity-1].
int numControls = (int)Math.Round(fraction * numSamples);
if (numControls >= numSamples)
{
this.controlData = inControls;
return;
}
if (numControls < degree + 1)
{
numControls = degree + 1;
}
// Set up basis function parameters. Function 0 corresponds to
// the output curve. Function 1 corresponds to the input curve.
this.degree = degree;
this.quantity[0] = numControls;
this.quantity[1] = numSamples;
for (int j = 0; j <= 1; j++)
{
this.numKnots[j] = this.quantity[j] + this.degree + 1;
this.knot[j] = new double[this.numKnots[j]];
int i;
for (i = 0; i <= this.degree; ++i)
{
this.knot[j][i] = 0.0;
}
double denom = this.quantity[j] - this.degree;
double factor = 1.0 / denom;
for (/**/; i < this.quantity[j]; ++i)
{
this.knot[j][i] = (i - this.degree) * factor;
}
for (/**/; i < this.numKnots[j]; ++i)
{
this.knot[j][i] = 1.0;
}
}
// Construct matrix A (depends only on the output basis function).
double value, tmin, tmax;
int i0, i1;
this.mBasis[0] = 0;
this.mBasis[1] = 0;
double integrand(double t)
{
double value0 = this.F(this.mBasis[0], this.mIndex[0], this.degree, t);
double value1 = this.F(this.mBasis[1], this.mIndex[1], this.degree, t);
double result = value0 * value1;
return(result);
}
BandedMatrix A = new BandedMatrix(this.quantity[0], this.degree, this.degree);
for (i0 = 0; i0 < this.quantity[0]; ++i0)
{
this.mIndex[0] = i0;
tmax = this.MaxSupport(0, i0);
for (i1 = i0; i1 <= i0 + this.degree && i1 < this.quantity[0]; ++i1)
{
this.mIndex[1] = i1;
tmin = this.MinSupport(0, i1);
value = Integration.Romberg(8, tmin, tmax, integrand);
A[i0, i1] = value;
A[i1, i0] = value;
}
}
// Construct A^{-1}.
// scheme to invert A?
GMatrix invA = new GMatrix(this.quantity[0], this.quantity[0]);
bool invertible = A.ComputeInverse(invA.Elements.Vector);
Debug.Assert(invertible, "Failed to invert matrix.");
// Construct B (depends on both input and output basis functions).
this.mBasis[1] = 1;
GMatrix B = new GMatrix(this.quantity[0], this.quantity[1]);
for (i0 = 0; i0 < this.quantity[0]; ++i0)
{
this.mIndex[0] = i0;
double tmin0 = this.MinSupport(0, i0);
double tmax0 = this.MaxSupport(0, i0);
for (i1 = 0; i1 < this.quantity[1]; ++i1)
{
this.mIndex[1] = i1;
double tmin1 = this.MinSupport(1, i1);
double tmax1 = this.MaxSupport(1, i1);
double[] interval0 = { tmin0, tmax0 };
double[] interval1 = { tmin1, tmax1 };
FIQueryIntervals result = new FIQueryIntervals(interval0, interval1);
if (result.NumIntersections == 2)
{
value = Integration.Romberg(8, result.Overlap[0], result.Overlap[1], integrand);
B[i0, i1] = value;
}
else
{
B[i0, i1] = 0.0;
}
}
}
// Construct A^{-1}*B.
GMatrix prod = invA * B;
// Allocate output control points.
// Construct the control points for the least-squares curve.
this.controlData = new Vector3[numControls];
for (i0 = 0; i0 < this.quantity[0]; ++i0)
{
for (i1 = 0; i1 < this.quantity[1]; ++i1)
{
this.controlData[i0] += inControls[i1] * prod[i0, i1];
}
}
// TRANSLATION NOTE
// In the original code the first and last controls are not set to the same position of the sample data
// but when reducing the control points of a curve I expect that
// the first and last controls are exactly int the same positions as the first and last controls of the original curve
// A similar situation we have with the BSplineFit class but vice versa.
// The original code sets the first and last points to be the same as the original control points,
// but in this case I do not expect them to be the same.
// END
// Set the first and last output control points to match the first
// and last input samples. This supports the application of
// reducing keyframe data with B-spline curves. The user expects
// that the curve passes through the first and last positions in
// order to support matching two consecutive keyframe sequences.
this.controlData[0] = inControls[0];
this.controlData[this.controlData.Length - 1] = inControls[inControls.Length - 1];
BasisFunctionInput input = new BasisFunctionInput(numControls, degree);
this.basisFunction = new BasisFunction(input);
}
public double GetLength(double t0, double t1)
{
double LowerBound(double[] array, int first, int last, double val)
{
for (int i = first; i < last; i++)
{
if (array[i] >= val)
{
return(array[i]);
}
}
return(double.NaN);
}
double speed(double t)
{
return(this.GetSpeed(t));
}
if (Math.Abs(this.segmentLength[0]) < double.Epsilon)
{
// Lazy initialization of lengths of segments.
int numSegments = this.segmentLength.Length;
double accumulated = 0;
for (int i = 0, ip1 = 1; i < numSegments; ++i, ++ip1)
{
this.segmentLength[i] = Integration.Romberg(this.rombergOrder, this.times[i], this.times[ip1], speed);
accumulated += this.segmentLength[i];
this.acumulatedLength[i] = accumulated;
}
}
t0 = Math.Max(t0, this.TMin);
t1 = Math.Min(t1, this.TMax);
double iter0 = LowerBound(this.times, 0, this.times.Length, t0);
int index0 = (int)(iter0 - this.times[0]);
double iter1 = LowerBound(this.times, 0, this.times.Length, t1);
int index1 = (int)(iter1 - this.times[0]);
double length;
if (index0 < index1)
{
length = 0;
if (t0 < iter0)
{
length += Integration.Romberg(this.rombergOrder, t0, this.times[index0], speed);
}
int isup;
if (t1 < iter1)
{
length += Integration.Romberg(this.rombergOrder, this.times[index1 - 1], t1, speed);
isup = index1 - 1;
}
else
{
isup = index1;
}
for (int i = index0; i < isup; ++i)
{
length += this.segmentLength[i];
}
}
else
{
length = Integration.Romberg(this.rombergOrder, t0, t1, speed);
}
return(length);
}