// ---------------------------------------------------------------------
// ---
// ---------------------------------------------------------------------
int main(int argc, char** argv)
{
glutInit(&argc,argv);
sofa::simulation::tree::init();
sofa::helper::parse("This is a SOFA application.")
(argc,argv);
sofa::component::initComponentBase();
sofa::component::initComponentCommon();
sofa::component::initComponentGeneral();
sofa::component::initComponentAdvanced();
sofa::component::initComponentMisc();
sofa::gui::initMain();
sofa::gui::GUIManager::Init(argv[0]);
// The graph root node : gravity already exists in a GNode by default
sofa::simulation::setSimulation(new sofa::simulation::tree::TreeSimulation());
sofa::simulation::Node::SPtr groot = sofa::simulation::getSimulation()->createNewGraph("root");
groot->setGravity( Coord3(0,-10,0) );
// One solver for all the graph
EulerImplicitSolver::SPtr solver = sofa::core::objectmodel::New<EulerImplicitSolver>();
solver->setName("solver");
solver->f_printLog.setValue(false);
groot->addObject(solver);
CGLinearSolver::SPtr linearSolver = New<CGLinearSolver>();
linearSolver->setName("linearSolver");
groot->addObject(linearSolver);
// Tetrahedron degrees of freedom
MechanicalObject3::SPtr DOF = sofa::core::objectmodel::New<MechanicalObject3>();
groot->addObject(DOF);
DOF->resize(4);
DOF->setName("DOF");
//get write access to the position vector of mechanical object DOF
WriteAccessor<Data<VecCoord3> > x = *DOF->write(VecId::position());
x[0] = Coord3(0,10,0);
x[1] = Coord3(10,0,0);
x[2] = Coord3(-10*0.5,0,10*0.866);
x[3] = Coord3(-10*0.5,0,-10*0.866);
DOF->showObject.setValue(true);
DOF->showObjectScale.setValue(10.);
// Tetrahedron uniform mass
UniformMass3::SPtr mass = sofa::core::objectmodel::New<UniformMass3>();
groot->addObject(mass);
mass->setMass(2);
mass->setName("mass");
// Tetrahedron topology
MeshTopology::SPtr topology = sofa::core::objectmodel::New<MeshTopology>();
topology->setName("mesh topology");
groot->addObject( topology );
topology->addTetra(0,1,2,3);
// Tetrahedron constraints
FixedConstraint3::SPtr constraints = sofa::core::objectmodel::New<FixedConstraint3>();
constraints->setName("constraints");
groot->addObject(constraints);
constraints->addConstraint(0);
// Tetrahedron force field
TetrahedronFEMForceField3::SPtr fem = sofa::core::objectmodel::New<TetrahedronFEMForceField3>();
fem->setName("FEM");
groot->addObject(fem);
fem->setMethod("polar");
fem->setUpdateStiffnessMatrix(true);
fem->setYoungModulus(6);
// Tetrahedron skin
Node::SPtr skin = groot.get()->createChild("skin");
// The visual model
OglModel::SPtr visual = sofa::core::objectmodel::New<OglModel>();
visual->setName( "visual" );
visual->load(sofa::helper::system::DataRepository.getFile("mesh/liver-smooth.obj"), "", "");
visual->setColor("red");
visual->applyScale(0.7, 0.7, 0.7);
visual->applyTranslation(1.2, 0.8, 0);
skin->addObject(visual);
// The mapping between the tetrahedron (DOF) and the liver (visual)
BarycentricMapping3_to_Ext3::SPtr mapping = sofa::core::objectmodel::New<BarycentricMapping3_to_Ext3>();
mapping->setModels(DOF.get(), visual.get());
mapping->setName( "mapping" );
skin->addObject(mapping);
// Display Flags
sofa::component::visualmodel::VisualStyle::SPtr style = sofa::core::objectmodel::New<sofa::component::visualmodel::VisualStyle>();
groot->addObject(style);
sofa::core::visual::DisplayFlags& flags = *style->displayFlags.beginEdit();
flags.setShowNormals(false);
flags.setShowInteractionForceFields(false);
flags.setShowMechanicalMappings(false);
flags.setShowCollisionModels(false);
flags.setShowBoundingCollisionModels(false);
flags.setShowMappings(false);
flags.setShowForceFields(true);
flags.setShowWireFrame(true);
flags.setShowVisualModels(true);
flags.setShowBehaviorModels(true);
style->displayFlags.endEdit();
// Init the scene
sofa::simulation::tree::getSimulation()->init(groot.get());
groot->setAnimate(false);
//=======================================
// Run the main loop
sofa::gui::GUIManager::MainLoop(groot);
sofa::simulation::tree::cleanup();
return 0;
}
/// 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 );
}
