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

Last change on this file since 5643 was 5643, checked in by Ivan Raikov, 13 years ago

Added procedure make-V

File size: 8.8 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 d=~S d=~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-d x)
65           (nordsieck-def-dhist x)
66           (nordsieck-def-p1 x)
67           (nordsieck-def-p2 x)
68           (nordsieck-def-m x)
69           (nordsieck-def-n x)))
70
71
72
73(define (make-B order a b . rest)
74  (let-optionals rest ((va #f) (vb #f))
75                 
76  (let* ((implicit? (and va vb))
77         (alen  (f64vector-length a))
78         (blen  ((lambda (x) (if implicit? (fx- x 1) x)) (f64vector-length b)))
79         (p1    ((lambda (x) (if (fx> x 0) (fx- x 1) 
80                                 (nordsieck:error 'make-B "empty coefficient vector a"))) alen))
81         (p2    ((lambda (x) (if (fx> x 0) (fx- x 1) 
82                                 (nordsieck:error 'make-B "empty coefficient vector b"))) blen))
83         
84         (vd  (and implicit?
85                   (let ((vd   (make-f64vector alen 0.0))
86                         (b-1  (f64vector-ref  b  0)))
87                     (do-ec (:range i 0 alen)
88                            (let ((vai  (f64vector-ref va i))
89                                  (ai   (f64vector-ref a  i)))
90                              (f64vector-set! vd i (/ (- vai ai) b-1))))
91                     vd)))
92         
93         (ve  (and implicit?
94                   (let ((ve    (make-f64vector (fx+ 1 p2) 0.0))
95                         (b-1   (f64vector-ref  b  0)))
96                     (do-ec (:range i 0 blen)
97                            (let ((vbi  (f64vector-ref vb i))
98                                  (bi   (f64vector-ref b  (fx+ 1 i))))
99                              (f64vector-set! ve i (/ (- vbi bi) b-1))))
100                     ve))))
101
102    (let ((N    (fx+ 2 (fx+ p1 p2))))
103      (let ((v    (make-f64vector (fx* N N) 0.0)))
104        (if implicit?
105            (begin
106              ;; set va and vb coefficients in the first row of the
107              ;; coefficient matrix B
108              (fill-matrix! order N N v (lambda (i j ax) 
109                                          (values (if (fx<= j p1) (f64vector-ref va j) (f64vector-ref vb (fx- j alen)))
110                                                  ax)) 
111                            #f 0 0 1 N)
112              ;; set vd and ve coefficients in the p1+1-th row of the
113              ;; coefficient matrix B
114              (fill-matrix! order N N v (lambda (i j ax) 
115                                          (values (if (fx<= j p1) (f64vector-ref vd j) (f64vector-ref ve (fx- j alen)))
116                                                  ax)) 
117                            #f (fx+ 1 p1) 0 (fx+ 2 p1) N))
118            ;; set a and b coefficients in the first row of the
119            ;; coefficient matrix B
120            (fill-matrix! order N N v (lambda (i j ax) 
121                                        (values (if (fx<= j p1) (f64vector-ref a j) (f64vector-ref b (fx- j alen)))
122                                                ax)) 
123                          #f 0 0 1 N))
124
125        ;; set the identity matrices for p1 and p2, respectively
126        (if (fx> p1 0)
127            (fill-matrix! order N N v (lambda (i j ax) 
128                                        (values (if (fx= i j) 1.0 0.0) ax))
129                          #f 1 1 (fx+ 1 p1) (fx+ 1 p1)))
130        (fill-matrix! order N N v (lambda (i j ax) 
131                                    (values (if (fx= i j) 1.0 0.0) ax))
132                      #f (fx+ 2 p1) (fx+ 1 p1) N N)
133        v)))))
134
135
136(define (make-c a b . rest)
137  (let-optionals rest ((va #f) (vb #f))
138                 
139    (define implicit? (and va vb))
140 
141    (define p1  (fx- (f64vector-length a) 1))
142    (define p2  (fx- (f64vector-length b) (if implicit? 2 1)))
143   
144    (let ((v (make-f64vector (fx+ 2 (fx+ p1 p2)) 0.0)))
145      (f64vector-set! v (fx+ 1 p1) 1.0)
146      (if implicit? (f64vector-set! v 0 (f64vector-ref b 0)))
147      v)))
148 
149
150
151(define (make-Ainverse order p1 p2)
152  (define N (fx+ 2 (fx+ p1 p2)))
153  (define N-1 (fx- N 1))
154
155  (let ((A-1  (matrix-zeros N N))
156        (bot  (fx* N (fx+ 1 p1))))
157
158    ;; initialize the top 1 .. p1 rows of the A-1 matrix
159    (fill-matrix! order N N A-1
160                  (lambda (i j ax)
161                    (cond ((fx= i 0)  (values (if (fx= j 0) 1.0 0.0) 1.0))
162                          (else       (let ((c1 (* i ax)))
163                                        (values (if (odd? j) c1 (- c1)) (if (fx= j N-1) 1.0 c1))))))
164                  1.0 0 0 (fx+ 1 p1) N)
165
166    ;; initialize the bottom p2-p1 .. p2 rows of the A-1 matrix
167    (fill-matrix! order N N A-1
168                  (lambda (i j ax)
169                      (cond ((fx= i 0)  (values (if (fx= j 0) 1.0 0.0) 1.0))
170                            ((fx= j 0)  (values 1.0 1.0))
171                            (else      
172                             (let* ((ax1 (* i ax))
173                                    (c1  (* (fx+ 1 j) ax1)))
174                               (values (if (odd? j) (- c1) c1)
175                                       (if (fx= j N-1) 1.0 ax1))))))
176                  1.0 (fx+ 1 p1) 1 N N)
177
178    A-1))
179
180
181
182(define (make-V order p1 p2)
183  (define N (fx+ 2 (fx+ p1 p2)))
184 
185  (define (f1 i n)
186    (if (> i 0) (* n (f1 (- i 1) (- n 1))) 1))
187
188  (let ((V (matrix-zeros N N)))
189    (fill-matrix! order N N V 
190                  (lambda (i j ax) 
191                    (values (cond ((fx= i 0)  (+ 1 j))
192                                  ((fx= i j)  1.0)
193                                  ((fx> i j)  0.0)
194                                  (else (* (/ 1 (f1 i (+ 1 i))) (f1 (+ 1 i) (+ 1 j))))) #f))
195                  #f)))
196                                 
197
198
199(define (nordsieck-form order a b . rest)
200  (let-optionals rest ((va #f) (vb #f) (dhistlen 3) (m 1))
201    (let* ((implicit? (and va vb))
202           (p1  (- (f64vector-length a) 1))
203           (p2  (- (f64vector-length b) (if implicit? 2 1)))
204           (n   (fx+ 2 (fx+ p1 p2))))
205      (let ((Ainv   (make-Ainverse order p1 p2)))
206        (let* ((LU+p   (let-values (((LU p) (atlas-lapack:dgetrf order n n Ainv))) (cons LU p)))
207               (LU     (car LU+p))
208               (p      (cdr LU+p))
209               (A      (atlas-lapack:dgetri order n LU p)))
210          (let ((B  (make-B order a b va vb))
211                (c  (make-c a b va vb)))
212            (let* ((BAinv  (let ((K  (matrix-zeros n n order)))
213                             (blas:dgemm! order blas:NoTrans blas:NoTrans n n n 1.0 B Ainv 0.0 K))))
214              (let ((D  (let ((K (matrix-ones n n order)))
215                          (blas:dgemm! order blas:NoTrans blas:NoTrans n n n 1.0 A BAinv 2.0 K)))
216                    (d     (let ((v (matrix-zeros n 1)))
217                             (blas:dgemv! order blas:NoTrans n n 1.0 A c 0.0 v))))
218                  (print "Ainv = " Ainv)
219                  (print "A = " A)
220                  (print "B = " B)
221                  (print "BAinv = " BAinv)
222                  (print "D = " D)
223                  (let ((C   (let ((K (matrix-ones n n order)))
224                               (matrix-map round (blas:daxpy! (fx* n n) -2.0 K D))))
225                        (V   (make-V order p1 p2)))
226                    (print "C = " C)
227                    (print "V = " V)
228                    (make-nordsieck-def order Ainv A B C V d (list) dhistlen p1 p2 m n))))))))))
229 
230(define (nordsieck-step def prev h frhs xrhs)
231  (let ((order (nordsieck-def-order def))
232        (m     (nordsieck-def-m def))
233        (n     (nordsieck-def-n def))
234        (C     (nordsieck-def-C def))
235        (d     (nordsieck-def-d def)))
236  (let ((y0  (let ((v (matrix-zeros n 1)))
237               (blas:dgemv! blas:NoTrans n n 1.0 C prev 0.0 v))))
238    (let loop ((i 0) (m m) (ym (list y0)))
239      (if (fx> m 0)  ym
240          (let ((yi  (let ((fy  (- (* h (frhs xrhs (f64vector-ref (car ym) 0))) 
241                                   (* h (f64vector-ref (car ym) 1)))))
242                       (blas:daxpy n fy d (car ym)))))
243            (loop (fx+ 1 i) (fx- m 1) (cons yi ym))))))))
244
245 
246(define (nordsieck-hscale def h r)
247  (let ((order (nordsieck-def-order def))
248        (n     (nordsieck-def-n def))
249        (V     (nordsieck-def-V def))
250        (d     (nordsieck-def-d def)))
251    (let* ((gamma  (blas:dicopy n d)))
252      (let ((D  (let ((s  (list->f64vector (list-tabulate n (lambda (x) (expt r x))))))
253                  (matrix-diag n n s 0 order)))
254            (e  (let ((e (matrix-zeros 1 n order)))
255                  (f64vector-set! e 0 1.0)
256                  (f64vector-set! e 1 1.0)
257                  e)))
258        (let ((K  (let ((K (matrix-eye n n order)))
259                    (blas:dgemm! order blas:Trans blas:NoTrans n n 1 -1.0 gamma e 1.0 K))))
260          (let ((KV  (let ((R (matrix-zeros n n)))
261                       (blas:dgemm! order blas:Trans blas:Trans n n n 1.0 K V 0.0 R))))
262            (let ((DKV  (let ((R (matrix-zeros n n)))
263                          (blas:dgemm! order blas:Trans blas:Trans n n n 1.0 D KV 0.0 R))))
264              (let ((d1  (let ((v (matrix-zeros n 1)))
265                           (blas:dgemv! order blas:NoTrans n n 1.0 DKV gamma 0.0 v))))
266                (match def 
267                       (($ nordsieck-def order Ainv A B C V d dhist dhistlen p1 p2 m n)
268                        (let ((dhist  (if (fx>= (length dhist) dhistlen)
269                                          (take dhist (fx- dhistlen 1)) dhist)))
270                          (make-nordsieck-def order Ainv A B C V d1 (cons d dhist) dhistlen p1 p2 m n))))))))))))
271       
Note: See TracBrowser for help on using the repository browser.