/* calculate the point t in [0..1] on the (convex) bezier curve
   (p0,p1,p2,p3) which is tangent to q1-q0. Return -1.0 if there is no
   solution in [0..1]. */
static double tangent(dpoint_t p0, dpoint_t p1, dpoint_t p2, dpoint_t p3, dpoint_t q0, dpoint_t q1) {
  double A, B, C;   /* (1-t)^2 A + 2(1-t)t B + t^2 C = 0 */
  double a, b, c;   /* a t^2 + b t + c = 0 */
  double d, s, r1, r2;

  A = cprod(p0, p1, q0, q1);
  B = cprod(p1, p2, q0, q1);
  C = cprod(p2, p3, q0, q1);

  a = A - 2*B + C;
  b = -2*A + 2*B;
  c = A;
  
  d = b*b - 4*a*c;

  if (a==0 || d<0) {
    return -1.0;
  }

  s = sqrt(d);

  r1 = (-b + s) / (2 * a);
  r2 = (-b - s) / (2 * a);

  if (r1 >= 0 && r1 <= 1) {
    return r1;
  } else if (r2 >= 0 && r2 <= 1) {
    return r2;
  } else {
    return -1.0;
  }
}

real calc_cm(FILE *log,int natoms,real mass[],rvec x[],rvec v[],
	     rvec xcm,rvec vcm,rvec acm,matrix L)
{
  rvec dx,a0;
  real tm,m0;
  int  i,m;

  clear_rvec(xcm);
  clear_rvec(vcm);
  clear_rvec(acm);
  tm=0.0;
  for(i=0; (i<natoms); i++) {
    m0=mass[i];
    tm+=m0;
    cprod(x[i],v[i],a0);
    for(m=0; (m<DIM); m++) {
      xcm[m]+=m0*x[i][m]; /* c.o.m. position */
      vcm[m]+=m0*v[i][m]; /* c.o.m. velocity */
      acm[m]+=m0*a0[m];   /* rotational velocity around c.o.m. */
    }
  }
  cprod(xcm,vcm,a0);
  for(m=0; (m<DIM); m++) {
    xcm[m]/=tm;
    vcm[m]/=tm;
    acm[m]-=a0[m]/tm;
  }

#define PVEC(str,v) fprintf(log,\
			    "%s[X]: %10.5e  %s[Y]: %10.5e  %s[Z]: %10.5e\n", \
			   str,v[0],str,v[1],str,v[2])
#ifdef DEBUG
  PVEC("xcm",xcm);
  PVEC("acm",acm);
  PVEC("vcm",vcm);
#endif
  
  clear_mat(L);
  for(i=0; (i<natoms); i++) {
    m0=mass[i];
    for(m=0; (m<DIM); m++)
      dx[m]=x[i][m]-xcm[m];
    L[XX][XX]+=dx[XX]*dx[XX]*m0;
    L[XX][YY]+=dx[XX]*dx[YY]*m0;
    L[XX][ZZ]+=dx[XX]*dx[ZZ]*m0;
    L[YY][YY]+=dx[YY]*dx[YY]*m0;
    L[YY][ZZ]+=dx[YY]*dx[ZZ]*m0;
    L[ZZ][ZZ]+=dx[ZZ]*dx[ZZ]*m0;
  }
#ifdef DEBUG
  PVEC("L-x",L[XX]);
  PVEC("L-y",L[YY]);
  PVEC("L-z",L[ZZ]);
#endif

  return tm;
}

void calc_rotmatrix(rvec principal_axis, rvec targetvec, matrix rotmatrix)
{
	rvec rotvec;
	real ux,uy,uz,costheta,sintheta;
	
	costheta = cos_angle(principal_axis,targetvec);
	sintheta=sqrt(1.0-costheta*costheta); /* sign is always positive since 0<theta<pi */
                
	/* Determine rotation from cross product with target vector */
	cprod(principal_axis,targetvec,rotvec);
	unitv(rotvec,rotvec);
	printf("Aligning %g %g %g to %g %g %g : xprod  %g %g %g\n",
		principal_axis[XX],principal_axis[YY],principal_axis[ZZ],targetvec[XX],targetvec[YY],targetvec[ZZ],
		rotvec[XX],rotvec[YY],rotvec[ZZ]);
		
	ux=rotvec[XX]; 
	uy=rotvec[YY]; 
	uz=rotvec[ZZ]; 
	rotmatrix[0][0]=ux*ux + (1.0-ux*ux)*costheta;
	rotmatrix[0][1]=ux*uy*(1-costheta)-uz*sintheta;
	rotmatrix[0][2]=ux*uz*(1-costheta)+uy*sintheta;
	rotmatrix[1][0]=ux*uy*(1-costheta)+uz*sintheta;
	rotmatrix[1][1]=uy*uy + (1.0-uy*uy)*costheta;
	rotmatrix[1][2]=uy*uz*(1-costheta)-ux*sintheta;
	rotmatrix[2][0]=ux*uz*(1-costheta)-uy*sintheta;
	rotmatrix[2][1]=uy*uz*(1-costheta)+ux*sintheta;
	rotmatrix[2][2]=uz*uz + (1.0-uz*uz)*costheta;
	
	printf("Rotation matrix: \n%g %g %g\n%g %g %g\n%g %g %g\n",
		rotmatrix[0][0],rotmatrix[0][1],rotmatrix[0][2],
		rotmatrix[1][0],rotmatrix[1][1],rotmatrix[1][2],
		rotmatrix[2][0],rotmatrix[2][1],rotmatrix[2][2]);
}

