自学教程：C++ sqrlen函数代码示例

/** Computes the three body angle interaction energy (see /ref tclcommand_inter, /ref tclcommand_analyze).     @param p_mid        Pointer to first particle.    @param p_left        Pointer to second/middle particle.    @param p_right        Pointer to third particle.    @param iaparams  bond type number of the angle interaction (see /ref tclcommand_inter).    @param _energy   return energy pointer.    @return 0.*/inline int angle_harmonic_energy(Particle *p_mid, Particle *p_left, Particle *p_right,			     Bonded_ia_parameters *iaparams, double *_energy){  double cosine, vec1[3], vec2[3],  d1i, d2i, dist2;  int j;  cosine=0.0;  /* vector from p_mid to p_left */  get_mi_vector(vec1, p_mid->r.p, p_left->r.p);  dist2 = sqrlen(vec1);  d1i = 1.0 / sqrt(dist2);  for(j=0;j<3;j++) vec1[j] *= d1i;  /* vector from p_right to p_mid */  get_mi_vector(vec2, p_right->r.p, p_mid->r.p);  dist2 = sqrlen(vec2);  d2i = 1.0 / sqrt(dist2);  for(j=0;j<3;j++) vec2[j] *= d2i;  /* scalar produvt of vec1 and vec2 */  cosine = scalar(vec1, vec2);  if ( cosine >  TINY_COS_VALUE)  cosine = TINY_COS_VALUE;  if ( cosine < -TINY_COS_VALUE)  cosine = -TINY_COS_VALUE;  /* bond angle energy */  {    double phi;    phi =  acos(-cosine);    *_energy = 0.5*iaparams->p.angle_harmonic.bend*SQR(phi - iaparams->p.angle_harmonic.phi0);  }  return 0;}

/** Computes the three body angle interaction force and adds this    force to the particle forces (see /ref tclcommand_inter).     @param p_mid     Pointer to second/middle particle.    @param p_left    Pointer to first/left particle.    @param p_right   Pointer to third/right particle.    @param iaparams  bond type number of the angle interaction (see /ref tclcommand_inter).    @param force1 returns force of particle 1    @param force2 returns force of particle 2    @return 0*/inline int calc_angle_cosine_force(Particle *p_mid, Particle *p_left, Particle *p_right,			      Bonded_ia_parameters *iaparams, double force1[3], double force2[3]){  double cosine, vec1[3], vec2[3], d1i, d2i, dist2,  fac, f1=0.0, f2=0.0;  int j;  cosine=0.0;  /* vector from p_left to p_mid */  get_mi_vector(vec1, p_mid->r.p, p_left->r.p);  dist2 = sqrlen(vec1);  d1i = 1.0 / sqrt(dist2);  for(j=0;j<3;j++) vec1[j] *= d1i;  /* vector from p_mid to p_right */  get_mi_vector(vec2, p_right->r.p, p_mid->r.p);  dist2 = sqrlen(vec2);  d2i = 1.0 / sqrt(dist2);  for(j=0;j<3;j++) vec2[j] *= d2i;  /* scalar produvt of vec1 and vec2 */  cosine = scalar(vec1, vec2);  fac    = iaparams->p.angle_cosine.bend;  if ( cosine >  TINY_COS_VALUE ) cosine = TINY_COS_VALUE;  if ( cosine < -TINY_COS_VALUE)  cosine = -TINY_COS_VALUE;  fac *= iaparams->p.angle_cosine.sin_phi0 * (cosine/sqrt(1-SQR(cosine))) + iaparams->p.angle_cosine.cos_phi0;    for(j=0;j<3;j++) {    f1               = fac * (cosine * vec1[j] - vec2[j]) * d1i;    f2               = fac * (cosine * vec2[j] - vec1[j]) * d2i;    force1[j] = (f1-f2);    force2[j] = -f1;  }  return 0;}

/** Computes the three body angle interaction energy (see /ref tclcommand_inter, /ref tclcommand_analyze).     @param p_mid        Pointer to first particle.    @param p_left        Pointer to second/middle particle.    @param p_right        Pointer to third particle.    @param iaparams  bond type number of the angle interaction (see /ref tclcommand_inter).    @param _energy   return energy pointer.    @return 0.*/inline int angle_cosine_energy(Particle *p_mid, Particle *p_left, Particle *p_right,			     Bonded_ia_parameters *iaparams, double *_energy){  double cosine, vec1[3], vec2[3],  d1i, d2i, dist2;  int j;  cosine=0.0;  /* vector from p_mid to p_left */  get_mi_vector(vec1, p_mid->r.p, p_left->r.p);  dist2 = sqrlen(vec1);  d1i = 1.0 / sqrt(dist2);  for(j=0;j<3;j++) vec1[j] *= d1i;  /* vector from p_right to p_mid */  get_mi_vector(vec2, p_right->r.p, p_mid->r.p);  dist2 = sqrlen(vec2);  d2i = 1.0 / sqrt(dist2);  for(j=0;j<3;j++) vec2[j] *= d2i;  /* scalar produvt of vec1 and vec2 */  cosine = scalar(vec1, vec2);  if ( cosine >  TINY_COS_VALUE)  cosine = TINY_COS_VALUE;  if ( cosine < -TINY_COS_VALUE)  cosine = -TINY_COS_VALUE;  /* bond angle energy */  *_energy = iaparams->p.angle_cosine.bend*(cosine*iaparams->p.angle_cosine.cos_phi0 - sqrt(1-SQR(cosine))*iaparams->p.angle_cosine.sin_phi0+1);  return 0;}

/** Computes the QUARTIC pair force and adds this    force to the particle forces (see /ref interaction_data.cpp).     @param p1        Pointer to first particle.    @param p2        Pointer to second/middle particle.    @param iaparams  bond type number of the angle interaction (see /ref interaction_data.cpp).    @param dx        particle distance vector    @param force     returns force of particle 1    @return 0.*/inline int calc_quartic_pair_force(Particle *p1, Particle *p2, Bonded_ia_parameters *iaparams, double dx[3], double force[3]){  int i;  double fac;  double dist2 = sqrlen(dx);  double dist = sqrt(dist2);  double dr;  if ((iaparams->p.quartic.r_cut > 0.0) &&      (dist > iaparams->p.quartic.r_cut))     return 1;  dr = dist - iaparams->p.quartic.r;  if (fabs(dr) > ROUND_ERROR_PREC) {     if(dist>ROUND_ERROR_PREC) {  /* Regular case */        fac = dr / dist;     } else { /* dx[] == 0: the force is undefined. Let's use a random direction */        for(i=0;i<3;i++) dx[i] = d_random()-0.5;        fac = dr / sqrt(sqrlen(dx));     }  } else {      fac=0;  }    for(i=0;i<3;i++)    force[i] = -(iaparams->p.quartic.k0 + iaparams->p.quartic.k1 * dr * dr ) * fac*dx[i];  ONEPART_TRACE(if(p1->p.identity==check_id) fprintf(stderr,"%d: OPT: QUARTIC f = (%.3e,%.3e,%.3e) with part id=%d at dist %f fac %.3e/n",this_node,p1->f.f[0],p1->f.f[1],p1->f.f[2],p2->p.identity,dist2,fac));  ONEPART_TRACE(if(p2->p.identity==check_id) fprintf(stderr,"%d: OPT: QUARTIC f = (%.3e,%.3e,%.3e) with part id=%d at dist %f fac %.3e/n",this_node,p2->f.f[0],p2->f.f[1],p2->f.f[2],p1->p.identity,dist2,fac));  return 0;}

