1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18 package org.apache.commons.math.ode.nonstiff;
19
20 import java.io.IOException;
21 import java.io.ObjectInput;
22 import java.io.ObjectOutput;
23
24 import org.apache.commons.math.ode.DerivativeException;
25 import org.apache.commons.math.ode.sampling.AbstractStepInterpolator;
26 import org.apache.commons.math.ode.sampling.StepInterpolator;
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78 class GraggBulirschStoerStepInterpolator
79 extends AbstractStepInterpolator {
80
81
82 private double[] y0Dot;
83
84
85 private double[] y1;
86
87
88 private double[] y1Dot;
89
90
91
92
93 private double[][] yMidDots;
94
95
96 private double[][] polynoms;
97
98
99 private double[] errfac;
100
101
102 private int currentDegree;
103
104
105
106
107
108
109 private void resetTables(final int maxDegree) {
110
111 if (maxDegree < 0) {
112 polynoms = null;
113 errfac = null;
114 currentDegree = -1;
115 } else {
116
117 final double[][] newPols = new double[maxDegree + 1][];
118 if (polynoms != null) {
119 System.arraycopy(polynoms, 0, newPols, 0, polynoms.length);
120 for (int i = polynoms.length; i < newPols.length; ++i) {
121 newPols[i] = new double[currentState.length];
122 }
123 } else {
124 for (int i = 0; i < newPols.length; ++i) {
125 newPols[i] = new double[currentState.length];
126 }
127 }
128 polynoms = newPols;
129
130
131 if (maxDegree <= 4) {
132 errfac = null;
133 } else {
134 errfac = new double[maxDegree - 4];
135 for (int i = 0; i < errfac.length; ++i) {
136 final int ip5 = i + 5;
137 errfac[i] = 1.0 / (ip5 * ip5);
138 final double e = 0.5 * Math.sqrt (((double) (i + 1)) / ip5);
139 for (int j = 0; j <= i; ++j) {
140 errfac[i] *= e / (j + 1);
141 }
142 }
143 }
144
145 currentDegree = 0;
146
147 }
148
149 }
150
151
152
153
154
155 public GraggBulirschStoerStepInterpolator() {
156 y0Dot = null;
157 y1 = null;
158 y1Dot = null;
159 yMidDots = null;
160 resetTables(-1);
161 }
162
163
164
165
166
167
168
169
170
171
172
173
174
175 public GraggBulirschStoerStepInterpolator(final double[] y, final double[] y0Dot,
176 final double[] y1, final double[] y1Dot,
177 final double[][] yMidDots,
178 final boolean forward) {
179
180 super(y, forward);
181 this.y0Dot = y0Dot;
182 this.y1 = y1;
183 this.y1Dot = y1Dot;
184 this.yMidDots = yMidDots;
185
186 resetTables(yMidDots.length + 4);
187
188 }
189
190
191
192
193
194
195 public GraggBulirschStoerStepInterpolator
196 (final GraggBulirschStoerStepInterpolator interpolator) {
197
198 super(interpolator);
199
200 final int dimension = currentState.length;
201
202
203
204 y0Dot = null;
205 y1 = null;
206 y1Dot = null;
207 yMidDots = null;
208
209
210 if (interpolator.polynoms == null) {
211 polynoms = null;
212 currentDegree = -1;
213 } else {
214 resetTables(interpolator.currentDegree);
215 for (int i = 0; i < polynoms.length; ++i) {
216 polynoms[i] = new double[dimension];
217 System.arraycopy(interpolator.polynoms[i], 0,
218 polynoms[i], 0, dimension);
219 }
220 currentDegree = interpolator.currentDegree;
221 }
222
223 }
224
225
226 @Override
227 protected StepInterpolator doCopy() {
228 return new GraggBulirschStoerStepInterpolator(this);
229 }
230
231
232
233
234
235
236 public void computeCoefficients(final int mu, final double h) {
237
238 if ((polynoms == null) || (polynoms.length <= (mu + 4))) {
239 resetTables(mu + 4);
240 }
241
242 currentDegree = mu + 4;
243
244 for (int i = 0; i < currentState.length; ++i) {
245
246 final double yp0 = h * y0Dot[i];
247 final double yp1 = h * y1Dot[i];
248 final double ydiff = y1[i] - currentState[i];
249 final double aspl = ydiff - yp1;
250 final double bspl = yp0 - ydiff;
251
252 polynoms[0][i] = currentState[i];
253 polynoms[1][i] = ydiff;
254 polynoms[2][i] = aspl;
255 polynoms[3][i] = bspl;
256
257 if (mu < 0) {
258 return;
259 }
260
261
262 final double ph0 = 0.5 * (currentState[i] + y1[i]) + 0.125 * (aspl + bspl);
263 polynoms[4][i] = 16 * (yMidDots[0][i] - ph0);
264
265 if (mu > 0) {
266 final double ph1 = ydiff + 0.25 * (aspl - bspl);
267 polynoms[5][i] = 16 * (yMidDots[1][i] - ph1);
268
269 if (mu > 1) {
270 final double ph2 = yp1 - yp0;
271 polynoms[6][i] = 16 * (yMidDots[2][i] - ph2 + polynoms[4][i]);
272
273 if (mu > 2) {
274 final double ph3 = 6 * (bspl - aspl);
275 polynoms[7][i] = 16 * (yMidDots[3][i] - ph3 + 3 * polynoms[5][i]);
276
277 for (int j = 4; j <= mu; ++j) {
278 final double fac1 = 0.5 * j * (j - 1);
279 final double fac2 = 2 * fac1 * (j - 2) * (j - 3);
280 polynoms[j+4][i] =
281 16 * (yMidDots[j][i] + fac1 * polynoms[j+2][i] - fac2 * polynoms[j][i]);
282 }
283
284 }
285 }
286 }
287 }
288
289 }
290
291
292
293
294
295 public double estimateError(final double[] scale) {
296 double error = 0;
297 if (currentDegree >= 5) {
298 for (int i = 0; i < currentState.length; ++i) {
299 final double e = polynoms[currentDegree][i] / scale[i];
300 error += e * e;
301 }
302 error = Math.sqrt(error / currentState.length) * errfac[currentDegree-5];
303 }
304 return error;
305 }
306
307
308 @Override
309 protected void computeInterpolatedStateAndDerivatives(final double theta,
310 final double oneMinusThetaH)
311 throws DerivativeException {
312
313 final int dimension = currentState.length;
314
315 final double oneMinusTheta = 1.0 - theta;
316 final double theta05 = theta - 0.5;
317 final double tOmT = theta * oneMinusTheta;
318 final double t4 = tOmT * tOmT;
319 final double t4Dot = 2 * tOmT * (1 - 2 * theta);
320 final double dot1 = 1.0 / h;
321 final double dot2 = theta * (2 - 3 * theta) / h;
322 final double dot3 = ((3 * theta - 4) * theta + 1) / h;
323
324 for (int i = 0; i < dimension; ++i) {
325
326 final double p0 = polynoms[0][i];
327 final double p1 = polynoms[1][i];
328 final double p2 = polynoms[2][i];
329 final double p3 = polynoms[3][i];
330 interpolatedState[i] = p0 + theta * (p1 + oneMinusTheta * (p2 * theta + p3 * oneMinusTheta));
331 interpolatedDerivatives[i] = dot1 * p1 + dot2 * p2 + dot3 * p3;
332
333 if (currentDegree > 3) {
334 double cDot = 0;
335 double c = polynoms[currentDegree][i];
336 for (int j = currentDegree - 1; j > 3; --j) {
337 final double d = 1.0 / (j - 3);
338 cDot = d * (theta05 * cDot + c);
339 c = polynoms[j][i] + c * d * theta05;
340 }
341 interpolatedState[i] += t4 * c;
342 interpolatedDerivatives[i] += (t4 * cDot + t4Dot * c) / h;
343 }
344
345 }
346
347 if (h == 0) {
348
349
350 System.arraycopy(yMidDots[1], 0, interpolatedDerivatives, 0, dimension);
351 }
352
353 }
354
355
356 @Override
357 public void writeExternal(final ObjectOutput out)
358 throws IOException {
359
360 final int dimension = (currentState == null) ? -1 : currentState.length;
361
362
363 writeBaseExternal(out);
364
365
366 out.writeInt(currentDegree);
367 for (int k = 0; k <= currentDegree; ++k) {
368 for (int l = 0; l < dimension; ++l) {
369 out.writeDouble(polynoms[k][l]);
370 }
371 }
372
373 }
374
375
376 @Override
377 public void readExternal(final ObjectInput in)
378 throws IOException {
379
380
381 final double t = readBaseExternal(in);
382 final int dimension = (currentState == null) ? -1 : currentState.length;
383
384
385 final int degree = in.readInt();
386 resetTables(degree);
387 currentDegree = degree;
388
389 for (int k = 0; k <= currentDegree; ++k) {
390 for (int l = 0; l < dimension; ++l) {
391 polynoms[k][l] = in.readDouble();
392 }
393 }
394
395
396 setInterpolatedTime(t);
397
398 }
399
400
401 private static final long serialVersionUID = 7320613236731409847L;
402
403 }