Laboratoire RDP » Biophysics Team » AFM_plane

/******************************************************************************

*       SOFA, Simulation Open-Framework Architecture, version 1.0 RC 1        *

*                (c) 2006-2011 MGH, INRIA, USTL, UJF, CNRS                    *

*                                                                             *

* This library is free software; you can redistribute it and/or modify it     *

* under the terms of the GNU Lesser General Public License as published by    *

* the Free Software Foundation; either version 2.1 of the License, or (at     *

* your option) any later version.                                             *

*                                                                             *

* This library is distributed in the hope that it will be useful, but WITHOUT *

* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *

* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *

* for more details.                                                           *

*                                                                             *

* You should have received a copy of the GNU Lesser General Public License    *

* along with this library; if not, write to the Free Software Foundation,     *

* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *

*******************************************************************************

*                               SOFA :: Modules                               *

*                                                                             *

* Authors: The SOFA Team and external contributors (see Authors.txt)          *

*                                                                             *

* Contact information: contact@sofa-framework.org                             *

******************************************************************************/

#ifndef SOFA_COMPONENT_COLLISION_MYDEFAULTCONTACTMANAGER_H

#define SOFA_COMPONENT_COLLISION_MYDEFAULTCONTACTMANAGER_H

#include <sofa/core/collision/ContactManager.h>

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

#include <sofa/component/component.h>

#include <sofa/helper/OptionsGroup.h>

#include <sofa/helper/map_ptr_stable_compare.h>

#include <vector>

#include "initAFM_plane.h"

namespace sofa

{

namespace component

{

namespace collision

{

class SOFA_AFM_plane_API MyDefaultContactManager : public core::collision::ContactManager

{

public :

    SOFA_CLASS(MyDefaultContactManager,sofa::core::collision::ContactManager);

protected:

    typedef sofa::helper::map_ptr_stable_compare<std::pair<core::CollisionModel*,core::CollisionModel*>,core::collision::Contact::SPtr> ContactMap;

    ContactMap contactMap;

    //typedef struct MaProfondeur{bool aTouche; double origine[3];} Profondeur;

    void cleanup();

public:

    //typedef struct MaProfondeur{bool aTouche; double origine[3];} Profondeur;

    Data<double> deepInMeristem;

    Data<bool> aTouche;

    Data<sofa::helper::vector<double> > origine;

    Data<sofa::helper::OptionsGroup> response;

    Data<std::string> responseParams;

protected:

    MyDefaultContactManager();

    ~MyDefaultContactManager();

    void setContactTags(core::CollisionModel* model1, core::CollisionModel* model2, core::collision::Contact::SPtr contact);

    //double deepInMeristem;

    //Profondeur penetration;

public:

//	bool CollisionDetected(){return penetration.aTouche;}

//	double getDepth(){return deepInMeristem;}

    bool CollisionDetected(){return aTouche.getValue();}

    double getDepth(){return deepInMeristem.getValue();}

    /// outputsVec fixes the reproducibility problems by storing contacts in the collision detection saved order

    /// if not given, it is still working but with eventual reproducibility problems

    void createContacts(const DetectionOutputMap& outputs);

    void init();

    void draw(const core::visual::VisualParams* vparams);

    template<class T>

    static typename T::SPtr create(T*, core::objectmodel::BaseContext* context, core::objectmodel::BaseObjectDescription* arg)

    {

        typename T::SPtr obj = sofa::core::objectmodel::New<T>();

        if (context)

        {

            context->addObject(obj);

            core::collision::Pipeline *pipeline = static_cast<simulation::Node*>(context)->collisionPipeline;

            sofa::helper::OptionsGroup options = initializeResponseOptions(pipeline);

            obj->response.setValue(options);

        }

        if (arg)

            obj->parse(arg);

        return obj;

    }

    virtual std::string getContactResponse(core::CollisionModel* model1, core::CollisionModel* model2);

    /// virtual methods used for cleaning the pipeline after a dynamic graph node deletion.

    /**

     * Contacts can be attached to a deleted node and their deletion is a problem for the pipeline.

     * @param c is the list of deleted contacts.

     */

    virtual void removeContacts(const ContactVector &/*c*/);

