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);
}
}