/* Center of mass code for groups */
void calc_vcm_grp(int start, int homenr, t_mdatoms *md,
                  rvec x[], rvec v[], t_vcm *vcm)
{
    int    i, g, m;
    real   m0;
    rvec   j0;

    if (vcm->mode != ecmNO)
    {
        /* Also clear a possible rest group */
        for (g = 0; (g < vcm->nr+1); g++)
        {
            /* Reset linear momentum */
            vcm->group_mass[g] = 0;
            clear_rvec(vcm->group_p[g]);

            if (vcm->mode == ecmANGULAR)
            {
                /* Reset anular momentum */
                clear_rvec(vcm->group_j[g]);
                clear_rvec(vcm->group_x[g]);
                clear_rvec(vcm->group_w[g]);
                clear_mat(vcm->group_i[g]);
            }
        }

        g = 0;
        for (i = start; (i < start+homenr); i++)
        {
            m0 = md->massT[i];
            if (md->cVCM)
            {
                g = md->cVCM[i];
            }

            /* Calculate linear momentum */
            vcm->group_mass[g]  += m0;
            for (m = 0; (m < DIM); m++)
            {
                vcm->group_p[g][m] += m0*v[i][m];
            }

            if (vcm->mode == ecmANGULAR)
            {
                /* Calculate angular momentum */
                cprod(x[i], v[i], j0);

                for (m = 0; (m < DIM); m++)
                {
                    vcm->group_j[g][m] += m0*j0[m];
                    vcm->group_x[g][m] += m0*x[i][m];
                }
                /* Update inertia tensor */
                update_tensor(x[i], m0, vcm->group_i[g]);
            }
        }
    }
}

static void calculate_normal(atom_id index[],rvec x[],rvec result,rvec center)
{
  rvec c1,c2;
  int i;
  
  /* calculate centroid of triangle spanned by the three points */
  for(i=0;i<3;i++)
    center[i] = (x[index[0]][i] + x[index[1]][i] + x[index[2]][i])/3;
  
  /* use P1P2 x P1P3 to calculate normal, given three points P1-P3 */
  rvec_sub(x[index[1]],x[index[0]],c1);    /* find two vectors */
  rvec_sub(x[index[2]],x[index[0]],c2);
  
  cprod(c1,c2,result);                    /* take crossproduct between these */
}

void do_stopcm_grp(FILE *fp,int start,int homenr,unsigned short *group_id,
		   rvec x[],rvec v[],t_vcm *vcm)
{
  int  i,g,m;
  real tm,tm_1;
  rvec dv,dx;
  
  if (vcm->mode != ecmNO) {
    /* Subtract linear momentum */
    g = 0;
    switch (vcm->ndim) {
    case 1:
      for(i=start; (i<start+homenr); i++) {
	if (group_id)
	  g = group_id[i];
	v[i][XX] -= vcm->group_v[g][XX];
      }
      break;
    case 2:
      for(i=start; (i<start+homenr); i++) {
	if (group_id)
	  g = group_id[i];
	v[i][XX] -= vcm->group_v[g][XX];
	v[i][YY] -= vcm->group_v[g][YY];
      }
      break;
    case 3:
      for(i=start; (i<start+homenr); i++) {
	if (group_id)
	  g = group_id[i];
	rvec_dec(v[i],vcm->group_v[g]);
      }
      break;
    }
    if (vcm->mode == ecmANGULAR) {
      /* Subtract angular momentum */
      for(i=start; (i<start+homenr); i++) {
	if (group_id)
	  g = group_id[i];
	/* Compute the correction to the velocity for each atom */
	rvec_sub(x[i],vcm->group_x[g],dx);
	cprod(vcm->group_w[g],dx,dv);
	rvec_dec(v[i],dv);
      }
    }
  }
}

//! Helper method to calculate a vector from two or three positions.
static void
calc_vec(int natoms, rvec x[], t_pbc *pbc, rvec xout, rvec cout)
{
    switch (natoms)
    {
        case 2:
            if (pbc)
            {
                pbc_dx(pbc, x[1], x[0], xout);
            }
            else
            {
                rvec_sub(x[1], x[0], xout);
            }
            svmul(0.5, xout, cout);
            rvec_add(x[0], cout, cout);
            break;
        case 3:
        {
            rvec v1, v2;
            if (pbc)
            {
                pbc_dx(pbc, x[1], x[0], v1);
                pbc_dx(pbc, x[2], x[0], v2);
            }
            else
            {
                rvec_sub(x[1], x[0], v1);
                rvec_sub(x[2], x[0], v2);
            }
            cprod(v1, v2, xout);
            rvec_add(x[0], x[1], cout);
            rvec_add(cout, x[2], cout);
            svmul(1.0/3.0, cout, cout);
            break;
        }
        default:
            GMX_RELEASE_ASSERT(false, "Incorrectly initialized number of atoms");
    }
}

static void
calc_vec(int natoms, rvec x[], t_pbc *pbc, rvec xout, rvec cout)
{
    switch (natoms)
    {
        case 2:
            if (pbc)
            {
                pbc_dx(pbc, x[1], x[0], xout);
            }
            else
            {
                rvec_sub(x[1], x[0], xout);
            }
            svmul(0.5, xout, cout);
            rvec_add(x[0], cout, cout);
            break;
        case 3: {
            rvec v1, v2;
            if (pbc)
            {
                pbc_dx(pbc, x[1], x[0], v1);
                pbc_dx(pbc, x[2], x[0], v2);
            }
            else
            {
                rvec_sub(x[1], x[0], v1);
                rvec_sub(x[2], x[0], v2);
            }
            cprod(v1, v2, xout);
            rvec_add(x[0], x[1], cout);
            rvec_add(cout, x[2], cout);
            svmul(1.0/3.0, cout, cout);
            break;
        }
    }
}

