// Compute the inverse of the banded matrix. The return value is
// 'true' when the matrix is invertible, in which case the 'inverse'
// output is valid. The return value is 'false' when the matrix is
// not invertible, in which case 'inverse' is invalid and should not
// be used. The input matrix 'inverse' must be the same size as
// 'this'.
//
// 'bMatrix' must have the storage order specified by the template
// parameter.
public bool ComputeInverse(double[] inverse)
{
LexicoArray2 invA = new LexicoArray2(this.size, this.size, inverse);
BandedMatrix tmpA = this;
for (int row = 0; row < this.size; row++)
{
for (int col = 0; col < this.size; col++)
{
if (row != col)
{
invA[row, col] = 0.0;
}
else
{
invA[row, row] = 1.0;
}
}
}
// Forward elimination.
for (int row = 0; row < this.size; row++)
{
// The pivot must be nonzero in order to proceed.
double diag = tmpA[row, row];
if (Math.Abs(diag) < double.Epsilon)
{
return(false);
}
double invDiag = 1.0 / diag;
tmpA[row, row] = 1.0;
// Multiply the row to be consistent with diagonal term of 1.
int colMin = row + 1;
int colMax = colMin + this.uBands.Length;
if (colMax > this.size)
{
colMax = this.size;
}
int c;
for (c = colMin; c < colMax; c++)
{
tmpA[row, c] *= invDiag;
}
for (c = 0; c <= row; c++)
{
invA[row, c] *= invDiag;
}
// Reduce the remaining rows.
int rowMin = row + 1;
int rowMax = rowMin + this.lBands.Length;
if (rowMax > this.size)
{
rowMax = this.size;
}
for (int r = rowMin; r < rowMax; r++)
{
double mult = tmpA[r, row];
tmpA[r, row] = 0.0;
for (c = colMin; c < colMax; c++)
{
tmpA[r, c] -= mult * tmpA[row, c];
}
for (c = 0; c <= row; c++)
{
invA[r, c] -= mult * invA[row, c];
}
}
}
// Backward elimination.
for (int row = this.size - 1; row >= 1; row--)
{
int rowMax = row - 1;
int rowMin = row - this.uBands.Length;
if (rowMin < 0)
{
rowMin = 0;
}
for (int r = rowMax; r >= rowMin; r--)
{
double mult = tmpA[r, row];
tmpA[r, row] = 0.0;
for (int c = 0; c < this.size; c++)
{
invA[r, c] -= mult * invA[row, c];
}
}
}
return(true);
}
// 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];
}
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);
}
