int gmx_dos(int argc, char *argv[])
{
    const char         *desc[] = {
        "[TT]g_dos[tt] computes the Density of States from a simulations.",
        "In order for this to be meaningful the velocities must be saved",
        "in the trajecotry with sufficiently high frequency such as to cover",
        "all vibrations. For flexible systems that would be around a few fs",
        "between saving. Properties based on the DoS are printed on the",
        "standard output."
    };
    const char         *bugs[] = {
        "This program needs a lot of memory: total usage equals the number of atoms times 3 times number of frames times 4 (or 8 when run in double precision)."
    };
    FILE               *fp, *fplog;
    t_topology          top;
    int                 ePBC = -1;
    t_trxframe          fr;
    matrix              box;
    int                 gnx;
    char                title[256];
    real                t0, t1, m;
    t_trxstatus        *status;
    int                 nV, nframes, n_alloc, i, j, k, l, fftcode, Nmol, Natom;
    double              rho, dt, V2sum, Vsum, V, tmass, dostot, dos2, dosabs;
    real              **c1, **dos, mi, beta, bfac, *nu, *tt, stddev, c1j;
    output_env_t        oenv;
    gmx_fft_t           fft;
    double              cP, S, A, E, DiffCoeff, Delta, f, y, z, sigHS, Shs, Sig, DoS0, recip_fac;
    double              wCdiff, wSdiff, wAdiff, wEdiff;

    static     gmx_bool bVerbose = TRUE, bAbsolute = FALSE, bNormalize = FALSE;
    static     gmx_bool bRecip   = FALSE, bDump = FALSE;
    static     real     Temp     = 298.15, toler = 1e-6;
    t_pargs             pa[]     = {
        { "-v", FALSE, etBOOL, {&bVerbose},
          "Be loud and noisy." },
        { "-recip", FALSE, etBOOL, {&bRecip},
          "Use cm^-1 on X-axis instead of 1/ps for DoS plots." },
        { "-abs", FALSE, etBOOL, {&bAbsolute},
          "Use the absolute value of the Fourier transform of the VACF as the Density of States. Default is to use the real component only" },
        { "-normdos", FALSE, etBOOL, {&bNormalize},
          "Normalize the DoS such that it adds up to 3N. This is a hack that should not be necessary." },
        { "-T", FALSE, etREAL, {&Temp},
          "Temperature in the simulation" },
        { "-toler", FALSE, etREAL, {&toler},
          "[HIDDEN]Tolerance when computing the fluidicity using bisection algorithm" },
        { "-dump", FALSE, etBOOL, {&bDump},
          "[HIDDEN]Dump the y/fy plot corresponding to Fig. 2 inLin2003a and the and the weighting functions corresponding to Fig. 1 in Berens1983a." }
    };

    t_filenm            fnm[] = {
        { efTRN, "-f",    NULL,    ffREAD  },
        { efTPX, "-s",    NULL,    ffREAD  },
        { efNDX, NULL,    NULL,    ffOPTRD },
        { efXVG, "-vacf", "vacf",  ffWRITE },
        { efXVG, "-mvacf", "mvacf", ffWRITE },
        { efXVG, "-dos",  "dos",   ffWRITE },
        { efLOG, "-g",    "dos",   ffWRITE },
    };
#define NFILE asize(fnm)
    int                 npargs;
    t_pargs            *ppa;
    const char         *DoSlegend[] = {
        "DoS(v)", "DoS(v)[Solid]", "DoS(v)[Diff]"
    };

    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,
                      asize(bugs), bugs, &oenv);

    beta = 1/(Temp*BOLTZ);
    if (bDump)
    {
        printf("Dumping reference figures. Thanks for your patience.\n");
        dump_fy(oenv, toler);
        dump_w(oenv, beta);
        exit(0);
    }

    fplog = gmx_fio_fopen(ftp2fn(efLOG, NFILE, fnm), "w");
    fprintf(fplog, "Doing density of states analysis based on trajectory.\n");
    please_cite(fplog, "Pascal2011a");
    please_cite(fplog, "Caleman2011b");

    read_tps_conf(ftp2fn(efTPX, NFILE, fnm), title, &top, &ePBC, NULL, NULL, box,
                  TRUE);
    V     = det(box);
    tmass = 0;
    for (i = 0; (i < top.atoms.nr); i++)
    {
        tmass += top.atoms.atom[i].m;
    }

    Natom = top.atoms.nr;
    Nmol  = top.mols.nr;
    gnx   = Natom*DIM;

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

