00001 #ifndef SPLINE
00002 #define SPLINE
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00038
00039
00040 #include <map>
00041 #include <set>
00042 #include <functional>
00043 #include <utility>
00044 #include <vector>
00045
00046 namespace cxxtls
00047 {
00048
00049
00050
00051
00052
00053
00054 template<class X, class Y, class D=double>
00055 class spline
00163 {
00164
00165 public:
00166
00167 typedef X Xt;
00168 typedef Y Yt;
00169 typedef D Dt;
00170
00171 struct info
00172 {
00173 X y_;
00174
00175 D s_;
00176
00177
00178
00179 info(X y, D s=0)
00180 : y_(y),
00181 s_(s)
00182 {
00183 }
00184
00185 operator X() const { return y_; }
00186 };
00187
00188 typedef std::multimap<X,info> samples_t;
00189 typedef typename samples_t::value_type sample_t;
00190
00191 typedef typename samples_t::iterator iterator;
00192 typedef typename samples_t::const_iterator const_iterator;
00193
00194
00195 typedef Y (spline::* interpolator_t) (X const &) const;
00198
00199 typedef spline<X,Y,D> self;
00200
00201 private:
00202
00203 samples_t samples_;
00204 interpolator_t interpolator_;
00205 bool dirty_;
00206
00207
00208
00209
00210
00211 public:
00212
00213 inline void swap(self &rhs);
00214
00215 inline spline();
00216 inline spline(interpolator_t a);
00217 inline spline(self const& rhs);
00218 inline self &operator= (self const& rhs);
00219
00220 inline void add_sample(X const &x, Y const &y) ;
00221 inline void add_sample(sample_t const &p);
00222
00223 inline interpolator_t interpolator() const ;
00224 inline void set_interpolator(interpolator_t t);
00225
00226 template<class SampleIterator>
00227 inline
00228 spline(SampleIterator first, SampleIterator last, interpolator_t t);
00229
00230 template<class OtherKind>
00231 inline
00232 spline(OtherKind const &rhs, interpolator_t terp);
00233
00234 inline Y operator() (X location) const;
00235
00236
00237 template<class Operation>
00238 void apply(Operation const&);
00239
00240 inline const_iterator ceil(X const& x) const;
00241
00242 const_iterator floor(X const &x) const;
00243
00244
00245
00246
00247 Y interpolate_step (X const&) const;
00248 Y interpolate_line (X const&) const;
00249 Y interpolate_cubic(X const&) const;
00250
00251 inline const_iterator begin() const;
00252 inline const_iterator end() const;
00253
00254 inline sample_t front() const;
00255 inline sample_t back() const ;
00256
00257 size_t size() const;
00258
00259 void domain(std::set<X> *target) const;
00260
00261 template<class Iterator> void include(Iterator, Iterator);
00262
00263 template<class SF> inline void include(SF const &rhs);
00264
00265 template<class Other, class Operation>
00266 void combine(self &output, Other const&rhs, Operation const &op);
00267
00268 template<class T>
00269 inline self &operator+= (T const &rhs);
00270
00271 template<class T>
00272 inline self &operator-= (T const &rhs);
00273
00274 template<class T>
00275 inline self &operator*= (T const &rhs);
00276
00277 template<class T>
00278 inline self &operator/= (T const &rhs);
00279
00280 template<class OutputIterator>
00281 void
00282 evaluate(X start_x,
00283 X delta_x,
00284 OutputIterator begin,
00285 OutputIterator end
00286 ) const;
00287
00288 void compute_coefficients();
00289
00290 };
00291
00292
00294
00295 template<class X, class Y, class D>
00296 inline
00297 typename spline<X,Y,D>::const_iterator
00298 spline<X,Y,D>::
00299 begin() const
00300 {
00301 return samples_.begin();
00302 }
00303
00304 template<class X, class Y, class D>
00305 inline
00306 typename spline<X,Y,D>::const_iterator
00307 spline<X,Y,D>::
00308 end() const
00309 {
00310 return samples_.end();
00311 }
00312
00314
00315 template<class X, class Y, class D>
00316 inline
00317 spline<X,Y,D>::
00318 spline()
00319 : interpolator_(&self::interpolate_step),
00320 dirty_(true)
00321 {
00326
00327 }
00328
00330 template<class X, class Y, class D>
00331 inline
00332 spline<X,Y,D>::
00333 spline(interpolator_t a)
00334 : interpolator_(a),
00335 dirty_(true)
00336 {
00338 }
00339
00341 template<class X, class Y, class D>
00342 inline
00343 spline<X,Y,D>::
00344 spline(self const& rhs)
00345 : interpolator_(rhs.interpolator_)
00346 {
00348 include(rhs);
00349 }
00350
00351
00353 template<class X, class Y, class D>
00354 template<class SampleIterator>
00355 inline
00356 spline<X,Y,D>::
00357 spline(SampleIterator first,
00358 SampleIterator last,
00359 typename spline<X,Y,D>::interpolator_t t
00360 )
00361 : interpolator_(t),
00362 dirty_(true)
00363 {
00367
00368 include(first,last);
00369
00370 }
00371
00372
00374 template<class X, class Y, class D>
00375 template<class OtherKind>
00376 inline
00377 spline<X,Y,D>::
00378 spline(OtherKind const &rhs,
00379 typename spline<X,Y,D>::interpolator_t terp
00380 )
00381 : interpolator_(terp),
00382 dirty_(true)
00383 {
00386
00387 include(rhs.begin(), rhs.end());
00388 }
00390 template<class X, class Y, class D>
00391 inline
00392 void
00393 spline<X,Y,D>::
00394 swap(self &rhs)
00400 {
00401 samples_.swap(rhs.samples_);
00402
00403 std::swap (dirty_, rhs.dirty_);
00404 std::swap (interpolator_, rhs.interpolator_);
00405
00406 }
00407
00409 template<class X, class Y, class D>
00410 inline
00411 typename spline<X,Y,D>::self&
00412 spline<X,Y,D>::
00413 operator=( typename spline<X,Y,D>::self const& rhs)
00414 {
00416
00417 samples_.erase(samples_.begin(), samples_.end());
00418 include(rhs);
00419 interpolator_ = rhs.interpolator_;
00420 return *this;
00421 }
00422
00423
00425
00426 template<class X, class Y, class D>
00427 inline
00428 typename spline<X,Y,D>::const_iterator
00429 spline<X,Y,D>::
00430 ceil(typename spline<X,Y,D>::Xt const& x) const
00433 {
00434 return samples_.upper_bound(x);
00435 }
00436
00438
00439 template<class X, class Y, class D>
00440 inline
00441 typename spline<X,Y,D>::Yt
00442 spline<X,Y,D>::
00443 operator() (typename spline<X,Y,D>::Xt location) const
00444 {
00449
00450 return (this->*interpolator_)(location);
00451 }
00452
00454 template<class X, class Y, class D>
00455 inline
00456 void
00457 spline<X,Y,D>::
00458 add_sample(X const &x, Y const &y)
00459
00460
00461
00462 {
00463 add_sample(sample_t(x,y));
00464 }
00466
00467 template<class X, class Y, class D>
00468 inline
00469 void
00470 spline<X, Y,D>::
00471 add_sample(sample_t const &p)
00472
00473
00474 {
00475 dirty_ = true;
00476
00477 iterator where = samples_.find(p.first);
00478
00479 if(where == samples_.end())
00480 samples_.insert(p);
00481 else
00482 where->second = p.second;
00483
00484 }
00485
00487 template<class X, class Y, class D>
00488 template<class Other, class Operation>
00489 void
00490 spline<X,Y,D>::
00491 combine(spline<X,Y,D> &output, Other const&rhs, Operation const &op)
00497 {
00498 std::set<X> xs;
00499
00500 domain(&xs);
00501 rhs.domain(&xs);
00502
00503 self tmp(this->interpolator_);
00504
00505 typename std::set<X>::const_iterator first = xs.begin(), last = xs.end();
00506
00507 while(first != last)
00508 {
00509 X const &x = *first++;
00510
00511 Y y = op( (*this)(x) , rhs(x) );
00512
00513 tmp.add_sample(sample_t(x, y) );
00514
00515 }
00516
00517 tmp.swap(output);
00518
00519 }
00521
00522 template<class X, class Y, class D>
00523 template<class Iterator>
00524 void
00525 spline<X, Y, D>::
00526 include(Iterator l, Iterator g)
00527
00535 {
00536
00537 while(l != g)
00538 {
00539 sample_t tmp(l->first, static_cast<Y>(l->second) );
00540
00541
00542
00543
00544
00545 add_sample(tmp);
00546 ++l;
00547 }
00548 }
00549
00551
00552 template<class X, class Y, class D>
00553 template<class SF>
00554 inline
00555 void
00556 spline<X, Y, D>::
00557 include(SF const &rhs)
00563 {
00564 include(rhs.begin(), rhs.end());
00565 }
00566
00568
00569 template<class X, class Y, class D>
00570 inline
00571 typename spline<X, Y, D>::interpolator_t
00572 spline<X, Y, D>::
00573 interpolator() const
00575 {
00576 return interpolator_;
00577 }
00578
00580
00581 template<class X, class Y, class D>
00582 inline
00583 void
00584 spline<X,Y,D>::
00585 set_interpolator(interpolator_t t)
00586 {
00587 interpolator_ = t;
00588 dirty_ = true;
00589 }
00590
00592
00593 template<class X, class Y, class D>
00594 inline
00595 typename spline<X, Y, D>::sample_t
00596 spline<X,Y,D>::
00597 front() const
00599 {
00600 if(begin() != end())
00601 return *begin();
00602
00603 return sample_t(X(), Y());
00604 }
00605
00606 template<class X, class Y, class D>
00607 inline
00608 typename spline<X, Y, D>::sample_t
00609 spline<X,Y,D>::
00610 back() const
00612 {
00613 if(begin() != end())
00614 {
00615 const_iterator e = end();
00616 --e;
00617
00618 return *e;
00619 }
00620
00621
00622 return sample_t(X(), Y());
00623 }
00624
00625
00626 template<class X, class Y, class D>
00627 typename spline<X, Y, D>::const_iterator
00628 spline<X,Y,D>::
00629 floor(X const& x) const
00630
00640
00641 {
00642 const_iterator ub = samples_.upper_bound(x);
00643 const_iterator begin = samples_.begin();
00644
00645 while(ub != begin)
00646 {
00647 --ub;
00648
00649 if(ub->first <= x)
00650 return ub;
00651
00652 }
00653
00654 return samples_.end();
00655
00656 }
00657
00659
00660 template<class X, class Y, class D>
00661 Y
00662 spline<X, Y,D>::
00663 interpolate_step(X const& x) const
00696 {
00697 const_iterator loc = floor(x);
00698
00699 if(loc == end())
00700 {
00701 if(begin() == end())
00702 return Y();
00703
00704 return begin()->second;
00705 }
00706
00707 return loc->second;
00708
00709 }
00710
00712
00713 template<class X, class Y, class D>
00714 Y
00715 spline<X, Y,D>::
00716 interpolate_line(X const& x) const
00717
00766 {
00767 const_iterator loc = floor(x);
00768
00769 if(loc == end())
00770 {
00771
00772
00773 if(loc == begin())
00774 {
00775 return Y();
00776 }
00777
00778 return begin()->second;
00779
00780 }
00781
00782 const_iterator ub = loc;
00783 ++ub;
00784
00785 if(ub == end())
00786 {
00787
00788
00789 return loc->second;
00790 }
00791
00792
00793
00794
00795
00796
00797
00798
00799 D OLD_Y = loc->second;
00800 D OLD_X = loc->first;
00801 D X1 = D(x);
00802 D DELTAX = D(ub->first) - OLD_X;
00803
00804 if(DELTAX == 0)
00805 return static_cast<Y>(OLD_Y);
00806
00807 D DELTAY = D(ub->second) - OLD_Y;
00808 D SLOPE = DELTAY / DELTAX;
00809
00810 return static_cast<Y>(OLD_Y + SLOPE * (X1 - OLD_X));
00811
00812 }
00813
00815
00816 template<class X, class Y, class D>
00817 template<class Operation>
00818 void
00819 spline<X,Y,D>::
00820 apply(Operation const &op)
00821
00825
00826 {
00827
00828 iterator first = samples_.begin(),
00829 last = samples_.end();
00830
00831 while(first != last)
00832 {
00833 first->second = op(*first);
00834 ++first;
00835 }
00836
00837 }
00838
00840
00841 template<class X, class Y, class D>
00842 void
00843 spline<X, Y,D>::
00844 domain(std::set<X>* output)
00845 const
00846
00849
00850
00851 {
00852 const_iterator l = samples_.begin(),
00853 g = samples_.end();
00854 while(l != g )
00855 {
00856 output->insert(l->first);
00857 ++l;
00858 }
00859 }
00860
00862
00863 template<class X, class Y, class D>
00864 template<class T>
00865 inline
00866 spline<X,Y,D>&
00867 spline<X,Y,D>::
00868 operator+= (T const &rhs)
00871 {
00872 self tmp;
00873
00874 combine(tmp, rhs, std::plus<Y>());
00875
00876 swap(tmp);
00877
00878 return *this;
00879 }
00880
00882
00883 template<class X, class Y, class D>
00884 template<class T>
00885 inline
00886 spline<X,Y,D>&
00887 spline<X,Y,D>::operator-= (T const &rhs)
00890 {
00891 self tmp;
00892
00893 combine(tmp, rhs, std::minus<Y>());
00894
00895 swap(tmp);
00896
00897 return *this;
00898 }
00899
00901
00902 template<class X, class Y, class D>
00903 template<class T>
00904 inline
00905 spline<X,Y,D>&
00906 spline<X,Y,D>::
00907 operator*= (T const &rhs)
00910 {
00911 self tmp;
00912
00913 combine(tmp, rhs, std::multiplies<Y>());
00914
00915 swap(tmp);
00916
00917 return *this;
00918 }
00919
00921 template<class X, class Y, class D>
00922 template<class T>
00923 inline
00924 spline<X,Y,D>&
00925 spline<X,Y,D>::
00926 operator/= (T const &rhs)
00929 {
00930 self tmp;
00931
00932 combine(tmp, rhs, std::divides<Y>());
00933
00934 swap(tmp);
00935
00936 return *this;
00937 }
00938
00940 template<class X, class Y, class D>
00941 template<class OutputIterator>
00942 void
00943 spline<X,Y,D>::
00944 evaluate(X start_x,
00945 X delta_x,
00946 OutputIterator begin,
00947 OutputIterator end
00948 ) const
00949
00956 {
00957 while(begin != end)
00958 {
00959 begin->first = start_x;
00960 begin->second = (this->*interpolator_)(start_x);
00961
00962 ++begin;
00963
00964 start_x += delta_x;
00965 }
00966 }
00968
00969 template<class X, class Y, class D>
00970 void
00971 spline<X,Y,D>::
00972 compute_coefficients()
00976 {
00977 using std::vector;
00978
00979 dirty_ = false;
00980
00981 size_t n = samples_.size();
00982
00983
00984
00985 vector<D> xn(n+1), fn(n+1), rho(n+1), tau(n+1), s(n+1);
00986
00987 {
00988 const_iterator first = samples_.begin(),
00989 last = samples_.end();
00990
00991 typename vector<D>::iterator xout = xn.begin() + 1;
00992 typename vector<D>::iterator yout = fn.begin() + 1;
00993
00994 while(first != last)
00995 {
00996 *xout++ = first->first;
00997 *yout++ = first->second.y_;
00998
00999 ++first;
01000 }
01001
01002 }
01003
01004
01005
01006 size_t nm1 = n-1;
01007
01008 rho[2] = 0;
01009 tau[2] = 0;
01010
01011 int i;
01012
01013 for(i=2; i <= static_cast<int>(nm1); ++i)
01014 {
01015 size_t iim1 = i-1;
01016 size_t ii = i;
01017 size_t iip1 = i+1;
01018
01019 D him1 = xn[ii] - xn[iim1];
01020 D hi = xn[iip1]-xn[ii];
01021 D temp = (him1/hi)*(rho[i]+2)+2;
01022
01023 rho[i+1]=-1.0/temp;
01024
01025 D d = 6.0 *((fn[iip1]-fn[ii])/hi-(fn[ii]-fn[iim1])/him1)/hi;
01026
01027 tau[i+1] = (d-him1*tau[i]/hi)/temp;
01028
01029 }
01030
01031 s[1] = 0;
01032 s[n] = 0;
01033
01034 int nm2 = n-2;
01035
01036 for(i=1; i <= nm2; ++i)
01037 {
01038 size_t ib = n-i;
01039 s[ib] = rho[ib+1] * s[ib+1] +tau[ib+1];
01040 }
01041
01042 {
01043 typename vector<D>::iterator first = s.begin() + 1,
01044 last = s.end();
01045
01046 iterator output = samples_.begin();
01047
01048 while(first != last)
01049 {
01050 output->second.s_ = *first++;
01051 ++output;
01052 }
01053
01054 }
01055
01056
01057 }
01059
01060 template<class X, class Y, class D>
01061 Y
01062 spline<X,Y,D>::
01063 interpolate_cubic(X const& x) const
01064 {
01065 using std::vector;
01066
01067 if(dirty_)
01068 {
01069 self* me = const_cast<self*>(this);
01070 me->compute_coefficients();
01071 }
01072
01073 size_t n = samples_.size();
01074
01075 if(n == 0)
01076 return 0;
01077
01078 if(samples_.size() == 1)
01079 {
01080 return samples_.begin()->second.y_;
01081 }
01082
01083 if( x < samples_.begin()->first )
01084 {
01085 const_iterator lb = samples_.begin();
01086 const_iterator ub = lb; ++ub;
01087
01088 D m = (ub->second.y_ - lb->second.y_) /
01089 (ub->first - lb->first );
01090
01091 D deltax = lb->first - x;
01092 D deltay = m * deltax;
01093
01094 return lb->second.y_ - deltay;
01095 }
01096
01097 const_iterator end = samples_.end(); --end;
01098
01099 if( x >= end->first)
01100 {
01101 const_iterator ub = samples_.end(); --ub;
01102 const_iterator lb = ub; --lb;
01103
01104 D m = (ub->second.y_ - lb->second.y_) /
01105 (ub->first - lb->first );
01106
01107 D deltax = x - ub->first ;
01108 D deltay = m * deltax;
01109
01110 return ub->second.y_ + deltay;
01111 }
01112
01113 const_iterator ilp1 = ceil(x);
01114 const_iterator il = floor(x);
01115
01116 D a = ilp1->first - x;
01117 D b = x - il->first;
01118 D h = ilp1->first - il->first;
01119
01120 return a * il->second.s_ * (a * a / h - h)/6.0 +
01121 b * ilp1->second.s_ * (b * b / h - h)/6.0 +
01122 ( a * il->second.y_ + b * ilp1->second.y_)/h;
01123
01124 }
01126
01127 template<class X, class Y, class D>
01128 inline
01129 size_t
01130 spline<X,Y,D>::
01131 size() const
01132 {
01133 return samples_.size();
01134 }
01135
01137
01138 }
01139
01140 #endif