src/mdlib/flincs.f

   1       subroutine forlincs(x,xp,ncons,
   2      $     bla1,bla2,blnr,blbnb,bllen,blc,blcc,blm,
   3      $     nit,nrec,invmass,r,rhs1,rhs2,sol,wangle,warn,
   4      $     lambda)
   5
   6       implicit none
   7
   8       real  x(*),xp(*),bllen(*),blc(*),blcc(*),blm(*),invmass(*)
   9       real  r(*),rhs1(*),rhs2(*),sol(*),wangle,lambda(*)
  10       integer*4 ncons,nit,nrec,bla1(*),bla2(*),blnr(*),blbnb(*)
  11       integer*4 warn
  12
  13       integer*4 b,i,j,k,n,b3,i3,j3,it,rec
  14       real  tmp0,tmp1,tmp2,im1,im2,mvb,rlen,len,wfac,lam
  15       real  u0,u1,u2,v0,v1,v2
  16
  17
  18       warn=0
  19
  20       do b=1,ncons
  21          b3=3*(b-1)
  22          i=3*bla1(b)
  23          j=3*bla2(b)
  24          tmp0=x(i+1)-x(j+1)
  25          tmp1=x(i+2)-x(j+2)
  26          tmp2=x(i+3)-x(j+3)
  27          rlen=1.0/sqrt(tmp0*tmp0+tmp1*tmp1+tmp2*tmp2)
  28          r(b3+1)=rlen*tmp0
  29          r(b3+2)=rlen*tmp1
  30          r(b3+3)=rlen*tmp2
  31       enddo
  32
  33       do b=1,ncons
  34          b3=3*(b-1)
  35          tmp0=r(b3+1)
  36          tmp1=r(b3+2)
  37          tmp2=r(b3+3)
  38          len=bllen(b)
  39          i=3*bla1(b)
  40          j=3*bla2(b)
  41          do n=blnr(b)+1,blnr(b+1)
  42             k=3*blbnb(n)
  43             blm(n)=blcc(n)*(tmp0*r(k+1)+tmp1*r(k+2)+tmp2*r(k+3))
  44          enddo
  45          mvb=blc(b)*(tmp0*(xp(i+1)-xp(j+1))+
  46      $               tmp1*(xp(i+2)-xp(j+2))+
  47      $               tmp2*(xp(i+3)-xp(j+3))-len)
  48          rhs1(b)=mvb
  49          sol(b)=mvb
  50       enddo
  51
  52
  53       do rec=1,nrec,2
  54          do b=1,ncons
  55             mvb=0.
  56             do n=blnr(b)+1,blnr(b+1)
  57                j=blbnb(n)+1
  58                mvb=mvb+blm(n)*rhs1(j)
  59             enddo
  60             rhs2(b)=mvb
  61             sol(b)=sol(b)+mvb
  62          enddo
  63          if (rec .lt. nrec) then
  64             do b=1,ncons
  65                mvb=0.
  66                do n=blnr(b)+1,blnr(b+1)
  67                   j=blbnb(n)+1
  68                   mvb=mvb+blm(n)*rhs2(j)
  69                enddo
  70                rhs1(b)=mvb
  71                sol(b)=sol(b)+mvb
  72             enddo
  73             endif
  74          enddo
  75
  76
  77       do b=1,ncons
  78          b3=3*(b-1)
  79          i=bla1(b)
  80          j=bla2(b)
  81          i3=3*i
  82          j3=3*j
  83          mvb=blc(b)*sol(b)
  84          lambda(b)=-mvb
  85          im1=invmass(i+1)
  86          im2=invmass(j+1)
  87          tmp0=r(b3+1)*mvb
  88          tmp1=r(b3+2)*mvb
  89          tmp2=r(b3+3)*mvb
  90          u0=xp(i3+1)-tmp0*im1
  91          u1=xp(i3+2)-tmp1*im1
  92          u2=xp(i3+3)-tmp2*im1
  93          v0=xp(j3+1)+tmp0*im2
  94          v1=xp(j3+2)+tmp1*im2
  95          v2=xp(j3+3)+tmp2*im2
  96          xp(i3+1)=u0
  97          xp(i3+2)=u1
  98          xp(i3+3)=u2
  99          xp(j3+1)=v0
 100          xp(j3+2)=v1
 101          xp(j3+3)=v2
 102       enddo
 103
 104
 105
 106 c     ********  Correction for centripetal effects  ********
 107
 108       wfac=cos(0.01745*wangle)
 109       wfac=wfac*wfac
 110
 111       do it=1,nit
 112
 113          do b=1,ncons
 114             len=bllen(b)
 115             i=3*bla1(b)
 116             j=3*bla2(b)
 117             tmp0=xp(i+1)-xp(j+1)
 118             tmp1=xp(i+2)-xp(j+2)
 119             tmp2=xp(i+3)-xp(j+3)
 120             u1=len*len
 121             u0=2.*u1-(tmp0*tmp0+tmp1*tmp1+tmp2*tmp2)
 122             if (u0 .lt. wfac*u1) warn=b
 123             if (u0 .lt. 0.) u0=0.
 124             mvb=blc(b)*(len-sqrt(u0))
 125             rhs1(b)=mvb
 126             sol(b)=mvb
 127          enddo
 128
 129
 130          do rec=1,nrec,2
 131             do b=1,ncons
 132                mvb=0.
 133                do n=blnr(b)+1,blnr(b+1)
 134                   j=blbnb(n)+1
 135                   mvb=mvb+blm(n)*rhs1(j)
 136                enddo
 137                rhs2(b)=mvb
 138                sol(b)=sol(b)+mvb
 139             enddo
 140             if (rec .lt. nrec) then
 141                do b=1,ncons
 142                   mvb=0.
 143                   do n=blnr(b)+1,blnr(b+1)
 144                      j=blbnb(n)+1
 145                      mvb=mvb+blm(n)*rhs2(j)
 146                   enddo
 147                   rhs1(b)=mvb
 148                   sol(b)=sol(b)+mvb
 149                enddo
 150             endif
 151          enddo
 152
 153
 154          do b=1,ncons
 155             b3=3*(b-1)
 156             i=bla1(b)
 157             j=bla2(b)
 158             i3=3*i
 159             j3=3*j
 160             lam=lambda(b)
 161             mvb=blc(b)*sol(b)
 162             lambda(b)=lam-mvb
 163             im1=invmass(i+1)
 164             im2=invmass(j+1)
 165             tmp0=r(b3+1)*mvb
 166             tmp1=r(b3+2)*mvb
 167             tmp2=r(b3+3)*mvb
 168             u0=xp(i3+1)-tmp0*im1
 169             u1=xp(i3+2)-tmp1*im1
 170             u2=xp(i3+3)-tmp2*im1
 171             v0=xp(j3+1)+tmp0*im2
 172             v1=xp(j3+2)+tmp1*im2
 173             v2=xp(j3+3)+tmp2*im2
 174             xp(i3+1)=u0
 175             xp(i3+2)=u1
 176             xp(i3+3)=u2
 177             xp(j3+1)=v0
 178             xp(j3+2)=v1
 179             xp(j3+3)=v2
 180          enddo
 181
 182       enddo
 183
 184       return
 185
 186       end