    void setDefaultResponseType(const std::string &responseT)

    {

        if (response.getValue().size() == 0)

        {

            helper::vector<std::string> listResponse(1,responseT);

            sofa::helper::OptionsGroup responseOptions(listResponse);

            response.setValue(responseOptions);

        }

        else

        {

            sofa::helper::OptionsGroup* options = response.beginEdit();

            options->setSelectedItem(responseT);

            response.endEdit();

        }

    }

    std::string getDefaultResponseType() const { return response.getValue().getSelectedItem(); }

protected:

    static sofa::helper::OptionsGroup initializeResponseOptions(core::collision::Pipeline *pipeline);

    std::map<Instance,ContactMap> storedContactMap;

    virtual void changeInstance(Instance inst)

    {

        core::collision::ContactManager::changeInstance(inst);

        storedContactMap[instance].swap(contactMap);

        contactMap.swap(storedContactMap[inst]);

    }

    // count failure messages, so we don't continuously repeat them

    std::map<std::pair<std::string,std::pair<std::string,std::string> >, int> errorMsgCount;

};

} // namespace collision

} // namespace component

} // namespace sofa

#endif

/******************************************************************************

*       SOFA, Simulation Open-Framework Architecture, version 1.0 RC 1        *

*                (c) 2006-2011 MGH, INRIA, USTL, UJF, CNRS                    *

*                                                                             *

* This library is free software; you can redistribute it and/or modify it     *

* under the terms of the GNU Lesser General Public License as published by    *

* the Free Software Foundation; either version 2.1 of the License, or (at     *

* your option) any later version.                                             *

*                                                                             *

* This library is distributed in the hope that it will be useful, but WITHOUT *

* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *

* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *

* for more details.                                                           *

*                                                                             *

* You should have received a copy of the GNU Lesser General Public License    *

* along with this library; if not, write to the Free Software Foundation,     *

* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *

*******************************************************************************

*                               SOFA :: Plugins                               *

*                                                                             *

* Authors: The SOFA Team and external contributors (see Authors.txt)          *

*                                                                             *

* Contact information: contact@sofa-framework.org                             *

******************************************************************************/

#include "initAFM_plane.h"

namespace sofa

{

namespace component

{

	//Here are just several convenient functions to help user to know what contains the plugin

	extern "C" {

                SOFA_AFM_plane_API void initExternalModule();

                SOFA_AFM_plane_API const char* getModuleName();

                SOFA_AFM_plane_API const char* getModuleVersion();

                SOFA_AFM_plane_API const char* getModuleLicense();

                SOFA_AFM_plane_API const char* getModuleDescription();

                SOFA_AFM_plane_API const char* getModuleComponentList();

	}

	void initExternalModule()

	{

		static bool first = true;

		if (first)

		{

			first = false;

		}

	}

	const char* getModuleName()

	{

	  return "AFM_plane";

	}

	const char* getModuleVersion()

	{

		return "0.2";

	}

	const char* getModuleLicense()

	{

		return "LGPL";

	}

	const char* getModuleDescription()

	{

		return "TODO: replace this with the description of your plugin";

	}

	const char* getModuleComponentList()

	{

	  /// string containing the names of the classes provided by the plugin

	  return "";

	  //return "MyMappingPendulumInPlane, MyBehaviorModel, MyProjectiveConstraintSet";

	}

} 

} 

/// Use the SOFA_LINK_CLASS macro for each class, to enable linking on all platforms

SOFA_LINK_CLASS(MyDefaultContactManager)

Author : R. Malgat, F. Faure, A. Boudaoud.

This plugin works with the Software Open Framework Architecture (SOFA).

It is a simulated Atomic Force Microscope acting on a plane.

To get an idea of the scene, you can launch Sofa on the example examples/plane.py. The scene appearing should be like the image plane.png and gives you an overview of the scene described.

Then to use the AFM and trace curves, simply install the folder as a SOFA plugin. Go to the examples folder and launch the application called AFM which should be under "YourPathToSofa/build_release/bin/AFM". You should get a resulting curve called AFM-results.txt, where, for each line, the force and the corresponding depth is written.

