1 | |
---|
2 | ! |
---|
3 | ! This program may be freely redistributed under the |
---|
4 | ! condition that the copyright notices (including this |
---|
5 | ! entire header) are not removed, and no compensation |
---|
6 | ! is received through use of the software. Private, |
---|
7 | ! research, and institutional use is free. You may |
---|
8 | ! distribute modified versions of this code UNDER THE |
---|
9 | ! CONDITION THAT THIS CODE AND ANY MODIFICATIONS MADE |
---|
10 | ! TO IT IN THE SAME FILE REMAIN UNDER COPYRIGHT OF THE |
---|
11 | ! ORIGINAL AUTHOR, BOTH SOURCE AND OBJECT CODE ARE |
---|
12 | ! MADE FREELY AVAILABLE WITHOUT CHARGE, AND CLEAR |
---|
13 | ! NOTICE IS GIVEN OF THE MODIFICATIONS. Distribution |
---|
14 | ! of this code as part of a commercial system is |
---|
15 | ! permissible ONLY BY DIRECT ARRANGEMENT WITH THE |
---|
16 | ! AUTHOR. (If you are not directly supplying this |
---|
17 | ! code to a customer, and you are instead telling them |
---|
18 | ! how they can obtain it for free, then you are not |
---|
19 | ! required to make any arrangement with me.) |
---|
20 | ! |
---|
21 | ! Disclaimer: Neither I nor: Columbia University, the |
---|
22 | ! National Aeronautics and Space Administration, nor |
---|
23 | ! the Massachusetts Institute of Technology warrant |
---|
24 | ! or certify this code in any way whatsoever. This |
---|
25 | ! code is provided "as-is" to be used at your own risk. |
---|
26 | ! |
---|
27 | ! |
---|
28 | |
---|
29 | ! |
---|
30 | ! INV.f90: block-wise solution of small linear systems. |
---|
31 | ! |
---|
32 | ! Darren Engwirda |
---|
33 | ! 25-Mar-2019 |
---|
34 | ! de2363 [at] columbia [dot] edu |
---|
35 | ! |
---|
36 | ! |
---|
37 | |
---|
38 | pure subroutine inv_2x2(amat,adim,ainv,vdim, & |
---|
39 | & adet) |
---|
40 | |
---|
41 | implicit none |
---|
42 | |
---|
43 | !------------------------------------------- arguments ! |
---|
44 | integer, intent( in) :: adim |
---|
45 | real*8 , intent( in) :: amat(adim,*) |
---|
46 | integer, intent( in) :: vdim |
---|
47 | real*8 , intent(out) :: ainv(vdim,*) |
---|
48 | real*8 , intent(out) :: adet |
---|
49 | |
---|
50 | !------------------------------------------- form A^-1 ! |
---|
51 | |
---|
52 | adet = amat(1,1) * amat(2,2) & |
---|
53 | - amat(1,2) * amat(2,1) |
---|
54 | |
---|
55 | ainv(1,1) = amat(2,2) |
---|
56 | ainv(1,2) = - amat(1,2) |
---|
57 | ainv(2,1) = - amat(2,1) |
---|
58 | ainv(2,2) = amat(1,1) |
---|
59 | |
---|
60 | return |
---|
61 | |
---|
62 | end subroutine |
---|
63 | |
---|
64 | pure subroutine inv_3x3(amat,adim,ainv,vdim, & |
---|
65 | & adet) |
---|
66 | |
---|
67 | implicit none |
---|
68 | |
---|
69 | !------------------------------------------- arguments ! |
---|
70 | integer, intent( in) :: adim |
---|
71 | real*8 , intent( in) :: amat(adim,*) |
---|
72 | integer, intent( in) :: vdim |
---|
73 | real*8 , intent(out) :: ainv(vdim,*) |
---|
74 | real*8 , intent(out) :: adet |
---|
75 | |
---|
76 | !------------------------------------------- variables ! |
---|
77 | real*8 :: & |
---|
78 | aa2233,aa2332,aa2133,aa2331,aa2132,& |
---|
79 | aa2231,aa1233,aa1332,aa1223,aa1322,& |
---|
80 | aa1133,aa1331,aa1123,aa1321,aa1132,& |
---|
81 | aa1231,aa1122,aa1221 |
---|
82 | |
---|
83 | !------------------------------------------- form A^-1 ! |
---|
84 | |
---|
85 | aa2233 = amat(2,2) * amat(3,3) |
---|
86 | aa2332 = amat(2,3) * amat(3,2) |
---|
87 | aa2133 = amat(2,1) * amat(3,3) |
---|
88 | aa2331 = amat(2,3) * amat(3,1) |
---|
89 | aa2132 = amat(2,1) * amat(3,2) |
---|
90 | aa2231 = amat(2,2) * amat(3,1) |
---|
91 | |
---|
92 | adet = & |
---|
93 | amat(1,1) * (aa2233 - aa2332) - & |
---|
94 | amat(1,2) * (aa2133 - aa2331) + & |
---|
95 | amat(1,3) * (aa2132 - aa2231) |
---|
96 | |
---|
97 | aa1233 = amat(1,2) * amat(3,3) |
---|
98 | aa1332 = amat(1,3) * amat(3,2) |
---|
99 | aa1223 = amat(1,2) * amat(2,3) |
---|
100 | aa1322 = amat(1,3) * amat(2,2) |
---|
101 | aa1133 = amat(1,1) * amat(3,3) |
---|
102 | aa1331 = amat(1,3) * amat(3,1) |
---|
103 | aa1123 = amat(1,1) * amat(2,3) |
---|
104 | aa1321 = amat(1,3) * amat(2,1) |
---|
105 | aa1132 = amat(1,1) * amat(3,2) |
---|
106 | aa1231 = amat(1,2) * amat(3,1) |
---|
107 | aa1122 = amat(1,1) * amat(2,2) |
---|
108 | aa1221 = amat(1,2) * amat(2,1) |
---|
109 | |
---|
110 | ainv(1,1) = (aa2233 - aa2332) |
---|
111 | ainv(1,2) = -(aa1233 - aa1332) |
---|
112 | ainv(1,3) = (aa1223 - aa1322) |
---|
113 | |
---|
114 | ainv(2,1) = -(aa2133 - aa2331) |
---|
115 | ainv(2,2) = (aa1133 - aa1331) |
---|
116 | ainv(2,3) = -(aa1123 - aa1321) |
---|
117 | |
---|
118 | ainv(3,1) = (aa2132 - aa2231) |
---|
119 | ainv(3,2) = -(aa1132 - aa1231) |
---|
120 | ainv(3,3) = (aa1122 - aa1221) |
---|
121 | |
---|
122 | return |
---|
123 | |
---|
124 | end subroutine |
---|
125 | |
---|
126 | pure subroutine mul_2x2(amat,adim,bmat,bdim, & |
---|
127 | & scal,cmat,cdim) |
---|
128 | |
---|
129 | implicit none |
---|
130 | |
---|
131 | !------------------------------------------- arguments ! |
---|
132 | integer, intent(in) :: adim |
---|
133 | real*8 , intent(in) :: amat(adim,*) |
---|
134 | integer, intent(in) :: bdim |
---|
135 | real*8 , intent(in) :: bmat(bdim,*) |
---|
136 | real*8 , intent(in) :: scal |
---|
137 | integer, intent(in) :: cdim |
---|
138 | real*8 , intent(inout) :: cmat(cdim,*) |
---|
139 | |
---|
140 | !-------------------------------- C = C + scal * A * B ! |
---|
141 | |
---|
142 | if (scal .eq. +1.d0) then |
---|
143 | |
---|
144 | cmat(1,1) = cmat(1,1) & |
---|
145 | + ( amat(1,1) * bmat(1,1) & |
---|
146 | + amat(1,2) * bmat(2,1) ) |
---|
147 | cmat(2,1) = cmat(2,1) & |
---|
148 | + ( amat(2,1) * bmat(1,1) & |
---|
149 | + amat(2,2) * bmat(2,1) ) |
---|
150 | |
---|
151 | cmat(1,2) = cmat(1,2) & |
---|
152 | + ( amat(1,1) * bmat(1,2) & |
---|
153 | + amat(1,2) * bmat(2,2) ) |
---|
154 | cmat(2,2) = cmat(2,2) & |
---|
155 | + ( amat(2,1) * bmat(1,2) & |
---|
156 | + amat(2,2) * bmat(2,2) ) |
---|
157 | |
---|
158 | else & |
---|
159 | if (scal .eq. -1.d0) then |
---|
160 | |
---|
161 | cmat(1,1) = cmat(1,1) & |
---|
162 | - ( amat(1,1) * bmat(1,1) & |
---|
163 | + amat(1,2) * bmat(2,1) ) |
---|
164 | cmat(2,1) = cmat(2,1) & |
---|
165 | - ( amat(2,1) * bmat(1,1) & |
---|
166 | + amat(2,2) * bmat(2,1) ) |
---|
167 | |
---|
168 | cmat(1,2) = cmat(1,2) & |
---|
169 | - ( amat(1,1) * bmat(1,2) & |
---|
170 | + amat(1,2) * bmat(2,2) ) |
---|
171 | cmat(2,2) = cmat(2,2) & |
---|
172 | - ( amat(2,1) * bmat(1,2) & |
---|
173 | + amat(2,2) * bmat(2,2) ) |
---|
174 | |
---|
175 | else |
---|
176 | |
---|
177 | cmat(1,1) = cmat(1,1) + & |
---|
178 | scal * ( amat(1,1) * bmat(1,1) & |
---|
179 | + amat(1,2) * bmat(2,1) ) |
---|
180 | cmat(2,1) = cmat(2,1) + & |
---|
181 | scal * ( amat(2,1) * bmat(1,1) & |
---|
182 | + amat(2,2) * bmat(2,1) ) |
---|
183 | |
---|
184 | cmat(1,2) = cmat(1,2) + & |
---|
185 | scal * ( amat(1,1) * bmat(1,2) & |
---|
186 | + amat(1,2) * bmat(2,2) ) |
---|
187 | cmat(2,2) = cmat(2,2) + & |
---|
188 | scal * ( amat(2,1) * bmat(1,2) & |
---|
189 | + amat(2,2) * bmat(2,2) ) |
---|
190 | |
---|
191 | end if |
---|
192 | |
---|
193 | return |
---|
194 | |
---|
195 | end subroutine |
---|
196 | |
---|
197 | pure subroutine mul_3x3(amat,adim,bmat,bdim, & |
---|
198 | & scal,cmat,cdim) |
---|
199 | |
---|
200 | implicit none |
---|
201 | |
---|
202 | !------------------------------------------- arguments ! |
---|
203 | integer, intent(in) :: adim |
---|
204 | real*8 , intent(in) :: amat(adim,*) |
---|
205 | integer, intent(in) :: bdim |
---|
206 | real*8 , intent(in) :: bmat(bdim,*) |
---|
207 | real*8 , intent(in) :: scal |
---|
208 | integer, intent(in) :: cdim |
---|
209 | real*8 , intent(inout) :: cmat(cdim,*) |
---|
210 | |
---|
211 | !-------------------------------- C = C + scal * A * B ! |
---|
212 | |
---|
213 | if (scal .eq. +1.d0) then |
---|
214 | |
---|
215 | cmat(1,1) = cmat(1,1) & |
---|
216 | + ( amat(1,1) * bmat(1,1) & |
---|
217 | + amat(1,2) * bmat(2,1) & |
---|
218 | + amat(1,3) * bmat(3,1) ) |
---|
219 | cmat(2,1) = cmat(2,1) & |
---|
220 | + ( amat(2,1) * bmat(1,1) & |
---|
221 | + amat(2,2) * bmat(2,1) & |
---|
222 | + amat(2,3) * bmat(3,1) ) |
---|
223 | cmat(3,1) = cmat(3,1) & |
---|
224 | + ( amat(3,1) * bmat(1,1) & |
---|
225 | + amat(3,2) * bmat(2,1) & |
---|
226 | + amat(3,3) * bmat(3,1) ) |
---|
227 | |
---|
228 | cmat(1,2) = cmat(1,2) & |
---|
229 | + ( amat(1,1) * bmat(1,2) & |
---|
230 | + amat(1,2) * bmat(2,2) & |
---|
231 | + amat(1,3) * bmat(3,2) ) |
---|
232 | cmat(2,2) = cmat(2,2) & |
---|
233 | + ( amat(2,1) * bmat(1,2) & |
---|
234 | + amat(2,2) * bmat(2,2) & |
---|
235 | + amat(2,3) * bmat(3,2) ) |
---|
236 | cmat(3,2) = cmat(3,2) & |
---|
237 | + ( amat(3,1) * bmat(1,2) & |
---|
238 | + amat(3,2) * bmat(2,2) & |
---|
239 | + amat(3,3) * bmat(3,2) ) |
---|
240 | |
---|
241 | cmat(1,3) = cmat(1,3) & |
---|
242 | + ( amat(1,1) * bmat(1,3) & |
---|
243 | + amat(1,2) * bmat(2,3) & |
---|
244 | + amat(1,3) * bmat(3,3) ) |
---|
245 | cmat(2,3) = cmat(2,3) & |
---|
246 | + ( amat(2,1) * bmat(1,3) & |
---|
247 | + amat(2,2) * bmat(2,3) & |
---|
248 | + amat(2,3) * bmat(3,3) ) |
---|
249 | cmat(3,3) = cmat(3,3) & |
---|
250 | + ( amat(3,1) * bmat(1,3) & |
---|
251 | + amat(3,2) * bmat(2,3) & |
---|
252 | + amat(3,3) * bmat(3,3) ) |
---|
253 | |
---|
254 | else & |
---|
255 | if (scal .eq. -1.d0) then |
---|
256 | |
---|
257 | cmat(1,1) = cmat(1,1) & |
---|
258 | - ( amat(1,1) * bmat(1,1) & |
---|
259 | + amat(1,2) * bmat(2,1) & |
---|
260 | + amat(1,3) * bmat(3,1) ) |
---|
261 | cmat(2,1) = cmat(2,1) & |
---|
262 | - ( amat(2,1) * bmat(1,1) & |
---|
263 | + amat(2,2) * bmat(2,1) & |
---|
264 | + amat(2,3) * bmat(3,1) ) |
---|
265 | cmat(3,1) = cmat(3,1) & |
---|
266 | - ( amat(3,1) * bmat(1,1) & |
---|
267 | + amat(3,2) * bmat(2,1) & |
---|
268 | + amat(3,3) * bmat(3,1) ) |
---|
269 | |
---|
270 | cmat(1,2) = cmat(1,2) & |
---|
271 | - ( amat(1,1) * bmat(1,2) & |
---|
272 | + amat(1,2) * bmat(2,2) & |
---|
273 | + amat(1,3) * bmat(3,2) ) |
---|
274 | cmat(2,2) = cmat(2,2) & |
---|
275 | - ( amat(2,1) * bmat(1,2) & |
---|
276 | + amat(2,2) * bmat(2,2) & |
---|
277 | + amat(2,3) * bmat(3,2) ) |
---|
278 | cmat(3,2) = cmat(3,2) & |
---|
279 | - ( amat(3,1) * bmat(1,2) & |
---|
280 | + amat(3,2) * bmat(2,2) & |
---|
281 | + amat(3,3) * bmat(3,2) ) |
---|
282 | |
---|
283 | cmat(1,3) = cmat(1,3) & |
---|
284 | - ( amat(1,1) * bmat(1,3) & |
---|
285 | + amat(1,2) * bmat(2,3) & |
---|
286 | + amat(1,3) * bmat(3,3) ) |
---|
287 | cmat(2,3) = cmat(2,3) & |
---|
288 | - ( amat(2,1) * bmat(1,3) & |
---|
289 | + amat(2,2) * bmat(2,3) & |
---|
290 | + amat(2,3) * bmat(3,3) ) |
---|
291 | cmat(3,3) = cmat(3,3) & |
---|
292 | - ( amat(3,1) * bmat(1,3) & |
---|
293 | + amat(3,2) * bmat(2,3) & |
---|
294 | + amat(3,3) * bmat(3,3) ) |
---|
295 | |
---|
296 | else |
---|
297 | |
---|
298 | cmat(1,1) = cmat(1,1) + & |
---|
299 | scal * ( amat(1,1) * bmat(1,1) & |
---|
300 | + amat(1,2) * bmat(2,1) & |
---|
301 | + amat(1,3) * bmat(3,1) ) |
---|
302 | cmat(2,1) = cmat(2,1) + & |
---|
303 | scal * ( amat(2,1) * bmat(1,1) & |
---|
304 | + amat(2,2) * bmat(2,1) & |
---|
305 | + amat(2,3) * bmat(3,1) ) |
---|
306 | cmat(3,1) = cmat(3,1) + & |
---|
307 | scal * ( amat(3,1) * bmat(1,1) & |
---|
308 | + amat(3,2) * bmat(2,1) & |
---|
309 | + amat(3,3) * bmat(3,1) ) |
---|
310 | |
---|
311 | cmat(1,2) = cmat(1,2) + & |
---|
312 | scal * ( amat(1,1) * bmat(1,2) & |
---|
313 | + amat(1,2) * bmat(2,2) & |
---|
314 | + amat(1,3) * bmat(3,2) ) |
---|
315 | cmat(2,2) = cmat(2,2) + & |
---|
316 | scal * ( amat(2,1) * bmat(1,2) & |
---|
317 | + amat(2,2) * bmat(2,2) & |
---|
318 | + amat(2,3) * bmat(3,2) ) |
---|
319 | cmat(3,2) = cmat(3,2) + & |
---|
320 | scal * ( amat(3,1) * bmat(1,2) & |
---|
321 | + amat(3,2) * bmat(2,2) & |
---|
322 | + amat(3,3) * bmat(3,2) ) |
---|
323 | |
---|
324 | cmat(1,3) = cmat(1,3) + & |
---|
325 | scal * ( amat(1,1) * bmat(1,3) & |
---|
326 | + amat(1,2) * bmat(2,3) & |
---|
327 | + amat(1,3) * bmat(3,3) ) |
---|
328 | cmat(2,3) = cmat(2,3) + & |
---|
329 | scal * ( amat(2,1) * bmat(1,3) & |
---|
330 | + amat(2,2) * bmat(2,3) & |
---|
331 | + amat(2,3) * bmat(3,3) ) |
---|
332 | cmat(3,3) = cmat(3,3) + & |
---|
333 | scal * ( amat(3,1) * bmat(1,3) & |
---|
334 | + amat(3,2) * bmat(2,3) & |
---|
335 | + amat(3,3) * bmat(3,3) ) |
---|
336 | |
---|
337 | end if |
---|
338 | |
---|
339 | return |
---|
340 | |
---|
341 | end subroutine |
---|
342 | |
---|
343 | pure subroutine slv_2x2(amat,adim,vrhs,vdim, & |
---|
344 | & nrhs,fEPS,okay) |
---|
345 | |
---|
346 | implicit none |
---|
347 | |
---|
348 | !------------------------------------------- arguments ! |
---|
349 | integer, intent(in) :: adim |
---|
350 | real*8 , intent(in) :: amat(adim,*) |
---|
351 | integer, intent(in) :: vdim |
---|
352 | real*8 , intent(inout) :: vrhs(vdim,*) |
---|
353 | integer, intent(in) :: nrhs |
---|
354 | real*8 , intent(in) :: fEPS |
---|
355 | logical, intent(inout) :: okay |
---|
356 | |
---|
357 | !------------------------------------------- variables ! |
---|
358 | real*8 :: ainv(2,2) |
---|
359 | real*8 :: adet |
---|
360 | real*8 :: vtmp( 2) |
---|
361 | integer :: irhs |
---|
362 | |
---|
363 | integer, parameter :: LDIM = 2 |
---|
364 | |
---|
365 | !---------------------------------------- calc. inv(A) ! |
---|
366 | |
---|
367 | call inv_2x2(amat,adim,ainv,LDIM,& |
---|
368 | adet) |
---|
369 | |
---|
370 | okay = (abs(adet) .gt. fEPS) |
---|
371 | |
---|
372 | if (okay.eqv..false.) return |
---|
373 | |
---|
374 | !---------------------------------------- v = A^-1 * v ! |
---|
375 | |
---|
376 | do irhs = 1, nrhs |
---|
377 | |
---|
378 | vtmp(1) = & |
---|
379 | + ( & |
---|
380 | ainv(1, 1) * vrhs(1,irhs) & |
---|
381 | + ainv(1, 2) * vrhs(2,irhs) & |
---|
382 | ) / adet |
---|
383 | |
---|
384 | vtmp(2) = & |
---|
385 | + ( & |
---|
386 | ainv(2, 1) * vrhs(1,irhs) & |
---|
387 | + ainv(2, 2) * vrhs(2,irhs) & |
---|
388 | ) / adet |
---|
389 | |
---|
390 | vrhs(1,irhs) = vtmp(1) |
---|
391 | vrhs(2,irhs) = vtmp(2) |
---|
392 | |
---|
393 | end do |
---|
394 | |
---|
395 | return |
---|
396 | |
---|
397 | end subroutine |
---|
398 | |
---|
399 | pure subroutine slv_3x3(amat,adim,vrhs,vdim, & |
---|
400 | & nrhs,fEPS,okay) |
---|
401 | |
---|
402 | implicit none |
---|
403 | |
---|
404 | !------------------------------------------- arguments ! |
---|
405 | integer, intent(in) :: adim |
---|
406 | real*8 , intent(in) :: amat(adim,*) |
---|
407 | integer, intent(in) :: vdim |
---|
408 | real*8 , intent(inout) :: vrhs(vdim,*) |
---|
409 | integer, intent(in) :: nrhs |
---|
410 | real*8 , intent(in) :: fEPS |
---|
411 | logical, intent(inout) :: okay |
---|
412 | |
---|
413 | !------------------------------------------- variables ! |
---|
414 | real*8 :: ainv(3,3) |
---|
415 | real*8 :: adet |
---|
416 | real*8 :: vtmp( 3) |
---|
417 | integer :: irhs |
---|
418 | |
---|
419 | integer, parameter :: LDIM = 3 |
---|
420 | |
---|
421 | !---------------------------------------- calc. inv(A) ! |
---|
422 | |
---|
423 | call inv_3x3(amat,adim,ainv,LDIM,& |
---|
424 | adet) |
---|
425 | |
---|
426 | okay = (abs(adet) .gt. fEPS) |
---|
427 | |
---|
428 | if (okay.eqv..false.) return |
---|
429 | |
---|
430 | !---------------------------------------- v = A^-1 * v ! |
---|
431 | |
---|
432 | do irhs = 1, nrhs |
---|
433 | |
---|
434 | vtmp(1) = & |
---|
435 | + ( & |
---|
436 | ainv(1, 1) * vrhs(1,irhs) & |
---|
437 | + ainv(1, 2) * vrhs(2,irhs) & |
---|
438 | + ainv(1, 3) * vrhs(3,irhs) & |
---|
439 | ) / adet |
---|
440 | |
---|
441 | vtmp(2) = & |
---|
442 | + ( & |
---|
443 | ainv(2, 1) * vrhs(1,irhs) & |
---|
444 | + ainv(2, 2) * vrhs(2,irhs) & |
---|
445 | + ainv(2, 3) * vrhs(3,irhs) & |
---|
446 | ) / adet |
---|
447 | |
---|
448 | vtmp(3) = & |
---|
449 | + ( & |
---|
450 | ainv(3, 1) * vrhs(1,irhs) & |
---|
451 | + ainv(3, 2) * vrhs(2,irhs) & |
---|
452 | + ainv(3, 3) * vrhs(3,irhs) & |
---|
453 | ) / adet |
---|
454 | |
---|
455 | vrhs(1,irhs) = vtmp(1) |
---|
456 | vrhs(2,irhs) = vtmp(2) |
---|
457 | vrhs(3,irhs) = vtmp(3) |
---|
458 | |
---|
459 | end do |
---|
460 | |
---|
461 | return |
---|
462 | |
---|
463 | end subroutine |
---|
464 | |
---|
465 | pure subroutine slv_4x4(amat,adim,vrhs,vdim, & |
---|
466 | & nrhs,fEPS,okay) |
---|
467 | |
---|
468 | implicit none |
---|
469 | |
---|
470 | !------------------------------------------- arguments ! |
---|
471 | integer, intent(in) :: adim |
---|
472 | real*8 , intent(in) :: amat(adim,*) |
---|
473 | integer, intent(in) :: vdim |
---|
474 | real*8 , intent(inout) :: vrhs(vdim,*) |
---|
475 | integer, intent(in) :: nrhs |
---|
476 | real*8 , intent(in) :: fEPS |
---|
477 | logical, intent(inout) :: okay |
---|
478 | |
---|
479 | !------------------------------------------- variables ! |
---|
480 | real*8 :: ainv(2,2) |
---|
481 | real*8 :: lmat(2,2) |
---|
482 | real*8 :: umat(2,2) |
---|
483 | real*8 :: smat(2,2) |
---|
484 | real*8 :: sinv(2,2) |
---|
485 | real*8 :: adet,sdet |
---|
486 | real*8 :: vtmp( 2) |
---|
487 | integer :: irhs |
---|
488 | |
---|
489 | integer, parameter :: LDIM = 2 |
---|
490 | |
---|
491 | !---------------------- form a block LDU factorisation ! |
---|
492 | |
---|
493 | call inv_2x2(amat(1,1),adim,ainv,LDIM, & |
---|
494 | adet) |
---|
495 | |
---|
496 | okay = (abs(adet) .gt. fEPS) |
---|
497 | |
---|
498 | if (okay.eqv..false.) return |
---|
499 | |
---|
500 | !---------------------------------------- L = C * A^-1 ! |
---|
501 | |
---|
502 | lmat(1,1) = +0.d0 |
---|
503 | lmat(1,2) = +0.d0 |
---|
504 | lmat(2,1) = +0.d0 |
---|
505 | lmat(2,2) = +0.d0 |
---|
506 | |
---|
507 | call mul_2x2(amat(3,1),adim,ainv,LDIM, & |
---|
508 | +1.d0,lmat,LDIM) |
---|
509 | |
---|
510 | !---------------------------------------- U = A^-1 * B ! |
---|
511 | |
---|
512 | umat(1,1) = +0.d0 |
---|
513 | umat(1,2) = +0.d0 |
---|
514 | umat(2,1) = +0.d0 |
---|
515 | umat(2,2) = +0.d0 |
---|
516 | |
---|
517 | call mul_2x2(ainv,LDIM,amat(1,3),adim, & |
---|
518 | +1.d0,umat,LDIM) |
---|
519 | |
---|
520 | !-------------------------------- S = D - C * A^-1 * B ! |
---|
521 | |
---|
522 | smat(1,1) = amat(3,3) |
---|
523 | smat(1,2) = amat(3,4) |
---|
524 | smat(2,1) = amat(4,3) |
---|
525 | smat(2,2) = amat(4,4) |
---|
526 | |
---|
527 | call mul_2x2(lmat,LDIM,amat(1,3),adim, & |
---|
528 | -1.d0/adet,smat,LDIM) |
---|
529 | |
---|
530 | call inv_2x2(smat,LDIM,sinv,LDIM,sdet) |
---|
531 | |
---|
532 | okay = (abs(adet) .gt. fEPS) |
---|
533 | |
---|
534 | if (okay.eqv..false.) return |
---|
535 | |
---|
536 | !-------------------------------- back-solve LDU = rhs ! |
---|
537 | |
---|
538 | do irhs = 1, nrhs |
---|
539 | |
---|
540 | !---------------------------------------- solve L part ! |
---|
541 | |
---|
542 | vrhs(3,irhs) = vrhs(3,irhs) & |
---|
543 | - ( & |
---|
544 | lmat(1, 1) * vrhs(1,irhs) & |
---|
545 | + lmat(1, 2) * vrhs(2,irhs) & |
---|
546 | ) / adet |
---|
547 | |
---|
548 | vrhs(4,irhs) = vrhs(4,irhs) & |
---|
549 | - ( & |
---|
550 | lmat(2, 1) * vrhs(1,irhs) & |
---|
551 | + lmat(2, 2) * vrhs(2,irhs) & |
---|
552 | ) / adet |
---|
553 | |
---|
554 | !---------------------------------------- solve D part ! |
---|
555 | |
---|
556 | vtmp(1) = & |
---|
557 | + ( & |
---|
558 | ainv(1, 1) * vrhs(1,irhs) & |
---|
559 | + ainv(1, 2) * vrhs(2,irhs) & |
---|
560 | ) / adet |
---|
561 | |
---|
562 | vtmp(2) = & |
---|
563 | + ( & |
---|
564 | ainv(2, 1) * vrhs(1,irhs) & |
---|
565 | + ainv(2, 2) * vrhs(2,irhs) & |
---|
566 | ) / adet |
---|
567 | |
---|
568 | vrhs(1,irhs) = vtmp(1) |
---|
569 | vrhs(2,irhs) = vtmp(2) |
---|
570 | |
---|
571 | vtmp(1) = & |
---|
572 | + ( & |
---|
573 | sinv(1, 1) * vrhs(3,irhs) & |
---|
574 | + sinv(1, 2) * vrhs(4,irhs) & |
---|
575 | ) / sdet |
---|
576 | |
---|
577 | vtmp(2) = & |
---|
578 | + ( & |
---|
579 | sinv(2, 1) * vrhs(3,irhs) & |
---|
580 | + sinv(2, 2) * vrhs(4,irhs) & |
---|
581 | ) / sdet |
---|
582 | |
---|
583 | vrhs(3,irhs) = vtmp(1) |
---|
584 | vrhs(4,irhs) = vtmp(2) |
---|
585 | |
---|
586 | !---------------------------------------- solve U part ! |
---|
587 | |
---|
588 | vrhs(1,irhs) = vrhs(1,irhs) & |
---|
589 | - ( & |
---|
590 | umat(1, 1) * vrhs(3,irhs) & |
---|
591 | + umat(1, 2) * vrhs(4,irhs) & |
---|
592 | ) / adet |
---|
593 | |
---|
594 | vrhs(2,irhs) = vrhs(2,irhs) & |
---|
595 | - ( & |
---|
596 | umat(2, 1) * vrhs(3,irhs) & |
---|
597 | + umat(2, 2) * vrhs(4,irhs) & |
---|
598 | ) / adet |
---|
599 | |
---|
600 | end do |
---|
601 | |
---|
602 | return |
---|
603 | |
---|
604 | end subroutine |
---|
605 | |
---|
606 | pure subroutine slv_6x6(amat,adim,vrhs,vdim, & |
---|
607 | & nrhs,fEPS,okay) |
---|
608 | |
---|
609 | implicit none |
---|
610 | |
---|
611 | !------------------------------------------- arguments ! |
---|
612 | integer, intent(in) :: adim |
---|
613 | real*8 , intent(in) :: amat(adim,*) |
---|
614 | integer, intent(in) :: vdim |
---|
615 | real*8 , intent(inout) :: vrhs(vdim,*) |
---|
616 | integer, intent(in) :: nrhs |
---|
617 | real*8 , intent(in) :: fEPS |
---|
618 | logical, intent(inout) :: okay |
---|
619 | |
---|
620 | !------------------------------------------- variables ! |
---|
621 | real*8 :: ainv(3,3) |
---|
622 | real*8 :: lmat(3,3) |
---|
623 | real*8 :: umat(3,3) |
---|
624 | real*8 :: smat(3,3) |
---|
625 | real*8 :: sinv(3,3) |
---|
626 | real*8 :: adet,sdet |
---|
627 | real*8 :: vtmp( 3) |
---|
628 | integer :: irhs |
---|
629 | |
---|
630 | integer, parameter :: LDIM = 3 |
---|
631 | |
---|
632 | !---------------------- form a block LDU factorisation ! |
---|
633 | |
---|
634 | call inv_3x3(amat(1,1),adim,ainv,LDIM, & |
---|
635 | adet) |
---|
636 | |
---|
637 | okay = (abs(adet) .gt. fEPS) |
---|
638 | |
---|
639 | if (okay.eqv..false.) return |
---|
640 | |
---|
641 | !---------------------------------------- L = C * A^-1 ! |
---|
642 | |
---|
643 | lmat(1,1) = +0.d0 |
---|
644 | lmat(1,2) = +0.d0 |
---|
645 | lmat(1,3) = +0.d0 |
---|
646 | lmat(2,1) = +0.d0 |
---|
647 | lmat(2,2) = +0.d0 |
---|
648 | lmat(2,3) = +0.d0 |
---|
649 | lmat(3,1) = +0.d0 |
---|
650 | lmat(3,2) = +0.d0 |
---|
651 | lmat(3,3) = +0.d0 |
---|
652 | |
---|
653 | call mul_3x3(amat(4,1),adim,ainv,LDIM, & |
---|
654 | +1.d0,lmat,LDIM) |
---|
655 | |
---|
656 | !---------------------------------------- U = A^-1 * B ! |
---|
657 | |
---|
658 | umat(1,1) = +0.d0 |
---|
659 | umat(1,2) = +0.d0 |
---|
660 | umat(1,3) = +0.d0 |
---|
661 | umat(2,1) = +0.d0 |
---|
662 | umat(2,2) = +0.d0 |
---|
663 | umat(2,3) = +0.d0 |
---|
664 | umat(3,1) = +0.d0 |
---|
665 | umat(3,2) = +0.d0 |
---|
666 | umat(3,3) = +0.d0 |
---|
667 | |
---|
668 | call mul_3x3(ainv,LDIM,amat(1,4),adim, & |
---|
669 | +1.d0,umat,LDIM) |
---|
670 | |
---|
671 | !-------------------------------- S = D - C * A^-1 * B ! |
---|
672 | |
---|
673 | smat(1,1) = amat(4,4) |
---|
674 | smat(1,2) = amat(4,5) |
---|
675 | smat(1,3) = amat(4,6) |
---|
676 | smat(2,1) = amat(5,4) |
---|
677 | smat(2,2) = amat(5,5) |
---|
678 | smat(2,3) = amat(5,6) |
---|
679 | smat(3,1) = amat(6,4) |
---|
680 | smat(3,2) = amat(6,5) |
---|
681 | smat(3,3) = amat(6,6) |
---|
682 | |
---|
683 | call mul_3x3(lmat,LDIM,amat(1,4),adim, & |
---|
684 | -1.d0/adet,smat,LDIM) |
---|
685 | |
---|
686 | call inv_3x3(smat,LDIM,sinv,LDIM,sdet) |
---|
687 | |
---|
688 | okay = (abs(adet) .gt. fEPS) |
---|
689 | |
---|
690 | if (okay.eqv..false.) return |
---|
691 | |
---|
692 | !-------------------------------- back-solve LDU = rhs ! |
---|
693 | |
---|
694 | do irhs = 1, nrhs |
---|
695 | |
---|
696 | !---------------------------------------- solve L part ! |
---|
697 | |
---|
698 | vrhs(4,irhs) = vrhs(4,irhs) & |
---|
699 | - ( & |
---|
700 | lmat(1, 1) * vrhs(1,irhs) & |
---|
701 | + lmat(1, 2) * vrhs(2,irhs) & |
---|
702 | + lmat(1, 3) * vrhs(3,irhs) & |
---|
703 | ) / adet |
---|
704 | |
---|
705 | vrhs(5,irhs) = vrhs(5,irhs) & |
---|
706 | - ( & |
---|
707 | lmat(2, 1) * vrhs(1,irhs) & |
---|
708 | + lmat(2, 2) * vrhs(2,irhs) & |
---|
709 | + lmat(2, 3) * vrhs(3,irhs) & |
---|
710 | ) / adet |
---|
711 | |
---|
712 | vrhs(6,irhs) = vrhs(6,irhs) & |
---|
713 | - ( & |
---|
714 | lmat(3, 1) * vrhs(1,irhs) & |
---|
715 | + lmat(3, 2) * vrhs(2,irhs) & |
---|
716 | + lmat(3, 3) * vrhs(3,irhs) & |
---|
717 | ) / adet |
---|
718 | |
---|
719 | !---------------------------------------- solve D part ! |
---|
720 | |
---|
721 | vtmp(1) = & |
---|
722 | + ( & |
---|
723 | ainv(1, 1) * vrhs(1,irhs) & |
---|
724 | + ainv(1, 2) * vrhs(2,irhs) & |
---|
725 | + ainv(1, 3) * vrhs(3,irhs) & |
---|
726 | ) / adet |
---|
727 | |
---|
728 | vtmp(2) = & |
---|
729 | + ( & |
---|
730 | ainv(2, 1) * vrhs(1,irhs) & |
---|
731 | + ainv(2, 2) * vrhs(2,irhs) & |
---|
732 | + ainv(2, 3) * vrhs(3,irhs) & |
---|
733 | ) / adet |
---|
734 | |
---|
735 | vtmp(3) = & |
---|
736 | + ( & |
---|
737 | ainv(3, 1) * vrhs(1,irhs) & |
---|
738 | + ainv(3, 2) * vrhs(2,irhs) & |
---|
739 | + ainv(3, 3) * vrhs(3,irhs) & |
---|
740 | ) / adet |
---|
741 | |
---|
742 | vrhs(1,irhs) = vtmp(1) |
---|
743 | vrhs(2,irhs) = vtmp(2) |
---|
744 | vrhs(3,irhs) = vtmp(3) |
---|
745 | |
---|
746 | vtmp(1) = & |
---|
747 | + ( & |
---|
748 | sinv(1, 1) * vrhs(4,irhs) & |
---|
749 | + sinv(1, 2) * vrhs(5,irhs) & |
---|
750 | + sinv(1, 3) * vrhs(6,irhs) & |
---|
751 | ) / sdet |
---|
752 | |
---|
753 | vtmp(2) = & |
---|
754 | + ( & |
---|
755 | sinv(2, 1) * vrhs(4,irhs) & |
---|
756 | + sinv(2, 2) * vrhs(5,irhs) & |
---|
757 | + sinv(2, 3) * vrhs(6,irhs) & |
---|
758 | ) / sdet |
---|
759 | |
---|
760 | vtmp(3) = & |
---|
761 | + ( & |
---|
762 | sinv(3, 1) * vrhs(4,irhs) & |
---|
763 | + sinv(3, 2) * vrhs(5,irhs) & |
---|
764 | + sinv(3, 3) * vrhs(6,irhs) & |
---|
765 | ) / sdet |
---|
766 | |
---|
767 | vrhs(4,irhs) = vtmp(1) |
---|
768 | vrhs(5,irhs) = vtmp(2) |
---|
769 | vrhs(6,irhs) = vtmp(3) |
---|
770 | |
---|
771 | !---------------------------------------- solve U part ! |
---|
772 | |
---|
773 | vrhs(1,irhs) = vrhs(1,irhs) & |
---|
774 | - ( & |
---|
775 | umat(1, 1) * vrhs(4,irhs) & |
---|
776 | + umat(1, 2) * vrhs(5,irhs) & |
---|
777 | + umat(1, 3) * vrhs(6,irhs) & |
---|
778 | ) / adet |
---|
779 | |
---|
780 | vrhs(2,irhs) = vrhs(2,irhs) & |
---|
781 | - ( & |
---|
782 | umat(2, 1) * vrhs(4,irhs) & |
---|
783 | + umat(2, 2) * vrhs(5,irhs) & |
---|
784 | + umat(2, 3) * vrhs(6,irhs) & |
---|
785 | ) / adet |
---|
786 | |
---|
787 | vrhs(3,irhs) = vrhs(3,irhs) & |
---|
788 | - ( & |
---|
789 | umat(3, 1) * vrhs(4,irhs) & |
---|
790 | + umat(3, 2) * vrhs(5,irhs) & |
---|
791 | + umat(3, 3) * vrhs(6,irhs) & |
---|
792 | ) / adet |
---|
793 | |
---|
794 | end do |
---|
795 | |
---|
796 | return |
---|
797 | |
---|
798 | end subroutine |
---|
799 | |
---|
800 | |
---|
801 | |
---|