/// 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);
}
