| 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.h90: 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 | |
|---|
