// Construction. The preconditions for calling the constructor are
// 1 <= degree && degree < numControls <= numSamples - degree - 1.
// The samples points are contiguous blocks of 'dimension' double values
// stored in sampleData.
public BSplineCurveFit(Vector3[] sampleData, int degree, int numControls)
{
this.numSamples = sampleData.Length;
this.sampleData = sampleData;
this.degree = degree;
this.numControls = numControls;
this.controlData = new Vector3[numControls];
Debug.Assert(1 <= degree && degree < numControls, "Invalid degree.");
Debug.Assert(sampleData != null, "Invalid sample data.");
Debug.Assert(numControls <= this.numSamples - degree - 1, "Invalid number of controls.");
BasisFunctionInput input = new BasisFunctionInput();
input.NumControls = numControls;
input.Degree = degree;
input.Uniform = true;
input.Periodic = false;
input.NumUniqueKnots = numControls - degree + 1;
input.UniqueKnots = new UniqueKnot[input.NumUniqueKnots];
input.UniqueKnots[0].T = 0.0;
input.UniqueKnots[0].Multiplicity = degree + 1;
int last = input.NumUniqueKnots - 1;
double factor = 1.0 / last;
for (int i = 1; i < last; ++i)
{
input.UniqueKnots[i].T = factor * i;
input.UniqueKnots[i].Multiplicity = 1;
}
input.UniqueKnots[last].T = 1.0;
input.UniqueKnots[last].Multiplicity = degree + 1;
this.basisFunction = new BasisFunction(input);
// Fit the data points with a B-spline curve using a least-squares
// error metric. The problem is of the form A^T*A*Q = A^T*P,
// where A^T*A is a banded matrix, P contains the sample data, and
// Q is the unknown vector of control points.
double tMultiplier = 1.0 / (this.numSamples - 1.0);
double t;
int i0, i1, i2, imin, imax;
// Construct the matrix A^T*A.
int degp1 = this.degree + 1;
int numBands = this.numControls > degp1 ? degp1 : this.degree;
BandedMatrix ATAMat = new BandedMatrix(this.numControls, numBands, numBands);
for (i0 = 0; i0 < this.numControls; i0++)
{
for (i1 = 0; i1 < i0; i1++)
{
ATAMat[i0, i1] = ATAMat[i1, i0];
}
int i1Max = i0 + this.degree;
if (i1Max >= this.numControls)
{
i1Max = this.numControls - 1;
}
for (i1 = i0; i1 <= i1Max; i1++)
{
double value = 0.0;
for (i2 = 0; i2 < this.numSamples; i2++)
{
t = tMultiplier * i2;
this.basisFunction.Evaluate(t, 0, out imin, out imax);
if (imin <= i0 && i0 <= imax && imin <= i1 && i1 <= imax)
{
double b0 = this.basisFunction.GetValue(0, i0);
double b1 = this.basisFunction.GetValue(0, i1);
value += b0 * b1;
}
}
ATAMat[i0, i1] = value;
}
}
// Construct the matrix A^T.
double[] ATMat = new double[this.numControls * this.numSamples];
for (i0 = 0; i0 < this.numControls; i0++)
{
for (i1 = 0; i1 < this.numSamples; i1++)
{
t = tMultiplier * i1;
this.basisFunction.Evaluate(t, 0, out imin, out imax);
if (imin <= i0 && i0 <= imax)
{
ATMat[i0 * this.numSamples + i1] = this.basisFunction.GetValue(0, i0);
}
}
}
// Compute X0 = (A^T*A)^{-1}*A^T by solving the linear system
// A^T*A*X = A^T.
bool solved = ATAMat.SolveSystem(ref ATMat, this.numSamples);
Debug.Assert(solved, "Failed to solve linear system.");
// The control points for the fitted curve are stored in the
// vector Q = X0*P, where P is the vector of sample data.
for (i0 = 0; i0 < this.numControls; i0++)
{
Vector3 Q = this.controlData[i0];
for (i1 = 0; i1 < this.numSamples; i1++)
{
Vector3 P = this.sampleData[i1];
double xValue = ATMat[i0 * this.numSamples + i1];
Q += xValue * P;
}
this.controlData[i0] = Q;
}
// TRANSLATION NOTE
// In the original code the first and last controls are set to the same position of the sample data
// but the purpose of fitting a curve that averages the points of the sample data I do not expect that
// the first and last controls are exactly int the same positions as the first and last points of the sample data
// A similar situation we have with the BSplineReduction class but vice versa.
// The original code do not set the first and last points to be the same as the original control points,
// but in this case I 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
// fitting 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] = this.sampleData[0];
//this.controlData[this.controlData.Length - 1] = this.sampleData[this.sampleData.Length - 1];
}
// Support for explicit creation in classes that have std::array
// members involving BasisFunction. This is a call-once function.
public void Create(BasisFunctionInput input)
{
Debug.Assert(input.NumControls >= 2, "Invalid number of control points.");
Debug.Assert(1 <= input.Degree && input.Degree < input.NumControls, "Invalid degree.");
Debug.Assert(input.NumUniqueKnots >= 2, "Invalid number of unique knots.");
this.numControls = input.Periodic ? input.NumControls + input.Degree : input.NumControls;
this.degree = input.Degree;
this.tMin = 0;
this.tMax = 0;
this.tLength = 0;
this.open = false;
this.uniform = input.Uniform;
this.periodic = input.Periodic;
this.jet = new double[4][][];
this.uniqueKnots = new UniqueKnot[input.UniqueKnots.Length];
input.UniqueKnots.CopyTo(this.uniqueKnots, 0);
double u = this.uniqueKnots[0].T;
for (int i = 1; i < input.NumUniqueKnots - 1; i++)
{
double uNext = this.uniqueKnots[i].T;
Debug.Assert(u < uNext, "Unique knots are not strictly increasing.");
u = uNext;
}
int mult0 = this.uniqueKnots[0].Multiplicity;
Debug.Assert(mult0 >= 1 && mult0 <= this.degree + 1, "Invalid first multiplicity.");
int mult1 = this.uniqueKnots[this.uniqueKnots.Length - 1].Multiplicity;
Debug.Assert(mult1 >= 1 && mult1 <= this.degree + 1, "Invalid last multiplicity.");
for (int i = 1; i <= input.NumUniqueKnots - 2; i++)
{
int mult = this.uniqueKnots[i].Multiplicity;
Debug.Assert(mult >= 1 && mult <= this.degree + 1, "Invalid interior multiplicity.");
}
this.open = mult0 == mult1 && mult0 == this.degree + 1;
this.knots = new double[this.numControls + this.degree + 1];
this.keys = new Key[input.NumUniqueKnots];
int sum = 0;
for (int i = 0, j = 0; i < input.NumUniqueKnots; i++)
{
double tCommon = this.uniqueKnots[i].T;
int mult = this.uniqueKnots[i].Multiplicity;
for (int k = 0; k < mult; k++, j++)
{
this.knots[j] = tCommon;
}
this.keys[i] = new Key(tCommon, sum - 1);
sum += mult;
}
this.tMin = this.knots[this.degree];
this.tMax = this.knots[this.numControls];
this.tLength = this.tMax - this.tMin;
int numRows = this.degree + 1;
int numCols = this.numControls + this.degree;
for (int i = 0; i < 4; ++i)
{
this.jet[i] = new double[numRows][];
for (int j = 0; j < numRows; j++)
{
this.jet[i][j] = new double[numCols];
}
}
}
// Construction. The preconditions for calling the constructor are
// 1 <= degree0 && degree0 + 1 < numControls0 <= numSamples0
// 1 <= degree1 && degree1 + 1 < numControls1 <= numSamples1
// The sample data must be in row-major order. The control data is
// also stored in row-major order.
public BSplineSurfaceFit(int degree0, int numControls0, int numSamples0, int degree1, int numControls1, int numSamples1, Vector3[] sampleData)
{
Debug.Assert(1 <= degree0 && degree0 + 1 < numControls0, "Invalid degree.");
Debug.Assert(numControls0 <= numSamples0, "Invalid number of controls.");
Debug.Assert(1 <= degree1 && degree1 + 1 < numControls1, "Invalid degree.");
Debug.Assert(numControls1 <= numSamples1, "Invalid number of controls.");
Debug.Assert(sampleData != null, "Invalid sample data.");
this.sampleData = sampleData;
this.controlData = new Vector3[numControls0 * numControls1];
this.degree = new[] { degree0, degree1 };
this.numSamples = new[] { numSamples0, numSamples1 };
this.numControls = new[] { numControls0, numControls1 };
this.basisFunctions = new BasisFunction[2];
BasisFunctionInput input = new BasisFunctionInput();
double[] tMultiplier = new double[2];
int dim;
for (dim = 0; dim < 2; dim++)
{
input.NumControls = this.numControls[dim];
input.Degree = this.degree[dim];
input.Uniform = true;
input.Periodic = false;
input.NumUniqueKnots = this.numControls[dim] - this.degree[dim] + 1;
input.UniqueKnots = new UniqueKnot[input.NumUniqueKnots];
input.UniqueKnots[0].T = 0.0;
input.UniqueKnots[0].Multiplicity = this.degree[dim] + 1;
int last = input.NumUniqueKnots - 1;
double factor = 1.0 / last;
for (int i = 1; i < last; i++)
{
input.UniqueKnots[i].T = factor * i;
input.UniqueKnots[i].Multiplicity = 1;
}
input.UniqueKnots[last].T = 1.0;
input.UniqueKnots[last].Multiplicity = this.degree[dim] + 1;
tMultiplier[dim] = 1.0 / (this.numSamples[dim] - 1.0);
this.basisFunctions[dim] = new BasisFunction(input);
}
// Fit the data points with a B-spline surface using a
// least-squares error metric. The problem is of the form
// A0^T*A0*Q*A1^T*A1 = A0^T*P*A1, where A0^T*A0 and A1^T*A1 are
// banded matrices, P contains the sample data, and Q is the
// unknown matrix of control points.
double t;
int i0, i1, i2, imin, imax;
// Construct the matrices A0^T*A0 and A1^T*A1.
BandedMatrix[] ATAMat =
{
new BandedMatrix(this.numControls[0], this.degree[0] + 1, this.degree[0] + 1),
new BandedMatrix(this.numControls[1], this.degree[1] + 1, this.degree[1] + 1)
};
for (dim = 0; dim < 2; dim++)
{
for (i0 = 0; i0 < this.numControls[dim]; i0++)
{
for (i1 = 0; i1 < i0; i1++)
{
ATAMat[dim][i0, i1] = ATAMat[dim][i1, i0];
}
int i1Max = i0 + this.degree[dim];
if (i1Max >= this.numControls[dim])
{
i1Max = this.numControls[dim] - 1;
}
for (i1 = i0; i1 <= i1Max; i1++)
{
double value = 0;
for (i2 = 0; i2 < this.numSamples[dim]; i2++)
{
t = tMultiplier[dim] * i2;
this.basisFunctions[dim].Evaluate(t, 0, out imin, out imax);
if (imin <= i0 && i0 <= imax && imin <= i1 && i1 <= imax)
{
double b0 = this.basisFunctions[dim].GetValue(0, i0);
double b1 = this.basisFunctions[dim].GetValue(0, i1);
value += b0 * b1;
}
}
ATAMat[dim][i0, i1] = value;
}
}
}
// Construct the matrices A0^T and A1^T. A[d]^T has
// mNumControls[d] rows and mNumSamples[d] columns.
double[][] ATMat = new double[2][];
for (dim = 0; dim < 2; dim++)
{
ATMat[dim] = new double[this.numControls[dim] * this.numSamples[dim]];
for (i0 = 0; i0 < this.numControls[dim]; i0++)
{
for (i1 = 0; i1 < this.numSamples[dim]; i1++)
{
t = tMultiplier[dim] * i1;
this.basisFunctions[dim].Evaluate(t, 0, out imin, out imax);
if (imin <= i0 && i0 <= imax)
{
ATMat[dim][i0 * this.numSamples[dim] + i1] = this.basisFunctions[dim].GetValue(0, i0);
}
}
}
}
// Compute X0 = (A0^T*A0)^{-1}*A0^T and X1 = (A1^T*A1)^{-1}*A1^T
// by solving the linear systems A0^T*A0*X0 = A0^T and
// A1^T*A1*X1 = A1^T.
for (dim = 0; dim < 2; dim++)
{
bool solved = ATAMat[dim].SolveSystem(ref ATMat[dim], this.numSamples[dim]);
Debug.Assert(solved, "Failed to solve linear system in BSplineSurfaceFit constructor.");
}
// The control points for the fitted surface are stored in the matrix
// Q = X0*P*X1^T, where P is the matrix of sample data.
for (i1 = 0; i1 < this.numControls[1]; i1++)
{
for (i0 = 0; i0 < this.numControls[0]; i0++)
{
Vector3 sum = Vector3.Zero;
for (int j1 = 0; j1 < this.numSamples[1]; j1++)
{
double x1Value = ATMat[1][i1 * this.numSamples[1] + j1];
for (int j0 = 0; j0 < this.numSamples[0]; j0++)
{
double x0Value = ATMat[0][i0 * this.numSamples[0] + j0];
Vector3 sample = this.sampleData[j0 + this.numSamples[0] * j1];
sum += x0Value * x1Value * sample;
}
}
this.controlData[i0 + this.numControls[0] * i1] = sum;
}
}
}
// Construction and destruction. The determination that the curve is
// open or floating is based on the multiplicities. The 'uniform'
// input is used to avoid misclassifications due to floating-point
// rounding errors. Specifically, the breakpoints might be equally
// spaced (uniform) as real numbers, but the floating-point
// representations can have rounding errors that cause the knot
// differences not to be exactly the same constant. A periodic curve
// can have uniform or nonuniform knots. This object makes copies of
// the input arrays.
public BasisFunction(BasisFunctionInput input)
{
this.Create(input);
}
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);
}