src/2geom/piecewise.h

   1 /**
   2  * \file
   3  * \brief Piecewise function class
   4  *
   5  * Copyright 2007 Michael Sloan <mgsloan@gmail.com>
   6  *
   7  * This library is free software; you can redistribute it and/or
   8  * modify it either under the terms of the GNU Lesser General Public
   9  * License version 2.1 as published by the Free Software Foundation
  10  * (the "LGPL") or, at your option, under the terms of the Mozilla
  11  * Public License Version 1.1 (the "MPL"). If you do not alter this
  12  * notice, a recipient may use your version of this file under either
  13  * the MPL or the LGPL.
  14  *
  15  * You should have received a copy of the LGPL along with this library
  16  * in the file COPYING-LGPL-2.1; if not, output to the Free Software
  17  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  18  * You should have received a copy of the MPL along with this library
  19  * in the file COPYING-MPL-1.1
  20  *
  21  * The contents of this file are subject to the Mozilla Public License
  22  * Version 1.1 (the "License"); you may not use this file except in
  23  * compliance with the License. You may obtain a copy of the License at
  24  * http://www.mozilla.org/MPL/
  25  *
  26  * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY
  27  * OF ANY KIND, either express or implied. See the LGPL or the MPL for
  28  * the specific language governing rights and limitations.
  29  *
  30  */
  31
  32 #ifndef SEEN_GEOM_PW_SB_H
  33 #define SEEN_GEOM_PW_SB_H
  34
  35 #include <2geom/sbasis.h>
  36 #include <vector>
  37 #include <map>
  38
  39 #include <2geom/concepts.h>
  40 #include <2geom/isnan.h>
  41 #include <boost/concept_check.hpp>
  42
  43 namespace Geom {
  44 /**
  45  * %Piecewise function class.
  46  * The Piecewise class manages a sequence of elements of a type as segments and
  47  * the ’cuts’ between them. These cuts are time values which separate the pieces.
  48  * This function representation allows for more interesting functions, as it provides
  49  * a viable output for operations such as inversion, which may require multiple
  50  * SBasis to properly invert the original.
  51  * As for technical details, while the actual SBasis segments begin on the ﬁrst
  52  * cut and end on the last, the function is deﬁned throughout all inputs by ex-
  53  * tending the ﬁrst and last segments. The exact switching between segments is
  54  * arbitrarily such that beginnings (t=0) have preference over endings (t=1). This
  55  * only matters if it is discontinuous at the location.
  56  * \f[
  57  *      f(t) \rightarrow \left\{
  58  *      \begin{array}{cc}
  59  *      s_1,& t <= c_2 \\
  60  *      s_2,& c_2 <= t <= c_3\\
  61  *      \ldots \\
  62  *      s_n,& c_n <= t
  63  *      \end{array}\right.
  64  * \f]
  65  */
  66 template <typename T>
  67 class Piecewise {
  68   BOOST_CLASS_REQUIRE(T, Geom, FragmentConcept);
  69
  70   public:
  71     std::vector<double> cuts;
  72     std::vector<T> segs;
  73     //segs[i] stretches from cuts[i] to cuts[i+1].
  74
  75     Piecewise() {}
  76
  77     explicit Piecewise(const T &s) {
  78         push_cut(0.);
  79         push_seg(s);
  80         push_cut(1.);
  81     }
  82
  83     unsigned input_dim(){return 1;}
  84
  85     typedef typename T::output_type output_type;
  86
  87     explicit Piecewise(const output_type & v) {
  88         push_cut(0.);
  89         push_seg(T(v));
  90         push_cut(1.);
  91     }
  92
  93     inline void reserve(unsigned i) { segs.reserve(i); cuts.reserve(i + 1); }
  94
  95     inline T operator[](unsigned i) const { return segs[i]; }
  96     inline T &operator[](unsigned i) { return segs[i]; }
  97     inline output_type operator()(double t) const { return valueAt(t); }
  98     inline output_type valueAt(double t) const {
  99         unsigned n = segN(t);
 100         return segs[n](segT(t, n));
 101     }
 102     inline output_type firstValue() const {
 103         return valueAt(cuts.front());
 104     }
 105     inline output_type lastValue() const {
 106         return valueAt(cuts.back());
 107     }
 108
 109     /**
 110     *  The size of the returned vector equals n_derivs+1.
 111     */
 112     std::vector<output_type> valueAndDerivatives(double t, unsigned n_derivs) const {
 113         unsigned n = segN(t);
 114         std::vector<output_type> ret, val = segs[n].valueAndDerivatives(segT(t, n), n_derivs);
 115         double mult = 1;
 116         for(unsigned i = 0; i < val.size(); i++) {
 117             ret.push_back(val[i] * mult);
 118             mult /= cuts[n+1] - cuts[n];
 119         }
 120         return ret;
 121     }
 122
 123     //TODO: maybe it is not a good idea to have this?
 124     Piecewise<T> operator()(SBasis f);
 125     Piecewise<T> operator()(Piecewise<SBasis>f);
 126
 127     inline unsigned size() const { return segs.size(); }
 128     inline bool empty() const { return segs.empty(); }
 129     inline void clear() {
 130         segs.clear();
 131         cuts.clear();
 132     }
 133
 134     /**Convenience/implementation hiding function to add segment/cut pairs.
 135      * Asserts that basic size and order invariants are correct
 136      */
 137     inline void push(const T &s, double to) {
 138         assert(cuts.size() - segs.size() == 1);
 139         push_seg(s);
 140         push_cut(to);
 141     }
 142     //Convenience/implementation hiding function to add cuts.
 143     inline void push_cut(double c) {
 144         ASSERT_INVARIANTS(cuts.empty() || c > cuts.back());
 145         cuts.push_back(c);
 146     }
 147     //Convenience/implementation hiding function to add segments.
 148     inline void push_seg(const T &s) { segs.push_back(s); }
 149
 150     /**Returns the segment index which corresponds to a 'global' piecewise time.
 151      * Also takes optional low/high parameters to expedite the search for the segment.
 152      */
 153     inline unsigned segN(double t, int low = 0, int high = -1) const {
 154         high = (high == -1) ? size() : high;
 155         if(t < cuts[0]) return 0;
 156         if(t >= cuts[size()]) return size() - 1;
 157         while(low < high) {
 158             int mid = (high + low) / 2; //Lets not plan on having huge (> INT_MAX / 2) cut sequences
 159             double mv = cuts[mid];
 160             if(mv < t) {
 161                 if(t < cuts[mid + 1]) return mid; else low = mid + 1;
 162             } else if(t < mv) {
 163                 if(cuts[mid - 1] < t) return mid - 1; else high = mid - 1;
 164             } else {
 165                 return mid;
 166             }
 167         }
 168         return low;
 169     }
 170
 171     /**Returns the time within a segment, given the 'global' piecewise time.
 172      * Also takes an optional index parameter which may be used for efficiency or to find the time on a
 173      * segment outside its range.  If it is left to its default, -1, it will call segN to find the index.
 174      */
 175     inline double segT(double t, int i = -1) const {
 176         if(i == -1) i = segN(t);
 177         assert(i >= 0);
 178         return (t - cuts[i]) / (cuts[i+1] - cuts[i]);
 179     }
 180
 181     inline double mapToDomain(double t, unsigned i) const {
 182         return (1-t)*cuts[i] + t*cuts[i+1]; //same as: t * (cuts[i+1] - cuts[i]) + cuts[i]
 183     }
 184
 185     //Offsets the piecewise domain
 186     inline void offsetDomain(double o) {
 187         assert(IS_FINITE(o));
 188         if(o != 0)
 189             for(unsigned i = 0; i <= size(); i++)
 190                 cuts[i] += o;
 191     }
 192
 193     //Scales the domain of the function by a value. 0 will result in an empty Piecewise.
 194     inline void scaleDomain(double s) {
 195         assert(s > 0);
 196         if(s == 0) {
 197             cuts.clear(); segs.clear();
 198             return;
 199         }
 200         for(unsigned i = 0; i <= size(); i++)
 201             cuts[i] *= s;
 202     }
 203
 204     //Retrieves the domain in interval form
 205     inline Interval domain() const { return Interval(cuts.front(), cuts.back()); }
 206
 207     //Transforms the domain into another interval
 208     inline void setDomain(Interval dom) {
 209         if(empty()) return;
 210         /* dom can not be empty
 211         if(dom.isEmpty()) {
 212             cuts.clear(); segs.clear();
 213             return;
 214         }*/
 215         double cf = cuts.front();
 216         double o = dom.min() - cf, s = dom.extent() / (cuts.back() - cf);
 217         for(unsigned i = 0; i <= size(); i++)
 218             cuts[i] = (cuts[i] - cf) * s + o;
 219         //fix floating point precision errors.
 220         cuts[0] = dom.min();
 221         cuts[size()] = dom.max();
 222     }
 223
 224     //Concatenates this Piecewise function with another, offseting time of the other to match the end.
 225     inline void concat(const Piecewise<T> &other) {
 226         if(other.empty()) return;
 227
 228         if(empty()) {
 229             cuts = other.cuts; segs = other.segs;
 230             return;
 231         }
 232
 233         segs.insert(segs.end(), other.segs.begin(), other.segs.end());
 234         double t = cuts.back() - other.cuts.front();
 235         cuts.reserve(cuts.size() + other.size());
 236         for(unsigned i = 0; i < other.size(); i++)
 237             push_cut(other.cuts[i + 1] + t);
 238     }
 239
 240     //Like concat, but ensures continuity.
 241     inline void continuousConcat(const Piecewise<T> &other) {
 242         boost::function_requires<AddableConcept<typename T::output_type> >();
 243         if(other.empty()) return;
 244
 245         if(empty()) { segs = other.segs; cuts = other.cuts; return; }
 246
 247         typename T::output_type y = segs.back().at1() - other.segs.front().at0();
 248         double t = cuts.back() - other.cuts.front();
 249         reserve(size() + other.size());
 250         for(unsigned i = 0; i < other.size(); i++)
 251             push(other[i] + y, other.cuts[i + 1] + t);
 252     }
 253
 254     //returns true if the Piecewise<T> meets some basic invariants.
 255     inline bool invariants() const {
 256         // segs between cuts
 257         if(!(segs.size() + 1 == cuts.size() || (segs.empty() && cuts.empty())))
 258             return false;
 259         // cuts in order
 260         for(unsigned i = 0; i < segs.size(); i++)
 261             if(cuts[i] >= cuts[i+1])
 262                 return false;
 263         return true;
 264     }
 265
 266 };
 267
 268 template<typename T>
 269 inline typename FragmentConcept<T>::BoundsType bounds_fast(const Piecewise<T> &f) {
 270     boost::function_requires<FragmentConcept<T> >();
 271
 272     if(f.empty()) return typename FragmentConcept<T>::BoundsType();
 273     typename FragmentConcept<T>::BoundsType ret(bounds_fast(f[0]));
 274     for(unsigned i = 1; i < f.size(); i++)
 275         ret.unionWith(bounds_fast(f[i]));
 276     return ret;
 277 }
 278
 279 template<typename T>
 280 inline typename FragmentConcept<T>::BoundsType bounds_exact(const Piecewise<T> &f) {
 281     boost::function_requires<FragmentConcept<T> >();
 282
 283     if(f.empty()) return typename FragmentConcept<T>::BoundsType();
 284     typename FragmentConcept<T>::BoundsType ret(bounds_exact(f[0]));
 285     for(unsigned i = 1; i < f.size(); i++)
 286         ret.unionWith(bounds_exact(f[i]));
 287     return ret;
 288 }
 289
 290 template<typename T>
 291 inline typename FragmentConcept<T>::BoundsType bounds_local(const Piecewise<T> &f, const OptInterval &_m) {
 292     boost::function_requires<FragmentConcept<T> >();
 293
 294     if(f.empty() || !_m) return typename FragmentConcept<T>::BoundsType();
 295     Interval const &m = *_m;
 296     if(m.isSingular()) return typename FragmentConcept<T>::BoundsType(f(m.min()));
 297
 298     unsigned fi = f.segN(m.min()), ti = f.segN(m.max());
 299     double ft = f.segT(m.min(), fi), tt = f.segT(m.max(), ti);
 300
 301     if(fi == ti) return bounds_local(f[fi], Interval(ft, tt));
 302
 303     typename FragmentConcept<T>::BoundsType ret(bounds_local(f[fi], Interval(ft, 1.)));
 304     for(unsigned i = fi + 1; i < ti; i++)
 305         ret.unionWith(bounds_exact(f[i]));
 306     if(tt != 0.) ret.unionWith(bounds_local(f[ti], Interval(0., tt)));
 307
 308     return ret;
 309 }
 310
 311 //returns a portion of a piece of a Piecewise<T>, given the piece's index and a to/from time.
 312 template<typename T>
 313 T elem_portion(const Piecewise<T> &a, unsigned i, double from, double to) {
 314     assert(i < a.size());
 315     double rwidth = 1 / (a.cuts[i+1] - a.cuts[i]);
 316     return portion( a[i], (from - a.cuts[i]) * rwidth, (to - a.cuts[i]) * rwidth );
 317 }
 318
 319 /**Piecewise<T> partition(const Piecewise<T> &pw, std::vector<double> const &c);
 320  * Further subdivides the Piecewise<T> such that there is a cut at every value in c.
 321  * Precondition: c sorted lower to higher.
 322  *
 323  * //Given Piecewise<T> a and b:
 324  * Piecewise<T> ac = a.partition(b.cuts);
 325  * Piecewise<T> bc = b.partition(a.cuts);
 326  * //ac.cuts should be equivalent to bc.cuts
 327  */
 328 template<typename T>
 329 Piecewise<T> partition(const Piecewise<T> &pw, std::vector<double> const &c) {
 330     assert(pw.invariants());
 331     if(c.empty()) return Piecewise<T>(pw);
 332
 333     Piecewise<T> ret = Piecewise<T>();
 334     ret.reserve(c.size() + pw.cuts.size() - 1);
 335
 336     if(pw.empty()) {
 337         ret.cuts = c;
 338         for(unsigned i = 0; i < c.size() - 1; i++)
 339             ret.push_seg(T());
 340         return ret;
 341     }
 342
 343     unsigned si = 0, ci = 0;     //Segment index, Cut index
 344
 345     //if the cuts have something earlier than the Piecewise<T>, add portions of the first segment
 346     while(c[ci] < pw.cuts.front() && ci < c.size()) {
 347         bool isLast = (ci == c.size()-1 || c[ci + 1] >= pw.cuts.front());
 348         ret.push_cut(c[ci]);
 349         ret.push_seg( elem_portion(pw, 0, c[ci], isLast ? pw.cuts.front() : c[ci + 1]) );
 350         ci++;
 351     }
 352
 353     ret.push_cut(pw.cuts[0]);
 354     double prev = pw.cuts[0];    //previous cut
 355     //Loop which handles cuts within the Piecewise<T> domain
 356     //Should have the cuts = segs + 1 invariant
 357     while(si < pw.size() && ci <= c.size()) {
 358         if(ci == c.size() && prev <= pw.cuts[si]) { //cuts exhausted, straight copy the rest
 359             ret.segs.insert(ret.segs.end(), pw.segs.begin() + si, pw.segs.end());
 360             ret.cuts.insert(ret.cuts.end(), pw.cuts.begin() + si + 1, pw.cuts.end());
 361             return ret;
 362         }else if(ci == c.size() || c[ci] >= pw.cuts[si + 1]) {  //no more cuts within this segment, finalize
 363             if(prev > pw.cuts[si]) {      //segment already has cuts, so portion is required
 364                 ret.push_seg(portion(pw[si], pw.segT(prev, si), 1.0));
 365             } else {                     //plain copy is fine
 366                 ret.push_seg(pw[si]);
 367             }
 368             ret.push_cut(pw.cuts[si + 1]);
 369             prev = pw.cuts[si + 1];
 370             si++;
 371         } else if(c[ci] == pw.cuts[si]){                  //coincident
 372             //Already finalized the seg with the code immediately above
 373             ci++;
 374         } else {                                         //plain old subdivision
 375             ret.push(elem_portion(pw, si, prev, c[ci]), c[ci]);
 376             prev = c[ci];
 377             ci++;
 378         }
 379     }
 380
 381     //input cuts extend further than this Piecewise<T>, extend the last segment.
 382     while(ci < c.size()) {
 383         if(c[ci] > prev) {
 384             ret.push(elem_portion(pw, pw.size() - 1, prev, c[ci]), c[ci]);
 385             prev = c[ci];
 386         }
 387         ci++;
 388     }
 389     return ret;
 390 }
 391
 392 /**Piecewise<T> portion(const Piecewise<T> &pw, double from, double to);
 393  * Returns a Piecewise<T> with a defined domain of [min(from, to), max(from, to)].
 394  */
 395 template<typename T>
 396 Piecewise<T> portion(const Piecewise<T> &pw, double from, double to) {
 397     if(pw.empty() || from == to) return Piecewise<T>();
 398
 399     Piecewise<T> ret;
 400
 401     double temp = from;
 402     from = std::min(from, to);
 403     to = std::max(temp, to);
 404
 405     unsigned i = pw.segN(from);
 406     ret.push_cut(from);
 407     if(i == pw.size() - 1 || to <= pw.cuts[i + 1]) {    //to/from inhabit the same segment
 408         ret.push(elem_portion(pw, i, from, to), to);
 409         return ret;
 410     }
 411     ret.push_seg(portion( pw[i], pw.segT(from, i), 1.0 ));
 412     i++;
 413     unsigned fi = pw.segN(to, i);
 414     ret.reserve(fi - i + 1);
 415     if (to == pw.cuts[fi]) fi-=1;
 416
 417     ret.segs.insert(ret.segs.end(), pw.segs.begin() + i, pw.segs.begin() + fi);  //copy segs
 418     ret.cuts.insert(ret.cuts.end(), pw.cuts.begin() + i, pw.cuts.begin() + fi + 1);  //and their cuts
 419
 420     ret.push_seg( portion(pw[fi], 0.0, pw.segT(to, fi)));
 421     if(to != ret.cuts.back()) ret.push_cut(to);
 422     ret.invariants();
 423     return ret;
 424 }
 425
 426 //TODO: seems like these should be mutating
 427 template<typename T>
 428 Piecewise<T> remove_short_cuts(Piecewise<T> const &f, double tol) {
 429     if(f.empty()) return f;
 430     Piecewise<T> ret;
 431     ret.reserve(f.size());
 432     ret.push_cut(f.cuts[0]);
 433     for(unsigned i=0; i<f.size(); i++){
 434         if (f.cuts[i+1]-f.cuts[i] >= tol || i==f.size()-1) {
 435             ret.push(f[i], f.cuts[i+1]);
 436         }
 437     }
 438     return ret;
 439 }
 440
 441 //TODO: seems like these should be mutating
 442 template<typename T>
 443 Piecewise<T> remove_short_cuts_extending(Piecewise<T> const &f, double tol) {
 444     if(f.empty()) return f;
 445     Piecewise<T> ret;
 446     ret.reserve(f.size());
 447     ret.push_cut(f.cuts[0]);
 448     double last = f.cuts[0]; // last cut included
 449     for(unsigned i=0; i<f.size(); i++){
 450         if (f.cuts[i+1]-f.cuts[i] >= tol) {
 451             ret.push(elem_portion(f, i, last, f.cuts[i+1]), f.cuts[i+1]);
 452             last = f.cuts[i+1];
 453         }
 454     }
 455     return ret;
 456 }
 457
 458 template<typename T>
 459 std::vector<double> roots(const Piecewise<T> &pw) {
 460     std::vector<double> ret;
 461     for(unsigned i = 0; i < pw.size(); i++) {
 462         std::vector<double> sr = roots(pw[i]);
 463         for (unsigned j = 0; j < sr.size(); j++) ret.push_back(sr[j] * (pw.cuts[i + 1] - pw.cuts[i]) + pw.cuts[i]);
 464
 465     }
 466     return ret;
 467 }
 468
 469 //IMPL: OffsetableConcept
 470 template<typename T>
 471 Piecewise<T> operator+(Piecewise<T> const &a, typename T::output_type b) {
 472     boost::function_requires<OffsetableConcept<T> >();
 473 //TODO:empty
 474     Piecewise<T> ret;
 475     ret.segs.reserve(a.size());
 476     ret.cuts = a.cuts;
 477     for(unsigned i = 0; i < a.size();i++)
 478         ret.push_seg(a[i] + b);
 479     return ret;
 480 }
 481 template<typename T>
 482 Piecewise<T> operator-(Piecewise<T> const &a, typename T::output_type b) {
 483     boost::function_requires<OffsetableConcept<T> >();
 484 //TODO: empty
 485     Piecewise<T> ret;
 486     ret.segs.reserve(a.size());
 487     ret.cuts = a.cuts;
 488     for(unsigned i = 0; i < a.size();i++)
 489         ret.push_seg(a[i] - b);
 490     return ret;
 491 }
 492 template<typename T>
 493 Piecewise<T>& operator+=(Piecewise<T>& a, typename T::output_type b) {
 494     boost::function_requires<OffsetableConcept<T> >();
 495
 496     if(a.empty()) { a.push_cut(0.); a.push(T(b), 1.); return a; }
 497
 498     for(unsigned i = 0; i < a.size();i++)
 499         a[i] += b;
 500     return a;
 501 }
 502 template<typename T>
 503 Piecewise<T>& operator-=(Piecewise<T>& a, typename T::output_type b) {
 504     boost::function_requires<OffsetableConcept<T> >();
 505
 506     if(a.empty()) { a.push_cut(0.); a.push(T(-b), 1.); return a; }
 507
 508     for(unsigned i = 0;i < a.size();i++)
 509         a[i] -= b;
 510     return a;
 511 }
 512
 513 //IMPL: ScalableConcept
 514 template<typename T>
 515 Piecewise<T> operator-(Piecewise<T> const &a) {
 516     boost::function_requires<ScalableConcept<T> >();
 517
 518     Piecewise<T> ret;
 519     ret.segs.reserve(a.size());
 520     ret.cuts = a.cuts;
 521     for(unsigned i = 0; i < a.size();i++)
 522         ret.push_seg(- a[i]);
 523     return ret;
 524 }
 525 template<typename T>
 526 Piecewise<T> operator*(Piecewise<T> const &a, double b) {
 527     boost::function_requires<ScalableConcept<T> >();
 528
 529     if(a.empty()) return Piecewise<T>();
 530
 531     Piecewise<T> ret;
 532     ret.segs.reserve(a.size());
 533     ret.cuts = a.cuts;
 534     for(unsigned i = 0; i < a.size();i++)
 535         ret.push_seg(a[i] * b);
 536     return ret;
 537 }
 538 template<typename T>
 539 Piecewise<T> operator*(Piecewise<T> const &a, T b) {
 540     boost::function_requires<ScalableConcept<T> >();
 541
 542     if(a.empty()) return Piecewise<T>();
 543
 544     Piecewise<T> ret;
 545     ret.segs.reserve(a.size());
 546     ret.cuts = a.cuts;
 547     for(unsigned i = 0; i < a.size();i++)
 548         ret.push_seg(a[i] * b);
 549     return ret;
 550 }
 551 template<typename T>
 552 Piecewise<T> operator/(Piecewise<T> const &a, double b) {
 553     boost::function_requires<ScalableConcept<T> >();
 554
 555     //FIXME: b == 0?
 556     if(a.empty()) return Piecewise<T>();
 557
 558     Piecewise<T> ret;
 559     ret.segs.reserve(a.size());
 560     ret.cuts = a.cuts;
 561     for(unsigned i = 0; i < a.size();i++)
 562         ret.push_seg(a[i] / b);
 563     return ret;
 564 }
 565 template<typename T>
 566 Piecewise<T>& operator*=(Piecewise<T>& a, double b) {
 567     boost::function_requires<ScalableConcept<T> >();
 568
 569     for(unsigned i = 0; i < a.size();i++)
 570         a[i] *= b;
 571     return a;
 572 }
 573 template<typename T>
 574 Piecewise<T>& operator/=(Piecewise<T>& a, double b) {
 575     boost::function_requires<ScalableConcept<T> >();
 576
 577     //FIXME: b == 0?
 578
 579     for(unsigned i = 0; i < a.size();i++)
 580         a[i] /= b;
 581     return a;
 582 }
 583
 584 //IMPL: AddableConcept
 585 template<typename T>
 586 Piecewise<T> operator+(Piecewise<T> const &a, Piecewise<T> const &b) {
 587     boost::function_requires<AddableConcept<T> >();
 588
 589     Piecewise<T> pa = partition(a, b.cuts), pb = partition(b, a.cuts);
 590     Piecewise<T> ret;
 591     assert(pa.size() == pb.size());
 592     ret.segs.reserve(pa.size());
 593     ret.cuts = pa.cuts;
 594     for (unsigned i = 0; i < pa.size(); i++)
 595         ret.push_seg(pa[i] + pb[i]);
 596     return ret;
 597 }
 598 template<typename T>
 599 Piecewise<T> operator-(Piecewise<T> const &a, Piecewise<T> const &b) {
 600     boost::function_requires<AddableConcept<T> >();
 601
 602     Piecewise<T> pa = partition(a, b.cuts), pb = partition(b, a.cuts);
 603     Piecewise<T> ret = Piecewise<T>();
 604     assert(pa.size() == pb.size());
 605     ret.segs.reserve(pa.size());
 606     ret.cuts = pa.cuts;
 607     for (unsigned i = 0; i < pa.size(); i++)
 608         ret.push_seg(pa[i] - pb[i]);
 609     return ret;
 610 }
 611 template<typename T>
 612 inline Piecewise<T>& operator+=(Piecewise<T> &a, Piecewise<T> const &b) {
 613     a = a+b;
 614     return a;
 615 }
 616 template<typename T>
 617 inline Piecewise<T>& operator-=(Piecewise<T> &a, Piecewise<T> const &b) {
 618     a = a-b;
 619     return a;
 620 }
 621
 622 template<typename T1,typename T2>
 623 Piecewise<T2> operator*(Piecewise<T1> const &a, Piecewise<T2> const &b) {
 624     //function_requires<MultiplicableConcept<T1> >();
 625     //function_requires<MultiplicableConcept<T2> >();
 626
 627     Piecewise<T1> pa = partition(a, b.cuts);
 628     Piecewise<T2> pb = partition(b, a.cuts);
 629     Piecewise<T2> ret = Piecewise<T2>();
 630     assert(pa.size() == pb.size());
 631     ret.segs.reserve(pa.size());
 632     ret.cuts = pa.cuts;
 633     for (unsigned i = 0; i < pa.size(); i++)
 634         ret.push_seg(pa[i] * pb[i]);
 635     return ret;
 636 }
 637
 638 template<typename T>
 639 inline Piecewise<T>& operator*=(Piecewise<T> &a, Piecewise<T> const &b) {
 640     a = a * b;
 641     return a;
 642 }
 643
 644 Piecewise<SBasis> divide(Piecewise<SBasis> const &a, Piecewise<SBasis> const &b, unsigned k);
 645 //TODO: replace divide(a,b,k) by divide(a,b,tol,k)?
 646 //TODO: atm, relative error is ~(tol/a)%. Find a way to make it independant of a.
 647 //Nota: the result is 'truncated' where b is smaller than 'zero': ~ a/max(b,zero).
 648 Piecewise<SBasis>
 649 divide(Piecewise<SBasis> const &a, Piecewise<SBasis> const &b, double tol, unsigned k, double zero=1.e-3);
 650 Piecewise<SBasis>
 651 divide(SBasis const &a, Piecewise<SBasis> const &b, double tol, unsigned k, double zero=1.e-3);
 652 Piecewise<SBasis>
 653 divide(Piecewise<SBasis> const &a, SBasis const &b, double tol, unsigned k, double zero=1.e-3);
 654 Piecewise<SBasis>
 655 divide(SBasis const &a, SBasis const &b, double tol, unsigned k, double zero=1.e-3);
 656
 657 //Composition: functions called compose_* are pieces of compose that are factored out in pw.cpp.
 658 std::map<double,unsigned> compose_pullback(std::vector<double> const &cuts, SBasis const &g);
 659 int compose_findSegIdx(std::map<double,unsigned>::iterator  const &cut,
 660                        std::map<double,unsigned>::iterator  const &next,
 661                        std::vector<double>  const &levels,
 662                        SBasis const &g);
 663
 664 //TODO: add concept check
 665 template<typename T>
 666 Piecewise<T> compose(Piecewise<T> const &f, SBasis const &g){
 667     Piecewise<T> result;
 668     if (f.empty()) return result;
 669     if (g.isZero()) return Piecewise<T>(f(0));
 670     if (f.size()==1){
 671         double t0 = f.cuts[0], width = f.cuts[1] - t0;
 672         return (Piecewise<T>) compose(f.segs[0],compose(Linear(-t0 / width, (1-t0) / width), g));
 673     }
 674
 675     //first check bounds...
 676     Interval bs = *bounds_fast(g);
 677     if (f.cuts.front() > bs.max()  || bs.min() > f.cuts.back()){
 678         int idx = (bs.max() < f.cuts[1]) ? 0 : f.cuts.size()-2;
 679         double t0 = f.cuts[idx], width = f.cuts[idx+1] - t0;
 680         return (Piecewise<T>) compose(f.segs[idx],compose(Linear(-t0 / width, (1-t0) / width), g));
 681     }
 682
 683     std::vector<double> levels;//we can forget first and last cuts...
 684     levels.insert(levels.begin(),f.cuts.begin()+1,f.cuts.end()-1);
 685     //TODO: use a std::vector<pairs<double,unsigned> > instead of a map<double,unsigned>.
 686     std::map<double,unsigned> cuts_pb = compose_pullback(levels,g);
 687
 688     //-- Compose each piece of g with the relevant seg of f.
 689     result.cuts.push_back(0.);
 690     std::map<double,unsigned>::iterator cut=cuts_pb.begin();
 691     std::map<double,unsigned>::iterator next=cut; next++;
 692     while(next!=cuts_pb.end()){
 693         //assert(std::abs(int((*cut).second-(*next).second))<1);
 694         //TODO: find a way to recover from this error? the root finder missed some root;
 695         //  the levels/variations of f might be too close/fast...
 696         int idx = compose_findSegIdx(cut,next,levels,g);
 697         double t0=(*cut).first;
 698         double t1=(*next).first;
 699
 700         SBasis sub_g=compose(g, Linear(t0,t1));
 701         sub_g=compose(Linear(-f.cuts[idx]/(f.cuts[idx+1]-f.cuts[idx]),
 702                              (1-f.cuts[idx])/(f.cuts[idx+1]-f.cuts[idx])),sub_g);
 703         result.push(compose(f[idx],sub_g),t1);
 704         cut++;
 705         next++;
 706     }
 707     return(result);
 708 }
 709
 710 //TODO: add concept check for following composition functions
 711 template<typename T>
 712 Piecewise<T> compose(Piecewise<T> const &f, Piecewise<SBasis> const &g){
 713   Piecewise<T> result;
 714   for(unsigned i = 0; i < g.segs.size(); i++){
 715       Piecewise<T> fgi=compose(f, g.segs[i]);
 716       fgi.setDomain(Interval(g.cuts[i], g.cuts[i+1]));
 717       result.concat(fgi);
 718   }
 719   return result;
 720 }
 721
 722 /*
 723 Piecewise<D2<SBasis> > compose(D2<SBasis2d> const &sb2d, Piecewise<D2<SBasis> > const &pwd2sb){
 724   Piecewise<D2<SBasis> > result;
 725   result.push_cut(0.);
 726   for(unsigned i = 0; i < pwd2sb.size(); i++){
 727      result.push(compose_each(sb2d,pwd2sb[i]),i+1);
 728   }
 729   return result;
 730 }*/
 731
 732 template <typename T>
 733 Piecewise<T> Piecewise<T>::operator()(SBasis f){return compose((*this),f);}
 734 template <typename T>
 735 Piecewise<T> Piecewise<T>::operator()(Piecewise<SBasis>f){return compose((*this),f);}
 736
 737 template<typename T>
 738 Piecewise<T> integral(Piecewise<T> const &a) {
 739     Piecewise<T> result;
 740     result.segs.resize(a.segs.size());
 741     result.cuts = a.cuts;
 742     typename T::output_type c = a.segs[0].at0();
 743     for(unsigned i = 0; i < a.segs.size(); i++){
 744         result.segs[i] = integral(a.segs[i])*(a.cuts[i+1]-a.cuts[i]);
 745         result.segs[i]+= c-result.segs[i].at0();
 746         c = result.segs[i].at1();
 747     }
 748     return result;
 749 }
 750
 751 template<typename T>
 752 Piecewise<T> derivative(Piecewise<T> const &a) {
 753     Piecewise<T> result;
 754     result.segs.resize(a.segs.size());
 755     result.cuts = a.cuts;
 756     for(unsigned i = 0; i < a.segs.size(); i++){
 757         result.segs[i] = derivative(a.segs[i])/(a.cuts[i+1]-a.cuts[i]);
 758     }
 759     return result;
 760 }
 761
 762 std::vector<double> roots(Piecewise<SBasis> const &f);
 763
 764 std::vector<std::vector<double> >multi_roots(Piecewise<SBasis> const &f, std::vector<double> const &values);
 765
 766 template<typename T>
 767 Piecewise<T> reverse(Piecewise<T> const &f) {
 768     Piecewise<T> ret = Piecewise<T>();
 769     ret.reserve(f.size());
 770     double start = f.cuts[0];
 771     double end = f.cuts.back();
 772     for (unsigned i = 0; i < f.cuts.size(); i++) {
 773         double x = f.cuts[f.cuts.size() - 1 - i];
 774         ret.push_cut(end - (x - start));
 775     }
 776     for (unsigned i = 0; i < f.segs.size(); i++)
 777         ret.push_seg(reverse(f[f.segs.size() - i - 1]));
 778     return ret;
 779 }
 780
 781 /**
 782  *  Interpolates between a and b.
 783  *  \return a if t = 0, b if t = 1, or an interpolation between a and b for t in [0,1]
 784  *  \relates Piecewise
 785  */
 786 template<typename T>
 787 Piecewise<T> lerp(double t, Piecewise<T> const &a, Piecewise<T> b) {
 788     // Make sure both paths have the same number of segments and cuts at the same locations
 789     b.setDomain(a.domain());
 790     Piecewise<T> pA = partition(a, b.cuts);
 791     Piecewise<T> pB = partition(b, a.cuts);
 792
 793     return (pA*(1-t)  +  pB*t);
 794 }
 795
 796 }
 797 #endif //SEEN_GEOM_PW_SB_H
 798 /*
 799   Local Variables:
 800   mode:c++
 801   c-file-style:"stroustrup"
 802   c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
 803   indent-tabs-mode:nil
 804   fill-column:99
 805   End:
 806 */
 807 // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:encoding=utf-8:textwidth=99 :