Feel free to change the parameters : pressure, elastic modulus, poisson ratio... 

If Sofa does not find the path to the plugin when you launch the AFM application, you may have to change the paths in the file AFM/AFM.cpp line 112 and eventually line 129. 

/******************************************************************************

*       SOFA, Simulation Open-Framework Architecture, version 1.0 RC 1        *

*                (c) 2006-2011 MGH, INRIA, USTL, UJF, CNRS                    *

*                                                                             *

* This library is free software; you can redistribute it and/or modify it     *

* under the terms of the GNU Lesser General Public License as published by    *

* the Free Software Foundation; either version 2.1 of the License, or (at     *

* your option) any later version.                                             *

*                                                                             *

* This library is distributed in the hope that it will be useful, but WITHOUT *

* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *

* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *

* for more details.                                                           *

*                                                                             *

* You should have received a copy of the GNU Lesser General Public License    *

* along with this library; if not, write to the Free Software Foundation,     *

* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *

*******************************************************************************

*                               SOFA :: Plugins                               *

*                                                                             *

* Authors: The SOFA Team and external contributors (see Authors.txt)          *

*                                                                             *

* Contact information: contact@sofa-framework.org                             *

******************************************************************************/

#ifndef INITAFM_plane_H

#define INITAFM_plane_H

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

#ifdef SOFA_BUILD_AFM_plane

#define SOFA_AFM_plane_API SOFA_EXPORT_DYNAMIC_LIBRARY

#else

#define SOFA_AFM_plane_API  SOFA_IMPORT_DYNAMIC_LIBRARY

#endif

#endif // INITAFM_plane_H

include(${SOFA_CMAKE_DIR}/preProject.cmake)

##plugin external dependencies

#find_package( REQUIRED)

#include_directories(${})

set(HEADER_FILES

initAFM_plane.h

MyDefaultContactManager.h

	)

set(SOURCE_FILES

initAFM_plane.cpp

MyDefaultContactManager.cpp

	)

set(README_FILES

	)

add_library(${PROJECT_NAME} SHARED ${HEADER_FILES} ${MOC_FILES} ${SOURCE_FILES} ${README_FILES})

AddCompilerDefinitions("SOFA_BUILD_AFM-plane")

AddLinkerDependencies(SofaGuiQt)

include(${SOFA_CMAKE_DIR}/postProject.cmake)

### first the force then the depth ###

0.1 0.0566023

0.2 0.0629732

0.3 0.0707535

0.4 0.0788602

0.5 0.0870693

0.6 0.0951414

0.7 0.103042

0.8 0.110816

0.9 0.118506

1 0.126105

1.1 0.13365

1.2 0.141175

1.3 0.148656

1.4 0.156105

import Sofa

poissonRatio = 0.49

youngModulus = 100

pressure = 5

indentorRadius = 0.3

