001    /*
002     * Licensed to the Apache Software Foundation (ASF) under one or more
003     * contributor license agreements.  See the NOTICE file distributed with
004     * this work for additional information regarding copyright ownership.
005     * The ASF licenses this file to You under the Apache License, Version 2.0
006     * (the "License"); you may not use this file except in compliance with
007     * the License.  You may obtain a copy of the License at
008     *
009     *      http://www.apache.org/licenses/LICENSE-2.0
010     *
011     * Unless required by applicable law or agreed to in writing, software
012     * distributed under the License is distributed on an "AS IS" BASIS,
013     * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
014     * See the License for the specific language governing permissions and
015     * limitations under the License.
016     */
017    
018    package org.apache.commons.math.ode.nonstiff;
019    
020    import java.util.Arrays;
021    
022    import org.apache.commons.math.linear.Array2DRowRealMatrix;
023    import org.apache.commons.math.linear.MatrixVisitorException;
024    import org.apache.commons.math.linear.RealMatrixPreservingVisitor;
025    import org.apache.commons.math.ode.DerivativeException;
026    import org.apache.commons.math.ode.FirstOrderDifferentialEquations;
027    import org.apache.commons.math.ode.IntegratorException;
028    import org.apache.commons.math.ode.events.CombinedEventsManager;
029    import org.apache.commons.math.ode.sampling.NordsieckStepInterpolator;
030    import org.apache.commons.math.ode.sampling.StepHandler;
031    
032    
033    /**
034     * This class implements implicit Adams-Moulton integrators for Ordinary
035     * Differential Equations.
036     *
037     * <p>Adams-Moulton methods (in fact due to Adams alone) are implicit
038     * multistep ODE solvers. This implementation is a variation of the classical
039     * one: it uses adaptive stepsize to implement error control, whereas
040     * classical implementations are fixed step size. The value of state vector
041     * at step n+1 is a simple combination of the value at step n and of the
042     * derivatives at steps n+1, n, n-1 ... Since y'<sub>n+1</sub> is needed to
043     * compute y<sub>n+1</sub>,another method must be used to compute a first
044     * estimate of y<sub>n+1</sub>, then compute y'<sub>n+1</sub>, then compute
045     * a final estimate of y<sub>n+1</sub> using the following formulas. Depending
046     * on the number k of previous steps one wants to use for computing the next
047     * value, different formulas are available for the final estimate:</p>
048     * <ul>
049     *   <li>k = 1: y<sub>n+1</sub> = y<sub>n</sub> + h y'<sub>n+1</sub></li>
050     *   <li>k = 2: y<sub>n+1</sub> = y<sub>n</sub> + h (y'<sub>n+1</sub>+y'<sub>n</sub>)/2</li>
051     *   <li>k = 3: y<sub>n+1</sub> = y<sub>n</sub> + h (5y'<sub>n+1</sub>+8y'<sub>n</sub>-y'<sub>n-1</sub>)/12</li>
052     *   <li>k = 4: y<sub>n+1</sub> = y<sub>n</sub> + h (9y'<sub>n+1</sub>+19y'<sub>n</sub>-5y'<sub>n-1</sub>+y'<sub>n-2</sub>)/24</li>
053     *   <li>...</li>
054     * </ul>
055     *
056     * <p>A k-steps Adams-Moulton method is of order k+1.</p>
057     *
058     * <h3>Implementation details</h3>
059     *
060     * <p>We define scaled derivatives s<sub>i</sub>(n) at step n as:
061     * <pre>
062     * s<sub>1</sub>(n) = h y'<sub>n</sub> for first derivative
063     * s<sub>2</sub>(n) = h<sup>2</sup>/2 y''<sub>n</sub> for second derivative
064     * s<sub>3</sub>(n) = h<sup>3</sup>/6 y'''<sub>n</sub> for third derivative
065     * ...
066     * s<sub>k</sub>(n) = h<sup>k</sup>/k! y(k)<sub>n</sub> for k<sup>th</sup> derivative
067     * </pre></p>
068     *
069     * <p>The definitions above use the classical representation with several previous first
070     * derivatives. Lets define
071     * <pre>
072     *   q<sub>n</sub> = [ s<sub>1</sub>(n-1) s<sub>1</sub>(n-2) ... s<sub>1</sub>(n-(k-1)) ]<sup>T</sup>
073     * </pre>
074     * (we omit the k index in the notation for clarity). With these definitions,
075     * Adams-Moulton methods can be written:
076     * <ul>
077     *   <li>k = 1: y<sub>n+1</sub> = y<sub>n</sub> + s<sub>1</sub>(n+1)</li>
078     *   <li>k = 2: y<sub>n+1</sub> = y<sub>n</sub> + 1/2 s<sub>1</sub>(n+1) + [ 1/2 ] q<sub>n+1</sub></li>
079     *   <li>k = 3: y<sub>n+1</sub> = y<sub>n</sub> + 5/12 s<sub>1</sub>(n+1) + [ 8/12 -1/12 ] q<sub>n+1</sub></li>
080     *   <li>k = 4: y<sub>n+1</sub> = y<sub>n</sub> + 9/24 s<sub>1</sub>(n+1) + [ 19/24 -5/24 1/24 ] q<sub>n+1</sub></li>
081     *   <li>...</li>
082     * </ul></p>
083     *
084     * <p>Instead of using the classical representation with first derivatives only (y<sub>n</sub>,
085     * s<sub>1</sub>(n+1) and q<sub>n+1</sub>), our implementation uses the Nordsieck vector with
086     * higher degrees scaled derivatives all taken at the same step (y<sub>n</sub>, s<sub>1</sub>(n)
087     * and r<sub>n</sub>) where r<sub>n</sub> is defined as:
088     * <pre>
089     * r<sub>n</sub> = [ s<sub>2</sub>(n), s<sub>3</sub>(n) ... s<sub>k</sub>(n) ]<sup>T</sup>
090     * </pre>
091     * (here again we omit the k index in the notation for clarity)
092     * </p>
093     *
094     * <p>Taylor series formulas show that for any index offset i, s<sub>1</sub>(n-i) can be
095     * computed from s<sub>1</sub>(n), s<sub>2</sub>(n) ... s<sub>k</sub>(n), the formula being exact
096     * for degree k polynomials.
097     * <pre>
098     * s<sub>1</sub>(n-i) = s<sub>1</sub>(n) + &sum;<sub>j</sub> j (-i)<sup>j-1</sup> s<sub>j</sub>(n)
099     * </pre>
100     * The previous formula can be used with several values for i to compute the transform between
101     * classical representation and Nordsieck vector. The transform between r<sub>n</sub>
102     * and q<sub>n</sub> resulting from the Taylor series formulas above is:
103     * <pre>
104     * q<sub>n</sub> = s<sub>1</sub>(n) u + P r<sub>n</sub>
105     * </pre>
106     * where u is the [ 1 1 ... 1 ]<sup>T</sup> vector and P is the (k-1)&times;(k-1) matrix built
107     * with the j (-i)<sup>j-1</sup> terms:
108     * <pre>
109     *        [  -2   3   -4    5  ... ]
110     *        [  -4  12  -32   80  ... ]
111     *   P =  [  -6  27 -108  405  ... ]
112     *        [  -8  48 -256 1280  ... ]
113     *        [          ...           ]
114     * </pre></p>
115     * 
116     * <p>Using the Nordsieck vector has several advantages:
117     * <ul>
118     *   <li>it greatly simplifies step interpolation as the interpolator mainly applies
119     *   Taylor series formulas,</li>
120     *   <li>it simplifies step changes that occur when discrete events that truncate
121     *   the step are triggered,</li>
122     *   <li>it allows to extend the methods in order to support adaptive stepsize.</li>
123     * </ul></p>
124     * 
125     * <p>The predicted Nordsieck vector at step n+1 is computed from the Nordsieck vector at step
126     * n as follows:
127     * <ul>
128     *   <li>Y<sub>n+1</sub> = y<sub>n</sub> + s<sub>1</sub>(n) + u<sup>T</sup> r<sub>n</sub></li>
129     *   <li>S<sub>1</sub>(n+1) = h f(t<sub>n+1</sub>, Y<sub>n+1</sub>)</li>
130     *   <li>R<sub>n+1</sub> = (s<sub>1</sub>(n) - S<sub>1</sub>(n+1)) P<sup>-1</sup> u + P<sup>-1</sup> A P r<sub>n</sub></li>
131     * </ul>
132     * where A is a rows shifting matrix (the lower left part is an identity matrix):
133     * <pre>
134     *        [ 0 0   ...  0 0 | 0 ]
135     *        [ ---------------+---]
136     *        [ 1 0   ...  0 0 | 0 ]
137     *    A = [ 0 1   ...  0 0 | 0 ]
138     *        [       ...      | 0 ]
139     *        [ 0 0   ...  1 0 | 0 ]
140     *        [ 0 0   ...  0 1 | 0 ]
141     * </pre>
142     * From this predicted vector, the corrected vector is computed as follows:
143     * <ul>
144     *   <li>y<sub>n+1</sub> = y<sub>n</sub> + S<sub>1</sub>(n+1) + [ -1 +1 -1 +1 ... &plusmn;1 ] r<sub>n+1</sub></li>
145     *   <li>s<sub>1</sub>(n+1) = h f(t<sub>n+1</sub>, y<sub>n+1</sub>)</li>
146     *   <li>r<sub>n+1</sub> = R<sub>n+1</sub> + (s<sub>1</sub>(n+1) - S<sub>1</sub>(n+1)) P<sup>-1</sup> u</li>
147     * </ul>
148     * where the upper case Y<sub>n+1</sub>, S<sub>1</sub>(n+1) and R<sub>n+1</sub> represent the
149     * predicted states whereas the lower case y<sub>n+1</sub>, s<sub>n+1</sub> and r<sub>n+1</sub>
150     * represent the corrected states.</p>
151     *
152     * <p>The P<sup>-1</sup>u vector and the P<sup>-1</sup> A P matrix do not depend on the state,
153     * they only depend on k and therefore are precomputed once for all.</p>
154     *
155     * @version $Revision: 789159 $ $Date: 2009-06-28 17:56:20 -0400 (Sun, 28 Jun 2009) $
156     * @since 2.0
157     */
158    public class AdamsMoultonIntegrator extends AdamsIntegrator {
159    
160        /**
161         * Build an Adams-Moulton integrator with the given order and error control parameters.
162         * @param nSteps number of steps of the method excluding the one being computed
163         * @param minStep minimal step (must be positive even for backward
164         * integration), the last step can be smaller than this
165         * @param maxStep maximal step (must be positive even for backward
166         * integration)
167         * @param scalAbsoluteTolerance allowed absolute error
168         * @param scalRelativeTolerance allowed relative error
169         * @exception IllegalArgumentException if order is 1 or less
170         */
171        public AdamsMoultonIntegrator(final int nSteps,
172                                      final double minStep, final double maxStep,
173                                      final double scalAbsoluteTolerance,
174                                      final double scalRelativeTolerance)
175            throws IllegalArgumentException {
176            super("Adams-Moulton", nSteps, nSteps + 1, minStep, maxStep,
177                  scalAbsoluteTolerance, scalRelativeTolerance);
178        }
179    
180        /**
181         * Build an Adams-Moulton integrator with the given order and error control parameters.
182         * @param nSteps number of steps of the method excluding the one being computed
183         * @param minStep minimal step (must be positive even for backward
184         * integration), the last step can be smaller than this
185         * @param maxStep maximal step (must be positive even for backward
186         * integration)
187         * @param vecAbsoluteTolerance allowed absolute error
188         * @param vecRelativeTolerance allowed relative error
189         * @exception IllegalArgumentException if order is 1 or less
190         */
191        public AdamsMoultonIntegrator(final int nSteps,
192                                      final double minStep, final double maxStep,
193                                      final double[] vecAbsoluteTolerance,
194                                      final double[] vecRelativeTolerance)
195            throws IllegalArgumentException {
196            super("Adams-Moulton", nSteps, nSteps + 1, minStep, maxStep,
197                  vecAbsoluteTolerance, vecRelativeTolerance);
198        }
199          
200        
201        /** {@inheritDoc} */
202        @Override
203        public double integrate(final FirstOrderDifferentialEquations equations,
204                                final double t0, final double[] y0,
205                                final double t, final double[] y)
206            throws DerivativeException, IntegratorException {
207    
208            final int n = y0.length;
209            sanityChecks(equations, t0, y0, t, y);
210            setEquations(equations);
211            resetEvaluations();
212            final boolean forward = (t > t0);
213    
214            // initialize working arrays
215            if (y != y0) {
216                System.arraycopy(y0, 0, y, 0, n);
217            }
218            final double[] yDot = new double[y0.length];
219            final double[] yTmp = new double[y0.length];
220    
221            // set up two interpolators sharing the integrator arrays
222            final NordsieckStepInterpolator interpolator = new NordsieckStepInterpolator();
223            interpolator.reinitialize(y, forward);
224            final NordsieckStepInterpolator interpolatorTmp = new NordsieckStepInterpolator();
225            interpolatorTmp.reinitialize(yTmp, forward);
226    
227            // set up integration control objects
228            for (StepHandler handler : stepHandlers) {
229                handler.reset();
230            }
231            CombinedEventsManager manager = addEndTimeChecker(t0, t, eventsHandlersManager);
232    
233    
234            // compute the initial Nordsieck vector using the configured starter integrator
235            start(t0, y, t);
236            interpolator.reinitialize(stepStart, stepSize, scaled, nordsieck);
237            interpolator.storeTime(stepStart);
238    
239            double hNew = stepSize;
240            interpolator.rescale(hNew);
241            
242            boolean lastStep = false;
243            while (!lastStep) {
244    
245                // shift all data
246                interpolator.shift();
247    
248                double error = 0;
249                for (boolean loop = true; loop;) {
250    
251                    stepSize = hNew;
252    
253                    // predict a first estimate of the state at step end (P in the PECE sequence)
254                    final double stepEnd = stepStart + stepSize;
255                    interpolator.setInterpolatedTime(stepEnd);
256                    System.arraycopy(interpolator.getInterpolatedState(), 0, yTmp, 0, y0.length);
257    
258                    // evaluate a first estimate of the derivative (first E in the PECE sequence)
259                    computeDerivatives(stepEnd, yTmp, yDot);
260    
261                    // update Nordsieck vector
262                    final double[] predictedScaled = new double[y0.length];
263                    for (int j = 0; j < y0.length; ++j) {
264                        predictedScaled[j] = stepSize * yDot[j];
265                    }
266                    final Array2DRowRealMatrix nordsieckTmp = updateHighOrderDerivativesPhase1(nordsieck);
267                    updateHighOrderDerivativesPhase2(scaled, predictedScaled, nordsieckTmp);
268    
269                    // apply correction (C in the PECE sequence)
270                    error = nordsieckTmp.walkInOptimizedOrder(new Corrector(y, predictedScaled, yTmp));
271    
272                    if (error <= 1.0) {
273    
274                        // evaluate a final estimate of the derivative (second E in the PECE sequence)
275                        computeDerivatives(stepEnd, yTmp, yDot);
276    
277                        // update Nordsieck vector
278                        final double[] correctedScaled = new double[y0.length];
279                        for (int j = 0; j < y0.length; ++j) {
280                            correctedScaled[j] = stepSize * yDot[j];
281                        }
282                        updateHighOrderDerivativesPhase2(predictedScaled, correctedScaled, nordsieckTmp);
283    
284                        // discrete events handling
285                        interpolatorTmp.reinitialize(stepEnd, stepSize, correctedScaled, nordsieckTmp);
286                        interpolatorTmp.storeTime(stepStart);
287                        interpolatorTmp.shift();
288                        interpolatorTmp.storeTime(stepEnd);
289                        if (manager.evaluateStep(interpolatorTmp)) {
290                            final double dt = manager.getEventTime() - stepStart;
291                            if (Math.abs(dt) <= Math.ulp(stepStart)) {
292                                // rejecting the step would lead to a too small next step, we accept it
293                                loop = false;
294                            } else {
295                                // reject the step to match exactly the next switch time
296                                hNew = dt;
297                                interpolator.rescale(hNew);
298                            }
299                        } else {
300                            // accept the step
301                            scaled    = correctedScaled;
302                            nordsieck = nordsieckTmp;
303                            interpolator.reinitialize(stepEnd, stepSize, scaled, nordsieck);
304                            loop = false;
305                        }
306    
307                    } else {
308                        // reject the step and attempt to reduce error by stepsize control
309                        final double factor = computeStepGrowShrinkFactor(error);
310                        hNew = filterStep(stepSize * factor, forward, false);
311                        interpolator.rescale(hNew);
312                    }
313    
314                }
315    
316                // the step has been accepted (may have been truncated)
317                final double nextStep = stepStart + stepSize;
318                System.arraycopy(yTmp, 0, y, 0, n);
319                interpolator.storeTime(nextStep);
320                manager.stepAccepted(nextStep, y);
321                lastStep = manager.stop();
322    
323                // provide the step data to the step handler
324                for (StepHandler handler : stepHandlers) {
325                    interpolator.setInterpolatedTime(nextStep);
326                    handler.handleStep(interpolator, lastStep);
327                }
328                stepStart = nextStep;
329    
330                if (!lastStep && manager.reset(stepStart, y)) {
331    
332                    // some events handler has triggered changes that
333                    // invalidate the derivatives, we need to restart from scratch
334                    start(stepStart, y, t);
335                    interpolator.reinitialize(stepStart, stepSize, scaled, nordsieck);
336    
337                }
338    
339                if (! lastStep) {
340                    // in some rare cases we may get here with stepSize = 0, for example
341                    // when an event occurs at integration start, reducing the first step
342                    // to zero; we have to reset the step to some safe non zero value
343                    stepSize = filterStep(stepSize, forward, true);
344    
345                    // stepsize control for next step
346                    final double  factor     = computeStepGrowShrinkFactor(error);
347                    final double  scaledH    = stepSize * factor;
348                    final double  nextT      = stepStart + scaledH;
349                    final boolean nextIsLast = forward ? (nextT >= t) : (nextT <= t);
350                    hNew = filterStep(scaledH, forward, nextIsLast);
351                    interpolator.rescale(hNew);
352                }
353    
354            }
355    
356            final double stopTime  = stepStart;
357            stepStart = Double.NaN;
358            stepSize  = Double.NaN;
359            return stopTime;
360    
361        }
362    
363        /** Corrector for current state in Adams-Moulton method.
364         * <p>
365         * This visitor implements the Taylor series formula:
366         * <pre>
367         * Y<sub>n+1</sub> = y<sub>n</sub> + s<sub>1</sub>(n+1) + [ -1 +1 -1 +1 ... &plusmn;1 ] r<sub>n+1</sub>
368         * </pre>
369         * </p>
370         */
371        private class Corrector implements RealMatrixPreservingVisitor {
372    
373            /** Previous state. */
374            private final double[] previous;
375    
376            /** Current scaled first derivative. */
377            private final double[] scaled;
378    
379            /** Current state before correction. */
380            private final double[] before;
381    
382            /** Current state after correction. */
383            private final double[] after;
384    
385            /** Simple constructor.
386             * @param previous previous state
387             * @param scaled current scaled first derivative
388             * @param state state to correct (will be overwritten after visit)
389             */
390            public Corrector(final double[] previous, final double[] scaled, final double[] state) {
391                this.previous = previous;
392                this.scaled   = scaled;
393                this.after    = state;
394                this.before   = state.clone();
395            }
396    
397            /** {@inheritDoc} */
398            public void start(int rows, int columns,
399                              int startRow, int endRow, int startColumn, int endColumn) {
400                Arrays.fill(after, 0.0);
401            }
402    
403            /** {@inheritDoc} */
404            public void visit(int row, int column, double value)
405                throws MatrixVisitorException {
406                if ((row & 0x1) == 0) {
407                    after[column] -= value;
408                } else {
409                    after[column] += value;
410                }
411            }
412    
413            /**
414             * End visiting te Nordsieck vector.
415             * <p>The correction is used to control stepsize. So its amplitude is
416             * considered to be an error, which must be normalized according to
417             * error control settings. If the normalized value is greater than 1,
418             * the correction was too large and the step must be rejected.</p>
419             * @return the normalized correction, if greater than 1, the step
420             * must be rejected
421             */
422            public double end() {
423    
424                double error = 0;
425                for (int i = 0; i < after.length; ++i) {
426                    after[i] += previous[i] + scaled[i];
427                    final double yScale = Math.max(Math.abs(previous[i]), Math.abs(after[i]));
428                    final double tol = (vecAbsoluteTolerance == null) ?
429                                       (scalAbsoluteTolerance + scalRelativeTolerance * yScale) :
430                                       (vecAbsoluteTolerance[i] + vecRelativeTolerance[i] * yScale);
431                    final double ratio  = (after[i] - before[i]) / tol;
432                    error += ratio * ratio;
433                }
434    
435                return Math.sqrt(error / after.length);
436    
437            }
438        }
439    
440    }