Laboratoire RDP » Biophysics Team » AFM_plane

#include <sofa/simulation/graph/DAGSimulation.h>

#include <plugins/SofaPython/SceneLoaderPY.h>

#include <sofa/helper/system/PluginManager.h>

#include <sofa/defaulttype/Mat.h>

#include <SofaBaseLinearSolver/FullVector.h>

#include <sofa/simulation/common/MechanicalVisitor.h>

#include <SofaComponentMain/init.h>

#include <../MyDefaultContactManager.h>

#include <sofa/core/objectmodel/BaseObject.h>

#include <SofaBoundaryCondition/ConstantForceField.h>

#include <sofa/simulation/common/FindByTypeVisitor.h>

#include <SofaBaseMechanics/MechanicalObject.h>

#include <sofa/helper/accessor.h>

#include <sofa/defaulttype/DataTypeInfo.h>

#include <SofaBoundaryCondition/ProjectToLineConstraint.h>

#include <cmath>

#include <cstdlib>

#include <fstream>

#include <vector>

using namespace sofa;

using namespace simulation;

typedef sofa::component::linearsolver::FullVector<double> FullVector;

typedef defaulttype::Vec3dTypes::Deriv Deriv;

typedef defaulttype::Vec3d myVec3d;

 /// return the maximum difference between corresponding entries, or the infinity if the vectors have different sizes

template< typename Vector1, typename Vector2>

static double vectorCompare( const Vector1& m1, const Vector2& m2 )

{

    if( m1.size()!=m2.size() ) {

         printf("Comparison between vectors of different sizes\n");

                 std::fflush(stdout);

        return std::numeric_limits<double>::infinity();

        }

    double result = 0;

    for( unsigned i=0; i<m1.size(); i++ ){

        double diff = fabs(m1.element(i)-m2.element(i));

        if( diff>result  ) result=diff;

    }

    return result;

}

 /** fill the vector with the positions.

      @param x the position vector

      */

void getAssembledPositionVector( FullVector* x, unsigned xSize, 

    std::vector<component::container::MechanicalObject<defaulttype::Vec3dTypes> *> DOFs )

{

    x->resize(xSize);

        unsigned offset = 0;

        for (unsigned i = 0; i < DOFs.size(); i++){

        helper::ReadAccessor< Data<defaulttype::Vec3dTypes::VecCoord> > vSrc =  DOFs[i]->readPositions();

                for (unsigned j=0; j < vSrc.size(); j++){

                        double tmp = 0.0;

            tmp = vSrc[j][0];

            //            defaulttype::Vec3dTypes::Coord getValue(vSrc[j], 0, tmp);

            x->set(offset + 3*j,tmp);

           // DataTypeInfo<defaulttype::Vec3dTypes::Coord>::getValue(vSrc[j], 1, tmp);

            tmp = vSrc[j][1];

            x->set(offset + 3*j+1,tmp);

            //DataTypeInfo<defaulttype::Vec3dTypes::Coord>::getValue(vSrc[j], 2, tmp);

            tmp = vSrc[j][2];

            x->set(offset + 3*j+2,tmp);

                }

                offset += DOFs[i]->getSize() * 3;

        }

}

    /** fill the vector with the velocities.

      @param v the velocity vector

      */

void getAssembledVelocityVector( FullVector* v, unsigned vSize, 

    std::vector<component::container::MechanicalObject<defaulttype::Vec3dTypes> *> DOFs )

{

    v->resize(vSize);

        unsigned offset = 0;

        for (unsigned i = 0; i < DOFs.size(); i++){

        helper::ReadAccessor< Data<defaulttype::Vec3dTypes::VecDeriv> > vSrc =  DOFs[i]->readVelocities();

                for (unsigned j=0; j < vSrc.size(); j++){

                        double tmp = 0.0;

            //DataTypeInfo<defaulttype::Vec3dTypes::Deriv>::getValue(vSrc[j], 0, tmp);

            tmp = vSrc[j][0];

            v->set(offset + 3*j,tmp);

            //DataTypeInfo<defaulttype::Vec3dTypes::Deriv>::getValue(vSrc[j], 1, tmp);

            tmp = vSrc[j][1];

            v->set(offset + 3*j+1,tmp);

            //DataTypeInfo<defaulttype::Vec3dTypes::Deriv>::getValue(vSrc[j], 2, tmp);

            tmp = vSrc[j][2];

            v->set(offset + 3*j+2,tmp);

                }

                offset += DOFs[i]->getSize() * 3;

        }

}

