void DefaultCollisionGroupManager::createGroups(core::objectmodel::BaseContext* scene, const sofa::helper::vector<Contact::SPtr>& contacts) { int groupIndex = 1; // Map storing group merging history std::map<simulation::Node*, simulation::Node::SPtr > mergedGroups; sofa::helper::vector< simulation::Node::SPtr > contactGroup; sofa::helper::vector< simulation::Node::SPtr > removedGroup; contactGroup.reserve(contacts.size()); for(sofa::helper::vector<Contact::SPtr>::const_iterator cit = contacts.begin(); cit != contacts.end(); ++cit) { Contact* contact = cit->get(); simulation::Node* group1 = getIntegrationNode(contact->getCollisionModels().first); simulation::Node* group2 = getIntegrationNode(contact->getCollisionModels().second); simulation::Node::SPtr group = NULL; if (group1==NULL || group2==NULL) { } else if (group1 == group2) { // same group, no new group necessary group = group1; } else if (simulation::Node* parent=group1->findCommonParent(group2)) { // we can merge the groups // if solvers are compatible... bool mergeSolvers = (!group1->solver.empty() || !group2->solver.empty()); SolverSet solver; if (mergeSolvers) solver = SolverMerger::merge(group1->solver[0], group2->solver[0]); if (!mergeSolvers || solver.odeSolver!=NULL) { bool group1IsColl = groupSet.find(group1)!=groupSet.end(); bool group2IsColl = groupSet.find(group2)!=groupSet.end(); if (!group1IsColl && !group2IsColl) { char groupName[32]; snprintf(groupName,sizeof(groupName),"collision%d",groupIndex++); // create a new group group = parent->createChild(groupName); group->moveChild(BaseNode::SPtr(group1)); group->moveChild(BaseNode::SPtr(group2)); groupSet.insert(group.get()); } else if (group1IsColl) { group = group1; // merge group2 in group1 if (!group2IsColl) { group->moveChild(BaseNode::SPtr(group2)); } else { // merge groups and remove group2 SolverSet solver2; if (mergeSolvers) { solver2.odeSolver = group2->solver[0]; group2->removeObject(solver2.odeSolver); if (!group2->linearSolver.empty()) { solver2.linearSolver = group2->linearSolver[0]; group2->removeObject(solver2.linearSolver); } if (!group2->constraintSolver.empty()) { solver2.constraintSolver = group2->constraintSolver[0]; group2->removeObject(solver2.constraintSolver); } } while(!group2->object.empty()) group->moveObject(*group2->object.begin()); while(!group2->child.empty()) group->moveChild(BaseNode::SPtr(*group2->child.begin())); parent->removeChild(group2); groupSet.erase(group2); mergedGroups[group2] = group; if (solver2.odeSolver) solver2.odeSolver.reset(); if (solver2.linearSolver) solver2.linearSolver.reset(); if (solver2.constraintSolver) solver2.constraintSolver.reset(); // BUGFIX(2007-06-23 Jeremie A): we can't remove group2 yet, to make sure the keys in mergedGroups are unique. removedGroup.push_back(group2); //delete group2; } } else { // group1 is not a collision group while group2 is group = group2; group->moveChild(BaseNode::SPtr(group1)); } if (!group->solver.empty()) { core::behavior::OdeSolver* solver2 = group->solver[0]; group->removeObject(solver2); } if (!group->linearSolver.empty()) { core::behavior::BaseLinearSolver* solver2 = group->linearSolver[0]; group->removeObject(solver2); } if (!group->constraintSolver.empty()) { core::behavior::ConstraintSolver* solver2 = group->constraintSolver[0]; group->removeObject(solver2); } if (solver.odeSolver) { group->addObject(solver.odeSolver); } if (solver.linearSolver) { group->addObject(solver.linearSolver); } if (solver.constraintSolver) { group->addObject(solver.constraintSolver); } // perform init only once everyone has been added (in case of explicit dependencies) if (solver.odeSolver) { solver.odeSolver->init(); } if (solver.linearSolver) { solver.linearSolver->init(); } if (solver.constraintSolver) { solver.constraintSolver->init(); } } } contactGroup.push_back(group); } // now that the groups are final, attach contacts' response for(unsigned int i=0; i<contacts.size(); i++) { Contact* contact = contacts[i].get(); simulation::Node::SPtr group = contactGroup[i]; while (group!=NULL && mergedGroups.find(group.get())!=mergedGroups.end()) group = mergedGroups[group.get()]; if (group!=NULL) contact->createResponse(group.get()); else contact->createResponse(scene); } // delete removed groups for (sofa::helper::vector<simulation::Node::SPtr>::iterator it = removedGroup.begin(); it!=removedGroup.end(); ++it) { simulation::Node::SPtr node = *it; node->detachFromGraph(); node->execute<simulation::DeleteVisitor>(sofa::core::ExecParams::defaultInstance()); it->reset(); } removedGroup.clear(); // finally recreate group vector groups.clear(); for (std::set<simulation::Node::SPtr>::iterator it = groupSet.begin(); it!=groupSet.end(); ++it) groups.push_back(*it); }
void ParallelCollisionPipeline::doCollisionResponse() { core::objectmodel::BaseContext* scene = getContext(); simulation::Node* node = dynamic_cast<simulation::Node*>(scene); if (node && !node->getLogTime()) node=NULL; // Only use node for time logging ctime_t t0 = 0; const std::string category = "collision"; // then we start the creation of contacts if (contactManager==NULL) return; // can't go further VERBOSE(sout << "Create Contacts "<<contactManager->getName()<<sendl); if (node) t0 = node->startTime(); contactManager->createContacts(narrowPhaseDetection->getDetectionOutputs()); if (node) t0 = node->endTime(t0, category, contactManager, this); // finally we start the creation of collisionGroup const sofa::helper::vector<Contact*>& contacts = contactManager->getContacts(); // First we remove all contacts with non-simulated objects and directly add them sofa::helper::vector<Contact*> notStaticContacts; long int c=0; for (sofa::helper::vector<Contact*>::const_iterator it = contacts.begin(); it!=contacts.end(); ++it) { c+=(long int)*it; } if(c!=contactSum) { std::cout << "RESET" << std::endl; doRealCollisionReset(); contactSum=c; } else { //std::cerr<<"EQUAL!"<<c<<std::endl; } for (sofa::helper::vector<Contact*>::const_iterator it = contacts.begin(); it!=contacts.end(); ++it) { Contact* c = *it; if (!c->getCollisionModels().first->isSimulated()) { c->createResponse(c->getCollisionModels().second->getContext()); } else if (!c->getCollisionModels().second->isSimulated()) { c->createResponse(c->getCollisionModels().first->getContext()); } else notStaticContacts.push_back(c); } if (groupManager==NULL) { VERBOSE(sout << "Linking all contacts to Scene"<<sendl); for (sofa::helper::vector<Contact*>::const_iterator it = notStaticContacts.begin(); it!=notStaticContacts.end(); ++it) { (*it)->createResponse(scene); } } else { VERBOSE(sout << "Create Groups "<<groupManager->getName()<<sendl); if (node) t0 = node->startTime(); groupManager->createGroups(scene, notStaticContacts); if (node) t0 = node->endTime(t0, category, groupManager, this); } }