/* Apply constraint using SHAKE */
static void do_constraint(t_pull *pull, t_pbc *pbc,
rvec *x, rvec *v,
gmx_bool bMaster, tensor vir,
double dt, double t)
{
dvec *r_ij; /* x[i] com of i in prev. step. Obeys constr. -> r_ij[i] */
dvec unc_ij; /* xp[i] com of i this step, before constr. -> unc_ij */
dvec *rnew; /* current 'new' positions of the groups */
double *dr_tot; /* the total update of the coords */
double ref;
dvec vec;
double d0, inpr;
double lambda, rm, mass, invdt = 0;
gmx_bool bConverged_all, bConverged = FALSE;
int niter = 0, g, c, ii, j, m, max_iter = 100;
double a;
dvec f; /* the pull force */
dvec tmp, tmp3;
t_pull_group *pdyna, *pgrp0, *pgrp1;
t_pull_coord *pcrd;
snew(r_ij, pull->ncoord);
snew(dr_tot, pull->ncoord);
snew(rnew, pull->ngroup);
/* copy the current unconstrained positions for use in iterations. We
iterate until rinew[i] and rjnew[j] obey the constraints. Then
rinew - pull.x_unc[i] is the correction dr to group i */
for (g = 0; g < pull->ngroup; g++)
{
copy_dvec(pull->group[g].xp, rnew[g]);
}
if (PULL_CYL(pull))
{
/* There is only one pull coordinate and reference group */
copy_dvec(pull->dyna[0].xp, rnew[pull->coord[0].group[0]]);
}
/* Determine the constraint directions from the old positions */
for (c = 0; c < pull->ncoord; c++)
{
get_pull_coord_dr(pull, c, pbc, t, r_ij[c]);
/* Store the difference vector at time t for printing */
copy_dvec(r_ij[c], pull->coord[c].dr);
if (debug)
{
fprintf(debug, "Pull coord %d dr %f %f %f\n",
c, r_ij[c][XX], r_ij[c][YY], r_ij[c][ZZ]);
}
if (pull->eGeom == epullgDIR || pull->eGeom == epullgDIRPBC)
{
/* Select the component along vec */
a = 0;
for (m = 0; m < DIM; m++)
{
a += pull->coord[c].vec[m]*r_ij[c][m];
}
for (m = 0; m < DIM; m++)
{
r_ij[c][m] = a*pull->coord[c].vec[m];
}
}
}
bConverged_all = FALSE;
while (!bConverged_all && niter < max_iter)
{
bConverged_all = TRUE;
/* loop over all constraints */
for (c = 0; c < pull->ncoord; c++)
{
dvec dr0, dr1;
pcrd = &pull->coord[c];
pgrp0 = &pull->group[pcrd->group[0]];
pgrp1 = &pull->group[pcrd->group[1]];
/* Get the current difference vector */
low_get_pull_coord_dr(pull, pcrd, pbc, t,
rnew[pcrd->group[1]],
rnew[pcrd->group[0]],
-1, unc_ij);
ref = pcrd->init + pcrd->rate*t;
if (debug)
{
fprintf(debug, "Pull coord %d, iteration %d\n", c, niter);
}
rm = 1.0/(pgrp0->invtm + pgrp1->invtm);
switch (pull->eGeom)
{
case epullgDIST:
if (ref <= 0)
{
gmx_fatal(FARGS, "The pull constraint reference distance for group %d is <= 0 (%f)", c, ref);
}
{
double q, c_a, c_b, c_c;
c_a = diprod(r_ij[c], r_ij[c]);
c_b = diprod(unc_ij, r_ij[c])*2;
c_c = diprod(unc_ij, unc_ij) - dsqr(ref);
if (c_b < 0)
{
q = -0.5*(c_b - sqrt(c_b*c_b - 4*c_a*c_c));
lambda = -q/c_a;
}
else
{
q = -0.5*(c_b + sqrt(c_b*c_b - 4*c_a*c_c));
lambda = -c_c/q;
}
if (debug)
{
fprintf(debug,
"Pull ax^2+bx+c=0: a=%e b=%e c=%e lambda=%e\n",
c_a, c_b, c_c, lambda);
}
}
/* The position corrections dr due to the constraints */
dsvmul(-lambda*rm*pgrp1->invtm, r_ij[c], dr1);
dsvmul( lambda*rm*pgrp0->invtm, r_ij[c], dr0);
dr_tot[c] += -lambda*dnorm(r_ij[c]);
break;
case epullgDIR:
case epullgDIRPBC:
case epullgCYL:
/* A 1-dimensional constraint along a vector */
a = 0;
for (m = 0; m < DIM; m++)
{
vec[m] = pcrd->vec[m];
a += unc_ij[m]*vec[m];
}
/* Select only the component along the vector */
dsvmul(a, vec, unc_ij);
lambda = a - ref;
if (debug)
{
fprintf(debug, "Pull inpr %e lambda: %e\n", a, lambda);
}
/* The position corrections dr due to the constraints */
dsvmul(-lambda*rm*pgrp1->invtm, vec, dr1);
dsvmul( lambda*rm*pgrp0->invtm, vec, dr0);
dr_tot[c] += -lambda;
break;
}
/* DEBUG */
if (debug)
{
int g0, g1;
g0 = pcrd->group[0];
g1 = pcrd->group[1];
low_get_pull_coord_dr(pull, pcrd, pbc, t, rnew[g1], rnew[g0], -1, tmp);
low_get_pull_coord_dr(pull, pcrd, pbc, t, dr1, dr0, -1, tmp3);
fprintf(debug,
"Pull cur %8.5f %8.5f %8.5f j:%8.5f %8.5f %8.5f d: %8.5f\n",
rnew[g0][0], rnew[g0][1], rnew[g0][2],
rnew[g1][0], rnew[g1][1], rnew[g1][2], dnorm(tmp));
fprintf(debug,
"Pull ref %8s %8s %8s %8s %8s %8s d: %8.5f\n",
"", "", "", "", "", "", ref);
fprintf(debug,
"Pull cor %8.5f %8.5f %8.5f j:%8.5f %8.5f %8.5f d: %8.5f\n",
dr0[0], dr0[1], dr0[2],
dr1[0], dr1[1], dr1[2],
dnorm(tmp3));
} /* END DEBUG */
/* Update the COMs with dr */
dvec_inc(rnew[pcrd->group[1]], dr1);
dvec_inc(rnew[pcrd->group[0]], dr0);
}
/* Check if all constraints are fullfilled now */
for (c = 0; c < pull->ncoord; c++)
{
pcrd = &pull->coord[c];
low_get_pull_coord_dr(pull, pcrd, pbc, t,
rnew[pcrd->group[1]],
rnew[pcrd->group[0]],
-1, unc_ij);
switch (pull->eGeom)
{
case epullgDIST:
bConverged = fabs(dnorm(unc_ij) - ref) < pull->constr_tol;
break;
case epullgDIR:
case epullgDIRPBC:
case epullgCYL:
for (m = 0; m < DIM; m++)
{
vec[m] = pcrd->vec[m];
}
inpr = diprod(unc_ij, vec);
dsvmul(inpr, vec, unc_ij);
bConverged =
fabs(diprod(unc_ij, vec) - ref) < pull->constr_tol;
break;
}
if (!bConverged)
{
if (debug)
{
fprintf(debug, "NOT CONVERGED YET: Group %d:"
"d_ref = %f, current d = %f\n",
g, ref, dnorm(unc_ij));
}
bConverged_all = FALSE;
}
}
niter++;
/* if after all constraints are dealt with and bConverged is still TRUE
we're finished, if not we do another iteration */
}
if (niter > max_iter)
{
gmx_fatal(FARGS, "Too many iterations for constraint run: %d", niter);
}
/* DONE ITERATING, NOW UPDATE COORDINATES AND CALC. CONSTRAINT FORCES */
if (v)
{
invdt = 1/dt;
}
/* update atoms in the groups */
for (g = 0; g < pull->ngroup; g++)
{
const t_pull_group *pgrp;
dvec dr;
if (PULL_CYL(pull) && g == pull->coord[0].group[0])
{
pgrp = &pull->dyna[0];
}
else
{
pgrp = &pull->group[g];
}
/* get the final constraint displacement dr for group g */
dvec_sub(rnew[g], pgrp->xp, dr);
/* select components of dr */
for (m = 0; m < DIM; m++)
{
dr[m] *= pull->dim[m];
}
/* update the atom positions */
copy_dvec(dr, tmp);
for (j = 0; j < pgrp->nat_loc; j++)
{
ii = pgrp->ind_loc[j];
if (pgrp->weight_loc)
{
dsvmul(pgrp->wscale*pgrp->weight_loc[j], dr, tmp);
}
for (m = 0; m < DIM; m++)
{
x[ii][m] += tmp[m];
}
if (v)
{
for (m = 0; m < DIM; m++)
{
v[ii][m] += invdt*tmp[m];
}
}
}
}
/* calculate the constraint forces, used for output and virial only */
for (c = 0; c < pull->ncoord; c++)
{
pcrd = &pull->coord[c];
pcrd->f_scal = dr_tot[c]/((pull->group[pcrd->group[0]].invtm + pull->group[pcrd->group[1]].invtm)*dt*dt);
if (vir && bMaster)
{
double f_invr;
/* Add the pull contribution to the virial */
f_invr = pcrd->f_scal/dnorm(r_ij[c]);
for (j = 0; j < DIM; j++)
{
for (m = 0; m < DIM; m++)
{
vir[j][m] -= 0.5*f_invr*r_ij[c][j]*r_ij[c][m];
}
}
}
}
/* finished! I hope. Give back some memory */
sfree(r_ij);
sfree(dr_tot);
sfree(rnew);
}
/* Apply constraint using SHAKE */
static void do_constraint(t_pull *pull, t_mdatoms *md, t_pbc *pbc,
rvec *x, rvec *v,
gmx_bool bMaster, tensor vir,
double dt, double t)
{
dvec *r_ij; /* x[i] com of i in prev. step. Obeys constr. -> r_ij[i] */
dvec unc_ij; /* xp[i] com of i this step, before constr. -> unc_ij */
dvec *rinew; /* current 'new' position of group i */
dvec *rjnew; /* current 'new' position of group j */
dvec ref,vec;
double d0,inpr;
double lambda, rm, mass, invdt=0;
gmx_bool bConverged_all,bConverged=FALSE;
int niter=0,g,ii,j,m,max_iter=100;
double q,a,b,c; /* for solving the quadratic equation,
see Num. Recipes in C ed 2 p. 184 */
dvec *dr; /* correction for group i */
dvec ref_dr; /* correction for group j */
dvec f; /* the pull force */
dvec tmp,tmp3;
t_pullgrp *pdyna,*pgrp,*pref;
snew(r_ij,pull->ngrp+1);
if (PULL_CYL(pull))
{
snew(rjnew,pull->ngrp+1);
}
else
{
snew(rjnew,1);
}
snew(dr,pull->ngrp+1);
snew(rinew,pull->ngrp+1);
/* copy the current unconstrained positions for use in iterations. We
iterate until rinew[i] and rjnew[j] obey the constraints. Then
rinew - pull.x_unc[i] is the correction dr to group i */
for(g=1; g<1+pull->ngrp; g++)
{
copy_dvec(pull->grp[g].xp,rinew[g]);
}
if (PULL_CYL(pull))
{
for(g=1; g<1+pull->ngrp; g++)
{
copy_dvec(pull->dyna[g].xp,rjnew[g]);
}
}
else
{
copy_dvec(pull->grp[0].xp,rjnew[0]);
}
/* Determine the constraint directions from the old positions */
for(g=1; g<1+pull->ngrp; g++)
{
get_pullgrp_dr(pull,pbc,g,t,r_ij[g]);
/* Store the difference vector at time t for printing */
copy_dvec(r_ij[g],pull->grp[g].dr);
if (debug)
{
fprintf(debug,"Pull group %d dr %f %f %f\n",
g,r_ij[g][XX],r_ij[g][YY],r_ij[g][ZZ]);
}
if (pull->eGeom == epullgDIR || pull->eGeom == epullgDIRPBC)
{
/* Select the component along vec */
a = 0;
for(m=0; m<DIM; m++)
{
a += pull->grp[g].vec[m]*r_ij[g][m];
}
for(m=0; m<DIM; m++)
{
r_ij[g][m] = a*pull->grp[g].vec[m];
}
}
}
bConverged_all = FALSE;
while (!bConverged_all && niter < max_iter)
{
bConverged_all = TRUE;
/* loop over all constraints */
for(g=1; g<1+pull->ngrp; g++)
{
pgrp = &pull->grp[g];
if (PULL_CYL(pull))
pref = &pull->dyna[g];
else
pref = &pull->grp[0];
/* Get the current difference vector */
get_pullgrps_dr(pull,pbc,g,t,rinew[g],rjnew[PULL_CYL(pull) ? g : 0],
-1,unc_ij);
if (pull->eGeom == epullgPOS)
{
for(m=0; m<DIM; m++)
{
ref[m] = pgrp->init[m] + pgrp->rate*t*pgrp->vec[m];
}
}
else
{
ref[0] = pgrp->init[0] + pgrp->rate*t;
/* Keep the compiler happy */
ref[1] = 0;
ref[2] = 0;
}
if (debug)
{
fprintf(debug,"Pull group %d, iteration %d\n",g,niter);
}
rm = 1.0/(pull->grp[g].invtm + pref->invtm);
switch (pull->eGeom)
{
case epullgDIST:
if (ref[0] <= 0)
{
gmx_fatal(FARGS,"The pull constraint reference distance for group %d is <= 0 (%f)",g,ref[0]);
}
a = diprod(r_ij[g],r_ij[g]);
b = diprod(unc_ij,r_ij[g])*2;
c = diprod(unc_ij,unc_ij) - dsqr(ref[0]);
if (b < 0)
{
q = -0.5*(b - sqrt(b*b - 4*a*c));
lambda = -q/a;
}
else
{
q = -0.5*(b + sqrt(b*b - 4*a*c));
lambda = -c/q;
}
if (debug)
{
fprintf(debug,
"Pull ax^2+bx+c=0: a=%e b=%e c=%e lambda=%e\n",
a,b,c,lambda);
}
/* The position corrections dr due to the constraints */
dsvmul(-lambda*rm*pgrp->invtm, r_ij[g], dr[g]);
dsvmul( lambda*rm*pref->invtm, r_ij[g], ref_dr);
break;
case epullgDIR:
case epullgDIRPBC:
case epullgCYL:
/* A 1-dimensional constraint along a vector */
a = 0;
for(m=0; m<DIM; m++)
{
vec[m] = pgrp->vec[m];
a += unc_ij[m]*vec[m];
}
/* Select only the component along the vector */
dsvmul(a,vec,unc_ij);
lambda = a - ref[0];
if (debug)
{
fprintf(debug,"Pull inpr %e lambda: %e\n",a,lambda);
}
/* The position corrections dr due to the constraints */
dsvmul(-lambda*rm*pull->grp[g].invtm, vec, dr[g]);
dsvmul( lambda*rm* pref->invtm, vec,ref_dr);
break;
case epullgPOS:
for(m=0; m<DIM; m++)
{
if (pull->dim[m])
{
lambda = r_ij[g][m] - ref[m];
/* The position corrections dr due to the constraints */
dr[g][m] = -lambda*rm*pull->grp[g].invtm;
ref_dr[m] = lambda*rm*pref->invtm;
}
else
{
dr[g][m] = 0;
ref_dr[m] = 0;
}
}
break;
}
/* DEBUG */
if (debug)
{
j = (PULL_CYL(pull) ? g : 0);
get_pullgrps_dr(pull,pbc,g,t,rinew[g],rjnew[j],-1,tmp);
get_pullgrps_dr(pull,pbc,g,t,dr[g] ,ref_dr ,-1,tmp3);
fprintf(debug,
"Pull cur %8.5f %8.5f %8.5f j:%8.5f %8.5f %8.5f d: %8.5f\n",
rinew[g][0],rinew[g][1],rinew[g][2],
rjnew[j][0],rjnew[j][1],rjnew[j][2], dnorm(tmp));
if (pull->eGeom == epullgPOS)
{
fprintf(debug,
"Pull ref %8.5f %8.5f %8.5f\n",
pgrp->vec[0],pgrp->vec[1],pgrp->vec[2]);
}
else
{
fprintf(debug,
"Pull ref %8s %8s %8s %8s %8s %8s d: %8.5f %8.5f %8.5f\n",
"","","","","","",ref[0],ref[1],ref[2]);
}
fprintf(debug,
"Pull cor %8.5f %8.5f %8.5f j:%8.5f %8.5f %8.5f d: %8.5f\n",
dr[g][0],dr[g][1],dr[g][2],
ref_dr[0],ref_dr[1],ref_dr[2],
dnorm(tmp3));
fprintf(debug,
"Pull cor %10.7f %10.7f %10.7f\n",
dr[g][0],dr[g][1],dr[g][2]);
} /* END DEBUG */
/* Update the COMs with dr */
dvec_inc(rinew[g], dr[g]);
dvec_inc(rjnew[PULL_CYL(pull) ? g : 0],ref_dr);
}
/* Check if all constraints are fullfilled now */
for(g=1; g<1+pull->ngrp; g++)
{
pgrp = &pull->grp[g];
get_pullgrps_dr(pull,pbc,g,t,rinew[g],rjnew[PULL_CYL(pull) ? g : 0],
-1,unc_ij);
switch (pull->eGeom)
{
case epullgDIST:
bConverged = fabs(dnorm(unc_ij) - ref[0]) < pull->constr_tol;
break;
case epullgDIR:
case epullgDIRPBC:
case epullgCYL:
for(m=0; m<DIM; m++)
{
vec[m] = pgrp->vec[m];
}
inpr = diprod(unc_ij,vec);
dsvmul(inpr,vec,unc_ij);
bConverged =
fabs(diprod(unc_ij,vec) - ref[0]) < pull->constr_tol;
break;
case epullgPOS:
bConverged = TRUE;
for(m=0; m<DIM; m++)
{
if (pull->dim[m] &&
fabs(unc_ij[m] - ref[m]) >= pull->constr_tol)
{
bConverged = FALSE;
}
}
break;
}
if (!bConverged)
{
if (debug)
{
fprintf(debug,"NOT CONVERGED YET: Group %d:"
"d_ref = %f %f %f, current d = %f\n",
g,ref[0],ref[1],ref[2],dnorm(unc_ij));
}
bConverged_all = FALSE;
}
}
niter++;
/* if after all constraints are dealt with and bConverged is still TRUE
we're finished, if not we do another iteration */
}
if (niter > max_iter)
{
gmx_fatal(FARGS,"Too many iterations for constraint run: %d",niter);
}
/* DONE ITERATING, NOW UPDATE COORDINATES AND CALC. CONSTRAINT FORCES */
if (v)
{
invdt = 1/dt;
}
/* update the normal groups */
for(g=1; g<1+pull->ngrp; g++)
{
pgrp = &pull->grp[g];
/* get the final dr and constraint force for group i */
dvec_sub(rinew[g],pgrp->xp,dr[g]);
/* select components of dr */
for(m=0; m<DIM; m++)
{
dr[g][m] *= pull->dim[m];
}
dsvmul(1.0/(pgrp->invtm*dt*dt),dr[g],f);
dvec_inc(pgrp->f,f);
switch (pull->eGeom)
{
case epullgDIST:
for(m=0; m<DIM; m++)
{
pgrp->f_scal += r_ij[g][m]*f[m]/dnorm(r_ij[g]);
}
break;
case epullgDIR:
case epullgDIRPBC:
case epullgCYL:
for(m=0; m<DIM; m++)
{
pgrp->f_scal += pgrp->vec[m]*f[m];
}
break;
case epullgPOS:
break;
}
if (vir && bMaster) {
/* Add the pull contribution to the virial */
for(j=0; j<DIM; j++)
{
for(m=0; m<DIM; m++)
{
vir[j][m] -= 0.5*f[j]*r_ij[g][m];
}
}
}
/* update the atom positions */
copy_dvec(dr[g],tmp);
for(j=0;j<pgrp->nat_loc;j++)
{
ii = pgrp->ind_loc[j];
if (pgrp->weight_loc)
{
dsvmul(pgrp->wscale*pgrp->weight_loc[j],dr[g],tmp);
}
for(m=0; m<DIM; m++)
{
x[ii][m] += tmp[m];
}
if (v)
{
for(m=0; m<DIM; m++)
{
v[ii][m] += invdt*tmp[m];
}
}
}
}
/* update the reference groups */
if (PULL_CYL(pull))
{
/* update the dynamic reference groups */
for(g=1; g<1+pull->ngrp; g++)
{
pdyna = &pull->dyna[g];
dvec_sub(rjnew[g],pdyna->xp,ref_dr);
/* select components of ref_dr */
for(m=0; m<DIM; m++)
{
ref_dr[m] *= pull->dim[m];
}
for(j=0;j<pdyna->nat_loc;j++)
{
/* reset the atoms with dr, weighted by w_i */
dsvmul(pdyna->wscale*pdyna->weight_loc[j],ref_dr,tmp);
ii = pdyna->ind_loc[j];
for(m=0; m<DIM; m++)
{
x[ii][m] += tmp[m];
}
if (v)
{
for(m=0; m<DIM; m++)
{
v[ii][m] += invdt*tmp[m];
}
}
}
}
}
else
{
pgrp = &pull->grp[0];
/* update the reference group */
dvec_sub(rjnew[0],pgrp->xp, ref_dr);
/* select components of ref_dr */
for(m=0;m<DIM;m++)
{
ref_dr[m] *= pull->dim[m];
}
copy_dvec(ref_dr,tmp);
for(j=0; j<pgrp->nat_loc;j++)
{
ii = pgrp->ind_loc[j];
if (pgrp->weight_loc)
{
dsvmul(pgrp->wscale*pgrp->weight_loc[j],ref_dr,tmp);
}
for(m=0; m<DIM; m++)
{
x[ii][m] += tmp[m];
}
if (v)
{
for(m=0; m<DIM; m++)
{
v[ii][m] += invdt*tmp[m];
}
}
}
}
/* finished! I hope. Give back some memory */
sfree(r_ij);
sfree(rinew);
sfree(rjnew);
sfree(dr);
}