src/gromacs/mdlib/shakef.c

   1 /*
   2  * This file is part of the GROMACS molecular simulation package.
   3  *
   4  * Copyright (c) 1991-2000, University of Groningen, The Netherlands.
   5  * Copyright (c) 2001-2004, The GROMACS development team.
   6  * Copyright (c) 2013,2014, by the GROMACS development team, led by
   7  * Mark Abraham, David van der Spoel, Berk Hess, and Erik Lindahl,
   8  * and including many others, as listed in the AUTHORS file in the
   9  * top-level source directory and at http://www.gromacs.org.
  10  *
  11  * GROMACS is free software; you can redistribute it and/or
  12  * modify it under the terms of the GNU Lesser General Public License
  13  * as published by the Free Software Foundation; either version 2.1
  14  * of the License, or (at your option) any later version.
  15  *
  16  * GROMACS is distributed in the hope that it will be useful,
  17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  19  * Lesser General Public License for more details.
  20  *
  21  * You should have received a copy of the GNU Lesser General Public
  22  * License along with GROMACS; if not, see
  23  * http://www.gnu.org/licenses, or write to the Free Software Foundation,
  24  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA.
  25  *
  26  * If you want to redistribute modifications to GROMACS, please
  27  * consider that scientific software is very special. Version
  28  * control is crucial - bugs must be traceable. We will be happy to
  29  * consider code for inclusion in the official distribution, but
  30  * derived work must not be called official GROMACS. Details are found
  31  * in the README & COPYING files - if they are missing, get the
  32  * official version at http://www.gromacs.org.
  33  *
  34  * To help us fund GROMACS development, we humbly ask that you cite
  35  * the research papers on the package. Check out http://www.gromacs.org.
  36  */
  37 #include "gmxpre.h"
  38
  39 #include <math.h>
  40
  41 #include "gromacs/legacyheaders/constr.h"
  42 #include "gromacs/legacyheaders/nrnb.h"
  43 #include "gromacs/legacyheaders/txtdump.h"
  44 #include "gromacs/legacyheaders/typedefs.h"
  45 #include "gromacs/math/vec.h"
  46 #include "gromacs/utility/smalloc.h"
  47
  48 typedef struct gmx_shakedata
  49 {
  50     rvec *rij;
  51     real *half_of_reduced_mass;
  52     real *distance_squared_tolerance;
  53     real *constraint_distance_squared;
  54     int   nalloc;
  55     /* SOR stuff */
  56     real  delta;
  57     real  omega;
  58     real  gamma;
  59 } t_gmx_shakedata;
  60
  61 gmx_shakedata_t shake_init()
  62 {
  63     gmx_shakedata_t d;
  64
  65     snew(d, 1);
  66
  67     d->nalloc                      = 0;
  68     d->rij                         = NULL;
  69     d->half_of_reduced_mass        = NULL;
  70     d->distance_squared_tolerance  = NULL;
  71     d->constraint_distance_squared = NULL;
  72
  73     /* SOR initialization */
  74     d->delta = 0.1;
  75     d->omega = 1.0;
  76     d->gamma = 1000000;
  77
  78     return d;
  79 }
  80
  81 static void pv(FILE *log, char *s, rvec x)
  82 {
  83     int m;
  84
  85     fprintf(log, "%5s:", s);
  86     for (m = 0; (m < DIM); m++)
  87     {
  88         fprintf(log, "  %10.3f", x[m]);
  89     }
  90     fprintf(log, "\n");
  91     fflush(log);
  92 }
  93
  94 /*! \brief Inner kernel for SHAKE constraints
  95  *
  96  * Original implementation from R.C. van Schaik and W.F. van Gunsteren
  97  * (ETH Zuerich, June 1992), adapted for GROMACS by David van der
  98  * Spoel November 1992.
  99  *
 100  * The algorithm here is based section five of Ryckaert, Ciccotti and
 101  * Berendsen, J Comp Phys, 23, 327, 1977.
 102  *
 103  * \param[in]    iatom                         Mini-topology of triples of constraint type (unused in this
 104  *                                             function) and indices of the two atoms involved
 105  * \param[in]    ncon                          Number of constraints
 106  * \param[out]   nnit                          Number of iterations performed
 107  * \param[in]    maxnit                        Maximum number of iterations permitted
 108  * \param[in]    constraint_distance_squared   The objective value for each constraint
 109  * \param[inout] positions                     The initial (and final) values of the positions of all atoms
 110  * \param[in]    initial_displacements         The initial displacements of each constraint
 111  * \param[in]    half_of_reduced_mass          Half of the reduced mass for each constraint
 112  * \param[in]    omega                         SHAKE over-relaxation factor (set non-1.0 by
 113  *                                             using shake-sor=yes in the .mdp, but there is no documentation anywhere)
 114  * \param[in]    invmass                       Inverse mass of each atom
 115  * \param[in]    distance_squared_tolerance    Multiplicative tolerance on the difference in the
 116  *                                             square of the constrained distance (see code)
 117  * \param[out]   scaled_lagrange_multiplier    Scaled Lagrange multiplier for each constraint (-2 * eta from p. 336
 118  *                                             of the paper, divided by the constraint distance)
 119  * \param[out]   nerror                        Zero upon success, returns one more than the index of the
 120  *                                             problematic constraint if the input was malformed
 121  *
 122  * \todo Make SHAKE use better data structures, in particular for iatom. */
 123 void cshake(const atom_id iatom[], int ncon, int *nnit, int maxnit,
 124             const real constraint_distance_squared[], real positions[],
 125             const real initial_displacements[], const real half_of_reduced_mass[], real omega,
 126             const real invmass[], const real distance_squared_tolerance[],
 127             real scaled_lagrange_multiplier[], int *nerror)
 128 {
 129     /* default should be increased! MRS 8/4/2009 */
 130     const real mytol = 1e-10;
 131
 132     int        ll, i, j, i3, j3, l3;
 133     int        ix, iy, iz, jx, jy, jz;
 134     real       r_dot_r_prime;
 135     real       constraint_distance_squared_ll;
 136     real       r_prime_squared;
 137     real       scaled_lagrange_multiplier_ll;
 138     real       r_prime_x, r_prime_y, r_prime_z, diff, im, jm;
 139     real       xh, yh, zh, rijx, rijy, rijz;
 140     real       tix, tiy, tiz;
 141     real       tjx, tjy, tjz;
 142     int        nit, error, nconv;
 143     real       iconvf;
 144
 145     error = 0;
 146     nconv = 1;
 147     for (nit = 0; (nit < maxnit) && (nconv != 0) && (error == 0); nit++)
 148     {
 149         nconv = 0;
 150         for (ll = 0; (ll < ncon) && (error == 0); ll++)
 151         {
 152             l3    = 3*ll;
 153             rijx  = initial_displacements[l3+XX];
 154             rijy  = initial_displacements[l3+YY];
 155             rijz  = initial_displacements[l3+ZZ];
 156             i     = iatom[l3+1];
 157             j     = iatom[l3+2];
 158             i3    = 3*i;
 159             j3    = 3*j;
 160             ix    = i3+XX;
 161             iy    = i3+YY;
 162             iz    = i3+ZZ;
 163             jx    = j3+XX;
 164             jy    = j3+YY;
 165             jz    = j3+ZZ;
 166
 167             /* Compute r prime between atoms i and j, which is the
 168                displacement *before* this update stage */
 169             r_prime_x       = positions[ix]-positions[jx];
 170             r_prime_y       = positions[iy]-positions[jy];
 171             r_prime_z       = positions[iz]-positions[jz];
 172             r_prime_squared = (r_prime_x * r_prime_x +
 173                                r_prime_y * r_prime_y +
 174                                r_prime_z * r_prime_z);
 175             constraint_distance_squared_ll = constraint_distance_squared[ll];
 176             diff    = constraint_distance_squared_ll - r_prime_squared;
 177
 178             /* iconvf is less than 1 when the error is smaller than a bound */
 179             iconvf = fabs(diff) * distance_squared_tolerance[ll];
 180
 181             if (iconvf > 1.0)
 182             {
 183                 nconv         = iconvf;
 184                 r_dot_r_prime = (rijx * r_prime_x +
 185                                  rijy * r_prime_y +
 186                                  rijz * r_prime_z);
 187
 188                 if (r_dot_r_prime < constraint_distance_squared_ll * mytol)
 189                 {
 190                     error = ll+1;
 191                 }
 192                 else
 193                 {
 194                     /* The next line solves equation 5.6 (neglecting
 195                        the term in g^2), for g */
 196                     scaled_lagrange_multiplier_ll   = omega*diff*half_of_reduced_mass[ll]/r_dot_r_prime;
 197                     scaled_lagrange_multiplier[ll] += scaled_lagrange_multiplier_ll;
 198                     xh                              = rijx * scaled_lagrange_multiplier_ll;
 199                     yh                              = rijy * scaled_lagrange_multiplier_ll;
 200                     zh                              = rijz * scaled_lagrange_multiplier_ll;
 201                     im                              = invmass[i];
 202                     jm                              = invmass[j];
 203                     positions[ix]                  += xh*im;
 204                     positions[iy]                  += yh*im;
 205                     positions[iz]                  += zh*im;
 206                     positions[jx]                  -= xh*jm;
 207                     positions[jy]                  -= yh*jm;
 208                     positions[jz]                  -= zh*jm;
 209                 }
 210             }
 211         }
 212     }
 213     *nnit   = nit;
 214     *nerror = error;
 215 }
 216
 217 int vec_shakef(FILE *fplog, gmx_shakedata_t shaked,
 218                real invmass[], int ncon,
 219                t_iparams ip[], t_iatom *iatom,
 220                real tol, rvec x[], rvec prime[], real omega,
 221                gmx_bool bFEP, real lambda, real scaled_lagrange_multiplier[],
 222                real invdt, rvec *v,
 223                gmx_bool bCalcVir, tensor vir_r_m_dr, int econq,
 224                t_vetavars *vetavar)
 225 {
 226     rvec    *rij;
 227     real    *half_of_reduced_mass, *distance_squared_tolerance, *constraint_distance_squared;
 228     int      maxnit = 1000;
 229     int      nit    = 0, ll, i, j, d, d2, type;
 230     t_iatom *ia;
 231     real     L1;
 232     real     mm    = 0., tmp;
 233     int      error = 0;
 234     real     vscale, rscale, rvscale;
 235     real     constraint_distance;
 236
 237     if (ncon > shaked->nalloc)
 238     {
 239         shaked->nalloc = over_alloc_dd(ncon);
 240         srenew(shaked->rij, shaked->nalloc);
 241         srenew(shaked->half_of_reduced_mass, shaked->nalloc);
 242         srenew(shaked->distance_squared_tolerance, shaked->nalloc);
 243         srenew(shaked->constraint_distance_squared, shaked->nalloc);
 244     }
 245     rij                          = shaked->rij;
 246     half_of_reduced_mass         = shaked->half_of_reduced_mass;
 247     distance_squared_tolerance   = shaked->distance_squared_tolerance;
 248     constraint_distance_squared  = shaked->constraint_distance_squared;
 249
 250     L1   = 1.0-lambda;
 251     ia   = iatom;
 252     for (ll = 0; (ll < ncon); ll++, ia += 3)
 253     {
 254         type  = ia[0];
 255         i     = ia[1];
 256         j     = ia[2];
 257
 258         mm                       = 2.0*(invmass[i]+invmass[j]);
 259         rij[ll][XX]              = x[i][XX]-x[j][XX];
 260         rij[ll][YY]              = x[i][YY]-x[j][YY];
 261         rij[ll][ZZ]              = x[i][ZZ]-x[j][ZZ];
 262         half_of_reduced_mass[ll] = 1.0/mm;
 263         if (bFEP)
 264         {
 265             constraint_distance = L1*ip[type].constr.dA + lambda*ip[type].constr.dB;
 266         }
 267         else
 268         {
 269             constraint_distance = ip[type].constr.dA;
 270         }
 271         constraint_distance_squared[ll]  = sqr(constraint_distance);
 272         distance_squared_tolerance[ll]   = 0.5/(constraint_distance_squared[ll]*tol);
 273     }
 274
 275     switch (econq)
 276     {
 277         case econqCoord:
 278             cshake(iatom, ncon, &nit, maxnit, constraint_distance_squared, prime[0], rij[0], half_of_reduced_mass, omega, invmass, distance_squared_tolerance, scaled_lagrange_multiplier, &error);
 279             break;
 280         case econqVeloc:
 281             crattle(iatom, ncon, &nit, maxnit, constraint_distance_squared, prime[0], rij[0], half_of_reduced_mass, omega, invmass, distance_squared_tolerance, scaled_lagrange_multiplier, &error, invdt, vetavar);
 282             break;
 283     }
 284
 285     if (nit >= maxnit)
 286     {
 287         if (fplog)
 288         {
 289             fprintf(fplog, "Shake did not converge in %d steps\n", maxnit);
 290         }
 291         fprintf(stderr, "Shake did not converge in %d steps\n", maxnit);
 292         nit = 0;
 293     }
 294     else if (error != 0)
 295     {
 296         if (fplog)
 297         {
 298             fprintf(fplog, "Inner product between old and new vector <= 0.0!\n"
 299                     "constraint #%d atoms %d and %d\n",
 300                     error-1, iatom[3*(error-1)+1]+1, iatom[3*(error-1)+2]+1);
 301         }
 302         fprintf(stderr, "Inner product between old and new vector <= 0.0!\n"
 303                 "constraint #%d atoms %d and %d\n",
 304                 error-1, iatom[3*(error-1)+1]+1, iatom[3*(error-1)+2]+1);
 305         nit = 0;
 306     }
 307
 308     /* Constraint virial and correct the Lagrange multipliers for the length */
 309
 310     ia = iatom;
 311
 312     for (ll = 0; (ll < ncon); ll++, ia += 3)
 313     {
 314         type  = ia[0];
 315         i     = ia[1];
 316         j     = ia[2];
 317
 318         if ((econq == econqCoord) && v != NULL)
 319         {
 320             /* Correct the velocities */
 321             mm = scaled_lagrange_multiplier[ll]*invmass[i]*invdt/vetavar->rscale;
 322             for (d = 0; d < DIM; d++)
 323             {
 324                 v[ia[1]][d] += mm*rij[ll][d];
 325             }
 326             mm = scaled_lagrange_multiplier[ll]*invmass[j]*invdt/vetavar->rscale;
 327             for (d = 0; d < DIM; d++)
 328             {
 329                 v[ia[2]][d] -= mm*rij[ll][d];
 330             }
 331             /* 16 flops */
 332         }
 333
 334         /* constraint virial */
 335         if (bCalcVir)
 336         {
 337             if (econq == econqCoord)
 338             {
 339                 mm = scaled_lagrange_multiplier[ll]/vetavar->rvscale;
 340             }
 341             if (econq == econqVeloc)
 342             {
 343                 mm = scaled_lagrange_multiplier[ll]/(vetavar->vscale*vetavar->vscale_nhc[0]);
 344             }
 345             for (d = 0; d < DIM; d++)
 346             {
 347                 tmp = mm*rij[ll][d];
 348                 for (d2 = 0; d2 < DIM; d2++)
 349                 {
 350                     vir_r_m_dr[d][d2] -= tmp*rij[ll][d2];
 351                 }
 352             }
 353             /* 21 flops */
 354         }
 355
 356         /* cshake and crattle produce Lagrange multipliers scaled by
 357            the reciprocal of the constraint length, so fix that */
 358         if (bFEP)
 359         {
 360             constraint_distance = L1*ip[type].constr.dA + lambda*ip[type].constr.dB;
 361         }
 362         else
 363         {
 364             constraint_distance = ip[type].constr.dA;
 365         }
 366         scaled_lagrange_multiplier[ll] *= constraint_distance;
 367     }
 368
 369     return nit;
 370 }
 371
 372 static void check_cons(FILE *log, int nc, rvec x[], rvec prime[], rvec v[],
 373                        t_iparams ip[], t_iatom *iatom,
 374                        real invmass[], int econq)
 375 {
 376     t_iatom *ia;
 377     int      ai, aj;
 378     int      i;
 379     real     d, dp;
 380     rvec     dx, dv;
 381
 382     fprintf(log,
 383             "    i     mi      j     mj      before       after   should be\n");
 384     ia = iatom;
 385     for (i = 0; (i < nc); i++, ia += 3)
 386     {
 387         ai = ia[1];
 388         aj = ia[2];
 389         rvec_sub(x[ai], x[aj], dx);
 390         d = norm(dx);
 391
 392         switch (econq)
 393         {
 394             case econqCoord:
 395                 rvec_sub(prime[ai], prime[aj], dx);
 396                 dp = norm(dx);
 397                 fprintf(log, "%5d  %5.2f  %5d  %5.2f  %10.5f  %10.5f  %10.5f\n",
 398                         ai+1, 1.0/invmass[ai],
 399                         aj+1, 1.0/invmass[aj], d, dp, ip[ia[0]].constr.dA);
 400                 break;
 401             case econqVeloc:
 402                 rvec_sub(v[ai], v[aj], dv);
 403                 d = iprod(dx, dv);
 404                 rvec_sub(prime[ai], prime[aj], dv);
 405                 dp = iprod(dx, dv);
 406                 fprintf(log, "%5d  %5.2f  %5d  %5.2f  %10.5f  %10.5f  %10.5f\n",
 407                         ai+1, 1.0/invmass[ai],
 408                         aj+1, 1.0/invmass[aj], d, dp, 0.);
 409                 break;
 410         }
 411     }
 412 }
 413
 414 gmx_bool bshakef(FILE *log, gmx_shakedata_t shaked,
 415                  real invmass[], int nblocks, int sblock[],
 416                  t_idef *idef, t_inputrec *ir, rvec x_s[], rvec prime[],
 417                  t_nrnb *nrnb, real *scaled_lagrange_multiplier, real lambda, real *dvdlambda,
 418                  real invdt, rvec *v, gmx_bool bCalcVir, tensor vir_r_m_dr,
 419                  gmx_bool bDumpOnError, int econq, t_vetavars *vetavar)
 420 {
 421     t_iatom *iatoms;
 422     real     dt_2, dvdl;
 423     int      i, n0, ncon, blen, type, ll;
 424     int      tnit = 0, trij = 0;
 425
 426 #ifdef DEBUG
 427     fprintf(log, "nblocks=%d, sblock[0]=%d\n", nblocks, sblock[0]);
 428 #endif
 429
 430     ncon = idef->il[F_CONSTR].nr/3;
 431
 432     for (ll = 0; ll < ncon; ll++)
 433     {
 434         scaled_lagrange_multiplier[ll] = 0;
 435     }
 436
 437     iatoms = &(idef->il[F_CONSTR].iatoms[sblock[0]]);
 438     for (i = 0; (i < nblocks); )
 439     {
 440         blen  = (sblock[i+1]-sblock[i]);
 441         blen /= 3;
 442         n0    = vec_shakef(log, shaked, invmass, blen, idef->iparams,
 443                            iatoms, ir->shake_tol, x_s, prime, shaked->omega,
 444                            ir->efep != efepNO, lambda, scaled_lagrange_multiplier, invdt, v, bCalcVir, vir_r_m_dr,
 445                            econq, vetavar);
 446
 447 #ifdef DEBUGSHAKE
 448         check_cons(log, blen, x_s, prime, v, idef->iparams, iatoms, invmass, econq);
 449 #endif
 450
 451         if (n0 == 0)
 452         {
 453             if (bDumpOnError && log)
 454             {
 455                 {
 456                     check_cons(log, blen, x_s, prime, v, idef->iparams, iatoms, invmass, econq);
 457                 }
 458             }
 459             return FALSE;
 460         }
 461         tnit                       += n0*blen;
 462         trij                       += blen;
 463         iatoms                     += 3*blen; /* Increment pointer! */
 464         scaled_lagrange_multiplier += blen;
 465         i++;
 466     }
 467     /* only for position part? */
 468     if (econq == econqCoord)
 469     {
 470         if (ir->efep != efepNO)
 471         {
 472             real bondA, bondB;
 473             /* TODO This should probably use invdt, so that sd integrator scaling works properly */
 474             dt_2 = 1/sqr(ir->delta_t);
 475             dvdl = 0;
 476             for (ll = 0; ll < ncon; ll++)
 477             {
 478                 type  = idef->il[F_CONSTR].iatoms[3*ll];
 479
 480                 /* Per equations in the manual, dv/dl = -2 \sum_ll lagrangian_ll * r_ll * (d_B - d_A) */
 481                 /* The vector scaled_lagrange_multiplier[ll] contains the value -2 r_ll eta_ll (eta_ll is the
 482                    estimate of the Langrangian, definition on page 336 of Ryckaert et al 1977),
 483                    so the pre-factors are already present. */
 484                 bondA = idef->iparams[type].constr.dA;
 485                 bondB = idef->iparams[type].constr.dB;
 486                 dvdl += scaled_lagrange_multiplier[ll] * dt_2 * (bondB - bondA);
 487             }
 488             *dvdlambda += dvdl;
 489         }
 490     }
 491 #ifdef DEBUG
 492     fprintf(log, "tnit: %5d  omega: %10.5f\n", tnit, omega);
 493 #endif
 494     if (ir->bShakeSOR)
 495     {
 496         if (tnit > shaked->gamma)
 497         {
 498             shaked->delta *= -0.5;
 499         }
 500         shaked->omega += shaked->delta;
 501         shaked->gamma  = tnit;
 502     }
 503     inc_nrnb(nrnb, eNR_SHAKE, tnit);
 504     inc_nrnb(nrnb, eNR_SHAKE_RIJ, trij);
 505     if (v)
 506     {
 507         inc_nrnb(nrnb, eNR_CONSTR_V, trij*2);
 508     }
 509     if (bCalcVir)
 510     {
 511         inc_nrnb(nrnb, eNR_CONSTR_VIR, trij);
 512     }
 513
 514     return TRUE;
 515 }
 516
 517 void crattle(atom_id iatom[], int ncon, int *nnit, int maxnit,
 518              real constraint_distance_squared[], real vp[], real rij[], real m2[], real omega,
 519              real invmass[], real distance_squared_tolerance[], real scaled_lagrange_multiplier[],
 520              int *nerror, real invdt, t_vetavars *vetavar)
 521 {
 522     /*
 523      *     r.c. van schaik and w.f. van gunsteren
 524      *     eth zuerich
 525      *     june 1992
 526      *     Adapted for use with Gromacs by David van der Spoel november 92 and later.
 527      *     rattle added by M.R. Shirts, April 2004, from code written by Jay Ponder in TINKER
 528      *     second part of rattle algorithm
 529      */
 530
 531     const   real mytol = 1e-10;
 532
 533     int          ll, i, j, i3, j3, l3, ii;
 534     int          ix, iy, iz, jx, jy, jz;
 535     real         constraint_distance_squared_ll;
 536     real         rijd, vpijd, vx, vy, vz, diff, acor, xdotd, fac, im, jm, imdt, jmdt;
 537     real         xh, yh, zh, rijx, rijy, rijz;
 538     real         tix, tiy, tiz;
 539     real         tjx, tjy, tjz;
 540     int          nit, error, nconv;
 541     real         veta, vscale_nhc, iconvf;
 542
 543     veta       = vetavar->veta;
 544     vscale_nhc = vetavar->vscale_nhc[0];  /* for now, just use the first state */
 545
 546     error = 0;
 547     nconv = 1;
 548     for (nit = 0; (nit < maxnit) && (nconv != 0) && (error == 0); nit++)
 549     {
 550         nconv = 0;
 551         for (ll = 0; (ll < ncon) && (error == 0); ll++)
 552         {
 553             l3      = 3*ll;
 554             rijx    = rij[l3+XX];
 555             rijy    = rij[l3+YY];
 556             rijz    = rij[l3+ZZ];
 557             i       = iatom[l3+1];
 558             j       = iatom[l3+2];
 559             i3      = 3*i;
 560             j3      = 3*j;
 561             ix      = i3+XX;
 562             iy      = i3+YY;
 563             iz      = i3+ZZ;
 564             jx      = j3+XX;
 565             jy      = j3+YY;
 566             jz      = j3+ZZ;
 567             vx      = vp[ix]-vp[jx];
 568             vy      = vp[iy]-vp[jy];
 569             vz      = vp[iz]-vp[jz];
 570
 571             vpijd   = vx*rijx+vy*rijy+vz*rijz;
 572             constraint_distance_squared_ll = constraint_distance_squared[ll];
 573             /* this is r(t+dt) \dotproduct \dot{r}(t+dt) */
 574             xdotd   = vpijd*vscale_nhc + veta*constraint_distance_squared_ll;
 575
 576             /* iconv is zero when the error is smaller than a bound */
 577             iconvf   = fabs(xdotd)*(distance_squared_tolerance[ll]/invdt);
 578
 579             if (iconvf > 1)
 580             {
 581                 nconv     = iconvf;
 582                 fac       = omega*2.0*m2[ll]/constraint_distance_squared_ll;
 583                 acor      = -fac*xdotd;
 584                 scaled_lagrange_multiplier[ll] += acor;
 585
 586                 xh        = rijx*acor;
 587                 yh        = rijy*acor;
 588                 zh        = rijz*acor;
 589
 590                 im        = invmass[i]/vscale_nhc;
 591                 jm        = invmass[j]/vscale_nhc;
 592
 593                 vp[ix] += xh*im;
 594                 vp[iy] += yh*im;
 595                 vp[iz] += zh*im;
 596                 vp[jx] -= xh*jm;
 597                 vp[jy] -= yh*jm;
 598                 vp[jz] -= zh*jm;
 599             }
 600         }
 601     }
 602     *nnit   = nit;
 603     *nerror = error;
 604 }