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.jacobians;
019
020 import java.io.IOException;
021 import java.io.ObjectInput;
022 import java.io.ObjectOutput;
023 import java.lang.reflect.Array;
024 import java.util.ArrayList;
025 import java.util.Collection;
026
027 import org.apache.commons.math.MathRuntimeException;
028 import org.apache.commons.math.MaxEvaluationsExceededException;
029 import org.apache.commons.math.ode.DerivativeException;
030 import org.apache.commons.math.ode.FirstOrderDifferentialEquations;
031 import org.apache.commons.math.ode.FirstOrderIntegrator;
032 import org.apache.commons.math.ode.IntegratorException;
033 import org.apache.commons.math.ode.events.EventException;
034 import org.apache.commons.math.ode.events.EventHandler;
035 import org.apache.commons.math.ode.sampling.StepHandler;
036 import org.apache.commons.math.ode.sampling.StepInterpolator;
037
038 /** This class enhances a first order integrator for differential equations to
039 * compute also partial derivatives of the solution with respect to initial state
040 * and parameters.
041 * <p>In order to compute both the state and its derivatives, the ODE problem
042 * is extended with jacobians of the raw ODE and the variational equations are
043 * added to form a new compound problem of higher dimension. If the original ODE
044 * problem has dimension n and there are p parameters, the compound problem will
045 * have dimension n × (1 + n + p).</p>
046 * @see ParameterizedODE
047 * @see ODEWithJacobians
048 * @version $Revision: 927342 $ $Date: 2010-03-25 06:55:55 -0400 (Thu, 25 Mar 2010) $
049 * @since 2.1
050 */
051 public class FirstOrderIntegratorWithJacobians {
052
053 /** Underlying integrator for compound problem. */
054 private final FirstOrderIntegrator integrator;
055
056 /** Raw equations to integrate. */
057 private final ODEWithJacobians ode;
058
059 /** Maximal number of evaluations allowed. */
060 private int maxEvaluations;
061
062 /** Number of evaluations already performed. */
063 private int evaluations;
064
065 /** Build an enhanced integrator using internal differentiation to compute jacobians.
066 * @param integrator underlying integrator to solve the compound problem
067 * @param ode original problem (f in the equation y' = f(t, y))
068 * @param p parameters array (may be null if {@link
069 * ParameterizedODE#getParametersDimension()
070 * getParametersDimension()} from original problem is zero)
071 * @param hY step sizes to use for computing the jacobian df/dy, must have the
072 * same dimension as the original problem
073 * @param hP step sizes to use for computing the jacobian df/dp, must have the
074 * same dimension as the original problem parameters dimension
075 * @see #FirstOrderIntegratorWithJacobians(FirstOrderIntegrator,
076 * ODEWithJacobians)
077 */
078 public FirstOrderIntegratorWithJacobians(final FirstOrderIntegrator integrator,
079 final ParameterizedODE ode,
080 final double[] p, final double[] hY, final double[] hP) {
081 checkDimension(ode.getDimension(), hY);
082 checkDimension(ode.getParametersDimension(), p);
083 checkDimension(ode.getParametersDimension(), hP);
084 this.integrator = integrator;
085 this.ode = new FiniteDifferencesWrapper(ode, p, hY, hP);
086 setMaxEvaluations(-1);
087 }
088
089 /** Build an enhanced integrator using ODE builtin jacobian computation features.
090 * @param integrator underlying integrator to solve the compound problem
091 * @param ode original problem, which can compute the jacobians by itself
092 * @see #FirstOrderIntegratorWithJacobians(FirstOrderIntegrator,
093 * ParameterizedODE, double[], double[], double[])
094 */
095 public FirstOrderIntegratorWithJacobians(final FirstOrderIntegrator integrator,
096 final ODEWithJacobians ode) {
097 this.integrator = integrator;
098 this.ode = ode;
099 setMaxEvaluations(-1);
100 }
101
102 /** Add a step handler to this integrator.
103 * <p>The handler will be called by the integrator for each accepted
104 * step.</p>
105 * @param handler handler for the accepted steps
106 * @see #getStepHandlers()
107 * @see #clearStepHandlers()
108 */
109 public void addStepHandler(StepHandlerWithJacobians handler) {
110 final int n = ode.getDimension();
111 final int k = ode.getParametersDimension();
112 integrator.addStepHandler(new StepHandlerWrapper(handler, n, k));
113 }
114
115 /** Get all the step handlers that have been added to the integrator.
116 * @return an unmodifiable collection of the added events handlers
117 * @see #addStepHandler(StepHandlerWithJacobians)
118 * @see #clearStepHandlers()
119 */
120 public Collection<StepHandlerWithJacobians> getStepHandlers() {
121 final Collection<StepHandlerWithJacobians> handlers =
122 new ArrayList<StepHandlerWithJacobians>();
123 for (final StepHandler handler : integrator.getStepHandlers()) {
124 if (handler instanceof StepHandlerWrapper) {
125 handlers.add(((StepHandlerWrapper) handler).getHandler());
126 }
127 }
128 return handlers;
129 }
130
131 /** Remove all the step handlers that have been added to the integrator.
132 * @see #addStepHandler(StepHandlerWithJacobians)
133 * @see #getStepHandlers()
134 */
135 public void clearStepHandlers() {
136 integrator.clearStepHandlers();
137 }
138
139 /** Add an event handler to the integrator.
140 * @param handler event handler
141 * @param maxCheckInterval maximal time interval between switching
142 * function checks (this interval prevents missing sign changes in
143 * case the integration steps becomes very large)
144 * @param convergence convergence threshold in the event time search
145 * @param maxIterationCount upper limit of the iteration count in
146 * the event time search
147 * @see #getEventHandlers()
148 * @see #clearEventHandlers()
149 */
150 public void addEventHandler(EventHandlerWithJacobians handler,
151 double maxCheckInterval,
152 double convergence,
153 int maxIterationCount) {
154 final int n = ode.getDimension();
155 final int k = ode.getParametersDimension();
156 integrator.addEventHandler(new EventHandlerWrapper(handler, n, k),
157 maxCheckInterval, convergence, maxIterationCount);
158 }
159
160 /** Get all the event handlers that have been added to the integrator.
161 * @return an unmodifiable collection of the added events handlers
162 * @see #addEventHandler(EventHandlerWithJacobians, double, double, int)
163 * @see #clearEventHandlers()
164 */
165 public Collection<EventHandlerWithJacobians> getEventHandlers() {
166 final Collection<EventHandlerWithJacobians> handlers =
167 new ArrayList<EventHandlerWithJacobians>();
168 for (final EventHandler handler : integrator.getEventHandlers()) {
169 if (handler instanceof EventHandlerWrapper) {
170 handlers.add(((EventHandlerWrapper) handler).getHandler());
171 }
172 }
173 return handlers;
174 }
175
176 /** Remove all the event handlers that have been added to the integrator.
177 * @see #addEventHandler(EventHandlerWithJacobians, double, double, int)
178 * @see #getEventHandlers()
179 */
180 public void clearEventHandlers() {
181 integrator.clearEventHandlers();
182 }
183
184 /** Integrate the differential equations and the variational equations up to the given time.
185 * <p>This method solves an Initial Value Problem (IVP) and also computes the derivatives
186 * of the solution with respect to initial state and parameters. This can be used as
187 * a basis to solve Boundary Value Problems (BVP).</p>
188 * <p>Since this method stores some internal state variables made
189 * available in its public interface during integration ({@link
190 * #getCurrentSignedStepsize()}), it is <em>not</em> thread-safe.</p>
191 * @param t0 initial time
192 * @param y0 initial value of the state vector at t0
193 * @param dY0dP initial value of the state vector derivative with respect to the
194 * parameters at t0
195 * @param t target time for the integration
196 * (can be set to a value smaller than <code>t0</code> for backward integration)
197 * @param y placeholder where to put the state vector at each successful
198 * step (and hence at the end of integration), can be the same object as y0
199 * @param dYdY0 placeholder where to put the state vector derivative with respect
200 * to the initial state (dy[i]/dy0[j] is in element array dYdY0[i][j]) at each successful
201 * step (and hence at the end of integration)
202 * @param dYdP placeholder where to put the state vector derivative with respect
203 * to the parameters (dy[i]/dp[j] is in element array dYdP[i][j]) at each successful
204 * step (and hence at the end of integration)
205 * @return stop time, will be the same as target time if integration reached its
206 * target, but may be different if some event handler stops it at some point.
207 * @throws IntegratorException if the integrator cannot perform integration
208 * @throws DerivativeException this exception is propagated to the caller if
209 * the underlying user function triggers one
210 */
211 public double integrate(final double t0, final double[] y0, final double[][] dY0dP,
212 final double t, final double[] y,
213 final double[][] dYdY0, final double[][] dYdP)
214 throws DerivativeException, IntegratorException {
215
216 final int n = ode.getDimension();
217 final int k = ode.getParametersDimension();
218 checkDimension(n, y0);
219 checkDimension(n, y);
220 checkDimension(n, dYdY0);
221 checkDimension(n, dYdY0[0]);
222 if (k != 0) {
223 checkDimension(n, dY0dP);
224 checkDimension(k, dY0dP[0]);
225 checkDimension(n, dYdP);
226 checkDimension(k, dYdP[0]);
227 }
228
229 // set up initial state, including partial derivatives
230 // the compound state z contains the raw state y and its derivatives
231 // with respect to initial state y0 and to parameters p
232 // y[i] is stored in z[i]
233 // dy[i]/dy0[j] is stored in z[n + i * n + j]
234 // dy[i]/dp[j] is stored in z[n * (n + 1) + i * k + j]
235 final double[] z = new double[n * (1 + n + k)];
236 System.arraycopy(y0, 0, z, 0, n);
237 for (int i = 0; i < n; ++i) {
238
239 // set diagonal element of dy/dy0 to 1.0 at t = t0
240 z[i * (1 + n) + n] = 1.0;
241
242 // set initial derivatives with respect to parameters
243 System.arraycopy(dY0dP[i], 0, z, n * (n + 1) + i * k, k);
244
245 }
246
247 // integrate the compound state variational equations
248 evaluations = 0;
249 final double stopTime = integrator.integrate(new MappingWrapper(), t0, z, t, z);
250
251 // dispatch the final compound state into the state and partial derivatives arrays
252 dispatchCompoundState(z, y, dYdY0, dYdP);
253
254 return stopTime;
255
256 }
257
258 /** Dispatch a compound state array into state and jacobians arrays.
259 * @param z compound state
260 * @param y raw state array to fill
261 * @param dydy0 jacobian array to fill
262 * @param dydp jacobian array to fill
263 */
264 private static void dispatchCompoundState(final double[] z, final double[] y,
265 final double[][] dydy0, final double[][] dydp) {
266
267 final int n = y.length;
268 final int k = dydp[0].length;
269
270 // state
271 System.arraycopy(z, 0, y, 0, n);
272
273 // jacobian with respect to initial state
274 for (int i = 0; i < n; ++i) {
275 System.arraycopy(z, n * (i + 1), dydy0[i], 0, n);
276 }
277
278 // jacobian with respect to parameters
279 for (int i = 0; i < n; ++i) {
280 System.arraycopy(z, n * (n + 1) + i * k, dydp[i], 0, k);
281 }
282
283 }
284
285 /** Get the current value of the step start time t<sub>i</sub>.
286 * <p>This method can be called during integration (typically by
287 * the object implementing the {@link FirstOrderDifferentialEquations
288 * differential equations} problem) if the value of the current step that
289 * is attempted is needed.</p>
290 * <p>The result is undefined if the method is called outside of
291 * calls to <code>integrate</code>.</p>
292 * @return current value of the step start time t<sub>i</sub>
293 */
294 public double getCurrentStepStart() {
295 return integrator.getCurrentStepStart();
296 }
297
298 /** Get the current signed value of the integration stepsize.
299 * <p>This method can be called during integration (typically by
300 * the object implementing the {@link FirstOrderDifferentialEquations
301 * differential equations} problem) if the signed value of the current stepsize
302 * that is tried is needed.</p>
303 * <p>The result is undefined if the method is called outside of
304 * calls to <code>integrate</code>.</p>
305 * @return current signed value of the stepsize
306 */
307 public double getCurrentSignedStepsize() {
308 return integrator.getCurrentSignedStepsize();
309 }
310
311 /** Set the maximal number of differential equations function evaluations.
312 * <p>The purpose of this method is to avoid infinite loops which can occur
313 * for example when stringent error constraints are set or when lots of
314 * discrete events are triggered, thus leading to many rejected steps.</p>
315 * @param maxEvaluations maximal number of function evaluations (negative
316 * values are silently converted to maximal integer value, thus representing
317 * almost unlimited evaluations)
318 */
319 public void setMaxEvaluations(int maxEvaluations) {
320 this.maxEvaluations = (maxEvaluations < 0) ? Integer.MAX_VALUE : maxEvaluations;
321 }
322
323 /** Get the maximal number of functions evaluations.
324 * @return maximal number of functions evaluations
325 */
326 public int getMaxEvaluations() {
327 return maxEvaluations;
328 }
329
330 /** Get the number of evaluations of the differential equations function.
331 * <p>
332 * The number of evaluations corresponds to the last call to the
333 * <code>integrate</code> method. It is 0 if the method has not been called yet.
334 * </p>
335 * @return number of evaluations of the differential equations function
336 */
337 public int getEvaluations() {
338 return evaluations;
339 }
340
341 /** Check array dimensions.
342 * @param expected expected dimension
343 * @param array (may be null if expected is 0)
344 * @throws IllegalArgumentException if the array dimension does not match the expected one
345 */
346 private void checkDimension(final int expected, final Object array)
347 throws IllegalArgumentException {
348 int arrayDimension = (array == null) ? 0 : Array.getLength(array);
349 if (arrayDimension != expected) {
350 throw MathRuntimeException.createIllegalArgumentException(
351 "dimension mismatch {0} != {1}", arrayDimension, expected);
352 }
353 }
354
355 /** Wrapper class used to map state and jacobians into compound state. */
356 private class MappingWrapper implements FirstOrderDifferentialEquations {
357
358 /** Current state. */
359 private final double[] y;
360
361 /** Time derivative of the current state. */
362 private final double[] yDot;
363
364 /** Derivatives of yDot with respect to state. */
365 private final double[][] dFdY;
366
367 /** Derivatives of yDot with respect to parameters. */
368 private final double[][] dFdP;
369
370 /** Simple constructor.
371 */
372 public MappingWrapper() {
373
374 final int n = ode.getDimension();
375 final int k = ode.getParametersDimension();
376 y = new double[n];
377 yDot = new double[n];
378 dFdY = new double[n][n];
379 dFdP = new double[n][k];
380
381 }
382
383 /** {@inheritDoc} */
384 public int getDimension() {
385 final int n = y.length;
386 final int k = dFdP[0].length;
387 return n * (1 + n + k);
388 }
389
390 /** {@inheritDoc} */
391 public void computeDerivatives(final double t, final double[] z, final double[] zDot)
392 throws DerivativeException {
393
394 final int n = y.length;
395 final int k = dFdP[0].length;
396
397 // compute raw ODE and its jacobians: dy/dt, d[dy/dt]/dy0 and d[dy/dt]/dp
398 System.arraycopy(z, 0, y, 0, n);
399 if (++evaluations > maxEvaluations) {
400 throw new DerivativeException(new MaxEvaluationsExceededException(maxEvaluations));
401 }
402 ode.computeDerivatives(t, y, yDot);
403 ode.computeJacobians(t, y, yDot, dFdY, dFdP);
404
405 // state part of the compound equations
406 System.arraycopy(yDot, 0, zDot, 0, n);
407
408 // variational equations: from d[dy/dt]/dy0 to d[dy/dy0]/dt
409 for (int i = 0; i < n; ++i) {
410 final double[] dFdYi = dFdY[i];
411 for (int j = 0; j < n; ++j) {
412 double s = 0;
413 final int startIndex = n + j;
414 int zIndex = startIndex;
415 for (int l = 0; l < n; ++l) {
416 s += dFdYi[l] * z[zIndex];
417 zIndex += n;
418 }
419 zDot[startIndex + i * n] = s;
420 }
421 }
422
423 // variational equations: from d[dy/dt]/dy0 and d[dy/dt]/dp to d[dy/dp]/dt
424 for (int i = 0; i < n; ++i) {
425 final double[] dFdYi = dFdY[i];
426 final double[] dFdPi = dFdP[i];
427 for (int j = 0; j < k; ++j) {
428 double s = dFdPi[j];
429 final int startIndex = n * (n + 1) + j;
430 int zIndex = startIndex;
431 for (int l = 0; l < n; ++l) {
432 s += dFdYi[l] * z[zIndex];
433 zIndex += k;
434 }
435 zDot[startIndex + i * k] = s;
436 }
437 }
438
439 }
440
441 }
442
443 /** Wrapper class to compute jacobians by finite differences for ODE which do not compute them themselves. */
444 private class FiniteDifferencesWrapper
445 implements ODEWithJacobians {
446
447 /** Raw ODE without jacobians computation. */
448 private final ParameterizedODE ode;
449
450 /** Parameters array (may be null if parameters dimension from original problem is zero) */
451 private final double[] p;
452
453 /** Step sizes to use for computing the jacobian df/dy. */
454 private final double[] hY;
455
456 /** Step sizes to use for computing the jacobian df/dp. */
457 private final double[] hP;
458
459 /** Temporary array for state derivatives used to compute jacobians. */
460 private final double[] tmpDot;
461
462 /** Simple constructor.
463 * @param ode original ODE problem, without jacobians computations
464 * @param p parameters array (may be null if parameters dimension from original problem is zero)
465 * @param hY step sizes to use for computing the jacobian df/dy
466 * @param hP step sizes to use for computing the jacobian df/dp
467 */
468 public FiniteDifferencesWrapper(final ParameterizedODE ode,
469 final double[] p, final double[] hY, final double[] hP) {
470 this.ode = ode;
471 this.p = p.clone();
472 this.hY = hY.clone();
473 this.hP = hP.clone();
474 tmpDot = new double[ode.getDimension()];
475 }
476
477 /** {@inheritDoc} */
478 public int getDimension() {
479 return ode.getDimension();
480 }
481
482 /** {@inheritDoc} */
483 public void computeDerivatives(double t, double[] y, double[] yDot) throws DerivativeException {
484 // this call to computeDerivatives has already been counted,
485 // we must not increment the counter again
486 ode.computeDerivatives(t, y, yDot);
487 }
488
489 /** {@inheritDoc} */
490 public int getParametersDimension() {
491 return ode.getParametersDimension();
492 }
493
494 /** {@inheritDoc} */
495 public void computeJacobians(double t, double[] y, double[] yDot,
496 double[][] dFdY, double[][] dFdP)
497 throws DerivativeException {
498
499 final int n = hY.length;
500 final int k = hP.length;
501
502 evaluations += n + k;
503 if (evaluations > maxEvaluations) {
504 throw new DerivativeException(new MaxEvaluationsExceededException(maxEvaluations));
505 }
506
507 // compute df/dy where f is the ODE and y is the state array
508 for (int j = 0; j < n; ++j) {
509 final double savedYj = y[j];
510 y[j] += hY[j];
511 ode.computeDerivatives(t, y, tmpDot);
512 for (int i = 0; i < n; ++i) {
513 dFdY[i][j] = (tmpDot[i] - yDot[i]) / hY[j];
514 }
515 y[j] = savedYj;
516 }
517
518 // compute df/dp where f is the ODE and p is the parameters array
519 for (int j = 0; j < k; ++j) {
520 ode.setParameter(j, p[j] + hP[j]);
521 ode.computeDerivatives(t, y, tmpDot);
522 for (int i = 0; i < n; ++i) {
523 dFdP[i][j] = (tmpDot[i] - yDot[i]) / hP[j];
524 }
525 ode.setParameter(j, p[j]);
526 }
527
528 }
529
530 }
531
532 /** Wrapper for step handlers. */
533 private static class StepHandlerWrapper implements StepHandler {
534
535 /** Underlying step handler with jacobians. */
536 private final StepHandlerWithJacobians handler;
537
538 /** Dimension of the original ODE. */
539 private final int n;
540
541 /** Number of parameters. */
542 private final int k;
543
544 /** Simple constructor.
545 * @param handler underlying step handler with jacobians
546 * @param n dimension of the original ODE
547 * @param k number of parameters
548 */
549 public StepHandlerWrapper(final StepHandlerWithJacobians handler,
550 final int n, final int k) {
551 this.handler = handler;
552 this.n = n;
553 this.k = k;
554 }
555
556 /** Get the underlying step handler with jacobians.
557 * @return underlying step handler with jacobians
558 */
559 public StepHandlerWithJacobians getHandler() {
560 return handler;
561 }
562
563 /** {@inheritDoc} */
564 public void handleStep(StepInterpolator interpolator, boolean isLast)
565 throws DerivativeException {
566 handler.handleStep(new StepInterpolatorWrapper(interpolator, n, k), isLast);
567 }
568
569 /** {@inheritDoc} */
570 public boolean requiresDenseOutput() {
571 return handler.requiresDenseOutput();
572 }
573
574 /** {@inheritDoc} */
575 public void reset() {
576 handler.reset();
577 }
578
579 }
580
581 /** Wrapper for step interpolators. */
582 private static class StepInterpolatorWrapper
583 implements StepInterpolatorWithJacobians {
584
585 /** Wrapped interpolator. */
586 private StepInterpolator interpolator;
587
588 /** State array. */
589 private double[] y;
590
591 /** Jacobian with respect to initial state dy/dy0. */
592 private double[][] dydy0;
593
594 /** Jacobian with respect to parameters dy/dp. */
595 private double[][] dydp;
596
597 /** Time derivative of the state array. */
598 private double[] yDot;
599
600 /** Time derivative of the sacobian with respect to initial state dy/dy0. */
601 private double[][] dydy0Dot;
602
603 /** Time derivative of the jacobian with respect to parameters dy/dp. */
604 private double[][] dydpDot;
605
606 /** Simple constructor.
607 * <p>This constructor is used only for externalization. It does nothing.</p>
608 */
609 @SuppressWarnings("unused")
610 public StepInterpolatorWrapper() {
611 }
612
613 /** Simple constructor.
614 * @param interpolator wrapped interpolator
615 * @param n dimension of the original ODE
616 * @param k number of parameters
617 */
618 public StepInterpolatorWrapper(final StepInterpolator interpolator,
619 final int n, final int k) {
620 this.interpolator = interpolator;
621 y = new double[n];
622 dydy0 = new double[n][n];
623 dydp = new double[n][k];
624 yDot = new double[n];
625 dydy0Dot = new double[n][n];
626 dydpDot = new double[n][k];
627 }
628
629 /** {@inheritDoc} */
630 public void setInterpolatedTime(double time) {
631 interpolator.setInterpolatedTime(time);
632 }
633
634 /** {@inheritDoc} */
635 public boolean isForward() {
636 return interpolator.isForward();
637 }
638
639 /** {@inheritDoc} */
640 public double getPreviousTime() {
641 return interpolator.getPreviousTime();
642 }
643
644 /** {@inheritDoc} */
645 public double getInterpolatedTime() {
646 return interpolator.getInterpolatedTime();
647 }
648
649 /** {@inheritDoc} */
650 public double[] getInterpolatedY() throws DerivativeException {
651 double[] extendedState = interpolator.getInterpolatedState();
652 System.arraycopy(extendedState, 0, y, 0, y.length);
653 return y;
654 }
655
656 /** {@inheritDoc} */
657 public double[][] getInterpolatedDyDy0() throws DerivativeException {
658 double[] extendedState = interpolator.getInterpolatedState();
659 final int n = y.length;
660 int start = n;
661 for (int i = 0; i < n; ++i) {
662 System.arraycopy(extendedState, start, dydy0[i], 0, n);
663 start += n;
664 }
665 return dydy0;
666 }
667
668 /** {@inheritDoc} */
669 public double[][] getInterpolatedDyDp() throws DerivativeException {
670 double[] extendedState = interpolator.getInterpolatedState();
671 final int n = y.length;
672 final int k = dydp[0].length;
673 int start = n * (n + 1);
674 for (int i = 0; i < n; ++i) {
675 System.arraycopy(extendedState, start, dydp[i], 0, k);
676 start += k;
677 }
678 return dydp;
679 }
680
681 /** {@inheritDoc} */
682 public double[] getInterpolatedYDot() throws DerivativeException {
683 double[] extendedDerivatives = interpolator.getInterpolatedDerivatives();
684 System.arraycopy(extendedDerivatives, 0, yDot, 0, yDot.length);
685 return yDot;
686 }
687
688 /** {@inheritDoc} */
689 public double[][] getInterpolatedDyDy0Dot() throws DerivativeException {
690 double[] extendedDerivatives = interpolator.getInterpolatedDerivatives();
691 final int n = y.length;
692 int start = n;
693 for (int i = 0; i < n; ++i) {
694 System.arraycopy(extendedDerivatives, start, dydy0Dot[i], 0, n);
695 start += n;
696 }
697 return dydy0Dot;
698 }
699
700 /** {@inheritDoc} */
701 public double[][] getInterpolatedDyDpDot() throws DerivativeException {
702 double[] extendedDerivatives = interpolator.getInterpolatedDerivatives();
703 final int n = y.length;
704 final int k = dydpDot[0].length;
705 int start = n * (n + 1);
706 for (int i = 0; i < n; ++i) {
707 System.arraycopy(extendedDerivatives, start, dydpDot[i], 0, k);
708 start += k;
709 }
710 return dydpDot;
711 }
712
713 /** {@inheritDoc} */
714 public double getCurrentTime() {
715 return interpolator.getCurrentTime();
716 }
717
718 /** {@inheritDoc} */
719 public StepInterpolatorWithJacobians copy() throws DerivativeException {
720 final int n = y.length;
721 final int k = dydp[0].length;
722 StepInterpolatorWrapper copied =
723 new StepInterpolatorWrapper(interpolator.copy(), n, k);
724 copyArray(y, copied.y);
725 copyArray(dydy0, copied.dydy0);
726 copyArray(dydp, copied.dydp);
727 copyArray(yDot, copied.yDot);
728 copyArray(dydy0Dot, copied.dydy0Dot);
729 copyArray(dydpDot, copied.dydpDot);
730 return copied;
731 }
732
733 /** {@inheritDoc} */
734 public void writeExternal(ObjectOutput out) throws IOException {
735 out.writeObject(interpolator);
736 out.writeInt(y.length);
737 out.writeInt(dydp[0].length);
738 writeArray(out, y);
739 writeArray(out, dydy0);
740 writeArray(out, dydp);
741 writeArray(out, yDot);
742 writeArray(out, dydy0Dot);
743 writeArray(out, dydpDot);
744 }
745
746 /** {@inheritDoc} */
747 public void readExternal(ObjectInput in) throws IOException, ClassNotFoundException {
748 interpolator = (StepInterpolator) in.readObject();
749 final int n = in.readInt();
750 final int k = in.readInt();
751 y = new double[n];
752 dydy0 = new double[n][n];
753 dydp = new double[n][k];
754 yDot = new double[n];
755 dydy0Dot = new double[n][n];
756 dydpDot = new double[n][k];
757 readArray(in, y);
758 readArray(in, dydy0);
759 readArray(in, dydp);
760 readArray(in, yDot);
761 readArray(in, dydy0Dot);
762 readArray(in, dydpDot);
763 }
764
765 /** Copy an array.
766 * @param src source array
767 * @param dest destination array
768 */
769 private static void copyArray(final double[] src, final double[] dest) {
770 System.arraycopy(src, 0, dest, 0, src.length);
771 }
772
773 /** Copy an array.
774 * @param src source array
775 * @param dest destination array
776 */
777 private static void copyArray(final double[][] src, final double[][] dest) {
778 for (int i = 0; i < src.length; ++i) {
779 copyArray(src[i], dest[i]);
780 }
781 }
782
783 /** Write an array.
784 * @param out output stream
785 * @param array array to write
786 * @exception IOException if array cannot be read
787 */
788 private static void writeArray(final ObjectOutput out, final double[] array)
789 throws IOException {
790 for (int i = 0; i < array.length; ++i) {
791 out.writeDouble(array[i]);
792 }
793 }
794
795 /** Write an array.
796 * @param out output stream
797 * @param array array to write
798 * @exception IOException if array cannot be read
799 */
800 private static void writeArray(final ObjectOutput out, final double[][] array)
801 throws IOException {
802 for (int i = 0; i < array.length; ++i) {
803 writeArray(out, array[i]);
804 }
805 }
806
807 /** Read an array.
808 * @param in input stream
809 * @param array array to read
810 * @exception IOException if array cannot be read
811 */
812 private static void readArray(final ObjectInput in, final double[] array)
813 throws IOException {
814 for (int i = 0; i < array.length; ++i) {
815 array[i] = in.readDouble();
816 }
817 }
818
819 /** Read an array.
820 * @param in input stream
821 * @param array array to read
822 * @exception IOException if array cannot be read
823 */
824 private static void readArray(final ObjectInput in, final double[][] array)
825 throws IOException {
826 for (int i = 0; i < array.length; ++i) {
827 readArray(in, array[i]);
828 }
829 }
830
831 }
832
833 /** Wrapper for event handlers. */
834 private static class EventHandlerWrapper implements EventHandler {
835
836 /** Underlying event handler with jacobians. */
837 private final EventHandlerWithJacobians handler;
838
839 /** State array. */
840 private double[] y;
841
842 /** Jacobian with respect to initial state dy/dy0. */
843 private double[][] dydy0;
844
845 /** Jacobian with respect to parameters dy/dp. */
846 private double[][] dydp;
847
848 /** Simple constructor.
849 * @param handler underlying event handler with jacobians
850 * @param n dimension of the original ODE
851 * @param k number of parameters
852 */
853 public EventHandlerWrapper(final EventHandlerWithJacobians handler,
854 final int n, final int k) {
855 this.handler = handler;
856 y = new double[n];
857 dydy0 = new double[n][n];
858 dydp = new double[n][k];
859 }
860
861 /** Get the underlying event handler with jacobians.
862 * @return underlying event handler with jacobians
863 */
864 public EventHandlerWithJacobians getHandler() {
865 return handler;
866 }
867
868 /** {@inheritDoc} */
869 public int eventOccurred(double t, double[] z, boolean increasing)
870 throws EventException {
871 dispatchCompoundState(z, y, dydy0, dydp);
872 return handler.eventOccurred(t, y, dydy0, dydp, increasing);
873 }
874
875 /** {@inheritDoc} */
876 public double g(double t, double[] z)
877 throws EventException {
878 dispatchCompoundState(z, y, dydy0, dydp);
879 return handler.g(t, y, dydy0, dydp);
880 }
881
882 /** {@inheritDoc} */
883 public void resetState(double t, double[] z)
884 throws EventException {
885 dispatchCompoundState(z, y, dydy0, dydp);
886 handler.resetState(t, y, dydy0, dydp);
887 }
888
889 }
890
891 }