src/gmxlib/clincs.c

   1 /*
   2  *       $Id$
   3  *
   4  *       This source code is part of
   5  *
   6  *        G   R   O   M   A   C   S
   7  *
   8  * GROningen MAchine for Chemical Simulations
   9  *
  10  *            VERSION 2.0
  11  *
  12  * Copyright (c) 1991-1997
  13  * BIOSON Research Institute, Dept. of Biophysical Chemistry
  14  * University of Groningen, The Netherlands
  15  *
  16  * Please refer to:
  17  * GROMACS: A message-passing parallel molecular dynamics implementation
  18  * H.J.C. Berendsen, D. van der Spoel and R. van Drunen
  19  * Comp. Phys. Comm. 91, 43-56 (1995)
  20  *
  21  * Also check out our WWW page:
  22  * http://rugmd0.chem.rug.nl/~gmx
  23  * or e-mail to:
  24  * gromacs@chem.rug.nl
  25  *
  26  * And Hey:
  27  * S  C  A  M  O  R  G
  28  */
  29 static char *SRCID_clincs_c = "$Id$";
  30
  31 #include <math.h>
  32 #include "main.h"
  33 #include "update.h"
  34 #include "physics.h"
  35 #include "vec.h"
  36
  37 void clincs(rvec *x,rvec *xp,int ncons,int ncm,int cmax,
  38             int *bla1,int *bla2,int *blnr,int *blbnb,real *bllen,
  39             real *blc,real *blcc,real *blm,
  40             int nrec,real *invmass,rvec * r,
  41             real *rhs1,real *rhs2,real *sol,real wangle,int *warn,
  42             real *lambda)
  43 {
  44   int     b,i,j,k,n,b4,rec,nr,n1,nc4;
  45   real    tmp0,tmp1,tmp2,im1,im2,mvb,rlen,len,wfac,lam;
  46   real    u0,u1,u2,v0,v1,v2;
  47   real    *tmp;
  48
  49   *warn=0;
  50   n1=ncons-ncm;
  51   nc4=(cmax-4)*n1;
  52
  53   /* Compute normalized i-j vectors */
  54   for(b=0;b<ncons;b++) {
  55     i=bla1[b];
  56     j=bla2[b];
  57     tmp0=x[i][0]-x[j][0];
  58     tmp1=x[i][1]-x[j][1];
  59     tmp2=x[i][2]-x[j][2];
  60     rlen=invsqrt(tmp0*tmp0+tmp1*tmp1+tmp2*tmp2);
  61     r[b][0]=rlen*tmp0;
  62     r[b][1]=rlen*tmp1;
  63     r[b][2]=rlen*tmp2;
  64   }
  65
  66   for(b=0;b<n1;b++) {
  67     b4=4*b;
  68     tmp0=r[b][0];
  69     tmp1=r[b][1];
  70     tmp2=r[b][2];
  71     len=bllen[b];
  72     i=bla1[b];
  73     j=bla2[b];
  74     nr=blnr[b];
  75     for(n=0;n<nr;n++) {
  76       k=blbnb[b4+n];
  77       blm[b4+n]=blcc[b4+n]*(tmp0*r[k][0]+tmp1*r[k][1]+tmp2*r[k][2]);
  78     }
  79     mvb=blc[b]*(tmp0*(xp[i][0]-xp[j][0])+
  80                 tmp1*(xp[i][1]-xp[j][1])+
  81                 tmp2*(xp[i][2]-xp[j][2])-len);
  82     rhs1[b]=mvb;
  83     sol[b]=mvb;
  84   }
  85
  86   for(b=n1;b<ncons;b++) {
  87     b4=cmax*b-nc4;
  88     tmp0=r[b][0];
  89     tmp1=r[b][1];
  90     tmp2=r[b][2];
  91     len=bllen[b];
  92     i=bla1[b];
  93     j=bla2[b];
  94     nr=blnr[b];
  95     for(n=0;n<nr;n++) {
  96       k=blbnb[b4+n];
  97       blm[b4+n]=blcc[b4+n]*(tmp0*r[k][0]+tmp1*r[k][1]+tmp2*r[k][2]);
  98     }
  99     mvb=blc[b]*(tmp0*(xp[i][0]-xp[j][0])+
 100                 tmp1*(xp[i][1]-xp[j][1])+
 101                 tmp2*(xp[i][2]-xp[j][2])-len);
 102     rhs1[b]=mvb;
 103     sol[b]=mvb;
 104   }
 105
 106
 107   for(rec=0;rec<nrec;rec++) {
 108     for(b=0;b<n1;b++) {
 109       b4=4*b;
 110       mvb=0;
 111       for(n=0;n<4;n++) {
 112         j=blbnb[b4+n];
 113         mvb=mvb+blm[b4+n]*rhs1[j];
 114       }
 115       rhs2[b]=mvb;
 116       sol[b]=sol[b]+mvb;
 117     }
 118     for(b=n1;b<ncons;b++) {
 119       b4=cmax*b-nc4;
 120       mvb=0;
 121       nr=blnr[b];
 122       for(n=0;n<nr;n++) {
 123         j=blbnb[b4+n];
 124         mvb=mvb+blm[b4+n]*rhs1[j];
 125       }
 126       rhs2[b]=mvb;
 127       sol[b]=sol[b]+mvb;
 128     }
 129     tmp=rhs1;
 130     rhs1=rhs2;
 131     rhs2=tmp;
 132   }
 133
 134   for(b=0;b<ncons;b++) {
 135     i=bla1[b];
 136     j=bla2[b];
 137     mvb=blc[b]*sol[b];
 138     lambda[b]=mvb;
 139     im1=invmass[i];
 140     im2=invmass[j];
 141     tmp0=r[b][0]*mvb;
 142     tmp1=r[b][1]*mvb;
 143     tmp2=r[b][2]*mvb;
 144     u0=xp[i][0]-tmp0*im1;
 145     u1=xp[i][1]-tmp1*im1;
 146     u2=xp[i][2]-tmp2*im1;
 147     v0=xp[j][0]+tmp0*im2;
 148     v1=xp[j][1]+tmp1*im2;
 149     v2=xp[j][2]+tmp2*im2;
 150     xp[i][0]=u0;
 151     xp[i][1]=u1;
 152     xp[i][2]=u2;
 153     xp[j][0]=v0;
 154     xp[j][1]=v1;
 155     xp[j][2]=v2;
 156   }
 157
 158
 159
 160   /*
 161   ********  Correction for centripetal effects  ********
 162   */
 163
 164   wfac=cos(DEG2RAD*wangle);
 165   wfac=wfac*wfac;
 166
 167   for(b=0;b<ncons;b++) {
 168     len=bllen[b];
 169     i=bla1[b];
 170     j=bla2[b];
 171     tmp0=xp[i][0]-xp[j][0];
 172     tmp1=xp[i][1]-xp[j][1];
 173     tmp2=xp[i][2]-xp[j][2];
 174     u1=len*len;
 175     u0=2.*u1-(tmp0*tmp0+tmp1*tmp1+tmp2*tmp2);
 176     if (u0 < wfac*u1) *warn=b;
 177     if (u0 < 0) u0=0;
 178     mvb=blc[b]*(len-sqrt(u0));
 179     rhs1[b]=mvb;
 180     sol[b]=mvb;
 181   }
 182
 183   for(rec=0;rec<nrec;rec++) {
 184     for(b=0;b<n1;b++) {
 185       b4=4*b;
 186       mvb=0;
 187       for(n=0;n<4;n++) {
 188         j=blbnb[b4+n];
 189         mvb=mvb+blm[b4+n]*rhs1[j];
 190       }
 191       rhs2[b]=mvb;
 192       sol[b]=sol[b]+mvb;
 193     }
 194     for(b=n1;b<ncons;b++) {
 195       b4=cmax*b-nc4;
 196       mvb=0;
 197       nr=blnr[b];
 198       for(n=0;n<nr;n++) {
 199         j=blbnb[b4+n];
 200         mvb=mvb+blm[b4+n]*rhs1[j];
 201       }
 202       rhs2[b]=mvb;
 203       sol[b]=sol[b]+mvb;
 204     }
 205     tmp=rhs1;
 206     rhs1=rhs2;
 207     rhs2=tmp;
 208   }
 209
 210   for(b=0;b<ncons;b++) {
 211     i=bla1[b];
 212     j=bla2[b];
 213     lam=lambda[b];
 214     mvb=blc[b]*sol[b];
 215     lambda[b]=lam+mvb;
 216     im1=invmass[i];
 217     im2=invmass[j];
 218     tmp0=r[b][0]*mvb;
 219     tmp1=r[b][1]*mvb;
 220     tmp2=r[b][2]*mvb;
 221     u0=xp[i][0]-tmp0*im1;
 222     u1=xp[i][1]-tmp1*im1;
 223     u2=xp[i][2]-tmp2*im1;
 224     v0=xp[j][0]+tmp0*im2;
 225     v1=xp[j][1]+tmp1*im2;
 226     v2=xp[j][2]+tmp2*im2;
 227     xp[i][0]=u0;
 228     xp[i][1]=u1;
 229     xp[i][2]=u2;
 230     xp[j][0]=v0;
 231     xp[j][1]=v1;
 232     xp[j][2]=v2;
 233   }
 234 }
 235
 236
 237