// 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); }