From c9d7bb0ca4db9197c12e28dea0cab22a1f3a1bf2 Mon Sep 17 00:00:00 2001 From: erik pernod Date: Wed, 28 Jul 2021 13:52:19 +0200 Subject: [PATCH] [MeshMatrixMass] Fix massDensity vector update when adding new elements. Add method to compute the good element density using ancestors and coefs values (#2257) --- .../src/SofaMiscForceField/MeshMatrixMass.h | 114 +++++---- .../src/SofaMiscForceField/MeshMatrixMass.inl | 238 ++++++++++++------ 2 files changed, 219 insertions(+), 133 deletions(-) diff --git a/modules/SofaMiscForceField/src/SofaMiscForceField/MeshMatrixMass.h b/modules/SofaMiscForceField/src/SofaMiscForceField/MeshMatrixMass.h index 2434fc3b64d..4339458f13f 100644 --- a/modules/SofaMiscForceField/src/SofaMiscForceField/MeshMatrixMass.h +++ b/modules/SofaMiscForceField/src/SofaMiscForceField/MeshMatrixMass.h @@ -177,6 +177,10 @@ class MeshMatrixMass : public core::behavior::Mass virtual void setMassDensity(sofa::type::vector< Real > massDensity); virtual void setMassDensity(Real massDensityValue); virtual void setTotalMass(Real totalMass); + + virtual void addMassDensity(const sofa::type::vector< Index >& indices, + const sofa::type::vector< sofa::type::vector< Index > >& ancestors, + const sofa::type::vector< sofa::type::vector< double > >& coefs); /// @} @@ -253,73 +257,73 @@ class MeshMatrixMass : public core::behavior::Mass /// Mass coefficient Creation/Destruction functions for Triangular Mesh: /// Vertex coefficient of mass matrix creation function to handle creation of new triangles - void applyTriangleCreation(const sofa::type::vector< Index >& /*indices*/, - const sofa::type::vector< core::topology::BaseMeshTopology::Triangle >& /*elems*/, - const sofa::type::vector< sofa::type::vector< Index > >& /*ancestors*/, - const sofa::type::vector< sofa::type::vector< double > >& /*coefs*/); + void applyTriangleCreation(const sofa::type::vector< Index >& triangleAdded, + const sofa::type::vector< core::topology::BaseMeshTopology::Triangle >& elems, + const sofa::type::vector< sofa::type::vector< Index > >& ancestors, + const sofa::type::vector< sofa::type::vector< double > >& coefs); /// Vertex coefficient of mass matrix destruction function to handle creation of new triangles - void applyTriangleDestruction(const sofa::type::vector & /*indices*/); + void applyTriangleDestruction(const sofa::type::vector & triangleRemoved); using topology::TopologyDataHandler::ApplyTopologyChange; /// Callback to add triangles elements. - void ApplyTopologyChange(const core::topology::TrianglesAdded* /*event*/); + void ApplyTopologyChange(const core::topology::TrianglesAdded* topoEvent); /// Callback to remove triangles elements. - void ApplyTopologyChange(const core::topology::TrianglesRemoved* /*event*/); + void ApplyTopologyChange(const core::topology::TrianglesRemoved* topoEvent); ///////////////////////// Functions on Quads ////////////////////////////////////// /// Mass coefficient Creation/Destruction functions for Quad Mesh: /// Vertex coefficient of mass matrix creation function to handle creation of new quads - void applyQuadCreation(const sofa::type::vector< Index >& /*indices*/, - const sofa::type::vector< core::topology::BaseMeshTopology::Quad >& /*elems*/, - const sofa::type::vector< sofa::type::vector< Index > >& /*ancestors*/, - const sofa::type::vector< sofa::type::vector< double > >& /*coefs*/); + void applyQuadCreation(const sofa::type::vector< Index >& quadAdded, + const sofa::type::vector< core::topology::BaseMeshTopology::Quad >& elems, + const sofa::type::vector< sofa::type::vector< Index > >& ancestors, + const sofa::type::vector< sofa::type::vector< double > >& coefs); /// Vertex coefficient of mass matrix destruction function to handle creation of new quads - void applyQuadDestruction(const sofa::type::vector & /*indices*/); + void applyQuadDestruction(const sofa::type::vector & quadRemoved); /// Callback to add quads elements. - void ApplyTopologyChange(const core::topology::QuadsAdded* /*event*/); + void ApplyTopologyChange(const core::topology::QuadsAdded* topoEvent); /// Callback to remove quads elements. - void ApplyTopologyChange(const core::topology::QuadsRemoved* /*event*/); + void ApplyTopologyChange(const core::topology::QuadsRemoved* topoEvent); ///////////////////////// Functions on Tetrahedron ////////////////////////////////////// /// Mass coefficient Creation/Destruction functions for Tetrahedral Mesh: /// Vertex coefficient of mass matrix creation function to handle creation of new tetrahedra - void applyTetrahedronCreation(const sofa::type::vector< Index >& /*indices*/, - const sofa::type::vector< core::topology::BaseMeshTopology::Tetrahedron >& /*elems*/, - const sofa::type::vector< sofa::type::vector< Index > >& /*ancestors*/, - const sofa::type::vector< sofa::type::vector< double > >& /*coefs*/); + void applyTetrahedronCreation(const sofa::type::vector< Index >& tetrahedronAdded, + const sofa::type::vector< core::topology::BaseMeshTopology::Tetrahedron >& elems, + const sofa::type::vector< sofa::type::vector< Index > >& ancestors, + const sofa::type::vector< sofa::type::vector< double > >& coefs); /// Vertex coefficient of mass matrix destruction function to handle creation of new tetrahedra - void applyTetrahedronDestruction(const sofa::type::vector & /*indices*/); + void applyTetrahedronDestruction(const sofa::type::vector & tetrahedronRemoved); /// Callback to add tetrahedron elements. - void ApplyTopologyChange(const core::topology::TetrahedraAdded* /*event*/); + void ApplyTopologyChange(const core::topology::TetrahedraAdded* topoEvent); /// Callback to remove tetrahedron elements. - void ApplyTopologyChange(const core::topology::TetrahedraRemoved* /*event*/); + void ApplyTopologyChange(const core::topology::TetrahedraRemoved* topoEvent); ///////////////////////// Functions on Hexahedron ////////////////////////////////////// /// Mass coefficient Creation/Destruction functions for Hexahedral Mesh: /// Vertex coefficient of mass matrix creation function to handle creation of new hexahedra - void applyHexahedronCreation(const sofa::type::vector< Index >& /*indices*/, - const sofa::type::vector< core::topology::BaseMeshTopology::Hexahedron >& /*elems*/, - const sofa::type::vector< sofa::type::vector< Index > >& /*ancestors*/, - const sofa::type::vector< sofa::type::vector< double > >& /*coefs*/); + void applyHexahedronCreation(const sofa::type::vector< Index >& hexahedronAdded, + const sofa::type::vector< core::topology::BaseMeshTopology::Hexahedron >& elems, + const sofa::type::vector< sofa::type::vector< Index > >& ancestors, + const sofa::type::vector< sofa::type::vector< double > >& coefs); /// Vertex coefficient of mass matrix destruction function to handle creation of new hexahedra - void applyHexahedronDestruction(const sofa::type::vector & /*indices*/); + void applyHexahedronDestruction(const sofa::type::vector & hexahedronRemoved); /// Callback to add hexahedron elements. - virtual void ApplyTopologyChange(const core::topology::HexahedraAdded* /*event*/); + virtual void ApplyTopologyChange(const core::topology::HexahedraAdded* topoEvent); /// Callback to remove hexahedron elements. - virtual void ApplyTopologyChange(const core::topology::HexahedraRemoved* /*event*/); + virtual void ApplyTopologyChange(const core::topology::HexahedraRemoved* topoEvent); protected: MeshMatrixMass* m; @@ -345,69 +349,69 @@ class MeshMatrixMass : public core::behavior::Mass ///////////////////////// Functions on Triangles ////////////////////////////////////// /// Edge coefficient of mass matrix creation function to handle creation of new triangles - void applyTriangleCreation(const sofa::type::vector< Index >& /*indices*/, - const sofa::type::vector< core::topology::BaseMeshTopology::Triangle >& /*elems*/, - const sofa::type::vector< sofa::type::vector< Index > >& /*ancestors*/, - const sofa::type::vector< sofa::type::vector< double > >& /*coefs*/); + void applyTriangleCreation(const sofa::type::vector< Index >& triangleAdded, + const sofa::type::vector< core::topology::BaseMeshTopology::Triangle >& elems, + const sofa::type::vector< sofa::type::vector< Index > >& ancestors, + const sofa::type::vector< sofa::type::vector< double > >& coefs); /// Edge coefficient of mass matrix destruction function to handle creation of new triangles - void applyTriangleDestruction(const sofa::type::vector & /*indices*/); + void applyTriangleDestruction(const sofa::type::vector & triangleRemoved); /// Callback to add triangles elements. - void ApplyTopologyChange(const core::topology::TrianglesAdded* /*event*/); + void ApplyTopologyChange(const core::topology::TrianglesAdded* topoEvent); /// Callback to remove triangles elements. - void ApplyTopologyChange(const core::topology::TrianglesRemoved* /*event*/); + void ApplyTopologyChange(const core::topology::TrianglesRemoved* topoEvent); ///////////////////////// Functions on Quads ////////////////////////////////////// /// Edge coefficient of mass matrix creation function to handle creation of new quads - void applyQuadCreation(const sofa::type::vector< Index >& /*indices*/, - const sofa::type::vector< core::topology::BaseMeshTopology::Quad >& /*elems*/, - const sofa::type::vector< sofa::type::vector< Index > >& /*ancestors*/, - const sofa::type::vector< sofa::type::vector< double > >& /*coefs*/); + void applyQuadCreation(const sofa::type::vector< Index >& quadAdded, + const sofa::type::vector< core::topology::BaseMeshTopology::Quad >& elems, + const sofa::type::vector< sofa::type::vector< Index > >& ancestors, + const sofa::type::vector< sofa::type::vector< double > >& coefs); /// Edge coefficient of mass matrix destruction function to handle creation of new quads - void applyQuadDestruction(const sofa::type::vector & /*indices*/); + void applyQuadDestruction(const sofa::type::vector & quadRemoved); /// Callback to add quads elements. - void ApplyTopologyChange(const core::topology::QuadsAdded* /*event*/); + void ApplyTopologyChange(const core::topology::QuadsAdded* topoEvent); /// Callback to remove quads elements. - void ApplyTopologyChange(const core::topology::QuadsRemoved* /*event*/); + void ApplyTopologyChange(const core::topology::QuadsRemoved* topoEvent); ///////////////////////// Functions on Tetrahedron ////////////////////////////////////// /// Edge coefficient of mass matrix creation function to handle creation of new tetrahedra - void applyTetrahedronCreation(const sofa::type::vector< Index >& /*indices*/, - const sofa::type::vector< core::topology::BaseMeshTopology::Tetrahedron >& /*elems*/, - const sofa::type::vector< sofa::type::vector< Index > >& /*ancestors*/, - const sofa::type::vector< sofa::type::vector< double > >& /*coefs*/); + void applyTetrahedronCreation(const sofa::type::vector< Index >& tetrahedronAdded, + const sofa::type::vector< core::topology::BaseMeshTopology::Tetrahedron >& elems, + const sofa::type::vector< sofa::type::vector< Index > >& ancestors, + const sofa::type::vector< sofa::type::vector< double > >& coefs); /// Edge coefficient of mass matrix destruction function to handle creation of new tetrahedra - void applyTetrahedronDestruction(const sofa::type::vector & /*indices*/); + void applyTetrahedronDestruction(const sofa::type::vector & tetrahedronRemoved); /// Callback to add tetrahedron elements. - void ApplyTopologyChange(const core::topology::TetrahedraAdded* /*event*/); + void ApplyTopologyChange(const core::topology::TetrahedraAdded* topoEvent); /// Callback to remove tetrahedron elements. - void ApplyTopologyChange(const core::topology::TetrahedraRemoved* /*event*/); + void ApplyTopologyChange(const core::topology::TetrahedraRemoved* topoEvent); ///////////////////////// Functions on Hexahedron ////////////////////////////////////// /// Edge coefficient of mass matrix creation function to handle creation of new hexahedra - void applyHexahedronCreation(const sofa::type::vector< Index >& /*indices*/, - const sofa::type::vector< core::topology::BaseMeshTopology::Hexahedron >& /*elems*/, - const sofa::type::vector< sofa::type::vector< Index > >& /*ancestors*/, - const sofa::type::vector< sofa::type::vector< double > >& /*coefs*/); + void applyHexahedronCreation(const sofa::type::vector< Index >& hexahedronAdded, + const sofa::type::vector< core::topology::BaseMeshTopology::Hexahedron >& elems, + const sofa::type::vector< sofa::type::vector< Index > >& ancestors, + const sofa::type::vector< sofa::type::vector< double > >& coefs); /// Edge coefficient of mass matrix destruction function to handle creation of new hexahedra void applyHexahedronDestruction(const sofa::type::vector & /*indices*/); /// Callback to add hexahedron elements. - void ApplyTopologyChange(const core::topology::HexahedraAdded* /*event*/); + void ApplyTopologyChange(const core::topology::HexahedraAdded* topoEvent); /// Callback to remove hexahedron elements. - void ApplyTopologyChange(const core::topology::HexahedraRemoved* /*event*/); + void ApplyTopologyChange(const core::topology::HexahedraRemoved* topoEvent); protected: MeshMatrixMass* m; diff --git a/modules/SofaMiscForceField/src/SofaMiscForceField/MeshMatrixMass.inl b/modules/SofaMiscForceField/src/SofaMiscForceField/MeshMatrixMass.inl index 5f6dfe0b213..1b5b2726e5b 100644 --- a/modules/SofaMiscForceField/src/SofaMiscForceField/MeshMatrixMass.inl +++ b/modules/SofaMiscForceField/src/SofaMiscForceField/MeshMatrixMass.inl @@ -121,16 +121,23 @@ void MeshMatrixMass::EdgeMassHandler::applyDestroyFunction( /// Creation fonction for mass stored on vertices template< class DataTypes, class MassType> void MeshMatrixMass::VertexMassHandler::applyTriangleCreation(const sofa::type::vector< Index >& triangleAdded, - const sofa::type::vector< core::topology::BaseMeshTopology::Triangle >& /*elems*/, - const sofa::type::vector< sofa::type::vector< Index > >& /*ancestors*/, - const sofa::type::vector< sofa::type::vector< double > >& /*coefs*/) + const sofa::type::vector< core::topology::BaseMeshTopology::Triangle >& elems, + const sofa::type::vector< sofa::type::vector< Index > >& ancestors, + const sofa::type::vector< sofa::type::vector< double > >& coefs) { + SOFA_UNUSED(elems); MeshMatrixMass *MMM = this->m; if (MMM && MMM->getMassTopologyType() == TopologyElementType::TRIANGLE) { helper::WriteAccessor< Data< type::vector > > VertexMasses ( MMM->d_vertexMass ); - helper::WriteAccessor > totalMass(MMM->d_totalMass); + helper::WriteAccessor > totalMass(MMM->d_totalMass); + + // update mass density vector + sofa::Size nbMass = MMM->getMassDensity().size(); + sofa::Size nbTri = MMM->m_topology->getNbTriangles(); + if (nbMass < nbTri) + MMM->addMassDensity(triangleAdded, ancestors, coefs); // Initialisation const type::vector densityM = MMM->getMassDensity(); @@ -167,10 +174,11 @@ void MeshMatrixMass::VertexMassHandler::applyTriangleCreati /// Creation fonction for mass stored on edges template< class DataTypes, class MassType> void MeshMatrixMass::EdgeMassHandler::applyTriangleCreation(const sofa::type::vector< Index >& triangleAdded, - const sofa::type::vector< core::topology::BaseMeshTopology::Triangle >& /*elems*/, - const sofa::type::vector< sofa::type::vector< Index > >& /*ancestors*/, - const sofa::type::vector< sofa::type::vector< double > >& /*coefs*/) + const sofa::type::vector< core::topology::BaseMeshTopology::Triangle >& elems, + const sofa::type::vector< sofa::type::vector< Index > >& ancestors, + const sofa::type::vector< sofa::type::vector< double > >& coefs) { + SOFA_UNUSED(elems); MeshMatrixMass *MMM = this->m; if (MMM && MMM->getMassTopologyType() == TopologyElementType::TRIANGLE) @@ -178,6 +186,12 @@ void MeshMatrixMass::EdgeMassHandler::applyTriangleCreation helper::WriteAccessor< Data< type::vector > > EdgeMasses ( MMM->d_edgeMass ); helper::WriteAccessor > totalMass(MMM->d_totalMass); + // update mass density vector + sofa::Size nbMass = MMM->getMassDensity().size(); + sofa::Size nbTri = MMM->m_topology->getNbTriangles(); + if (nbMass < nbTri) + MMM->addMassDensity(triangleAdded, ancestors, coefs); + // Initialisation const type::vector densityM = MMM->getMassDensity(); typename DataTypes::Real mass = typename DataTypes::Real(0); @@ -295,12 +309,12 @@ void MeshMatrixMass::EdgeMassHandler::applyTriangleDestruct } template< class DataTypes, class MassType> -void MeshMatrixMass::VertexMassHandler::ApplyTopologyChange(const core::topology::TrianglesAdded* e) +void MeshMatrixMass::VertexMassHandler::ApplyTopologyChange(const core::topology::TrianglesAdded* topoEvent) { - const auto &triangleAdded = e->getIndexArray(); - const sofa::type::vector &elems = e->getElementArray(); - const auto & ancestors = e->ancestorsList; - const sofa::type::vector > & coefs = e->coefs; + const auto &triangleAdded = topoEvent->getIndexArray(); + const sofa::type::vector &elems = topoEvent->getElementArray(); + const auto & ancestors = topoEvent->ancestorsList; + const sofa::type::vector > & coefs = topoEvent->coefs; applyTriangleCreation(triangleAdded, elems, ancestors, coefs); } @@ -314,12 +328,12 @@ void MeshMatrixMass::VertexMassHandler::ApplyTopologyChange } template< class DataTypes, class MassType> -void MeshMatrixMass::EdgeMassHandler::ApplyTopologyChange(const core::topology::TrianglesAdded* e) +void MeshMatrixMass::EdgeMassHandler::ApplyTopologyChange(const core::topology::TrianglesAdded* topoEvent) { - const auto &triangleAdded = e->getIndexArray(); - const sofa::type::vector &elems = e->getElementArray(); - const auto & ancestors = e->ancestorsList; - const sofa::type::vector > & coefs = e->coefs; + const auto &triangleAdded = topoEvent->getIndexArray(); + const sofa::type::vector &elems = topoEvent->getElementArray(); + const auto & ancestors = topoEvent->ancestorsList; + const sofa::type::vector > & coefs = topoEvent->coefs; applyTriangleCreation(triangleAdded, elems, ancestors, coefs); } @@ -342,10 +356,11 @@ void MeshMatrixMass::EdgeMassHandler::ApplyTopologyChange(c /// Creation fonction for mass stored on vertices template< class DataTypes, class MassType> void MeshMatrixMass::VertexMassHandler::applyQuadCreation(const sofa::type::vector< Index >& quadAdded, - const sofa::type::vector< core::topology::BaseMeshTopology::Quad >& /*elems*/, - const sofa::type::vector< sofa::type::vector< Index > >& /*ancestors*/, - const sofa::type::vector< sofa::type::vector< double > >& /*coefs*/) + const sofa::type::vector< core::topology::BaseMeshTopology::Quad >& elems, + const sofa::type::vector< sofa::type::vector< Index > >& ancestors, + const sofa::type::vector< sofa::type::vector< double > >& coefs) { + SOFA_UNUSED(elems); MeshMatrixMass *MMM = this->m; if (MMM && MMM->getMassTopologyType() == TopologyElementType::QUAD) @@ -353,6 +368,12 @@ void MeshMatrixMass::VertexMassHandler::applyQuadCreation(c helper::WriteAccessor< Data< type::vector > > VertexMasses ( MMM->d_vertexMass ); helper::WriteAccessor > totalMass(MMM->d_totalMass); + // update mass density vector + sofa::Size nbMass = MMM->getMassDensity().size(); + sofa::Size nbQ = MMM->m_topology->getNbQuads(); + if (nbMass < nbQ) + MMM->addMassDensity(quadAdded, ancestors, coefs); + // Initialisation const type::vector densityM = MMM->getMassDensity(); typename DataTypes::Real mass = typename DataTypes::Real(0); @@ -388,16 +409,23 @@ void MeshMatrixMass::VertexMassHandler::applyQuadCreation(c /// Creation fonction for mass stored on edges template< class DataTypes, class MassType> void MeshMatrixMass::EdgeMassHandler::applyQuadCreation(const sofa::type::vector< Index >& quadAdded, - const sofa::type::vector< core::topology::BaseMeshTopology::Quad >& /*elems*/, - const sofa::type::vector< sofa::type::vector< Index > >& /*ancestors*/, - const sofa::type::vector< sofa::type::vector< double > >& /*coefs*/) + const sofa::type::vector< core::topology::BaseMeshTopology::Quad >& elems, + const sofa::type::vector< sofa::type::vector< Index > >& ancestors, + const sofa::type::vector< sofa::type::vector< double > >& coefs) { + SOFA_UNUSED(elems); MeshMatrixMass *MMM = this->m; if (MMM && MMM->getMassTopologyType() == TopologyElementType::QUAD) { helper::WriteAccessor< Data< type::vector > > EdgeMasses ( MMM->d_edgeMass ); helper::WriteAccessor > totalMass(MMM->d_totalMass); + + // update mass density vector + sofa::Size nbMass = MMM->getMassDensity().size(); + sofa::Size nbQ = MMM->m_topology->getNbQuads(); + if (nbMass < nbQ) + MMM->addMassDensity(quadAdded, ancestors, coefs); // Initialisation const type::vector densityM = MMM->getMassDensity(); @@ -516,39 +544,39 @@ void MeshMatrixMass::EdgeMassHandler::applyQuadDestruction( } template< class DataTypes, class MassType> -void MeshMatrixMass::VertexMassHandler::ApplyTopologyChange(const core::topology::QuadsAdded* e) +void MeshMatrixMass::VertexMassHandler::ApplyTopologyChange(const core::topology::QuadsAdded* topoEvent) { - const auto &quadAdded = e->getIndexArray(); - const sofa::type::vector &elems = e->getElementArray(); - const auto & ancestors = e->ancestorsList; - const sofa::type::vector > & coefs = e->coefs; + const auto &quadAdded = topoEvent->getIndexArray(); + const sofa::type::vector &elems = topoEvent->getElementArray(); + const auto & ancestors = topoEvent->ancestorsList; + const sofa::type::vector > & coefs = topoEvent->coefs; applyQuadCreation(quadAdded, elems, ancestors, coefs); } template< class DataTypes, class MassType> -void MeshMatrixMass::VertexMassHandler::ApplyTopologyChange(const core::topology::QuadsRemoved* e) +void MeshMatrixMass::VertexMassHandler::ApplyTopologyChange(const core::topology::QuadsRemoved* topoEvent) { - const auto &quadRemoved = e->getArray(); + const auto &quadRemoved = topoEvent->getArray(); applyQuadDestruction(quadRemoved); } template< class DataTypes, class MassType> -void MeshMatrixMass::EdgeMassHandler::ApplyTopologyChange(const core::topology::QuadsAdded* e) +void MeshMatrixMass::EdgeMassHandler::ApplyTopologyChange(const core::topology::QuadsAdded* topoEvent) { - const auto &quadAdded = e->getIndexArray(); - const sofa::type::vector &elems = e->getElementArray(); - const auto& ancestors = e->ancestorsList; - const sofa::type::vector > & coefs = e->coefs; + const auto &quadAdded = topoEvent->getIndexArray(); + const sofa::type::vector &elems = topoEvent->getElementArray(); + const auto& ancestors = topoEvent->ancestorsList; + const sofa::type::vector > & coefs = topoEvent->coefs; applyQuadCreation(quadAdded, elems, ancestors, coefs); } template< class DataTypes, class MassType> -void MeshMatrixMass::EdgeMassHandler::ApplyTopologyChange(const core::topology::QuadsRemoved* e) +void MeshMatrixMass::EdgeMassHandler::ApplyTopologyChange(const core::topology::QuadsRemoved* topoEvent) { - const auto &quadRemoved = e->getArray(); + const auto &quadRemoved = topoEvent->getArray(); applyQuadDestruction(quadRemoved); } @@ -565,10 +593,11 @@ void MeshMatrixMass::EdgeMassHandler::ApplyTopologyChange(c /// Creation fonction for mass stored on vertices template< class DataTypes, class MassType> void MeshMatrixMass::VertexMassHandler::applyTetrahedronCreation(const sofa::type::vector< Index >& tetrahedronAdded, - const sofa::type::vector< core::topology::BaseMeshTopology::Tetrahedron >& /*elems*/, - const sofa::type::vector< sofa::type::vector< Index > >& /*ancestors*/, - const sofa::type::vector< sofa::type::vector< double > >& /*coefs*/) + const sofa::type::vector< core::topology::BaseMeshTopology::Tetrahedron >& elems, + const sofa::type::vector< sofa::type::vector< Index > >& ancestors, + const sofa::type::vector< sofa::type::vector< double > >& coefs) { + SOFA_UNUSED(elems); MeshMatrixMass *MMM = this->m; if (MMM && MMM->getMassTopologyType() == TopologyElementType::TETRAHEDRON) @@ -576,6 +605,12 @@ void MeshMatrixMass::VertexMassHandler::applyTetrahedronCre helper::WriteAccessor< Data< type::vector > > VertexMasses ( MMM->d_vertexMass ); helper::WriteAccessor > totalMass(MMM->d_totalMass); + // update mass density vector + sofa::Size nbMass = MMM->getMassDensity().size(); + sofa::Size nbT = MMM->m_topology->getNbTetrahedra(); + if (nbMass < nbT) + MMM->addMassDensity(tetrahedronAdded, ancestors, coefs); + // Initialisation const type::vector densityM = MMM->getMassDensity(); typename DataTypes::Real mass = typename DataTypes::Real(0); @@ -611,16 +646,23 @@ void MeshMatrixMass::VertexMassHandler::applyTetrahedronCre /// Creation fonction for mass stored on edges template< class DataTypes, class MassType> void MeshMatrixMass::EdgeMassHandler::applyTetrahedronCreation(const sofa::type::vector< Index >& tetrahedronAdded, - const sofa::type::vector< core::topology::BaseMeshTopology::Tetrahedron >& /*elems*/, - const sofa::type::vector< sofa::type::vector< Index > >& /*ancestors*/, - const sofa::type::vector< sofa::type::vector< double > >& /*coefs*/) + const sofa::type::vector< core::topology::BaseMeshTopology::Tetrahedron >& elems, + const sofa::type::vector< sofa::type::vector< Index > >& ancestors, + const sofa::type::vector< sofa::type::vector< double > >& coefs) { + SOFA_UNUSED(elems); MeshMatrixMass *MMM = this->m; if (MMM && MMM->getMassTopologyType() == TopologyElementType::TETRAHEDRON) { helper::WriteAccessor< Data< type::vector > > EdgeMasses ( MMM->d_edgeMass ); helper::WriteAccessor > totalMass(MMM->d_totalMass); + + // update mass density vector + sofa::Size nbMass = MMM->getMassDensity().size(); + sofa::Size nbT = MMM->m_topology->getNbTetrahedra(); + if (nbMass < nbT) + MMM->addMassDensity(tetrahedronAdded, ancestors, coefs); // Initialisation const type::vector densityM = MMM->getMassDensity(); @@ -739,39 +781,39 @@ void MeshMatrixMass::EdgeMassHandler::applyTetrahedronDestr } template< class DataTypes, class MassType> -void MeshMatrixMass::VertexMassHandler::ApplyTopologyChange(const core::topology::TetrahedraAdded* e) +void MeshMatrixMass::VertexMassHandler::ApplyTopologyChange(const core::topology::TetrahedraAdded* topoEvent) { - const auto &tetraAdded = e->getIndexArray(); - const sofa::type::vector &elems = e->getElementArray(); - const auto & ancestors = e->ancestorsList; - const sofa::type::vector > & coefs = e->coefs; + const auto &tetraAdded = topoEvent->getIndexArray(); + const sofa::type::vector &elems = topoEvent->getElementArray(); + const auto & ancestors = topoEvent->ancestorsList; + const sofa::type::vector > & coefs = topoEvent->coefs; applyTetrahedronCreation(tetraAdded, elems, ancestors, coefs); } template< class DataTypes, class MassType> -void MeshMatrixMass::VertexMassHandler::ApplyTopologyChange(const core::topology::TetrahedraRemoved* e) +void MeshMatrixMass::VertexMassHandler::ApplyTopologyChange(const core::topology::TetrahedraRemoved* topoEvent) { - const auto &tetraRemoved = e->getArray(); + const auto &tetraRemoved = topoEvent->getArray(); applyTetrahedronDestruction(tetraRemoved); } template< class DataTypes, class MassType> -void MeshMatrixMass::EdgeMassHandler::ApplyTopologyChange(const core::topology::TetrahedraAdded* e) +void MeshMatrixMass::EdgeMassHandler::ApplyTopologyChange(const core::topology::TetrahedraAdded* topoEvent) { - const auto &tetraAdded = e->getIndexArray(); - const sofa::type::vector &elems = e->getElementArray(); - const auto& ancestors = e->ancestorsList; - const sofa::type::vector > & coefs = e->coefs; + const auto &tetraAdded = topoEvent->getIndexArray(); + const sofa::type::vector &elems = topoEvent->getElementArray(); + const auto& ancestors = topoEvent->ancestorsList; + const sofa::type::vector > & coefs = topoEvent->coefs; applyTetrahedronCreation(tetraAdded, elems, ancestors, coefs); } template< class DataTypes, class MassType> -void MeshMatrixMass::EdgeMassHandler::ApplyTopologyChange(const core::topology::TetrahedraRemoved* e) +void MeshMatrixMass::EdgeMassHandler::ApplyTopologyChange(const core::topology::TetrahedraRemoved* topoEvent) { - const auto& tetraRemoved = e->getArray(); + const auto& tetraRemoved = topoEvent->getArray(); applyTetrahedronDestruction(tetraRemoved); } @@ -787,16 +829,23 @@ void MeshMatrixMass::EdgeMassHandler::ApplyTopologyChange(c /// Creation fonction for mass stored on vertices template< class DataTypes, class MassType> void MeshMatrixMass::VertexMassHandler::applyHexahedronCreation(const sofa::type::vector< Index >& hexahedronAdded, - const sofa::type::vector< core::topology::BaseMeshTopology::Hexahedron >& /*elems*/, - const sofa::type::vector< sofa::type::vector< Index > >& /*ancestors*/, - const sofa::type::vector< sofa::type::vector< double > >& /*coefs*/) + const sofa::type::vector< core::topology::BaseMeshTopology::Hexahedron >& elems, + const sofa::type::vector< sofa::type::vector< Index > >& ancestors, + const sofa::type::vector< sofa::type::vector< double > >& coefs) { + SOFA_UNUSED(elems); MeshMatrixMass *MMM = this->m; if (MMM && MMM->getMassTopologyType() == TopologyElementType::HEXAHEDRON) { helper::WriteAccessor< Data< type::vector > > VertexMasses ( MMM->d_vertexMass ); helper::WriteAccessor > totalMass(MMM->d_totalMass); + + // update mass density vector + sofa::Size nbMass = MMM->getMassDensity().size(); + sofa::Size nbT = MMM->m_topology->getNbHexahedra(); + if (nbMass < nbT) + MMM->addMassDensity(hexahedronAdded, ancestors, coefs); // Initialisation const type::vector densityM = MMM->getMassDensity(); @@ -833,10 +882,11 @@ void MeshMatrixMass::VertexMassHandler::applyHexahedronCrea /// Creation fonction for mass stored on edges template< class DataTypes, class MassType> void MeshMatrixMass::EdgeMassHandler::applyHexahedronCreation(const sofa::type::vector< Index >& hexahedronAdded, - const sofa::type::vector< core::topology::BaseMeshTopology::Hexahedron >& /*elems*/, - const sofa::type::vector< sofa::type::vector< Index > >& /*ancestors*/, - const sofa::type::vector< sofa::type::vector< double > >& /*coefs*/) + const sofa::type::vector< core::topology::BaseMeshTopology::Hexahedron >& elems, + const sofa::type::vector< sofa::type::vector< Index > >& ancestors, + const sofa::type::vector< sofa::type::vector< double > >& coefs) { + SOFA_UNUSED(elems); MeshMatrixMass *MMM = this->m; if (MMM && MMM->getMassTopologyType() == TopologyElementType::HEXAHEDRON) @@ -844,6 +894,12 @@ void MeshMatrixMass::EdgeMassHandler::applyHexahedronCreati helper::WriteAccessor< Data< type::vector > > EdgeMasses ( MMM->d_edgeMass ); helper::WriteAccessor > totalMass(MMM->d_totalMass); + // update mass density vector + sofa::Size nbMass = MMM->getMassDensity().size(); + sofa::Size nbT = MMM->m_topology->getNbHexahedra(); + if (nbMass < nbT) + MMM->addMassDensity(hexahedronAdded, ancestors, coefs); + // Initialisation const type::vector densityM = MMM->getMassDensity(); typename DataTypes::Real mass = typename DataTypes::Real(0); @@ -961,39 +1017,39 @@ void MeshMatrixMass::EdgeMassHandler::applyHexahedronDestru } template< class DataTypes, class MassType> -void MeshMatrixMass::VertexMassHandler::ApplyTopologyChange(const core::topology::HexahedraAdded* e) +void MeshMatrixMass::VertexMassHandler::ApplyTopologyChange(const core::topology::HexahedraAdded* topoEvent) { - const auto &hexaAdded = e->getIndexArray(); - const sofa::type::vector &elems = e->getElementArray(); - const auto & ancestors = e->ancestorsList; - const sofa::type::vector > & coefs = e->coefs; + const auto &hexaAdded = topoEvent->getIndexArray(); + const sofa::type::vector &elems = topoEvent->getElementArray(); + const auto & ancestors = topoEvent->ancestorsList; + const sofa::type::vector > & coefs = topoEvent->coefs; applyHexahedronCreation(hexaAdded, elems, ancestors, coefs); } template< class DataTypes, class MassType> -void MeshMatrixMass::VertexMassHandler::ApplyTopologyChange(const core::topology::HexahedraRemoved* e) +void MeshMatrixMass::VertexMassHandler::ApplyTopologyChange(const core::topology::HexahedraRemoved* topoEvent) { - const auto &hexaRemoved = e->getArray(); + const auto &hexaRemoved = topoEvent->getArray(); applyHexahedronDestruction(hexaRemoved); } template< class DataTypes, class MassType> -void MeshMatrixMass::EdgeMassHandler::ApplyTopologyChange(const core::topology::HexahedraAdded* e) +void MeshMatrixMass::EdgeMassHandler::ApplyTopologyChange(const core::topology::HexahedraAdded* topoEvent) { - const auto &hexaAdded = e->getIndexArray(); - const sofa::type::vector &elems = e->getElementArray(); - const auto & ancestors = e->ancestorsList; - const sofa::type::vector > & coefs = e->coefs; + const auto &hexaAdded = topoEvent->getIndexArray(); + const sofa::type::vector &elems = topoEvent->getElementArray(); + const auto & ancestors = topoEvent->ancestorsList; + const sofa::type::vector > & coefs = topoEvent->coefs; applyHexahedronCreation(hexaAdded, elems, ancestors, coefs); } template< class DataTypes, class MassType> -void MeshMatrixMass::EdgeMassHandler::ApplyTopologyChange(const core::topology::HexahedraRemoved* e) +void MeshMatrixMass::EdgeMassHandler::ApplyTopologyChange(const core::topology::HexahedraRemoved* topoEvent) { - const auto &hexaRemoved = e->getArray(); + const auto &hexaRemoved = topoEvent->getArray(); applyHexahedronDestruction(hexaRemoved); } @@ -1864,6 +1920,32 @@ void MeshMatrixMass::setMassDensity(Real massDensityValue) } } +template +void MeshMatrixMass::addMassDensity(const sofa::type::vector< Index >& indices, + const sofa::type::vector< sofa::type::vector< Index > >& ancestors, + const sofa::type::vector< sofa::type::vector< double > >& coefs) +{ + helper::WriteOnlyAccessor > > massDensity = d_massDensity; + for (unsigned int id = 0; id < indices.size(); ++id) + { + // if no ancestor apply the first density of the vector + if (ancestors[id].empty()) + { + massDensity.push_back(massDensity[0]); + continue; + } + + // Accumulate mass of their neighbours + Real massD = 0.0; + for (unsigned int j = 0; j < ancestors[id].size(); ++j) + { + Index ancId = ancestors[id][j]; + double coef = coefs[id][j]; + massD += massDensity[ancId] * coef; + } + massDensity.push_back(massD); + } +} template void MeshMatrixMass::setTotalMass(Real totalMass) @@ -2115,7 +2197,7 @@ void MeshMatrixMass::addMToMatrix(const core::MechanicalPar const MassVector &edgeMass= d_edgeMass.getValue(); size_t nbEdges=m_topology->getNbEdges(); - size_t v0,v1; + sofa::Index v0,v1; const int N = defaulttype::DataTypeInfo::size(); AddMToMatrixFunctor calc;