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