def createScene(node): 

    print 'Creation of Sofa SceneGraph'    

    node.gravity = [0,0,0]

    node.createObject('RequiredPlugin', pluginName="Flexible") 

    node.createObject('VisualStyle', displayFlags = 'showVisual hideBehavior showCollisionModels')

    node.createObject('DefaultPipeline', name="DefaultCollisionPipeline",  verbose="0",  draw="0",  depth="6")

    node.createObject('BruteForceDetection', name="Detection")

    node.createObject('MinProximityIntersection', name="Proximity",  alarmDistance="0.1",  contactDistance="0.0")

    node.createObject('DefaultContactManager', name="Response",  response="default", responseParams="")

    node.createObject('TreeCollisionGroupManager', name="Group")

    node.createObject('CGLinearSolver', template="GraphScattered",iterations=500,threshold=1e-10,tolerance=1e-10)

    node.createObject('EulerImplicitSolver', rayleighStiffness=0.5,rayleighMass=0.5)

    node.createObject('GridMeshCreator', name='Grid', resolution="10 10", triangulate=1)

    node.createObject('MeshTopology', name='Topology',src="@Grid")

    node.createObject('MechanicalObject', name='DOFs', template="Vec3d")    

    node.createObject('UniformMass',name='mass', template='Vec3d')

    node.createObject('FixedConstraint',template="Vec3d", indices="0 9 90 99" )

    ## Pressure

    node.createObject("SurfacePressureForceField",  pressure=pressure)

    ## Deformation, Stress and Strain

    node.createObject('BarycentricShapeFunction', template="ShapeFunctiond")

    deformation = node.createChild("deformation")

    deformation.createObject('TopologyGaussPointSampler',name='sampler', inPosition='@../Topology.position' , showSamples="false", method="0", order="1" )

    deformation.createObject('MechanicalObject', name='deform', template='F321')

    deformation.createObject('LinearMapping', template='Vec3d,F321')

    stressStrain = deformation.createChild('stressStrain')

    stressStrain.createObject('MechanicalObject', name='Strain',template='E321')

    stressStrain.createObject('CorotationalStrainMapping', template="F321,E321")

    stressStrain.createObject('HookeForceField', template="E321", youngModulus=youngModulus,poissonRatio=poissonRatio, viscosity=0  )

    ## Collision

    node.createObject('TriangleModel',contactStiffness=1000)

    node.createObject('LineModel',contactStiffness=1000)

    node.createObject('PointModel',contactStiffness=1000)

    Visu = node.createChild('Visu')

    Visu.createObject('OglModel')

    Visu.createObject('IdentityMapping', template='Vec3d,ExtVec3f')

    ## indentor

    indentor = node.createChild('indentor')

    indentor.createObject('MechanicalObject',name='indentor', position="0.5 0.5 2")

    indentor.createObject('UniformMass')

    indentor.createObject('ConstantForceField', points=0, force="0 0 -1")

    indentor.createObject('TSphereModel', radius= indentorRadius, ContactStiffness=1000) 

    indentor.createObject('ProjectToLineConstraint',endTime='-1', indices=0, origin="0.5 0.5 2", direction="0 0 1") 

    indentor.createObject('PointConstraint', endTime=3, indices=0)                     

import Sofa

poissonRatio = 0.49

youngModulus = 100

pressure = 5

indentorRadius = 0.3

def createScene(node): 

    print 'Creation of Sofa SceneGraph'    

    node.gravity = [0,0,0]

    node.createObject('RequiredPlugin', pluginName="Flexible") 

    node.createObject('RequiredPlugin', pluginName="AFM_plane")

    node.createObject('VisualStyle', displayFlags = 'showVisual hideBehavior showCollisionModels')

    node.createObject('DefaultPipeline', name="DefaultCollisionPipeline",  verbose="0",  draw="0",  depth="6")

    node.createObject('BruteForceDetection', name="Detection")

    node.createObject('MinProximityIntersection', name="Proximity",  alarmDistance="0.05",  contactDistance="0.0")

    node.createObject('MyDefaultContactManager', name="Response",  response="default", responseParams="")

    node.createObject('TreeCollisionGroupManager', name="Group")

    node.createObject('CGLinearSolver', template="GraphScattered",iterations=500,threshold=1e-10,tolerance=1e-10)

    node.createObject('EulerImplicitSolver', rayleighStiffness=0.5,rayleighMass=0.5)

    node.createObject('GridMeshCreator', name='Grid', resolution="10 10", triangulate=1)

    node.createObject('MeshTopology', name='Topology',src="@Grid")

    node.createObject('MechanicalObject', name='DOFs', template="Vec3d")    

    node.createObject('UniformMass',name='mass', template='Vec3d')

    node.createObject('FixedConstraint',template="Vec3d", indices="0 9 90 99" )

    ## Pressure

    node.createObject("SurfacePressureForceField",  pressure=pressure)

    ## Deformation, Stress and Strain

    node.createObject('BarycentricShapeFunction', template="ShapeFunctiond")

    deformation = node.createChild("deformation")

    deformation.createObject('TopologyGaussPointSampler',name='sampler', inPosition='@../Topology.position' , showSamples="false", method="0", order="1" )

    deformation.createObject('MechanicalObject', name='deform', template='F321')

    deformation.createObject('LinearMapping', template='Vec3d,F321')

    stressStrain = deformation.createChild('stressStrain')

    stressStrain.createObject('MechanicalObject', name='Strain',template='E321')

    stressStrain.createObject('CorotationalStrainMapping', template="F321,E321")

    stressStrain.createObject('HookeForceField', template="E321", youngModulus=youngModulus,poissonRatio=poissonRatio, viscosity=0  )

    ## Collision

    node.createObject('TriangleModel',contactStiffness=1000,group=0)

    node.createObject('LineModel',contactStiffness=1000,group=0)

    node.createObject('PointModel',contactStiffness=1000,group=0)

    Visu = node.createChild('Visu')

    Visu.createObject('OglModel')

    Visu.createObject('IdentityMapping', template='Vec3d,ExtVec3f')

    ## indentor

    indentor = node.createChild('indentor')

    indentor.createObject('MechanicalObject',name='indentor', position="0.5 0.5 2")

    indentor.createObject('UniformMass')

    indentor.createObject('ConstantForceField', points=0, force="0 0 -1")

    indentor.createObject('TSphereModel', radius= indentorRadius, ContactStiffness=1000, group=1) 

    indentor.createObject('ProjectToLineConstraint',endTime='-1', indices=0, origin="0.5 0.5 2", direction="0 0 1") 

    indentor.createObject('PointConstraint', endTime=3, indices=0)                     