//.........这里部分代码省略.........

    clear_rvecs(3, unitaxes);
    unitaxes[0][0] = 1;
    unitaxes[1][1] = 1;
    unitaxes[2][2] = 1;

    gpbc = gmx_rmpbc_init(&top->idef, ePBC, natoms);

    do
    {
        /* initialisation for correct distance calculations */
        set_pbc(&pbc, ePBC, box);
        /* make molecules whole again */
        gmx_rmpbc(gpbc, natoms, box, x);

        /* copy coords to our smaller arrays */
        for (i = 0; i < iCA; i++)
        {
            copy_rvec(x[ind_CA[i]], x_CA[i]);
            if (bSC)
            {
                copy_rvec(x[ind_SC[i]], x_SC[i]);
            }
        }

        for (i = 0; i < iCA-3; i++)
        {
            rvec_sub(x_CA[i+1], x_CA[i], r12[i]);
            rvec_sub(x_CA[i+2], x_CA[i+1], r23[i]);
            rvec_sub(x_CA[i+3], x_CA[i+2], r34[i]);
            rvec_sub(r12[i], r23[i], diff13[i]);
            rvec_sub(r23[i], r34[i], diff24[i]);
            /* calculate helix axis */
            cprod(diff13[i], diff24[i], helixaxis[i]);
            svmul(1.0/norm(helixaxis[i]), helixaxis[i], helixaxis[i]);

            tmp       = cos_angle(diff13[i], diff24[i]);
            twist[i]  = 180.0/M_PI * std::acos( tmp );
            radius[i] = std::sqrt( norm(diff13[i])*norm(diff24[i]) ) / (2.0* (1.0-tmp) );
            rise[i]   = std::abs(iprod(r23[i], helixaxis[i]));

            svmul(radius[i]/norm(diff13[i]), diff13[i], v1);
            svmul(radius[i]/norm(diff24[i]), diff24[i], v2);

            rvec_sub(x_CA[i+1], v1, residueorigin[i+1]);
            rvec_sub(x_CA[i+2], v2, residueorigin[i+2]);
        }
        residueradius[0] = residuetwist[0] = residuerise[0] = 0;

        residueradius[1] = radius[0];
        residuetwist[1]  = twist[0];
        residuerise[1]   = rise[0];

        residuebending[0] = residuebending[1] = 0;
        for (i = 2; i < iCA-2; i++)
        {
            residueradius[i]  = 0.5*(radius[i-2]+radius[i-1]);
            residuetwist[i]   = 0.5*(twist[i-2]+twist[i-1]);
            residuerise[i]    = 0.5*(rise[i-2]+rise[i-1]);
            residuebending[i] = 180.0/M_PI*std::acos( cos_angle(helixaxis[i-2], helixaxis[i-1]) );
        }
        residueradius[iCA-2]  = radius[iCA-4];
        residuetwist[iCA-2]   = twist[iCA-4];
        residuerise[iCA-2]    = rise[iCA-4];
        residueradius[iCA-1]  = residuetwist[iCA-1] = residuerise[iCA-1] = 0;
        residuebending[iCA-2] = residuebending[iCA-1] = 0;

//.........这里部分代码省略.........
		      output_env_get_xvgr_tlabel(oenv),"(degrees)",oenv);
  }

  if (opt2bSet("-oa",NFILE,fnm)) {
    init_t_atoms(&outatoms,3*n,FALSE);
    outatoms.nr = 3*n;
    for(i=0; i<3*n; i++) {
      outatoms.atomname[i] = &anm;
      outatoms.atom[i].resind = i/3;
      outatoms.resinfo[i/3].name = &rnm;
      outatoms.resinfo[i/3].nr   = i/3 + 1;
      outatoms.resinfo[i/3].ic   = ' ';
    }
    fpdb = open_trx(opt2fn("-oa",NFILE,fnm),"w");
  } else
    fpdb = NULL;
  
  read_first_frame(oenv,&status,ftp2fn(efTRX,NFILE,fnm),&fr,TRX_NEED_X); 
  gpbc = gmx_rmpbc_init(&top.idef,ePBC,fr.natoms,fr.box);

  do {
    gmx_rmpbc_trxfr(gpbc,&fr);
    calc_axes(fr.x,top.atoms.atom,gnx,index,!bZ,&bun);
    t = output_env_conv_time(oenv,fr.time);
    fprintf(flen," %10g",t);
    fprintf(fdist," %10g",t);
    fprintf(fz," %10g",t);
    fprintf(ftilt," %10g",t);
    fprintf(ftiltr," %10g",t);
    fprintf(ftiltl," %10g",t);
    if (bKink) {
      fprintf(fkink," %10g",t);
      fprintf(fkinkr," %10g",t);
      fprintf(fkinkl," %10g",t);
    }

    for(i=0; i<bun.n; i++) {
      fprintf(flen," %6g",bun.len[i]);
      fprintf(fdist," %6g",norm(bun.mid[i]));
      fprintf(fz," %6g",bun.mid[i][ZZ]);
      fprintf(ftilt," %6g",RAD2DEG*acos(bun.dir[i][ZZ]));
      comp = bun.mid[i][XX]*bun.dir[i][XX]+bun.mid[i][YY]*bun.dir[i][YY];
      fprintf(ftiltr," %6g",RAD2DEG*
	      asin(comp/sqrt(sqr(comp)+sqr(bun.dir[i][ZZ]))));
      comp = bun.mid[i][YY]*bun.dir[i][XX]-bun.mid[i][XX]*bun.dir[i][YY];
      fprintf(ftiltl," %6g",RAD2DEG*
	      asin(comp/sqrt(sqr(comp)+sqr(bun.dir[i][ZZ]))));
      if (bKink) {
	rvec_sub(bun.end[0][i],bun.end[2][i],va);
	rvec_sub(bun.end[2][i],bun.end[1][i],vb);
	unitv_no_table(va,va);
	unitv_no_table(vb,vb);
	fprintf(fkink," %6g",RAD2DEG*acos(iprod(va,vb)));
	cprod(va,vb,vc);
	copy_rvec(bun.mid[i],vr);
	vr[ZZ] = 0;
	unitv_no_table(vr,vr);
	fprintf(fkinkr," %6g",RAD2DEG*asin(iprod(vc,vr)));
	vl[XX] = vr[YY];
	vl[YY] = -vr[XX];
	vl[ZZ] = 0;
	fprintf(fkinkl," %6g",RAD2DEG*asin(iprod(vc,vl)));
      }
    }
    fprintf(flen,"\n");
    fprintf(fdist,"\n");
    fprintf(fz,"\n");
    fprintf(ftilt,"\n");
    fprintf(ftiltr,"\n");
    fprintf(ftiltl,"\n");
    if (bKink) {
      fprintf(fkink,"\n");
      fprintf(fkinkr,"\n");
      fprintf(fkinkl,"\n");
    }
    if (fpdb )
      dump_axes(fpdb,&fr,&outatoms,&bun);
  } while(read_next_frame(oenv,status,&fr));
  gmx_rmpbc_done(gpbc);

  close_trx(status);
  
  if (fpdb )
    close_trx(fpdb);
  ffclose(flen);
  ffclose(fdist);
  ffclose(fz);
  ffclose(ftilt);
  ffclose(ftiltr);
  ffclose(ftiltl);
  if (bKink) {
    ffclose(fkink);
    ffclose(fkinkr);
    ffclose(fkinkl);
  }
  
  thanx(stderr);
  
  return 0;
}

