1 /******************************************************************************
2
3 This source file is part of the Avogadro project.
4
5 Copyright 2010 Eric C. Brown
6
7 This source code is released under the New BSD License, (the "License").
8
9 Unless required by applicable law or agreed to in writing, software
10 distributed under the License is distributed on an "AS IS" BASIS,
11 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 See the License for the specific language governing permissions and
13 limitations under the License.
14
15 ******************************************************************************/
16
17 /*
18 From tam@dragonfly.wri.com Wed Apr 24 01:35:52 1991
19 Return-Path:
20 Date: Wed, 24 Apr 91 03:35:24 CDT
21 From: tam@dragonfly.wri.com
22 To: whitbeck@wheeler.wrc.unr.edu
23 Subject: lsoda.c
24 Cc: augenbau@sparc0.brc.uconn.edu
25
26
27 I'm told by Steve Nichols at Georgia Tech that you are interested in
28 a stiff integrator. Here's a translation of the fortran code LSODA.
29
30 Please note
31 that there is no comment. The interface is the same as the FORTRAN
32 code and I believe the documentation in LSODA will suffice.
33 As usual, a free software comes with no guarantee.
34
35 Hon Wah Tam
36 Wolfram Research, Inc.
37 tam@wri.com
38 */
39
40 #include "qtaimlsodaintegrator.h"
41
42 namespace Avogadro {
43 namespace QtPlugins {
44
QTAIMLSODAIntegrator(QTAIMWavefunctionEvaluator & eval,const qint64 mode)45 QTAIMLSODAIntegrator::QTAIMLSODAIntegrator(QTAIMWavefunctionEvaluator& eval,
46 const qint64 mode)
47 {
48 m_eval = &eval;
49 m_mode = mode;
50
51 m_betaSpheres.empty();
52 m_associatedSphere = 0;
53 }
54
integrate(QVector3D x0y0z0)55 QVector3D QTAIMLSODAIntegrator::integrate(QVector3D x0y0z0)
56 {
57 qreal x0 = x0y0z0.x();
58 qreal y0 = x0y0z0.y();
59 qreal z0 = x0y0z0.z();
60
61 mord[0] = 0;
62 mord[1] = 12;
63 mord[2] = 5;
64
65 sm1[0] = 0.;
66 sm1[1] = 0.5;
67 sm1[2] = 0.575;
68 sm1[3] = 0.55;
69 sm1[4] = 0.45;
70 sm1[5] = 0.35;
71 sm1[6] = 0.25;
72 sm1[7] = 0.2;
73 sm1[8] = 0.15;
74 sm1[9] = 0.1;
75 sm1[10] = 0.075;
76 sm1[11] = 0.05;
77 sm1[12] = 0.025;
78
79 illin = 0;
80 init = 0;
81 ntrep = 0;
82 ixpr = 0;
83 mesflg = 1;
84
85 double rwork1, rwork5, rwork6, rwork7;
86 double atol[4], rtol[4], t, tout, y[4];
87 int iwork1, iwork2, iwork5, iwork6, iwork7, iwork8, iwork9;
88 int neq = 3;
89 int itol, itask, istate, iopt, jt;
90
91 y[1] = x0;
92 y[2] = y0;
93 y[3] = z0;
94
95 double t0;
96 double tf = 100.0;
97 double dt = 0.1;
98
99 m_path.clear();
100 m_path.append(QVector3D(y[1], y[2], y[3]));
101
102 t = 0.0;
103 for (t0 = 0.0; t0 < tf; t0 = t0 + dt) {
104
105 iwork1 = iwork2 = iwork5 = iwork6 = iwork7 = iwork8 = iwork9 = 0;
106 rwork1 = rwork5 = rwork6 = rwork7 = 0.0;
107
108 t = t0;
109 tout = t0 + dt;
110 itol = 2;
111 rtol[0] = 0.0;
112 atol[0] = 0.0;
113 rtol[1] = 0.0;
114 rtol[2] = 0.0;
115 rtol[3] = 0.0;
116 atol[1] = 5.0E-5;
117 atol[2] = 5.0E-5;
118 atol[3] = 5.0E-5;
119 itask = 1;
120 istate = 1;
121 iopt = 0;
122 jt = 2;
123
124 // if( t0 + dt > tf ) qDebug() << "END";
125 // beta spheres
126 if (m_mode == QTAIMLSODAIntegrator::SteepestAscentPathInElectronDensity) {
127 if (m_betaSpheres.length() > 0) {
128 for (qint64 n_ = 0; n_ < m_betaSpheres.length(); ++n_) {
129 Matrix<qreal, 3, 1> a(y[1], y[2], y[3]);
130 Matrix<qreal, 3, 1> b(m_betaSpheres.at(n_).first.x(),
131 m_betaSpheres.at(n_).first.y(),
132 m_betaSpheres.at(n_).first.z());
133
134 qreal distance = QTAIMMathUtilities::distance(a, b);
135
136 if (distance < m_betaSpheres.at(n_).second) {
137 m_status = 0;
138 m_associatedSphere = n_;
139 return QVector3D(m_betaSpheres.at(n_).first.x(),
140 m_betaSpheres.at(n_).first.y(),
141 m_betaSpheres.at(n_).first.z());
142 }
143 }
144 }
145 } // beta spheres
146
147 lsoda(neq, y, &t, tout, itol, rtol, atol, itask, &istate, iopt, jt, iwork1,
148 iwork2, iwork5, iwork6, iwork7, iwork8, iwork9, rwork1, rwork5,
149 rwork6, rwork7);
150
151 m_path.append(QVector3D(y[1], y[2], y[3]));
152
153 // qDebug(" at t= %12.4e y= %14.6e %14.6e %14.6e", t,y[1],y[2],y[3]);
154 if (istate <= 0) {
155 // qDebug("error istate = %d",istate);
156 // qDebug(" at t= %12.4e y= %14.6e %14.6e %14.6e",
157 // t,y[1],y[2],y[3]);
158 return QVector3D(y[1], y[2], y[3]);
159 }
160
161 } // ode step
162
163 return QVector3D(y[1], y[2], y[3]);
164 }
165
f(int neq,double t,double * y,double * ydot)166 void QTAIMLSODAIntegrator::f(int neq, double t, double* y, double* ydot)
167 {
168
169 neq = neq; // suppress warning
170 t = t; // suppress warning
171
172 Matrix<qreal, 3, 1> gradient;
173
174 Matrix<qreal, 3, 4> gH;
175 Matrix<qreal, 3, 1> g;
176 Matrix<qreal, 3, 3> H;
177
178 Matrix<qreal, 3, 1> xyz;
179 xyz << y[1], y[2], y[3];
180
181 if (m_mode == SteepestAscentPathInElectronDensity) {
182 g = m_eval->gradientOfElectronDensity(xyz);
183 } else {
184 if (m_mode == 1 || m_mode == 2 || m_mode == 3 || m_mode == 4) {
185 gH = m_eval->gradientAndHessianOfElectronDensity(xyz);
186 } else {
187 gH = m_eval->gradientAndHessianOfElectronDensityLaplacian(xyz);
188 }
189
190 g(0) = gH(0, 0);
191 g(1) = gH(1, 0);
192 g(2) = gH(2, 0);
193 H(0, 0) = gH(0, 1);
194 H(1, 0) = gH(1, 1);
195 H(2, 0) = gH(2, 1);
196 H(0, 1) = gH(0, 2);
197 H(1, 1) = gH(1, 2);
198 H(2, 1) = gH(2, 2);
199 H(0, 2) = gH(0, 3);
200 H(1, 2) = gH(1, 3);
201 H(2, 2) = gH(2, 3);
202 }
203
204 switch (m_mode) {
205 case SteepestAscentPathInElectronDensity:
206 gradient = g;
207 break;
208 case CMBPMinusThreeGradientInElectronDensity:
209 gradient = QTAIMMathUtilities::minusThreeSignatureLocatorGradient(g, H);
210 break;
211 case CMBPMinusOneGradientInElectronDensity:
212 gradient = QTAIMMathUtilities::minusOneSignatureLocatorGradient(g, H);
213 break;
214 case CMBPPlusOneGradientInElectronDensity:
215 gradient = QTAIMMathUtilities::plusOneSignatureLocatorGradient(g, H);
216 break;
217 case CMBPPlusThreeGradientInElectronDensity:
218 gradient = QTAIMMathUtilities::plusThreeSignatureLocatorGradient(g, H);
219 break;
220 case CMBPMinusThreeGradientInElectronDensityLaplacian:
221 gradient = QTAIMMathUtilities::minusThreeSignatureLocatorGradient(g, H);
222 break;
223 case CMBPMinusOneGradientInElectronDensityLaplacian:
224 gradient = QTAIMMathUtilities::minusOneSignatureLocatorGradient(g, H);
225 break;
226 case CMBPPlusOneGradientInElectronDensityLaplacian:
227 gradient = QTAIMMathUtilities::plusOneSignatureLocatorGradient(g, H);
228 break;
229 case CMBPPlusThreeGradientInElectronDensityLaplacian:
230 gradient = QTAIMMathUtilities::plusThreeSignatureLocatorGradient(g, H);
231 break;
232 default:
233 qDebug() << "Catastrophic: No ODE parameters for this property.";
234 break;
235 }
236
237 qreal normGradient =
238 sqrt(gradient(0) * gradient(0) + gradient(1) * gradient(1) +
239 gradient(2) * gradient(2));
240
241 ydot[1] = gradient(0) / normGradient;
242 ydot[2] = gradient(1) / normGradient;
243 ydot[3] = gradient(2) / normGradient;
244 }
245
daxpy(int n_,double da,double * dx,int incx,double * dy,int incy)246 void QTAIMLSODAIntegrator::daxpy(int n_, double da, double* dx, int incx,
247 double* dy, int incy)
248
249 /*
250 Purpose : To compute
251
252 dy = da * dx + dy
253
254
255 --- Input ---
256
257 n : number of elements in input vector(s)
258 da : double scalar multiplier
259 dx : double vector with n+1 elements, dx[0] is not used
260 incx : storage spacing between elements of dx
261 dy : double vector with n+1 elements, dy[0] is not used
262 incy : storage spacing between elements of dy
263
264
265 --- Output ---
266
267 dy = da * dx + dy, unchanged if n <= 0
268
269
270 For i = 0 to n-1, replace dy[ly+i*incy] with
271 da*dx[lx+i*incx] + dy[ly+i*incy], where lx = 1
272 if incx >= 0, else lx = (-incx)*(n-1)+1 and ly is
273 defined in a similar way using incy.
274
275 */
276
277 {
278 int ix, iy, i, m;
279
280 if (n_ < 0 || da == 0.)
281 return;
282
283 /* Code for nonequal or nonpositive increments. */
284
285 if (incx != incy || incx < 1) {
286 ix = 1;
287 iy = 1;
288 if (incx < 0)
289 ix = (-n_ + 1) * incx + 1;
290 if (incy < 0)
291 iy = (-n_ + 1) * incy + 1;
292 for (i = 1; i <= n_; i++) {
293 dy[iy] = dy[iy] + da * dx[ix];
294 ix = ix + incx;
295 iy = iy + incy;
296 }
297 return;
298 }
299
300 /* Code for both increments equal to 1. */
301
302 /* Clean-up loop so remaining vector length is a multiple of 4. */
303
304 if (incx == 1) {
305 m = n_ % 4;
306 if (m != 0) {
307 for (i = 1; i <= m; i++)
308 dy[i] = dy[i] + da * dx[i];
309 if (n_ < 4)
310 return;
311 }
312 for (i = m + 1; i <= n_; i = i + 4) {
313 dy[i] = dy[i] + da * dx[i];
314 dy[i + 1] = dy[i + 1] + da * dx[i + 1];
315 dy[i + 2] = dy[i + 2] + da * dx[i + 2];
316 dy[i + 3] = dy[i + 3] + da * dx[i + 3];
317 }
318 return;
319 }
320
321 /* Code for equal, positive, nonunit increments. */
322
323 for (i = 1; i <= n_ * incx; i = i + incx)
324 dy[i] = da * dx[i] + dy[i];
325 return;
326 }
327
ddot(int n_,double * dx,int incx,double * dy,int incy)328 double QTAIMLSODAIntegrator::ddot(int n_, double* dx, int incx, double* dy,
329 int incy)
330
331 /*
332 Purpose : Inner product dx . dy
333
334
335 --- Input ---
336
337 n : number of elements in input vector(s)
338 dx : double vector with n+1 elements, dx[0] is not used
339 incx : storage spacing between elements of dx
340 dy : double vector with n+1 elements, dy[0] is not used
341 incy : storage spacing between elements of dy
342
343
344 --- Output ---
345
346 ddot : dot product dx . dy, 0 if n <= 0
347
348
349 ddot = sum for i = 0 to n-1 of
350 dx[lx+i*incx] * dy[ly+i*incy] where lx = 1 if
351 incx >= 0, else lx = (-incx)*(n-1)+1, and ly
352 is defined in a similar way using incy.
353
354 */
355
356 {
357 double dotprod;
358 int ix, iy, i, m;
359
360 dotprod = 0.;
361 if (n_ <= 0)
362 return dotprod;
363
364 /* Code for unequal or nonpositive increments. */
365
366 if (incx != incy || incx < 1) {
367 ix = 1;
368 iy = 1;
369 if (incx < 0)
370 ix = (-n_ + 1) * incx + 1;
371 if (incy < 0)
372 iy = (-n_ + 1) * incy + 1;
373 for (i = 1; i <= n_; i++) {
374 dotprod = dotprod + dx[ix] * dy[iy];
375 ix = ix + incx;
376 iy = iy + incy;
377 }
378 return dotprod;
379 }
380
381 /* Code for both increments equal to 1. */
382
383 /* Clean-up loop so remaining vector length is a multiple of 5. */
384
385 if (incx == 1) {
386 m = n_ % 5;
387 if (m != 0) {
388 for (i = 1; i <= m; i++)
389 dotprod = dotprod + dx[i] * dy[i];
390 if (n_ < 5)
391 return dotprod;
392 }
393 for (i = m + 1; i <= n_; i = i + 5)
394 dotprod = dotprod + dx[i] * dy[i] + dx[i + 1] * dy[i + 1] +
395 dx[i + 2] * dy[i + 2] + dx[i + 3] * dy[i + 3] +
396 dx[i + 4] * dy[i + 4];
397 return dotprod;
398 }
399
400 /* Code for positive equal nonunit increments. */
401
402 for (i = 1; i <= n_ * incx; i = i + incx)
403 dotprod = dotprod + dx[i] * dy[i];
404 return dotprod;
405 }
406
dgefa(double ** a,int n_,int * ipvt_,int * info)407 void QTAIMLSODAIntegrator::dgefa(double** a, int n_, int* ipvt_, int* info)
408
409 /*
410 Purpose : dgefa factors a double matrix by Gaussian elimination.
411
412 dgefa is usually called by dgeco, but it can be called directly
413 with a saving in time if rcond is not needed.
414 (Time for dgeco) = (1+9/n)*(time for dgefa).
415
416 This c version uses algorithm kji rather than the kij in dgefa.f.
417 Note that the fortran version input variable lda is not needed.
418
419
420 On Entry :
421
422 a : double matrix of dimension ( n+1, n+1 ),
423 the 0-th row and column are not used.
424 a is created using NewDoubleMatrix, hence
425 lda is unnecessary.
426 n : the row dimension of a.
427
428 On Return :
429
430 a : a lower triangular matrix and the multipliers
431 which were used to obtain it. The factorization
432 can be written a = L * U where U is a product of
433 permutation and unit upper triangular matrices
434 and L is lower triangular.
435 ipvt : an n+1 integer vector of pivot indices.
436 *info : = 0 normal value,
437 = k if U[k][k] == 0. This is not an error
438 condition for this subroutine, but it does
439 indicate that dgesl or dgedi will divide by
440 zero if called. Use rcond in dgeco for
441 a reliable indication of singularity.
442
443 Notice that the calling program must use &info.
444
445 BLAS : daxpy, dscal, idamax
446 */
447
448 {
449 int j, k, i;
450 double t;
451
452 /* Gaussian elimination with partial pivoting. */
453
454 *info = 0;
455 for (k = 1; k <= n_ - 1; k++) {
456 /*
457 Find j = pivot index. Note that a[k]+k-1 is the address of
458 the 0-th element of the row vector whose 1st element is a[k][k].
459 */
460 j = idamax(n_ - k + 1, a[k] + k - 1, 1) + k - 1;
461 ipvt_[k] = j;
462 /*
463 Zero pivot implies this row already triangularized.
464 */
465 if (a[k][j] == 0.) {
466 *info = k;
467 continue;
468 }
469 /*
470 Interchange if necessary.
471 */
472 if (j != k) {
473 t = a[k][j];
474 a[k][j] = a[k][k];
475 a[k][k] = t;
476 }
477 /*
478 Compute multipliers.
479 */
480 t = -1. / a[k][k];
481 dscal(n_ - k, t, a[k] + k, 1);
482 /*
483 Column elimination with row indexing.
484 */
485 for (i = k + 1; i <= n_; i++) {
486 t = a[i][j];
487 if (j != k) {
488 a[i][j] = a[i][k];
489 a[i][k] = t;
490 }
491 daxpy(n_ - k, t, a[k] + k, 1, a[i] + k, 1);
492 }
493 } /* end k-loop */
494
495 ipvt_[n_] = n_;
496 if (a[n_][n_] == 0.)
497 *info = n_;
498 }
499
dgesl(double ** a,int n_,int * ipvt_,double * b,int job)500 void QTAIMLSODAIntegrator::dgesl(double** a, int n_, int* ipvt_, double* b,
501 int job)
502
503 /*
504 Purpose : dgesl solves the linear system
505 a * x = b or Transpose(a) * x = b
506 using the factors computed by dgeco or degfa.
507
508
509 On Entry :
510
511 a : double matrix of dimension ( n+1, n+1 ),
512 the output from dgeco or dgefa.
513 The 0-th row and column are not used.
514 n : the row dimension of a.
515 ipvt : the pivot vector from degco or dgefa.
516 b : the right hand side vector.
517 job : = 0 to solve a * x = b,
518 = nonzero to solve Transpose(a) * x = b.
519
520
521 On Return :
522
523 b : the solution vector x.
524
525
526 Error Condition :
527
528 A division by zero will occur if the input factor contains
529 a zero on the diagonal. Technically this indicates
530 singularity but it is often caused by improper arguments or
531 improper setting of the pointers of a. It will not occur
532 if the subroutines are called correctly and if dgeco has
533 set rcond > 0 or dgefa has set info = 0.
534
535
536 BLAS : daxpy, ddot
537 */
538
539 {
540 int k, j;
541 double t;
542
543 /*
544 Job = 0, solve a * x = b.
545 */
546 if (job == 0) {
547 /*
548 First solve L * y = b.
549 */
550 for (k = 1; k <= n_; k++) {
551 t = ddot(k - 1, a[k], 1, b, 1);
552 b[k] = (b[k] - t) / a[k][k];
553 }
554 /*
555 Now solve U * x = y.
556 */
557 for (k = n_ - 1; k >= 1; k--) {
558 b[k] = b[k] + ddot(n_ - k, a[k] + k, 1, b + k, 1);
559 j = ipvt_[k];
560 if (j != k) {
561 t = b[j];
562 b[j] = b[k];
563 b[k] = t;
564 }
565 }
566 return;
567 }
568
569 /*
570 Job = nonzero, solve Transpose(a) * x = b.
571
572 First solve Transpose(U) * y = b.
573 */
574 for (k = 1; k <= n_ - 1; k++) {
575 j = ipvt_[k];
576 t = b[j];
577 if (j != k) {
578 b[j] = b[k];
579 b[k] = t;
580 }
581 daxpy(n_ - k, t, a[k] + k, 1, b + k, 1);
582 }
583 /*
584 Now solve Transpose(L) * x = y.
585 */
586 for (k = n_; k >= 1; k--) {
587 b[k] = b[k] / a[k][k];
588 t = -b[k];
589 daxpy(k - 1, t, a[k], 1, b, 1);
590 }
591 }
592
dscal(int n_,double da,double * dx,int incx)593 void QTAIMLSODAIntegrator::dscal(int n_, double da, double* dx, int incx)
594
595 /* Purpose : scalar vector multiplication
596
597 dx = da * dx
598
599
600 --- Input ---
601
602 n : number of elements in input vector
603 da : double scale factor
604 dx : double vector with n+1 elements, dx[0] is not used
605 incx : storage spacing between elements of dx
606
607
608 --- Output ---
609
610 dx = da * dx, unchanged if n <= 0
611
612
613 For i = 0 to n-1, replace dx[1+i*incx] with
614 da * dx[1+i*incx].
615
616 */
617
618 {
619 int m, i;
620
621 if (n_ <= 0)
622 return;
623
624 /* Code for increments not equal to 1. */
625
626 if (incx != 1) {
627 for (i = 1; i <= n_ * incx; i = i + incx)
628 dx[i] = da * dx[i];
629 return;
630 }
631
632 /* Code for increments equal to 1. */
633
634 /* Clean-up loop so remaining vector length is a multiple of 5. */
635
636 m = n_ % 5;
637 if (m != 0) {
638 for (i = 1; i <= m; i++)
639 dx[i] = da * dx[i];
640 if (n_ < 5)
641 return;
642 }
643 for (i = m + 1; i <= n_; i = i + 5) {
644 dx[i] = da * dx[i];
645 dx[i + 1] = da * dx[i + 1];
646 dx[i + 2] = da * dx[i + 2];
647 dx[i + 3] = da * dx[i + 3];
648 dx[i + 4] = da * dx[i + 4];
649 }
650 return;
651 }
652
idamax(int n_,double * dx,int incx)653 int QTAIMLSODAIntegrator::idamax(int n_, double* dx, int incx)
654
655 /* Purpose : Find largest component of double vector dx
656
657
658 --- Input ---
659
660 n : number of elements in input vector
661 dx : double vector with n+1 elements, dx[0] is not used
662 incx : storage spacing between elements of dx
663
664
665 --- Output ---
666
667 idamax : smallest index, 0 if n <= 0
668
669
670 Find smallest index of maximum magnitude of dx.
671 idamax = first i, i=1 to n, to minimize fabs( dx[1-incx+i*incx] ).
672
673 */
674
675 {
676 double dmax, xmag;
677 int i, ii, xindex;
678
679 xindex = 0;
680 if (n_ <= 0)
681 return xindex;
682 xindex = 1;
683 if (n_ <= 1 || incx <= 0)
684 return xindex;
685
686 /* Code for increments not equal to 1. */
687
688 if (incx != 1) {
689 dmax = fabs(dx[1]);
690 ii = 2;
691 for (i = 1 + incx; i <= n_ * incx; i = i + incx) {
692 xmag = fabs(dx[i]);
693 if (xmag > dmax) {
694 xindex = ii;
695 dmax = xmag;
696 }
697 ii++;
698 }
699 return xindex;
700 }
701
702 /* Code for increments equal to 1. */
703
704 dmax = fabs(dx[1]);
705 for (i = 2; i <= n_; i++) {
706 xmag = fabs(dx[i]);
707 if (xmag > dmax) {
708 xindex = i;
709 dmax = xmag;
710 }
711 }
712 return xindex;
713 }
714
715 // lsoda.c
716
717 #define max(a, b) ((a) > (b) ? (a) : (b))
718 #define min(a, b) ((a) < (b) ? (a) : (b))
719
720 #define ETA 2.2204460492503131e-16
721
722 /*
723 static void
724 prja(),
725 solsy(),
726 stoda(),
727 cfode(),
728 ewset(),
729 intdy(),
730 terminate(),
731 terminate2(),
732 successreturn(),
733 scaleh(),
734 correction(),
735 methodswitch(),
736 orderswitch(),
737 endstoda(),
738 resetcoeff(),
739 freevectors(),
740 corfailure();
741
742 static double
743 vmnorm(), bnorm(), fnorm();
744 */
745
746 /*
747 The following are useful statistics.
748
749 hu,
750 h,
751 tn,
752 tolsf,
753 tsw,
754 nst,
755 nfe,
756 nje,
757 nqu,
758 nq,
759 imxer,
760 mused,
761 meth
762 */
763
764 /*
765 Terminate lsoda due to illegal input.
766 */
767
terminate(int * istate)768 void QTAIMLSODAIntegrator::terminate(int* istate)
769 {
770 if (illin == 5) {
771 qDebug("lsoda -- repeated occurrence of illegal input");
772 qDebug(" run aborted.. apparent infinite loop");
773 } else {
774 illin++;
775 *istate = -3;
776 }
777 } /* end terminate */
778
779 /*
780 Terminate lsoda due to various error conditions.
781 */
782
terminate2(double * y,double * t)783 void QTAIMLSODAIntegrator::terminate2(double* y, double* t)
784 {
785 int i;
786
787 yp1 = yh[1];
788 for (i = 1; i <= n; i++)
789 y[i] = yp1[i];
790 *t = tn;
791 illin = 0;
792 freevectors();
793 return;
794
795 } /* end terminate2 */
796
797 /*
798 The following block handles all successful returns from lsoda.
799 If itask != 1, y is loaded from yh and t is set accordingly.
800 *Istate is set to 2, the illegal input counter is zeroed, and the
801 optional outputs are loaded into the work arrays before returning.
802 */
803
successreturn(double * y,double * t,int itask,int ihit,double tcrit,int * istate)804 void QTAIMLSODAIntegrator::successreturn(double* y, double* t, int itask,
805 int ihit, double tcrit, int* istate)
806 {
807 int i;
808
809 yp1 = yh[1];
810 for (i = 1; i <= n; i++)
811 y[i] = yp1[i];
812 *t = tn;
813 if (itask == 4 || itask == 5)
814 if (ihit)
815 *t = tcrit;
816 *istate = 2;
817 illin = 0;
818 freevectors();
819
820 } /* end successreturn */
821
822 /*
823 In this version all memory allocated using malloc is freed upon exit.
824 Therefore *istate = 2 and *istate = 3 will not work.
825 */
826
lsoda(int neq,double * y,double * t,double tout,int itol,double * rtol,double * atol,int itask,int * istate,int iopt,int jt,int iwork1,int iwork2,int iwork5,int iwork6,int iwork7,int iwork8,int iwork9,double rwork1,double rwork5,double rwork6,double rwork7)827 void QTAIMLSODAIntegrator::lsoda(int neq, double* y, double* t, double tout,
828 int itol, double* rtol, double* atol,
829 int itask, int* istate, int iopt, int jt,
830 int iwork1, int iwork2, int iwork5, int iwork6,
831 int iwork7, int iwork8, int iwork9,
832 double rwork1, double rwork5, double rwork6,
833 double rwork7)
834 /*
835 If the user does not supply any of these values, the calling program
836 should initialize those untouched working variables to zero.
837
838 ml = iwork1
839 mu = iwork2
840 ixpr = iwork5
841 mxstep = iwork6
842 mxhnil = iwork7
843 mxordn = iwork8
844 mxords = iwork9
845
846 tcrit = rwork1
847 h0 = rwork5
848 hmax = rwork6
849 hmin = rwork7
850 */
851 {
852 int mxstp0 = 500, mxhnl0 = -1 /* 10 */;
853
854 int i, iflag, lenyh;
855 double atoli, ayi, big, hmax, hmx, rh, rtoli, tdist, tnext, tol, tolsf, tp,
856 size, sum, w0;
857
858 /*
859 Block a.
860 This code block is executed on every call.
861 It tests *istate and itask for legality and branches appropriately.
862 If *istate > 1 but the flag init shows that initialization has not
863 yet been done, an error return occurs.
864 If *istate = 1 and tout = t, return immediately.
865 */
866
867 if (*istate < 1 || *istate > 3) {
868 qDebug("lsoda -- illegal istate = %d", *istate);
869 terminate(istate);
870 return;
871 }
872 if (itask < 1 || itask > 5) {
873 qDebug("lsoda -- illegal itask = %d", itask);
874 terminate(istate);
875 return;
876 }
877 if (init == 0 && (*istate == 2 || *istate == 3)) {
878 qDebug("lsoda -- istate > 1 but lsoda not initialized");
879 terminate(istate);
880 return;
881 }
882 if (*istate == 1) {
883 init = 0;
884 if (tout == *t) {
885 ntrep++;
886 if (ntrep < 5)
887 return;
888 qDebug("lsoda -- repeated calls with istate = 1 and tout = t");
889 qDebug(" run aborted.. apparent infinite loop");
890 return;
891 }
892 }
893
894 /*
895 Block b.
896 The next code block is executed for the initial call ( *istate = 1 ),
897 or for a continuation call with parameter changes ( *istate = 3 ).
898 It contains checking of all inputs and various initializations.
899
900 First check legality of the non-optional inputs neq, itol, iopt,
901 jt, ml, and mu.
902 */
903 double h0(0.0);
904 if (*istate == 1 || *istate == 3) {
905 ntrep = 0;
906 if (neq <= 0) {
907 qDebug("lsoda -- neq = %d is less than 1", neq);
908 terminate(istate);
909 return;
910 }
911 if (*istate == 3 && neq > n) {
912 qDebug("lsoda -- istate = 3 and neq increased");
913 terminate(istate);
914 return;
915 }
916 n = neq;
917 if (itol < 1 || itol > 4) {
918 qDebug("lsoda -- itol = %d illegal", itol);
919 terminate(istate);
920 return;
921 }
922 if (iopt < 0 || iopt > 1) {
923 qDebug("lsoda -- iopt = %d illegal", iopt);
924 terminate(istate);
925 return;
926 }
927 if (jt == 3 || jt < 1 || jt > 5) {
928 qDebug("lsoda -- jt = %d illegal", jt);
929 terminate(istate);
930 return;
931 }
932 jtyp = jt;
933 if (jt > 2) {
934 ml = iwork1;
935 mu = iwork2;
936 if (ml < 0 || ml >= n) {
937 qDebug("lsoda -- ml = %d not between 1 and neq", ml);
938 terminate(istate);
939 return;
940 }
941 if (mu < 0 || mu >= n) {
942 qDebug("lsoda -- mu = %d not between 1 and neq", mu);
943 terminate(istate);
944 return;
945 }
946 }
947
948 /* Next process and check the optional inpus. */
949
950 /* Default options. */
951 if (iopt == 0) {
952 ixpr = 0;
953 mxstep = mxstp0;
954 mxhnil = mxhnl0;
955 hmxi = 0.;
956 hmin = 0.;
957 if (*istate == 1) {
958 h0 = 0.;
959 mxordn = mord[1];
960 mxords = mord[2];
961 }
962 } /* end if ( iopt == 0 ) */
963
964 /* Optional inputs. */
965
966 else { /* if ( iopt = 1 ) */
967 ixpr = iwork5;
968 if (ixpr < 0 || ixpr > 1) {
969 qDebug("lsoda -- ixpr = %d is illegal", ixpr);
970 terminate(istate);
971 return;
972 }
973 mxstep = iwork6;
974 if (mxstep < 0) {
975 qDebug("lsoda -- mxstep < 0");
976 terminate(istate);
977 return;
978 }
979 if (mxstep == 0)
980 mxstep = mxstp0;
981 mxhnil = iwork7;
982 if (mxhnil < 0) {
983 qDebug("lsoda -- mxhnil < 0");
984 terminate(istate);
985 return;
986 }
987 if (*istate == 1) {
988 h0 = rwork5;
989 mxordn = iwork8;
990 if (mxordn < 0) {
991 qDebug("lsoda -- mxordn = %d is less than 0", mxordn);
992 terminate(istate);
993 return;
994 }
995 if (mxordn == 0)
996 mxordn = 100;
997 mxordn = min(mxordn, mord[1]);
998 mxords = iwork9;
999 if (mxords < 0) {
1000 qDebug("lsoda -- mxords = %d is less than 0", mxords);
1001 terminate(istate);
1002 return;
1003 }
1004 if (mxords == 0)
1005 mxords = 100;
1006 mxords = min(mxords, mord[2]);
1007 if ((tout - *t) * h0 < 0.) {
1008 qDebug("lsoda -- tout = %g behind t = %g", tout, *t);
1009 qDebug(" integration direction is given by %g", h0);
1010 terminate(istate);
1011 return;
1012 }
1013 } /* end if ( *istate == 1 ) */
1014 hmax = rwork6;
1015 if (hmax < 0.) {
1016 qDebug("lsoda -- hmax < 0.");
1017 terminate(istate);
1018 return;
1019 }
1020 hmxi = 0.;
1021 if (hmax > 0)
1022 hmxi = 1. / hmax;
1023 hmin = rwork7;
1024 if (hmin < 0.) {
1025 qDebug("lsoda -- hmin < 0.");
1026 terminate(istate);
1027 return;
1028 }
1029 } /* end else */ /* end iopt = 1 */
1030 } /* end if ( *istate == 1 || *istate == 3 ) */
1031 /*
1032 If *istate = 1, meth is initialized to 1.
1033
1034 Also allocate memory for yh, wm, ewt, savf, acor, ipvt.
1035 */
1036 if (*istate == 1) {
1037 /*
1038 If memory were not freed, *istate = 3 need not reallocate memory.
1039 Hence this section is not executed by *istate = 3.
1040 */
1041 sqrteta = sqrt(ETA);
1042 meth = 1;
1043 nyh = n;
1044 lenyh = 1 + max(mxordn, mxords);
1045
1046 m_lenyh = lenyh;
1047 m_nyh = nyh;
1048
1049 yh = (double**)malloc((1 + lenyh) * sizeof(*yh));
1050 if (yh == nullptr) {
1051 qDebug("lsoda -- insufficient memory for your problem");
1052 terminate(istate);
1053 return;
1054 }
1055 for (i = 1; i <= lenyh; i++)
1056 yh[i] = (double*)malloc((1 + nyh) * sizeof(double));
1057
1058 wm = (double**)malloc((1 + nyh) * sizeof(*wm));
1059 if (wm == nullptr) {
1060 free(yh);
1061 qDebug("lsoda -- insufficient memory for your problem");
1062 terminate(istate);
1063 return;
1064 }
1065 for (i = 1; i <= nyh; i++)
1066 wm[i] = (double*)malloc((1 + nyh) * sizeof(double));
1067
1068 ewt = (double*)malloc((1 + nyh) * sizeof(double));
1069 if (ewt == nullptr) {
1070 free(yh);
1071 free(wm);
1072 qDebug("lsoda -- insufficient memory for your problem");
1073 terminate(istate);
1074 return;
1075 }
1076
1077 savf = (double*)malloc((1 + nyh) * sizeof(double));
1078 if (savf == nullptr) {
1079 free(yh);
1080 free(wm);
1081 free(ewt);
1082 qDebug("lsoda -- insufficient memory for your problem");
1083 terminate(istate);
1084 return;
1085 }
1086
1087 acor = (double*)malloc((1 + nyh) * sizeof(double));
1088 if (acor == nullptr) {
1089 free(yh);
1090 free(wm);
1091 free(ewt);
1092 free(savf);
1093 qDebug("lsoda -- insufficient memory for your problem");
1094 terminate(istate);
1095 return;
1096 }
1097
1098 ipvt = (int*)malloc((1 + nyh) * sizeof(int));
1099 if (ipvt == nullptr) {
1100 free(yh);
1101 free(wm);
1102 free(ewt);
1103 free(savf);
1104 free(acor);
1105 qDebug("lsoda -- insufficient memory for your problem");
1106 terminate(istate);
1107 return;
1108 }
1109 }
1110 /*
1111 Check rtol and atol for legality.
1112 */
1113 if (*istate == 1 || *istate == 3) {
1114 rtoli = rtol[1];
1115 atoli = atol[1];
1116 for (i = 1; i <= n; i++) {
1117 if (itol >= 3)
1118 rtoli = rtol[i];
1119 if (itol == 2 || itol == 4)
1120 atoli = atol[i];
1121 if (rtoli < 0.) {
1122 qDebug("lsoda -- rtol = %g is less than 0.", rtoli);
1123 terminate(istate);
1124 freevectors();
1125 return;
1126 }
1127 if (atoli < 0.) {
1128 qDebug("lsoda -- atol = %g is less than 0.", atoli);
1129 terminate(istate);
1130 freevectors();
1131 return;
1132 }
1133 } /* end for */
1134 } /* end if ( *istate == 1 || *istate == 3 ) */
1135 /*
1136 If *istate = 3, set flag to signal parameter changes to stoda.
1137 */
1138 if (*istate == 3) {
1139 jstart = -1;
1140 }
1141 /*
1142 Block c.
1143 The next block is for the initial call only ( *istate = 1 ).
1144 It contains all remaining initializations, the initial call to f,
1145 and the calculation of the initial step size.
1146 The error weights in ewt are inverted after being loaded.
1147 */
1148 int tcrit(0);
1149 if (*istate == 1) {
1150 tn = *t;
1151 tsw = *t;
1152 maxord = mxordn;
1153 if (itask == 4 || itask == 5) {
1154 tcrit = rwork1;
1155 if ((tcrit - tout) * (tout - *t) < 0.) {
1156 qDebug("lsoda -- itask = 4 or 5 and tcrit behind tout");
1157 terminate(istate);
1158 freevectors();
1159 return;
1160 }
1161 if (h0 != 0. && (*t + h0 - tcrit) * h0 > 0.)
1162 h0 = tcrit - *t;
1163 }
1164 jstart = 0;
1165 nhnil = 0;
1166 nst = 0;
1167 nje = 0;
1168 nslast = 0;
1169 hu = 0.;
1170 nqu = 0;
1171 mused = 0;
1172 miter = 0;
1173 ccmax = 0.3;
1174 maxcor = 3;
1175 msbp = 20;
1176 mxncf = 10;
1177 /*
1178 Initial call to f.
1179 */
1180 f(neq, *t, y, yh[2]);
1181 nfe = 1;
1182 /*
1183 Load the initial value vector in yh.
1184 */
1185 yp1 = yh[1];
1186 for (i = 1; i <= n; i++)
1187 yp1[i] = y[i];
1188 /*
1189 Load and invert the ewt array. ( h is temporarily set to 1. )
1190 */
1191 nq = 1;
1192 h = 1.;
1193 ewset(itol, rtol, atol, y);
1194 for (i = 1; i <= n; i++) {
1195 if (ewt[i] <= 0.) {
1196 qDebug("lsoda -- ewt[%d] = %g <= 0.", i, ewt[i]);
1197 // ECB Comment out because wrong number of arguments.
1198 // terminate( y, yh, t, tn );
1199 return;
1200 }
1201 ewt[i] = 1. / ewt[i];
1202 }
1203
1204 /*
1205 The coding below computes the step size, h0, to be attempted on the
1206 first step, unless the user has supplied a value for this.
1207 First check that tout - *t differs significantly from zero.
1208 A scalar tolerance quantity tol is computed, as max(rtol[i])
1209 if this is positive, or max(atol[i]/fabs(y[i])) otherwise, adjusted
1210 so as to be between 100*ETA and 0.001.
1211 Then the computed value h0 is given by
1212
1213 h0^(-2) = 1. / ( tol * w0^2 ) + tol * ( norm(f) )^2
1214
1215 where w0 = max( fabs(*t), fabs(tout) ),
1216 f = the initial value of the vector f(t,y), and
1217 norm() = the weighted vector norm used throughout, given by
1218 the vmnorm function routine, and weighted by the
1219 tolerances initially loaded into the ewt array.
1220
1221 The sign of h0 is inferred from the initial values of tout and *t.
1222 fabs(h0) is made < fabs(tout-*t) in any case.
1223 */
1224 if (h0 == 0.) {
1225 tdist = fabs(tout - *t);
1226 w0 = max(fabs(*t), fabs(tout));
1227 if (tdist < 2. * ETA * w0) {
1228 qDebug("lsoda -- tout too close to t to start integration ");
1229 terminate(istate);
1230 freevectors();
1231 return;
1232 }
1233 tol = rtol[1];
1234 if (itol > 2) {
1235 for (i = 2; i <= n; i++)
1236 tol = max(tol, rtol[i]);
1237 }
1238 if (tol <= 0.) {
1239 atoli = atol[1];
1240 for (i = 1; i <= n; i++) {
1241 if (itol == 2 || itol == 4)
1242 atoli = atol[i];
1243 ayi = fabs(y[i]);
1244 if (ayi != 0.)
1245 tol = max(tol, atoli / ayi);
1246 }
1247 }
1248 tol = max(tol, 100. * ETA);
1249 tol = min(tol, 0.001);
1250 sum = vmnorm(n, yh[2], ewt);
1251 sum = 1. / (tol * w0 * w0) + tol * sum * sum;
1252 h0 = 1. / sqrt(sum);
1253 h0 = min(h0, tdist);
1254 h0 = h0 * ((tout - *t >= 0.) ? 1. : -1.);
1255 } /* end if ( h0 == 0. ) */
1256 /*
1257 Adjust h0 if necessary to meet hmax bound.
1258 */
1259 rh = fabs(h0) * hmxi;
1260 if (rh > 1.)
1261 h0 /= rh;
1262 /*
1263 Load h with h0 and scale yh[2] by h0.
1264 */
1265 h = h0;
1266 yp1 = yh[2];
1267 for (i = 1; i <= n; i++)
1268 yp1[i] *= h0;
1269 } /* if ( *istate == 1 ) */
1270 /*
1271 Block d.
1272 The next code block is for continuation calls only ( *istate = 2 or 3 )
1273 and is to check stop conditions before taking a step.
1274 */
1275 int ihit(0);
1276 if (*istate == 2 || *istate == 3) {
1277 nslast = nst;
1278 switch (itask) {
1279 case 1:
1280 if ((tn - tout) * h >= 0.) {
1281 intdy(tout, 0, y, &iflag);
1282 if (iflag != 0) {
1283 qDebug("lsoda -- trouble from intdy, itask = %d, tout = %g", itask,
1284 tout);
1285 terminate(istate);
1286 freevectors();
1287 return;
1288 }
1289 *t = tout;
1290 *istate = 2;
1291 illin = 0;
1292 freevectors();
1293 return;
1294 }
1295 break;
1296 case 2:
1297 break;
1298 case 3:
1299 tp = tn - hu * (1. + 100. * ETA);
1300 if ((tp - tout) * h > 0.) {
1301 qDebug("lsoda -- itask = %d and tout behind tcur - hu", itask);
1302 terminate(istate);
1303 freevectors();
1304 return;
1305 }
1306 if ((tn - tout) * h < 0.)
1307 break;
1308 successreturn(y, t, itask, ihit, tcrit, istate);
1309 return;
1310 case 4:
1311 tcrit = rwork1;
1312 if ((tn - tcrit) * h > 0.) {
1313 qDebug("lsoda -- itask = 4 or 5 and tcrit behind tcur");
1314 terminate(istate);
1315 freevectors();
1316 return;
1317 }
1318 if ((tcrit - tout) * h < 0.) {
1319 qDebug("lsoda -- itask = 4 or 5 and tcrit behind tout");
1320 terminate(istate);
1321 freevectors();
1322 return;
1323 }
1324 if ((tn - tout) * h >= 0.) {
1325 intdy(tout, 0, y, &iflag);
1326 if (iflag != 0) {
1327 qDebug("lsoda -- trouble from intdy, itask = %d, tout = %g", itask,
1328 tout);
1329 terminate(istate);
1330 freevectors();
1331 return;
1332 }
1333 *t = tout;
1334 *istate = 2;
1335 illin = 0;
1336 freevectors();
1337 return;
1338 }
1339 case 5:
1340 if (itask == 5) {
1341 tcrit = rwork1;
1342 if ((tn - tcrit) * h > 0.) {
1343 qDebug("lsoda -- itask = 4 or 5 and tcrit behind tcur");
1344 terminate(istate);
1345 freevectors();
1346 return;
1347 }
1348 }
1349 hmx = fabs(tn) + fabs(h);
1350 ihit = fabs(tn - tcrit) <= (100. * ETA * hmx);
1351 if (ihit) {
1352 *t = tcrit;
1353 successreturn(y, t, itask, ihit, tcrit, istate);
1354 return;
1355 }
1356 tnext = tn + h * (1. + 4. * ETA);
1357 if ((tnext - tcrit) * h <= 0.)
1358 break;
1359 h = (tcrit - tn) * (1. - 4. * ETA);
1360 if (*istate == 2)
1361 jstart = -2;
1362 break;
1363 } /* end switch */
1364 } /* end if ( *istate == 2 || *istate == 3 ) */
1365
1366 /*
1367 Block e.
1368 The next block is normally executed for all calls and contains
1369 the call to the one-step core integrator stoda.
1370
1371 This is a looping point for the integration steps.
1372
1373 First check for too many steps being taken, update ewt ( if not at
1374 start of problem). Check for too much accuracy being requested, and
1375 check for h below the roundoff level in *t.
1376 */
1377 while (1) {
1378 if (*istate != 1 || nst != 0) {
1379 if ((nst - nslast) >= mxstep) {
1380 // qDebug( "lsoda -- %d steps taken before reaching tout",
1381 // mxstep );
1382 *istate = -1;
1383 terminate2(y, t);
1384 return;
1385 }
1386 ewset(itol, rtol, atol, yh[1]);
1387 for (i = 1; i <= n; i++) {
1388 if (ewt[i] <= 0.) {
1389 qDebug("lsoda -- ewt[%d] = %g <= 0.", i, ewt[i]);
1390 *istate = -6;
1391 terminate2(y, t);
1392 return;
1393 }
1394 ewt[i] = 1. / ewt[i];
1395 }
1396 }
1397 tolsf = ETA * vmnorm(n, yh[1], ewt);
1398 if (tolsf > 0.01) {
1399 tolsf = tolsf * 200.;
1400 if (nst == 0) {
1401 qDebug("lsoda -- at start of problem, too much accuracy");
1402 qDebug(" requested for precision of machine,");
1403 qDebug(" suggested scaling factor = %g", tolsf);
1404 terminate(istate);
1405 freevectors();
1406 return;
1407 }
1408 qDebug("lsoda -- at t = %g, too much accuracy requested", *t);
1409 qDebug(" for precision of machine, suggested");
1410 qDebug(" scaling factor = %g", tolsf);
1411 *istate = -2;
1412 terminate2(y, t);
1413 return;
1414 }
1415 if ((tn + h) == tn) {
1416 nhnil++;
1417 if (nhnil <= mxhnil) {
1418 qDebug("lsoda -- warning..internal t = %g and h = %g are", tn, h);
1419 qDebug(" such that in the machine, t + h = t on the next step");
1420 qDebug(" solver will continue anyway.");
1421 if (nhnil == mxhnil) {
1422 qDebug("lsoda -- above warning has been issued %d times,", nhnil);
1423 qDebug(" it will not be issued again for this problem");
1424 }
1425 }
1426 }
1427
1428 /*
1429 Call stoda
1430 */
1431 stoda(neq, y);
1432
1433 /*
1434 qDebug( "meth= %d, order= %d, nfe= %d, nje= %d",
1435 meth, nq, nfe, nje );
1436 qDebug( "t= %20.15e, h= %20.15e, nst=%d", tn, h, nst );
1437 qDebug( "y= %20.15e, %20.15e, %20.15e",
1438 yh[1][1], yh[1][2], yh[1][3] );
1439 */
1440
1441 if (kflag == 0) {
1442 /*
1443 Block f.
1444 The following block handles the case of a successful return from the
1445 core integrator ( kflag = 0 ).
1446 If a method switch was just made, record tsw, reset maxord,
1447 set jstart to -1 to signal stoda to complete the switch,
1448 and do extra printing of data if ixpr = 1.
1449 Then, in any case, check for stop conditions.
1450 */
1451 init = 1;
1452 if (meth != mused) {
1453 tsw = tn;
1454 maxord = mxordn;
1455 if (meth == 2)
1456 maxord = mxords;
1457 jstart = -1;
1458 if (ixpr) {
1459 if (meth == 2)
1460 qDebug() << "lsoda -- a switch to the stiff method has occurred";
1461 if (meth == 1)
1462 qDebug() << "lsoda -- a switch to the nonstiff method has occurred";
1463 qDebug() << " at t = " << tn
1464 << ", tentative step size h = " << h
1465 << ", step nst = " << nst;
1466 }
1467 } /* end if ( meth != mused ) */
1468 /*
1469 itask = 1.
1470 If tout has been reached, interpolate.
1471 */
1472 if (itask == 1) {
1473 if ((tn - tout) * h < 0.)
1474 continue;
1475 intdy(tout, 0, y, &iflag);
1476 *t = tout;
1477 *istate = 2;
1478 illin = 0;
1479 freevectors();
1480 return;
1481 }
1482 /*
1483 itask = 2.
1484 */
1485 if (itask == 2) {
1486 successreturn(y, t, itask, ihit, tcrit, istate);
1487 return;
1488 }
1489 /*
1490 itask = 3.
1491 Jump to exit if tout was reached.
1492 */
1493 if (itask == 3) {
1494 if ((tn - tout) * h >= 0.) {
1495 successreturn(y, t, itask, ihit, tcrit, istate);
1496 return;
1497 }
1498 continue;
1499 }
1500 /*
1501 itask = 4.
1502 See if tout or tcrit was reached. Adjust h if necessary.
1503 */
1504 if (itask == 4) {
1505 if ((tn - tout) * h >= 0.) {
1506 intdy(tout, 0, y, &iflag);
1507 *t = tout;
1508 *istate = 2;
1509 illin = 0;
1510 freevectors();
1511 return;
1512 } else {
1513 hmx = fabs(tn) + fabs(h);
1514 ihit = fabs(tn - tcrit) <= (100. * ETA * hmx);
1515 if (ihit) {
1516 successreturn(y, t, itask, ihit, tcrit, istate);
1517 return;
1518 }
1519 tnext = tn + h * (1. + 4. * ETA);
1520 if ((tnext - tcrit) * h <= 0.)
1521 continue;
1522 h = (tcrit - tn) * (1. - 4. * ETA);
1523 jstart = -2;
1524 continue;
1525 }
1526 } /* end if ( itask == 4 ) */
1527 /*
1528 itask = 5.
1529 See if tcrit was reached and jump to exit.
1530 */
1531 if (itask == 5) {
1532 hmx = fabs(tn) + fabs(h);
1533 ihit = fabs(tn - tcrit) <= (100. * ETA * hmx);
1534 successreturn(y, t, itask, ihit, tcrit, istate);
1535 return;
1536 }
1537 } /* end if ( kflag == 0 ) */
1538 /*
1539 kflag = -1, error test failed repeatedly or with fabs(h) = hmin.
1540 kflag = -2, convergence failed repeatedly or with fabs(h) = hmin.
1541 */
1542 if (kflag == -1 || kflag == -2) {
1543 qDebug("lsoda -- at t = %g and step size h = %g, the", tn, h);
1544 if (kflag == -1) {
1545 qDebug(" error test failed repeatedly or");
1546 qDebug(" with fabs(h) = hmin");
1547 *istate = -4;
1548 }
1549 if (kflag == -2) {
1550 qDebug(" corrector convergence failed repeatedly or");
1551 qDebug(" with fabs(h) = hmin");
1552 *istate = -5;
1553 }
1554 big = 0.;
1555 imxer = 1;
1556 for (i = 1; i <= n; i++) {
1557 size = fabs(acor[i]) * ewt[i];
1558 if (big < size) {
1559 big = size;
1560 imxer = i;
1561 }
1562 }
1563 terminate2(y, t);
1564 return;
1565 } /* end if ( kflag == -1 || kflag == -2 ) */
1566 } /* end while */
1567
1568 } /* end lsoda */
1569
stoda(int neq,double * y)1570 void QTAIMLSODAIntegrator::stoda(int neq, double* y)
1571 {
1572 int corflag, orderflag;
1573 int i, i1, j, m, ncf;
1574 double del, delp, dup, exup, r, rh, rhup, told;
1575 double pdh, pnorm;
1576
1577 /*
1578 stoda performs one step of the integration of an initial value
1579 problem for a system of ordinary differential equations.
1580 Note.. stoda is independent of the value of the iteration method
1581 indicator miter, when this is != 0, and hence is independent
1582 of the type of chord method used, or the Jacobian structure.
1583 Communication with stoda is done with the following variables:
1584
1585 jstart = an integer used for input only, with the following
1586 values and meanings:
1587
1588 0 perform the first step,
1589 > 0 take a new step continuing from the last,
1590 -1 take the next step with a new value of h,
1591 n, meth, miter, and/or matrix parameters.
1592 -2 take the next step with a new value of h,
1593 but with other inputs unchanged.
1594
1595 kflag = a completion code with the following meanings:
1596
1597 0 the step was successful,
1598 -1 the requested error could not be achieved,
1599 -2 corrector convergence could not be achieved,
1600 -3 fatal error in prja or solsy.
1601
1602 miter = corrector iteration method:
1603
1604 0 functional iteration,
1605 >0 a chord method corresponding to jacobian type jt.
1606
1607 */
1608 kflag = 0;
1609 told = tn;
1610 ncf = 0;
1611 ierpj = 0;
1612 iersl = 0;
1613 jcur = 0;
1614 delp = 0.;
1615
1616 /*
1617 On the first call, the order is set to 1, and other variables are
1618 initialized. rmax is the maximum ratio by which h can be increased
1619 in a single step. It is initially 1.e4 to compensate for the small
1620 initial h, but then is normally equal to 10. If a filure occurs
1621 (in corrector convergence or error test), rmax is set at 2 for
1622 the next increase.
1623 cfode is called to get the needed coefficients for both methods.
1624 */
1625 if (jstart == 0) {
1626 lmax = maxord + 1;
1627 nq = 1;
1628 l = 2;
1629 ialth = 2;
1630 rmax = 10000.;
1631 rc = 0.;
1632 el0 = 1.;
1633 crate = 0.7;
1634 hold = h;
1635 nslp = 0;
1636 ipup = miter;
1637 /*
1638 Initialize switching parameters. meth = 1 is assumed initially.
1639 */
1640 icount = 20;
1641 irflag = 0;
1642 pdest = 0.;
1643 pdlast = 0.;
1644 ratio = 5.;
1645 cfode(2);
1646 for (i = 1; i <= 5; i++)
1647 cm2[i] = tesco[i][2] * elco[i][i + 1];
1648 cfode(1);
1649 for (i = 1; i <= 12; i++)
1650 cm1[i] = tesco[i][2] * elco[i][i + 1];
1651 resetcoeff();
1652 } /* end if ( jstart == 0 ) */
1653 /*
1654 The following block handles preliminaries needed when jstart = -1.
1655 ipup is set to miter to force a matrix update.
1656 If an order increase is about to be considered ( ialth = 1 ),
1657 ialth is reset to 2 to postpone consideration one more step.
1658 If the caller has changed meth, cfode is called to reset
1659 the coefficients of the method.
1660 If h is to be changed, yh must be rescaled.
1661 If h or meth is being changed, ialth is reset to l = nq + 1
1662 to prevent further changes in h for that many steps.
1663 */
1664 if (jstart == -1) {
1665 ipup = miter;
1666 lmax = maxord + 1;
1667 if (ialth == 1)
1668 ialth = 2;
1669 if (meth != mused) {
1670 cfode(meth);
1671 ialth = l;
1672 resetcoeff();
1673 }
1674 if (h != hold) {
1675 rh = h / hold;
1676 h = hold;
1677 scaleh(&rh, &pdh);
1678 }
1679 } /* if ( jstart == -1 ) */
1680
1681 if (jstart == -2) {
1682 if (h != hold) {
1683 rh = h / hold;
1684 h = hold;
1685 scaleh(&rh, &pdh);
1686 }
1687 } /* if ( jstart == -2 ) */
1688
1689 /*
1690 Prediction.
1691 This section computes the predicted values by effectively
1692 multiplying the yh array by the pascal triangle matrix.
1693 rc is the ratio of new to old values of the coefficient h * el[1].
1694 When rc differs from 1 by more than ccmax, ipup is set to miter
1695 to force pjac to be called, if a jacobian is involved.
1696 In any case, prja is called at least every msbp steps.
1697 */
1698
1699 while (1) {
1700 while (1) {
1701 if (fabs(rc - 1.) > ccmax)
1702 ipup = miter;
1703 if (nst >= nslp + msbp)
1704 ipup = miter;
1705 tn += h;
1706 for (j = nq; j >= 1; j--)
1707 for (i1 = j; i1 <= nq; i1++) {
1708 yp1 = yh[i1];
1709 yp2 = yh[i1 + 1];
1710 for (i = 1; i <= n; i++)
1711 yp1[i] += yp2[i];
1712 }
1713 pnorm = vmnorm(n, yh[1], ewt);
1714
1715 correction(neq, y, &corflag, pnorm, &del, &delp, &told, &ncf, &rh, &m);
1716 if (corflag == 0)
1717 break;
1718 if (corflag == 1) {
1719 rh = max(rh, hmin / fabs(h));
1720 scaleh(&rh, &pdh);
1721 continue;
1722 }
1723 if (corflag == 2) {
1724 kflag = -2;
1725 hold = h;
1726 jstart = 1;
1727 return;
1728 }
1729 } /* end inner while ( corrector loop ) */
1730 /*
1731 The corrector has converged. jcur is set to 0
1732 to signal that the Jacobian involved may need updating later.
1733 The local error test is done now.
1734 */
1735 jcur = 0;
1736 double dsm(0.0);
1737 if (m == 0)
1738 dsm = del / tesco[nq][2];
1739 if (m > 0)
1740 dsm = vmnorm(n, acor, ewt) / tesco[nq][2];
1741 if (dsm <= 1.) {
1742 /*
1743 After a successful step, update the yh array.
1744 Decrease icount by 1, and if it is -1, consider switching methods.
1745 If a method switch is made, reset various parameters,
1746 rescale the yh array, and exit. If there is no switch,
1747 consider changing h if ialth = 1. Otherwise decrease ialth by 1.
1748 If ialth is then 1 and nq < maxord, then acor is saved for
1749 use in a possible order increase on the next step.
1750 If a change in h is considered, an increase or decrease in order
1751 by one is considered also. A change in h is made only if it is by
1752 a factor of at least 1.1. If not, ialth is set to 3 to prevent
1753 testing for that many steps.
1754 */
1755 kflag = 0;
1756 nst++;
1757 hu = h;
1758 nqu = nq;
1759 mused = meth;
1760 for (j = 1; j <= l; j++) {
1761 yp1 = yh[j];
1762 r = el[j];
1763 for (i = 1; i <= n; i++)
1764 yp1[i] += r * acor[i];
1765 }
1766 icount--;
1767 if (icount < 0) {
1768 methodswitch(dsm, pnorm, &pdh, &rh);
1769 if (meth != mused) {
1770 rh = max(rh, hmin / fabs(h));
1771 scaleh(&rh, &pdh);
1772 rmax = 10.;
1773 endstoda();
1774 break;
1775 }
1776 }
1777 /*
1778 No method switch is being made. Do the usual step/order selection.
1779 */
1780 ialth--;
1781 if (ialth == 0) {
1782 rhup = 0.;
1783 if (l != lmax) {
1784 yp1 = yh[lmax];
1785 for (i = 1; i <= n; i++)
1786 savf[i] = acor[i] - yp1[i];
1787 dup = vmnorm(n, savf, ewt) / tesco[nq][3];
1788 exup = 1. / (double)(l + 1);
1789 rhup = 1. / (1.4 * pow(dup, exup) + 0.0000014);
1790 }
1791 orderswitch(&rhup, dsm, &pdh, &rh, &orderflag);
1792 /*
1793 No change in h or nq.
1794 */
1795 if (orderflag == 0) {
1796 endstoda();
1797 break;
1798 }
1799 /*
1800 h is changed, but not nq.
1801 */
1802 if (orderflag == 1) {
1803 rh = max(rh, hmin / fabs(h));
1804 scaleh(&rh, &pdh);
1805 rmax = 10.;
1806 endstoda();
1807 break;
1808 }
1809 /*
1810 both nq and h are changed.
1811 */
1812 if (orderflag == 2) {
1813 resetcoeff();
1814 rh = max(rh, hmin / fabs(h));
1815 scaleh(&rh, &pdh);
1816 rmax = 10.;
1817 endstoda();
1818 break;
1819 }
1820 } /* end if ( ialth == 0 ) */
1821 if (ialth > 1 || l == lmax) {
1822 endstoda();
1823 break;
1824 }
1825 yp1 = yh[lmax];
1826 for (i = 1; i <= n; i++)
1827 yp1[i] = acor[i];
1828 endstoda();
1829 break;
1830 } /* end if ( dsm <= 1. ) */
1831 /*
1832 The error test failed. kflag keeps track of multiple failures.
1833 Restore tn and the yh array to their previous values, and prepare
1834 to try the step again. Compute the optimum step size for this or
1835 one lower. After 2 or more failures, h is forced to decrease
1836 by a factor of 0.2 or less.
1837 */
1838 else {
1839 kflag--;
1840 tn = told;
1841 for (j = nq; j >= 1; j--)
1842 for (i1 = j; i1 <= nq; i1++) {
1843 yp1 = yh[i1];
1844 yp2 = yh[i1 + 1];
1845 for (i = 1; i <= n; i++)
1846 yp1[i] -= yp2[i];
1847 }
1848 rmax = 2.;
1849 if (fabs(h) <= hmin * 1.00001) {
1850 kflag = -1;
1851 hold = h;
1852 jstart = 1;
1853 break;
1854 }
1855 if (kflag > -3) {
1856 rhup = 0.;
1857 orderswitch(&rhup, dsm, &pdh, &rh, &orderflag);
1858 if (orderflag == 1 || orderflag == 0) {
1859 if (orderflag == 0)
1860 rh = min(rh, 0.2);
1861 rh = max(rh, hmin / fabs(h));
1862 scaleh(&rh, &pdh);
1863 }
1864 if (orderflag == 2) {
1865 resetcoeff();
1866 rh = max(rh, hmin / fabs(h));
1867 scaleh(&rh, &pdh);
1868 }
1869 continue;
1870 } /* if ( kflag > -3 ) */
1871 /*
1872 Control reaches this section if 3 or more failures have occurred.
1873 If 10 failures have occurred, exit with kflag = -1.
1874 It is assumed that the derivatives that have accumulated in the
1875 yh array have errors of the wrong order. Hence the first
1876 derivative is recomputed, and the order is set to 1. Then
1877 h is reduced by a factor of 10, and the step is retried,
1878 until it succeeds or h reaches hmin.
1879 */
1880 else {
1881 if (kflag == -10) {
1882 kflag = -1;
1883 hold = h;
1884 jstart = 1;
1885 break;
1886 } else {
1887 rh = 0.1;
1888 rh = max(hmin / fabs(h), rh);
1889 h *= rh;
1890 yp1 = yh[1];
1891 for (i = 1; i <= n; i++)
1892 y[i] = yp1[i];
1893 f(neq, tn, y, savf);
1894 nfe++;
1895 yp1 = yh[2];
1896 for (i = 1; i <= n; i++)
1897 yp1[i] = h * savf[i];
1898 ipup = miter;
1899 ialth = 5;
1900 if (nq == 1)
1901 continue;
1902 nq = 1;
1903 l = 2;
1904 resetcoeff();
1905 continue;
1906 }
1907 } /* end else -- kflag <= -3 */
1908 } /* end error failure handling */
1909 } /* end outer while */
1910
1911 } /* end stoda */
1912
ewset(int itol,double * rtol,double * atol,double * ycur)1913 void QTAIMLSODAIntegrator::ewset(int itol, double* rtol, double* atol,
1914 double* ycur)
1915 {
1916 int i;
1917
1918 switch (itol) {
1919 case 1:
1920 for (i = 1; i <= n; i++)
1921 ewt[i] = rtol[1] * fabs(ycur[i]) + atol[1];
1922 break;
1923 case 2:
1924 for (i = 1; i <= n; i++)
1925 ewt[i] = rtol[1] * fabs(ycur[i]) + atol[i];
1926 break;
1927 case 3:
1928 for (i = 1; i <= n; i++)
1929 ewt[i] = rtol[i] * fabs(ycur[i]) + atol[1];
1930 break;
1931 case 4:
1932 for (i = 1; i <= n; i++)
1933 ewt[i] = rtol[i] * fabs(ycur[i]) + atol[i];
1934 break;
1935 }
1936
1937 } /* end ewset */
1938
intdy(double t,int k,double * dky,int * iflag)1939 void QTAIMLSODAIntegrator::intdy(double t, int k, double* dky, int* iflag)
1940 /*
1941 Intdy computes interpolated values of the k-th derivative of the
1942 dependent variable vector y, and stores it in dky. This routine
1943 is called within the package with k = 0 and *t = tout, but may
1944 also be called by the user for any k up to the current order.
1945 ( See detailed instructions in the usage documentation. )
1946
1947 The computed values in dky are gotten by interpolation using the
1948 Nordsieck history array yh. This array corresponds uniquely to a
1949 vector-valued polynomial of degree nqcur or less, and dky is set
1950 to the k-th derivative of this polynomial at t.
1951 The formula for dky is
1952
1953 q
1954 dky[i] = sum c[k][j] * ( t - tn )^(j-k) * h^(-j) * yh[j+1][i]
1955 j=k
1956
1957 where c[k][j] = j*(j-1)*...*(j-k+1), q = nqcur, tn = tcur, h = hcur.
1958 The quantities nq = nqcur, l = nq+1, n = neq, tn, and h are declared
1959 static globally. The above sum is done in reverse order.
1960 *iflag is returned negative if either k or t is out of bounds.
1961 */
1962
1963 {
1964 int i, ic, j, jj, jp1;
1965 double c, r, s, tp;
1966
1967 *iflag = 0;
1968 if (k < 0 || k > nq) {
1969 qDebug("intdy -- k = %d illegal", k);
1970 *iflag = -1;
1971 return;
1972 }
1973 tp = tn - hu - 100. * ETA * (tn + hu);
1974 if ((t - tp) * (t - tn) > 0.) {
1975 qDebug("intdy -- t = %g illegal", t);
1976 qDebug(" t not in interval tcur - hu to tcur");
1977 *iflag = -2;
1978 return;
1979 }
1980
1981 s = (t - tn) / h;
1982 ic = 1;
1983 for (jj = l - k; jj <= nq; jj++)
1984 ic *= jj;
1985 c = (double)ic;
1986 yp1 = yh[l];
1987 for (i = 1; i <= n; i++)
1988 dky[i] = c * yp1[i];
1989 for (j = nq - 1; j >= k; j--) {
1990 jp1 = j + 1;
1991 ic = 1;
1992 for (jj = jp1 - k; jj <= j; jj++)
1993 ic *= jj;
1994 c = (double)ic;
1995 yp1 = yh[jp1];
1996 for (i = 1; i <= n; i++)
1997 dky[i] = c * yp1[i] + s * dky[i];
1998 }
1999 if (k == 0)
2000 return;
2001 r = pow(h, (double)(-k));
2002 for (i = 1; i <= n; i++)
2003 dky[i] *= r;
2004
2005 } /* end intdy */
2006
cfode(int meth_)2007 void QTAIMLSODAIntegrator::cfode(int meth_)
2008 {
2009 int i, nq_, nqm1, nqp1;
2010 double agamq, fnq, fnqm1, pc[13], pint, ragq, rqfac, rq1fac, tsign, xpin;
2011 /*
2012 cfode is called by the integrator routine to set coefficients
2013 needed there. The coefficients for the current method, as
2014 given by the value of meth, are set for all orders and saved.
2015 The maximum order assumed here is 12 if meth = 1 and 5 if meth = 2.
2016 ( A smaller value of the maximum order is also allowed. )
2017 cfode is called once at the beginning of the problem, and
2018 is not called again unless and until meth is changed.
2019
2020 The elco array contains the basic method coefficients.
2021 The coefficients el[i], 1 < i < nq+1, for the method of
2022 order nq are stored in elco[nq][i]. They are given by a generating
2023 polynomial, i.e.,
2024
2025 l(x) = el[1] + el[2]*x + ... + el[nq+1]*x^nq.
2026
2027 For the implicit Adams method, l(x) is given by
2028
2029 dl/dx = (x+1)*(x+2)*...*(x+nq-1)/factorial(nq-1), l(-1) = 0.
2030
2031 For the bdf methods, l(x) is given by
2032
2033 l(x) = (x+1)*(x+2)*...*(x+nq)/k,
2034
2035 where k = factorial(nq)*(1+1/2+...+1/nq).
2036
2037 The tesco array contains test constants used for the
2038 local error test and the selection of step size and/or order.
2039 At order nq, tesco[nq][k] is used for the selection of step
2040 size at order nq-1 if k = 1, at order nq if k = 2, and at order
2041 nq+1 if k = 3.
2042 */
2043 if (meth_ == 1) {
2044 elco[1][1] = 1.;
2045 elco[1][2] = 1.;
2046 tesco[1][1] = 0.;
2047 tesco[1][2] = 2.;
2048 tesco[2][1] = 1.;
2049 tesco[12][3] = 0.;
2050 pc[1] = 1.;
2051 rqfac = 1.;
2052 for (nq_ = 2; nq_ <= 12; nq_++) {
2053 /*
2054 The pc array will contain the coefficients of the polynomial
2055
2056 p(x) = (x+1)*(x+2)*...*(x+nq-1).
2057
2058 Initially, p(x) = 1.
2059 */
2060 rq1fac = rqfac;
2061 rqfac = rqfac / (double)nq_;
2062 nqm1 = nq_ - 1;
2063 fnqm1 = (double)nqm1;
2064 nqp1 = nq_ + 1;
2065 /*
2066 Form coefficients of p(x)*(x+nq-1).
2067 */
2068 pc[nq_] = 0.;
2069 for (i = nq_; i >= 2; i--)
2070 pc[i] = pc[i - 1] + fnqm1 * pc[i];
2071 pc[1] = fnqm1 * pc[1];
2072 /*
2073 Compute integral, -1 to 0, of p(x) and x*p(x).
2074 */
2075 pint = pc[1];
2076 xpin = pc[1] / 2.;
2077 tsign = 1.;
2078 for (i = 2; i <= nq_; i++) {
2079 tsign = -tsign;
2080 pint += tsign * pc[i] / (double)i;
2081 xpin += tsign * pc[i] / (double)(i + 1);
2082 }
2083 /*
2084 Store coefficients in elco and tesco.
2085 */
2086 elco[nq_][1] = pint * rq1fac;
2087 elco[nq_][2] = 1.;
2088 for (i = 2; i <= nq_; i++)
2089 elco[nq_][i + 1] = rq1fac * pc[i] / (double)i;
2090 agamq = rqfac * xpin;
2091 ragq = 1. / agamq;
2092 tesco[nq_][2] = ragq;
2093 if (nq_ < 12)
2094 tesco[nqp1][1] = ragq * rqfac / (double)nqp1;
2095 tesco[nqm1][3] = ragq;
2096 } /* end for */
2097 return;
2098 } /* end if ( meth == 1 ) */
2099
2100 /*
2101 meth = 2.
2102 */
2103 pc[1] = 1.;
2104 rq1fac = 1.;
2105 /*
2106 The pc array will contain the coefficients of the polynomial
2107
2108 p(x) = (x+1)*(x+2)*...*(x+nq).
2109
2110 Initially, p(x) = 1.
2111 */
2112 for (nq_ = 1; nq_ <= 5; nq_++) {
2113 fnq = (double)nq_;
2114 nqp1 = nq_ + 1;
2115 /*
2116 Form coefficients of p(x)*(x+nq).
2117 */
2118 pc[nqp1] = 0.;
2119 for (i = nq_ + 1; i >= 2; i--)
2120 pc[i] = pc[i - 1] + fnq * pc[i];
2121 pc[1] *= fnq;
2122 /*
2123 Store coefficients in elco and tesco.
2124 */
2125 for (i = 1; i <= nqp1; i++)
2126 elco[nq_][i] = pc[i] / pc[2];
2127 elco[nq_][2] = 1.;
2128 tesco[nq_][1] = rq1fac;
2129 tesco[nq_][2] = ((double)nqp1) / elco[nq_][1];
2130 tesco[nq_][3] = ((double)(nq_ + 2)) / elco[nq_][1];
2131 rq1fac /= fnq;
2132 }
2133 return;
2134
2135 } /* end cfode */
2136
scaleh(double * rh,double * pdh)2137 void QTAIMLSODAIntegrator::scaleh(double* rh, double* pdh)
2138 {
2139 double r;
2140 int j, i;
2141 /*
2142 If h is being changed, the h ratio rh is checked against
2143 rmax, hmin, and hmxi, and the yh array is rescaled. ialth is set to
2144 l = nq + 1 to prevent a change of h for that many steps, unless
2145 forced by a convergence or error test failure.
2146 */
2147 *rh = min(*rh, rmax);
2148 *rh = *rh / max(1., fabs(h) * hmxi * *rh);
2149 /*
2150 If meth = 1, also restrict the new step size by the stability region.
2151 If this reduces h, set irflag to 1 so that if there are roundoff
2152 problems later, we can assume that is the cause of the trouble.
2153 */
2154 if (meth == 1) {
2155 irflag = 0;
2156 *pdh = max(fabs(h) * pdlast, 0.000001);
2157 if ((*rh * *pdh * 1.00001) >= sm1[nq]) {
2158 *rh = sm1[nq] / *pdh;
2159 irflag = 1;
2160 }
2161 }
2162 r = 1.;
2163 for (j = 2; j <= l; j++) {
2164 r *= *rh;
2165 yp1 = yh[j];
2166 for (i = 1; i <= n; i++)
2167 yp1[i] *= r;
2168 }
2169 h *= *rh;
2170 rc *= *rh;
2171 ialth = l;
2172
2173 } /* end scaleh */
2174
prja(int neq,double * y)2175 void QTAIMLSODAIntegrator::prja(int neq, double* y)
2176 {
2177 int i, ier, j;
2178 double fac, hl0, r, r0, yj;
2179 /*
2180 prja is called by stoda to compute and process the matrix
2181 P = I - h * el[1] * J, where J is an approximation to the Jacobian.
2182 Here J is computed by finite differencing.
2183 J, scaled by -h * el[1], is stored in wm. Then the norm of J ( the
2184 matrix norm consistent with the weighted max-norm on vectors given
2185 by vmnorm ) is computed, and J is overwritten by P. P is then
2186 subjected to LU decomposition in preparation for later solution
2187 of linear systems with p as coefficient matrix. This is done
2188 by dgefa if miter = 2, and by dgbfa if miter = 5.
2189 */
2190 nje++;
2191 ierpj = 0;
2192 jcur = 1;
2193 hl0 = h * el0;
2194 /*
2195 If miter = 2, make n calls to f to approximate J.
2196 */
2197 if (miter != 2) {
2198 qDebug("prja -- miter != 2");
2199 return;
2200 }
2201
2202 if (miter == 2) {
2203 fac = vmnorm(n, savf, ewt);
2204 r0 = 1000. * fabs(h) * ETA * ((double)n) * fac;
2205 if (r0 == 0.)
2206 r0 = 1.;
2207 for (j = 1; j <= n; j++) {
2208 yj = y[j];
2209 r = max(sqrteta * fabs(yj), r0 / ewt[j]);
2210 y[j] += r;
2211 fac = -hl0 / r;
2212 f(neq, tn, y, acor);
2213 for (i = 1; i <= n; i++)
2214 wm[i][j] = (acor[i] - savf[i]) * fac;
2215 y[j] = yj;
2216 }
2217 nfe += n;
2218 /*
2219 Compute norm of Jacobian.
2220 */
2221 pdnorm = fnorm(n, wm, ewt) / fabs(hl0);
2222 /*
2223 Add identity matrix.
2224 */
2225 for (i = 1; i <= n; i++)
2226 wm[i][i] += 1.;
2227 /*
2228 Do LU decomposition on P.
2229 */
2230 dgefa(wm, n, ipvt, &ier);
2231 if (ier != 0)
2232 ierpj = 1;
2233 return;
2234 }
2235
2236 } /* end prja */
2237
vmnorm(int n_,double * v,double * w)2238 double QTAIMLSODAIntegrator::vmnorm(int n_, double* v, double* w)
2239 /*
2240 This function routine computes the weighted max-norm
2241 of the vector of length n contained in the array v, with weights
2242 contained in the array w of length n.
2243
2244 vmnorm = max( i = 1, ..., n ) fabs( v[i] ) * w[i].
2245 */
2246
2247 {
2248 int i;
2249 double vm;
2250
2251 vm = 0.;
2252 for (i = 1; i <= n_; i++)
2253 vm = max(vm, fabs(v[i]) * w[i]);
2254 return vm;
2255
2256 } /* end vmnorm */
2257
fnorm(int n_,double ** a,double * w)2258 double QTAIMLSODAIntegrator::fnorm(int n_, double** a, double* w)
2259 /*
2260 This subroutine computes the norm of a full n by n matrix,
2261 stored in the array a, that is consistent with the weighted max-norm
2262 on vectors, with weights stored in the array w.
2263
2264 fnorm = max(i=1,...,n) ( w[i] * sum(j=1,...,n) fabs( a[i][j] ) / w[j] )
2265 */
2266
2267 {
2268 int i, j;
2269 double an, sum, *ap1;
2270
2271 an = 0.;
2272 for (i = 1; i <= n_; i++) {
2273 sum = 0.;
2274 ap1 = a[i];
2275 for (j = 1; j <= n_; j++)
2276 sum += fabs(ap1[j]) / w[j];
2277 an = max(an, sum * w[i]);
2278 }
2279 return an;
2280
2281 } /* end fnorm */
2282
2283 // double QTAIMLSODAIntegrator::bnorm()
2284 // {
2285 // } /* end bnorm */
2286
correction(int neq,double * y,int * corflag,double pnorm,double * del,double * delp,double * told,int * ncf,double * rh,int * m)2287 void QTAIMLSODAIntegrator::correction(int neq, double* y, int* corflag,
2288 double pnorm, double* del, double* delp,
2289 double* told, int* ncf, double* rh,
2290 int* m)
2291 /*
2292 *corflag = 0 : corrector converged,
2293 1 : step size to be reduced, redo prediction,
2294 2 : corrector cannot converge, failure flag.
2295 */
2296
2297 {
2298 int i;
2299 double rm, rate, dcon;
2300
2301 /*
2302 Up to maxcor corrector iterations are taken. A convergence test is
2303 made on the r.m.s. norm of each correction, weighted by the error
2304 weight vector ewt. The sum of the corrections is accumulated in the
2305 vector acor[i]. The yh array is not altered in the corrector loop.
2306 */
2307
2308 *m = 0;
2309 *corflag = 0;
2310 rate = 0.;
2311 *del = 0.;
2312 yp1 = yh[1];
2313 for (i = 1; i <= n; i++)
2314 y[i] = yp1[i];
2315 f(neq, tn, y, savf);
2316 nfe++;
2317 /*
2318 If indicated, the matrix P = I - h * el[1] * J is reevaluated and
2319 preprocessed before starting the corrector iteration. ipup is set
2320 to 0 as an indicator that this has been done.
2321 */
2322 while (1) {
2323 if (*m == 0) {
2324 if (ipup > 0) {
2325 prja(neq, y);
2326 ipup = 0;
2327 rc = 1.;
2328 nslp = nst;
2329 crate = 0.7;
2330 if (ierpj != 0) {
2331 corfailure(told, rh, ncf, corflag);
2332 return;
2333 }
2334 }
2335 for (i = 1; i <= n; i++)
2336 acor[i] = 0.;
2337 } /* end if ( *m == 0 ) */
2338 if (miter == 0) {
2339 /*
2340 In case of functional iteration, update y directly from
2341 the result of the last function evaluation.
2342 */
2343 yp1 = yh[2];
2344 for (i = 1; i <= n; i++) {
2345 savf[i] = h * savf[i] - yp1[i];
2346 y[i] = savf[i] - acor[i];
2347 }
2348 *del = vmnorm(n, y, ewt);
2349 yp1 = yh[1];
2350 for (i = 1; i <= n; i++) {
2351 y[i] = yp1[i] + el[1] * savf[i];
2352 acor[i] = savf[i];
2353 }
2354 } /* end functional iteration */
2355 /*
2356 In the case of the chord method, compute the corrector error,
2357 and solve the linear system with that as right-hand side and
2358 P as coefficient matrix.
2359 */
2360 else {
2361 yp1 = yh[2];
2362 for (i = 1; i <= n; i++)
2363 y[i] = h * savf[i] - (yp1[i] + acor[i]);
2364 solsy(y);
2365 *del = vmnorm(n, y, ewt);
2366 yp1 = yh[1];
2367 for (i = 1; i <= n; i++) {
2368 acor[i] += y[i];
2369 y[i] = yp1[i] + el[1] * acor[i];
2370 }
2371 } /* end chord method */
2372 /*
2373 Test for convergence. If *m > 0, an estimate of the convergence
2374 rate constant is stored in crate, and this is used in the test.
2375
2376 We first check for a change of iterates that is the size of
2377 roundoff error. If this occurs, the iteration has converged, and a
2378 new rate estimate is not formed.
2379 In all other cases, force at least two iterations to estimate a
2380 local Lipschitz constant estimate for Adams method.
2381 On convergence, form pdest = local maximum Lipschitz constant
2382 estimate. pdlast is the most recent nonzero estimate.
2383 */
2384 if (*del <= 100. * pnorm * ETA)
2385 break;
2386 if (*m != 0 || meth != 1) {
2387 if (*m != 0) {
2388 rm = 1024.0;
2389 if (*del <= (1024. * *delp))
2390 rm = *del / *delp;
2391 rate = max(rate, rm);
2392 crate = max(0.2 * crate, rm);
2393 }
2394 dcon = *del * min(1., 1.5 * crate) / (tesco[nq][2] * conit);
2395 if (dcon <= 1.) {
2396 pdest = max(pdest, rate / fabs(h * el[1]));
2397 if (pdest != 0.)
2398 pdlast = pdest;
2399 break;
2400 }
2401 }
2402 /*
2403 The corrector iteration failed to converge.
2404 If miter != 0 and the Jacobian is out of date, prja is called for
2405 the next try. Otherwise the yh array is retracted to its values
2406 before prediction, and h is reduced, if possible. If h cannot be
2407 reduced or mxncf failures have occurred, exit with corflag = 2.
2408 */
2409 (*m)++;
2410 if (*m == maxcor || (*m >= 2 && *del > 2. * *delp)) {
2411 if (miter == 0 || jcur == 1) {
2412 corfailure(told, rh, ncf, corflag);
2413 return;
2414 }
2415 ipup = miter;
2416 /*
2417 Restart corrector if Jacobian is recomputed.
2418 */
2419 *m = 0;
2420 rate = 0.;
2421 *del = 0.;
2422 yp1 = yh[1];
2423 for (i = 1; i <= n; i++)
2424 y[i] = yp1[i];
2425 f(neq, tn, y, savf);
2426 nfe++;
2427 }
2428 /*
2429 Iterate corrector.
2430 */
2431 else {
2432 *delp = *del;
2433 f(neq, tn, y, savf);
2434 nfe++;
2435 }
2436 } /* end while */
2437 } /* end correction */
2438
corfailure(double * told,double * rh,int * ncf,int * corflag)2439 void QTAIMLSODAIntegrator::corfailure(double* told, double* rh, int* ncf,
2440 int* corflag)
2441 {
2442 int j, i1, i;
2443
2444 (*ncf)++;
2445 rmax = 2.;
2446 tn = *told;
2447 for (j = nq; j >= 1; j--)
2448 for (i1 = j; i1 <= nq; i1++) {
2449 yp1 = yh[i1];
2450 yp2 = yh[i1 + 1];
2451 for (i = 1; i <= n; i++)
2452 yp1[i] -= yp2[i];
2453 }
2454 if (fabs(h) <= hmin * 1.00001 || *ncf == mxncf) {
2455 *corflag = 2;
2456 return;
2457 }
2458 *corflag = 1;
2459 *rh = 0.25;
2460 ipup = miter;
2461
2462 } /* end corfailure */
2463
solsy(double * y)2464 void QTAIMLSODAIntegrator::solsy(double* y)
2465 /*
2466 This routine manages the solution of the linear system arising from
2467 a chord iteration. It is called if miter != 0.
2468 If miter is 2, it calls dgesl to accomplish this.
2469 If miter is 5, it calls dgbsl.
2470
2471 y = the right-hand side vector on input, and the solution vector
2472 on output.
2473 */
2474 {
2475 iersl = 0;
2476 if (miter != 2) {
2477 qDebug("solsy -- miter != 2");
2478 return;
2479 }
2480
2481 if (miter == 2)
2482 dgesl(wm, n, ipvt, y, 0);
2483 return;
2484
2485 } /* end solsy */
2486
methodswitch(double dsm,double pnorm,double * pdh,double * rh)2487 void QTAIMLSODAIntegrator::methodswitch(double dsm, double pnorm, double* pdh,
2488 double* rh)
2489 {
2490 int lm1, lm1p1, lm2, lm2p1, nqm1, nqm2;
2491 double rh1, rh2, rh1it, exm2, dm2, exm1, dm1, alpha, exsm;
2492
2493 /*
2494 We are current using an Adams method. Consider switching to bdf.
2495 If the current order is greater than 5, assume the problem is
2496 not stiff, and skip this section.
2497 If the Lipschitz constant and error estimate are not polluted
2498 by roundoff, perform the usual test.
2499 Otherwise, switch to the bdf methods if the last step was
2500 restricted to insure stability ( irflag = 1 ), and stay with Adams
2501 method if not. When switching to bdf with polluted error estimates,
2502 in the absence of other information, double the step size.
2503
2504 When the estimates are ok, we make the usual test by computing
2505 the step size we could have (ideally) used on this step,
2506 with the current (Adams) method, and also that for the bdf.
2507 If nq > mxords, we consider changing to order mxords on switching.
2508 Compare the two step sizes to decide whether to switch.
2509 The step size advantage must be at least ratio = 5 to switch.
2510 */
2511 if (meth == 1) {
2512 if (nq > 5)
2513 return;
2514 if (dsm <= (100. * pnorm * ETA) || pdest == 0.) {
2515 if (irflag == 0)
2516 return;
2517 rh2 = 2.;
2518 nqm2 = min(nq, mxords);
2519 } else {
2520 exsm = 1. / (double)l;
2521 rh1 = 1. / (1.2 * pow(dsm, exsm) + 0.0000012);
2522 rh1it = 2. * rh1;
2523 *pdh = pdlast * fabs(h);
2524 if ((*pdh * rh1) > 0.00001)
2525 rh1it = sm1[nq] / *pdh;
2526 rh1 = min(rh1, rh1it);
2527 if (nq > mxords) {
2528 nqm2 = mxords;
2529 lm2 = mxords + 1;
2530 exm2 = 1. / (double)lm2;
2531 lm2p1 = lm2 + 1;
2532 dm2 = vmnorm(n, yh[lm2p1], ewt) / cm2[mxords];
2533 rh2 = 1. / (1.2 * pow(dm2, exm2) + 0.0000012);
2534 } else {
2535 dm2 = dsm * (cm1[nq] / cm2[nq]);
2536 rh2 = 1. / (1.2 * pow(dm2, exsm) + 0.0000012);
2537 nqm2 = nq;
2538 }
2539 if (rh2 < ratio * rh1)
2540 return;
2541 }
2542 /*
2543 The method switch test passed. Reset relevant quantities for bdf.
2544 */
2545 *rh = rh2;
2546 icount = 20;
2547 meth = 2;
2548 miter = jtyp;
2549 pdlast = 0.;
2550 nq = nqm2;
2551 l = nq + 1;
2552 return;
2553 } /* end if ( meth == 1 ) */
2554 /*
2555 We are currently using a bdf method, considering switching to Adams.
2556 Compute the step size we could have (ideally) used on this step,
2557 with the current (bdf) method, and also that for the Adams.
2558 If nq > mxordn, we consider changing to order mxordn on switching.
2559 Compare the two step sizes to decide whether to switch.
2560 The step size advantage must be at least 5/ratio = 1 to switch.
2561 If the step size for Adams would be so small as to cause
2562 roundoff pollution, we stay with bdf.
2563 */
2564 exsm = 1. / (double)l;
2565 if (mxordn < nq) {
2566 nqm1 = mxordn;
2567 lm1 = mxordn + 1;
2568 exm1 = 1. / (double)lm1;
2569 lm1p1 = lm1 + 1;
2570 dm1 = vmnorm(n, yh[lm1p1], ewt) / cm1[mxordn];
2571 rh1 = 1. / (1.2 * pow(dm1, exm1) + 0.0000012);
2572 } else {
2573 dm1 = dsm * (cm2[nq] / cm1[nq]);
2574 rh1 = 1. / (1.2 * pow(dm1, exsm) + 0.0000012);
2575 nqm1 = nq;
2576 exm1 = exsm;
2577 }
2578 rh1it = 2. * rh1;
2579 *pdh = pdnorm * fabs(h);
2580 if ((*pdh * rh1) > 0.00001)
2581 rh1it = sm1[nqm1] / *pdh;
2582 rh1 = min(rh1, rh1it);
2583 rh2 = 1. / (1.2 * pow(dsm, exsm) + 0.0000012);
2584 if ((rh1 * ratio) < (5. * rh2))
2585 return;
2586 alpha = max(0.001, rh1);
2587 dm1 *= pow(alpha, exm1);
2588 if (dm1 <= 1000. * ETA * pnorm)
2589 return;
2590 /*
2591 The switch test passed. Reset relevant quantities for Adams.
2592 */
2593 *rh = rh1;
2594 icount = 20;
2595 meth = 1;
2596 miter = 0;
2597 pdlast = 0.;
2598 nq = nqm1;
2599 l = nq + 1;
2600
2601 } /* end methodswitch */
2602
2603 /*
2604 This routine returns from stoda to lsoda. Hence freevectors() is
2605 not executed.
2606 */
2607
endstoda()2608 void QTAIMLSODAIntegrator::endstoda()
2609 {
2610 double r;
2611 int i;
2612
2613 r = 1. / tesco[nqu][2];
2614 for (i = 1; i <= n; i++)
2615 acor[i] *= r;
2616 hold = h;
2617 jstart = 1;
2618
2619 } /* end endstoda */
2620
orderswitch(double * rhup,double dsm,double * pdh,double * rh,int * orderflag)2621 void QTAIMLSODAIntegrator::orderswitch(double* rhup, double dsm, double* pdh,
2622 double* rh, int* orderflag)
2623 /*
2624 Regardless of the success or failure of the step, factors
2625 rhdn, rhsm, and rhup are computed, by which h could be multiplied
2626 at order nq - 1, order nq, or order nq + 1, respectively.
2627 In the case of a failure, rhup = 0. to avoid an order increase.
2628 The largest of these is determined and the new order chosen
2629 accordingly. If the order is to be increased, we compute one
2630 additional scaled derivative.
2631
2632 orderflag = 0 : no change in h or nq,
2633 1 : change in h but not nq,
2634 2 : change in both h and nq.
2635 */
2636 {
2637 int newq, i;
2638 double exsm, rhdn, rhsm, ddn, exdn, r;
2639
2640 *orderflag = 0;
2641
2642 exsm = 1. / (double)l;
2643 rhsm = 1. / (1.2 * pow(dsm, exsm) + 0.0000012);
2644
2645 rhdn = 0.;
2646 if (nq != 1) {
2647 ddn = vmnorm(n, yh[l], ewt) / tesco[nq][1];
2648 exdn = 1. / (double)nq;
2649 rhdn = 1. / (1.3 * pow(ddn, exdn) + 0.0000013);
2650 }
2651 /*
2652 If meth = 1, limit rh according to the stability region also.
2653 */
2654 if (meth == 1) {
2655 *pdh = max(fabs(h) * pdlast, 0.000001);
2656 if (l < lmax)
2657 *rhup = min(*rhup, sm1[l] / *pdh);
2658 rhsm = min(rhsm, sm1[nq] / *pdh);
2659 if (nq > 1)
2660 rhdn = min(rhdn, sm1[nq - 1] / *pdh);
2661 pdest = 0.;
2662 }
2663 if (rhsm >= *rhup) {
2664 if (rhsm >= rhdn) {
2665 newq = nq;
2666 *rh = rhsm;
2667 } else {
2668 newq = nq - 1;
2669 *rh = rhdn;
2670 if (kflag < 0 && *rh > 1.)
2671 *rh = 1.;
2672 }
2673 } else {
2674 if (*rhup <= rhdn) {
2675 newq = nq - 1;
2676 *rh = rhdn;
2677 if (kflag < 0 && *rh > 1.)
2678 *rh = 1.;
2679 } else {
2680 *rh = *rhup;
2681 if (*rh >= 1.1) {
2682 r = el[l] / (double)l;
2683 nq = l;
2684 l = nq + 1;
2685 yp1 = yh[l];
2686 for (i = 1; i <= n; i++)
2687 yp1[i] = acor[i] * r;
2688 *orderflag = 2;
2689 return;
2690 } else {
2691 ialth = 3;
2692 return;
2693 }
2694 }
2695 }
2696 /*
2697 If meth = 1 and h is restricted by stability, bypass 10 percent test.
2698 */
2699 if (meth == 1) {
2700 if ((*rh * *pdh * 1.00001) < sm1[newq])
2701 if (kflag == 0 && *rh < 1.1) {
2702 ialth = 3;
2703 return;
2704 }
2705 } else {
2706 if (kflag == 0 && *rh < 1.1) {
2707 ialth = 3;
2708 return;
2709 }
2710 }
2711 if (kflag <= -2)
2712 *rh = min(*rh, 0.2);
2713 /*
2714 If there is a change of order, reset nq, l, and the coefficients.
2715 In any case h is reset according to rh and the yh array is rescaled.
2716 Then exit or redo the step.
2717 */
2718 if (newq == nq) {
2719 *orderflag = 1;
2720 return;
2721 }
2722 nq = newq;
2723 l = nq + 1;
2724 *orderflag = 2;
2725
2726 } /* end orderswitch */
2727
resetcoeff()2728 void QTAIMLSODAIntegrator::resetcoeff()
2729 /*
2730 The el vector and related constants are reset
2731 whenever the order nq is changed, or at the start of the problem.
2732 */
2733 {
2734 int i;
2735 double* ep1;
2736
2737 ep1 = elco[nq];
2738 for (i = 1; i <= l; i++)
2739 el[i] = ep1[i];
2740 rc = rc * el[1] / el0;
2741 el0 = el[1];
2742 conit = 0.5 / (double)(nq + 2);
2743
2744 } /* end resetcoeff */
2745
freevectors()2746 void QTAIMLSODAIntegrator::freevectors()
2747 {
2748 int i;
2749 for (i = 1; i <= m_lenyh; ++i) {
2750 free(yh[i]);
2751 }
2752 free(yh);
2753
2754 for (i = 1; i <= m_nyh; ++i) {
2755 free(wm[i]);
2756 }
2757 free(wm);
2758
2759 free(ewt);
2760 free(savf);
2761 free(acor);
2762 free(ipvt);
2763 } /* end freevectors */
2764
2765 } // namespace QtPlugins
2766 } // namespace Avogadro
2767