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 SetUp()
{
setSimulation(m_simu = new DAGSimulation());
m_node = m_simu->createNewGraph("root");
m_thisObject = New<ThisClass >() ;
m_node->addObject(m_thisObject) ;
}
typename Component::SPtr addNew( Node::SPtr parentNode, std::string name="" )
{
typename Component::SPtr component = New<Component>();
parentNode->addObject(component);
component->setName(parentNode->getName()+"_"+name);
return component;
}
void SetUp()
{
setSimulation(m_simu = new DAGSimulation());
m_node = m_simu->createNewGraph("root");
m_thisObject = New<ThisClass>() ;
m_mecaobject = New<MechanicalObject<DataTypes>>() ;
m_mecaobject->init() ;
m_node->addObject(m_mecaobject) ;
m_node->addObject(m_thisObject) ;
}
void SetUp()
{
// SetUp1
setSimulation(m_simu = new DAGSimulation());
m_thisObject = New<ThisClass >();
m_node1 = m_simu->createNewGraph("root");
m_node1->addObject(m_thisObject);
// SetUp2
string scene1 =
"<?xml version='1.0'?>"
"<Node name='Root' gravity='0 0 0' time='0' animate='0' > "
" <Node name='node'> "
" <PlaneROI template='Vec3d' name='PlaneROI' plane='2 0 0 0 0 0 2 2 0 2'/> "
" </Node> "
"</Node> " ;
m_node2 = SceneLoaderXML::loadFromMemory ("testscene",
scene1.c_str(),
scene1.size()) ;
}
/// It is important to freeze what are the available Data field
/// of a component and rise warning/errors when some one removed.
///
void attributesTests(){
m_node = m_root->createChild("node") ;
m_mass = New< TheUniformMass >() ;
m_node->addObject(m_mass) ;
EXPECT_TRUE( m_mass->findData("mass") != nullptr ) ;
EXPECT_TRUE( m_mass->findData("totalmass") != nullptr ) ;
EXPECT_TRUE( m_mass->findData("filename") != nullptr ) ;
EXPECT_TRUE( m_mass->findData("localRange") != nullptr ) ;
EXPECT_TRUE( m_mass->findData("showGravityCenter") != nullptr ) ;
EXPECT_TRUE( m_mass->findData("showAxisSizeFactor") != nullptr ) ;
EXPECT_TRUE( m_mass->findData("showInitialCenterOfGravity") != nullptr ) ;
EXPECT_TRUE( m_mass->findData("indices") != nullptr ) ;
EXPECT_TRUE( m_mass->findData("handleTopoChange") != nullptr ) ;
EXPECT_TRUE( m_mass->findData("preserveTotalMass") != nullptr ) ;
EXPECT_TRUE( m_mass->findData("compute_mapping_inertia") != nullptr ) ;
EXPECT_TRUE( m_mass->findData("totalMass") != nullptr ) ;
return ;
}
// ---------------------------------------------------------------------
// ---
// ---------------------------------------------------------------------
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;
}
/// 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;
}