C++ typenamemechanicalobject::SPtr类代码示例

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

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

    }

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

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

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