BSplineSurfaceFit<Real>::BSplineSurfaceFit (int iDegree0,
int iControlQuantity0, int iSampleQuantity0, int iDegree1,
int iControlQuantity1, int iSampleQuantity1,
Vector3<Real>** aakSamplePoint)
{
assert(1 <= iDegree0 && iDegree0 < iControlQuantity0);
assert(iControlQuantity0 <= iSampleQuantity0);
assert(1 <= iDegree1 && iDegree1 < iControlQuantity1);
assert(iControlQuantity1 <= iSampleQuantity1);
m_aiDegree[0] = iDegree0;
m_aiSampleQuantity[0] = iSampleQuantity0;
m_aiControlQuantity[0] = iControlQuantity0;
m_aiDegree[1] = iDegree1;
m_aiSampleQuantity[1] = iSampleQuantity1;
m_aiControlQuantity[1] = iControlQuantity1;
m_aakSamplePoint = aakSamplePoint;
Allocate(iControlQuantity0,iControlQuantity1,m_aakControlPoint);
// The double-precision basis functions are used to help with the
// numerical round-off errors.
BSplineFitBasisd* apkDBasis[2];
double adTMultiplier[2];
int iDim;
for (iDim = 0; iDim < 2; iDim++)
{
m_apkBasis[iDim] = WM4_NEW BSplineFitBasis<Real>(
m_aiControlQuantity[iDim],m_aiDegree[iDim]);
apkDBasis[iDim] = WM4_NEW BSplineFitBasisd(
m_aiControlQuantity[iDim],m_aiDegree[iDim]);
adTMultiplier[iDim] = 1.0/(double)(m_aiSampleQuantity[iDim] - 1);
}
// 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 dT;
int i0, i1, i2, iMin, iMax;
// Construct the matrices A0^T*A0 and A1^T*A1.
BandedMatrixd* apkATAMat[2];
for (iDim = 0; iDim < 2; iDim++)
{
apkATAMat[iDim] = WM4_NEW BandedMatrixd(m_aiControlQuantity[iDim],
m_aiDegree[iDim]+1,m_aiDegree[iDim]+1);
for (i0 = 0; i0 < m_aiControlQuantity[iDim]; i0++)
{
for (i1 = 0; i1 < i0; i1++)
{
(*apkATAMat[iDim])(i0,i1) = (*apkATAMat[iDim])(i1,i0);
}
int i1Max = i0 + m_aiDegree[iDim];
if (i1Max >= m_aiControlQuantity[iDim])
{
i1Max = m_aiControlQuantity[iDim] - 1;
}
for (i1 = i0; i1 <= i1Max; i1++)
{
double dValue = 0.0;
for (i2 = 0; i2 < m_aiSampleQuantity[iDim]; i2++)
{
dT = adTMultiplier[iDim]*(double)i2;
apkDBasis[iDim]->Compute(dT,iMin,iMax);
if (iMin <= i0 && i0 <= iMax && iMin <= i1 && i1 <= iMax)
{
double dB0 = apkDBasis[iDim]->GetValue(i0 - iMin);
double dB1 = apkDBasis[iDim]->GetValue(i1 - iMin);
dValue += dB0*dB1;
}
}
(*apkATAMat[iDim])(i0,i1) = dValue;
}
}
}
// Construct the matrices A0^T and A1^T.
double** aaadATMat[2];
for (iDim = 0; iDim < 2; iDim++)
{
Allocate(m_aiSampleQuantity[iDim],m_aiControlQuantity[iDim],
aaadATMat[iDim]);
memset(aaadATMat[iDim][0],0,m_aiControlQuantity[iDim]
*m_aiSampleQuantity[iDim]*sizeof(double));
for (i0 = 0; i0 < m_aiControlQuantity[iDim]; i0++)
{
for (i1 = 0; i1 < m_aiSampleQuantity[iDim]; i1++)
{
dT = adTMultiplier[iDim]*(double)i1;
apkDBasis[iDim]->Compute(dT,iMin,iMax);
if (iMin <= i0 && i0 <= iMax)
{
aaadATMat[iDim][i0][i1] =
apkDBasis[iDim]->GetValue(i0 - iMin);
}
}
}
}
// 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 (iDim = 0; iDim < 2; iDim++)
{
bool bSolved = apkATAMat[iDim]->SolveSystem(aaadATMat[iDim],
m_aiSampleQuantity[iDim]);
assert(bSolved);
(void)bSolved; // Avoid warning in release build.
}
// 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 (i0 = 0; i0 < m_aiControlQuantity[0]; i0++)
{
for (i1 = 0; i1 < m_aiControlQuantity[1]; i1++)
{
Vector3<Real> kSum = Vector3<Real>::ZERO;
for (int j0 = 0; j0 < m_aiSampleQuantity[0]; j0++)
{
Real fX0Value = (Real)aaadATMat[0][i0][j0];
for (int j1 = 0; j1 < m_aiSampleQuantity[1]; j1++)
{
Real fX1Value = (Real)aaadATMat[1][i1][j1];
kSum += (fX0Value*fX1Value)*m_aakSamplePoint[j0][j1];
}
}
m_aakControlPoint[i0][i1] = kSum;
}
}
for (iDim = 0; iDim < 2; iDim++)
{
WM4_DELETE apkDBasis[iDim];
WM4_DELETE apkATAMat[iDim];
Deallocate(aaadATMat[iDim]);
}
}
BSplineCurveFit<Real>::BSplineCurveFit (int dimension, int numSamples,
const Real* sampleData, int degree, int numControls)
:
mBasis(numControls, degree)
{
assertion(dimension >= 1, "Invalid input\n");
assertion(1 <= degree && degree < numControls, "Invalid input\n");
assertion(numControls <= numSamples, "Invalid input\n");
mDimension = dimension;
mNumSamples = numSamples;
mSampleData = sampleData;
mDegree = degree;
mNumControls = numControls;
mControlData = new1<Real>(mDimension*numControls);
// The double-precision basis functions are used to help with the
// numerical round-off errors.
BSplineFitBasisd dBasis(mNumControls,mDegree);
double tMultiplier = 1.0/(double)(mNumSamples - 1);
// 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 t;
int i0, i1, i2, imin, imax, j;
// Construct the matrix A^T*A.
BandedMatrixd* ATAMat = new0 BandedMatrixd(mNumControls,
mDegree + 1, mDegree + 1);
for (i0 = 0; i0 < mNumControls; ++i0)
{
for (i1 = 0; i1 < i0; ++i1)
{
(*ATAMat)(i0, i1) = (*ATAMat)(i1, i0);
}
int i1Max = i0 + mDegree;
if (i1Max >= mNumControls)
{
i1Max = mNumControls - 1;
}
for (i1 = i0; i1 <= i1Max; ++i1)
{
double value = 0.0;
for (i2 = 0; i2 < mNumSamples; ++i2)
{
t = tMultiplier*(double)i2;
dBasis.Compute(t, imin, imax);
if (imin <= i0 && i0 <= imax && imin <= i1 && i1 <= imax)
{
double dB0 = dBasis.GetValue(i0 - imin);
double dB1 = dBasis.GetValue(i1 - imin);
value += dB0*dB1;
}
}
(*ATAMat)(i0, i1) = value;
}
}
// Construct the matrix A^T.
double** ATMat = new2<double>(mNumSamples, mNumControls);
memset(ATMat[0], 0, mNumControls*mNumSamples*sizeof(double));
for (i0 = 0; i0 < mNumControls; ++i0)
{
for (i1 = 0; i1 < mNumSamples; ++i1)
{
t = tMultiplier*(double)i1;
dBasis.Compute(t, imin, imax);
if (imin <= i0 && i0 <= imax)
{
ATMat[i0][i1] = dBasis.GetValue(i0 - imin);
}
}
}
// Compute X0 = (A^T*A)^{-1}*A^T by solving the linear system
// A^T*A*X = A^T.
bool solved = ATAMat->SolveSystem(ATMat,mNumSamples);
assertion(solved, "Failed to solve linear system\n");
WM5_UNUSED(solved);
// The control points for the fitted curve are stored in the vector
// Q = X0*P, where P is the vector of sample data.
memset(mControlData, 0, mNumControls*mDimension*sizeof(Real));
for (i0 = 0; i0 < mNumControls; ++i0)
{
Real* Q = mControlData + i0*mDimension;
for (i1 = 0; i1 < mNumSamples; ++i1)
{
const Real* P = mSampleData + i1*mDimension;
Real xValue = (Real)ATMat[i0][i1];
for (j = 0; j < mDimension; j++)
{
Q[j] += xValue*P[j];
}
}
}
// 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.
Real* cEnd0 = mControlData;
const Real* sEnd0 = mSampleData;
Real* cEnd1 = &mControlData[mDimension*(mNumControls-1)];
const Real* sEnd1 = &mSampleData[mDimension*(mNumSamples-1)];
for (j = 0; j < mDimension; j++)
{
*cEnd0++ = *sEnd0++;
*cEnd1++ = *sEnd1++;
}
delete2(ATMat);
delete0(ATAMat);
}
BSplineSurfaceFit<Real>::BSplineSurfaceFit (int degree0, int numControls0,
int numSamples0, int degree1, int numControls1, int numSamples1,
Vector3<Real>** samples)
{
assertion(1 <= degree0 && degree0 + 1 < numControls0, "Invalid input\n");
assertion(numControls0 <= numSamples0, "Invalid input\n");
assertion(1 <= degree1 && degree1 + 1 < numControls1, "Invalid input\n");
assertion(numControls1 <= numSamples1, "Invalid input\n");
mDegree[0] = degree0;
mNumSamples[0] = numSamples0;
mNumControls[0] = numControls0;
mDegree[1] = degree1;
mNumSamples[1] = numSamples1;
mNumControls[1] = numControls1;
mSamples = samples;
mControls = new2<Vector3<Real> >(numControls0, numControls1);
// The double-precision basis functions are used to help with the
// numerical round-off errors.
BSplineFitBasisd* dBasis[2];
double tMultiplier[2];
int dim;
for (dim = 0; dim < 2; ++dim)
{
mBasis[dim] = new0 BSplineFitBasis<Real>(mNumControls[dim],
mDegree[dim]);
dBasis[dim] = new0 BSplineFitBasisd(mNumControls[dim],
mDegree[dim]);
tMultiplier[dim] = 1.0/(double)(mNumSamples[dim] - 1);
}
// 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.
BandedMatrixd* ATAMat[2];
for (dim = 0; dim < 2; ++dim)
{
ATAMat[dim] = new0 BandedMatrixd(mNumControls[dim],
mDegree[dim] + 1, mDegree[dim] + 1);
for (i0 = 0; i0 < mNumControls[dim]; ++i0)
{
for (i1 = 0; i1 < i0; ++i1)
{
(*ATAMat[dim])(i0, i1) = (*ATAMat[dim])(i1, i0);
}
int i1Max = i0 + mDegree[dim];
if (i1Max >= mNumControls[dim])
{
i1Max = mNumControls[dim] - 1;
}
for (i1 = i0; i1 <= i1Max; ++i1)
{
double value = 0.0;
for (i2 = 0; i2 < mNumSamples[dim]; ++i2)
{
t = tMultiplier[dim]*(double)i2;
dBasis[dim]->Compute(t, imin, imax);
if (imin <= i0 && i0 <= imax && imin <= i1 && i1 <= imax)
{
double b0 = dBasis[dim]->GetValue(i0 - imin);
double b1 = dBasis[dim]->GetValue(i1 - imin);
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[2];
for (dim = 0; dim < 2; dim++)
{
ATMat[dim] = new2<double>(mNumSamples[dim], mNumControls[dim]);
memset(ATMat[dim][0], 0,
mNumControls[dim]*mNumSamples[dim]*sizeof(double));
for (i0 = 0; i0 < mNumControls[dim]; ++i0)
{
for (i1 = 0; i1 < mNumSamples[dim]; ++i1)
{
t = tMultiplier[dim]*(double)i1;
dBasis[dim]->Compute(t, imin, imax);
if (imin <= i0 && i0 <= imax)
{
ATMat[dim][i0][i1] = dBasis[dim]->GetValue(i0 - imin);
}
}
}
}
// 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(ATMat[dim], mNumSamples[dim]);
assertion(solved, "Failed to solve linear system\n");
WM5_UNUSED(solved);
}
// 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 < mNumControls[1]; ++i1)
{
for (i0 = 0; i0 < mNumControls[0]; ++i0)
{
Vector3<Real> sum = Vector3<Real>::ZERO;
for (int j1 = 0; j1 < mNumSamples[1]; ++j1)
{
Real x1Value = (Real)ATMat[1][i1][j1];
for (int j0 = 0; j0 < mNumSamples[0]; ++j0)
{
Real x0Value = (Real)ATMat[0][i0][j0];
sum += (x0Value*x1Value)*mSamples[j1][j0];
}
}
mControls[i1][i0] = sum;
}
}
for (dim = 0; dim < 2; ++dim)
{
delete0(dBasis[dim]);
delete0(ATAMat[dim]);
delete2(ATMat[dim]);
}
}
