source/fragments/fitsq.f

   1       subroutine fitsq(rms,x,y,nn,t,b)
   2 c  x and y are the vectors of coordinates (dimensioned (3,n)) of the two
   3 c  structures to be superimposed.  nn is 3*n, where n is the number of
   4 c  points.   t and b are respectively the translation vector and the
   5 c  rotation matrix that transforms the second set of coordinates to the
   6 c  frame of the first set.
   7 c  eta =  machine-specific variable
   8
   9 c     dimension x(1),y(1),t(3)
  10       dimension x(3*nn),y(3*nn),t(3)
  11       dimension b(3,3),q(3,3),r(3,3),v(3),xav(3),yav(3),e(3),c(3,3)
  12       eta = z00100000
  13 c     small=25.0*rmdcon(3)
  14 c     small=25.0*eta
  15 c     small=25.0*10.e-10
  16 c the following is a very lenient value for 'small'
  17       small = 0.0001
  18       fn=nn
  19       do 10 i=1,3
  20       xav(i)=0.0
  21       yav(i)=0.0
  22       do 10 j=1,3
  23    10 b(j,i)=0.0
  24       nc=0
  25 c
  26       do 30 n=1,nn
  27       do 20 i=1,3
  28 c      print*,'x = ',x(nc+i),'  y = ',y(nc+i)
  29       xav(i)=xav(i)+x(nc+i)/fn
  30    20 yav(i)=yav(i)+y(nc+i)/fn
  31    30 nc=nc+3
  32 c
  33
  34 *     print*,'xav = ',(xav(j),j=1,3)
  35 *     print*,'yav = ',(yav(j),j=1,3)
  36
  37       nc=0
  38       rms=0.0
  39       do 50 n=1,nn
  40       do 40 i=1,3
  41       rms=rms+((x(nc+i)-xav(i))**2+(y(nc+i)-yav(i))**2)/fn
  42       do 40 j=1,3
  43       b(j,i)=b(j,i)+(x(nc+i)-xav(i))*(y(nc+j)-yav(j))/fn
  44    40 c(j,i)=b(j,i)
  45    50 nc=nc+3
  46       call sivade(b,q,r,d)
  47       sn3=sign(1.0,d)
  48       do 120 i=1,3
  49       do 120 j=1,3
  50   120 b(j,i)=-q(j,1)*r(i,1)-q(j,2)*r(i,2)-sn3*q(j,3)*r(i,3)
  51       call mvvad(b,xav,yav,t)
  52       do 130 i=1,3
  53       do 130 j=1,3
  54       rms=rms+2.0*c(j,i)*b(j,i)
  55   130 b(j,i)=-b(j,i)
  56       if (abs(rms).gt.small) go to 140
  57 *     write (6,301)
  58       return
  59   140 if (rms.gt.0.0) go to 150
  60 *     write (6,303) rms
  61       rms=0.0
  62 *     stop
  63 * 150 write (6,302) sqrt(rms)
  64   150 rms=sqrt(rms)
  65       return
  66   301 format (5x,'rms deviation negligible')
  67   302 format (5x,'rms deviation ',f14.6)
  68   303 format (//,5x,'negative ms deviation - ',f14.6)
  69       end
  70       subroutine sivade(x,q,r,dt)
  71 c  computes q,e and r such that q(t)xr = diag(e)
  72       dimension x(3,3),q(3,3),r(3,3),e(3)
  73       dimension h(3,3),p(3,3),u(3,3),d(3)
  74       eta = z00100000
  75       small=25.0*10.e-10
  76 c     small=25.0*eta
  77 c     small=2.0*rmdcon(3)
  78       xnrm=0.0
  79       do 20 i=1,3
  80       do 10 j=1,3
  81       xnrm=xnrm+x(j,i)*x(j,i)
  82       u(j,i)=0.0
  83       r(j,i)=0.0
  84    10 h(j,i)=0.0
  85       u(i,i)=1.0
  86    20 r(i,i)=1.0
  87       xnrm=sqrt(xnrm)
  88       do 110 n=1,2
  89       xmax=0.0
  90       do 30 j=n,3
  91    30 if (abs(x(j,n)).gt.xmax) xmax=abs(x(j,n))
  92       a=0.0
  93       do 40 j=n,3
  94       h(j,n)=x(j,n)/xmax
  95    40 a=a+h(j,n)*h(j,n)
  96       a=sqrt(a)
  97       den=a*(a+abs(h(n,n)))
  98       d(n)=1.0/den
  99       h(n,n)=h(n,n)+sign(a,h(n,n))
 100       do 70 i=n,3
 101       s=0.0
 102       do 50 j=n,3
 103    50 s=s+h(j,n)*x(j,i)
 104       s=d(n)*s
 105       do 60 j=n,3
 106    60 x(j,i)=x(j,i)-s*h(j,n)
 107    70 continue
 108       if (n.gt.1) go to 110
 109       xmax=amax1(abs(x(1,2)),abs(x(1,3)))
 110       h(2,3)=x(1,2)/xmax
 111       h(3,3)=x(1,3)/xmax
 112       a=sqrt(h(2,3)*h(2,3)+h(3,3)*h(3,3))
 113       den=a*(a+abs(h(2,3)))
 114       d(3)=1.0/den
 115       h(2,3)=h(2,3)+sign(a,h(2,3))
 116       do 100 i=1,3
 117       s=0.0
 118       do 80 j=2,3
 119    80 s=s+h(j,3)*x(i,j)
 120       s=d(3)*s
 121       do 90 j=2,3
 122    90 x(i,j)=x(i,j)-s*h(j,3)
 123   100 continue
 124   110 continue
 125       do 130 i=1,3
 126       do 120 j=1,3
 127   120 p(j,i)=-d(1)*h(j,1)*h(i,1)
 128   130 p(i,i)=1.0+p(i,i)
 129       do 140 i=2,3
 130       do 140 j=2,3
 131       u(j,i)=u(j,i)-d(2)*h(j,2)*h(i,2)
 132   140 r(j,i)=r(j,i)-d(3)*h(j,3)*h(i,3)
 133       call mmmul(p,u,q)
 134   150 np=1
 135       nq=1
 136       if (abs(x(2,3)).gt.small*(abs(x(2,2))+abs(x(3,3)))) go to 160
 137       x(2,3)=0.0
 138       nq=nq+1
 139   160 if (abs(x(1,2)).gt.small*(abs(x(1,1))+abs(x(2,2)))) go to 180
 140       x(1,2)=0.0
 141       if (x(2,3).ne.0.0) go to 170
 142       nq=nq+1
 143       go to 180
 144   170 np=np+1
 145   180 if (nq.eq.3) go to 310
 146       npq=4-np-nq
 147       if (np.gt.npq) go to 230
 148       n0=0
 149       do 220 n=np,npq
 150       nn=n+np-1
 151       if (abs(x(nn,nn)).gt.small*xnrm) go to 220
 152       x(nn,nn)=0.0
 153       if (x(nn,nn+1).eq.0.0) go to 220
 154       n0=n0+1
 155       go to (190,210,220),nn
 156   190 do 200 j=2,3
 157   200 call givns(x,q,1,j)
 158       go to 220
 159   210 call givns(x,q,2,3)
 160   220 continue
 161       if (n0.ne.0) go to 150
 162   230 nn=3-nq
 163       a=x(nn,nn)*x(nn,nn)
 164       if (nn.gt.1) a=a+x(nn-1,nn)*x(nn-1,nn)
 165       b=x(nn+1,nn+1)*x(nn+1,nn+1)+x(nn,nn+1)*x(nn,nn+1)
 166       c=x(nn,nn)*x(nn,nn+1)
 167       dd=0.5*(a-b)
 168       xn2=c*c
 169       rt=b-xn2/(dd+sign(sqrt(dd*dd+xn2),dd))
 170       y=x(np,np)*x(np,np)-rt
 171       z=x(np,np)*x(np,np+1)
 172       do 300 n=np,nn
 173       if (abs(y).lt.abs(z)) go to 240
 174       t=z/y
 175       c=1.0/sqrt(1.0+t*t)
 176       s=c*t
 177       go to 250
 178   240 t=y/z
 179       s=1.0/sqrt(1.0+t*t)
 180       c=s*t
 181   250 do 260 j=1,3
 182       v=x(j,n)
 183       w=x(j,n+1)
 184       x(j,n)=c*v+s*w
 185       x(j,n+1)=-s*v+c*w
 186       a=r(j,n)
 187       b=r(j,n+1)
 188       r(j,n)=c*a+s*b
 189   260 r(j,n+1)=-s*a+c*b
 190       y=x(n,n)
 191       z=x(n+1,n)
 192       if (abs(y).lt.abs(z)) go to 270
 193       t=z/y
 194       c=1.0/sqrt(1.0+t*t)
 195       s=c*t
 196       go to 280
 197   270 t=y/z
 198       s=1.0/sqrt(1.0+t*t)
 199       c=s*t
 200   280 do 290 j=1,3
 201       v=x(n,j)
 202       w=x(n+1,j)
 203       a=q(j,n)
 204       b=q(j,n+1)
 205       x(n,j)=c*v+s*w
 206       x(n+1,j)=-s*v+c*w
 207       q(j,n)=c*a+s*b
 208   290 q(j,n+1)=-s*a+c*b
 209       if (n.ge.nn) go to 300
 210       y=x(n,n+1)
 211       z=x(n,n+2)
 212   300 continue
 213       go to 150
 214   310 do 320 i=1,3
 215   320 e(i)=x(i,i)
 216   330 n0=0
 217       do 360 i=1,3
 218       if (e(i).ge.0.0) go to 350
 219       e(i)=-e(i)
 220       do 340 j=1,3
 221   340 q(j,i)=-q(j,i)
 222   350 if (i.eq.1) go to 360
 223       if (abs(e(i)).lt.abs(e(i-1))) go to 360
 224       call switch(i,1,q,r,e)
 225       n0=n0+1
 226   360 continue
 227       if (n0.ne.0) go to 330
 228       if (abs(e(3)).gt.small*xnrm) go to 370
 229       e(3)=0.0
 230       if (abs(e(2)).gt.small*xnrm) go to 370
 231       e(2)=0.0
 232   370 dt=det(q(1,1),q(1,2),q(1,3))*det(r(1,1),r(1,2),r(1,3))
 233 *     write (1,501) (e(i),i=1,3)
 234       return
 235   501 format (/,5x,'singular values - ',3e15.5)
 236       end
 237       subroutine givns(a,b,m,n)
 238       dimension a(3,3),b(3,3)
 239       if (abs(a(m,n)).lt.abs(a(n,n))) go to 10
 240       t=a(n,n)/a(m,n)
 241       s=1.0/sqrt(1.0+t*t)
 242       c=s*t
 243       go to 20
 244    10 t=a(m,n)/a(n,n)
 245       c=1.0/sqrt(1.0+t*t)
 246       s=c*t
 247    20 do 30 j=1,3
 248       v=a(m,j)
 249       w=a(n,j)
 250       x=b(j,m)
 251       y=b(j,n)
 252       a(m,j)=c*v-s*w
 253       a(n,j)=s*v+c*w
 254       b(j,m)=c*x-s*y
 255    30 b(j,n)=s*x+c*y
 256       return
 257       end
 258       subroutine switch(n,m,u,v,d)
 259       dimension u(3,3),v(3,3),d(3)
 260       do 10 i=1,3
 261       tem=u(i,n)
 262       u(i,n)=u(i,n-1)
 263       u(i,n-1)=tem
 264       if (m.eq.0) go to 10
 265       tem=v(i,n)
 266       v(i,n)=v(i,n-1)
 267       v(i,n-1)=tem
 268    10 continue
 269       tem=d(n)
 270       d(n)=d(n-1)
 271       d(n-1)=tem
 272       return
 273       end
 274 c     subroutine mvvad(a,b,c,d)
 275       subroutine mvvad(b,xav,yav,t)
 276       dimension b(3,3),xav(3),yav(3),t(3)
 277 c     dimension a(3,3),b(3),c(3),d(3)
 278 c     do 10 j=1,3
 279 c     d(j)=c(j)
 280 c     do 10 i=1,3
 281 c  10 d(j)=d(j)+a(j,i)*b(i)
 282       do 10 j=1,3
 283       t(j)=yav(j)
 284       do 10 i=1,3
 285    10 t(j)=t(j)+b(j,i)*xav(i)
 286       return
 287       end
 288       real function det (a,b,c)
 289       dimension a(3),b(3),c(3)
 290       det=a(1)*(b(2)*c(3)-b(3)*c(2))+a(2)*(b(3)*c(1)-b(1)*c(3))
 291      1  +a(3)*(b(1)*c(2)-b(2)*c(1))
 292       return
 293       end
 294       subroutine mmmul(a,b,c)
 295       dimension a(3,3),b(3,3),c(3,3)
 296       do 10 i=1,3
 297       do 10 j=1,3
 298       c(i,j)=0.0
 299       do 10 k=1,3
 300    10 c(i,j)=c(i,j)+a(i,k)*b(k,j)
 301       return
 302       end
 303       subroutine matvec(uvec,tmat,pvec,nback)
 304       real*4 tmat(3,3),uvec(3,nback), pvec(3,nback)
 305 c
 306       do 2 j=1,nback
 307          do 1 i=1,3
 308          uvec(i,j) = 0.0
 309          do 1 k=1,3
 310     1    uvec(i,j)=uvec(i,j)+tmat(i,k)*pvec(k,j)
 311     2 continue
 312       return
 313       end