MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
interp.cc File Reference
#include "mbconfig.h"
#include "interp.h"
Include dependency graph for interp.cc:

Go to the source code of this file.

Functions

void spline (const std::vector< doublereal > &x, const std::vector< doublereal > &y, std::vector< doublereal > &b, std::vector< doublereal > &c, std::vector< doublereal > &d)
 
doublereal seval (const doublereal &u, const std::vector< doublereal > &x, const std::vector< doublereal > &y, const std::vector< doublereal > &b, const std::vector< doublereal > &c, const std::vector< doublereal > &d, const int diff)
 
doublereal leval (const doublereal &u, const std::vector< doublereal > &x, const std::vector< doublereal > &y, const int diff)
 

Function Documentation

doublereal leval ( const doublereal u,
const std::vector< doublereal > &  x,
const std::vector< doublereal > &  y,
const int  diff 
)

Definition at line 242 of file interp.cc.

References ASSERTMSGBREAK.

Referenced by MultiLinearScalarFunction::ComputeDiff(), and MultiLinearScalarFunction::operator()().

246 {
247 /*
248 * this subroutine evaluates the multilinear funcion
249 *
250 * seval = y(i) + (y(i+1)-y(i))(x(i+1)-x(i))*(u-x(i))
251 *
252 * where x(i) .lt. u .lt. x(i+1), using horner's rule
253 *
254 * if u .lt. x(1) then i = 1 is used.
255 * if u .ge. x(n) then i = n is used.
256 *
257 * input..
258 *
259 * n = the number of data points
260 * u = the abscissa at which the spline is to be evaluated
261 * x,y = the arrays of data abscissas and ordinates
262 * b,c,d = arrays of spline coefficients computed by spline
263 */
264  int i, j, n;
265  doublereal dx, m;
266 
267  ASSERTMSGBREAK(x.size() == y.size(), "Error in leval, x.size()!=y.size()");
268 
269 /*
270 * binary search
271 */
272 
273  n = x.size();
274  i = 0;
275  j = n;
276  do {
277  int k = (i + j)/2;
278  if (u < x[k]) {
279  j = k;
280  } else {
281  i = k;
282  }
283  } while (j > i + 1);
284 /*
285 * evaluate
286 */
287  if (i == n-1) {
288  i--;
289  }
290  m = (y[i+1] - y[i])/(x[i+1] - x[i]);
291  dx = u - x[i];
292 
293 
294  ASSERTMSGBREAK(diff >=0, "Error in multilinear evaluation, diff<0");
295 
296  switch (diff) {
297  case 0:
298  return y[i] + m*dx;
299  break;
300  case 1:
301  return m;
302  break;
303  default:
304  return 0.;
305  break;
306  }
307  return 0.;
308 }
#define ASSERTMSGBREAK(expr, msg)
Definition: myassert.h:222
double doublereal
Definition: colamd.c:52
doublereal seval ( const doublereal u,
const std::vector< doublereal > &  x,
const std::vector< doublereal > &  y,
const std::vector< doublereal > &  b,
const std::vector< doublereal > &  c,
const std::vector< doublereal > &  d,
const int  diff 
)

Definition at line 128 of file interp.cc.

References ASSERTMSGBREAK.

Referenced by CubicSplineScalarFunction::ComputeDiff(), and CubicSplineScalarFunction::operator()().

135 {
136 /*
137 * this subroutine evaluates the cubic spline function
138 *
139 * seval = y(i) + b(i)*(u-x(i)) + c(i)*(u-x(i))**2 + d(i)*(u-x(i))**3
140 *
141 * where x(i) .lt. u .lt. x(i+1), using horner's rule
142 *
143 * if u .lt. x(1) then i = 1 is used.
144 * if u .ge. x(n) then i = n is used.
145 *
146 * input..
147 *
148 * n = the number of data points
149 * u = the abscissa at which the spline is to be evaluated
150 * x,y = the arrays of data abscissas and ordinates
151 * b,c,d = arrays of spline coefficients computed by spline
152 */
153  int n;
154  doublereal dx;
155 
156  ASSERTMSGBREAK(x.size() == y.size(), "Error in seval, x.size()!=y.size()");
157  ASSERTMSGBREAK(x.size() == b.size(), "Error in seval, x.size()!=b.size()");
158  ASSERTMSGBREAK(x.size() == c.size(), "Error in seval, x.size()!=c.size()");
159  ASSERTMSGBREAK(x.size() == d.size(), "Error in seval, x.size()!=d.size()");
160 
161  n = x.size();
162 
163 
164 /*
165 * outside the interval
166 */
167 
168  if (u <= x[0]) {
169  dx = u - x[0];
170  switch (diff) {
171  case 0:
172  return y[0] + dx*b[0];
173  break;
174  case 1:
175  return b[0];
176  break;
177  default:
178  return 0.;
179  break;
180  }
181 
182  } else if (u >= x[n - 1]) {
183  dx = u - x[n - 1];
184  switch (diff) {
185  case 0:
186  return y[n - 1] + dx*b[n - 1];
187  break;
188  case 1:
189  return b[n - 1];
190  break;
191  default:
192  return 0.;
193  break;
194  }
195 
196  } else {
197  int i, j;
198 /*
199 * binary search
200 */
201 
202  i = 0;
203  j = n;
204  do {
205  int k = (i + j)/2;
206  if (u < x[k]) {
207  j = k;
208  } else {
209  i = k;
210  }
211  } while (j > i + 1);
212 /*
213 * evaluate spline
214 */
215  dx = u - x[i];
216 
217  ASSERTMSGBREAK(diff >=0, "Error in spline evaluation, diff<0");
218 
219  switch (diff) {
220  case 0:
221  return y[i] + dx*(b[i] + dx*(c[i] + dx*d[i]));
222  break;
223  case 1:
224  return b[i] + dx*(2.*c[i] + 3.*dx*d[i]);
225  break;
226  case 2:
227  return 2.*c[i] + 6.*dx*d[i];
228  break;
229  case 3:
230  return 6.*d[i];
231  break;
232  default:
233  return 0.;
234  break;
235  }
236  }
237 
238  return 0.;
239 }
#define ASSERTMSGBREAK(expr, msg)
Definition: myassert.h:222
static std::stack< cleanup * > c
Definition: cleanup.cc:59
double doublereal
Definition: colamd.c:52
void spline ( const std::vector< doublereal > &  x,
const std::vector< doublereal > &  y,
std::vector< doublereal > &  b,
std::vector< doublereal > &  c,
std::vector< doublereal > &  d 
)

