spline.C 3.66 KB
 Francois Gygi committed Aug 13, 2008 1 2 3 4 5 6 //////////////////////////////////////////////////////////////////////////////// // // Copyright (c) 2008 The Regents of the University of California // // This file is part of Qbox //  Francois Gygi committed Sep 08, 2008 7 8 // Qbox is distributed under the terms of the GNU General Public License // as published by the Free Software Foundation, either version 2 of  Francois Gygi committed Aug 13, 2008 9 10 11 12 // the License, or (at your option) any later version. // See the file COPYING in the root directory of this distribution // or . //  Francois Gygi committed Aug 13, 2007 13 14 15 16 17 /////////////////////////////////////////////////////////////////////////////// // // spline.C // ///////////////////////////////////////////////////////////////////////////////  Francois Gygi committed Sep 08, 2008 18 // $Id: spline.C,v 1.5 2008-09-08 15:56:20 fgygi Exp$  Francois Gygi committed Oct 02, 2003 19 #include "spline.h"  Francois Gygi committed Aug 13, 2007 20 #include  Francois Gygi committed Oct 02, 2003 21   Francois Gygi committed Aug 13, 2007 22 void tridsolve(int n, double* d, double* e, double* f, double* x)  Francois Gygi committed Oct 02, 2003 23 {  Francois Gygi committed Aug 13, 2007 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38  // solve the tridiagonal system Ax=b // d[i] = a(i,i) // e[i] = a(i,i+1) (superdiagonal of A, e[n-1] not defined) // f[i] = a(i,i-1) (subdiagonal of A, f[0] not defined) // x[i] = right-hand side b as input // x[i] = solution as output for ( int i = 1; i < n; i++ ) { f[i] /= d[i-1]; d[i] -= f[i]*e[i-1]; } for ( int i = 1; i < n; i++ ) x[i] -= f[i]*x[i-1];  Francois Gygi committed Oct 02, 2003 39   Francois Gygi committed Aug 13, 2007 40  x[n-1] /= d[n-1];  Francois Gygi committed Oct 02, 2003 41   Francois Gygi committed Aug 13, 2007 42 43 44 45  for ( int i = n-2; i >= 0; i-- ) x[i] = (x[i]-e[i]*x[i+1])/d[i]; }  Francois Gygi committed Oct 19, 2007 46 void spline(int n, double *x, double *y, double yp_left, double yp_right,  Francois Gygi committed Aug 13, 2007 47 48 49 50 51 52 53 54  int bcnat_left, int bcnat_right, double *y2) { const double third = 1.0/3.0; const double sixth = 1.0/6.0; double *d = new double[n]; double *e = new double[n]; double *f = new double[n]; if ( bcnat_left == 0 )  Francois Gygi committed Oct 02, 2003 55  {  Francois Gygi committed Aug 13, 2007 56 57 58 59 60 61 62  // use derivative yp_left at x[0] const double h = x[1]-x[0]; assert(h>0.0); d[0] = third*h; e[0] = sixth*h; f[0] = 0.0; y2[0] = (y[1]-y[0])/h - yp_left;  Francois Gygi committed Oct 02, 2003 63 64 65  } else {  Francois Gygi committed Aug 13, 2007 66 67 68 69 70  // use natural spline at x[0] d[0] = 1.0; e[0] = 0.0; f[0] = 0.0; y2[0] = 0.0;  Francois Gygi committed Oct 02, 2003 71  }  Francois Gygi committed Aug 13, 2007 72  if ( bcnat_right == 0 )  Francois Gygi committed Oct 02, 2003 73  {  Francois Gygi committed Aug 13, 2007 74 75 76 77 78 79 80  // use derivative yp_right at x[n-1] const double h = x[n-1]-x[n-2]; assert(h>0.0); d[n-1] = third*h; e[n-1] = 0.0; f[n-1] = sixth*h; y2[n-1] = yp_right - (y[n-1]-y[n-2])/h;  Francois Gygi committed Oct 02, 2003 81 82 83  } else {  Francois Gygi committed Aug 13, 2007 84 85 86 87 88  // use natural spline at x[n-1] d[n-1] = 1.0; e[n-1] = 0.0; f[n-1] = 0.0; y2[n-1] = 0.0;  Francois Gygi committed Oct 02, 2003 89 90  }  Francois Gygi committed Aug 13, 2007 91 92  // tridiagonal matrix for ( int i = 1; i < n-1; i++ )  Francois Gygi committed Oct 02, 2003 93  {  Francois Gygi committed Aug 13, 2007 94 95 96 97 98 99 100 101  const double hp = x[i+1]-x[i]; const double hm = x[i]-x[i-1]; assert(hp>0.0); assert(hm>0.0); d[i] = third * (hp+hm); e[i] = sixth * hp; f[i] = sixth * hm; y2[i] = (y[i+1]-y[i])/hp - (y[i]-y[i-1])/hm;  Francois Gygi committed Oct 02, 2003 102 103  }  Francois Gygi committed Aug 13, 2007 104 105 106 107 108  tridsolve(n,d,e,f,y2); delete [] d; delete [] e; delete [] f;  Francois Gygi committed Oct 02, 2003 109 110 }  Francois Gygi committed Aug 13, 2007 111 void splint (int n, double *xa, double *ya, double *y2a, double x, double *y)  Francois Gygi committed Oct 02, 2003 112 {  Francois Gygi committed Aug 13, 2007 113  int k;  Francois Gygi committed Oct 02, 2003 114 115  double a,b,h;  Francois Gygi committed Aug 13, 2007 116 117  int kl = 0; int kh = n-1;  Francois Gygi committed Oct 02, 2003 118   Francois Gygi committed Aug 13, 2007 119  while ( kh - kl > 1 )  Francois Gygi committed Oct 02, 2003 120  {  Francois Gygi committed Aug 13, 2007 121  k = ( kh + kl ) / 2;  Francois Gygi committed Oct 02, 2003 122  if ( xa[k] > x )  Francois Gygi committed Aug 13, 2007 123  kh = k;  Francois Gygi committed Oct 02, 2003 124  else  Francois Gygi committed Aug 13, 2007 125  kl = k;  Francois Gygi committed Oct 02, 2003 126 127  }  Francois Gygi committed Aug 13, 2007 128  h = xa[kh] - xa[kl];  Francois Gygi committed Oct 02, 2003 129 130  assert ( h > 0.0 );  Francois Gygi committed Aug 13, 2007 131 132  a = ( xa[kh] - x ) / h; b = ( x - xa[kl] ) / h;  Francois Gygi committed Oct 02, 2003 133   Francois Gygi committed Aug 13, 2007 134 135  *y = a * ya[kl] + b * ya[kh] + h * h * (1.0/6.0) * ( (a*a*a-a) * y2a[kl] + (b*b*b-b) * y2a[kh] );  Francois Gygi committed Oct 02, 2003 136 137 138  }  Francois Gygi committed Aug 13, 2007 139 140 void splintd (int n, double *xa, double *ya, double *y2a, double x, double *y, double *dy)  Francois Gygi committed Oct 02, 2003 141 {  Francois Gygi committed Aug 13, 2007 142  int k;  Francois Gygi committed Oct 02, 2003 143 144  double a,b,h;  Francois Gygi committed Aug 13, 2007 145 146  int kl = 0; int kh = n-1;  Francois Gygi committed Oct 02, 2003 147   Francois Gygi committed Aug 13, 2007 148  while ( kh - kl > 1 )  Francois Gygi committed Oct 02, 2003 149  {  Francois Gygi committed Aug 13, 2007 150  k = ( kh + kl ) / 2;  Francois Gygi committed Oct 02, 2003 151  if ( xa[k] > x )  Francois Gygi committed Aug 13, 2007 152  kh = k;  Francois Gygi committed Oct 02, 2003 153  else  Francois Gygi committed Aug 13, 2007 154  kl = k;  Francois Gygi committed Oct 02, 2003 155 156  }  Francois Gygi committed Aug 13, 2007 157  h = xa[kh] - xa[kl];  Francois Gygi committed Oct 02, 2003 158 159  assert ( h > 0.0 );  Francois Gygi committed Aug 13, 2007 160 161  a = ( xa[kh] - x ) / h; b = ( x - xa[kl] ) / h;  Francois Gygi committed Oct 02, 2003 162   Francois Gygi committed Aug 13, 2007 163 164  *y = a * ya[kl] + b * ya[kh] + h * h * (1.0/6.0) * ( (a*a*a-a) * y2a[kl] + (b*b*b-b) * y2a[kh] );  Francois Gygi committed Oct 02, 2003 165   Francois Gygi committed Aug 13, 2007 166 167 168  *dy = ( ya[kh] - ya[kl] ) / h + h * ( ( (1.0/6.0) - 0.5 * a * a ) * y2a[kl] + ( 0.5 * b * b - (1.0/6.0) ) * y2a[kh] );  Francois Gygi committed Oct 02, 2003 169 }