interpolators/detail/quintic_hermite_detail.hpp

0001 /*
0002  * Copyright Nick Thompson, 2020
0003  * Use, modification and distribution are subject to the
0004  * Boost Software License, Version 1.0. (See accompanying file
0005  * LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
0006  */
0007 #ifndef BOOST_MATH_INTERPOLATORS_DETAIL_QUINTIC_HERMITE_DETAIL_HPP
0008 #define BOOST_MATH_INTERPOLATORS_DETAIL_QUINTIC_HERMITE_DETAIL_HPP
0009 #include <algorithm>
0010 #include <stdexcept>
0011 #include <sstream>
0012 #include <limits>
0013 #include <cmath>
0014
0015 namespace boost {
0016 namespace math {
0017 namespace interpolators {
0018 namespace detail {
0019
0020 template<class RandomAccessContainer>
0021 class quintic_hermite_detail {
0022 public:
0023     using Real = typename RandomAccessContainer::value_type;
0024     quintic_hermite_detail(RandomAccessContainer && x, RandomAccessContainer && y, RandomAccessContainer && dydx, RandomAccessContainer && d2ydx2) : x_{std::move(x)}, y_{std::move(y)}, dydx_{std::move(dydx)}, d2ydx2_{std::move(d2ydx2)}
0025     {
0026         if (x_.size() != y_.size())
0027         {
0028             throw std::domain_error("Number of abscissas must = number of ordinates.");
0029         }
0030         if (x_.size() != dydx_.size())
0031         {
0032             throw std::domain_error("Numbers of derivatives must = number of abscissas.");
0033         }
0034         if (x_.size() != d2ydx2_.size())
0035         {
0036             throw std::domain_error("Number of second derivatives must equal number of abscissas.");
0037         }
0038         if (x_.size() < 2)
0039         {
0040             throw std::domain_error("At least 2 abscissas are required.");
0041         }
0042         Real x0 = x_[0];
0043         for (decltype(x_.size()) i = 1; i < x_.size(); ++i)
0044         {
0045             Real x1 = x_[i];
0046             if (x1 <= x0)
0047             {
0048                 throw std::domain_error("Abscissas must be sorted in strictly increasing order x0 < x1 < ... < x_{n-1}");
0049             }
0050             x0 = x1;
0051         }
0052     }
0053
0054     void push_back(Real x, Real y, Real dydx, Real d2ydx2)
0055     {
0056         using std::abs;
0057         using std::isnan;
0058         if (x <= x_.back())
0059         {
0060              throw std::domain_error("Calling push_back must preserve the monotonicity of the x's");
0061         }
0062         x_.push_back(x);
0063         y_.push_back(y);
0064         dydx_.push_back(dydx);
0065         d2ydx2_.push_back(d2ydx2);
0066     }
0067
0068     inline Real operator()(Real x) const
0069     {
0070         if  (x < x_[0] || x > x_.back())
0071         {
0072             std::ostringstream oss;
0073             oss.precision(std::numeric_limits<Real>::digits10+3);
0074             oss << "Requested abscissa x = " << x << ", which is outside of allowed range ["
0075                 << x_[0] << ", " << x_.back() << "]";
0076             throw std::domain_error(oss.str());
0077         }
0078         // We need t := (x-x_k)/(x_{k+1}-x_k) \in [0,1) for this to work.
0079         // Sadly this neccessitates this loathesome check, otherwise we get t = 1 at x = xf.
0080         if (x == x_.back())
0081         {
0082             return y_.back();
0083         }
0084
0085         auto it = std::upper_bound(x_.begin(), x_.end(), x);
0086         auto i = std::distance(x_.begin(), it) -1;
0087         Real x0 = *(it-1);
0088         Real x1 = *it;
0089         Real y0 = y_[i];
0090         Real y1 = y_[i+1];
0091         Real v0 = dydx_[i];
0092         Real v1 = dydx_[i+1];
0093         Real a0 = d2ydx2_[i];
0094         Real a1 = d2ydx2_[i+1];
0095
0096         Real dx = (x1-x0);
0097         Real t = (x-x0)/dx;
0098         Real t2 = t*t;
0099         Real t3 = t2*t;
0100
0101         // See the 'Basis functions' section of:
0102         // https://www.rose-hulman.edu/~finn/CCLI/Notes/day09.pdf
0103         // Also: https://github.com/MrHexxx/QuinticHermiteSpline/blob/master/HermiteSpline.cs
0104         Real y = (1- t3*(10 + t*(-15 + 6*t)))*y0;
0105         y += t*(1+ t2*(-6 + t*(8 -3*t)))*v0*dx;
0106         y += t2*(1 + t*(-3 + t*(3-t)))*a0*dx*dx/2;
0107         y += t3*((1 + t*(-2 + t))*a1*dx*dx/2 + (-4 + t*(7 - 3*t))*v1*dx + (10 + t*(-15 + 6*t))*y1);
0108         return y;
0109     }
0110
0111     inline Real prime(Real x) const
0112     {
0113         if  (x < x_[0] || x > x_.back())
0114         {
0115             std::ostringstream oss;
0116             oss.precision(std::numeric_limits<Real>::digits10+3);
0117             oss << "Requested abscissa x = " << x << ", which is outside of allowed range ["
0118                 << x_[0] << ", " << x_.back() << "]";
0119             throw std::domain_error(oss.str());
0120         }
0121         if (x == x_.back())
0122         {
0123             return dydx_.back();
0124         }
0125
0126         auto it = std::upper_bound(x_.begin(), x_.end(), x);
0127         auto i = std::distance(x_.begin(), it) -1;
0128         Real x0 = *(it-1);
0129         Real x1 = *it;
0130         Real dx = x1 - x0;
0131
0132         Real y0 = y_[i];
0133         Real y1 = y_[i+1];
0134         Real v0 = dydx_[i];
0135         Real v1 = dydx_[i+1];
0136         Real a0 = d2ydx2_[i];
0137         Real a1 = d2ydx2_[i+1];
0138         Real t= (x-x0)/dx;
0139         Real t2 = t*t;
0140
0141         Real dydx = 30*t2*(1 - 2*t + t*t)*(y1-y0)/dx;
0142         dydx += (1-18*t*t + 32*t*t*t - 15*t*t*t*t)*v0 - t*t*(12 - 28*t + 15*t*t)*v1;
0143         dydx += (t*dx/2)*((2 - 9*t + 12*t*t - 5*t*t*t)*a0 + t*(3 - 8*t + 5*t*t)*a1);
0144         return dydx;
0145     }
0146
0147     inline Real double_prime(Real x) const
0148     {
0149         if  (x < x_[0] || x > x_.back())
0150         {
0151             std::ostringstream oss;
0152             oss.precision(std::numeric_limits<Real>::digits10+3);
0153             oss << "Requested abscissa x = " << x << ", which is outside of allowed range ["
0154                 << x_[0] << ", " << x_.back() << "]";
0155             throw std::domain_error(oss.str());
0156         }
0157         if (x == x_.back())
0158         {
0159             return d2ydx2_.back();
0160         }
0161
0162         auto it = std::upper_bound(x_.begin(), x_.end(), x);
0163         auto i = std::distance(x_.begin(), it) -1;
0164         Real x0 = *(it-1);
0165         Real x1 = *it;
0166         Real dx = x1 - x0;
0167
0168         Real y0 = y_[i];
0169         Real y1 = y_[i+1];
0170         Real v0 = dydx_[i];
0171         Real v1 = dydx_[i+1];
0172         Real a0 = d2ydx2_[i];
0173         Real a1 = d2ydx2_[i+1];
0174         Real t = (x-x0)/dx;
0175
0176         Real d2ydx2 = 60*t*(1 + t*(-3 + 2*t))*(y1-y0)/(dx*dx);
0177         d2ydx2 += 12*t*(-3 + t*(8 - 5*t))*v0/dx;
0178         d2ydx2 -= 12*t*(2 + t*(-7 + 5*t))*v1/dx;
0179         d2ydx2 += (1 + t*(-9 + t*(18 - 10*t)))*a0;
0180         d2ydx2 += t*(3 + t*(-12 + 10*t))*a1;
0181
0182         return d2ydx2;
0183     }
0184
0185     friend std::ostream& operator<<(std::ostream & os, const quintic_hermite_detail & m)
0186     {
0187         os << "(x,y,y') = {";
0188         for (size_t i = 0; i < m.x_.size() - 1; ++i) {
0189             os << "(" << m.x_[i] << ", " << m.y_[i] << ", " << m.dydx_[i] << ", " << m.d2ydx2_[i] << "),  ";
0190         }
0191         auto n = m.x_.size()-1;
0192         os << "(" << m.x_[n] << ", " << m.y_[n] << ", " << m.dydx_[n] << ", " << m.d2ydx2_[n] << ")}";
0193         return os;
0194     }
0195
0196     int64_t bytes() const
0197     {
0198         return 4*x_.size()*sizeof(x_);
0199     }
0200
0201     std::pair<Real, Real> domain() const
0202     {
0203         return {x_.front(), x_.back()};
0204     }
0205
0206 private:
0207     RandomAccessContainer x_;
0208     RandomAccessContainer y_;
0209     RandomAccessContainer dydx_;
0210     RandomAccessContainer d2ydx2_;
0211 };
0212
0213
0214 template<class RandomAccessContainer>
0215 class cardinal_quintic_hermite_detail {
0216 public:
0217     using Real = typename RandomAccessContainer::value_type;
0218     cardinal_quintic_hermite_detail(RandomAccessContainer && y, RandomAccessContainer && dydx, RandomAccessContainer && d2ydx2, Real x0, Real dx)
0219     : y_{std::move(y)}, dy_{std::move(dydx)}, d2y_{std::move(d2ydx2)}, x0_{x0}, inv_dx_{1/dx}
0220     {
0221         if (y_.size() != dy_.size())
0222         {
0223             throw std::domain_error("Numbers of derivatives must = number of abscissas.");
0224         }
0225         if (y_.size() != d2y_.size())
0226         {
0227             throw std::domain_error("Number of second derivatives must equal number of abscissas.");
0228         }
0229         if (y_.size() < 2)
0230         {
0231             throw std::domain_error("At least 2 abscissas are required.");
0232         }
0233         if (dx <= 0)
0234         {
0235             throw std::domain_error("dx > 0 is required.");
0236         }
0237
0238         for (auto & dy : dy_)
0239         {
0240             dy *= dx;
0241         }
0242
0243         for (auto & d2y : d2y_)
0244         {
0245             d2y *= (dx*dx)/2;
0246         }
0247     }
0248
0249
0250     inline Real operator()(Real x) const
0251     {
0252         const Real xf = x0_ + (y_.size()-1)/inv_dx_;
0253         if  (x < x0_ || x > xf)
0254         {
0255             std::ostringstream oss;
0256             oss.precision(std::numeric_limits<Real>::digits10+3);
0257             oss << "Requested abscissa x = " << x << ", which is outside of allowed range ["
0258                 << x0_ << ", " << xf << "]";
0259             throw std::domain_error(oss.str());
0260         }
0261         if (x == xf)
0262         {
0263             return y_.back();
0264         }
0265         return this->unchecked_evaluation(x);
0266     }
0267
0268     inline Real unchecked_evaluation(Real x) const
0269     {
0270         using std::floor;
0271         Real s = (x-x0_)*inv_dx_;
0272         Real ii = floor(s);
0273         auto i = static_cast<decltype(y_.size())>(ii);
0274         Real t = s - ii;
0275         if (t == 0)
0276         {
0277             return y_[i];
0278         }
0279         Real y0 = y_[i];
0280         Real y1 = y_[i+1];
0281         Real dy0 = dy_[i];
0282         Real dy1 = dy_[i+1];
0283         Real d2y0 = d2y_[i];
0284         Real d2y1 = d2y_[i+1];
0285
0286         // See the 'Basis functions' section of:
0287         // https://www.rose-hulman.edu/~finn/CCLI/Notes/day09.pdf
0288         // Also: https://github.com/MrHexxx/QuinticHermiteSpline/blob/master/HermiteSpline.cs
0289         Real y = (1- t*t*t*(10 + t*(-15 + 6*t)))*y0;
0290         y += t*(1+ t*t*(-6 + t*(8 -3*t)))*dy0;
0291         y += t*t*(1 + t*(-3 + t*(3-t)))*d2y0;
0292         y += t*t*t*((1 + t*(-2 + t))*d2y1 + (-4 + t*(7 -3*t))*dy1 + (10 + t*(-15 + 6*t))*y1);
0293         return y;
0294     }
0295
0296     inline Real prime(Real x) const
0297     {
0298         const Real xf = x0_ + (y_.size()-1)/inv_dx_;
0299         if  (x < x0_ || x > xf)
0300         {
0301             std::ostringstream oss;
0302             oss.precision(std::numeric_limits<Real>::digits10+3);
0303             oss << "Requested abscissa x = " << x << ", which is outside of allowed range ["
0304                 << x0_ << ", " << xf << "]";
0305             throw std::domain_error(oss.str());
0306         }
0307         if (x == xf)
0308         {
0309             return dy_.back()*inv_dx_;
0310         }
0311
0312         return this->unchecked_prime(x);
0313     }
0314
0315     inline Real unchecked_prime(Real x) const
0316     {
0317         using std::floor;
0318         Real s = (x-x0_)*inv_dx_;
0319         Real ii = floor(s);
0320         auto i = static_cast<decltype(y_.size())>(ii);
0321         Real t = s - ii;
0322         if (t == 0)
0323         {
0324             return dy_[i]*inv_dx_;
0325         }
0326         Real y0 = y_[i];
0327         Real y1 = y_[i+1];
0328         Real dy0 = dy_[i];
0329         Real dy1 = dy_[i+1];
0330         Real d2y0 = d2y_[i];
0331         Real d2y1 = d2y_[i+1];
0332
0333         Real dydx = 30*t*t*(1 - 2*t + t*t)*(y1-y0);
0334         dydx += (1-18*t*t + 32*t*t*t - 15*t*t*t*t)*dy0 - t*t*(12 - 28*t + 15*t*t)*dy1;
0335         dydx += t*((2 - 9*t + 12*t*t - 5*t*t*t)*d2y0 + t*(3 - 8*t + 5*t*t)*d2y1);
0336         dydx *= inv_dx_;
0337         return dydx;
0338     }
0339
0340     inline Real double_prime(Real x) const
0341     {
0342         const Real xf = x0_ + (y_.size()-1)/inv_dx_;
0343         if  (x < x0_ || x > xf) {
0344             std::ostringstream oss;
0345             oss.precision(std::numeric_limits<Real>::digits10+3);
0346             oss << "Requested abscissa x = " << x << ", which is outside of allowed range ["
0347                 << x0_ << ", " << xf << "]";
0348             throw std::domain_error(oss.str());
0349         }
0350         if (x == xf)
0351         {
0352             return d2y_.back()*2*inv_dx_*inv_dx_;
0353         }
0354
0355         return this->unchecked_double_prime(x);
0356     }
0357
0358     inline Real unchecked_double_prime(Real x) const
0359     {
0360         using std::floor;
0361         Real s = (x-x0_)*inv_dx_;
0362         Real ii = floor(s);
0363         auto i = static_cast<decltype(y_.size())>(ii);
0364         Real t = s - ii;
0365         if (t==0)
0366         {
0367             return d2y_[i]*2*inv_dx_*inv_dx_;
0368         }
0369
0370         Real y0 = y_[i];
0371         Real y1 = y_[i+1];
0372         Real dy0 = dy_[i];
0373         Real dy1 = dy_[i+1];
0374         Real d2y0 = d2y_[i];
0375         Real d2y1 = d2y_[i+1];
0376
0377         Real d2ydx2 = 60*t*(1 - 3*t + 2*t*t)*(y1 - y0)*inv_dx_*inv_dx_;
0378         d2ydx2 += (12*t)*((-3 + 8*t - 5*t*t)*dy0 - (2 - 7*t + 5*t*t)*dy1);
0379         d2ydx2 += (1 - 9*t + 18*t*t - 10*t*t*t)*d2y0*(2*inv_dx_*inv_dx_) + t*(3 - 12*t + 10*t*t)*d2y1*(2*inv_dx_*inv_dx_);
0380         return d2ydx2;
0381     }
0382
0383     int64_t bytes() const
0384     {
0385         return 3*y_.size()*sizeof(Real) + 2*sizeof(Real);
0386     }
0387
0388     std::pair<Real, Real> domain() const
0389     {
0390         Real xf = x0_ + (y_.size()-1)/inv_dx_;
0391         return {x0_, xf};
0392     }
0393
0394 private:
0395     RandomAccessContainer y_;
0396     RandomAccessContainer dy_;
0397     RandomAccessContainer d2y_;
0398     Real x0_;
0399     Real inv_dx_;
0400 };
0401
0402
0403 template<class RandomAccessContainer>
0404 class cardinal_quintic_hermite_detail_aos {
0405 public:
0406     using Point = typename RandomAccessContainer::value_type;
0407     using Real = typename Point::value_type;
0408     cardinal_quintic_hermite_detail_aos(RandomAccessContainer && data, Real x0, Real dx)
0409     : data_{std::move(data)} , x0_{x0}, inv_dx_{1/dx}
0410     {
0411         if (data_.size() < 2)
0412         {
0413             throw std::domain_error("At least two points are required to interpolate using cardinal_quintic_hermite_aos");
0414         }
0415
0416         if (data_[0].size() != 3)
0417         {
0418             throw std::domain_error("Each datum passed to the cardinal_quintic_hermite_aos must have three elements: {y, y', y''}");
0419         }
0420         if (dx <= 0)
0421         {
0422             throw std::domain_error("dx > 0 is required.");
0423         }
0424
0425         for (auto & datum : data_)
0426         {
0427             datum[1] *= dx;
0428             datum[2] *= (dx*dx/2);
0429         }
0430     }
0431
0432
0433     inline Real operator()(Real x) const
0434     {
0435         const Real xf = x0_ + (data_.size()-1)/inv_dx_;
0436         if  (x < x0_ || x > xf)
0437         {
0438             std::ostringstream oss;
0439             oss.precision(std::numeric_limits<Real>::digits10+3);
0440             oss << "Requested abscissa x = " << x << ", which is outside of allowed range ["
0441                 << x0_ << ", " << xf << "]";
0442             throw std::domain_error(oss.str());
0443         }
0444         if (x == xf)
0445         {
0446             return data_.back()[0];
0447         }
0448         return this->unchecked_evaluation(x);
0449     }
0450
0451     inline Real unchecked_evaluation(Real x) const
0452     {
0453         using std::floor;
0454         Real s = (x-x0_)*inv_dx_;
0455         Real ii = floor(s);
0456         auto i = static_cast<decltype(data_.size())>(ii);
0457         Real t = s - ii;
0458         if (t == 0)
0459         {
0460             return data_[i][0];
0461         }
0462
0463         Real y0 = data_[i][0];
0464         Real dy0 = data_[i][1];
0465         Real d2y0 = data_[i][2];
0466         Real y1 = data_[i+1][0];
0467         Real dy1 = data_[i+1][1];
0468         Real d2y1 = data_[i+1][2];
0469
0470         Real y = (1 - t*t*t*(10 + t*(-15 + 6*t)))*y0;
0471         y += t*(1 + t*t*(-6 + t*(8 - 3*t)))*dy0;
0472         y += t*t*(1 + t*(-3 + t*(3 - t)))*d2y0;
0473         y += t*t*t*((1 + t*(-2 + t))*d2y1 + (-4 + t*(7 - 3*t))*dy1 + (10 + t*(-15 + 6*t))*y1);
0474         return y;
0475     }
0476
0477     inline Real prime(Real x) const
0478     {
0479         const Real xf = x0_ + (data_.size()-1)/inv_dx_;
0480         if  (x < x0_ || x > xf)
0481         {
0482             std::ostringstream oss;
0483             oss.precision(std::numeric_limits<Real>::digits10+3);
0484             oss << "Requested abscissa x = " << x << ", which is outside of allowed range ["
0485                 << x0_ << ", " << xf << "]";
0486             throw std::domain_error(oss.str());
0487         }
0488         if (x == xf)
0489         {
0490             return data_.back()[1]*inv_dx_;
0491         }
0492
0493         return this->unchecked_prime(x);
0494     }
0495
0496     inline Real unchecked_prime(Real x) const
0497     {
0498         using std::floor;
0499         Real s = (x-x0_)*inv_dx_;
0500         Real ii = floor(s);
0501         auto i = static_cast<decltype(data_.size())>(ii);
0502         Real t = s - ii;
0503         if (t == 0)
0504         {
0505             return data_[i][1]*inv_dx_;
0506         }
0507
0508
0509         Real y0 = data_[i][0];
0510         Real y1 = data_[i+1][0];
0511         Real v0 = data_[i][1];
0512         Real v1 = data_[i+1][1];
0513         Real a0 = data_[i][2];
0514         Real a1 = data_[i+1][2];
0515
0516         Real dy = 30*t*t*(1 - 2*t + t*t)*(y1-y0);
0517         dy += (1-18*t*t + 32*t*t*t - 15*t*t*t*t)*v0 - t*t*(12 - 28*t + 15*t*t)*v1;
0518         dy += t*((2 - 9*t + 12*t*t - 5*t*t*t)*a0 + t*(3 - 8*t + 5*t*t)*a1);
0519         return dy*inv_dx_;
0520     }
0521
0522     inline Real double_prime(Real x) const
0523     {
0524         const Real xf = x0_ + (data_.size()-1)/inv_dx_;
0525         if  (x < x0_ || x > xf)
0526         {
0527             std::ostringstream oss;
0528             oss.precision(std::numeric_limits<Real>::digits10+3);
0529             oss << "Requested abscissa x = " << x << ", which is outside of allowed range ["
0530                 << x0_ << ", " << xf << "]";
0531             throw std::domain_error(oss.str());
0532         }
0533         if (x == xf)
0534         {
0535             return data_.back()[2]*2*inv_dx_*inv_dx_;
0536         }
0537
0538         return this->unchecked_double_prime(x);
0539     }
0540
0541     inline Real unchecked_double_prime(Real x) const
0542     {
0543         using std::floor;
0544         Real s = (x-x0_)*inv_dx_;
0545         Real ii = floor(s);
0546         auto i = static_cast<decltype(data_.size())>(ii);
0547         Real t = s - ii;
0548         if (t == 0) {
0549             return data_[i][2]*2*inv_dx_*inv_dx_;
0550         }
0551         Real y0 = data_[i][0];
0552         Real dy0 = data_[i][1];
0553         Real d2y0 = data_[i][2];
0554         Real y1 = data_[i+1][0];
0555         Real dy1 = data_[i+1][1];
0556         Real d2y1 = data_[i+1][2];
0557
0558         Real d2ydx2 = 60*t*(1 - 3*t + 2*t*t)*(y1 - y0)*inv_dx_*inv_dx_;
0559         d2ydx2 += (12*t)*((-3 + 8*t - 5*t*t)*dy0 - (2 - 7*t + 5*t*t)*dy1);
0560         d2ydx2 += (1 - 9*t + 18*t*t - 10*t*t*t)*d2y0*(2*inv_dx_*inv_dx_) + t*(3 - 12*t + 10*t*t)*d2y1*(2*inv_dx_*inv_dx_);
0561         return d2ydx2;
0562     }
0563
0564     int64_t bytes() const
0565     {
0566         return data_.size()*data_[0].size()*sizeof(Real) + 2*sizeof(Real);
0567     }
0568
0569     std::pair<Real, Real> domain() const
0570     {
0571         Real xf = x0_ + (data_.size()-1)/inv_dx_;
0572         return {x0_, xf};
0573     }
0574
0575 private:
0576     RandomAccessContainer data_;
0577     Real x0_;
0578     Real inv_dx_;
0579 };
0580
0581 }
0582 }
0583 }
0584 }
0585 #endif