spline.C 3.66 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
// $Id: spline.C,v 1.5 2008-09-08 15:56:20 fgygi Exp $
Francois Gygi committed
19
#include "spline.h"
20
#include <cassert>
Francois Gygi committed
21

22
void tridsolve(int n, double* d, double* e, double* f, double* x)
Francois Gygi committed
23
{
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
39

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

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

46
void spline(int n, double *x, double *y, double yp_left, double yp_right,
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
55
  {
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
63 64 65
  }
  else
  {
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
71
  }
72
  if ( bcnat_right == 0 )
Francois Gygi committed
73
  {
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
81 82 83
  }
  else
  {
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
89 90
  }

91 92
  // tridiagonal matrix
  for ( int i = 1; i < n-1; i++ )
Francois Gygi committed
93
  {
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
102 103
  }

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

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

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

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

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

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

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

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
136 137 138

}

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

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

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

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

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

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
165

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
169
}