Collision entre un cube et le sol

using sofa::helper::system::DataRepository;
using sofa::core::Mapping;
using sofa::core::componentmodel::behavior:MechanicalState;
using sofa::core::componentmodel::behavior::MappedModel;
using sofa::component::mapping::BarycentricMapping;
using sofa::component::mapping::SurfaceIdentityMapping;
using sofa::component::visualmodel::GLExtVec3fTypes;

typedef sofa::defaulttype::Vec3Types MyTypes;
typedef MyTypes::Deriv Vec3;
typedef GLExtVec3fTypes OglTypes;
typedef MechanicalState<MyTypes> MyMechanicalState;
typedef MappedModel<OglTypes> OglMappedModel;
typedef SurfaceIdentityMapping< Mapping
   < MyMechanicalState, OglMappedModel > > MyMapping;

int main(int argc, char** argv)
{
	parse("This is a SOFA application.")
	(argc,argv);

        sofa::gui::SofaGUI::Init(argv[0]);
	
	/*
           Root of the scene graph
	 */

        /// Creation of the graph root node
	sofa::simulation::tree::GNode* groot = 
          new sofa::simulation::tree::GNode;
        groot->setName("root");

	
	/*
		Properties for all the graph: 
               objects directly relied to the root 	
	 */
	/// Pipeline for all the graph
	// Default collision detection and modeling pipeline
	sofa::component::collision::DefaultPipeline* pipeline = 
            new sofa::component::collision::DefaultPipeline();
	pipeline->setName("Pipeline");
	groot->addObject(pipeline);
	
	/// Collision detection treatement for all the graph
	sofa::component::collision::BruteForceDetection* force_detect = 
           new sofa::component::collision::BruteForceDetection;
	force_detect->setName("N2");
	groot->addObject(force_detect);
	
	/// Proximity intersection for all the graph
	sofa::component::collision::ProximityIntersection* proximity_intersection 
          = new sofa::component::collision::ProximityIntersection();
	proximity_intersection->setName("Proximity");
	proximity_intersection->setAlarmDistance(0.8);
	proximity_intersection->setContactDistance(0.5);
	groot->addObject(proximity_intersection);
	
	/// Contact manager for all the graph
	sofa::component::collision::DefaultContactManager* contact_manager 
          = new sofa::component::collision::DefaultContactManager();
	contact_manager->setName("Response");
	groot->addObject(contact_manager);
	
	/// Collision group manager for all the graph
	sofa::component::collision::DefaultCollisionGroupManager* group_manager 
          = new sofa::component::collision::DefaultCollisionGroupManager;
	group_manager->setName("Group");
	groot->addObject(group_manager);
	
	/*
		Object inside the scene: a cube
	 */
	/// Creation of the node cube FEM
	sofa::simulation::tree::GNode* cubeFEM = 
          new sofa::simulation::tree::GNode;
	cubeFEM->setName("cubeFEM");
	groot->addChild(cubeFEM);
	
	/// Solver implicit for the cube
	// nb iteration = 25
	sofa::component::odesolver::CGImplicitSolver* solver =
          new sofa::component::odesolver::CGImplicitSolver;
	solver->setName("Solver");
	cubeFEM->addObject(solver);
	
	///	Mechanical properties of the cube
	sofa::component::MechanicalObject<MyTypes>* DOF = 
         new sofa::component::MechanicalObject<MyTypes>;
	DOF->setName("DOF");
	DOF->load("data/smCube125.obj");
	cubeFEM->addObject(DOF);	
	
	/// Mass uniform for the cube
	sofa::component::mass::UniformMass<MyTypes,double>* mass = 
          new sofa::component::mass::UniformMass<MyTypes,double>;
	mass->setMass(0.25);
	mass->setName("Mass");
	cubeFEM->addObject(mass);	
	
	/// Regular grid for the cube
	sofa::component::topology::RegularGridTopology* topology = 
          new sofa::component::topology::RegularGridTopology();
	topology->setName("topology");
	topology->setNx(5);
	topology->setNy(5);
	topology->setNz(5);
	topology->setPos(-3.5, 3.5, -3.5, 3.5, -3.5, 3.5);	
	cubeFEM->addObject(topology);
	
	/// The cube force field
	sofa::component::forcefield::TetrahedronFEMForceField<MyTypes>* FEM 
              = new sofa::component::forcefield::TetrahedronFEMForceField<MyTypes>;
	FEM->setYoungModulus(25);
	FEM->setPoissonRatio(0.3);
	FEM->setUpdateStiffnessMatrix(false);
	FEM->setComputeGlobalMatrix(false);
	FEM->setMethod(1);
	FEM->setName("FEM");
	cubeFEM->addObject(FEM);

	
	/*
	  The rendering part of the cube: Visu (child of the cubeFEM node)
	 */
	/// Creation of node for the rendering part of the cube: Visu
	sofa::simulation::tree::GNode* Visu = new sofa::simulation::tree::GNode;
	Visu->setName("Visu");
	cubeFEM->addChild(Visu);
	
	/// The visual model of Visu
	sofa::component::visualmodel::OglModel* Visual 
           = new sofa::component::visualmodel::OglModel();
        Visual->setName( "Visual" );
	Visual->load(DataRepository.getFile("smCube125.obj"), "", "");
        Visual->setColor("red");
	Visual->drawTransparent();
        Visu->addObject(Visual);
	
	/// Mapping for the visual part of the cube
	MyMapping* MappingVisu = new MyMapping(DOF, Visual);
	MappingVisu->setName( "Mapping" );
	Visu->addObject(MappingVisu);
	
	/*
	   The mecanical part of the cube: Surf (child of the cubeFEM node)
	 */
	/// Creation of the node for the mecanical part of the cube: Surf
	sofa::simulation::tree::GNode* Surf = new sofa::simulation::tree::GNode;
	Surf->setName("Surf");
	cubeFEM->addChild(Surf);

	//// For this object, we used two collision models: line and triangle 
	// (according the raffinement of the detection)
	/// Triangle Model
	sofa::component::collision::TriangleModel *TriangleModel 
         = new sofa::component::collision::TriangleModel();
	TriangleModel->setName("Triangle");
	TriangleModel->computeBoundingTree();
	TriangleModel->update();
	Surf->addObject(TriangleModel);
	
	/// Line Model
	sofa::component::collision::LineModel *LineModel 
          = new sofa::component::collision::LineModel();
	LineModel->setName("Line");
	Surf->addObject(LineModel);
	
	/*
	   Object inside the scene: the floor
	 */
	// Creation of the node for the floor
	sofa::simulation::tree::GNode* Floor = new sofa::simulation::tree::GNode;
	Floor->setName("Floor");
	groot->addChild(Floor);
	
	/// Mesh topology of the floor
	sofa::component::topology::MeshTopology* MeshTopologyFloor 
           = new sofa::component::topology::MeshTopology();
	MeshTopologyFloor->setName("MeshTopology");
	MeshTopologyFloor->load("data/floor3.obj");
	Floor->addObject(MeshTopologyFloor);
	
	///	Mechanical properties of the floor
	sofa::component::MechanicalObject<MyTypes>* DOFFloor 
          = new sofa::component::MechanicalObject<MyTypes>;
	DOFFloor->applyTranslation(0.0, -10.0, 0.0);
	DOFFloor->applyScale(0.75); 
	DOFFloor->setName("DOFFloor");
	Floor->addObject(DOFFloor);
	
	/// The visual model of the floor
	sofa::component::visualmodel::OglModel* VisualFloor 
         = new sofa::component::visualmodel::OglModel();
        VisualFloor->setName( "VisualFloor" );

	VisualFloor->load(DataRepository.getFile("floor3.obj"), "", "");
	VisualFloor->applyScale(0.75);
	VisualFloor->applyTranslation(0.0, -10.0, 0.0);
        Floor->addObject(VisualFloor);
	
	/// Triangle Model for collision
	sofa::component::collision::TriangleModel *TriangleModelFloor 
         = new sofa::component::collision::TriangleModel();
	TriangleModelFloor->setName("Triangle");
	TriangleModelFloor->computeBoundingTree();
	TriangleModelFloor->update();
	Floor->addObject(TriangleModelFloor);

	/// Line Model for collision
	sofa::component::collision::LineModel *LineModelFloor = new          
           sofa::component::collision::LineModel();
	LineModelFloor->setName("Line");
	Floor->addObject(LineModelFloor);
	
	
	/*
	   Gestion of the scene 
	 */
        /// Initialisation of the scene
        sofa::simulation::tree::Simulation::init(groot);
	
	groot->setDt(0.02);
        groot->setAnimate(false);
        groot->setShowNormals(false);
        groot->setShowInteractionForceFields(false);
        groot->setShowMechanicalMappings(false);
	groot->setShowCollisionModels(false);
	groot->setShowBoundingCollisionModels(false);	
	groot->setShowMappings(false);
	groot->setShowWireFrame(false);
	groot->setShowBehaviorModels(true);
	groot->setShowForceFields(true);
	groot->setShowVisualModels(true);

	/// Run the main loop
	sofa::gui::SofaGUI::MainLoop(groot);

	return 0;
}