/** Computes the FENE pair force and adds this    force to the particle forces (see /ref interaction_data.cpp).     @param p1        Pointer to first particle.    @param p2        Pointer to second/middle particle.    @param iaparams  bond type number of the angle interaction (see /ref interaction_data.cpp).    @param dx        particle distance vector    @param force     returns force of particle 1    @return true if the bond is broken*/inline int calc_fene_pair_force(Particle *p1, Particle *p2, Bonded_ia_parameters *iaparams, double dx[3], double force[3]){  int i;  double fac, dr, len2, len;   len2 = sqrlen(dx);  len = sqrt(len2);  dr = len - iaparams->p.fene.r0;  if(dr >= iaparams->p.fene.drmax)    return 1;  fac = -iaparams->p.fene.k * dr / ((1.0 - dr*dr*iaparams->p.fene.drmax2i));  if (fabs(dr) > ROUND_ERROR_PREC) {     if(len > ROUND_ERROR_PREC) {  /* Regular case */	fac /= len ;      } else { /* dx[] == 0: the force is undefined. Let's use a random direction */        for(i=0;i<3;i++) dx[i] = d_random()-0.5;        fac /= sqrt(sqrlen(dx));     }  } else {     fac = 0.0;  }    FENE_TRACE(if(fac > 50) fprintf(stderr,"WARNING: FENE force factor between Pair (%d,%d) large: %f at distance %f/n", p1->p.identity,p2->p.identity,fac,sqrt(len2)) );    for(i=0;i<3;i++)    force[i] = fac*dx[i];  ONEPART_TRACE(if(p1->p.identity==check_id) fprintf(stderr,"%d: OPT: FENE f = (%.3e,%.3e,%.3e) with part id=%d at dist %f fac %.3e/n",this_node,p1->f.f[0],p1->f.f[1],p1->f.f[2],p2->p.identity,sqrt(len2),fac));  ONEPART_TRACE(if(p2->p.identity==check_id) fprintf(stderr,"%d: OPT: FENE f = (%.3e,%.3e,%.3e) with part id=%d at dist %f fac %.3e/n",this_node,p2->f.f[0],p2->f.f[1],p2->f.f[2],p1->p.identity,sqrt(len2),fac));    return 0;}

