SP::SiconosVector Simulation::lambda(unsigned int level, unsigned int coor) { // return input(level) (ie with lambda[level]) for all Interactions. // assert(level>=0); DEBUG_BEGIN("Simulation::input(unsigned int level, unsigned int coor)\n"); DEBUG_PRINTF("with level = %i and coor = %i \n", level,coor); InteractionsGraph::VIterator ui, uiend; SP::Interaction inter; SP::InteractionsGraph indexSet0 = _nsds->topology()->indexSet0(); SP::SiconosVector lambda (new SiconosVector (_nsds->topology()->indexSet0()->size() )); int i=0; for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui) { inter = indexSet0->bundle(*ui); assert(inter->lowerLevelForOutput() <= level); assert(inter->upperLevelForOutput() >= level); lambda->setValue(i,inter->lambda(level)->getValue(coor)); i++; } DEBUG_END("Simulation::input(unsigned int level, unsigned int coor)\n"); return lambda; }
void EventDriven::updateIndexSetsWithDoubleCondition() { assert(_nsds); assert(_nsds->topology()); // for all Interactions in indexSet[i-1], compute y[i-1] and // update the indexSet[i] SP::Topology topo = _nsds->topology(); SP::InteractionsGraph indexSet2 = topo->indexSet(2); InteractionsGraph::VIterator ui, uiend, vnext; std11::tie(ui, uiend) = indexSet2->vertices(); for (vnext = ui; ui != uiend; ui = vnext) { ++vnext; SP::Interaction inter = indexSet2->bundle(*ui); double gamma = inter->getYRef(2); double F = inter->getLambdaRef(2); if (fabs(F) < _TOL_ED) indexSet2->remove_vertex(inter); else if ((gamma < -_TOL_ED) || (F < -_TOL_ED)) RuntimeException::selfThrow("EventDriven::updateIndexSetsWithDoubleCondition(), output[2] and lambda[2] for Interactionof indexSet[2] must be higher or equal to zero."); else if (((fabs(gamma) > _TOL_ED) && (fabs(F) > _TOL_ED))) RuntimeException::selfThrow("EventDriven::updateIndexSetsWithDoubleCondition(), something is wrong for the LCP resolution."); } }
std::pair<DynamicalSystemsGraph::EDescriptor, InteractionsGraph::VDescriptor> Topology::link(SP::Interaction inter, SP::DynamicalSystem ds, SP::DynamicalSystem ds2) { DEBUG_PRINTF("Topology::link : inter %p, ds1 %p, ds2 %p\n", &*inter, &*ds, &*ds2); if (indexSet0()->is_vertex(inter)) { removeInteractionFromIndexSet(inter); } unsigned int sumOfDSSizes = 0, sumOfZSizes = 0; sumOfDSSizes += ds->getDim(); if(ds->z()) sumOfZSizes += ds->z()->size(); if(ds2) { sumOfDSSizes += ds2->getDim(); if(ds->z()) sumOfZSizes += ds2->z()->size(); inter->setHas2Bodies(true); } DEBUG_PRINTF("sumOfDSSizes = %i\t, sumOfZSizes = %i\n ", sumOfDSSizes, sumOfZSizes); inter->setDSSizes(sumOfDSSizes, sumOfZSizes); return addInteractionInIndexSet0(inter, ds, ds2); }
void MLCPProjectOnConstraints::postCompute() { _hasBeenUpdated = true; // This function is used to set y/lambda values using output from // lcp_driver (w,z). Only Interactions (ie Interactions) of // indexSet(leveMin) are concerned. // === Get index set from Topology === SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel()); // y and lambda vectors SP::SiconosVector lambda; SP::SiconosVector y; // === Loop through "active" Interactions (ie present in // indexSets[1]) === /** We chose to do a small step _alpha in view of stabilized the algorithm.*/ #ifdef MLCPPROJ_DEBUG printf("MLCPProjectOnConstraints::postCompute damping value = %f\n", _alpha); #endif (*_z) *= _alpha; unsigned int pos = 0; #ifdef MLCPPROJ_DEBUG printf("MLCPProjectOnConstraints::postCompute _z\n"); _z->display(); display(); #endif InteractionsGraph::VIterator ui, uiend; for (std11::tie(ui, uiend) = indexSet->vertices(); ui != uiend; ++ui) { SP::Interaction inter = indexSet->bundle(*ui); // Get the relative position of inter-interactionBlock in the vector w // or z pos = _M->getPositionOfInteractionBlock(*inter); RELATION::TYPES relationType = inter->relation()->getType(); if (relationType == NewtonEuler) { postComputeNewtonEulerR(inter, pos); } else if (relationType == Lagrangian) { postComputeLagrangianR(inter, pos); } else { RuntimeException::selfThrow("MLCPProjectOnConstraints::computeInteractionBlock - relation type is not from Lagrangian type neither NewtonEuler."); } } }
void visit(const Hem5OSI&) { unsigned int lowerLevelForOutput = LEVELMAX; unsigned int upperLevelForOutput = 0; unsigned int lowerLevelForInput = LEVELMAX; unsigned int upperLevelForInput = 0; Type::Siconos dsType = Type::value(*_ds); /** there is only a test on the dstype and simulation since we assume that * we implicitely the nonsmooth law when a DS type is chosen */ if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS || dsType == Type::NewtonEulerDS) { if (Type::value(*_parent) == Type::EventDriven) { Type::Siconos nslType = Type::value(*_nonSmoothLaw); if (nslType == Type::NewtonImpactNSL || nslType == Type::MultipleImpactNSL) { lowerLevelForOutput = 0; upperLevelForOutput = 2 ; lowerLevelForInput = 1; upperLevelForInput = 2; } else if (nslType == Type::NewtonImpactFrictionNSL) { lowerLevelForOutput = 0; upperLevelForOutput = 4; lowerLevelForInput = 1; upperLevelForInput = 2; RuntimeException::selfThrow("Simulation::SetupLevels::visit - simulation of type: " + Type::name(*_parent) + " not yet implemented for nonsmooth law of type NewtonImpactFrictionNSL"); } else { RuntimeException::selfThrow("Simulation::SetupLevels::visit - simulation of type: " + Type::name(*_parent) + "not yet implemented for nonsmooth of type"); } } else RuntimeException::selfThrow("Simulation::SetupLevels::visit - unknown simulation type: " + Type::name(*_parent)); } else RuntimeException::selfThrow("Simulation::SetupLevels::visit - not yet implemented for Dynamical system type :" + dsType); _parent->_levelMinForInput = std::min<int>(lowerLevelForInput, _parent->_levelMinForInput); _parent->_levelMaxForInput = std::max<int>(upperLevelForInput, _parent->_levelMaxForInput); _parent->_levelMinForOutput = std::min<int>(lowerLevelForOutput, _parent->_levelMinForInput); _parent->_levelMaxForOutput = std::max<int>(upperLevelForOutput, _parent->_levelMaxForInput); _parent->_numberOfIndexSets = std::max<int>(_parent->_levelMaxForOutput + 1, _parent->_numberOfIndexSets); _interaction->setLowerLevelForOutput(lowerLevelForOutput); _interaction->setUpperLevelForOutput(upperLevelForOutput); _interaction->setLowerLevelForInput(lowerLevelForInput); _interaction->setUpperLevelForInput(upperLevelForInput); _interaction->setSteps(1); };
void visit(const NewtonImpactFrictionNSL& nslaw) { double e; e = nslaw.en(); // Only the normal part is multiplied by e SP::SiconosVector y_k_1 ; y_k_1 = _inter->yMemory(_osnsp->inputOutputLevel())->getSiconosVector(1); (*_inter->yForNSsolver())(0) += e * (*y_k_1)(0); }
void visit(const NewtonImpactNSL& nslaw) { double e; e = nslaw.e(); Index subCoord(4); subCoord[0] = 0; subCoord[1] = _inter->nonSmoothLaw()->size(); subCoord[2] = 0; subCoord[3] = subCoord[1]; subscal(e, *_inter->yOld(_osnsp->inputOutputLevel()), *(_inter->yForNSsolver()), subCoord, false); // q = q + e * q }
void MLCPProjectOnConstraints::computeqBlock(InteractionsGraph::VDescriptor& vertex_inter, unsigned int pos) { SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel()); SP::Interaction inter = indexSet->bundle(vertex_inter); unsigned int sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints> (_M)->computeSizeForProjection(inter); for (unsigned int i = 0; i < sizeY; i++) _q->setValue(pos + i, inter->y(0)->getValue(0 + i)); #ifdef MLCPPROJ_DEBUG printf("MLCPProjectOnConstraints::computeqBlock, _q from y(0)\n"); _q->display(); #endif }
void MLCPProjectOnConstraints::postComputeNewtonEulerR(SP::Interaction inter, unsigned int pos) { SP::NewtonEulerR ner = (std11::static_pointer_cast<NewtonEulerR>(inter->relation())); SP::SiconosVector lambda = inter->lambda(0); SP::SiconosVector y = inter->y(0); unsigned int sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints> (_M)->computeSizeForProjection(inter); // Copy _w/_z values, starting from index pos into y/lambda. //setBlock(*_w, y, sizeY, pos, 0); setBlock(*_z, lambda, sizeY, pos, 0); }
void visit(const MoreauJeanCombinedProjectionOSI& moreauCPOSI) { unsigned int lowerLevelForOutput = LEVELMAX; unsigned int upperLevelForOutput = 0; unsigned int lowerLevelForInput = LEVELMAX; unsigned int upperLevelForInput = 0; Type::Siconos dsType = Type::value(*_ds); if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS || dsType == Type::NewtonEulerDS) { if (Type::value(*_parent) == Type::TimeStepping) { lowerLevelForOutput = 0; upperLevelForOutput = 1; lowerLevelForInput = 1; upperLevelForInput = 1; } else if (Type::value(*_parent) == Type::TimeSteppingCombinedProjection) { // Warning : we never enter this case !!! lowerLevelForOutput = 0; upperLevelForOutput = 1 ; lowerLevelForInput = 0; upperLevelForInput = 1; } else { RuntimeException::selfThrow("Simulation::SetupLevels::visit(const MoreauJeanCombinedProjectionOSI) - unknown simulation type: " + Type::name(*_parent)); } } else RuntimeException::selfThrow("Simulation::SetupLevels::visit(const MoreauJeanCombinedProjectionOSI) - not yet implemented for Dynamical system type :" + dsType); _parent->_levelMinForInput = std::min<int>(lowerLevelForInput, _parent->_levelMinForInput); _parent->_levelMaxForInput = std::max<int>(upperLevelForInput, _parent->_levelMaxForInput); _parent->_levelMinForOutput = std::min<int>(lowerLevelForOutput, _parent->_levelMinForInput); _parent->_levelMaxForOutput = std::max<int>(upperLevelForOutput, _parent->_levelMaxForInput); _parent->_numberOfIndexSets = std::max<int>(_parent->_levelMaxForOutput + 1, _parent->_numberOfIndexSets); _interaction->setLowerLevelForOutput(lowerLevelForOutput); _interaction->setUpperLevelForOutput(upperLevelForOutput); _interaction->setLowerLevelForInput(lowerLevelForInput); _interaction->setUpperLevelForInput(upperLevelForInput); _interaction->setSteps(1); };
void visit(const D1MinusLinearOSI& d1OSI) { unsigned int lowerLevelForOutput = LEVELMAX; unsigned int upperLevelForOutput = 0; unsigned int lowerLevelForInput = LEVELMAX; unsigned int upperLevelForInput = 0; Type::Siconos dsType = Type::value(*_ds); /** there is only a test on the dstype and simulation since we assume that * we implicitely the nonsmooth law when a DS type is chosen */ if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS || dsType == Type::NewtonEulerDS) { if (Type::value(*_parent) == Type::TimeSteppingD1Minus) { lowerLevelForOutput = 0; upperLevelForOutput = 2 ; lowerLevelForInput = 1; upperLevelForInput = 2; } else RuntimeException::selfThrow("Simulation::SetupLevels::visit(const D1MinusLinearOSI&) - unknown simulation type: " + Type::name(*_parent)); } else RuntimeException::selfThrow("Simulation::SetupLevels::visit(const D1MinusLinearOSI&) - not yet implemented for Dynamical system type :" + dsType); _parent->_levelMinForInput = std::min<int>(lowerLevelForInput, _parent->_levelMinForInput); _parent->_levelMaxForInput = std::max<int>(upperLevelForInput, _parent->_levelMaxForInput); _parent->_levelMinForOutput = std::min<int>(lowerLevelForOutput, _parent->_levelMinForInput); _parent->_levelMaxForOutput = std::max<int>(upperLevelForOutput, _parent->_levelMaxForInput); /* Get the number of required index sets. */ unsigned int nbIndexSets = d1OSI.numberOfIndexSets(); _parent->_numberOfIndexSets = std::max<int>(nbIndexSets, _parent->_numberOfIndexSets); _interaction->setLowerLevelForOutput(lowerLevelForOutput); _interaction->setUpperLevelForOutput(upperLevelForOutput); _interaction->setLowerLevelForInput(lowerLevelForInput); _interaction->setUpperLevelForInput(upperLevelForInput); _interaction->setSteps(2); // Two evaluations of lambda(2) are made for each time--step };
void visit(const NewtonImpactNSL& nslaw) { double e; e = nslaw.e(); Index subCoord(4); subCoord[0] = 0; subCoord[1] = _inter->nonSmoothLaw()->size(); subCoord[2] = 0; subCoord[3] = subCoord[1]; // Only the normal part is multiplied by e SP::SiconosVector y_k_1 ; y_k_1 = _inter->yMemory(_osnsp->inputOutputLevel())->getSiconosVector(1); // std::cout << "y_k_1 " << std::endl; // y_k_1->display(); subscal(e, *y_k_1, *(_inter->yForNSsolver()), subCoord, false); }
void visit(const NewtonImpactNSL& nslaw) { double e; e = nslaw.e(); Index subCoord(4); subCoord[0] = 0; subCoord[1] = _inter->nonSmoothLaw()->size(); subCoord[2] = 0; subCoord[3] = subCoord[1]; // Only the normal part is multiplied by e const SiconosVector& y_k_1( _inter->yMemory(_osnsp->inputOutputLevel()).getSiconosVector(1)); DEBUG_PRINTF("_osnsp->inputOutputLevel() = %i \n ",_osnsp->inputOutputLevel() ); DEBUG_EXPR(y_k_1.display());; SiconosVector & osnsp_rhs = *(*_interProp.workVectors)[SchatzmanPaoliOSI::OSNSP_RHS]; subscal(e, y_k_1, osnsp_rhs, subCoord, false); }
void visit(const NewtonImpactFrictionNSL& nslaw) { double e; e = nslaw.en(); // Only the normal part is multiplied by e const SiconosVector& y_k_1( _inter->yMemory(_osnsp->inputOutputLevel()).getSiconosVector(1)); SiconosVector & osnsp_rhs = *(*_interProp.workVectors)[SchatzmanPaoliOSI::OSNSP_RHS]; osnsp_rhs (0) += e * (y_k_1)(0); }
void Simulation::updateOutput(unsigned int level) { // To compute output(level) (ie with y[level]) for all Interactions. // assert(level>=0); DEBUG_PRINTF("Simulation::updateOutput(unsigned int level) starts for level = %i\n", level); double time = model()->currentTime(); InteractionsGraph::VIterator ui, uiend; SP::Interaction inter; SP::InteractionsGraph indexSet0 = model()->nonSmoothDynamicalSystem()->topology()->indexSet0(); for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui) { inter = indexSet0->bundle(*ui); assert(inter->lowerLevelForOutput() <= level); assert(inter->upperLevelForOutput() >= level); inter->computeOutput(time, indexSet0->properties(*ui), level); } DEBUG_PRINTF("Simulation::updateOutput(unsigned int level) ends for level = %i\n", level); }
void Simulation::updateInput(unsigned int level) { // To compute input(level) (ie with lambda[level]) for all Interactions. // assert(level>=0); // double time = nextTime(); double time = model()->currentTime(); // Set dynamical systems non-smooth part to zero. reset(level); // We compute input using lambda(level). InteractionsGraph::VIterator ui, uiend; SP::Interaction inter; SP::InteractionsGraph indexSet0 = model()->nonSmoothDynamicalSystem()->topology()->indexSet0(); for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui) { inter = indexSet0->bundle(*ui); assert(inter->lowerLevelForInput() <= level); assert(inter->upperLevelForInput() >= level); inter->computeInput(time, indexSet0->properties(*ui), level); } }
void Simulation::initializeInteraction(double time, SP::Interaction inter) { // determine which (lower and upper) levels are required for this Interaction // in this Simulation. computeLevelsForInputAndOutput(inter); // Get the interaction properties from the topology for initialization. SP::InteractionsGraph indexSet0 = _nsds->topology()->indexSet0(); InteractionsGraph::VDescriptor ui = indexSet0->descriptor(inter); // This calls computeOutput() and initializes qMemory and q_k. inter->initialize(time, indexSet0->properties(ui)); }
void visit(const SchatzmanPaoliOSI&) { unsigned int lowerLevelForOutput = LEVELMAX; unsigned int upperLevelForOutput = 0; unsigned int lowerLevelForInput = LEVELMAX; unsigned int upperLevelForInput = 0; Type::Siconos dsType = Type::value(*_ds); if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS || dsType == Type::NewtonEulerDS) { if (Type::value(*_parent) == Type::TimeStepping) { lowerLevelForOutput = 0; upperLevelForOutput = 0; lowerLevelForInput = 0; upperLevelForInput = 0; } else RuntimeException::selfThrow("Simulation::SetupLevels::visit - unknown simulation type: " + Type::name(*_parent)); } else RuntimeException::selfThrow("Simulation::SetupLevels::visit - not yet implemented for Dynamical system type :" + dsType); _parent->_levelMinForInput = std::min<int>(lowerLevelForInput, _parent->_levelMinForInput); _parent->_levelMaxForInput = std::max<int>(upperLevelForInput, _parent->_levelMaxForInput); _parent->_levelMinForOutput = std::min<int>(lowerLevelForOutput, _parent->_levelMinForInput); _parent->_levelMaxForOutput = std::max<int>(upperLevelForOutput, _parent->_levelMaxForInput); _parent->_numberOfIndexSets = std::max<int>(_parent->_levelMaxForOutput + 1, _parent->_numberOfIndexSets); _interaction->setLowerLevelForOutput(lowerLevelForOutput); _interaction->setUpperLevelForOutput(upperLevelForOutput); _interaction->setLowerLevelForInput(lowerLevelForInput); _interaction->setUpperLevelForInput(upperLevelForInput); _interaction->setSteps(2); };
void SchatzmanPaoliOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp) { DEBUG_BEGIN("SchatzmanPaoliOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp)\n"); /** \warning: ensures that it can also work with two different osi for two different ds ? */ SP::InteractionsGraph indexSet = osnsp->simulation()->indexSet(osnsp->indexSetLevel()); SP::Interaction inter = indexSet->bundle(vertex_inter); SP::OneStepNSProblems allOSNS = _simulation->oneStepNSProblems(); VectorOfBlockVectors& inter_work_block = *indexSet->properties(vertex_inter).workBlockVectors; // Get relation and non smooth law types RELATION::TYPES relationType = inter->relation()->getType(); RELATION::SUBTYPES relationSubType = inter->relation()->getSubType(); unsigned int sizeY = inter->nonSmoothLaw()->size(); unsigned int relativePosition = 0; Index coord(8); coord[0] = relativePosition; coord[1] = relativePosition + sizeY; coord[2] = 0; coord[4] = 0; coord[6] = 0; coord[7] = sizeY; SP::SiconosMatrix C; SP::SiconosMatrix D; SP::SiconosMatrix F; SP::BlockVector deltax; SiconosVector& osnsp_rhs = *(*indexSet->properties(vertex_inter).workVectors)[SchatzmanPaoliOSI::OSNSP_RHS]; SP::SiconosVector e; SP::BlockVector Xfree = inter_work_block[SchatzmanPaoliOSI::xfree];; assert(Xfree); SP::Interaction mainInteraction = inter; assert(mainInteraction); assert(mainInteraction->relation()); DEBUG_EXPR(inter->display(););
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); } } }
SetupLevels(SP::Simulation s, SP::Interaction inter, SP::DynamicalSystem ds) : _parent(s), _interaction(inter), _ds(ds) { _nonSmoothLaw = inter->nonSmoothLaw(); };
void EulerMoreauOSI::computeW(double t, DynamicalSystem& ds, DynamicalSystemsGraph::VDescriptor& dsgVD) { // Compute W matrix of the Dynamical System ds, at time t and for the current ds state. // When this function is called, WMap[ds] is supposed to exist and not to be null // Memory allocation has been done during initW. unsigned int dsN = ds.number(); assert((WMap.find(dsN) != WMap.end()) && "EulerMoreauOSI::computeW(t,ds) - W(ds) does not exists. Maybe you forget to initialize the osi?"); double h = simulationLink->timeStep(); Type::Siconos dsType = Type::value(ds); SiconosMatrix& W = *WMap[dsN]; // 1 - First order non linear systems if (dsType == Type::FirstOrderNonLinearDS) { // W = M - h*_theta* [jacobian_x f(t,x,z)] FirstOrderNonLinearDS& d = static_cast<FirstOrderNonLinearDS&> (ds); // Copy M or I if M is Null into W if (d.M()) W = *d.M(); else W.eye(); d.computeJacobianfx(t); // Add -h*_theta*jacobianfx to W scal(-h * _theta, *d.jacobianfx(), W, false); } // 2 - First order linear systems else if (dsType == Type::FirstOrderLinearDS || dsType == Type::FirstOrderLinearTIDS) { FirstOrderLinearDS& d = static_cast<FirstOrderLinearDS&> (ds); if (dsType == Type::FirstOrderLinearDS) d.computeA(t); if (d.M()) W = *d.M(); else W.eye(); scal(-h * _theta, *d.A(), W, false); } else RuntimeException::selfThrow("EulerMoreauOSI::computeW - not yet implemented for Dynamical system type :" + dsType); // if (_useGamma) { Topology& topo = *simulationLink->model()->nonSmoothDynamicalSystem()->topology(); DynamicalSystemsGraph& DSG0 = *topo.dSG(0); InteractionsGraph& indexSet = *topo.indexSet(0); DynamicalSystemsGraph::OEIterator oei, oeiend; InteractionsGraph::VDescriptor ivd; SP::SiconosMatrix K; SP::Interaction inter; for (std11::tie(oei, oeiend) = DSG0.out_edges(dsgVD); oei != oeiend; ++oei) { inter = DSG0.bundle(*oei); ivd = indexSet.descriptor(inter); FirstOrderR& rel = static_cast<FirstOrderR&>(*inter->relation()); K = rel.K(); if (!K) K = (*indexSet.properties(ivd).workMatrices)[FirstOrderR::mat_K]; if (K) { scal(-h * _gamma, *K, W, false); } } } // Remark: W is not LU-factorized here. // Function PLUForwardBackward will do that if required. }
void EventDriven::updateIndexSet(unsigned int i) { assert(_nsds); assert(_nsds->topology()); SP::Topology topo = _nsds->topology(); assert(i < topo->indexSetsSize() && "EventDriven::updateIndexSet(i), indexSets[i] does not exist."); // IndexSets[0] must not be updated in simulation, since it belongs to Topology. assert(i > 0 && "EventDriven::updateIndexSet(i=0), indexSets[0] cannot be updated."); // For all Interactions in indexSet[i-1], compute y[i-1] and // update the indexSet[i]. SP::InteractionsGraph indexSet1 = topo->indexSet(1); SP::InteractionsGraph indexSet2 = topo->indexSet(2); assert(_indexSet0); assert(indexSet1); assert(indexSet2); // DEBUG_PRINTF("update indexSets start : indexSet0 size : %ld\n", indexSet0->size()); // DEBUG_PRINTF("update IndexSets start : indexSet1 size : %ld\n", indexSet1->size()); // DEBUG_PRINTF("update IndexSets start : indexSet2 size : %ld\n", indexSet2->size()); InteractionsGraph::VIterator uibegin, uipend, uip; std11::tie(uibegin, uipend) = _indexSet0->vertices(); // loop over all vertices of the indexSet[i-1] for (uip = uibegin; uip != uipend; ++uip) { SP::Interaction inter = _indexSet0->bundle(*uip); if (i == 1) // IndexSet[1] { // if indexSet[1]=>getYRef(0): output y // if indexSet[2]=>getYRef(1): output ydot double y = inter->getYRef(0); // output to define the IndexSets at this Interaction if (y < -_TOL_ED) // y[0] < 0 { inter->display(); cout << "y = " << y << " < -_TOL_ED = " << -_TOL_ED <<endl; RuntimeException::selfThrow("EventDriven::updateIndexSet, output of level 0 must be positive!!! "); } // 1 - If the Interaction is not yet in the set if (!indexSet1->is_vertex(inter)) // Interaction is not yet in the indexSet[i] { if (fabs(y) <= _TOL_ED) { // vertex and edges insertions indexSet1->copy_vertex(inter, *_indexSet0); } } else // if the Interaction was already in the set { if (fabs(y) > _TOL_ED) { indexSet1->remove_vertex(inter); // remove the Interaction from IndexSet[1] inter->lambda(1)->zero(); // reset the lambda[1] to zero } } } else if (i == 2) // IndexSet[2] { if (indexSet1->is_vertex(inter)) // Interaction is in the indexSet[1] { double y = inter->getYRef(1); // output of level 1 at this Interaction if (!indexSet2->is_vertex(inter)) // Interaction is not yet in the indexSet[2] { if (fabs(y) <= _TOL_ED) { // vertex and edges insertions indexSet2->copy_vertex(inter, *_indexSet0); } } else // if the Interaction was already in the set { if (fabs(y) > _TOL_ED) { indexSet2->remove_vertex(inter); // remove the Interaction from IndexSet[1] inter->lambda(2)->zero(); // reset the lambda[i] to zero } } } else // Interaction is not in the indexSet[1] { if (indexSet2->is_vertex(inter)) // Interaction is in the indexSet[2] { indexSet2->remove_vertex(inter); // remove the Interaction from IndexSet[2] inter->lambda(2)->zero(); // reset the lambda[i] to zero } } } else { RuntimeException::selfThrow("EventDriven::updateIndexSet, IndexSet[i > 2] doesn't exist"); } } // DEBUG_PRINTF("update indexSets end : indexSet0 size : %ld\n", indexSet0->size()); // DEBUG_PRINTF("update IndexSets end : indexSet1 size : %ld\n", indexSet1->size()); // DEBUG_PRINTF("update IndexSets end : indexSet2 size : %ld\n", indexSet2->size()); }
void EventDriven::computeg(SP::OneStepIntegrator osi, integer * sizeOfX, doublereal* time, doublereal* x, integer * ng, doublereal * gOut) { assert(_nsds); assert(_nsds->topology()); InteractionsGraph::VIterator ui, uiend; SP::Topology topo = _nsds->topology(); SP::InteractionsGraph indexSet2 = topo->indexSet(2); unsigned int nsLawSize, k = 0 ; SP::SiconosVector y, ydot, yddot, lambda; SP::LsodarOSI lsodar = std11::static_pointer_cast<LsodarOSI>(osi); // Solve LCP at acceleration level to calculate the lambda[2] at Interaction of indexSet[2] lsodar->fillXWork(sizeOfX, x); // double t = *time; if (!_allNSProblems->empty()) { if (((*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_ACC]->hasInteractions())) { (*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_ACC]->compute(t); } }; /* double * xdottmp = (double *)malloc(*sizeOfX*sizeof(double)); computef(osi, sizeOfX,time,x,xdottmp); free(xdottmp); */ // Update the output from level 0 to level 1 _nsds->updateOutput(t,0); _nsds->updateOutput(t,1); _nsds->updateOutput(t,2); // for (std11::tie(ui, uiend) = _indexSet0->vertices(); ui != uiend; ++ui) { SP::Interaction inter = _indexSet0->bundle(*ui); nsLawSize = inter->nonSmoothLaw()->size(); y = inter->y(0); // output y at this Interaction ydot = inter->y(1); // output of level 1 at this Interaction yddot = inter->y(2); lambda = inter->lambda(2); // input of level 2 at this Interaction if (!(indexSet2->is_vertex(inter))) // if Interaction is not in the indexSet[2] { for (unsigned int i = 0; i < nsLawSize; ++i) { if ((*y)(i) > _TOL_ED) { gOut[k] = (*y)(i); } else { if ((*ydot)(i) > -_TOL_ED) { gOut[k] = 100 * _TOL_ED; } else { gOut[k] = (*y)(i); } } k++; } } else // If Interaction is in the indexSet[2] { for (unsigned int i = 0; i < nsLawSize; ++i) { if ((*lambda)(i) > _TOL_ED) { gOut[k] = (*lambda)(i); // g = lambda[2] } else { if ((*yddot)(i) > _TOL_ED) { gOut[k] = (*lambda)(i); } else { gOut[k] = 100 * _TOL_ED; } } k++; } } } }
void LsodarOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp) { SP::OneStepNSProblems allOSNS = simulationLink->oneStepNSProblems(); SP::InteractionsGraph indexSet = osnsp->simulation()->indexSet(osnsp->indexSetLevel()); SP::Interaction inter = indexSet->bundle(vertex_inter); VectorOfBlockVectors& DSlink = *indexSet->properties(vertex_inter).DSlink; // Get relation and non smooth law types RELATION::TYPES relationType = inter->relation()->getType(); RELATION::SUBTYPES relationSubType = inter->relation()->getSubType(); unsigned int sizeY = inter->nonSmoothLaw()->size(); unsigned int relativePosition = 0; SP::Interaction mainInteraction = inter; Index coord(8); coord[0] = relativePosition; coord[1] = relativePosition + sizeY; coord[2] = 0; coord[4] = 0; coord[6] = 0; coord[7] = sizeY; SP::SiconosMatrix C; // SP::SiconosMatrix D; // SP::SiconosMatrix F; SiconosVector& yForNSsolver = *inter->yForNSsolver(); SP::BlockVector Xfree; // All of these values should be stored in the node corrseponding to the Interactionwhen a MoreauJeanOSI scheme is used. /* V.A. 10/10/2010 * Following the type of OSNS we need to retrieve the velocity or the acceleration * This tricks is not very nice but for the moment the OSNS do not known if * it is in accelaration of not */ //SP::OneStepNSProblems allOSNS = _simulation->oneStepNSProblems(); if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp) { if (relationType == Lagrangian) { Xfree = DSlink[LagrangianR::xfree]; } // else if (relationType == NewtonEuler) // { // Xfree = inter->data(NewtonEulerR::free); // } assert(Xfree); // std::cout << "Computeqblock Xfree (Gamma)========" << std::endl; // Xfree->display(); } else if (((*allOSNS)[SICONOS_OSNSP_ED_IMPACT]).get() == osnsp) { Xfree = DSlink[LagrangianR::q1]; // std::cout << "Computeqblock Xfree (Velocity)========" << std::endl; // Xfree->display(); } else RuntimeException::selfThrow(" computeqBlock for Event Event-driven is wrong "); if (relationType == Lagrangian) { C = mainInteraction->relation()->C(); if (C) { assert(Xfree); coord[3] = C->size(1); coord[5] = C->size(1); subprod(*C, *Xfree, yForNSsolver, coord, true); } SP::SiconosMatrix ID(new SimpleMatrix(sizeY, sizeY)); ID->eye(); Index xcoord(8); xcoord[0] = 0; xcoord[1] = sizeY; xcoord[2] = 0; xcoord[3] = sizeY; xcoord[4] = 0; xcoord[5] = sizeY; xcoord[6] = 0; xcoord[7] = sizeY; // For the relation of type LagrangianRheonomousR if (relationSubType == RheonomousR) { if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp) { RuntimeException::selfThrow("LsodarOSI::computeFreeOutput not yet implemented for LCP at acceleration level with LagrangianRheonomousR"); } else if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY]).get() == osnsp) { SiconosVector q = *DSlink[LagrangianR::q0]; SiconosVector z = *DSlink[LagrangianR::z]; std11::static_pointer_cast<LagrangianRheonomousR>(inter->relation())->computehDot(simulation()->getTkp1(), q, z); *DSlink[LagrangianR::z] = z; subprod(*ID, *(std11::static_pointer_cast<LagrangianRheonomousR>(inter->relation())->hDot()), yForNSsolver, xcoord, false); // y += hDot } else RuntimeException::selfThrow("LsodarOSI::computeFreeOutput not implemented for SICONOS_OSNSP "); } // For the relation of type LagrangianScleronomousR if (relationSubType == ScleronomousR) { if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp) { std11::static_pointer_cast<LagrangianScleronomousR>(inter->relation())->computedotjacqhXqdot(simulation()->getTkp1(), *inter, DSlink); subprod(*ID, *(std11::static_pointer_cast<LagrangianScleronomousR>(inter->relation())->dotjacqhXqdot()), yForNSsolver, xcoord, false); // y += NonLinearPart } } } else RuntimeException::selfThrow("LsodarOSI::computeFreeOutput not yet implemented for Relation of type " + relationType); if (((*allOSNS)[SICONOS_OSNSP_ED_IMPACT]).get() == osnsp) { if (inter->relation()->getType() == Lagrangian || inter->relation()->getType() == NewtonEuler) { SP::SiconosVisitor nslEffectOnFreeOutput(new _NSLEffectOnFreeOutput(osnsp, inter)); inter->nonSmoothLaw()->accept(*nslEffectOnFreeOutput); } } }
void LinearOSNS::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd) { DEBUG_BEGIN("LinearOSNS::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)\n"); // Computes matrix _interactionBlocks[inter1][inter1] (and allocates memory if // necessary) one or two DS are concerned by inter1 . How // _interactionBlocks are computed depends explicitely on the type of // Relation of each Interaction. // Warning: we suppose that at this point, all non linear // operators (G for lagrangian relation for example) have been // computed through plug-in mechanism. // Get dimension of the NonSmoothLaw (ie dim of the interactionBlock) SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel()); SP::Interaction inter = indexSet->bundle(vd); // Get osi property from interaction // We assume that all ds in vertex_inter have the same osi. SP::OneStepIntegrator Osi = indexSet->properties(vd).osi; //SP::OneStepIntegrator Osi = simulation()->integratorOfDS(ds); OSI::TYPES osiType = Osi->getType(); // At most 2 DS are linked by an Interaction SP::DynamicalSystem DS1; SP::DynamicalSystem DS2; unsigned int pos1, pos2; // --- Get the dynamical system(s) (edge(s)) connected to the current interaction (vertex) --- if (indexSet->properties(vd).source != indexSet->properties(vd).target) { DEBUG_PRINT("a two DS Interaction\n"); DS1 = indexSet->properties(vd).source; DS2 = indexSet->properties(vd).target; } else { DEBUG_PRINT("a single DS Interaction\n"); DS1 = indexSet->properties(vd).source; DS2 = DS1; // \warning this looks like some debug code, but it gets executed even with NDEBUG. // may be compiler does something smarter, but still it should be rewritten. --xhub InteractionsGraph::OEIterator oei, oeiend; for (std11::tie(oei, oeiend) = indexSet->out_edges(vd); oei != oeiend; ++oei) { // note : at most 4 edges DS2 = indexSet->bundle(*oei); if (DS2 != DS1) { assert(false); break; } } } assert(DS1); assert(DS2); pos1 = indexSet->properties(vd).source_pos; pos2 = indexSet->properties(vd).target_pos; // --- Check block size --- assert(indexSet->properties(vd).block->size(0) == inter->nonSmoothLaw()->size()); assert(indexSet->properties(vd).block->size(1) == inter->nonSmoothLaw()->size()); // --- Compute diagonal block --- // Block to be set in OSNS Matrix, corresponding to // the current interaction SP::SiconosMatrix currentInteractionBlock = indexSet->properties(vd).block; SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock; RELATION::TYPES relationType; double h = simulation()->currentTimeStep(); // General form of the interactionBlock is : interactionBlock = // a*extraInteractionBlock + b * leftInteractionBlock * centralInteractionBlocks // * rightInteractionBlock a and b are scalars, centralInteractionBlocks a // matrix depending on the integrator (and on the DS), the // simulation type ... left, right and extra depend on the relation // type and the non smooth law. relationType = inter->relation()->getType(); VectorOfSMatrices& workMInter = *indexSet->properties(vd).workMatrices; inter->getExtraInteractionBlock(currentInteractionBlock, workMInter); unsigned int nslawSize = inter->nonSmoothLaw()->size(); // loop over the DS connected to the interaction. bool endl = false; unsigned int pos = pos1; for (SP::DynamicalSystem ds = DS1; !endl; ds = DS2) { assert(ds == DS1 || ds == DS2); endl = (ds == DS2); unsigned int sizeDS = ds->dimension(); // get _interactionBlocks corresponding to the current DS // These _interactionBlocks depends on the relation type. leftInteractionBlock.reset(new SimpleMatrix(nslawSize, sizeDS)); inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock, workMInter); DEBUG_EXPR(leftInteractionBlock->display();); // Computing depends on relation type -> move this in Interaction method? if (relationType == FirstOrder) { rightInteractionBlock.reset(new SimpleMatrix(sizeDS, nslawSize)); inter->getRightInteractionBlockForDS(pos, rightInteractionBlock, workMInter); if (osiType == OSI::EULERMOREAUOSI) { if ((std11::static_pointer_cast<EulerMoreauOSI> (Osi))->useGamma() || (std11::static_pointer_cast<EulerMoreauOSI> (Osi))->useGammaForRelation()) { *rightInteractionBlock *= (std11::static_pointer_cast<EulerMoreauOSI> (Osi))->gamma(); } } // for ZOH, we have a different formula ... if (osiType == OSI::ZOHOSI && indexSet->properties(vd).forControl) { *rightInteractionBlock = std11::static_pointer_cast<ZeroOrderHoldOSI>(Osi)->Bd(ds); prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false); } else { // centralInteractionBlock contains a lu-factorized matrix and we solve // centralInteractionBlock * X = rightInteractionBlock with PLU SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds); centralInteractionBlock->PLUForwardBackwardInPlace(*rightInteractionBlock); inter->computeKhat(*rightInteractionBlock, workMInter, h); // if K is non 0 // integration of r with theta method removed // *currentInteractionBlock += h *Theta[*itDS]* *leftInteractionBlock * (*rightInteractionBlock); //left = C, right = W.B //gemm(h,*leftInteractionBlock,*rightInteractionBlock,1.0,*currentInteractionBlock); *leftInteractionBlock *= h; prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false); //left = C, right = inv(W).B } } else if (relationType == Lagrangian || relationType == NewtonEuler) { SP::BoundaryCondition bc; Type::Siconos dsType = Type::value(*ds); if (dsType == Type::LagrangianLinearTIDS || dsType == Type::LagrangianDS) { SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); if (d->boundaryConditions()) bc = d->boundaryConditions(); } else if (dsType == Type::NewtonEulerDS) { SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); if (d->boundaryConditions()) bc = d->boundaryConditions(); } if (bc) { for (std::vector<unsigned int>::iterator itindex = bc->velocityIndices()->begin() ; itindex != bc->velocityIndices()->end(); ++itindex) { // (nslawSize,sizeDS)); SP::SiconosVector coltmp(new SiconosVector(nslawSize)); coltmp->zero(); leftInteractionBlock->setCol(*itindex, *coltmp); } } DEBUG_PRINT("leftInteractionBlock after application of boundary conditions\n"); DEBUG_EXPR(leftInteractionBlock->display(););
std::pair<DynamicalSystemsGraph::EDescriptor, InteractionsGraph::VDescriptor> Topology::addInteractionInIndexSet0(SP::Interaction inter, SP::DynamicalSystem ds1, SP::DynamicalSystem ds2) { // !! Private function !! // // Add inter and ds into IG/DSG // Compute number of constraints unsigned int nsLawSize = inter->nonSmoothLaw()->size(); unsigned int m = inter->getSizeOfY() / nsLawSize; if (m > 1) RuntimeException::selfThrow("Topology::addInteractionInIndexSet0 - m > 1. Obsolete !"); _numberOfConstraints += nsLawSize; SP::DynamicalSystem ds2_ = ds2; // _DSG is the hyper forest : (vertices : dynamical systems, edges : // Interactions) // // _IG is the hyper graph : (vertices : Interactions, edges : // dynamical systems) assert(_DSG[0]->edges_number() == _IG[0]->size()); // _IG = L(_DSG), L is the line graph transformation // vector of the Interaction DynamicalSystemsGraph::VDescriptor dsgv1, dsgv2; dsgv1 = _DSG[0]->add_vertex(ds1); SP::VectorOfVectors workVds1 = _DSG[0]->properties(dsgv1).workVectors; SP::VectorOfVectors workVds2; if (!workVds1) { workVds1.reset(new VectorOfVectors()); _DSG[0]->properties(dsgv1).workMatrices.reset(new VectorOfMatrices()); ds1->initWorkSpace(*workVds1, *_DSG[0]->properties(dsgv1).workMatrices); } if(ds2) { dsgv2 = _DSG[0]->add_vertex(ds2); workVds2 = _DSG[0]->properties(dsgv2).workVectors; if (!workVds2) { workVds2.reset(new VectorOfVectors()); _DSG[0]->properties(dsgv2).workMatrices.reset(new VectorOfMatrices()); ds2->initWorkSpace(*workVds2, *_DSG[0]->properties(dsgv2).workMatrices); } } else { dsgv2 = dsgv1; ds2_ = ds1; workVds2 = workVds1; } // this may be a multi edges graph assert(!_DSG[0]->is_edge(dsgv1, dsgv2, inter)); assert(!_IG[0]->is_vertex(inter)); InteractionsGraph::VDescriptor ig_new_ve; DynamicalSystemsGraph::EDescriptor new_ed; std11::tie(new_ed, ig_new_ve) = _DSG[0]->add_edge(dsgv1, dsgv2, inter, *_IG[0]); InteractionProperties& interProp = _IG[0]->properties(ig_new_ve); interProp.DSlink.reset(new VectorOfBlockVectors); interProp.workVectors.reset(new VectorOfVectors); interProp.workMatrices.reset(new VectorOfSMatrices); unsigned int nslawSize = inter->nonSmoothLaw()->size(); interProp.block.reset(new SimpleMatrix(nslawSize, nslawSize)); inter->setDSLinkAndWorkspace(interProp, *ds1, *workVds1, *ds2_, *workVds2); // add self branches in vertex properties // note : boost graph SEGFAULT on self branch removal // see https://svn.boost.org/trac/boost/ticket/4622 _IG[0]->properties(ig_new_ve).source = ds1; _IG[0]->properties(ig_new_ve).source_pos = 0; if(!ds2) { _IG[0]->properties(ig_new_ve).target = ds1; _IG[0]->properties(ig_new_ve).target_pos = 0; } else { _IG[0]->properties(ig_new_ve).target = ds2; _IG[0]->properties(ig_new_ve).target_pos = ds1->getDim(); } assert(_IG[0]->bundle(ig_new_ve) == inter); assert(_IG[0]->is_vertex(inter)); assert(_DSG[0]->is_edge(dsgv1, dsgv2, inter)); assert(_DSG[0]->edges_number() == _IG[0]->size()); return std::pair<DynamicalSystemsGraph::EDescriptor, InteractionsGraph::VDescriptor>(new_ed, ig_new_ve); }
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ double EventDriven::detectEvents(bool updateIstate) { double _minResiduOutput = 0.0; // maximum of g_i with i running over all activated or deactivated contacts // Loop over all interactions to detect whether some constraints are activated or deactivated bool _IsContactClosed = false; bool _IsContactOpened = false; bool _IsFirstTime = true; InteractionsGraph::VIterator ui, uiend; SP::SiconosVector y, ydot, lambda; SP::Topology topo = _nsds->topology(); SP::InteractionsGraph indexSet2 = topo->indexSet(2); // #ifdef DEBUG_MESSAGES cout << "======== In EventDriven::detectEvents =========" <<endl; #endif for (std11::tie(ui, uiend) = _indexSet0->vertices(); ui != uiend; ++ui) { SP::Interaction inter = _indexSet0->bundle(*ui); double nsLawSize = inter->nonSmoothLaw()->size(); if (nsLawSize != 1) { RuntimeException::selfThrow("In EventDriven::detectEvents, the interaction size > 1 has not been implemented yet!!!"); } y = inter->y(0); // output y at this Interaction ydot = inter->y(1); // output of level 1 at this Interaction lambda = inter->lambda(2); // input of level 2 at this Interaction if (!(indexSet2->is_vertex(inter))) // if Interaction is not in the indexSet[2] { if ((*y)(0) < _TOL_ED) // gap at the current interaction <= 0 { _IsContactClosed = true; } if (_IsFirstTime) { _minResiduOutput = (*y)(0); _IsFirstTime = false; } else { if (_minResiduOutput > (*y)(0)) { _minResiduOutput = (*y)(0); } } } else // If interaction is in the indexSet[2] { if ((*lambda)(0) < _TOL_ED) // normal force at the current interaction <= 0 { _IsContactOpened = true; } if (_IsFirstTime) { _minResiduOutput = (*lambda)(0); _IsFirstTime = false; } else { if (_minResiduOutput > (*lambda)(0)) { _minResiduOutput = (*lambda)(0); } } } // #ifdef DEBUG_MESSAGES cout.precision(15); cout << "Contact number: " << inter->number() <<endl; cout << "Contact gap: " << (*y)(0) <<endl; cout << "Contact force: " << (*lambda)(0) <<endl; cout << "Is contact is closed: " << _IsContactClosed <<endl; cout << "Is contact is opened: " << _IsContactOpened <<endl; #endif // } // if (updateIstate) { if ((!_IsContactClosed) && (!_IsContactOpened)) { _istate = 2; //no event is detected } else if ((_IsContactClosed) && (!_IsContactOpened)) { _istate = 3; // Only some contacts are closed } else if ((!_IsContactClosed) && (_IsContactOpened)) { _istate = 4; // Only some contacts are opened } else { _istate = 5; // Some contacts are closed AND some contacts are opened } } // return _minResiduOutput; }
// Fill the SparseMat void BlockCSRMatrix::fill(SP::InteractionsGraph indexSet) { // ======> Aim: find inter1 and inter2 both in indexSets[level] and which // have common DynamicalSystems. Then get the corresponding matrix // from map blocks. assert(indexSet); // Number of blocks in a row = number of active constraints. _nr = indexSet->size(); // (re)allocate memory for ublas matrix _blockCSR->resize(_nr, _nr, false); _diagsize0->resize(_nr); _diagsize1->resize(_nr); // === Loop through "active" Interactions (ie present in // indexSets[level]) === int sizeV = 0; InteractionsGraph::VIterator vi, viend; for (std11::tie(vi, viend) = indexSet->vertices(); vi != viend; ++vi) { SP::Interaction inter = indexSet->bundle(*vi); assert(inter->nonSmoothLaw()->size() > 0); sizeV += inter->nonSmoothLaw()->size(); (*_diagsize0)[indexSet->index(*vi)] = sizeV; (*_diagsize1)[indexSet->index(*vi)] = sizeV; assert((*_diagsize0)[indexSet->index(*vi)] > 0); assert((*_diagsize1)[indexSet->index(*vi)] > 0); (*_blockCSR)(indexSet->index(*vi), indexSet->index(*vi)) = indexSet->properties(*vi).block->getArray(); } InteractionsGraph::EIterator ei, eiend; for (std11::tie(ei, eiend) = indexSet->edges(); ei != eiend; ++ei) { InteractionsGraph::VDescriptor vd1 = indexSet->source(*ei); InteractionsGraph::VDescriptor vd2 = indexSet->target(*ei); SP::Interaction inter1 = indexSet->bundle(vd1); SP::Interaction inter2 = indexSet->bundle(vd2); assert(indexSet->index(vd1) < _nr); assert(indexSet->index(vd2) < _nr); assert(indexSet->is_vertex(inter2)); assert(vd2 == indexSet->descriptor(inter2)); assert(indexSet->index(vd2) == indexSet->index(indexSet->descriptor(inter2))); unsigned int pos = indexSet->index(vd1); unsigned int col = indexSet->index(vd2); assert(pos != col); (*_blockCSR)(std::min(pos, col), std::max(pos, col)) = indexSet->properties(*ei).upper_block->getArray(); (*_blockCSR)(std::max(pos, col), std::min(pos, col)) = indexSet->properties(*ei).lower_block->getArray(); } DEBUG_EXPR(display(););
int main(int argc, char* argv[]) { try { // ================= Creation of the model ======================= // User-defined main parameters unsigned int nDof = 3; // degrees of freedom for the ball double t0 = 0; // initial computation time double T = 2.0; // final computation time double h = 0.0005; // time step double position_init = 1.0; // initial position for lowest bead. double velocity_init = 0.0; // initial velocity for lowest bead. double theta = 0.5; // theta for MoreauJeanOSI integrator double R = 0.1; // Ball radius double m = 1; // Ball mass double g = 9.81; // Gravity // ------------------------- // --- Dynamical systems --- // ------------------------- cout << "====> Model loading ..." << endl << endl; // Number of Beads unsigned int nBeads = 10; double initialGap = 0.25; double alert = 0.02; SP::SiconosMatrix Mass(new SimpleMatrix(nDof, nDof)); (*Mass)(0, 0) = m; (*Mass)(1, 1) = m; (*Mass)(2, 2) = 3. / 5 * m * R * R; // -- Initial positions and velocities -- std::vector<SP::SiconosVector> q0(nBeads); std::vector<SP::SiconosVector> v0(nBeads); for (unsigned int i = 0; i < nBeads; i++) { (q0[i]).reset(new SiconosVector(nDof)); (v0[i]).reset(new SiconosVector(nDof)); (q0[i])->setValue(0, position_init + i * initialGap); (v0[i])->setValue(0, velocity_init); } // -- The dynamical system -- SP::SiconosVector weight(new SiconosVector(nDof)); (*weight)(0) = -m * g; std::vector<SP::LagrangianLinearTIDS> beads(nBeads); for (unsigned int i = 0; i < nBeads; i++) { beads[i].reset(new LagrangianLinearTIDS(q0[i], v0[i], Mass)); // -- Set external forces (weight) -- beads[i]->setFExtPtr(weight); } // -------------------- // --- Interactions --- // -------------------- // -- nslaw -- double e = 0.9; // Interaction ball-floor // SP::SimpleMatrix H(new SimpleMatrix(1, nDof)); (*H)(0, 0) = 1.0; SP::SiconosVector b(new SiconosVector(1)); (*b)(0) = -R; SP::NonSmoothLaw nslaw(new NewtonImpactNSL(e)); SP::Relation relation(new LagrangianLinearTIR(H, b)); SP::Interaction inter; // beads/beads interactions SP::SimpleMatrix HOfBeads(new SimpleMatrix(1, 2 * nDof)); (*HOfBeads)(0, 0) = -1.0; (*HOfBeads)(0, 3) = 1.0; SP::SiconosVector bOfBeads(new SiconosVector(1)); (*bOfBeads)(0) = -2 * R; // This doesn't work !!! //SP::Relation relationOfBeads(new LagrangianLinearTIR(HOfBeads)); //std::vector<SP::Interaction > interOfBeads(nBeads-1); // for (unsigned int i =0; i< nBeads-1; i++) // { // interOfBeads[i].reset(new Interaction(1, nslaw, relationOfBeads)); // } // This works !! std::vector<SP::Relation > relationOfBeads(nBeads - 1); std::vector<SP::Interaction > interOfBeads(nBeads - 1); // for (unsigned int i =0; i< nBeads-1; i++) // { // relationOfBeads[i].reset(new LagrangianLinearTIR(HOfBeads,bOfBeads)); // interOfBeads[i].reset(new Interaction(1, nslaw, relationOfBeads[i])); // } // -------------------------------------- // --- Model and simulation --- // -------------------------------------- SP::Model columnOfBeads(new Model(t0, T)); // -- (1) OneStepIntegrators -- SP::MoreauJeanOSI OSI(new MoreauJeanOSI(theta)); // add the dynamical system in the non smooth dynamical system for (unsigned int i = 0; i < nBeads; i++) { columnOfBeads->nonSmoothDynamicalSystem()->insertDynamicalSystem(beads[i]); } // // link the interaction and the dynamical system // for (unsigned int i =0; i< nBeads-1; i++) // { // columnOfBeads->nonSmoothDynamicalSystem()->link(interOfBeads[i],beads[i]); // columnOfBeads->nonSmoothDynamicalSystem()->link(interOfBeads[i],beads[i+1]); // } // -- (2) Time discretisation -- SP::TimeDiscretisation t(new TimeDiscretisation(t0, h)); // -- (3) one step non smooth problem SP::OneStepNSProblem osnspb(new LCP()); // -- (4) Simulation setup with (1) (2) (3) SP::TimeStepping s(new TimeStepping(t, OSI, osnspb)); columnOfBeads->setSimulation(s); // =========================== End of model definition =========================== // ================================= Computation ================================= // --- Simulation initialization --- cout << "====> Initialisation ..." << endl << endl; columnOfBeads->initialize(); int N = ceil((T - t0) / h); // Number of time steps // --- Get the values to be plotted --- // -> saved in a matrix dataPlot unsigned int outputSize = 1 + nBeads * 4; SimpleMatrix dataPlot(N + 1, outputSize); dataPlot(0, 0) = columnOfBeads->t0(); for (unsigned int i = 0; i < nBeads; i++) { dataPlot(0, 1 + i * 2) = (beads[i]->q())->getValue(0); dataPlot(0, 2 + i * 2) = (beads[i]->velocity())->getValue(0); // dataPlot(0,3+i*4) = (beads[i]->p(1))->getValue(0); } // for (unsigned int i =1; i< nBeads; i++) // { // dataPlot(0,4+i*4) = (interOfBeads[i-1]->lambda(1))->getValue(0); // } // --- Time loop --- cout << "====> Start computation ... " << endl << endl; // ==== Simulation loop - Writing without explicit event handling ===== int k = 1; boost::progress_display show_progress(N); boost::timer time; time.restart(); int ncontact = 0 ; bool isOSNSinitialized = false; while (s->hasNextEvent()) { // Rough contact detection for (unsigned int i = 0; i < nBeads - 1; i++) { // Between first bead and plane if (abs(((beads[i])->q())->getValue(0) - R) < alert) { if (!inter) { ncontact++; // std::cout << "Number of contact = " << ncontact << std::endl; inter.reset(new Interaction(1, nslaw, relation)); columnOfBeads->nonSmoothDynamicalSystem()->link(inter, beads[0]); s->initializeInteraction(s->nextTime(), inter); if (!isOSNSinitialized) { s->initOSNS(); isOSNSinitialized = true; } assert(inter->y(0)->getValue(0) >= 0); } } // Between two beads if (abs(((beads[i + 1])->q())->getValue(0) - ((beads[i])->q())->getValue(0) - 2 * R) < alert) { //std::cout << "Alert distance for declaring contact = "; //std::cout << abs(((beads[i])->q())->getValue(0)-((beads[i+1])->q())->getValue(0)) <<std::endl; if (!interOfBeads[i].get()) { ncontact++; // std::cout << "Number of contact = " << ncontact << std::endl; relationOfBeads[i].reset(new LagrangianLinearTIR(HOfBeads, bOfBeads)); interOfBeads[i].reset(new Interaction(1, nslaw, relationOfBeads[i])); columnOfBeads->nonSmoothDynamicalSystem()->link(interOfBeads[i], beads[i], beads[i+1]); s->initializeInteraction(s->nextTime(), interOfBeads[i]); if (!isOSNSinitialized) { s->initOSNS(); isOSNSinitialized = true; } assert(interOfBeads[i]->y(0)->getValue(0) >= 0); } } } s->computeOneStep(); // --- Get values to be plotted --- dataPlot(k, 0) = s->nextTime(); for (unsigned int i = 0; i < nBeads; i++) { dataPlot(k, 1 + i * 2) = (beads[i]->q())->getValue(0); dataPlot(k, 2 + i * 2) = (beads[i]->velocity())->getValue(0); } // for (unsigned int i =1; i< nBeads; i++) // { // dataPlot(k,4+i*4) = (interOfBeads[i-1]->lambda(1))->getValue(0); // } // for (unsigned int i =1; i< nBeads; i++) // { // std::cout << (interOfBeads[i-1]->y(0))->getValue(0) << std::endl ; // } s->nextStep(); ++show_progress; k++; } cout << endl << "End of computation - Number of iterations done: " << k - 1 << endl; cout << "Computation Time " << time.elapsed() << endl; // --- Output files --- cout << "====> Output file writing ..." << endl; dataPlot.resize(k, outputSize); ioMatrix::write("result.dat", "ascii", dataPlot, "noDim"); // Comparison with a reference file SimpleMatrix dataPlotRef(dataPlot); dataPlotRef.zero(); ioMatrix::read("result.ref", "ascii", dataPlotRef); cout << "====> Comparison with reference file ..." << endl; std::cout << "Error w.r.t. reference file : " << (dataPlot - dataPlotRef).normInf() << std::endl; if ((dataPlot - dataPlotRef).normInf() > 1e-12) { std::cout << "Warning. The result is rather different from the reference file." << std::endl; return 1; } } catch (SiconosException e) { cout << e.report() << endl; } catch (...) { cout << "Exception caught in ColumnOfBeadsTS.cpp" << endl; } }