C++ arr::append方法代码示例

void SocSystem_Analytical::getConstraints(arr& cdir,arr& coff,uint t,const arr& qt){
  cdir.clear();
  coff.clear();
#ifndef USE_TRUNCATION
   return;
#endif
  uint i,M=obstacles.d0;
  arr d;
  
#if 0 //direct and clean way to do it -- but depends simple scenario
  cdir.resize(M,x.N);
  coff.resize(M);
  for(i=0;i<M;i++){
    cdir[i] = qt-obstacles[i];
    coff(i) = scalarProduct(cdir[i],obstacles[i]);
  }
#elif 1 //assume that the task vector is a list of scalars, each constrained >0
  arr J,y;
  for(i=0;i<M;i++){
    real haty = norm(x-obstacles[i]);
    if(haty>.5) continue; //that's good enough -> don't add the constraint
    J = (x-obstacles[i])/norm(x-obstacles[i]);
    coff.append(-haty + scalarProduct(J,x));
    cdir.append(J);
  }
  cdir.reshape(coff.N,x.N);
  coff.reshape(coff.N);
#else //messy: try to combine all constraints into a single scalar, doesn't really work...
  //first compute squared collision meassure...
  arr J(1,qt.N),phiHatQ(1);
  J.setZero();
  phiHatQ.setZero();
  for(i=0;i<obstacles.d0;i++){
    real margin = .25;
    real d = 1.-norm(x-obstacles[i])/margin;
    //if(d<0) continue;
    //phiHatQ(0) += d*d;
    //J += (2.*d/margin)*(obstacles[i]-x)/norm(x-obstacles[i]);
    phiHatQ(0) += d;
    J += (1./margin)*(obstacles[i]-x)/norm(x-obstacles[i]);
  }
  //...then add a single constraint
  if(phiHatQ(0)>0.){ //potential violation, else discard
    cdir.append(-J);
    coff.append(phiHatQ-scalarProduct(J,x)-1.);
    cdir.reshape(1,x.N);
    coff.reshape(1);
  }
#endif
}

void ParticleAroundWalls::phi_t(arr& phi, arr& J, uint t, const arr& x_bar){
  uint T=get_T(), n=dim_x(), k=get_k();

  //assert some dimensions
  CHECK(x_bar.d0==k+1,"");
  CHECK(x_bar.d1==n,"");
  CHECK(t<=T,"");

  //-- transition costs: append to phi
  if(k==1)  phi = x_bar[1]-x_bar[0]; //penalize velocity
  if(k==2)  phi = x_bar[2]-2.*x_bar[1]+x_bar[0]; //penalize acceleration
  if(k==3)  phi = x_bar[3]-3.*x_bar[2]+3.*x_bar[1]-x_bar[0]; //penalize jerk

  //-- walls: append to phi
  //Note: here we append to phi ONLY in certain time slices: the dimensionality of phi may very with time slices; see dim_phi(uint t)
  double eps=.1, power=2.;
  if(!hardConstrained){
    //-- wall costs
    for(uint i=0;i<n;i++){ //add barrier costs to each dimension
      if(t==T/4)   phi.append(MT::ineqConstraintCost(i+1.-x_bar(k,i), eps, power));  //middle factor: ``greater than i''
      if(t==T/2)   phi.append(MT::ineqConstraintCost(x_bar(k,i)+i+1., eps, power));  //last factor: ``lower than -i''
      if(t==3*T/4) phi.append(MT::ineqConstraintCost(i+1.-x_bar(k,i), eps, power));  //middle factor: ``greater than i''
      if(t==T)     phi.append(MT::ineqConstraintCost(x_bar(k,i)+i+1., eps, power));  //last factor: ``lower than -i''
    }
  }else{
    //-- wall constraints
    for(uint i=0;i<n;i++){ //add barrier costs to each dimension
      if(t==T/4)   phi.append((i+1.-x_bar(k,i)));  //middle factor: ``greater than i''
      if(t==T/2)   phi.append((x_bar(k,i)+i+1.));  //last factor: ``lower than -i''
      if(t==3*T/4) phi.append((i+1.-x_bar(k,i)));  //middle factor: ``greater than i''
      if(t==T)     phi.append((x_bar(k,i)+i+1.));  //last factor: ``lower than -i''
    }
  }

  uint m=phi.N;
  CHECK(m==dim_phi(t),"");

  if(&J){ //we also need to return the Jacobian
    J.resize(m,k+1,n).setZero();

    //-- transition costs
    for(uint i=0;i<n;i++){
      if(k==1){ J(i,1,i) = 1.;  J(i,0,i) = -1.; }
      if(k==2){ J(i,2,i) = 1.;  J(i,1,i) = -2.;  J(i,0,i) = 1.; }
      if(k==3){ J(i,3,i) = 1.;  J(i,2,i) = -3.;  J(i,1,i) = +3.;  J(i,0,i) = -1.; }
    }

    //-- walls
    if(!hardConstrained){
      for(uint i=0;i<n;i++){
        if(t==T/4)   J(n+i,k,i) = -MT::d_ineqConstraintCost(i+1.-x_bar(k,i), eps, power);
        if(t==T/2)   J(n+i,k,i) =  MT::d_ineqConstraintCost(x_bar(k,i)+i+1., eps, power);
        if(t==3*T/4) J(n+i,k,i) = -MT::d_ineqConstraintCost(i+1.-x_bar(k,i), eps, power);
        if(t==T)     J(n+i,k,i) =  MT::d_ineqConstraintCost(x_bar(k,i)+i+1., eps, power);
      }
    }else{
      for(uint i=0;i<n;i++){
        if(t==T/4)   J(n+i,k,i) = -1.;
        if(t==T/2)   J(n+i,k,i) = +1.;
        if(t==3*T/4) J(n+i,k,i) = -1.;
        if(t==T)     J(n+i,k,i) = +1.;
      }
    }
  }
}