void QuadSetTopologyModifier::removeEdgesProcess( const sofa::helper::vector<unsigned int> &indices,
const bool removeIsolatedItems)
{
// Note: this does not check if an edge is removed from an existing quad (it should never happen)
if(m_container->hasEdgesInQuad()) // this method should only be called when edges exist
{
if(!m_container->hasQuadsAroundEdge())
m_container->createQuadsAroundEdgeArray();
unsigned int lastEdge = m_container->getNumberOfEdges() - 1;
for(unsigned int i = 0; i < indices.size(); ++i, --lastEdge)
{
// updating the quads connected to the edge replacing the removed one:
// for all quads connected to the last point
for(sofa::helper::vector<unsigned int>::iterator itt = m_container->m_quadsAroundEdge[lastEdge].begin();
itt != m_container->m_quadsAroundEdge[lastEdge].end(); ++itt)
{
unsigned int edgeIndex = m_container->getEdgeIndexInQuad(m_container->m_edgesInQuad[*itt], lastEdge);
m_container->m_edgesInQuad[*itt][edgeIndex] = indices[i];
}
// updating the edge shell itself (change the old index for the new one)
m_container->m_quadsAroundEdge[ indices[i] ] = m_container->m_quadsAroundEdge[ lastEdge ];
}
m_container->m_quadsAroundEdge.resize( m_container->getNumberOfEdges() - indices.size() );
}
// call the parent's method.
EdgeSetTopologyModifier::removeEdgesProcess(indices, removeIsolatedItems);
}
void ManifoldTriangleSetTopologyModifier::updateRemovingModifications (const sofa::helper::vector< unsigned int >& edgeToBeRemoved,
const sofa::helper::vector< unsigned int >& vertexToBeRemoved)
{
//???
for (unsigned int i = 0; i <vertexToBeRemoved.size(); i++)
{
it_modif = m_modifications.find( vertexToBeRemoved[i] );
if(it_modif != m_modifications.end())
m_modifications.erase( vertexToBeRemoved[i] );
}
for (unsigned int i = 0; i <edgeToBeRemoved.size(); i++)
{
for (unsigned int j = 0; j<m_modificationsEdge.size(); j++)
{
if (m_modificationsEdge[j] == edgeToBeRemoved[i])
{
m_modificationsEdge.erase(m_modificationsEdge.begin()+j);
break;
}
}
}
}
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 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::addQuadsProcess(const sofa::helper::vector< Quad > &quads)
{
helper::WriteAccessor< Data< sofa::helper::vector<Quad> > > m_quad = m_container->d_quad;
m_quad.reserve(m_quad.size() + quads.size());
for(unsigned int i=0; i<quads.size(); ++i)
{
addQuadProcess(quads[i]);
}
}
/// This function is named this way because someone give the getDataFields to the one
/// that returns a dictionary of (name, value) which is not coherente with the c++
/// name of the function.
/// If you are a brave hacker, courageous enough to break backward compatibility you can
/// probably fix all this mess.
static PyObject * Base_getListOfDataFields(PyObject *self, PyObject * /*args*/) {
Base * component = get_base(self);
const sofa::helper::vector<BaseData*> dataFields = component->getDataFields();
PyObject * pyList = PyList_New(dataFields.size());
for (unsigned int i = 0; i < dataFields.size(); ++i) {
PyList_SetItem(pyList, i, SP_BUILD_PYPTR(Data,BaseData,dataFields[i],false)) ;
}
return pyList;
}
static PyObject * Base_getListOfLinks(PyObject *self, PyObject * /*args*/) {
Base * component = get_base(self);
const sofa::helper::vector<BaseLink*> links = component->getLinks() ;
PyObject * pyList = PyList_New(links.size());
for (unsigned int i = 0; i < links.size(); ++i) {
PyList_SetItem(pyList, i, SP_BUILD_PYPTR(Link, BaseLink, links[i], false)) ;
}
return pyList;
}
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);
}
static PyObject * Base_getDataFields(PyObject *self, PyObject * /*args*/) {
Base * component = get_base(self);
const sofa::helper::vector<BaseData*> dataFields = component->getDataFields();
PyObject * pyDict = PyDict_New();
for (size_t i=0; i<dataFields.size(); i++) {
PyDict_SetItem(pyDict, PyString_FromString(dataFields[i]->getName().c_str()),
GetDataValuePython(dataFields[i]));
}
return pyDict;
}
void TriangleSetTopologyModifier::removeTriangles(sofa::helper::vector< unsigned int >& triangles,
const bool removeIsolatedEdges,
const bool removeIsolatedPoints)
{
helper::ReadAccessor< Data< sofa::helper::vector<Triangle> > > m_triangle = m_container->d_triangle;
for (unsigned int i = 0; i < triangles.size(); i++)
{
if( triangles[i] >= m_triangle.size())
{
std::cout << "Error: TriangleSetTopologyModifier::removeTriangles: Triangle: "<< triangles[i] <<" is out of bound" << std::endl;
return;
}
}
if (removeTrianglesPreconditions(triangles)) // Test if the topology will still fullfil the conditions if these triangles are removed.
{
/// add the topological changes in the queue
removeTrianglesWarning(triangles);
// inform other objects that the triangles are going to be removed
propagateTopologicalChanges();
// now destroy the old triangles.
removeTrianglesProcess( triangles ,removeIsolatedEdges, removeIsolatedPoints);
m_container->checkTopology();
}
else
{
std::cout << " TriangleSetTopologyModifier::removeItems(), preconditions for removal are not fullfil. " << std::endl;
}
}
void HexahedronSetTopologyModifier::addHexahedraProcess(const sofa::helper::vector< Hexahedron > &hexahedra)
{
for(unsigned int i = 0; i < hexahedra.size(); ++i)
{
addHexahedronProcess(hexahedra[i]);
}
}
void TriangleSetTopologyModifier::renumberPointsProcess( const sofa::helper::vector<unsigned int> &index,
const sofa::helper::vector<unsigned int> &inv_index,
const bool renumberDOF)
{
if(m_container->hasTriangles())
{
if(m_container->hasTrianglesAroundVertex())
{
sofa::helper::vector< sofa::helper::vector< unsigned int > > trianglesAroundVertex_cp = m_container->m_trianglesAroundVertex;
for(unsigned int i=0; i<index.size(); ++i)
{
m_container->m_trianglesAroundVertex[i] = trianglesAroundVertex_cp[ index[i] ];
}
}
helper::WriteAccessor< Data< sofa::helper::vector<Triangle> > > m_triangle = m_container->d_triangle;
for(unsigned int i=0; i<m_triangle.size(); ++i)
{
m_triangle[i][0] = inv_index[ m_triangle[i][0] ];
m_triangle[i][1] = inv_index[ m_triangle[i][1] ];
m_triangle[i][2] = inv_index[ m_triangle[i][2] ];
}
}
// call the parent's method
EdgeSetTopologyModifier::renumberPointsProcess( index, inv_index, renumberDOF );
}
void HexahedronSetTopologyModifier::removeQuadsProcess( const sofa::helper::vector<unsigned int> &indices,
const bool removeIsolatedEdges,
const bool removeIsolatedPoints)
{
if(!m_container->hasQuads()) // this method should only be called when quads exist
return;
if(m_container->hasQuadsInHexahedron())
{
if(!m_container->hasHexahedraAroundQuad())
m_container->createHexahedraAroundQuadArray();
unsigned int lastQuad = m_container->getNumberOfQuads() - 1;
for(unsigned int i=0; i<indices.size(); ++i, --lastQuad)
{
for(sofa::helper::vector<unsigned int>::iterator itt=m_container->m_hexahedraAroundQuad[lastQuad].begin();
itt!=m_container->m_hexahedraAroundQuad[lastQuad].end(); ++itt)
{
unsigned int quadIndex=m_container->getQuadIndexInHexahedron(m_container->m_quadsInHexahedron[*itt],lastQuad);
m_container->m_quadsInHexahedron[*itt][quadIndex]=indices[i];
}
// updating the quad shell itself (change the old index for the new one)
m_container->m_hexahedraAroundQuad[ indices[i] ] = m_container->m_hexahedraAroundQuad[ lastQuad ];
}
m_container->m_hexahedraAroundQuad.resize( m_container->m_hexahedraAroundQuad.size() - indices.size() );
}
// call the parent's method.
QuadSetTopologyModifier::removeQuadsProcess( indices, removeIsolatedEdges, removeIsolatedPoints );
}
void HexahedronSetTopologyModifier::removePointsProcess(const sofa::helper::vector<unsigned int> &indices,
const bool removeDOF)
{
if(m_container->hasHexahedra())
{
if(!m_container->hasHexahedraAroundVertex())
{
m_container->createHexahedraAroundVertexArray();
}
helper::WriteAccessor< Data< sofa::helper::vector<Hexahedron> > > m_hexahedron = m_container->d_hexahedron;
unsigned int lastPoint = m_container->getNbPoints() - 1;
for(unsigned int i = 0; i < indices.size(); ++i, --lastPoint)
{
// updating the edges connected to the point replacing the removed one:
// for all edges connected to the last point
for(sofa::helper::vector<unsigned int>::iterator itt=m_container->m_hexahedraAroundVertex[lastPoint].begin();
itt!=m_container->m_hexahedraAroundVertex[lastPoint].end(); ++itt)
{
unsigned int vertexIndex = m_container->getVertexIndexInHexahedron(m_hexahedron[*itt], lastPoint);
m_hexahedron[*itt][ vertexIndex] = indices[i];
}
// updating the edge shell itself (change the old index for the new one)
m_container->m_hexahedraAroundVertex[ indices[i] ] = m_container->m_hexahedraAroundVertex[ lastPoint ];
}
m_container->m_hexahedraAroundVertex.resize( m_container->m_hexahedraAroundVertex.size() - indices.size() );
}
// Important : the points are actually deleted from the mechanical object's state vectors iff (removeDOF == true)
// call the parent's method.
QuadSetTopologyModifier::removePointsProcess( indices, removeDOF );
}
void ManifoldTriangleSetTopologyModifier::createRemovingEdgesFutureModifications (const sofa::helper::vector <unsigned int> items)
{
EdgesInTriangle EdgesInTriangleArray;
bool test = true;
for (unsigned int i = 0; i < items.size(); i++)
{
EdgesInTriangleArray = m_container->getEdgesInTriangle( items[i] );
for (unsigned int j =0; j < 3 ; j++)
{
for (unsigned int k =0; k< m_modificationsEdge.size(); k++)
{
if (EdgesInTriangleArray[j] == m_modificationsEdge[k])
{
test = false;
break;
}
}
if (test)
{
m_modificationsEdge.push_back(EdgesInTriangleArray[j]);
}
test = true;
}
}
}
void QuadSetTopologyModifier::renumberPointsProcess( const sofa::helper::vector<unsigned int> &index,
const sofa::helper::vector<unsigned int> &inv_index,
const bool renumberDOF)
{
if(m_container->hasQuads())
{
helper::WriteAccessor< Data< sofa::helper::vector<Quad> > > m_quad = m_container->d_quad;
if(m_container->hasQuadsAroundVertex())
{
sofa::helper::vector< sofa::helper::vector< unsigned int > > quadsAroundVertex_cp = m_container->m_quadsAroundVertex;
for(unsigned int i=0; i<index.size(); ++i)
{
m_container->m_quadsAroundVertex[i] = quadsAroundVertex_cp[ index[i] ];
}
}
for(unsigned int i=0; i<m_quad.size(); ++i)
{
m_quad[i][0] = inv_index[ m_quad[i][0] ];
m_quad[i][1] = inv_index[ m_quad[i][1] ];
m_quad[i][2] = inv_index[ m_quad[i][2] ];
m_quad[i][3] = inv_index[ m_quad[i][3] ];
}
}
// call the parent's method
EdgeSetTopologyModifier::renumberPointsProcess( index, inv_index, renumberDOF );
}
void HexahedronSetTopologyModifier::renumberPointsProcess( const sofa::helper::vector<unsigned int> &index, const sofa::helper::vector<unsigned int> &inv_index, const bool renumberDOF)
{
if(m_container->hasHexahedra())
{
if(m_container->hasHexahedraAroundVertex())
{
sofa::helper::vector< sofa::helper::vector< unsigned int > > hexahedraAroundVertex_cp = m_container->m_hexahedraAroundVertex;
for(unsigned int i=0; i<index.size(); ++i)
{
m_container->m_hexahedraAroundVertex[i] = hexahedraAroundVertex_cp[ index[i] ];
}
}
helper::WriteAccessor< Data< sofa::helper::vector<Hexahedron> > > m_hexahedron = m_container->d_hexahedron;
for(unsigned int i=0; i<m_hexahedron.size(); ++i)
{
m_hexahedron[i][0] = inv_index[ m_hexahedron[i][0] ];
m_hexahedron[i][1] = inv_index[ m_hexahedron[i][1] ];
m_hexahedron[i][2] = inv_index[ m_hexahedron[i][2] ];
m_hexahedron[i][3] = inv_index[ m_hexahedron[i][3] ];
m_hexahedron[i][4] = inv_index[ m_hexahedron[i][4] ];
m_hexahedron[i][5] = inv_index[ m_hexahedron[i][5] ];
m_hexahedron[i][6] = inv_index[ m_hexahedron[i][6] ];
m_hexahedron[i][7] = inv_index[ m_hexahedron[i][7] ];
}
}
// call the parent's method.
QuadSetTopologyModifier::renumberPointsProcess( index, inv_index, renumberDOF );
}
void TriangleSetTopologyModifier::movePointsProcess (const sofa::helper::vector <unsigned int>& id,
const sofa::helper::vector< sofa::helper::vector< unsigned int > >& ancestors,
const sofa::helper::vector< sofa::helper::vector< double > >& coefs,
const bool moveDOF)
{
(void)moveDOF;
unsigned int nbrVertex = id.size();
bool doublet;
sofa::helper::vector< unsigned int > trianglesAroundVertex2Move;
sofa::helper::vector< Triangle > trianglesArray;
// Step 1/4 - Creating trianglesAroundVertex to moved due to moved points:
for (unsigned int i = 0; i<nbrVertex; ++i)
{
const sofa::helper::vector <unsigned int>& trianglesAroundVertex = m_container->getTrianglesAroundVertex( id[i] );
for (unsigned int j = 0; j<trianglesAroundVertex.size(); ++j)
{
doublet = false;
for (unsigned int k =0; k<trianglesAroundVertex2Move.size(); ++k) //Avoid double
{
if (trianglesAroundVertex2Move[k] == trianglesAroundVertex[j])
{
doublet = true;
break;
}
}
if(!doublet)
trianglesAroundVertex2Move.push_back (trianglesAroundVertex[j]);
}
}
std::sort( trianglesAroundVertex2Move.begin(), trianglesAroundVertex2Move.end(), std::greater<unsigned int>() );
// Step 2/4 - Create event to delete all elements before moving and propagate it:
TrianglesMoved_Removing *ev1 = new TrianglesMoved_Removing (trianglesAroundVertex2Move);
this->addTopologyChange(ev1);
propagateTopologicalChanges();
// Step 3/4 - Physically move all dof:
PointSetTopologyModifier::movePointsProcess (id, ancestors, coefs);
// Step 4/4 - Create event to recompute all elements concerned by moving and propagate it:
// Creating the corresponding array of Triangles for ancestors
for (unsigned int i = 0; i<trianglesAroundVertex2Move.size(); i++)
trianglesArray.push_back (m_container->getTriangleArray()[ trianglesAroundVertex2Move[i] ]);
TrianglesMoved_Adding *ev2 = new TrianglesMoved_Adding (trianglesAroundVertex2Move, trianglesArray);
this->addTopologyChange(ev2); // This event should be propagated with global workflow
}
void DynamicSparseGridTopologyModifier::addHexahedraProcess ( const sofa::helper::vector< Hexahedron > &hexahedra, const sofa::helper::vector< unsigned int> &indices )
{
assert( hexahedra.size() == indices.size());
unsigned int hexaSize = m_DynContainer->getNumberOfHexahedra(); // Get the size before adding elements
HexahedronSetTopologyModifier::addHexahedraProcess ( hexahedra );
helper::vector<core::topology::BaseMeshTopology::HexaID>& iirg = *m_DynContainer->idxInRegularGrid.beginEdit();
std::map< unsigned int, core::topology::BaseMeshTopology::HexaID> &idrg2topo=*m_DynContainer->idInRegularGrid2IndexInTopo.beginEdit();
for ( unsigned int i = 0; i < hexahedra.size(); i++ ) // For each element
{
iirg[hexaSize + i] = indices[i];
idrg2topo.insert( std::make_pair ( indices[i], hexaSize + i ) );
//TODO// init the values too ...
}
m_DynContainer->idInRegularGrid2IndexInTopo.endEdit();
m_DynContainer->idxInRegularGrid.endEdit();
}
void SimpleTesselatedTetraTopologicalMapping::removeOutputTetrahedra( const sofa::helper::vector<unsigned int>& index )
{
helper::vector< fixed_array<int, 8> >& tetrahedraMappedFromTetraData = *(tetrahedraMappedFromTetra.beginEdit());
helper::vector<int>& tetraSourceData = *(tetraSource.beginEdit());
int last = tetraSourceData.size() -1;
for (unsigned int i = 0; i < index.size(); ++i)
{
swapOutputTetrahedra( index[i], last );
int source = tetraSourceData[last];
if (source != -1)
{
int nbt = 0;
for (int j=0; j<8; ++j)
{
if (tetrahedraMappedFromTetraData[source][j] == last)
{
tetrahedraMappedFromTetraData[source][j] = -1;
}
else if (tetrahedraMappedFromTetraData[source][j] != -1)
++nbt;
}
if (nbt == 0) // we need to remove the source tetra
{
// sout << "SimpleTesselatedTetraTopologicalMapping: source tetra "<<source<<" needs to be removed."<<sendl;
tetrahedraToRemove.insert(source);
}
else
{
// sout << "SimpleTesselatedTetraTopologicalMapping: source tetra "<<source<<" now has "<<nbt<<" / 8 childs."<<sendl;
}
--last;
}
}
tetraSourceData.resize( tetraSourceData.size() - index.size() );
tetrahedraMappedFromTetra.endEdit();
tetraSource.endEdit();
}
void Mesh2PointTopologicalMapping::removeOutputPoints( const sofa::helper::vector<unsigned int>& index )
{
unsigned int last = pointSource.size() - 1;
for (unsigned int i = 0; i < index.size(); ++i)
{
swapOutputPoints( index[i], last, true );
--last;
}
pointSource.resize(last + 1);
}
void SimpleTesselatedTetraTopologicalMapping::renumberInputPoints( const sofa::helper::vector<unsigned int>& index )
{
helper::WriteAccessor< Data< sofa::helper::vector<int> > > pointSourceData = d_pointSource;
helper::WriteAccessor< Data< sofa::helper::vector<int> > > pointMappedFromPointData = d_pointMappedFromPoint;
for (unsigned int i = 0; i < index.size(); ++i)
{
int map = pointMappedFromPointData[index[i]];
pointMappedFromPointData[i] = map;
if (map != -1)
pointSourceData[map] = i+1;
}
}
void SimpleTesselatedTetraTopologicalMapping::removeInputPoints( const sofa::helper::vector<unsigned int>& index )
{
helper::WriteAccessor< Data< sofa::helper::vector<int> > > pointSourceData = d_pointSource;
helper::WriteAccessor< Data< sofa::helper::vector<int> > > pointMappedFromPointData = d_pointMappedFromPoint;
unsigned int last = pointMappedFromPointData.size() -1;
for (unsigned int i = 0; i < index.size(); ++i)
{
swapInputPoints( index[i], last );
int map = pointMappedFromPointData[last];
if (map != -1)
pointSourceData[map] = 0;
--last;
}
pointMappedFromPointData.resize( last + 1 );
}
// Return the number of connected components from the graph containing all edges and give, for each vertex, which component it belongs to (use BOOST GRAPH LIBRAIRY)
int EdgeSetTopologyContainer::getNumberConnectedComponents(sofa::helper::vector<unsigned int>& components)
{
using namespace boost;
typedef adjacency_list <vecS, vecS, undirectedS> Graph;
Graph G;
helper::ReadAccessor< Data< sofa::helper::vector<Edge> > > m_edge = d_edge;
for (size_t k=0; k<m_edge.size(); ++k)
{
add_edge(m_edge[k][0], m_edge[k][1], G);
}
components.resize(num_vertices(G));
int num = (int) connected_components(G, &components[0]);
return num;
}
void HexahedronSetTopologyModifier::removeHexahedra(const sofa::helper::vector< unsigned int >& hexahedraIds)
{
sofa::helper::vector<unsigned int> hexahedraIds_filtered;
for (unsigned int i = 0; i < hexahedraIds.size(); i++)
{
if( hexahedraIds[i] >= m_container->getNumberOfHexahedra())
msg_error() << "Unable to remove the hexahedra: "<< hexahedraIds[i] <<" its index is out of bound." ;
else
hexahedraIds_filtered.push_back(hexahedraIds[i]);
}
// add the topological changes in the queue
removeHexahedraWarning(hexahedraIds_filtered);
// inform other objects that the hexa are going to be removed
propagateTopologicalChanges();
// now destroy the old hexahedra.
removeHexahedraProcess(hexahedraIds_filtered ,true);
m_container->checkTopology();
}
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 SimpleTesselatedTetraTopologicalMapping::removeInputTetrahedra( const sofa::helper::vector<unsigned int>& index )
{
helper::vector< fixed_array<int, 8> >& tetrahedraMappedFromTetraData = *(tetrahedraMappedFromTetra.beginEdit());
helper::vector<int>& tetraSourceData = *(tetraSource.beginEdit());
unsigned int last = tetrahedraMappedFromTetraData.size() -1;
for (unsigned int i = 0; i < index.size(); ++i)
{
swapInputTetrahedra( index[i], last );
fixed_array<int, 8> map = tetrahedraMappedFromTetraData[last];
for (int j=0; j<8; ++j)
if (map[j] != -1)
tetraSourceData[map[j]] = -1;
--last;
}
tetrahedraMappedFromTetraData.resize( last + 1 );
tetrahedraMappedFromTetra.endEdit();
tetraSource.endEdit();
}
void QuadSetTopologyModifier::removeQuads(const sofa::helper::vector< unsigned int >& quadIds,
const bool removeIsolatedEdges,
const bool removeIsolatedPoints)
{
sofa::helper::vector<unsigned int> quadIds_filtered;
for (unsigned int i = 0; i < quadIds.size(); i++)
{
if( quadIds[i] >= m_container->getNumberOfQuads())
std::cout << "Error: QuadSetTopologyModifier::removeQuads: quad: "<< quadIds[i] <<" is out of bound and won't be removed." << std::endl;
else
quadIds_filtered.push_back(quadIds[i]);
}
/// add the topological changes in the queue
removeQuadsWarning(quadIds_filtered);
// inform other objects that the quads are going to be removed
propagateTopologicalChanges();
// now destroy the old quads.
removeQuadsProcess( quadIds_filtered, removeIsolatedEdges, removeIsolatedPoints);
m_container->checkTopology();
}
void TriangleSetTopologyModifier::removePointsProcess( sofa::helper::vector<unsigned int> &indices,
const bool removeDOF)
{
if(m_container->hasTriangles())
{
if(!m_container->hasTrianglesAroundVertex())
m_container->createTrianglesAroundVertexArray();
helper::WriteAccessor< Data< sofa::helper::vector<Triangle> > > m_triangle = m_container->d_triangle;
unsigned int lastPoint = m_container->getNbPoints() - 1;
for(unsigned int i=0; i<indices.size(); ++i, --lastPoint)
{
// updating the triangles connected to the point replacing the removed one:
// for all triangles connected to the last point
sofa::helper::vector<unsigned int> &shell = m_container->m_trianglesAroundVertex[lastPoint];
for(unsigned int j=0; j<shell.size(); ++j)
{
const unsigned int q = shell[j];
for(unsigned int k=0; k<3; ++k)
{
if(m_triangle[q][k] == lastPoint)
m_triangle[q][k] = indices[i];
}
}
// updating the edge shell itself (change the old index for the new one)
m_container->m_trianglesAroundVertex[ indices[i] ] = m_container->m_trianglesAroundVertex[ lastPoint ];
}
m_container->m_trianglesAroundVertex.resize( m_container->m_trianglesAroundVertex.size() - indices.size() );
}
// Important : the points are actually deleted from the mechanical object's state vectors iff (removeDOF == true)
// call the parent's method.
EdgeSetTopologyModifier::removePointsProcess( indices, removeDOF );
}
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);
}