bool testBezierTetrahedronGeometry() { BezierTetrahedronSetTopologyContainer *container=root->get<BezierTetrahedronSetTopologyContainer>(root->SearchDown); typename MechanicalObject::SPtr dofs = root->get<MechanicalObject>(std::string("BezierTetrahedronTopology/")); typename MechanicalObject::WriteVecCoord coords = dofs->writePositions(); size_t i,j; BezierDegreeType degree=container->getDegree(); sofa::component::topology::VecPointID indexArray; sofa::helper::vector<TetrahedronBezierIndex> tbiArray=container->getTetrahedronBezierIndexArray(); for ( i = 0; i<container->getNumberOfTetrahedra(); i++) { indexArray.clear(); container->getGlobalIndexArrayOfBezierPointsInTetrahedron(i, indexArray); for (j=0;j<tbiArray.size();++j) { if (j>=4) { // test if the position is correct Coord pos=coords[indexArray[0]]*(Real)tbiArray[j][0]/degree+coords[indexArray[1]]*(Real)tbiArray[j][1]/degree+coords[indexArray[2]]*(Real)tbiArray[j][2]/degree+coords[indexArray[3]]*(Real)tbiArray[j][3]/degree; if ((pos-coords[indexArray[j]]).norm()>1e-5) { ADD_FAILURE() << "Wrong control point position in tetrahedron no : "<<i <<" for point of local index " <<j << " Got point position="<<coords[indexArray[j]]<<" instead of "<<pos<<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; 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 testBezierTetrahedronMass() { BezierTetrahedronSetTopologyContainer *container=root->get<BezierTetrahedronSetTopologyContainer>(root->SearchDown); BezierTetrahedronSetGeometryAlgorithms *geo=root->get<BezierTetrahedronSetGeometryAlgorithms>(root->SearchDown); typename MechanicalObject::SPtr dofs = root->get<MechanicalObject>(std::string("BezierTetrahedronTopology/")); typename MechanicalObject::WriteVecCoord coords = dofs->writePositions(); MeshMatrixMass *mass=root->get<MeshMatrixMass>(root->SearchDown); const sofa::helper::vector<typename MeshMatrixMass::MassVector> & mv=mass->tetrahedronMassInfo.getValue(); const sofa::helper::vector<typename MeshMatrixMass::MassType> &ma =mass->vertexMassInfo.getValue(); size_t i,j,k,rank; BezierDegreeType degree=container->getDegree(); Real tetraVol1,tetraVol2,totalVol1,totalVol2; sofa::helper::vector<TetrahedronBezierIndex> tbiArray=container->getTetrahedronBezierIndexArray(); size_t nbControlPoints=(degree+1)*(degree+2)*(degree+3)/6; totalVol1=0; for ( i = 0; i<container->getNumberOfTetrahedra(); i++) { // const BezierTetrahedronSetTopologyContainer::VecPointID &indexArray=container->getGlobalIndexArrayOfBezierPoints(i); /// get the volume of the tetrahedron tetraVol1=geo->computeTetrahedronVolume(i); tetraVol2=0; // compute the total volume totalVol1+=tetraVol1; /// check that the sum of the volume matrix elements is equal to the volume of the tetrahedron for (rank=0,j=0;j<nbControlPoints;j++) { for (k=j;k<nbControlPoints;k++,rank++) { if (k==j) // add diagonal term tetraVol2+=mv[i][rank]; else // add 2 times off-diagonal term tetraVol2+=2*mv[i][rank]; } } if (fabs(tetraVol1-tetraVol2)>1e-5) { ADD_FAILURE() << "Wrong mass matrix in tetrahedron no : "<<i << " Got total mass="<<tetraVol2<<" instead of "<<tetraVol1<<std::endl; return false; } } // compute totalVol2 as the total of the lumped volume totalVol2=0; for ( i = 0; i<ma.size(); i++) { totalVol2+=ma[i]; } if (fabs(totalVol1-totalVol2)>1e-5) { ADD_FAILURE() << "Wrong total vertex mass value." << " Got total vertex mass="<<totalVol2<<" instead of "<<totalVol1<<std::endl; return false; } return true; }
/// 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; }
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; }
/// 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); }
/// 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); }
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; }
/// 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 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; }