 |
TAPs 0.7.7.3
|
|
TAPs 0.7.7.3
|
00001 /****************************************************************************** 00002 TAPsAdvSimSupport_DS.cpp 00003 ******************************************************************************/ 00007 /****************************************************************************** 00008 SUKITTI PUNAK (09/29/2008) 00009 UPDATE (03/15/2011) 00010 ******************************************************************************/ 00011 #include "TAPsAdvSimSupport_DS.hpp" 00012 // Using Inclusion Model (i.e. definitions are included in declarations) 00013 // (this name.cpp is included in name.hpp) 00014 // Each friend is defined directly inside its declaration. 00015 00016 BEGIN_NAMESPACE_TAPs 00017 //============================================================================= 00018 //----------------------------------------------------------------------------- 00019 template <typename T, typename DATA> 00020 AdvSimSupport_DS<T,DATA>::AdvSimSupport_DS () 00021 : m_OffsetToSutureLastSlidablePoint( 1 ) 00022 {} 00023 //----------------------------------------------------------------------------- 00024 template <typename T, typename DATA> 00025 AdvSimSupport_DS<T,DATA>::~AdvSimSupport_DS () 00026 { 00027 ClearAllConstraints(); 00028 } 00029 //----------------------------------------------------------------------------- 00030 template <typename T, typename DATA> 00031 std::string AdvSimSupport_DS<T,DATA>::StrInfo () const 00032 { 00033 std::stringstream ss; 00034 ss << "AdvSimSupport_DS<T,DATA>\n"; 00035 return ss.str(); 00036 } 00037 //----------------------------------------------------------------------------- 00038 //============================================================================= 00039 00040 00041 00042 00043 //============================================================================= 00044 // Specific for suture models 00045 //----------------------------------------------------------------------------- 00046 template <typename T, typename DATA> 00047 unsigned int AdvSimSupport_DS<T,DATA>::GetTheOffsetToSutureLastSlidablePoint () const 00048 { 00049 return m_OffsetToSutureLastSlidablePoint; 00050 } 00051 //----------------------------------------------------------------------------- 00052 template <typename T, typename DATA> 00053 void AdvSimSupport_DS<T,DATA>::SetTheOffsetToSutureLastSlidablePoint ( unsigned int i ) 00054 { 00055 m_OffsetToSutureLastSlidablePoint = i; 00056 } 00057 //----------------------------------------------------------------------------- 00058 // Specific for suture models 00059 //============================================================================= 00060 00061 00062 00063 00064 //============================================================================= 00065 // Add models 00066 //----------------------------------------------------------------------------- 00067 //template <typename T, typename DATA> 00068 //int AdvSimSupport_DS<T,DATA>::AddModel ( OpenGL::ModelSuture<T> * pModel ) 00069 //{ 00070 // m_ListOfSutureModelsBasedOnMSS.push_back( pModel ); 00071 //} 00072 //----------------------------------------------------------------------------- 00073 //template <typename T, typename DATA> 00074 //int AdvSimSupport_DS<T,DATA>::AddModel ( ModelForSurgery<T> * pModel ) 00075 //{ 00076 // m_ModelLists.ListOfModelsForSurgeryBasedOnMSS.push_back( pModel ); 00077 // 00078 // // Add each part of the surgery model to the list of submodels 00079 // for ( int i = 0; i < pModel->GetNumberOfParts(); ++i ) { 00080 // m_ListOfHETriMeshOneModelMultiParts.push_back( pModel->GetPtrToPartNo(i) ); 00081 // } 00082 //} 00083 //----------------------------------------------------------------------------- 00084 template <typename T, typename DATA> 00085 int AdvSimSupport_DS<T,DATA>::AddModel ( ModelElasticRod<T> * pModel ) 00086 { 00087 for ( unsigned int i = 0; i < m_ModelLists.BasedOnER.size(); ++i ) { 00088 if ( m_ModelLists.BasedOnER[i] == pModel ) { 00089 for ( unsigned int j = 0; j < m_IDForModel.size(); ++j ) { 00090 if ( m_IDForModel[j].Type == Enum::MODEL_BASED_ON_ER 00091 && m_ModelLists.BasedOnER[m_IDForModel[j].Slot] == pModel ) 00092 { 00093 return j; 00094 } 00095 } 00096 } 00097 } 00098 00099 m_IDForModel.push_back( AdvSimSupport_DATA<T,DATA>::IDForModel( Enum::MODEL_BASED_ON_ER, m_ModelLists.BasedOnER.size() ) ); 00100 m_ModelLists.BasedOnER.push_back( pModel ); 00101 /*return*/ AddERBasedModel(); 00102 return m_IDForModel.size()-1; 00103 } 00104 //----------------------------------------------------------------------------- 00105 template <typename T, typename DATA> 00106 int AdvSimSupport_DS<T,DATA>::AddModel ( ModelDefBasedOnFEM<T,DATA> * pModel ) 00107 { 00108 for ( unsigned int i = 0; i < m_ModelLists.BasedOnFEM.size(); ++i ) { 00109 if ( m_ModelLists.BasedOnFEM[i] == pModel ) { 00110 for ( unsigned int j = 0; j < m_IDForModel.size(); ++j ) { 00111 if ( m_IDForModel[j].Type == Enum::MODEL_BASED_ON_FEM 00112 && m_ModelLists.BasedOnFEM[m_IDForModel[j].Slot] == pModel ) 00113 { 00114 return j; 00115 } 00116 } 00117 } 00118 } 00119 00120 m_IDForModel.push_back( AdvSimSupport_DATA<T,DATA>::IDForModel( Enum::MODEL_BASED_ON_FEM, m_ModelLists.BasedOnFEM.size() ) ); 00121 m_ModelLists.BasedOnFEM.push_back( pModel ); 00122 /*return*/ AddHEBasedModel(); 00123 return m_IDForModel.size()-1; 00124 } 00125 //----------------------------------------------------------------------------- 00126 template <typename T, typename DATA> 00127 int AdvSimSupport_DS<T,DATA>::AddModel ( RigidBodyDynamics<T> * pModel ) 00128 { 00129 for ( unsigned int i = 0; i < m_ModelLists.BasedOnRBD.size(); ++i ) { 00130 if ( m_ModelLists.BasedOnRBD[i] == pModel ) { 00131 for ( unsigned int j = 0; j < m_IDForModel.size(); ++j ) { 00132 if ( m_IDForModel[j].Type == Enum::MODEL_BASED_ON_IPG_RBD 00133 && m_ModelLists.BasedOnRBD[m_IDForModel[j].Slot] == pModel ) 00134 { 00135 return j; 00136 } 00137 } 00138 } 00139 } 00140 00141 m_IDForModel.push_back( AdvSimSupport_DATA<T,DATA>::IDForModel( Enum::MODEL_BASED_ON_IPG_RBD, m_ModelLists.BasedOnRBD.size() ) ); 00142 m_ModelLists.BasedOnRBD.push_back( pModel ); 00143 /*return*/ AddIPGBasedModel(); 00144 return m_IDForModel.size()-1; 00145 } 00146 //----------------------------------------------------------------------------- 00147 template <typename T, typename DATA> 00148 int AdvSimSupport_DS<T,DATA>::AddERBasedModel () 00149 { 00150 // Add data storage for the added model 00151 m_InteractionLists.ERModelVsERModel.push_back( std::vector< AdvSimSupport_DATA<T,DATA>::Interaction_ERModelVsERModel >() ); 00152 AdvSimSupport_DATA<T,DATA>::ListOfInteractions_ERModelVsERModel::iterator it = m_InteractionLists.ERModelVsERModel.begin(); 00153 while ( it != m_InteractionLists.ERModelVsERModel.end() ) { 00154 // Add data storage for the existed models with the added model 00155 it->push_back( AdvSimSupport_DATA<T,DATA>::Interaction_ERModelVsERModel() ); 00156 ++it; 00157 } 00158 00159 // Add data storage for the added ER-based model interaction with HE-based models 00160 { 00161 m_InteractionLists.ERModelVsHEModel.push_back( std::vector< AdvSimSupport_DATA<T,DATA>::Interaction_ERModelVsHEModel >() ); 00162 AdvSimSupport_DATA<T,DATA>::ListOfInteractions_HEModelVsHEModel::iterator it = m_InteractionLists.HEModelVsHEModel.begin(); 00163 while ( it != m_InteractionLists.HEModelVsHEModel.end() ) { 00164 // Add data storage for interaction of the existed HE-based models with the added ER-based model 00165 m_InteractionLists.ERModelVsHEModel.back().push_back( AdvSimSupport_DATA<T,DATA>::Interaction_ERModelVsHEModel() ); 00166 ++it; 00167 } 00168 } 00169 00170 // Add data storage for the added ER-based model interaction with IPG-based models 00171 { 00172 m_InteractionLists.ERModelVsIPGModel.push_back( std::vector< AdvSimSupport_DATA<T,DATA>::Interaction_ERModelVsIPGModel >() ); 00173 AdvSimSupport_DATA<T,DATA>::ListOfInteractions_IPGModelVsIPGModel::iterator it = m_InteractionLists.IPGModelVsIPGModel.begin(); 00174 while ( it != m_InteractionLists.IPGModelVsIPGModel.end() ) { 00175 // Add data storage for interaction of the existed IPG-based models with the added ER-based model 00176 m_InteractionLists.ERModelVsIPGModel.back().push_back( AdvSimSupport_DATA<T,DATA>::Interaction_ERModelVsIPGModel() ); 00177 ++it; 00178 } 00179 } 00180 00181 int id = m_InteractionLists.ERModelVsHEModel.size()-1; 00182 if ( id != m_InteractionLists.ERModelVsIPGModel.size()-1 ) { 00183 PrtErrorWithFileNameAndLineNumber( "Inconsistent size!" ); 00184 exit(-1); 00185 } 00186 00187 // Set data for controlling forward and backward movements of an Elastic Rod 00188 m_ModelLists.AllERNodePropsList.push_back( AdvSimSupport_DATA<T,DATA>::ERNodePropsList( m_ModelLists.BasedOnER.back()->GetNumberOfPoints() ) ); 00189 00190 // returns the assigned ID for the added ER-based model 00191 return id; 00192 } 00193 //----------------------------------------------------------------------------- 00194 template <typename T, typename DATA> 00195 int AdvSimSupport_DS<T,DATA>::AddHEBasedModel () 00196 { 00197 // Add data storage for the added model 00198 m_InteractionLists.HEModelVsHEModel.push_back( std::vector< AdvSimSupport_DATA<T,DATA>::Interaction_HEModelVsHEModel >() ); 00199 AdvSimSupport_DATA<T,DATA>::ListOfInteractions_HEModelVsHEModel::iterator it = m_InteractionLists.HEModelVsHEModel.begin(); 00200 while ( it != m_InteractionLists.HEModelVsHEModel.end() ) { 00201 // Add data storage for the existed models with the added model 00202 it->push_back( AdvSimSupport_DATA<T,DATA>::Interaction_HEModelVsHEModel() ); 00203 ++it; 00204 } 00205 00206 // DEBUG 00207 //std::cout << "size of m_InteractionLists.HEModelVsHEModel: " << m_InteractionLists.HEModelVsHEModel.size() << "\n"; 00208 //for ( unsigned int i = 0; i < m_InteractionLists.HEModelVsHEModel.size(); ++i ) { 00209 // std::cout << "size of m_InteractionLists.HEModelVsHEModel["<<i<<"]: " << m_InteractionLists.HEModelVsHEModel[i].size() << "\n"; 00210 //} 00211 00212 // Add data storage for interaction with ER-based models 00213 AdvSimSupport_DATA<T,DATA>::ListOfInteractions_ERModelVsHEModel::iterator itS = m_InteractionLists.ERModelVsHEModel.begin(); 00214 00215 while ( itS != m_InteractionLists.ERModelVsHEModel.end() ) { 00216 itS->push_back( AdvSimSupport_DATA<T,DATA>::Interaction_ERModelVsHEModel() ); 00217 ++itS; 00218 } 00219 00220 // Add data storage for interaction with IPG-based models 00221 AdvSimSupport_DATA<T,DATA>::ListOfInteractions_IPGModelVsHEModel::iterator itP = m_InteractionLists.IPGModelVsHEModel.begin(); 00222 while ( itP != m_InteractionLists.IPGModelVsHEModel.end() ) { 00223 itP->push_back( AdvSimSupport_DATA<T,DATA>::Interaction_IPGModelVsHEModel() ); 00224 ++itP; 00225 } 00226 00227 // returns the assigned ID for the added model 00228 return m_InteractionLists.HEModelVsHEModel.size()-1; 00229 } 00230 //----------------------------------------------------------------------------- 00231 template <typename T, typename DATA> 00232 int AdvSimSupport_DS<T,DATA>::AddIPGBasedModel () 00233 { 00234 // Add data storage for the added model 00235 m_InteractionLists.IPGModelVsIPGModel.push_back( std::vector< AdvSimSupport_DATA<T,DATA>::Interaction_IPGModelVsIPGModel >() ); 00236 AdvSimSupport_DATA<T,DATA>::ListOfInteractions_IPGModelVsIPGModel::iterator it = m_InteractionLists.IPGModelVsIPGModel.begin(); 00237 while ( it != m_InteractionLists.IPGModelVsIPGModel.end() ) { 00238 // Add data storage for the existed models with the added model 00239 it->push_back( AdvSimSupport_DATA<T,DATA>::Interaction_IPGModelVsIPGModel() ); 00240 ++it; 00241 } 00242 00243 // Add data storage for interaction with ER-based models 00244 AdvSimSupport_DATA<T,DATA>::ListOfInteractions_ERModelVsIPGModel::iterator itS = m_InteractionLists.ERModelVsIPGModel.begin(); 00245 while ( itS != m_InteractionLists.ERModelVsIPGModel.end() ) { 00246 itS->push_back( AdvSimSupport_DATA<T,DATA>::Interaction_ERModelVsIPGModel() ); 00247 ++itS; 00248 } 00249 00250 // Add data storage for the added IPG-based model interaction with HE-based models 00251 { 00252 m_InteractionLists.IPGModelVsHEModel.push_back( std::vector< AdvSimSupport_DATA<T,DATA>::Interaction_IPGModelVsHEModel >() ); 00253 AdvSimSupport_DATA<T,DATA>::ListOfInteractions_HEModelVsHEModel::iterator it = m_InteractionLists.HEModelVsHEModel.begin(); 00254 while ( it != m_InteractionLists.HEModelVsHEModel.end() ) { 00255 // Add data storage for interaction of the existed HE-based models with the added IPG-based model 00256 m_InteractionLists.IPGModelVsHEModel.back().push_back( AdvSimSupport_DATA<T,DATA>::Interaction_IPGModelVsHEModel() ); 00257 ++it; 00258 } 00259 } 00260 00261 // returns the assigned ID for the added model 00262 return m_InteractionLists.IPGModelVsIPGModel.size()-1; 00263 } 00264 //----------------------------------------------------------------------------- 00265 // Add models 00266 //============================================================================= 00267 00268 00269 00270 00271 //============================================================================= 00272 // Add constraints 00273 //----------------------------------------------------------------------------- 00274 template <typename T, typename DATA> 00275 typename AdvSimSupport_DATA<T,DATA>::Constraint_HEModelVsHEModel * AdvSimSupport_DS<T,DATA>::AddVertexConnectionHEModelAndHEModel ( 00276 unsigned int modelA, 00277 HEVertex<T> * pVertexA, 00278 enum Enum::AddOn::SimConstraints flagsA, 00279 unsigned int modelB, 00280 HEVertex<T> * pVertexB, 00281 enum Enum::AddOn::SimConstraints flagsB, 00282 T forceRatio, 00283 T forceScaleForModelA, 00284 T forceScaleForModelB, 00285 T forceThresholdForModelA, 00286 T forceThresholdForModelB, 00287 bool bEnforceVertexOfModelA, 00288 bool bEnforceVertexOfModelB 00289 ) 00290 { 00291 unsigned int locA, locB; 00292 if ( m_IDForModel[modelA].Type != Enum::MODEL_BASED_ON_FEM ) return NULL; 00293 else locA = m_IDForModel[modelA].Slot; 00294 if ( m_IDForModel[modelB].Type != Enum::MODEL_BASED_ON_FEM ) return NULL; 00295 else locB = m_IDForModel[modelB].Slot; 00296 00297 pVertexA->SimFlags.SetSimulationConstraints( flagsA ); 00298 pVertexB->SimFlags.SetSimulationConstraints( flagsB ); 00299 m_InteractionLists.HEModelVsHEModel[locA][locB].Constraints.push_back( 00300 new AdvSimSupport_DATA<T,DATA>::Constraint_HEModelVsHEModel( pVertexA, pVertexB, forceRatio, forceScaleForModelA, forceScaleForModelB, forceThresholdForModelA, forceThresholdForModelB, bEnforceVertexOfModelA, bEnforceVertexOfModelB ) ); 00301 m_InteractionLists.HEModelVsHEModel[locA][locB].Constraints.back()->RefToSavedPositionA() = pVertexA->GetPosition(); 00302 m_InteractionLists.HEModelVsHEModel[locA][locB].Constraints.back()->RefToSavedPositionB() = pVertexB->GetPosition(); 00303 00304 return m_InteractionLists.HEModelVsHEModel[locA][locB].Constraints.back(); 00305 } 00306 //----------------------------------------------------------------------------- 00307 template <typename T, typename DATA> 00308 bool AdvSimSupport_DS<T,DATA>::GroupSetOfLinks ( 00309 unsigned int ERModelID, 00310 unsigned int startLink, 00311 unsigned int numOfLinks 00312 ) 00313 { 00314 unsigned int ERloc; 00315 if ( m_IDForModel[ERModelID].Type != Enum::MODEL_BASED_ON_ER ) return false; 00316 else ERloc = m_IDForModel[ERModelID].Slot; 00317 00318 int endLink = numOfLinks + startLink - 1; 00319 if ( endLink > m_ModelLists.BasedOnER[ERloc]->GetNumOfPoints() ) return false; 00320 00321 unsigned short groupID = static_cast<unsigned short>( numOfLinks ); 00322 for ( int i = startLink; i <= endLink; ++i ) { 00323 m_ModelLists.AllERNodePropsList[ERloc][i].GroupID = groupID; 00324 } 00325 m_ModelLists.AllERNodePropsList[ERloc][startLink].GroupID += 2000; // the first link of the group 00326 // where 1 represents the first and last link of the group 00327 m_ModelLists.AllERNodePropsList[ERloc][endLink].GroupID += 1000; // the last link of the group 00328 00329 return true; 00330 } 00331 //----------------------------------------------------------------------------- 00332 template <typename T, typename DATA> 00333 void AdvSimSupport_DS<T,DATA>::AddVertexConnectionERModelAndHEModel ( 00334 unsigned int ERModelID, 00335 unsigned int ERModelVertexID, 00336 enum Enum::AddOn::SimConstraints flagsERModel, 00337 unsigned int HEModelID, 00338 HEVertex<T> * pVertexHEModel, 00339 enum Enum::AddOn::SimConstraints flagsHEModel, 00340 T forceRatio, 00341 T forceScaleForERModel, 00342 T forceScaleForHEModel, 00343 T forceThresholdForERModel, 00344 T forceThresholdForHEModel, 00345 bool bEnforceERModelVertex, 00346 bool bEnforceHEModelVertex 00347 ) 00348 { 00349 unsigned int ERloc, HEloc; 00350 if ( m_IDForModel[ERModelID].Type != Enum::MODEL_BASED_ON_ER ) return; 00351 else ERloc = m_IDForModel[ERModelID].Slot; 00352 if ( m_IDForModel[HEModelID].Type != Enum::MODEL_BASED_ON_FEM ) return; 00353 else HEloc = m_IDForModel[HEModelID].Slot; 00354 00355 //std::cout << "ERModel's vertex id: " << vertexIDSuture << "\n"; 00356 //std::cout << "SIM FLAG" << m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelVertexID].SimFlags << "\n"; 00357 00358 m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( flagsERModel ); 00359 m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 00360 m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 00361 00362 //std::cout << "SIM FLAG" << m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelVertexID].SimFlags << "\n"; 00363 00364 pVertexHEModel->SimFlags.SetSimulationConstraints( flagsHEModel ); 00365 00366 typename AdvSimSupport_DATA<T,DATA>::Constraint_ERModelVsHEModel * pConstraint = new AdvSimSupport_DATA<T,DATA>::Constraint_ERModelVsHEModel( ERModelVertexID, pVertexHEModel, forceRatio, forceScaleForERModel, forceScaleForHEModel, forceThresholdForERModel, forceThresholdForHEModel, bEnforceERModelVertex, bEnforceHEModelVertex ); 00367 00368 // Insert to the list (sorted from low to high) 00369 unsigned int size = m_InteractionLists.ERModelVsHEModel[ERloc][HEloc].Constraints.size(); 00370 if ( size == 0 ) { 00371 m_InteractionLists.ERModelVsHEModel[ERloc][HEloc].Constraints.push_back( pConstraint ); 00372 } 00373 else { 00374 typename AdvSimSupport_DATA<T,DATA>::InteractionConstraints_ERModelVsHEModel & List = m_InteractionLists.ERModelVsHEModel[ERloc][HEloc].Constraints; 00375 typename AdvSimSupport_DATA<T,DATA>::InteractionConstraints_ERModelVsHEModel::iterator it = List.begin(); 00376 00377 //----------------------------------------------------------- 00378 // (binary) insert from low to high 00379 unsigned int start = 0, middle = size/2, end = size; 00380 while ( end - start > 0 ) { 00381 00382 if ( List[middle]->GetVertexIDFromModel_1() > ERModelVertexID ) { 00383 end = middle; 00384 middle = (end - start)/2 + start; 00385 } 00386 else if ( List[middle]->GetVertexIDFromModel_1() < ERModelVertexID ) { 00387 start = middle+1; 00388 middle = (end - start)/2 + start; 00389 } 00390 else { 00391 break; 00392 } 00393 } 00394 List.insert( it+middle, pConstraint ); 00395 //----------------------------------------------------------- 00396 00397 /* 00398 // DEBUG 00399 std::cout << "Sorted List:"; 00400 for ( it = List.begin(); it != List.end(); ++it ) { 00401 std::cout << "\t" << (*it)->GetVertexIDFromModel_1(); 00402 } 00403 std::cout << "\n"; 00404 //*/ 00405 } 00406 00407 00408 //pConstraint->RefToSavedPositionA() = m_ModelLists.BasedOnER[ERModelID]->GetListOfNodes()[ERModelVertexID].GetPosition(); 00409 pConstraint->RefToSavedPositionB() = pVertexHEModel->GetPosition(); 00410 00411 // Set data for controlling forward and backward movements of an Elastic Rod 00412 m_ModelLists.AllERNodePropsList[ERloc][ERModelVertexID].IsConstrained = true; 00413 m_ModelLists.AllERNodePropsList[ERloc][ERModelVertexID].GroupID = 2001; 00414 } 00415 //----------------------------------------------------------------------------- 00416 template <typename T, typename DATA> 00417 void AdvSimSupport_DS<T,DATA>::AddVertexConnectionIPGModelAndHEModel ( 00418 unsigned int IPGModelID, 00419 unsigned int IPGModelVertexID, 00420 enum Enum::AddOn::SimConstraints flagsIPGModel, 00421 unsigned int HEModelID, 00422 HEVertex<T> * pVertexHEModel, 00423 enum Enum::AddOn::SimConstraints flagsHEModel, 00424 T forceRatio, 00425 T forceScaleForIPGModel, 00426 T forceScaleForHEModel, 00427 T forceThresholdForIPGModel, 00428 T forceThresholdForHEModel, 00429 bool bEnforceIPGModelVertex, 00430 bool bEnforceHEModelVertex 00431 ) 00432 { 00433 unsigned int locIPG, locHE; 00434 Vector3<T> IPGVertexPos; 00435 if ( m_IDForModel[IPGModelID].Type != Enum::MODEL_BASED_ON_IPG_RBD ) return; 00436 else locIPG = m_IDForModel[IPGModelID].Slot; 00437 if ( m_IDForModel[HEModelID].Type != Enum::MODEL_BASED_ON_FEM ) return; 00438 else locHE = m_IDForModel[HEModelID].Slot; 00439 00440 if ( m_IDForModel[IPGModelID].Type == Enum::MODEL_BASED_ON_IPG_RBD ) { 00441 m_ModelLists.BasedOnRBD[locIPG]->RefToInteractionPointGroup().GetInteractionPointAtIndex( IPGModelVertexID ).GetSimFlags().SetSimulationConstraints( flagsIPGModel ); 00442 //m_ModelLists.BasedOnRBD[locIPG]->RefToInteractionPointGroup().GetInteractionPointAtIndex( IPGModelVertexID ).GetSimFlags().SetSimulationConstraints( Enum::AddOn::ATTACHED ); 00443 //m_ModelLists.BasedOnRBD[locIPG]->RefToInteractionPointGroup().GetInteractionPointAtIndex( IPGModelVertexID ).GetFlags().SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 00444 IPGVertexPos = m_ModelLists.BasedOnRBD[locIPG]->RefToInteractionPointGroup().GetInteractionPointAtIndex( IPGModelVertexID ).GetPosition(); 00445 } 00446 if ( m_IDForModel[HEModelID].Type == Enum::MODEL_BASED_ON_FEM ) { 00447 pVertexHEModel->SimFlags.SetSimulationConstraints( flagsHEModel ); 00448 } 00449 00450 // NEED TO PROTECT DUPLICATED!!! 00451 00452 //std::cout << "TEST 1: " << m_InteractionLists.IPGModelVsHEModel.size() << "\n"; 00453 //std::cout << "TEST 2: " << m_InteractionLists.IPGModelVsHEModel[locIPG].size() << "\n"; 00454 //std::cout << "TEST 3: " << m_InteractionLists.IPGModelVsHEModel[locIPG][locHE].size() << "\n"; 00455 00456 m_InteractionLists.IPGModelVsHEModel[locIPG][locHE].push_back( 00457 new AdvSimSupport_DATA<T,DATA>::Constraint_IPGModelVsHEModel( IPGModelVertexID, pVertexHEModel, forceRatio, forceScaleForIPGModel, forceScaleForHEModel, forceThresholdForIPGModel, forceThresholdForHEModel, bEnforceIPGModelVertex, bEnforceHEModelVertex ) ); 00458 m_InteractionLists.IPGModelVsHEModel[locIPG][locHE].back()->RefToSavedPositionA() = IPGVertexPos; 00459 m_InteractionLists.IPGModelVsHEModel[locIPG][locHE].back()->RefToSavedPositionB() = pVertexHEModel->GetPosition(); 00460 } 00461 //----------------------------------------------------------------------------- 00462 // Add constraints 00463 //============================================================================= 00464 00465 00466 00467 00468 //============================================================================= 00469 // Process specific interactions 00470 //----------------------------------------------------------------------------- 00471 template <typename T, typename DATA> 00472 void AdvSimSupport_DS<T,DATA>::ProcessPuncturingOfCurveFormedByIPGToHEModel ( unsigned int IPGModel, unsigned int HEModel ) 00473 { 00474 Enum::ModelType IPGType = m_IDForModel[IPGModel].Type; 00475 Enum::ModelType HEType = m_IDForModel[HEModel].Type; 00476 if ( IPGType == Enum::MODEL_BASED_ON_IPG_RBD 00477 && HEType == Enum::MODEL_BASED_ON_FEM ) { 00478 ProcessPuncturingOfCurveFormedByIPGonRBDModelToFEMModel( IPGModel, HEModel ); 00479 } 00480 else { 00481 PrtNotImplementedYet( "Supported only IPGType == Enum::MODEL_BASED_ON_IPG_RBD and HEType == Enum::MODEL_BASED_ON_FEM" ); 00482 } 00483 } 00484 //----------------------------------------------------------------------------- 00485 template <typename T, typename DATA> 00486 void AdvSimSupport_DS<T,DATA>::ProcessPuncturingOfCurveFormedByIPGonRBDModelToFEMModel ( unsigned int RBDModel, unsigned int FEMModel ) 00487 { 00488 //#ifdef TAPs_COLLISION_DETECTION_FNS_HPP 00489 00490 unsigned int RBDloc = m_IDForModel[RBDModel].Slot; 00491 unsigned int FEMloc = m_IDForModel[FEMModel].Slot; 00492 00493 ModelDefBasedOnFEMHex<T,DATA> * pFEMHex = dynamic_cast< ModelDefBasedOnFEMHex<T,DATA> * >( m_ModelLists.BasedOnFEM[FEMloc] ); 00494 ModelDefBasedOnFEMTet<T,DATA> * pFEMTet = dynamic_cast< ModelDefBasedOnFEMTet<T,DATA> * >( m_ModelLists.BasedOnFEM[FEMloc] ); 00495 00496 if ( pFEMHex ) 00497 { 00498 InteractionPointGroup<T> & IPG = m_ModelLists.BasedOnRBD[RBDloc]->RefToInteractionPointGroup(); 00499 if ( IPG.GetNumOfInteractionPoints() == 0 ) return; 00500 std::vector< Vector3<T> > IPs = m_ModelLists.BasedOnRBD[RBDloc]->GetAllInteractionPointPositionsInWorldSpace(); 00501 //if ( IPs.size() == 0 ) return; 00502 00503 //bool bCheckPunctureIn = true; 00504 bool bCheckPunctureOut = true; 00505 00506 00507 00508 unsigned int sections[6]; 00509 sections[0] = 0; // always 0 00510 unsigned int i = 0; 00511 while ( i < IPs.size() && IPG.GetInteractionPointAtIndex( i ).GetSimFlags().CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) == false ) { 00512 ++i; 00513 } 00514 sections[1] = i; 00515 while ( i < IPs.size() && IPG.GetInteractionPointAtIndex( i ).GetSimFlags().CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) == true ) { 00516 ++i; 00517 } 00518 sections[2] = i; 00519 while ( i < IPs.size() && IPG.GetInteractionPointAtIndex( i ).GetSimFlags().CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) == false ) { 00520 ++i; 00521 } 00522 sections[3] = i; 00523 while ( i < IPs.size() && IPG.GetInteractionPointAtIndex( i ).GetSimFlags().CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) == true ) { 00524 ++i; 00525 } 00526 sections[4] = i; 00527 while ( i < IPs.size() && IPG.GetInteractionPointAtIndex( i ).GetSimFlags().CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) == false ) { 00528 ++i; 00529 } 00530 sections[5] = i; 00531 00532 //* 00533 // DEBUG 00534 for ( unsigned int i = 0; i < IPs.size(); ++i ) { 00535 std::cout << i << "\t: " << IPG.GetInteractionPointAtIndex( i ).GetSimFlags() << std::endl; 00536 } 00537 std::cout << "size: " << IPs.size() << "\n"; 00538 std::cout << "sections: " 00539 << "[" << sections[0] << "," << sections[1] << "), " 00540 << "[" << sections[1] << "," << sections[2] << "), " 00541 << "[" << sections[2] << "," << sections[3] << "), " 00542 << "[" << sections[3] << "," << sections[4] << "), " 00543 << "[" << sections[4] << "," << sections[5] << "), "; 00544 //*/ 00545 00546 i = 0; 00547 if ( sections[i+1] > 0 ) { 00548 ProcessForwardPuncturingIPGonRBDModelToHexFEMModel_SectionCleared( RBDModel, FEMModel, RBDloc, FEMloc, sections[i], sections[i+1], IPs ); 00549 } 00550 i = 1; 00551 if ( sections[i] < IPs.size() ) { 00552 ProcessForwardPuncturingIPGonRBDModelToHexFEMModel_SectionPunctured( RBDModel, FEMModel, RBDloc, FEMloc, sections[i], sections[i+1], IPs, pFEMHex ); 00553 } 00554 //i = 2; 00555 //if ( sections[i+1] > IPs.size() ) { 00556 // ProcessForwardPuncturingIPGonRBDModelToHexFEMModel_SectionCleared( RBDModel, FEMModel, RBDloc, FEMloc, sections[i], sections[i+1], IPs ); 00557 //} 00558 //i = 3; 00559 //if ( sections[i+1] > IPs.size() ) { 00560 // ProcessForwardPuncturingIPGonRBDModelToHexFEMModel_SectionPunctured( RBDModel, FEMModel, RBDloc, FEMloc, sections[i], sections[i+1], IPs ); 00561 //} 00562 //i = 4; 00563 //if ( sections[i+1] > IPs.size() ) { 00564 // ProcessForwardPuncturingIPGonRBDModelToHexFEMModel_SectionCleared( RBDModel, FEMModel, RBDloc, FEMloc, sections[i], sections[i+1], IPs ); 00565 //} 00566 } 00567 } 00568 //----------------------------------------------------------------------------- 00569 template <typename T, typename DATA> 00570 void AdvSimSupport_DS<T,DATA>::ProcessForwardPuncturingIPGonRBDModelToHexFEMModel_SectionCleared ( 00571 unsigned int RBDModel, unsigned int FEMModel, 00572 unsigned int RBDloc, unsigned int FEMloc, 00573 unsigned int start, unsigned int end, 00574 std::vector< Vector3<T> > const & IPs 00575 ) 00576 { 00577 if ( end - start < 2 ) return; 00578 00579 // Check intersection 00580 AdvSimSupport_DATA<T,DATA>::TEMP_ListOfPoints.clear(); 00581 AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEFaces.clear(); 00582 AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEVertices.clear(); 00583 AdvSimSupport_DATA<T,DATA>::TEMP_ListOfRatios.clear(); 00584 CD::Fn::CD_PolygonalMesh_vs_LineSegment( 00585 &m_ModelLists.BasedOnFEM[FEMloc]->RefToSurfaceMesh(), // HE-based mesh model 00586 &IPs[start], &IPs[start+1], // line segment 00587 &AdvSimSupport_DATA<T,DATA>::TEMP_ListOfPoints, // list of intersected points 00588 &AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEFaces, // list of intersected HE-based faces 00589 &AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEVertices, // list of closest HE-based vertices to the intersected points 00590 &AdvSimSupport_DATA<T,DATA>::TEMP_ListOfRatios // list of ratios of the closest vertices relative to the line segment [0(ptA)-1(ptB)] 00591 ); 00592 00593 if ( AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEVertices.size() > 0 ) { 00594 00595 std::cout << "Add punctured pt\n"; 00596 00597 AddVertexConnectionIPGModelAndHEModel( 00598 RBDModel, 00599 start, 00600 Enum::AddOn::SLIDABLE, 00601 FEMModel, 00602 AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEVertices[0], // only the first point 00603 Enum::AddOn::ATTACHED, 00604 AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio, 00605 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA, 00606 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB, 00607 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA, 00608 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB, 00609 false, false 00610 //AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA, 00611 //AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB 00612 ); 00613 } 00614 } 00615 //----------------------------------------------------------------------------- 00616 template <typename T, typename DATA> 00617 void AdvSimSupport_DS<T,DATA>::ProcessForwardPuncturingIPGonRBDModelToHexFEMModel_SectionPunctured ( 00618 unsigned int RBDModel, unsigned int FEMModel, 00619 unsigned int RBDloc, unsigned int FEMloc, 00620 unsigned int start, unsigned int end, 00621 std::vector< Vector3<T> > const & IPs, 00622 ModelDefBasedOnFEMHex<T,DATA> * pFEMHex 00623 ) 00624 { 00625 00626 std::cout << "section#1 start: " << start << " end: " << end << std::endl; 00627 00628 // the first IP point is still inside the HexFEMModel 00629 if ( start == 0 ) { 00630 00631 00632 /* 00633 // DEBUG 00634 std::cout << "_2" << std::endl; 00635 std::cout << "size: " << m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc].size() << std::endl; 00636 for ( unsigned int i = 0; i < m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc].size(); ++i ) { 00637 std::cout << *m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][i] << std::endl; 00638 std::cout << *(m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][i]->GetPtrToVertexFromModel_2()->GetBarycentricPtr()) << std::endl; 00639 //std::cout << m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][i]->GetPtrToVertexFromModel_2()->GetBarycentricPtr()->GetNewVertexLocation() << std::endl; 00640 } 00641 //*/ 00642 00643 00644 // Check intersection 00645 AdvSimSupport_DATA<T,DATA>::TEMP_ListOfPoints.clear(); 00646 AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEFaces.clear(); 00647 AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEVertices.clear(); 00648 AdvSimSupport_DATA<T,DATA>::TEMP_ListOfRatios.clear(); 00649 CD::Fn::CD_PolygonalMesh_vs_LineSegment( 00650 &pFEMHex->RefToSurfaceMesh(), // HE-based mesh model 00651 &IPs[0], &IPs[1], // line segment 00652 &AdvSimSupport_DATA<T,DATA>::TEMP_ListOfPoints, // list of intersected points 00653 &AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEFaces, // list of intersected HE-based faces 00654 &AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEVertices, // list of closest HE-based vertices to the intersected points 00655 &AdvSimSupport_DATA<T,DATA>::TEMP_ListOfRatios // list of ratios of the closest vertices relative to the line segment [0(ptA)-1(ptB)] 00656 ); 00657 00658 // Puncture out 00659 if ( AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEVertices.size() > 0 ) { 00660 00661 std::cout << "Puncture out" << std::endl; 00662 00663 //return; 00664 00665 if ( end < IPs.size() ) { 00666 00667 // Add the new constraint at the end of the list of constraints 00668 AddVertexConnectionIPGModelAndHEModel( 00669 RBDModel, 00670 end, 00671 Enum::AddOn::SLIDABLE, 00672 FEMModel, 00673 AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEVertices[0], // only the first point 00674 Enum::AddOn::ATTACHED, 00675 AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio, 00676 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA, 00677 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB, 00678 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA, 00679 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB, 00680 //false, false 00681 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA, 00682 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB 00683 ); 00684 00685 00686 } 00687 else { 00688 --end; 00689 00690 std::cout << "end is end-1: " << end << std::endl; 00691 00692 std::cout << "New pt: " << AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEVertices[0] << std::endl; 00693 00694 // DEBUG 00695 for ( unsigned int i = 0; i <= end; ++i ) { 00696 std::cout << "unsort: " << m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][i]->GetPtrToVertexFromModel_2() << "\n"; 00697 } 00698 00699 // 00700 // Arrange the data 00701 // 00702 std::vector< HEVertex<T> * > pHEVert; 00703 // the second last vertex is be cleared in the first constraint 00704 pHEVert.push_back( m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][end-2]->GetPtrToVertexFromModel_2() ); 00705 // the new vertex will be in the first constraint after the clear 00706 pHEVert.push_back( AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEVertices[0] ); 00707 // the first last vertex has to be cleared 00708 m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][end-1]->GetPtrToVertexFromModel_2()->SimFlags.ClearAllFlags(); 00709 // arrange the rest of HEVertices and increase the vertex IDs by one 00710 for ( unsigned int i = 0; i < end-1; ++i ) { 00711 pHEVert.push_back( m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][i]->GetPtrToVertexFromModel_2() ); 00712 } 00713 // arrange the HEVertices 00714 for ( unsigned int i = 0; i <= end; ++i ) { 00715 std::cout << "sort: " << pHEVert[i] << "\n"; 00716 m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][i]->SetPtrToVertexFromModel_2( pHEVert[i] ); 00717 } 00718 00719 // Clear the first constraint 00720 ClearTheConstraintNumberOfRBDModelAndFEMModel( RBDloc, FEMloc, 0 ); 00721 } 00722 } 00723 00724 // Still puncturing inside 00725 else { 00726 00727 Matrix4x4<T> TrxFEMModel = m_ModelLists.BasedOnFEM[FEMloc]->RefToTransformationSupport().RefToMatrixTransform(); 00728 Vector3<T> pos1 = TrxFEMModel * m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][0]->GetPtrToVertexFromModel_2()->GetBarycentricPtr()->GetNewVertexLocation(); 00729 00730 T dist = (IPs[0] - pos1).Length(); 00731 T distThreshold = ( IPs[0] - IPs[1] ).Length(); 00732 00733 if ( dist >= distThreshold ) { 00734 std::cout << "dist: " << dist << std::endl; 00735 00736 // FIND THE VERTEX AT INTERACTION POINT#0 00737 bool bFound = false; 00738 unsigned int elementID; 00739 Vector3<T> position = pFEMHex->RefToTransformationSupport().GetInverseMatrixTransform() * IPs[0]; 00740 00741 // Create a new HEvertex to stick with IP point number 0 00742 T coords[3]; 00743 unsigned int Es[3] = { 00744 pFEMHex->GetNumberOfElementsPerGridSize_X(), 00745 pFEMHex->GetNumberOfElementsPerGridSize_Y(), 00746 pFEMHex->GetNumberOfElementsPerGridSize_Z() 00747 }; 00748 00749 unsigned int x = 0; 00750 unsigned int y = 0; 00751 unsigned int z = 0; 00752 for ( x = 0; !bFound && x < Es[0]; ++x ) { 00753 for ( y = 0; !bFound && y < Es[1]; ++y ) { 00754 for ( z = 0; !bFound && z < Es[2]; ++z ) { 00755 elementID = pFEMHex->GetElementIDFromGridLocation( x, y, z ); 00756 if ( elementID >= 0 ) { 00757 bFound = pFEMHex->RefToFEMMesh().RefToElement( elementID ).EstimateParametricCoords( 00758 position[0], position[1], position[2], 00759 coords[0], coords[1], coords[2] ) 00760 > 0 ? true : false; 00761 } 00762 } 00763 } 00764 } 00765 // Replace the surface HEVertex with a created inner HEVertex inside the FEMModel 00766 if ( bFound ) { 00767 00768 if ( end < IPs.size() ) { 00769 00770 HEVertex<T> * pNewHEVertex = pFEMHex->AddExtraVertexInLocalSpace( 00771 IPs[0], elementID, &pFEMHex->RefToFEMMesh().RefToElement( elementID ), Vector3<T>(x,y,z), coords ); 00772 00773 AddVertexConnectionIPGModelAndHEModel( 00774 RBDModel, 00775 end, 00776 Enum::AddOn::SLIDABLE, 00777 FEMModel, 00778 pNewHEVertex, 00779 Enum::AddOn::ATTACHED, 00780 AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio, 00781 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA, 00782 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB, 00783 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA, 00784 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB, 00785 //false, false 00786 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA, 00787 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB 00788 ); 00789 00790 // Slide the IP points from number 0 thru end-1 00791 // pt 1 will inherit the sticked position from pt 0, and so on. 00792 HEVertex<T> * pHEVert; 00793 for ( unsigned int i = end; i > 0; --i ) { 00794 //std::cout << "p1: " << m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][i-1]->GetPtrToVertexFromModel_2() << std::endl; 00795 //std::cout << "p2: " << m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][i]->GetPtrToVertexFromModel_2() << std::endl; 00796 00797 pHEVert = m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][i-1]->GetPtrToVertexFromModel_2(); 00798 m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][i]->SetPtrToVertexFromModel_2( pHEVert ); 00799 } 00800 // the fist IP pt 00801 m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][0]->SetPtrToVertexFromModel_2( pNewHEVertex ); 00802 } 00803 } 00804 } 00805 }// end puncturing inside 00806 } 00807 00808 // The first IP point has already punctured out from the HexFEMModel 00809 else { 00810 } 00811 } 00812 //----------------------------------------------------------------------------- 00813 00814 00815 00816 00817 //----------------------------------------------------------------------------- 00818 //template <typename T, typename DATA> 00819 //void AdvSimSupport_DS<T,DATA>::ProcessPuncturingOfCurveFormedByIPGonRBDModelToFEMModel ( unsigned int RBDModel, unsigned int FEMModel ) 00820 //{ 00822 // 00823 // unsigned int RBDloc = m_IDForModel[RBDModel].Slot; 00824 // unsigned int FEMloc = m_IDForModel[FEMModel].Slot; 00825 // 00826 // ModelDefBasedOnFEMHex<T,DATA> * pFEMHex = dynamic_cast< ModelDefBasedOnFEMHex<T,DATA> * >( m_ModelLists.BasedOnFEM[FEMloc] ); 00827 // ModelDefBasedOnFEMTet<T,DATA> * pFEMTet = dynamic_cast< ModelDefBasedOnFEMTet<T,DATA> * >( m_ModelLists.BasedOnFEM[FEMloc] ); 00828 // 00829 // if ( pFEMHex ) 00830 // { 00831 // InteractionPointGroup<T> & IPG = m_ModelLists.BasedOnRBD[RBDloc]->RefToInteractionPointGroup(); 00832 // if ( IPG.GetNumOfInteractionPoints() == 0 ) return; 00833 // std::vector< Vector3<T> > IPs = m_ModelLists.BasedOnRBD[RBDloc]->GetAllInteractionPointPositionsInWorldSpace(); 00834 // //if ( IPs.size() == 0 ) return; 00835 // 00836 // //bool bCheckPunctureIn = true; 00837 // bool bCheckPunctureOut = true; 00838 // 00839 // {//START: CASE PUNCTURING IN 00840 // 00841 // // Point numbers 00842 // unsigned int A = 0; 00843 // unsigned int B = 1; 00844 // 00845 // //* 00846 // // DEBUG 00847 // for ( unsigned int i = 0; i < IPs.size(); ++i ) { 00848 // std::cout << i << "\t: " << IPG.GetInteractionPointAtIndex( i ).GetSimFlags() << std::endl; 00849 // } 00850 // //*/ 00851 // 00852 // while ( IPG.GetInteractionPointAtIndex( A ).GetSimFlags().CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) ) { 00853 // std::cout << "\t" << A; 00854 // ++A; 00855 // if ( A > IPs.size() - 2 ) { 00856 // --A; 00857 // break; 00858 // } 00859 // } 00860 // B = A + 1; 00861 // 00862 // // A is SLIDABLE and B is CLEARED 00863 // if ( IPG.GetInteractionPointAtIndex( B ).GetSimFlags().CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) == false ) 00864 // { 00865 // // Check intersection 00866 // TEMP_ListOfPoints.clear(); 00867 // TEMP_ListOfHEFaces.clear(); 00868 // TEMP_ListOfHEVertices.clear(); 00869 // TEMP_ListOfRatios.clear(); 00870 // CD::Fn::CD_PolygonalMesh_vs_LineSegment( 00871 // &m_ModelLists.BasedOnFEM[FEMloc]->RefToSurfaceMesh(), // HE-based mesh model 00872 // &IPs[A], &IPs[B], // line segment 00873 // &TEMP_ListOfPoints, // list of intersected points 00874 // &TEMP_ListOfHEFaces, // list of intersected HE-based faces 00875 // &TEMP_ListOfHEVertices, // list of closest HE-based vertices to the intersected points 00876 // &TEMP_ListOfRatios // list of ratios of the closest vertices relative to the line segment [0(ptA)-1(ptB)] 00877 // ); 00878 // 00879 // if ( TEMP_ListOfHEVertices.size() > 0 ) { 00880 // 00881 // bCheckPunctureOut = false; 00882 // 00883 // if ( A > 0 ) { 00884 // 00885 // std::cout << "IPs' size: " << IPs.size() << std::endl; 00886 // std::cout << "A: " << A << std::endl; 00887 // std::cout << "B: " << B << std::endl; 00888 // 00889 // 00890 // // FIND THE VERTEX AT INTERACTION POINT#0 00891 // bool bFound = false; 00892 // unsigned int elementID; 00893 // Vector3<T> position = IPG.GetInteractionPointAtIndex( 0 ).GetPosition(); 00894 // position = m_ModelLists.BasedOnRBD[RBDloc]->CalMatrixTransform() * position; 00895 // position = pFEMHex->RefToTransformationSupport().GetInverseMatrixTransform() * position; 00896 // 00897 // // Create a new HEvertex to stick with IP point number 0 00898 // T coords[3]; 00899 // unsigned int Es[3] = { 00900 // pFEMHex->GetNumberOfElementsPerGridSize_X(), 00901 // pFEMHex->GetNumberOfElementsPerGridSize_Y(), 00902 // pFEMHex->GetNumberOfElementsPerGridSize_Z() 00903 // }; 00904 // for ( unsigned int x = 0; !bFound && x < Es[0]; ++x ) { 00905 // for ( unsigned int y = 0; !bFound && y < Es[1]; ++y ) { 00906 // for ( unsigned int z = 0; !bFound && z < Es[2]; ++z ) { 00907 // elementID = pFEMHex->GetElementIDFromGridLocation( x, y, z ); 00908 // if ( elementID >= 0 ) { 00909 // bFound = pFEMHex->RefToFEMMesh().RefToElement( elementID ).EstimateParametricCoords( 00910 // position[0], position[1], position[2], 00911 // coords[0], coords[1], coords[2] ) 00912 // > 0 ? true : false; 00913 // //* 00914 // // Replace the surface HEVertex with a created inner HEVertex inside the FEMModel 00915 // if ( bFound ) { 00916 // TEMP_ListOfHEVertices[0] = 00917 // pFEMHex->AddExtraVertexInLocalSpace( 00918 // position, elementID, Vector3<T>(x,y,z), coords 00919 // ); 00920 // } 00921 // //*/ 00922 // } 00923 // } 00924 // } 00925 // } 00926 // 00927 // //* 00928 // if ( bFound ) 00929 // { 00930 // AddVertexConnectionIPGModelAndHEModel( 00931 // RBDModel, 00932 // A, 00933 // Enum::AddOn::SLIDABLE, 00934 // FEMModel, 00935 // TEMP_ListOfHEVertices[0], // only the first point 00936 // Enum::AddOn::ATTACHED, 00937 // STATE_ForceRatio, 00938 // STATE_ForceScaleA, 00939 // STATE_ForceScaleB, 00940 // STATE_ForceThresholdA, 00941 // STATE_ForceThresholdB, 00942 // false, false 00943 // //STATE_EnforcePositionA, 00944 // //STATE_EnforcePositionB 00945 // ); 00946 // 00947 // // Slide the IP points from number 0 thru A-1 00948 // // pt 1 will inherit the sticked position from pt 0, and so on. 00949 // // So pt B will inherit the sticked position from pt A. 00950 // HEVertex<T> * pHEVert; 00951 // for ( unsigned int i = A; i > 0; --i ) { 00952 // pHEVert = m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][i-1]->GetPtrToVertexFromModel_2(); 00953 // m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][i]->SetPtrToVertexFromModel_2( pHEVert ); 00954 // } 00955 // // the fist IP pt 00956 // m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][0]->SetPtrToVertexFromModel_2( TEMP_ListOfHEVertices[0] ); 00957 // } 00958 // //*/ 00959 // 00960 // /* 00961 // // Slide the IP points from number 0 thru A-1 00962 // // pt 1 will inherit the sticked position from pt 0, and so on. 00963 // // So pt B will inherit the sticked position from pt A. 00964 // HEVertex<T> * pHEVert1 = TEMP_ListOfHEVertices[0]; 00965 // HEVertex<T> * pHEVert2; 00966 // for ( unsigned int i = 0; i < A; ++i ) { 00967 // pHEVert2 = m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][i]->GetPtrToVertexFromModel_2(); 00968 // m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][i]->SetPtrToVertexFromModel_2( pHEVert1 ); 00969 // pHEVert1 = pHEVert2; 00970 // } 00971 // TEMP_ListOfHEVertices[0] = pHEVert1; 00972 // //*/ 00973 // } 00974 // else 00975 // {// A == 0 00976 // AddVertexConnectionIPGModelAndHEModel( 00977 // RBDModel, 00978 // A, 00979 // Enum::AddOn::SLIDABLE, 00980 // FEMModel, 00981 // TEMP_ListOfHEVertices[0], // only the first point 00982 // Enum::AddOn::ATTACHED, 00983 // STATE_ForceRatio, 00984 // STATE_ForceScaleA, 00985 // STATE_ForceScaleB, 00986 // STATE_ForceThresholdA, 00987 // STATE_ForceThresholdB, 00988 // false, false 00989 // //STATE_EnforcePositionA, 00990 // //STATE_EnforcePositionB 00991 // ); 00992 // } 00993 // } 00994 // } 00995 // }//END: CASE PUNCTURING IN 00996 // 00997 // 00998 // 00999 // 01000 // bCheckPunctureOut = false; 01001 // 01002 // if ( bCheckPunctureOut ) 01003 // {//START: CASE PUNCTURING OUT 01004 // // Point numbers 01005 // unsigned int A = 0; // start of IP pt number with flag:CLEARED (before the slidable set) 01006 // unsigned int B = 1; // start of IP pt number with flag:SLIDABLE 01007 // unsigned int C = 2; // start of IP pt number-1 with flag:CLEARED (after the slidable set) 01008 // 01009 // while ( IPG.GetInteractionPointAtIndex( A ).GetSimFlags().CheckSimulationConstraints( Enum::AddOn::CLEARED ) ) { 01010 // ++A; 01011 // if ( A > IPs.size() - 2 ) return; 01012 // } 01013 // C = B = A + 1; 01014 // while ( IPG.GetInteractionPointAtIndex( C ).GetSimFlags().CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) ) { 01015 // ++C; 01016 // if ( C > IPs.size() - 1 ) return; 01017 // } 01018 // ++C; 01019 // 01020 // // A is CLEARED, B is SLIDABLE, and C-1 is CLEARED (or not exist, i.e. beyond the end of IPG) 01021 // if ( IPG.GetInteractionPointAtIndex( B ).GetSimFlags().CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) ) 01022 // { 01023 // // Check intersection 01024 // TEMP_ListOfPoints.clear(); 01025 // TEMP_ListOfHEFaces.clear(); 01026 // TEMP_ListOfHEVertices.clear(); 01027 // TEMP_ListOfRatios.clear(); 01028 // CD::Fn::CD_PolygonalMesh_vs_LineSegment( 01029 // &m_ModelLists.BasedOnFEM[FEMloc]->RefToSurfaceMesh(), // HE-based mesh model 01030 // &IPs[A], &IPs[B], // line segment 01031 // &TEMP_ListOfPoints, // list of intersected points 01032 // &TEMP_ListOfHEFaces, // list of intersected HE-based faces 01033 // &TEMP_ListOfHEVertices, // list of closest HE-based vertices to the intersected points 01034 // &TEMP_ListOfRatios // list of ratios of the closest vertices relative to the line segment [0(ptA)-1(ptB)] 01035 // ); 01036 // 01037 // if ( TEMP_ListOfHEVertices.size() > 0 ) { 01038 // 01039 // // IP pt#A punctures out from the FEM model 01040 // if ( A == 0 && IPG.GetInteractionPointAtIndex( A ).GetSimFlags().CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) ) { 01041 // // Add the surface point to the constraint 01042 // AddVertexConnectionIPGModelAndHEModel( 01043 // RBDModel, 01044 // C-1, 01045 // Enum::AddOn::SLIDABLE, 01046 // FEMModel, 01047 // TEMP_ListOfHEVertices[0], // only the first point 01048 // Enum::AddOn::ATTACHED, 01049 // STATE_ForceRatio, 01050 // STATE_ForceScaleA, 01051 // STATE_ForceScaleB, 01052 // STATE_ForceThresholdA, 01053 // STATE_ForceThresholdB, 01054 // STATE_EnforcePositionA, 01055 // STATE_EnforcePositionB 01056 // ); 01057 // 01058 // // Slide the IP points from number 0 thru A-1 01059 // // pt 1 will inherit the sticked position from pt 0, and so on. 01060 // // So pt B will inherit the sticked position from pt A. 01061 // HEVertex<T> * pHEVert; 01062 // for ( unsigned int i = C-1; i > 0; --i ) { 01063 // pHEVert = m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][i-1]->GetPtrToVertexFromModel_2(); 01064 // m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][i]->SetPtrToVertexFromModel_2( pHEVert ); 01065 // } 01066 // // the fist IP pt 01067 // m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc][0]->SetPtrToVertexFromModel_2( TEMP_ListOfHEVertices[0] ); 01068 // } 01069 // else { 01070 // } 01071 // } 01072 // } 01073 // }//END: CASE PUNCTURING OUT 01074 // } 01075 // else if ( pFEMTet ) { 01076 // PrtNotImplementedYet( "ProcessPuncturingOfCurveFormedByIPGonRBDModelToFEMModel for FEMTet" ); 01077 // } 01079 //} 01080 //----------------------------------------------------------------------------- 01081 // Process specific interactions 01082 //============================================================================= 01083 01084 01085 //----------------------------------------------------------------------------- 01086 template <typename T, typename DATA> 01087 void AdvSimSupport_DS<T,DATA>::SetERNodesForwardBackwardMovement_InteractHEModel ( unsigned int ERloc, unsigned int HEloc ) 01088 { 01089 AdvSimSupport_DATA<T,DATA>::ERNodePropsList & NodeProps = m_ModelLists.AllERNodePropsList[ERloc]; 01090 01091 // 01092 // Calculate length differences and clear the move status 01093 // 01094 // The first node 01095 unsigned int i = 0; 01096 NodeProps[i].MoveStatus = 0; 01097 if ( NodeProps[i].IsConstrained ) { 01098 Vector3<T> B = m_ModelLists.BasedOnER[ERloc]->RefToCurrentVertexPositionOfNodeNumber( i ); 01099 Vector3<T> C = m_ModelLists.BasedOnER[ERloc]->RefToCurrentVertexPositionOfNodeNumber( i+1 ); 01100 NodeProps[i].DiffLength = (B-C).Length() - m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[i].CenterlineRestLength; 01101 01102 //std::cout << "NodeProps[0].DiffLength: " << NodeProps[0].DiffLength << "\n"; 01103 } 01104 // The nodes between the first and the last nodes 01105 for ( i = 1; i < NodeProps.size()-1; ++i ) { 01106 NodeProps[i].MoveStatus = 0; // clear the move status 01107 if ( NodeProps[i].IsConstrained ) { 01108 Vector3<T> A = m_ModelLists.BasedOnER[ERloc]->RefToCurrentVertexPositionOfNodeNumber( i-1 ); 01109 Vector3<T> B = m_ModelLists.BasedOnER[ERloc]->RefToCurrentVertexPositionOfNodeNumber( i ); 01110 //Vector3<T> C = m_ModelLists.BasedOnER[ERloc]->RefToCurrentVertexPositionOfNodeNumber( i+1 ); 01111 //NodeProps[i].DiffLength = ( (A-B).Length() - (B-C).Length() ); 01112 NodeProps[i].DiffLength = (A-B).Length() - m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[i].CenterlineRestLength; 01113 //NodeProps[i].DiffLength = (B-C).Length() - m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[i].CenterlineRestLength; 01114 } 01115 } 01116 // The last node 01117 NodeProps[i].MoveStatus = 0; 01118 01119 //std::stringstream ss; // DEBUG 01120 //std::stringstream ss2; // DEBUG 01121 01122 // 01123 // Assign forward move status 01124 // 01125 // The first node 01126 //T LenThreshold = 0.05; // in unit of 1 means equal to the whole restlength of node i 01127 T ForwardMoveLenThreshold = m_InteractionLists.ERModelVsHEModel[ERloc][HEloc].Constants.DiffLengthForTriggeringForwardMove; 01128 i = 0; 01129 while ( i < NodeProps.size()-2 ) { 01130 if ( NodeProps[i].IsConstrained ) { 01131 // The first node of a group is always set to greater than 2000 01132 int numOfLinks = NodeProps[i].GroupID - 2000; 01133 if ( NodeProps[i].DiffLength / m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[i].CenterlineRestLength > ForwardMoveLenThreshold ) { 01134 NodeProps[i].MoveStatus += 4; // initiate move forward (4) 01135 do { 01136 unsigned int numOfLinks = NodeProps[i].GroupID - 2000; 01137 unsigned int p = i + numOfLinks; 01138 for ( ; i < p; ++i ) { 01139 NodeProps[i].MoveStatus += 1; // move forward (1) 01140 01141 //ss << ' ' << i; // DEBUG 01142 //ss2 << ' ' << (int) NodeProps[i].MoveStatus; // DEBUG 01143 01144 } 01145 } while ( i < NodeProps.size()-2 && NodeProps[i].IsConstrained ); 01146 } 01147 else { 01148 i += numOfLinks; 01149 } 01150 } 01151 else { 01152 ++i; 01153 } 01154 } 01155 01156 /* 01157 gDebugStr[11] = ss.str(); // DEBUG 01158 gDebugStr[12] = ss2.str(); // DEBUG 01159 if ( ss.str().size() > 0 ) { 01160 std::cout << "Links#: " << ss.str() << "\n"; 01161 std::cout << "Forward: " << ss2.str() << "\n"; 01162 } 01163 //*/ 01164 01165 // Assign backward move status 01166 //... 01167 } 01168 //----------------------------------------------------------------------------- 01169 01170 01171 //============================================================================= 01172 // Enfoce constraints 01173 //----------------------------------------------------------------------------- 01174 template <typename T, typename DATA> 01175 void AdvSimSupport_DS<T,DATA>::EnforceAllConstraints () 01176 { 01177 // Clear all connection (aka interaction) loads from FEM-based models 01178 for ( unsigned int i = 0; i < m_ModelLists.BasedOnFEM.size(); ++i ) { 01179 m_ModelLists.BasedOnFEM[i]->ClearAllConnectionLoads(); 01180 } 01181 01182 // Clear all point forces 01183 AdvSimSupport_DATA<T,DATA>::ListOfPointForces::iterator it = m_ListOfPointForces.begin(); 01184 while ( it != m_ListOfPointForces.end() ) { 01185 delete *it; 01186 ++it; 01187 } 01188 m_ListOfPointForces.clear(); 01189 01190 for ( unsigned int i = 0; i < GetNumberOfTotalModels(); ++i ) { 01191 for ( unsigned int j = 0; j < GetNumberOfTotalModels(); ++j ) { 01192 EnforceAllConstraintsBetweenTwoModels( i, j ); 01193 } 01194 } 01195 01196 #ifdef TAPs_MODEL_SURGICAL_SUTURE_WITH_HEAD_NEEDLE_HPP 01197 for ( unsigned int i = 0; i < m_ListOfInteraction_SwHDModelVsHEModel.size(); ++i ) { 01198 EnforceAllConstraintsOfSutureWithHeadNeedle( i ); 01199 } 01200 #endif//TAPs_MODEL_SURGICAL_SUTURE_WITH_HEAD_NEEDLE_HPP 01201 } 01202 //----------------------------------------------------------------------------- 01203 template <typename T, typename DATA> 01204 void AdvSimSupport_DS<T,DATA>::EnforceAllConstraintsBetweenTwoModels ( unsigned int modelA, unsigned int modelB ) 01205 { 01206 Enum::ModelType typeA = m_IDForModel[modelA].Type; 01207 Enum::ModelType typeB = m_IDForModel[modelB].Type; 01208 01209 // MODEL_BASED_ON_FEM vs MODEL_BASED_ON_FEM 01210 if ( typeA == Enum::MODEL_BASED_ON_FEM 01211 && typeB == Enum::MODEL_BASED_ON_FEM ) 01212 { 01213 unsigned int locA = m_IDForModel[modelA].Slot; 01214 unsigned int locB = m_IDForModel[modelB].Slot; 01215 if ( locA > locB ) return; 01216 Matrix4x4<T> TrxModelA = m_ModelLists.BasedOnFEM[locA]->RefToTransformationSupport().RefToMatrixTransform(); 01217 Matrix4x4<T> InvTrxModelA = TrxModelA.GetInverse(); 01218 Matrix4x4<T> TrxModelB = m_ModelLists.BasedOnFEM[locB]->RefToTransformationSupport().RefToMatrixTransform(); 01219 Matrix4x4<T> InvTrxModelB = TrxModelB.GetInverse(); 01220 locB -= locA; // since the data in the std::vector starts from 0, not from the model id 01221 for ( unsigned int i = 0; i < m_InteractionLists.HEModelVsHEModel[locA][locB].Constraints.size(); ++i ) { 01222 EnforceTheConstraintNumberOfFEMModelAndFEMModel( locA, TrxModelA, InvTrxModelA, locB, TrxModelB, InvTrxModelB, i ); 01223 } 01224 } 01225 // MODEL_BASED_ON_ER vs MODEL_BASED_ON_FEM 01226 else if ( typeA == Enum::MODEL_BASED_ON_ER 01227 && typeB == Enum::MODEL_BASED_ON_FEM ) 01228 { 01229 unsigned int locER = m_IDForModel[modelA].Slot; 01230 unsigned int locFEM = m_IDForModel[modelB].Slot; 01231 Matrix4x4<T> TrxFEMModel = m_ModelLists.BasedOnFEM[locFEM]->RefToTransformationSupport().RefToMatrixTransform(); 01232 Matrix4x4<T> InvTrxFEMModel = TrxFEMModel.GetInverse(); 01233 01234 SetERNodesForwardBackwardMovement_InteractHEModel( locER, locFEM ); 01235 01236 EnforceForwardAndBackwardMovementOfERModelAndFEMModel( locER, locFEM, TrxFEMModel, InvTrxFEMModel ); 01237 01238 /* 01239 // DEBUG 01240 AdvSimSupport_DATA<T,DATA>::ERNodePropsList & NodeProps = m_ModelLists.AllERNodePropsList[locER]; 01241 for ( unsigned int i = 0; i < NodeProps.size(); ++i ) { 01242 std::cout << "Check: " << i << "\t" << static_cast<int>( NodeProps[i].MoveStatus ) << "\n"; 01243 } 01244 //*/ 01245 01246 01247 // 01248 // For moving forward 01249 // 01250 // The constraints are sorted from low to high (of ERVertexID). 01251 // For forwarding moving, running backward from high to low to propagate the free ER node from the back to the front 01252 for ( int i = static_cast<int>( m_InteractionLists.ERModelVsHEModel[locER][locFEM].Constraints.size() ) - 1; i >= 0; --i ) { 01253 EnforceTheConstraintNumberOfERModelAndFEMModel( locER, locFEM, TrxFEMModel, InvTrxFEMModel, i ); 01254 } 01255 01256 /* 01257 // DEBUG 01258 { 01259 std::stringstream ss; 01260 ss << "#constraints: " << static_cast<int>( m_InteractionLists.ERModelVsHEModel[locER][locFEM].Constraints.size() ); 01261 gDebugStr[12] = ss.str(); 01262 } 01263 //*/ 01264 01265 // 01266 // For moving backward -- HAS TO DO SOMETHING WITH MOVING FORWARD -- TO AVOID BOTH FORWARD AND BACKWARD MOVING AT THE SAME TIME 01267 // 01268 //for ( unsigned int i = 0; i < m_InteractionLists.ERModelVsHEModel[locER][locFEM].size(); ++i ) { 01269 // ... 01270 //} 01271 } 01272 01273 //* 01274 // MODEL_BASED_ON_IPG_RBD vs MODEL_BASED_ON_FEM 01275 else if ( typeA == Enum::MODEL_BASED_ON_IPG_RBD 01276 && typeB == Enum::MODEL_BASED_ON_FEM ) 01277 { 01278 unsigned int locRBD = m_IDForModel[modelA].Slot; 01279 Matrix4x4<T> TrxModelRBD = m_ModelLists.BasedOnRBD[locRBD]->CalMatrixTransform(); 01280 Matrix4x4<T> InvTrxModelRBD = TrxModelRBD.GetInverse(); 01281 unsigned int locFEM = m_IDForModel[modelB].Slot; 01282 Matrix4x4<T> TrxModelFEM = m_ModelLists.BasedOnFEM[locFEM]->RefToTransformationSupport().RefToMatrixTransform(); 01283 Matrix4x4<T> InvTrxModelFEM = TrxModelFEM.GetInverse(); 01284 for ( unsigned int i = 0; i < m_InteractionLists.IPGModelVsHEModel[locRBD][locFEM].Constraints.size(); ++i ) { 01285 EnforceTheConstraintNumberOfRBDModelAndFEMModel( locRBD, TrxModelRBD, InvTrxModelRBD, locFEM, TrxModelFEM, InvTrxModelFEM, i ); 01286 } 01287 } 01288 //*/ 01289 } 01290 //----------------------------------------------------------------------------- 01291 template <typename T, typename DATA> 01292 void AdvSimSupport_DS<T,DATA>::EnforceTheConstraintNumberOfFEMModelAndFEMModel ( 01293 unsigned int locA, 01294 Matrix4x4<T> const & TrxModelA, 01295 Matrix4x4<T> const & InvTrxModelA, 01296 unsigned int locB, 01297 Matrix4x4<T> const & TrxModelB, 01298 Matrix4x4<T> const & InvTrxModelB, 01299 unsigned int constraintNumber 01300 ) 01301 { 01302 typename AdvSimSupport_DATA<T,DATA>::Constraint_HEModelVsHEModel * pConstraint = m_InteractionLists.HEModelVsHEModel[locA][locB].Constraints[constraintNumber]; 01303 01304 HEVertex<T> * heA = pConstraint->GetPtrToVertexFromModel_1(); 01305 HEVertex<T> * heB = pConstraint->GetPtrToVertexFromModel_2(); 01306 Vector3<T> V1 = heA->GetProtectedPosition() = pConstraint->RefToSavedPositionA(); 01307 Vector3<T> V2 = heB->GetProtectedPosition() = pConstraint->RefToSavedPositionB(); 01308 V1 = TrxModelA * V1; 01309 V2 = TrxModelB * V2; 01310 T ratio = pConstraint->GetForceRatio(); 01311 pConstraint->RefToTargetPositionA() = pConstraint->RefToTargetPositionB() = ratio*V1 + (1-ratio)*V2; 01312 V1 = InvTrxModelA * pConstraint->RefToTargetPositionA(); 01313 V2 = InvTrxModelB * pConstraint->RefToTargetPositionB(); 01314 pConstraint->RefToTargetPositionA() = V1; 01315 pConstraint->RefToTargetPositionB() = V2; 01316 01317 // locA is FEM-based, add the connection as a point force. 01318 { 01319 Vector3<T> Fa = V1 - heA->GetProtectedPosition(); 01320 PointForce<T> * pF = new PointForce<T>(); 01321 m_ListOfPointForces.push_back( pF ); 01322 01323 // Set connection (i.e. point) force 01324 pF->SetID( heA->GetID() ); 01325 pF->RefToPosition() = heA->GetBarycentricPtr()->RetGenBaryCoords(); 01326 pF->RefToForce() = Fa * pConstraint->GetForceScaleA(); 01327 01328 // Scale by the timestep and mass 01329 pF->RefToForce() /= m_ModelLists.BasedOnFEM[locA]->GetTimeStep() 01330 * m_ModelLists.BasedOnFEM[locA]->PtrToFEMMesh()->MassOfNode( 0 ).Length(); 01331 01332 m_ModelLists.BasedOnFEM[locA]->AddConnectionForceInLocalCoordinates( m_ListOfPointForces.back() ); 01333 } 01334 // locB is FEM-based, add the connection as a point force. 01335 { 01336 Vector3<T> Fb = V2 - heB->GetProtectedPosition(); 01337 PointForce<T> * pF = new PointForce<T>(); 01338 m_ListOfPointForces.push_back( pF ); 01339 01340 // Set connection (i.e. point) force 01341 pF->SetID( heB->GetID() ); 01342 pF->RefToPosition() = heB->GetBarycentricPtr()->RetGenBaryCoords(); 01343 pF->RefToForce() = Fb * pConstraint->GetForceScaleB(); 01344 01345 // Scale by the timestep and mass 01346 pF->RefToForce() /= m_ModelLists.BasedOnFEM[locB]->GetTimeStep() 01347 * m_ModelLists.BasedOnFEM[locB]->PtrToFEMMesh()->MassOfNode( 0 ).Length(); 01348 01349 m_ModelLists.BasedOnFEM[locB]->AddConnectionForceInLocalCoordinates( m_ListOfPointForces.back() ); 01350 } 01351 } 01352 //----------------------------------------------------------------------------- 01353 template <typename T, typename DATA> 01354 void AdvSimSupport_DS<T,DATA>::EnforceForwardAndBackwardMovementOfERModelAndFEMModel ( 01355 unsigned int ERloc, 01356 unsigned int FEMloc, 01357 Matrix4x4<T> const & TrxFEMModel, 01358 Matrix4x4<T> const & InvTrxFEMModel 01359 ) 01360 { 01361 // Set the ERModel's last slidable point 01362 int ERModel_last_slidable_point = m_ModelLists.BasedOnER[ERloc]->GetNumberOfLinks() - m_OffsetToSutureLastSlidablePoint; 01363 01364 // 01365 // For moving forward 01366 // 01367 // The constraints are sorted from low to high (of ERVertexID). 01368 // For forwarding moving, running backward from high to low to propagate the free ER node from the back to the front 01369 for ( int i = static_cast<int>( m_InteractionLists.ERModelVsHEModel[ERloc][FEMloc].Constraints.size() ) - 1; i >= 0; --i ) { 01370 01371 AdvSimSupport_DATA<T,DATA>::Constraint_ERModelVsHEModel * pConstraint = m_InteractionLists.ERModelVsHEModel[ERloc][FEMloc].Constraints[i]; 01372 int ERModelPt = pConstraint->GetVertexIDFromModel_1(); 01373 01374 if ( ERModelPt < ERModel_last_slidable_point ) { // PROTECTED FROM REACHING THE LAST POINT 01375 if ( 01376 // Check if the ERModel's point is slidable 01377 m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].SimFlags.CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) 01378 && 01379 // Check if the next ERModel's point is not attached 01380 m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt+1].SimFlags.CheckSimulationConstraints( Enum::AddOn::ATTACHED ) == false 01381 ) 01382 { 01383 bool bMoveForward = m_ModelLists.AllERNodePropsList[ERloc][ERModelPt].MoveStatus & 0x01; 01384 if ( bMoveForward ) 01385 { 01386 //if ( 01387 // Backward ??? 01388 01389 //----------------------------------------------- 01390 // Forward 01391 //----------------------------------------------- 01392 // Clear simulation states of the current node, e.g. clear ATTACHED status 01393 m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].ExternalForce_ForAdvSimCtrl.Clear(); 01394 m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].SimFlags.ClearSimulationConstraints( Enum::AddOn::ATTACHED ); 01395 m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].SimFlags.ClearSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 01396 01397 // Setting PropsList 01398 int savedGroupID = m_ModelLists.AllERNodePropsList[ERloc][ERModelPt].GroupID; 01399 m_ModelLists.AllERNodePropsList[ERloc][ERModelPt].GroupID = 0; 01400 m_ModelLists.AllERNodePropsList[ERloc][ERModelPt].IsConstrained = false; 01401 01402 bool bEnforcedPos = m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].UseEnforcedPosition; 01403 if ( bEnforcedPos ) { 01404 m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].UseEnforcedPosition = false; 01405 } 01406 01407 ++ERModelPt; // move to next ERModel's point 01408 01409 pConstraint->SetVertexIDFromModel_1( ERModelPt ); 01410 01411 // Set the status of the new ERModel's point 01412 01413 // Set ERModel's point's status 01414 if ( ERModelPt < ERModel_last_slidable_point ) { 01415 m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 01416 } 01417 m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 01418 m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 01419 01420 // Setting PropsList 01421 m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].UseEnforcedPosition = bEnforcedPos; 01422 m_ModelLists.AllERNodePropsList[ERloc][ERModelPt].GroupID = savedGroupID; 01423 m_ModelLists.AllERNodePropsList[ERloc][ERModelPt].IsConstrained = true; 01424 } 01425 } 01426 } 01427 } 01428 } 01429 //----------------------------------------------------------------------------- 01430 template <typename T, typename DATA> 01431 void AdvSimSupport_DS<T,DATA>::EnforceTheConstraintNumberOfERModelAndFEMModel ( 01432 unsigned int ERloc, 01433 unsigned int FEMloc, 01434 Matrix4x4<T> const & TrxFEMModel, 01435 Matrix4x4<T> const & InvTrxFEMModel, 01436 unsigned int constraintNumber 01437 ) 01438 { 01439 AdvSimSupport_DATA<T,DATA>::Constraint_ERModelVsHEModel * pConstraint = m_InteractionLists.ERModelVsHEModel[ERloc][FEMloc].Constraints[constraintNumber]; 01440 int ERModelPt = pConstraint->GetVertexIDFromModel_1(); 01441 01442 01443 // DEBUG 01444 //std::cout << "Constraint# " << constraintNumber << " has ERvertex# " << ERModelPt << "\n"; 01445 01446 { 01447 Vector3<T> & S = m_ModelLists.BasedOnER[ERloc]->RefToCurrentVertexPositionOfNodeNumber( ERModelPt ); 01448 HEVertex<T> * he = pConstraint->GetPtrToVertexFromModel_2(); 01449 Vector3<T> V = he->GetProtectedPosition() = pConstraint->RefToSavedPositionB(); 01450 V = TrxFEMModel * V; 01451 T ratio = pConstraint->GetForceRatio(); 01452 pConstraint->RefToTargetPositionA() = 01453 pConstraint->RefToTargetPositionB() = ratio*S + (1-ratio)*V; 01454 //S = pConstraint->RefToTargetPositionA(); // ERModel's point's current position 01455 V = InvTrxFEMModel * pConstraint->RefToTargetPositionB(); 01456 pConstraint->RefToTargetPositionB() = V; 01457 01458 // FEM-based model, add the connection as a point force. 01459 { 01460 Vector3<T> F = V - he->GetProtectedPosition(); 01461 PointForce<T> * pF = new PointForce<T>(); 01462 m_ListOfPointForces.push_back( pF ); 01463 01464 pF->SetID( he->GetID() ); 01465 pF->RefToPosition() = he->GetBarycentricPtr()->RetGenBaryCoords(); 01466 01467 #ifdef TAPs_USE_NONLINEAR_FORCES 01468 T mag = F.Length(); 01469 F /= mag; 01470 pF->RefToForce() = F * pConstraint->GetForceScaleB() * ( pow( 2.718281828, mag ) - 1 ); 01471 //pF->RefToForce() = F * pConstraint->GetForceScaleB() * ( pow( 10, mag ) - 1 ); 01472 #else //TAPs_USE_NONLINEAR_FORCES 01473 pF->RefToForce() = F * pConstraint->GetForceScaleB(); 01474 #endif//TAPs_USE_NONLINEAR_FORCES 01475 01476 // Scale pF by the timestep and mass 01477 pF->RefToForce() /= m_ModelLists.BasedOnFEM[FEMloc]->GetTimeStep() 01478 * m_ModelLists.BasedOnFEM[FEMloc]->PtrToFEMMesh()->MassOfNode( 0 ).Length(); 01479 01480 m_ModelLists.BasedOnFEM[FEMloc]->AddConnectionForceInLocalCoordinates( m_ListOfPointForces.back() ); 01481 01482 // ??????? 01483 //std::cout << "WHY THE SIMFLAGS IS NOT SET PROPERLY (GOT RESET SOMEWHERE) ???\n"; 01484 01485 //std::cout << "F mag: " << pF->RefToForce().Magnitude() << "\n"; 01486 //std::cout << "Force Ratio: " << pConstraint->GetForceRatio() << "\n"; 01487 //std::cout << "Force Scale A: " << pConstraint->GetForceScaleA() << "\n"; 01488 //std::cout << "Force Threshold A: " << pConstraint->GetForceThresholdA() << "\n"; 01489 //std::cout << "Force Scale B: " << pConstraint->GetForceScaleB() << "\n"; 01490 //std::cout << "Force Threshold B: " << pConstraint->GetForceThresholdB() << "\n"; 01491 //std::cout << "SuturePt: " << SuturePt << "\n"; 01492 //std::cout << m_ModelLists.BasedOnER[ERModel]->GetListOfNodes()[SuturePt].SimFlags << "\n"; 01493 01494 //if ( SuturePt < m_ModelLists.BasedOnER[ERModel]->GetNumberOfLinks()-1 ) { 01495 // //if ( m_ModelLists.BasedOnER[ERModel]->GetListOfNodes()[SuturePt].SimFlags.CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) ) 01496 // // WHY THE SIMFLAGS IS NOT SET PROPERLY (GOT RESET SOMEWHERE) ??? 01497 // { 01498 // if ( pF->RefToForce().Magnitude() > pConstraint->GetForceThresholdA() ) 01499 // { 01500 // //if ( 01501 // // Backward ??? 01502 01503 // // Forward 01504 // ++SuturePt; 01505 01506 // std::cout << "SuturePt after: " << SuturePt << "\n"; 01507 01508 // pConstraint->SetVertexIDFromSuture( SuturePt ); 01509 // m_ModelLists.BasedOnER[ERModel]->GetListOfNodes()[SuturePt].SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 01510 // } 01511 // } 01512 //} 01513 } 01514 01515 // Calculate the external force apply to Suture 01516 Vector3<T> Fs = pConstraint->RefToTargetPositionA() - S; 01517 01518 //----------------------------------------- 01519 #ifdef TAPs_USE_NONLINEAR_FORCES 01520 T mag = Fs.Length(); 01521 Fs /= mag; 01522 Fs *= pConstraint->GetForceScaleA() * ( pow( 2.718281828, mag ) - 1 ); 01523 //Fs *= pConstraint->GetForceScaleA() * ( pow( 10, mag ) - 1 ); 01524 #else //TAPs_USE_NONLINEAR_FORCES 01525 Fs *= pConstraint->GetForceScaleA(); 01526 #endif//TAPs_USE_NONLINEAR_FORCES 01527 //----------------------------------------- 01528 01529 // Scale Fs by the timestep, iteration times, and mass 01530 Fs /= m_ModelLists.BasedOnER[ERloc]->GetSimTimeStep() 01531 * m_ModelLists.BasedOnER[ERloc]->GetSimIterationTimes() 01532 * m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].GetMass(); 01533 01534 T SforceMag = Fs.Magnitude(); 01535 //Fs /= m_ModelLists.BasedOnER[ERModel]->GetSimTimeStep(); 01536 01537 //std::cout << "GetForceScaleA: " << pConstraint->GetForceScaleA() << "\n"; 01538 //std::cout << "Fs: " << Fs << "\n"; 01539 m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].ExternalForce_ForAdvSimCtrl = Fs; 01540 01541 // 01542 // Move the current centerline position to the (current) target position 01543 // 01544 //m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].Centerline[m_ModelLists.BasedOnER[ERloc]->GetPreviousIndex()].SetPosition( pConstraint->RefToTargetPositionA() ); 01545 m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].Centerline[m_ModelLists.BasedOnER[ERloc]->GetCurrentIndex()].SetPosition( pConstraint->RefToTargetPositionA() ); 01546 01547 // Set the ERModel's last slidable point 01548 int ERModel_last_slidable_point = m_ModelLists.BasedOnER[ERloc]->GetNumberOfLinks() - m_OffsetToSutureLastSlidablePoint; 01549 01550 //std::cout << "Enforced ERModelPt: " << ERModelPt << "\n"; 01551 01552 // For slidable ERModel's points FORWARD 01553 //if ( !m_ModelLists.BasedOnER[ERModel]->GetListOfNodes()[SuturePt].UseEnforcedPosition ... && ... 01554 //if ( false ) { 01556 // if ( 01557 // // Check if the ERModel's point is slidable 01558 // m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].SimFlags.CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) 01559 // && 01560 // // Check if the next ERModel's point is not attached 01561 // m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt+1].SimFlags.CheckSimulationConstraints( Enum::AddOn::ATTACHED ) == false 01562 // ) { 01563 // // WHY THE SIMFLAGS IS NOT SET PROPERLY (GOT RESET SOMEWHERE) ??? 01564 // // If the next ERModel's point is not attached, then check if the ERModel's point will slide due to the high force. 01565 // //if ( m_ModelLists.BasedOnER[ERModel]->GetListOfNodes()[ERModelPt+1].SimFlags.CheckSimulationConstraints( Enum::AddOn::ATTACHED ) ) 01566 // { 01567 // /* 01568 // // Calculate distance 01569 // T distDiff = 0; 01570 // if ( ERModelPt > 0 ) { 01571 // Vector3<T> A = m_ModelLists.BasedOnER[ERloc]->RefToCurrentVertexPositionOfNodeNumber( ERModelPt-1 ); 01572 // Vector3<T> B = m_ModelLists.BasedOnER[ERloc]->RefToCurrentVertexPositionOfNodeNumber( ERModelPt ); 01573 // Vector3<T> C = m_ModelLists.BasedOnER[ERloc]->RefToCurrentVertexPositionOfNodeNumber( ERModelPt+1 ); 01574 // distDiff = (A-B).Length() - (B-C).Length(); 01575 01576 // //std::cout << "distDiff: " << distDiff << "\n"; 01577 // //std::cout << "ERModelPt: " << ERModelPt << "\n"; 01578 // } 01579 // if ( SforceMag > pConstraint->GetForceThresholdA() || distDiff > 0.001 ) 01580 // //*/ 01581 01582 // //* 01583 // bool bMoveForward = m_ModelLists.AllERNodePropsList[ERloc][ERModelPt].MoveStatus & 0x01; 01584 // if ( bMoveForward || SforceMag > pConstraint->GetForceThresholdA() ) 01585 // //*/ 01586 01587 // { 01588 // //if ( 01589 // // Backward ??? 01590 01591 // //----------------------------------------------- 01592 // // Forward 01593 // //----------------------------------------------- 01594 // // Clear simulation states of the current node, e.g. clear ATTACHED status 01595 // m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].ExternalForce_ForAdvSimCtrl.Clear(); 01596 // m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].SimFlags.ClearSimulationConstraints( Enum::AddOn::ATTACHED ); 01597 // m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].SimFlags.ClearSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 01598 01599 // // Setting PropsList 01600 // int savedGroupID = m_ModelLists.AllERNodePropsList[ERloc][ERModelPt].GroupID; 01601 // m_ModelLists.AllERNodePropsList[ERloc][ERModelPt].GroupID = 0; 01602 // m_ModelLists.AllERNodePropsList[ERloc][ERModelPt].IsConstrained = false; 01603 01604 // bool bEnforcedPos = m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].UseEnforcedPosition; 01605 // if ( bEnforcedPos ) { 01606 // m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].UseEnforcedPosition = false; 01607 // } 01608 01609 // { 01610 // ++ERModelPt; // move to next ERModel's point 01611 01612 // pConstraint->SetVertexIDFromModel_1( ERModelPt ); 01613 01614 // // Set the status of the new ERModel's point 01615 01616 // // Calculate the external force apply to ERModel's point 01617 // Vector3<T> Fs = pConstraint->RefToTargetPositionA() - m_ModelLists.BasedOnER[ERloc]->RefToCurrentVertexPositionOfNodeNumber( ERModelPt ); 01618 01619 // //----------------------------------------- 01620 // #ifdef TAPs_USE_NONLINEAR_FORCES 01621 // T mag = Fs.Length(); 01622 // Fs /= mag; 01623 // Fs *= pConstraint->GetForceScaleA() * ( pow( 2.718281828, mag ) - 1 ); 01624 // //Fs *= pConstraint->GetForceScaleA() * ( pow( 10, mag ) - 1 ); 01625 // #else //TAPs_USE_NONLINEAR_FORCES 01626 // Fs *= pConstraint->GetForceScaleA(); 01627 // #endif//TAPs_USE_NONLINEAR_FORCES 01628 // //----------------------------------------- 01629 01630 // // Scale Fs by the timestep, iteration times, and mass 01631 // Fs /= m_ModelLists.BasedOnER[ERloc]->GetSimTimeStep() 01632 // * m_ModelLists.BasedOnER[ERloc]->GetSimIterationTimes() 01633 // * m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].GetMass(); 01634 01635 // // Set ERModel's point's status 01636 // m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].ExternalForce_ForAdvSimCtrl = Fs; 01637 // if ( ERModelPt < ERModel_last_slidable_point ) { 01638 // m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 01639 // } 01640 // m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 01641 // m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 01642 01643 // // Setting PropsList 01644 // m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[ERModelPt].UseEnforcedPosition = bEnforcedPos; 01645 // m_ModelLists.AllERNodePropsList[ERloc][ERModelPt].GroupID = savedGroupID; 01646 // m_ModelLists.AllERNodePropsList[ERloc][ERModelPt].IsConstrained = true; 01647 // } 01648 // } 01649 // } 01650 // } 01651 //} 01652 } 01653 } 01654 //----------------------------------------------------------------------------- 01655 template <typename T, typename DATA> 01656 void AdvSimSupport_DS<T,DATA>::EnforceTheConstraintNumberOfRBDModelAndFEMModel ( 01657 unsigned int RBDloc, 01658 Matrix4x4<T> const & TrxRBDModel, 01659 Matrix4x4<T> const & InvTrxRBDModel, 01660 unsigned int FEMloc, 01661 Matrix4x4<T> const & TrxFEMModel, 01662 Matrix4x4<T> const & InvTrxFEMModel, 01663 unsigned int constraintNumber 01664 ) 01665 { 01666 //return; 01667 01668 AdvSimSupport_DATA<T,DATA>::Constraint_IPGModelVsHEModel * pConstraint = m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc].Constraints[constraintNumber]; 01669 01670 unsigned int vertID = pConstraint->GetVertexIDFromModel_1(); 01671 HEVertex<T> * pHEvert = pConstraint->GetPtrToVertexFromModel_2(); 01672 Vector3<T> V1 = m_ModelLists.BasedOnRBD[RBDloc]->RefToInteractionPointGroup().GetInteractionPointAtIndex( vertID ).GetPosition(); 01673 Vector3<T> V2 = pHEvert->GetProtectedPosition() = pConstraint->RefToSavedPositionB(); 01674 V1 = TrxRBDModel * V1; 01675 V2 = TrxFEMModel * V2; 01676 T ratio = pConstraint->GetForceRatio(); 01677 pConstraint->RefToTargetPositionA() = pConstraint->RefToTargetPositionB() = ratio*V1 + (1-ratio)*V2; 01678 V1 = InvTrxRBDModel * pConstraint->RefToTargetPositionA(); 01679 V2 = InvTrxFEMModel * pConstraint->RefToTargetPositionB(); 01680 pConstraint->RefToTargetPositionA() = V1; 01681 pConstraint->RefToTargetPositionB() = V2; 01682 01683 // RBD-based, add the connection as a point force. 01684 { 01685 } 01686 // FEM-based, add the connection as a point force. 01687 { 01688 Vector3<T> Fb = V2 - pHEvert->GetProtectedPosition(); 01689 PointForce<T> * pF = new PointForce<T>(); 01690 m_ListOfPointForces.push_back( pF ); 01691 01692 // Set connection (i.e. point) force 01693 pF->SetID( pHEvert->GetID() ); 01694 pF->RefToPosition() = pHEvert->GetBarycentricPtr()->RetGenBaryCoords(); 01695 pF->RefToForce() = Fb * pConstraint->GetForceScaleB(); 01696 01697 // Scale pF by the timestep and mass 01698 pF->RefToForce() /= m_ModelLists.BasedOnFEM[FEMloc]->GetTimeStep() 01699 * m_ModelLists.BasedOnFEM[FEMloc]->PtrToFEMMesh()->MassOfNode( 0 ).Length(); 01700 01701 m_ModelLists.BasedOnFEM[FEMloc]->AddConnectionForceInLocalCoordinates( m_ListOfPointForces.back() ); 01702 } 01703 } 01704 //----------------------------------------------------------------------------- 01705 // Enfoce constraints 01706 //============================================================================= 01707 01708 01709 01710 01711 //============================================================================= 01712 // Enforce consistent positions 01713 //----------------------------------------------------------------------------- 01714 template <typename T, typename DATA> 01715 void AdvSimSupport_DS<T,DATA>::EnforceAllConsistentPositions () 01716 { 01717 // HEModel-HEModel interactions 01718 for ( unsigned int i = 0; i < m_InteractionLists.HEModelVsHEModel.size(); ++i ) { 01719 for ( unsigned int j = i; j < m_InteractionLists.HEModelVsHEModel[i].size(); ++j ) { 01720 EnforceAllConsistentPositionsOfHEModelAndHEModel( i, j ); 01721 } 01722 } 01723 01724 // ERModel-HEModel interactions 01725 for ( unsigned int i = 0; i < m_InteractionLists.ERModelVsHEModel.size(); ++i ) { 01726 for ( unsigned int j = 0; j < m_InteractionLists.ERModelVsHEModel[i].size(); ++j ) { 01727 EnforceAllConsistentPositionsOfERModelAndHEModel( i, j ); 01728 } 01729 } 01730 01731 // IPGModel-HEModel interactions 01732 for ( unsigned int i = 0; i < m_InteractionLists.IPGModelVsHEModel.size(); ++i ) { 01733 for ( unsigned int j = 0; j < m_InteractionLists.IPGModelVsHEModel[i].size(); ++j ) { 01734 EnforceAllConsistentPositionsOfIPGModelAndHEModel( i, j ); 01735 } 01736 } 01737 01738 #ifdef TAPs_MODEL_SURGICAL_SUTURE_WITH_HEAD_NEEDLE_HPP 01739 // Head needles with FEM-based models 01740 // For the head needle's IPG 01741 for ( unsigned int i = 0; i < m_ListOfInteraction_SwHDModelVsHEModel.size(); ++i ) { 01742 m_ListOfInteraction_SwHDModelVsHEModel[i]->EnforceAllConsistentPositions_ForIPG( 01743 m_IDForModel, 01744 m_ModelLists.BasedOnER, 01745 m_ModelLists.BasedOnFEM, 01746 m_ModelLists.BasedOnRBD 01747 ); 01748 } 01749 // For the head needle's circle path 01750 for ( unsigned int i = 0; i < m_ListOfInteraction_SwHDModelVsHEModel.size(); ++i ) { 01751 m_ListOfInteraction_SwHDModelVsHEModel[i]->EnforceAllConsistentPositions_ForCirclePath( 01752 m_IDForModel, 01753 m_ModelLists.BasedOnER, 01754 m_ModelLists.BasedOnFEM, 01755 m_ModelLists.BasedOnRBD 01756 ); 01757 } 01758 #endif//TAPs_MODEL_SURGICAL_SUTURE_WITH_HEAD_NEEDLE_HPP 01759 } 01760 //----------------------------------------------------------------------------- 01761 template <typename T, typename DATA> 01762 void AdvSimSupport_DS<T,DATA>::EnforceAllConsistentPositionsOfHEModelAndHEModel ( unsigned int locA, unsigned int locB ) 01763 { 01764 locB -= locA; // since the data in the std::vector starts from 0, not from the model id 01765 AdvSimSupport_DATA<T,DATA>::InteractionConstraints_HEModelVsHEModel::iterator it = m_InteractionLists.HEModelVsHEModel[locA][locB].Constraints.begin(); 01766 while ( it != m_InteractionLists.HEModelVsHEModel[locA][locB].Constraints.end() ) { 01767 (*it)->RefToSavedPositionA() = (*it)->GetPtrToVertexFromModel_1()->GetProtectedPosition(); 01768 (*it)->RefToSavedPositionB() = (*it)->GetPtrToVertexFromModel_2()->GetProtectedPosition(); 01769 (*it)->GetPtrToVertexFromModel_1()->GetProtectedPosition() = (*it)->RefToTargetPositionA(); 01770 (*it)->GetPtrToVertexFromModel_2()->GetProtectedPosition() = (*it)->RefToTargetPositionB(); 01771 ++it; 01772 } 01773 } 01774 //----------------------------------------------------------------------------- 01775 template <typename T, typename DATA> 01776 void AdvSimSupport_DS<T,DATA>::EnforceAllConsistentPositionsOfERModelAndHEModel ( unsigned int ERloc, unsigned int HEloc ) 01777 { 01778 Matrix4x4<T> TrxModel = m_ModelLists.BasedOnFEM[HEloc]->RefToTransformationSupport().RefToMatrixTransform(); 01779 Matrix4x4<T> InvTrxModel = TrxModel.GetInverse(); 01780 01781 AdvSimSupport_DATA<T,DATA>::InteractionConstraints_ERModelVsHEModel::iterator it = m_InteractionLists.ERModelVsHEModel[ERloc][HEloc].Constraints.begin(); 01782 while ( it != m_InteractionLists.ERModelVsHEModel[ERloc][HEloc].Constraints.end() ) { 01783 01784 // Move the ERModel's constrained point to the target point 01785 //Vector3<T> & Sp = m_ModelLists.BasedOnER[ERloc]->RefToPreviousVertexPositionOfNodeNumber( (*it)->GetVertexIDFromModel_1() ); 01786 //Vector3<T> & Sc = m_ModelLists.BasedOnER[ERloc]->RefToCurrentVertexPositionOfNodeNumber( (*it)->GetVertexIDFromModel_1() ); 01787 //Sp = Sc = (*it)->RefToTargetPositionA(); 01788 01789 unsigned int nodeID = (*it)->GetVertexIDFromModel_1(); 01790 01791 // Enforce ER-based model's Position 01792 if ( (*it)->GetEnforcePositionStatusA() ) { 01793 m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[nodeID].UseEnforcedPosition = true; 01794 m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[nodeID].EnforcedPosition = (*it)->RefToTargetPositionA(); 01795 } 01796 else { 01797 } 01798 01799 (*it)->RefToSavedPositionB() = (*it)->GetPtrToVertexFromModel_2()->GetProtectedPosition(); 01800 01801 // Move the model's constrained point to the target point 01802 if ( false ) 01803 { 01804 // TO THE TARGET POSITION B 01805 if ( (*it)->GetEnforcePositionStatusB() ) { 01806 (*it)->GetPtrToVertexFromModel_2()->GetProtectedPosition() = (*it)->RefToTargetPositionB(); 01807 } 01808 } 01809 else { 01810 // TO THE SUTURE POINT 01811 if ( (*it)->GetEnforcePositionStatusB() ) { 01812 01813 //#ifdef TAPs_USE_VERTEX_POSITION_AVERAGING 01814 // std::vector< HEVertex<T> * > firstRing = (*it)->GetPtrToVertexFromModel_2()->FindTheFirstRingOfVertices(); 01815 // unsigned int size = firstRing.size(); 01816 // if ( size > 0 ) { 01817 // Vector3<T> avgPos; 01818 // for ( unsigned int i = 0; i < size; ++i ) { 01819 // avgPos += firstRing[i]->GetPosition(); 01820 // } 01821 // T scale = size;// / T(1); 01822 // Vector3<T> targetPos = InvTrxModel * m_ModelLists.BasedOnER[ERloc]->RefToCurrentVertexPositionOfNodeNumber( (*it)->GetVertexIDFromModel_1() ); 01823 // avgPos = ( avgPos + targetPos*scale ) / ( size + scale ); 01824 // (*it)->GetPtrToVertexFromModel_2()->GetProtectedPosition() = avgPos; 01825 // } 01826 // else { 01827 // (*it)->GetPtrToVertexFromModel_2()->GetProtectedPosition() = InvTrxModel * 01828 // m_ModelLists.BasedOnER[ERloc]->RefToCurrentVertexPositionOfNodeNumber( 01829 // (*it)->GetVertexIDFromModel_1() ); 01830 // } 01831 //#else //TAPs_USE_VERTEX_POSITION_AVERAGING 01832 //(*it)->GetPtrToVertexFromModel_2()->GetProtectedPosition() = InvTrxModel * 01833 //m_ModelLists.BasedOnER[ERloc]->RefToCurrentVertexPositionOfNodeNumber( 01834 // (*it)->GetVertexIDFromModel_1() ); 01835 //#endif//TAPs_USE_VERTEX_POSITION_AVERAGING 01836 01837 01838 #ifdef TAPs_USE_VERTEX_POSITION_AVERAGING 01839 std::vector< HEVertex<T> * > firstRing = (*it)->GetPtrToVertexFromModel_2()->FindTheFirstRingOfVertices(); 01840 unsigned int size = firstRing.size(); 01841 if ( size > 0 ) { 01842 Vector3<T> targetPos = InvTrxModel * m_ModelLists.BasedOnER[ERloc]->RefToCurrentVertexPositionOfNodeNumber( (*it)->GetVertexIDFromModel_1() ); 01843 Vector3<T> avgPos; 01844 for ( unsigned int i = 0; i < size; ++i ) { 01845 avgPos = ( firstRing[i]->GetPosition() + targetPos ) / 2; 01846 firstRing[i]->GetProtectedPosition() = avgPos; 01847 } 01848 (*it)->GetPtrToVertexFromModel_2()->GetProtectedPosition() = targetPos; 01849 } 01850 else { 01851 (*it)->GetPtrToVertexFromModel_2()->GetProtectedPosition() = InvTrxModel * 01852 m_ModelLists.BasedOnER[ERloc]->RefToCurrentVertexPositionOfNodeNumber( 01853 (*it)->GetVertexIDFromModel_1() ); 01854 } 01855 #else //TAPs_USE_VERTEX_POSITION_AVERAGING 01856 (*it)->GetPtrToVertexFromModel_2()->GetProtectedPosition() = InvTrxModel * 01857 m_ModelLists.BasedOnER[ERloc]->RefToCurrentVertexPositionOfNodeNumber( 01858 (*it)->GetVertexIDFromModel_1() ); 01859 #endif//TAPs_USE_VERTEX_POSITION_AVERAGING 01860 01861 } 01862 } 01863 01864 ++it; 01865 } 01866 } 01867 //----------------------------------------------------------------------------- 01868 template <typename T, typename DATA> 01869 void AdvSimSupport_DS<T,DATA>::EnforceAllConsistentPositionsOfIPGModelAndHEModel ( unsigned int IPGloc, unsigned int HEloc ) 01870 { 01871 AdvSimSupport_DATA<T,DATA>::InteractionConstraints_IPGModelVsHEModel::iterator it = m_InteractionLists.IPGModelVsHEModel[IPGloc][HEloc].Constraints.begin(); 01872 while ( it != m_InteractionLists.IPGModelVsHEModel[IPGloc][HEloc].Constraints.end() ) { 01873 //(*it)->RefToSavedPositionA() = (*it)->GetPtrToVertexFromModel_1()->GetProtectedPosition(); 01874 (*it)->RefToSavedPositionB() = (*it)->GetPtrToVertexFromModel_2()->GetProtectedPosition(); 01875 //(*it)->GetPtrToVertexFromModel_1()->GetProtectedPosition() = (*it)->RefToTargetPositionA(); 01876 (*it)->GetPtrToVertexFromModel_2()->GetProtectedPosition() = (*it)->RefToTargetPositionB(); 01877 ++it; 01878 } 01879 } 01880 //----------------------------------------------------------------------------- 01881 // Enforce consistent positions 01882 //============================================================================= 01883 01884 01885 01886 01887 //============================================================================= 01888 // Clear constraints 01889 //----------------------------------------------------------------------------- 01890 template <typename T, typename DATA> 01891 void AdvSimSupport_DS<T,DATA>::ClearAllConstraints () 01892 { 01893 for ( unsigned int i = 0; i < GetNumberOfTotalModels(); ++i ) { 01894 for ( unsigned int j = 0; j < GetNumberOfTotalModels(); ++j ) { 01895 ClearAllConstraintsBetweenTwoModels( i, j ); 01896 } 01897 } 01898 01899 #ifdef TAPs_MODEL_SURGICAL_SUTURE_WITH_HEAD_NEEDLE_HPP 01900 for ( unsigned int i = 0; i < m_ListOfInteraction_SwHDModelVsHEModel.size(); ++i ) { 01901 ClearAllConstraintsOfSutureWithHeadNeedle( i ); 01902 } 01903 #endif//TAPs_MODEL_SURGICAL_SUTURE_WITH_HEAD_NEEDLE_HPP 01904 01905 // Clear all point forces 01906 AdvSimSupport_DATA<T,DATA>::ListOfPointForces::iterator it = m_ListOfPointForces.begin(); 01907 while ( it != m_ListOfPointForces.end() ) { 01908 delete *it; 01909 ++it; 01910 } 01911 m_ListOfPointForces.clear(); 01912 01913 // Clear data for controlling forward and backward movements of an Elastic Rod 01914 m_ModelLists.ResetAllERNodePropsList(); 01915 } 01916 //----------------------------------------------------------------------------- 01917 template <typename T, typename DATA> 01918 void AdvSimSupport_DS<T,DATA>::ClearAllConstraintsBetweenTwoModels ( unsigned int modelA, unsigned int modelB ) 01919 { 01920 Enum::ModelType typeA = m_IDForModel[modelA].Type; 01921 Enum::ModelType typeB = m_IDForModel[modelB].Type; 01922 01923 // Clear FEMModel-FEMModel interactions 01924 if ( typeA == Enum::MODEL_BASED_ON_FEM 01925 && typeB == Enum::MODEL_BASED_ON_FEM ) 01926 { 01927 unsigned int locA = m_IDForModel[modelA].Slot; 01928 unsigned int locB = m_IDForModel[modelB].Slot; 01929 if ( locA > locB ) return; 01930 ClearAllConstraintsOfFEMModelAndFEMModel( locA, locB ); 01931 } 01932 // Clear ERModel-FEMModel interactions 01933 else if ( typeA == Enum::MODEL_BASED_ON_ER 01934 && typeB == Enum::MODEL_BASED_ON_FEM ) 01935 { 01936 unsigned int locA = m_IDForModel[modelA].Slot; 01937 unsigned int locB = m_IDForModel[modelB].Slot; 01938 ClearAllConstraintsOfERModelAndFEMModel( locA, locB ); 01939 } 01940 // Clear RBDModel-FEMModel interactions 01941 else if ( typeA == Enum::MODEL_BASED_ON_IPG_RBD 01942 && typeB == Enum::MODEL_BASED_ON_FEM ) 01943 { 01944 unsigned int locA = m_IDForModel[modelA].Slot; 01945 unsigned int locB = m_IDForModel[modelB].Slot; 01946 ClearAllConstraintsOfRBDModelAndFEMModel( locA, locB ); 01947 } 01948 01949 01950 01951 /* 01952 for ( unsigned int i = 0; i < m_InteractionLists.HEModelVsHEModel.size(); ++i ) { 01953 for ( unsigned int j = i; j < m_InteractionLists.HEModelVsHEModel.size(); ++j ) { 01954 ClearAllConstraintsOfFEMModelAndFEMModel 01955 } 01956 } 01957 01958 // Clear ERModel-HEModel interactions 01959 for ( unsigned int i = 0; i < m_InteractionLists.ERModelVsHEModel.size(); ++i ) { 01960 for ( unsigned int j = 0; j < m_InteractionLists.ERModelVsHEModel[i].size(); ++j ) { 01961 ClearAllConstraintsOfERModelAndFEMModel 01962 } 01963 } 01964 01965 // Clear IPGModel-HEModel interactions 01966 for ( unsigned int i = 0; i < m_InteractionLists.IPGModelVsHEModel.size(); ++i ) { 01967 for ( unsigned int j = 0; j < m_InteractionLists.IPGModelVsHEModel[i].size(); ++j ) { 01968 ClearAllConstraintsOfRBDModelAndFEMModel 01969 } 01970 } 01971 //*/ 01972 } 01973 //----------------------------------------------------------------------------- 01974 template <typename T, typename DATA> 01975 void AdvSimSupport_DS<T,DATA>::ClearAllConstraintsOfFEMModelAndFEMModel ( unsigned int locA, unsigned int locB ) 01976 { 01977 //* 01978 unsigned int i = m_InteractionLists.HEModelVsHEModel[locA][locB].Constraints.size(); 01979 while ( i > 0 ) { 01980 ClearTheConstraintNumberOfFEMModelAndFEMModel( locA, locB, i-1 ); 01981 i = m_InteractionLists.HEModelVsHEModel[locA][locB].Constraints.size(); 01982 } 01983 //*/ 01984 /* 01985 locB -= locA; // since the data in the std::vector starts from 0, not from the model id 01986 AdvSimSupport_DATA<T,DATA>::InteractionConstraints_HEModelVsHEModel::iterator it = m_InteractionLists.HEModelVsHEModel[locA][locB].begin(); 01987 while ( it != m_InteractionLists.HEModelVsHEModel[locA][locB].end() ) { 01988 01989 // 01990 01991 delete *it; 01992 ++it; 01993 } 01994 m_InteractionLists.HEModelVsHEModel[locA][locB].clear(); 01995 //*/ 01996 } 01997 //----------------------------------------------------------------------------- 01998 template <typename T, typename DATA> 01999 void AdvSimSupport_DS<T,DATA>::ClearAllConstraintsOfERModelAndFEMModel ( unsigned int ERloc, unsigned int FEMloc ) 02000 { 02001 //* 02002 unsigned int i = m_InteractionLists.ERModelVsHEModel[ERloc][FEMloc].Constraints.size(); 02003 while ( i > 0 ) { 02004 ClearTheConstraintNumberOfERModelAndFEMModel( ERloc, FEMloc, i-1 ); 02005 i = m_InteractionLists.ERModelVsHEModel[ERloc][FEMloc].Constraints.size(); 02006 } 02007 //*/ 02008 /* 02009 AdvSimSupport_DATA<T,DATA>::InteractionConstraints_ERModelVsHEModel::iterator it = m_InteractionLists.ERModelVsHEModel[ERloc].Constraints[FEMloc].begin(); 02010 while ( it != m_InteractionLists.ERModelVsHEModel[ERloc].Constraints[FEMloc].end() ) { 02011 02012 // COMMENTED OUT BECAUSE IT CAUSES A RUN-TIME ERROR DURING THE EXIT OF THE PROGRAM 02013 // SO ALL OF THESE COMMENTED STATEMENTS BELOW ARE ADDED INTO ModelElasticRod's Reset(). 02014 //m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[(*it)->GetVertexIDFromModel_1()].EnforcedPosition.Clear(); 02015 //m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[(*it)->GetVertexIDFromModel_1()].UseEnforcedPosition = false; 02016 //m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[(*it)->GetVertexIDFromModel_1()].ExternalForce_ForAdvSimCtrl.Clear(); 02017 02018 delete *it; 02019 ++it; 02020 } 02021 m_InteractionLists.ERModelVsHEModel[ERloc][FEMloc].clear(); 02022 //*/ 02023 } 02024 //----------------------------------------------------------------------------- 02025 template <typename T, typename DATA> 02026 void AdvSimSupport_DS<T,DATA>::ClearAllConstraintsOfRBDModelAndFEMModel ( unsigned int RBDloc, unsigned int FEMloc ) 02027 { 02028 //* 02029 unsigned int i = m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc].Constraints.size(); 02030 while ( i > 0 ) { 02031 ClearTheConstraintNumberOfRBDModelAndFEMModel( RBDloc, FEMloc, i-1 ); 02032 i = m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc].Constraints.size(); 02033 } 02034 //*/ 02035 /* 02036 AdvSimSupport_DATA<T,DATA>::InteractionConstraints_IPGModelVsHEModel::iterator it = m_InteractionLists.IPGModelVsHEModel[RBDloc].Constraints[FEMloc].begin(); 02037 while ( it != m_InteractionLists.IPGModelVsHEModel[RBDloc].Constraints[FEMloc].end() ) { 02038 delete *it; 02039 ++it; 02040 } 02041 m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc].clear(); 02042 //*/ 02043 } 02044 //----------------------------------------------------------------------------- 02045 template <typename T, typename DATA> 02046 void AdvSimSupport_DS<T,DATA>::ClearAContraintBetweenTwoModels ( unsigned int modelA, unsigned int modelB, unsigned int constraintNumber ) 02047 { 02048 Enum::ModelType typeA = m_IDForModel[modelA].Type; 02049 Enum::ModelType typeB = m_IDForModel[modelB].Type; 02050 02051 if ( typeA == Enum::MODEL_BASED_ON_FEM 02052 && typeB == Enum::MODEL_BASED_ON_FEM ) 02053 { 02054 unsigned int locA = m_IDForModel[modelA].Slot; 02055 unsigned int locB = m_IDForModel[modelB].Slot; 02056 if ( locA > locB ) return; 02057 ClearTheConstraintNumberOfFEMModelAndFEMModel( locA, locB, constraintNumber ); 02058 } 02059 else if ( typeA == Enum::MODEL_BASED_ON_ER 02060 && typeB == Enum::MODEL_BASED_ON_FEM ) 02061 { 02062 unsigned int locA = m_IDForModel[modelA].Slot; 02063 unsigned int locB = m_IDForModel[modelB].Slot; 02064 ClearTheConstraintNumberOfERModelAndFEMModel( locA, locB, constraintNumber ); 02065 } 02066 else if ( typeA == Enum::MODEL_BASED_ON_IPG_RBD 02067 && typeB == Enum::MODEL_BASED_ON_FEM ) 02068 { 02069 unsigned int locA = m_IDForModel[modelA].Slot; 02070 unsigned int locB = m_IDForModel[modelB].Slot; 02071 ClearTheConstraintNumberOfRBDModelAndFEMModel( locA, locB, constraintNumber ); 02072 } 02073 } 02074 //----------------------------------------------------------------------------- 02075 template <typename T, typename DATA> 02076 void AdvSimSupport_DS<T,DATA>::ClearTheConstraintNumberOfFEMModelAndFEMModel ( unsigned int locA, unsigned int locB, unsigned int constraintNumber ) 02077 { 02078 locB -= locA; // since the data in the std::vector starts from 0, not from the model number 02079 if ( constraintNumber >= m_InteractionLists.HEModelVsHEModel[locA][locB].Constraints.size() ) return; 02080 02081 AdvSimSupport_DATA<T,DATA>::InteractionConstraints_HEModelVsHEModel::iterator it = m_InteractionLists.HEModelVsHEModel[locA][locB].Constraints.begin() + constraintNumber; 02082 02083 // Clear simulation related data 02084 (*it)->GetPtrToVertexFromModel_1()->SimFlags.ClearAllFlags(); 02085 (*it)->GetPtrToVertexFromModel_2()->SimFlags.ClearAllFlags(); 02086 02087 // REMARK: The erased element's destructor is called before it is removed from the std::vector. 02088 // Therefore, calling the desctructor after the erase will generate a runtime error. 02089 m_InteractionLists.HEModelVsHEModel[locA][locB].Constraints.erase( it ); 02090 } 02091 //----------------------------------------------------------------------------- 02092 template <typename T, typename DATA> 02093 void AdvSimSupport_DS<T,DATA>::ClearTheConstraintNumberOfERModelAndFEMModel ( unsigned int ERloc, unsigned int HEloc, unsigned int constraintNumber ) 02094 { 02095 if ( constraintNumber >= m_InteractionLists.ERModelVsHEModel[ERloc][HEloc].Constraints.size() ) return; 02096 02097 AdvSimSupport_DATA<T,DATA>::InteractionConstraints_ERModelVsHEModel::iterator it = m_InteractionLists.ERModelVsHEModel[ERloc][HEloc].Constraints.begin() + constraintNumber; 02098 02099 // Clear simulation related data 02100 //m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[(*it)->GetVertexIDFromModel_1()].EnforcedPosition.Clear(); 02101 //m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[(*it)->GetVertexIDFromModel_1()].UseEnforcedPosition = false; 02102 m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[(*it)->GetVertexIDFromModel_1()].ExternalForce_ForAdvSimCtrl.Clear(); 02103 m_ModelLists.BasedOnER[ERloc]->GetListOfNodes()[(*it)->GetVertexIDFromModel_1()].SimFlags.ClearAllFlags(); 02104 (*it)->GetPtrToVertexFromModel_2()->SimFlags.ClearAllFlags(); 02105 02106 // REMARK: The erased element's destructor is called before it is removed from the std::vector. 02107 // Therefore, calling the desctructor after the erase will generate a runtime error. 02108 m_InteractionLists.ERModelVsHEModel[ERloc][HEloc].Constraints.erase( it ); 02109 } 02110 //----------------------------------------------------------------------------- 02111 template <typename T, typename DATA> 02112 void AdvSimSupport_DS<T,DATA>::ClearTheConstraintNumberOfRBDModelAndFEMModel ( unsigned int IPGloc, unsigned int HEloc, unsigned int constraintNumber ) 02113 { 02114 if ( constraintNumber >= m_InteractionLists.IPGModelVsHEModel[IPGloc][HEloc].Constraints.size() ) return; 02115 02116 AdvSimSupport_DATA<T,DATA>::InteractionConstraints_IPGModelVsHEModel::iterator it = m_InteractionLists.IPGModelVsHEModel[IPGloc][HEloc].Constraints.begin() + constraintNumber; 02117 02118 // Clear simulation related data 02119 m_ModelLists.BasedOnRBD[IPGloc]->RefToInteractionPointGroup().GetInteractionPointAtIndex( (*it)->GetVertexIDFromModel_1() ).GetSimFlags().ClearAllFlags(); 02120 (*it)->GetPtrToVertexFromModel_2()->SimFlags.ClearAllFlags(); 02121 02122 // REMARK: The erased element's destructor is called before it is removed from the std::vector. 02123 // Therefore, calling the desctructor after the erase will generate a runtime error. 02124 m_InteractionLists.IPGModelVsHEModel[IPGloc][HEloc].Constraints.erase( it ); 02125 } 02126 //----------------------------------------------------------------------------- 02127 // Clear constraints 02128 //============================================================================= 02129 02130 02131 02132 02133 //----------------------------------------------------------------------------- 02134 template <typename T, typename DATA> 02135 void AdvSimSupport_DS<T,DATA>::AdvSimAllModels ( 02136 T timestep, 02137 bool bEnforceAllConstraints, 02138 bool bEnforceAllConsistentPositions 02139 ) 02140 { 02141 //static int ITERATE_LIMIT = 2; 02142 //for ( int iterate = 1; iterate >= ITERATE_LIMIT; ++iterate ) 02143 { 02144 02145 // Enforce all constraints 02146 if ( bEnforceAllConstraints ) { 02147 EnforceAllConstraints(); 02148 } 02149 02150 //for ( unsigned int i = 0; i < m_ListOfSutureModelsBasedOnMSS.size(); ++i ) { 02151 // AdvSimSutureModelBasedOnMSS( i, timestep ); 02152 //} 02153 //for ( unsigned int i = 0; i < m_ModelLists.ListOfModelsForSurgeryBasedOnMSS.size(); ++i ) { 02154 // AdvSimModelForSurgeryBasedOnMSS( i, timestep ); 02155 //} 02156 02157 //const int NUM_OF_SUTURE_LOOPS = 1; 02158 for ( unsigned int i = 0; i < m_ModelLists.BasedOnER.size(); ++i ) { 02159 //for ( int s = 0; s < NUM_OF_SUTURE_LOOPS; ++s ) { 02160 AdvSimModelBasedOnER( i, timestep ); 02161 //} 02162 } 02163 for ( unsigned int i = 0; i < m_ModelLists.BasedOnFEM.size(); ++i ) { 02164 AdvSimModelBasedOnFEM( i, timestep ); 02165 } 02166 02167 if ( bEnforceAllConsistentPositions ) { 02168 EnforceAllConsistentPositions(); 02169 } 02170 } 02171 02172 // Update surface mesh models 02173 for ( unsigned int i = 0; i < m_ModelLists.BasedOnFEM.size(); ++i ) { 02174 m_ModelLists.BasedOnFEM[i]->UpdateSurfaceMeshNormals(); 02175 } 02176 } 02177 //----------------------------------------------------------------------------- 02178 //template <typename T, typename DATA> 02179 //void AdvSimSupport_DS<T,DATA>::AdvSimSutureModelBasedOnMSS ( unsigned int sutureID, T timestep ) 02180 //{ 02181 // m_ListOfSutureModelsBasedOnMSS[sutureID]->AdvanceSimulation( 0, timestep ); 02182 //} 02183 //----------------------------------------------------------------------------- 02184 //template <typename T, typename DATA> 02185 //void AdvSimSupport_DS<T,DATA>::AdvSimModelForSurgeryBasedOnMSS ( unsigned int sutureID, T timestep ) 02186 //{ 02187 // m_ModelLists.ListOfModelsForSurgeryBasedOnMSS[sutureID]->AdvanceSimulation( 0, timestep ); 02188 //} 02189 //----------------------------------------------------------------------------- 02190 template <typename T, typename DATA> 02191 void AdvSimSupport_DS<T,DATA>::AdvSimModelBasedOnER ( unsigned int ERModelID, T timestep ) 02192 { 02193 m_ModelLists.BasedOnER[ERModelID]->AdvanceSimulation(); 02194 } 02195 //----------------------------------------------------------------------------- 02196 template <typename T, typename DATA> 02197 void AdvSimSupport_DS<T,DATA>::AdvSimModelBasedOnFEM ( unsigned int sutureID, T timestep ) 02198 { 02199 m_ModelLists.BasedOnFEM[sutureID]->AdvanceSimulationDynamic(); 02200 } 02201 //----------------------------------------------------------------------------- 02202 02203 02204 02205 02206 //============================================================================= 02207 #ifdef TAPs_MODEL_SURGICAL_SUTURE_WITH_HEAD_NEEDLE_HPP 02208 //----------------------------------------------------------------------------- 02209 template <typename T, typename DATA> 02210 int AdvSimSupport_DS<T,DATA>::AddModelSutureWithHeadNeedle ( ModelSurgicalSutureWithHeadNeedle<T> * pModel ) 02211 { 02212 // Add suture's head needle 02213 // Check whether the suture's head needle, which is an RBD-based model, is already added. 02214 bool bNotFound = true; 02215 int sutureHeadNeedleID = -1; 02216 for ( unsigned int i = 0; i < m_ModelLists.BasedOnRBD.size() && bNotFound; ++i ) { 02217 if ( m_ModelLists.BasedOnRBD[i] == &pModel->GetHeadNeedle() ) { 02218 bNotFound = false; 02219 } 02220 } 02221 if ( bNotFound ) sutureHeadNeedleID = AddModel( &pModel->GetHeadNeedle() ); // Add the suture's head needle as an RBD-based model 02222 else { 02223 for ( unsigned int i = 0; i < m_IDForModel.size(); ++i ) { 02224 if ( m_IDForModel[i].Type == Enum::MODEL_BASED_ON_IPG_RBD 02225 && m_ModelLists.BasedOnRBD[m_IDForModel[i].Slot] == &pModel->GetHeadNeedle() ) 02226 { 02227 sutureHeadNeedleID = i; 02228 break; 02229 } 02230 } 02231 } 02232 02233 // Add suture's thread 02234 // Check whether the suture's thread, which is an ER-based model, is already added. 02235 bNotFound = true; 02236 int sutureThreadID = -1; 02237 for ( unsigned int i = 0; i < m_ModelLists.BasedOnER.size() && bNotFound; ++i ) { 02238 if ( m_ModelLists.BasedOnER[i] == pModel ) { 02239 bNotFound = false; 02240 } 02241 } 02242 if ( bNotFound ) sutureThreadID = AddModel( pModel ); // Add the suture's thread as an ER-based model 02243 else { 02244 for ( unsigned int i = 0; i < m_IDForModel.size(); ++i ) { 02245 if ( m_IDForModel[i].Type == Enum::MODEL_BASED_ON_ER 02246 && m_ModelLists.BasedOnER[m_IDForModel[i].Slot] == pModel ) 02247 { 02248 sutureThreadID = i; 02249 break; 02250 } 02251 } 02252 } 02253 02254 // Add the set of contraints for this suture with head needle for interaction with HE-based models 02255 AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T> * pData = new 02256 AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T>( 02257 pModel->GetHeadNeedle().RefToInteractionPointGroup().GetNumOfInteractionPoints(), 02258 pModel->RefToHeadNeedleCirclePathCtrl().InterpolatedPts.size(), 02259 sutureHeadNeedleID, 02260 sutureThreadID 02261 ); 02262 m_ListOfInteraction_SwHDModelVsHEModel.push_back( pData ); 02263 02264 m_IDForHeadNeedlesInTheMainModelList.push_back( sutureHeadNeedleID ); 02265 02266 std::cout << *pData << "\n"; 02267 02268 return m_ListOfInteraction_SwHDModelVsHEModel.size()-1; // return the suture's head needle ID 02269 } 02270 //----------------------------------------------------------------------------- 02271 template <typename T, typename DATA> 02272 void AdvSimSupport_DS<T,DATA>::EnforceAllConstraintsOfSutureWithHeadNeedle ( unsigned int SwHDModel ) 02273 { 02274 //std::cout << "m_InteractionLists.SwHDModelVsHEModel[SwHDModel]->EnforceAllConstraints\n"; 02275 // For the head needle's IPG 02276 m_ListOfInteraction_SwHDModelVsHEModel[SwHDModel]->EnforceAllConstraints_ForIPG( 02277 m_IDForModel, 02278 m_ModelLists.BasedOnER, 02279 m_ModelLists.BasedOnFEM, 02280 m_ModelLists.BasedOnRBD, 02281 m_ListOfPointForces 02282 ); 02283 // For the head needle's circle path 02284 m_ListOfInteraction_SwHDModelVsHEModel[SwHDModel]->EnforceAllConstraints_ForCirclePath( 02285 m_IDForModel, 02286 m_ModelLists.BasedOnER, 02287 m_ModelLists.BasedOnFEM, 02288 m_ModelLists.BasedOnRBD, 02289 m_ListOfPointForces 02290 ); 02291 } 02292 //----------------------------------------------------------------------------- 02293 template <typename T, typename DATA> 02294 void AdvSimSupport_DS<T,DATA>::ClearAllConstraintsOfSutureWithHeadNeedle ( unsigned int SwHDModel ) 02295 { 02296 //std::cout << "m_InteractionLists.SwHDModelVsHEModel[SwHDModel]->ClearAllConstraints\n"; 02297 02298 // For both of the head needle's IPG and circle path 02299 m_ListOfInteraction_SwHDModelVsHEModel[SwHDModel]->ResetPunctureController(); 02300 // For the head needle's IPG 02301 m_ListOfInteraction_SwHDModelVsHEModel[SwHDModel]->ClearAllConstraints_ForIPG(); 02302 // For the head needle's circle path 02303 m_ListOfInteraction_SwHDModelVsHEModel[SwHDModel]->ClearAllConstraints_ForCirclePath(); 02304 } 02305 //----------------------------------------------------------------------------- 02306 template <typename T, typename DATA> 02307 HEVertex<T> * AdvSimSupport_DS<T,DATA>::ProcessPuncturingOfSwHDRepresentedByIPGToHEModel ( 02308 unsigned int SwHDModel, 02309 unsigned int HEModel, 02310 bool & bPunctureIn, 02311 bool & bPunctureOut, 02312 bool & bAborted, 02313 std::string * info 02314 ) 02315 { 02316 HEVertex<T> * pHEVertex = NULL; 02317 Enum::ModelType IPGType = m_IDForModel[m_IDForHeadNeedlesInTheMainModelList[SwHDModel]].Type; 02318 Enum::ModelType HEType = m_IDForModel[HEModel].Type; 02319 if ( IPGType == Enum::MODEL_BASED_ON_IPG_RBD 02320 && HEType == Enum::MODEL_BASED_ON_FEM ) { 02321 pHEVertex = m_ListOfInteraction_SwHDModelVsHEModel[SwHDModel]->ProcessPuncturingOfHeadNeedleRepresentedByIPGWithFEMModel( 02322 HEModel, 02323 m_IDForModel, 02324 m_ModelLists, 02325 m_InteractionLists.ERModelVsHEModel, 02326 bPunctureIn, 02327 bPunctureOut, 02328 bAborted 02329 ); 02330 02331 if ( info ) { 02332 *info = m_ListOfInteraction_SwHDModelVsHEModel[SwHDModel]->GetPunctureControllerCurrentInfo(); 02333 } 02334 } 02335 else { 02336 PrtNotImplementedYet( "Supported only IPGType == Enum::MODEL_BASED_ON_IPG_RBD and HEType == Enum::MODEL_BASED_ON_FEM" ); 02337 } 02338 return pHEVertex; 02339 } 02340 //----------------------------------------------------------------------------- 02341 template <typename T, typename DATA> 02342 HEVertex<T> * AdvSimSupport_DS<T,DATA>::ProcessPuncturingOfSwHDUsingCirclePathToHEModel ( 02343 typename ModelSurgicalSutureWithHeadNeedle<T>::CirclePath & CirclePathCtrl, 02344 unsigned int SwHDModel, 02345 unsigned int HEModel, 02346 bool & bPunctureIn, 02347 bool & bPunctureOut, 02348 bool & bAborted, 02349 bool & bSuccess 02350 ) 02351 { 02352 HEVertex<T> * pHEVertex = NULL; 02353 Enum::ModelType IPGType = m_IDForModel[m_IDForHeadNeedlesInTheMainModelList[SwHDModel]].Type; 02354 Enum::ModelType HEType = m_IDForModel[HEModel].Type; 02355 if ( IPGType == Enum::MODEL_BASED_ON_IPG_RBD 02356 && HEType == Enum::MODEL_BASED_ON_FEM ) { 02357 pHEVertex = m_ListOfInteraction_SwHDModelVsHEModel[SwHDModel]->ProcessPuncturingOfHeadNeedleUsingCirclePathWithFEMModel( 02358 CirclePathCtrl, 02359 HEModel, 02360 m_IDForModel, 02361 m_ModelLists, 02362 m_InteractionLists.ERModelVsHEModel, 02363 bPunctureIn, 02364 bPunctureOut, 02365 bAborted, 02366 bSuccess 02367 ); 02368 } 02369 else { 02370 PrtNotImplementedYet( "Supported only IPGType == Enum::MODEL_BASED_ON_IPG_RBD and HEType == Enum::MODEL_BASED_ON_FEM" ); 02371 } 02372 return pHEVertex; 02373 } 02374 //----------------------------------------------------------------------------- 02375 template <typename T, typename DATA> 02376 HEVertex<T> * AdvSimSupport_DS<T,DATA>::ProcessPuncturingOfSwHDUsingCirclePathToHEModel_MoreAccuButUnfinishYet ( 02377 typename ModelSurgicalSutureWithHeadNeedle<T>::CirclePath & CirclePathCtrl, 02378 unsigned int SwHDModel, 02379 unsigned int HEModel, 02380 bool & bPunctureIn, 02381 bool & bPunctureOut, 02382 bool & bAborted 02383 ) 02384 { 02385 HEVertex<T> * pHEVertex = NULL; 02386 Enum::ModelType IPGType = m_IDForModel[m_IDForHeadNeedlesInTheMainModelList[SwHDModel]].Type; 02387 Enum::ModelType HEType = m_IDForModel[HEModel].Type; 02388 if ( IPGType == Enum::MODEL_BASED_ON_IPG_RBD 02389 && HEType == Enum::MODEL_BASED_ON_FEM ) { 02390 pHEVertex = m_ListOfInteraction_SwHDModelVsHEModel[SwHDModel]->ProcessPuncturingOfHeadNeedleUsingCirclePathWithFEMModel_MoreAccuButUnfinishYet( 02391 CirclePathCtrl, 02392 HEModel, 02393 m_IDForModel, 02394 m_ModelLists, 02395 m_InteractionLists.ERModelVsHEModel, 02396 bPunctureIn, 02397 bPunctureOut, 02398 bAborted 02399 ); 02400 } 02401 else { 02402 PrtNotImplementedYet( "Supported only IPGType == Enum::MODEL_BASED_ON_IPG_RBD and HEType == Enum::MODEL_BASED_ON_FEM" ); 02403 } 02404 return pHEVertex; 02405 } 02406 //----------------------------------------------------------------------------- 02407 #endif//TAPs_MODEL_SURGICAL_SUTURE_WITH_HEAD_NEEDLE_HPP 02408 //============================================================================= 02409 02410 02411 02412 02413 //============================================================================= 02414 #if defined(__gl_h_) || defined(__GL_H__) 02415 //----------------------------------------------------------------------------- 02416 template <typename T, typename DATA> 02417 void AdvSimSupport_DS<T,DATA>::DrawAllModels () 02418 { 02419 //std::vector< OpenGL::ModelSuture<T> * >::iterator A = m_ListOfSutureModelsBasedOnMSS.begin(); 02420 //while ( A != m_ListOfSutureModelsBasedOnMSS.end() ) { 02421 // (*A)->Draw(); 02422 // ++A; 02423 //} 02424 //std::vector< ModelForSurgery<T> * >::iterator B = m_ModelLists.ListOfModelsForSurgeryBasedOnMSS.begin(); 02425 //while ( B != m_ModelLists.ListOfModelsForSurgeryBasedOnMSS.end() ) { 02426 // (*B)->Draw(); 02427 // ++B; 02428 //} 02429 std::vector< ModelElasticRod<T> * >::iterator C = m_ModelLists.BasedOnER.begin(); 02430 while ( C != m_ModelLists.BasedOnER.end() ) { 02431 (*C)->Draw(); 02432 ++C; 02433 } 02434 std::vector< ModelDefBasedOnFEM<T,DATA> * >::iterator D = m_ModelLists.BasedOnFEM.begin(); 02435 while ( D != m_ModelLists.BasedOnFEM.end() ) { 02436 (*D)->Draw(); 02437 ++D; 02438 } 02439 } 02440 //----------------------------------------------------------------------------- 02441 template <typename T, typename DATA> 02442 void AdvSimSupport_DS<T,DATA>::DrawAddedInteractions () 02443 { 02444 // Draw model-model interactions 02445 for ( unsigned int i = 0; i < GetNumberOfTotalModels(); ++i ) { 02446 for ( unsigned int j = 0; j < GetNumberOfTotalModels(); ++j ) { 02447 DrawAddedInteractionsBetweenTwoModels( i, j ); 02448 } 02449 } 02450 02451 // DEBUG 02452 #ifdef TAPs_MODEL_SURGICAL_SUTURE_WITH_HEAD_NEEDLE_HPP 02453 m_ListOfInteraction_SwHDModelVsHEModel[0]->Draw(); 02454 #endif//TAPs_MODEL_SURGICAL_SUTURE_WITH_HEAD_NEEDLE_HPP 02455 } 02456 //----------------------------------------------------------------------------- 02457 template <typename T, typename DATA> 02458 void AdvSimSupport_DS<T,DATA>::DrawAddedInteractionsBetweenTwoModels ( unsigned int modelA, unsigned int modelB ) 02459 { 02460 Enum::ModelType typeA = m_IDForModel[modelA].Type; 02461 Enum::ModelType typeB = m_IDForModel[modelB].Type; 02462 if ( typeA == Enum::MODEL_BASED_ON_FEM 02463 && typeB == Enum::MODEL_BASED_ON_FEM ) 02464 { 02465 unsigned int locA = m_IDForModel[modelA].Slot; 02466 unsigned int locB = m_IDForModel[modelB].Slot; 02467 if ( locA > locB ) return; 02468 DrawAddedInteractionsBetweenFEMModelAndFEMModel( locA, locB ); 02469 } 02470 else if ( typeA == Enum::MODEL_BASED_ON_ER 02471 && typeB == Enum::MODEL_BASED_ON_FEM ) 02472 { 02473 unsigned int locA = m_IDForModel[modelA].Slot; 02474 unsigned int locB = m_IDForModel[modelB].Slot; 02475 DrawAddedInteractionsBetweenERModelAndFEMModel( locA, locB ); 02476 } 02477 else if ( typeA == Enum::MODEL_BASED_ON_IPG_RBD 02478 && typeB == Enum::MODEL_BASED_ON_FEM ) 02479 { 02480 unsigned int locA = m_IDForModel[modelA].Slot; 02481 unsigned int locB = m_IDForModel[modelB].Slot; 02482 DrawAddedInteractionsBetweenRBDModelAndFEMModel( locA, locB ); 02483 } 02484 } 02485 //----------------------------------------------------------------------------- 02486 template <typename T, typename DATA> 02487 void AdvSimSupport_DS<T,DATA>::DrawAddedInteractionsBetweenFEMModelAndFEMModel ( unsigned int locA, unsigned int locB ) 02488 { 02489 glPushAttrib( GL_ALL_ATTRIB_BITS ); 02490 glDisable( GL_LIGHTING ); 02491 glLineWidth( 2 ); 02492 glPointSize( 5 ); 02493 GLfloat V[2][3]; 02494 02495 Matrix4x4<T> Trx1 = m_ModelLists.BasedOnFEM[locA]->RefToTransformationSupport().RefToMatrixTransform(); 02496 Matrix4x4<T> Trx2 = m_ModelLists.BasedOnFEM[locB]->RefToTransformationSupport().RefToMatrixTransform(); 02497 02498 locB -= locA; // since the data in the std::vector starts from 0, not from the model id 02499 AdvSimSupport_DATA<T,DATA>::InteractionConstraints_HEModelVsHEModel::iterator it = m_InteractionLists.HEModelVsHEModel[locA][locB].Constraints.begin(); 02500 while ( it != m_InteractionLists.HEModelVsHEModel[locA][locB].Constraints.end() ) { 02501 Vector3<T> v1 = (Trx1 * Vector4<T>( (*it)->GetPtrToVertexFromModel_1()->GetPosition() ) ).GetVector3(); 02502 Vector3<T> v2 = (Trx2 * Vector4<T>( (*it)->GetPtrToVertexFromModel_2()->GetPosition() ) ).GetVector3(); 02503 V[0][0] = v1[0]; V[0][1] = v1[1]; V[0][2] = v1[2]; 02504 V[1][0] = v2[0]; V[1][1] = v2[1]; V[1][2] = v2[2]; 02505 glBegin( GL_POINTS ); 02506 glColor3f( 0.3f, 0.7f, 0.2f ); 02507 glVertex3fv( V[0] ); 02508 glVertex3fv( V[1] ); 02509 glColor3f( 1.0f, 1.0f, 0.0f ); 02510 glVertex3fv( (Trx1*Vector4<T>((*it)->RefToTargetPositionA())).GetVector3().GetDataFloat() ); 02511 glColor3f( 1.0f, 0.0f, 1.0f ); 02512 glVertex3fv( (Trx2*Vector4<T>((*it)->RefToTargetPositionB())).GetVector3().GetDataFloat() ); 02513 glEnd(); 02514 glBegin( GL_LINES ); 02515 glColor3f( 0.3f, 0.7f, 0.5f ); 02516 glVertex3fv( V[0] ); 02517 glVertex3fv( V[1] ); 02518 glEnd(); 02519 ++it; 02520 } 02521 glPopAttrib(); 02522 } 02523 //----------------------------------------------------------------------------- 02524 template <typename T, typename DATA> 02525 void AdvSimSupport_DS<T,DATA>::DrawAddedInteractionsBetweenERModelAndFEMModel ( unsigned int ERloc, unsigned int FEMloc ) 02526 { 02527 glPushAttrib( GL_ALL_ATTRIB_BITS ); 02528 glDisable( GL_LIGHTING ); 02529 glLineWidth( 2 ); 02530 glPointSize( 5 ); 02531 GLfloat V[2][3]; 02532 02533 Matrix4x4<T> Trx = m_ModelLists.BasedOnFEM[FEMloc]->RefToTransformationSupport().RefToMatrixTransform(); 02534 02535 AdvSimSupport_DATA<T,DATA>::InteractionConstraints_ERModelVsHEModel::iterator it = m_InteractionLists.ERModelVsHEModel[ERloc][FEMloc].Constraints.begin(); 02536 while ( it != m_InteractionLists.ERModelVsHEModel[ERloc][FEMloc].Constraints.end() ) { 02537 Vector3<T> v1 = m_ModelLists.BasedOnER[ERloc]->RefToCurrentVertexPositionOfNodeNumber( (*it)->GetVertexIDFromModel_1() ); 02538 Vector3<T> v2 = (Trx * Vector4<T>( (*it)->GetPtrToVertexFromModel_2()->GetPosition() ) ).GetVector3(); 02539 V[0][0] = v1[0]; V[0][1] = v1[1]; V[0][2] = v1[2]; 02540 V[1][0] = v2[0]; V[1][1] = v2[1]; V[1][2] = v2[2]; 02541 glBegin( GL_POINTS ); 02542 glColor3f( 0.3f, 0.7f, 0.2f ); 02543 glVertex3fv( V[0] ); 02544 glVertex3fv( V[1] ); 02545 glColor3f( 1.0f, 1.0f, 0.0f ); 02546 glVertex3fv( (*it)->RefToTargetPositionA().GetDataFloat() ); 02547 glColor3f( 1.0f, 0.0f, 1.0f ); 02548 glVertex3fv( (Trx*Vector4<T>((*it)->RefToTargetPositionB())).GetVector3().GetDataFloat() ); 02549 glEnd(); 02550 glBegin( GL_LINES ); 02551 glColor3f( 0.3f, 0.7f, 0.5f ); 02552 glVertex3fv( V[0] ); 02553 glVertex3fv( V[1] ); 02554 glEnd(); 02555 ++it; 02556 } 02557 glPopAttrib(); 02558 } 02559 //----------------------------------------------------------------------------- 02560 template <typename T, typename DATA> 02561 void AdvSimSupport_DS<T,DATA>::DrawAddedInteractionsBetweenRBDModelAndFEMModel ( unsigned int RBDloc, unsigned int FEMloc ) 02562 { 02563 glPushAttrib( GL_ALL_ATTRIB_BITS ); 02564 glDisable( GL_LIGHTING ); 02565 glLineWidth( 2 ); 02566 glPointSize( 15 ); 02567 GLfloat V[2][3]; 02568 02569 Matrix4x4<T> TrxRBD = m_ModelLists.BasedOnRBD[RBDloc]->CalMatrixTransform(); 02570 Matrix4x4<T> TrxFEM = m_ModelLists.BasedOnFEM[FEMloc]->RefToTransformationSupport().RefToMatrixTransform(); 02571 02572 AdvSimSupport_DATA<T,DATA>::InteractionConstraints_IPGModelVsHEModel::iterator it = m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc].Constraints.begin(); 02573 while ( it != m_InteractionLists.IPGModelVsHEModel[RBDloc][FEMloc].Constraints.end() ) { 02574 Vector3<T> v1 = (TrxRBD * Vector4<T>( m_ModelLists.BasedOnRBD[RBDloc]->RefToInteractionPointGroup().GetInteractionPointAtIndex( (*it)->GetVertexIDFromModel_1() ).GetPosition() ) ).GetVector3(); 02575 Vector3<T> v2 = (TrxFEM * Vector4<T>( (*it)->GetPtrToVertexFromModel_2()->GetPosition() ) ).GetVector3(); 02576 V[0][0] = v1[0]; V[0][1] = v1[1]; V[0][2] = v1[2]; 02577 V[1][0] = v2[0]; V[1][1] = v2[1]; V[1][2] = v2[2]; 02578 glBegin( GL_POINTS ); 02579 glColor3f( 0.3f, 0.7f, 0.2f ); 02580 //glVertex3fv( V[0] ); 02581 glColor3f( 0.7f, 0.3f, 0.2f ); 02582 glVertex3fv( V[1] ); 02583 glColor3f( 1.0f, 1.0f, 0.0f ); 02584 //glVertex3fv( (TrxRBD*Vector4<T>((*it)->RefToTargetPositionA())).GetVector3().GetDataFloat() ); 02585 glColor3f( 1.0f, 0.0f, 1.0f ); 02586 //glVertex3fv( (TrxFEM*Vector4<T>((*it)->RefToTargetPositionB())).GetVector3().GetDataFloat() ); 02587 glEnd(); 02588 glBegin( GL_LINES ); 02589 glColor3f( 0.3f, 0.7f, 0.5f ); 02590 glVertex3fv( V[0] ); 02591 glVertex3fv( V[1] ); 02592 glEnd(); 02593 ++it; 02594 } 02595 glPopAttrib(); 02596 } 02597 //----------------------------------------------------------------------------- 02598 #endif // #if defined(__gl_h_) || defined(__GL_H__) 02599 //============================================================================= 02600 02601 //============================================================================= 02602 END_NAMESPACE_TAPs 02603 //34567890123456789012345678901234567890123456789012345678901234567890123456789 02604 //--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----
1.7.4 
