//----------------------------------------------------------------------------
void TestEllipsoidsE1ContainsE0 ()
{
Ellipsoid3f ellipsoid0;
ellipsoid0.Center = Vector3f::ZERO;
ellipsoid0.Axis[0] = Vector3f::UNIT_X;
ellipsoid0.Axis[1] = Vector3f::UNIT_Y;
ellipsoid0.Axis[2] = Vector3f::UNIT_Z;
ellipsoid0.Extent[0] = 1.0f;
ellipsoid0.Extent[1] = 1.0f;
ellipsoid0.Extent[2] = 1.0f;
Ellipsoid3f ellipsoid1;
ellipsoid1.Center = Vector3f(0.01f, 0.02f, 0.03f);
ellipsoid1.Axis[0] = Vector3f::UNIT_X;
ellipsoid1.Axis[1] = Vector3f::UNIT_Y;
ellipsoid1.Axis[2] = Vector3f::UNIT_Z;
ellipsoid1.Extent[0] = 9.0f;
ellipsoid1.Extent[1] = 10.0f;
ellipsoid1.Extent[2] = 8.0f;
AffineTransform(3, ellipsoid0, ellipsoid1);
IntrEllipsoid3Ellipsoid3f calc(ellipsoid0, ellipsoid1);
IntrEllipsoid3Ellipsoid3f::Classification type = calc.GetClassification();
assertion(
type == IntrEllipsoid3Ellipsoid3f::EC_ELLIPSOID1_CONTAINS_ELLIPSOID0,
"Incorrect result.\n");
WM5_UNUSED(type);
}
//----------------------------------------------------------------------------
void TestEllipsoidsIntersecting ()
{
Ellipsoid3f ellipsoid0;
ellipsoid0.Center = Vector3f::ZERO;
ellipsoid0.Axis[0] = Vector3f::UNIT_X;
ellipsoid0.Axis[1] = Vector3f::UNIT_Y;
ellipsoid0.Axis[2] = Vector3f::UNIT_Z;
ellipsoid0.Extent[0] = 1.0f;
ellipsoid0.Extent[1] = 1.0f;
ellipsoid0.Extent[2] = 1.0f;
Ellipsoid3f ellipsoid1;
ellipsoid1.Center = Vector3f(0.01f, 0.02f, 0.03f);
ellipsoid1.Axis[0] = Vector3f::UNIT_X;
ellipsoid1.Axis[1] = Vector3f::UNIT_Y;
ellipsoid1.Axis[2] = Vector3f::UNIT_Z;
ellipsoid1.Extent[0] = 0.1f;
ellipsoid1.Extent[1] = 10.0f;
ellipsoid1.Extent[2] = 8.0f;
AffineTransform(4, ellipsoid0, ellipsoid1);
IntrEllipsoid3Ellipsoid3f calc(ellipsoid0, ellipsoid1);
IntrEllipsoid3Ellipsoid3f::Classification type = calc.GetClassification();
assertion(
type == IntrEllipsoid3Ellipsoid3f::EC_ELLIPSOIDS_INTERSECTING,
"Incorrect result.\n");
WM5_UNUSED(type);
}
//----------------------------------------------------------------------------
void TestEllipsoidsSeparated1 ()
{
Ellipsoid3f ellipsoid0;
ellipsoid0.Center = Vector3f::ZERO;
ellipsoid0.Axis[0] = Vector3f::UNIT_X;
ellipsoid0.Axis[1] = Vector3f::UNIT_Y;
ellipsoid0.Axis[2] = Vector3f::UNIT_Z;
ellipsoid0.Extent[0] = 1.0f;
ellipsoid0.Extent[1] = 1.0f;
ellipsoid0.Extent[2] = 1.0f;
Ellipsoid3f ellipsoid1;
ellipsoid1.Center = Vector3f(4.0f, 4.0f, 4.0f);
ellipsoid1.Axis[0] = Vector3f::UNIT_X;
ellipsoid1.Axis[1] = Vector3f::UNIT_Y;
ellipsoid1.Axis[2] = Vector3f::UNIT_Z;
ellipsoid1.Extent[0] = 0.1f;
ellipsoid1.Extent[1] = 0.3f;
ellipsoid1.Extent[2] = 0.2f;
AffineTransform(1, ellipsoid0, ellipsoid1);
IntrEllipsoid3Ellipsoid3f calc(ellipsoid0, ellipsoid1);
IntrEllipsoid3Ellipsoid3f::Classification type = calc.GetClassification();
assertion(type == IntrEllipsoid3Ellipsoid3f::EC_ELLIPSOIDS_SEPARATED,
"Incorrect result.\n");
WM5_UNUSED(type);
}
Delaunay1<Real>::Delaunay1 (const char* filename, int mode)
:
Delaunay<Real>(0, (Real)0, false, Query::QT_REAL)
{
mVertices = 0;
bool loaded = Load(filename, mode);
assertion(loaded, "Cannot open file %s\n", filename);
WM5_UNUSED(loaded);
}
//----------------------------------------------------------------------------
void CollisionsMovingSpheres::UpdateSpheres ()
{
float contactTime;
Colliders::CollisionType eType = mColliders.Find(mSimTime, mVelocity0,
mVelocity1, contactTime);
WM5_UNUSED(eType);
if (contactTime < 0.0f)
{
contactTime = 0.0f;
}
if (contactTime <= mSimTime)
{
// Move the spheres through the time to make contact.
mSphere0.Center += contactTime*mVelocity0;
mSphere1.Center += contactTime*mVelocity1;
Vector3f contactPoint = mColliders.GetContactPoint();
// Compute the unit-length normals at the contact point.
Vector3f normal0 = contactPoint - mSphere0.Center;
normal0.Normalize();
Vector3f normal1 = contactPoint - mSphere1.Center;
normal1.Normalize();
// Reflect the velocities through the normals.
mVelocity0 -= 2.0f*mVelocity0.Dot(normal1)*normal1;
mVelocity1 -= 2.0f*mVelocity1.Dot(normal0)*normal0;
mSimTime -= contactTime;
}
else
{
mSphere0.Center += mSimDelta*mVelocity0;
mSphere1.Center += mSimDelta*mVelocity1;
mSimTime = mSimDelta;
}
// Keep the spheres inside the world sphere.
Vector3f diff = mSphere0.Center - mBoundingSphere.Center;
float length = diff.Normalize();
if (length >= mBoundingSphere.Radius)
{
mSphere0.Center = mBoundingSphere.Radius*diff;
mVelocity0 -= 2.0f*mVelocity0.Dot(diff)*diff;
}
diff = mSphere1.Center - mBoundingSphere.Center;
length = diff.Normalize();
if (length >= mBoundingSphere.Radius)
{
mSphere1.Center = mBoundingSphere.Radius*diff;
mVelocity1 -= 2.0f*mVelocity1.Dot(diff)*diff;
}
mMesh0->LocalTransform.SetTranslate(mSphere0.Center);
mMesh1->LocalTransform.SetTranslate(mSphere1.Center);
mScene->Update();
}
ConvexHull2<Real>::ConvexHull2 ( const char* filename, int mode )
:
ConvexHull<Real>( 0, ( Real )0, false, Query::QT_REAL ),
mVertices( 0 ),
mSVertices( 0 ),
mQuery( 0 )
{
bool loaded = Load( filename, mode );
assertion( loaded, "Cannot open file %s\n", filename );
WM5_UNUSED( loaded );
}
void NaturalSpline1<Real>::CreateClampedSpline ( Real slopeFirst,
Real slopeLast )
{
mB = new1<Real>( mNumSegments + 1 );
mC = new1<Real>( mNumSegments );
mD = new1<Real>( mNumSegments );
Real* delta = new1<Real>( mNumSamples );
Real* invDelta = new1<Real>( mNumSamples );
Real* fDeriv = new1<Real>( mNumSamples );
int i;
for ( i = 0; i < mNumSamples; ++i )
{
delta[i] = mTimes[i + 1] - mTimes[i];
invDelta[i] = ( ( Real )1 ) / delta[i];
fDeriv[i] = ( mA[i + 1] - mA[i] ) * invDelta[i];
}
const int numSegmentsM1 = mNumSegments - 1;
Real* diagonal = new1<Real>( numSegmentsM1 );
Real* rhs = new1<Real>( numSegmentsM1 );
for ( i = 0; i < numSegmentsM1; ++i )
{
diagonal[i] = ( ( Real )2 ) * ( delta[i + 1] + delta[i] );
rhs[i] = ( ( Real )3 ) * ( delta[i] * fDeriv[i + 1] + delta[i + 1] * fDeriv[i] );
}
rhs[0] -= slopeFirst * delta[1];
rhs[mNumSegments - 2] -= slopeLast * delta[mNumSegments - 2];
// The boundary conditions.
mB[0] = slopeFirst;
mB[mNumSegments] = slopeLast;
// The linear system that determines B[1] through B[numSegs-1].
bool solved = LinearSystem<Real>().SolveTri( numSegmentsM1, &delta[2],
diagonal, delta, rhs, &mB[1] );
assertion( solved, "Failed to solve linear system\n" );
WM5_UNUSED( solved );
const Real oneThird = ( ( Real )1 ) / ( Real )3;
for ( i = 0; i < mNumSegments; ++i )
{
mC[i] = ( ( ( Real )3 ) * fDeriv[i] - mB[i + 1] - ( ( Real )2 ) * mB[i] ) * invDelta[i];
mD[i] = oneThird * ( mB[i + 1] - mB[i] -
( ( Real )2 ) * delta[i] * mC[i] ) * invDelta[i] * invDelta[i];
}
delete1( delta );
delete1( invDelta );
delete1( fDeriv );
delete1( diagonal );
delete1( rhs );
}
//----------------------------------------------------------------------------
void NonlocalBlowup::SquareGaussFour0p50 (float dt, float dx, float dy)
{
BuildSquareDomain();
BuildInitialDataGaussFour();
float p = 0.50f;
bool success = false;
mSolver = new0 NonlocalSolver2(DIMENSION, DIMENSION, &mInitialData,
&mDomain, dt, dx, dy, p, success);
assertion(success, "Failed to create solver.\n");
WM5_UNUSED(success);
mPrefix = ThePath + "Movies/SquareGaussFour0p50_";
}
void NaturalSpline1<Real>::CreateFreeSpline ()
{
mB = new1<Real>( mNumSegments );
mC = new1<Real>( mNumSegments + 1 );
mD = new1<Real>( mNumSegments );
Real* delta = new1<Real>( mNumSamples );
Real* invDelta = new1<Real>( mNumSamples );
Real* fDeriv = new1<Real>( mNumSamples );
int i;
for ( i = 0; i < mNumSamples; ++i )
{
delta[i] = mTimes[i + 1] - mTimes[i];
invDelta[i] = ( ( Real )1 ) / delta[i];
fDeriv[i] = ( mA[i + 1] - mA[i] ) * invDelta[i];
}
const int numSegmentsM1 = mNumSegments - 1;
Real* diagonal = new1<Real>( numSegmentsM1 );
Real* rhs = new1<Real>( numSegmentsM1 );
for ( i = 0; i < numSegmentsM1; ++i )
{
diagonal[i] = ( ( Real )2 ) * ( delta[i + 1] + delta[i] );
rhs[i] = ( ( Real )3 ) * ( fDeriv[i + 1] - fDeriv[i] );
}
// The boundary conditions.
mC[0] = ( Real )0;
mC[mNumSegments] = ( Real )0;
// The linear system that determines C[1] through C[numSegs-1].
bool solved = LinearSystem<Real>().SolveTri( numSegmentsM1, &delta[1],
diagonal, &delta[1], rhs, &mC[1] );
assertion( solved, "Failed to solve linear system\n" );
WM5_UNUSED( solved );
const Real oneThird = ( ( Real )1 ) / ( Real )3;
for ( i = 0; i < mNumSegments; ++i )
{
mB[i] = fDeriv[i] - delta[i] * oneThird * ( mC[i + 1] + ( ( Real )2 ) * mC[i] );
mD[i] = oneThird * ( mC[i + 1] - mC[i] ) * invDelta[i];
}
delete1( delta );
delete1( invDelta );
delete1( fDeriv );
delete1( diagonal );
delete1( rhs );
}
//----------------------------------------------------------------------------
void NonlocalBlowup::NonconvexDomain0p50 (float dt, float dx, float dy)
{
BuildPolygonDomain();
//BuildInitialData0();
//BuildInitialDataGaussXY(); // Appears to blow up everywhere.
BuildInitialDataGaussXYOff();
float p = 0.50f;
bool success = false;
mSolver = new0 NonlocalSolver2(DIMENSION, DIMENSION, &mInitialData,
&mDomain, dt, dx, dy, p, success);
assertion(success, "Failed to create solver.\n");
WM5_UNUSED(success);
mPrefix = ThePath + "Movies/Nonconvex0p50_";
}
//----------------------------------------------------------------------------
bool GpuLocalSolver2::OnPreIteration (uint64_t iteration)
{
#ifdef PRE_GAUSSSEIDEL_SAVE
int j0 = mDimension[0]*(mDimension[1]/2);
for (int i0 = 0; i0 < mDimension[0]; ++i0, ++j0)
{
mSlice[i0] = mReadBack[j0];
}
int frame = (int)iteration;
SaveGraph(mFolder, frame, 100.0f, mDimension[0], mSlice);
float umax = mSlice[mDimension[0]/2];
std::cout << "frame = " << frame << " : umax = " << umax << std::endl;
#else
WM5_UNUSED(iteration);
#endif
return true;
}
//----------------------------------------------------------------------------
bool GpuLocalSolver3::OnPreIteration (uint64_t iteration)
{
#ifdef PRE_GAUSSSEIDEL_SAVE
int u, v;
for (int i0 = 0; i0 < mDimension[0]; ++i0)
{
Map3Dto2D(i0, mDimension[1]/2, mDimension[2]/2, u, v);
mSlice[i0] = mReadBack[u + mBound[0]*v];
}
int frame = (int)iteration;
SaveGraph(mFolder, frame, 100.0f, mDimension[0], mSlice);
float umax = mSlice[mDimension[0]/2];
std::cout << "frame = " << frame << " : umax = " << umax << std::endl;
#else
WM5_UNUSED(iteration);
#endif
return true;
}
//----------------------------------------------------------------------------
void TestEllipsesE1ContainsE0 ()
{
Ellipse2f ellipse0;
ellipse0.Center = Vector2f::ZERO;
ellipse0.Axis[0] = Vector2f::UNIT_X;
ellipse0.Axis[1] = Vector2f::UNIT_Y;
ellipse0.Extent[0] = 1.0f;
ellipse0.Extent[1] = 1.0f;
Ellipse2f ellipse1;
ellipse1.Center = Vector2f(0.01f, 0.02f);
ellipse1.Axis[0] = Vector2f::UNIT_X;
ellipse1.Axis[1] = Vector2f::UNIT_Y;
ellipse1.Extent[0] = 9.0f;
ellipse1.Extent[1] = 10.0f;
AffineTransform(3, ellipse0, ellipse1);
IntrEllipse2Ellipse2f calc(ellipse0, ellipse1);
IntrEllipse2Ellipse2f::Classification type = calc.GetClassification();
assertion(type == IntrEllipse2Ellipse2f::EC_ELLIPSE1_CONTAINS_ELLIPSE0,
"Incorrect result.\n");
WM5_UNUSED(type);
}
//----------------------------------------------------------------------------
void TestEllipsesIntersecting ()
{
Ellipse2f ellipse0;
ellipse0.Center = Vector2f::ZERO;
ellipse0.Axis[0] = Vector2f::UNIT_X;
ellipse0.Axis[1] = Vector2f::UNIT_Y;
ellipse0.Extent[0] = 1.0f;
ellipse0.Extent[1] = 1.0f;
Ellipse2f ellipse1;
ellipse1.Center = Vector2f(0.01f, 0.02f);
ellipse1.Axis[0] = Vector2f::UNIT_X;
ellipse1.Axis[1] = Vector2f::UNIT_Y;
ellipse1.Extent[0] = 0.1f;
ellipse1.Extent[1] = 10.0f;
AffineTransform(4, ellipse0, ellipse1);
IntrEllipse2Ellipse2f calc(ellipse0, ellipse1);
IntrEllipse2Ellipse2f::Classification type = calc.GetClassification();
assertion(type == IntrEllipse2Ellipse2f::EC_ELLIPSES_INTERSECTING,
"Incorrect result.\n");
WM5_UNUSED(type);
}
//----------------------------------------------------------------------------
void TestEllipsesSeparated1 ()
{
Ellipse2f ellipse0;
ellipse0.Center = Vector2f::ZERO;
ellipse0.Axis[0] = Vector2f::UNIT_X;
ellipse0.Axis[1] = Vector2f::UNIT_Y;
ellipse0.Extent[0] = 1.0f;
ellipse0.Extent[1] = 1.0f;
Ellipse2f ellipse1;
ellipse1.Center = Vector2f(4.0f, 4.0f);
ellipse1.Axis[0] = Vector2f::UNIT_X;
ellipse1.Axis[1] = Vector2f::UNIT_Y;
ellipse1.Extent[0] = 0.1f;
ellipse1.Extent[1] = 0.3f;
AffineTransform(1, ellipse0, ellipse1);
IntrEllipse2Ellipse2f calc(ellipse0, ellipse1);
IntrEllipse2Ellipse2f::Classification type = calc.GetClassification();
assertion(type == IntrEllipse2Ellipse2f::EC_ELLIPSES_SEPARATED,
"Incorrect result.\n");
WM5_UNUSED(type);
}
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]);
}
}
void NaturalSpline1<Real>::CreatePeriodicSpline ()
{
mB = new1<Real>( mNumSegments );
mC = new1<Real>( mNumSegments );
mD = new1<Real>( mNumSegments );
#if 1
// Solving the system using a standard linear solver appears to be
// numerically stable.
const int size = 4 * mNumSegments;
GMatrix<Real> mat( size, size );
GVector<Real> rhs( size );
int i, j, k;
Real delta, delta2, delta3;
for ( i = 0, j = 0; i < mNumSegments - 1; ++i, j += 4 )
{
delta = mTimes[i + 1] - mTimes[i];
delta2 = delta * delta;
delta3 = delta * delta2;
mat[j + 0][j + 0] = ( Real )1;
mat[j + 0][j + 1] = ( Real )0;
mat[j + 0][j + 2] = ( Real )0;
mat[j + 0][j + 3] = ( Real )0;
mat[j + 1][j + 0] = ( Real )1;
mat[j + 1][j + 1] = delta;
mat[j + 1][j + 2] = delta2;
mat[j + 1][j + 3] = delta3;
mat[j + 2][j + 0] = ( Real )0;
mat[j + 2][j + 1] = ( Real )1;
mat[j + 2][j + 2] = ( ( Real )2 ) * delta;
mat[j + 2][j + 3] = ( ( Real )3 ) * delta2;
mat[j + 3][j + 0] = ( Real )0;
mat[j + 3][j + 1] = ( Real )0;
mat[j + 3][j + 2] = ( Real )1;
mat[j + 3][j + 3] = ( ( Real )3 ) * delta;
k = j + 4;
mat[j + 0][k + 0] = ( Real )0;
mat[j + 0][k + 1] = ( Real )0;
mat[j + 0][k + 2] = ( Real )0;
mat[j + 0][k + 3] = ( Real )0;
mat[j + 1][k + 0] = ( Real ) - 1;
mat[j + 1][k + 1] = ( Real )0;
mat[j + 1][k + 2] = ( Real )0;
mat[j + 1][k + 3] = ( Real )0;
mat[j + 2][k + 0] = ( Real )0;
mat[j + 2][k + 1] = ( Real ) - 1;
mat[j + 2][k + 2] = ( Real )0;
mat[j + 2][k + 3] = ( Real )0;
mat[j + 3][k + 0] = ( Real )0;
mat[j + 3][k + 1] = ( Real )0;
mat[j + 3][k + 2] = ( Real ) - 1;
mat[j + 3][k + 3] = ( Real )0;
}
delta = mTimes[i + 1] - mTimes[i];
delta2 = delta * delta;
delta3 = delta * delta2;
mat[j + 0][j + 0] = ( Real )1;
mat[j + 0][j + 1] = ( Real )0;
mat[j + 0][j + 2] = ( Real )0;
mat[j + 0][j + 3] = ( Real )0;
mat[j + 1][j + 0] = ( Real )1;
mat[j + 1][j + 1] = delta;
mat[j + 1][j + 2] = delta2;
mat[j + 1][j + 3] = delta3;
mat[j + 2][j + 0] = ( Real )0;
mat[j + 2][j + 1] = ( Real )1;
mat[j + 2][j + 2] = ( ( Real )2 ) * delta;
mat[j + 2][j + 3] = ( ( Real )3 ) * delta2;
mat[j + 3][j + 0] = ( Real )0;
mat[j + 3][j + 1] = ( Real )0;
mat[j + 3][j + 2] = ( Real )1;
mat[j + 3][j + 3] = ( ( Real )3 ) * delta;
k = 0;
mat[j + 0][k + 0] = ( Real )0;
mat[j + 0][k + 1] = ( Real )0;
mat[j + 0][k + 2] = ( Real )0;
mat[j + 0][k + 3] = ( Real )0;
mat[j + 1][k + 0] = ( Real ) - 1;
mat[j + 1][k + 1] = ( Real )0;
mat[j + 1][k + 2] = ( Real )0;
mat[j + 1][k + 3] = ( Real )0;
mat[j + 2][k + 0] = ( Real )0;
mat[j + 2][k + 1] = ( Real ) - 1;
mat[j + 2][k + 2] = ( Real )0;
mat[j + 2][k + 3] = ( Real )0;
mat[j + 3][k + 0] = ( Real )0;
mat[j + 3][k + 1] = ( Real )0;
mat[j + 3][k + 2] = ( Real ) - 1;
mat[j + 3][k + 3] = ( Real )0;
for ( i = 0, j = 0; i < mNumSegments; ++i, j += 4 )
{
rhs[j + 0] = mA[i];
rhs[j + 1] = ( Real )0;
rhs[j + 2] = ( Real )0;
rhs[j + 3] = ( Real )0;
}
GVector<Real> coeff( size );
bool solved = LinearSystem<Real>().Solve( mat, rhs, coeff );
assertion( solved, "Failed to solve linear system\n" );
WM5_UNUSED( solved );
for ( i = 0, j = 0; i < mNumSegments; ++i )
{
j++;
mB[i] = coeff[j++];
mC[i] = coeff[j++];
mD[i] = coeff[j++];
}
#endif
#if 0
// Solving the system using the equations derived in the PDF
// "Fitting a Natural Spline to Samples of the Form (t,f(t))"
// is ill-conditioned. TODO: Find a way to row-reduce the matrix of the
// PDF in a numerically stable manner yet retaining the O(n) asymptotic
// behavior.
// Compute the inverses M[i]^{-1}.
const int numSegmentsM1 = mNumSegments - 1;
Matrix4<Real>* invM = new1<Matrix4<Real> >( numSegmentsM1 );
Real delta;
int i;
for ( i = 0; i < numSegmentsM1; i++ )
{
delta = mTimes[i + 1] - mTimes[i];
Real invDelta1 = ( ( Real )1 ) / delta;
Real invDelta2 = invDelta1 / delta;
Real invDelta3 = invDelta2 / delta;
Matrix4<Real>& invMi = invM[i];
invMi[0][0] = ( Real )1;
invMi[0][1] = ( Real )0;
invMi[0][2] = ( Real )0;
invMi[0][3] = ( Real )0;
invMi[1][0] = ( ( Real )( -3 ) ) * invDelta1;
invMi[1][1] = ( ( Real )3 ) * invDelta1;
invMi[1][2] = ( Real )( -2 );
invMi[1][3] = delta;
invMi[2][0] = ( ( Real )3 ) * invDelta2;
invMi[2][1] = ( ( Real )( -3 ) ) * invDelta2;
invMi[2][2] = ( ( Real )3 ) * invDelta1;
invMi[2][3] = ( Real )( -2 );
invMi[3][0] = -invDelta3;
invMi[3][1] = invDelta3;
invMi[3][2] = -invDelta2;
invMi[3][3] = invDelta1;
}
// Matrix M[n-1].
delta = mTimes[i + 1] - mTimes[i];
Real delta2 = delta * delta;
Real delta3 = delta2 * delta;
Matrix4<Real> lastM
(
( Real )1, ( Real )0, ( Real )0, ( Real )0,
( Real )1, delta, delta2, delta3,
( Real )0, ( Real )1, ( ( Real )2 )*delta, ( ( Real )3 )*delta2,
( Real )0, ( Real )0, ( Real )1, ( ( Real )3 )*delta
);
// Matrix L.
Matrix4<Real> LMat
(
( Real )0, ( Real )0, ( Real )0, ( Real )0,
( Real )1, ( Real )0, ( Real )0, ( Real )0,
( Real )0, ( Real )1, ( Real )0, ( Real )0,
( Real )0, ( Real )0, ( Real )1, ( Real )0
);
// Vector U.
Vector<4, Real> U( ( Real )1, ( Real )0, ( Real )0, ( Real )0 );
// Initialize P = L and Q = f[n-2]*U.
Matrix4<Real> P = LMat;
const int numSegmentsM2 = mNumSegments - 2;
Vector<4, Real> Q = mA[numSegmentsM2] * U;
// Compute P and Q.
for ( i = numSegmentsM2; i >= 0; --i )
{
// Matrix L*M[i]^{-1}.
Matrix4<Real> LMInv = LMat * invM[i];
// Update P.
P = LMInv * P;
// Update Q.
if ( i > 0 )
{
Q = mA[i - 1] * U + LMInv * Q;
}
else
{
Q = mA[numSegmentsM1] * U + LMInv * Q;
}
}
// Final update of P.
P = lastM - P;
// Compute P^{-1}.
Matrix4<Real> invP = P.Inverse();
// Compute K[n-1].
Vector<4, Real> coeff = invP * Q;
mB[numSegmentsM1] = coeff[1];
mC[numSegmentsM1] = coeff[2];
mD[numSegmentsM1] = coeff[3];
// Back substitution for the other K[i].
for ( i = numSegmentsM2; i >= 0; i-- )
{
coeff = invM[i] * ( mA[i] * U + LMat * coeff );
mB[i] = coeff[1];
mC[i] = coeff[2];
mD[i] = coeff[3];
}
delete1( invM );
#endif
}
Real* PolyFit3 (int numSamples, const Real* xSamples, const Real* ySamples,
const Real* wSamples, int xDegree, int yDegree)
{
int xBound = xDegree + 1;
int yBound = yDegree + 1;
int quantity = xBound*yBound;
Real* coeff = new1<Real>(quantity);
int i0, j0, i1, j1, s;
// Powers of x, y.
Real** xPower = new2<Real>(2*xDegree+1, numSamples);
Real** yPower = new2<Real>(2*yDegree+1, numSamples);
for (s = 0; s < numSamples; ++s)
{
xPower[s][0] = (Real)1;
for (i0 = 1; i0 <= 2*xDegree; ++i0)
{
xPower[s][i0] = xSamples[s]*xPower[s][i0-1];
}
yPower[s][0] = (Real)1;
for (j0 = 1; j0 <= 2*yDegree; ++j0)
{
yPower[s][j0] = ySamples[s]*yPower[s][j0-1];
}
}
// Vandermonde matrix and right-hand side of linear system.
GMatrix<Real> A(quantity, quantity);
Real* B = new1<Real>(quantity);
for (j0 = 0; j0 <= yDegree; ++j0)
{
for (i0 = 0; i0 <= xDegree; ++i0)
{
int index0 = i0 + xBound*j0;
Real sum = (Real)0;
for (s = 0; s < numSamples; ++s)
{
sum += wSamples[s]*xPower[s][i0]*yPower[s][j0];
}
B[index0] = sum;
for (j1 = 0; j1 <= yDegree; ++j1)
{
for (i1 = 0; i1 <= xDegree; ++i1)
{
int index1 = i1 + xBound*j1;
sum = (Real)0;
for (s = 0; s < numSamples; ++s)
{
sum += xPower[s][i0 + i1]*yPower[s][j0 + j1];
}
A(index0,index1) = sum;
}
}
}
}
// Solve for the polynomial coefficients.
bool hasSolution = LinearSystem<Real>().Solve(A, B, coeff);
assertion(hasSolution, "Failed to solve linear system\n");
WM5_UNUSED(hasSolution);
delete1(B);
delete2(xPower);
delete2(yPower);
return coeff;
}
double LCPPolyDist<Dimension,FVector,DVector>::ProcessLoop (
bool halfspaceConstructor, int& statusCode, FVector closest[2])
{
int i;
double* Q = new1<double>(mNumEquations);
double** M = new2<double>(mNumEquations, mNumEquations);
statusCode = 0;
bool processing = true;
if (!BuildMatrices(M, Q))
{
WM5_LCPPOLYDIST_FUNCTION(CloseLog());
closest[0] = FVector::ZERO;
closest[1] = FVector::ZERO;
processing = false;
}
WM5_LCPPOLYDIST_FUNCTION(PrintMatrices(M, Q));
int tryNumber = 0;
double distance = -Mathd::MAX_REAL;
while (processing)
{
double* zResult = new1<double>(mNumEquations);
double* wResult = new1<double>(mNumEquations);
int lcpStatusCode;
LCPSolver solver(mNumEquations, M, Q, zResult, wResult, lcpStatusCode);
WM5_UNUSED(solver);
switch (lcpStatusCode)
{
case SC_FOUND_TRIVIAL_SOLUTION:
{
statusCode = SC_FOUND_TRIVIAL_SOLUTION;
break;
}
case SC_FOUND_SOLUTION:
{
DVector lcpClosest[2];
for (i = 0; i < mDimension; ++i)
{
lcpClosest[0][i] = (float)zResult[i];
lcpClosest[1][i] = (float)zResult[i + mDimension];
}
DVector diff = lcpClosest[0] - lcpClosest[1];
distance = diff.Length();
statusCode = VerifySolution(lcpClosest);
if (!halfspaceConstructor)
{
WM5_LCPPOLYDIST_FUNCTION(VerifyWithTestPoints(lcpClosest,
statusCode));
}
for (i = 0; i < mDimension; ++i)
{
closest[0][i] = (float)lcpClosest[0][i];
closest[1][i] = (float)lcpClosest[1][i];
}
processing = false;
break;
}
case SC_CANNOT_REMOVE_COMPLEMENTARY:
{
WM5_LCPPOLYDIST_FUNCTION(LogRetries(tryNumber));
if (tryNumber == 3)
{
statusCode = SC_CANNOT_REMOVE_COMPLEMENTARY;
processing = false;
break;
}
MoveHalfspaces(mNumFaces1, mA1, mB1);
WM5_LCPPOLYDIST_FUNCTION(LogHalfspaces(mNumFaces1, mA1, mB1));
MoveHalfspaces(mNumFaces2, mA2, mB2);
WM5_LCPPOLYDIST_FUNCTION(LogHalfspaces(mNumFaces2, mA2, mB2));
BuildMatrices(M, Q);
++tryNumber;
break;
}
case SC_EXCEEDED_MAX_RETRIES:
{
WM5_LCPPOLYDIST_FUNCTION(LCPSolverLoopLimit());
statusCode = SC_EXCEEDED_MAX_RETRIES;
processing = false;
break;
}
}
delete1(wResult);
delete1(zResult);
}
delete2(M);
delete1(Q);
return distance;
}
void ContEllipse2MinCR<Real>::MaxProduct (std::vector<Vector2<Real> >& A,
Real D[2])
{
// Keep track of which constraint lines have already been used in the
// search.
int numConstraints = (int)A.size();
bool* used = new1<bool>(numConstraints);
memset(used, 0, numConstraints*sizeof(bool));
// Find the constraint line whose y-intercept (0,ymin) is closest to the
// origin. This line contributes to the convex hull of the constraints
// and the search for the maximum starts here. Also find the constraint
// line whose x-intercept (xmin,0) is closest to the origin. This line
// contributes to the convex hull of the constraints and the search for
// the maximum terminates before or at this line.
int i, iYMin = -1;
int iXMin = -1;
Real axMax = (Real)0, ayMax = (Real)0; // A[i] >= (0,0) by design
for (i = 0; i < numConstraints; ++i)
{
// The minimum x-intercept is 1/A[iXMin].X() for A[iXMin].X() the
// maximum of the A[i].X().
if (A[i].X() > axMax)
{
axMax = A[i].X();
iXMin = i;
}
// The minimum y-intercept is 1/A[iYMin].Y() for A[iYMin].Y() the
// maximum of the A[i].Y().
if (A[i].Y() > ayMax)
{
ayMax = A[i].Y();
iYMin = i;
}
}
assertion(iXMin != -1 && iYMin != -1, "Unexpected condition\n");
WM5_UNUSED(iXMin);
used[iYMin] = true;
// The convex hull is searched in a clockwise manner starting with the
// constraint line constructed above. The next vertex of the hull occurs
// as the closest point to the first vertex on the current constraint
// line. The following loop finds each consecutive vertex.
Real x0 = (Real)0, xMax = ((Real)1)/axMax;
int j;
for (j = 0; j < numConstraints; ++j)
{
// Find the line whose intersection with the current line is closest
// to the last hull vertex. The last vertex is at (x0,y0) on the
// current line.
Real x1 = xMax;
int line = -1;
for (i = 0; i < numConstraints; ++i)
{
if (!used[i])
{
// This line not yet visited, process it. Given current
// constraint line a0*x+b0*y =1 and candidate line
// a1*x+b1*y = 1, find the point of intersection. The
// determinant of the system is d = a0*b1-a1*b0. We only
// care about lines that have more negative slope than the
// previous one, that is, -a1/b1 < -a0/b0, in which case we
// process only lines for which d < 0.
Real det = A[iYMin].DotPerp(A[i]);
if (det < (Real)0) // TO DO. Need epsilon test here?
{
// Compute the x-value for the point of intersection,
// (x1,y1). There may be floating point error issues in
// the comparision 'D[0] <= fX1'. Consider modifying to
// 'D[0] <= fX1+epsilon'.
D[0] = (A[i].Y() - A[iYMin].Y())/det;
if (x0 < D[0] && D[0] <= x1)
{
line = i;
x1 = D[0];
}
}
}
}
// Next vertex is at (x1,y1) whose x-value was computed above. First
// check for the maximum of x*y on the current line for x in [x0,x1].
// On this interval the function is f(x) = x*(1-a0*x)/b0. The
// derivative is f'(x) = (1-2*a0*x)/b0 and f'(r) = 0 when
// r = 1/(2*a0). The three candidates for the maximum are f(x0),
// f(r), and f(x1). Comparisons are made between r and the end points
// x0 and x1. Since a0 = 0 is possible (constraint line is horizontal
// and f is increasing on line), the division in r is not performed
// and the comparisons are made between 1/2 = a0*r and a0*x0 or a0*x1.
// Compare r < x0.
if ((Real)0.5 < A[iYMin].X()*x0)
{
// The maximum is f(x0) since the quadratic f decreases for
// x > r.
D[0] = x0;
D[1] = ((Real)1 - A[iYMin].X()*D[0])/A[iYMin].Y(); // = f(x0)
break;
}
// Compare r < x1.
if ((Real)0.5 < A[iYMin].X()*x1)
{
// The maximum is f(r). The search ends here because the
// current line is tangent to the level curve of f(x)=f(r)
// and x*y can therefore only decrease as we traverse further
// around the hull in the clockwise direction.
D[0] = ((Real)0.5)/A[iYMin].X();
D[1] = ((Real)0.5)/A[iYMin].Y(); // = f(r)
break;
}
// The maximum is f(x1). The function x*y is potentially larger
// on the next line, so continue the search.
assertion(line != -1, "Unexpected condition\n");
x0 = x1;
x1 = xMax;
used[line] = true;
iYMin = line;
}
assertion(j < numConstraints, "Unexpected condition\n");
delete1(used);
}
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);
}
bool ConvexHull3<Real>::Update (int i)
{
// Locate a triangle visible to the input point (if possible).
Triangle* visible = 0;
Triangle* tri;
typename std::set<Triangle*>::iterator iter = mHull.begin();
typename std::set<Triangle*>::iterator end = mHull.end();
for (/**/; iter != end; ++iter)
{
tri = *iter;
if (tri->GetSign(i, mQuery) > 0)
{
visible = tri;
break;
}
}
if (!visible)
{
// The point is inside the current hull; nothing to do.
return true;
}
// Locate and remove the visible triangles.
std::stack<Triangle*> visibleSet;
std::map<int,TerminatorData> terminator;
visibleSet.push(visible);
visible->OnStack = true;
int j, v0, v1;
while (!visibleSet.empty())
{
tri = visibleSet.top();
visibleSet.pop();
tri->OnStack = false;
for (j = 0; j < 3; ++j)
{
Triangle* adj = tri->Adj[j];
if (adj)
{
// Detach triangle and adjacent triangle from each other.
int nullIndex = tri->DetachFrom(j, adj);
if (adj->GetSign(i, mQuery) > 0)
{
if (!adj->OnStack)
{
// Adjacent triangle is visible.
visibleSet.push(adj);
adj->OnStack = true;
}
}
else
{
// Adjacent triangle is invisible.
v0 = tri->V[j];
v1 = tri->V[(j+1)%3];
terminator[v0] = TerminatorData(v0, v1, nullIndex, adj);
}
}
}
mHull.erase(tri);
delete0(tri);
}
// Insert the new edges formed by the input point and the terminator
// between visible and invisible triangles.
int size = (int)terminator.size();
assertion(size >= 3, "Terminator must be at least a triangle\n");
typename std::map<int,TerminatorData>::iterator edge = terminator.begin();
v0 = edge->second.V[0];
v1 = edge->second.V[1];
tri = new0 Triangle(i, v0, v1);
mHull.insert(tri);
// Save information for linking first/last inserted new triangles.
int saveV0 = edge->second.V[0];
Triangle* saveTri = tri;
// Establish adjacency links across terminator edge.
tri->Adj[1] = edge->second.T;
edge->second.T->Adj[edge->second.NullIndex] = tri;
for (j = 1; j < size; ++j)
{
edge = terminator.find(v1);
assertion(edge != terminator.end(), "Unexpected condition\n");
v0 = v1;
v1 = edge->second.V[1];
Triangle* next = new0 Triangle(i, v0, v1);
mHull.insert(next);
// Establish adjacency links across terminator edge.
next->Adj[1] = edge->second.T;
edge->second.T->Adj[edge->second.NullIndex] = next;
// Establish adjacency links with previously inserted triangle.
next->Adj[0] = tri;
tri->Adj[2] = next;
tri = next;
}
assertion(v1 == saveV0, "Expecting initial vertex\n");
WM5_UNUSED(saveV0);
// Establish adjacency links between first/last triangles.
saveTri->Adj[0] = tri;
tri->Adj[2] = saveTri;
return true;
}
ConformalMap<Real>::ConformalMap (int numPoints,
const Vector3<Real>* points, int numTriangles, const int* indices,
int punctureTriangle)
{
// Construct a vertex-triangle-edge representation of mesh.
BasicMesh mesh(numPoints, points, numTriangles, indices);
int numEdges = mesh.GetNumEdges();
const BasicMesh::Edge* edges = mesh.GetEdges();
const BasicMesh::Triangle* triangles = mesh.GetTriangles();
mPlanes = new1<Vector2<Real> >(numPoints);
mSpheres = new1<Vector3<Real> >(numPoints);
// Construct sparse matrix A nondiagonal entries.
typename LinearSystem<Real>::SparseMatrix AMat;
int i, e, t, v0, v1, v2;
Real value = (Real)0;
for (e = 0; e < numEdges; ++e)
{
const BasicMesh::Edge& edge = edges[e];
v0 = edge.V[0];
v1 = edge.V[1];
Vector3<Real> E0, E1;
const BasicMesh::Triangle& T0 = triangles[edge.T[0]];
for (i = 0; i < 3; ++i)
{
v2 = T0.V[i];
if (v2 != v0 && v2 != v1)
{
E0 = points[v0] - points[v2];
E1 = points[v1] - points[v2];
value = E0.Dot(E1)/E0.Cross(E1).Length();
}
}
const BasicMesh::Triangle& T1 = triangles[edge.T[1]];
for (i = 0; i < 3; ++i)
{
v2 = T1.V[i];
if (v2 != v0 && v2 != v1)
{
E0 = points[v0] - points[v2];
E1 = points[v1] - points[v2];
value += E0.Dot(E1)/E0.Cross(E1).Length();
}
}
value *= -(Real)0.5;
AMat[std::make_pair(v0, v1)] = value;
}
// Aonstruct sparse matrix A diagonal entries.
Real* tmp = new1<Real>(numPoints);
memset(tmp, 0, numPoints*sizeof(Real));
typename LinearSystem<Real>::SparseMatrix::iterator iter = AMat.begin();
typename LinearSystem<Real>::SparseMatrix::iterator end = AMat.end();
for (/**/; iter != end; ++iter)
{
v0 = iter->first.first;
v1 = iter->first.second;
value = iter->second;
assertion(v0 != v1, "Unexpected condition\n");
tmp[v0] -= value;
tmp[v1] -= value;
}
for (int v = 0; v < numPoints; ++v)
{
AMat[std::make_pair(v, v)] = tmp[v];
}
assertion(numPoints + numEdges == (int)AMat.size(),
"Mismatch in sizes\n");
// Construct column vector B (happens to be sparse).
const BasicMesh::Triangle& tri = triangles[punctureTriangle];
v0 = tri.V[0];
v1 = tri.V[1];
v2 = tri.V[2];
Vector3<Real> V0 = points[v0];
Vector3<Real> V1 = points[v1];
Vector3<Real> V2 = points[v2];
Vector3<Real> E10 = V1 - V0;
Vector3<Real> E20 = V2 - V0;
Vector3<Real> E12 = V1 - V2;
Vector3<Real> cross = E20.Cross(E10);
Real len10 = E10.Length();
Real invLen10 = ((Real)1)/len10;
Real twoArea = cross.Length();
Real invLenCross = ((Real)1)/twoArea;
Real invProd = invLen10*invLenCross;
Real re0 = -invLen10;
Real im0 = invProd*E12.Dot(E10);
Real re1 = invLen10;
Real im1 = invProd*E20.Dot(E10);
Real re2 = (Real)0;
Real im2 = -len10*invLenCross;
// Solve sparse system for real parts.
memset(tmp, 0, numPoints*sizeof(Real));
tmp[v0] = re0;
tmp[v1] = re1;
tmp[v2] = re2;
Real* result = new1<Real>(numPoints);
bool solved = LinearSystem<Real>().SolveSymmetricCG(numPoints, AMat, tmp,
result);
assertion(solved, "Failed to solve linear system\n");
WM5_UNUSED(solved);
for (i = 0; i < numPoints; ++i)
{
mPlanes[i].X() = result[i];
}
// Solve sparse system for imaginary parts.
memset(tmp, 0, numPoints*sizeof(Real));
tmp[v0] = -im0;
tmp[v1] = -im1;
tmp[v2] = -im2;
solved = LinearSystem<Real>().SolveSymmetricCG(numPoints, AMat, tmp,
result);
assertion(solved, "Failed to solve linear system\n");
for (i = 0; i < numPoints; ++i)
{
mPlanes[i].Y() = result[i];
}
delete1(tmp);
delete1(result);
// Scale to [-1,1]^2 for numerical conditioning in later steps.
Real fmin = mPlanes[0].X(), fmax = fmin;
for (i = 0; i < numPoints; i++)
{
if (mPlanes[i].X() < fmin)
{
fmin = mPlanes[i].X();
}
else if (mPlanes[i].X() > fmax)
{
fmax = mPlanes[i].X();
}
if (mPlanes[i].Y() < fmin)
{
fmin = mPlanes[i].Y();
}
else if (mPlanes[i].Y() > fmax)
{
fmax = mPlanes[i].Y();
}
}
Real halfRange = ((Real)0.5)*(fmax - fmin);
Real invHalfRange = ((Real)1)/halfRange;
for (i = 0; i < numPoints; ++i)
{
mPlanes[i].X() = -(Real)1 + invHalfRange*(mPlanes[i].X() - fmin);
mPlanes[i].Y() = -(Real)1 + invHalfRange*(mPlanes[i].Y() - fmin);
}
// Map plane points to sphere using inverse stereographic projection.
// The main issue is selecting a translation in (x,y) and a radius of
// the projection sphere. Both factors strongly influence the final
// result.
// Use the average as the south pole. The points tend to be clustered
// approximately in the middle of the conformally mapped punctured
// triangle, so the average is a good choice to place the pole.
Vector2<Real> origin((Real)0 ,(Real)0 );
for (i = 0; i < numPoints; ++i)
{
origin += mPlanes[i];
}
origin /= (Real)numPoints;
for (i = 0; i < numPoints; ++i)
{
mPlanes[i] -= origin;
}
mPlaneMin = mPlanes[0];
mPlaneMax = mPlanes[0];
for (i = 1; i < numPoints; ++i)
{
if (mPlanes[i].X() < mPlaneMin.X())
{
mPlaneMin.X() = mPlanes[i].X();
}
else if (mPlanes[i].X() > mPlaneMax.X())
{
mPlaneMax.X() = mPlanes[i].X();
}
if (mPlanes[i].Y() < mPlaneMin.Y())
{
mPlaneMin.Y() = mPlanes[i].Y();
}
else if (mPlanes[i].Y() > mPlaneMax.Y())
{
mPlaneMax.Y() = mPlanes[i].Y();
}
}
// Select the radius of the sphere so that the projected punctured
// triangle has an area whose fraction of total spherical area is the
// same fraction as the area of the punctured triangle to the total area
// of the original triangle mesh.
Real twoTotalArea = (Real)0;
for (t = 0; t < numTriangles; ++t)
{
const BasicMesh::Triangle& T0 = triangles[t];
const Vector3<Real>& V0 = points[T0.V[0]];
const Vector3<Real>& V1 = points[T0.V[1]];
const Vector3<Real>& V2 = points[T0.V[2]];
Vector3<Real> E0 = V1 - V0, E1 = V2 - V0;
twoTotalArea += E0.Cross(E1).Length();
}
mRadius = ComputeRadius(mPlanes[v0], mPlanes[v1], mPlanes[v2],
twoArea/twoTotalArea);
Real radiusSqr = mRadius*mRadius;
// Inverse stereographic projection to obtain sphere coordinates. The
// sphere is centered at the origin and has radius 1.
for (i = 0; i < numPoints; i++)
{
Real rSqr = mPlanes[i].SquaredLength();
Real mult = ((Real)1)/(rSqr + radiusSqr);
Real x = ((Real)2)*mult*radiusSqr*mPlanes[i].X();
Real y = ((Real)2)*mult*radiusSqr*mPlanes[i].Y();
Real z = mult*mRadius*(rSqr - radiusSqr);
mSpheres[i] = Vector3<Real>(x,y,z)/mRadius;
}
}
BSplineReduction<Real,TVector>::BSplineReduction (int numCtrlPoints,
const TVector* ctrlPoints, int degree, Real fraction, int& outNumCtrlPoints,
TVector*& outCtrlPoints)
{
// Check for valid input. If invalid, return a "null" curve.
assertion(numCtrlPoints >= 2, "Invalid input\n");
assertion(ctrlPoints != 0, "Invalid input\n");
assertion(1 <= degree && degree < numCtrlPoints, "Invalid input\n");
if (numCtrlPoints < 2 || !ctrlPoints || degree < 1
|| degree >= numCtrlPoints)
{
outNumCtrlPoints = 0;
outCtrlPoints = 0;
return;
}
// Clamp the number of control points to [degree+1,quantity-1].
outNumCtrlPoints = (int)(fraction*numCtrlPoints);
if (outNumCtrlPoints > numCtrlPoints)
{
outNumCtrlPoints = numCtrlPoints;
outCtrlPoints = new1<TVector>(outNumCtrlPoints);
memcpy(outCtrlPoints, ctrlPoints, numCtrlPoints*sizeof(TVector));
return;
}
if (outNumCtrlPoints < degree + 1)
{
outNumCtrlPoints = degree + 1;
}
// Allocate output control points and set all to the zero vector.
outCtrlPoints = new1<TVector>(outNumCtrlPoints);
// Set up basis function parameters. Function 0 corresponds to the
// output curve. Function 1 corresponds to the input curve.
mDegree = degree;
mQuantity[0] = outNumCtrlPoints;
mQuantity[1] = numCtrlPoints;
for (int j = 0; j <= 1; ++j)
{
mNumKnots[j] = mQuantity[j] + mDegree + 1;
mKnot[j] = new1<Real>(mNumKnots[j]);
int i;
for (i = 0; i <= mDegree; ++i)
{
mKnot[j][i] = (Real)0;
}
Real factor = ((Real)1)/(Real)(mQuantity[j] - mDegree);
for (/**/; i < mQuantity[j]; ++i)
{
mKnot[j][i] = (i-mDegree)*factor;
}
for (/**/; i < mNumKnots[j]; i++)
{
mKnot[j][i] = (Real)1;
}
}
// Construct matrix A (depends only on the output basis function).
Real value, tmin, tmax;
int i0, i1;
mBasis[0] = 0;
mBasis[1] = 0;
BandedMatrix<Real> A(mQuantity[0], mDegree, mDegree);
for (i0 = 0; i0 < mQuantity[0]; ++i0)
{
mIndex[0] = i0;
tmax = MaxSupport(0, i0);
for (i1 = i0; i1 <= i0 + mDegree && i1 < mQuantity[0]; ++i1)
{
mIndex[1] = i1;
tmin = MinSupport(0, i1);
value = Integrate1<Real>::RombergIntegral(8, tmin, tmax,
Integrand, this);
A(i0, i1) = value;
A(i1, i0) = value;
}
}
// Construct A^{-1}.
GMatrix<Real> invA(mQuantity[0], mQuantity[0]);
bool solved = LinearSystem<Real>().Invert(A, invA);
assertion(solved, "Failed to solve linear system\n");
WM5_UNUSED(solved);
// Construct B (depends on both input and output basis functions).
mBasis[1] = 1;
GMatrix<Real> B(mQuantity[0], mQuantity[1]);
for (i0 = 0; i0 < mQuantity[0]; ++i0)
{
mIndex[0] = i0;
Real tmin0 = MinSupport(0, i0);
Real tmax0 = MaxSupport(0, i0);
for (i1 = 0; i1 < mQuantity[1]; ++i1)
{
mIndex[1] = i1;
Real tmin1 = MinSupport(1, i1);
Real tmax1 = MaxSupport(1, i1);
Intersector1<Real> intr(tmin0, tmax0, tmin1, tmax1);
intr.Find();
int quantity = intr.GetNumIntersections();
if (quantity == 2)
{
value = Integrate1<Real>::RombergIntegral(8,
intr.GetIntersection(0), intr.GetIntersection(1),
Integrand, this);
B[i0][i1] = value;
}
else
{
B[i0][i1] = (Real)0;
}
}
}
// Construct A^{-1}*B.
GMatrix<Real> prod = invA*B;
// Construct the control points for the least-squares curve.
memset(outCtrlPoints,0,outNumCtrlPoints*sizeof(TVector));
for (i0 = 0; i0 < mQuantity[0]; ++i0)
{
for (i1 = 0; i1 < mQuantity[1]; ++i1)
{
outCtrlPoints[i0] += ctrlPoints[i1]*prod[i0][i1];
}
}
}
