root / MyDefaultContactManager.cpp
Historique | Voir | Annoter | Télécharger (15,15 ko)
| 1 |
/******************************************************************************
|
|---|---|
| 2 |
* SOFA, Simulation Open-Framework Architecture, version 1.0 RC 1 *
|
| 3 |
* (c) 2006-2011 MGH, INRIA, USTL, UJF, CNRS *
|
| 4 |
* *
|
| 5 |
* This library is free software; you can redistribute it and/or modify it *
|
| 6 |
* under the terms of the GNU Lesser General Public License as published by *
|
| 7 |
* the Free Software Foundation; either version 2.1 of the License, or (at *
|
| 8 |
* your option) any later version. *
|
| 9 |
* *
|
| 10 |
* This library is distributed in the hope that it will be useful, but WITHOUT *
|
| 11 |
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
|
| 12 |
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
|
| 13 |
* for more details. *
|
| 14 |
* *
|
| 15 |
* You should have received a copy of the GNU Lesser General Public License *
|
| 16 |
* along with this library; if not, write to the Free Software Foundation, *
|
| 17 |
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. *
|
| 18 |
*******************************************************************************
|
| 19 |
* SOFA :: Modules *
|
| 20 |
* *
|
| 21 |
* Authors: The SOFA Team and external contributors (see Authors.txt) *
|
| 22 |
* *
|
| 23 |
* Contact information: contact@sofa-framework.org *
|
| 24 |
******************************************************************************/
|
| 25 |
//#include <sofa/component/collision/DefaultContactManager.h>
|
| 26 |
|
| 27 |
#include "MyDefaultContactManager.h" |
| 28 |
#include <sofa/core/visual/VisualParams.h> |
| 29 |
#include <sofa/core/ObjectFactory.h> |
| 30 |
#include <sofa/core/objectmodel/Tag.h> |
| 31 |
|
| 32 |
|
| 33 |
namespace sofa
|
| 34 |
{
|
| 35 |
|
| 36 |
namespace component
|
| 37 |
{
|
| 38 |
|
| 39 |
namespace collision
|
| 40 |
{
|
| 41 |
|
| 42 |
SOFA_DECL_CLASS(MyDefaultContactManager) |
| 43 |
|
| 44 |
int MyDefaultContactManagerClass = core::RegisterObject("My Default class to create reactions to the collisions") |
| 45 |
.add< MyDefaultContactManager >() |
| 46 |
.addAlias("MyCollisionResponse")
|
| 47 |
; |
| 48 |
|
| 49 |
MyDefaultContactManager::MyDefaultContactManager() |
| 50 |
: response(initData(&response, "response", "contact response class")) |
| 51 |
, responseParams(initData(&responseParams, "responseParams", "contact response parameters (syntax: name1=value1&name2=value2&...)")) |
| 52 |
//,deepInMeristem(0.0)
|
| 53 |
,deepInMeristem(initData(&deepInMeristem,"deepInMeristem","how deep we are in the meristem")) |
| 54 |
,aTouche(initData(&aTouche,"aTouche","has there been a collision between the meristem and the indentor")) |
| 55 |
,origine(initData(&origine,"origine","origine of collision between the meristem and the indentor")) |
| 56 |
{
|
| 57 |
} |
| 58 |
|
| 59 |
MyDefaultContactManager::~MyDefaultContactManager() |
| 60 |
{
|
| 61 |
// Contacts are now attached to the graph.
|
| 62 |
// So they will be deleted by the DeleteVisitor
|
| 63 |
// FIX crash on unload bug. -- J. Allard
|
| 64 |
//clear();
|
| 65 |
} |
| 66 |
|
| 67 |
sofa::helper::OptionsGroup MyDefaultContactManager::initializeResponseOptions(core::collision::Pipeline *pipeline) |
| 68 |
{
|
| 69 |
helper::set<std::string> listResponse;
|
| 70 |
if (pipeline) listResponse=pipeline->getResponseList();
|
| 71 |
else
|
| 72 |
{
|
| 73 |
core::collision::Contact::Factory::iterator it; |
| 74 |
for (it=core::collision::Contact::Factory::getInstance()->begin(); it!=core::collision::Contact::Factory::getInstance()->end(); ++it)
|
| 75 |
{
|
| 76 |
listResponse.insert(it->first); |
| 77 |
} |
| 78 |
} |
| 79 |
sofa::helper::OptionsGroup responseOptions(listResponse); |
| 80 |
if (listResponse.find("default") != listResponse.end()) |
| 81 |
responseOptions.setSelectedItem("default");
|
| 82 |
return responseOptions;
|
| 83 |
} |
| 84 |
|
| 85 |
void MyDefaultContactManager::init()
|
| 86 |
{
|
| 87 |
|
| 88 |
aTouche.setValue(false);
|
| 89 |
|
| 90 |
if (response.getValue().size() == 0) |
| 91 |
{
|
| 92 |
core::collision::Pipeline *pipeline=static_cast<simulation::Node*>(getContext())->collisionPipeline;
|
| 93 |
response.setValue(initializeResponseOptions(pipeline)); |
| 94 |
} |
| 95 |
} |
| 96 |
|
| 97 |
void MyDefaultContactManager::cleanup()
|
| 98 |
{
|
| 99 |
for (sofa::helper::vector<core::collision::Contact::SPtr>::iterator it=contacts.begin(); it!=contacts.end(); ++it)
|
| 100 |
{
|
| 101 |
(*it)->removeResponse(); |
| 102 |
(*it)->cleanup(); |
| 103 |
//delete *it;
|
| 104 |
it->reset(); |
| 105 |
} |
| 106 |
contacts.clear(); |
| 107 |
contactMap.clear(); |
| 108 |
} |
| 109 |
|
| 110 |
void MyDefaultContactManager::createContacts(const DetectionOutputMap& outputsMap) |
| 111 |
{
|
| 112 |
using core::CollisionModel;
|
| 113 |
using core::collision::Contact;
|
| 114 |
|
| 115 |
int nbContact = 0; |
| 116 |
|
| 117 |
double origineActuelle[3]; |
| 118 |
|
| 119 |
|
| 120 |
// First iterate on the collision detection outputs and look for existing or new contacts
|
| 121 |
for (DetectionOutputMap::const_iterator outputsIt = outputsMap.begin(),
|
| 122 |
outputsItEnd = outputsMap.end(); outputsIt != outputsItEnd ; ++outputsIt) |
| 123 |
{
|
| 124 |
const helper::set<int>& myGroup_first=outputsIt->first.first->getGroups(); |
| 125 |
const helper::set<int>& myGroup_second=outputsIt->first.second->getGroups(); |
| 126 |
if (aTouche.getValue()){
|
| 127 |
helper::set<int>::iterator it_first=myGroup_first.begin();
|
| 128 |
helper::set<int>::iterator it_second=myGroup_second.begin();
|
| 129 |
if (*it_first == 1){ |
| 130 |
origineActuelle[0] = outputsIt->first.first->getContext()->getMechanicalState()->getPX(0); |
| 131 |
origineActuelle[1] = outputsIt->first.first->getContext()->getMechanicalState()->getPY(0); |
| 132 |
origineActuelle[2] = outputsIt->first.first->getContext()->getMechanicalState()->getPZ(0); |
| 133 |
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))); |
| 134 |
} |
| 135 |
else if (*it_second==1){ |
| 136 |
origineActuelle[0] = outputsIt->first.second->getContext()->getMechanicalState()->getPX(0); |
| 137 |
origineActuelle[1] = outputsIt->first.second->getContext()->getMechanicalState()->getPY(0); |
| 138 |
origineActuelle[2] = outputsIt->first.second->getContext()->getMechanicalState()->getPZ(0); |
| 139 |
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))); |
| 140 |
} |
| 141 |
} |
| 142 |
|
| 143 |
|
| 144 |
|
| 145 |
std::pair<ContactMap::iterator,bool> contactInsert =
|
| 146 |
contactMap.insert(ContactMap::value_type(outputsIt->first,Contact::SPtr())); |
| 147 |
ContactMap::iterator contactIt = contactInsert.first; |
| 148 |
if (contactInsert.second)
|
| 149 |
{
|
| 150 |
// new contact
|
| 151 |
//sout << "Creation new "<<contacttype<<" contact"<<sendl;
|
| 152 |
CollisionModel* model1 = outputsIt->first.first; |
| 153 |
CollisionModel* model2 = outputsIt->first.second; |
| 154 |
std::string responseUsed = getContactResponse(model1, model2);
|
| 155 |
|
| 156 |
|
| 157 |
if (*model1->getGroups().begin() == 1 && !aTouche.getValue()){ |
| 158 |
if (!aTouche.getValue()){
|
| 159 |
printf("Collision detected\n");
|
| 160 |
std::fflush(stdout); |
| 161 |
|
| 162 |
aTouche.setValue(true);
|
| 163 |
sofa::helper::vector<double> actuelorigine;
|
| 164 |
actuelorigine.push_back(model1->getContext()->getMechanicalState()->getPX(0));
|
| 165 |
actuelorigine.push_back(model1->getContext()->getMechanicalState()->getPY(0));
|
| 166 |
actuelorigine.push_back(model1->getContext()->getMechanicalState()->getPZ(0));
|
| 167 |
origine.setValue(actuelorigine); |
| 168 |
|
| 169 |
} |
| 170 |
} |
| 171 |
else if(*model2->getGroups().begin() == 1 && !aTouche.getValue()){ |
| 172 |
printf("Collision detected\n");
|
| 173 |
std::fflush(stdout); |
| 174 |
|
| 175 |
aTouche.setValue(true);
|
| 176 |
sofa::helper::vector<double> actuelorigine;
|
| 177 |
actuelorigine.push_back(model2->getContext()->getMechanicalState()->getPX(0));
|
| 178 |
actuelorigine.push_back(model2->getContext()->getMechanicalState()->getPY(0));
|
| 179 |
actuelorigine.push_back(model2->getContext()->getMechanicalState()->getPZ(0));
|
| 180 |
origine.setValue(actuelorigine); |
| 181 |
} |
| 182 |
|
| 183 |
std::fflush(stdout); |
| 184 |
|
| 185 |
|
| 186 |
|
| 187 |
// We can create rules in order to not respond to specific collisions
|
| 188 |
if (!responseUsed.compare("null")) |
| 189 |
{
|
| 190 |
contactMap.erase(contactIt); |
| 191 |
} |
| 192 |
else
|
| 193 |
{
|
| 194 |
Contact::SPtr contact = Contact::Create(responseUsed, model1, model2, intersectionMethod, |
| 195 |
this->f_printLog.getValue());
|
| 196 |
|
| 197 |
if (contact == NULL) |
| 198 |
{
|
| 199 |
std::string model1class = model1->getClassName();
|
| 200 |
std::string model2class = model2->getClassName();
|
| 201 |
int count = ++errorMsgCount[std::make_pair(responseUsed,
|
| 202 |
std::make_pair(model1class, model2class))]; |
| 203 |
if (count <= 10) |
| 204 |
{
|
| 205 |
serr << "Contact " << responseUsed << " between " << model1->getClassName() |
| 206 |
<< " and " << model2->getClassName() << " creation failed" << sendl; |
| 207 |
if (count == 1) |
| 208 |
{
|
| 209 |
serr << "Supported models for contact " << responseUsed << ":" << sendl; |
| 210 |
for (Contact::Factory::const_iterator it =
|
| 211 |
Contact::Factory::getInstance()->begin(), |
| 212 |
itend = Contact::Factory::getInstance()->end(); it != itend; ++it) |
| 213 |
{
|
| 214 |
if (it->first != responseUsed) continue; |
| 215 |
serr << " " << helper::gettypename(it->second->type()) << sendl;
|
| 216 |
} |
| 217 |
serr << sendl; |
| 218 |
} |
| 219 |
if (count == 10) serr << "further messages suppressed" << sendl; |
| 220 |
} |
| 221 |
contactMap.erase(contactIt); |
| 222 |
} |
| 223 |
else
|
| 224 |
{
|
| 225 |
contactIt->second = contact; |
| 226 |
contact->setName(model1->getName() + std::string("-") + model2->getName()); |
| 227 |
setContactTags(model1, model2, contact); |
| 228 |
contact->f_printLog.setValue(this->f_printLog.getValue());
|
| 229 |
contact->init(); |
| 230 |
contact->setDetectionOutputs(outputsIt->second); |
| 231 |
++nbContact; |
| 232 |
} |
| 233 |
} |
| 234 |
} |
| 235 |
else
|
| 236 |
{
|
| 237 |
// pre-existing and still active contact
|
| 238 |
contactIt->second->setDetectionOutputs(outputsIt->second); |
| 239 |
++nbContact; |
| 240 |
} |
| 241 |
} |
| 242 |
|
| 243 |
// Then look at previous contacts
|
| 244 |
// and remove inactive contacts
|
| 245 |
std::stack<ContactMap::iterator> deadContacts; |
| 246 |
for (ContactMap::iterator contactIt = contactMap.begin(), contactItEnd = contactMap.end();
|
| 247 |
contactIt != contactItEnd;) |
| 248 |
{
|
| 249 |
bool remove = false; |
| 250 |
DetectionOutputMap::const_iterator outputsIt = outputsMap.find(contactIt->first); |
| 251 |
if (outputsIt == outputsMap.end())
|
| 252 |
{
|
| 253 |
// inactive contact
|
| 254 |
if (contactIt->second->keepAlive())
|
| 255 |
{
|
| 256 |
contactIt->second->setDetectionOutputs(NULL);
|
| 257 |
++nbContact; |
| 258 |
} |
| 259 |
else
|
| 260 |
{
|
| 261 |
remove = true;
|
| 262 |
} |
| 263 |
} |
| 264 |
if (remove)
|
| 265 |
{
|
| 266 |
if (contactIt->second)
|
| 267 |
{
|
| 268 |
contactIt->second->removeResponse(); |
| 269 |
contactIt->second->cleanup(); |
| 270 |
contactIt->second.reset(); |
| 271 |
} |
| 272 |
ContactMap::iterator eraseIt = contactIt; |
| 273 |
++contactIt; |
| 274 |
contactMap.erase(eraseIt); |
| 275 |
} |
| 276 |
else
|
| 277 |
{
|
| 278 |
++contactIt; |
| 279 |
} |
| 280 |
} |
| 281 |
|
| 282 |
// now update contactVec
|
| 283 |
contacts.clear(); |
| 284 |
contacts.reserve(nbContact); |
| 285 |
for (ContactMap::const_iterator contactIt = contactMap.begin(), contactItEnd = contactMap.end();
|
| 286 |
contactIt != contactItEnd; ++contactIt) |
| 287 |
{
|
| 288 |
contacts.push_back(contactIt->second); |
| 289 |
} |
| 290 |
|
| 291 |
// compute number of contacts attached to each collision model
|
| 292 |
std::map< CollisionModel*, int > nbContactsMap;
|
| 293 |
for (unsigned int i = 0; i < contacts.size(); ++i) |
| 294 |
{
|
| 295 |
std::pair< CollisionModel*, CollisionModel* > cms = contacts[i]->getCollisionModels(); |
| 296 |
nbContactsMap[cms.first]++; |
| 297 |
if (cms.second != cms.first)
|
| 298 |
nbContactsMap[cms.second]++; |
| 299 |
} |
| 300 |
|
| 301 |
// TODO: this is VERY inefficient, should be replaced with a visitor
|
| 302 |
helper::vector< CollisionModel* > collisionModels; |
| 303 |
simulation::Node* context = dynamic_cast< simulation::Node* >(getContext());
|
| 304 |
context->getTreeObjects< CollisionModel >(&collisionModels); |
| 305 |
|
| 306 |
for (unsigned int i = 0; i < collisionModels.size(); ++i) |
| 307 |
{
|
| 308 |
collisionModels[i]->setNumberOfContacts(nbContactsMap[collisionModels[i]]); |
| 309 |
} |
| 310 |
} |
| 311 |
|
| 312 |
std::string MyDefaultContactManager::getContactResponse(core::CollisionModel* model1, core::CollisionModel* model2)
|
| 313 |
{
|
| 314 |
std::string responseUsed = response.getValue().getSelectedItem();
|
| 315 |
std::string params = responseParams.getValue();
|
| 316 |
if (!params.empty())
|
| 317 |
{
|
| 318 |
responseUsed += '?';
|
| 319 |
responseUsed += params; |
| 320 |
} |
| 321 |
std::string response1 = model1->getContactResponse();
|
| 322 |
std::string response2 = model2->getContactResponse();
|
| 323 |
|
| 324 |
if (response1.empty() && response2.empty()) return responseUsed; |
| 325 |
if (response1.empty() && !response2.empty()) return response2; |
| 326 |
if (!response1.empty() && response2.empty()) return response1; |
| 327 |
|
| 328 |
if (response1 != response2) return responseUsed; |
| 329 |
else return response1; |
| 330 |
} |
| 331 |
|
| 332 |
void MyDefaultContactManager::draw(const core::visual::VisualParams* vparams) |
| 333 |
{
|
| 334 |
for (sofa::helper::vector<core::collision::Contact::SPtr>::iterator it = contacts.begin(); it!=contacts.end(); it++)
|
| 335 |
{
|
| 336 |
if ((*it)!=NULL) |
| 337 |
(*it)->draw(vparams); |
| 338 |
} |
| 339 |
} |
| 340 |
|
| 341 |
void MyDefaultContactManager::removeContacts(const ContactVector &c) |
| 342 |
{
|
| 343 |
ContactVector::const_iterator remove_it = c.begin(); |
| 344 |
ContactVector::const_iterator remove_itEnd = c.end(); |
| 345 |
|
| 346 |
ContactVector::iterator it; |
| 347 |
ContactVector::iterator itEnd; |
| 348 |
|
| 349 |
ContactMap::iterator map_it; |
| 350 |
ContactMap::iterator map_itEnd; |
| 351 |
|
| 352 |
while (remove_it != remove_itEnd)
|
| 353 |
{
|
| 354 |
// Whole scene contacts
|
| 355 |
it = contacts.begin(); |
| 356 |
itEnd = contacts.end(); |
| 357 |
|
| 358 |
while (it != itEnd)
|
| 359 |
{
|
| 360 |
if (*it == *remove_it)
|
| 361 |
{
|
| 362 |
(*it)->removeResponse(); |
| 363 |
(*it)->cleanup(); |
| 364 |
it->reset(); |
| 365 |
contacts.erase(it); |
| 366 |
break;
|
| 367 |
} |
| 368 |
|
| 369 |
++it; |
| 370 |
} |
| 371 |
|
| 372 |
// Stored contacts (keeping alive)
|
| 373 |
map_it = contactMap.begin(); |
| 374 |
map_itEnd = contactMap.end(); |
| 375 |
|
| 376 |
while (map_it != map_itEnd)
|
| 377 |
{
|
| 378 |
if (map_it->second == *remove_it)
|
| 379 |
{
|
| 380 |
ContactMap::iterator erase_it = map_it; |
| 381 |
++map_it; |
| 382 |
|
| 383 |
erase_it->second->removeResponse(); |
| 384 |
erase_it->second->cleanup(); |
| 385 |
erase_it->second.reset(); |
| 386 |
contactMap.erase(erase_it); |
| 387 |
} |
| 388 |
else
|
| 389 |
{
|
| 390 |
++map_it; |
| 391 |
} |
| 392 |
} |
| 393 |
|
| 394 |
++remove_it; |
| 395 |
} |
| 396 |
} |
| 397 |
|
| 398 |
void MyDefaultContactManager::setContactTags(core::CollisionModel* model1, core::CollisionModel* model2, core::collision::Contact::SPtr contact)
|
| 399 |
{
|
| 400 |
sofa::core::objectmodel::TagSet tagsm1 = model1->getTags(); |
| 401 |
sofa::core::objectmodel::TagSet tagsm2 = model2->getTags(); |
| 402 |
sofa::core::objectmodel::TagSet::iterator it; |
| 403 |
for(it=tagsm1.begin(); it != tagsm1.end(); it++)
|
| 404 |
contact->addTag(*it); |
| 405 |
for(it=tagsm2.begin(); it!=tagsm2.end(); it++)
|
| 406 |
contact->addTag(*it); |
| 407 |
} |
| 408 |
|
| 409 |
} // namespace collision
|
| 410 |
|
| 411 |
} // namespace component
|
| 412 |
|
| 413 |
} // namespace sofa
|