source: project/nordsieck/trunk/nordsieck.scm @ 5831

Last change on this file since 5831 was 5831, checked in by Ivan Raikov, 12 years ago

Moved lmm to ode-lmm.

File size: 9.5 KB
Line 
1;;
2;;
3;; Nordsieck array implementation.
4;;
5;;
6;; Copyright 2007 Ivan Raikov and the Okinawa Institute of Science and Technology
7;;
8;;
9;; This program is free software: you can redistribute it and/or
10;; modify it under the terms of the GNU General Public License as
11;; published by the Free Software Foundation, either version 3 of the
12;; License, or (at your option) any later version.
13;;
14;; This program is distributed in the hope that it will be useful, but
15;; WITHOUT ANY WARRANTY; without even the implied warranty of
16;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
17;; General Public License for more details.
18;;
19;; A full copy of the GPL license can be found at
20;; <http://www.gnu.org/licenses/>.
21;;
22;;
23
24(require-extension syntax-case)
25(require-extension srfi-1)
26(require-extension srfi-4)
27(require-extension srfi-42)
28(require-extension blas)
29(require-extension atlas-lapack)
30(require-extension matrix-utils)
31
32(define-extension nordsieck)
33
34;;(declare (export nordsieck-form
35;;               nordsieck-step
36;;               nordsieck-hscale))
37
38
39(define (nordsieck:error x . rest)
40  (let ((port (open-output-string)))
41    (let loop ((objs (cons x rest)))
42      (if (null? objs)
43          (begin
44            (newline port)
45            (error 'nordsieck (get-output-string port)))
46          (begin (display (car objs) port)
47                 (display " " port)
48                 (loop (cdr objs)))))))
49
50(define-utility-matrices f64)
51(define-matrix-map f64)
52
53(define-record nordsieck-def order Ainv A B C V Econst d dhist dhistlen p1 p2 m n) 
54
55(define-record-printer (nordsieck-def x out)
56  (fprintf out "#(nordsieck order=~A Ainv=~S A=~S B=~S C=~S V=~S Econst=~S d=~S dhist=~S p1=~S p2=~S m=~S n=~S)"
57           (if (= blas:RowMajor (nordsieck-def-order x))
58               "row" "col")
59           (nordsieck-def-Ainv x)
60           (nordsieck-def-A x)
61           (nordsieck-def-B x)
62           (nordsieck-def-C x)
63           (nordsieck-def-V x)
64           (nordsieck-def-Econst x)
65           (nordsieck-def-d x)
66           (nordsieck-def-dhist x)
67           (nordsieck-def-p1 x)
68           (nordsieck-def-p2 x)
69           (nordsieck-def-m x)
70           (nordsieck-def-n x)))
71
72
73
74(define (make-B order a b . rest)
75  (let-optionals rest ((va #f) (vb #f))
76                 
77  (let* ((implicit? (and va vb))
78         (alen  (f64vector-length a))
79         (blen  ((lambda (x) (if implicit? (fx- x 1) x)) (f64vector-length b)))
80         (p1    ((lambda (x) (if (fx> x 0) (fx- x 1) 
81                                 (nordsieck:error 'make-B "empty coefficient vector a"))) alen))
82         (p2    ((lambda (x) (if (fx> x 0) (fx- x 1) 
83                                 (nordsieck:error 'make-B "empty coefficient vector b"))) blen))
84         
85         (vd  (and implicit?
86                   (let ((vd   (make-f64vector alen 0.0))
87                         (b-1  (f64vector-ref  b  0)))
88                     (do-ec (:range i 0 alen)
89                            (let ((vai  (f64vector-ref va i))
90                                  (ai   (f64vector-ref a  i)))
91                              (f64vector-set! vd i (/ (- vai ai) b-1))))
92                     vd)))
93         
94         (ve  (and implicit?
95                   (let ((ve    (make-f64vector (fx+ 1 p2) 0.0))
96                         (b-1   (f64vector-ref  b  0)))
97                     (do-ec (:range i 0 blen)
98                            (let ((vbi  (f64vector-ref vb i))
99                                  (bi   (f64vector-ref b  (fx+ 1 i))))
100                              (f64vector-set! ve i (/ (- vbi bi) b-1))))
101                     ve))))
102
103    (let ((N    (fx+ 2 (fx+ p1 p2))))
104      (let ((v    (make-f64vector (fx* N N) 0.0)))
105        (if implicit?
106            (begin
107              ;; set va and vb coefficients in the first row of the
108              ;; coefficient matrix B
109              (fill-matrix! order N N v (lambda (i j ax) 
110                                          (values (if (fx<= j p1) (f64vector-ref va j) (f64vector-ref vb (fx- j alen)))
111                                                  ax)) 
112                            #f 0 0 1 N)
113              ;; set vd and ve coefficients in the p1+1-th row of the
114              ;; coefficient matrix B
115              (fill-matrix! order N N v (lambda (i j ax) 
116                                          (values (if (fx<= j p1) (f64vector-ref vd j) (f64vector-ref ve (fx- j alen)))
117                                                  ax)) 
118                            #f (fx+ 1 p1) 0 (fx+ 2 p1) N))
119            ;; set a and b coefficients in the first row of the
120            ;; coefficient matrix B
121            (fill-matrix! order N N v (lambda (i j ax) 
122                                        (values (if (fx<= j p1) (f64vector-ref a j) (f64vector-ref b (fx- j alen)))
123                                                ax)) 
124                          #f 0 0 1 N))
125
126        ;; set the identity matrices for p1 and p2, respectively
127        (if (fx> p1 0)
128            (fill-matrix! order N N v (lambda (i j ax) 
129                                        (values (if (fx= i j) 1.0 0.0) ax))
130                          #f 1 1 (fx+ 1 p1) (fx+ 1 p1)))
131        (fill-matrix! order N N v (lambda (i j ax) 
132                                    (values (if (fx= i j) 1.0 0.0) ax))
133                      #f (fx+ 2 p1) (fx+ 1 p1) N N)
134        v)))))
135
136
137(define (make-c a b . rest)
138  (let-optionals rest ((va #f) (vb #f))
139                 
140    (define implicit? (and va vb))
141 
142    (define p1  (fx- (f64vector-length a) 1))
143    (define p2  (fx- (f64vector-length b) (if implicit? 2 1)))
144   
145    (let ((v (make-f64vector (fx+ 2 (fx+ p1 p2)) 0.0)))
146      (f64vector-set! v (fx+ 1 p1) 1.0)
147      (if implicit? (f64vector-set! v 0 (f64vector-ref b 0)))
148      v)))
149 
150
151
152(define (make-Ainverse order p1 p2)
153  (define N (fx+ 2 (fx+ p1 p2)))
154  (define N-1 (fx- N 1))
155
156  (let ((A-1  (matrix-zeros N N))
157        (bot  (fx* N (fx+ 1 p1))))
158
159    ;; initialize the top 1 .. p1 rows of the A-1 matrix
160    (fill-matrix! order N N A-1
161                  (lambda (i j ax)
162                    (cond ((fx= i 0)  (values (if (fx= j 0) 1.0 0.0) 1.0))
163                          (else       (let ((c1 (* i ax)))
164                                        (values (if (odd? j) c1 (- c1)) (if (fx= j N-1) 1.0 c1))))))
165                  1.0 0 0 (fx+ 1 p1) N)
166
167    ;; initialize the bottom p2-p1 .. p2 rows of the A-1 matrix
168    (fill-matrix! order N N A-1
169                  (lambda (i j ax)
170                      (cond ((fx= i 0)  (values (if (fx= j 0) 1.0 0.0) 1.0))
171                            ((fx= j 0)  (values 1.0 1.0))
172                            (else      
173                             (let* ((ax1 (* i ax))
174                                    (c1  (* (fx+ 1 j) ax1)))
175                               (values (if (odd? j) (- c1) c1)
176                                       (if (fx= j N-1) 1.0 ax1))))))
177                  1.0 (fx+ 1 p1) 1 N N)
178
179    A-1))
180
181
182
183(define (make-V order p1 p2)
184  (define N (fx+ 2 (fx+ p1 p2)))
185 
186  (define (f1 i n)
187    (if (> i 0) (* n (f1 (- i 1) (- n 1))) 1))
188
189  (let ((V (matrix-zeros N N)))
190    (fill-matrix! order N N V 
191                  (lambda (i j ax) 
192                    (values (cond ((fx= i 0)  (+ 1 j))
193                                  ((fx= i j)  1.0)
194                                  ((fx> i j)  0.0)
195                                  (else (* (/ 1 (f1 i (+ 1 i))) (f1 (+ 1 i) (+ 1 j))))) #f))
196                  #f)))
197                                 
198
199
200(define (nordsieck-form order Econst a b . rest)
201  (let-optionals rest ((va #f) (vb #f) (dhistlen 3) (m 1))
202    (let* ((implicit? (and va vb))
203           (p1  (- (f64vector-length a) 1))
204           (p2  (- (f64vector-length b) (if implicit? 2 1)))
205           (n   (fx+ 2 (fx+ p1 p2))))
206      (let ((Ainv   (make-Ainverse order p1 p2)))
207        (let* ((LU+p   (let-values (((LU p) (atlas-lapack:dgetrf order n n Ainv))) (cons LU p)))
208               (LU     (car LU+p))
209               (p      (cdr LU+p))
210               (A      (atlas-lapack:dgetri order n LU p)))
211          (let ((B  (make-B order a b va vb))
212                (c  (make-c a b va vb)))
213            (let* ((BAinv  (let ((K  (matrix-zeros n n order)))
214                             (blas:dgemm! order blas:NoTrans blas:NoTrans n n n 1.0 B Ainv 0.0 K))))
215              (let ((D  (let ((K (matrix-ones n n order)))
216                          (blas:dgemm! order blas:NoTrans blas:NoTrans n n n 1.0 A BAinv 2.0 K)))
217                    (d     (let ((v (matrix-zeros n 1)))
218                             (blas:dgemv! order blas:NoTrans n n 1.0 A c 0.0 v))))
219                  (print "Ainv = " Ainv)
220                  (print "A = " A)
221                  (print "B = " B)
222                  (print "BAinv = " BAinv)
223                  (print "D = " D)
224                  (let ((C   (let ((K (matrix-ones n n order)))
225                               (matrix-map round (blas:daxpy! (fx* n n) -2.0 K D))))
226                        (V   (make-V order p1 p2)))
227                    (print "C = " C)
228                    (print "V = " V)
229                    (make-nordsieck-def order Ainv A B C V Econst d (list) dhistlen p1 p2 m n))))))))))
230
231 
232(define (nordsieck-step def h prev frhs)
233  (let ((order (nordsieck-def-order def))
234        (m     (nordsieck-def-m def))
235        (n     (nordsieck-def-n def))
236        (C     (nordsieck-def-C def))
237        (d     (nordsieck-def-d def)))
238  (let ((y0  (let ((v (matrix-zeros n 1)))
239               (blas:dgemv! blas:NoTrans n n 1.0 C prev 0.0 v))))
240    (let loop ((i 0) (m m) (ym (list y0)))
241      (if (fx<= m 1)  ym
242          (let ((yi  (let ((fy  (fp- (fp* h (frhs (f64vector-ref (car ym) 0))) 
243                                     (fp* h (f64vector-ref (car ym) 1)))))
244                       (blas:daxpy n fy d (car ym)))))
245            (loop (fx+ 1 i) (fx- m 1) (cons yi ym))))))))
246
247
248(define (nordsieck-step-0 def h prev)
249  (let ((order (nordsieck-def-order def))
250        (n     (nordsieck-def-n def))
251        (C     (nordsieck-def-C def))
252        (d     (nordsieck-def-d def)))
253    (let ((y0  (let ((v (matrix-zeros n 1)))
254                 (blas:dgemv! blas:NoTrans n n 1.0 C prev 0.0 v))))
255      y0)))
256
257
258(define (nordsieck-step-i def h prev ym)
259  (let ((order (nordsieck-def-order def))
260        (n     (nordsieck-def-n def))
261        (C     (nordsieck-def-C def))
262        (d     (nordsieck-def-d def)))
263    (let ((yi  (let ((fy  (fp- (fp* h prev)
264                               (fp* h (f64vector-ref (car ym) 1)))))
265                 (blas:daxpy n fy d (car ym)))))
266      (cons yi ym))))
267
268
269 
270(define (nordsieck-hscale def h r)
271  (let ((order (nordsieck-def-order def))
272        (n     (nordsieck-def-n def))
273        (V     (nordsieck-def-V def))
274        (d     (nordsieck-def-d def)))
275    (let* ((gamma  (blas:dicopy n d)))
276      (let ((D  (let ((s  (list->f64vector (list-tabulate n (lambda (x) (expt r x))))))
277                  (matrix-diag n n s 0 order)))
278            (e  (let ((e (matrix-zeros 1 n order)))
279                  (f64vector-set! e 0 1.0)
280                  (f64vector-set! e 1 1.0)
281                  e)))
282        (let ((K  (let ((K (matrix-eye n n order)))
283                    (blas:dgemm! order blas:Trans blas:NoTrans n n 1 -1.0 gamma e 1.0 K))))
284          (let ((KV  (let ((R (matrix-zeros n n)))
285                       (blas:dgemm! order blas:Trans blas:Trans n n n 1.0 K V 0.0 R))))
286            (let ((DKV  (let ((R (matrix-zeros n n)))
287                          (blas:dgemm! order blas:Trans blas:Trans n n n 1.0 D KV 0.0 R))))
288              (let ((d1  (let ((v (matrix-zeros n 1)))
289                           (blas:dgemv! order blas:NoTrans n n 1.0 DKV gamma 0.0 v))))
290                (match def 
291                       (($ nordsieck-def order Ainv A B C V Econst d dhist dhistlen p1 p2 m n)
292                        (let ((dhist  (if (fx>= (length dhist) dhistlen)
293                                          (take dhist (fx- dhistlen 1)) dhist)))
294                          (make-nordsieck-def order Ainv A B C V Econst d1 (cons d dhist) dhistlen p1 p2 m n))))))))))))
295       
Note: See TracBrowser for help on using the repository browser.