Vector Rosen34( Fun &F , size_t M , const Scalar &ti , const Scalar &tf , const Vector &xi , Vector &e ) { CPPAD_ASSERT_FIRST_CALL_NOT_PARALLEL; // check numeric type specifications CheckNumericType<Scalar>(); // check simple vector class specifications CheckSimpleVector<Scalar, Vector>(); // Parameters for Shampine's Rosenbrock method // are static to avoid recalculation on each call and // do not use Vector to avoid possible memory leak static Scalar a[3] = { Scalar(0), Scalar(1), Scalar(3) / Scalar(5) }; static Scalar b[2 * 2] = { Scalar(1), Scalar(0), Scalar(24) / Scalar(25), Scalar(3) / Scalar(25) }; static Scalar ct[4] = { Scalar(1) / Scalar(2), - Scalar(3) / Scalar(2), Scalar(121) / Scalar(50), Scalar(29) / Scalar(250) }; static Scalar cg[3 * 3] = { - Scalar(4), Scalar(0), Scalar(0), Scalar(186) / Scalar(25), Scalar(6) / Scalar(5), Scalar(0), - Scalar(56) / Scalar(125), - Scalar(27) / Scalar(125), - Scalar(1) / Scalar(5) }; static Scalar d3[3] = { Scalar(97) / Scalar(108), Scalar(11) / Scalar(72), Scalar(25) / Scalar(216) }; static Scalar d4[4] = { Scalar(19) / Scalar(18), Scalar(1) / Scalar(4), Scalar(25) / Scalar(216), Scalar(125) / Scalar(216) }; CPPAD_ASSERT_KNOWN( M >= 1, "Error in Rosen34: the number of steps is less than one" ); CPPAD_ASSERT_KNOWN( e.size() == xi.size(), "Error in Rosen34: size of e not equal to size of xi" ); size_t i, j, k, l, m; // indices size_t n = xi.size(); // number of components in X(t) Scalar ns = Scalar(double(M)); // number of steps as Scalar object Scalar h = (tf - ti) / ns; // step size Scalar zero = Scalar(0); // some constants Scalar one = Scalar(1); Scalar two = Scalar(2); // permutation vectors needed for LU factorization routine CppAD::vector<size_t> ip(n), jp(n); // vectors used to store values returned by F Vector E(n * n), Eg(n), f_t(n); Vector g(n * 3), x3(n), x4(n), xf(n), ftmp(n), xtmp(n), nan_vec(n); // initialize e = 0, nan_vec = nan for(i = 0; i < n; i++) { e[i] = zero; nan_vec[i] = nan(zero); } xf = xi; // initialize solution for(m = 0; m < M; m++) { // time at beginning of this interval Scalar t = ti * (Scalar(int(M - m)) / ns) + tf * (Scalar(int(m)) / ns); // value of x at beginning of this interval x3 = x4 = xf; // evaluate partial derivatives at beginning of this interval F.Ode_ind(t, xf, f_t); F.Ode_dep(t, xf, E); // E = f_x if( hasnan(f_t) || hasnan(E) ) { e = nan_vec; return nan_vec; } // E = I - f_x * h / 2 for(i = 0; i < n; i++) { for(j = 0; j < n; j++) E[i * n + j] = - E[i * n + j] * h / two; E[i * n + i] += one; } // LU factor the matrix E # ifndef NDEBUG int sign = LuFactor(ip, jp, E); # else LuFactor(ip, jp, E); # endif CPPAD_ASSERT_KNOWN( sign != 0, "Error in Rosen34: I - f_x * h / 2 not invertible" ); // loop over integration steps for(k = 0; k < 3; k++) { // set location for next function evaluation xtmp = xf; for(l = 0; l < k; l++) { // loop over previous function evaluations Scalar bkl = b[(k-1)*2 + l]; for(i = 0; i < n; i++) { // loop over elements of x xtmp[i] += bkl * g[i*3 + l] * h; } } // ftmp = F(t + a[k] * h, xtmp) F.Ode(t + a[k] * h, xtmp, ftmp); if( hasnan(ftmp) ) { e = nan_vec; return nan_vec; } // Form Eg for this integration step for(i = 0; i < n; i++) Eg[i] = ftmp[i] + ct[k] * f_t[i] * h; for(l = 0; l < k; l++) { for(i = 0; i < n; i++) Eg[i] += cg[(k-1)*3 + l] * g[i*3 + l]; } // Solve the equation E * g = Eg LuInvert(ip, jp, E, Eg); // save solution and advance x3, x4 for(i = 0; i < n; i++) { g[i*3 + k] = Eg[i]; x3[i] += h * d3[k] * Eg[i]; x4[i] += h * d4[k] * Eg[i]; } } // Form Eg for last update to x4 only for(i = 0; i < n; i++) Eg[i] = ftmp[i] + ct[3] * f_t[i] * h; for(l = 0; l < 3; l++) { for(i = 0; i < n; i++) Eg[i] += cg[2*3 + l] * g[i*3 + l]; } // Solve the equation E * g = Eg LuInvert(ip, jp, E, Eg); // advance x4 and accumulate error bound for(i = 0; i < n; i++) { x4[i] += h * d4[3] * Eg[i]; // cant use abs because cppad.hpp may not be included Scalar diff = x4[i] - x3[i]; if( diff < zero ) e[i] -= diff; else e[i] += diff; } // advance xf for this step using x4 xf = x4; } return xf; }
Vector OdeErrControl( Method &method, const Scalar &ti , const Scalar &tf , const Vector &xi , const Scalar &smin , const Scalar &smax , Scalar &scur , const Vector &eabs , const Scalar &erel , Vector &ef , Vector &maxabs, size_t &nstep ) { // check simple vector class specifications CheckSimpleVector<Scalar, Vector>(); size_t n = size_t(xi.size()); CPPAD_ASSERT_KNOWN( smin <= smax, "Error in OdeErrControl: smin > smax" ); CPPAD_ASSERT_KNOWN( size_t(eabs.size()) == n, "Error in OdeErrControl: size of eabs is not equal to n" ); CPPAD_ASSERT_KNOWN( size_t(maxabs.size()) == n, "Error in OdeErrControl: size of maxabs is not equal to n" ); size_t m = method.order(); CPPAD_ASSERT_KNOWN( m > 1, "Error in OdeErrControl: m is less than or equal one" ); bool ok; bool minimum_step; size_t i; Vector xa(n), xb(n), eb(n), nan_vec(n); // initialization Scalar zero(0); Scalar one(1); Scalar two(2); Scalar three(3); Scalar m1(m-1); Scalar ta = ti; for(i = 0; i < n; i++) { nan_vec[i] = nan(zero); ef[i] = zero; xa[i] = xi[i]; if( zero <= xi[i] ) maxabs[i] = xi[i]; else maxabs[i] = - xi[i]; } nstep = 0; Scalar tb, step, lambda, axbi, a, r, root; while( ! (ta == tf) ) { // start with value suggested by error criteria step = scur; // check maximum if( smax <= step ) step = smax; // check minimum minimum_step = step <= smin; if( minimum_step ) step = smin; // check if near the end if( tf <= ta + step * three / two ) tb = tf; else tb = ta + step; // try using this step size nstep++; method.step(ta, tb, xa, xb, eb); step = tb - ta; // check if this steps error estimate is ok ok = ! (hasnan(xb) || hasnan(eb)); if( (! ok) && minimum_step ) { ef = nan_vec; return nan_vec; } // compute value of lambda for this step lambda = Scalar(10) * scur / step; for(i = 0; i < n; i++) { if( zero <= xb[i] ) axbi = xb[i]; else axbi = - xb[i]; a = eabs[i] + erel * axbi; if( ! (eb[i] == zero) ) { r = ( a / eb[i] ) * step / (tf - ti); root = exp( log(r) / m1 ); if( root <= lambda ) lambda = root; } } if( ok && ( one <= lambda || step <= smin * three / two) ) { // this step is within error limits or // close to the minimum size ta = tb; for(i = 0; i < n; i++) { xa[i] = xb[i]; ef[i] = ef[i] + eb[i]; if( zero <= xb[i] ) axbi = xb[i]; else axbi = - xb[i]; if( axbi > maxabs[i] ) maxabs[i] = axbi; } } if( ! ok ) { // decrease step an see if method will work this time scur = step / two; } else if( ! (ta == tf) ) { // step suggested by the error criteria is not used // on the last step because it may be very small. scur = lambda * step / two; } } return xa; }