1 /*
2 * Licensed to the Hipparchus project under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * The Hipparchus project licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * https://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17
18 package org.hipparchus.ode.nonstiff.interpolators;
19
20 import org.hipparchus.ode.EquationsMapper;
21 import org.hipparchus.ode.ODEStateAndDerivative;
22 import org.hipparchus.ode.nonstiff.GraggBulirschStoerIntegrator;
23 import org.hipparchus.ode.sampling.AbstractODEStateInterpolator;
24 import org.hipparchus.util.FastMath;
25
26 /**
27 * This class implements an interpolator for the Gragg-Bulirsch-Stoer
28 * integrator.
29 *
30 * <p>This interpolator compute dense output inside the last step
31 * produced by a Gragg-Bulirsch-Stoer integrator.</p>
32 *
33 * <p>
34 * This implementation is basically a reimplementation in Java of the
35 * <a
36 * href="http://www.unige.ch/math/folks/hairer/prog/nonstiff/odex.f">odex</a>
37 * fortran code by E. Hairer and G. Wanner. The redistribution policy
38 * for this code is available <a
39 * href="http://www.unige.ch/~hairer/prog/licence.txt">here</a>, for
40 * convenience, it is reproduced below.</p>
41 *
42 * <blockquote>
43 * <p>Copyright (c) 2004, Ernst Hairer</p>
44 *
45 * <p>Redistribution and use in source and binary forms, with or
46 * without modification, are permitted provided that the following
47 * conditions are met:</p>
48 * <ul>
49 * <li>Redistributions of source code must retain the above copyright
50 * notice, this list of conditions and the following disclaimer.</li>
51 * <li>Redistributions in binary form must reproduce the above copyright
52 * notice, this list of conditions and the following disclaimer in the
53 * documentation and/or other materials provided with the distribution.</li>
54 * </ul>
55 *
56 * <p><strong>THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
57 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
58 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
59 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
60 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
61 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
62 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
63 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
64 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
65 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
66 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.</strong></p>
67 * </blockquote>
68 *
69 * @see GraggBulirschStoerIntegrator
70 */
71
72 public class GraggBulirschStoerStateInterpolator extends AbstractODEStateInterpolator {
73
74 /** Serializable version identifier. */
75 private static final long serialVersionUID = 20160329L;
76
77 /** Scaled derivatives at the middle of the step $\tau$.
78 * (element k is $h^{k} d^{k}y(\tau)/dt^{k}$ where h is step size...)
79 */
80 private final double[][] yMidDots;
81
82 /** Interpolation polynomials. */
83 private final double[][] polynomials;
84
85 /** Error coefficients for the interpolation. */
86 private final double[] errfac;
87
88 /** Degree of the interpolation polynomials. */
89 private final int currentDegree;
90
91 /** Simple constructor.
92 * @param forward integration direction indicator
93 * @param globalPreviousState start of the global step
94 * @param globalCurrentState end of the global step
95 * @param softPreviousState start of the restricted step
96 * @param softCurrentState end of the restricted step
97 * @param mapper equations mapper for the all equations
98 * @param yMidDots scaled derivatives at the middle of the step $\tau$
99 * (element k is $h^{k} d^{k}y(\tau)/dt^{k}$ where h is step size...)
100 * @param mu degree of the interpolation polynomial
101 */
102 public GraggBulirschStoerStateInterpolator(final boolean forward,
103 final ODEStateAndDerivative globalPreviousState,
104 final ODEStateAndDerivative globalCurrentState,
105 final ODEStateAndDerivative softPreviousState,
106 final ODEStateAndDerivative softCurrentState,
107 final EquationsMapper mapper,
108 final double[][] yMidDots,
109 final int mu) {
110 super(forward, globalPreviousState, globalCurrentState, softPreviousState, softCurrentState, mapper);
111
112 this.yMidDots = yMidDots.clone();
113 this.currentDegree = mu + 4;
114 this.polynomials = new double[currentDegree + 1][getCurrentState().getCompleteStateDimension()];
115
116 // initialize the error factors array for interpolation
117 if (currentDegree <= 4) {
118 errfac = null;
119 } else {
120 errfac = new double[currentDegree - 4];
121 for (int i = 0; i < errfac.length; ++i) {
122 final int ip5 = i + 5;
123 errfac[i] = 1.0 / (ip5 * ip5);
124 final double e = 0.5 * FastMath.sqrt (((double) (i + 1)) / ip5);
125 for (int j = 0; j <= i; ++j) {
126 errfac[i] *= e / (j + 1);
127 }
128 }
129 }
130
131 // compute the interpolation coefficients
132 computeCoefficients(mu);
133
134 }
135
136 /** {@inheritDoc} */
137 @Override
138 protected GraggBulirschStoerStateInterpolator create(final boolean newForward,
139 final ODEStateAndDerivative newGlobalPreviousState,
140 final ODEStateAndDerivative newGlobalCurrentState,
141 final ODEStateAndDerivative newSoftPreviousState,
142 final ODEStateAndDerivative newSoftCurrentState,
143 final EquationsMapper newMapper) {
144 return new GraggBulirschStoerStateInterpolator(newForward,
145 newGlobalPreviousState, newGlobalCurrentState,
146 newSoftPreviousState, newSoftCurrentState,
147 newMapper, yMidDots, currentDegree - 4);
148 }
149
150 /** Compute the interpolation coefficients for dense output.
151 * @param mu degree of the interpolation polynomial
152 */
153 private void computeCoefficients(final int mu) {
154
155 final double[] y0Dot = getGlobalPreviousState().getCompleteDerivative();
156 final double[] y1Dot = getGlobalCurrentState().getCompleteDerivative();
157 final double[] y1 = getGlobalCurrentState().getCompleteState();
158
159 final double[] previousState = getGlobalPreviousState().getCompleteState();
160 final double h = getGlobalCurrentState().getTime() - getGlobalPreviousState().getTime();
161 for (int i = 0; i < previousState.length; ++i) {
162
163 final double yp0 = h * y0Dot[i];
164 final double yp1 = h * y1Dot[i];
165 final double ydiff = y1[i] - previousState[i];
166 final double aspl = ydiff - yp1;
167 final double bspl = yp0 - ydiff;
168
169 polynomials[0][i] = previousState[i];
170 polynomials[1][i] = ydiff;
171 polynomials[2][i] = aspl;
172 polynomials[3][i] = bspl;
173
174 if (mu < 0) {
175 return;
176 }
177
178 // compute the remaining coefficients
179 final double ph0 = 0.5 * (previousState[i] + y1[i]) + 0.125 * (aspl + bspl);
180 polynomials[4][i] = 16 * (yMidDots[0][i] - ph0);
181
182 if (mu > 0) {
183 final double ph1 = ydiff + 0.25 * (aspl - bspl);
184 polynomials[5][i] = 16 * (yMidDots[1][i] - ph1);
185
186 if (mu > 1) {
187 final double ph2 = yp1 - yp0;
188 polynomials[6][i] = 16 * (yMidDots[2][i] - ph2 + polynomials[4][i]);
189
190 if (mu > 2) {
191 final double ph3 = 6 * (bspl - aspl);
192 polynomials[7][i] = 16 * (yMidDots[3][i] - ph3 + 3 * polynomials[5][i]);
193
194 for (int j = 4; j <= mu; ++j) {
195 final double fac1 = 0.5 * j * (j - 1);
196 final double fac2 = 2 * fac1 * (j - 2) * (j - 3);
197 polynomials[j+4][i] =
198 16 * (yMidDots[j][i] + fac1 * polynomials[j+2][i] - fac2 * polynomials[j][i]);
199 }
200
201 }
202 }
203 }
204 }
205
206 }
207
208 /** Estimate interpolation error.
209 * @param scale scaling array
210 * @return estimate of the interpolation error
211 */
212 public double estimateError(final double[] scale) {
213 double error = 0;
214 if (currentDegree >= 5) {
215 for (int i = 0; i < scale.length; ++i) {
216 final double e = polynomials[currentDegree][i] / scale[i];
217 error += e * e;
218 }
219 error = FastMath.sqrt(error / scale.length) * errfac[currentDegree - 5];
220 }
221 return error;
222 }
223
224 /** {@inheritDoc} */
225 @Override
226 protected ODEStateAndDerivative computeInterpolatedStateAndDerivatives(final EquationsMapper mapper,
227 final double time, final double theta,
228 final double thetaH, final double oneMinusThetaH) {
229
230 final int dimension = mapper.getTotalDimension();
231
232 final double h = thetaH / theta;
233 final double oneMinusTheta = 1.0 - theta;
234 final double theta05 = theta - 0.5;
235 final double tOmT = theta * oneMinusTheta;
236 final double t4 = tOmT * tOmT;
237 final double t4Dot = 2 * tOmT * (1 - 2 * theta);
238 final double dot1 = 1.0 / h;
239 final double dot2 = theta * (2 - 3 * theta) / h;
240 final double dot3 = ((3 * theta - 4) * theta + 1) / h;
241
242 final double[] interpolatedState = new double[dimension];
243 final double[] interpolatedDerivatives = new double[dimension];
244 for (int i = 0; i < dimension; ++i) {
245
246 final double p0 = polynomials[0][i];
247 final double p1 = polynomials[1][i];
248 final double p2 = polynomials[2][i];
249 final double p3 = polynomials[3][i];
250 interpolatedState[i] = p0 + theta * (p1 + oneMinusTheta * (p2 * theta + p3 * oneMinusTheta));
251 interpolatedDerivatives[i] = dot1 * p1 + dot2 * p2 + dot3 * p3;
252
253 if (currentDegree > 3) {
254 double cDot = 0;
255 double c = polynomials[currentDegree][i];
256 for (int j = currentDegree - 1; j > 3; --j) {
257 final double d = 1.0 / (j - 3);
258 cDot = d * (theta05 * cDot + c);
259 c = polynomials[j][i] + c * d * theta05;
260 }
261 interpolatedState[i] += t4 * c;
262 interpolatedDerivatives[i] += (t4 * cDot + t4Dot * c) / h;
263 }
264
265 }
266
267 if (h == 0) {
268 // in this degenerated case, the previous computation leads to NaN for derivatives
269 // we fix this by using the derivatives at midpoint
270 System.arraycopy(yMidDots[1], 0, interpolatedDerivatives, 0, dimension);
271 }
272
273 return mapper.mapStateAndDerivative(time, interpolatedState, interpolatedDerivatives);
274
275 }
276
277 }