src/mdlib/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 3.1
  11  * Copyright (c) 1991-2001, University of Groningen, The Netherlands
  12  * This program is free software; you can redistribute it and/or
  13  * modify it under the terms of the GNU General Public License
  14  * as published by the Free Software Foundation; either version 2
  15  * of the License, or (at your option) any later version.
  16  *
  17  * If you want to redistribute modifications, please consider that
  18  * scientific software is very special. Version control is crucial -
  19  * bugs must be traceable. We will be happy to consider code for
  20  * inclusion in the official distribution, but derived work must not
  21  * be called official GROMACS. Details are found in the README & COPYING
  22  * files - if they are missing, get the official version at www.gromacs.org.
  23  *
  24  * To help us fund GROMACS development, we humbly ask that you cite
  25  * the papers on the package - you can find them in the top README file.
  26  *
  27  * For more info, check our website at http://www.gromacs.org
  28  *
  29  * And Hey:
  30  * Getting the Right Output Means no Artefacts in Calculating Stuff
  31  */
  32 static char *SRCID_clincs_c = "$Id$";
  33 #include <math.h>
  34 #include "main.h"
  35 #include "constr.h"
  36 #include "physics.h"
  37 #include "vec.h"
  38
  39 void clincsp(rvec *x,rvec *f,rvec *fp,int ncons,
  40              int *bla1,int *bla2,int *blnr,int *blbnb,
  41              real *blc,real *blcc,real *blm,
  42              int nrec,real *invmass,rvec *r,
  43              real *rhs1,real *rhs2,real *sol)
  44 {
  45   int     b,i,j,k,n,it,rec;
  46   real    tmp0,tmp1,tmp2,im1,im2,mvb,rlen,len,wfac,lam;
  47   real    u0,u1,u2,v0,v1,v2;
  48   real    *tmp;
  49
  50   /* Compute normalized i-j vectors */
  51   for(b=0;b<ncons;b++) {
  52     i=bla1[b];
  53     j=bla2[b];
  54     tmp0=x[i][0]-x[j][0];
  55     tmp1=x[i][1]-x[j][1];
  56     tmp2=x[i][2]-x[j][2];
  57     rlen=invsqrt(tmp0*tmp0+tmp1*tmp1+tmp2*tmp2);
  58     r[b][0]=rlen*tmp0;
  59     r[b][1]=rlen*tmp1;
  60     r[b][2]=rlen*tmp2;
  61   } /* 16 ncons flops */
  62
  63   for(b=0;b<ncons;b++) {
  64     tmp0=r[b][0];
  65     tmp1=r[b][1];
  66     tmp2=r[b][2];
  67     i=bla1[b];
  68     j=bla2[b];
  69     for(n=blnr[b];n<blnr[b+1];n++) {
  70       k=blbnb[n];
  71       blm[n]=blcc[n]*(tmp0*r[k][0]+tmp1*r[k][1]+tmp2*r[k][2]);
  72     } /* 6 nr flops */
  73     mvb=blc[b]*(tmp0*(f[i][0]-f[j][0])+
  74                 tmp1*(f[i][1]-f[j][1])+
  75                 tmp2*(f[i][2]-f[j][2]));
  76     rhs1[b]=mvb;
  77     sol[b]=mvb;
  78     /* 7 flops */
  79   }
  80   /* Together: 23*ncons + 6*nrtot flops */
  81
  82
  83   for(rec=0;rec<nrec;rec++) {
  84     for(b=0;b<ncons;b++) {
  85       mvb=0;
  86       for(n=blnr[b];n<blnr[b+1];n++) {
  87         j=blbnb[n];
  88         mvb=mvb+blm[n]*rhs1[j];
  89       }
  90       rhs2[b]=mvb;
  91       sol[b]=sol[b]+mvb;
  92     }
  93     tmp=rhs1;
  94     rhs1=rhs2;
  95     rhs2=tmp;
  96   } /* nrec*(ncons+2*nrtot) flops */
  97
  98   for(b=0;b<ncons;b++) {
  99     i=bla1[b];
 100     j=bla2[b];
 101     mvb=blc[b]*sol[b];
 102     im1=invmass[i];
 103     im2=invmass[j];
 104     tmp0=r[b][0]*mvb;
 105     tmp1=r[b][1]*mvb;
 106     tmp2=r[b][2]*mvb;
 107     u0=fp[i][0]-tmp0*im1;
 108     u1=fp[i][1]-tmp1*im1;
 109     u2=fp[i][2]-tmp2*im1;
 110     v0=fp[j][0]+tmp0*im2;
 111     v1=fp[j][1]+tmp1*im2;
 112     v2=fp[j][2]+tmp2*im2;
 113     fp[i][0]=u0;
 114     fp[i][1]=u1;
 115     fp[i][2]=u2;
 116     fp[j][0]=v0;
 117     fp[j][1]=v1;
 118     fp[j][2]=v2;
 119   } /* 16 ncons flops */
 120 }
 121
 122 void clincs(rvec *x,rvec *xp,int ncons,
 123             int *bla1,int *bla2,int *blnr,int *blbnb,real *bllen,
 124             real *blc,real *blcc,real *blm,
 125             int nit,int nrec,real *invmass,rvec *r,
 126             real *rhs1,real *rhs2,real *sol,real wangle,int *warn,
 127             real *lambda)
 128 {
 129   int     b,i,j,k,n,it,rec;
 130   real    tmp0,tmp1,tmp2,im1,im2,mvb,rlen,len,wfac,lam;
 131   real    u0,u1,u2,v0,v1,v2;
 132   real    *tmp;
 133
 134   *warn=0;
 135
 136   /* Compute normalized i-j vectors */
 137   for(b=0;b<ncons;b++) {
 138     i=bla1[b];
 139     j=bla2[b];
 140     tmp0=x[i][0]-x[j][0];
 141     tmp1=x[i][1]-x[j][1];
 142     tmp2=x[i][2]-x[j][2];
 143     rlen=invsqrt(tmp0*tmp0+tmp1*tmp1+tmp2*tmp2);
 144     r[b][0]=rlen*tmp0;
 145     r[b][1]=rlen*tmp1;
 146     r[b][2]=rlen*tmp2;
 147   } /* 16 ncons flops */
 148
 149   for(b=0;b<ncons;b++) {
 150     tmp0=r[b][0];
 151     tmp1=r[b][1];
 152     tmp2=r[b][2];
 153     len=bllen[b];
 154     i=bla1[b];
 155     j=bla2[b];
 156     for(n=blnr[b];n<blnr[b+1];n++) {
 157       k=blbnb[n];
 158       blm[n]=blcc[n]*(tmp0*r[k][0]+tmp1*r[k][1]+tmp2*r[k][2]);
 159     } /* 6 nr flops */
 160     mvb=blc[b]*(tmp0*(xp[i][0]-xp[j][0])+
 161                 tmp1*(xp[i][1]-xp[j][1])+
 162                 tmp2*(xp[i][2]-xp[j][2])-len);
 163     rhs1[b]=mvb;
 164     sol[b]=mvb;
 165     /* 8 flops */
 166   }
 167   /* Together: 24*ncons + 6*nrtot flops */
 168
 169
 170   for(rec=0;rec<nrec;rec++) {
 171     for(b=0;b<ncons;b++) {
 172       mvb=0;
 173       for(n=blnr[b];n<blnr[b+1];n++) {
 174         j=blbnb[n];
 175         mvb=mvb+blm[n]*rhs1[j];
 176       }
 177       rhs2[b]=mvb;
 178       sol[b]=sol[b]+mvb;
 179     }
 180     tmp=rhs1;
 181     rhs1=rhs2;
 182     rhs2=tmp;
 183   } /* nrec*(ncons+2*nrtot) flops */
 184
 185   for(b=0;b<ncons;b++) {
 186     i=bla1[b];
 187     j=bla2[b];
 188     mvb=blc[b]*sol[b];
 189     lambda[b]=-mvb;
 190     im1=invmass[i];
 191     im2=invmass[j];
 192     tmp0=r[b][0]*mvb;
 193     tmp1=r[b][1]*mvb;
 194     tmp2=r[b][2]*mvb;
 195     u0=xp[i][0]-tmp0*im1;
 196     u1=xp[i][1]-tmp1*im1;
 197     u2=xp[i][2]-tmp2*im1;
 198     v0=xp[j][0]+tmp0*im2;
 199     v1=xp[j][1]+tmp1*im2;
 200     v2=xp[j][2]+tmp2*im2;
 201     xp[i][0]=u0;
 202     xp[i][1]=u1;
 203     xp[i][2]=u2;
 204     xp[j][0]=v0;
 205     xp[j][1]=v1;
 206     xp[j][2]=v2;
 207   } /* 16 ncons flops */
 208
 209
 210
 211   /*
 212   ********  Correction for centripetal effects  ********
 213   */
 214
 215   wfac=cos(DEG2RAD*wangle);
 216   wfac=wfac*wfac;
 217
 218   for(it=0; it<nit; it++) {
 219
 220     for(b=0;b<ncons;b++) {
 221       len=bllen[b];
 222       i=bla1[b];
 223       j=bla2[b];
 224       tmp0=xp[i][0]-xp[j][0];
 225       tmp1=xp[i][1]-xp[j][1];
 226       tmp2=xp[i][2]-xp[j][2];
 227       u1=len*len;
 228       u0=2.*u1-(tmp0*tmp0+tmp1*tmp1+tmp2*tmp2);
 229       if (u0 < wfac*u1) *warn=b;
 230       if (u0 < 0) u0=0;
 231       mvb=blc[b]*(len-sqrt(u0));
 232       rhs1[b]=mvb;
 233       sol[b]=mvb;
 234     } /* 18*ncons flops */
 235
 236     for(rec=0;rec<nrec;rec++) {
 237       for(b=0;b<ncons;b++) {
 238         mvb=0;
 239         for(n=blnr[b];n<blnr[b+1];n++) {
 240           j=blbnb[n];
 241           mvb=mvb+blm[n]*rhs1[j];
 242         }
 243         rhs2[b]=mvb;
 244         sol[b]=sol[b]+mvb;
 245       }
 246       tmp=rhs1;
 247       rhs1=rhs2;
 248       rhs2=tmp;
 249     } /* nrec*(ncons+2*nrtot) flops */
 250
 251     for(b=0;b<ncons;b++) {
 252       i=bla1[b];
 253       j=bla2[b];
 254       lam=lambda[b];
 255       mvb=blc[b]*sol[b];
 256       lambda[b]=lam-mvb;
 257       im1=invmass[i];
 258       im2=invmass[j];
 259       tmp0=r[b][0]*mvb;
 260       tmp1=r[b][1]*mvb;
 261       tmp2=r[b][2]*mvb;
 262       u0=xp[i][0]-tmp0*im1;
 263       u1=xp[i][1]-tmp1*im1;
 264       u2=xp[i][2]-tmp2*im1;
 265       v0=xp[j][0]+tmp0*im2;
 266       v1=xp[j][1]+tmp1*im2;
 267       v2=xp[j][2]+tmp2*im2;
 268       xp[i][0]=u0;
 269       xp[i][1]=u1;
 270       xp[i][2]=u2;
 271       xp[j][0]=v0;
 272       xp[j][1]=v1;
 273       xp[j][2]=v2;
 274     } /* 17 ncons flops */
 275   } /* nit*ncons*(35+9*nrec) flops */
 276   /* Total:
 277    * 24*ncons + 6*nrtot + nrec*(ncons+2*nrtot)
 278    * + nit * (18*ncons + nrec*(ncons+2*nrtot) + 17 ncons)
 279    *
 280    * (24+nrec)*ncons + (6+2*nrec)*nrtot
 281    * + nit * ((35+nrec)*ncons + 2*nrec*nrtot)
 282    * if nit=1
 283    * (59+nrec)*ncons + (6+4*nrec)*nrtot
 284    */
 285 }
 286
 287