void HexahedronSetTopologyModifier::removeHexahedraWarning( sofa::helper::vector<unsigned int> &hexahedra )
{
m_container->setHexahedronTopologyToDirty();
/// sort vertices to remove in a descendent order
std::sort( hexahedra.begin(), hexahedra.end(), std::greater<unsigned int>() );
// Warning that these edges will be deleted
HexahedraRemoved *e = new HexahedraRemoved(hexahedra);
addTopologyChange(e);
}
void QuadSetTopologyModifier::removeQuadsWarning( sofa::helper::vector<unsigned int> &quads)
{
m_container->setQuadTopologyToDirty();
/// sort vertices to remove in a descendent order
std::sort( quads.begin(), quads.end(), std::greater<unsigned int>() );
// Warning that these quads will be deleted
QuadsRemoved *e=new QuadsRemoved(quads);
addTopologyChange(e);
}
void TriangleSetTopologyModifier::removeTrianglesWarning(sofa::helper::vector<unsigned int> &triangles)
{
m_container->setTriangleTopologyToDirty();
/// sort vertices to remove in a descendent order
std::sort( triangles.begin(), triangles.end(), std::greater<unsigned int>() );
// Warning that these triangles will be deleted
TrianglesRemoved *e=new TrianglesRemoved(triangles);
addTopologyChange(e);
}
void DynamicSparseGridTopologyModifier::renumberAttributes( const sofa::helper::vector<unsigned int> &hexahedra )
{
helper::vector<core::topology::BaseMeshTopology::HexaID>& iirg = *m_DynContainer->idxInRegularGrid.beginEdit();
// Update the data
unsigned int nbElt = iirg.size();
std::map< unsigned int, core::topology::BaseMeshTopology::HexaID>& regularG2Topo = *m_DynContainer->idInRegularGrid2IndexInTopo.beginEdit();
for ( sofa::helper::vector<unsigned int>::const_iterator it = hexahedra.begin(); it != hexahedra.end(); ++it )
{
nbElt--;
// Update the voxels value
unsigned int idHexaInRegularGrid = iirg[*it];
(*( m_DynContainer->valuesIndexedInRegularGrid.beginEdit()))[idHexaInRegularGrid] = 0;
m_DynContainer->valuesIndexedInRegularGrid.endEdit();
// Renumbering the map.
// We delete the reference of the delete elt.
std::map< unsigned int, core::topology::BaseMeshTopology::HexaID>::iterator itMap = regularG2Topo.find( idHexaInRegularGrid);
if( itMap != regularG2Topo.end())
{
regularG2Topo.erase( itMap);
}
// Then, we change the id of the last elt moved in the topology.
itMap = regularG2Topo.find( iirg[nbElt]);// Index in the regular grid of the last elt in the topology
if( itMap != regularG2Topo.end())
{
itMap->second = *it;
}
// renumber iirg
iirg[*it] = iirg[nbElt];
}
iirg.resize( nbElt);
m_DynContainer->idInRegularGrid2IndexInTopo.endEdit();
m_DynContainer->idxInRegularGrid.endEdit();
everRenumbered = true;
}
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 ManifoldTriangleSetTopologyModifier::addRemoveTriangles (const unsigned int nTri2Add,
const sofa::helper::vector< Triangle >& triangles2Add,
const sofa::helper::vector< unsigned int >& trianglesIndex2Add,
const sofa::helper::vector< sofa::helper::vector< unsigned int > > & ancestors,
const sofa::helper::vector< sofa::helper::vector< double > >& baryCoefs,
sofa::helper::vector< unsigned int >& trianglesIndex2remove)
{
// I - Create ROI to remesh: step 1/2
sofa::helper::vector <unsigned int> vertexROI2Remesh;
// Look for every vertices concerned by the modifications
for (unsigned int i = 0; i <trianglesIndex2remove.size(); i++)
{
Triangle new_tri = m_container->getTriangleArray()[ trianglesIndex2remove[i] ];
for (unsigned int j = 0; j <3; j++)
{
vertexROI2Remesh.push_back (new_tri[j]); // these index vertex could change due to removing point.... TODO??
}
}
// II - Add the triangles
for (unsigned int i = 0; i <nTri2Add; i++)
{
// Use the most low level function to add triangle. I.e without any preliminary test.
TriangleSetTopologyModifier::addTriangleProcess (triangles2Add[i]);
}
// Warn for the creation of all the triangles registered to be created
TriangleSetTopologyModifier::addTrianglesWarning (nTri2Add, triangles2Add, trianglesIndex2Add, ancestors, baryCoefs);
// III - removes the triangles
// add the topological changes in the queue
TriangleSetTopologyModifier::removeTrianglesWarning (trianglesIndex2remove);
// inform other objects that the triangles are going to be removed
propagateTopologicalChanges();
// now destroy the old triangles.
TriangleSetTopologyModifier::removeTrianglesProcess (trianglesIndex2remove ,true, true);
// IV - Create ROI to remesh: step 2/2
sofa::helper::vector <unsigned int> trianglesFinalList;
trianglesFinalList = trianglesIndex2Add;
std::sort( trianglesIndex2remove.begin(), trianglesIndex2remove.end(), std::greater<unsigned int>() );
// Update the list of triangles (removing triangles change the index order)
for (unsigned int i = 0; i<trianglesIndex2remove.size(); i++)
{
trianglesFinalList[trianglesFinalList.size()-1-i] = trianglesIndex2remove[i];
}
// Look for every vertices concerned by the modifications
for (unsigned int i = 0; i <nTri2Add; i++)
{
Triangle new_tri = m_container->getTriangleArray()[ trianglesFinalList[i] ];
for (unsigned int j = 0; j <3; j++)
{
vertexROI2Remesh.push_back (new_tri[j]);
}
}
reorderingTopologyOnROI (vertexROI2Remesh);
bool topo = m_container->checkTopology();
if (!topo) //IN DEVELOPMENT (probably only in NDEBUG
{
std::cout <<"WARNING. [ManifoldTriangleSetTopologyModifier::addRemoveTriangles] The topology wasn't manifold after reordering the ROI. Reordering the whole triangulation." << std::endl;
sofa::helper::vector <unsigned int> allmesh;
for (int i = 0; i <m_container->getNbPoints(); i++)
allmesh.push_back (i);
reorderingTopologyOnROI (allmesh);
}
#ifndef NDEBUG
if(!m_container->checkTopology())
sout << "Error. [ManifoldTriangleSetTopologyModifier::addRemoveTriangles] The topology is not any more Manifold." << endl;
#endif
}