Example #1
0
static void execute_test(unsigned i)
{
  rtems_status_code sc = RTEMS_SUCCESSFUL;
  rtems_bdbuf_buffer *bd = NULL;
  bool suspend = false;

  print(
    1,
    "[%05u]T(%c,%c,%s,%c)L(%c,%s,%c)H(%c,%s,%c)\n",
    i,
    trig_style_desc [trig],
    get_type_desc [trig_get],
    blk_kind_desc [trig_blk],
    rel_type_desc [trig_rel],
    get_type_desc [low_get],
    blk_kind_desc [low_blk],
    rel_type_desc [low_rel],
    get_type_desc [high_get],
    blk_kind_desc [high_blk],
    rel_type_desc [high_rel]
  );

  set_task_prio(task_id_low, PRIORITY_LOW);

  finish_low = false;
  finish_high = false;

  sc = rtems_task_resume(task_id_low);
  ASSERT_SC(sc);

  sc = rtems_task_resume(task_id_high);
  ASSERT_SC(sc);

  sc = rtems_bdbuf_get(dd_b, 0, &bd);
  rtems_test_assert(sc == RTEMS_SUCCESSFUL && bd->dd == dd_b && bd->block == 0);

  sc = rtems_bdbuf_release(bd);
  ASSERT_SC(sc);

  switch (trig) {
    case SUSP:
      suspend = true;
      break;
    case CONT:
      suspend = false;
      break;
    default:
      rtems_test_assert(false);
      break;
  }

  print(2, "TG\n");
  bd = get(trig_get, trig_blk);

  if (suspend) {
    print(2, "S\n");
    resume = RESUME_IMMEDIATE;
    sc = rtems_task_suspend(RTEMS_SELF);
    ASSERT_SC(sc);
  }
  resume = RESUME_FINISH;

  print(2, "TR\n");
  rel(bd, trig_rel);

  sc = rtems_task_suspend(RTEMS_SELF);
  ASSERT_SC(sc);

  print(2, "F\n");

  sc = rtems_bdbuf_syncdev(dd_a);
  ASSERT_SC(sc);

  sc = rtems_bdbuf_syncdev(dd_b);
  ASSERT_SC(sc);
}
Example #2
0
 virtual void post(Home home, IntRelType irt, int c,
                   BoolVar b, IntConLevel icl) const {
     rel(home, post(home,NULL,icl), irt, c, b);
 }
Example #3
0
void ZOHTest::testMatrixIntegration4()
{
  std::cout << "===========================================" <<std::endl;
  std::cout << " ===== ZOH tests start ... ===== " <<std::endl;
  std::cout << "===========================================" <<std::endl;
  std::cout << "------- Integrate Orlov's controller  -------" <<std::endl;
  _h = .001;
  _A->zero();
  (*_A)(0, 1) = 1;
  _x0->zero();
  (*_x0)(0) = 1;
  (*_x0)(1) = 1;
  SP::SimpleMatrix B(new SimpleMatrix(_n, _n, 0));
  SP::SimpleMatrix C(new SimpleMatrix(_n, _n));
  (*B)(1, 0) = 2;
  (*B)(1, 1) = 1;
  C->eye();
  SP::FirstOrderLinearTIR rel(new FirstOrderLinearTIR(C, B));
  SP::SimpleMatrix D(new SimpleMatrix(_n, _n, 0));
  rel->setDPtr(D);
  SP::NonSmoothLaw nslaw(new RelayNSL(_n));
  _DS.reset(new FirstOrderLinearDS(_x0, _A, _b));
  _TD.reset(new TimeDiscretisation(_t0, _h));
  _model.reset(new Model(_t0, _T));
  SP::Interaction inter(new Interaction(_n, nslaw, rel));
  _ZOH.reset(new ZeroOrderHoldOSI());
  _model->nonSmoothDynamicalSystem()->insertDynamicalSystem(_DS);
  _model->nonSmoothDynamicalSystem()->topology()->setOSI(_DS, _ZOH);
  _model->nonSmoothDynamicalSystem()->link(inter, _DS);
  _model->nonSmoothDynamicalSystem()->setControlProperty(inter, true);
  _sim.reset(new TimeStepping(_TD, 1));
  _sim->insertIntegrator(_ZOH);
  SP::Relay osnspb(new Relay());
  _sim->insertNonSmoothProblem(osnspb);
  _model->setSimulation(_sim);
  _model->initialize();
  SimpleMatrix dataPlot((unsigned)ceil((_T - _t0) / _h) + 10, 7);
  SiconosVector& xProc = *_DS->x();
  SiconosVector& lambda = *inter->lambda(0);
  SiconosVector sampledControl(_n);
  unsigned int k = 0;
  dataPlot(0, 0) = _t0;
  dataPlot(0, 1) = (*_x0)(0);
  dataPlot(0, 2) = (*_x0)(1);
  dataPlot(0, 3) = 0;
  dataPlot(0, 4) = 0;
  dataPlot(0, 5) = 0;
  dataPlot(0, 6) = 0;
  while (_sim->hasNextEvent())
  {
    _sim->computeOneStep();
    prod(*B, lambda, sampledControl, true);
    k++;
    dataPlot(k, 0) = _sim->nextTime();
    dataPlot(k, 1) = xProc(0);
    dataPlot(k, 2) = xProc(1);
    dataPlot(k, 3) = sampledControl(0);
    dataPlot(k, 4) = sampledControl(1);
    dataPlot(k, 5) = lambda(0);
    dataPlot(k, 6) = lambda(1);
    _sim->nextStep();
  }
  dataPlot.resize(k, 7);
  dataPlot.display();
  std::cout <<std::endl <<std::endl;
  ioMatrix::write("testMatrixIntegration4.dat", "ascii", dataPlot, "noDim");
  // Reference Matrix
  SimpleMatrix dataPlotRef(dataPlot);
  dataPlotRef.zero();
  ioMatrix::read("testMatrixIntegration4.ref", "ascii", dataPlotRef);
  std::cout << "------- Integration Ok, error = " << (dataPlot - dataPlotRef).normInf() << " -------" <<std::endl;
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testMatrixExp4 : ", (dataPlot - dataPlotRef).normInf() < _tol, true);
}
Example #4
0
void
sfsconst_init (bool lite_mode)
{
  if (const_set)
    return;
  const_set = true;

  {
    char *p = safegetenv ("SFS_RELEASE");
    if (!p || !convertint (p, &sfs_release)) {
      str rel (strbuf () << "SFS_RELEASE=" << sfs_release);
      xputenv (const_cast<char *> (rel.cstr ()));
    }
  }

#ifdef MAINTAINER
  if (char *p = safegetenv ("SFS_RUNINPLACE")) {
    runinplace = true;
    builddir = p;
    buildtmpdir = builddir << "/runinplace";
  }
  if (char *p = safegetenv ("SFS_ROOT"))
    if (*p == '/')
      sfsroot = p;
#endif /* MAINTAINER */
  sfsdevdb = strbuf ("%s/.devdb", sfsroot);

#ifdef MAINTAINER
  if (runinplace) {
    sfsdir = buildtmpdir;
    sfssockdir = sfsdir;
    etc3dir = etc1dir;
    etc1dir = sfsdir;
    etc2dir = xstrdup ((str (builddir << "/etc")).cstr());
  }
#endif /* MAINTAINER */
  if (char *ps = safegetenv ("SFS_PORT"))
    if (int pv = atoi (ps))
      sfs_defport = pv;

  str sfs_config = safegetenv ("SFS_CONFIG");
  if (sfs_config && sfs_config[0] == '/') {
    if (!parseconfig (NULL, sfs_config.cstr()))
      fatal << sfs_config << ": " << strerror (errno) << "\n";
  }
  else {
    if (!parseconfig (etc3dir, sfs_config.cstr())) {
      parseconfig (etc3dir, "sfs_config");
      if (!parseconfig (etc2dir, sfs_config.cstr())) {
	parseconfig (etc2dir, "sfs_config");
	if (!parseconfig (etc1dir, sfs_config.cstr())) 
	  parseconfig (etc1dir, "sfs_config");
      }
    }
  }

  if (!lite_mode) {
    if (!sfs_uid)
      idlookup (NULL, NULL);
  }

  if (char *p = getenv ("SFS_HASHCOST")) {
    sfs_hashcost = strtoi64 (p);
    if (sfs_hashcost > sfs_maxhashcost)
      sfs_hashcost = sfs_maxhashcost;
  }

  if (!getuid () && !runinplace) {
    mksfsdir (sfsdir, 0755);
    mksfsdir (sfssockdir, 0750);
  }
  else if (runinplace && access (sfsdir.cstr(), 0) < 0) {
    struct stat sb;
    if (!stat (builddir.cstr(), &sb)) {
      mode_t m = umask (0);
      if (!getuid ()) {
	if (pid_t pid = fork ())
	  waitpid (pid, NULL, 0);
	else {
	  umask (0);
	  setgid (sfs_gid);
	  setuid (sb.st_uid);
	  if (mkdir (sfsdir.cstr(), 02770) >= 0)
	    rc_ignore (chown (sfsdir.cstr(), (uid_t) -1, sfs_gid));
	  _exit (0);
	}
      }
      else
        mkdir (sfsdir.cstr(), 0777);
      umask (m);
    }
  }
}
Example #5
0
 void
 post(Home home, Term* t, int n, FloatRelType frt, FloatVal c, Reify r) {
   Region re(home);
   rel(home, extend(home,re,t,n), frt, c, r);
   dopost(home, t, n, FRT_EQ, 0.0);
 }
