//===========================================================================
void SplineCurve::appendSelfPeriodic()
//===========================================================================
{
// Testing that the curve actually is knot-periodic.
// This test may be superfluous, the caller is supposed to know
// that the curve is periodic before calling this function.
// If that test was done with a larger tolerance than default,
// this test may fail, making a mess of things.
// Eventually, the cont number could be supplied from outside,
// but this is hard to make work without changing calling code
// a lot (in subCurve). Maybe we should let the tolerance be an
// argument?
int cont = GeometryTools::analyzePeriodicity(*this);
if (cont < 0) {
THROW ("Curve seems to be nonperiodic. Should have been periodic!");
}
// Fill in new knot vector.
std::vector<double> new_knots(basis_.begin(), basis_.end());
double delta = endparam() - startparam();
std::transform(basis_.begin() + order() + cont + 1, basis_.end(),
std::back_inserter(new_knots),
std::bind1st(std::plus<double>(), delta));
int newn = 2*numCoefs() - cont - 1;
ASSERT(newn + order() == int(new_knots.size()));
// Fill in new coefficient vector.
std::vector<double>& c = rational_ ? rcoefs_ : coefs_;
std::vector<double> new_coefs(c);
int effdim = rational_ ? dim_ + 1 : dim_;
std::copy(c.begin() + effdim*(cont + 1), c.end(),
std::back_inserter(new_coefs));
ASSERT(effdim*newn == int(new_coefs.size()));
// Make a BsplineBasis object in order to swap.
BsplineBasis temp(newn, order(), new_knots.begin());
basis_.swap(temp);
c.swap(new_coefs);
}
//===========================================================================
void SplineCurve::makeKnotEndRegular()
//===========================================================================
{
// Testing whether knotstart is already d+1-regular.
if (basis_.begin()[numCoefs()] < basis_.begin()[numCoefs() + order() - 1]) {
double tpar = basis_.endparam();
int ti = numCoefs(); // Index of first occurence of tpar.
int mt = 1; // Multiplicity of tpar.
while ((basis_.begin()[ti + mt] == tpar) && (mt < order())) ++mt;
std::vector<double> new_knots;
for (int i = 0; i < order() - mt; ++i) new_knots.push_back(tpar);
insertKnot(new_knots);
coefs_.erase(coefs_begin() + (numCoefs() - order() + mt) * dim_,
coefs_begin() + numCoefs() * dim_);
basis_ = BsplineBasis(order(), basis_.begin(),
basis_.begin() + numCoefs() + mt);
}
}
//===========================================================================
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));
}
}