int main()

{

    //the file in which the resulting curve will be put

    std::ofstream fileResult("./AFM-results.txt");

    fileResult << "### first the force then the depth ###\n\n" ;

        //load the required plugins

    std::string pluginPath = "Flexible";

    std::string pluginPath1 = "AFM_plane";

    sofa::helper::system::PluginRepository.addLastPath("../../../../build_release/lib/");

    if (sofa::helper::system::PluginManager::getInstance().loadPlugin(pluginPath) &&

            sofa::helper::system::PluginManager::getInstance().loadPlugin(pluginPath1))

        sofa::helper::system::PluginManager::getInstance().init();

        unsigned xsize; ///< size of assembled position vector, 

    unsigned vsize; ///< size of assembled velocity vector, 

    // loading the scene which is in the file examples/AFM-plane.py

        setSimulation(new simulation::graph::DAGSimulation());

    sofa::component::init();

    Node::SPtr root = getSimulation()->createNewGraph("root");

        SceneLoaderPY LoaderPy;  

    root = LoaderPy.load("./AFM-plane.py");

        getSimulation()->init(root.get());

        //we take the direction of the force 

    sofa::component::forcefield::ConstantForceField<defaulttype::Vec3dTypes>::SPtr CFF;

        root->getContext()->get(CFF, sofa::core::objectmodel::BaseContext::SearchRoot);

        Deriv forceInit = CFF->force.getValue();

        Deriv forceActu;

        forceActu[0] = 0;

        forceActu[1] = 0;

        forceActu[2] = 0;

        CFF->force = forceActu;

        // first find the size of the DOFs

        core::ExecParams* params = core::ExecParams::defaultInstance();

    FindByTypeVisitor<component::container::MechanicalObject<defaulttype::Vec3dTypes> > FBTVisitor(params);

        root->execute(FBTVisitor);

    std::vector<component::container::MechanicalObject<defaulttype::Vec3dTypes> *> allPoints;

        allPoints = FBTVisitor.found;

    std::vector<component::container::MechanicalObject<defaulttype::Vec3dTypes> *> DOFs, mObject;

        for (unsigned i=0; i<allPoints.size();i++)

        if (allPoints[i]->getName() == "DOFs" || allPoints[i]->getName() == "indentor" )

                        DOFs.push_back(allPoints[i]);

        for (unsigned i=0; i<allPoints.size();i++)

                if (allPoints[i]->getName() == "DOFs")

                        mObject.push_back(allPoints[i]);

        unsigned xInitSize = mObject[0]->getSize();

        xsize = DOFs[0]->getSize() * DOFs[0]->getCoordDimension() +  DOFs[1]->getSize() * DOFs[1]->getCoordDimension();

        vsize = DOFs[0]->getMatrixSize() + DOFs[1]->getMatrixSize(); 

        FullVector x0, x1, v0, v1, xinit, xend;

    getAssembledPositionVector(&x0, xsize, DOFs); // cerr<<"My_test, initial positions : " << x0 << endl;

    getAssembledVelocityVector(&v0, vsize, DOFs); // cerr<<"My_test, initial velocities: " << v0 << endl;

    double dx, dv;

    unsigned n=0;

    const double  precision = 1.e-4;

        unsigned const nmax = 1000;

    // first loop to stabilize the plane growth under pressure

    do {

        getSimulation()->animate(root.get(),0.01);

        getAssembledPositionVector(&x1, xsize, DOFs); // cerr<<"My_test, new positions : " << x1 << endl;

        getAssembledVelocityVector(&v1, vsize, DOFs); // cerr<<"My_test, new velocities: " << v1 << endl;

        dx = vectorCompare(x0,x1);

        dv = vectorCompare(v0,v1);

        x0 = x1;

        v0 = v1;

        n++;

        } while((dx>precision || dv>precision) && n < nmax);           // 

    printf("\n %i iterations to stabilize the plane \n",n);

        std::fflush(stdout);

        getAssembledPositionVector(&xinit, xInitSize*3, mObject);

        //second loop : to detect a collision

        //we place the indentor first and calculate the force

        forceInit[0] = 0;

        forceInit[1] = 0;

    forceInit[2] = -0.1;

        //we put the position of indentor

    component::container::MechanicalObject<defaulttype::Vec3dTypes> *indentor = NULL ;

        if (DOFs[0]->getName()=="DOFs")

                indentor = DOFs[1];

        else indentor = DOFs[0];

        helper::vector<double> centerPos;

    centerPos.push_back(0.5);

    centerPos.push_back(0.5);

    centerPos.push_back(2);

        indentor->forcePointPosition(0,centerPos);

        myVec3d indentPos;

        myVec3d indentForce;

    indentPos.set(0.5 ,0.5, 2);

    indentForce.set(0,0,1);

    component::projectiveconstraintset::ProjectToLineConstraint<defaulttype::Vec3dTypes>::SPtr constraint;

        root->getContext()->get(constraint, sofa::core::objectmodel::BaseContext::SearchRoot);

        constraint->f_origin.setValue(indentPos);

        constraint->f_direction.setValue(indentForce);

    forceActu[2] = -0.1;

        CFF->force = forceActu;

        component::collision::MyDefaultContactManager::SPtr MDCM;

        root->getContext()->get(MDCM, sofa::core::objectmodel::BaseContext::SearchRoot);

        while (!MDCM->CollisionDetected())

        {

                getSimulation()->animate(root.get(),0.01);

                root->getContext()->get(MDCM, sofa::core::objectmodel::BaseContext::SearchRoot);

        }

        //third loop : until the end we increment the force

        n = 0;

        unsigned compteur = 0;

        getAssembledPositionVector(&x0, xsize, DOFs); // cerr<<"My_test, initial positions : " << x0 << endl;

    getAssembledVelocityVector(&v0, vsize, DOFs); // cerr<<"My_test, initial velocities: " << v0 << endl;

    while (forceActu.norm() < 150 &&  MDCM->getDepth() < 0.15 ){

                n = 0;

                fileResult << forceActu.norm() <<" " ;

                printf("new force %f : ",forceActu.norm());

                std::fflush(stdout);

                do {

                        getSimulation()->animate(root.get(),0.01);

                getAssembledPositionVector(&x1, xsize, DOFs); // cerr<<"My_test, new positions : " << x1 << endl;

                    getAssembledVelocityVector(&v1, vsize, DOFs); // cerr<<"My_test, new velocities: " << v1 << endl;

                        dx = vectorCompare(x0,x1);

                        dv = vectorCompare(v0,v1);

                        x0 = x1;

                    v0 = v1;

                        n++;

                } while((dx>precision || dv>precision) && n < nmax);           // 

        printf("%i iterations, depth: %f \n",n,MDCM->getDepth());

                std::fflush(stdout);

                compteur++;

                fileResult << MDCM->getDepth() << std::endl;

                forceActu += forceInit;

                CFF->force = forceActu;

        }

        //forth loop to decrement the force 

    /*while (forceActu.norm() > 0.1){

                n = 0;

                fileResult << forceActu.norm() <<" " ;

                printf("new force %f : ",forceActu.norm());

                std::fflush(stdout);

                do {

                        getSimulation()->animate(root.get(),0.01);

                getAssembledPositionVector(&x1, xsize, DOFs); // cerr<<"My_test, new positions : " << x1 << endl;

                    getAssembledVelocityVector(&v1, vsize, DOFs); // cerr<<"My_test, new velocities: " << v1 << endl;

                        dx = vectorCompare(x0,x1);

                        dv = vectorCompare(v0,v1);

                        x0 = x1;

                    v0 = v1;

                        n++;

                } while((dx>precision || dv>precision) && n < nmax);           // 

        printf("%i iterations, depth : %f \n",n,MDCM->getDepth() );

                std::fflush(stdout);

                compteur++;

                fileResult << MDCM->getDepth() << std::endl;

                forceActu -= forceInit;

                CFF->force = forceActu;

        }

        */

        return 0;

}