| 1 | subroutine setup(resp,a,im,ic,nm,nc) |
|---|
| 2 | |
|---|
| 3 | ! **************************************************** |
|---|
| 4 | ! * |
|---|
| 5 | ! Set DOUBLE PRECISION (A) * |
|---|
| 6 | ! response matrix in FORTRAN storage format * |
|---|
| 7 | ! RESP comes from "C" routine * |
|---|
| 8 | ! * |
|---|
| 9 | ! Author: WFH 05.02.02 * |
|---|
| 10 | ! * |
|---|
| 11 | ! **************************************************** |
|---|
| 12 | implicit none |
|---|
| 13 | |
|---|
| 14 | integer im,ic,nm,nc ! here we are |
|---|
| 15 | double precision resp,a(nm, nc) |
|---|
| 16 | |
|---|
| 17 | ! write(*,*) 'in setup: ',resp, im+1, ic+1 |
|---|
| 18 | a(im+1,ic+1) = resp |
|---|
| 19 | |
|---|
| 20 | return |
|---|
| 21 | end |
|---|
| 22 | |
|---|
| 23 | subroutine setupi(resp,a,im,ic,nm,nc) |
|---|
| 24 | |
|---|
| 25 | ! **************************************************** |
|---|
| 26 | ! * |
|---|
| 27 | ! Set INTEGER (A) * |
|---|
| 28 | ! matrix in FORTRAN storage format * |
|---|
| 29 | ! RESP comes from "C" routine * |
|---|
| 30 | ! * |
|---|
| 31 | ! Author: WFH 05.02.02 * |
|---|
| 32 | ! * |
|---|
| 33 | ! **************************************************** |
|---|
| 34 | implicit none |
|---|
| 35 | |
|---|
| 36 | integer im,ic,nm,nc |
|---|
| 37 | integer resp,a(nm, nc) |
|---|
| 38 | |
|---|
| 39 | ! write(*,*) 'in setupi: ',resp, im+1, ic+1 |
|---|
| 40 | a(im+1,ic+1) = resp |
|---|
| 41 | ! write(*,*) 'done in setupi' |
|---|
| 42 | |
|---|
| 43 | return |
|---|
| 44 | end |
|---|
| 45 | |
|---|
| 46 | subroutine micit(a,conm,xin,cin,res,nx,rms,im,ic,iter,ny,ax,cinx, & |
|---|
| 47 | &xinx,resx,rho,ptop,rmss,xrms,xptp,xiter,ifail) |
|---|
| 48 | |
|---|
| 49 | |
|---|
| 50 | ! **************************************************** |
|---|
| 51 | ! * |
|---|
| 52 | ! Driving routine for MICADO correction * |
|---|
| 53 | ! * |
|---|
| 54 | ! Author: WFH 05.02.02 * |
|---|
| 55 | ! * |
|---|
| 56 | ! **************************************************** |
|---|
| 57 | |
|---|
| 58 | implicit none |
|---|
| 59 | |
|---|
| 60 | integer im,ic,iter,i,j,nx(ic),ny(ic) |
|---|
| 61 | real rms,ax(im,ic),cinx(ic),xinx(im),resx(im),rho(3*ic),ptop(ic), & |
|---|
| 62 | &rmss(ic),xrms(ic),xptp(ic),xiter(ic),rzero |
|---|
| 63 | parameter(rzero=0e0) |
|---|
| 64 | double precision a(im,ic),xin(im),cin(ic),res(im) |
|---|
| 65 | character*16 conm(ic) |
|---|
| 66 | integer n |
|---|
| 67 | integer ifail |
|---|
| 68 | |
|---|
| 69 | do j = 1,ic |
|---|
| 70 | call f_ctof(n, conm(j), 16) |
|---|
| 71 | enddo |
|---|
| 72 | do i = 1,im |
|---|
| 73 | do j = 1,ic |
|---|
| 74 | ax(i,j) = a(i,j) |
|---|
| 75 | ! write(*,*) i,j,ax(i,j),a(i,j) |
|---|
| 76 | ! ny(j) = j-1 |
|---|
| 77 | ny(j) = j |
|---|
| 78 | cinx(j) = rzero |
|---|
| 79 | enddo |
|---|
| 80 | enddo |
|---|
| 81 | do i = 1,im |
|---|
| 82 | xinx(i) = xin(i) |
|---|
| 83 | resx(i) = rzero |
|---|
| 84 | enddo |
|---|
| 85 | write(*,*) ' ' |
|---|
| 86 | write(*,*) 'start MICADO correction with ',iter,' correctors' |
|---|
| 87 | write(*,*) ' ' |
|---|
| 88 | call micado(ax,conm,xinx,resx,cinx,ny,rms,im,ic,iter,rho,ptop, & |
|---|
| 89 | &rmss,xrms,xptp,xiter,ifail) |
|---|
| 90 | do i = 1,ic |
|---|
| 91 | cin(i) = cinx(i) |
|---|
| 92 | nx(ny(i)) = i |
|---|
| 93 | enddo |
|---|
| 94 | do i = 1,im |
|---|
| 95 | res(i) = resx(i) |
|---|
| 96 | enddo |
|---|
| 97 | return |
|---|
| 98 | end |
|---|
| 99 | subroutine haveit(a,xin,cin,res,nx,im,ic,cb,xmeas,xres,y,z,xd) |
|---|
| 100 | ! **************************************************** |
|---|
| 101 | ! * |
|---|
| 102 | ! Driving routine for LSQ correction * |
|---|
| 103 | ! * |
|---|
| 104 | ! Author: WFH 05.02.02 * |
|---|
| 105 | ! * |
|---|
| 106 | ! **************************************************** |
|---|
| 107 | implicit none |
|---|
| 108 | |
|---|
| 109 | |
|---|
| 110 | integer im,ic,nx(ic),i,j |
|---|
| 111 | double precision a(im,ic),xin(im),cin(ic),res(im),cb(ic), & |
|---|
| 112 | &xmeas(im),xres(im),y(ic,im),z(ic,ic),xd(ic),zero |
|---|
| 113 | parameter(zero=0d0) |
|---|
| 114 | |
|---|
| 115 | do i = 1,im |
|---|
| 116 | do j = 1,ic |
|---|
| 117 | ! write(*,*) i,j,a(i,j) |
|---|
| 118 | enddo |
|---|
| 119 | enddo |
|---|
| 120 | do i = 1,im |
|---|
| 121 | res(i) = zero |
|---|
| 122 | enddo |
|---|
| 123 | |
|---|
| 124 | ! write(*,*) '==> ',res |
|---|
| 125 | write(*,*) ' ' |
|---|
| 126 | write(*,*) 'start LEAST SQUARES correction with all correctors' |
|---|
| 127 | write(*,*) ' ' |
|---|
| 128 | call solsql(im,ic,a,xin,res,cin,cb,xmeas,xres,y,z,xd) |
|---|
| 129 | 6001 format(1X,'Corrector: ',I4,' strength: ',F12.8) |
|---|
| 130 | write(*,*) ' ' |
|---|
| 131 | write(*,*) 'end LEAST SQUARES correction with all correctors' |
|---|
| 132 | write(*,*) ' ' |
|---|
| 133 | do i = 1,ic |
|---|
| 134 | ! write(*,*) i,cin(i) |
|---|
| 135 | nx(i) = i |
|---|
| 136 | enddo |
|---|
| 137 | return |
|---|
| 138 | end |
|---|
| 139 | subroutine svddec_m(a,svdmat,umat,vmat,wmat,utmat,vtmat,wtmat, & |
|---|
| 140 | &ws,wvec,sortw, & |
|---|
| 141 | &sngcut, sngval, & |
|---|
| 142 | &im,ic,iflag,sing,dbg) |
|---|
| 143 | ! **************************************************** |
|---|
| 144 | ! * |
|---|
| 145 | ! Performs SVD and analysis for matrix with more * |
|---|
| 146 | ! monitors than correctors. * |
|---|
| 147 | ! * |
|---|
| 148 | ! Author: WFH 12.09.02 * |
|---|
| 149 | ! * |
|---|
| 150 | ! **************************************************** |
|---|
| 151 | implicit none |
|---|
| 152 | integer im,ic,i,j,jj,ii |
|---|
| 153 | integer iflag,sing(2,ic) |
|---|
| 154 | double precision a(im,ic) |
|---|
| 155 | double precision svdmat(im,ic) |
|---|
| 156 | double precision umat(im,ic), utmat(ic,im) |
|---|
| 157 | double precision vmat(im,ic), vtmat(ic,im) |
|---|
| 158 | double precision wmat(im,ic), wtmat(ic,im) |
|---|
| 159 | double precision wvec(ic) |
|---|
| 160 | ! double precision svdtest(10,3) |
|---|
| 161 | double precision rat, zero, sngval, sngcut |
|---|
| 162 | double precision ws(ic) |
|---|
| 163 | integer amater, svdmx, svdnx, svdnm |
|---|
| 164 | integer sortw(ic) |
|---|
| 165 | integer nsing |
|---|
| 166 | logical matu, matv |
|---|
| 167 | integer dbg |
|---|
| 168 | parameter(zero = 0d0) |
|---|
| 169 | parameter(nsing = 5) |
|---|
| 170 | ! data svdtest/3,8,8,8,4,9,9,2,8,8, & |
|---|
| 171 | ! &4,1,7,2,6,1,6,4,6,2, & |
|---|
| 172 | ! &1,9,7,2,2,6,9,8,3,5/ |
|---|
| 173 | |
|---|
| 174 | |
|---|
| 175 | if(dbg.eq.2) then |
|---|
| 176 | write(*,*) 'SVD parameters: ' |
|---|
| 177 | write(*,*) 'SNGCUT: ',sngcut |
|---|
| 178 | write(*,*) 'SNGVAL: ',sngval |
|---|
| 179 | endif |
|---|
| 180 | |
|---|
| 181 | MATU = .TRUE. |
|---|
| 182 | MATV = .TRUE. |
|---|
| 183 | iflag = 0 |
|---|
| 184 | |
|---|
| 185 | SVDNM = max(ic,im) |
|---|
| 186 | svdmx = im |
|---|
| 187 | svdnx = ic |
|---|
| 188 | |
|---|
| 189 | do i = 1,im |
|---|
| 190 | do j = 1,ic |
|---|
| 191 | svdmat(i,j) = a(i,j) |
|---|
| 192 | enddo |
|---|
| 193 | enddo |
|---|
| 194 | |
|---|
| 195 | if(dbg.eq.2) then |
|---|
| 196 | write(*,*) 'A0:' |
|---|
| 197 | do j = 1,im |
|---|
| 198 | write(*,6003) (svdmat(j,i),i=1,ic) |
|---|
| 199 | enddo |
|---|
| 200 | endif |
|---|
| 201 | call svd(svdnm,svdmx,svdnx,svdmat,wvec,matu,umat, & |
|---|
| 202 | &matv,vmat,amater,ws) |
|---|
| 203 | 6001 format(1X,'Corrector: ',I4,' sing: ',F12.4) |
|---|
| 204 | 6002 format('VMAT: ',I4,I4,5X,F12.6,2X,F12.6) |
|---|
| 205 | 6003 format(16(2X,F7.2)) |
|---|
| 206 | 6004 format(16(2X,F7.2)) |
|---|
| 207 | if(amater.ne.0) then |
|---|
| 208 | write(*,*) 'end SVD with error code: ',amater |
|---|
| 209 | endif |
|---|
| 210 | if(dbg.eq.1) then |
|---|
| 211 | do i = 1,ic |
|---|
| 212 | write(*,*) i,wvec(I) |
|---|
| 213 | write(*,6001) i,wvec(I) |
|---|
| 214 | wmat(i,i) = wvec(i) |
|---|
| 215 | enddo |
|---|
| 216 | endif |
|---|
| 217 | call rvord(wvec,sortw,ws,ic) |
|---|
| 218 | if(dbg.eq.1) then |
|---|
| 219 | do i = 1,ic |
|---|
| 220 | write(*,*) i,sortw(i),wvec(sortw(i)) |
|---|
| 221 | enddo |
|---|
| 222 | endif |
|---|
| 223 | do ii = 1,min(nsing,ic) |
|---|
| 224 | i = sortw(ii) |
|---|
| 225 | if(dbg.eq.1) then |
|---|
| 226 | write(61,*) wvec(i) |
|---|
| 227 | endif |
|---|
| 228 | if(abs(wvec(i)).lt.sngval) then |
|---|
| 229 | if(dbg.eq.1) then |
|---|
| 230 | do j = 1,ic |
|---|
| 231 | write(*,6002) i,j,vmat(j,i) |
|---|
| 232 | enddo |
|---|
| 233 | endif |
|---|
| 234 | do j = 1,ic-1 |
|---|
| 235 | do jj = j+1,ic |
|---|
| 236 | if(abs(vmat(j,i)).gt.1.0E-4) then |
|---|
| 237 | rat = abs(vmat(j,i)) + abs(vmat(jj,i)) |
|---|
| 238 | ! rat = abs(vmat(j,i) - vmat(jj,i)) |
|---|
| 239 | ! rat = abs(vmat(j,i) + vmat(jj,i)) |
|---|
| 240 | rat = rat/abs(abs(vmat(j,i)) - abs(vmat(jj,i))) |
|---|
| 241 | if(rat.gt.sngcut) then |
|---|
| 242 | IF(DBG.EQ.1) THEN |
|---|
| 243 | write(*,*) 'dependent pair: ',i,j,jj,rat |
|---|
| 244 | write(65,*) 'dependent pair: ',i,j,jj,rat |
|---|
| 245 | ENDIF |
|---|
| 246 | if(iflag.lt.(ic*ic*ic)) then |
|---|
| 247 | iflag = iflag + 1 |
|---|
| 248 | sing(1,iflag) = j - 1 |
|---|
| 249 | sing(2,iflag) = jj - 1 |
|---|
| 250 | endif |
|---|
| 251 | endif |
|---|
| 252 | endif |
|---|
| 253 | enddo |
|---|
| 254 | enddo |
|---|
| 255 | endif |
|---|
| 256 | enddo |
|---|
| 257 | if(dbg.eq.1) then |
|---|
| 258 | do j=1,iflag |
|---|
| 259 | write(66,*) j,sing(1,j),sing(2,j) |
|---|
| 260 | enddo |
|---|
| 261 | endif |
|---|
| 262 | |
|---|
| 263 | return |
|---|
| 264 | end |
|---|
| 265 | subroutine svddec_c(a,svdmat,umat,vmat,wmat,utmat,vtmat,wtmat, & |
|---|
| 266 | &ws,wvec,sortw, & |
|---|
| 267 | &sngcut, sngval, & |
|---|
| 268 | &im,ic,iflag,sing,dbg) |
|---|
| 269 | ! **************************************************** |
|---|
| 270 | ! * |
|---|
| 271 | ! Performs SVD and analysis for matrix with more * |
|---|
| 272 | ! correctors than monitors. * |
|---|
| 273 | ! * |
|---|
| 274 | ! Author: WFH 12.09.02 * |
|---|
| 275 | ! * |
|---|
| 276 | ! **************************************************** |
|---|
| 277 | implicit none |
|---|
| 278 | integer im,ic,i,j,jj,ii |
|---|
| 279 | integer iflag,sing(2,ic) |
|---|
| 280 | double precision a(im,ic) |
|---|
| 281 | double precision svdmat(ic,im) |
|---|
| 282 | double precision umat(ic,im), utmat(im,ic) |
|---|
| 283 | double precision vmat(ic,im), vtmat(im,ic) |
|---|
| 284 | double precision wmat(ic,im), wtmat(im,ic) |
|---|
| 285 | double precision wvec(ic) |
|---|
| 286 | double precision rat, zero, sngcut, sngval |
|---|
| 287 | double precision ws(ic) |
|---|
| 288 | integer sortw(ic) |
|---|
| 289 | integer amater, svdmx, svdnx, svdnm |
|---|
| 290 | integer nsing |
|---|
| 291 | logical matu, matv |
|---|
| 292 | integer dbg |
|---|
| 293 | parameter(zero = 0d0) |
|---|
| 294 | parameter(nsing = 5) |
|---|
| 295 | |
|---|
| 296 | if(dbg.eq.2) then |
|---|
| 297 | write(*,*) 'SVD parameters: ' |
|---|
| 298 | write(*,*) 'SNGCUT: ',sngcut |
|---|
| 299 | write(*,*) 'SNGVAL: ',sngval |
|---|
| 300 | endif |
|---|
| 301 | |
|---|
| 302 | MATU = .TRUE. |
|---|
| 303 | MATV = .TRUE. |
|---|
| 304 | iflag = 0 |
|---|
| 305 | |
|---|
| 306 | ! im = 10 |
|---|
| 307 | ! ic = 3 |
|---|
| 308 | SVDNM = max(ic,im) |
|---|
| 309 | svdmx = ic |
|---|
| 310 | svdnx = im |
|---|
| 311 | |
|---|
| 312 | do i = 1,im |
|---|
| 313 | do j = 1,ic |
|---|
| 314 | svdmat(j,i) = a(i,j) |
|---|
| 315 | ! svdmat(i,j) = a(i,j) |
|---|
| 316 | enddo |
|---|
| 317 | enddo |
|---|
| 318 | |
|---|
| 319 | 8373 continue |
|---|
| 320 | |
|---|
| 321 | if(dbg.eq.2) then |
|---|
| 322 | write(*,*) 'A0:' |
|---|
| 323 | do j = 1,ic |
|---|
| 324 | write(*,6003) (svdmat(j,i),i=1,im) |
|---|
| 325 | enddo |
|---|
| 326 | endif |
|---|
| 327 | call svd(svdnm,svdmx,svdnx,svdmat,wvec,matu,umat, & |
|---|
| 328 | &matv,vmat,amater,ws) |
|---|
| 329 | 6001 format(1X,'Corrector: ',I4,' sing: ',F12.4) |
|---|
| 330 | 6002 format('UMAT: ',I4,I4,5X,F12.6,2X,F12.6) |
|---|
| 331 | 6003 format(16(2X,F7.2)) |
|---|
| 332 | 6004 format(16(2X,F7.2)) |
|---|
| 333 | if(amater.ne.0) then |
|---|
| 334 | write(*,*) 'end SVD with error code: ',amater |
|---|
| 335 | endif |
|---|
| 336 | if(dbg.eq.1) then |
|---|
| 337 | do i = 1,im |
|---|
| 338 | write(*,*) i,wvec(I) |
|---|
| 339 | write(*,6001) i,wvec(I) |
|---|
| 340 | wmat(i,i) = wvec(i) |
|---|
| 341 | enddo |
|---|
| 342 | endif |
|---|
| 343 | call rvord(wvec,sortw,ws,im) |
|---|
| 344 | if(dbg.eq.1) then |
|---|
| 345 | do i = 1,im |
|---|
| 346 | write(*,*) i,sortw(i),wvec(sortw(i)) |
|---|
| 347 | enddo |
|---|
| 348 | endif |
|---|
| 349 | do ii = 1,min(nsing,im) |
|---|
| 350 | i = sortw(ii) |
|---|
| 351 | if(dbg.eq.1) then |
|---|
| 352 | write(61,*) wvec(i) |
|---|
| 353 | endif |
|---|
| 354 | if(abs(wvec(i)).lt.sngval) then |
|---|
| 355 | if(dbg.eq.1) then |
|---|
| 356 | do j = 1,ic |
|---|
| 357 | write(*,6002) i,j,umat(j,i) |
|---|
| 358 | enddo |
|---|
| 359 | endif |
|---|
| 360 | do j = 1,ic-1 |
|---|
| 361 | do jj = j+1,ic |
|---|
| 362 | if(abs(umat(j,i)).gt.1.0E-4) then |
|---|
| 363 | ! rat = abs(umat(j,i) - umat(jj,i)) |
|---|
| 364 | rat = abs(umat(j,i)) + abs(umat(jj,i)) |
|---|
| 365 | rat = rat/abs(abs(umat(j,i)) - abs(umat(jj,i))) |
|---|
| 366 | if(dbg.eq.1) then |
|---|
| 367 | write(62,*) wvec(i),rat |
|---|
| 368 | endif |
|---|
| 369 | if(rat.gt.sngcut) then |
|---|
| 370 | if(dbg.eq.1) then |
|---|
| 371 | write(*,*) 'dependent pair: ',j,jj,rat |
|---|
| 372 | write(65,*) 'dependent pair: ',j,jj,rat |
|---|
| 373 | endif |
|---|
| 374 | if(iflag.lt.ic) then |
|---|
| 375 | iflag = iflag + 1 |
|---|
| 376 | sing(1,iflag) = j - 1 |
|---|
| 377 | sing(2,iflag) = jj - 1 |
|---|
| 378 | endif |
|---|
| 379 | endif |
|---|
| 380 | endif |
|---|
| 381 | enddo |
|---|
| 382 | enddo |
|---|
| 383 | endif |
|---|
| 384 | enddo |
|---|
| 385 | |
|---|
| 386 | return |
|---|
| 387 | end |
|---|
| 388 | subroutine svdcorr_m(a,svdmat,umat,vmat,wmat,utmat,vtmat,wtmat, & |
|---|
| 389 | &xin,xc,xout,xa,xb,xpred,ws,wvec, & |
|---|
| 390 | &sortw,nx,im,ic,iflag,dbg) |
|---|
| 391 | ! ****************************************************** |
|---|
| 392 | ! * |
|---|
| 393 | ! Performs SVD and correction for matrix with more * |
|---|
| 394 | ! monitors than correctors. * |
|---|
| 395 | ! * |
|---|
| 396 | ! Author: WFH 12.09.02 * |
|---|
| 397 | ! * |
|---|
| 398 | ! ****************************************************** |
|---|
| 399 | implicit none |
|---|
| 400 | integer im,ic,nx(ic),i,j |
|---|
| 401 | integer iflag |
|---|
| 402 | double precision a(im,ic) |
|---|
| 403 | double precision svdmat(im,ic) |
|---|
| 404 | double precision umat(im,ic), utmat(ic,im) |
|---|
| 405 | double precision vmat(im,ic), vtmat(ic,im) |
|---|
| 406 | double precision wmat(im,ic), wtmat(ic,im) |
|---|
| 407 | double precision xin(im),xout(im),xc(ic) |
|---|
| 408 | double precision xa(im),xb(im),xpred(im),ws(ic),wvec(ic) |
|---|
| 409 | integer amater, svdmx, svdnx, svdnm |
|---|
| 410 | integer sortw(ic) |
|---|
| 411 | logical matu, matv |
|---|
| 412 | integer dbg |
|---|
| 413 | |
|---|
| 414 | MATU = .TRUE. |
|---|
| 415 | MATV = .TRUE. |
|---|
| 416 | iflag = 0 |
|---|
| 417 | write(*,*) ' ' |
|---|
| 418 | write(*,*) 'start SVD correction using ',ic,' correctors' |
|---|
| 419 | write(*,*) ' ' |
|---|
| 420 | |
|---|
| 421 | SVDNM = max(ic,im) |
|---|
| 422 | svdmx = im |
|---|
| 423 | svdnx = ic |
|---|
| 424 | |
|---|
| 425 | do i = 1,ic |
|---|
| 426 | nx(i) = i |
|---|
| 427 | enddo |
|---|
| 428 | |
|---|
| 429 | do i = 1,im |
|---|
| 430 | do j = 1,ic |
|---|
| 431 | svdmat(i,j) = a(i,j) |
|---|
| 432 | enddo |
|---|
| 433 | enddo |
|---|
| 434 | |
|---|
| 435 | if(dbg.eq.1) then |
|---|
| 436 | write(*,*) 'A0:' |
|---|
| 437 | do j = 1,im |
|---|
| 438 | write(*,6003) (svdmat(j,i),i=1,ic) |
|---|
| 439 | enddo |
|---|
| 440 | endif |
|---|
| 441 | call svd(svdnm,svdmx,svdnx,svdmat,wvec,matu,umat, & |
|---|
| 442 | &matv,vmat,amater,ws) |
|---|
| 443 | 6001 format(1X,'Corrector: ',I4,' sing: ',F12.4) |
|---|
| 444 | 6002 format('VMAT: ',I4,I4,5X,F12.6,2X,F12.6) |
|---|
| 445 | 6003 format(16(2X,F7.2)) |
|---|
| 446 | 6004 format(16(2X,F7.2)) |
|---|
| 447 | if(amater.ne.0) then |
|---|
| 448 | write(*,*) 'end SVD with error code: ',amater |
|---|
| 449 | endif |
|---|
| 450 | do i = 1,ic |
|---|
| 451 | if(dbg.eq.1) then |
|---|
| 452 | write(*,*) i,wvec(I) |
|---|
| 453 | write(*,6001) i,wvec(I) |
|---|
| 454 | endif |
|---|
| 455 | wmat(i,i) = wvec(i) |
|---|
| 456 | if(abs(wvec(I)).gt.1.0001) then |
|---|
| 457 | wtmat(i,i) = 1/wvec(i) |
|---|
| 458 | else |
|---|
| 459 | wtmat(i,i) = 0.0 |
|---|
| 460 | endif |
|---|
| 461 | enddo |
|---|
| 462 | |
|---|
| 463 | if(dbg.eq.1) then |
|---|
| 464 | call rvord(wvec,sortw,ws,ic) |
|---|
| 465 | do i = 1,ic |
|---|
| 466 | write(*,*) i,sortw(i),wvec(sortw(i)) |
|---|
| 467 | enddo |
|---|
| 468 | endif |
|---|
| 469 | |
|---|
| 470 | do i = 1,ic |
|---|
| 471 | do j = 1,im |
|---|
| 472 | vtmat(i,j) = vmat(j,i) |
|---|
| 473 | utmat(i,j) = umat(j,i) |
|---|
| 474 | enddo |
|---|
| 475 | enddo |
|---|
| 476 | |
|---|
| 477 | if(dbg.eq.1) then |
|---|
| 478 | write(*,*) 'A1:' |
|---|
| 479 | do j = 1,im |
|---|
| 480 | write(*,6003) (svdmat(j,i),i=1,ic) |
|---|
| 481 | enddo |
|---|
| 482 | write(*,*) ' ' |
|---|
| 483 | |
|---|
| 484 | write(*,*) 'Va:' |
|---|
| 485 | do j = 1,ic |
|---|
| 486 | write(*,6004) (vmat(j,i),i=1,ic) |
|---|
| 487 | enddo |
|---|
| 488 | write(*,*) 'Vt:' |
|---|
| 489 | do j = 1,ic |
|---|
| 490 | write(*,6004) (vtmat(j,i),i=1,ic) |
|---|
| 491 | enddo |
|---|
| 492 | write(*,*) 'W:' |
|---|
| 493 | do j = 1,im |
|---|
| 494 | write(*,6004) (wmat(j,i),i=1,ic) |
|---|
| 495 | enddo |
|---|
| 496 | write(*,*) 'Wt:' |
|---|
| 497 | do j = 1,ic |
|---|
| 498 | write(*,6004) (wtmat(j,i),i=1,im) |
|---|
| 499 | enddo |
|---|
| 500 | write(*,*) 'U:' |
|---|
| 501 | do j = 1,im |
|---|
| 502 | write(*,6004) (umat(j,i),i=1,im) |
|---|
| 503 | enddo |
|---|
| 504 | write(*,*) 'Ut:' |
|---|
| 505 | do j = 1,im |
|---|
| 506 | write(*,6004) (utmat(j,i),i=1,im) |
|---|
| 507 | enddo |
|---|
| 508 | endif |
|---|
| 509 | |
|---|
| 510 | call dmmpy(im,im,utmat(1,1),utmat(1,2),utmat(2,1), & |
|---|
| 511 | &xin(1),xin(2),xa(1),xa(2)) |
|---|
| 512 | call dmmpy(ic,im,wtmat(1,1),wtmat(1,2),wtmat(2,1), & |
|---|
| 513 | &xa(1),xa(2),xb(1),xb(2)) |
|---|
| 514 | call dmmpy(ic,ic,vmat(1,1),vmat(1,2),vmat(2,1), & |
|---|
| 515 | &xb(1),xb(2),xc(1),xc(2)) |
|---|
| 516 | call dmmpy(im,ic,svdmat(1,1),svdmat(1,2),svdmat(2,1), & |
|---|
| 517 | &xc(1),xc(2),xpred(1),xpred(2)) |
|---|
| 518 | if(dbg.eq.1) then |
|---|
| 519 | write(*,*) xc |
|---|
| 520 | write(*,*) xpred |
|---|
| 521 | endif |
|---|
| 522 | |
|---|
| 523 | do i = 1,im |
|---|
| 524 | xout(i) = xin(i) - xpred(i) |
|---|
| 525 | enddo |
|---|
| 526 | do i = 1,ic |
|---|
| 527 | xc(i) = -xc(i) |
|---|
| 528 | enddo |
|---|
| 529 | |
|---|
| 530 | return |
|---|
| 531 | end |
|---|
| 532 | |
|---|
| 533 | subroutine svdcorr_c(a,svdmat,umat,vmat,wmat,utmat,vtmat,wtmat, & |
|---|
| 534 | &xin,xc,xout,xa,xb,xpred,ws,wvec, & |
|---|
| 535 | &sortw,nx,im,ic,iflag,dbg) |
|---|
| 536 | ! ****************************************************** |
|---|
| 537 | ! * |
|---|
| 538 | ! Performs SVD and correction for matrix with more * |
|---|
| 539 | ! correctors than monitors. * |
|---|
| 540 | ! * |
|---|
| 541 | ! Author: WFH 12.09.02 * |
|---|
| 542 | ! * |
|---|
| 543 | ! ****************************************************** |
|---|
| 544 | implicit none |
|---|
| 545 | integer im,ic,nx(ic),i,j |
|---|
| 546 | integer iflag |
|---|
| 547 | double precision a(im,ic) |
|---|
| 548 | double precision svdmat(ic,im) |
|---|
| 549 | double precision umat(ic,im), utmat(im,ic) |
|---|
| 550 | double precision vmat(ic,im), vtmat(im,ic) |
|---|
| 551 | double precision wmat(ic,im), wtmat(im,ic) |
|---|
| 552 | double precision xin(im),xout(im),xc(ic) |
|---|
| 553 | double precision xa(im),xpred(im),ws(ic),wvec(ic) |
|---|
| 554 | integer sortw(ic) |
|---|
| 555 | integer amater, svdmx, svdnx, svdnm |
|---|
| 556 | logical matu, matv |
|---|
| 557 | integer dbg |
|---|
| 558 | double precision xb(2000) |
|---|
| 559 | |
|---|
| 560 | MATU = .TRUE. |
|---|
| 561 | MATV = .TRUE. |
|---|
| 562 | iflag = 0 |
|---|
| 563 | write(*,*) ' ' |
|---|
| 564 | write(*,*) 'start SVD correction using ',ic,' correctors' |
|---|
| 565 | write(*,*) ' ' |
|---|
| 566 | |
|---|
| 567 | SVDNM = max(ic,im) |
|---|
| 568 | svdmx = ic |
|---|
| 569 | svdnx = im |
|---|
| 570 | |
|---|
| 571 | do i = 1,im |
|---|
| 572 | do j = 1,ic |
|---|
| 573 | svdmat(j,i) = a(i,j) |
|---|
| 574 | enddo |
|---|
| 575 | enddo |
|---|
| 576 | do i = 1,ic |
|---|
| 577 | nx(i) = i |
|---|
| 578 | enddo |
|---|
| 579 | |
|---|
| 580 | 8373 continue |
|---|
| 581 | |
|---|
| 582 | if(dbg.eq.1) then |
|---|
| 583 | write(*,*) 'A0:' |
|---|
| 584 | do j = 1,ic |
|---|
| 585 | write(*,6003) (svdmat(j,i),i=1,im) |
|---|
| 586 | enddo |
|---|
| 587 | endif |
|---|
| 588 | |
|---|
| 589 | call svd(svdnm,svdmx,svdnx,svdmat,wvec,matu,umat, & |
|---|
| 590 | &matv,vmat,amater,ws) |
|---|
| 591 | 6001 format(1X,'Corrector: ',I4,' sing: ',F12.4) |
|---|
| 592 | 6002 format('UMAT: ',I4,I4,5X,F12.6,2X,F12.6) |
|---|
| 593 | 6003 format(16(2X,F7.2)) |
|---|
| 594 | 6004 format(16(2X,F7.2)) |
|---|
| 595 | if(amater.ne.0) then |
|---|
| 596 | write(*,*) 'end SVD with error code: ',amater |
|---|
| 597 | endif |
|---|
| 598 | do i = 1,im |
|---|
| 599 | if(dbg.eq.1) then |
|---|
| 600 | write(*,*) i,wvec(I) |
|---|
| 601 | write(*,6001) i,wvec(I) |
|---|
| 602 | endif |
|---|
| 603 | wmat(i,i) = wvec(i) |
|---|
| 604 | if(abs(wvec(i)).gt.1.0001) then |
|---|
| 605 | wtmat(i,i) = 1/wvec(i) |
|---|
| 606 | else |
|---|
| 607 | wtmat(i,i) = 0.0 |
|---|
| 608 | endif |
|---|
| 609 | enddo |
|---|
| 610 | if(dbg.eq.1) then |
|---|
| 611 | call rvord(wvec,sortw,ws,im) |
|---|
| 612 | do i = 1,im |
|---|
| 613 | write(*,*) i,sortw(i),wvec(sortw(i)) |
|---|
| 614 | enddo |
|---|
| 615 | endif |
|---|
| 616 | |
|---|
| 617 | do i = 1,im |
|---|
| 618 | do j = 1,ic |
|---|
| 619 | vtmat(i,j) = vmat(j,i) |
|---|
| 620 | utmat(i,j) = umat(j,i) |
|---|
| 621 | enddo |
|---|
| 622 | enddo |
|---|
| 623 | |
|---|
| 624 | if(dbg.eq.1) then |
|---|
| 625 | write(*,*) 'A1:' |
|---|
| 626 | do j = 1,ic |
|---|
| 627 | write(*,6003) (svdmat(j,i),i=1,im) |
|---|
| 628 | enddo |
|---|
| 629 | write(*,*) ' ' |
|---|
| 630 | |
|---|
| 631 | write(*,*) 'Va:' |
|---|
| 632 | do j = 1,im |
|---|
| 633 | write(*,6004) (vmat(j,i),i=1,im) |
|---|
| 634 | enddo |
|---|
| 635 | write(*,*) 'Vt:' |
|---|
| 636 | do j = 1,im |
|---|
| 637 | write(*,6004) (vtmat(j,i),i=1,im) |
|---|
| 638 | enddo |
|---|
| 639 | write(*,*) 'W:' |
|---|
| 640 | do j = 1,ic |
|---|
| 641 | write(*,6004) (wmat(j,i),i=1,im) |
|---|
| 642 | enddo |
|---|
| 643 | write(*,*) 'Wt:' |
|---|
| 644 | do j = 1,im |
|---|
| 645 | write(*,6004) (wtmat(j,i),i=1,ic) |
|---|
| 646 | enddo |
|---|
| 647 | write(*,*) 'U:' |
|---|
| 648 | do j = 1,ic |
|---|
| 649 | write(*,6004) (umat(j,i),i=1,ic) |
|---|
| 650 | enddo |
|---|
| 651 | write(*,*) 'Ut:' |
|---|
| 652 | do j = 1,ic |
|---|
| 653 | write(*,6004) (utmat(j,i),i=1,ic) |
|---|
| 654 | enddo |
|---|
| 655 | endif |
|---|
| 656 | |
|---|
| 657 | call dmmpy(im,im,vtmat(1,1),vtmat(1,2),vtmat(2,1), & |
|---|
| 658 | &xin(1),xin(2),xa(1),xa(2)) |
|---|
| 659 | ! write(*,*) 'xa: ',xa |
|---|
| 660 | call dmmpy(im,im,wtmat(1,1),wtmat(1,2),wtmat(2,1), & |
|---|
| 661 | &xa(1),xa(2),xb(1),xb(2)) |
|---|
| 662 | ! write(*,*) 'xb: ',xb |
|---|
| 663 | call dmmpy(ic,im,umat(1,1),umat(1,2),umat(2,1), & |
|---|
| 664 | &xb(1),xb(2),xc(1),xc(2)) |
|---|
| 665 | ! write(*,*) 'xc: ',xc |
|---|
| 666 | call dmmpy(im,ic,a(1,1),a(1,2),a(2,1), & |
|---|
| 667 | &xc(1),xc(2),xpred(1),xpred(2)) |
|---|
| 668 | |
|---|
| 669 | if(dbg.eq.1) then |
|---|
| 670 | write(*,*) xc |
|---|
| 671 | write(*,*) xpred |
|---|
| 672 | endif |
|---|
| 673 | |
|---|
| 674 | do i = 1,im |
|---|
| 675 | xout(i) = xin(i) - xpred(i) |
|---|
| 676 | enddo |
|---|
| 677 | do i = 1,ic |
|---|
| 678 | xc(i) = -xc(i) |
|---|
| 679 | enddo |
|---|
| 680 | |
|---|
| 681 | return |
|---|
| 682 | end |
|---|
| 683 | !CDECK ID>, SOLSQL. |
|---|
| 684 | subroutine solsql(m,n,xad,orb0,orbr,xinc,cb,xmeas,xres,y,z,xd) |
|---|
| 685 | implicit none |
|---|
| 686 | !********************************************************************* |
|---|
| 687 | ! Subroutine SOLSQL to solve least sq. problem for orbit * |
|---|
| 688 | ! after matrix has been reconditioned * |
|---|
| 689 | ! * |
|---|
| 690 | ! Authors: WFH Date: 21.03.1995 * |
|---|
| 691 | ! * |
|---|
| 692 | !********************************************************************* |
|---|
| 693 | integer m,n,i,j,ifail |
|---|
| 694 | double precision xad(m,n),orb0(m),orbr(m),xinc(n),cb(n),xmeas(m), & |
|---|
| 695 | &xres(m),y(n,m),z(n,n),xd(n),zero |
|---|
| 696 | parameter(zero=0d0) |
|---|
| 697 | |
|---|
| 698 | ! --- copy from original matrix, sizes are not equal ! |
|---|
| 699 | do i = 1,m |
|---|
| 700 | xmeas(i) = orb0(i) |
|---|
| 701 | xres(i) = zero |
|---|
| 702 | enddo |
|---|
| 703 | do i = 1,m |
|---|
| 704 | do j = 1,n |
|---|
| 705 | ! write(*,*) i,j,xad(i,j) |
|---|
| 706 | enddo |
|---|
| 707 | enddo |
|---|
| 708 | |
|---|
| 709 | call lstsql(m,n,xad,xmeas,cb,xres,ifail,y,z,xd) |
|---|
| 710 | write(*,*) 'IFAIL from lstsql: ',ifail |
|---|
| 711 | |
|---|
| 712 | do i=1,n |
|---|
| 713 | xinc(i) = cb(i) |
|---|
| 714 | ! write(*,*) i,cb(i) |
|---|
| 715 | enddo |
|---|
| 716 | do j = 1,m |
|---|
| 717 | orbr(j) = xres(j) + xmeas(j) |
|---|
| 718 | enddo |
|---|
| 719 | 6001 format(3x,i4,2x,f10.5) |
|---|
| 720 | 6002 format(3x,i4,2x,f10.5,2x,f10.5,2x,f10.5) |
|---|
| 721 | return |
|---|
| 722 | end |
|---|
| 723 | !----------------------------------------------------------------- |
|---|
| 724 | !CDECK ID>, LSTSQL. |
|---|
| 725 | subroutine lstsql(m,n,x,d,cb,xpred,ifail,y,z,xd) |
|---|
| 726 | implicit none |
|---|
| 727 | !********************************************************************* |
|---|
| 728 | ! Subroutine LSTSQR to make a least sqared minimization * |
|---|
| 729 | ! * |
|---|
| 730 | ! Authors: WFH Date: 21.03.1995 * |
|---|
| 731 | ! * |
|---|
| 732 | !********************************************************************* |
|---|
| 733 | integer m,n,ifail,i,j,w(200000) |
|---|
| 734 | double precision x(m,n),d(m),cb(n),xpred(m),y(n,m),z(n,n),xd(n), & |
|---|
| 735 | &dw(100000) |
|---|
| 736 | equivalence (w(1),dw(1)) |
|---|
| 737 | |
|---|
| 738 | do i = 1,m |
|---|
| 739 | do j = 1,n |
|---|
| 740 | y(j,i) = x(i,j) |
|---|
| 741 | enddo |
|---|
| 742 | enddo |
|---|
| 743 | |
|---|
| 744 | call dmmlt(n,m,n,y,y(1,2),y(2,1),x,x(1,2),x(2,1),z, & |
|---|
| 745 | &z(1,2),z(2,1),dw) |
|---|
| 746 | |
|---|
| 747 | call dinv(n,z,n,w,ifail) |
|---|
| 748 | |
|---|
| 749 | call dmmpy(n,m,y(1,1),y(1,2),y(2,1),d(1),d(2),xd(1),xd(2)) |
|---|
| 750 | |
|---|
| 751 | call dmmpy(n,n,z(1,1),z(1,2),z(2,1),xd(1),xd(2),cb(1),cb(2)) |
|---|
| 752 | |
|---|
| 753 | do i=1,n |
|---|
| 754 | cb(i)=-cb(i) |
|---|
| 755 | ! write(*,*) '=> ',i,cb(i) |
|---|
| 756 | enddo |
|---|
| 757 | |
|---|
| 758 | call dmmpy(m,n,x(1,1),x(1,2),x(2,1),cb(1),cb(2),xpred(1),xpred(2)) |
|---|
| 759 | |
|---|
| 760 | return |
|---|
| 761 | end |
|---|
| 762 | !CDECK ID>, MICADO. |
|---|
| 763 | subroutine micado(a,conm,b,orbr,xinc,nx,rms,m,n,iter,rho,ptop, & |
|---|
| 764 | &rmss,xrms,xptp,xiter,ifail) |
|---|
| 765 | implicit none |
|---|
| 766 | !********************************************************************* |
|---|
| 767 | ! Subroutine MICADO to run MICADO minimisation * |
|---|
| 768 | ! * |
|---|
| 769 | ! Authors: many Date: 17.09.1989 * |
|---|
| 770 | ! * |
|---|
| 771 | !********************************************************************* |
|---|
| 772 | ! interface between ORBCOR and HTLS routine |
|---|
| 773 | ! ARRAY and loop dimensions are transmitted as subroutine arguments |
|---|
| 774 | ! A(M,N) : response matrix correctors ==>> monitors |
|---|
| 775 | ! B(M) : orbit to be corrected |
|---|
| 776 | ! ORBR(M) : residual orbit |
|---|
| 777 | ! XINC(N) : strength of correctors |
|---|
| 778 | ! NX(N) : sequence number of correctors |
|---|
| 779 | integer m,n,nx(n),prtlev,iter |
|---|
| 780 | integer ifail |
|---|
| 781 | real a(m,n),b(m),orbr(m),xinc(n),rms,rho(3*n),ptop(n),rmss(n), & |
|---|
| 782 | &xrms(n),xptp(n),xiter(n) |
|---|
| 783 | character*16 conm(n) |
|---|
| 784 | |
|---|
| 785 | prtlev = 3 |
|---|
| 786 | |
|---|
| 787 | call htls(a,conm,b,m,n,xinc,nx,orbr,rms,prtlev,iter,rho,ptop,rmss,& |
|---|
| 788 | &xrms,xptp,xiter,ifail) |
|---|
| 789 | |
|---|
| 790 | ! --- energy shift caused by corrector strength changes |
|---|
| 791 | ! (inhibited) |
|---|
| 792 | ! if(iplane.eq.2) go to 85 |
|---|
| 793 | ! do 75 ij1=1,iter |
|---|
| 794 | ! 75 dp=dp-apc(nx(ij1))*xinc(ij1)*mcfl |
|---|
| 795 | ! write(61,*)' DP NEW CORR=',dp |
|---|
| 796 | |
|---|
| 797 | return |
|---|
| 798 | end |
|---|
| 799 | !CDECK ID>, HHT. |
|---|
| 800 | subroutine htls(a,conm,b,m,n,x,ipiv,r,rms,prtlev,iter,rho,ptop, & |
|---|
| 801 | &rmss,xrms,xptp,xiter,ifail) |
|---|
| 802 | implicit none |
|---|
| 803 | !********************************************************************* |
|---|
| 804 | ! Subroutine HTLS to make Householder transform * |
|---|
| 805 | ! * |
|---|
| 806 | ! Authors: many Date: 17.09.1989 * |
|---|
| 807 | ! * |
|---|
| 808 | !********************************************************************* |
|---|
| 809 | ! dimension of array RHO should be 3*N |
|---|
| 810 | ! M = NMTOT nr available monitors |
|---|
| 811 | ! N = NCTOT nr available independent correctors |
|---|
| 812 | logical interm |
|---|
| 813 | integer m,n,ipiv(n),prtlev,iter,ij1,k2,k,i,kpiv,k3,j,ip,j1,kk,ki, & |
|---|
| 814 | &iii,kkk |
|---|
| 815 | real a(m,n),b(m),x(n),r(m),rms,rho(3*n),ptop(n),rmss(n),xrms(n), & |
|---|
| 816 | &xptp(n),xiter(n),xxcal,ptp,g,h,sig,beta,piv,pivt,rm,pt,rzero, & |
|---|
| 817 | &reps7 |
|---|
| 818 | parameter(rzero=0e0,reps7=1e-7) |
|---|
| 819 | character*4 units |
|---|
| 820 | character*16 conm(n) |
|---|
| 821 | integer ifail |
|---|
| 822 | |
|---|
| 823 | interm = .true. |
|---|
| 824 | ifail = 0 |
|---|
| 825 | units = 'mrad' |
|---|
| 826 | ptp = rzero |
|---|
| 827 | |
|---|
| 828 | call calrms(b,m,rm,pt) |
|---|
| 829 | |
|---|
| 830 | if(rm.le.rms) then |
|---|
| 831 | write(*,*) '++++++ WARNING: RMS already smaller than desired ' |
|---|
| 832 | write(*,*) '++++++ WARNING: no correction is done ' |
|---|
| 833 | rms = rm |
|---|
| 834 | iter = 0 |
|---|
| 835 | ifail = -2 |
|---|
| 836 | return |
|---|
| 837 | endif |
|---|
| 838 | |
|---|
| 839 | ! --- calculate first pivot |
|---|
| 840 | !========================== |
|---|
| 841 | |
|---|
| 842 | do ij1=1,3*n |
|---|
| 843 | rho(ij1)=rzero |
|---|
| 844 | enddo |
|---|
| 845 | |
|---|
| 846 | k2=n + 1 |
|---|
| 847 | piv=rzero |
|---|
| 848 | |
|---|
| 849 | do k=1,n |
|---|
| 850 | ipiv(k)=k |
|---|
| 851 | h=rzero |
|---|
| 852 | g=rzero |
|---|
| 853 | do i=1,m |
|---|
| 854 | h=h+a(i,k)*a(i,k) |
|---|
| 855 | g=g+a(i,k)*b(i) |
|---|
| 856 | enddo |
|---|
| 857 | rho(k)=h |
|---|
| 858 | rho(k2) = g |
|---|
| 859 | if(h.ne.rzero) then |
|---|
| 860 | pivt = g*g/h |
|---|
| 861 | else |
|---|
| 862 | pivt = rzero |
|---|
| 863 | endif |
|---|
| 864 | if(pivt-piv.gt.rzero) then |
|---|
| 865 | piv = pivt |
|---|
| 866 | kpiv=k |
|---|
| 867 | endif |
|---|
| 868 | k2 = k2 + 1 |
|---|
| 869 | enddo |
|---|
| 870 | |
|---|
| 871 | ! --- boucle pour chaque iteration |
|---|
| 872 | |
|---|
| 873 | do k=1,iter |
|---|
| 874 | |
|---|
| 875 | !X WRITE(*,*) ' Start iteration ',K |
|---|
| 876 | if(kpiv.eq.k)go to 8 |
|---|
| 877 | !X write(*,*) 'change row, KPIV, K: ',KPIV, K |
|---|
| 878 | |
|---|
| 879 | ! --- on echange les K et KPIV si KPIV plus grand que K |
|---|
| 880 | h=rho(k) |
|---|
| 881 | rho(k)=rho(kpiv) |
|---|
| 882 | rho(kpiv)=h |
|---|
| 883 | k2=n+k |
|---|
| 884 | k3=n+kpiv |
|---|
| 885 | g = rho(k2) |
|---|
| 886 | rho(k2) = rho(k3) |
|---|
| 887 | rho(k3) = g |
|---|
| 888 | do i=1,m |
|---|
| 889 | h=a(i,k) |
|---|
| 890 | a(i,k)=a(i,kpiv) |
|---|
| 891 | a(i,kpiv)=h |
|---|
| 892 | enddo |
|---|
| 893 | |
|---|
| 894 | ! --- calcul de beta,sigma et uk dans htul |
|---|
| 895 | 8 continue |
|---|
| 896 | !X write(*,*) 'call HTUL' |
|---|
| 897 | call htul(a,m,n,k,sig,beta) |
|---|
| 898 | !X write(*,*) 'back from HTUL' |
|---|
| 899 | |
|---|
| 900 | ! --- on garde SIGMA dans RHO(N+K) |
|---|
| 901 | j=n+k |
|---|
| 902 | rho(j)=-sig |
|---|
| 903 | ip=ipiv(kpiv) |
|---|
| 904 | ipiv(kpiv)=ipiv(k) |
|---|
| 905 | ipiv(k)=ip |
|---|
| 906 | if(k.eq.n) go to 13 |
|---|
| 907 | |
|---|
| 908 | ! --- transformation de A dans HTAL |
|---|
| 909 | !X write(*,*) 'call HTAL' |
|---|
| 910 | call htal(a,m,n,k,beta) |
|---|
| 911 | !X write(*,*) 'back from HTAL' |
|---|
| 912 | |
|---|
| 913 | ! --- transformation de B dans HTBL |
|---|
| 914 | 13 continue |
|---|
| 915 | !X write(*,*) 'call HTBL' |
|---|
| 916 | call htbl(a,b,m,n,k,beta) |
|---|
| 917 | !X write(*,*) 'back from HTBL' |
|---|
| 918 | |
|---|
| 919 | ! --- recherche du pivot (K+1) |
|---|
| 920 | !============================= |
|---|
| 921 | |
|---|
| 922 | rho(k)=sqrt(piv) |
|---|
| 923 | if(k.eq.n) go to 11 |
|---|
| 924 | piv=rzero |
|---|
| 925 | kpiv = k + 1 |
|---|
| 926 | j1 = kpiv |
|---|
| 927 | k2=n + j1 |
|---|
| 928 | !x write(*,*) 'loop 18, ',j1,n |
|---|
| 929 | do j=j1,n |
|---|
| 930 | h=rho(j)-(a(k,j))*(a(k,j)) |
|---|
| 931 | !X write(*,*) 'K,J: ',K,J |
|---|
| 932 | !X write(*,*) RHO(J),A(K,J) |
|---|
| 933 | !X write(*,*) 'H: ',H |
|---|
| 934 | |
|---|
| 935 | if(abs(h).lt.reps7) then |
|---|
| 936 | write(*,*) 'Correction process aborted' |
|---|
| 937 | write(*,*) 'during ',k,'th iteration' |
|---|
| 938 | write(*,*) 'Last r.m.s.: ',rmss(k-1) |
|---|
| 939 | write(*,*) 'Last p-t-p.: ',ptop(k-1) |
|---|
| 940 | write(*,*) 'Division by zero expected' |
|---|
| 941 | write(*,*) 'Probably two kickers too close: ',h |
|---|
| 942 | write(*,*) 'SUSPECTED KICKER: ',J,' ' |
|---|
| 943 | write(*,*) conm(ipiv(j)) |
|---|
| 944 | ifail = -1 |
|---|
| 945 | return |
|---|
| 946 | ! stop 777 |
|---|
| 947 | endif |
|---|
| 948 | |
|---|
| 949 | rho(j)=h |
|---|
| 950 | g=rho(k2)-(a(k,j))*(b(k)) |
|---|
| 951 | rho(k2) = g |
|---|
| 952 | if(h.ne.rzero) then |
|---|
| 953 | pivt = g*g/h |
|---|
| 954 | else |
|---|
| 955 | pivt = rzero |
|---|
| 956 | endif |
|---|
| 957 | !X write(*,*) 'compare for pivot K,J,PIVT,PIV: ',K,J,PIVT,PIV |
|---|
| 958 | if(pivt.lt.piv)go to 18 |
|---|
| 959 | !X write(*,*) 'comparison succeeded, set pivot' |
|---|
| 960 | kpiv=j |
|---|
| 961 | piv=pivt |
|---|
| 962 | 18 continue |
|---|
| 963 | k2 = k2 + 1 |
|---|
| 964 | enddo |
|---|
| 965 | |
|---|
| 966 | ! --- calcul des x |
|---|
| 967 | 11 x(k)=b(k)/rho(n+k) |
|---|
| 968 | if(k.eq.1)go to 27 |
|---|
| 969 | do i=2,k |
|---|
| 970 | kk=k-i+1 |
|---|
| 971 | x(kk)=b(kk) |
|---|
| 972 | ki=kk+1 |
|---|
| 973 | do j=ki,k |
|---|
| 974 | x(kk)=x(kk)-a(kk,j)*x(j) |
|---|
| 975 | enddo |
|---|
| 976 | x(kk)=x(kk)/rho(n+kk) |
|---|
| 977 | enddo |
|---|
| 978 | ! write(*,*) 'after 15' |
|---|
| 979 | 27 continue |
|---|
| 980 | |
|---|
| 981 | ! --- save residual orbit and inverse sign of corrections (convention!) |
|---|
| 982 | do iii= 1,m |
|---|
| 983 | r(iii) = b(iii) |
|---|
| 984 | enddo |
|---|
| 985 | do iii= 1,k |
|---|
| 986 | x(iii) =-x(iii) |
|---|
| 987 | enddo |
|---|
| 988 | |
|---|
| 989 | ! --- calcul du vecteur residuel dans htrl |
|---|
| 990 | !========================================= |
|---|
| 991 | |
|---|
| 992 | ! transform orbit R back to "normal space" |
|---|
| 993 | ! write(*,*) 'transform back to normal space' |
|---|
| 994 | call htrl(a,r,m,n,k,rho) |
|---|
| 995 | call calrms(r,m,rmss(k),ptop(k)) |
|---|
| 996 | ! WRITE(61,'(I10,2F15.2)')K,RMSS(K),PTOP(K) |
|---|
| 997 | if(k.lt.n) then |
|---|
| 998 | xiter(k+1) = k |
|---|
| 999 | xrms(k+1) = rmss(k) |
|---|
| 1000 | xptp(k+1) = ptop(k) |
|---|
| 1001 | endif |
|---|
| 1002 | ! write(*,*) 'orbit back in normal space ',prtlev,k |
|---|
| 1003 | |
|---|
| 1004 | ! --- write intermediate results to 61 files |
|---|
| 1005 | ! IF(.TRUE.)THEN |
|---|
| 1006 | if(k.eq.1)then |
|---|
| 1007 | if (prtlev .ge. 2) then |
|---|
| 1008 | ! write(61,52) |
|---|
| 1009 | ! write(61,54)units |
|---|
| 1010 | ! write(61,'(4x,"0",42x,f12.8,f15.8)')rm,pt |
|---|
| 1011 | endif |
|---|
| 1012 | 52 FORMAT(/' *********** start MICADO ***********'/) |
|---|
| 1013 | 54 FORMAT(' iter',5X,'corrector',13X,A4,6X,'mrad', & |
|---|
| 1014 | &5X," rms",10X," ptop",/) |
|---|
| 1015 | endif |
|---|
| 1016 | |
|---|
| 1017 | if (prtlev .ge. 2) then |
|---|
| 1018 | ! write(61,'(/,1x,72("-"),/, & |
|---|
| 1019 | ! &1x,i4,3x,38(" "),1x,f12.8,f15.8)')k,rmss(k),ptop(k) |
|---|
| 1020 | endif |
|---|
| 1021 | |
|---|
| 1022 | do kkk = 1,k |
|---|
| 1023 | xxcal=x(kkk) |
|---|
| 1024 | if (prtlev .ge. 2) then |
|---|
| 1025 | ! write(61,'(I3,1X,A16,9x,f8.4,2x,f8.4)') & |
|---|
| 1026 | ! &kkk,conm(ipiv(kkk)),x(kkk),xxcal |
|---|
| 1027 | endif |
|---|
| 1028 | enddo |
|---|
| 1029 | |
|---|
| 1030 | if(interm)then |
|---|
| 1031 | if (prtlev .ge. 2) then |
|---|
| 1032 | ! write(61,58)k |
|---|
| 1033 | ! write(61,'(1x,8f9.3)')(r(kkk),kkk=1,m) |
|---|
| 1034 | endif |
|---|
| 1035 | 58 format(/,' residual orbit after iteration ',i2,':') |
|---|
| 1036 | endif |
|---|
| 1037 | |
|---|
| 1038 | if(k.eq.iter) then |
|---|
| 1039 | if (prtlev .ge. 2) then |
|---|
| 1040 | ! write(61,53) |
|---|
| 1041 | endif |
|---|
| 1042 | endif |
|---|
| 1043 | 53 format(/' *********** end MICADO ***********'/) |
|---|
| 1044 | ! endif |
|---|
| 1045 | |
|---|
| 1046 | if(ptop(k).le.ptp)go to 202 |
|---|
| 1047 | if(rmss(k).le.rms)go to 202 |
|---|
| 1048 | enddo |
|---|
| 1049 | return |
|---|
| 1050 | |
|---|
| 1051 | ! --- correction is already good enough: |
|---|
| 1052 | !======================================= |
|---|
| 1053 | |
|---|
| 1054 | 202 ptp=ptop(k) |
|---|
| 1055 | rms=rmss(k) |
|---|
| 1056 | iter=k |
|---|
| 1057 | return |
|---|
| 1058 | end |
|---|
| 1059 | |
|---|
| 1060 | !CDECK ID>, HTAL. |
|---|
| 1061 | subroutine htal(a,m,n,k,beta) |
|---|
| 1062 | implicit none |
|---|
| 1063 | !********************************************************************* |
|---|
| 1064 | ! Subroutine HTAL to make Householder transform * |
|---|
| 1065 | ! * |
|---|
| 1066 | ! Authors: many Date: 17.09.1989 * |
|---|
| 1067 | ! * |
|---|
| 1068 | !********************************************************************* |
|---|
| 1069 | ! Householder transform of matrix A |
|---|
| 1070 | integer m,n,nc,k,j,k1 |
|---|
| 1071 | real beta,a(m,n),h,rzero |
|---|
| 1072 | parameter(rzero=0e0) |
|---|
| 1073 | |
|---|
| 1074 | nc=n-k |
|---|
| 1075 | |
|---|
| 1076 | do j=1,nc |
|---|
| 1077 | h=rzero |
|---|
| 1078 | |
|---|
| 1079 | do k1=k,m |
|---|
| 1080 | h=h+a(k1,k)*a(k1,k+j) |
|---|
| 1081 | enddo |
|---|
| 1082 | |
|---|
| 1083 | h=beta*h |
|---|
| 1084 | do k1=k,m |
|---|
| 1085 | a(k1,k+j)=a(k1,k+j)-a(k1,k)*h |
|---|
| 1086 | enddo |
|---|
| 1087 | enddo |
|---|
| 1088 | |
|---|
| 1089 | end |
|---|
| 1090 | |
|---|
| 1091 | |
|---|
| 1092 | !CDECK ID>, HTBL. |
|---|
| 1093 | subroutine htbl(a,b,m,n,k,beta) |
|---|
| 1094 | implicit none |
|---|
| 1095 | !********************************************************************* |
|---|
| 1096 | ! Subroutine HTBL to make Householder transform * |
|---|
| 1097 | ! * |
|---|
| 1098 | ! Authors: many Date: 17.09.1989 * |
|---|
| 1099 | ! * |
|---|
| 1100 | !********************************************************************* |
|---|
| 1101 | ! Householder transform of vector B |
|---|
| 1102 | integer m,n,k,k1 |
|---|
| 1103 | real beta,a(m,n),b(m),h,rzero |
|---|
| 1104 | parameter(rzero=0e0) |
|---|
| 1105 | |
|---|
| 1106 | h=rzero |
|---|
| 1107 | |
|---|
| 1108 | do k1=k,m |
|---|
| 1109 | h=h+a(k1,k)*b(k1) |
|---|
| 1110 | enddo |
|---|
| 1111 | |
|---|
| 1112 | h=beta*h |
|---|
| 1113 | |
|---|
| 1114 | do k1=k,m |
|---|
| 1115 | b(k1)=b(k1)-a(k1,k)*h |
|---|
| 1116 | enddo |
|---|
| 1117 | |
|---|
| 1118 | end |
|---|
| 1119 | |
|---|
| 1120 | !CDECK ID>, HTRL. |
|---|
| 1121 | subroutine htrl(a,b,m,n,k,rho) |
|---|
| 1122 | implicit none |
|---|
| 1123 | !********************************************************************* |
|---|
| 1124 | ! Subroutine HTRL to make Householder transform * |
|---|
| 1125 | ! * |
|---|
| 1126 | ! Authors: many Date: 17.09.1989 * |
|---|
| 1127 | ! * |
|---|
| 1128 | !********************************************************************* |
|---|
| 1129 | ! calculate residual orbit vector |
|---|
| 1130 | integer m,n,k,kk,kl,i,lv,kn |
|---|
| 1131 | real beta,a(m,n),b(m),rho(3*n),rzero |
|---|
| 1132 | parameter(rzero=0e0) |
|---|
| 1133 | |
|---|
| 1134 | do i= 1,k,1 |
|---|
| 1135 | b(i)=rzero |
|---|
| 1136 | enddo |
|---|
| 1137 | |
|---|
| 1138 | do kk=1,k |
|---|
| 1139 | lv=m-k+kk |
|---|
| 1140 | kn=n+k-kk+1 |
|---|
| 1141 | kl=k-kk+1 |
|---|
| 1142 | |
|---|
| 1143 | beta=-1d0/(rho(kn)*a(kl,kl)) |
|---|
| 1144 | call htbl(a,b,m,n,kl,beta) |
|---|
| 1145 | enddo |
|---|
| 1146 | |
|---|
| 1147 | end |
|---|
| 1148 | |
|---|
| 1149 | |
|---|
| 1150 | !CDECK ID>, HTUL. |
|---|
| 1151 | subroutine htul(a,m,n,k,sig,beta) |
|---|
| 1152 | implicit none |
|---|
| 1153 | !********************************************************************* |
|---|
| 1154 | ! Subroutine HTUL to make Householder transform * |
|---|
| 1155 | ! * |
|---|
| 1156 | ! Authors: many Date: 17.09.1989 * |
|---|
| 1157 | ! * |
|---|
| 1158 | !********************************************************************* |
|---|
| 1159 | ! calculate vector U |
|---|
| 1160 | integer m,n,k,i |
|---|
| 1161 | real beta,sig,a(m,n),h,rzero |
|---|
| 1162 | parameter(rzero=0e0) |
|---|
| 1163 | |
|---|
| 1164 | sig=rzero |
|---|
| 1165 | |
|---|
| 1166 | do i=k,m |
|---|
| 1167 | sig=sig+a(i,k)* a(i,k) |
|---|
| 1168 | enddo |
|---|
| 1169 | |
|---|
| 1170 | sig=sqrt(sig) |
|---|
| 1171 | ! on choisit le signe correct pour sig: |
|---|
| 1172 | h=a(k,k) |
|---|
| 1173 | if(h.lt.rzero)sig=-sig |
|---|
| 1174 | beta=h + sig |
|---|
| 1175 | a(k,k)=beta |
|---|
| 1176 | beta=1d0/(sig*beta) |
|---|
| 1177 | end |
|---|
| 1178 | !CDECK ID>, LEQU2. |
|---|
| 1179 | subroutine lequ2(a,ifail,b) |
|---|
| 1180 | implicit none |
|---|
| 1181 | !********************************************************************* |
|---|
| 1182 | ! Subroutine LEQU2 to solve X * A = b * |
|---|
| 1183 | ! for 2 dimensions * |
|---|
| 1184 | ! * |
|---|
| 1185 | ! Authors: WFH Date: 26.02.1992 * |
|---|
| 1186 | ! * |
|---|
| 1187 | !********************************************************************* |
|---|
| 1188 | integer ifail |
|---|
| 1189 | real a(2,2),b(2),x(2),det,reps6 |
|---|
| 1190 | parameter(reps6=1e-6) |
|---|
| 1191 | |
|---|
| 1192 | det = a(2,2)*a(1,1) - a(1,2)*a(2,1) |
|---|
| 1193 | if(abs(det).lt.reps6) then |
|---|
| 1194 | ifail = -1 |
|---|
| 1195 | return |
|---|
| 1196 | endif |
|---|
| 1197 | |
|---|
| 1198 | x(1) = (b(1)*a(2,2) - b(2)*a(1,2))/det |
|---|
| 1199 | x(2) = (b(2) - a(2,1)*x(1))/a(2,2) |
|---|
| 1200 | |
|---|
| 1201 | b(1) = x(1) |
|---|
| 1202 | b(2) = x(2) |
|---|
| 1203 | ifail = 0 |
|---|
| 1204 | return |
|---|
| 1205 | end |
|---|
| 1206 | !CDECK ID>, CALRMS. |
|---|
| 1207 | subroutine calrms(r,m,rms,ptp) |
|---|
| 1208 | implicit none |
|---|
| 1209 | !********************************************************************* |
|---|
| 1210 | ! Subroutine CALRMS to calculate rms * |
|---|
| 1211 | ! * |
|---|
| 1212 | ! Authors: many Date: 17.09.1989 * |
|---|
| 1213 | ! * |
|---|
| 1214 | !********************************************************************* |
|---|
| 1215 | ! calculates rms and p.to.p value of R(1) .... R(M) |
|---|
| 1216 | integer m,i,imax,imin,maxmin |
|---|
| 1217 | real r(m),xave,xrms,rms,ptp,ave,rzero |
|---|
| 1218 | parameter(rzero=0e0) |
|---|
| 1219 | |
|---|
| 1220 | xave = rzero |
|---|
| 1221 | xrms = rzero |
|---|
| 1222 | |
|---|
| 1223 | do i=1,m |
|---|
| 1224 | xave = xave + r(i) |
|---|
| 1225 | xrms = xrms + (r(i)*r(i)) |
|---|
| 1226 | enddo |
|---|
| 1227 | |
|---|
| 1228 | ave = xave / float(m) |
|---|
| 1229 | rms = xrms / float(m) |
|---|
| 1230 | |
|---|
| 1231 | imax=maxmin(r(1),m,1) |
|---|
| 1232 | imin=maxmin(r(1),m,0) |
|---|
| 1233 | ptp=r(imax)-r(imin) |
|---|
| 1234 | rms=sqrt(rms) |
|---|
| 1235 | return |
|---|
| 1236 | end |
|---|
| 1237 | !CDECK ID>, MAXMIN. |
|---|
| 1238 | function maxmin (a,n,m) |
|---|
| 1239 | implicit none |
|---|
| 1240 | !********************************************************************* |
|---|
| 1241 | ! Subroutine MAXMIN to find maximum and minimum of a list * |
|---|
| 1242 | ! * |
|---|
| 1243 | ! Authors: WFH Date: 21.08.2000 * |
|---|
| 1244 | ! * |
|---|
| 1245 | !********************************************************************* |
|---|
| 1246 | ! if M=0, MAXMIN=lowest index of minimum element in A |
|---|
| 1247 | ! if M=1, MAXMIN=lowest index of maximun element in A |
|---|
| 1248 | ! if N<1, MAXMIN=1 |
|---|
| 1249 | integer m,n,maxmin,i |
|---|
| 1250 | real a(n),curent |
|---|
| 1251 | |
|---|
| 1252 | maxmin=1 |
|---|
| 1253 | if (n.lt.1) return |
|---|
| 1254 | curent=a(1) |
|---|
| 1255 | do i=2,n |
|---|
| 1256 | if ((m.eq.0).and.(a(i).ge.curent)) go to 10 |
|---|
| 1257 | if ((m.eq.1).and.(a(i).le.curent)) go to 10 |
|---|
| 1258 | curent=a(i) |
|---|
| 1259 | maxmin=i |
|---|
| 1260 | 10 continue |
|---|
| 1261 | enddo |
|---|
| 1262 | return |
|---|
| 1263 | end |
|---|
| 1264 | subroutine dinv(n,a,idim,r,ifail) |
|---|
| 1265 | implicit none |
|---|
| 1266 | ! |
|---|
| 1267 | ! ****************************************************************** |
|---|
| 1268 | ! |
|---|
| 1269 | ! REPLACES A BY ITS INVERSE. |
|---|
| 1270 | ! |
|---|
| 1271 | ! (PARAMETERS AS FOR DEQINV.) |
|---|
| 1272 | ! |
|---|
| 1273 | ! CALLS ... DFACT, DFINV, F010PR, ABEND. |
|---|
| 1274 | ! |
|---|
| 1275 | ! ****************************************************************** |
|---|
| 1276 | integer n,idim,ifail,jfail |
|---|
| 1277 | integer r(n),t1,t2,t3 |
|---|
| 1278 | double precision a(idim,n),det,temp,s,c11,c12,c13,c21,c22,c23,c31,& |
|---|
| 1279 | &c32,c33,zero |
|---|
| 1280 | parameter(zero=0d0) |
|---|
| 1281 | |
|---|
| 1282 | ! |
|---|
| 1283 | ! TEST FOR PARAMETER ERRORS. |
|---|
| 1284 | ! |
|---|
| 1285 | if((n.lt.1).or.(n.gt.idim)) go to 7 |
|---|
| 1286 | ! |
|---|
| 1287 | ! TEST FOR N.LE.3. |
|---|
| 1288 | ! |
|---|
| 1289 | if(n.gt.3) go to 6 |
|---|
| 1290 | ifail=0 |
|---|
| 1291 | if(n.lt.3) go to 4 |
|---|
| 1292 | ! |
|---|
| 1293 | ! N=3 CASE. |
|---|
| 1294 | ! |
|---|
| 1295 | ! COMPUTE COFACTORS. |
|---|
| 1296 | c11=a(2,2)*a(3,3)-a(2,3)*a(3,2) |
|---|
| 1297 | c12=a(2,3)*a(3,1)-a(2,1)*a(3,3) |
|---|
| 1298 | c13=a(2,1)*a(3,2)-a(2,2)*a(3,1) |
|---|
| 1299 | c21=a(3,2)*a(1,3)-a(3,3)*a(1,2) |
|---|
| 1300 | c22=a(3,3)*a(1,1)-a(3,1)*a(1,3) |
|---|
| 1301 | c23=a(3,1)*a(1,2)-a(3,2)*a(1,1) |
|---|
| 1302 | c31=a(1,2)*a(2,3)-a(1,3)*a(2,2) |
|---|
| 1303 | c32=a(1,3)*a(2,1)-a(1,1)*a(2,3) |
|---|
| 1304 | c33=a(1,1)*a(2,2)-a(1,2)*a(2,1) |
|---|
| 1305 | t1=abs(sngl(a(1,1))) |
|---|
| 1306 | t2=abs(sngl(a(2,1))) |
|---|
| 1307 | t3=abs(sngl(a(3,1))) |
|---|
| 1308 | ! |
|---|
| 1309 | ! (SET TEMP=PIVOT AND DET=PIVOT*DET.) |
|---|
| 1310 | if(t1.ge.t2) go to 1 |
|---|
| 1311 | if(t3.ge.t2) go to 2 |
|---|
| 1312 | ! (PIVOT IS A21) |
|---|
| 1313 | temp=a(2,1) |
|---|
| 1314 | det=c13*c32-c12*c33 |
|---|
| 1315 | go to 3 |
|---|
| 1316 | 1 if(t3.ge.t1) go to 2 |
|---|
| 1317 | ! (PIVOT IS A11) |
|---|
| 1318 | temp=a(1,1) |
|---|
| 1319 | det=c22*c33-c23*c32 |
|---|
| 1320 | go to 3 |
|---|
| 1321 | ! (PIVOT IS A31) |
|---|
| 1322 | 2 temp=a(3,1) |
|---|
| 1323 | det=c23*c12-c22*c13 |
|---|
| 1324 | ! |
|---|
| 1325 | ! SET ELEMENTS OF INVERSE IN A. |
|---|
| 1326 | 3 if(det.eq.zero) go to 8 |
|---|
| 1327 | s=temp/det |
|---|
| 1328 | a(1,1)=s*c11 |
|---|
| 1329 | a(1,2)=s*c21 |
|---|
| 1330 | a(1,3)=s*c31 |
|---|
| 1331 | a(2,1)=s*c12 |
|---|
| 1332 | a(2,2)=s*c22 |
|---|
| 1333 | a(2,3)=s*c32 |
|---|
| 1334 | a(3,1)=s*c13 |
|---|
| 1335 | a(3,2)=s*c23 |
|---|
| 1336 | a(3,3)=s*c33 |
|---|
| 1337 | return |
|---|
| 1338 | ! |
|---|
| 1339 | 4 if(n.lt.2) go to 5 |
|---|
| 1340 | ! |
|---|
| 1341 | ! N=2 CASE BY CRAMERS RULE. |
|---|
| 1342 | ! |
|---|
| 1343 | det=a(1,1)*a(2,2)-a(1,2)*a(2,1) |
|---|
| 1344 | if(det.eq.zero) go to 8 |
|---|
| 1345 | s=1d0/det |
|---|
| 1346 | c11 =s*a(2,2) |
|---|
| 1347 | a(1,2)=-s*a(1,2) |
|---|
| 1348 | a(2,1)=-s*a(2,1) |
|---|
| 1349 | a(2,2)=s*a(1,1) |
|---|
| 1350 | a(1,1)=c11 |
|---|
| 1351 | return |
|---|
| 1352 | ! |
|---|
| 1353 | ! N=1 CASE. |
|---|
| 1354 | ! |
|---|
| 1355 | 5 if(a(1,1).eq.zero) go to 8 |
|---|
| 1356 | a(1,1)=1d0/a(1,1) |
|---|
| 1357 | return |
|---|
| 1358 | ! |
|---|
| 1359 | ! N.GT.3 CASES. FACTORIZE MATRIX AND INVERT. |
|---|
| 1360 | ! |
|---|
| 1361 | 6 call dfact(n,a,idim,r,ifail,det,jfail) |
|---|
| 1362 | if(ifail.ne.0) return |
|---|
| 1363 | call dfinv(n,a,idim,r) |
|---|
| 1364 | return |
|---|
| 1365 | ! |
|---|
| 1366 | ! ERROR EXITS. |
|---|
| 1367 | ! |
|---|
| 1368 | 7 ifail=+1 |
|---|
| 1369 | return |
|---|
| 1370 | ! |
|---|
| 1371 | 8 ifail=-1 |
|---|
| 1372 | return |
|---|
| 1373 | ! |
|---|
| 1374 | end |
|---|
| 1375 | subroutine dfact(n,a,idim,ir,ifail,det,jfail) |
|---|
| 1376 | implicit none |
|---|
| 1377 | integer idim,j,k,n,ifail,nxch,jp1,i,l,jm1,jfail,ir(*),normal, & |
|---|
| 1378 | &imposs,jrange,jover,junder |
|---|
| 1379 | parameter(normal=0,imposs=-1,jrange=0,jover=1,junder=-1) |
|---|
| 1380 | real p,q,t,g1,g2 |
|---|
| 1381 | parameter(g1=1e-19,g2=1e19) |
|---|
| 1382 | double precision a(idim,*),det,tf,s11,s12,zero,one |
|---|
| 1383 | parameter(zero=0d0,one=1d0) |
|---|
| 1384 | character*6 hname |
|---|
| 1385 | data hname / ' DFACT' / |
|---|
| 1386 | |
|---|
| 1387 | if(idim .ge. n .and. n .gt. 0) goto 110 |
|---|
| 1388 | call tmprnt(hname,n,idim,0) |
|---|
| 1389 | return |
|---|
| 1390 | 110 ifail = normal |
|---|
| 1391 | jfail = jrange |
|---|
| 1392 | nxch = 0 |
|---|
| 1393 | det = one |
|---|
| 1394 | do j = 1, n |
|---|
| 1395 | 120 k = j |
|---|
| 1396 | p = abs(sngl(a(j,j))) |
|---|
| 1397 | if(j .eq. n) goto 122 |
|---|
| 1398 | jp1 = j+1 |
|---|
| 1399 | do i = jp1, n |
|---|
| 1400 | q = abs(sngl(a(i,j))) |
|---|
| 1401 | if(q .le. p) goto 121 |
|---|
| 1402 | k = i |
|---|
| 1403 | p = q |
|---|
| 1404 | 121 continue |
|---|
| 1405 | enddo |
|---|
| 1406 | if(k .ne. j) goto 123 |
|---|
| 1407 | 122 if(p .gt. zero) goto 130 |
|---|
| 1408 | det = zero |
|---|
| 1409 | ifail = imposs |
|---|
| 1410 | jfail = jrange |
|---|
| 1411 | return |
|---|
| 1412 | 123 do l = 1, n |
|---|
| 1413 | tf = a(j,l) |
|---|
| 1414 | a(j,l) = a(k,l) |
|---|
| 1415 | a(k,l) = tf |
|---|
| 1416 | enddo |
|---|
| 1417 | nxch = nxch + 1 |
|---|
| 1418 | ir(nxch) = j*2**12 + k |
|---|
| 1419 | 130 det = det * a(j,j) |
|---|
| 1420 | a(j,j) = one / a(j,j) |
|---|
| 1421 | t = abs(sngl(det)) |
|---|
| 1422 | if(t .lt. g1) then |
|---|
| 1423 | det = zero |
|---|
| 1424 | if(jfail .eq. jrange) jfail = junder |
|---|
| 1425 | elseif(t .gt. g2) then |
|---|
| 1426 | det = one |
|---|
| 1427 | if(jfail .eq. jrange) jfail = jover |
|---|
| 1428 | endif |
|---|
| 1429 | if(j .eq. n) goto 144 |
|---|
| 1430 | jm1 = j-1 |
|---|
| 1431 | jp1 = j+1 |
|---|
| 1432 | do k = jp1, n |
|---|
| 1433 | s11 = -a(j,k) |
|---|
| 1434 | s12 = -a(k,j+1) |
|---|
| 1435 | if(j .eq. 1) goto 142 |
|---|
| 1436 | do i = 1, jm1 |
|---|
| 1437 | s11 = a(i,k)*a(j,i)+s11 |
|---|
| 1438 | s12 = a(i,j+1)*a(k,i)+s12 |
|---|
| 1439 | enddo |
|---|
| 1440 | 142 a(j,k) = -s11 * a(j,j) |
|---|
| 1441 | a(k,j+1) = -(a(j,j+1)*a(k,j)+s12) |
|---|
| 1442 | enddo |
|---|
| 1443 | 144 continue |
|---|
| 1444 | enddo |
|---|
| 1445 | 150 if(mod(nxch,2) .ne. 0) det = -det |
|---|
| 1446 | if(jfail .ne. jrange) det = zero |
|---|
| 1447 | ir(n) = nxch |
|---|
| 1448 | return |
|---|
| 1449 | end |
|---|
| 1450 | subroutine dfeqn(n,a,idim,ir,k,b) |
|---|
| 1451 | implicit none |
|---|
| 1452 | integer idim,i,j,k,n,m,nxch,ij,l,im1,nm1,nmi,nmjp1,ir(*) |
|---|
| 1453 | double precision a(idim,*),b(idim,*),te,s21, s22 |
|---|
| 1454 | character*6 hname |
|---|
| 1455 | data hname / ' DFEQN' / |
|---|
| 1456 | |
|---|
| 1457 | if(idim .ge. n .and. n .gt. 0 .and. k .gt. 0) goto 210 |
|---|
| 1458 | call tmprnt(hname,n,idim,k) |
|---|
| 1459 | return |
|---|
| 1460 | 210 nxch = ir(n) |
|---|
| 1461 | if(nxch .eq. 0) goto 220 |
|---|
| 1462 | do m = 1, nxch |
|---|
| 1463 | ij = ir(m) |
|---|
| 1464 | i = ij / 4096 |
|---|
| 1465 | j = mod(ij,4096) |
|---|
| 1466 | do l = 1, k |
|---|
| 1467 | te = b(i,l) |
|---|
| 1468 | b(i,l) = b(j,l) |
|---|
| 1469 | b(j,l) = te |
|---|
| 1470 | enddo |
|---|
| 1471 | enddo |
|---|
| 1472 | 220 do l = 1, k |
|---|
| 1473 | b(1,l) = a(1,1)*b(1,l) |
|---|
| 1474 | enddo |
|---|
| 1475 | if(n .eq. 1) goto 299 |
|---|
| 1476 | do l = 1, k |
|---|
| 1477 | do i = 2, n |
|---|
| 1478 | im1 = i-1 |
|---|
| 1479 | s21 = - b(i,l) |
|---|
| 1480 | do j = 1, im1 |
|---|
| 1481 | s21 = a(i,j)*b(j,l)+s21 |
|---|
| 1482 | enddo |
|---|
| 1483 | b(i,l) = - a(i,i)*s21 |
|---|
| 1484 | enddo |
|---|
| 1485 | nm1 = n-1 |
|---|
| 1486 | do i = 1, nm1 |
|---|
| 1487 | nmi = n-i |
|---|
| 1488 | s22 = - b(nmi,l) |
|---|
| 1489 | do j = 1, i |
|---|
| 1490 | nmjp1 = n - j+1 |
|---|
| 1491 | s22 = a(nmi,nmjp1)*b(nmjp1,l)+s22 |
|---|
| 1492 | enddo |
|---|
| 1493 | b(nmi,l) = - s22 |
|---|
| 1494 | enddo |
|---|
| 1495 | enddo |
|---|
| 1496 | 299 continue |
|---|
| 1497 | return |
|---|
| 1498 | end |
|---|
| 1499 | subroutine dfinv(n,a,idim,ir) |
|---|
| 1500 | implicit none |
|---|
| 1501 | integer idim,i,j,k,n,m,nxch,ij,nm1,nmi,im2,ir(*) |
|---|
| 1502 | double precision a(idim,*),ti,s31,s32,s33,s34,zero |
|---|
| 1503 | parameter(zero=0d0) |
|---|
| 1504 | character*6 hname |
|---|
| 1505 | data hname / ' DFINV' / |
|---|
| 1506 | |
|---|
| 1507 | if(idim .ge. n .and. n .gt. 0) goto 310 |
|---|
| 1508 | call tmprnt(hname,n,idim,0) |
|---|
| 1509 | return |
|---|
| 1510 | 310 if(n .eq. 1) return |
|---|
| 1511 | a(2,1) = -a(2,2) * a(1,1) * a(2,1) |
|---|
| 1512 | a(1,2) = -a(1,2) |
|---|
| 1513 | if(n .eq. 2) goto 330 |
|---|
| 1514 | do i = 3, n |
|---|
| 1515 | im2 = i-2 |
|---|
| 1516 | do j = 1, im2 |
|---|
| 1517 | s31 = zero |
|---|
| 1518 | s32 = a(j,i) |
|---|
| 1519 | do k = j, im2 |
|---|
| 1520 | s31 = a(k,j)*a(i,k)+s31 |
|---|
| 1521 | s32 = a(j,k+1)*a(k+1,i)+s32 |
|---|
| 1522 | enddo |
|---|
| 1523 | a(i,j) = -a(i,i) * (a(i-1,j)*a(i,i-1)+s31) |
|---|
| 1524 | a(j,i) = -s32 |
|---|
| 1525 | enddo |
|---|
| 1526 | a(i,i-1) = -a(i,i) * a(i-1,i-1) * a(i,i-1) |
|---|
| 1527 | a(i-1,i) = -a(i-1,i) |
|---|
| 1528 | enddo |
|---|
| 1529 | 330 nm1 = n-1 |
|---|
| 1530 | do i = 1, nm1 |
|---|
| 1531 | nmi = n-i |
|---|
| 1532 | do j = 1, i |
|---|
| 1533 | s33 = a(i,j) |
|---|
| 1534 | do k = 1, nmi |
|---|
| 1535 | s33 = a(i+k,j)*a(i,i+k)+s33 |
|---|
| 1536 | enddo |
|---|
| 1537 | a(i,j) = s33 |
|---|
| 1538 | enddo |
|---|
| 1539 | do j = 1, nmi |
|---|
| 1540 | s34 = zero |
|---|
| 1541 | do k = j, nmi |
|---|
| 1542 | s34 = a(i+k,i+j)*a(i,i+k)+s34 |
|---|
| 1543 | enddo |
|---|
| 1544 | a(i,i+j) = s34 |
|---|
| 1545 | enddo |
|---|
| 1546 | enddo |
|---|
| 1547 | nxch = ir(n) |
|---|
| 1548 | if(nxch .eq. 0) return |
|---|
| 1549 | do m = 1, nxch |
|---|
| 1550 | k = nxch - m+1 |
|---|
| 1551 | ij = ir(k) |
|---|
| 1552 | i = ij / 4096 |
|---|
| 1553 | j = mod(ij,4096) |
|---|
| 1554 | do k = 1, n |
|---|
| 1555 | ti = a(k,i) |
|---|
| 1556 | a(k,i) = a(k,j) |
|---|
| 1557 | a(k,j) = ti |
|---|
| 1558 | enddo |
|---|
| 1559 | enddo |
|---|
| 1560 | return |
|---|
| 1561 | end |
|---|
| 1562 | subroutine tmprnt(name,n,idim,k) |
|---|
| 1563 | implicit none |
|---|
| 1564 | logical mflag,rflag |
|---|
| 1565 | integer idim,k,n,ifmt |
|---|
| 1566 | character*6 name |
|---|
| 1567 | |
|---|
| 1568 | mflag=.true. |
|---|
| 1569 | rflag=.true. |
|---|
| 1570 | if(name(3:6) .eq. 'FEQN') ifmt=1001 |
|---|
| 1571 | if(name(3:6) .ne. 'FEQN') ifmt=1002 |
|---|
| 1572 | if(mflag) then |
|---|
| 1573 | if(name(3:6) .eq. 'feqn') then |
|---|
| 1574 | if(ifmt.eq.1001) write(*,1001) name, n, idim, k |
|---|
| 1575 | if(ifmt.eq.1002) write(*,1002) name, n, idim, k |
|---|
| 1576 | else |
|---|
| 1577 | if(ifmt.eq.1001) write(*,1001) name, n, idim |
|---|
| 1578 | if(ifmt.eq.1002) write(*,1002) name, n, idim |
|---|
| 1579 | endif |
|---|
| 1580 | endif |
|---|
| 1581 | if(.not. rflag) call abend |
|---|
| 1582 | return |
|---|
| 1583 | 1001 FORMAT(7X," PARAMETER ERROR IN SUBROUTINE ", A6, & |
|---|
| 1584 | &" ... (N.LT.1 OR IDIM.LT.N).", & |
|---|
| 1585 | &5X,"N =", I4, 5X,"IDIM =", I4,".") |
|---|
| 1586 | 1002 FORMAT(7X," PARAMETER ERROR IN SUBROUTINE ", A6, & |
|---|
| 1587 | &" ... (N.LT.1 OR IDIM.LT.N OR K.LT.1).", & |
|---|
| 1588 | &5X,"N =", I4, 5X,"IDIM =", I4, 5X,"K =", I4,".") |
|---|
| 1589 | end |
|---|
| 1590 | subroutine abend |
|---|
| 1591 | implicit none |
|---|
| 1592 | write(*,*) 'Abnormal end ...' |
|---|
| 1593 | stop 888 |
|---|
| 1594 | end |
|---|
| 1595 | subroutine dmmpy(m,n,x,x12,x21,y,y2,z,z2) |
|---|
| 1596 | |
|---|
| 1597 | |
|---|
| 1598 | implicit none |
|---|
| 1599 | integer m,n,ix,jx,jy,iz,lxi1,lzi,i,lxij,lyj,j,locf |
|---|
| 1600 | double precision x(*),x12(*),x21(*),y(*),y2(*),z(*),z2(*),sum,zero |
|---|
| 1601 | parameter(zero=0d0) |
|---|
| 1602 | |
|---|
| 1603 | if(m .le. 0 .or. n .le. 0) return |
|---|
| 1604 | ix = (locf(x21) - locf(x)) / 2 |
|---|
| 1605 | jx = (locf(x12) - locf(x)) / 2 |
|---|
| 1606 | jy = (locf(y2) - locf(y)) / 2 |
|---|
| 1607 | iz = (locf(z2) - locf(z)) / 2 |
|---|
| 1608 | lxi1 = 1 |
|---|
| 1609 | lzi = 1 |
|---|
| 1610 | do i = 1, m |
|---|
| 1611 | lxij = lxi1 |
|---|
| 1612 | lyj = 1 |
|---|
| 1613 | sum = zero |
|---|
| 1614 | do j = 1, n |
|---|
| 1615 | sum = x(lxij)*y(lyj)+sum |
|---|
| 1616 | lxij = lxij + jx |
|---|
| 1617 | lyj = lyj + jy |
|---|
| 1618 | enddo |
|---|
| 1619 | z(lzi) = sum |
|---|
| 1620 | lxi1 = lxi1 + ix |
|---|
| 1621 | lzi = lzi + iz |
|---|
| 1622 | enddo |
|---|
| 1623 | return |
|---|
| 1624 | end |
|---|
| 1625 | subroutine dmmlt(m,n,k,x,x12,x21,y,y12,y21,z,z12,z21,t) |
|---|
| 1626 | |
|---|
| 1627 | |
|---|
| 1628 | implicit none |
|---|
| 1629 | integer m,n,k,ix,jx,jy,iz,lz,ly1l,lz1l,l,lxi1,lzil,i,lxij,lyjl, & |
|---|
| 1630 | &j,locf,lzii,lxk1,lzik,kdash,lxkj,ltl,lxil,lti,lyil,lxii,ltk,lxik, & |
|---|
| 1631 | &lxki,locx,locy,ly,lzki,locz |
|---|
| 1632 | double precision x(*),x12(*),x21(*),y(*),y12(*),y21(*),z(*), & |
|---|
| 1633 | &z12(*),z21(*),t(*),s11,s21,s22,s31,s41,s51,s52,zero |
|---|
| 1634 | parameter(zero=0d0) |
|---|
| 1635 | |
|---|
| 1636 | if(min0(m,n,k) .le. 0) return |
|---|
| 1637 | locx = locf(x(1)) |
|---|
| 1638 | locy = locf(y(1)) |
|---|
| 1639 | locz = locf(z(1)) |
|---|
| 1640 | ix = (locf(x21(1)) - locx) / 2 |
|---|
| 1641 | jx = (locf(x12(1)) - locx) / 2 |
|---|
| 1642 | jy = (locf(y21(1)) - locy) / 2 |
|---|
| 1643 | ly = (locf(y12(1)) - locy) / 2 |
|---|
| 1644 | iz = (locf(z21(1)) - locz) / 2 |
|---|
| 1645 | lz = (locf(z12(1)) - locz) / 2 |
|---|
| 1646 | if(locz .eq. locx) goto 30 |
|---|
| 1647 | if(locz .eq. locy) goto 40 |
|---|
| 1648 | if(locx .eq. locy) goto 20 |
|---|
| 1649 | 10 ly1l = 1 |
|---|
| 1650 | lz1l = 1 |
|---|
| 1651 | do l = 1, k |
|---|
| 1652 | lxi1 = 1 |
|---|
| 1653 | lzil = lz1l |
|---|
| 1654 | do i = 1, m |
|---|
| 1655 | s11 = zero |
|---|
| 1656 | lxij = lxi1 |
|---|
| 1657 | lyjl = ly1l |
|---|
| 1658 | do j = 1, n |
|---|
| 1659 | s11 = x(lxij)*y(lyjl)+s11 |
|---|
| 1660 | lxij = lxij + jx |
|---|
| 1661 | lyjl = lyjl + jy |
|---|
| 1662 | enddo |
|---|
| 1663 | z(lzil) = s11 |
|---|
| 1664 | lxi1 = lxi1 + ix |
|---|
| 1665 | lzil = lzil + iz |
|---|
| 1666 | enddo |
|---|
| 1667 | ly1l = ly1l + ly |
|---|
| 1668 | lz1l = lz1l + lz |
|---|
| 1669 | enddo |
|---|
| 1670 | return |
|---|
| 1671 | 20 if(m .ne. k .or. ix .ne. ly .or. jx .ne. jy) goto 10 |
|---|
| 1672 | lxi1 = 1 |
|---|
| 1673 | lzii = 1 |
|---|
| 1674 | do i = 1, m |
|---|
| 1675 | s21 = zero |
|---|
| 1676 | lxij = lxi1 |
|---|
| 1677 | do j = 1, n |
|---|
| 1678 | s21 = x(lxij)*x(lxij)+s21 |
|---|
| 1679 | lxij = lxij + jx |
|---|
| 1680 | enddo |
|---|
| 1681 | z(lzii) = s21 |
|---|
| 1682 | if(i .eq. m) goto 24 |
|---|
| 1683 | lxk1 = lxi1 + ix |
|---|
| 1684 | lzik = lzii + lz |
|---|
| 1685 | lzki = lzii + iz |
|---|
| 1686 | do kdash = i+1, m |
|---|
| 1687 | s22 = zero |
|---|
| 1688 | lxij = lxi1 |
|---|
| 1689 | lxkj = lxk1 |
|---|
| 1690 | do j = 1, n |
|---|
| 1691 | s22 = x(lxij)*x(lxkj)+s22 |
|---|
| 1692 | lxij = lxij + jx |
|---|
| 1693 | lxkj = lxkj + jx |
|---|
| 1694 | enddo |
|---|
| 1695 | z(lzik) = s22 |
|---|
| 1696 | z(lzki) = z(lzik) |
|---|
| 1697 | lxk1 = lxk1 + ix |
|---|
| 1698 | lzik = lzik + lz |
|---|
| 1699 | lzki = lzki + iz |
|---|
| 1700 | enddo |
|---|
| 1701 | lxi1 = lxi1 + ix |
|---|
| 1702 | lzii = lzii + iz + lz |
|---|
| 1703 | 24 continue |
|---|
| 1704 | enddo |
|---|
| 1705 | return |
|---|
| 1706 | 30 if(locx .eq. locy) goto 50 |
|---|
| 1707 | lxi1 = 1 |
|---|
| 1708 | do i = 1, m |
|---|
| 1709 | ly1l = 1 |
|---|
| 1710 | ltl = 1 |
|---|
| 1711 | do l = 1, k |
|---|
| 1712 | s31 = zero |
|---|
| 1713 | lxij = lxi1 |
|---|
| 1714 | lyjl = ly1l |
|---|
| 1715 | do j = 1, n |
|---|
| 1716 | s31 = x(lxij)*y(lyjl)+s31 |
|---|
| 1717 | lxij = lxij + jx |
|---|
| 1718 | lyjl = lyjl + jy |
|---|
| 1719 | enddo |
|---|
| 1720 | t(ltl) = s31 |
|---|
| 1721 | ly1l = ly1l + ly |
|---|
| 1722 | ltl = ltl + 1 |
|---|
| 1723 | enddo |
|---|
| 1724 | lxil = lxi1 |
|---|
| 1725 | ltl = 1 |
|---|
| 1726 | do l = 1, k |
|---|
| 1727 | x(lxil) = t(ltl) |
|---|
| 1728 | lxil = lxil + jx |
|---|
| 1729 | ltl = ltl + 1 |
|---|
| 1730 | enddo |
|---|
| 1731 | lxi1 = lxi1 + ix |
|---|
| 1732 | enddo |
|---|
| 1733 | return |
|---|
| 1734 | 40 ly1l = 1 |
|---|
| 1735 | do l = 1, k |
|---|
| 1736 | lxi1 = 1 |
|---|
| 1737 | lti = 1 |
|---|
| 1738 | do i = 1, m |
|---|
| 1739 | s41 = zero |
|---|
| 1740 | lxij = lxi1 |
|---|
| 1741 | lyjl = ly1l |
|---|
| 1742 | do j = 1, n |
|---|
| 1743 | s41 = x(lxij)*y(lyjl)+s41 |
|---|
| 1744 | lxij = lxij + jx |
|---|
| 1745 | lyjl = lyjl + jy |
|---|
| 1746 | enddo |
|---|
| 1747 | t(lti) = s41 |
|---|
| 1748 | lxi1 = lxi1 + ix |
|---|
| 1749 | lti = lti + 1 |
|---|
| 1750 | enddo |
|---|
| 1751 | lyil = ly1l |
|---|
| 1752 | lti = 1 |
|---|
| 1753 | do i = 1, m |
|---|
| 1754 | y(lyil) = t(lti) |
|---|
| 1755 | lyil = lyil + jy |
|---|
| 1756 | lti = lti + 1 |
|---|
| 1757 | enddo |
|---|
| 1758 | ly1l = ly1l + ly |
|---|
| 1759 | enddo |
|---|
| 1760 | return |
|---|
| 1761 | 50 lxi1 = 1 |
|---|
| 1762 | lxii = 1 |
|---|
| 1763 | do i = 1, m |
|---|
| 1764 | s51 = zero |
|---|
| 1765 | lxij = lxi1 |
|---|
| 1766 | do j = 1, n |
|---|
| 1767 | s51 = x(lxij)*x(lxij)+s51 |
|---|
| 1768 | lxij = lxij + jx |
|---|
| 1769 | enddo |
|---|
| 1770 | t(1) = s51 |
|---|
| 1771 | if(i .eq. m) goto 54 |
|---|
| 1772 | lxk1 = lxi1 + ix |
|---|
| 1773 | ltk = 2 |
|---|
| 1774 | do kdash = i+1, m |
|---|
| 1775 | s52 = zero |
|---|
| 1776 | lxij = lxi1 |
|---|
| 1777 | lxkj = lxk1 |
|---|
| 1778 | do j = 1, n |
|---|
| 1779 | s52 = x(lxij)*x(lxkj)+s52 |
|---|
| 1780 | lxij = lxij + jx |
|---|
| 1781 | lxkj = lxkj + jx |
|---|
| 1782 | enddo |
|---|
| 1783 | t(ltk) = s52 |
|---|
| 1784 | lxk1 = lxk1 + ix |
|---|
| 1785 | ltk = ltk + 1 |
|---|
| 1786 | enddo |
|---|
| 1787 | 54 lxik = lxii |
|---|
| 1788 | ltk = 1 |
|---|
| 1789 | do kdash = i, m |
|---|
| 1790 | x(lxik) = t(ltk) |
|---|
| 1791 | lxik = lxik + jx |
|---|
| 1792 | ltk = ltk + 1 |
|---|
| 1793 | enddo |
|---|
| 1794 | lxi1 = lxi1 + ix |
|---|
| 1795 | lxii = lxii + ix + jx |
|---|
| 1796 | enddo |
|---|
| 1797 | if(m .eq. 1) return |
|---|
| 1798 | lxii = 1 |
|---|
| 1799 | do i = 1, m-1 |
|---|
| 1800 | lxik = lxii + jx |
|---|
| 1801 | lxki = lxii + ix |
|---|
| 1802 | do kdash = i+1, m |
|---|
| 1803 | x(lxki) = x(lxik) |
|---|
| 1804 | lxik = lxik + jx |
|---|
| 1805 | lxki = lxki + ix |
|---|
| 1806 | enddo |
|---|
| 1807 | lxii = lxii + ix + jx |
|---|
| 1808 | enddo |
|---|
| 1809 | return |
|---|
| 1810 | end |
|---|
| 1811 | !DECK ID>, SVD. |
|---|
| 1812 | SUBROUTINE SVD(NM,M,N,A,W,MATU,U,MATV,V,IERR,RV1) |
|---|
| 1813 | ! |
|---|
| 1814 | implicit none |
|---|
| 1815 | integer i,j,k,l,m,n,ii,i1,kk,k1,ll,l1,mn,nm,its,ierr |
|---|
| 1816 | double precision a(nm,n),w(n),u(nm,n),v(nm,n),rv1(n) |
|---|
| 1817 | double precision c,f,g,h,s,x,y,z,tst1,tst2,scale,pythag |
|---|
| 1818 | logical matu,matv |
|---|
| 1819 | ! |
|---|
| 1820 | ! ------------------------------------------------------------------ |
|---|
| 1821 | ! this subroutine is a translation of the algol procedure svd, |
|---|
| 1822 | ! NUM. MATH. 14, 403-420(1970) by golub and reinsch. |
|---|
| 1823 | ! handbook for auto. comp., vol 2 -linear algebra, 134-151(1971). |
|---|
| 1824 | ! |
|---|
| 1825 | ! this subroutine determines the singular value decomposition |
|---|
| 1826 | ! t |
|---|
| 1827 | ! a=usv of a real m by n rectangular matrix. householder |
|---|
| 1828 | ! bidiagonalization and a variant of the qr algorithm are used. |
|---|
| 1829 | ! |
|---|
| 1830 | ! on input |
|---|
| 1831 | ! |
|---|
| 1832 | ! nm must be set to the row dimension of two-dimensional |
|---|
| 1833 | ! array parameters as declared in the calling program |
|---|
| 1834 | ! dimension statement. note that nm must be at least |
|---|
| 1835 | ! as large as the maximum of m and n. |
|---|
| 1836 | ! |
|---|
| 1837 | ! m is the number of rows of a (and u). |
|---|
| 1838 | ! |
|---|
| 1839 | ! n is the number of columns of a (and u) and the order of v. |
|---|
| 1840 | ! |
|---|
| 1841 | ! a contains the rectangular input matrix to be decomposed. |
|---|
| 1842 | ! |
|---|
| 1843 | ! matu should be set to .true. if the u matrix in the |
|---|
| 1844 | ! decomposition is desired, and to .false. otherwise. |
|---|
| 1845 | ! |
|---|
| 1846 | ! matv should be set to .true. if the v matrix in the |
|---|
| 1847 | ! decomposition is desired, and to .false. otherwise. |
|---|
| 1848 | ! |
|---|
| 1849 | ! on output |
|---|
| 1850 | ! |
|---|
| 1851 | ! a is unaltered (unless overwritten by u or v). |
|---|
| 1852 | ! |
|---|
| 1853 | ! w contains the n (non-negative) singular values of a (the |
|---|
| 1854 | ! diagonal elements of s). they are unordered. if an |
|---|
| 1855 | ! error exit is made, the singular values should be correct |
|---|
| 1856 | ! for indices ierr+1,ierr+2,...,n. |
|---|
| 1857 | ! |
|---|
| 1858 | ! u contains the matrix u (orthogonal column vectors) of the |
|---|
| 1859 | ! decomposition if matu has been set to .true. otherwise |
|---|
| 1860 | ! u is used as a temporary array. u may coincide with a. |
|---|
| 1861 | ! if an error exit is made, the columns of u corresponding |
|---|
| 1862 | ! to indices of correct singular values should be correct. |
|---|
| 1863 | ! |
|---|
| 1864 | ! v contains the matrix v (orthogonal) of the decomposition if |
|---|
| 1865 | ! matv has been set to .true. otherwise v is not referenced. |
|---|
| 1866 | ! v may also coincide with a if u is not needed. if an error |
|---|
| 1867 | ! exit is made, the columns of v corresponding to indices of |
|---|
| 1868 | ! correct singular values should be correct. |
|---|
| 1869 | ! |
|---|
| 1870 | ! ierr is set to |
|---|
| 1871 | ! zero for normal return, |
|---|
| 1872 | ! k if the k-th singular value has not been |
|---|
| 1873 | ! determined after 30 iterations. |
|---|
| 1874 | ! |
|---|
| 1875 | ! rv1 is a temporary storage array. |
|---|
| 1876 | ! |
|---|
| 1877 | ! calls pythag for dsqrt(a*a + b*b) . |
|---|
| 1878 | ! |
|---|
| 1879 | ! questions and comments should be directed to burton s. garbow, |
|---|
| 1880 | ! mathematics and computer science div, argonne national laboratory |
|---|
| 1881 | ! |
|---|
| 1882 | ! this version dated august 1983. |
|---|
| 1883 | ! ------------------------------------------------------------------ |
|---|
| 1884 | ! |
|---|
| 1885 | ierr = 0 |
|---|
| 1886 | ! |
|---|
| 1887 | do i = 1, m |
|---|
| 1888 | do j = 1, n |
|---|
| 1889 | u(i,j) = a(i,j) |
|---|
| 1890 | enddo |
|---|
| 1891 | enddo |
|---|
| 1892 | ! .......... householder reduction to bidiagonal form .......... |
|---|
| 1893 | g = 0.0d0 |
|---|
| 1894 | scale = 0.0d0 |
|---|
| 1895 | x = 0.0d0 |
|---|
| 1896 | ! |
|---|
| 1897 | do i = 1, n |
|---|
| 1898 | l = i + 1 |
|---|
| 1899 | rv1(i) = scale * g |
|---|
| 1900 | g = 0.0d0 |
|---|
| 1901 | s = 0.0d0 |
|---|
| 1902 | scale = 0.0d0 |
|---|
| 1903 | if (i .gt. m) go to 210 |
|---|
| 1904 | ! |
|---|
| 1905 | do k = i, m |
|---|
| 1906 | scale = scale + dabs(u(k,i)) |
|---|
| 1907 | enddo |
|---|
| 1908 | ! |
|---|
| 1909 | if (scale .eq. 0.0d0) go to 210 |
|---|
| 1910 | ! |
|---|
| 1911 | do k = i, m |
|---|
| 1912 | u(k,i) = u(k,i) / scale |
|---|
| 1913 | s = s + u(k,i)**2 |
|---|
| 1914 | enddo |
|---|
| 1915 | ! |
|---|
| 1916 | f = u(i,i) |
|---|
| 1917 | g = -dsign(dsqrt(s),f) |
|---|
| 1918 | h = f * g - s |
|---|
| 1919 | u(i,i) = f - g |
|---|
| 1920 | if (i .eq. n) go to 190 |
|---|
| 1921 | ! |
|---|
| 1922 | do j = l, n |
|---|
| 1923 | s = 0.0d0 |
|---|
| 1924 | ! |
|---|
| 1925 | do k = i, m |
|---|
| 1926 | s = s + u(k,i) * u(k,j) |
|---|
| 1927 | enddo |
|---|
| 1928 | ! |
|---|
| 1929 | f = s / h |
|---|
| 1930 | ! |
|---|
| 1931 | do k = i, m |
|---|
| 1932 | u(k,j) = u(k,j) + f * u(k,i) |
|---|
| 1933 | enddo |
|---|
| 1934 | enddo |
|---|
| 1935 | ! |
|---|
| 1936 | 190 do k = i, m |
|---|
| 1937 | u(k,i) = scale * u(k,i) |
|---|
| 1938 | enddo |
|---|
| 1939 | ! |
|---|
| 1940 | 210 w(i) = scale * g |
|---|
| 1941 | g = 0.0d0 |
|---|
| 1942 | s = 0.0d0 |
|---|
| 1943 | scale = 0.0d0 |
|---|
| 1944 | if (i .gt. m .or. i .eq. n) go to 290 |
|---|
| 1945 | ! |
|---|
| 1946 | do k = l, n |
|---|
| 1947 | scale = scale + dabs(u(i,k)) |
|---|
| 1948 | enddo |
|---|
| 1949 | ! |
|---|
| 1950 | if (scale .eq. 0.0d0) go to 290 |
|---|
| 1951 | ! |
|---|
| 1952 | do k = l, n |
|---|
| 1953 | u(i,k) = u(i,k) / scale |
|---|
| 1954 | s = s + u(i,k)**2 |
|---|
| 1955 | enddo |
|---|
| 1956 | ! |
|---|
| 1957 | f = u(i,l) |
|---|
| 1958 | g = -dsign(dsqrt(s),f) |
|---|
| 1959 | h = f * g - s |
|---|
| 1960 | u(i,l) = f - g |
|---|
| 1961 | ! |
|---|
| 1962 | do k = l, n |
|---|
| 1963 | rv1(k) = u(i,k) / h |
|---|
| 1964 | enddo |
|---|
| 1965 | ! |
|---|
| 1966 | if (i .eq. m) go to 270 |
|---|
| 1967 | ! |
|---|
| 1968 | do j = l, m |
|---|
| 1969 | s = 0.0d0 |
|---|
| 1970 | ! |
|---|
| 1971 | do k = l, n |
|---|
| 1972 | s = s + u(j,k) * u(i,k) |
|---|
| 1973 | enddo |
|---|
| 1974 | ! |
|---|
| 1975 | do k = l, n |
|---|
| 1976 | u(j,k) = u(j,k) + s * rv1(k) |
|---|
| 1977 | enddo |
|---|
| 1978 | enddo |
|---|
| 1979 | ! |
|---|
| 1980 | 270 do k = l, n |
|---|
| 1981 | u(i,k) = scale * u(i,k) |
|---|
| 1982 | enddo |
|---|
| 1983 | ! |
|---|
| 1984 | 290 x = dmax1(x,dabs(w(i))+dabs(rv1(i))) |
|---|
| 1985 | enddo |
|---|
| 1986 | ! .......... accumulation of right-hand transformations .......... |
|---|
| 1987 | if (.not. matv) go to 410 |
|---|
| 1988 | ! .......... for i=n step -1 until 1 do -- .......... |
|---|
| 1989 | do ii = 1, n |
|---|
| 1990 | i = n + 1 - ii |
|---|
| 1991 | if (i .eq. n) go to 390 |
|---|
| 1992 | if (g .eq. 0.0d0) go to 360 |
|---|
| 1993 | ! |
|---|
| 1994 | do j = l, n |
|---|
| 1995 | ! .......... double division avoids possible underflow .......... |
|---|
| 1996 | v(j,i) = (u(i,j) / u(i,l)) / g |
|---|
| 1997 | enddo |
|---|
| 1998 | ! |
|---|
| 1999 | do j = l, n |
|---|
| 2000 | s = 0.0d0 |
|---|
| 2001 | ! |
|---|
| 2002 | do k = l, n |
|---|
| 2003 | s = s + u(i,k) * v(k,j) |
|---|
| 2004 | enddo |
|---|
| 2005 | ! |
|---|
| 2006 | do k = l, n |
|---|
| 2007 | v(k,j) = v(k,j) + s * v(k,i) |
|---|
| 2008 | enddo |
|---|
| 2009 | enddo |
|---|
| 2010 | ! |
|---|
| 2011 | 360 do j = l, n |
|---|
| 2012 | v(i,j) = 0.0d0 |
|---|
| 2013 | v(j,i) = 0.0d0 |
|---|
| 2014 | enddo |
|---|
| 2015 | ! |
|---|
| 2016 | 390 v(i,i) = 1.0d0 |
|---|
| 2017 | g = rv1(i) |
|---|
| 2018 | l = i |
|---|
| 2019 | enddo |
|---|
| 2020 | ! .......... accumulation of left-hand transformations .......... |
|---|
| 2021 | 410 if (.not. matu) go to 510 |
|---|
| 2022 | ! ..........for i=min(m,n) step -1 until 1 do -- .......... |
|---|
| 2023 | mn = n |
|---|
| 2024 | if (m .lt. n) mn = m |
|---|
| 2025 | ! |
|---|
| 2026 | do ii = 1, mn |
|---|
| 2027 | i = mn + 1 - ii |
|---|
| 2028 | l = i + 1 |
|---|
| 2029 | g = w(i) |
|---|
| 2030 | if (i .eq. n) go to 430 |
|---|
| 2031 | ! |
|---|
| 2032 | do j = l, n |
|---|
| 2033 | u(i,j) = 0.0d0 |
|---|
| 2034 | enddo |
|---|
| 2035 | ! |
|---|
| 2036 | 430 if (g .eq. 0.0d0) go to 475 |
|---|
| 2037 | if (i .eq. mn) go to 460 |
|---|
| 2038 | ! |
|---|
| 2039 | do j = l, n |
|---|
| 2040 | s = 0.0d0 |
|---|
| 2041 | ! |
|---|
| 2042 | do k = l, m |
|---|
| 2043 | s = s + u(k,i) * u(k,j) |
|---|
| 2044 | enddo |
|---|
| 2045 | ! .......... double division avoids possible underflow .......... |
|---|
| 2046 | f = (s / u(i,i)) / g |
|---|
| 2047 | ! |
|---|
| 2048 | do k = i, m |
|---|
| 2049 | u(k,j) = u(k,j) + f * u(k,i) |
|---|
| 2050 | enddo |
|---|
| 2051 | enddo |
|---|
| 2052 | ! |
|---|
| 2053 | 460 do j = i, m |
|---|
| 2054 | u(j,i) = u(j,i) / g |
|---|
| 2055 | enddo |
|---|
| 2056 | ! |
|---|
| 2057 | go to 490 |
|---|
| 2058 | ! |
|---|
| 2059 | 475 do j = i, m |
|---|
| 2060 | u(j,i) = 0.0d0 |
|---|
| 2061 | enddo |
|---|
| 2062 | ! |
|---|
| 2063 | 490 u(i,i) = u(i,i) + 1.0d0 |
|---|
| 2064 | enddo |
|---|
| 2065 | ! .......... diagonalization of the bidiagonal form .......... |
|---|
| 2066 | 510 tst1 = x |
|---|
| 2067 | ! .......... for k=n step -1 until 1 do -- .......... |
|---|
| 2068 | do kk = 1, n |
|---|
| 2069 | k1 = n - kk |
|---|
| 2070 | k = k1 + 1 |
|---|
| 2071 | its = 0 |
|---|
| 2072 | ! .......... test for splitting. |
|---|
| 2073 | ! for l=k step -1 until 1 do -- .......... |
|---|
| 2074 | 520 do ll = 1, k |
|---|
| 2075 | l1 = k - ll |
|---|
| 2076 | l = l1 + 1 |
|---|
| 2077 | tst2 = tst1 + dabs(rv1(l)) |
|---|
| 2078 | if (tst2 .eq. tst1) go to 565 |
|---|
| 2079 | ! .......... rv1(1) is always zero, so there is no exit |
|---|
| 2080 | ! through the bottom of the loop .......... |
|---|
| 2081 | tst2 = tst1 + dabs(w(l1)) |
|---|
| 2082 | if (tst2 .eq. tst1) go to 540 |
|---|
| 2083 | enddo |
|---|
| 2084 | ! .......... cancellation of rv1(l) if l greater than 1 .......... |
|---|
| 2085 | 540 c = 0.0d0 |
|---|
| 2086 | s = 1.0d0 |
|---|
| 2087 | ! |
|---|
| 2088 | do i = l, k |
|---|
| 2089 | f = s * rv1(i) |
|---|
| 2090 | rv1(i) = c * rv1(i) |
|---|
| 2091 | tst2 = tst1 + dabs(f) |
|---|
| 2092 | if (tst2 .eq. tst1) go to 565 |
|---|
| 2093 | g = w(i) |
|---|
| 2094 | h = pythag(f,g) |
|---|
| 2095 | w(i) = h |
|---|
| 2096 | c = g / h |
|---|
| 2097 | s = -f / h |
|---|
| 2098 | if (.not. matu) go to 560 |
|---|
| 2099 | ! |
|---|
| 2100 | do j = 1, m |
|---|
| 2101 | y = u(j,l1) |
|---|
| 2102 | z = u(j,i) |
|---|
| 2103 | u(j,l1) = y * c + z * s |
|---|
| 2104 | u(j,i) = -y * s + z * c |
|---|
| 2105 | enddo |
|---|
| 2106 | ! |
|---|
| 2107 | 560 continue |
|---|
| 2108 | enddo |
|---|
| 2109 | ! .......... test for convergence .......... |
|---|
| 2110 | 565 z = w(k) |
|---|
| 2111 | if (l .eq. k) go to 650 |
|---|
| 2112 | ! .......... shift from bottom 2 by 2 minor .......... |
|---|
| 2113 | if (its .eq. 30) go to 1000 |
|---|
| 2114 | its = its + 1 |
|---|
| 2115 | x = w(l) |
|---|
| 2116 | y = w(k1) |
|---|
| 2117 | g = rv1(k1) |
|---|
| 2118 | h = rv1(k) |
|---|
| 2119 | f = 0.5d0 * (((g + z) / h) * ((g - z) / y) + y / h - h / y) |
|---|
| 2120 | g = pythag(f,1.0d0) |
|---|
| 2121 | f = x - (z / x) * z + (h / x) * (y / (f + dsign(g,f)) - h) |
|---|
| 2122 | ! .......... next qr transformation .......... |
|---|
| 2123 | c = 1.0d0 |
|---|
| 2124 | s = 1.0d0 |
|---|
| 2125 | ! |
|---|
| 2126 | do i1 = l, k1 |
|---|
| 2127 | i = i1 + 1 |
|---|
| 2128 | g = rv1(i) |
|---|
| 2129 | y = w(i) |
|---|
| 2130 | h = s * g |
|---|
| 2131 | g = c * g |
|---|
| 2132 | z = pythag(f,h) |
|---|
| 2133 | rv1(i1) = z |
|---|
| 2134 | c = f / z |
|---|
| 2135 | s = h / z |
|---|
| 2136 | f = x * c + g * s |
|---|
| 2137 | g = -x * s + g * c |
|---|
| 2138 | h = y * s |
|---|
| 2139 | y = y * c |
|---|
| 2140 | if (.not. matv) go to 575 |
|---|
| 2141 | ! |
|---|
| 2142 | do j = 1, n |
|---|
| 2143 | x = v(j,i1) |
|---|
| 2144 | z = v(j,i) |
|---|
| 2145 | v(j,i1) = x * c + z * s |
|---|
| 2146 | v(j,i) = -x * s + z * c |
|---|
| 2147 | enddo |
|---|
| 2148 | ! |
|---|
| 2149 | 575 z = pythag(f,h) |
|---|
| 2150 | w(i1) = z |
|---|
| 2151 | ! .......... rotation can be arbitrary if z is zero .......... |
|---|
| 2152 | if (z .eq. 0.0d0) go to 580 |
|---|
| 2153 | c = f / z |
|---|
| 2154 | s = h / z |
|---|
| 2155 | 580 f = c * g + s * y |
|---|
| 2156 | x = -s * g + c * y |
|---|
| 2157 | if (.not. matu) go to 600 |
|---|
| 2158 | ! |
|---|
| 2159 | do j = 1, m |
|---|
| 2160 | y = u(j,i1) |
|---|
| 2161 | z = u(j,i) |
|---|
| 2162 | u(j,i1) = y * c + z * s |
|---|
| 2163 | u(j,i) = -y * s + z * c |
|---|
| 2164 | enddo |
|---|
| 2165 | ! |
|---|
| 2166 | 600 continue |
|---|
| 2167 | enddo |
|---|
| 2168 | ! |
|---|
| 2169 | rv1(l) = 0.0d0 |
|---|
| 2170 | rv1(k) = f |
|---|
| 2171 | w(k) = x |
|---|
| 2172 | go to 520 |
|---|
| 2173 | ! .......... convergence .......... |
|---|
| 2174 | 650 if (z .ge. 0.0d0) go to 700 |
|---|
| 2175 | ! .......... w(k) is made non-negative .......... |
|---|
| 2176 | w(k) = -z |
|---|
| 2177 | if (.not. matv) go to 700 |
|---|
| 2178 | ! |
|---|
| 2179 | do j = 1, n |
|---|
| 2180 | v(j,k) = -v(j,k) |
|---|
| 2181 | enddo |
|---|
| 2182 | ! |
|---|
| 2183 | 700 continue |
|---|
| 2184 | enddo |
|---|
| 2185 | ! |
|---|
| 2186 | go to 1001 |
|---|
| 2187 | ! .......... set error -- no convergence to a |
|---|
| 2188 | ! singular value after 30 iterations .......... |
|---|
| 2189 | 1000 ierr = k |
|---|
| 2190 | 1001 return |
|---|
| 2191 | end |
|---|
| 2192 | !DECK ID>, PYTHAG. |
|---|
| 2193 | DOUBLE PRECISION FUNCTION PYTHAG(A,B) |
|---|
| 2194 | implicit none |
|---|
| 2195 | double precision a,b |
|---|
| 2196 | ! |
|---|
| 2197 | ! finds dsqrt(a**2+b**2) without overflow or destructive underflow |
|---|
| 2198 | ! |
|---|
| 2199 | double precision p,r,s,t,u |
|---|
| 2200 | p = dmax1(dabs(a),dabs(b)) |
|---|
| 2201 | if (p .eq. 0.0d0) go to 20 |
|---|
| 2202 | r = (dmin1(dabs(a),dabs(b))/p)**2 |
|---|
| 2203 | 10 continue |
|---|
| 2204 | t = 4.0d0 + r |
|---|
| 2205 | if (t .eq. 4.0d0) go to 20 |
|---|
| 2206 | s = r/t |
|---|
| 2207 | u = 1.0d0 + 2.0d0*s |
|---|
| 2208 | p = u*p |
|---|
| 2209 | r = (s/u)**2 * r |
|---|
| 2210 | go to 10 |
|---|
| 2211 | 20 pythag = p |
|---|
| 2212 | return |
|---|
| 2213 | end |
|---|
| 2214 | subroutine rvord(inv,outv,ws,n) |
|---|
| 2215 | implicit none |
|---|
| 2216 | integer n |
|---|
| 2217 | double precision inv(n), ws(n) |
|---|
| 2218 | integer i,j,jmax |
|---|
| 2219 | integer outv(n) |
|---|
| 2220 | do i = 1,n |
|---|
| 2221 | ws(i) = inv(i) |
|---|
| 2222 | enddo |
|---|
| 2223 | |
|---|
| 2224 | do j = 1,n |
|---|
| 2225 | jmax = 1 |
|---|
| 2226 | do i = 1,n |
|---|
| 2227 | if(ws(i).gt.ws(jmax)) then |
|---|
| 2228 | jmax = i |
|---|
| 2229 | endif |
|---|
| 2230 | enddo |
|---|
| 2231 | outv(n-j+1) = jmax |
|---|
| 2232 | ws(jmax) = 0.0 |
|---|
| 2233 | enddo |
|---|
| 2234 | return |
|---|
| 2235 | end |
|---|
| 2236 | |
|---|
| 2237 | subroutine primat(a,nc,nm) |
|---|
| 2238 | |
|---|
| 2239 | implicit none |
|---|
| 2240 | integer i, j |
|---|
| 2241 | integer nm,nc |
|---|
| 2242 | integer a(nc, nm) |
|---|
| 2243 | |
|---|
| 2244 | do I = 1,nc |
|---|
| 2245 | write(*,*) (a(i,j),j=1,nm) |
|---|
| 2246 | enddo |
|---|
| 2247 | |
|---|
| 2248 | return |
|---|
| 2249 | end |
|---|
| 2250 | |
|---|
| 2251 | subroutine prdmat(a,nc,nm) |
|---|
| 2252 | |
|---|
| 2253 | implicit none |
|---|
| 2254 | integer i, j |
|---|
| 2255 | integer nm,nc |
|---|
| 2256 | double precision a(nc, nm) |
|---|
| 2257 | |
|---|
| 2258 | do I = 1,nc |
|---|
| 2259 | write(*,*) (a(i,j),j=1,nm) |
|---|
| 2260 | enddo |
|---|
| 2261 | |
|---|
| 2262 | return |
|---|
| 2263 | end |
|---|
![(please configure the [header_logo] section in trac.ini)](/PSPA/chrome/site/pspa.gif)