/******************************************************************************

*       SOFA, Simulation Open-Framework Architecture, version 1.0 RC 1        *

*                (c) 2006-2011 MGH, INRIA, USTL, UJF, CNRS                    *

*                                                                             *

* This library is free software; you can redistribute it and/or modify it     *

* under the terms of the GNU Lesser General Public License as published by    *

* the Free Software Foundation; either version 2.1 of the License, or (at     *

* your option) any later version.                                             *

*                                                                             *

* This library is distributed in the hope that it will be useful, but WITHOUT *

* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *

* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *

* for more details.                                                           *

*                                                                             *

* You should have received a copy of the GNU Lesser General Public License    *

* along with this library; if not, write to the Free Software Foundation,     *

* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *

*******************************************************************************

*                               SOFA :: Modules                               *

*                                                                             *

* Authors: The SOFA Team and external contributors (see Authors.txt)          *

*                                                                             *

* Contact information: contact@sofa-framework.org                             *

******************************************************************************/

//#include <sofa/component/collision/DefaultContactManager.h>

#include "MyDefaultContactManager.h"

#include <sofa/core/visual/VisualParams.h>

#include <sofa/core/ObjectFactory.h>

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

namespace sofa

{

namespace component

{

namespace collision

{

SOFA_DECL_CLASS(MyDefaultContactManager)

int MyDefaultContactManagerClass = core::RegisterObject("My Default class to create reactions to the collisions")

        .add< MyDefaultContactManager >()

        .addAlias("MyCollisionResponse")

        ;

MyDefaultContactManager::MyDefaultContactManager()

    : response(initData(&response, "response", "contact response class"))

    , responseParams(initData(&responseParams, "responseParams", "contact response parameters (syntax: name1=value1&name2=value2&...)"))

    //,deepInMeristem(0.0)

    ,deepInMeristem(initData(&deepInMeristem,"deepInMeristem","how deep we are in the meristem"))

    ,aTouche(initData(&aTouche,"aTouche","has there been a collision between the meristem and the indentor"))

