C++ P0函数代码示例

int main() {
  __CPROVER_ASYNC_0: P0(0);
  __CPROVER_ASYNC_1: P1(0);
  __CPROVER_ASYNC_2: P2(0);
  __CPROVER_assume(__unbuffered_cnt==3);
  fence();
  // EXPECT:exists
  __CPROVER_assert(!(y==2 && __unbuffered_p1_r1==2 && __unbuffered_p2_r1==1 && __unbuffered_p2_r3==0), "Program was expected to be safe for PPC, model checker should have said NO.\nThis likely is a bug in the tool chain.");
  return 0;
}

int main() {
  __CPROVER_ASYNC_0: P0(0);
  __CPROVER_ASYNC_1: P1(0);
  __CPROVER_ASYNC_2: P2(0);
  __CPROVER_assume(__unbuffered_cnt==3);
  fence();
  // EXPECT:exists
  __CPROVER_assert(!(x==2 && y==2 && __unbuffered_p2_r3==0), "Program proven to be relaxed for PPC, model checker says YES.");
  return 0;
}

AREXPORT ArMapObject::ArMapObject(const char *type, 
					                        ArPose pose, 
                                  const char *description,
 		                              const char *iconName, 
                                  const char *name,
 		                              bool hasFromTo, 
                                  ArPose fromPose, 
                                  ArPose toPose) :
  myType((type != NULL) ? type : ""),
  myBaseType(),
  myName((name != NULL) ? name : "" ),
  myDescription((description != NULL) ? description : "" ),
  myPose(pose),
  myIconName((iconName != NULL) ? iconName : "" ),
  myHasFromTo(hasFromTo),
  myFromPose(fromPose),
  myToPose(toPose),
  myFromToSegments(),
  myStringRepresentation()
{
  if (myHasFromTo)
  {
    double angle = myPose.getTh();
    double sa = ArMath::sin(angle);
    double ca = ArMath::cos(angle);
    double fx = fromPose.getX();
    double fy = fromPose.getY();
    double tx = toPose.getX();
    double ty = toPose.getY();
    ArPose P0((fx*ca - fy*sa), (fx*sa + fy*ca));
    ArPose P1((tx*ca - fy*sa), (tx*sa + fy*ca));
    ArPose P2((tx*ca - ty*sa), (tx*sa + ty*ca));
    ArPose P3((fx*ca - ty*sa), (fx*sa + ty*ca));
    myFromToSegments.push_back(ArLineSegment(P0, P1));
    myFromToSegments.push_back(ArLineSegment(P1, P2));
    myFromToSegments.push_back(ArLineSegment(P2, P3));
    myFromToSegments.push_back(ArLineSegment(P3, P0));
    
    myFromToSegment.newEndPoints(fromPose, toPose);
  }
  else { // pose
    size_t whPos = myType.rfind("WithHeading");
    size_t whLen = 11;
    if (whPos > 0) {
      if (whPos == myType.size() - whLen) {
        myBaseType = myType.substr(0, whPos);
      }
    }
  } // end else pose

  IFDEBUG(
  ArLog::log(ArLog::Normal, 
             "ArMapObject::ctor() created %s (%s)",
             myName.c_str(), myType.c_str());
  );

int main() {
  __CPROVER_ASYNC_0: P0(0);
  __CPROVER_ASYNC_1: P1(0);
  __CPROVER_ASYNC_2: P2(0);
  __CPROVER_ASYNC_3: P3(0);
  __CPROVER_assume(__unbuffered_cnt==4);
  fence();
  // EXPECT:exists
  __CPROVER_assert(!(x==2 && z==2 && __unbuffered_p1_EAX==0 && __unbuffered_p3_EAX==2 && __unbuffered_p3_EBX==0), "Program proven to be relaxed for X86, model checker says YES.");
  return 0;
}

PFilter::PFilter(std::size_t x_size, std::size_t s_size) :
        Sample_state_filter (x_size, s_size),
        Kalman_state_filter(x_size),
        SIR_kalman_scheme(x_size, s_size, rnd),
        w(wir)
{
    PFilter::x_size = x_size;
    FM::Vec x0(x_size);
    x0.clear();
    FM::SymMatrix P0(x_size, x_size);
    P0.clear();
    init(x0, P0);
}

void PolynomialEpipolaireCoordinate::write(class  ELISE_fp & aFile) const
{
   aFile.write(P0());
   aFile.write(DirX());
   aFile.write(TrFin());
   
   aFile.write(mPolToYEpip);
   aFile.write(AmplInv());
   aFile.write(DeltaDegre());

   aFile.write(bool(mGridCor!=0));
   if (mGridCor!=0)
      mGridCor->write(aFile);
}

Point Rectangle::Sample(float u, float v, Normal *Ns) const {

    // u and v are random samples on the surface of the light source
    Point P0(-x/2, y/2, height), P1(x/2, y/2, height);
    Point P2(-x/2, -y/2, height), P3(x/2, -y/2, height);

    Point p = P0 + (P1 - P0) * u + (P2 - P0) * v;

    Normal n = Normal(Cross(P2-P0, P1-P0));
    *Ns = Normalize(n);

    // NORMAL ON THE PLANE
    *Ns = Normalize((*ObjectToWorld)(n));
    if (ReverseOrientation) *Ns *= -1.f;
    return (*ObjectToWorld)(p);
}

void srTSpherMirror::SetupInAndOutPlanes(TVector3d* InPlane, TVector3d* OutPlane)
{// Assumes InPlaneNorm and transverse part InPlaneCenPo already defined !!!
	TVector3d &InPlaneCenPo = *InPlane, &InPlaneNorm = InPlane[1];
	TVector3d &OutPlaneCenPo = *OutPlane, &OutPlaneNorm = OutPlane[1];

	TVector3d LocInPlaneNorm = InPlaneNorm;
	FromLabToLocFrame_Vector(LocInPlaneNorm);

	double xP = -0.5*Dx, yP = -0.5*Dy;
	TVector3d P0(xP, yP, SurfaceFunction(xP, yP, 0));
	TVector3d P1(-xP, yP, SurfaceFunction(-xP, yP, 0));
	TVector3d P2(xP, -yP, SurfaceFunction(xP, -yP, 0));
	TVector3d P3(-xP, -yP, SurfaceFunction(-xP, -yP, 0));
	TVector3d EdgePoints[] = { P0, P1, P2, P3 };

	int LowestInd, UppestInd;
	FindLowestAndUppestPoints(LocInPlaneNorm, EdgePoints, 4, LowestInd, UppestInd);

	TVector3d PointForInPlane = EdgePoints[LowestInd];
	FromLocToLabFrame_Point(PointForInPlane);
	InPlaneCenPo.y = PointForInPlane.y;

	TVector3d LocInPlaneCenPo = InPlaneCenPo;
	FromLabToLocFrame_Point(LocInPlaneCenPo);
	TVector3d LocInPlane[] = { LocInPlaneCenPo, LocInPlaneNorm };

	TVector3d LocP, LocN;
	FindRayIntersectWithSurface(LocInPlane, 0, LocP);
	SurfaceNormalAtPoint(LocP.x, LocP.y, 0, LocN);

	TVector3d LocOutPlaneNorm = LocInPlaneNorm;
	ReflectVect(LocN, LocOutPlaneNorm);
	FindLowestAndUppestPoints(LocOutPlaneNorm, EdgePoints, 4, LowestInd, UppestInd);
	OutPlaneCenPo = (EdgePoints[UppestInd]*LocOutPlaneNorm)*LocOutPlaneNorm;
	*OutPlaneInLocFrame = OutPlaneCenPo; OutPlaneInLocFrame[1] = LocOutPlaneNorm;
	FromLocToLabFrame_Point(OutPlaneCenPo);
	OutPlaneNorm = LocOutPlaneNorm;
	FromLocToLabFrame_Vector(OutPlaneNorm);

	TVector3d LabN = LocN;
	FromLocToLabFrame_Vector(LabN);
	ExRefInLabFrameBeforeProp = TVector3d(1.,0.,0.);
	ReflectVect(LabN, ExRefInLabFrameBeforeProp);
	EzRefInLabFrameBeforeProp = TVector3d(0.,0.,1.);
	ReflectVect(LabN, EzRefInLabFrameBeforeProp);
}

void Centerline::buildKdTree()
{
  FILE * f = Fopen("myPOINTS.pos","w");
  fprintf(f, "View \"\"{\n");

  int nbPL = 3;  //10 points per line
  //int nbNodes  = (lines.size()+1) + (nbPL*lines.size());
  int nbNodes  = (colorp.size()) + (nbPL*lines.size());

  ANNpointArray nodes = annAllocPts(nbNodes, 3);
  int ind = 0;
  std::map<MVertex*, int>::iterator itp = colorp.begin();
  while (itp != colorp.end()){
     MVertex *v = itp->first;
     nodes[ind][0] = v->x();
     nodes[ind][1] = v->y();
     nodes[ind][2] = v->z();
     itp++; ind++;
  }
  for(unsigned int k = 0; k < lines.size(); ++k){
   MVertex *v0 = lines[k]->getVertex(0);
   MVertex *v1 = lines[k]->getVertex(1);
   SVector3 P0(v0->x(),v0->y(), v0->z());
   SVector3 P1(v1->x(),v1->y(), v1->z());
   for (int j= 1; j < nbPL+1; j++){
     double inc = (double)j/(double)(nbPL+1);
     SVector3 Pj = P0+inc*(P1-P0);
     nodes[ind][0] = Pj.x();
     nodes[ind][1] = Pj.y();
     nodes[ind][2] = Pj.z();
     ind++;
   }
 }

 kdtree = new ANNkd_tree(nodes, nbNodes, 3);

 for(int i = 0; i < nbNodes; ++i){
   fprintf(f, "SP(%g,%g,%g){%g};\n",
	   nodes[i][0], nodes[i][1],nodes[i][2],1.0);
 }
 fprintf(f,"};\n");
 fclose(f);
}

    /**
    * Returns the probability of static empty
    */
    float getFreeStaticLikelihood()
    {
        if( (b_entry_event + b_exit_event)<20.0 )
        {
            return 0.5f;
        }
        Eigen::Matrix2f P;
        Eigen::Vector2f P0;
        Eigen::Vector2f u1(0.5, 0.5);
        float Lex = exitL();
        float Len = entryL();
        P(0,0) = (1.0-Len);
        P(0,1) =  Len;
        P(1,0)	 = Lex;
        P(1,1) = (1-Lex);

        P0 = u1.transpose() * P;
        return (P0(0));
    }

AREXPORT void ArForbiddenRangeDevice::processMap(void)
{
  std::list<ArMapObject *>::const_iterator it;
  ArMapObject *obj;

  myDataMutex.lock();
  ArUtil::deleteSet(mySegments.begin(), mySegments.end());
  mySegments.clear();

  for (it = myMap->getMapObjects()->begin();
       it != myMap->getMapObjects()->end();
       it++)
  {
    obj = (*it);
    if (strcmp(obj->getType(), "ForbiddenLine") == 0 &&
	obj->hasFromTo())
    {
      mySegments.push_back(new ArLineSegment(obj->getFromPose(), 
					     obj->getToPose()));
    }
    if (strcmp(obj->getType(), "ForbiddenArea") == 0 &&
	obj->hasFromTo())
    {
      double angle = obj->getPose().getTh();
      double sa = ArMath::sin(angle);
      double ca = ArMath::cos(angle);
      double fx = obj->getFromPose().getX();
      double fy = obj->getFromPose().getY();
      double tx = obj->getToPose().getX();
      double ty = obj->getToPose().getY();
      ArPose P0((fx*ca - fy*sa), (fx*sa + fy*ca));
      ArPose P1((tx*ca - fy*sa), (tx*sa + fy*ca));
      ArPose P2((tx*ca - ty*sa), (tx*sa + ty*ca));
      ArPose P3((fx*ca - ty*sa), (fx*sa + ty*ca));
      mySegments.push_back(new ArLineSegment(P0, P1));
      mySegments.push_back(new ArLineSegment(P1, P2));
      mySegments.push_back(new ArLineSegment(P2, P3));
      mySegments.push_back(new ArLineSegment(P3, P0));
    }
  }
  myDataMutex.unlock();
}

int main(int argc, char* argv[]) {
	srand(time(0));
	point_pf::initialize();

	Matrix<X_DIM> x0, xGoal;
	x0[0] = -3.5; x0[1] = 2;
	xGoal[0] = -3.5; xGoal[1] = -2;

	SymmetricMatrix<X_DIM> Sigma0 = .01*identity<X_DIM>();

	Matrix<U_DIM> u = (xGoal - x0) / (DT*(T-1));

	std::vector<Matrix<X_DIM> > P0(M);
	for(int m=0; m < M; ++m) {
		P0[m] = sampleGaussian(x0, Sigma0);
	}

	std::vector<std::vector<Matrix<X_DIM>> > P_t(T);
	std::vector<Matrix<Q_DIM> > dyn_noise;
	std::vector<Matrix<R_DIM> > obs_noise;
	float sampling_noise;
	P_t[0] = P0;
	for(int t=0; t < T-1; ++t) {
		dyn_noise = sampleGaussianN(zeros<Q_DIM,1>(), Q, M);
		obs_noise = sampleGaussianN(zeros<R_DIM,1>(), R, M);
		sampling_noise = (1/float(M))*(rand() / float(RAND_MAX));
		P_t[t+1] = point_pf::beliefDynamics(P_t[t], u, dyn_noise, obs_noise, sampling_noise);
		//P_t[t+1] = point_pf::casadiBeliefDynamics(P_t[t], u, dyn_noise, obs_noise, sampling_noise);
	}

	for(int t=0; t < T; ++t) {
		std::cout << "\nt: " << t << "\n";
		for(int m=0; m < M; ++m) {
			std::cout << ~P_t[t][m];
		}
	}

	point_pf::pythonDisplayParticles(P_t);

	return 0;
}

PolynomialEpipolaireCoordinate * 
    PolynomialEpipolaireCoordinate::PolMapingChScale
   (REAL aChSacle) const
{
     PolynomialEpipolaireCoordinate * aRes =
	           new PolynomialEpipolaireCoordinate
		   (
		        P0() * aChSacle,
			DirX(),
                        mPolToYEpip.MapingChScale(aChSacle),
			AmplInv() * aChSacle,
			DeltaDegre(),
                        TrFin() * aChSacle

		   );

    if (mGridCor)
      aRes->mGridCor  = mGridCor->NewChScale(aChSacle,true);

    return aRes;
}

float Rectangle::Pdf(const Point &p, const Vector &wi) const
{
    // Intersect sample ray with area light geometry
    DifferentialGeometry dgLight;
    Ray ray(p, wi, 1e-3f);
    ray.depth = -1; // temporary hack to ignore alpha mask
    float thit, rayEpsilon;
    if (!Intersect(ray, &thit, &rayEpsilon, &dgLight)) return 0.;

    float pdf;
    Point pObject = (*WorldToObject)(p);

    // TODO: Check with the normal (n.p)
    if (!(pObject.x < x/2 && pObject.x > -x/2
            && pObject.y < y/2 && pObject.y > -y/2))
        return 0;

    // N & wi
    Point P0(-x/2, y/2, height);
    Point P1(x/2, y/2, height);
    Point P2(-x/2, -y/2, height);

    Point P0_W = (*WorldToObject)(P0);
    Point P1_W = (*WorldToObject)(P1);
    Point P2_W = (*WorldToObject)(P2);


    Normal n = Normal(Cross(P2_W-P0_W, P1_W-P0_W));
    Normal Ns = Normalize(n);

    Normal Nwi(wi.x, wi.y, wi.z);

    if (Dot(Ns,  Nwi) < 0.0010)
        pdf = 1.0;

    if (isinf(pdf)) pdf = 0.f;
        return pdf;
}

void ConstitutiveModelDriver<EvalT, Traits>::
evaluateFields(typename Traits::EvalData workset)
{
  bool print = false;
  if (typeid(ScalarT) == typeid(RealType)) print = true;
  std::cout.precision(15);

  std::cout << "ConstitutiveModelDriver<EvalT, Traits>::evaluateFields" << std::endl;
  Intrepid2::Tensor<ScalarT> F(num_dims_), P(num_dims_), sig(num_dims_);

  Intrepid2::Tensor<ScalarT> F0(num_dims_), P0(num_dims_);

  for (int cell = 0; cell < workset.numCells; ++cell) {
    for (int pt = 0; pt < num_pts_; ++pt) {
      F0.fill(prescribed_def_grad_,cell,pt,0,0);
      F.fill(def_grad_,cell,pt,0,0);
      sig.fill(stress_,cell,pt,0,0);
      P = Intrepid2::piola(F,sig);
      if (print) {
        std::cout << "F: \n" << F << std::endl;
        std::cout << "P: \n" << P << std::endl;
        std::cout << "sig: \n" << sig << std::endl;
      }
      for (int node = 0; node < num_nodes_; ++node) {
        for (int dim1 = 0; dim1 < num_dims_; ++dim1) {
          for (int dim2 = 0; dim2 < num_dims_; ++dim2) {
            residual_(cell,node,dim1,dim2) = 
              (F(dim1,dim2) - F0(dim1,dim2));
            //* (P(dim1,dim2) - P0(dim1,dim2));
          }
        }
      }
    }
  }

}