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

real SocSystem_Analytical::getCosts(arr& R,arr& r,uint t,const arr& qt){
  uint N=x.N;
  R.resize(N,N); R.setZero();
  r.resize(N);   r.setZero();
  real C=0.;
  
#ifndef USE_TRUNCATION //potentials for collision cost
  arr J(1,qt.N),phiHatQ(1);
  J.setZero();
  phiHatQ.setZero();
  for(uint i=0;i<obstacles.d0;i++){
    real margin = .1;
    real d = (1.-norm(x-obstacles[i])/margin);
    if(d<0) continue;
    phiHatQ(0) += d*d;
    J += ((real)2.*d/margin)*(obstacles[i]-x)/norm(x-obstacles[i]);
  }
  J.reshape(1,J.N);
  arr tJ,target(1);
  target=(real)0.;
  transpose(tJ,J);
  real colprec = (real)5e2;
  C += colprec*sqrDistance(target,phiHatQ);
  R += colprec*tJ*J;
  r += colprec*tJ*(target - phiHatQ + J*qt);
#endif
  
  if(t!=T-1) return C;
  R.setDiag(1.);
  r = x1;
  R *= prec;
  r *= prec;
  C += prec*sqrDistance(x1,x);
  return C;
}

bool sRprop::step(arr& w, const arr& grad, uint *singleI) {
    if(!stepSize.N) { //initialize
        stepSize.resize(w.N);
        lastGrad.resize(w.N);
        lastGrad.setZero();
        stepSize = delta0;
    }
    CHECK(grad.N==stepSize.N, "Rprop: gradient dimensionality changed!");
    CHECK(w.N==stepSize.N   , "Rprop: parameter dimensionality changed!");

    uint i=0, I=w.N;
    if(singleI) {
        i=*(singleI);
        I=i+1;
    }
    for(; i<I; i++) {
        if(grad.elem(i) * lastGrad(i) > 0) { //same direction as last time
            if(rMax) dMax=fabs(rMax*w.elem(i));
            stepSize(i) = _mymin(dMax, incr * stepSize(i)); //increase step size
            w.elem(i) += stepSize(i) * -_sgn(grad.elem(i)); //step in right direction
            lastGrad(i) = grad.elem(i);                    //memorize gradient
        } else if(grad.elem(i) * lastGrad(i) < 0) { //change of direction
            stepSize(i) = _mymax(dMin, decr * stepSize(i)); //decrease step size
            w.elem(i) += stepSize(i) * -_sgn(grad.elem(i)); //step in right direction (undo half the step)
            lastGrad(i) = 0;                               //memorize to continue below next time
        } else {                              //after change of direcion
            w.elem(i) += stepSize(i) * -_sgn(grad.elem(i)); //step in right direction
            lastGrad(i) = grad.elem(i);                    //memorize gradient
        }
    }

    return stepSize.max() < incr*dMin;
}

 virtual double fs(arr& g, arr& H, const arr& x) {
   double A=.5, f=A*x.N;
   for(uint i=0; i<x.N; i++) f += x(i)*x(i) - A*::cos(10.*x(i));
   if(&g) {
     g.resize(x.N);
     for(uint i=0; i<x.N; i++) g(i) = 2*x(i) + 10.*A*::sin(10.*x(i));
   }
   if(&H) {
     H.resize(x.N,x.N);  H.setZero();
     for(uint i=0; i<x.N; i++) H(i,i) = 2 + 100.*A*::cos(10.*x(i));
   }
   return f;
 }

 //initialization methods
 void initKinematic(uint dim,uint trajectory_length, real w, real endPrec){
   x0.resize(dim); x0.setZero();
   x1=x0; x1(0)=1.;
   x=x0;
   W.setDiag(w,x.N);
   prec=endPrec;
   T=trajectory_length;
   obstacles.resize(2,x.N);
   obstacles(0,0)=.3; obstacles(0,1)=.05;
   obstacles(1,0)=.7; obstacles(1,1)=-.05;
   dynamic=false;
   //os = &cout;
 }

void soc::SocSystem_Ors::getMF(arr& M,arr& F){
  if(!WS->pseudoDynamic){
    if(WS->Qlin.N) NIY;
    ors->clearForces();
    ors->gravityToForces();
    //ors->frictionToForces(1.1);
    ors->equationOfMotion(M,F,WS->v_act);
    //M.setId();  F = .1;
    //Minv *= .2;//1e-1;
  }else{
    uint n=qDim();
    M.setId(n);
    F.resize(n); F.setZero();
  }
}

void SocSystem_Analytical::getProcess(arr& A,arr& a,arr& B){
  uint N=x.N;
  A.setDiag(1.,N);
  B.setDiag(1.,N);
  a.resize(N); a.setZero();
}