/* The force on each particle due to a three-body bonded potential   is computed. */inline void calc_angle_cossquare_3body_forces(Particle *p_mid, Particle *p_left,        Particle *p_right, Bonded_ia_parameters *iaparams,        double force1[3], double force2[3], double force3[3]) {    int j;    double pot_dep;    double cos_phi;    double sin_phi;    double vec31[3];    double vec21[3];    double vec12[3]; // espresso convention    double vec21_sqr;    double vec31_sqr;    double vec21_magn;    double vec31_magn;    double fj[3];    double fk[3];    double fac;    get_mi_vector(vec12, p_mid->r.p, p_left->r.p);    for(j = 0; j < 3; j++)        vec21[j] = -vec12[j];    get_mi_vector(vec31, p_right->r.p, p_mid->r.p);    vec21_sqr = sqrlen(vec21);    vec21_magn = sqrt(vec21_sqr);    vec31_sqr = sqrlen(vec31);    vec31_magn = sqrt(vec31_sqr);    cos_phi = scalar(vec21, vec31) / (vec21_magn * vec31_magn);    sin_phi = sqrt(1.0 - SQR(cos_phi));    /* uncomment this block if interested in the angle    if(cos_phi < -1.0) cos_phi = -TINY_COS_VALUE;    if(cos_phi >  1.0) cos_phi =  TINY_COS_VALUE;    phi = acos(cos_phi);    */    {        double K, cos_phi0;        K = iaparams->p.angle_cossquare.bend;        cos_phi0 = iaparams->p.angle_cossquare.cos_phi0;        // potential dependent term [dU/dphi = K * (sin_phi * cos_phi0 - cos_phi * sin_phi)]        pot_dep = K * (sin_phi * cos_phi0 - cos_phi * sin_phi);    }    fac = pot_dep / sin_phi;    for(j = 0; j < 3; j++) {        fj[j] = vec31[j] / (vec21_magn * vec31_magn) - cos_phi * vec21[j] / vec21_sqr;        fk[j] = vec21[j] / (vec21_magn * vec31_magn) - cos_phi * vec31[j] / vec31_sqr;    }    // note that F1 = -(F2 + F3)    for(j = 0; j < 3; j++) {        force1[j] = force1[j] - fac * (fj[j] + fk[j]);        force2[j] = force2[j] + fac * fj[j];        force3[j] = force3[j] + fac * fk[j];    }}

int calc_angledist_force(Particle *p_mid, Particle *p_left,				  Particle *p_right,                                  Bonded_ia_parameters *iaparams,				  double force1[3], double force2[3]){  double cosine=0.0, vec1[3], vec2[3], d1i=0.0, d2i=0.0, dist2, fac=0.0, f1=0.0, f2=0.0;  int j;  /* vector from p_left to p_mid */  get_mi_vector(vec1, p_mid->r.p, p_left->r.p);  dist2 = sqrlen(vec1);  d1i = 1.0 / sqrt(dist2);  for(j=0;j<3;j++) vec1[j] *= d1i;  /* vector from p_mid to p_right */  get_mi_vector(vec2, p_right->r.p, p_mid->r.p);  dist2 = sqrlen(vec2);  d2i = 1.0 / sqrt(dist2);  for(j=0;j<3;j++) vec2[j] *= d2i;  /* scalar product of vec1 and vec2 */  cosine = scalar(vec1, vec2);  fac    = iaparams->p.angledist.bend;#ifdef BOND_ANGLEDIST_HARMONIC  {    double phi,sinphi;    /* NOTE: The angledist is ONLY implemented for the HARMONIC case */    double phi0 = calc_angledist_param(p_mid, p_left, p_right, iaparams);      if ( cosine >  TINY_COS_VALUE) cosine =  TINY_COS_VALUE;    if ( cosine < -TINY_COS_VALUE) cosine = -TINY_COS_VALUE;    phi =  acos(-cosine);    sinphi = sin(phi);    if ( sinphi < TINY_SIN_VALUE ) sinphi = TINY_SIN_VALUE;    fac *= (phi - phi0)/sinphi;    //    fprintf(stdout,"/n force:  z_pmid=%f, phi0=%f  phi=%f fac=%f",p_mid->r.p[2],phi0*180.0/PI,phi*180.0/PI,fac);  }#endif#ifdef BOND_ANGLEDIST_COSINE  #error angledist ONLY implemented for harmonic case!#endif#ifdef BOND_ANGLEDIST_COSSQUARE  #error angledist ONLY implemented for harmonic case!#endif  for(j=0;j<3;j++) {    f1               = fac * (cosine * vec1[j] - vec2[j]) * d1i;    f2               = fac * (cosine * vec2[j] - vec1[j]) * d2i;    force1[j] = (f1-f2);    force2[j] = -f1;  }  return 0;}

/** This function returns the angle btw the triangle p1,p2,p3 and p2,p3,p4.  *  Be careful, the angle depends on the orientation of the trianlges!  *  You need to be sure that the orientation (direction of normal vector)  *  of p1p2p3 is given by the cross product p2p1 x p2p3.  *  The orientation of p2p3p4 must be given by p2p3 x p2p4.  *  *  Example: p1 = (0,0,1), p2 = (0,0,0), p3=(1,0,0), p4=(0,1,0).  *  The orientation of p1p2p3 should be in the direction (0,1,0)  *  and indeed: p2p1 x p2p3 = (0,0,1)x(1,0,0) = (0,1,0) *  This function is called in the beginning of the simulation when creating  *  bonds depending on the angle btw the triangles, the bending_force. *  Here, we determine the orientations by looping over the triangles  *  and checking the correct orientation. So when defining the bonds by tcl command *  "part p2 bond xxxx p1 p3 p4", we correctly input the particle id's. *  So if you have the access to the order of particles, you are safe to call this *  function with exactly this order. Otherwise you need to check the orientations. */inline double angle_btw_triangles(double *P1, double *P2, double *P3, double *P4) {	double phi;	double u[3],v[3],normal1[3],normal2[3]; //auxiliary variables	u[0] = P1[0] - P2[0]; // u = P2P1	u[1] = P1[1] - P2[1]; 	u[2] = P1[2] - P2[2]; 	v[0] = P3[0] - P2[0]; // v = P2P3	v[1] = P3[1] - P2[1]; 	v[2] = P3[2] - P2[2]; 	vector_product(u,v,normal1); 	u[0] = P3[0] - P2[0]; // u = P2P3	u[1] = P3[1] - P2[1]; 	u[2] = P3[2] - P2[2]; 	v[0] = P4[0] - P2[0]; // v = P2P4	v[1] = P4[1] - P2[1]; 	v[2] = P4[2] - P2[2]; 	vector_product(u,v,normal2); 	double tmp11,tmp22,tmp33;	// Now we compute the scalar product of n1 and n2 divided by the norms of n1 and n2	//tmp11 = dot(3,normal1,normal2);         // tmp11 = n1.n2	tmp11 = scalar(normal1,normal2);         // tmp11 = n1.n2		/*		tmp22 = normr(normal1);	tmp33 = normr(normal2);	tmp11 /= (tmp22*tmp33);  // tmp11 = n1.n2/(|n1||n2|)*/	tmp11 *= fabs(tmp11); // tmp11 = (n1.n2)^2	tmp22 = sqrlen(normal1);  //tmp22 = |n1|^2	tmp33 = sqrlen(normal2);  //tmp33 = |n1|^2	tmp11 /= (tmp22*tmp33);  // tmp11 = (n1.n2/(|n1||n2|))^2	if (tmp11 > 0 ) {		tmp11 = sqrt(tmp11);	} else {		tmp11 = - sqrt(- tmp11);	}			if(tmp11>=1.) { tmp11=0.0;}	else if(tmp11<=-1.) { tmp11=M_PI;}	phi = M_PI - acos(tmp11); 	// The angle between the faces (not considering the orientation, always less or equal to Pi) is								// equal to Pi minus angle between the normals		// Now we need to determine, if the angle btw two triangles is less than Pi or more than Pi. To do this we check, 	// if the point P4 lies in the halfspace given by trianlge P1P2P3 and the normal to this triangle. If yes, we have 	// angle less than Pi, if not, we have angle more than Pi.	// General equation of the plane is n_x*x + n_y*y + n_z*z + d = 0 where (n_x,n_y,n_z) is the normal to the plane.	// Point P1 lies in the plane, therefore d = -(n_x*P1_x + n_y*P1_y + n_z*P1_z)	// Point P4 lies in the halfspace given by normal iff n_x*P4_x + n_y*P4_y + n_z*P4_z + d >= 0	tmp11 = - (normal1[0]*P1[0] + normal1[1]*P1[1] + normal1[2]*P1[2]);	if (normal1[0]*P4[0] + normal1[1]*P4[1] + normal1[2]*P4[2] + tmp11 < 0) phi = 2*M_PI - phi;	return phi;}

/** Computes the three body overlapped angle interaction force.    Adds this force to the particle forces in forces.hpp (see /ref tclcommand_inter).     @param p_mid     Pointer to second/middle particle.    @param p_left    Pointer to first/left particle.    @param p_right   Pointer to third/right particle.    @param iaparams  bond type number of the angle interaction (see /ref tclcommand_inter).    @param force1 returns force of particle 1    @param force2 returns force of particle 2    @return 0    Needs feature OVERLAPPED compiled in (see /ref config.hpp). */inline int calc_overlap_angle_force(Particle *p_mid, Particle *p_left, 				  Particle *p_right, Bonded_ia_parameters *iaparams,				  double force1[3], double force2[3]){  double cosine, vec1[3], vec2[3], d1i, d2i, dist2,  fac, f1=0.0, f2=0.0;  int j;     int ig;  double ev;  double prob=0.;   double Apart=0.;  cosine=0.0;  /* vector from p_left to p_mid */  get_mi_vector(vec1, p_mid->r.p, p_left->r.p);  dist2 = sqrlen(vec1);  d1i = 1.0 / sqrt(dist2);  for(j=0;j<3;j++) vec1[j] *= d1i;  /* vector from p_mid to p_right */  get_mi_vector(vec2, p_right->r.p, p_mid->r.p);  dist2 = sqrlen(vec2);  d2i = 1.0 / sqrt(dist2);  for(j=0;j<3;j++) vec2[j] *= d2i;  /* scalar produvt of vec1 and vec2 */  /* Notice: cosine = - costheta */  cosine = scalar(vec1, vec2);  if ( cosine >  TINY_COS_VALUE ) cosine = TINY_COS_VALUE;  if ( cosine < -TINY_COS_VALUE)  cosine = -TINY_COS_VALUE;  /* compute fac = - dU/d(costheta) */  /* prob = sum_(i=1,N) [ a_i * exp(-(costheta -b_i)^2/c_i^2))] */  /* Apart = sum_(i=1,N) [ a_i * exp(-(costheta -b_i)^2/c_i^2)) * (costheta-b_i) / c_i^2] */  /* fac = -2.0 * ( Apart / prob ) */  for(ig=0; ig<iaparams->p.overlap.noverlaps; ig++) {        ev = (-cosine - iaparams->p.overlap.para_b[ig]) / iaparams->p.overlap.para_c[ig];        prob = prob + iaparams->p.overlap.para_a[ig] * exp (-1.0*ev*ev);        Apart = Apart + iaparams->p.overlap.para_a[ig] * exp (-1.0*ev*ev) * (-cosine-iaparams->p.overlap.para_b[ig]) / (iaparams->p.overlap.para_c[ig] * iaparams->p.overlap.para_c[ig]);  }  fac = -2. * ( Apart / prob);  /* compute force */  for(j=0;j<3;j++) {    f1               = fac * (cosine * vec1[j] - vec2[j]) * d1i;    f2               = fac * (cosine * vec2[j] - vec1[j]) * d2i;    force1[j] = (f1-f2);    force2[j] = -f1;  }  return 0;}

/** Computes the three body angle interaction force and adds this    force to the particle forces (see /ref tclcommand_inter).     @param p_mid     Pointer to second/middle particle.    @param p_left    Pointer to first/left particle.    @param p_right   Pointer to third/right particle.    @param iaparams  bond type number of the angle interaction (see /ref tclcommand_inter).    @param force1 returns force of particle 1    @param force2 returns force of particle 2    @return 0*/inline int calc_angle_harmonic_force(Particle *p_mid, Particle *p_left, Particle *p_right,			      Bonded_ia_parameters *iaparams, double force1[3], double force2[3]){  double cosine, vec1[3], vec2[3], d1i, d2i, dist2,  fac, f1=0.0, f2=0.0;  int j;  cosine=0.0;  /* vector from p_left to p_mid */  get_mi_vector(vec1, p_mid->r.p, p_left->r.p);  dist2 = sqrlen(vec1);  d1i = 1.0 / sqrt(dist2);  for(j=0;j<3;j++) vec1[j] *= d1i;  /* vector from p_mid to p_right */  get_mi_vector(vec2, p_right->r.p, p_mid->r.p);  dist2 = sqrlen(vec2);  d2i = 1.0 / sqrt(dist2);  for(j=0;j<3;j++) vec2[j] *= d2i;  /* scalar produvt of vec1 and vec2 */  cosine = scalar(vec1, vec2);  fac    = iaparams->p.angle_harmonic.bend;  {    double phi,sinphi;    if ( cosine >  TINY_COS_VALUE) cosine = TINY_COS_VALUE;    if ( cosine < -TINY_COS_VALUE)  cosine = -TINY_COS_VALUE;    phi =  acos(-cosine);    sinphi = sin(phi);    if ( sinphi < TINY_SIN_VALUE ) sinphi = TINY_SIN_VALUE;    fac *= (phi - iaparams->p.angle_harmonic.phi0)/sinphi;#ifdef CONFIGTEMP  extern double configtemp[2];  if (p_left->p.configtemp) {    configtemp[0] += SQR(fac*sinphi*d1i);    configtemp[1] -= iaparams->p.angle_harmonic.bend*(1+cosine/sinphi*(phi - iaparams->p.angle_harmonic.phi0))*SQR(d1i);  }  if (p_mid->p.configtemp) {    configtemp[0] += SQR(fac*sinphi) * (1./sqrlen(vec1) + 1./sqrlen(vec2) - 2*cosine*d1i*d2i);    configtemp[1] -= iaparams->p.angle_harmonic.bend*((1/sqrlen(vec1)+1/sqrlen(vec2)-cosine*d1i*d2i)      +cosine/sinphi * (1./sqrlen(vec1)+1./sqrlen(vec2)-d1i*d2i/cosine) *(phi - iaparams->p.angle_harmonic.phi0));  }  if (p_right->p.configtemp) {    configtemp[0] += SQR(fac*sinphi*d2i);    configtemp[1] -= iaparams->p.angle_harmonic.bend*(1+cosine/sinphi*(phi - iaparams->p.angle_harmonic.phi0))*SQR(d2i);  }#endif  }  for(j=0;j<3;j++) {    f1               = fac * (cosine * vec1[j] - vec2[j]) * d1i;    f2               = fac * (cosine * vec2[j] - vec1[j]) * d2i;    force1[j] = (f1-f2);    force2[j] = -f1;  }  return 0;}

/** Computes the three body angle interaction force and adds this    force to the particle forces (see /ref tclcommand_inter).     @param p_mid     Pointer to second/middle particle.    @param p_left    Pointer to first/left particle.    @param p_right   Pointer to third/right particle.    @param iaparams  bond type number of the angle interaction (see /ref tclcommand_inter).    @param force1 returns force of particle 1    @param force2 returns force of particle 2    @return 0*/inline int calc_angle_force(Particle *p_mid, Particle *p_left, Particle *p_right,			      Bonded_ia_parameters *iaparams, double force1[3], double force2[3]){  double cosine, vec1[3], vec2[3], d1i, d2i, dist2,  fac, f1=0.0, f2=0.0;  int j;  cosine=0.0;  /* vector from p_left to p_mid */  get_mi_vector(vec1, p_mid->r.p, p_left->r.p);  dist2 = sqrlen(vec1);  d1i = 1.0 / sqrt(dist2);  for(j=0;j<3;j++) vec1[j] *= d1i;  /* vector from p_mid to p_right */  get_mi_vector(vec2, p_right->r.p, p_mid->r.p);  dist2 = sqrlen(vec2);  d2i = 1.0 / sqrt(dist2);  for(j=0;j<3;j++) vec2[j] *= d2i;  /* scalar produvt of vec1 and vec2 */  cosine = scalar(vec1, vec2);  fac    = iaparams->p.angle.bend;#ifdef BOND_ANGLE_HARMONIC  {    double phi,sinphi;    if ( cosine >  TINY_COS_VALUE) cosine = TINY_COS_VALUE;    if ( cosine < -TINY_COS_VALUE)  cosine = -TINY_COS_VALUE;    phi =  acos(-cosine);    sinphi = sin(phi);    if ( sinphi < TINY_SIN_VALUE ) sinphi = TINY_SIN_VALUE;    fac *= (phi - iaparams->p.angle.phi0)/sinphi;  }#endif#ifdef BOND_ANGLE_COSINE  if ( cosine >  TINY_COS_VALUE ) cosine = TINY_COS_VALUE;  if ( cosine < -TINY_COS_VALUE)  cosine = -TINY_COS_VALUE;  fac *= iaparams->p.angle.sin_phi0 * (cosine/sqrt(1-SQR(cosine))) + iaparams->p.angle.cos_phi0;#endif#ifdef BOND_ANGLE_COSSQUARE  fac *= iaparams->p.angle.cos_phi0 + cosine;#endif  for(j=0;j<3;j++) {    f1               = fac * (cosine * vec1[j] - vec2[j]) * d1i;    f2               = fac * (cosine * vec2[j] - vec1[j]) * d2i;    force1[j] = (f1-f2);    force2[j] = -f1;  }  return 0;}

void nsq_calculate_virials(){  Particle *partl, *partg;  Particle *pt1, *pt2;  int p, p2, npl, npg, c;  double d[3], dist2, dist;  npl   = local->n;  partl = local->part;  /* calculate bonded interactions and non bonded node-node */  for (p = 0; p < npl; p++) {    pt1 = &partl[p];    add_kinetic_virials(pt1,0);    add_bonded_virials(pt1);#ifdef BOND_ANGLE    add_three_body_bonded_stress(pt1);#endif    /* other particles, same node */    for (p2 = p + 1; p2 < npl; p2++) {      pt2 = &partl[p2];      get_mi_vector(d, pt1->r.p, pt2->r.p);      dist2 = sqrlen(d);      dist = sqrt(dist2);#ifdef EXCLUSIONS      if (do_nonbonded(pt1, pt2))#endif	add_non_bonded_pair_virials(pt1, pt2, d, dist, dist2);    }    /* calculate with my ghosts */    for (c = 0; c < me_do_ghosts.n; c++) {      npg   = me_do_ghosts.cell[c]->n;      partg = me_do_ghosts.cell[c]->part;      for (p2 = 0; p2 < npg; p2++) {	pt2 = &partg[p2];	get_mi_vector(d, pt1->r.p, pt2->r.p);	dist2 = sqrlen(d);	dist = sqrt(dist2);#ifdef EXCLUSIONS	if (do_nonbonded(pt1, pt2))#endif	  add_non_bonded_pair_virials(pt1, pt2, d, dist, dist2);      }    }  }}

inline int bonded_coulomb_pair_energy(Particle *p1, Particle *p2, Bonded_ia_parameters *iaparams, double dx[3], double *_energy){  double dist = sqrt(sqrlen(dx));  *_energy = iaparams->p.bonded_coulomb.prefactor * p1->p.q * p2->p.q / dist;  return 0;}

void print_bond_len(){  int i, k, c, np;  Cell *cell;  Particle *p;  Bonded_ia_parameters *b_ia;  double r_ij[3];  printf("%d: ", this_node);  for (c = 0; c < local_cells.n; c++) {    cell = local_cells.cell[c];    p  = cell->part;    np = cell->n;    for(i = 0; i < np; i++) {       k=0;       while(k<p[i].bl.n)       {             b_ia = &bonded_ia_params[p[i].bl.e[k]];	     if(b_ia->type == BONDED_IA_RIGID_BOND)             {	       Particle *p2 = local_particles[p[i].bl.e[k++]];           if (!p2) {           runtimeErrorMsg() <<"rigid bond broken between particles " << p[i].p.identity << " and " << p[i].bl.e[k-1] << " (particles not stored on the same node)";		 return;	       }	       get_mi_vector(r_ij, p[i].r.p , p2->r.p);	       printf(" bl (%d %d): %f/t", p[i].p.identity, p2->p.identity,sqrlen(r_ij));             }	     else	       k += b_ia->num;       } //while k loop    } //for i loop  }// for c loop  printf("/n");}

/* Rectify an image point * input and output in normalized coordinates */Vec2d Intrinsic::rectify (Vec2d xd){	double r2 = len(xd);		if (dist_model == POLYNOMIAL_DISTORTION) {		double rd4 = r2*r2;		double rd6 = rd4*r2;		double rd8 = rd4*rd4;		double beta = getKC0()*r2+getKC1()*rd4+getKC0()*getKC0()*rd4+getKC1()*getKC1()*rd8+/			2*getKC0()*getKC1()*rd6;		double alpha = 1 + 4*getKC0()*r2 + 6*getKC1()*rd4;		return xd * (1 - beta/alpha);		} else if (dist_model == SPHERICAL_DISTORTION) {			Vec2d x = xd-cc;			r2 = sqrlen(x);			double r_new = value*value / sqrt(fabs(1 - value*value*value*value*r2));			x = x * r_new+cc; //			return x;	}		printf("undefined distortion model!/n");		assert(false);		return Vec2d (0,0);}

inline int subt_lj_pair_energy(Particle *p1, Particle *p2, Bonded_ia_parameters *iaparams, double dx[3], double *_energy){  double energy=0.0;  IA_parameters *ia_params;  double r_off, frac2, frac6;  double dist2 = sqrlen(dx);  double dist = sqrt(dist2);    if(dist >= iaparams->p.subt_lj.r)    return 1;    ia_params = get_ia_param(p1->p.type,p2->p.type);    if(dist < ia_params->LJ_cut+ia_params->LJ_offset) {    r_off = dist - ia_params->LJ_offset;    if(r_off > ia_params->LJ_capradius) {      /* normal case: resulting force/energy smaller than capping. */      frac2 = SQR(ia_params->LJ_sig/r_off);      frac6 = frac2*frac2*frac2;      energy = 4.0*ia_params->LJ_eps*(SQR(frac6)-frac6+ia_params->LJ_shift);    }       else if(dist > 0.0) {      /* capped part of lj potential. */      frac2 = SQR(ia_params->LJ_sig/ia_params->LJ_capradius);      frac6 = frac2*frac2*frac2;      energy = 4.0*ia_params->LJ_eps*(SQR(frac6)-frac6+ia_params->LJ_shift);    }  }  *_energy = -energy;  return 0;}

void nbhood(double pt[3], double r, IntList *il, int planedims[3] ){  double d[3];  int i,j;  double r2;  r2 = r*r;  init_intlist(il);   updatePartCfg(WITHOUT_BONDS);  for (i = 0; i<n_part; i++) {    if ( (planedims[0] + planedims[1] + planedims[2]) == 3 ) {      get_mi_vector(d, pt, partCfg[i].r.p);    } else {      /* Calculate the in plane distance */      for ( j= 0 ; j < 3 ; j++ ) {	d[j] = planedims[j]*(partCfg[i].r.p[j]-pt[j]);      }    }    if (sqrlen(d) < r2) {      realloc_intlist(il, il->n + 1);      il->e[il->n] = partCfg[i].p.identity;      il->n++;    }  }}

// Update the pos of the given virtual particle as defined by the real // particles in the same moleculevoid update_mol_pos_particle(Particle *p){ // First obtain the real particle responsible for this virtual particle: // Find the 1st real particle in the topology for the virtual particle's mol_id Particle *p_real = vs_relative_get_real_particle(p); // Check, if a real particle was found if (!p_real) {   char *errtxt = runtime_error(128 + 3*ES_INTEGER_SPACE);   ERROR_SPRINTF(errtxt,"virtual_sites_relative.cpp - update_mol_pos_particle(): No real particle associated with virtual site./n");   return; }  // Calculate the quaternion defining the orientation of the vecotr connectinhg // the virtual site and the real particle // This is obtained, by multiplying the quaternion representing the director // of the real particle with the quaternion of the virtual particle, which  // specifies the relative orientation. double q[4]; multiply_quaternions(p_real->r.quat,p->r.quat,q); // Calculate the director resulting from the quaternions double director[3]; convert_quat_to_quatu(q,director); // normalize double l =sqrt(sqrlen(director)); // Division comes in the loop below  // Calculate the new position of the virtual sites from // position of real particle + director  int i; double new_pos[3]; double tmp; for (i=0;i<3;i++) {  new_pos[i] =p_real->r.p[i] +director[i]/l*p->p.vs_relative_distance;  // Handle the case that one of the particles had gone over the periodic  // boundary and its coordinate has been folded  #ifdef PARTIAL_PERIODIC   if (PERIODIC(i))   #endif  {    tmp =p->r.p[i] -new_pos[i];    //printf("%f/n",tmp);    if (tmp > box_l[i]/2.) {     //printf("greater than box_l/2 %f/n",tmp);     p->r.p[i] =new_pos[i] + box_l[i];    }    else if (tmp < -box_l[i]/2.) {     //printf("smaller than box_l/2 %f/n",tmp);     p->r.p[i] =new_pos[i] - box_l[i];    }    else p->r.p[i] =new_pos[i];   }   #ifdef PARTIAL_PERIODIC    else p->r.p[i] =new_pos[i];   #endif//   fold_coordinate(p->r.p,p->l.i,i); }}

/// Calculate angle that p1--p2 makes with wall constraintstatic double calc_pwangle(Particle *p1, Particle *p2, Bonded_ia_parameters *iaparams, int *constr){  int j;  double dist,di,cosine,phi;  double vec[3];  /* vector from p1 to p2 */  get_mi_vector(vec, p2->r.p, p1->r.p);  dist = sqrlen(vec);  di = 1.0/sqrt(dist);  for(j=0;j<3;j++) vec[j] *= di;  /*  fprintf(stdout,"Normalised: p1= %9.6f %9.6f %9.6f   p1= %9.6f %9.6f %9.6f   vec= %9.6f %9.6f %9.6f/n",p1->r.p[0],p1->r.p[1],p1->r.p[2],p2->r.p[0],p2->r.p[1],p2->r.p[2],vec[0],vec[1],vec[2]);  */  /* vectors are normalised so cosine is just cos(angle_between_vec1_and_vec2)   * Wall is closest wall to particle   */  cosine = scalar(vec, constraints[*constr].c.wal.n);  if ( cosine >  TINY_COS_VALUE)  cosine =  TINY_COS_VALUE;  if ( cosine < -TINY_COS_VALUE)  cosine = -TINY_COS_VALUE;  phi=acos(cosine);  /*  fprintf(stdout,"Angle with wall 0=%f  ",(acos(scalar(vec, constraints[0].c.wal.n)))*180.0/PI);  fprintf(stdout,"Angle with wall 1=%f  ",(acos(scalar(vec, constraints[1].c.wal.n)))*180.0/PI);  fprintf(stdout,"dxy=%f  dz=%f  angle=%f/n",sqrt(vec[0]*vec[0]+vec[1]*vec[1]),vec[2],atan(sqrt(vec[0]*vec[0]+vec[1]*vec[1])/vec[2])*180.0/PI);  fprintf(stdout,"Angle with closest wall %d=%f  ",*constr,(acos(scalar(vec, constraints[*constr].c.wal.n)))*180.0/PI);  */  return phi;}

int angledist_energy(Particle *p_mid, Particle *p_left, Particle *p_right, 		     Bonded_ia_parameters *iaparams, double *_energy){  int j;  double cosine=0.0, d1i, d2i, dist1, dist2;  double vec1[3], vec2[3];  //  if (phi0 < PI) {  //    fprintf(stdout,"/nIn angledist_energy:  z_p_mid=%f, phi0=%f/n",p_mid->r.p[2],phi0*180.0/PI);  //  }  /* vector from p_mid to p_left */  get_mi_vector(vec1, p_mid->r.p, p_left->r.p);  dist1 = sqrlen(vec1);  d1i = 1.0 / sqrt(dist1);  for(j=0;j<3;j++) vec1[j] *= d1i;  /* vector from p_right to p_mid */  get_mi_vector(vec2, p_right->r.p, p_mid->r.p);  dist2 = sqrlen(vec2);  d2i = 1.0 / sqrt(dist2);  for(j=0;j<3;j++) vec2[j] *= d2i;  /* scalar produvt of vec1 and vec2 */  cosine = scalar(vec1, vec2);  if ( cosine >  TINY_COS_VALUE)  cosine = TINY_COS_VALUE;  if ( cosine < -TINY_COS_VALUE)  cosine = -TINY_COS_VALUE;#ifdef BOND_ANGLEDIST_HARMONIC  {    double phi;    double phi0=calc_angledist_param(p_mid, p_left, p_right, iaparams);    phi =  acos(-cosine);    *_energy = 0.5*iaparams->p.angledist.bend*SQR(phi - phi0);    //    fprintf(stdout,"/n energy:  z_pmid=%f  bend=%f  phi0=%f  phi=%f energy=%f",p_mid->r.p[2],iaparams->p.angledist.bend,phi0*180.0/PI,phi*180.0/PI,0.5*iaparams->p.angledist.bend*SQR(phi - phi0));  }#endif#ifdef BOND_ANGLEDIST_COSINE  #error angledist ONLY implemented for harmonic case!#endif#ifdef BOND_ANGLEDIST_COSSQUARE  #error angledist ONLY implemented for harmonic case!#endif  return 0;}

/** calculates unit vector */inline void unit_vector(double v[3],double y[3]) {  double d = 0.0;  int i;  d=sqrt( sqrlen(v) );    for(i=0;i<3;i++)    y[i] = v[i]/d;      return;}

/**Compute positional corrections*/void compute_pos_corr_vec(int *repeat_){  Bonded_ia_parameters *ia_params;  int i, j, k, c, np, cnt=-1;  Cell *cell;  Particle *p, *p1, *p2;  double r_ij_t[3], r_ij[3], r_ij_dot, G, pos_corr, r_ij2;  for (c = 0; c < local_cells.n; c++) {    cell = local_cells.cell[c];    p  = cell->part;    np = cell->n;    for(i = 0; i < np; i++) {      p1 = &p[i];      k=0;      while(k<p1->bl.n) {	ia_params = &bonded_ia_params[p1->bl.e[k++]];	if( ia_params->type == BONDED_IA_RIGID_BOND ) {	  cnt++;	  p2 = local_particles[p1->bl.e[k++]];	  if (!p2) {	    char *errtxt = runtime_error(128 + 2*ES_INTEGER_SPACE);	    ERROR_SPRINTF(errtxt,"{051 rigid bond broken between particles %d and %d (particles not stored on the same node)} ",		    p1->p.identity, p1->bl.e[k-1]);	    return;	  }	  get_mi_vector(r_ij  , p1->r.p    , p2->r.p    );	  r_ij2 = sqrlen(r_ij);	  if(fabs(1.0 - r_ij2/ia_params->p.rigid_bond.d2) > ia_params->p.rigid_bond.p_tol) {            get_mi_vector(r_ij_t, p1->r.p_old, p2->r.p_old);	    r_ij_dot = scalar(r_ij_t, r_ij);	    G = 0.50*(ia_params->p.rigid_bond.d2 - r_ij2 )/r_ij_dot;#ifdef MASS	    G /= (PMASS(*p1)+PMASS(*p2));#else	    G /= 2;#endif	    for (j=0;j<3;j++) {	      pos_corr = G*r_ij_t[j];	      p1->f.f[j] += pos_corr*PMASS(*p2);	      p2->f.f[j] -= pos_corr*PMASS(*p1);	    }	    /*Increase the 'repeat' flag by one */	      *repeat_ = *repeat_ + 1;	  }	}	else	  /* skip bond partners of nonrigid bond */          k+=ia_params->num;      } //while loop    } //for i loop  } //for c loop}

/** Computes the HARMONIC pair force and adds this    force to the particle forces (see /ref interaction_data.cpp).     @param p1        Pointer to first particle.    @param p2        Pointer to second/middle particle.    @param iaparams  bond type number of the angle interaction (see /ref interaction_data.cpp).    @param dx        particle distance vector    @param force     returns force of particle 1    @return 0.*/inline int calc_harmonic_pair_force(Particle *p1, Particle *p2, Bonded_ia_parameters *iaparams, double dx[3], double force[3]){  int i;  double fac;  double dist2 = sqrlen(dx);  double dist = sqrt(dist2);  double dr;  if ((iaparams->p.harmonic.r_cut > 0.0) &&      (dist > iaparams->p.harmonic.r_cut))     return 1;  dr = dist - iaparams->p.harmonic.r;  fac = -iaparams->p.harmonic.k * dr;  if (fabs(dr) > ROUND_ERROR_PREC) {     if(dist>ROUND_ERROR_PREC) {  /* Regular case */        fac /= dist;     } else { /* dx[] == 0: the force is undefined. Let's use a random direction */        for(i=0;i<3;i++) dx[i] = d_random()-0.5;        fac /= sqrt(sqrlen(dx));     }  } else {      fac=0;  }    for(i=0;i<3;i++)    force[i] = fac*dx[i];  ONEPART_TRACE(if(p1->p.identity==check_id) fprintf(stderr,"%d: OPT: HARMONIC f = (%.3e,%.3e,%.3e) with part id=%d at dist %f fac %.3e/n",this_node,p1->f.f[0],p1->f.f[1],p1->f.f[2],p2->p.identity,dist2,fac));  ONEPART_TRACE(if(p2->p.identity==check_id) fprintf(stderr,"%d: OPT: HARMONIC f = (%.3e,%.3e,%.3e) with part id=%d at dist %f fac %.3e/n",this_node,p2->f.f[0],p2->f.f[1],p2->f.f[2],p1->p.identity,dist2,fac));#ifdef CONFIGTEMP  extern double configtemp[2];  int numfac = 0;  if (p1->p.configtemp) numfac+=1;  if (p2->p.configtemp) numfac+=1;  configtemp[0] += numfac*SQR(iaparams->p.harmonic.k * dr);  configtemp[1] -= numfac*iaparams->p.harmonic.k*(3-2.*iaparams->p.harmonic.r/dist);#endif  return 0;}

inline int harmonic_pair_energy(Particle *p1, Particle *p2, Bonded_ia_parameters *iaparams, double dx[3], double *_energy){  double dist2 = sqrlen(dx);  double dist = sqrt(dist2);  if ((iaparams->p.harmonic.r_cut > 0.0) &&       (dist > iaparams->p.harmonic.r_cut))     return 1;  *_energy = 0.5*iaparams->p.harmonic.k*SQR(dist - iaparams->p.harmonic.r);  return 0;}

/** Computes the negative of the LENNARD-JONES pair forces     and adds this force to the particle forces (see /ref tclcommand_inter).     @param p1        Pointer to first particle.    @param p2        Pointer to second/middle particle.    @param iaparams  Parameters of interaction    @param dx        change in position    @param force     force on particles    @return true if bond is broken*/inline int calc_subt_lj_pair_force(Particle *p1, Particle *p2, Bonded_ia_parameters *iaparams, double dx[3], double force[3]){  int i;  double fac_lj=0.0;  IA_parameters *ia_params;  double r_off, frac2, frac6;  double dist2 = sqrlen(dx);  double dist = sqrt(dist2);  if(dist >= iaparams->p.subt_lj.r)    return 1;  ia_params = get_ia_param(p1->p.type,p2->p.type);  if(dist < ia_params->LJ_cut+ia_params->LJ_offset) {     r_off = dist - ia_params->LJ_offset;    if(r_off > ia_params->LJ_capradius) {      /* normal case: resulting force/energy smaller than capping. */      frac2 = SQR(ia_params->LJ_sig/r_off);      frac6 = frac2*frac2*frac2;      fac_lj = 48.0 * ia_params->LJ_eps * frac6*(frac6 - 0.5) / (r_off * dist);			      }    else if(dist > 0.0) {      /* capped part of lj potential. */      frac2 = SQR(ia_params->LJ_sig/ia_params->LJ_capradius);      frac6 = frac2*frac2*frac2;      fac_lj = 48.0 * ia_params->LJ_eps * frac6*(frac6 - 0.5) / (ia_params->LJ_capradius * dist);// #ifdef CONFIGTEMP//       extern double configtemp[2];//       int numfac = 0;//       if (p1->p.configtemp) numfac+=1;//       if (p2->p.configtemp) numfac+=1;//       configtemp[0] -= numfac*SQR(48.0 * ia_params->LJ_eps * frac6*(frac6 - 0.5) / r_off);//       configtemp[1] -= numfac*24.0 * ia_params->LJ_eps * frac6*(-22.0*frac6+5.0) / (SQR(r_off));// #endif    }  }   for(i=0;i<3;i++)    force[i] = -fac_lj*dx[i];  ONEPART_TRACE(if(p1->p.identity==check_id) fprintf(stderr,"%d: OPT: SUBT_LJ f = (%.3e,%.3e,%.3e) with part id=%d at dist %f fac_lj %.3e/n",this_node,p1->f.f[0],p1->f.f[1],p1->f.f[2],p2->p.identity,sqrt(dist2),fac_lj));  ONEPART_TRACE(if(p2->p.identity==check_id) fprintf(stderr,"%d: OPT: SUBT_LJ f = (%.3e,%.3e,%.3e) with part id=%d at dist %f fac_lj %.3e/n",this_node,p2->f.f[0],p2->f.f[1],p2->f.f[2],p1->p.identity,sqrt(dist2),fac_lj));  return 0;}

/** Computes the three body angle interaction energy (see /ref tclcommand_inter, /ref tclcommand_analyze).     @param p_mid        Pointer to first particle.    @param p_left        Pointer to second/middle particle.    @param p_right        Pointer to third particle.    @param iaparams  bond type number of the angle interaction (see /ref tclcommand_inter).    @param _energy   return energy pointer.    @return 0.*/inline int angle_energy(Particle *p_mid, Particle *p_left, Particle *p_right,			     Bonded_ia_parameters *iaparams, double *_energy){  double cosine, vec1[3], vec2[3],  d1i, d2i, dist2;  int j;  cosine=0.0;  /* vector from p_mid to p_left */  get_mi_vector(vec1, p_mid->r.p, p_left->r.p);  dist2 = sqrlen(vec1);  d1i = 1.0 / sqrt(dist2);  for(j=0;j<3;j++) vec1[j] *= d1i;  /* vector from p_right to p_mid */  get_mi_vector(vec2, p_right->r.p, p_mid->r.p);  dist2 = sqrlen(vec2);  d2i = 1.0 / sqrt(dist2);  for(j=0;j<3;j++) vec2[j] *= d2i;  /* scalar produvt of vec1 and vec2 */  cosine = scalar(vec1, vec2);  if ( cosine >  TINY_COS_VALUE)  cosine = TINY_COS_VALUE;  if ( cosine < -TINY_COS_VALUE)  cosine = -TINY_COS_VALUE;  /* bond angle energy */#ifdef BOND_ANGLE_HARMONIC  {    double phi;    phi =  acos(-cosine);    *_energy = 0.5*iaparams->p.angle.bend*SQR(phi - iaparams->p.angle.phi0);  }#endif#ifdef BOND_ANGLE_COSINE  *_energy = iaparams->p.angle.bend*(cosine*iaparams->p.angle.cos_phi0 - sqrt(1-SQR(cosine))*iaparams->p.angle.sin_phi0+1);#endif#ifdef BOND_ANGLE_COSSQUARE  *_energy = 0.5*iaparams->p.angle.bend*SQR(cosine + iaparams->p.angle.cos_phi0);#endif  return 0;}

/** Rotate the particle p around the NORMALIZED axis aSpaceFrame by amount phi */void rotate_particle(Particle* p, double* aSpaceFrame, double phi){  // Convert rotation axis to body-fixed frame  double a[3];  convert_vec_space_to_body(p,aSpaceFrame,a);  // Apply restrictions from the rotation_per_particle feature#ifdef ROTATION_PER_PARTICLE//  printf("%g %g %g - ",a[0],a[1],a[2]);  // Rotation turned off entirely?  if (p->p.rotation <2) return;  // Per coordinate fixing  if (!(p->p.rotation & 2)) a[0]=0;  if (!(p->p.rotation & 4)) a[1]=0;  if (!(p->p.rotation & 8)) a[2]=0;  // Re-normalize rotation axis  double l=sqrt(sqrlen(a));  // Check, if the rotation axis is nonzero  if (l<1E-10) return;  for (int i=0;i<3;i++)    a[i]/=l;//  printf("%g %g %g/n",a[0],a[1],a[2]);#endif  double q[4];  q[0]=cos(phi/2);  double tmp=sin(phi/2);  q[1]=tmp*a[0];  q[2]=tmp*a[1];  q[3]=tmp*a[2];    // Normalize  normalize_quaternion(q);  // Rotate the particle  double qn[4]; // Resulting quaternion  multiply_quaternions(p->r.quat,q,qn);  for (int k=0; k<4; k++)    p->r.quat[k]=qn[k];  convert_quat_to_quatu(p->r.quat, p->r.quatu);#ifdef DIPOLES  // When dipoles are enabled, update dipole moment  convert_quatu_to_dip(p->r.quatu, p->p.dipm, p->r.dip);#endif}

/* TODO: this function is not used anywhere. To be removed? */double calc_mol_hydro_radius(Molecule mol) {  int i, j, id1, id2;  double rh=0.0, diff_vec[3];  for(i=0; i<mol.part.n; i++) {    id1 = mol.part.e[i];    for(j=i+1; j<mol.part.n; j++) {      id2 = mol.part.e[i];      vecsub(partCfg[id1].r.p, partCfg[id2].r.p, diff_vec);      rh += 1.0/sqrt(sqrlen(diff_vec));    }  }  return 0.5*(mol.part.n*(mol.part.n-1))/rh;}