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