/// Create the context for the matrix tests. void SetUp() { sofa::component::init(); // if( sofa::simulation::getSimulation()==NULL ) sofa::simulation::setSimulation(simulation = new sofa::simulation::graph::DAGSimulation()); /// Create the scene root = simulation->createNewGraph("root"); PointSetTopologyContainer::SPtr topology = New<PointSetTopologyContainer>(); root->addObject(topology); dofs = New<MechanicalObject>(); root->addObject(dofs); projection = New<ProjectToPlaneConstraint>(); root->addObject(projection); /// Set the values numNodes = 3; dofs->resize(numNodes); origin = CPos(0,0,0); projection->f_origin.setValue(origin); normal = CPos(1,1,1); projection->f_normal.setValue(normal); }
/** Constrain one particle, and not the last one. Detects bugs like not setting the projection matrix entries beyond the last constrained particle */ void init_2bones() { joints.clear(); typename MechanicalObject::WriteVecCoord x = dofs->writePositions(); x.resize(2); VecCoord rigids(2); DataTypes::setCPos(x[1], CPos(0,1,0)); DataTypes::setCRot(x[1], CRot(0.707107,0, 0, 0.707107)); // rotation x: 90 degree Coord target(CPos(1,1,1), CRot(0,0.382683,0,0.92388)); //rotation y : 45 degrees joints.resize(2); joints[0].addChannel(x[0], 0); joints[0].addChannel(target, 1); joints[0].setRestPosition(x[0]); joints[1].addChannel(x[1], 0); joints[1].setRestPosition(x[1]); joints[1].mParentIndex = 0; helper::vector<int> bones(2,0); bones[1] = 1; projection->setSkeletalMotion(joints, bones); /// Init sofa::simulation::getSimulation()->init(root.get()); simulation->animate(root.get(),0.25); simulation->animate(root.get(),0.25); }
/** * @brief Create the physical model * * @param m Mass of the particle * @param s Stiffness of the spring * @param d Damping ratio of the spring * @param xx0 initial position * @param vv0 initial velocity */ void setup( SReal m, SReal s, SReal d, SReal xx0, SReal vv0 ) { mass = m; stiffness = s; damping = d; x0=xx0; v0=vv0; node = clearScene(); node->setGravity( Vec3(0,0,0) ); // The oscillator simulation::Node::SPtr oscillator = node->createChild("oscillator"); DOF = addNew<MechanicalObject1>(oscillator,"DOF"); DOF->resize(1); DOF->writePositions()[0] = Vec1(x0); DOF->writeVelocities()[0] = Vec1(v0); UniformMass1::SPtr Mass = addNew<UniformMass1>(oscillator,"mass"); Mass->mass.setValue( mass ); compliance = addNew<UniformCompliance1>(oscillator,"compliance"); compliance->isCompliance.setValue(false); compliance->compliance.setValue(1.0/stiffness); compliance->damping.setValue(damping); }
/** * @brief Perform the time integration over several time steps and compare with the theoretical solution * @param endTime Simulation stops when time is higher than this * @param dt Time step * @param tolerance Admissible absolute error * @param debug Print debug info */ void testTimeIntegration( SReal endTime, SReal dt, SReal tolerance, bool debug ) { node->setDt(dt); //************************************************** sofa::simulation::getSimulation()->init(node.get()); //************************************************** if(debug) simulation::getSimulation()->exportXML ( node.get(), "/tmp/oscilator.scn" ); //************************************************** // Simulation loop SReal t=0; printInfo(debug,t); while( t<endTime ) { simulation::getSimulation()->animate(node.get(),dt); t+=dt ; printInfo(debug,t); SReal x = DOF->readPositions()[0][0]; ASSERT_TRUE( fabs(x-theoreticalPosition(t)) < tolerance ); } //************************************************** }
/// Create the context for the tests. void SetUp() { // if( sofa::simulation::getSimulation()==NULL ) sofa::simulation::setSimulation(simulation = new sofa::simulation::graph::DAGSimulation()); /// Create the scene root = simulation->createNewGraph("root"); PointSetTopologyContainer::SPtr topology = core::objectmodel::New<PointSetTopologyContainer>(); root->addObject(topology); dofs = core::objectmodel::New<MechanicalObject>(); root->addObject(dofs); projection = core::objectmodel::New<ProjectToLineConstraint>(); root->addObject(projection); /// Set the values numNodes = 3; dofs->resize(numNodes); origin = CPos(0,0,0); projection->f_origin.setValue(origin); direction = CPos(1,1,1); projection->f_direction.setValue(direction); }
/// After simulation compare the positions of points to the theoretical positions. bool compareSimulatedToTheoreticalPositions(double convergenceAccuracy, double diffMaxBetweenSimulatedAndTheoreticalPosition) { // Init simulation sofa::simulation::getSimulation()->init(root.get()); // Compute the theoretical final positions VecCoord finalPos; typename PatchTestMovementConstraint::SPtr bilinearConstraint = root->get<PatchTestMovementConstraint>(root->SearchDown); typename MechanicalObject::SPtr dofs = root->get<MechanicalObject>(root->SearchDown); typename MechanicalObject::ReadVecCoord x0 = dofs->readPositions(); bilinearConstraint->getFinalPositions( finalPos,*dofs->write(core::VecCoordId::position()) ); // Initialize size_t numNodes = finalPos.size(); VecCoord xprev(numNodes); VecDeriv dx(numNodes); bool hasConverged = true; for (size_t i=0; i<numNodes; i++) { xprev[i] = CPos(0,0,0); } // Animate do { hasConverged = true; sofa::simulation::getSimulation()->animate(root.get(),0.5); typename MechanicalObject::ReadVecCoord x = dofs->readPositions(); // Compute dx for (size_t i=0; i<x.size(); i++) { dx[i] = x[i]-xprev[i]; // Test convergence if(dx[i].norm()>convergenceAccuracy) hasConverged = false; } // xprev = x for (size_t i=0; i<numNodes; i++) { xprev[i]=x[i]; } } while(!hasConverged); // not converged // Compare the theoretical positions and the simulated positions bool succeed=true; for(size_t i=0; i<finalPos.size(); i++ ) { if((finalPos[i]-x0[i]).norm()>diffMaxBetweenSimulatedAndTheoreticalPosition) { succeed = false; ADD_FAILURE() << "final Position of point " << i << " is wrong: " << x0[i] << std::endl <<"the expected Position is " << finalPos[i] << std::endl << "difference = " <<(finalPos[i]-x0[i]).norm() << std::endl; } } return succeed; }
/// After simulation compare the positions of points to the theoretical positions. bool compareSimulatedToTheoreticalPositions( double h, double tolerancePosition, double toleranceEnergy = 1e-13, double checkEnergyConservation=false) { int i = 0; // Init simulation sofa::simulation::getSimulation()->init(root.get()); double time = root->getTime(); // Get mechanical object simulation::Node::SPtr massNode = root->getChild("MassNode"); typename MechanicalObject::SPtr dofs = massNode->get<MechanicalObject>(root->SearchDown); // Animate do { // Record the mass position Coord p0=dofs.get()->read(sofa::core::ConstVecCoordId::position())->getValue()[0]; double absoluteError = fabs(p0[1]-positionsArray[i]); // Compare mass position to the theoretical position if( absoluteError > tolerancePosition ) { ADD_FAILURE() << "Position of mass at time " << time << " is wrong: " << std::endl <<" expected Position is " << positionsArray[i] << std::endl <<" actual Position is " << p0[1] << std::endl << "absolute error = " << absoluteError << std::endl; return false; } //Animate sofa::simulation::getSimulation()->animate(root.get(),h); time = root->getTime(); // Check if hamiltonian energy is constant when there is no damping if(checkEnergyConservation && fabs(variationalSolver->f_hamiltonianEnergy.getValue() -totalEnergy) > toleranceEnergy ) { ADD_FAILURE() << "Hamiltonian energy at time " << time << " is wrong: " << std::endl <<" expected Energy is " << totalEnergy << std::endl <<" actual Energy is " << variationalSolver->f_hamiltonianEnergy.getValue() << std::endl << "absolute error = " << fabs(variationalSolver->f_hamiltonianEnergy.getValue() -totalEnergy) << std::endl; return false; } // Iterate i++; } while (time < 2); return true; }
void DefaultCollisionGroupManager::clearGroup(const Container &inNodes, simulation::Node::SPtr group) { core::objectmodel::BaseNode::SPtr parent = *inNodes.begin(); while(!group->child.empty()) parent->moveChild(core::objectmodel::BaseNode::SPtr(group->child.begin()->get()->toBaseNode())); simulation::CleanupVisitor cleanupvis(sofa::core::ExecParams::defaultInstance()); cleanupvis.execute(group.get()); simulation::DeleteVisitor vis(sofa::core::ExecParams::defaultInstance()); vis.execute(group.get()); group->detachFromGraph(); //delete group; group.reset(); }
// Load the scene BezierTetrahedronTopology.sc from the Scenes directory void createScene() { // GenerateCylinder object typename sofa::component::engine::GenerateCylinder<DataTypes>::SPtr eng= sofa::modeling::addNew<sofa::component::engine::GenerateCylinder<DataTypes> >(root,"cylinder"); eng->f_radius=0.2; eng->f_height=1.0; // TetrahedronSetTopologyContainer object sofa::component::topology::TetrahedronSetTopologyContainer::SPtr container1= sofa::modeling::addNew<sofa::component::topology::TetrahedronSetTopologyContainer>(root,"Container1"); sofa::modeling::setDataLink(&eng->f_tetrahedra,&container1->d_tetrahedron); sofa::modeling::setDataLink(&eng->f_outputTetrahedraPositions,&container1->d_initPoints); // TetrahedronSetGeometryAlgorithms object typename sofa::component::topology::TetrahedronSetGeometryAlgorithms<DataTypes>::SPtr geo1= sofa::modeling::addNew<sofa::component::topology::TetrahedronSetGeometryAlgorithms<DataTypes> >(root); // mechanicalObject object typename MechanicalObject::SPtr meca1= sofa::modeling::addNew<MechanicalObject>(root); sofa::modeling::setDataLink(&eng->f_outputTetrahedraPositions,&meca1->x); // subnode simulation::Node::SPtr bezierNode = root->createChild("BezierTetrahedronTopology"); // BezierTetrahedronSetTopologyContainer sofa::component::topology::BezierTetrahedronSetTopologyContainer::SPtr container2= sofa::modeling::addNew<sofa::component::topology::BezierTetrahedronSetTopologyContainer>(bezierNode,"Container2"); // Mesh2BezierTopologicalMapping sofa::component::topology::Mesh2BezierTopologicalMapping::SPtr mapping= sofa::modeling::addNew<sofa::component::topology::Mesh2BezierTopologicalMapping>(bezierNode,"Mapping"); mapping->setTopologies(container1.get(),container2.get()); mapping->bezierTetrahedronDegree=3; // mechanicalObject object typename MechanicalObject::SPtr meca2= sofa::modeling::addNew<MechanicalObject>(bezierNode,"BezierMechanicalObject"); // BezierTetrahedronSetGeometryAlgorithms typename sofa::component::topology::BezierTetrahedronSetGeometryAlgorithms<DataTypes>::SPtr geo2= sofa::modeling::addNew<sofa::component::topology::BezierTetrahedronSetGeometryAlgorithms<DataTypes> >(bezierNode); // MeshMatrixMass typename MeshMatrixMass::SPtr mass= sofa::modeling::addNew<MeshMatrixMass >(bezierNode,"BezierMass"); mass->m_massDensity=1.0; mass->d_integrationMethod.setValue(std::string("analytical")); }
/// Create the context for the tests. void SetUp() { // if( sofa::simulation::getSimulation()==NULL ) sofa::simulation::setSimulation(simulation = new sofa::simulation::graph::DAGSimulation()); /// Create the scene root = simulation->createNewGraph("root"); dofs = core::objectmodel::New<MechanicalObject>(); root->addObject(dofs); projection = core::objectmodel::New<SkeletalMotionConstraint>(); root->addObject(projection); }
/** Constrain all the particles. */ void init_allParticlesConstrained() { indices.clear(); for(unsigned i = 0; i<numNodes; i++) indices.push_back(i); projection->f_indices.setValue(indices); /// Init sofa::simulation::getSimulation()->init(root.get()); }
/** Constrain one particle, and not the last one. Detects bugs like not setting the projection matrix entries beyond the last constrained particle */ void init_oneConstrainedParticle() { indices.clear(); indices.push_back(1); std::sort(indices.begin(),indices.end()); // checking vectors in linear time requires sorted indices projection->f_indices.setValue(indices); /// Init sofa::simulation::getSimulation()->init(root.get()); }
/// After simulation compare the positions of points to the theoretical positions. bool compareSimulatedToTheoreticalPositions(double tolerance) { // Init simulation sofa::simulation::getSimulation()->init(root.get()); double time = root->getTime(); double stiffnessSpring = 100; double mass = 10; double w = sqrt(stiffnessSpring/mass); // Get mechanical object simulation::Node::SPtr massNode = root->getChild("MassNode"); typename MechanicalObject::SPtr dofs = massNode->get<MechanicalObject>(root->SearchDown); // Animate do { // Record the mass position Coord p0=dofs.get()->read(sofa::core::ConstVecCoordId::position())->getValue()[0]; // Absolute error double absoluteError = fabs(p0[1]-(cos(w*time))); // Compare mass position to the theoretical position if( absoluteError > tolerance ) { ADD_FAILURE() << "Position of mass at time " << time << " is wrong: " << std::endl <<" expected Position is " << cos(sqrt(stiffnessSpring/mass)*time) << std::endl <<" actual Position is " << p0[1] << std::endl << "absolute error = " << absoluteError << std::endl; return false; } //Animate sofa::simulation::getSimulation()->animate(root.get(),0.001); time = root->getTime(); } while (time < 2); return true; }
bool initScene (std::string sceneName) { LoadScene(sceneName); // Init the scene sofa::simulation::getSimulation()->init(root.get()); // Test if root is not null if(!root) { ADD_FAILURE() << "Error in init for the scene: " << sceneName << std::endl; return false; } return true; }
/// Create the context for the scene void createScene(double K, double m, double l0, double rm=0, double rk=0) { // Init simulation sofa::simulation::setSimulation(simulation = new sofa::simulation::graph::DAGSimulation()); root = simulation::getSimulation()->createNewGraph("root"); // Create the scene root->setGravity(Coord(0,-10,0)); // Solver variationalSolver = addNew<VariationalSymplecticSolver> (getRoot()); variationalSolver->f_rayleighStiffness.setValue(rk); variationalSolver->f_rayleighMass.setValue(rm); variationalSolver->f_computeHamiltonian.setValue(1); variationalSolver->f_newtonError.setValue(1e-12);//1e-18 variationalSolver->f_newtonSteps.setValue(4);//7 CGLinearSolver::SPtr cgLinearSolver = addNew<CGLinearSolver> (getRoot()); cgLinearSolver->f_maxIter=3000; cgLinearSolver->f_tolerance =1e-12; cgLinearSolver->f_smallDenominatorThreshold=1e-12; // Set initial positions and velocities of fixed point and mass MechanicalObject3::VecCoord xFixed(1); MechanicalObject3::DataTypes::set( xFixed[0], 0., 2.,0.); MechanicalObject3::VecDeriv vFixed(1); MechanicalObject3::DataTypes::set( vFixed[0], 0.,0.,0.); MechanicalObject3::VecCoord xMass(1); MechanicalObject3::DataTypes::set( xMass[0], 0., 1.,0.); MechanicalObject3::VecDeriv vMass(1); MechanicalObject3::DataTypes::set( vMass[0], 0., 0., 0.); // Mass spring system root = this-> createMassSpringSystem( root, // add mass spring system to the node containing solver K, // stiffness m, // mass l0, // spring rest length xFixed, // Initial position of fixed point vFixed, // Initial velocity of fixed point xMass, // Initial position of mass vMass); // Initial velocity of mass }
/// After simulation compare the positions of points to the theoretical positions. bool compareSimulatedToTheoreticalPositions(double convergenceAccuracy, double diffMaxBetweenSimulatedAndTheoreticalPosition) { // Init simulation sofa::simulation::getSimulation()->init(root.get()); // Compute the theoretical final positions VecCoord finalPos; patchStruct.affineConstraint->getFinalPositions( finalPos,*patchStruct.dofs->write(core::VecCoordId::position()) ); // Initialize size_t numNodes = finalPos.size(); VecCoord xprev(numNodes); VecDeriv dx(numNodes); bool hasConverged = true; for (size_t i=0; i<numNodes; i++) { xprev[i] = CPos(0,0,0); } // Animate do { hasConverged = true; sofa::simulation::getSimulation()->animate(root.get(),0.5); typename MechanicalObject::ReadVecCoord x = patchStruct.dofs->readPositions(); // Compute dx for (size_t i=0; i<x.size(); i++) { dx[i] = x[i]-xprev[i]; // Test convergence if(dx[i].norm()>convergenceAccuracy) hasConverged = false; } // xprev = x for (size_t i=0; i<numNodes; i++) { xprev[i]=x[i]; } } while(!hasConverged); // not converged // Get simulated positions typename MechanicalObject::WriteVecCoord x = patchStruct.dofs->writePositions(); // Compare the theoretical positions and the simulated positions bool succeed=true; for(size_t i=0; i<finalPos.size(); i++ ) { if((finalPos[i]-x[i]).norm()>diffMaxBetweenSimulatedAndTheoreticalPosition) { succeed = false; ADD_FAILURE() << "final Position of point " << i << " is wrong: " << x[i] << std::endl <<"the expected Position is " << finalPos[i] << std::endl << "difference = " <<(finalPos[i]-x[i]).norm() << std::endl <<"rotation = " << testedRotation << std::endl << " translation = " << testedTranslation << std::endl << "Failed seed number =" << BaseSofa_test::seed; } } return succeed; }
bool testBezierTetrahedronTopology() { // Init simulation sofa::simulation::getSimulation()->init(root.get()); BezierTetrahedronSetTopologyContainer *container=root->get<BezierTetrahedronSetTopologyContainer>(root->SearchDown); size_t nTetras,elem; BezierDegreeType degree=container->getDegree(); // check the total number of vertices. size_t nbPoints=container->getNumberOfTetrahedralPoints()+container->getNumberOfEdges()*(degree-1)+container->getNumberOfTriangles()*(degree-1)*(degree-2)/2+container->getNumberOfTetrahedra()*((degree-1)*(degree-2)*(degree-3)/6); if((size_t)container->getNbPoints()!=nbPoints) { ADD_FAILURE() << "wrong number of points " <<container->getNbPoints() << " is wrong. It should be " <<nbPoints << std::endl; return false; } sofa::helper::vector<TetrahedronBezierIndex> tbiArray=container->getTetrahedronBezierIndexArray(); BezierTetrahedronPointLocation location; size_t elementIndex, elementOffset/*,localIndex*/; for (nTetras=0;nTetras<container->getNumberOfTetrahedra();++nTetras) { const BezierTetrahedronSetTopologyContainer::VecPointID &indexArray=container->getGlobalIndexArrayOfBezierPoints(nTetras); // check the number of control points per tetrahedron is correct nbPoints=(4+6*(degree-1)+2*(degree-1)*(degree-2)+(degree-1)*(degree-2)*(degree-3)/6); if(indexArray.size()!=nbPoints) { ADD_FAILURE() << "wrong number of control points in tetrahedron " <<nTetras<< ". It is "<<indexArray.size() <<" and should be "<<nbPoints << std::endl; return false; } for(elem=0;elem<indexArray.size();++elem) { size_t globalIndex=container->getGlobalIndexOfBezierPoint(nTetras,tbiArray[elem]); // check that getGlobalIndexOfBezierPoint and getGlobalIndexArrayOfBezierPointsInTetrahedron give the same answer if(globalIndex!=indexArray[elem]) { ADD_FAILURE() << "wrong global index given by getGlobalIndexOfBezierPoint(). It is : "<<globalIndex <<" and should be "<<indexArray[elem] << std::endl; return false; } TetrahedronBezierIndex tbi=container->getTetrahedronBezierIndex(elem); if(elem!=container->getLocalIndexFromTetrahedronBezierIndex(tbi)) { ADD_FAILURE() << "wrong local index given by getLocalIndexFromTetrahedronBezierIndex(). It is : "<<container->getLocalIndexFromTetrahedronBezierIndex(tbi) <<" and should be "<<elem << std::endl; return false; } // check that getTetrahedronBezierIndex is consistant with getTetrahedronBezierIndexArray if ((tbiArray[elem][0]!=tbi[0]) || (tbiArray[elem][1]!=tbi[1]) || (tbiArray[elem][2]!=tbi[2]) || (tbiArray[elem][3]!=tbi[3])) { ADD_FAILURE() << "non consistent indices between getTetrahedronBezierIndexArray() and getTetrahedronBezierIndex(). Got : "<<tbiArray[elem] <<" versus "<<tbi << std::endl; return false; } // check that getLocationFromGlobalIndex is consistent with container->getLocationFromGlobalIndex(globalIndex,location,elementIndex,elementOffset); if (elem<4) { if ((location!=BezierTetrahedronSetTopologyContainer::POINT) || (elementIndex!=container->getTetrahedron(nTetras)[elem]) || (elementOffset!=0)) { ADD_FAILURE() << "non consistent indices given by getLocationFromGlobalIndex() for global index : "<<globalIndex <<std::endl; return false; } } else if (elem<(size_t)(4+6*(degree-1))){ if ((location!=BezierTetrahedronSetTopologyContainer::EDGE) || (elementIndex!=container->getEdgesInTetrahedron(nTetras)[(elem-4)/(degree-1)])) { ADD_FAILURE() << "non consistent indices given by getLocationFromGlobalIndex() for global index : "<<globalIndex <<std::endl; return false; } } else if (elem<(size_t)(4+6*(degree-1)+2*(degree-1)*(degree-2))){ size_t nbPointPerEdge=(degree-1)*(degree-2)/2; size_t val=(elem-4-6*(degree-1))/(nbPointPerEdge); if ((location!=BezierTetrahedronSetTopologyContainer::TRIANGLE) || (elementIndex!=container->getTrianglesInTetrahedron(nTetras)[val])) { ADD_FAILURE() << "non consistent indices given by getLocationFromGlobalIndex() for global index : "<<globalIndex <<std::endl; return false; } } } } return( true); }