static void set_tric_dir(ivec *dd_nc, gmx_ddbox_t *ddbox, matrix box)
{
    int   npbcdim, d, i, j;
    rvec *v, *normal;
    real  dep, inv_skew_fac2;

    npbcdim = ddbox->npbcdim;
    normal  = ddbox->normal;
    for (d = 0; d < DIM; d++)
    {
        ddbox->tric_dir[d] = 0;
        for (j = d+1; j < npbcdim; j++)
        {
            if (box[j][d] != 0)
            {
                ddbox->tric_dir[d] = 1;
                if (dd_nc != NULL && (*dd_nc)[j] > 1 && (*dd_nc)[d] == 1)
                {
                    gmx_fatal(FARGS, "Domain decomposition has not been implemented for box vectors that have non-zero components in directions that do not use domain decomposition: ncells = %d %d %d, box vector[%d] = %f %f %f",
                              dd_nc[XX], dd_nc[YY], dd_nc[ZZ],
                              j+1, box[j][XX], box[j][YY], box[j][ZZ]);
                }
            }
        }

        /* Convert box vectors to orthogonal vectors for this dimension,
         * for use in distance calculations.
         * Set the trilinic skewing factor that translates
         * the thickness of a slab perpendicular to this dimension
         * into the real thickness of the slab.
         */
        if (ddbox->tric_dir[d])
        {
            inv_skew_fac2 = 1;
            v             = ddbox->v[d];
            if (d == XX || d == YY)
            {
                /* Normalize such that the "diagonal" is 1 */
                svmul(1/box[d+1][d+1], box[d+1], v[d+1]);
                for (i = 0; i < d; i++)
                {
                    v[d+1][i] = 0;
                }
                inv_skew_fac2 += sqr(v[d+1][d]);
                if (d == XX)
                {
                    /* Normalize such that the "diagonal" is 1 */
                    svmul(1/box[d+2][d+2], box[d+2], v[d+2]);
                    for (i = 0; i < d; i++)
                    {
                        v[d+2][i] = 0;
                    }
                    /* Make vector [d+2] perpendicular to vector [d+1],
                     * this does not affect the normalization.
                     */
                    dep = iprod(v[d+1], v[d+2])/norm2(v[d+1]);
                    for (i = 0; i < DIM; i++)
                    {
                        v[d+2][i] -= dep*v[d+1][i];
                    }
                    inv_skew_fac2 += sqr(v[d+2][d]);

                    cprod(v[d+1], v[d+2], normal[d]);
                }
                else
                {
                    /* cross product with (1,0,0) */
                    normal[d][XX] =  0;
                    normal[d][YY] =  v[d+1][ZZ];
                    normal[d][ZZ] = -v[d+1][YY];
                }
                if (debug)
                {
                    fprintf(debug, "box[%d]  %.3f %.3f %.3f\n",
                            d, box[d][XX], box[d][YY], box[d][ZZ]);
                    for (i = d+1; i < DIM; i++)
                    {
                        fprintf(debug, "  v[%d]  %.3f %.3f %.3f\n",
                                i, v[i][XX], v[i][YY], v[i][ZZ]);
                    }
                }
            }
            ddbox->skew_fac[d] = 1.0/sqrt(inv_skew_fac2);
            /* Set the normal vector length to skew_fac */
            dep = ddbox->skew_fac[d]/norm(normal[d]);
            svmul(dep, normal[d], normal[d]);

            if (debug)
            {
                fprintf(debug, "skew_fac[%d] = %f\n", d, ddbox->skew_fac[d]);
                fprintf(debug, "normal[%d]  %.3f %.3f %.3f\n",
                        d, normal[d][XX], normal[d][YY], normal[d][ZZ]);
            }
        }
        else
        {
            ddbox->skew_fac[d] = 1;

            for (i = 0; i < DIM; i++)
            {
//.........这里部分代码省略.........

//.........这里部分代码省略.........
        for (p = 0; (p < nrefgrp); p++)
        {
            if (bCom)
            {
                calc_com_pbc(nrefat, &top, x, &pbc, index[0], xref, bPBC);
            }
            else
            {
                copy_rvec(x[index[0][p]], xref);
            }

            for (m = 0; m < isize[1]; m += 3)
            {
                sa0 = index[1][m];
                sa1 = index[1][m+1];
                sa2 = index[1][m+2];
                range_check(sa0, 0, natoms);
                range_check(sa1, 0, natoms);
                range_check(sa2, 0, natoms);
                pbc_dx(&pbc, x[sa0], xref, dx);
                r2  = norm2(dx);
                if (r2 < rcut2)
                {
                    r = std::sqrt(r2);
                    if (!bVec23)
                    {
                        /* Determine the normal to the plain */
                        rvec_sub(x[sa1], x[sa0], dxh1);
                        rvec_sub(x[sa2], x[sa0], dxh2);
                        rvec_inc(dxh1, dxh2);
                        svmul(1/r, dx, dx);
                        unitv(dxh1, dxh1);
                        inp = iprod(dx, dxh1);
                        cprod(dxh1, dxh2, outer);
                        unitv(outer, outer);
                        outp = iprod(dx, outer);
                    }
                    else
                    {
                        /* Use the vector between the 2nd and 3rd atom */
                        rvec_sub(x[sa2], x[sa1], dxh2);
                        unitv(dxh2, dxh2);
                        outp = iprod(dx, dxh2)/r;
                    }
                    {
                        int ii = static_cast<int>(invrbw*r);
                        range_check(ii, 0, nrbin);
                        histi1[ii] += inp;
                        histi2[ii] += 3*sqr(outp) - 1;
                        histn[ii]++;
                    }
                    if ((r2 >= rmin2) && (r2 < rmax2))
                    {
                        int ii1 = static_cast<int>(invbw*(inp + 1));
                        int ii2 = static_cast<int>(invbw*std::abs(outp));

                        range_check(ii1, 0, nbin1);
                        range_check(ii2, 0, nbin2);
                        hist1[ii1]++;
                        hist2[ii2]++;
                        sum1 += inp;
                        sum2 += outp;
                        n++;
                    }
                }
            }

static void list_trn(char *fn)
{
  static real mass[5] = { 15.9994, 1.008, 1.008, 0.0, 0.0 };
  int         i,j=0,m,fpread,fpwrite,nframe;
  rvec        *x,*v,*f,fmol[2],xcm[2],torque[j],dx;
  real        mmm,len;
  matrix      box;
  t_trnheader trn;
  gmx_bool        bOK;

  printf("Going to open %s\n",fn);
  fpread  = open_trn(fn,"r"); 
  fpwrite = open_tpx(NULL,"w");
  gmx_fio_setdebug(fpwrite,TRUE);
  
  mmm=mass[0]+2*mass[1];
  for(i=0; (i<5); i++) 
    mass[i] /= mmm;
  
  nframe = 0;
  while (fread_trnheader(fpread,&trn,&bOK)) {
    snew(x,trn.natoms);
    snew(v,trn.natoms);
    snew(f,trn.natoms);
    if (fread_htrn(fpread,&trn,
		   trn.box_size ? box : NULL,
		   trn.x_size   ? x : NULL,
		   trn.v_size   ? v : NULL,
		   trn.f_size   ? f : NULL)) {
		   
      if (trn.x_size && trn.f_size) {
	printf("There are %d atoms\n",trn.natoms);
	for(j=0; (j<2); j++) {
	  clear_rvec(xcm[j]);
	  clear_rvec(fmol[j]);
	  clear_rvec(torque[j]);
	  for(i=5*j; (i<5*j+5); i++) {
	    rvec_inc(fmol[j],f[i]);
	    for(m=0; (m<DIM); m++)
	      xcm[j][m] += mass[i%5]*x[i][m];
	  }
	  for(i=5*j; (i<5*j+5); i++) {
	    rvec_dec(x[i],xcm[j]);
	    cprod(x[i],f[i],dx);
	    rvec_inc(torque[j],dx);
	    rvec_inc(x[i],xcm[j]);
	  }
	}
	pr_rvecs(stdout,0,"FMOL  ",fmol,2);
	pr_rvecs(stdout,0,"TORQUE",torque,2);
	printf("Distance matrix Water1-Water2\n%5s","");
	for(j=0; (j<5); j++) 
	  printf("  %10s",nm[j]);
	printf("\n");
	for(j=0; (j<5); j++) {
	  printf("%5s",nm[j]);
	  for(i=5; (i<10); i++) {
	    rvec_sub(x[i],x[j],dx);
	    len = sqrt(iprod(dx,dx));
	    printf("  %10.7f",len);
	  }
	  printf("\n");
	}
      }
    }
    sfree(x);
    sfree(v);
    sfree(f);
    nframe++;
  }
  if (!bOK)
    fprintf(stderr,"\nWARNING: Incomplete frame header: nr %d, t=%g\n",
	    nframe,trn.t);
  close_tpx(fpwrite);
  close_trn(fpread);
}

//.........这里部分代码省略.........
                    /* first get Sz, the vector from Cn to Cn+1 */
                    rvec_sub(x1[a[index[i+1]+j]], x1[a[index[i]+j]], dist);
                    length = norm(dist);
                    check_length(length, a[index[i]+j], a[index[i+1]+j]);
                    svmul(1.0/length, dist, Sz);

                    /* this is actually the cosine of the angle between the double bond
                       and axis, because Sz is normalized and the two other components of
                       the axis on the bilayer are zero */
                    if (use_unitvector)
                    {
                        sdbangle += gmx_angle(direction, Sz); /*this can probably be optimized*/
                    }
                    else
                    {
                        sdbangle += std::acos(Sz[axis]);
                    }
                }
                else
                {
                    /* get vector dist(Cn-1,Cn+1) for tail atoms */
                    rvec_sub(x1[a[index[i+1]+j]], x1[a[index[i-1]+j]], dist);
                    length = norm(dist); /* determine distance between two atoms */
                    check_length(length, a[index[i-1]+j], a[index[i+1]+j]);

                    svmul(1.0/length, dist, Sz);
                    /* Sz is now the molecular axis Sz, normalized and all that */
                }

                /* now get Sx. Sx is normal to the plane of Cn-1, Cn and Cn+1 so
                   we can use the outer product of Cn-1->Cn and Cn+1->Cn, I hope */
                rvec_sub(x1[a[index[i+1]+j]], x1[a[index[i]+j]], tmp1);
                rvec_sub(x1[a[index[i-1]+j]], x1[a[index[i]+j]], tmp2);
                cprod(tmp1, tmp2, Sx);
                svmul(1.0/norm(Sx), Sx, Sx);

                /* now we can get Sy from the outer product of Sx and Sz   */
                cprod(Sz, Sx, Sy);
                svmul(1.0/norm(Sy), Sy, Sy);

                /* the square of cosine of the angle between dist and the axis.
                   Using the innerproduct, but two of the three elements are zero
                   Determine the sum of the orderparameter of all atoms in group
                 */
                if (use_unitvector)
                {
                    cossum[XX] = sqr(iprod(Sx, direction)); /* this is allowed, since Sa is normalized */
                    cossum[YY] = sqr(iprod(Sy, direction));
                    cossum[ZZ] = sqr(iprod(Sz, direction));
                }
                else
                {
                    cossum[XX] = sqr(Sx[axis]); /* this is allowed, since Sa is normalized */
                    cossum[YY] = sqr(Sy[axis]);
                    cossum[ZZ] = sqr(Sz[axis]);
                }

                for (m = 0; m < DIM; m++)
                {
                    frameorder[m] += 0.5 * (3.0 * cossum[m] - 1.0);
                }

                if (bSliced)
                {
                    /* get average coordinate in box length for slicing,
                       determine which slice atom is in, increase count for that

void
Angle::analyzeFrame(int frnr, const t_trxframe &fr, t_pbc *pbc,
                    TrajectoryAnalysisModuleData *pdata)
{
    AnalysisDataHandle *dh = pdata->dataHandle("angle");
    std::vector<Selection *> sel1 = pdata->parallelSelections(_sel1);
    std::vector<Selection *> sel2 = pdata->parallelSelections(_sel2);

    checkSelections(sel1, sel2);

    rvec  v1, v2;
    rvec  c1, c2;
    switch (_g2type[0])
    {
        case 'z':
            clear_rvec(v2);
            v2[ZZ] = 1.0;
            clear_rvec(c2);
            break;
        case 's':
            copy_rvec(_sel2[0]->x(0), c2);
            break;
    }

    dh->startFrame(frnr, fr.time);

    int incr1 = _bSplit1 ? 1 : _natoms1;
    int incr2 = _bSplit2 ? 1 : _natoms2;
    int ngrps = _bMulti ? _sel1.size() : 1;

    for (int g = 0; g < ngrps; ++g)
    {
        real ave = 0.0;
        int n = 0;
        int i, j;
        for (i = j = 0; i < sel1[g]->posCount(); i += incr1)
        {
            rvec x[4];
            real angle;
            copy_pos(sel1, _bSplit1, _natoms1, g, i, x);
            switch (_g1type[0])
            {
                case 'a':
                    if (pbc)
                    {
                        pbc_dx(pbc, x[0], x[1], v1);
                        pbc_dx(pbc, x[2], x[1], v2);
                    }
                    else
                    {
                        rvec_sub(x[0], x[1], v1);
                        rvec_sub(x[2], x[1], v2);
                    }
                    angle = gmx_angle(v1, v2);
                    break;
                case 'd': {
                    rvec dx[3];
                    if (pbc)
                    {
                        pbc_dx(pbc, x[0], x[1], dx[0]);
                        pbc_dx(pbc, x[2], x[1], dx[1]);
                        pbc_dx(pbc, x[2], x[3], dx[2]);
                    }
                    else
                    {
                        rvec_sub(x[0], x[1], dx[0]);
                        rvec_sub(x[2], x[1], dx[1]);
                        rvec_sub(x[2], x[3], dx[2]);
                    }
                    cprod(dx[0], dx[1], v1);
                    cprod(dx[1], dx[2], v2);
                    angle = gmx_angle(v1, v2);
                    real ipr = iprod(dx[0], v2);
                    if (ipr < 0)
                    {
                        angle = -angle;
                    }
                    break;
                }
                case 'v':
                case 'p':
                    calc_vec(_natoms1, x, pbc, v1, c1);
                    switch (_g2type[0])
                    {
                        case 'v':
                        case 'p':
                            copy_pos(sel2, _bSplit2, _natoms2, 0, j, x);
                            calc_vec(_natoms2, x, pbc, v2, c2);
                            j += incr2;
                            break;
                        case 't':
                            // FIXME: This is not parallelizable.
                            if (frnr == 0)
                            {
                                copy_rvec(v1, _vt0[n]);
                            }
                            copy_rvec(_vt0[n], v2);
                            break;
                        case 'z':
                            c1[XX] = c1[YY] = 0.0;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    { efNDX, NULL, NULL,  ffREAD  },
    { efXVG, "-o", "rotacf",  ffWRITE }
  };
#define NFILE asize(fnm)
  int     npargs;
  t_pargs *ppa;
  
  CopyRight(stderr,argv[0]);
  npargs = asize(pa);
  ppa    = add_acf_pargs(&npargs,pa);
  
  parse_common_args(&argc,argv,PCA_CAN_VIEW | PCA_CAN_TIME | PCA_BE_NICE,
		    NFILE,fnm,npargs,ppa,asize(desc),desc,0,NULL);
  
  rd_index(ftp2fn(efNDX,NFILE,fnm),1,&isize,&index,&grpname);
  
  if (bVec) 
    nvec = isize/2;
  else
    nvec = isize/3;
  
  if (((isize % 3) != 0) && !bVec)
    gmx_fatal(FARGS,"number of index elements not multiple of 3, "
		"these can not be atom triplets\n");
  if (((isize % 2) != 0) && bVec)
    gmx_fatal(FARGS,"number of index elements not multiple of 2, "
		"these can not be atom doublets\n");
  
  top=read_top(ftp2fn(efTPX,NFILE,fnm),&ePBC);
  
  snew(c1,nvec);
  for (i=0; (i<nvec); i++)
    c1[i]=NULL;
  n_alloc=0;

  natoms=read_first_x(&status,ftp2fn(efTRX,NFILE,fnm),&t,&x,box);
  snew(x_s,natoms);
  
  /* Start the loop over frames */
  t1 = t0 = t;
  teller  = 0;
  do {
    if (teller >= n_alloc) {
      n_alloc+=100;
      for (i=0; (i<nvec); i++)
	srenew(c1[i],DIM*n_alloc);
    }
    t1 = t;
    
    /* Remove periodicity */
    rm_pbc(&(top->idef),ePBC,natoms,box,x,x_s);
    
    /* Compute crossproducts for all vectors, if triplets.
     * else, just get the vectors in case of doublets.
     */
    if (bVec == FALSE) {
      for (i=0; (i<nvec); i++) {
	ai=index[3*i];
	aj=index[3*i+1];
	ak=index[3*i+2];
	rvec_sub(x_s[ai],x_s[aj],xij);
	rvec_sub(x_s[aj],x_s[ak],xjk);
	cprod(xij,xjk,n);
	for(m=0; (m<DIM); m++)
	  c1[i][DIM*teller+m]=n[m];
      }
    }
    else {
      for (i=0; (i<nvec); i++) {
	ai=index[2*i];
	aj=index[2*i+1];
	rvec_sub(x_s[ai],x_s[aj],n);
	for(m=0; (m<DIM); m++)
	  c1[i][DIM*teller+m]=n[m];
      }
    }
    /* Increment loop counter */
    teller++;
  } while (read_next_x(status,&t,natoms,x,box));  
  close_trj(status); 
  fprintf(stderr,"\nDone with trajectory\n");
  
  /* Autocorrelation function */
  if (teller < 2)
    fprintf(stderr,"Not enough frames for correlation function\n");
  else {
    dt=(t1 - t0)/(teller-1);
    
    mode = eacVector;
    
    do_autocorr(ftp2fn(efXVG,NFILE,fnm),"Rotational Correlation Function",
		teller,nvec,c1,dt,mode,bAver);
  }

  do_view(ftp2fn(efXVG,NFILE,fnm),NULL);
    
  thanx(stderr);
    
  return 0;
}

static void do_ac_core(int nframes,int nout,
		       real corr[],real c1[],int nrestart,
		       unsigned long mode)
{
  int     j,k,j3,jk3,m,n;
  real    ccc,cth;
  rvec    xj,xk,rr;

  if (nrestart < 1) {
    printf("WARNING: setting number of restarts to 1\n");
    nrestart = 1;
  }
  if (debug)
    fprintf(debug,
	    "Starting do_ac_core: nframes=%d, nout=%d, nrestart=%d,mode=%lu\n",
	    nframes,nout,nrestart,mode);
  
  for(j=0; (j<nout); j++)
    corr[j]=0;
  
  /* Loop over starting points. */
  for(j=0; (j<nframes); j+=nrestart) {
    j3  = DIM*j;
    
    /* Loop over the correlation length for this starting point */
    for(k=0; (k<nout) && (j+k < nframes); k++) {
      jk3 = DIM*(j+k);
      
      /* Switch over possible ACF types. 
       * It might be more efficient to put the loops inside the switch,
       * but this is more clear, and save development time!
       */      
      if (MODE(eacNormal)) {
	corr[k] += c1[j]*c1[j+k];
      }
      else if (MODE(eacCos)) {
	/* Compute the cos (phi(t)-phi(t+dt)) */
	corr[k] += cos(c1[j]-c1[j+k]);
      }
      else if (MODE(eacIden)) {
	/* Check equality (phi(t)==phi(t+dt)) */
	corr[k] += (c1[j]==c1[j+k])? 1 : 0;
      }
      else if (MODE(eacP1) || MODE(eacP2) || MODE(eacP3)) {
	for(m=0; (m<DIM); m++) {
	  xj[m] = c1[j3+m];
	  xk[m] = c1[jk3+m];
	}
	cth=cos_angle(xj,xk);
	
	if (cth-1.0 > 1.0e-15) {
	  printf("j: %d, k: %d, xj:(%g,%g,%g), xk:(%g,%g,%g)\n",
		  j,k,xj[XX],xj[YY],xj[ZZ],xk[XX],xk[YY],xk[ZZ]);
	}
	
	corr[k] += LegendreP(cth,mode);  /* 1.5*cth*cth-0.5; */
      }
      else if (MODE(eacRcross)) {
	for(m=0; (m<DIM); m++) {
	  xj[m] = c1[j3+m];
	  xk[m] = c1[jk3+m];
	}
	cprod(xj,xk,rr);
	
	corr[k] += iprod(rr,rr);
      }
      else if (MODE(eacVector)) {
	for(m=0; (m<DIM); m++) {
	  xj[m] = c1[j3+m];
	  xk[m] = c1[jk3+m];
	}
	ccc = iprod(xj,xk);
	
	corr[k] += ccc;
      }
      else
	gmx_fatal(FARGS,"\nInvalid mode (%d) in do_ac_core",mode);
    }
  }
  /* Correct for the number of points and copy results to the data array */
  for(j=0; (j<nout); j++) {
    n = (nframes-j+(nrestart-1))/nrestart;
    c1[j] = corr[j]/n;
  }
}

void check_cm_grp(FILE *fp, t_vcm *vcm, t_inputrec *ir, real Temp_Max)
{
    int    m, g;
    real   ekcm, ekrot, tm, tm_1, Temp_cm;
    rvec   jcm;
    tensor Icm;

    /* First analyse the total results */
    if (vcm->mode != ecmNO)
    {
        for (g = 0; (g < vcm->nr); g++)
        {
            tm = vcm->group_mass[g];
            if (tm != 0)
            {
                tm_1 = 1.0/tm;
                svmul(tm_1, vcm->group_p[g], vcm->group_v[g]);
            }
            /* Else it's zero anyway! */
        }
        if (vcm->mode == ecmANGULAR)
        {
            for (g = 0; (g < vcm->nr); g++)
            {
                tm = vcm->group_mass[g];
                if (tm != 0)
                {
                    tm_1 = 1.0/tm;

                    /* Compute center of mass for this group */
                    for (m = 0; (m < DIM); m++)
                    {
                        vcm->group_x[g][m] *= tm_1;
                    }

                    /* Subtract the center of mass contribution to the
                     * angular momentum
                     */
                    cprod(vcm->group_x[g], vcm->group_v[g], jcm);
                    for (m = 0; (m < DIM); m++)
                    {
                        vcm->group_j[g][m] -= tm*jcm[m];
                    }

                    /* Subtract the center of mass contribution from the inertia
                     * tensor (this is not as trivial as it seems, but due to
                     * some cancellation we can still do it, even in parallel).
                     */
                    clear_mat(Icm);
                    update_tensor(vcm->group_x[g], tm, Icm);
                    m_sub(vcm->group_i[g], Icm, vcm->group_i[g]);

                    /* Compute angular velocity, using matrix operation
                     * Since J = I w
                     * we have
                     * w = I^-1 J
                     */
                    get_minv(vcm->group_i[g], Icm);
                    mvmul(Icm, vcm->group_j[g], vcm->group_w[g]);
                }
                /* Else it's zero anyway! */
            }
        }
    }
    for (g = 0; (g < vcm->nr); g++)
    {
        ekcm    = 0;
        if (vcm->group_mass[g] != 0 && vcm->group_ndf[g] > 0)
        {
            for (m = 0; m < vcm->ndim; m++)
            {
                ekcm += sqr(vcm->group_v[g][m]);
            }
            ekcm   *= 0.5*vcm->group_mass[g];
            Temp_cm = 2*ekcm/vcm->group_ndf[g];

            if ((Temp_cm > Temp_Max) && fp)
            {
                fprintf(fp, "Large VCM(group %s): %12.5f, %12.5f, %12.5f, Temp-cm: %12.5e\n",
                        vcm->group_name[g], vcm->group_v[g][XX],
                        vcm->group_v[g][YY], vcm->group_v[g][ZZ], Temp_cm);
            }

            if (vcm->mode == ecmANGULAR)
            {
                ekrot = 0.5*iprod(vcm->group_j[g], vcm->group_w[g]);
                if ((ekrot > 1) && fp && !EI_RANDOM(ir->eI))
                {
                    /* if we have an integrator that may not conserve momenta, skip */
                    tm    = vcm->group_mass[g];
                    fprintf(fp, "Group %s with mass %12.5e, Ekrot %12.5e Det(I) = %12.5e\n",
                            vcm->group_name[g], tm, ekrot, det(vcm->group_i[g]));
                    fprintf(fp, "  COM: %12.5f  %12.5f  %12.5f\n",
                            vcm->group_x[g][XX], vcm->group_x[g][YY], vcm->group_x[g][ZZ]);
                    fprintf(fp, "  P:   %12.5f  %12.5f  %12.5f\n",
                            vcm->group_p[g][XX], vcm->group_p[g][YY], vcm->group_p[g][ZZ]);
                    fprintf(fp, "  V:   %12.5f  %12.5f  %12.5f\n",
                            vcm->group_v[g][XX], vcm->group_v[g][YY], vcm->group_v[g][ZZ]);
                    fprintf(fp, "  J:   %12.5f  %12.5f  %12.5f\n",
                            vcm->group_j[g][XX], vcm->group_j[g][YY], vcm->group_j[g][ZZ]);
//.........这里部分代码省略.........