/// 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;
}
bool test_projectPosition()
{
projection->projectPosition(core::MechanicalParams::defaultInstance(), *dofs->write(core::VecCoordId::position()));
typename MechanicalObject::ReadVecCoord x = dofs->readPositions();
Coord target0(CPos(0.5,0.5,0.5), CRot(0, 0.19509, 0, 0.980785));
Coord target1(CPos(0.5,1.5,0.5), CRot(0.69352, 0.13795, -0.13795, 0.69352));
bool succeed = true;
if( !Sofa_test<typename _DataTypes::Real>::isSmall((x[0].getCenter() - target0.getCenter()).norm(),100) ||
!Sofa_test<typename _DataTypes::Real>::isSmall((x[1].getCenter() - target1.getCenter()).norm(),100) )
{
succeed = false;
ADD_FAILURE() << "Position of constrained bones is wrong: "<<x[0].getCenter()<<", "<<x[1].getCenter();
}
if( !(x[0].getOrientation() == target0.getOrientation()) ||
!(x[1].getOrientation() == target1.getOrientation()) )
{
succeed = false;
ADD_FAILURE() << "Rotation of constrained bones is wrong: "<<x[0].getOrientation()<<", "<<x[1].getOrientation();;
}
return succeed;
}