Node::SPtr createCard(Node::SPtr parent, const Coord3& position, const Coord3& rotation)
{
const std::string visualModel="mesh/card.obj";
const std::string collisionModel="mesh/cardCollision.obj";
const std::string inertiaMatrix="BehaviorModels/card.rigid";
static int colorIdx=0;
std::vector<std::string> modelTypes;
modelTypes.push_back("Triangle");
modelTypes.push_back("Line");
modelTypes.push_back("Point");
Node::SPtr card = sofa::modeling::createEulerSolverNode(parent,"Rigid","Implicit");
sofa::component::odesolver::EulerImplicitSolver *odeSolver; card->get(odeSolver);
odeSolver->f_rayleighStiffness.setValue(0.1);
odeSolver->f_rayleighMass.setValue(0.1);
CGLinearSolverGraph *cgLinearSolver; card->get(cgLinearSolver);
cgLinearSolver->f_maxIter.setValue(15);
cgLinearSolver->f_tolerance.setValue(1e-5);
cgLinearSolver->f_smallDenominatorThreshold.setValue(1e-5);
sofa::component::constraintset::LMConstraintSolver::SPtr constraintSolver = sofa::core::objectmodel::New<sofa::component::constraintset::LMConstraintSolver>();
constraintSolver->constraintVel.setValue(true);
constraintSolver->constraintPos.setValue(true);
constraintSolver->numIterations.setValue(25);
card->addObject(constraintSolver);
MechanicalObjectRigid3::SPtr dofRigid = sofa::core::objectmodel::New<MechanicalObjectRigid3>(); dofRigid->setName("Rigid Object");
dofRigid->setTranslation(position[0],position[1],position[2]);
dofRigid->setRotation(rotation[0],rotation[1],rotation[2]);
card->addObject(dofRigid);
UniformMassRigid3::SPtr uniMassRigid = sofa::core::objectmodel::New<UniformMassRigid3>();
uniMassRigid->setTotalMass(0.5);
uniMassRigid->setFileMass(inertiaMatrix);
card->addObject(uniMassRigid);
//Node VISUAL
Node::SPtr RigidVisualNode = sofa::modeling::createVisualNodeRigid(card, dofRigid.get(), visualModel,colors[(colorIdx++)%7]);
//Node COLLISION
Node::SPtr RigidCollisionNode = sofa::modeling::createCollisionNodeRigid(card, dofRigid.get(),collisionModel,modelTypes);
return card;
}
void checkAttributes()
{
std::stringstream scene ;
scene << "<?xml version='1.0'?>"
"<Node name='Root' gravity='0 -9.81 0' time='0' animate='0' > \n"
" <OglLabel name='label1'/> \n"
"</Node> \n" ;
Node::SPtr root = SceneLoaderXML::loadFromMemory ("testscene",
scene.str().c_str(),
scene.str().size()) ;
ASSERT_NE(root.get(), nullptr) ;
root->init(ExecParams::defaultInstance()) ;
BaseObject* lm = root->getObject("label1") ;
ASSERT_NE(lm, nullptr) ;
/// List of the supported attributes the user expect to find
/// This list needs to be updated if you add an attribute.
vector<string> attrnames = {
"prefix", "label", "suffix", "x", "y", "fontsize", "color",
"selectContrastingColor", "updateLabelEveryNbSteps",
"visible"};
for(auto& attrname : attrnames)
EXPECT_NE( lm->findData(attrname), nullptr ) << "Missing attribute with name '" << attrname << "'." ;
}
void checkDeprecatedAttribute()
{
EXPECT_MSG_EMIT(Deprecated) ;
std::stringstream scene ;
scene << "<?xml version='1.0'?>"
"<Node name='Root' gravity='0 -9.81 0' time='0' animate='0' > \n"
" <OglLabel name='label1' color='contrast' printLog='true'/> \n"
"</Node> \n" ;
Node::SPtr root = SceneLoaderXML::loadFromMemory ("testscene",
scene.str().c_str(),
scene.str().size()) ;
ASSERT_NE(nullptr, root.get()) ;
root->init(ExecParams::defaultInstance()) ;
BaseObject* lm = root->getObject("label1") ;
ASSERT_NE(nullptr, lm) ;
OglLabel* ogllabel = dynamic_cast<OglLabel*>(lm);
ASSERT_NE(nullptr, ogllabel) ;
EXPECT_TRUE(ogllabel->d_selectContrastingColor.getValue()) ;
EXPECT_EQ(RGBAColor::fromFloat(1,1,1,1), ogllabel->d_color.getValue()) ;
}
void SofaModeler::fileOpen(std::string filename)
{
if ( sofa::helper::system::DataRepository.findFile ( filename ) )
{
filename = sofa::helper::system::DataRepository.getFile ( filename );
openPath = sofa::helper::system::SetDirectory::GetParentDir(filename.c_str());
Node::SPtr root = NULL;
root = down_cast<sofa::simulation::Node>( sofa::simulation::getSimulation()->load(filename.c_str()).get() );
if (root)
{
createTab();
fileNew(root.get());
sceneTab->setCurrentIndex(sceneTab->count()-1);
graph->setFilename(filename);
changeTabName(graph,QString(sofa::helper::system::SetDirectory::GetFileName(filename.c_str()).c_str()));
changeNameWindow(graph->getFilename());
QAction* act = windowMenu->addAction(graph->getFilename().c_str());
mapWindow.insert(std::make_pair(act, tabGraph));
recentlyOpenedFilesManager.openFile(filename);
}
}
displayHelpModeler();
}
BaseObject::SPtr createObject(Node::SPtr parent, BaseObjectDescription& desc)
{
/// Create the object.
BaseObject::SPtr obj = ObjectFactory::getInstance()->createObject(parent.get(), &desc);
if (obj==nullptr)
{
std::stringstream msg;
msg << "Component '" << desc.getName() << "' of type '" << desc.getAttribute("type","") << "' failed:" << msgendl ;
for (std::vector< std::string >::const_iterator it = desc.getErrors().begin(); it != desc.getErrors().end(); ++it)
msg << " " << *it << msgendl ;
msg_error(parent.get()) << msg.str() ;
return nullptr;
}
return obj ;
}
Node::SPtr GraphModeler::buildNodeFromBaseElement(Node::SPtr node,xml::BaseElement *elem, bool saveHistory)
{
const bool displayWarning=true;
Node::SPtr newNode = Node::create("");
//Configure the new Node
configureElement(newNode.get(), elem);
if (newNode->getName() == "Group")
{
//We can't use the parent node, as it is null
if (!node) return NULL;
//delete newNode;
newNode = node;
}
else
{
// if (node)
{
//Add as a child
addNode(node,newNode,saveHistory);
}
}
typedef xml::BaseElement::child_iterator<> elem_iterator;
for (elem_iterator it=elem->begin(); it != elem->end(); ++it)
{
if (std::string(it->getClass()) == std::string("Node"))
{
buildNodeFromBaseElement(newNode, it,true);
}
else
{
const ComponentLibrary *component = sofaLibrary->getComponent(it->getType());
//Configure the new Component
const std::string templateAttribute("template");
std::string templatename;
templatename = it->getAttribute(templateAttribute, "");
const ClassEntry::SPtr info = component->getEntry();
BaseObject::SPtr newComponent=addComponent(newNode, info, templatename, saveHistory,displayWarning);
if (!newComponent) continue;
configureElement(newComponent.get(), it);
QTreeWidgetItem* itemGraph = graphListener->items[newComponent.get()];
std::string name=itemGraph->text(0).toStdString();
std::string::size_type pos = name.find(' ');
if (pos != std::string::npos) name.resize(pos);
name += " ";
name+=newComponent->getName();
itemGraph->setText(0,name.c_str());
}
}
newNode->clearWarnings();
return newNode;
}
/// utility function testing a scene graph traversals with given expected results
void traverse_test( Node::SPtr node, std::string treeTraverse, std::string treeTraverseRepeatAll, std::string treeTraverseRepeatOnce, std::string dagTopDown )
{
// dagBottumUp must be the exact inverse of dagTopDown
std::string dagBottomUp(dagTopDown);
std::reverse(dagBottomUp.begin(), dagBottomUp.end());
TestVisitor t;
t.tree = true; // visitor as TREE traversal w/o repetition
t.execute( node.get() );
// cout<<"traverse_simple_tree: visited = " << t.visited << endl;
if( t.visited != treeTraverse ){
ADD_FAILURE() << "Dag_test::traverse_test treeTraverse: wrong traversal order, expected "<<treeTraverse<<", got " << t.visited;
}
t.clear();
t.repeat = simulation::Visitor::REPEAT_ALL; // visitor as TREE traversal with repetitions
t.execute( node.get() );
// cout<<"traverse_simple_tree: visited = " << t.visited << endl;
if( t.visited != treeTraverseRepeatAll ){
ADD_FAILURE() << "Dag_test::traverse_test treeTraverseRepeatAll: wrong traversal order, expected "<<treeTraverseRepeatAll<<", got " << t.visited;
}
t.clear();
t.repeat = simulation::Visitor::REPEAT_ONCE; // visitor as TREE traversal with first repetition
t.execute( node.get() );
// cout<<"traverse_simple_tree: visited = " << t.visited << endl;
if( t.visited != treeTraverseRepeatOnce ){
ADD_FAILURE() << "Dag_test::traverse_test treeTraverseRepeatOnce: wrong traversal order, expected "<<treeTraverseRepeatOnce<<", got " << t.visited;
}
t.clear();
t.tree = false; // visitor as DAG traversal
t.execute(node.get());
// cout<<"traverse_test: visited = " << t.visited << endl;
if( t.topdown != dagTopDown ){
ADD_FAILURE() << "Dag_test::traverse_test dagTopDown: wrong traversal order, expected "<<dagTopDown<<", got " << t.topdown;
}
if( t.bottomup != dagBottomUp ){
ADD_FAILURE() << "Dag_test::traverse_test dagBottomUp: wrong traversal order, expected "<<dagBottomUp<<", got " << t.bottomup;
}
// sofa::simulation::getSimulation()->print(node.get());
}
void QSofaListView::RemoveNode()
{
if( object_.type == typeNode)
{
emit Lock(true);
Node* node = object_.ptr.Node;
if ( node == node->getRoot() )
{
//Attempt to destroy the Root node : create an empty node to handle new graph interaction
Node::SPtr root = simulation::getSimulation()->createNewGraph( "Root" );
graphListener_->removeChild ( NULL, node);
graphListener_->addChild ( NULL, root.get() );
emit RootNodeChanged(root.get(),NULL);
}
else
{
node->detachFromGraph();
node->execute<simulation::DeleteVisitor>(sofa::core::ExecParams::defaultInstance());
emit NodeRemoved();
}
emit Lock(false);
}
}
/// Load a scene from a file
Node::SPtr SceneLoaderXML::processXML(xml::BaseElement* xml, const char *filename)
{
loadSucceed = true;
if ( xml==NULL )
{
return NULL;
}
sofa::core::ExecParams* params = sofa::core::ExecParams::defaultInstance();
// We go the the current file's directory so that all relative path are correct
helper::system::SetDirectory chdir ( filename );
// Temporarily set the numeric formatting locale to ensure that
// floating-point values are interpreted correctly by tinyXML. (I.e. the
// decimal separator is a dot '.').
helper::system::TemporaryLocale locale(LC_NUMERIC, "C");
sofa::simulation::xml::NodeElement* nodeElt = dynamic_cast<sofa::simulation::xml::NodeElement *>(xml);
if( nodeElt==NULL )
{
std::cerr << "LOAD ERROR: XML Root Node is not an Element."<<std::endl;
loadSucceed = false;
std::exit(EXIT_FAILURE);
}
else if( !(nodeElt->init()) )
{
std::cerr << "LOAD ERROR: Node initialization failed."<<std::endl;
loadSucceed = false;
}
core::objectmodel::BaseNode* baseroot = xml->getObject()->toBaseNode();
if ( baseroot == NULL )
{
std::cerr << "LOAD ERROR: Objects initialization failed."<<std::endl;
loadSucceed = false;
return NULL;
}
Node::SPtr root = down_cast<Node> ( baseroot );
// Find the Simulation component in the scene
FindByTypeVisitor<Simulation> findSimu(params);
findSimu.execute(root.get());
if( !findSimu.found.empty() )
setSimulation( findSimu.found[0] );
return root;
}
void testNoParameter()
{
EXPECT_MSG_EMIT(Error) ;
std::stringstream scene ;
scene << "<?xml version='1.0'?>"
"<Node name='Root' gravity='0 -9.81 0' time='0' animate='0' > \n"
" <RequiredPlugin /> \n"
"</Node> \n" ;
Node::SPtr root = SceneLoaderXML::loadFromMemory ("testscene",
scene.str().c_str(),
scene.str().size()) ;
ASSERT_NE(root.get(), nullptr) ;
root->init(ExecParams::defaultInstance()) ;
}
void TestDefaultPipeLine::checkDefaultPipelineWithMissingIntersection()
{
EXPECT_MSG_EMIT(Warning) ;
EXPECT_MSG_NOEMIT(Error) ;
std::stringstream scene ;
scene << "<?xml version='1.0'?> \n"
"<Node name='Root' gravity='0 -9.81 0' time='0' animate='0' > \n"
" <DefaultPipeline name='pipeline'/> \n"
"</Node> \n" ;
Node::SPtr root = SceneLoaderXML::loadFromMemory ("testscene",
scene.str().c_str(),
scene.str().size()) ;
ASSERT_NE(root.get(), nullptr) ;
root->init(ExecParams::defaultInstance()) ;
BaseObject* clp = root->getObject("pipeline") ;
ASSERT_NE(clp, nullptr) ;
clearSceneGraph();
}
void dumpScene(Node::SPtr root)
{
XMLPrintVisitor p(sofa::core::ExecParams::defaultInstance(), std::cout) ;
p.execute(root.get()) ;
}
/// One compliant spring, initially extendes
void testLinearOneFixedOneComplianceSpringX200( bool debug )
{
SReal dt=1;
Node::SPtr root = clearScene();
root->setGravity( Vec3(0,0,0) );
root->setDt(dt);
// The solver
typedef odesolver::AssembledSolver OdeSolver;
OdeSolver::SPtr odeSolver = addNew<OdeSolver>(root);
odeSolver->debug.setValue(debug);
odeSolver->alpha.setValue(1.0);
odeSolver->beta.setValue(1.0);
SReal precision = 1.0e-6;
linearsolver::LDLTSolver::SPtr linearSolver = addNew<linearsolver::LDLTSolver>(root);
linearSolver->debug.setValue(debug);
// The string
ParticleString string1( root, Vec3(0,0,0), Vec3(1,0,0), 2, 1.0*2 ); // two particles
string1.compliance->isCompliance.setValue(true);
string1.compliance->compliance.setValue(1.0e-3);
FixedConstraint3::SPtr fixed = modeling::addNew<FixedConstraint3>(string1.string_node,"fixedConstraint");
fixed->addConstraint(0); // attach first particle
{
MechanicalObject3::WriteVecCoord x = string1.DOF->writePositions();
x[1] = Vec3(2,0,0);
}
//**************************************************
sofa::simulation::getSimulation()->init(root.get());
//**************************************************
// initial state
Vector x0 = modeling::getVector( core::VecId::position() );
Vector v0 = modeling::getVector( core::VecId::velocity() );
//**************************************************
sofa::simulation::getSimulation()->animate(root.get(),dt);
//**************************************************
Vector x1 = modeling::getVector( core::VecId::position() );
Vector v1 = modeling::getVector( core::VecId::velocity() );
// We check the explicit step backward without a solver, because it would not accumulate compliance forces
core::MechanicalParams mparams;
mparams.setAccumulateComplianceForces(true);
simulation::common::MechanicalOperations mop (&mparams,getRoot()->getContext());
mop.computeForce( 0+dt, core::VecId::force(), core::VecId::position(), core::VecId::velocity() );
Vector f1 = modeling::getVector( core::VecId::force() );
// backward step
Vector v2 = v1 - f1 * dt;
Vector x2 = x1 - v1 * dt;
// cerr<<"AssembledSolver_test, initial positions : " << x0.transpose() << endl;
// cerr<<"AssembledSolver_test, initial velocities: " << v0.transpose() << endl;
// cerr<<"AssembledSolver_test, new positions : " << x1.transpose() << endl;
// cerr<<"AssembledSolver_test, new velocities : " << v1.transpose() << endl;
// cerr<<"AssembledSolver_test, new forces : " << f1.transpose() << endl;
// cerr<<"AssembledSolver_test, new positions after backward integration: " << x2.transpose() << endl;
// cerr<<"AssembledSolver_test, new velocities after backward integration: " << v2.transpose() << endl;
// check that the implicit integration satisfies the implicit integration equation
ASSERT_TRUE( (x2-x0).lpNorm<Eigen::Infinity>() < precision );
ASSERT_TRUE( (v2-v0).lpNorm<Eigen::Infinity>() < precision );
// The particle is initially in (2,0,0) and the closest rest configuration is (1,0,0)
// The solution should therefore be inbetween.
ASSERT_TRUE( x1(3)>1.0 ); // the spring should not get inversed
}
/// Same as @testLinearOneFixedOneStiffnessSpringV100, with a compliant spring instead of a stiff spring
void testLinearOneFixedOneComplianceSpringV100( bool debug )
{
SReal dt=0.1; // currently, this must be set in the root node AND passed to the animate function
Node::SPtr root = clearScene();
root->setGravity( Vec3(0,0,0) );
root->setDt(dt);
// The solver
using odesolver::AssembledSolver;
AssembledSolver::SPtr complianceSolver = addNew<AssembledSolver>(root);
complianceSolver->debug.setValue( debug );
complianceSolver->alpha.setValue(1.0);
complianceSolver->beta.setValue(1.0);
SReal precision = 1.0e-6;
linearsolver::LDLTSolver::SPtr linearSolver = addNew<linearsolver::LDLTSolver>(root);
linearSolver->debug.setValue(debug);
// The string
ParticleString string1( root, Vec3(0,0,0), Vec3(1,0,0), 2, 1.0*2 ); // two particles
string1.compliance->isCompliance.setValue(true);
string1.compliance->compliance.setValue(1.0e-3);
FixedConstraint3::SPtr fixed = modeling::addNew<FixedConstraint3>(string1.string_node,"fixedConstraint");
fixed->addConstraint(0); // attach first particle
// velocity parallel to the spring
{
MechanicalObject3::WriteVecCoord v = string1.DOF->writeVelocities();
v[1] = Vec3(1,0,0);
}
//**************************************************
sofa::simulation::getSimulation()->init(root.get());
//**************************************************
// initial state
Vector x0 = modeling::getVector( core::VecId::position() );
Vector v0 = modeling::getVector( core::VecId::velocity() );
//**************************************************
sofa::simulation::getSimulation()->animate(root.get(),dt);
//**************************************************
Vector x1 = modeling::getVector( core::VecId::position() );
Vector v1 = modeling::getVector( core::VecId::velocity() );
// We check the explicit step backward without a solver, because it would not accumulate compliance forces
core::MechanicalParams mparams;
mparams.setAccumulateComplianceForces(true);
simulation::common::MechanicalOperations mop (&mparams,getRoot()->getContext());
mop.computeForce( 0+dt, core::VecId::force(), core::VecId::position(), core::VecId::velocity() );
Vector f1 = modeling::getVector( core::VecId::force() );
// cerr<<"test, f1 = " << f1.transpose() << endl;
// backward step
Vector v2 = v1 - f1 * dt;
Vector x2 = x1 - v1 * dt;
// cerr<<"AssembledSolver_test, initial positions : " << x0.transpose() << endl;
// cerr<<"AssembledSolver_test, initial velocities: " << v0.transpose() << endl;
// cerr<<"AssembledSolver_test, new positions : " << x1.transpose() << endl;
// cerr<<"AssembledSolver_test, new velocities: " << v1.transpose() << endl;
// cerr<<"AssembledSolver_test, new positions after backward integration: " << x2.transpose() << endl;
// cerr<<"AssembledSolver_test, new velocities after backward integration: " << v2.transpose() << endl;
ASSERT_TRUE( (x2-x0).lpNorm<Eigen::Infinity>() < precision );
ASSERT_TRUE( (v2-v0).lpNorm<Eigen::Infinity>() < precision );
}
int main(int argc, char** argv)
{
sofa::simulation::tree::init();
sofa::component::initComponentBase();
sofa::component::initComponentCommon();
sofa::component::initComponentGeneral();
sofa::component::initComponentAdvanced();
sofa::component::initComponentMisc();
sofa::gui::initMain();
unsigned int sizeHouseOfCards=4;
SReal angle=20.0;
SReal distanceInBetween=0.1;
SReal friction=0.8;
SReal contactDistance=0.03;
std::string gui = "";
std::string gui_help = "choose the UI (";
gui_help += sofa::gui::GUIManager::ListSupportedGUI('|');
gui_help += ")";
sofa::helper::parse("This is a SOFA application. Here are the command line arguments")
.option(&sizeHouseOfCards,'l',"level","number of level of the house of cards")
.option(&angle,'a',"angle","angle formed by two cards")
.option(&distanceInBetween,'d',"distance","distance between two cards")
.option(&friction,'f',"friction","friction coeff")
.option(&contactDistance,'c',"contactDistance","contact distance")
.option(&gui,'g',"gui",gui_help.c_str())
(argc,argv);
sofa::simulation::setSimulation(new sofa::simulation::tree::TreeSimulation());
// The graph root node
Node::SPtr root = sofa::modeling::createRootWithCollisionPipeline("distanceLMConstraint");
root->setGravity( Coord3(0,-10,0) );
root->setDt(0.001);
sofa::component::collision::MinProximityIntersection *intersection;
root->get(intersection, sofa::core::objectmodel::BaseContext::SearchDown);
intersection->alarmDistance.setValue(contactDistance);
intersection->contactDistance.setValue(contactDistance*0.5);
//************************************
//Floor
Node::SPtr torusFixed = sofa::modeling::createObstacle(root.get(),"mesh/floor.obj", "mesh/floor.obj", "gray");
//Add the objects
createHouseOfCards(root.get(),sizeHouseOfCards,distanceInBetween, angle);
const SReal contactFriction=sqrt(friction);
sofa::helper::vector< sofa::core::CollisionModel* > listCollisionModels;
root->getTreeObjects<sofa::core::CollisionModel>(&listCollisionModels);
for (unsigned int i=0; i<listCollisionModels.size(); ++i) listCollisionModels[i]->setContactFriction(contactFriction);
root->setAnimate(false);
//=======================================
// Export the scene to file
const std::string fileName="HouseOfCards.xml";
sofa::simulation::getSimulation()->exportXML(root.get(),fileName.c_str());
//=======================================
// Destroy created scene: step needed, as I can't get rid of the locales (the mass can't init correctly as 0.1 is not considered as a floating point).
sofa::simulation::DeleteVisitor deleteScene(sofa::core::ExecParams::defaultInstance() );
root->execute(deleteScene);
root.reset();
//=======================================
// Create the GUI
if (int err=sofa::gui::GUIManager::Init(argv[0],gui.c_str()))
return err;
if (int err=sofa::gui::GUIManager::createGUI(NULL))
return err;
sofa::gui::GUIManager::SetDimension(800,600);
//=======================================
// Load the Scene
sofa::simulation::Node::SPtr groot = sofa::core::objectmodel::SPtr_dynamic_cast<sofa::simulation::Node>( sofa::simulation::getSimulation()->load(fileName.c_str()));
if (groot==NULL)
{
groot = sofa::simulation::getSimulation()->createNewGraph("");
}
sofa::simulation::getSimulation()->init(groot.get());
sofa::gui::GUIManager::SetScene(groot,fileName.c_str());
//=======================================
// Run the main loop
if (int err=sofa::gui::GUIManager::MainLoop(groot,fileName.c_str()))
return err;
Node* ggroot = sofa::gui::GUIManager::CurrentSimulation();
if (ggroot!=NULL) sofa::simulation::getSimulation()->unload(groot);
sofa::simulation::tree::cleanup();
return 0;
}
void Node::notifyAddChild(Node::SPtr node)
{
for (helper::vector<MutationListener*>::const_iterator it = listener.begin(); it != listener.end(); ++it)
(*it)->addChild(this, node.get());
}
void Node::notifyMoveChild(Node::SPtr node, Node* prev)
{
for (helper::vector<MutationListener*>::const_iterator it = listener.begin(); it != listener.end(); ++it)
(*it)->moveChild(prev, this, node.get());
}
/** Test in the linear case, with velocity parallel to the spring.
Convergence should occur in one iteration.
Integrate using backward Euler (alpha=1, beta=1).
Post-condition: an explicit Euler step with step -dt brings the system back to the original state.
*/
void testLinearOneFixedOneStiffnessSpringV100(bool debug)
{
SReal dt=0.1; // currently, this must be set in the root node AND passed to the animate function
Node::SPtr root = clearScene();
root->setGravity( Vec3(0,0,0) );
root->setDt(dt);
// The solver
using odesolver::AssembledSolver;
AssembledSolver::SPtr complianceSolver = addNew<AssembledSolver>(getRoot());
complianceSolver->debug.setValue(debug);
complianceSolver->alpha.setValue(1.0);
complianceSolver->beta.setValue(1.0);
SReal precision = 1.0e-6;
linearsolver::LDLTSolver::SPtr linearSolver = addNew<linearsolver::LDLTSolver>(getRoot());
linearSolver->debug.setValue(debug);
// The string
ParticleString string1( root, Vec3(0,0,0), Vec3(1,0,0), 2, 1.0*2 ); // two particles
string1.compliance->isCompliance.setValue(false); // handle it as a stiffness
string1.compliance->compliance.setValue(1.0e-3);
FixedConstraint3::SPtr fixed = addNew<FixedConstraint3>(string1.string_node,"fixedConstraint");
fixed->addConstraint(0); // attach first particle
// velocity parallel to the spring
{
MechanicalObject3::WriteVecCoord v = string1.DOF->writeVelocities();
v[1] = Vec3(1,0,0);
}
//**************************************************
sofa::simulation::getSimulation()->init(root.get());
//**************************************************
// initial state
Vector x0 = getVector( core::VecId::position() );
Vector v0 = getVector( core::VecId::velocity() );
//**************************************************
sofa::simulation::getSimulation()->animate(root.get(),dt);
//**************************************************
Vector x1 = getVector( core::VecId::position() );
Vector v1 = getVector( core::VecId::velocity() );
// Replace the backward (i.e. implicit) Euler solver with a forward (i.e. explicit) Euler solver.
// An integration step using the opposite dt should bring us back to the initial state
getRoot()->removeObject(complianceSolver);
odesolver::EulerSolver::SPtr eulerSolver = New<odesolver::EulerSolver>();
getRoot()->addObject(eulerSolver);
eulerSolver->symplectic.setValue(false);
sofa::simulation::getSimulation()->animate(root.get(),-dt);
Vector x2 = getVector( core::VecId::position() );
Vector v2 = getVector( core::VecId::velocity() );
// cerr<<"AssembledSolver_test, initial positions : " << x0.transpose() << endl;
// cerr<<"AssembledSolver_test, initial velocities: " << v0.transpose() << endl;
// cerr<<"AssembledSolver_test, new positions : " << x1.transpose() << endl;
// cerr<<"AssembledSolver_test, new velocities: " << v1.transpose() << endl;
// cerr<<"AssembledSolver_test, new positions after backward integration: " << x2.transpose() << endl;
// cerr<<"AssembledSolver_test, new velocities after backward integration: " << v2.transpose() << endl;
ASSERT_TRUE( (x2-x0).lpNorm<Eigen::Infinity>() < precision );
ASSERT_TRUE( (v2-v0).lpNorm<Eigen::Infinity>() < precision );
}