    ,origine(initData(&origine,"origine","origine of collision between the meristem and the indentor"))

{

}

MyDefaultContactManager::~MyDefaultContactManager()

{

    // Contacts are now attached to the graph.

    // So they will be deleted by the DeleteVisitor

    // FIX crash on unload bug. -- J. Allard

    //clear();

}

sofa::helper::OptionsGroup MyDefaultContactManager::initializeResponseOptions(core::collision::Pipeline *pipeline)

{

    helper::set<std::string> listResponse;

    if (pipeline) listResponse=pipeline->getResponseList();

    else

    {

        core::collision::Contact::Factory::iterator it;

        for (it=core::collision::Contact::Factory::getInstance()->begin(); it!=core::collision::Contact::Factory::getInstance()->end(); ++it)

        {

            listResponse.insert(it->first);

        }

    }

    sofa::helper::OptionsGroup responseOptions(listResponse);

    if (listResponse.find("default") != listResponse.end())

        responseOptions.setSelectedItem("default");

    return responseOptions;

}

void MyDefaultContactManager::init()

{

    aTouche.setValue(false);

    if (response.getValue().size() == 0)

    {

        core::collision::Pipeline *pipeline=static_cast<simulation::Node*>(getContext())->collisionPipeline;

        response.setValue(initializeResponseOptions(pipeline));

    }

}

void MyDefaultContactManager::cleanup()

{

    for (sofa::helper::vector<core::collision::Contact::SPtr>::iterator it=contacts.begin(); it!=contacts.end(); ++it)

    {

        (*it)->removeResponse();

        (*it)->cleanup();

        //delete *it;

        it->reset();

    }

    contacts.clear();

    contactMap.clear();

}

void MyDefaultContactManager::createContacts(const DetectionOutputMap& outputsMap)

{

    using core::CollisionModel;

    using core::collision::Contact;

    int nbContact = 0;

	double origineActuelle[3];

	// First iterate on the collision detection outputs and look for existing or new contacts

	for (DetectionOutputMap::const_iterator outputsIt = outputsMap.begin(),

		outputsItEnd = outputsMap.end(); outputsIt != outputsItEnd ; ++outputsIt)

	{

		const helper::set<int>& myGroup_first=outputsIt->first.first->getGroups();

		const helper::set<int>& myGroup_second=outputsIt->first.second->getGroups();

        if (aTouche.getValue()){

			helper::set<int>::iterator it_first=myGroup_first.begin();

			helper::set<int>::iterator it_second=myGroup_second.begin();

			if (*it_first == 1){

				origineActuelle[0] = outputsIt->first.first->getContext()->getMechanicalState()->getPX(0);

				origineActuelle[1] = outputsIt->first.first->getContext()->getMechanicalState()->getPY(0);

				origineActuelle[2] = outputsIt->first.first->getContext()->getMechanicalState()->getPZ(0);

                deepInMeristem.setValue(std::sqrt(pow(origine.getValue()[0]-origineActuelle[0],2)+pow(origine.getValue()[1]-origineActuelle[1],2)+pow(origine.getValue()[2]-origineActuelle[2],2)));

			}

			else if (*it_second==1){

				origineActuelle[0] = outputsIt->first.second->getContext()->getMechanicalState()->getPX(0);

				origineActuelle[1] = outputsIt->first.second->getContext()->getMechanicalState()->getPY(0);

				origineActuelle[2] = outputsIt->first.second->getContext()->getMechanicalState()->getPZ(0);

                deepInMeristem.setValue(std::sqrt(pow(origine.getValue()[0]-origineActuelle[0],2)+pow(origine.getValue()[1]-origineActuelle[1],2)+pow(origine.getValue()[2]-origineActuelle[2],2)));

			}

        }

		std::pair<ContactMap::iterator,bool> contactInsert =

			contactMap.insert(ContactMap::value_type(outputsIt->first,Contact::SPtr()));

		ContactMap::iterator contactIt = contactInsert.first;

		if (contactInsert.second)

		{

            // new contact

            //sout << "Creation new "<<contacttype<<" contact"<<sendl;

            CollisionModel* model1 = outputsIt->first.first;

            CollisionModel* model2 = outputsIt->first.second;

            std::string responseUsed = getContactResponse(model1, model2);

            if (*model1->getGroups().begin() == 1 && !aTouche.getValue()){

            if (!aTouche.getValue()){

                printf("Collision detected\n");

				std::fflush(stdout);

                aTouche.setValue(true);

                sofa::helper::vector<double> actuelorigine;

                actuelorigine.push_back(model1->getContext()->getMechanicalState()->getPX(0));

                actuelorigine.push_back(model1->getContext()->getMechanicalState()->getPY(0));

                actuelorigine.push_back(model1->getContext()->getMechanicalState()->getPZ(0));

                origine.setValue(actuelorigine);

            }

			}

            else if(*model2->getGroups().begin() == 1 && !aTouche.getValue()){

				printf("Collision detected\n");

				std::fflush(stdout);

                aTouche.setValue(true);

                sofa::helper::vector<double> actuelorigine;

                actuelorigine.push_back(model2->getContext()->getMechanicalState()->getPX(0));

                actuelorigine.push_back(model2->getContext()->getMechanicalState()->getPY(0));

                actuelorigine.push_back(model2->getContext()->getMechanicalState()->getPZ(0));

                origine.setValue(actuelorigine);

            }

			std::fflush(stdout);

            // We can create rules in order to not respond to specific collisions

            if (!responseUsed.compare("null"))

            {

				contactMap.erase(contactIt);

            }

			else

			{

				Contact::SPtr contact = Contact::Create(responseUsed, model1, model2, intersectionMethod,

					this->f_printLog.getValue());

				if (contact == NULL)

				{

					std::string model1class = model1->getClassName();

					std::string model2class = model2->getClassName();

					int count = ++errorMsgCount[std::make_pair(responseUsed,

						std::make_pair(model1class, model2class))];

					if (count <= 10)

					{

						serr << "Contact " << responseUsed << " between " << model1->getClassName()

							<< " and " << model2->getClassName() << " creation failed" << sendl;

						if (count == 1)

						{

							serr << "Supported models for contact " << responseUsed << ":" << sendl;

							for (Contact::Factory::const_iterator it =

								Contact::Factory::getInstance()->begin(),

								itend = Contact::Factory::getInstance()->end(); it != itend; ++it)

							{

								if (it->first != responseUsed) continue;

								serr << "   " << helper::gettypename(it->second->type()) << sendl;

							}

							serr << sendl;

						}

						if (count == 10) serr << "further messages suppressed" << sendl;

					}

					contactMap.erase(contactIt);

				}

				else

				{

					contactIt->second = contact;

					contact->setName(model1->getName() + std::string("-") + model2->getName());

					setContactTags(model1, model2, contact);

					contact->f_printLog.setValue(this->f_printLog.getValue());

					contact->init();

					contact->setDetectionOutputs(outputsIt->second);

					++nbContact;

				}

			}

		}

		else

		{

            // pre-existing and still active contact

            contactIt->second->setDetectionOutputs(outputsIt->second);

            ++nbContact;

		}

	}

	// Then look at previous contacts

	// and remove inactive contacts

	std::stack<ContactMap::iterator> deadContacts;

	for (ContactMap::iterator contactIt = contactMap.begin(), contactItEnd = contactMap.end();

		contactIt != contactItEnd;)

	{

		bool remove = false;

		DetectionOutputMap::const_iterator outputsIt = outputsMap.find(contactIt->first);

		if (outputsIt == outputsMap.end())

		{

            // inactive contact

            if (contactIt->second->keepAlive())

            {

				contactIt->second->setDetectionOutputs(NULL);

                ++nbContact;

            }

            else

            {

				remove = true;

			}

		}

		if (remove)

		{

			if (contactIt->second)

			{

                contactIt->second->removeResponse();

                contactIt->second->cleanup();

                contactIt->second.reset();

			}

			ContactMap::iterator eraseIt = contactIt;

			++contactIt;

			contactMap.erase(eraseIt);

		}

		else

		{

			++contactIt;

		}

	}

    // now update contactVec

    contacts.clear();

    contacts.reserve(nbContact);

    for (ContactMap::const_iterator contactIt = contactMap.begin(), contactItEnd = contactMap.end();

		contactIt != contactItEnd; ++contactIt)

    {

        contacts.push_back(contactIt->second);

    }

    // compute number of contacts attached to each collision model

    std::map< CollisionModel*, int > nbContactsMap;

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

    {

        std::pair< CollisionModel*, CollisionModel* > cms = contacts[i]->getCollisionModels();

        nbContactsMap[cms.first]++;

        if (cms.second != cms.first)

            nbContactsMap[cms.second]++;

    }

	// TODO: this is VERY inefficient, should be replaced with a visitor

    helper::vector< CollisionModel* > collisionModels;

    simulation::Node* context = dynamic_cast< simulation::Node* >(getContext());

    context->getTreeObjects< CollisionModel >(&collisionModels);

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

    {

        collisionModels[i]->setNumberOfContacts(nbContactsMap[collisionModels[i]]);

    }

}

std::string MyDefaultContactManager::getContactResponse(core::CollisionModel* model1, core::CollisionModel* model2)

{

    std::string responseUsed = response.getValue().getSelectedItem();

    std::string params = responseParams.getValue();

    if (!params.empty())

    {

        responseUsed += '?';

        responseUsed += params;

    }

    std::string response1 = model1->getContactResponse();

    std::string response2 = model2->getContactResponse();

    if (response1.empty()  &&  response2.empty()) return responseUsed;

    if (response1.empty()  && !response2.empty()) return response2;

    if (!response1.empty() &&  response2.empty()) return response1;

    if (response1 != response2) return responseUsed;

    else return response1;

}

void MyDefaultContactManager::draw(const core::visual::VisualParams* vparams)

{

    for (sofa::helper::vector<core::collision::Contact::SPtr>::iterator it = contacts.begin(); it!=contacts.end(); it++)

    {

        if ((*it)!=NULL)

            (*it)->draw(vparams);

    }

}

void MyDefaultContactManager::removeContacts(const ContactVector &c)

{

    ContactVector::const_iterator remove_it = c.begin();

    ContactVector::const_iterator remove_itEnd = c.end();

    ContactVector::iterator it;

    ContactVector::iterator itEnd;

    ContactMap::iterator map_it;

    ContactMap::iterator map_itEnd;

    while (remove_it != remove_itEnd)

    {

        // Whole scene contacts

        it = contacts.begin();

        itEnd = contacts.end();

        while (it != itEnd)

        {

            if (*it == *remove_it)

            {

                (*it)->removeResponse();

                (*it)->cleanup();

                it->reset();

                contacts.erase(it);

                break;

            }

            ++it;

        }

        // Stored contacts (keeping alive)

        map_it = contactMap.begin();

        map_itEnd = contactMap.end();

        while (map_it != map_itEnd)

        {

            if (map_it->second == *remove_it)

            {

                ContactMap::iterator erase_it = map_it;

                ++map_it;

                erase_it->second->removeResponse();

                erase_it->second->cleanup();

                erase_it->second.reset();

                contactMap.erase(erase_it);

            }

            else

            {

                ++map_it;

            }

        }

        ++remove_it;

    }

}

void MyDefaultContactManager::setContactTags(core::CollisionModel* model1, core::CollisionModel* model2, core::collision::Contact::SPtr contact)

{

    sofa::core::objectmodel::TagSet tagsm1 = model1->getTags();

    sofa::core::objectmodel::TagSet tagsm2 = model2->getTags();

    sofa::core::objectmodel::TagSet::iterator it;

    for(it=tagsm1.begin(); it != tagsm1.end(); it++)

        contact->addTag(*it);

    for(it=tagsm2.begin(); it!=tagsm2.end(); it++)

        contact->addTag(*it);

}

} // namespace collision

} // namespace component

} // namespace sofa

#cmake_minimum_required(VERSION 2.8)

include(${SOFA_CMAKE_DIR}/preProject.cmake)

set(HEADER_FILES

)

set(SOURCE_FILES

	AFM.cpp)

if(APPLE)

    set(RC_FILE "runSOFA.icns")

else()

    set(RC_FILE "sofa.rc")

endif()

add_executable(${PROJECT_NAME} ${HEADER_FILES} ${SOURCE_FILES} ${RC_FILES})

AddLinkerDependencies(SofaGuiMain SofaComponentMain SofaPython Flexible AFM_plane)

ADD_DEFINITIONS(-DNDEBUG )

ADD_DEFINITIONS(-DBOOST_UBLAS_NDEBUG -O3)

if(SOFA-LIB_SIMULATION_GRAPH_DAG)

    AddLinkerDependencies(SofaSimulationGraph)

endif()

if(UNIX)

    AddLinkerDependencies("dl")

endif()

TARGET_LINK_LIBRARIES( ${PROJECT_NAME} ${LINK_LIBRARIES} )

include(${SOFA_CMAKE_DIR}/postProject.cmake)

### E = 1000 ; P = 10 ; R = 0.4 = small ; mesh subsubsubdivided; first the force then the depth ###

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

}