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);
}
virtual void post(Home home, IntRelType irt, int c,
BoolVar b, IntConLevel icl) const {
rel(home, post(home,NULL,icl), irt, c, b);
}
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);
}
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);
}
}
}
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();
}
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);
}
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;
}
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);
}
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 );
}
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);
}
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;
}
}
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();
}