//===========================================================================
Vector3D gvCamera::eyeCoordsToObjectCoords(const Vector3D& eyept) const
//===========================================================================
{
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
use();
int viewport[4];
double projection[16];
double modelview[16];
glGetIntegerv(GL_VIEWPORT, viewport);
glGetDoublev(GL_PROJECTION_MATRIX, projection);
glGetDoublev(GL_MODELVIEW_MATRIX, modelview);
Vector3D pt;
int success = gluUnProject(eyept[0], eyept[1], eyept[2],
modelview, projection, viewport,
&pt[0], &pt[1], &pt[2]);
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
ALWAYS_ERROR_IF(!success, "gluUnProject() failed!");
return pt;
}
//===========================================================================
Vector3D gvCamera::objectCoordsToWindowCoords(const Vector3D& pt) const
//===========================================================================
{
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
use();
int viewport[4];
double projection[16];
double modelview[16];
glGetIntegerv(GL_VIEWPORT, viewport);
glGetDoublev(GL_PROJECTION_MATRIX, projection);
glGetDoublev(GL_MODELVIEW_MATRIX, modelview);
Vector3D winpt;
int success = gluProject(pt[0], pt[1], pt[2],
modelview, projection, viewport,
&winpt[0], &winpt[1], &winpt[2]);
// It seems that the y value is referring to the opposite edge. @@sbr Fix this!
winpt[1] = viewheight_ - winpt[1];
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
ALWAYS_ERROR_IF(!success, "gluProject() failed!");
return winpt;
}
void intersectCurveSurf(SplineCurve *cv, SplineSurface *sf,
double epsge,
vector<pair<double, Point> >& int_pts,
vector<int>& pretopology,
vector<pair<pair<double,Point>,
pair<double,Point> > >& int_crvs)
{
SISLSurf* sislsf = GoSurf2SISL(*sf, false);
SISLCurve* sislcv = Curve2SISL(*cv, false);
int kntrack = 0;
int trackflag = 0; // Do not make tracks.
SISLTrack **track =0;
int knpt=0, kncrv=0;
double *par1=0, *par2=0;
int *pretop = 0;
SISLIntcurve **vcrv = 0;
int stat = 0;
sh1858(sislsf, sislcv, 0.0, epsge, trackflag, &kntrack, &track,
&knpt, &par1, &par2, &pretop, &kncrv, &vcrv, &stat);
ALWAYS_ERROR_IF(stat<0,"Error in intersection, code: " << stat);
// Remember intersections points.
int ki;
for (ki=0; ki<knpt; ki++)
{
int_pts.push_back(std::make_pair(par2[ki],
Point(par1[2*ki],par1[2*ki+1])));
pretopology.insert(pretopology.end(), pretop+4*ki+2, pretop+4*(ki+1));
pretopology.insert(pretopology.end(), pretop+4*ki, pretop+4*ki+2);
}
// Remember intersection curves
for (ki=0; ki<kncrv; ++ki)
{
int nmb_pt = vcrv[ki]->ipoint;
Point par1 = Point(vcrv[ki]->epar1[0],vcrv[ki]->epar1[1]);
Point par2 = Point(vcrv[ki]->epar1[2*(nmb_pt-1)],
vcrv[ki]->epar1[2*nmb_pt-1]);
int_crvs.push_back(std::make_pair(std::make_pair(vcrv[ki]->epar2[0],
par1),
std::make_pair(vcrv[ki]->epar2[nmb_pt-1],
par2)));
}
if (kncrv > 0)
freeIntcrvlist(vcrv, kncrv);
if (par1 != NULL) free(par1);
if (par2 != NULL) free(par2);
if (sislsf != NULL) freeSurf(sislsf);
if (sislcv != NULL) freeCurve(sislcv);
if (pretop != NULL) free(pretop);
}
void intersectcurves(SplineCurve* cv1, SplineCurve* cv2, double epsge,
vector<std::pair<double,double> >& intersections)
//************************************************************************
//
// Intersect two spline curves. Collect intersection parameters.
//
//***********************************************************************
{
// Make sisl curves and call sisl.
SISLCurve *pc1 = Curve2SISL(*cv1, false);
SISLCurve *pc2 = Curve2SISL(*cv2, false);
int kntrack = 0;
int trackflag = 0; // Do not make tracks.
SISLTrack **track =0;
int knpt=0, kncrv=0;
double *par1=0, *par2=0;
int *pretop = 0;
SISLIntcurve **intcrvs = 0;
int stat = 0;
sh1857(pc1, pc2, 0.0, epsge, trackflag, &kntrack, &track,
&knpt, &par1, &par2, &pretop, &kncrv, &intcrvs, &stat);
ALWAYS_ERROR_IF(stat<0,"Error in intersection, code: " << stat);
// Remember intersections points. The intersection curves are
// skipped.
int ki;
for (ki=0; ki<knpt; ki++)
{
intersections.push_back(std::make_pair(par1[ki],par2[ki]));
}
if (kncrv > 0)
freeIntcrvlist(intcrvs, kncrv);
if (par1 != 0) free(par1);
if (par2 != 0) free(par2);
if (pretop != 0) free(pretop);
if (pc1 != 0) freeCurve(pc1);
if (pc2 != 0) freeCurve(pc2);
}
void intersectCurvePoint(const ParamCurve* crv, Point pnt, double epsge,
vector<double>& intersections,
vector<pair<double, double> >& int_crvs)
//************************************************************************
//
// Intersect a curve with a point
//
//***********************************************************************
{
// First make sure that the curve is a spline curve
ParamCurve* tmpcrv = const_cast<ParamCurve*>(crv);
SplineCurve* sc = tmpcrv->geometryCurve();
if (sc == NULL)
THROW("ParamCurve doesn't have a spline representation.");
// Make sisl curve and call sisl.
SISLCurve *pc = Curve2SISL(*sc, false);
int knpt=0, kncrv=0;
double *par=0;
SISLIntcurve **vcrv = 0;
int stat = 0;
s1871(pc, pnt.begin(), pnt.size(), epsge, &knpt, &par, &kncrv, &vcrv, &stat);
ALWAYS_ERROR_IF(stat<0,"Error in intersection, code: " << stat);
// Remember intersections points.
if (knpt > 0)
intersections.insert(intersections.end(), par, par+knpt);
// Remember intersection curves
for (int ki=0; ki<kncrv; ++ki)
int_crvs.push_back(std::make_pair(vcrv[ki]->epar1[0], vcrv[ki]->epar1[vcrv[ki]->ipoint-1]));
if (kncrv > 0)
freeIntcrvlist(vcrv, kncrv);
if (par != 0) free(par);
if (pc) freeCurve(pc);
delete sc;
}
shared_ptr<SplineSurface>
GeometryTools::surfaceSum(const SplineSurface& sf1, double fac1,
const SplineSurface& sf2, double fac2, double num_tol)
//********************************************************************
// Addition of two signed SplineSurfaces, i.e. this function can
// also be used for subtraction. The surfaces is assumed to live on
// the same parameter domain, but may have different knot vectors.
//********************************************************************
{
// Check input
ALWAYS_ERROR_IF(fabs(sf1.startparam_u() - sf2.startparam_u()) > num_tol ||
fabs(sf1.endparam_u() - sf2.endparam_u()) > num_tol ||
fabs(sf1.startparam_v() - sf2.startparam_v()) > num_tol ||
fabs(sf1.endparam_v() - sf2.endparam_v()) > num_tol,
"Inconsistent parameter domain.");
// For the time being
if (sf1.rational() || sf2.rational()) {
THROW("Sum of rational surfaces is not implemented");
}
// Make copy of surfaces
vector<shared_ptr<SplineSurface> > surfaces;
surfaces.reserve(2);
shared_ptr<SplineSurface> sf;
// #ifdef _MSC_VER
// sf = shared_ptr<SplineSurface>(dynamic_cast<SplineSurface*>(sf1.clone()));
// #else
sf = shared_ptr<SplineSurface>(sf1.clone());
// #endif
surfaces.push_back(sf);
// #ifdef _MSC_VER
// sf = shared_ptr<SplineSurface>(dynamic_cast<SplineSurface*>(sf2.clone()));
// #else
sf = shared_ptr<SplineSurface>(sf2.clone());
// #endif
surfaces.push_back(sf);
// Make sure that the surfaces live on the same knot vector
GeometryTools::unifySurfaceSplineSpace(surfaces, num_tol);
// Add signed coefficients
vector<double> coefs;
int nmb_coefs_u = surfaces[0]->numCoefs_u();
int nmb_coefs_v = surfaces[0]->numCoefs_v();
int dim = surfaces[0]->dimension();
coefs.resize(dim*nmb_coefs_u*nmb_coefs_v);
int ki;
std::vector<double>::iterator s1 = surfaces[0]->coefs_begin();
std::vector<double>::iterator s2 = surfaces[1]->coefs_begin();
for (ki=0; ki<dim*nmb_coefs_u*nmb_coefs_v; ki++)
coefs[ki] = fac1*s1[ki] + fac2*s2[ki];
// Create output curve
shared_ptr<SplineSurface>
surfacesum(new SplineSurface(nmb_coefs_u, nmb_coefs_v,
surfaces[0]->order_u(),
surfaces[0]->order_v(),
surfaces[0]->basis_u().begin(),
surfaces[0]->basis_v().begin(),
&coefs[0], dim, false));
return surfacesum;
}
//===========================================================================
void SplineCurve::appendCurve(ParamCurve* other_curve,
int continuity, double& dist, bool repar)
//===========================================================================
{
SplineCurve* other_cv = dynamic_cast<SplineCurve*>(other_curve);
ALWAYS_ERROR_IF(other_cv == 0,
"Given an empty curve or not a SplineCurve.");
ALWAYS_ERROR_IF(dim_ != other_cv->dimension(),
"The curves must lie in the same space.");
ALWAYS_ERROR_IF(continuity < -1 || continuity + 1 > order(),
"Specified continuity not attainable.");
#ifdef DEBUG
// DEBUG OUTPUT
std::ofstream of0("basis0.g2");
writeStandardHeader(of0);
write(of0);
other_cv->writeStandardHeader(of0);
other_cv->write(of0);
#endif
// Making sure the curves have the same order. Raise if necessary.
int diff_order = order() - other_cv->order();
if (diff_order > 0)
other_cv->raiseOrder(diff_order);
else if (diff_order < 0)
raiseOrder(abs(diff_order));
// Make sure that we have k-regularity at meeting ends.
makeKnotEndRegular();
other_cv->makeKnotStartRegular();
// Ensure that either none of the curves or both are rational
if (rational_ && !other_cv->rational())
other_cv->representAsRational();
if (!rational_ && other_cv->rational())
representAsRational();
#ifdef DEBUG
// DEBUG OUTPUT
std::ofstream of1("basis1.g2");
writeStandardHeader(of1);
write(of1);
other_cv->writeStandardHeader(of1);
other_cv->write(of1);
#endif
if (rational_)
{
// Set end weight to 1
setBdWeight(1.0, false);
other_cv->setBdWeight(1.0, true);
}
// Reparametrization (translatation and mult.) of other_cv->basis().knots_ .
if (repar && continuity > 0) {
if (rational_)
{
// The weights corresponding to the two coefficients close to the
// joints should be equal
equalBdWeights(false);
other_cv->equalBdWeights(true);
}
}
#ifdef DEBUG
// DEBUG OUTPUT
std::ofstream of1_2("basis1_2.g2");
writeStandardHeader(of1_2);
write(of1_2);
other_cv->writeStandardHeader(of1_2);
other_cv->write(of1_2);
#endif
#ifdef DEBUG
// DEBUG OUTPUT
std::ofstream of2("basis2.dat");
basis_.write(of2);
of2 << std::endl;
other_cv->basis_.write(of2);
of2 << std::endl;
#endif
if (continuity > -1 && rational_)
{
// Make sure that the rational curves have equal weights in the end
int k2 = (numCoefs() - 1) * (dim_+1);
double frac = rcoefs_[k2+dim_]/other_cv->rcoefs_[dim_];
int kn = other_cv->numCoefs();
for (int j=0; j<kn*(dim_+1); ++j)
other_cv->rcoefs_[j] *= frac;
}
#ifdef DEBUG
// DEBUG OUTPUT
std::ofstream of3("basis3.dat");
basis_.write(of3);
of3 << std::endl;
other_cv->basis_.write(of3);
of3 << std::endl;
#endif
if (repar && continuity > 0) {
double sum1 = 0;
double sum2 = 0;
if (rational_)
{
int k2 = (numCoefs() - 1) * (dim_+1);
int k1 = (numCoefs() - 2) * (dim_+1);
for (int j = 0; j < dim_; ++j)
{
double t0 = (rcoefs_[k2 + j] - rcoefs_[k1 + j])*rcoefs_[k2 + dim_] -
rcoefs_[k2 + j]*(rcoefs_[k2 + dim_] - rcoefs_[k1 + dim_]);
sum1 += t0*t0;
}
sum1 = sqrt(sum1)/(rcoefs_[k2+dim_]*rcoefs_[k2+dim_]);
k2 = dim_+1;
k1 = 0;
for (int j = 0; j < dim_; ++j)
{
double t0 = (other_cv->rcoefs_[k2 + j] -
other_cv->rcoefs_[k1 + j])*other_cv->rcoefs_[k1 + dim_] -
other_cv->rcoefs_[k1 + j]*(other_cv->rcoefs_[k2 + dim_] -
other_cv->rcoefs_[k1 + dim_]);
sum2 += t0*t0;
}
sum2 = sqrt(sum2)/(other_cv->rcoefs_[k1+dim_]*other_cv->rcoefs_[k1+dim_]);
}
else
{
for (int j = 0; j < dim_; ++j) {
sum1 += (coefs_[(numCoefs() - 1) * dim_ + j] -
coefs_[(numCoefs() - 2) * dim_ + j]) *
(coefs_[(numCoefs() - 1) * dim_ + j] -
coefs_[(numCoefs() - 2) * dim_ + j]);
sum2 += (other_cv->coefs_[dim_ + j] - other_cv->coefs_[j]) *
(other_cv->coefs_[dim_ + j] - other_cv->coefs_[j]);
}
sum1 = sqrt(sum1);
sum2 = sqrt(sum2);
}
#ifdef DEBUG
// DEBUG OUTPUT
std::ofstream of4("basis4.dat");
basis_.write(of4);
of4 << std::endl;
other_cv->basis_.write(of4);
of4 << std::endl;
#endif
if (sum1 > 1.0e-14) { // @@sbr We should have a universal noise-tolerance.
double del1 = basis_.begin()[numCoefs()] - basis_.begin()[numCoefs() - 1];
double del2 = other_cv->basis_.begin()[order()] -
other_cv->basis_.begin()[order() - 1];
double k = sum2*del1/(sum1*del2);
other_cv->basis_.rescale(endparam(), endparam() +
k * (other_cv->basis_.begin()
[other_cv->numCoefs() + order() - 1] -
other_cv->basis_.startparam()));
} else {
MESSAGE("Curve seems to be degenerated in end pt!");
}
} else {
other_cv->basis_.rescale(endparam(),
endparam() +
(other_cv->basis_.begin()
[other_cv->numCoefs() + order() - 1] -
other_cv->basis_.startparam()));
}
// Join the curve-segments (i.e. set endpoints equal), given that...
if (continuity != -1) {
for (int j = 0; j < dim_; ++j) {
other_cv->coefs_[j] =
coefs_[(numCoefs() - 1)*dim_ + j] =
(coefs_[(numCoefs() - 1)*dim_ + j] + other_cv->coefs_[j])/2;
}
}
double tpar = basis_.endparam();
int ti = numCoefs() + order() - 1; // Index of last occurence of tpar.
#ifdef DEBUG
// DEBUG OUTPUT
std::ofstream of5("basis5.dat");
basis_.write(of5);
of5 << std::endl;
other_cv->basis_.write(of5);
of5 << std::endl;
#endif
// Add other_cv's coefs.
if (rational_)
{
// Ensure identity of the weight in the joint
other_cv->setBdWeight(rcoefs_[rcoefs_.size()-1], true);
rcoefs_.insert(rcoefs_end(), other_cv->rcoefs_begin(),
other_cv->rcoefs_end());
}
else
coefs_.insert(coefs_end(), other_cv->coefs_begin(), other_cv->coefs_end());
#ifdef DEBUG
// DEBUG OUTPUT
std::ofstream of("basis.dat");
basis_.write(of);
of << std::endl;
other_cv->basis_.write(of);
of << std::endl;
#endif
// Make an updated basis_ .
std::vector<double> new_knotvector;
std::copy(basis_.begin(), basis_.end(), std::back_inserter(new_knotvector));
std::copy(other_cv->basis_.begin() + order(), other_cv->basis_.end(),
std::back_inserter(new_knotvector));
basis_ = BsplineBasis(order(), new_knotvector.begin(), new_knotvector.end());
if (rational_)
updateCoefsFromRcoefs();
SplineCurve orig_curve = *this; // Save curve for later estimates.
// Obtain wanted smoothness.
int i;
try {
for (i = 0; i < continuity + 1; ++i)
removeKnot(tpar);
}
catch (...)
{
// Leave the knots
}
// Estimate distance between curve and smoothed curve:
// Raise (copy of) smoothed curve to original spline space
// and calculate max distance between corresponding spline-coefs.
SplineCurve raised_smooth_curve = *this;
std::vector<double> knots;
for (i = 0; i < continuity + 1; ++i) knots.push_back(tpar);
raised_smooth_curve.insertKnot(knots);
double sum, root_sum;
dist = 0;
for (i = std::max(0, ti - (continuity + 1) - order());
i < ti - order() + continuity + 1; ++i) {
sum = 0;
for (int j = 0; j < dim_; ++j)
sum += (orig_curve.coefs_[i * dim_ + j] -
raised_smooth_curve.coefs_[i * dim_ + j])
* (orig_curve.coefs_[i * dim_ + j] -
raised_smooth_curve.coefs_[i * dim_ + j]);
// to avoid use of the max function, which is likely to cause
// trouble with the Microsoft Visual C++ Compiler, the following
// two lines are added, and the third one is commented out.
root_sum = sqrt(sum);
dist = dist > root_sum ? dist : root_sum;
//dist = std::max(dist, sqrt(sum));
}
}
Point ftPoint::normal() const
{
ALWAYS_ERROR_IF(surface_ == 0, "There is no underlying surface!");
return surface_->normal(u_, v_);
}
//===========================================================================
void SplineInterpolator::interpolate(int num_points,
int dimension,
const double* param_start,
const double* data_start,
std::vector<double>& coefs)
//===========================================================================
{
// Check that we have reasonable conditions set and a good param sequence
ALWAYS_ERROR_IF(ctype_ == None, "No end conditions set.");
for (int i = 1; i < num_points; ++i) {
ALWAYS_ERROR_IF(param_start[i] <= param_start[i-1],
"Parameter sequence must be strictly increasing.");
}
ALWAYS_ERROR_IF(ctype_ == Hermite && (dimension != start_tangent_->dimension() ||
dimension != end_tangent_->dimension()),
"In Hermite interpolation, the end tangents must have the "
"same dimension as the interpolation data.");
// First we make a knot vector and define the spline space
int additional_coefs = (ctype_ == Free) ? 0 :
(((ctype_ == NaturalAtStart && end_tangent_.get() == 0)
|| (ctype_ == NaturalAtEnd && start_tangent_.get() == 0)) ? 1 : 2);
int num_coefs = num_points + additional_coefs;
int order = std::min(4, num_coefs);
ALWAYS_ERROR_IF(num_coefs < 2,"Insufficient number of points.");
std::vector<double> knots;
knots.reserve(num_coefs + order);
knots.insert(knots.end(), order, param_start[0]);
if (ctype_ == Free) {
knots.insert(knots.end(),
param_start + 2,
param_start + num_points - 2);
}
else if (ctype_ == NaturalAtStart) {
if (end_tangent_.get() != 0)
knots.insert(knots.end(), param_start + 1,
param_start + num_points - 1);
else
knots.insert(knots.end(), param_start + 1,
param_start + num_points - 2);
}
else if (ctype_ == NaturalAtEnd) {
if (start_tangent_.get() != 0)
knots.insert(knots.end(), param_start + 1,
param_start + num_points - 1);
else
knots.insert(knots.end(), param_start + 2,
param_start + num_points - 1);
}
else { // ctype_ == Natural or Hermite
knots.insert(knots.end(),
param_start + 1,
param_start + num_points - 1);
}
knots.insert(knots.end(), order, param_start[num_points-1]);
basis_ = BsplineBasis(num_coefs, order, &knots[0]);
// Create the interpolation matrix.
// The first and last row (equation) depends on the boundary
// conditions (for Hermite and Natural conditions) or are
// nonexisting (for Free conditions, the matrix has two fewer
// rows/equations).
// The interpolation matrix is a tridiagonal matrix in the Free
// and Hermite cases, and a tridiagonal matrix plus two elements
// in the Natural case.
// This code dates from the time when this code was dependent on the NEWMAT
// library. This is no longer the case, and the code has been substituted with
// the code below. However, it is kept here for reference reasons.
//---------------------NEWMAT dependent-----------------------------
// #if 0
// cerr << "NEWMAT DEPENDENT" << endl;
// BandMatrix A(num_coefs, 2, 2);
// A = 0;
// ColumnVector c(num_coefs);
// ColumnVector b(num_coefs);
// // Boundary conditions
// if (ctype_ == Hermite) {
// // Derivative conditions
// double tmp[8];
// basis_.computeBasisValues(param_start[0], tmp, 1);
// A.element(0, 0) = tmp[1]; // derivative of first B-spline
// A.element(0, 1) = tmp[3]; // derivative of second B-spline
// basis_.computeBasisValues(param_start[num_points-1], tmp, 1);
// A.element(num_coefs - 1, num_coefs - 2) = tmp[5];
// A.element(num_coefs - 1, num_coefs - 1) = tmp[7];
// // Boundary element conditions
// A.element(1, 0) = 1.0;
// A.element(num_coefs - 2, num_coefs - 1) = 1.0;
// } else if (ctype_ == Natural) {
// // Derivative conditions
// double tmp[12];
// basis_.computeBasisValues(param_start[0], tmp, 2);
// A.element(0, 0) = tmp[2]; // second derivative of first B-spline
// A.element(0, 1) = tmp[5];
// A.element(0, 2) = tmp[8];
// basis_.computeBasisValues(param_start[num_points-1], tmp, 2);
// A.element(num_coefs - 1, num_coefs - 3) = tmp[5];
// A.element(num_coefs - 1, num_coefs - 2) = tmp[8];
// A.element(num_coefs - 1, num_coefs - 1) = tmp[11];
// // Boundary element conditions
// A.element(1, 0) = 1.0;
// A.element(num_coefs - 2, num_coefs - 1) = 1.0;
// } else if (ctype_ == NaturalAtStart) {
// // Derivative condition
// double tmp[12];
// basis_.computeBasisValues(param_start[0], tmp, 2);
// A.element(0, 0) = tmp[2]; // second derivative of first B-spline
// A.element(0, 1) = tmp[5];
// A.element(0, 2) = tmp[8];
// if (end_tangent_.get() != 0) {
// double tmp[8];
// basis_.computeBasisValues(param_start[num_points-1], tmp, 1);
// A.element(num_coefs - 1, num_coefs - 2) = tmp[5];
// A.element(num_coefs - 1, num_coefs - 1) = tmp[7];
// A.element(num_coefs - 2, num_coefs - 1) = 1.0;
// } else {
// A.element(num_coefs - 1, num_coefs - 1) = 1.0;
// }
// // Boundary element conditions
// A.element(1, 0) = 1.0;
// } else if (ctype_ == NaturalAtEnd) {
// // Derivative condition
// double tmp[12];
// basis_.computeBasisValues(param_start[num_points-1], tmp, 2);
// A.element(num_coefs - 1, num_coefs - 3) = tmp[5];
// A.element(num_coefs - 1, num_coefs - 2) = tmp[8];
// A.element(num_coefs - 1, num_coefs - 1) = tmp[11];
// if (start_tangent_.get() != 0) {
// basis_.computeBasisValues(param_start[0], tmp, 1);
// A.element(0, 0) = tmp[1]; // derivative of first B-spline
// A.element(0, 1) = tmp[3]; // derivative of second B-spline
// A.element(1, 0) = 1.0;
// } else {
// A.element(0, 0) = 1.0;
// }
// // Boundary element conditions
// A.element(num_coefs - 2, num_coefs - 1) = 1.0;
// } else if (ctype_ == Free) {
// // Boundary element conditions
// A.element(0, 0) = 1.0;
// A.element(num_coefs - 1, num_coefs - 1) = 1.0;
// } else {
// THROW("Unknown boundary condition type: " << ctype_);
// }
// // Interior conditions
// int rowoffset = ((ctype_ == Free) ||
// ((ctype_ == NaturalAtEnd) && (start_tangent_.get() == 0)) ? 1 : 2);
// int j;
// for (j = 0; j < num_points-2; ++j) {
// double tmp[4];
// basis_.computeBasisValues(param_start[j+1], tmp, 0);
// int column = 1 + (basis_.lastKnotInterval() - 4);
// A.element(j + rowoffset, column) = tmp[0];
// A.element(j + rowoffset, column + 1) = tmp[1];
// A.element(j + rowoffset, column + 2) = tmp[2];
// A.element(j + rowoffset, column + 3) = tmp[3];
// }
// // Now we are ready to repeatedly solve Ac = b for # = dimension different
// // right-hand-sides b.
// BandLUMatrix ALUfact = A; // Computes LU factorization
// ALWAYS_ERROR_IF(ALUfact.IsSingular(),
// "Matrix is singular! This should never happen!");
// coefs.resize(dimension*num_coefs);
// for (int dd = 0; dd < dimension; ++dd) {
// // Make the b vector
// int offset = (ctype_ == Free ||
// ((ctype_ == NaturalAtEnd) && start_tangent_.get() == 0) ? 0 : 1);
// if (ctype_ == Hermite) {
// b.element(0) = (*start_tangent_)[dd];
// b.element(num_coefs - 1) = (*end_tangent_)[dd];
// } else if (ctype_ == Natural) {
// b.element(0) = 0;
// b.element(num_coefs - 1) = 0;
// } else if (ctype_ == NaturalAtStart) {
// b.element(0) = 0;
// if (end_tangent_.get() != 0)
// b.element(num_coefs - 1) = (*end_tangent_)[dd];
// } else if (ctype_ == NaturalAtEnd) {
// b.element(num_coefs - 1) = 0;
// if (start_tangent_.get() != 0)
// b.element(0) = (*start_tangent_)[dd];
// }
// for (j = 0; j < num_points; ++j) {
// b.element(j+offset) = data_start[j*dimension + dd];
// }
// // Solve
// c = ALUfact.i() * b;
// // Copy results
// for (j = 0; j < num_coefs; ++j) {
// coefs[j*dimension + dd] = c.element(j);
// }
// }
// -------------------NEWMAT INDEPENDENT------------------------------
//#else
vector<vector<double> > A(num_coefs, vector<double>(num_coefs, 0));
vector<vector<double> > b(num_coefs, vector<double>(dimension));
double tmp[12];
// boundary conditions
switch (ctype_) {
case Hermite:
basis_.computeBasisValues(param_start[0], tmp, 1);
A[0][0] = tmp[1]; // derivative of first B-spline
A[0][1] = tmp[3]; // derivative of second B-spline
basis_.computeBasisValues(param_start[num_points-1], tmp, 1);
A[num_coefs - 1][num_coefs - 2] = tmp[5];
A[num_coefs - 1][num_coefs - 1] = tmp[7];
// Boundary element conditions
A[1][0] = 1.0;
A[num_coefs - 2][num_coefs - 1] = 1.0;
break;
case Natural:
// Derivative conditions
basis_.computeBasisValues(param_start[0], tmp, 2);
A[0][0] = tmp[2]; // second derivative of first B-spline
A[0][1] = tmp[5];
A[0][2] = tmp[8];
basis_.computeBasisValues(param_start[num_points-1], tmp, 2);
A[num_coefs - 1][num_coefs - 3] = tmp[5];
A[num_coefs - 1][num_coefs - 2] = tmp[8];
A[num_coefs - 1][num_coefs - 1] = tmp[11];
// Boundary element conditions
A[1][0] = 1.0;
A[num_coefs - 2][num_coefs - 1] = 1.0;
break;
case NaturalAtStart:
basis_.computeBasisValues(param_start[0], tmp, 2);
A[0][0] = tmp[2]; // second derivative of first B-spline
A[0][1] = tmp[5];
A[0][2] = tmp[8];
if (end_tangent_.get() != 0) {
double tmp[8];
basis_.computeBasisValues(param_start[num_points-1], tmp, 1);
A[num_coefs - 1][num_coefs - 2] = tmp[5];
A[num_coefs - 1][num_coefs - 1] = tmp[7];
A[num_coefs - 2][num_coefs - 1] = 1.0;
} else {
A[num_coefs - 1][num_coefs - 1] = 1.0;
}
// Boundary element conditions
A[1][0] = 1.0;
break;
case NaturalAtEnd:
basis_.computeBasisValues(param_start[num_points-1], tmp, 2);
A[num_coefs - 1][num_coefs - 3] = tmp[5];
A[num_coefs - 1][num_coefs - 2] = tmp[8];
A[num_coefs - 1][num_coefs - 1] = tmp[11];
if (start_tangent_.get() != 0) {
basis_.computeBasisValues(param_start[0], tmp, 1);
A[0][0] = tmp[1]; // derivative of first B-spline
A[0][1] = tmp[3]; // derivative of second B-spline
A[1][0] = 1.0;
} else {
A[0][0] = 1.0;
}
// Boundary element conditions
A[num_coefs - 2][num_coefs - 1] = 1.0;
break;
case Free:
// Boundary element conditions
A[0][0] = 1.0;
A[num_coefs - 1][num_coefs - 1] = 1.0;
break;
default:
THROW("Unknown boundary condition type." << ctype_);
}
// interior conditions
int rowoffset = ((ctype_ == Free) ||
((ctype_ == NaturalAtEnd) && (start_tangent_.get() == 0)) ? 1 : 2);
int j;
for (j = 0; j < num_points-2; ++j) {
basis_.computeBasisValues(param_start[j+1], tmp, 0);
int column = 1 + (basis_.lastKnotInterval() - 4);
A[j + rowoffset][column] = tmp[0];
A[j + rowoffset][column + 1] = tmp[1];
A[j + rowoffset][column + 2] = tmp[2];
A[j + rowoffset][column + 3] = tmp[3];
}
// make the b vectors boundary condition
int offset = (ctype_ == Free ||
((ctype_ == NaturalAtEnd) && start_tangent_.get() == 0) ? 0 : 1);
switch(ctype_) {
case Hermite:
copy(start_tangent_->begin(), start_tangent_->end(), b[0].begin());
copy(end_tangent_->begin(), end_tangent_->end(), b[num_coefs-1].begin());
break;
case Natural:
b[0] = b[num_coefs-1] = vector<double>(dimension, 0);
break;
case NaturalAtStart:
b[0] = vector<double>(dimension, 0);
if (end_tangent_.get() != 0)
copy(end_tangent_->begin(), end_tangent_->end(), b[num_coefs-1].begin());
break;
case NaturalAtEnd:
b[num_coefs-1] = vector<double>(dimension, 0);
if (start_tangent_.get() != 0)
copy(start_tangent_->begin(), start_tangent_->end(), b[0].begin());
break;
default:
// do nothing
break;
}
// fill in interior of the b vectors
for (j = 0; j < num_points; ++j) {
copy(&(data_start[j * dimension]),
&(data_start[(j+1) * dimension]),
b[j + offset].begin());
}
// computing the unknown vector A c = b. b is overwritten by this unknown vector
LUsolveSystem(A, num_coefs, &b[0]);
// writing result to coefficients
coefs.resize(dimension*num_coefs);
for (int j = 0; j < num_coefs; ++j) {
copy(b[j].begin(), b[j].end(), &coefs[j*dimension]);
}
//#endif
}
// @@@ So far we're assuming that the user input fulfills demands wrt.
// degrees of freedom.
//===========================================================================
void SplineInterpolator::interpolate(const std::vector<double>& params,
const std::vector<double>& points,
const std::vector<int>& tangent_index,
const std::vector<double>& tangent_points,
std::vector<double>& coefs)
//===========================================================================
{
ALWAYS_ERROR_IF(basis_set_ == false,
"When using routine the basis_ must first be set/made.");
int num_points = (int)params.size();
int dimension = (int)points.size() / num_points;
int num_coefs = basis_.numCoefs();
int order = basis_.order();
int tsize = (int)tangent_index.size();
DEBUG_ERROR_IF(num_coefs != num_points + tsize,
"Inconsistency between size of basis and interpolation conditions.");
DEBUG_ERROR_IF(num_coefs < order,
"Insufficient number of points.");
coefs.resize(dimension*num_coefs);
int i, j;
// In the future there may be reason to want higher derivative information
// in the points. Should present no problem; to be implemented when needed.
// This code dates from the time when this code was dependent on the NEWMAT
// library. This is no longer the case, and the code has been substituted with
// the code below. However, it is kept here for reference reasons.
//
// ---------------------newmat dependent ----------------------------
// #if 0
// // @@@ sbr There should be performed an analysis regarding size of band matrix.
// // BandMatrix A(num_coefs, (order+1)/2, (order+1)/2);
// // Ought to run through parameters (and tangents), checking against basis;
// // then set up band matrix. This may fail. But then system should be singular.
// // The failsafe solution would be to implement A as a full matrix...
// BandMatrix A(num_coefs, order, order);
// A = 0;
// ColumnVector c(num_coefs);
// ColumnVector b(num_coefs);
// // Setting up the interpolation matrix A.
// int ti = 0; // Index to first unused element of tangent_points.
// for (i = 0; i < num_points; ++i) {
// bool der = ((tsize > ti) && (tangent_index[ti] == i)) ?
// true : false; // true = using derivative info.
// std::vector<double> tmp(2*order);
// basis_.computeBasisValues(params[i], &tmp[0], 1);
// int ki = basis_.knotInterval(params[i]); // knot-interval of param.
// // int column = 1 + (basis.lastKnotInterval() - 4);
// for (j = 0; j < order; ++j)
// if ((ki-order+1+j>=0) && (ki-order+1+j<=num_coefs)) {
// A.element(i+ti, ki-order+1+j) = tmp[2*j];
// if (der)
// A.element(i+ti+1, ki-order+1+j) = tmp[2*j+1];
// }
// if (der)
// ++ti;
// }
// // Now we are ready to repeatedly solve Ac = b for # = dimension different
// // right-hand-sides b.
// BandLUMatrix ALUfact = A; // Computes LU factorization
// ALWAYS_ERROR_IF(ALUfact.IsSingular(),
// "Matrix is singular! This should never happen!");
// coefs.resize(dimension*num_coefs);
// for (int dd = 0; dd < dimension; ++dd) {
// // Make the b vector
// ti = 0;
// for (i = 0; i < num_points; ++i) {
// bool der = ((tsize > ti) && (tangent_index[ti] == i)) ?
// true : false;
// b.element(i+ti) = points[i*dimension + dd];
// if (der) {
// b.element(i+ti+1) = tangent_points[ti*dimension + dd];
// ++ti;
// }
// }
// // Solve
// c = ALUfact.i() * b;
// // Copy results
// for (i = 0; i < num_coefs; ++i) {
// coefs[i*dimension + dd] = c.element(i);
// }
// }
// //#else
//--------------------------- newmat independent ---------------------
vector<vector<double> > A(num_coefs, vector<double>(num_coefs, 0));
vector<vector<double> > b(num_coefs, vector<double>(dimension, 0));
// setting up interpolation matrix A
int ti = 0; // index to first unused element of tangent_points
std::vector<double> tmp(2*order);
for (i = 0; i < num_points; ++i) {
bool der = ((tsize > ti) && (tangent_index[ti] == i)) ?
true : false; // true = using derivative info.
double par = params[i];
int ki = basis_.knotIntervalFuzzy(par); // knot-interval of param.
basis_.computeBasisValues(params[i], &tmp[0], 1);
//int ki = basis_.knotInterval(params[i]); // knot-interval of param.
// int column = 1 + (basis.lastKnotInterval() - 4);
for (j = 0; j < order; ++j)
if ((ki-order+1+j>=0) && (ki-order+1+j<=num_coefs)) {
A[i+ti][ki-order+1+j] = tmp[2*j];
if (der)
A[i+ti+1][ki-order+1+j] = tmp[2*j+1];
}
if (der)
++ti;
}
// generating right-hand side
ti = 0;
for (i = 0; i < num_points; ++i) {
bool der = ((tsize > ti) && (tangent_index[ti] == i)) ?
true : false;
vector<double>::const_iterator pointit
= points.begin() + i * dimension;
copy(pointit, pointit + dimension, b[i+ti].begin());
if (der) {
vector<double>::const_iterator tanptsit
= tangent_points.begin() + ti * dimension;
copy(tanptsit, tanptsit + dimension, b[i+ti+1].begin());
++ti;
}
}
// Now we are ready to solve Ac = b. b will be overwritten by solution
LUsolveSystem(A, num_coefs, &b[0]);
// copy results
for (i = 0; i < num_coefs; ++i) {
copy(b[i].begin(), b[i].end(), &coefs[i * dimension]);
}
//#endif
}
void intersect2Dcurves(const ParamCurve* cv1, const ParamCurve* cv2, double epsge,
vector<pair<double,double> >& intersections,
vector<int>& pretopology,
vector<pair<pair<double,double>, pair<double,double> > >& int_crvs)
//************************************************************************
//
// Intersect two 2D spline curves. Collect intersection parameters
// and pretopology information.
//
//***********************************************************************
{
// First make sure that the curves are spline curves.
ParamCurve* tmpcv1 = const_cast<ParamCurve*>(cv1);
ParamCurve* tmpcv2 = const_cast<ParamCurve*>(cv2);
SplineCurve* sc1 = tmpcv1->geometryCurve();
SplineCurve* sc2 = tmpcv2->geometryCurve();
if (sc1 == NULL || sc2 == NULL)
THROW("ParamCurves doesn't have a spline representation.");
MESSAGE_IF(cv1->dimension() != 2,
"Dimension different from 2, pretopology not reliable.");
// Make sisl curves and call sisl.
SISLCurve *pc1 = Curve2SISL(*sc1, false);
SISLCurve *pc2 = Curve2SISL(*sc2, false);
int kntrack = 0;
int trackflag = 0; // Do not make tracks.
SISLTrack **track =0;
int knpt=0, kncrv=0;
double *par1=0, *par2=0;
int *pretop = 0;
SISLIntcurve **vcrv = 0;
int stat = 0;
sh1857(pc1, pc2, 0.0, epsge, trackflag, &kntrack, &track,
&knpt, &par1, &par2, &pretop, &kncrv, &vcrv, &stat);
ALWAYS_ERROR_IF(stat<0,"Error in intersection, code: " << stat);
// Remember intersections points.
int ki;
for (ki=0; ki<knpt; ki++)
{
intersections.push_back(std::make_pair(par1[ki],par2[ki]));
pretopology.insert(pretopology.end(), pretop+4*ki, pretop+4*(ki+1));
}
// Remember intersection curves
for (ki=0; ki<kncrv; ++ki)
int_crvs.push_back(std::make_pair(std::make_pair(vcrv[ki]->epar1[0], vcrv[ki]->epar2[0]),
std::make_pair(vcrv[ki]->epar1[vcrv[ki]->ipoint-1],
vcrv[ki]->epar2[vcrv[ki]->ipoint-1])));
if (kncrv > 0)
freeIntcrvlist(vcrv, kncrv);
if (par1 != 0) free(par1);
if (par2 != 0) free(par2);
if (pc1 != 0) freeCurve(pc1);
if (pc2 != 0) freeCurve(pc2);
if (pretop != 0) free(pretop);
delete sc1;
delete sc2;
}