int gmx_pme_error(int argc, char *argv[])
{
    const char     *desc[] = {
        "[THISMODULE] estimates the error of the electrostatic forces",
        "if using the sPME algorithm. The flag [TT]-tune[tt] will determine",
        "the splitting parameter such that the error is equally",
        "distributed over the real and reciprocal space part.",
        "The part of the error that stems from self interaction of the particles "
        "is computationally demanding. However, a good a approximation is to",
        "just use a fraction of the particles for this term which can be",
        "indicated by the flag [TT]-self[tt].[PAR]",
    };

    real            fs        = 0.0; /* 0 indicates: not set by the user */
    real            user_beta = -1.0;
    real            fracself  = 1.0;
    t_inputinfo     info;
    t_state         state;     /* The state from the tpr input file */
    gmx_mtop_t      mtop;      /* The topology from the tpr input file */
    t_inputrec     *ir = NULL; /* The inputrec from the tpr file */
    FILE           *fp = NULL;
    t_commrec      *cr;
    unsigned long   PCA_Flags;
    gmx_bool        bTUNE    = FALSE;
    gmx_bool        bVerbose = FALSE;
    int             seed     = 0;


    static t_filenm fnm[] = {
        { efTPX, "-s",     NULL,    ffREAD },
        { efOUT, "-o",    "error",  ffWRITE },
        { efTPX, "-so",   "tuned",  ffOPTWR }
    };

    output_env_t    oenv = NULL;

    t_pargs         pa[] = {
        { "-beta",     FALSE, etREAL, {&user_beta},
          "If positive, overwrite ewald_beta from [TT].tpr[tt] file with this value" },
        { "-tune",     FALSE, etBOOL, {&bTUNE},
          "Tune the splitting parameter such that the error is equally distributed between real and reciprocal space" },
        { "-self",     FALSE, etREAL, {&fracself},
          "If between 0.0 and 1.0, determine self interaction error from just this fraction of the charged particles" },
        { "-seed",     FALSE, etINT,  {&seed},
          "Random number seed used for Monte Carlo algorithm when [TT]-self[tt] is set to a value between 0.0 and 1.0" },
        { "-v",        FALSE, etBOOL, {&bVerbose},
          "Be loud and noisy" }
    };


#define NFILE asize(fnm)

    cr = init_commrec();
#ifdef GMX_LIB_MPI
    MPI_Barrier(MPI_COMM_WORLD);
#endif

    PCA_Flags  = PCA_NOEXIT_ON_ARGS;
    PCA_Flags |= (MASTER(cr) ? 0 : PCA_QUIET);

    if (!parse_common_args(&argc, argv, PCA_Flags,
                           NFILE, fnm, asize(pa), pa, asize(desc), desc,
                           0, NULL, &oenv))
    {
        return 0;
    }

    if (!bTUNE)
    {
        bTUNE = opt2bSet("-so", NFILE, fnm);
    }

    info.n_entries = 1;

    /* Allocate memory for the inputinfo struct: */
    create_info(&info);
    info.fourier_sp[0] = fs;

    /* Read in the tpr file and open logfile for reading */
    if (MASTER(cr))
    {
        snew(ir, 1);
        read_tpr_file(opt2fn("-s", NFILE, fnm), &info, &state, &mtop, ir, user_beta, fracself);

        fp = fopen(opt2fn("-o", NFILE, fnm), "w");
    }

    /* Check consistency if the user provided fourierspacing */
    if (fs > 0 && MASTER(cr))
    {
        /* Recalculate the grid dimensions using fourierspacing from user input */
        info.nkx[0] = 0;
        info.nky[0] = 0;
        info.nkz[0] = 0;
        calc_grid(stdout, state.box, info.fourier_sp[0], &(info.nkx[0]), &(info.nky[0]), &(info.nkz[0]));
        if ( (ir->nkx != info.nkx[0]) || (ir->nky != info.nky[0]) || (ir->nkz != info.nkz[0]) )
        {
            gmx_fatal(FARGS, "Wrong fourierspacing %f nm, input file grid = %d x %d x %d, computed grid = %d x %d x %d",
                      fs, ir->nkx, ir->nky, ir->nkz, info.nkx[0], info.nky[0], info.nkz[0]);
        }
//.........这里部分代码省略.........

int main(){

    Mecanismo P = Mecanismo(2);

    cube I1__; I1__.zeros(3,3,2);
    I1__.slice(0) << 0 << 0                       << 0          << endr
                  << 0 << 107.307e-6 + 146.869e-6 << 0          << endr
                  << 0 << 0                       << 107.307e-6 + 146.869e-6 << endr;

    I1__.slice(1) << 0 << 0        << 0        << endr
                  << 0 << 438.0e-6 << 0        << endr
                  << 0 << 0        << 438.0e-6 << endr;

    cube I2__; I2__.zeros(3,3,2);
    I2__.slice(0) << 0 << 0                        << 0          << endr
                  << 0 << 107.307e-6 + 188.738e-6  << 0          << endr
                  << 0 << 0                        << 107.307e-6 + 188.738e-6  << endr;

    I2__.slice(1) << 0 << 0          << 0          << endr
                  << 0 << 301.679e-6 << 0          << endr
                  << 0 << 0          << 301.679e-6 << endr;

    Serial RR1 = Serial(2, {0.12, 0.16}, {0.06, 0.078},{0.062, 0.124}, I1__ , {0, 0, 9.8}, &fDH_RR);
    Serial RR2 = Serial(2, {0.12, 0.16}, {0.06, 0.058},{0.062, 0.097}, I2__ , {0, 0, 9.8}, &fDH_RR);
    Serial **RR_ = new Serial* [2];
    RR_[0] = &RR1;
    RR_[1] = &RR2;

    //Matrizes que descrevem a arquitetura do mecanismo
    double l0 = 0.05;
    mat D_ = join_vert((mat)eye(2,2),2);
    mat E_ = join_diag( Roty(0)(span(0,1),span(0,2)), Roty(PI)(span(0,1),span(0,2)) );
    mat F_ = zeros(4,4);
    vec f_ = {l0,0,-l0,0};

    Parallel Robot = Parallel(2, &P, RR_, 2, {2,4}, D_, E_, F_, f_);
    Reference RefObj = Reference(0.12, {0.08, 0.16}, {-0.08, 0.4});

    //Plotar área de trabalho
    uint nx = 96.0;
    uint ny = 56.0;
    double lx = 0.24;
    double ly = 0.28;
    double xi = -lx;
    double xf = lx;
    double yi = 0.0;
    double yf = ly;
    double dx = (xf-xi)/(nx-1);
    double dy = (yf-yi)/(ny-1);
    double dl = 0.5*(dx+dy);
    
    Mat<int> M; M.zeros(nx,ny);
    field<mat> fZ_(nx,ny);
    field<mat> fMh_(nx,ny);
    field<vec> fgh_(nx,ny);
    field<vec> fa1_(nx,ny);
    field<vec> fa2_(nx,ny);
    field<vec> fa12_(nx,ny);
    for(uint i=0; i<nx; i++){
        for(uint j=0; j<ny; j++){
            fZ_(i,j).zeros(2,2);
            fMh_(i,j).zeros(2,2);
            fgh_(i,j).zeros(2);
            fa1_(i,j).zeros(2);
            fa2_(i,j).zeros(2);
            fa12_(i,j).zeros(2);
        }
    }
    vec v1_ = {1,0};
    vec v2_ = {0,1};
    vec v12_ = {1,1};
    double r = 0.07;
    double x0 = 0.0;
    double y0 = 0.17;

    uint rows = nx;
    uint cols = ny;

    vec q0_ = {0.823167, 1.81774, 0.823167, 1.81774};

    GNR2 gnr2 = GNR2("RK6", &Robot, 1e-6, 30);
    //gnr2.Doit(q0_, {0.05,0.08});
    //cout << gnr2.convergiu << endl;
    //cout << gnr2.x_ << endl;
    //cout << gnr2.res_ << endl;
    //cout << gnr2.n << endl;

    double x;
    double y;
    mat A2_;
    for(uint i=0; i<rows; i++){
        for(uint j=0; j<cols; j++){
            x = xi + i*dx;
            y = yi + j*dy;
            gnr2.Doit(q0_, {x, y});
            if(gnr2.convergiu){
                q0_ = gnr2.x_;
                A2_ = join_horiz(Robot.Ah_, join_horiz(Robot.Ao_.col(1), Robot.Ao_.col(3)) );
                if(abs(det(Robot.Ao_)) < 1.6*1e-6 || abs(det(A2_)) < 1e-11 ) M(i,j) = 2;
                else{ 
//.........这里部分代码省略.........

int gmx_genion(int argc, char *argv[])
{
    const char        *desc[] = {
        "[THISMODULE] randomly replaces solvent molecules with monoatomic ions.",
        "The group of solvent molecules should be continuous and all molecules",
        "should have the same number of atoms.",
        "The user should add the ion molecules to the topology file or use",
        "the [TT]-p[tt] option to automatically modify the topology.[PAR]",
        "The ion molecule type, residue and atom names in all force fields",
        "are the capitalized element names without sign. This molecule name",
        "should be given with [TT]-pname[tt] or [TT]-nname[tt], and the",
        "[TT][molecules][tt] section of your topology updated accordingly,",
        "either by hand or with [TT]-p[tt]. Do not use an atom name instead!",
        "[PAR]Ions which can have multiple charge states get the multiplicity",
        "added, without sign, for the uncommon states only.[PAR]",
        "For larger ions, e.g. sulfate we recommended using [gmx-insert-molecules]."
    };
    const char        *bugs[] = {
        "If you specify a salt concentration existing ions are not taken into "
        "account. In effect you therefore specify the amount of salt to be added.",
    };
    static int         p_num    = 0, n_num = 0, p_q = 1, n_q = -1;
    static const char *p_name   = "NA", *n_name = "CL";
    static real        rmin     = 0.6, conc = 0;
    static int         seed     = 1993;
    static gmx_bool    bNeutral = FALSE;
    static t_pargs     pa[]     = {
        { "-np",    FALSE, etINT,  {&p_num}, "Number of positive ions"       },
        { "-pname", FALSE, etSTR,  {&p_name}, "Name of the positive ion"      },
        { "-pq",    FALSE, etINT,  {&p_q},   "Charge of the positive ion"    },
        { "-nn",    FALSE, etINT,  {&n_num}, "Number of negative ions"       },
        { "-nname", FALSE, etSTR,  {&n_name}, "Name of the negative ion"      },
        { "-nq",    FALSE, etINT,  {&n_q},   "Charge of the negative ion"    },
        { "-rmin",  FALSE, etREAL, {&rmin},  "Minimum distance between ions" },
        { "-seed",  FALSE, etINT,  {&seed},  "Seed for random number generator" },
        {   "-conc",  FALSE, etREAL, {&conc},
            "Specify salt concentration (mol/liter). This will add sufficient ions to reach up to the specified concentration as computed from the volume of the cell in the input [REF].tpr[ref] file. Overrides the [TT]-np[tt] and [TT]-nn[tt] options."
        },
        { "-neutral", FALSE, etBOOL, {&bNeutral}, "This option will add enough ions to neutralize the system. These ions are added on top of those specified with [TT]-np[tt]/[TT]-nn[tt] or [TT]-conc[tt]. "}
    };
    t_topology         top;
    rvec              *x, *v;
    real               vol, qtot;
    matrix             box;
    t_atoms            atoms;
    t_pbc              pbc;
    int               *repl, ePBC;
    atom_id           *index;
    char              *grpname, title[STRLEN];
    gmx_bool          *bSet;
    int                i, nw, nwa, nsa, nsalt, iqtot;
    output_env_t       oenv;
    gmx_rng_t          rng;
    t_filenm           fnm[] = {
        { efTPR, NULL,  NULL,      ffREAD  },
        { efNDX, NULL,  NULL,      ffOPTRD },
        { efSTO, "-o",  NULL,      ffWRITE },
        { efTOP, "-p",  "topol",   ffOPTRW }
    };
#define NFILE asize(fnm)

    if (!parse_common_args(&argc, argv, 0, NFILE, fnm, asize(pa), pa,
                           asize(desc), desc, asize(bugs), bugs, &oenv))
    {
        return 0;
    }

    /* Check input for something sensible */
    if ((p_num < 0) || (n_num < 0))
    {
        gmx_fatal(FARGS, "Negative number of ions to add?");
    }

    if (conc > 0 && (p_num > 0 || n_num > 0))
    {
        fprintf(stderr, "WARNING: -conc specified, overriding -nn and -np.\n");
    }

    /* Read atom positions and charges */
    read_tps_conf(ftp2fn(efTPR, NFILE, fnm), title, &top, &ePBC, &x, &v, box, FALSE);
    atoms = top.atoms;

    /* Compute total charge */
    qtot = 0;
    for (i = 0; (i < atoms.nr); i++)
    {
        qtot += atoms.atom[i].q;
    }
    iqtot = gmx_nint(qtot);


    if (conc > 0)
    {
        /* Compute number of ions to be added */
        vol   = det(box);
        nsalt = gmx_nint(conc*vol*AVOGADRO/1e24);
        p_num = abs(nsalt*n_q);
        n_num = abs(nsalt*p_q);
    }
    if (bNeutral)
//.........这里部分代码省略.........

void calc_dispcorr(FILE *fplog,t_inputrec *ir,t_forcerec *fr,int step,
		   int natoms,matrix box,real lambda,
		   tensor pres,tensor virial,real ener[])
{
  static bool bFirst=TRUE;
  bool bCorrAll,bCorrPres;
  real dvdlambda,invvol,dens,ninter,avcsix,avctwelve,enerdiff,svir=0,spres=0;
  int  m;

  ener[F_DISPCORR] = 0.0;

  if (ir->eDispCorr != edispcNO) {
    bCorrAll  = (ir->eDispCorr == edispcAllEner ||
		 ir->eDispCorr == edispcAllEnerPres);
    bCorrPres = (ir->eDispCorr == edispcEnerPres ||
		 ir->eDispCorr == edispcAllEnerPres);

    if (bFirst)
      calc_enervirdiff(fplog,ir->eDispCorr,fr);
    
    invvol = 1/det(box);
    if (fr->n_tpi) {
      /* Only correct for the interactions with the inserted molecule */
      dens = (natoms - fr->n_tpi)*invvol;
      ninter = fr->n_tpi;
    } else {
      dens = natoms*invvol;
      ninter = 0.5*natoms;
    }

    if (ir->efep == efepNO) {
      avcsix    = fr->avcsix[0];
      avctwelve = fr->avctwelve[0];
    } else {
      avcsix    = (1 - lambda)*fr->avcsix[0]    + lambda*fr->avcsix[1];
      avctwelve = (1 - lambda)*fr->avctwelve[0] + lambda*fr->avctwelve[1];
    }
    
    enerdiff = ninter*(dens*fr->enerdiffsix - fr->enershiftsix);
    ener[F_DISPCORR] += avcsix*enerdiff;
    dvdlambda = 0.0;
    if (ir->efep != efepNO)
      dvdlambda += (fr->avcsix[1] - fr->avcsix[0])*enerdiff;

    if (bCorrAll) {
      enerdiff = ninter*(dens*fr->enerdifftwelve - fr->enershifttwelve);
      ener[F_DISPCORR] += avctwelve*enerdiff;
      if (fr->efep != efepNO)
	dvdlambda += (fr->avctwelve[1] - fr->avctwelve[0])*enerdiff;
    }

    if (bCorrPres) {
      svir = ninter*dens*avcsix*fr->virdiffsix/3.0;
      if (ir->eDispCorr == edispcAllEnerPres)
	svir += ninter*dens*avctwelve*fr->virdifftwelve/3.0;
      
      /* The factor 2 is because of the Gromacs virial definition */
      spres = -2.0*invvol*svir*PRESFAC;
      
      for(m=0; m<DIM; m++) {
	virial[m][m] += svir;
	pres[m][m] += spres;
      }
      ener[F_PRES] += spres;
    }
    
    if (bFirst && fplog) {
      if (bCorrAll)
	fprintf(fplog,"Long Range LJ corr.: <C6> %10.4e, <C12> %10.4e\n",
		avcsix,avctwelve);
      else
	fprintf(fplog,"Long Range LJ corr.: <C6> %10.4e\n",avcsix);
      
      if (bCorrPres)
	fprintf(fplog,
		"Long Range LJ corr.: Epot %10g, Pres: %10g, Vir: %10g\n",
                ener[F_DISPCORR],spres,svir);
      else
	fprintf(fplog,"Long Range LJ corr.: Epot %10g\n",ener[F_DISPCORR]);
    }
    bFirst = FALSE;

    if (fr->bSepDVDL && do_per_step(step,ir->nstlog))
      fprintf(fplog,sepdvdlformat,"Dispersion correction",
	      ener[F_DISPCORR],dvdlambda);
    
    ener[F_EPOT] += ener[F_DISPCORR];
    if (fr->efep != efepNO)
      ener[F_DVDL] += dvdlambda;
  }
}

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,Tcm;
    
  /* 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]);
	  fprintf(fp,"  w:   %12.5f  %12.5f  %12.5f\n",
		vcm->group_w[g][XX],vcm->group_w[g][YY],vcm->group_w[g][ZZ]);
	  pr_rvecs(fp,0,"Inertia tensor",vcm->group_i[g],DIM);
	}
      }
    }
  }
}

bool line_check_equivalent(line l1, line l2) {
    return fabs(det(l1.a, l1.b, l2.a, l2.b)) < eps &&
            fabs(det(l1.a, l1.c, l2.a, l2.c)) < eps &&
            fabs(det(l1.b, l1.c, l2.b, l2.c)) < eps;
}

position_tracker::Position getClosestRightLineProjection()
{

   //WHAT IF THERE IS NO LINE TO THE RIGHT OF THE ROBOT???

   double min_dist = 100000000;
   position_tracker::Position *tmp_pos = new position_tracker::Position();
   tmp_pos->x = -1;
   tmp_pos->y = -1;
   tmp_pos->theta = -1;

   list<Edge *>::iterator i;   
   for(i = map4.begin(); i != map4.end(); ++i) //for each edge in the map
   {
      Edge *e = *i;
      double D[2][2] = {{(e->Bx - e->Ax), (e->By - e->Ay)}, {(cur_pos.x - e->Ax), (cur_pos.y - e->Ay)}};

/*      cout << "cur_pos.x = " << cur_pos.x << endl;
      cout << "cur_pos.y = " << cur_pos.y << endl;
      cout << "cur_pos.theta = " << cur_pos.theta << endl;
      cout << "e->Ax = " << e->Ax << endl;
      cout << "e->Ay = " << e->Ay << endl;
      cout << "e->Bx = " << e->Bx << endl;
      cout << "e->By = " << e->By << endl;
      cout << "e->theta = " << e->theta << endl;
      cout << "det(D) = " << det(D) << endl;*/

      //keep only the lines to the right of the current robot pos
      if((det(D) > 0  && fabs(det(D)) > 0.0001 && (cur_pos.theta > e->theta - PI/2 && cur_pos.theta < e->theta + PI/2)) || (det(D) < 0  && fabs(det(D)) > 0.0001 && (cur_pos.theta < e->theta - PI/2 || cur_pos.theta > e->theta + PI/2)))
      {
          //find the projection of the current point to the last identified left line
          double A[2][2] = {{(e->Bx - e->Ax), (e->By - e->Ay)}, {(e->Ay - e->By), (e->Bx - e->Ax)}};
          double X[2] = {0, 0};
          double B[2] = {cur_pos.x*(e->Bx - e->Ax) + cur_pos.y*(e->By - e->Ay), e->Ay*(e->Bx - e->Ax) - e->Ax*(e->By - e->Ay)};
          solve(A, B, X);

          //discard line segment if the projection falls outside its boundaries
          if(e->theta < 0.1 ) //horizontal line
          {
              if(X[0] > e->Bx || X[0] < e->Ax)
                 continue;
          }
		  else{ //vertical line
              if(X[1] > e->By || X[1] < e->Ay)
                 continue; 
          }

          cout << "line to the RIGHT of the robot" << endl;

          //calculate the distance between the projection and the current robot location
          double sdist = pow(cur_pos.x - X[0], 2) + pow(cur_pos.y - X[1], 2);

          //keep the projection with the min distance from the current pos
          if(sdist < min_dist)
          {
              min_dist = sdist;
              tmp_pos->x = X[0] + (0.05/sdist)*(cur_pos.x - X[0]); //X[0];
              tmp_pos->y = X[1] + (0.05/sdist)*(cur_pos.y - X[1]); //X[1];


			  double angle1 = e->theta;
			  double angle2 = e->theta + PI;
              if(angle1 > PI)
                   angle1 = -2*PI + angle1;
              else if(angle1 < -PI)
                   angle1 = 2*PI + angle1;

              if(angle2 > PI)
                   angle2 = -2*PI + angle2;
              else if(angle2 < -PI)
                   angle2 = 2*PI + angle2;

              double diff1 = angle1 - cur_pos.theta;
              double diff2 = angle2 - cur_pos.theta;

              if(fabs(diff1) < fabs(diff2))
                   tmp_pos->theta = angle1; 
              else
                   tmp_pos->theta = angle2; 

      cout << "angle1 = " << angle1 << endl;
      cout << "angle2 = " << angle2 << endl;
      cout << "tmp_pos->theta = " << tmp_pos->theta << endl;

      cout << "cur_pos.x = " << cur_pos.x << endl;
      cout << "cur_pos.y = " << cur_pos.y << endl;
      cout << "cur_pos.theta = " << cur_pos.theta << endl;
      cout << "e->Ax = " << e->Ax << endl;
      cout << "e->Ay = " << e->Ay << endl;
      cout << "e->Bx = " << e->Bx << endl;
      cout << "e->By = " << e->By << endl;
      cout << "e->theta = " << e->theta << endl;
      cout << "det(D) = " << det(D) << endl;

          }
      }
   }

   return *tmp_pos;

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

//.........这里部分代码省略.........
    trotter_seq = trotter_seqlist[trotter_seqno];

    /* signal we are returning if nothing is going to be done in this routine */
    if ((trotter_seq[0] == etrtSKIPALL)  || !(bCouple))
    {
        return;
    }

    dtc = ir->nsttcouple*ir->delta_t;
    opts = &(ir->opts); /* just for ease of referencing */
    ngtc = opts->ngtc;
    snew(scalefac,opts->ngtc);
    for (i=0;i<ngtc;i++) 
    {
        scalefac[i] = 1;
    }
    /* execute the series of trotter updates specified in the trotterpart array */
    
    for (i=0;i<NTROTTERPARTS;i++){
        /* allow for doubled intgrators by doubling dt instead of making 2 calls */
        if ((trotter_seq[i] == etrtBAROV2) || (trotter_seq[i] == etrtBARONHC2) || (trotter_seq[i] == etrtNHC2))
        {
            dt = 2 * dtc;
        }
        else 
        {
            dt = dtc;
        }

        switch (trotter_seq[i])
        {
        case etrtBAROV:
        case etrtBAROV2:
            boxv_trotter(ir,&(state->veta),dt,state->box,ekind,vir,
                         enerd->term[F_PDISPCORR],enerd->term[F_DISPCORR],MassQ);
            break;
        case etrtBARONHC:
        case etrtBARONHC2:
            NHC_trotter(opts,state->nnhpres,ekind,dt,state->nhpres_xi,
                        state->nhpres_vxi,NULL,&(state->veta),MassQ,FALSE);      
            break;
        case etrtNHC:
        case etrtNHC2:
            NHC_trotter(opts,opts->ngtc,ekind,dt,state->nosehoover_xi,
                        state->nosehoover_vxi,scalefac,NULL,MassQ,(ir->eI==eiVV));
            /* need to rescale the kinetic energies and velocities here.  Could 
               scale the velocities later, but we need them scaled in order to 
               produce the correct outputs, so we'll scale them here. */
            
            for (i=0; i<ngtc;i++) 
            {
                tcstat = &ekind->tcstat[i];
                tcstat->vscale_nhc = scalefac[i]; 
                tcstat->ekinscaleh_nhc *= (scalefac[i]*scalefac[i]); 
                tcstat->ekinscalef_nhc *= (scalefac[i]*scalefac[i]); 
            }
            /* now that we've scaled the groupwise velocities, we can add them up to get the total */
            /* but do we actually need the total? */
            
            /* modify the velocities as well */
            for (n=md->start;n<md->start+md->homenr;n++) 
            {
                if (md->cTC) 
                { 
                    gc = md->cTC[n];
                }
                for (d=0;d<DIM;d++) 
                {
                    state->v[n][d] *= scalefac[gc];
                }
                
                if (debug) 
                {
                    for (d=0;d<DIM;d++) 
                    {
                        sumv[d] += (state->v[n][d])/md->invmass[n];
                    }
                }
            }          
            break;
        default:
            break;
        }
    }
    /* check for conserved momentum -- worth looking at this again eventually, but not working right now.*/  
#if 0
    if (debug) 
    {
        if (bFirstHalf) 
        {
            for (d=0;d<DIM;d++) 
            {
                consk[d] = sumv[d]*exp((1 + 1.0/opts->nrdf[0])*((1.0/DIM)*log(det(state->box)/state->vol0)) + state->nosehoover_xi[0]); 
            }
            fprintf(debug,"Conserved kappa: %15.8f %15.8f %15.8f\n",consk[0],consk[1],consk[2]);    
        }
    }
#endif
    sfree(scalefac);
}

gmx_bool replica_exchange(FILE *fplog, const t_commrec *cr, struct gmx_repl_ex *re,
                          t_state *state, gmx_enerdata_t *enerd,
                          t_state *state_local, gmx_int64_t step, real time)
{
    int i, j;
    int replica_id = 0;
    int exchange_partner;
    int maxswap = 0;
    /* Number of rounds of exchanges needed to deal with any multiple
     * exchanges. */
    /* Where each replica ends up after the exchange attempt(s). */
    /* The order in which multiple exchanges will occur. */
    gmx_bool bThisReplicaExchanged = FALSE;

    if (MASTER(cr))
    {
        replica_id  = re->repl;
        test_for_replica_exchange(fplog, cr->ms, re, enerd, det(state_local->box), step, time);
        prepare_to_do_exchange(fplog, re, replica_id, &maxswap, &bThisReplicaExchanged);
    }
    /* Do intra-simulation broadcast so all processors belonging to
     * each simulation know whether they need to participate in
     * collecting the state. Otherwise, they might as well get on with
     * the next thing to do. */
    if (DOMAINDECOMP(cr))
    {
#ifdef GMX_MPI
        MPI_Bcast(&bThisReplicaExchanged, sizeof(gmx_bool), MPI_BYTE, MASTERRANK(cr),
                  cr->mpi_comm_mygroup);
#endif
    }

    if (bThisReplicaExchanged)
    {
        /* Exchange the states */
        /* Collect the global state on the master node */
        if (DOMAINDECOMP(cr))
        {
            dd_collect_state(cr->dd, state_local, state);
        }
        else
        {
            copy_state_nonatomdata(state_local, state);
        }

        if (MASTER(cr))
        {
            /* There will be only one swap cycle with standard replica
             * exchange, but there may be multiple swap cycles if we
             * allow multiple swaps. */

            for (j = 0; j < maxswap; j++)
            {
                exchange_partner = re->order[replica_id][j];

                if (exchange_partner != replica_id)
                {
                    /* Exchange the global states between the master nodes */
                    if (debug)
                    {
                        fprintf(debug, "Exchanging %d with %d\n", replica_id, exchange_partner);
                    }
                    exchange_state(cr->ms, exchange_partner, state);
                }
            }
            /* For temperature-type replica exchange, we need to scale
             * the velocities. */
            if (re->type == ereTEMP || re->type == ereTL)
            {
                scale_velocities(state, sqrt(re->q[ereTEMP][replica_id]/re->q[ereTEMP][re->destinations[replica_id]]));
            }

        }

        /* With domain decomposition the global state is distributed later */
        if (!DOMAINDECOMP(cr))
        {
            /* Copy the global state to the local state data structure */
            copy_state_nonatomdata(state, state_local);
        }
    }

    return bThisReplicaExchanged;
}

//.........这里部分代码省略.........
    {
        n1      = (int)(2*amax/bin + 0.5);
        nradial = (int)(rmax/bin + 0.5);
        invspa  = n1/(2*amax);
        invspz  = nradial/rmax;
        if (bMirror)
        {
            n2 = 2*nradial;
        }
        else
        {
            n2 = nradial;
        }
    }

    snew(grid, n1);
    for (i = 0; i < n1; i++)
    {
        snew(grid[i], n2);
    }

    box1 = 0;
    box2 = 0;
    nfr  = 0;
    do
    {
        if (!bRadial)
        {
            box1      += box[c1][c1];
            box2      += box[c2][c2];
            invcellvol = n1*n2;
            if (nmpower == -3)
            {
                invcellvol /= det(box);
            }
            else if (nmpower == -2)
            {
                invcellvol /= box[c1][c1]*box[c2][c2];
            }
            for (i = 0; i < nindex; i++)
            {
                j = index[i];
                if ((!bXmin || x[j][cav] >= xmin) &&
                    (!bXmax || x[j][cav] <= xmax))
                {
                    m1 = x[j][c1]/box[c1][c1];
                    if (m1 >= 1)
                    {
                        m1 -= 1;
                    }
                    if (m1 < 0)
                    {
                        m1 += 1;
                    }
                    m2 = x[j][c2]/box[c2][c2];
                    if (m2 >= 1)
                    {
                        m2 -= 1;
                    }
                    if (m2 < 0)
                    {
                        m2 += 1;
                    }
                    grid[(int)(m1*n1)][(int)(m2*n2)] += invcellvol;
                }
            }

ll det (const pii & a,const pii & b,const pii & c){
 return det((b-a),(c-a));
}

//.........这里部分代码省略.........
    }
    if (!bProt && !bBox)
    {
        gmx_fatal(FARGS, "When no solute (-cp) is specified, "
                  "a box size (-box) must be specified");
    }

    aps = gmx_atomprop_init();

    if (bProt)
    {
        /*generate a solute configuration */
        conf_prot = opt2fn("-cp", NFILE, fnm);
        title     = read_prot(conf_prot, &atoms, &x, bReadV ? &v : NULL, &r, &ePBC, box,
                              aps, r_distance, r_scale);
        if (bReadV && !v)
        {
            fprintf(stderr, "Note: no velocities found\n");
        }
        if (atoms.nr == 0)
        {
            fprintf(stderr, "Note: no atoms in %s\n", conf_prot);
            bProt = FALSE;
        }
    }
    if (!bProt)
    {
        atoms.nr       = 0;
        atoms.nres     = 0;
        atoms.resinfo  = NULL;
        atoms.atomname = NULL;
        atoms.atom     = NULL;
        atoms.pdbinfo  = NULL;
        x              = NULL;
        r              = NULL;
    }
    if (bBox)
    {
        ePBC = epbcXYZ;
        clear_mat(box);
        box[XX][XX] = new_box[XX];
        box[YY][YY] = new_box[YY];
        box[ZZ][ZZ] = new_box[ZZ];
    }
    if (det(box) == 0)
    {
        gmx_fatal(FARGS, "Undefined solute box.\nCreate one with editconf "
                  "or give explicit -box command line option");
    }

    /* add nmol_ins molecules of atoms_ins
       in random orientation at random place */
    if (bInsert)
    {
        title_ins = insert_mols(opt2fn("-ci", NFILE, fnm), nmol_ins, nmol_try, seed,
                                &atoms, &x, &r, ePBC, box, aps, 
                                r_distance, r_scale, r_shell,
                                oenv, opt2fn_null("-ip", NFILE, fnm), deltaR, enum_rot,
                                bCheckAllPairDist);
    }
    else
    {
        title_ins = strdup("Generated by genbox");
    }

    /* add solvent */
    if (bSol)
    {
        add_solv(opt2fn("-cs", NFILE, fnm), &atoms, &x, v ? &v : NULL, &r, ePBC, box,
                 aps, r_distance, r_scale, &atoms_added, &residues_added, r_shell, max_sol,
                 oenv);
    }

    /* write new configuration 1 to file confout */
    confout = ftp2fn(efSTO, NFILE, fnm);
    fprintf(stderr, "Writing generated configuration to %s\n", confout);
    if (bProt)
    {
        write_sto_conf(confout, title, &atoms, x, v, ePBC, box);
        /* print box sizes and box type to stderr */
        fprintf(stderr, "%s\n", title);
    }
    else
    {
        write_sto_conf(confout, title_ins, &atoms, x, v, ePBC, box);
    }

    sfree(title_ins);

    /* print size of generated configuration */
    fprintf(stderr, "\nOutput configuration contains %d atoms in %d residues\n",
            atoms.nr, atoms.nres);
    update_top(&atoms, box, NFILE, fnm, aps);

    gmx_atomprop_destroy(aps);

    thanx(stderr);

    return 0;
}

int main()
{
//#ifdef _DEBUG
//	int iOrg = _CrtSetDbgFlag(_CRTDBG_REPORT_FLAG);	
//	_CrtSetDbgFlag(iOrg | _CRTDBG_LEAK_CHECK_DF);
//#endif
	
	cout << "tmat demo program" << endl;

	// build two 16 element double arrays
	double aa[] = { 1, 2, 3, 4,
				    4, 1, 2, 3,
					3, 4, 1, 2,
					2, 3, 4, 1 };

	double bb[] = { 3, 4, 5, 6,
		            6, 3, 4, 5,
					5, 6, 3, 4,
					4, 5, 6, 3 };

	// instantiate arrays as 4 x 4 matrices
	tmat<double> A(4,4,aa); cout << A << endl;
	tmat<double> B(4,4,bb); cout << B << endl;

	// check determinants
	cout << "det(A) = " << det(A) << endl;
	cout << "det(B) = " << det(B) << endl << endl;

	// exercise the matrix operators
	tmat<double> C;
	C = A + B; cout << "A + B =" << endl << C << endl;
	C = A - B; cout << "A - B =" << endl << C << endl;
	C = A * B; cout << "A * B =" << endl << C << endl;

	// exercise unary operators
	C = transpose(A); cout << "transpose(A) =" << endl << C << endl;
	C = inv(A); cout << "inv(A) =" << endl << C << endl;

	// check error flag
	C = hconcat(A, B); cout << "A | B =" << endl << C << endl;
	C = vconcat(A, B); cout << "A , B =" << endl << C << endl;
//	C = A + C; // error here
	delrow(C,5);cout << "A , B row=" << endl << C << endl;

	// string matrices
	tmat<string> S(3,3);  
	S(1,1) = "an";  S(1,2) = "to";  S(1,3) = "if";
	S(2,1) = "or";	S(2,2) = "no";	S(2,3) = "on";
	S(3,1) = "be";	S(3,2) = "am";  S(3,3) = "as";
	cout << S << endl;

	S = transpose(S); cout << S << endl;

	// integer matrices
	tmat<int> H(3,3);
	H(1,1) = 1;  H(1,2) = 0; H(1,3) = 1;
	H(2,1) = 0;  H(2,2) = 1; H(2,3) = 1;
	H(3,1) = 1;  H(3,2) = 0; H(3,3) = 0;
	cout << H << endl;

	H = bin_add(H, transpose(H));  cout << H << endl;
		


	cout << "end program" << endl;

	return 0;

}// main()

double cross(struct Point* a, struct Point* b, struct Point* c) {
	return det(b->x - a->x, b->y - a->y, c->x - a->x, c->y - a->y);
}