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                             *

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

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