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;
}
/// Create an assembly of a siff hexahedral grid with other objects
simulation::Node::SPtr createGridScene(Vec3 startPoint, Vec3 endPoint, unsigned numX, unsigned numY, unsigned numZ, double totalMass/*, double stiffnessValue, double dampingRatio=0.0*/ )
{
using helper::vector;
// The graph root node
Node::SPtr root = simulation::getSimulation()->createNewGraph("root");
root->setGravity( Coord3(0,-10,0) );
root->setAnimate(false);
root->setDt(0.01);
addVisualStyle(root)->setShowVisual(false).setShowCollision(false).setShowMapping(true).setShowBehavior(true);
Node::SPtr simulatedScene = root->createChild("simulatedScene");
EulerImplicitSolver::SPtr eulerImplicitSolver = New<EulerImplicitSolver>();
simulatedScene->addObject( eulerImplicitSolver );
CGLinearSolver::SPtr cgLinearSolver = New<CGLinearSolver>();
simulatedScene->addObject(cgLinearSolver);
// The rigid object
Node::SPtr rigidNode = simulatedScene->createChild("rigidNode");
MechanicalObjectRigid3::SPtr rigid_dof = addNew<MechanicalObjectRigid3>(rigidNode, "dof");
UniformMassRigid3::SPtr rigid_mass = addNew<UniformMassRigid3>(rigidNode,"mass");
FixedConstraintRigid3::SPtr rigid_fixedConstraint = addNew<FixedConstraintRigid3>(rigidNode,"fixedConstraint");
// Particles mapped to the rigid object
Node::SPtr mappedParticles = rigidNode->createChild("mappedParticles");
MechanicalObject3::SPtr mappedParticles_dof = addNew< MechanicalObject3>(mappedParticles,"dof");
RigidMappingRigid3_to_3::SPtr mappedParticles_mapping = addNew<RigidMappingRigid3_to_3>(mappedParticles,"mapping");
mappedParticles_mapping->setModels( rigid_dof.get(), mappedParticles_dof.get() );
// The independent particles
Node::SPtr independentParticles = simulatedScene->createChild("independentParticles");
MechanicalObject3::SPtr independentParticles_dof = addNew< MechanicalObject3>(independentParticles,"dof");
// The deformable grid, connected to its 2 parents using a MultiMapping
Node::SPtr deformableGrid = independentParticles->createChild("deformableGrid"); // first parent
mappedParticles->addChild(deformableGrid); // second parent
RegularGridTopology::SPtr deformableGrid_grid = addNew<RegularGridTopology>( deformableGrid, "grid" );
deformableGrid_grid->setNumVertices(numX,numY,numZ);
deformableGrid_grid->setPos(startPoint[0],endPoint[0],startPoint[1],endPoint[1],startPoint[2],endPoint[2]);
MechanicalObject3::SPtr deformableGrid_dof = addNew< MechanicalObject3>(deformableGrid,"dof");
SubsetMultiMapping3_to_3::SPtr deformableGrid_mapping = addNew<SubsetMultiMapping3_to_3>(deformableGrid,"mapping");
deformableGrid_mapping->addInputModel(independentParticles_dof.get()); // first parent
deformableGrid_mapping->addInputModel(mappedParticles_dof.get()); // second parent
deformableGrid_mapping->addOutputModel(deformableGrid_dof.get());
UniformMass3::SPtr mass = addNew<UniformMass3>(deformableGrid,"mass" );
mass->mass.setValue( totalMass/(numX*numY*numZ) );
HexahedronFEMForceField3::SPtr hexaFem = addNew<HexahedronFEMForceField3>(deformableGrid, "hexaFEM");
hexaFem->f_youngModulus.setValue(1000);
hexaFem->f_poissonRatio.setValue(0.4);
// ====== Set up the multimapping and its parents, based on its child
deformableGrid_grid->init(); // initialize the grid, so that the particles are located in space
deformableGrid_dof->init(); // create the state vectors
MechanicalObject3::ReadVecCoord xgrid = deformableGrid_dof->readPositions(); // cerr<<"xgrid = " << xgrid << endl;
// create the rigid frames and their bounding boxes
unsigned numRigid = 2;
vector<BoundingBox> boxes(numRigid);
vector< vector<unsigned> > indices(numRigid); // indices of the particles in each box
double eps = (endPoint[0]-startPoint[0])/(numX*2);
// first box, x=xmin
boxes[0] = BoundingBox(Vec3d(startPoint[0]-eps, startPoint[1]-eps, startPoint[2]-eps),
Vec3d(startPoint[0]+eps, endPoint[1]+eps, endPoint[2]+eps));
// second box, x=xmax
boxes[1] = BoundingBox(Vec3d(endPoint[0]-eps, startPoint[1]-eps, startPoint[2]-eps),
Vec3d(endPoint[0]+eps, endPoint[1]+eps, endPoint[2]+eps));
rigid_dof->resize(numRigid);
MechanicalObjectRigid3::WriteVecCoord xrigid = rigid_dof->writePositions();
xrigid[0].getCenter()=Vec3d(startPoint[0], 0.5*(startPoint[1]+endPoint[1]), 0.5*(startPoint[2]+endPoint[2]));
xrigid[1].getCenter()=Vec3d( endPoint[0], 0.5*(startPoint[1]+endPoint[1]), 0.5*(startPoint[2]+endPoint[2]));
// find the particles in each box
vector<bool> isFree(xgrid.size(),true);
unsigned numMapped = 0;
for(unsigned i=0; i<xgrid.size(); i++){
for(unsigned b=0; b<numRigid; b++ )
{
if( isFree[i] && boxes[b].contains(xgrid[i]) )
{
indices[b].push_back(i); // associate the particle with the box
isFree[i] = false;
numMapped++;
}
}
}
// distribution of the grid particles to the different parents (independent particle or solids.
vector< pair<MechanicalObject3*,unsigned> > parentParticles(xgrid.size());
// Copy the independent particles to their parent DOF
independentParticles_dof->resize( numX*numY*numZ - numMapped );
MechanicalObject3::WriteVecCoord xindependent = independentParticles_dof->writePositions(); // parent positions
unsigned independentIndex=0;
for( unsigned i=0; i<xgrid.size(); i++ ){
if( isFree[i] ){
parentParticles[i]=make_pair(independentParticles_dof.get(),independentIndex);
xindependent[independentIndex] = xgrid[i];
independentIndex++;
}
}
// Mapped particles. The RigidMapping requires to cluster the particles based on their parent frame.
mappedParticles_dof->resize(numMapped);
MechanicalObject3::WriteVecCoord xmapped = mappedParticles_dof->writePositions(); // parent positions
mappedParticles_mapping->globalToLocalCoords.setValue(true); // to define the mapped positions in world coordinates
vector<unsigned>* pointsPerFrame = mappedParticles_mapping->pointsPerFrame.beginEdit(); // to set how many particles are attached to each frame
unsigned mappedIndex=0;
for( unsigned b=0; b<numRigid; b++ )
{
const vector<unsigned>& ind = indices[b];
pointsPerFrame->push_back((unsigned)ind.size()); // Tell the mapping the number of points associated with this frame. One box per frame
for(unsigned i=0; i<ind.size(); i++)
{
parentParticles[ind[i]]=make_pair(mappedParticles_dof.get(),mappedIndex);
xmapped[mappedIndex] = xgrid[ ind[i] ];
mappedIndex++;
}
}
mappedParticles_mapping->pointsPerFrame.endEdit();
// Declare all the particles to the multimapping
for( unsigned i=0; i<xgrid.size(); i++ )
{
deformableGrid_mapping->addPoint( parentParticles[i].first, parentParticles[i].second );
}
return root;
}
/// 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 );
}
/** 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 );
}