spline.C 3.61 KB
Newer Older
Francois Gygi committed
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
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
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 <http://www.gnu.org/licenses/>.
//
13 14 15 16 17
///////////////////////////////////////////////////////////////////////////////
//
// spline.C
//
///////////////////////////////////////////////////////////////////////////////
Francois Gygi committed
18
#include "spline.h"
19
#include <cassert>
Francois Gygi committed
20

21
void tridsolve(int n, double* d, double* e, double* f, double* x)
Francois Gygi committed
22
{
23 24 25 26 27 28 29 30 31 32 33 34 35 36 37
  // 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
38

39
  x[n-1] /= d[n-1];
Francois Gygi committed
40

41 42 43 44
  for ( int i = n-2; i >= 0; i-- )
    x[i] = (x[i]-e[i]*x[i+1])/d[i];
}

45
void spline(int n, double *x, double *y, double yp_left, double yp_right,
46 47 48 49 50 51 52 53
  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
54
  {
55 56 57 58 59 60 61
    // 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
62 63 64
  }
  else
  {
65 66 67 68 69
    // 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
70
  }
71
  if ( bcnat_right == 0 )
Francois Gygi committed
72
  {
73 74 75 76 77 78 79
    // 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
80 81 82
  }
  else
  {
83 84 85 86 87
    // 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
88 89
  }

90 91
  // tridiagonal matrix
  for ( int i = 1; i < n-1; i++ )
Francois Gygi committed
92
  {
93 94 95 96 97 98 99 100
    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
101 102
  }

103 104 105 106 107
  tridsolve(n,d,e,f,y2);

  delete [] d;
  delete [] e;
  delete [] f;
Francois Gygi committed
108 109
}

110
void splint (int n, double *xa, double *ya, double *y2a, double x, double *y)
Francois Gygi committed
111
{
112
  int k;
Francois Gygi committed
113 114
  double a,b,h;

115 116
  int kl = 0;
  int kh = n-1;
Francois Gygi committed
117

118
  while ( kh - kl > 1 )
Francois Gygi committed
119
  {
120
    k = ( kh + kl ) / 2;
Francois Gygi committed
121
    if ( xa[k] > x )
122
      kh = k;
Francois Gygi committed
123
    else
124
      kl = k;
Francois Gygi committed
125 126
  }

127
  h = xa[kh] - xa[kl];
Francois Gygi committed
128 129
  assert ( h > 0.0 );

130 131
  a = ( xa[kh] - x ) / h;
  b = ( x - xa[kl] ) / h;
Francois Gygi committed
132

133 134
  *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
135 136 137

}

138 139
void splintd (int n, double *xa, double *ya, double *y2a,
              double x, double *y, double *dy)
Francois Gygi committed
140
{
141
  int k;
Francois Gygi committed
142 143
  double a,b,h;

144 145
  int kl = 0;
  int kh = n-1;
Francois Gygi committed
146

147
  while ( kh - kl > 1 )
Francois Gygi committed
148
  {
149
    k = ( kh + kl ) / 2;
Francois Gygi committed
150
    if ( xa[k] > x )
151
      kh = k;
Francois Gygi committed
152
    else
153
      kl = k;
Francois Gygi committed
154 155
  }

156
  h = xa[kh] - xa[kl];
Francois Gygi committed
157 158
  assert ( h > 0.0 );

159 160
  a = ( xa[kh] - x ) / h;
  b = ( x - xa[kl] ) / h;
Francois Gygi committed
161

162 163
  *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
164

165 166 167
  *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
168
}