// <@$M_{t2}$@> module
void foo () {
  acq();
  f(); g();
  rel();
}
Example #7
0
void BulletSpaceFilter::buildInteractions(double time)
{

    if (! _dynamicCollisionsObjectsInserted)
    {
        DynamicalSystemsGraph& dsg = *(_model->nonSmoothDynamicalSystem()->dynamicalSystems());
        DynamicalSystemsGraph::VIterator dsi, dsiend;
        std11::tie(dsi, dsiend) = dsg.vertices();
        for (; dsi != dsiend; ++dsi)
        {
            CollisionObjects& collisionObjects = *ask<ForCollisionObjects>(*dsg.bundle(*dsi));

            for (CollisionObjects::iterator ico = collisionObjects.begin();
                    ico != collisionObjects.end(); ++ico)
            {
                _collisionWorld->addCollisionObject(const_cast<btCollisionObject*>((*ico).first));
                DEBUG_PRINTF("add dynamic collision object %p\n", const_cast<btCollisionObject*>((*ico).first));
            }
        }

        _dynamicCollisionsObjectsInserted = true;
    }

    if (! _staticCollisionsObjectsInserted)
    {
        for(StaticObjects::iterator
                ic = _staticObjects->begin(); ic != _staticObjects->end(); ++ic)
        {
            _collisionWorld->addCollisionObject(const_cast<btCollisionObject*>((*ic).first));
            DEBUG_PRINTF("add static collision object %p\n", const_cast<btCollisionObject*>((*ic).first));
        }

        _staticCollisionsObjectsInserted = true;
    }

    DEBUG_PRINT("-----start build interaction\n");

    // 1. perform bullet collision detection
    gOrphanedInteractions.clear();
    _collisionWorld->performDiscreteCollisionDetection();

    // 2. collect old contact points from Siconos graph
    std::map<btManifoldPoint*, bool> contactPoints;

    std::map<Interaction*, bool> activeInteractions;

    SP::InteractionsGraph indexSet0 = model()->nonSmoothDynamicalSystem()->topology()->indexSet(0);
    InteractionsGraph::VIterator ui0, ui0end, v0next;
    std11::tie(ui0, ui0end) = indexSet0->vertices();
    for (v0next = ui0 ;
            ui0 != ui0end; ui0 = v0next)
    {
        ++v0next;  // trick to iterate on a dynamic bgl graph
        SP::Interaction inter0 = indexSet0->bundle(*ui0);

        if (gOrphanedInteractions.find(&*inter0) != gOrphanedInteractions.end())
        {
            model()->nonSmoothDynamicalSystem()->removeInteraction(inter0);
        }

        else
        {
            SP::btManifoldPoint cp = ask<ForContactPoint>(*(inter0->relation()));
            if(cp)
            {
                DEBUG_PRINTF("Contact point in interaction : %p\n", &*cp);

                contactPoints[&*cp] = false;
            }
        }
    }
    unsigned int numManifolds =
        _collisionWorld->getDispatcher()->getNumManifolds();

    DEBUG_PRINTF("number of manifolds : %d\n", numManifolds);

    for (unsigned int i = 0; i < numManifolds; ++i)
    {
        btPersistentManifold* contactManifold =
            _collisionWorld->getDispatcher()->getManifoldByIndexInternal(i);

        DEBUG_PRINTF("processing manifold %d : %p\n", i, contactManifold);

        const btCollisionObject* obA = contactManifold->getBody0();
        const btCollisionObject* obB = contactManifold->getBody1();


        DEBUG_PRINTF("object A : %p, object B: %p\n", obA, obB);

        //    contactManifold->refreshContactPoints(obA->getWorldTransform(),obB->getWorldTransform());

        unsigned int numContacts = contactManifold->getNumContacts();

        if ( (obA->getUserPointer() && obA->getUserPointer() != obB->getUserPointer()) ||
                (obB->getUserPointer() && obA->getUserPointer() != obB->getUserPointer()) )
        {

            for (unsigned int z = 0; z < numContacts; ++z)
            {

                SP::btManifoldPoint cpoint(createSPtrbtManifoldPoint(contactManifold->getContactPoint(z)));
                DEBUG_PRINTF("manifold %d, contact %d, &contact %p, lifetime %d\n", i, z, &*cpoint, cpoint->getLifeTime());


                // should no be mixed with something else that use UserPointer!
                SP::BulletDS dsa;
                SP::BulletDS dsb;
                unsigned long int gid1, gid2;

                if(obA->getUserPointer())
                {
                    dsa = static_cast<BulletDS*>(obA->getUserPointer())->shared_ptr();

                    assert(dsa->collisionObjects()->find(contactManifold->getBody0()) !=
                           dsa->collisionObjects()->end());
                    gid1 = boost::get<2>((*((*dsa->collisionObjects()).find(obA))).second);
                }
                else
                {
                    gid1 = (*_staticObjects->find(obA)).second.second;
                }

                SP::NonSmoothLaw nslaw;
                if (obB->getUserPointer())
                {
                    dsb = static_cast<BulletDS*>(obB->getUserPointer())->shared_ptr();

                    assert(dsb->collisionObjects()->find(obB) != dsb->collisionObjects()->end());

                    gid2 = boost::get<2>((*((*dsb->collisionObjects()).find(obB))).second);
                }

                else
                {
                    gid2 = (*_staticObjects->find(obB)).second.second;
                }


                DEBUG_PRINTF("collision between group %ld and %ld\n", gid1, gid2);

                nslaw = (*_nslaws)(gid1, gid2);

                if (!nslaw)
                {
                    RuntimeException::selfThrow(
                        (boost::format("Cannot find nslaw for collision between group %1% and %2%") %
                         gid1 % gid2).str());
                }

                assert(nslaw);

                std::map<btManifoldPoint*, bool>::iterator itc;
                itc = contactPoints.find(&*cpoint);

                DEBUG_EXPR(if (itc == contactPoints.end())
            {
                DEBUG_PRINT("contact point not found\n");
                    for(std::map<btManifoldPoint*, bool>::iterator itd=contactPoints.begin();
                            itd != contactPoints.end(); ++itd)
                    {
                        DEBUG_PRINTF("-->%p != %p\n", &*cpoint, &*(*itd).first);
                    }
                });


                if (itc == contactPoints.end() || !cpoint->m_userPersistentData)
                {
                    /* new interaction */

                    SP::Interaction inter;
                    if (nslaw->size() == 3)
                    {
                        SP::BulletR rel(new BulletR(cpoint, createSPtrbtPersistentManifold(*contactManifold)));
                        inter.reset(new Interaction(3, nslaw, rel, 4 * i + z));
                    }
                    else
                    {
                        if (nslaw->size() == 1)
                        {
                            SP::BulletFrom1DLocalFrameR rel(new BulletFrom1DLocalFrameR(cpoint));
                            inter.reset(new Interaction(1, nslaw, rel, 4 * i + z));
                        }
                    }

                    if (obB->getUserPointer())
                    {
                        SP::BulletDS dsb(static_cast<BulletDS*>(obB->getUserPointer())->shared_ptr());

                        if (dsa != dsb)
                        {
                            DEBUG_PRINTF("LINK obA:%p obB:%p inter:%p\n", obA, obB, &*inter);
                            assert(inter);

                            cpoint->m_userPersistentData = &*inter;
                            link(inter, dsa, dsb);
                        }
                        /* else collision shapes belong to the same object do nothing */
                    }
                    else
                    {
                        DEBUG_PRINTF("LINK obA:%p inter :%p\n", obA, &*inter);
                        assert(inter);

                        cpoint->m_userPersistentData = &*inter;
                        link(inter, dsa);
                    }
                }

                if (cpoint->m_userPersistentData)
                {
                    activeInteractions[static_cast<Interaction *>(cpoint->m_userPersistentData)] = true;
                    DEBUG_PRINTF("Interaction %p = true\n", static_cast<Interaction *>(cpoint->m_userPersistentData));
                    DEBUG_PRINTF("cpoint %p  = true\n", &*cpoint);
                }
                else
                {
                    assert(false);
                    DEBUG_PRINT("cpoint->m_userPersistentData is empty\n");
                }

                contactPoints[&*cpoint] = true;
                DEBUG_PRINTF("cpoint %p  = true\n", &*cpoint);
            }
        }
    }
bool CGeometricConstraintSolver::solve(CIKGraphObjCont& graphContainer,SGeomConstrSolverParam& parameters)
{
	if (graphContainer.identifyElements()==0)
		return(false); // Nothing to solve (no active joint in the mechanism)
	graphContainer.putElementsInPlace();

// We create a branched tree, where each extremity has a tip dummy
// (which will later be constrained to its respective target dummy)
	
	CIKGraphObject* baseIKGraphObject=graphContainer.getBaseObject();
	CIKGraphNode* graphIterator=baseIKGraphObject;
	CIKGraphNode* previousPosition=NULL;
	CIKGraphNode* nextPosition=NULL;
	C7Vector localTransformation;
	localTransformation.setIdentity();
	CIKObjCont ikObjs;
	CIKJoint* lastJoint=NULL;
	CIKJoint* treeHandle=NULL;

// Some precalculations of some fixed rotations:
	C4X4Matrix tmpRot;
	tmpRot.setIdentity();
	tmpRot.M(0,0)=-1.0f;
	tmpRot.M(2,2)=-1.0f;
	C7Vector rotY180(tmpRot.getTransformation());
	tmpRot.M.clear();
	tmpRot.M(0,0)=1.0f;
	tmpRot.M(2,1)=1.0f;
	tmpRot.M(1,2)=-1.0f;
	C7Vector rotX90(tmpRot.getTransformation().Q,C3Vector(0.0f,0.0f,0.0f));
	tmpRot.M.clear();
	tmpRot.M(2,0)=-1.0f;
	tmpRot.M(1,1)=1.0f;
	tmpRot.M(0,2)=1.0f;
	C7Vector rotY90(tmpRot.getTransformation().Q,C3Vector(0.0f,0.0f,0.0f));
	tmpRot.M.clear();
	tmpRot.M(1,0)=1.0f;
	tmpRot.M(0,1)=-1.0f;
	tmpRot.M(2,2)=1.0f;
	C7Vector rotZ90(tmpRot.getTransformation().Q,C3Vector(0.0f,0.0f,0.0f));
	

	std::vector<CIKGraphNode*> graphObjectsToBeExplored;
	graphObjectsToBeExplored.push_back(baseIKGraphObject);
	std::vector<CIKJoint*> lastJoints;
	lastJoints.push_back(NULL);
	std::vector<CIKGraphNode*> previousPositions;
	previousPositions.push_back(NULL);
	std::vector<C7Vector> localTransformations;
	localTransformations.push_back(localTransformation);

	int explorationID=0;
	while (graphObjectsToBeExplored.size()!=0)
	{
		graphIterator=graphObjectsToBeExplored.back();
		graphObjectsToBeExplored.pop_back();
		lastJoint=lastJoints.back();
		lastJoints.pop_back();
		previousPosition=previousPositions.back();
		previousPositions.pop_back();
		localTransformation=localTransformations.back();
		localTransformations.pop_back();
		bool doIt=(graphIterator->explorationID==-1);
		bool goingDown=false;
		bool closeComplexLoop=false;
		while (doIt)
		{
			if (graphIterator->explorationID==-1)
				graphIterator->explorationID=explorationID;
			explorationID++;
			C7Vector previousCT;
			if (previousPosition!=NULL)
			{
				if (previousPosition->type==IK_GRAPH_JOINT_TYPE)
					previousCT=((CIKGraphObject*)graphIterator)->cumulativeTransformation;
				else
					previousCT=((CIKGraphObject*)previousPosition)->cumulativeTransformation;
			}
			else
			{
				previousCT=baseIKGraphObject->cumulativeTransformation;
				localTransformation=previousCT;
			}

			if (graphIterator->type==IK_GRAPH_JOINT_TYPE)
			{ // Joint: we have to introduce a joint
				CIKGraphJoint* graphJoint=(CIKGraphJoint*)graphIterator;

				if (!graphJoint->disabled)
				{
					C7Vector sphTr;
					sphTr.setIdentity();
					sphTr.Q=graphJoint->sphericalTransformation;
					CIKJoint* newIKJoint;
					if (graphJoint->jointType==IK_GRAPH_SPHERICAL_JOINT_TYPE)
					{
						int dataValueBase=10*graphJoint->nodeID;
						CIKJoint* avatarParent;
						if (graphJoint->topObject==(CIKGraphObject*)previousPosition)
						{ // From tip to base
							C7Vector rel(localTransformation*rotY180);
							newIKJoint=new CIKJoint(graphJoint,rel,false,false);
							if (lastJoint==NULL)
							{
								treeHandle=newIKJoint;
								lastJoint=treeHandle;
								ikObjs.addRoot(lastJoint);
							}
							else
							{
								ikObjs.addChild(lastJoint,newIKJoint);
								lastJoint=newIKJoint;
							}
							avatarParent=ikObjs.getJointWithData(dataValueBase+3);
							if (avatarParent!=NULL) // This joint is used twice (going up and going down)
								avatarParent->addAvatar(lastJoint);
							lastJoint->data=dataValueBase+3;
						
							rel=rotX90;
							newIKJoint=new CIKJoint(graphJoint,rel,false,false);
							ikObjs.addChild(lastJoint,newIKJoint);
							lastJoint=newIKJoint;
							avatarParent=ikObjs.getJointWithData(dataValueBase+2);
							if (avatarParent!=NULL) // This joint is used twice (going up and going down)
								avatarParent->addAvatar(lastJoint);
							lastJoint->data=dataValueBase+2;	

							rel=rotY90;
							newIKJoint=new CIKJoint(graphJoint,rel,false,false);
							ikObjs.addChild(lastJoint,newIKJoint);
							lastJoint=newIKJoint;
							avatarParent=ikObjs.getJointWithData(dataValueBase+1);
							if (avatarParent!=NULL) // This joint is used twice (going up and going down)
								avatarParent->addAvatar(lastJoint);
							lastJoint->data=dataValueBase+1;
							
								rel=rotX90*rotZ90.getInverse()*sphTr.getInverse()*rotY180;
								newIKJoint=new CIKJoint(graphJoint,rel,true,false);
								lastJoint->topJoint=newIKJoint; // This is mainly needed by the joint-limitation part!
								ikObjs.addChild(lastJoint,newIKJoint);
								lastJoint=newIKJoint;
								lastJoint->active=false; // Inactive for now (we can activate it later)
								avatarParent=ikObjs.getJointWithData(dataValueBase+0);
								if (avatarParent!=NULL) // This joint is used twice (going up and going down)
									avatarParent->addAvatar(lastJoint);
								lastJoint->data=dataValueBase+0;
								localTransformation=rotY180;
						}
						else
						{ // From base to tip
							C7Vector rel(localTransformation);
							newIKJoint=new CIKJoint(graphJoint,rel,false,true);
							if (lastJoint==NULL)
							{
								treeHandle=newIKJoint;
								lastJoint=treeHandle;
								ikObjs.addRoot(lastJoint);
							}
							else
							{
								ikObjs.addChild(lastJoint,newIKJoint);
								lastJoint=newIKJoint;
							}
							lastJoint->active=false; // Inactive for now (we can activate it later)
							avatarParent=ikObjs.getJointWithData(dataValueBase+0);
							if (avatarParent!=NULL) // This joint is used twice (going up and going down)
								avatarParent->addAvatar(lastJoint);
							lastJoint->data=dataValueBase+0;

							rel=sphTr*rotY90;
							newIKJoint=new CIKJoint(graphJoint,rel,false,true);
							ikObjs.addChild(lastJoint,newIKJoint);
							lastJoint=newIKJoint;
							avatarParent=ikObjs.getJointWithData(dataValueBase+1);
							if (avatarParent!=NULL) // This joint is used twice (going up and going down)
								avatarParent->addAvatar(lastJoint);
							lastJoint->data=dataValueBase+1;
							
							rel=rotX90.getInverse();
							newIKJoint=new CIKJoint(graphJoint,rel,false,true);
							ikObjs.addChild(lastJoint,newIKJoint);
							lastJoint=newIKJoint;
							avatarParent=ikObjs.getJointWithData(dataValueBase+2);
							if (avatarParent!=NULL) // This joint is used twice (going up and going down)
								avatarParent->addAvatar(lastJoint);
							lastJoint->data=dataValueBase+2;

							rel=rotY90.getInverse()*rotZ90.getInverse();
							newIKJoint=new CIKJoint(graphJoint,rel,true,true);
							newIKJoint->topJoint=newIKJoint; // Top-joint is itself!
							ikObjs.addChild(lastJoint,newIKJoint);
							lastJoint=newIKJoint;
							avatarParent=ikObjs.getJointWithData(dataValueBase+3);
							if (avatarParent!=NULL) // This joint is used twice (going up and going down)
								avatarParent->addAvatar(lastJoint);
							lastJoint->data=dataValueBase+3;

							localTransformation.setIdentity();
						}
					}
					else
					{
						if (graphJoint->topObject==(CIKGraphObject*)previousPosition)
						{ // From tip to base
							C7Vector rel(localTransformation*rotY180);
							newIKJoint=new CIKJoint(graphJoint,rel,false,false);
							localTransformation=rotY180;
						}
						else
						{ // From base to tip
							C7Vector rel(localTransformation);
							newIKJoint=new CIKJoint(graphJoint,rel,false,false);
							localTransformation.setIdentity();
						}
						if (lastJoint==NULL)
						{
							treeHandle=newIKJoint;
							lastJoint=treeHandle;
							ikObjs.addRoot(lastJoint);
						}
						else
						{
							ikObjs.addChild(lastJoint,newIKJoint);
							lastJoint=newIKJoint;
						}
						int dataValue=10*graphJoint->nodeID+0;
						CIKJoint* avatarParent=ikObjs.getJointWithData(dataValue);
						if (avatarParent!=NULL) // This joint is used twice (going up and going down)
							avatarParent->addAvatar(lastJoint);
						lastJoint->data=dataValue;
					}
				}
				else
				{ // In case a graph-joint is disabled: 
					if (graphJoint->topObject==(CIKGraphObject*)previousPosition)
					{ // From tip to base
						localTransformation=localTransformation*graphJoint->getDownToTopTransformation().getInverse();
					}
					else
					{ // From base to tip
						localTransformation=localTransformation*graphJoint->getDownToTopTransformation();
					}
				}
			}
			else
			{
				CIKGraphObject* theObject=(CIKGraphObject*)graphIterator;
				if (theObject->objectType==IK_GRAPH_LINK_OBJECT_TYPE)
				{ // Link
					if (previousPosition!=NULL)
					{
						if (theObject->linkPartner!=previousPosition)
							localTransformation=localTransformation*previousCT.getInverse()*theObject->cumulativeTransformation;
						// If (theObject->linkPartner==previousPosition) then we don't do anything!
					}
				}
				else
				{ // Here we have a dummy we have to assign to a configuration or a passive object
					// We treat all cases first as passive objects:
					if (previousPosition!=NULL)
					{
						localTransformation=localTransformation*previousCT.getInverse()*theObject->cumulativeTransformation;
						if ( (theObject->objectType==IK_GRAPH_TIP_OBJECT_TYPE)&&(lastJoint!=NULL) )
						{ // This is a valid dummy-tip!
							CIKDummy* newIKDummy=new CIKDummy(localTransformation,theObject->targetCumulativeTransformation);
							ikObjs.addChild(lastJoint,newIKDummy);
							newIKDummy->constraints=(IK_X_CONSTRAINT|IK_Y_CONSTRAINT|IK_Z_CONSTRAINT);
							newIKDummy->dampingFactor=1.0f;
							newIKDummy->loopClosureDummy=false;
							if (graphIterator->getConnectionNumber()==1)
								break;
						}
					}
				}
			}
			int unexploredSize=graphIterator->getNumberOfUnexplored();
			if ( (unexploredSize==0)||goingDown||closeComplexLoop )
			{
				if ( (graphIterator->getConnectionNumber()==1)&&(!closeComplexLoop) )
					break; // This is a rare case where we have an endpoint without a tip-dummy mobile-part
				if (closeComplexLoop)
				{
					CIKDummy* tipDummy=new CIKDummy(localTransformation,baseIKGraphObject->cumulativeTransformation);
					ikObjs.addChild(lastJoint,tipDummy);
					break;
				}
				nextPosition=graphIterator->getExploredWithSmallestExplorationID();
				if ( (nextPosition->explorationID==0)&&(!goingDown) )
				{ // The loop can now be closed (simple loop with each joint present at most once)
					previousCT=((CIKGraphObject*)graphIterator)->cumulativeTransformation;
					localTransformation=localTransformation*previousCT.getInverse()*((CIKGraphObject*)nextPosition)->cumulativeTransformation;
					CIKDummy* tipDummy=new CIKDummy(localTransformation,baseIKGraphObject->cumulativeTransformation);
					ikObjs.addChild(lastJoint,tipDummy);
					break;
				}
				if ( (nextPosition->explorationID==0)&&goingDown )
					closeComplexLoop=true;
				goingDown=true;
			}
			else if ((graphIterator->getNeighbourWithExplorationID(0)!=NULL)&&(!goingDown)&&(previousPosition->explorationID!=0))
			{ // Here we have to close the loop too!
				// We first put unexplored paths onto the stack:
				for (int i=0;i<unexploredSize;i++)
				{ // We throw unexplored nodes onto the exploration stack:
					graphObjectsToBeExplored.push_back(graphIterator->getUnexplored(i));
					lastJoints.push_back(lastJoint);
					previousPositions.push_back(graphIterator);
					localTransformations.push_back(localTransformation);
				}
				nextPosition=graphIterator->getExploredWithSmallestExplorationID();
				previousCT=((CIKGraphObject*)previousPosition)->cumulativeTransformation;
				localTransformation=localTransformation*previousCT.getInverse()*((CIKGraphObject*)nextPosition)->cumulativeTransformation;
				CIKDummy* tipDummy=new CIKDummy(localTransformation,baseIKGraphObject->cumulativeTransformation);
				ikObjs.addChild(lastJoint,tipDummy);
				break;
			}
			else
			{
				if (previousPosition==NULL)
				{ // This is the start.	We should always explore first two links which belong together
					// or the 3 objects making up a joint!
					nextPosition=NULL;
					for (int i=0;i<unexploredSize;i++)
					{
						CIKGraphNode* nextPositionTmp=graphIterator->getUnexplored(i);
						if ( (((CIKGraphObject*)graphIterator)->linkPartner==nextPositionTmp)||
							(nextPositionTmp->type==IK_GRAPH_JOINT_TYPE) )
							nextPosition=nextPositionTmp;
						else
						{
							graphObjectsToBeExplored.push_back(graphIterator->getUnexplored(i));
							lastJoints.push_back(lastJoint);
							previousPositions.push_back(graphIterator);
							localTransformations.push_back(localTransformation);
							if (nextPosition==NULL)
								nextPosition=graphIterator->getUnexplored(i);
						}
					}
				}
				else
				{	
					nextPosition=graphIterator->getUnexplored(0);
					for (int i=1;i<unexploredSize;i++)
					{ // We throw unexplored nodes onto the exploration stack:
						graphObjectsToBeExplored.push_back(graphIterator->getUnexplored(i));
						lastJoints.push_back(lastJoint);
						previousPositions.push_back(graphIterator);
						localTransformations.push_back(localTransformation);
					}
				}
			}
			previousPosition=graphIterator;
			graphIterator=nextPosition;
		}

	}

	solveHierarchy(&ikObjs,parameters);

	for (int i=0;i<int(ikObjs.allObjects.size());i++)
	{
		CIKObject* it=ikObjs.allObjects[i];
		if (it->objectType==IK_JOINT_TYPE)
		{
			CIKJoint* theJoint=(CIKJoint*)it;
			if (theJoint->avatarParent==NULL)
			{
				if (theJoint->spherical)
				{
					if (theJoint->topSpherical)
					{
						float a0=theJoint->parameter;
						float a1=((CIKJoint*)theJoint->parent)->parameter;
						float a2=((CIKJoint*)theJoint->parent->parent)->parameter;
						float a3=((CIKJoint*)theJoint->parent->parent->parent)->parameter;
						if (theJoint->sphericalUp)
						{
							theJoint->graphJoint->sphericalTransformation=C4Vector(a3,C3Vector(0.0f,0.0f,1.0f))*theJoint->graphJoint->sphericalTransformation*C4Vector(C3Vector(a2,a1,a0));
						}
						else
						{
							theJoint->graphJoint->sphericalTransformation=C4Vector(a0,C3Vector(0.0f,0.0f,1.0f))*theJoint->graphJoint->sphericalTransformation*C4Vector(C3Vector(a1,a2,a3));
						}
					}
				}
				else
					theJoint->graphJoint->parameter=theJoint->parameter;
			}
		}
	}
	graphContainer.actualizeAllTransformations();
	graphContainer.putElementsInPlace();
	return(true);
}
Example #9
0
int main()
{
	printf("The productions used are:\n");
	printf("E-->E*E/E+E/E^E/E*E/E-E\n E-->E/E \n E-->a/b/c/d/e.../z");
	printf("\n Enter an expression that terminals with $:");
	fflush(stdin);
	i=-1;
	while(str[i]!='$')
	{
		i++;
		scanf("%c",&str[i]);
	}

	for(j=0;j<i;j++)
	{
		if((str[j]=='(')||(str[j]==')')||(str[j+1]=='(')||(str[j+1]==')'))
		{}
		else if (((isalpha(str[j])==0)&&(isalpha(str[j+1])==0))||((isalpha(str[j])!=0)&&(isalpha(str[j+1])!=0)))
		{
			printf("ERROR");
			exit(0);
		}
	}
	if((((isalpha(str[0]))!=0)||(str[0]=='('))&&(((isalpha(str[i-1]))!=0)||(str[i-1]==')')))
	{
		pushin('$');
		printf("\n\n\n\t+\t-\t*\t/\t^\ta\t(\t)\t$\n\n");
		for(i=0;i<9;i++)
		{
			printf("%c",c[i]);
			for(j=0;j<9;j++)
				printf("\t%c",q[i][j]);
			printf("\n");
		}

		while(1)
		{
			if(str[ptr]=='$' && stk[tos]=='$')
			{
				printf("\n The given expression is succesfully accepted!\n");
				break;
			}
			else if(rel(stk[tos],str[ptr],'<')||rel(stk[tos],str[ptr],'=='))
			{
				display(str[ptr]);
				pushin(str[ptr]);
				ptr++;
			}
			else if(rel(stk[tos],str[ptr],'>'))
			{
				do
				{
					rm++;
					pstk[rm]=popout();
					display1(pstk[rm]);
				}

				while(!rel(stk[tos],pstk[rm],'<'));
			}

			else
			{
				printf("\n NOT ACCEPTED!!!!!!!\n");
				exit(1);
			}
		}
	}
	else
	{
		printf("ERROR");
	}
	return 0;
}
Example #10
0
void OSNSPTest::testAVI()
{
  std::cout << "===========================================" <<std::endl;
  std::cout << " ===== OSNSP tests start ... ===== " <<std::endl;
  std::cout << "===========================================" <<std::endl;
  std::cout << "------- implicit Twisting relation  -------" <<std::endl;
  _h = 1e-1;
  _T = 20.0;
  double G = 10.0;
  double beta = .3;
  _A->zero();
  (*_A)(0, 1) = 1.0;
  _x0->zero();
  (*_x0)(0) = 10.0;
  (*_x0)(1) = 10.0;
  SP::SimpleMatrix B(new SimpleMatrix(_n, _n, 0));
  SP::SimpleMatrix C(new SimpleMatrix(_n, _n));
  (*B)(1, 0) = G;
  (*B)(1, 1) = G*beta;
  C->eye();
  SP::FirstOrderLinearTIR rel(new FirstOrderLinearTIR(C, B));
  SP::SimpleMatrix H(new SimpleMatrix(4, 2));
  (*H)(0, 0) = 1.0;
  (*H)(1, 0) = -_h/2.0;
  (*H)(2, 0) = -1.0;
  (*H)(3, 0) = _h/2.0;
  (*H)(1, 1) = 1.0;
  (*H)(3, 1) = -1.0;
  SP::SiconosVector K(new SiconosVector(4));
  (*K)(0) = -1.0;
  (*K)(1) = -1.0;
  (*K)(2) = -1.0;
  (*K)(3) = -1.0;
  SP::NonSmoothLaw nslaw(new NormalConeNSL(_n, H, K));
  _DS.reset(new FirstOrderLinearTIDS(_x0, _A, _b));
  _TD.reset(new TimeDiscretisation(_t0, _h));
  _model.reset(new Model(_t0, _T));
  SP::Interaction inter(new Interaction(_n, nslaw, rel));
  _osi.reset(new EulerMoreauOSI(_theta));
  _model->nonSmoothDynamicalSystem()->insertDynamicalSystem(_DS);
  _model->nonSmoothDynamicalSystem()->setOSI(_DS, _osi);
  _model->nonSmoothDynamicalSystem()->link(inter, _DS);
  _sim.reset(new TimeStepping(_TD));
  _sim->insertIntegrator(_osi);
  SP::AVI osnspb(new AVI());
  _sim->insertNonSmoothProblem(osnspb);
  _model->initialize(_sim);
  SimpleMatrix dataPlot((unsigned)ceil((_T - _t0) / _h) + 10, 5);
  SiconosVector& xProc = *_DS->x();
  SiconosVector& lambda = *inter->lambda(0);
  unsigned int k = 0;
  dataPlot(0, 0) = _t0;
  dataPlot(0, 1) = (*_x0)(0);
  dataPlot(0, 2) = (*_x0)(1);
  dataPlot(0, 3) = -1.0;
  dataPlot(0, 4) = -1.0;
  while (_sim->hasNextEvent())
  {
    _sim->computeOneStep();
    k++;
    dataPlot(k, 0) = _sim->nextTime();
    dataPlot(k, 1) = xProc(0);
    dataPlot(k, 2) = xProc(1);
    dataPlot(k, 3) = lambda(0);
    dataPlot(k, 4) = lambda(1);
    _sim->nextStep();
  }
  std::cout <<std::endl <<std::endl;
  dataPlot.resize(k, dataPlot.size(1));
  ioMatrix::write("testAVI.dat", "ascii", dataPlot, "noDim");
  // Reference Matrix
  SimpleMatrix dataPlotRef(dataPlot);
  dataPlotRef.zero();
  ioMatrix::read("testAVI.ref", "ascii", dataPlotRef);
  std::cout << "------- Integration Ok, error = " << (dataPlot - dataPlotRef).normInf() << " -------" <<std::endl;
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testAVI : ", (dataPlot - dataPlotRef).normInf() < _tol, true);
}
Example #11
0
REGISTER_UNIT_TEST( MojoRelationTest, Container )
{
  MojoRelation< MojoId > rel( "test" );

  // For 0..49, insert consecutively
  for( char c = 'a'; c <= 'z'; ++c )
  {
    char group[ 20 ];
    snprintf( group, sizeof group, "%c", c );
    MojoId parent = group;
    for( int i = 0; i < 50; ++i )
    {
      MojoId child = MakeId( group, i );
      rel.InsertChildParent( child, parent );
    }
  }

  // For 50..99, insert jumbled up
  for( int i = 50; i < 100; ++i )
  {
    for( char c = 'z'; c >= 'a'; --c )
    {
      char group[ 20 ];
      snprintf( group, sizeof group, "%c", c );
      MojoId parent = group;
      MojoId child = MakeId( group, i );
      rel.InsertChildParent( child, parent );
    }
  }

  // Now remove all even children, to exercise reordering.
  for( char c = 'a'; c <= 'z'; ++c )
  {
    char group[ 20 ];
    snprintf( group, sizeof group, "%c", c );
    MojoId parent = group;
    for( int i = 0; i < 100; i += 2 )
    {
      MojoId child = MakeId( group, i );
      rel.RemoveChild( child );
    }
  }
  
  // Now remove all parents above and including 'q'.
  for( char c = 'q'; c <= 'z'; ++c )
  {
    char group[ 20 ];
    snprintf( group, sizeof group, "%c", c );
    MojoId parent = group;
    for( int i = 1; i < 100; i += 2 )
    {
      MojoId child = MakeId( group, i );
      rel.RemoveChild( child );
    }
  }
  
  // Now table contains parents 'a'-'p', and only oddly numbered children

  // Find all children's parents
  for( char c = 'a'; c < 'q'; ++c )
  {
    char group[ 20 ];
    snprintf( group, sizeof group, "%c", c );
    MojoId parent = group;
    for( int i = 1; i < 100; i += 2 )
    {
      MojoId child = MakeId( group, i );
      MojoId found_parent = rel.FindParent( child );
      EXPECT_BOOL( false, found_parent.IsNull() );
      EXPECT_STRING( parent.AsCString(), found_parent.AsCString() );
    }
  }
  
  // Verify all even children are missing
  for( char c = 'a'; c < 'q'; ++c )
  {
    char group[ 20 ];
    snprintf( group, sizeof group, "%c", c );
    MojoId parent = group;
    for( int i = 0; i < 100; i += 2 )
    {
      MojoId child = MakeId( group, i );
      MojoId found_parent = rel.FindParent( child );
      EXPECT_BOOL( true, found_parent.IsNull() );
    }
  }
  
  // Verify children of groups 'q'-'z' are gone as well
  for( char c = 'q'; c <= 'z'; ++c )
  {
    char group[ 20 ];
    snprintf( group, sizeof group, "%c", c );
    MojoId parent = group;
    for( int i = 0; i < 100; i += 1 )
    {
      MojoId child = MakeId( group, i );
      MojoId found_parent = rel.FindParent( child );
      EXPECT_BOOL( true, found_parent.IsNull() );
    }
  }

  // Find children of parents
  for( char c = 'a'; c < 'q'; ++c )
  {
    char group[ 20 ];
    snprintf( group, sizeof group, "%c", c );
    MojoId parent = group;
    MojoId child;

    int count = 0;
    MojoForEachMultiValue( rel, parent, child )
    {
      count += 1;
      EXPECT_INT( c, child.AsCString()[ 0 ] );
    }
    EXPECT_INT( 50, count );
  }
Example #12
0
GraphAndValues Masseuse::LoadPoseGraphAndLCC(
    const string& pose_graph_file, bool read_lcc) {


  FILE *fp = (FILE *)fopen(pose_graph_file.c_str(), "rb");

  if (fp == NULL) {
    fprintf(stderr, "Could not open file %s\n",
            pose_graph_file.c_str());
  }


  if (fread(&origin, sizeof(Eigen::Vector6d), 1, fp) != 1) {
    throw invalid_argument("LoadPoseGraphAndLCC:  Cannot load the origin!");
  }

  //  std::cerr << "read origin: " << origin.transpose() << std::endl;

  unsigned numRelPoses = 0;
  unsigned numLCC = 0;

  if (fread(&numRelPoses, sizeof(unsigned), 1, fp) != 1) {
    printf("error! Cannot load num of relative poses.\n");
    throw invalid_argument("LoadPoseGraphAndLCC:  error loading file");
  }

  if(read_lcc){
    if (fread(&numLCC, sizeof(unsigned), 1, fp) != 1) {
      printf("error! Cannot load num of loop closure constraints.\n");
      throw invalid_argument("LoadPoseGraphAndLCC:  error loading file");
    }
  }

  std::cerr << "Will load " << numRelPoses << " rel poses, "
            << numLCC << " loop closure constranits." << std::endl;

  relative_poses.clear();
  // save all rel poses
  for (unsigned i = 0; i != numRelPoses; i++) {
    RelPose rPos;
    if (fread(&rPos.ref_id, sizeof(unsigned), 1, fp) != 1) {
      throw invalid_argument("LoadPoseGraphAndLCC:  error loading file");
    }
    if (fread(&rPos.live_id, sizeof(unsigned), 1, fp) != 1) {
      throw invalid_argument("LoadPoseGraphAndLCC:  error loading file");
    }
    if (fread(&rPos.rel_pose, sizeof(Eigen::Vector6d), 1, fp) != 1) {
      throw invalid_argument("LoadPoseGraphAndLCC:  error loading file");
    }
    if (fread(&rPos.cov, sizeof(Eigen::Matrix6d), 1, fp) != 1) {
      throw invalid_argument("LoadPoseGraphAndLCC:  error loading file");
    }

    relative_poses.push_back(rPos);
  }

  if(read_lcc){
    loop_closure_constraints.clear();
    // save all lcc here
    for (unsigned i = 0; i != numLCC; i++) {
      RelPose rPos;
      if (fread(&rPos.ref_id, sizeof(unsigned), 1, fp) != 1) {
        throw invalid_argument("LoadPoseGraphAndLCC:  error loading file");
      }
      if (fread(&rPos.live_id, sizeof(unsigned), 1, fp) != 1) {
        throw invalid_argument("LoadPoseGraphAndLCC:  error loading file");
      }
      if (fread(&rPos.rel_pose, sizeof(Eigen::Vector6d), 1, fp) != 1) {
        throw invalid_argument("LoadPoseGraphAndLCC:  error loading file");
      }
      if (fread(&rPos.cov, sizeof(Eigen::Matrix6d), 1, fp) != 1) {
        throw invalid_argument("LoadPoseGraphAndLCC:  error loading file");
      }

      loop_closure_constraints.push_back(rPos);
    }
  }
  fclose(fp);

  std::cerr << "Finished loading Pose Graph from  " << pose_graph_file
            << std::endl;

  std::shared_ptr<Values> initial(new Values);
  std::shared_ptr<Graph> graph(new Graph);
  Pose3 prev_pose;

  unsigned numICPfailed = 0;
  for (size_t ii = 0; ii < relative_poses.size(); ++ii){
    RelPose curr_pose = relative_poses[ii];
    if(ii == 0){
      // first relative pose, initialize graph at origin
      unsigned id = curr_pose.ref_id;
      Rot3 R(origin[3], origin[4], origin[5]);
      Point3 t = origin.head<3>();
      Pose3 orig(R, t);
      (*initial)[id] = orig;
      prev_pose = orig;

      //            std::cerr << "inserting origin: Rot: " << orig.rotationMatrix().eulerAngles
      //                         (0,1,2).transpose() << " Trans: " << orig.translation().transpose() <<
      //                         " at index:  " << id <<
      //                         std::endl;
    }

    // Build the next vertex using the relative contstraint
    Rot3 R(curr_pose.rel_pose[3], curr_pose.rel_pose[4],
        curr_pose.rel_pose[5]);
    Point3 t = curr_pose.rel_pose.head<3>();
    Pose3 rel(R, t);

    Pose3 new_pose = prev_pose*rel;
    (*initial)[curr_pose.live_id] = new_pose;

    //    std::cerr << "inserting pose: Rot: " << new_pose.rotationMatrix().eulerAngles
    //                 (0,1,2).transpose() << " Trans: " << new_pose.translation().transpose() <<
    //                 " at index:  " << curr_pose.live_id <<
    //                 std::endl;

    prev_pose = new_pose;

    // Also insert a factor for the binary pose constraint
    unsigned id1 = curr_pose.ref_id;
    unsigned id2 = curr_pose.live_id;

    Matrix m = curr_pose.cov;
    if(m.sum() == 0 || m.determinant() <= 0 || std::isnan(m.determinant())
       /*|| m.determinant() > options.cov_det_thresh*/){
      //      std::cerr << "ICP failed for rel pose between " << id1 << " and " << id2 <<
      //                   "Setting fixed covaraince..." <<
      //                   std::endl;
      Eigen::Vector6d cov_vec;
      // TODO: Improve handling of cases where the frame-to-frame ICP failed
      cov_vec << 7e-8, 7e-8, 7e-8, 8e-11, 8e-11, 8e-11;
      m = cov_vec.asDiagonal();
      numICPfailed++;
    }

    //        std::cerr <<  "Adding binary constraint between id: " << id1 << " and " <<
    //                      id2 << std::endl << "with cov: \n" << m << std::endl;

    // Create a new factor between poses
    if(options.use_identity_covariance){
      m.setIdentity();
    }

    m = m * options.rel_covariance_mult;

    Factor factor(id1, id2, rel, m);
    graph->push_back(factor);

  }

  std::cerr << std::setprecision(3) << std::fixed <<"ICP failed " << numICPfailed << " times " <<
               "out of " << relative_poses.size() << " ( " << (double)numICPfailed/(double)relative_poses.size() * 100 <<
               "% )" << std::endl;


  if( read_lcc ){
    std::cerr << "Reading LLC." << std::endl;

    int discarded_lcc = 0;
    for(size_t ii = 0; ii < loop_closure_constraints.size(); ++ii){
      RelPose curr_lcc = loop_closure_constraints[ii];
      unsigned id1 = curr_lcc.ref_id;
      unsigned id2 = curr_lcc.live_id;

      Rot3 R(curr_lcc.rel_pose[3], curr_lcc.rel_pose[4],
          curr_lcc.rel_pose[5]);
      Point3 t = curr_lcc.rel_pose.head<3>();
      Pose3 lcc(R, t);

      Matrix m = curr_lcc.cov;

      if(m.sum() == 0 ||
         m.determinant() > options.cov_det_thresh ||
         m.determinant() <= 0 ||
         std::isnan(m.determinant()))
      {
        // ICP failed or something went wrong for this loop closure, ignoring
        // The determinant of the cov. matrix may also be larger than the
        // specified threshold.
        discarded_lcc++;
        continue;
      }

      // check if the lcc is between far away poses. If so, downweight it's
      // covariance.

      //      const Pose3* lcc_0 = dynamic_cast<const Pose3*>(&initial->at(id1));
      //      const Pose3* lcc_1 = dynamic_cast<const Pose3*>(&initial->at(id2));

      //      Pose3 lcc_diff = lcc_0->compose(lcc).inverse().compose(*lcc_1);
      //            std::cerr << "distance between poses for lcc: " << ii <<
      //                         " between poses " <<  id1 << " and " << id2 << ": "
      //                      << lcc_diff.translation().norm() << std::endl;
      //      Pose3 diff = (*lcc_0).inverse().compose(*lcc_1);
      //      std::cerr << "distance between poses for lcc: " << ii <<
      //                   " between poses " <<  id1 << " and " << id2 << ": "
      //                << lcc.translation().norm() << std::endl;

      // Create a new factor between poses
      if(options.use_identity_covariance){
        m.setIdentity();
      }

      Factor lcc_factor(id1, id2, lcc, m);
      lcc_factor.isLCC = true;

      graph->push_back(lcc_factor);
      curr_lcc.ext_id = graph->size()-1;


      //      std::cerr << std::scientific <<
      //                   "Adding lcc between id: " << id1 << " and " <<
      //                    id2 << std::endl << "with cov: \n" << lcc_factor.cov << std::endl;

      //m_addedLCC[keyFromId(id2)] = curr_lcc;

      /*

      // If we already have a constraint between poses i and j, only use the one
      // with the highest degree of certainty (lowest cov. determinant)
      if(m_addedLCC.count(keyFromId(id2)) == 0){
        // LCC has not been added yet, just add
        graph->push_back(factor);
        curr_lcc.m_nExtId = graph->size()-1;
        m_addedLCC[keyFromId(id2)] = curr_lcc;
      }else{
        // Check if the covariance of the new lcc for poses i and j
        // is smaller than what we are currently using
        EstPose old_lcc = m_addedLCC[keyFromId(id2)];
        if(old_lcc.m_Cov.determinant() > curr_lcc.m_Cov.determinant()){
          // new det is smaller, replace
          curr_lcc.m_nExtId = old_lcc.m_nExtId;
          graph->replace(old_lcc.m_nExtId, factor);
          m_addedLCC[keyFromId(id2)] = curr_lcc;
//          std::cerr << "determinant for new lcc between " << id1 << " and " <<
//                       id2 << " is: " << curr_lcc.m_Cov.determinant() << " < "
//                    << old_lcc.m_Cov.determinant() << std::endl;

        }else{
          // Determinant for new constraint is larger, skip it
          continue;
        }

      }
      */

    }

    //    //ZZZZZZZZZZZZZ Temp, add wrong LCC to test switchable constraints
    //    unsigned id1 = 480;
    //    unsigned id2 = 870;
    //    Pose3& T2 = initial->at(id2);

    //    Pose3& T1 = initial->at(id1);

    //    Pose3 T12 = T1.inverse() * T2;


    //    // Add random values to the translation
    //    T12.translation()[0] += 5;
    //    T12.translation()[1] -= 2;
    //    T12.translation()[3] += 7;

    //    // Rotate the pose by an arbitrary amount
    //    Sophus::SO3d rot(0.5, 0.7, 0.1);

    //    T12.rotationMatrix() *= rot.matrix();
    //    Factor wrong_lcc(id1, id2, T12, Eigen::Matrix6d::Identity());
    //    wrong_lcc.isLCC = true;
    //    // now add the wrong LCC to the graph
    //    graph->push_back(wrong_lcc);


    std::cerr << std::setprecision(3) << std::fixed <<"Did not use " << discarded_lcc << " LCC " <<
                 "out of " << numLCC << " ( " << (double)discarded_lcc/(double)numLCC * 100 <<
                 "% )" << std::endl;

  }

  return make_pair(graph, initial);
}
Example #13
0
void Plugin::addFile(const std::string filepath)
{
    logger().debug("Adding file");
    // Check if file exists and it's not a directory
    File file(filepath);
    if (!file.exists())
    {
        std::cout << "Failed to add file " << filepath.c_str() << ": file does not exist" << std::endl;
        return;
    }
    if (file.isDirectory())
    {
        std::cout << "Failed to add file " << filepath.c_str() << ": file is a directory" << std::endl;
        return;

    }
    File index(Plugin::GIT_BIN_INDEX);
    if (!index.exists())
    {
        index.createFile();
    }
    // Find this file in the index
    if (isFileIndexed(filepath) && file.isLink())
    {
        logger().debug("File already in cache");
        auto entry = getIndexEntry(filepath);
        if (entry == nullptr)
        {
            std::cout << "Failed to retrieve index meta data for " << filepath << std::endl;
            return;
        }
        // Not fresh. Update existing index file
        // Remove file from index before recalculating everything
        for (auto it = _index.begin(); it != _index.end();) 
        {
            if ((*it).filepath == filepath)
            {
                // Remove link
                std::cout << "Replacing link with original file" << std::endl; 
                file.remove();
                Process::Args restoreArgs;
                std::string restorePath(Plugin::GIT_CACHE_DIR);
                restorePath.append("/wf/");
                restorePath.append((*it).uuid);
                restoreArgs.push_back(restorePath);
                restoreArgs.push_back(filepath);
                Process::launch("mv", restoreArgs, 0, 0, 0);
                it = _index.erase(it); 
            } else {
                it++;
            }
        } 

    }
    // Add new file into index
    UUIDGenerator gen;
    IndexEntry e;
    e.filepath = filepath;
    e.md5 = getFileMd5(filepath);
    e.uuid = gen.createRandom().toString();
    do 
    {
        e.uuid = gen.createRandom().toString();
    }
    while (!isUuidUnique(e.uuid));
    _index.push_back(e);
    writeIndex();

    // Place two copies of this file into cache
    // One copy is original file and don't tracked in any way
    // Second copy is a file we will create link to

    Process::Args args;
    args.push_back(filepath);
    std::string origPath(Plugin::GIT_CACHE_DIR);
    origPath.append("/of/").append(e.uuid);
    args.push_back(origPath);
    Poco::ProcessHandle copyProcess = Process::launch("cp", args, 0, 0, 0);
    if (copyProcess.wait() != 0)
    {
        std::cout << "Failed to move file into git-bin cache" << std::endl;
        return;
    }
    // Second stage copy
    args.clear();
    args.push_back(filepath);
    std::string workPath(Plugin::GIT_CACHE_DIR);
    workPath.append("/wf/").append(e.uuid);
    args.push_back(workPath);
    copyProcess = Process::launch("mv", args, 0, 0, 0);
    if (copyProcess.wait() != 0)
    {
        std::cout << "Failed to move file on stage 2" << std::endl;
        return;
    }

    // Make a link
    args.clear();
    args.push_back("-s");
    Path rel(filepath);
    std::string relPath;
    for (int i = 0; i < rel.depth(); i++)
    {
        relPath.append("../");
    }
    relPath.append(workPath);
    args.push_back(relPath);
    args.push_back(filepath);
    Poco::ProcessHandle linkProcess = Process::launch("ln", args, 0, 0, 0); 
    if (linkProcess.wait() != 0)
    {
        std::cout << "Failed to create link to file" << std::endl;
        return;
    }

    args.clear();
    args.push_back("add");
    args.push_back(filepath);
    Poco::ProcessHandle gitAddProcess = Process::launch("git", args, 0, 0, 0);
    if (gitAddProcess.wait() != 0)
    {
        std::cout << "Failed to add file into git index (git add)" << std::endl;
        return;
    }
}
Example #14
0
void PackArmPe::pack(OutputFile *fo)
{
    // FIXME: we need to think about better support for --exact
    if (opt->exact)
        throwCantPackExact();

    const unsigned objs = ih.objects;
    isection = new pe_section_t[objs];
    fi->seek(pe_offset+sizeof(ih),SEEK_SET);
    fi->readx(isection,sizeof(pe_section_t)*objs);

    rvamin = isection[0].vaddr;

    infoHeader("[Processing %s, format %s, %d sections]", fn_basename(fi->getName()), getName(), objs);

    // check the PE header
    // FIXME: add more checks
    if (!opt->force && (
           (ih.cpu != 0x1c0 && ih.cpu != 0x1c2)
        || (ih.opthdrsize != 0xe0)
        || ((ih.flags & EXECUTABLE) == 0)
        || (ih.subsystem != 9)
        || (ih.entry == 0 /*&& !isdll*/)
        || (ih.ddirsentries != 16)
//        || IDSIZE(PEDIR_EXCEPTION) // is this used on arm?
//        || IDSIZE(PEDIR_COPYRIGHT)
       ))
        throwCantPack("unexpected value in PE header (try --force)");

    if (IDSIZE(PEDIR_SEC))
        IDSIZE(PEDIR_SEC) = IDADDR(PEDIR_SEC) = 0;
    //    throwCantPack("compressing certificate info is not supported");

    if (IDSIZE(PEDIR_COMRT))
        throwCantPack(".NET files (win32/net) are not yet supported");

    if (isdll)
        opt->win32_pe.strip_relocs = false;
    else if (opt->win32_pe.strip_relocs < 0)
        opt->win32_pe.strip_relocs = (ih.imagebase >= 0x10000);
    if (opt->win32_pe.strip_relocs)
    {
        if (ih.imagebase < 0x10000)
            throwCantPack("--strip-relocs is not allowed when imagebase < 0x10000");
        else
            ih.flags |= RELOCS_STRIPPED;
    }

    if (memcmp(isection[0].name,"UPX",3) == 0)
        throwAlreadyPackedByUPX();
    if (!opt->force && IDSIZE(15))
        throwCantPack("file is possibly packed/protected (try --force)");
    if (ih.entry && ih.entry < rvamin)
        throwCantPack("run a virus scanner on this file!");
    if (!opt->force && ih.subsystem == 1)
        throwCantPack("subsystem 'native' is not supported (try --force)");
    if (ih.filealign < 0x200)
        throwCantPack("filealign < 0x200 is not yet supported");

    handleStub(fi,fo,pe_offset);
    const unsigned usize = ih.imagesize;
    const unsigned xtrasize = UPX_MAX(ih.datasize, 65536u) + IDSIZE(PEDIR_IMPORT) + IDSIZE(PEDIR_BOUNDIM) + IDSIZE(PEDIR_IAT) + IDSIZE(PEDIR_DELAYIMP) + IDSIZE(PEDIR_RELOC);
    ibuf.alloc(usize + xtrasize);

    // BOUND IMPORT support. FIXME: is this ok?
    fi->seek(0,SEEK_SET);
    fi->readx(ibuf,isection[0].rawdataptr);

    Interval holes(ibuf);

    unsigned ic,jc,overlaystart = 0;
    ibuf.clear(0, usize);
    for (ic = jc = 0; ic < objs; ic++)
    {
        if (isection[ic].rawdataptr && overlaystart < isection[ic].rawdataptr + isection[ic].size)
            overlaystart = ALIGN_UP(isection[ic].rawdataptr + isection[ic].size,ih.filealign);
        if (isection[ic].vsize == 0)
            isection[ic].vsize = isection[ic].size;
        if ((isection[ic].flags & PEFL_BSS) || isection[ic].rawdataptr == 0
            || (isection[ic].flags & PEFL_INFO))
        {
            holes.add(isection[ic].vaddr,isection[ic].vsize);
            continue;
        }
        if (isection[ic].vaddr + isection[ic].size > usize)
            throwCantPack("section size problem");
        if (((isection[ic].flags & (PEFL_WRITE|PEFL_SHARED))
            == (PEFL_WRITE|PEFL_SHARED)))
            if (!opt->force)
                throwCantPack("writable shared sections not supported (try --force)");
        if (jc && isection[ic].rawdataptr - jc > ih.filealign)
            throwCantPack("superfluous data between sections");
        fi->seek(isection[ic].rawdataptr,SEEK_SET);
        jc = isection[ic].size;
        if (jc > isection[ic].vsize)
            jc = isection[ic].vsize;
        if (isection[ic].vsize == 0) // hack for some tricky programs - may this break other progs?
            jc = isection[ic].vsize = isection[ic].size;
        if (isection[ic].vaddr + jc > ibuf.getSize())
            throwInternalError("buffer too small 1");
        fi->readx(ibuf + isection[ic].vaddr,jc);
        jc += isection[ic].rawdataptr;
    }

    // check for NeoLite
    if (find(ibuf + ih.entry, 64+7, "NeoLite", 7) >= 0)
        throwCantPack("file is already compressed with another packer");

    unsigned overlay = file_size - stripDebug(overlaystart);
    if (overlay >= (unsigned) file_size)
    {
#if 0
        if (overlay < file_size + ih.filealign)
            overlay = 0;
        else if (!opt->force)
            throwNotCompressible("overlay problem (try --force)");
#endif
        overlay = 0;
    }
    checkOverlay(overlay);

    Resource res;
    Interval tlsiv(ibuf);
    Export xport((char*)(unsigned char*)ibuf);

    const unsigned dllstrings = processImports();
    processTls(&tlsiv); // call before processRelocs!!
    processResources(&res);
    processExports(&xport);
    processRelocs();

    //OutputFile::dump("x1", ibuf, usize);

    // some checks for broken linkers - disable filter if necessary
    bool allow_filter = true;
    if (ih.codebase == ih.database
        || ih.codebase + ih.codesize > ih.imagesize
        || (isection[virta2objnum(ih.codebase,isection,objs)].flags & PEFL_CODE) == 0)
        allow_filter = false;

    const unsigned oam1 = ih.objectalign - 1;

    // FIXME: disabled: the uncompressor would not allocate enough memory
    //objs = tryremove(IDADDR(PEDIR_RELOC),objs);

    // FIXME: if the last object has a bss then this won't work
    // newvsize = (isection[objs-1].vaddr + isection[objs-1].size + oam1) &~ oam1;
    // temporary solution:
    unsigned newvsize = (isection[objs-1].vaddr + isection[objs-1].vsize + oam1) &~ oam1;

    //fprintf(stderr,"newvsize=%x objs=%d\n",newvsize,objs);
    if (newvsize + soimport + sorelocs > ibuf.getSize())
         throwInternalError("buffer too small 2");
    memcpy(ibuf+newvsize,oimport,soimport);
    memcpy(ibuf+newvsize+soimport,orelocs,sorelocs);

    cimports = newvsize - rvamin;   // rva of preprocessed imports
    crelocs = cimports + soimport;  // rva of preprocessed fixups

    ph.u_len = newvsize + soimport + sorelocs;

    // some extra data for uncompression support
    unsigned s = 0;
    upx_byte * const p1 = ibuf + ph.u_len;
    memcpy(p1 + s,&ih,sizeof (ih));
    s += sizeof (ih);
    memcpy(p1 + s,isection,ih.objects * sizeof(*isection));
    s += ih.objects * sizeof(*isection);
    if (soimport)
    {
        set_le32(p1 + s,cimports);
        set_le32(p1 + s + 4,dllstrings);
        s += 8;
    }
    if (sorelocs)
    {
        set_le32(p1 + s,crelocs);
        p1[s + 4] = (unsigned char) (big_relocs & 6);
        s += 5;
    }
    if (soresources)
    {
        set_le16(p1 + s,icondir_count);
        s += 2;
    }
    // end of extra data
    set_le32(p1 + s,ptr_diff(p1,ibuf) - rvamin);
    s += 4;
    ph.u_len += s;
    obuf.allocForCompression(ph.u_len);

    // prepare packheader
    ph.u_len -= rvamin;
    // prepare filter
    Filter ft(ph.level);
    ft.buf_len = ih.codesize;
    ft.addvalue = ih.codebase - rvamin;
    // compress
    int filter_strategy = allow_filter ? 0 : -3;

    // disable filters for files with broken headers
    if (ih.codebase + ih.codesize > ph.u_len)
    {
        ft.buf_len = 1;
        filter_strategy = -3;
    }

    // limit stack size needed for runtime decompression
    upx_compress_config_t cconf; cconf.reset();
    cconf.conf_lzma.max_num_probs = 1846 + (768 << 4); // ushort: ~28 KiB stack
    compressWithFilters(&ft, 2048, &cconf, filter_strategy,
                        ih.codebase, rvamin, 0, NULL, 0);
// info: see buildLoader()
    newvsize = (ph.u_len + rvamin + ph.overlap_overhead + oam1) &~ oam1;
    /*
    if (tlsindex && ((newvsize - ph.c_len - 1024 + oam1) &~ oam1) > tlsindex + 4)
    tlsindex = 0;
    */

    const unsigned lsize = getLoaderSize();

    int identsize = 0;
    const unsigned codesize = getLoaderSection("IDENTSTR",&identsize);
    assert(identsize > 0);
    getLoaderSection("UPX1HEAD",(int*)&ic);
    identsize += ic;

    pe_section_t osection[4];
    // section 0 : bss
    //         1 : [ident + header] + packed_data + unpacker + tls
    //         2 : not compressed data
    //         3 : resource data -- wince 5 needs a new section for this

    // identsplit - number of ident + (upx header) bytes to put into the PE header
    int identsplit = pe_offset + sizeof(osection) + sizeof(oh);
    if ((identsplit & 0x1ff) == 0)
        identsplit = 0;
    else if (((identsplit + identsize) ^ identsplit) < 0x200)
        identsplit = identsize;
    else
        identsplit = ALIGN_GAP(identsplit, 0x200);
    ic = identsize - identsplit;

    const unsigned c_len = ((ph.c_len + ic) & 15) == 0 ? ph.c_len : ph.c_len + 16 - ((ph.c_len + ic) & 15);
    obuf.clear(ph.c_len, c_len - ph.c_len);

    const unsigned s1size = ALIGN_UP(ic + c_len + codesize,4u) + sotls;
    const unsigned s1addr = (newvsize - (ic + c_len) + oam1) &~ oam1;

    const unsigned ncsection = (s1addr + s1size + oam1) &~ oam1;
    const unsigned upxsection = s1addr + ic + c_len;

    Reloc rel(1024); // new relocations are put here
    static const char* symbols_to_relocate[] = {
        "ONAM", "BIMP", "BREL", "FIBE", "FIBS", "ENTR", "DST0", "SRC0"
    };
    for (unsigned s2r = 0; s2r < TABLESIZE(symbols_to_relocate); s2r++)
    {
        unsigned off = linker->getSymbolOffset(symbols_to_relocate[s2r]);
        if (off != 0xdeaddead)
            rel.add(off + upxsection, 3);
    }

    // new PE header
    memcpy(&oh,&ih,sizeof(oh));
    oh.filealign = 0x200; // identsplit depends on this
    memset(osection,0,sizeof(osection));

    oh.entry = upxsection;
    oh.objects = 4;
    oh.chksum = 0;

    // fill the data directory
    ODADDR(PEDIR_DEBUG) = 0;
    ODSIZE(PEDIR_DEBUG) = 0;
    ODADDR(PEDIR_IAT) = 0;
    ODSIZE(PEDIR_IAT) = 0;
    ODADDR(PEDIR_BOUNDIM) = 0;
    ODSIZE(PEDIR_BOUNDIM) = 0;


    // tls is put into section 1
    ic = s1addr + s1size - sotls;
    super::processTls(&rel,&tlsiv,ic);
    ODADDR(PEDIR_TLS) = sotls ? ic : 0;
    ODSIZE(PEDIR_TLS) = sotls ? 0x18 : 0;
    ic += sotls;

    // these are put into section 2

    ic = ncsection;

    // wince wants relocation data at the beginning of a section
    processRelocs(&rel);
    ODADDR(PEDIR_RELOC) = soxrelocs ? ic : 0;
    ODSIZE(PEDIR_RELOC) = soxrelocs;
    ic += soxrelocs;

    processImports(ic, linker->getSymbolOffset("IATT") + upxsection);
    ODADDR(PEDIR_IMPORT) = ic;
    ODSIZE(PEDIR_IMPORT) = soimpdlls;
    ic += soimpdlls;

    processExports(&xport,ic);
    ODADDR(PEDIR_EXPORT) = soexport ? ic : 0;
    ODSIZE(PEDIR_EXPORT) = soexport;
    if (!isdll && opt->win32_pe.compress_exports)
    {
        ODADDR(PEDIR_EXPORT) = IDADDR(PEDIR_EXPORT);
        ODSIZE(PEDIR_EXPORT) = IDSIZE(PEDIR_EXPORT);
    }
    ic += soexport;

    ic = (ic + oam1) &~ oam1;
    const unsigned res_start = ic;
    if (soresources)
        processResources(&res,ic);
    ODADDR(PEDIR_RESOURCE) = soresources ? ic : 0;
    ODSIZE(PEDIR_RESOURCE) = soresources;
    ic += soresources;

    const unsigned onam = ncsection + soxrelocs + ih.imagebase;
    linker->defineSymbol("start_of_dll_names", onam);
    linker->defineSymbol("start_of_imports", ih.imagebase + rvamin + cimports);
    linker->defineSymbol("start_of_relocs", crelocs + rvamin + ih.imagebase);
    linker->defineSymbol("filter_buffer_end", ih.imagebase + ih.codebase + ih.codesize);
    linker->defineSymbol("filter_buffer_start", ih.imagebase + ih.codebase);
    linker->defineSymbol("original_entry", ih.entry + ih.imagebase);
    linker->defineSymbol("uncompressed_length", ph.u_len);
    linker->defineSymbol("start_of_uncompressed", ih.imagebase + rvamin);
    linker->defineSymbol("compressed_length", ph.c_len);
    linker->defineSymbol("start_of_compressed", ih.imagebase + s1addr + identsize - identsplit);
    defineDecompressorSymbols();
    relocateLoader();

    MemBuffer loader(lsize);
    memcpy(loader, getLoader(), lsize);
    patchPackHeader(loader, lsize);

    // this is computed here, because soxrelocs changes some lines above
    const unsigned ncsize = soxrelocs + soimpdlls + soexport;
    const unsigned fam1 = oh.filealign - 1;

    // fill the sections
    strcpy(osection[0].name,"UPX0");
    strcpy(osection[1].name,"UPX1");
    strcpy(osection[2].name, "UPX2");
    strcpy(osection[3].name, ".rsrc");

    osection[0].vaddr = rvamin;
    osection[1].vaddr = s1addr;
    osection[2].vaddr = ncsection;
    osection[3].vaddr = res_start;

    osection[0].size = 0;
    osection[1].size = (s1size + fam1) &~ fam1;
    osection[2].size = (ncsize + fam1) &~ fam1;
    osection[3].size = (soresources + fam1) &~ fam1;

    osection[0].vsize = osection[1].vaddr - osection[0].vaddr;
    //osection[1].vsize = (osection[1].size + oam1) &~ oam1;
    //osection[2].vsize = (osection[2].size + oam1) &~ oam1;
    osection[1].vsize = osection[1].size;
    osection[2].vsize = osection[2].size;
    osection[3].vsize = osection[3].size;

    osection[0].rawdataptr = 0;
    osection[1].rawdataptr = (pe_offset + sizeof(oh) + sizeof(osection) + fam1) &~ fam1;
    osection[2].rawdataptr = osection[1].rawdataptr + osection[1].size;
    osection[3].rawdataptr = osection[2].rawdataptr + osection[2].size;

    osection[0].flags = (unsigned) (PEFL_BSS|PEFL_EXEC|PEFL_WRITE|PEFL_READ);
    osection[1].flags = (unsigned) (PEFL_DATA|PEFL_EXEC|PEFL_WRITE|PEFL_READ);
    osection[2].flags = (unsigned) (PEFL_DATA|PEFL_READ);
    osection[3].flags = (unsigned) (PEFL_DATA|PEFL_READ);

    oh.imagesize = (osection[3].vaddr + osection[3].vsize + oam1) &~ oam1;
    oh.bsssize  = osection[0].vsize;
    oh.datasize = osection[2].vsize + osection[3].vsize;
    oh.database = osection[2].vaddr;
    oh.codesize = osection[1].vsize;
    oh.codebase = osection[1].vaddr;
    oh.headersize = osection[1].rawdataptr;
    if (rvamin < osection[0].rawdataptr)
        throwCantPack("object alignment too small");

    if (opt->win32_pe.strip_relocs && !isdll)
        oh.flags |= RELOCS_STRIPPED;

    //for (ic = 0; ic < oh.filealign; ic += 4)
    //    set_le32(ibuf + ic,get_le32("UPX "));
    ibuf.clear(0, oh.filealign);

    info("Image size change: %u -> %u KiB",
         ih.imagesize / 1024, oh.imagesize / 1024);

    infoHeader("[Writing compressed file]");

    if (soresources == 0)
    {
        oh.objects = 3;
        memset(&osection[3], 0, sizeof(osection[3]));
    }
    // write loader + compressed file
    fo->write(&oh,sizeof(oh));
    fo->write(osection,sizeof(osection));
    // some alignment
    if (identsplit == identsize)
    {
        unsigned n = osection[1].rawdataptr - fo->getBytesWritten() - identsize;
        assert(n <= oh.filealign);
        fo->write(ibuf, n);
    }
    fo->write(loader + codesize,identsize);
    infoWriting("loader", fo->getBytesWritten());
    fo->write(obuf,c_len);
    infoWriting("compressed data", c_len);
    fo->write(loader,codesize);
    if (opt->debug.dump_stub_loader)
        OutputFile::dump(opt->debug.dump_stub_loader, loader, codesize);
    if ((ic = fo->getBytesWritten() & 3) != 0)
        fo->write(ibuf,4 - ic);
    fo->write(otls,sotls);
    if ((ic = fo->getBytesWritten() & fam1) != 0)
        fo->write(ibuf,oh.filealign - ic);
    fo->write(oxrelocs,soxrelocs);
    fo->write(oimpdlls,soimpdlls);
    fo->write(oexport,soexport);

    if ((ic = fo->getBytesWritten() & fam1) != 0)
        fo->write(ibuf,oh.filealign - ic);

    fo->write(oresources,soresources);
    if ((ic = fo->getBytesWritten() & fam1) != 0)
        fo->write(ibuf,oh.filealign - ic);

#if 0
    printf("%-13s: program hdr  : %8ld bytes\n", getName(), (long) sizeof(oh));
    printf("%-13s: sections     : %8ld bytes\n", getName(), (long) sizeof(osection));
    printf("%-13s: ident        : %8ld bytes\n", getName(), (long) identsize);
    printf("%-13s: compressed   : %8ld bytes\n", getName(), (long) c_len);
    printf("%-13s: decompressor : %8ld bytes\n", getName(), (long) codesize);
    printf("%-13s: tls          : %8ld bytes\n", getName(), (long) sotls);
    printf("%-13s: resources    : %8ld bytes\n", getName(), (long) soresources);
    printf("%-13s: imports      : %8ld bytes\n", getName(), (long) soimpdlls);
    printf("%-13s: exports      : %8ld bytes\n", getName(), (long) soexport);
    printf("%-13s: relocs       : %8ld bytes\n", getName(), (long) soxrelocs);
#endif

    // verify
    verifyOverlappingDecompression();

    // copy the overlay
    copyOverlay(fo, overlay, &obuf);

    // finally check the compression ratio
    if (!checkFinalCompressionRatio(fo))
        throwNotCompressible();
}