Edge2QuadTopologicalMapping.cpp

Go to the documentation of this file.
00001 /******************************************************************************
00002 *       SOFA, Simulation Open-Framework Architecture, version 1.0 beta 3      *
00003 *                (c) 2006-2008 MGH, INRIA, USTL, UJF, CNRS                    *
00004 *                                                                             *
00005 * This library is free software; you can redistribute it and/or modify it     *
00006 * under the terms of the GNU Lesser General Public License as published by    *
00007 * the Free Software Foundation; either version 2.1 of the License, or (at     *
00008 * your option) any later version.                                             *
00009 *                                                                             *
00010 * This library is distributed in the hope that it will be useful, but WITHOUT *
00011 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
00012 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
00013 * for more details.                                                           *
00014 *                                                                             *
00015 * You should have received a copy of the GNU Lesser General Public License    *
00016 * along with this library; if not, write to the Free Software Foundation,     *
00017 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
00018 *******************************************************************************
00019 *                               SOFA :: Modules                               *
00020 *                                                                             *
00021 * Authors: The SOFA Team and external contributors (see Authors.txt)          *
00022 *                                                                             *
00023 * Contact information: contact@sofa-framework.org                             *
00024 ******************************************************************************/
00025 #include <sofa/component/topology/Edge2QuadTopologicalMapping.h>
00026 
00027 #include <sofa/core/ObjectFactory.h>
00028 
00029 #include <sofa/component/topology/QuadSetTopologyModifier.h>
00030 #include <sofa/component/topology/QuadSetTopologyContainer.h>
00031 
00032 #include <sofa/component/topology/EdgeSetTopologyChange.h>
00033 
00034 #include <sofa/component/topology/PointSetTopologyChange.h> 
00035 
00036 #include <sofa/defaulttype/Vec.h>
00037 #include <map>
00038 #include <sofa/defaulttype/VecTypes.h>
00039 
00040 #include <sofa/core/componentmodel/behavior/MechanicalState.h>
00041 #include <sofa/component/MechanicalObject.h>
00042 
00043 #include <math.h>
00044 #include <sofa/defaulttype/Vec.h>
00045 
00046 #include <sofa/defaulttype/RigidTypes.h>
00047 
00048 
00049 namespace sofa
00050 {
00051 
00052 namespace component
00053 {
00054 
00055 namespace topology
00056 {
00057 
00058 using namespace sofa::defaulttype;
00059 
00060 using namespace sofa::component::topology;
00061 using namespace sofa::core::componentmodel::topology;
00062 
00063 SOFA_DECL_CLASS(Edge2QuadTopologicalMapping)
00064 
00065 // Register in the Factory
00066 int Edge2QuadTopologicalMappingClass = core::RegisterObject("Special case of mapping where EdgeSetTopology is converted to QuadSetTopology")
00067 .add< Edge2QuadTopologicalMapping >()
00068 
00069 ;
00070 
00071 // Implementation
00072 
00073 void Edge2QuadTopologicalMapping::init()
00074 {
00075 
00076     unsigned int N = m_nbPointsOnEachCircle.getValue();
00077     double rho = m_radius.getValue();
00078 
00079     //std::cout << "INFO_print : init Edge2QuadTopologicalMapping" << std::endl;
00080 
00081     // INITIALISATION of QUADULAR mesh from EDGE mesh :
00082 
00083     component::MechanicalObject<Rigid3Types>* from_mstate = dynamic_cast<component::MechanicalObject<Rigid3Types>*>(fromModel->getContext()->getMechanicalState());
00084     component::MechanicalObject<Vec3Types>* to_mstate = dynamic_cast<component::MechanicalObject<Vec3Types>*>(toModel->getContext()->getMechanicalState());
00085     
00086     if (fromModel) {
00087         
00088         std::cout << "INFO_print : Edge2QuadTopologicalMapping - from = edge" << std::endl;
00089         
00090         if (toModel) {
00091 
00092             std::cout << "INFO_print : Edge2QuadTopologicalMapping - to = quad" << std::endl;           
00093 
00094             QuadSetTopologyModifier *to_tstm;
00095             toModel->getContext()->get(to_tstm);    
00096 
00097             QuadSetTopologyContainer *to_tstc;
00098             toModel->getContext()->get(to_tstc);    
00099 
00100             const sofa::helper::vector<Edge> &edgeArray=fromModel->getEdges();
00101             
00102             Loc2GlobVec.clear();
00103             In2OutMap.clear();
00104 
00105             // CREATION of the points (new DOFs for the output topology) along the circles around each point of the input topology
00106 
00107             Vec Y0;
00108             Vec Z0;
00109             Y0[0] = (Real) (0.0); Y0[1] = (Real) (1.0); Y0[2] = (Real) (0.0);
00110             Z0[0] = (Real) (0.0); Z0[1] = (Real) (0.0); Z0[2] = (Real) (1.0);
00111 
00112             to_mstate->resize(fromModel->getNbPoints() * N);
00113             to_tstc->clear();
00114 
00115             toModel->setNbPoints(fromModel->getNbPoints() * N);
00116 
00117             for (unsigned int i=0; i<(unsigned int) fromModel->getNbPoints(); ++i) 
00118             {
00119                 unsigned int p0=i;              
00120 
00121                 Mat rotation;
00122                 (*from_mstate->getX())[p0].writeRotationMatrix(rotation);
00123 
00124                 Vec t;
00125                 t=(*from_mstate->getX())[p0].getCenter();                                       
00126 
00127                 Vec Y; 
00128                 Vec Z;
00129 
00130                 Y = rotation * Y0;
00131                 Z = rotation * Z0;                              
00132 
00133                 for(unsigned int j=0; j<N; ++j){
00134 
00135                     Vec x = t + (Y*cos((Real) (2.0*j*M_PI/N)) + Z*sin((Real) (2.0*j*M_PI/N)))*((Real) rho);                 
00136                     
00137                     (*to_mstate->getX())[p0*N+j]=x;                                     
00138                 }                   
00139             }
00140 
00141             // CREATION of the quads based on the the circles
00142 
00143             for (unsigned int i=0; i<edgeArray.size(); ++i) {                   
00144 
00145                     unsigned int p0 = edgeArray[i][0]; 
00146                     unsigned int p1 = edgeArray[i][1];                                      
00147 
00148                     sofa::helper::vector<unsigned int> out_info;                                                    
00149 
00150                     for(unsigned int j=0; j<N; ++j){                        
00151 
00152                         unsigned int q0 = p0*N+j;
00153                         unsigned int q1 = p1*N+j;
00154                         unsigned int q2 = p1*N+((j+1)%N);
00155                         unsigned int q3 = p0*N+((j+1)%N);
00156 
00157                         to_tstm->addQuadProcess(Quad(helper::make_array<unsigned int>((unsigned int) q0, (unsigned int) q1, (unsigned int) q2, (unsigned int) q3)));
00158                         Loc2GlobVec.push_back(i);
00159                         out_info.push_back(Loc2GlobVec.size()-1);
00160                     }                   
00161 
00162                     In2OutMap[i]=out_info;
00163             }           
00164             
00165             to_tstm->notifyEndingEvent();
00166         }
00167         
00168     }
00169 }
00170 
00171 
00172 unsigned int Edge2QuadTopologicalMapping::getFromIndex(unsigned int ind){
00173     return ind; // identity
00174 }
00175 
00176 void Edge2QuadTopologicalMapping::updateTopologicalMapping(){   
00177 
00178     unsigned int N = m_nbPointsOnEachCircle.getValue();
00179 
00180     // INITIALISATION of QUADULAR mesh from EDGE mesh :
00181     
00182     if (fromModel) {    
00183 
00184         QuadSetTopologyModifier *to_tstm;
00185         toModel->getContext()->get(to_tstm);            
00186 
00187         if (toModel) {
00188 
00189             std::list<const TopologyChange *>::const_iterator itBegin=fromModel->firstChange();
00190             std::list<const TopologyChange *>::const_iterator itEnd=fromModel->lastChange();
00191 
00192             while( itBegin != itEnd )
00193             {
00194                 TopologyChangeType changeType = (*itBegin)->getChangeType();
00195 
00196                 switch( changeType ) {
00197 
00198                 case core::componentmodel::topology::ENDING_EVENT:
00199                     {
00200                         //std::cout << "INFO_print : TopologicalMapping - ENDING_EVENT" << std::endl;
00201                         to_tstm->notifyEndingEvent();
00202                         break;
00203                     }
00204 
00205                 case core::componentmodel::topology::EDGESADDED:
00206                     {                       
00207                         //std::cout << "INFO_print : TopologicalMapping - EDGESADDED" << std::endl;
00208                         if (fromModel) {
00209 
00210                             const sofa::helper::vector<Edge> &edgeArray=fromModel->getEdges();
00211 
00212                             const sofa::helper::vector<unsigned int> &tab = ( static_cast< const EdgesAdded *>( *itBegin ) )->edgeIndexArray;
00213 
00214                             sofa::helper::vector< Quad > quads_to_create;
00215                             sofa::helper::vector< unsigned int > quadsIndexList;
00216                             int nb_elems = toModel->getNbQuads();
00217 
00218                             for (unsigned int i = 0; i < tab.size(); ++i)
00219                             {                                           
00220                                 unsigned int k = tab[i];
00221                                 
00222                                 unsigned int p0 = edgeArray[k][0]; 
00223                                 unsigned int p1 = edgeArray[k][1];
00224 
00225                                 sofa::helper::vector<unsigned int> out_info;
00226 
00227                                 for(unsigned int j=0; j<N; ++j){
00228 
00229                                     unsigned int q0 = p0*N+j;
00230                                     unsigned int q1 = p1*N+j;
00231                                     unsigned int q2 = p1*N+((j+1)%N);
00232                                     unsigned int q3 = p0*N+((j+1)%N);
00233                                     
00234                                     Quad t = Quad(helper::make_array<unsigned int>((unsigned int) q0, (unsigned int) q1, (unsigned int) q2, (unsigned int) q3));
00235                                     
00236                                     //quads_to_create.clear();
00237                                     //quadsIndexList.clear();
00238 
00239                                     quads_to_create.push_back(t);
00240                                     quadsIndexList.push_back(nb_elems);
00241                                     nb_elems+=1;
00242 
00243                                     Loc2GlobVec.push_back(k);
00244                                     out_info.push_back(Loc2GlobVec.size()-1);
00245 
00246                                     //to_tstm->addQuadsProcess(quads_to_create) ;                           
00247                                     //to_tstm->addQuadsWarning(quads_to_create.size(), quads_to_create, quadsIndexList) ;                           
00248                                     //to_tstm->propagateTopologicalChanges();   
00249                                 }
00250                                 
00251                                 In2OutMap[k]=out_info;
00252                             }
00253                             
00254                             to_tstm->addQuadsProcess(quads_to_create) ;                         
00255                             to_tstm->addQuadsWarning(quads_to_create.size(), quads_to_create, quadsIndexList) ;                         
00256                             to_tstm->propagateTopologicalChanges();                             
00257                         }
00258                         break;
00259                     }
00260                 case core::componentmodel::topology::EDGESREMOVED:
00261                     {
00262                         //std::cout << "INFO_print : TopologicalMapping - EDGESREMOVED" << std::endl;
00263 
00264                         if (fromModel) {
00265 
00266                             const sofa::helper::vector<unsigned int> &tab = ( static_cast< const EdgesRemoved *>( *itBegin ) )->getArray();
00267 
00268                             int last= fromModel->getNbEdges() - 1;
00269 
00270                             unsigned int ind_tmp;
00271 
00272                             sofa::helper::vector<unsigned int> ind_real_last;
00273                             int ind_last=toModel->getNbQuads();
00274 
00275                             for (unsigned int i = 0; i < tab.size(); ++i)
00276                             {                               
00277                                 unsigned int k = tab[i];
00278                                 sofa::helper::vector<unsigned int> ind_k;   
00279 
00280                                 std::map<unsigned int, sofa::helper::vector<unsigned int> >::iterator iter_1 = In2OutMap.find(k);
00281                                 if(iter_1 != In2OutMap.end()) {                                 
00282 
00283                                     sofa::helper::vector<unsigned int> ind_list;
00284                                     for(unsigned int j=0; j<N; ++j){
00285                                         ind_list.push_back(In2OutMap[k][j]);
00286                                     }                                                                       
00287 
00288                                     ind_last = ind_last - 1;
00289 
00290                                     ind_k = In2OutMap[k];
00291                                     ind_real_last = ind_k;
00292 
00293                                     std::map<unsigned int, sofa::helper::vector<unsigned int> >::iterator iter_2 = In2OutMap.find(last);
00294                                     if(iter_2 != In2OutMap.end()) {                                             
00295 
00296                                         ind_real_last = In2OutMap[last];                                                                                                        
00297 
00298                                         if((int) k != last){                                                                                        
00299 
00300                                             In2OutMap.erase(In2OutMap.find(k));
00301                                             In2OutMap[k] = ind_real_last;
00302 
00303                                             In2OutMap.erase(In2OutMap.find(last));
00304                                             In2OutMap[last] = ind_k;                                            
00305 
00306                                             for(unsigned int j=0; j<N; ++j){                                                
00307 
00308                                                 ind_tmp = Loc2GlobVec[ind_real_last[j]];
00309                                                 Loc2GlobVec[ind_real_last[j]] = Loc2GlobVec[ind_k[j]];  
00310                                                 Loc2GlobVec[ind_k[j]] = ind_tmp;
00311                                             }                                                                                                                               
00312                                         }                                   
00313                                     }else{                              
00314                                         std::cout << "INFO_print : Edge2QuadTopologicalMapping - In2OutMap should have the edge " << last << std::endl;         
00315                                     }
00316 
00317                                     if( (int) ind_k[N-1] != ind_last){                                          
00318 
00319                                         In2OutMap.erase(In2OutMap.find(Loc2GlobVec[ind_last]));
00320                                         In2OutMap[Loc2GlobVec[ind_last]] = ind_k;                                                                           
00321 
00322                                         sofa::helper::vector<unsigned int> out_info;
00323                                         for(unsigned int j=0; j<N; ++j){
00324                                             out_info.push_back(ind_last-j);                                         
00325                                         }                                                                           
00326 
00327                                         In2OutMap.erase(In2OutMap.find(Loc2GlobVec[ind_k[N-1]]));
00328                                         In2OutMap[Loc2GlobVec[ind_k[N-1]]] = out_info;
00329 
00330                                         ind_tmp = Loc2GlobVec[ind_k[N-1]];
00331                                         Loc2GlobVec[ind_k[N-1]] = Loc2GlobVec[ind_last];
00332                                         Loc2GlobVec[ind_last] = ind_tmp;                                        
00333 
00334                                     }
00335 
00336                                     for(unsigned int j=1; j<N; ++j){                                                
00337 
00338                                         ind_last = ind_last-1;
00339 
00340                                         if( (int) ind_k[N-1-j] != ind_last){                                                    
00341 
00342                                             ind_tmp = Loc2GlobVec[ind_k[N-1-j]];
00343                                             Loc2GlobVec[ind_k[N-1-j]] = Loc2GlobVec[ind_last];
00344                                             Loc2GlobVec[ind_last] = ind_tmp;                                            
00345                                         }                                   
00346                                     }                                                                                                   
00347 
00348                                     In2OutMap.erase(In2OutMap.find(Loc2GlobVec[Loc2GlobVec.size() - 1]));                                   
00349                                     
00350                                     Loc2GlobVec.resize( Loc2GlobVec.size() - N );                                                                                                           
00351 
00352                                     sofa::helper::vector< unsigned int > quads_to_remove;
00353                                     for(unsigned int j=0; j<N; ++j){                                        
00354                                         quads_to_remove.push_back(ind_list[j]);                                                 
00355                                     }
00356                                                                                                             
00357                                     to_tstm->removeQuads(quads_to_remove, true, true);                                          
00358 
00359                                 }else{
00360                                     std::cout << "INFO_print : Edge2QuadTopologicalMapping - In2OutMap should have the edge " << k << std::endl;
00361                                 }
00362                                 
00363                                 --last;
00364                             }                       
00365                         }                       
00366 
00367                         break;
00368                     }
00369                     
00370                     case core::componentmodel::topology::POINTSRENUMBERING:
00371                     {
00372                         //std::cout << "INFO_print : Edge2QuadTopologicalMapping - POINTSRENUMBERING" << std::endl;
00373                         
00374                         const sofa::helper::vector<unsigned int> &tab = ( static_cast< const PointsRenumbering * >( *itBegin ) )->getIndexArray();
00375                         const sofa::helper::vector<unsigned int> &inv_tab = ( static_cast< const PointsRenumbering * >( *itBegin ) )->getinv_IndexArray();
00376 
00377                         sofa::helper::vector<unsigned int> indices;
00378                         sofa::helper::vector<unsigned int> inv_indices;
00379 
00380                         for(unsigned int i = 0; i < tab.size(); ++i){
00381 
00382                             for(unsigned int j=0; j<N; ++j){
00383                                 indices.push_back(tab[i]*N + j);
00384                                 inv_indices.push_back(inv_tab[i]*N + j);
00385                             }
00386 
00387                             //std::cout << "INFO_print : Edge2QuadTopologicalMapping - renumber point = " << tab[i] << std::endl;                                                       
00388                         }
00389 
00390                         sofa::helper::vector<unsigned int>& tab_indices = indices;
00391                         sofa::helper::vector<unsigned int>& inv_tab_indices = inv_indices;
00392                                                 
00393                         to_tstm->renumberPointsWarning(tab_indices, inv_tab_indices, true);                             
00394                         to_tstm->propagateTopologicalChanges();                         
00395                         to_tstm->renumberPointsProcess(tab_indices, inv_tab_indices, true);                         
00396 
00397                         break;
00398                     }
00399 
00400                     case core::componentmodel::topology::POINTSADDED:
00401                     {
00402                         //std::cout << "INFO_print : Edge2QuadTopologicalMapping - POINTSADDED" << std::endl;                       
00403 
00404                         const sofa::component::topology::PointsAdded *ta=static_cast< const sofa::component::topology::PointsAdded * >( *itBegin );
00405 
00406                         unsigned int to_nVertices = ta->getNbAddedVertices() * N;
00407                         sofa::helper::vector< sofa::helper::vector< unsigned int > > to_ancestorsList;
00408                         sofa::helper::vector< sofa::helper::vector< double > > to_coefs;
00409 
00410                         for(unsigned int i =0; i < ta->getNbAddedVertices(); i++){                              
00411 
00412                             sofa::helper::vector< unsigned int > my_ancestors;
00413                             sofa::helper::vector< double > my_coefs;
00414 
00415                             for(unsigned int j =0; j < N; j++){
00416 
00417                                 for(unsigned int k = 0; k < ta->ancestorsList[i].size(); k++){
00418                                     my_ancestors.push_back(ta->ancestorsList[i][k]*N + j);
00419                                 }
00420                                 for(unsigned int k = 0; k < ta->coefs[i].size(); k++){
00421                                     my_coefs.push_back(ta->coefs[i][k]*N + j);
00422                                 }
00423                                 
00424                                 to_ancestorsList.push_back(my_ancestors);
00425                                 to_coefs.push_back(my_coefs);
00426                             }
00427                         }
00428                         
00429                         to_tstm->addPointsProcess(to_nVertices);                                
00430                         to_tstm->addPointsWarning(to_nVertices, to_ancestorsList, to_coefs, true);                      
00431                         to_tstm->propagateTopologicalChanges();                                 
00432 
00433                         break;
00434                     }           
00435                 
00436                 default:
00437                     // Ignore events that are not Quad  related.
00438                     break;
00439                 };              
00440 
00441                 ++itBegin;
00442             }
00443             to_tstm->propagateTopologicalChanges();
00444         }
00445     }
00446 
00447     return;
00448 }
00449 
00450 } // namespace topology
00451 
00452 } // namespace component
00453 
00454 } // namespace sofa
00455