C++ Pair类代码示例

void Pair::write_file(int narg, char **arg)
{
  if (narg < 8) error->all(FLERR,"Illegal pair_write command");
  if (single_enable == 0)
    error->all(FLERR,"Pair style does not support pair_write");

  // parse arguments

  int itype = atoi(arg[0]);
  int jtype = atoi(arg[1]);
  if (itype < 1 || itype > atom->ntypes || jtype < 1 || jtype > atom->ntypes)
    error->all(FLERR,"Invalid atom types in pair_write command");

  int n = atoi(arg[2]);

  int style;
  if (strcmp(arg[3],"r") == 0) style = RLINEAR;
  else if (strcmp(arg[3],"rsq") == 0) style = RSQ;
  else if (strcmp(arg[3],"bitmap") == 0) style = BMP;
  else error->all(FLERR,"Invalid style in pair_write command");

  double inner = atof(arg[4]);
  double outer = atof(arg[5]);
  if (inner <= 0.0 || inner >= outer)
    error->all(FLERR,"Invalid cutoffs in pair_write command");

  // open file in append mode
  // print header in format used by pair_style table

  int me;
  MPI_Comm_rank(world,&me);
  FILE *fp;
  if (me == 0) {
    fp = fopen(arg[6],"a");
    if (fp == NULL) error->one(FLERR,"Cannot open pair_write file");
    fprintf(fp,"# Pair potential %s for atom types %d %d: i,r,energy,force\n",
            force->pair_style,itype,jtype);
    if (style == RLINEAR)
      fprintf(fp,"\n%s\nN %d R %g %g\n\n",arg[7],n,inner,outer);
    if (style == RSQ)
      fprintf(fp,"\n%s\nN %d RSQ %g %g\n\n",arg[7],n,inner,outer);
  }

  // initialize potentials before evaluating pair potential
  // insures all pair coeffs are set and force constants

  force->init();

  // if pair style = any of EAM, swap in dummy fp vector

  double eamfp[2];
  eamfp[0] = eamfp[1] = 0.0;
  double *eamfp_hold;

  Pair *epair = force->pair_match("eam",0);
  if (epair) epair->swap_eam(eamfp,&eamfp_hold);

  // if atom style defines charge, swap in dummy q vec

  double q[2];
  q[0] = q[1] = 1.0;
  if (narg == 10) {
    q[0] = atof(arg[8]);
    q[1] = atof(arg[9]);
  }
  double *q_hold;

  if (atom->q) {
    q_hold = atom->q;
    atom->q = q;
  }

  // evaluate energy and force at each of N distances

  int masklo,maskhi,nmask,nshiftbits;
  if (style == BMP) {
    init_bitmap(inner,outer,n,masklo,maskhi,nmask,nshiftbits);
    int ntable = 1 << n;
    if (me == 0)
      fprintf(fp,"\n%s\nN %d BITMAP %g %g\n\n",arg[7],ntable,inner,outer);
    n = ntable;
  }

  double r,e,f,rsq;
  union_int_float_t rsq_lookup;

  for (int i = 0; i < n; i++) {
    if (style == RLINEAR) {
      r = inner + (outer-inner) * i/(n-1);
      rsq = r*r;
    } else if (style == RSQ) {
      rsq = inner*inner + (outer*outer - inner*inner) * i/(n-1);
      r = sqrt(rsq);
    } else if (style == BMP) {
      rsq_lookup.i = i << nshiftbits;
      rsq_lookup.i |= masklo;
      if (rsq_lookup.f < inner*inner) {
        rsq_lookup.i = i << nshiftbits;
        rsq_lookup.i |= maskhi;
      }
//.........这里部分代码省略.........

void DFS_M::dfs(int iRoot)
{
  Stack2< Pair<int,int> > fringe;
  //Queue< Pair<int,int> > fringe;

  fringe.push(Pair<int,int>(-1,iRoot));
  int precount = 0; // pre-order counter
  int poscount = 0; // post-order counter
  int inocount = 0; // in-order counter
  int P = -1;
  int forest=0;
  int cyclecount = 0; // number of cycles found so far
  
  while (precount < _graph.getVertexSize())
  { 
    while ( !fringe.empty() )
    {
      Pair<int,int> pair = fringe.getTop(); fringe.pop();
      int v0 = pair.first();
      int v1 = pair.second();
      
      if (v0==-2) {
        _postorder[v1]=poscount;
        ++poscount;
        continue;
      }

      if ( !_visited[v1] )
      {
        // first time we see this vertex, push a dummy edge on the fringe
        // before pushing all its children. When the dummy edge will be popped
        // that means all the subtrees rooted under this vertex v1, will have been
        // explored completely, and we can assign the postorder index to the vertex
        fringe.push(Pair<int,int>(-2,v1));
        _preorder[v1]=precount; ++precount;
        _visited[v1]=1;
        _parent[v1]=v0;
        _forest[v1]=forest;
  
        if ( v0 > -1 )
        {
          _matrix[v0][v1]='T';
        }
        //std::cout << "(" << v1 << ")" << std::endl;
    
        for ( GraphEdgeIter it = _graph.getEdgeIter(v1); !it.end(); ++it )
        {
          int v2 = *it;
          
          if (!_visited[v2]) {
            fringe.push(Pair<int,int>(v1,v2));
          } else { // already visited
            char edgeType = tagEdge(v1,v2);
            cycleCheck(edgeType, v1, v2, cyclecount);
          }
        }
      } else { // already visited
        char edgeType = tagEdge(v0,v1);
        cycleCheck(edgeType, v0, v1, cyclecount);
      }

    }

    if (fringe.empty() && precount<_graph.getVertexSize())
    {
      for (int i=0; i<_graph.getVertexSize(); ++i)
      {
          if (!_visited[i])
          {
            fringe.push(Pair<int,int>(-1,i));
            break;
          }
      }
      ++forest;
    }
  }
}

void ComputeFEP::init()
{
    int i,j;

    if (!fepinitflag)    // avoid init to run entirely when called by write_data
        fepinitflag = 1;
    else return;

    // setup and error checks

    pairflag = 0;

    for (int m = 0; m < npert; m++) {
        Perturb *pert = &perturb[m];

        pert->ivar = input->variable->find(pert->var);
        if (pert->ivar < 0)
            error->all(FLERR,"Variable name for compute fep does not exist");
        if (!input->variable->equalstyle(pert->ivar))
            error->all(FLERR,"Variable for compute fep is of invalid style");

        if (force->pair == NULL)
            error->all(FLERR,"compute fep pair requires pair interactions");

        if (pert->which == PAIR) {
            pairflag = 1;

            Pair *pair = force->pair_match(pert->pstyle,1);
            if (pair == NULL) error->all(FLERR,"compute fep pair style "
                                             "does not exist");
            void *ptr = pair->extract(pert->pparam,pert->pdim);
            if (ptr == NULL)
                error->all(FLERR,"compute fep pair style param not supported");

            pert->array = (double **) ptr;

            // if pair hybrid, test that ilo,ihi,jlo,jhi are valid for sub-style

            if ((strcmp(force->pair_style,"hybrid") == 0 ||
                    strcmp(force->pair_style,"hybrid/overlay") == 0)) {
                PairHybrid *pair = (PairHybrid *) force->pair;
                for (i = pert->ilo; i <= pert->ihi; i++)
                    for (j = MAX(pert->jlo,i); j <= pert->jhi; j++)
                        if (!pair->check_ijtype(i,j,pert->pstyle))
                            error->all(FLERR,"compute fep type pair range is not valid for "
                                       "pair hybrid sub-style");
            }

        } else if (pert->which == ATOM) {
            if (pert->aparam == CHARGE) {
                if (!atom->q_flag)
                    error->all(FLERR,"compute fep requires atom attribute charge");
            }
        }
    }

    if (tailflag) {
        if (force->pair->tail_flag == 0)
            error->all(FLERR,"Compute fep tail when pair style does not "
                       "compute tail corrections");
    }

    // detect if package gpu is present

    int ifixgpu = modify->find_fix("package_gpu");
    if (ifixgpu >= 0) fixgpu = modify->fix[ifixgpu];

    if (comm->me == 0) {
        if (screen) {
            fprintf(screen, "FEP settings ...\n");
            fprintf(screen, "  temperature = %f\n", temp_fep);
            fprintf(screen, "  tail %s\n", (tailflag ? "yes":"no"));
            for (int m = 0; m < npert; m++) {
                Perturb *pert = &perturb[m];
                if (pert->which == PAIR)
                    fprintf(screen, "  %s %s %d-%d %d-%d\n", pert->pstyle, pert->pparam,
                            pert->ilo, pert->ihi, pert->jlo, pert->jhi);
                else if (pert->which == ATOM)
                    fprintf(screen, "  %d-%d charge\n", pert->ilo, pert->ihi);
            }
        }
        if (logfile) {
            fprintf(logfile, "FEP settings ...\n");
            fprintf(logfile, "  temperature = %f\n", temp_fep);
            fprintf(logfile, "  tail %s\n", (tailflag ? "yes":"no"));
            for (int m = 0; m < npert; m++) {
                Perturb *pert = &perturb[m];
                if (pert->which == PAIR)
                    fprintf(logfile, "  %s %s %d-%d %d-%d\n", pert->pstyle, pert->pparam,
                            pert->ilo, pert->ihi, pert->jlo, pert->jhi);
                else if (pert->which == ATOM)
                    fprintf(logfile, "  %d-%d charge\n", pert->ilo, pert->ihi);
            }
        }
    }

}

void FixAdapt::init()
{
    int i,j;

    // setup and error checks

    anypair = 0;

    for (int m = 0; m < nadapt; m++) {
        Adapt *ad = &adapt[m];

        ad->ivar = input->variable->find(ad->var);
        if (ad->ivar < 0)
            error->all("Variable name for fix adapt does not exist");
        if (!input->variable->equalstyle(ad->ivar))
            error->all("Variable for fix adapt is invalid style");

        if (ad->which == PAIR) {
            anypair = 1;

            Pair *pair = force->pair_match(ad->pstyle,1);
            if (pair == NULL) error->all("Fix adapt pair style does not exist");
            void *ptr = pair->extract(ad->pparam,ad->pdim);
            if (ptr == NULL) error->all("Fix adapt pair style param not supported");

            ad->pdim = 2;
            if (ad->pdim == 0) ad->scalar = (double *) ptr;
            if (ad->pdim == 2) ad->array = (double **) ptr;

            // if pair hybrid, test that ilo,ihi,jlo,jhi are valid for sub-style

            if (ad->pdim == 2 && (strcmp(force->pair_style,"hybrid") == 0 ||
                                  strcmp(force->pair_style,"hybrid/overlay") == 0)) {
                PairHybrid *pair = (PairHybrid *) force->pair;
                for (i = ad->ilo; i <= ad->ihi; i++)
                    for (j = MAX(ad->jlo,i); j <= ad->jhi; j++)
                        if (!pair->check_ijtype(i,j,ad->pstyle))
                            error->all("Fix adapt type pair range is not valid for "
                                       "pair hybrid sub-style");
            }

        } else if (ad->which == KSPACE) {
            if (force->kspace == NULL)
                error->all("Fix adapt is incompatible with KSpace style");
            kspace_scale = (double *) force->kspace->extract("scale");

        } else if (ad->which == ATOM) {
            if (ad->aparam == DIAMETER) {
                if (!atom->radius_flag)
                    error->all("Fix adapt requires atom attribute diameter");
            }
        }
    }

    // make copy of original pair array values

    for (int m = 0; m < nadapt; m++) {
        Adapt *ad = &adapt[m];
        if (ad->which == PAIR && ad->pdim == 2) {
            for (i = ad->ilo; i <= ad->ihi; i++)
                for (j = MAX(ad->jlo,i); j <= ad->jhi; j++)
                    ad->array_orig[i][j] = ad->array[i][j];
        }
    }
}

int NeighborKokkos::init_lists_kokkos()
{
  int i;

  for (i = 0; i < nlist_host; i++) delete lists_host[i];
  delete [] lists_host;
  delete [] pair_build_host;
  delete [] stencil_create_host;
  nlist_host = 0;

  for (i = 0; i < nlist_device; i++) delete lists_device[i];
  delete [] lists_device;
  delete [] pair_build_device;
  delete [] stencil_create_device;
  nlist_device = 0;

  nlist = 0;
  for (i = 0; i < nrequest; i++) {
    if (requests[i]->kokkos_device) nlist_device++;
    else if (requests[i]->kokkos_host) nlist_host++;
    else nlist++;
  }

  lists_host = new NeighListKokkos<LMPHostType>*[nrequest];
  pair_build_host = new PairPtrHost[nrequest];
  stencil_create_host = new StencilPtrHost[nrequest];
  for (i = 0; i < nrequest; i++) {
    lists_host[i] = NULL;
    pair_build_host[i] = NULL;
    stencil_create_host[i] = NULL;
  }

  for (i = 0; i < nrequest; i++) {
    if (!requests[i]->kokkos_host) continue;
    lists_host[i] = new NeighListKokkos<LMPHostType>(lmp);
    lists_host[i]->index = i;
    lists_host[i]->dnum = requests[i]->dnum;
    if (requests[i]->pair) {
      Pair *pair = (Pair *) requests[i]->requestor;
      pair->init_list(requests[i]->id,lists_host[i]);
    }
  }

  lists_device = new NeighListKokkos<LMPDeviceType>*[nrequest];
  pair_build_device = new PairPtrDevice[nrequest];
  stencil_create_device = new StencilPtrDevice[nrequest];
  for (i = 0; i < nrequest; i++) {
    lists_device[i] = NULL;
    pair_build_device[i] = NULL;
    stencil_create_device[i] = NULL;
  }

  for (i = 0; i < nrequest; i++) {
    if (!requests[i]->kokkos_device) continue;
    lists_device[i] = new NeighListKokkos<LMPDeviceType>(lmp);
    lists_device[i]->index = i;
    lists_device[i]->dnum = requests[i]->dnum;
    if (requests[i]->pair) {
      Pair *pair = (Pair *) requests[i]->requestor;
      pair->init_list(requests[i]->id,lists_device[i]);
    }
  }

  // 1st time allocation of xhold

  if (dist_check)
      xhold = DAT::tdual_x_array("neigh:xhold",maxhold);

  // return # of non-Kokkos lists

  return nlist;
}

void FixAdaptFEP::init()
{
  int i,j;

  // setup and error checks

  anypair = 0;

  for (int m = 0; m < nadapt; m++) {
    Adapt *ad = &adapt[m];

    ad->ivar = input->variable->find(ad->var);
    if (ad->ivar < 0)
      error->all(FLERR,"Variable name for fix adapt/fep does not exist");
    if (!input->variable->equalstyle(ad->ivar))
      error->all(FLERR,"Variable for fix adapt/fep is invalid style");

    if (ad->which == PAIR) {
      anypair = 1;

      Pair *pair = force->pair_match(ad->pstyle,1);
      if (pair == NULL) error->all(FLERR,"Fix adapt/fep pair style does not exist");
      void *ptr = pair->extract(ad->pparam,ad->pdim);
      if (ptr == NULL) 
        error->all(FLERR,"Fix adapt/fep pair style param not supported");

      ad->pdim = 2;
      if (ad->pdim == 0) ad->scalar = (double *) ptr;
      if (ad->pdim == 2) ad->array = (double **) ptr;

      // if pair hybrid, test that ilo,ihi,jlo,jhi are valid for sub-style

      if (ad->pdim == 2 && (strcmp(force->pair_style,"hybrid") == 0 ||
                            strcmp(force->pair_style,"hybrid/overlay") == 0)) {
        PairHybrid *pair = (PairHybrid *) force->pair;
        for (i = ad->ilo; i <= ad->ihi; i++)
          for (j = MAX(ad->jlo,i); j <= ad->jhi; j++)
            if (!pair->check_ijtype(i,j,ad->pstyle))
              error->all(FLERR,"Fix adapt/fep type pair range is not valid for "
                         "pair hybrid sub-style");
      }

    } else if (ad->which == KSPACE) {
      if (force->kspace == NULL)
        error->all(FLERR,"Fix adapt/fep kspace style does not exist");
      kspace_scale = (double *) force->kspace->extract("scale");

    } else if (ad->which == ATOM) {
      if (ad->aparam == DIAMETER) {
        if (!atom->radius_flag)
          error->all(FLERR,"Fix adapt/fep requires atom attribute diameter");
      }
      if (ad->aparam == CHARGE) {
        if (!atom->q_flag)
          error->all(FLERR,"Fix adapt/fep requires atom attribute charge");
      }
    }
  }

  // make copy of original pair array values

  for (int m = 0; m < nadapt; m++) {
    Adapt *ad = &adapt[m];
    if (ad->which == PAIR && ad->pdim == 2) {
      for (i = ad->ilo; i <= ad->ihi; i++)
        for (j = MAX(ad->jlo,i); j <= ad->jhi; j++)
          ad->array_orig[i][j] = ad->array[i][j];
    }
  }

#ifdef ADAPT_DEBUG
  if (comm->me == 0 && screen) {
    for (int m = 0; m < nadapt; m++) {
      Adapt *ad = &adapt[m];
      if (ad->which == PAIR && ad->pdim == 2) {
        fprintf(screen, "###ADAPT original %s %s\n", ad->pstyle, ad->pparam);
        fprintf(screen, "###ADAPT  I  J   old_param\n");
        for (i = ad->ilo; i <= ad->ihi; i++)
          for (j = MAX(ad->jlo,i); j <= ad->jhi; j++)
            fprintf(screen, "###ADAPT %2d %2d %9.5f\n", i, j,
                    ad->array_orig[i][j]);
      }
    }
  }
#endif

}

bool linearInterpolationWeights::integrationWeights
(
    const scalar t1,
    const scalar t2,
    labelList& indices,
    scalarField& weights
) const
{
    if (t2 < t1-VSMALL)
    {
        FatalErrorIn("linearInterpolationWeights::integrationWeights(..)")
            << "Integration should be in positive direction."
            <<  " t1:" << t1 << " t2:" << t2
            << exit(FatalError);
    }

    // Currently no fancy logic on cached index like in value

    //- Find lower or equal index
    label i1 = findLower(samples_, t1, 0, lessEqOp<scalar>());
    //- Find lower index
    label i2 = findLower(samples_, t2);

    // For now just fail if any outside table
    if (i1 == -1 || i2 == samples_.size()-1)
    {
        FatalErrorIn("linearInterpolationWeights::integrationWeights(..)")
            << "Integrating outside table " << samples_[0] << ".."
            << samples_.last() << " not implemented."
            << " t1:" << t1 << " t2:" << t2 << exit(FatalError);
    }

    label nIndices = i2-i1+2;


    // Determine if indices already correct
    bool anyChanged = false;

    if (nIndices != indices.size())
    {
        anyChanged = true;
    }
    else
    {
        // Closer check

        label index = i1;
        forAll(indices, i)
        {
            if (indices[i] != index)
            {
                anyChanged = true;
                break;
            }
            index++;
        }
    }

    indices.setSize(nIndices);
    weights.setSize(nIndices);
    weights = 0.0;

    // Sum from i1+1 to i2+1
    for (label i = i1+1; i <= i2; i++)
    {
        scalar d = samples_[i+1]-samples_[i];
        indices[i-i1] = i;
        weights[i-i1] += 0.5*d;
        indices[i+1-i1] = i+1;
        weights[i+1-i1] += 0.5*d;
    }

    // Add from i1 to t1
    {
        Pair<scalar> i1Tot1 = integrationWeights(i1, t1);
        indices[0] = i1;
        weights[0] += i1Tot1.first();
        indices[1] = i1+1;
        weights[1] += i1Tot1.second();
    }

    // Subtract from t2 to i2+1
    {
        Pair<scalar> wghts = integrationWeights(i2, t2);
        indices[i2-i1] = i2;
        weights[i2-i1] += -wghts.first();
        indices[i2-i1+1] = i2+1;
        weights[i2-i1+1] += -wghts.second();
    }

    return anyChanged;
}

int ComputeGranLocal::compute_pairs(int flag)
{
  int i,j,m,n,ii,jj,inum,jnum,itype,jtype;
  double xtmp,ytmp,ztmp,delx,dely,delz;
  double rsq,eng,fpair,factor_coul,factor_lj;
  int *ilist,*jlist,*numneigh,**firstneigh;
  double *ptr;

  double **x = atom->x;
  int *type = atom->type;
  int *mask = atom->mask;
  int nlocal = atom->nlocal;
  double *special_coul = force->special_coul;
  double *special_lj = force->special_lj;
  int newton_pair = force->newton_pair;

  // invoke half neighbor list (will copy or build if necessary)

  if (flag == 0) neighbor->build_one(list->index);

  inum = list->inum;
  ilist = list->ilist;
  numneigh = list->numneigh;
  firstneigh = list->firstneigh;

  // loop over neighbors of my atoms
  // skip if I or J are not in group
  // for flag = 0, just count pair interactions within force cutoff
  // for flag = 1, calculate requested output fields

  Pair *pair = force->pair;
  double **cutsq = force->pair->cutsq;

  tagint *tag = atom->tag;

  m = 0;
  for (ii = 0; ii < inum; ii++) {
    i = ilist[ii];
    if (!(mask[i] & groupbit)) continue;

    xtmp = x[i][0];
    ytmp = x[i][1];
    ztmp = x[i][2];
    itype = type[i];
    jlist = firstneigh[i];
    jnum = numneigh[i];

    for (jj = 0; jj < jnum; jj++) {
      j = jlist[jj];
      factor_lj = special_lj[sbmask(j)];
      factor_coul = special_coul[sbmask(j)];
      j &= NEIGHMASK;

      if (!(mask[j] & groupbit)) continue;
      if (newton_pair == 0 && j >= nlocal) continue;

      delx = xtmp - x[j][0];
      dely = ytmp - x[j][1];
      delz = ztmp - x[j][2];
      rsq = delx*delx + dely*dely + delz*delz;
      jtype = type[j];
      if (rsq >= cutsq[itype][jtype]) continue;

      if (flag) {
        if (singleflag)
          eng = pair->single(i,j,itype,jtype,rsq,factor_coul,factor_lj,fpair);

        if (nvalues == 1) ptr = &vector[m];
        else ptr = array[m];

        for (n = 0; n < nvalues; n++) {
          switch (pstyle[n]) {
          case DIST:
            ptr[n] = sqrt(rsq);
            break;
          case ENG:
            ptr[n] = eng;
            break;
          case FORCE:
            ptr[n] = sqrt(rsq)*fpair;
            break;
          case FX:
            ptr[n] = delx*fpair;
            break;
          case FY:
            ptr[n] = dely*fpair;
            break;
          case FZ:
            ptr[n] = delz*fpair;
            break;
          case PN:
            ptr[n] = pair->svector[pindex[n]];
            break;
          case TAG1:
            ptr[n] = tag[i];
            break;
          case TAG2:
            ptr[n] = tag[j];
            break;
          }
//.........这里部分代码省略.........

void PassingChallengePlay::executePlay(VisionData* vision, RobocupStrategyData* rsd) 
{
  RobotIndex r0=ROBOT0;
  RobotIndex r1=ROBOT1;
  
  Pair ballLoc=getBallLocation(*vision);
  ///----
  for(RobotIndex robotID = r0; robotID < ROBOT2; robotID++){
    Pair robotLoc=getLocation(robotID,*vision,rsd->getSystemParams());
    Pair otherPos;
    int side;
    if(robotID == r0){
      otherPos=getLocation(r1,*vision,rsd->getSystemParams());
      side=side0;
    }else{
      otherPos=getLocation(r0,*vision,rsd->getSystemParams());
      side=side1;
    }

    if((ballLoc.getX() > rsd->getSystemParams().field.HALF_LINE &&
        robotID==r0) ||
       (ballLoc.getX() < rsd->getSystemParams().field.HALF_LINE &&
        robotID==r1))
    {
      //ball on our side
      if(friendlyHasPossession(robotID,rsd->getSystemParams())){
        //go kick
        if(-robotLoc.getY()*side > (rsd->getSystemParams().field.SPLIT_LINE + .5f)){
          //we're there, kick
          rsd->getDestination(robotID)->setKick(KICK_PASS);

        }else{
          //not there yet.  
          rsd->getDestination(robotID)->setDribble(DRIBBLE_DEFAULT);
          rsd->getDestination(robotID)->setVerticalDribble(V_DRIBBLE_DEFAULT);
          //head across
          float backup=.3f;
          if(robotID == r1){
            rsd->getDestination(robotID)->setPos(rsd->getSystemParams().field.HALF_LINE - backup,-side*1.0f);
            rsd->getDestination(robotID)->setRotation(0.0f);
          }else{
            rsd->getDestination(robotID)->setPos(rsd->getSystemParams().field.HALF_LINE + backup,-side*1.0f);
            rsd->getDestination(robotID)->setRotation(PI);
          }
        }
      
      }else{
        //go grab ball
        if(!rsd->getStrategyModule().getSkillSet(robotID)->getSkill(AcquirePossessionSkill::skillNum)->isInitialized()){
          rsd->getStrategyModule().getSkillSet(robotID)->getSkill(AcquirePossessionSkill::skillNum)->initialize();
        }
        rsd->getStrategyModule().getSkillSet(robotID)->getSkill(AcquirePossessionSkill::skillNum)->run();
        rsd->getDestination(robotID)->setDribble(DRIBBLE_DEFAULT);
        rsd->getDestination(robotID)->setVerticalDribble(V_DRIBBLE_DEFAULT);
        rsd->getDestination(robotID)->setKick(NO_KICK);
        rsd->getDestination(robotID)->setSpeed(GOALIE_SPEED);

      }


    }else{
        int side;
        if(ballLoc.getY() > rsd->getSystemParams().field.SPLIT_LINE){
          side=1;
        }else{
          side=-1;
        }
        if(robotID == r0){
          side0=side;
        }else{
          side1=side;
        }

        float backup=.5f;
        if(robotID == r1){
          rsd->getDestination(robotID)->setPos(rsd->getSystemParams().field.HALF_LINE - backup,otherPos.getY());
          rsd->getDestination(robotID)->setRotation(0.0f);
        }else{
          rsd->getDestination(robotID)->setPos(rsd->getSystemParams().field.HALF_LINE + backup,otherPos.getY());
          rsd->getDestination(robotID)->setRotation(PI);
        }
    }
  }

/*
  Pair aggressorPos(getLocation(robot1ID,*vision,rsd->getSystemParams()));
  Pair creatorPos(getLocation(robot2ID,*vision,rsd->getSystemParams()));
  Pair ballLoc(getBallLocation(*vision));

  if(ballLoc.getX() > rsd->getSystemParams().field.HALF_LINE)
  { 
     turn = LEFT;
  }
  else
  {
     turn = RIGHT;
  }
  
  
  if(state == KICK_RECEIVE && readTimer() > MAX_ELAPSED_TIME)
//.........这里部分代码省略.........

inline void ICP::UpdateAndReject(Pair& init_f){
	
	double sigma=0.0;
	double mean=0.0;
	unsigned int N = data_indices.size() + init_f.size();
	
	Eigen::Vector4d point_s(0.0,0.0,0.0,1.0);
	Eigen::Vector4d point_d(0.0,0.0,0.0,1.0);
	
	std::vector<double> dists(N);
		
	//compute mean
	unsigned int k=0;
	for (unsigned int i=0 ; i < N; i++){
		
		if(i<data_indices.size()){
			point_s(0) = cloud_m->points[ model_indices[i] ].x;	
			point_s(1) = cloud_m->points[ model_indices[i] ].y;
			point_s(2) = cloud_m->points[ model_indices[i] ].z;
		
			point_d(0) = cloud_d->points[ data_indices[i] ].x;	
			point_d(1) = cloud_d->points[ data_indices[i] ].y;
			point_d(2) = cloud_d->points[ data_indices[i] ].z;
		}else{
			point_s(0) = cloud_m->points[ init_f[k].first ].x;	
			point_s(1) = cloud_m->points[ init_f[k].first ].y;
			point_s(2) = cloud_m->points[ init_f[k].first ].z;
		
			point_d(0) = cloud_d->points[ init_f[k].second ].x;	
			point_d(1) = cloud_d->points[ init_f[k].second ].y;
			point_d(2) = cloud_d->points[ init_f[k].second ].z;
			k++;
		}
		
		point_d = T*point_d;				
		
		dists[i]= (point_d - point_s).norm();
		mean = mean + dists[i];
	}
	
	mean = mean/N;
	
	//compute standart diviation
	for (unsigned int i=0; i < N; i++){
		sigma = sigma + (dists[i]-mean)*(dists[i]-mean);
	}
	
	sigma = sigma/N;
	sigma = sqrt(sigma);
	
	
	//How good is the registration
	if (mean<D)						//very good
		Dmax = mean + 3*sigma;
	else if (mean<3*D)				//good
		Dmax = mean + 2*sigma;
	else if (mean<6*D)				//bad
		Dmax = mean + sigma;
	else {							//very bad 
		std::vector<double> dists2 = dists;
		sort (dists2.begin(), dists2.end());
	
		if (dists2.size() % 2 == 0) {
			Dmax = (dists2[dists2.size()/2-1] + dists2[dists2.size()/2]) / 2.0;
		}else {
			Dmax = dists2[dists2.size()/2]; 
		}
	}
	
	//Update the maching
	k=0;
	unsigned int i=0;
	for (i=0 ; i <data_indices.size() ; i++){
		if (dists[i] < Dmax){
			model_indices[k] = model_indices[i];
			data_indices[k] = data_indices[i];
			k++;	
		}
	}
	
	model_indices.resize(k);
	data_indices.resize(k);	
		
	k=0;
	unsigned int j,l;
	for (j=i, l=0; j<N ; j++,l++){
		if (dists[j] < Dmax){
			init_f[k]= init_f[l];
			k++;	
		}
	}

	if(k!=init_f.size()) 
		init_f.resize(k);
	
}

//procustres
inline void ICP::minimize(const Pair& init_f){
	
	Eigen::Vector3d centroide_model(0.0,0.0,0.0), centroide_data(0.0,0.0,0.0);
	Eigen::Matrix3d M;
	
	unsigned int N = data_indices.size() + init_f.size();
	
	Eigen::MatrixXd model(3,N);
	Eigen::MatrixXd  data(3,N);
	
		
	//calcula os centroides
	int k=0;
	for(unsigned int i=0; i<N; i++){
		
		if (i<data_indices.size()){
			model(0,i) = cloud_m->points[ model_indices[i] ].x;
			model(1,i) = cloud_m->points[ model_indices[i] ].y;
			model(2,i) = cloud_m->points[ model_indices[i] ].z;
		
			data(0,i) = cloud_d->points[ data_indices[i] ].x*T(0,0) + cloud_d->points[ data_indices[i] ].y*T(0,1) + cloud_d->points[ data_indices[i] ].z*T(0,2) + T(0,3);
			data(1,i) = cloud_d->points[ data_indices[i] ].x*T(1,0) + cloud_d->points[ data_indices[i] ].y*T(1,1) + cloud_d->points[ data_indices[i] ].z*T(1,2) + T(1,3);
			data(2,i) = cloud_d->points[ data_indices[i] ].x*T(2,0) + cloud_d->points[ data_indices[i] ].y*T(2,1) + cloud_d->points[ data_indices[i] ].z*T(2,2) + T(2,3);
		}else{

			model(0,i) = cloud_m->points[ init_f[k].first ].x;
			model(1,i) = cloud_m->points[ init_f[k].first ].y;
			model(2,i) = cloud_m->points[ init_f[k].first ].z;
		
			data(0,i) = cloud_d->points[ init_f[k].second ].x*T(0,0) + cloud_d->points[ init_f[k].second ].y*T(0,1) + cloud_d->points[ init_f[k].second ].z*T(0,2) + T(0,3);
			data(1,i) = cloud_d->points[ init_f[k].second ].x*T(1,0) + cloud_d->points[ init_f[k].second ].y*T(1,1) + cloud_d->points[ init_f[k].second ].z*T(1,2) + T(1,3);
			data(2,i) = cloud_d->points[ init_f[k].second ].x*T(2,0) + cloud_d->points[ init_f[k].second ].y*T(2,1) + cloud_d->points[ init_f[k].second ].z*T(2,2) + T(2,3);	
			k++;
		}
		
		centroide_model += model.block(0,i,3,1);
		centroide_data  +=  data.block(0,i,3,1);
	}
	
	centroide_data = centroide_data/N;
	centroide_model = centroide_model/N;
	
	
  //subtrai os centroides aos dados
	for (unsigned int i=0; i<N; i++){
		model.block(0,i,3,1) -= centroide_model;
		data.block(0,i,3,1) -= centroide_data;
	}
	
	
	//Determina a transformacao
	M = data*model.transpose();
	
	Eigen::JacobiSVD<Eigen::Matrix3d> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
	
	Eigen::Matrix3d U = svd.matrixU();
	Eigen::Matrix3d V = svd.matrixV();
	
	if (U.determinant()*V.determinant()<0) {
		for (int i=0; i<3; ++i) 
			V(i,2) *=-1;
	}
	
		
	Eigen::Matrix3d r = V * U.transpose();
	Eigen::Vector3d t = centroide_model - r * centroide_data;
	
	//~ T.block<3,3>(0,0) =  r*T.block<3,3>(0,0);
	//~ T.block<3,1>(0,3) += t; 
	T.block<3,1>(0,3) = T.block<3,3>(0,0)*t + T.block<3,1>(0,3); 
	T.block<3,3>(0,0) = T.block<3,3>(0,0)*r;
	
	
} 

bool frameFieldBackgroundMesh2D::compute_RK_infos(double u,double v, double x, double y, double z, RK_form &infos)
{
    // check if point is in domain
    if (!inDomain(u,v)) return false;

    // get stored angle

    double angle_current = angle(u,v);

    // compute t1,t2: cross field directions

    // get the unit normal at that point
    GFace *face = dynamic_cast<GFace*>(gf);
    if(!face) {
        Msg::Error("Entity is not a face in background mesh");
        return false;
    }

    Pair<SVector3, SVector3> der = face->firstDer(SPoint2(u,v));
    SVector3 s1 = der.first();
    SVector3 s2 = der.second();
    SVector3 n = crossprod(s1,s2);
    n.normalize();
    SVector3 basis_u = s1;
    basis_u.normalize();
    SVector3 basis_v = crossprod(n,basis_u);
    // normalize vector t1 that is tangent to gf at uv
    SVector3 t1 = basis_u * cos(angle_current) + basis_v * sin(angle_current) ;
    t1.normalize();
    // compute the second direction t2 and normalize (t1,t2,n) is the tangent frame
    SVector3 t2 = crossprod(n,t1);
    t2.normalize();

    // get metric

    double L = size(u,v);
    infos.metricField = SMetric3(1./(L*L));
    FieldManager *fields = gf->model()->getFields();
    if(fields->getBackgroundField() > 0) {
        Field *f = fields->get(fields->getBackgroundField());
        if (!f->isotropic()) {
            (*f)(x,y,z, infos.metricField,gf);
        }
        else {
            L = (*f)(x,y,z,gf);
            infos.metricField = SMetric3(1./(L*L));
        }
    }
    double M = dot(s1,s1);
    double N = dot(s2,s2);
    double E = dot(s1,s2);
    // compute the first fundamental form i.e. the metric tensor at the point
    // M_{ij} = s_i \cdot s_j
    double metric[2][2] = {{M,E},{E,N}};

    // get sizes

    double size_1 = sqrt(1. / dot(t1,infos.metricField,t1));
    double size_2 = sqrt(1. / dot(t2,infos.metricField,t2));

    // compute covariant coordinates of t1 and t2 - cross field directions in parametric domain
    double covar1[2],covar2[2];
    // t1 = a s1 + b s2 -->
    // t1 . s1 = a M + b E
    // t1 . s2 = a E + b N --> solve the 2 x 2 system
    // and get covariant coordinates a and b
    double rhs1[2] = {dot(t1,s1),dot(t1,s2)};
    bool singular = false;
    if (!sys2x2(metric,rhs1,covar1)) {
        Msg::Info("Argh surface %d %g %g %g -- %g %g %g -- %g %g",gf->tag(),s1.x(),s1.y(),s1.z(),s2.x(),s2.y(),s2.z(),size_1,size_2);
        covar1[1] = 1.0;
        covar1[0] = 0.0;
        singular = true;
    }
    double rhs2[2] = {dot(t2,s1),dot(t2,s2)};
    if (!sys2x2(metric,rhs2,covar2)) {
        Msg::Info("Argh surface %d %g %g %g -- %g %g %g",gf->tag(),s1.x(),s1.y(),s1.z(),s2.x(),s2.y(),s2.z());
        covar2[0] = 1.0;
        covar2[1] = 0.0;
        singular = true;
    }

    // transform the sizes with respect to the metric
    // consider a vector v of size 1 in the parameter plane
    // its length is sqrt (v^T M v) --> if I want a real size
    // of size1 in direction v, it should be sqrt(v^T M v) * size1
    double l1 = sqrt(covar1[0]*covar1[0]+covar1[1]*covar1[1]);
    double l2 = sqrt(covar2[0]*covar2[0]+covar2[1]*covar2[1]);

    covar1[0] /= l1;
    covar1[1] /= l1;
    covar2[0] /= l2;
    covar2[1] /= l2;

    double size_param_1  = size_1 / sqrt (  M*covar1[0]*covar1[0]+
                                            2*E*covar1[1]*covar1[0]+
                                            N*covar1[1]*covar1[1]);
    double size_param_2  = size_2 / sqrt (  M*covar2[0]*covar2[0]+
                                            2*E*covar2[1]*covar2[0]+
                                            N*covar2[1]*covar2[1]);
//.........这里部分代码省略.........

void frameFieldBackgroundMesh2D::computeCrossField(simpleFunction<double> &eval_diffusivity)
{
    angles.clear();

    DoubleStorageType _cosines4,_sines4;

    list<GEdge*> e;
    GFace *face = dynamic_cast<GFace*>(gf);
    if(!face) {
        Msg::Error("Entity is not a face in background mesh");
        return;
    }

    replaceMeshCompound(face, e);

    list<GEdge*>::const_iterator it = e.begin();

    for( ; it != e.end(); ++it ) {
        if (!(*it)->isSeam(face)) {
            for(unsigned int i = 0; i < (*it)->lines.size(); i++ ) {
                MVertex *v[2];
                v[0] = (*it)->lines[i]->getVertex(0);
                v[1] = (*it)->lines[i]->getVertex(1);
                SPoint2 p1,p2;
                reparamMeshEdgeOnFace(v[0],v[1],face,p1,p2);
                Pair<SVector3, SVector3> der = face->firstDer((p1+p2)*.5);
                SVector3 t1 = der.first();
                SVector3 t2 = der.second();
                SVector3 n = crossprod(t1,t2);
                n.normalize();
                SVector3 d1(v[1]->x()-v[0]->x(),v[1]->y()-v[0]->y(),v[1]->z()-v[0]->z());
                t1.normalize();
                d1.normalize();
                double _angle = myAngle (t1,d1,n);
                normalizeAngle (_angle);
                for (int i=0; i<2; i++) {
                    DoubleStorageType::iterator itc = _cosines4.find(v[i]);
                    DoubleStorageType::iterator its = _sines4.find(v[i]);
                    if (itc != _cosines4.end()) {
                        itc->second  = 0.5*(itc->second + cos(4*_angle));
                        its->second  = 0.5*(its->second + sin(4*_angle));
                    }
                    else {
                        _cosines4[v[i]] = cos(4*_angle);
                        _sines4[v[i]] = sin(4*_angle);
                    }
                }
            }
        }
    }

    propagateValues(_cosines4,eval_diffusivity,false);
    propagateValues(_sines4,eval_diffusivity,false);

    std::map<MVertex*,MVertex*>::iterator itv2 = _2Dto3D.begin();
    for ( ; itv2 != _2Dto3D.end(); ++itv2) {
        MVertex *v_2D = itv2->first;
        MVertex *v_3D = itv2->second;
        double angle = atan2(_sines4[v_3D],_cosines4[v_3D]) / 4.0;
        normalizeAngle (angle);
        angles[v_2D] = angle;
    }
}

//.........这里部分代码省略.........
        string s = "b=1;c=2;a=a1:1,a2:2;a=a2:3,a3:4|ddeee";
        r.m_body = s;

        QA* qa = dat.Match(&r, true, "");
        RRMessage resp;
        bool res = qa->Answer(&resp, &r);
        string e;
        string name = "";
        {
            AddressBook ab;
            {
                Person* p = ab.add_person();
                p->set_name("zhangsan");
                p->set_id(3333);
            }
            ab.SerializeToString(&name);
        }
        {
            AddressBook ab;
            {
                Person* p = ab.add_person();
                p->set_name(name);
                p->set_id(123);
            }
            ab.SerializeToString(&e);
        }

        EXPECT_EQ(e, resp.m_body);
        EXPECT_EQ(true, res);
    }
    {
        RRMessage r;
        string s;
        Pair p;
        p.set_key("3;abc12");
        p.set_value(111);
        p.SerializeToString(&s);
        r.m_body = s;

        QA* qa = dat.Match(&r, true, "");
        RRMessage resp;
        bool res = qa->Answer(&resp, &r);
        EXPECT_EQ("a==aa;email==xx-132-100", resp.m_body);
        EXPECT_EQ(true, res);
    }
    {
        RRMessage r;
        string s;
        AddressBook ab;

        Person* p = ab.add_person();
        p->set_name("zhangsan");
        p->set_id(3);
        p->set_email("[email protected]");
        Person::PhoneNumber* pn = p->add_phone();
        pn->set_number("010-111");
        pn->set_type(Person::MOBILE);

        ab.SerializeToString(&s);
        r.m_body = s;

        QA* qa = dat.Match(&r, true, "");
        RRMessage resp;
        bool res = qa->Answer(&resp, &r);
        EXPECT_EQ("mytypethis is pb", resp.m_body);
        EXPECT_EQ(true, res);

void pair_Int_Int(){
    Pair<int,int> *pair = new Pair<int,int>(100, 75);
    cout << "Pair < "<< pair->component1() << " , " << pair->component2() <<" >" << endl;
}