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; }
/** 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 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; }
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 convergenceAccuracy, double diffMaxBetweenSimulatedAndTheoreticalPosition) { // Init simulation sofa::simulation::getSimulation()->init(root.get()); // Compute the theoretical final positions VecCoord finalPos; typename AffineMovementConstraint::SPtr affineConstraint = root->get<AffineMovementConstraint>(root->SearchDown); typename MechanicalObject::SPtr dofs = root->get<MechanicalObject>(root->SearchDown); typename MechanicalObject::WriteVecCoord x = dofs->writePositions(); affineConstraint->getFinalPositions( finalPos,*dofs->write(core::VecCoordId::position()) ); // Set random rotation and translation for affine constraint randomGenerator.initSeed(seed); this->SetRandomTestedRotationAndTranslation(seed); // Set data values of affine movement constraint affineConstraint->m_rotation.setValue(testedRotation); affineConstraint->m_translation.setValue(testedTranslation); // 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]-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; } } return succeed; }