/// 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_projectVelocity()
{
projection->projectVelocity(core::MechanicalParams::defaultInstance(), *dofs->write(core::VecDerivId::velocity()));
typename MechanicalObject::ReadVecDeriv x = dofs->readVelocities();
bool succeed = true;
Deriv target(CPos(1,1,1), typename Deriv::Rot(0,0.785397,0));
if(!(x[0]==target) || !(x[1]==target))
{
succeed = false;
ADD_FAILURE() << "velocities of constrained bones is wrong: "<<x[0]<<", "<<x[1]<<", expected: "<<target;
}
return succeed;
}
bool test_projectVelocity()
{
VecDeriv vprev(numNodes);
typename MechanicalObject::WriteVecDeriv v = dofs->writeVelocities();
for (unsigned i=0; i<numNodes; i++){
vprev[i] = v[i] = CPos(i,0,0);
}
// cerr<<"test_projectVelocity, v before = " << v << endl;
projection->projectVelocity(core::MechanicalParams::defaultInstance(), *dofs->write(core::VecDerivId::velocity()) );
// cerr<<"test_projectVelocity, v after = " << v << endl;
bool succeed=true;
typename Indices::const_iterator it = indices.begin(); // must be sorted
for(unsigned i=0; i<numNodes; i++ )
{
if ((it!=indices.end()) && ( i==*it )) // constrained particle
{
CPos crossprod = v[i].cross(direction); // should be parallel
Real scal = crossprod.norm(); // null if v is ok
// cerr<<"scal = "<< scal << endl;
if( !Sofa_test<typename _DataTypes::Real>::isSmall(scal,100) ){
succeed = false;
ADD_FAILURE() << "Velocity of constrained particle " << i << " is wrong: " << v[i] ;
}
it++;
}
else // unconstrained particle: check that it has not changed
{
CPos dv = v[i]-vprev[i];
Real scal = dv*dv;
// cerr<<"scal gap = "<< scal << endl;
if( !Sofa_test<typename _DataTypes::Real>::isSmall(scal,100) ){
succeed = false;
ADD_FAILURE() << "Velocity of unconstrained particle " << i << " is wrong: " << v[i] ;
}
}
}
return succeed;
}
bool test_projectPosition()
{
VecCoord xprev(numNodes);
typename MechanicalObject::WriteVecCoord x = dofs->writePositions();
for (unsigned i=0; i<numNodes; i++){
xprev[i] = x[i] = CPos(i,0,0);
}
// cerr<<"test_projectPosition, x before = " << x << endl;
projection->projectPosition(core::MechanicalParams::defaultInstance(), *dofs->write(core::VecCoordId::position()) );
// cerr<<"test_projectPosition, x after = " << x << endl;
bool succeed=true;
typename Indices::const_iterator it = indices.begin(); // must be sorted
for(unsigned i=0; i<numNodes; i++ )
{
if ((it!=indices.end()) && ( i==*it )) // constrained particle
{
CPos crossprod = (x[i]-origin).cross(direction); // should be parallel
Real scal = crossprod*crossprod; // null if x is on the line
// cerr<<"scal = "<< scal << endl;
if( !Sofa_test<typename _DataTypes::Real>::isSmall(scal,100) ){
succeed = false;
ADD_FAILURE() << "Position of constrained particle " << i << " is wrong: " << x[i] ;
}
it++;
}
else // unconstrained particle: check that it has not changed
{
CPos dx = x[i]-xprev[i];
Real scal = dx*dx;
// cerr<<"scal gap = "<< scal << endl;
if( !Sofa_test<typename _DataTypes::Real>::isSmall(scal,100) ){
succeed = false;
ADD_FAILURE() << "Position of unconstrained particle " << i << " is wrong: " << x[i] ;
}
}
}
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;
}
