[13926] | 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 | ! |
---|
[14212] | 30 | ! INV.h90: block-wise solution of small linear systems. |
---|
[13926] | 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 | |
---|
[14387] | 142 | if (scal .eq. +1.e0) then |
---|
[13926] | 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 & |
---|
[14387] | 159 | if (scal .eq. -1.e0) then |
---|
[13926] | 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 | |
---|
[14387] | 213 | if (scal .eq. +1.e0) then |
---|
[13926] | 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 & |
---|
[14387] | 255 | if (scal .eq. -1.e0) then |
---|
[13926] | 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 | |
---|
[14387] | 502 | lmat(1,1) = +0.e0 |
---|
| 503 | lmat(1,2) = +0.e0 |
---|
| 504 | lmat(2,1) = +0.e0 |
---|
| 505 | lmat(2,2) = +0.e0 |
---|
[13926] | 506 | |
---|
| 507 | call mul_2x2(amat(3,1),adim,ainv,LDIM, & |
---|
[14387] | 508 | +1.e0,lmat,LDIM) |
---|
[13926] | 509 | |
---|
| 510 | !---------------------------------------- U = A^-1 * B ! |
---|
| 511 | |
---|
[14387] | 512 | umat(1,1) = +0.e0 |
---|
| 513 | umat(1,2) = +0.e0 |
---|
| 514 | umat(2,1) = +0.e0 |
---|
| 515 | umat(2,2) = +0.e0 |
---|
[13926] | 516 | |
---|
| 517 | call mul_2x2(ainv,LDIM,amat(1,3),adim, & |
---|
[14387] | 518 | +1.e0,umat,LDIM) |
---|
[13926] | 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, & |
---|
[14387] | 528 | -1.e0/adet,smat,LDIM) |
---|
[13926] | 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 | |
---|
[14387] | 643 | lmat(1,1) = +0.e0 |
---|
| 644 | lmat(1,2) = +0.e0 |
---|
| 645 | lmat(1,3) = +0.e0 |
---|
| 646 | lmat(2,1) = +0.e0 |
---|
| 647 | lmat(2,2) = +0.e0 |
---|
| 648 | lmat(2,3) = +0.e0 |
---|
| 649 | lmat(3,1) = +0.e0 |
---|
| 650 | lmat(3,2) = +0.e0 |
---|
| 651 | lmat(3,3) = +0.e0 |
---|
[13926] | 652 | |
---|
| 653 | call mul_3x3(amat(4,1),adim,ainv,LDIM, & |
---|
[14387] | 654 | +1.e0,lmat,LDIM) |
---|
[13926] | 655 | |
---|
| 656 | !---------------------------------------- U = A^-1 * B ! |
---|
| 657 | |
---|
[14387] | 658 | umat(1,1) = +0.e0 |
---|
| 659 | umat(1,2) = +0.e0 |
---|
| 660 | umat(1,3) = +0.e0 |
---|
| 661 | umat(2,1) = +0.e0 |
---|
| 662 | umat(2,2) = +0.e0 |
---|
| 663 | umat(2,3) = +0.e0 |
---|
| 664 | umat(3,1) = +0.e0 |
---|
| 665 | umat(3,2) = +0.e0 |
---|
| 666 | umat(3,3) = +0.e0 |
---|
[13926] | 667 | |
---|
| 668 | call mul_3x3(ainv,LDIM,amat(1,4),adim, & |
---|
[14387] | 669 | +1.e0,umat,LDIM) |
---|
[13926] | 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, & |
---|
[14387] | 684 | -1.e0/adet,smat,LDIM) |
---|
[13926] | 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 | |
---|