Definition at line 8 of file interp.cc.

References ASSERTMSGBREAK.

Referenced by CubicSplineScalarFunction::CubicSplineScalarFunction().

13 {
14 /*
15 *
16 * the coefficients b(i), c(i), and d(i), i=1,2,...,n are computed
17 * for a cubic interpolating spline
18 *
19 * s(x) = y(i) + b(i)*(x-x(i)) + c(i)*(x-x(i))**2 + d(i)*(x-x(i))**3
20 *
21 * for x(i) .le. x .le. x(i+1)
22 *
23 * input..
24 *
25 * n = the number of data points or knots (n.ge.2)
26 * x = the abscissas of the knots in strictly increasing order
27 * y = the ordinates of the knots
28 *
29 * output..
30 *
31 * b, c, d = arrays of spline coefficients as defined above.
32 *
33 * using p to denote differentiation,
34 *
35 * y(i) = s(x(i))
36 * b(i) = sp(x(i))
37 * c(i) = spp(x(i))/2
38 * d(i) = sppp(x(i))/6 (derivative from the right)
39 *
40 * the accompanying function subprogram seval can be used
41 * to evaluate the spline.
42 *
43 *
44 */
45  int n, nm1;
46  ASSERTMSGBREAK(x.size() == y.size(), "Error in spline, x.size()!=y.size()");
47 
48  n = x.size();
49  b.resize(n);
50  c.resize(n);
51  d.resize(n);
52 
53  nm1 = n - 1;
54  if (n < 2) {
55  return;
56  }
57  if (n < 3) {
58  b[0] = (y[1] - y[0])/(x[1] - x[0]);
59  c[0] = 0.;
60  d[0] = 0.;
61  b[1] = b[0];
62  c[1] = 0.;
63  d[1] = 0.;
64  } else {
65 /*
66 * set up tridiagonal system
67 *
68 * b = diagonal, d = offdiagonal, c = right hand side.
69 */
70  d[0] = x[1] - x[0];
71  c[1] = (y[1] - y[0])/d[0];
72  for (int i = 1; i < nm1; i++) {
73  d[i] = x[i + 1] - x[i];
74  b[i] = 2.*(d[i - 1] + d[i]);
75  c[i + 1] = (y[i + 1] - y[i])/d[i];
76  c[i] = c[i + 1] - c[i];
77  }
78 /*
79 * end conditions. third derivatives at x(1) and x(n)
80 * obtained from divided differences
81 */
82  b[0] = -d[0];
83  b[n - 1] = -d[n - 2];
84  c[0] = 0.;
85  c[n - 1] = 0.;
86  if (n != 3) {
87  c[0] = c[2]/(x[3] - x[1]) - c[1]/(x[2] - x[0]);
88  c[n - 1] = c[n - 2]/(x[n - 1] - x[n - 3]) - c[n - 3]/(x[n - 2] - x[n - 4]);
89  c[0] = c[0]*d[0]*d[0]/(x[3] - x[0]);
90  c[n - 1] = -c[n - 1]*d[n - 2]*d[n - 2]/(x[n - 1] - x[n - 4]);
91  }
92 /*
93 * forward elimination
94 */
95  for (int i = 1; i < n; i++) {
96  doublereal t = d[i - 1]/b[i - 1];
97  b[i] = b[i] - t*d[i - 1];
98  c[i] = c[i] - t*c[i - 1];
99  }
100 
101 /*
102 * back substitution
103 */
104  c[n - 1] = c[n - 1]/b[n - 1];
105  for (int ib = 0; ib < nm1; ib++) {
106  int i = n - ib - 2;
107  c[i] = (c[i] - d[i]*c[i + 1])/b[i];
108  }
109 /*
110 * c(i) is now the sigma(i) of the text
111 *
112 * compute polynomial coefficients
113 */
114  b[n-1] = (y[n - 1] - y[nm1 - 1])/d[nm1 - 1] + d[nm1 - 1]*(c[nm1 - 1] + 2.*c[n - 1]);
115  for (int i=0; i<nm1; i++) {
116  b[i] = (y[i + 1] - y[i])/d[i] - d[i]*(c[i + 1] + 2.*c[i]);
117  d[i] = (c[i + 1] - c[i])/d[i];
118  c[i] = 3.*c[i];
119  }
120  c[n - 1] = 3.*c[n - 1];
121  d[n - 1] = d[n - 2];
122  }
123 
124  return;
125 }
#define ASSERTMSGBREAK(expr, msg)
Definition: myassert.h:222
static std::stack< cleanup * > c
Definition: cleanup.cc:59
double doublereal
Definition: colamd.c:52