-
Notifications
You must be signed in to change notification settings - Fork 2
/
jacobi.lisp
123 lines (114 loc) · 4.41 KB
/
jacobi.lisp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
;;;JACOBI eigenvector function from numerical recipes
(in-package "CL-USER")
(defun jacobi (a)
(declare (type (simple-array double-float (* *)) a))
(prog* ((nrot 0)
(n (array-dimension a 0))
(d (make-array n :element-type 'double-float :initial-element 0d0))
(v
(make-array (list n n) :element-type 'double-float :initial-element 0d0))
(b (make-array n :element-type 'double-float :initial-element 0d0))
(z (make-array n :element-type 'double-float :initial-element 0d0))
(sm 0d0) (tresh 0d0) (g 0d0) (h 0d0) (t0 0d0)
(theta 0d0) (c 0d0) (s 0d0) (tau 0d0))
(declare (type (simple-array double-float (*)) d))
(declare (type (simple-array double-float (* *)) v))
(declare (type (simple-array double-float (*)) b))
(declare (type (simple-array double-float (*)) z))
(declare (type fixnum n nrot))
(declare (type double-float sm tresh g h t0 theta c s tau))
(do ((ip 1 (+ ip 1)))
((> ip n) t)
(declare (type fixnum ip))
(do ((iq 1 (+ iq 1)))
((> iq n) t)
(declare (type fixnum iq))
(setf (fref v ip iq) 0d0))
(setf (fref v ip ip) 1d0))
(do ((ip 1 (+ ip 1)))
((> ip n) t)
(declare (type fixnum ip))
(setf (fref b ip) (fref a ip ip))
(setf (fref d ip) (fref b ip))
(setf (fref z ip) 0d0))
(setf nrot 0)
(do ((i 1 (+ i 1)))
((> i 50) t)
(declare (type fixnum i))
(setf sm 0d0)
(do ((ip 1 (+ ip 1)))
((> ip (+ n (- 1))) t)
(declare (type fixnum ip))
(do ((iq (+ ip 1) (+ iq 1)))
((> iq n) t)
(declare (type fixnum iq))
(setf sm (+ sm (abs (fref a ip iq))))))
(if (= sm 0d0) (go end))
(if (< i 4)
(setf tresh (/ (* 0.2d0 sm) (dfloat (expt n 2))))
(setf tresh 0d0))
(do ((ip 1 (+ ip 1)))
((> ip (+ n (- 1))) t)
(declare (type fixnum ip))
(do ((iq (+ ip 1) (+ iq 1)))
((> iq n) t)
(declare (type fixnum iq))
(setf g (* 100d0 (abs (fref a ip iq))))
(cond
((and (> i 4)
(= (+ (abs (fref d ip)) g) (abs (fref d ip)))
(= (+ (abs (fref d iq)) g) (abs (fref d iq))))
(setf (fref a ip iq) 0d0))
((> (abs (fref a ip iq)) tresh)
(setf h (+ (fref d iq) (- (fref d ip))))
(cond
((= (+ (abs h) g) (abs h))
(setf t0 (/ (fref a ip iq) h)))
(t
(setf theta (/ (* 0.5d0 h) (fref a ip iq)))
(setf t0 (/ 1d0 (+ (abs theta) (sqrt (1+ (expt theta 2))))))
(if (< theta 0d0) (setf t0 (- t0)))))
(setf c (/ 1d0 (sqrt (+ 1d0 (expt t0 2))))) (setf s (* t0 c))
(setf tau (/ s (1+ c))) (setf h (* t0 (fref a ip iq)))
(setf (fref z ip) (+ (fref z ip) (- h)))
(setf (fref z iq) (+ (fref z iq) h))
(setf (fref d ip) (+ (fref d ip) (- h)))
(setf (fref d iq) (+ (fref d iq) h)) (setf (fref a ip iq) 0d0)
(do ((j 1 (+ j 1)))
((> j (+ ip (- 1))) t)
(declare (type fixnum j))
(setf g (fref a j ip))
(setf h (fref a j iq))
(setf (fref a j ip) (+ g (* (- s) (+ h (* g tau)))))
(setf (fref a j iq) (+ h (* s (+ g (* (- h) tau))))))
(do ((j (+ ip 1) (+ j 1)))
((> j (+ iq (- 1))) t)
(declare (type fixnum j))
(setf g (fref a ip j))
(setf h (fref a j iq))
(setf (fref a ip j) (+ g (* (- s) (+ h (* g tau)))))
(setf (fref a j iq) (+ h (* s (+ g (* (- h) tau))))))
(do ((j (+ iq 1) (+ j 1)))
((> j n) t)
(declare (type fixnum j))
(setf g (fref a ip j))
(setf h (fref a iq j))
(setf (fref a ip j) (+ g (* (- s) (+ h (* g tau)))))
(setf (fref a iq j) (+ h (* s (+ g (* (- h) tau))))))
(do ((j 1 (+ j 1)))
((> j n) t)
(declare (type fixnum j))
(setf g (fref v j ip))
(setf h (fref v j iq))
(setf (fref v j ip) (+ g (* (- s) (+ h (* g tau)))))
(setf (fref v j iq) (+ h (* s (+ g (* (- h) tau))))))
(setf nrot (+ nrot 1))))))
(do ((ip 1 (+ ip 1)))
((> ip n) t)
(declare (type fixnum ip))
(setf (fref b ip) (+ (fref b ip) (fref z ip)))
(setf (fref d ip) (fref b ip))
(setf (fref z ip) 0d0)))
(error "jacobi should not reach this point")
end
(return (values d v nrot))))