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