lib/m_spline.cc

   1 /*$Id: m_spline.cc,v 26.83 2008/06/05 04:46:59 al Exp $ -*- C++ -*-
   2  * Copyright (C) 2001 Albert Davis
   3  * Author: Albert Davis <aldavis@gnu.org>
   4  *
   5  * This file is part of "Gnucap", the Gnu Circuit Analysis Package
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License as published by
   9  * the Free Software Foundation; either version 3, or (at your option)
  10  * any later version.
  11  *
  12  * This program is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License
  18  * along with this program; if not, write to the Free Software
  19  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
  20  * 02110-1301, USA.
  21  *------------------------------------------------------------------
  22  * piecewise polynomial interpolation
  23  */
  24 //testing=script 2006.04.18
  25 #include "m_cpoly.h"
  26 #include "l_denoise.h"
  27 #include "m_spline.h"
  28 /*--------------------------------------------------------------------------*/
  29 SPLINE::SPLINE(const std::vector<DPAIR>& table,
  30                double d0, double dn, int order)
  31   :_n(static_cast<int>(table.size())-1),
  32    _x( new double[_n+1]),
  33    _f0(new double[_n+1]),
  34    _f1(0),
  35    _f2(0),
  36    _f3(0),
  37    _order(order)
  38 {untested();
  39   if (_n < 0) {untested();
  40     throw Exception("no points in spline");
  41   }else{untested();
  42   }
  43
  44   for (int i=0; i<=_n; ++i) {untested();
  45     _x[i]  = table[static_cast<unsigned>(i)].first;
  46     _f0[i] = table[static_cast<unsigned>(i)].second;
  47   }
  48
  49   // set up h --------------------------------------------------
  50   double* h = new double[_n+1];
  51   for (int i=0; i<_n; ++i) {
  52     h[i] = _x[i+1] - _x[i];
  53     if (h[i] == 0.) {untested();
  54       throw Exception("duplicate points in spline: "
  55                       + to_string(_x[i]) +  ", " + to_string(_x[i+1]));
  56     }else{untested();
  57     }
  58   }
  59   h[_n] = NOT_VALID;
  60
  61   switch (_order) {
  62   case 3: construct_order_3(h, d0, dn); break;
  63   case 2: construct_order_2(h, d0, dn); break;
  64   case 1: construct_order_1(h, d0, dn); break;
  65   case 0: untested(); /* nothing to do */   break;
  66   default:
  67     untested();
  68     error(bDANGER, "illegal spline order (%d), must be 0, 1, 2, 3\n", _order);
  69     break;
  70   }
  71 }
  72 /*--------------------------------------------------------------------------*/
  73 SPLINE::SPLINE(const std::vector<std::pair<PARAMETER<double>, PARAMETER<double> > >& table,
  74                double d0, double dn, int order)
  75   :_n(static_cast<int>(table.size())-1),
  76    _x( new double[_n+1]),
  77    _f0(new double[_n+1]),
  78    _f1(0),
  79    _f2(0),
  80    _f3(0),
  81    _order(order)
  82 {
  83   if (_n < 0) {
  84     throw Exception("no points in spline");
  85   }else{
  86   }
  87
  88   for (int i=0; i<=_n; ++i) {
  89     _x[i]  = table[static_cast<unsigned>(i)].first;
  90     _f0[i] = table[static_cast<unsigned>(i)].second;
  91   }
  92
  93   // set up h --------------------------------------------------
  94   double* h = new double[_n+1];
  95   for (int i=0; i<_n; ++i) {
  96     h[i] = _x[i+1] - _x[i];
  97     if (h[i] == 0.) {
  98       throw Exception("duplicate points in spline: "
  99             + to_string(_x[i]) +  ", " + to_string(_x[i+1]));
 100     }else{
 101     }
 102   }
 103   h[_n] = NOT_VALID;
 104
 105   switch (_order) {
 106   case 3: construct_order_3(h, d0, dn); break;
 107   case 2: construct_order_2(h, d0, dn); break;
 108   case 1: construct_order_1(h, d0, dn); break;
 109   case 0: untested(); /* nothing to do */   break;
 110   default:
 111     untested();
 112     error(bDANGER, "illegal spline order (%d), must be 0, 1, 2, 3\n", _order);
 113     break;
 114   }
 115 }
 116 /*--------------------------------------------------------------------------*/
 117 void SPLINE::construct_order_1(double* h, double d0, double dn)
 118 {
 119   assert(_n >= 0);
 120
 121   _f1 = h; // reuse storage
 122   for (int i=0; i<_n; ++i) {
 123     _f1[i] = (_f0[i+1] - _f0[i]) / h[i];
 124   }
 125   //h = 0;
 126
 127   if (d0 == NOT_INPUT) {
 128     _d0 = _f1[0];
 129   }else{untested();
 130     _d0 = d0;
 131   }
 132   if (dn == NOT_INPUT) {
 133     _f1[_n] = _f1[_n-1];
 134   }else{untested();
 135     _f1[_n] = dn;
 136   }
 137 }
 138 /*--------------------------------------------------------------------------*/
 139 void SPLINE::construct_order_2(double* h, double d0, double dn)
 140 {
 141   assert(_n >= 0);
 142
 143   _f1 = new double[_n+1];
 144   if (d0 != NOT_INPUT && dn == NOT_INPUT) {
 145     _d0 = _f1[0] = d0;
 146     for (int i=0; i<_n; ++i) {
 147       _f1[i+1] = 2*(_f0[i+1]-_f0[i])/h[i] - _f1[i];
 148     }
 149   }else{
 150     if (dn == NOT_INPUT) {
 151       // neither bc .. must guess
 152       _f1[_n] = (_f0[_n] - _f0[_n-1]) / h[_n-1];
 153     }else{
 154       _f1[_n] = dn;
 155     }
 156     for (int i=_n-1; i>=0; --i) {
 157       _f1[i] = 2*(_f0[i+1]-_f0[i])/h[i] - _f1[i+1];
 158     }
 159     if (d0 == NOT_INPUT) {
 160       _d0 = _f1[0];
 161     }else{
 162       // both bc ... discontinuous derivative at _x[0]
 163       _d0 = d0;
 164     }
 165   }
 166
 167   // second derivative -- piecewise constant
 168   _f2 = h; // reuse storage
 169   for (int i=0; i<_n; ++i) {
 170     _f2[i] = .5 * (_f1[i+1] - _f1[i]) / h[i];
 171   }
 172   _f2[_n] = 0.;
 173   //h = 0;
 174 }
 175 /*--------------------------------------------------------------------------*/
 176 void SPLINE::construct_order_3(double* h, double d0, double dn)
 177 {
 178   assert(_n >= 0);
 179
 180   // set up right side -----------------------------------------
 181   double* b = new double[_n+1]; // b as in Ax=b
 182   for (int i=1; i<_n; ++i) {
 183     double num = _f0[i+1]*h[i-1] -_f0[i]*(h[i]+h[i-1]) +_f0[i-1]*h[i];
 184     double den = h[i-1] * h[i];
 185     b[i] = 3 * num / den;
 186   }
 187   // boundary conditions
 188   if (d0 == NOT_INPUT) {
 189     b[0] = 0.;
 190   }else{
 191     b[0] = 3 * ((_f0[1]-_f0[0])/h[0] - d0);
 192   }
 193   if (dn == NOT_INPUT) {
 194     b[_n] = 0.;
 195   }else{
 196     b[_n] = 3 * (dn - (_f0[_n]-_f0[_n-1])/h[_n-1]);
 197   }
 198
 199   // fill, LU decomp, fwd sub ----------------------------------
 200   double* u = new double[_n+1];
 201   double* z = b; // reuse storage.
 202   if (d0 == NOT_INPUT) {
 203     u[0] = 0.;
 204     z[0] = 0.;
 205   }else{
 206     u[0] = .5;  // h[0] / (2*h[0])
 207     z[0] = b[0] / (2*h[0]);
 208   }
 209   for (int i=1; i<_n; ++i) {
 210     double p = 2*(h[i]+h[i-1]) - h[i-1]*u[i-1]; // pivot
 211     u[i] = h[i] / p;
 212     z[i] = (b[i] - z[i-1]*h[i-1]) / p;
 213   }
 214   if (dn == NOT_INPUT) { // natural
 215     z[_n] = 0;
 216   }else{ // clamped
 217     double p = h[_n-1] * (2.-u[_n-1]);
 218     z[_n] = (b[_n] - z[_n-1]*h[_n-1]) / p;
 219   }
 220   u[_n] = NOT_VALID;
 221   //b = 0;
 222
 223   // back sub --------------------------------------------------
 224   _f1 = u; // reuse storage.
 225   _f2 = z;
 226   _f3 = h;
 227   _f2[_n] = z[_n];
 228   _f3[_n] = 0.;
 229   for (int i=_n-1; i>=0; --i) {
 230     _f2[i] = z[i] - u[i]*_f2[i+1];
 231     _f1[i] = (_f0[i+1]-_f0[i])/h[i] - h[i]*(_f2[i+1]+2*_f2[i])/3;
 232     _f3[i] = (_f2[i+1]-_f2[i])/(3*h[i]);
 233     trace4("", i, _f1[i], _f2[i], _f3[i]);
 234   }
 235
 236   _d0 = fixzero(_f1[0], _f1[1]);  //_f1[0];
 237   assert(d0 == NOT_INPUT  ||  _d0 == d0);
 238
 239   // set up last slot for extrapolation above. -----------------
 240   if (dn == NOT_INPUT) { // natural
 241     _f1[_n] = _f1[_n-1] + (_x[_n]-_x[_n-1])*_f2[_n-1];
 242   }else{ // clamped
 243     _f1[_n] = dn;
 244   }
 245   _f2[_n] = 0.;
 246   _f3[_n] = 0.;
 247   //u = z = h = 0;
 248 }
 249 /*--------------------------------------------------------------------------*/
 250 SPLINE::~SPLINE()
 251 {
 252   delete [] _x;
 253   delete [] _f0;
 254   delete [] _f1;
 255   delete [] _f2;
 256   delete [] _f3;
 257 }
 258 /*--------------------------------------------------------------------------*/
 259 FPOLY1 SPLINE::at(double x)const
 260 {
 261   assert(_n >= 0);
 262
 263   double* here = std::upper_bound(_x, _x+_n+1, x);
 264
 265   if (here == _x) {     // x out of range, below
 266     if (_order == 0) {untested();
 267       return FPOLY1(x, _f0[0], 0.);
 268     }else{
 269       double dx = x - _x[0];
 270       double f0 = _f0[0] + dx*_d0;
 271       return FPOLY1(x, f0, _d0);
 272     }
 273   }else{                // x in range, or out of range above
 274     int i = static_cast<int>(here - _x - 1);
 275     double dx = x - _x[i];
 276     switch (_order) {
 277     case 3:{
 278       double f0 = _f0[i] + dx*(_f1[i] + dx*(_f2[i] + dx*_f3[i]));
 279       double f1 = _f1[i] + dx*(2*_f2[i] + 3*dx*_f3[i]);
 280       return FPOLY1(x, f0, f1);
 281     }
 282     case 2:{
 283       double f0 = _f0[i] + dx*(_f1[i] + dx*_f2[i]);
 284       double f1 = _f1[i] + dx*(2*_f2[i]);
 285       return FPOLY1(x, f0, f1);
 286     }
 287     case 1:{
 288       double f0 = _f0[i] + dx*_f1[i];
 289       double f1 = _f1[i];
 290       return FPOLY1(x, f0, f1);
 291     }
 292     case 0:
 293       untested();
 294       return FPOLY1(x, _f0[i], 0.);
 295     default:
 296       untested();
 297       assert(!"spline problem");
 298       return FPOLY1();
 299     }
 300     untested();
 301     //trace4("", x, _x[i], dx, i);
 302     //trace4("", _f0[i],   _f1[i],   _f2[i],   _f3[i]);
 303     //trace4("", _f0[i+1], _f1[i+1], _f2[i+1], _f3[i+1]);
 304     //trace2("", f0, f1);
 305   }
 306 }
 307 /*--------------------------------------------------------------------------*/
 308 /*--------------------------------------------------------------------------*/
 309 // vim:ts=8:sw=2:noet: