C++ SPtr::write方法代码示例

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