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