 |
TAPs 0.7.7.3
|
|
TAPs 0.7.7.3
|
00001 /****************************************************************************** 00002 TAPsAdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle.cpp 00003 ******************************************************************************/ 00007 /****************************************************************************** 00008 SUKITTI PUNAK (09/29/2010) 00009 UPDATE (01/07/2011) 00010 ******************************************************************************/ 00011 #include "TAPsAdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle.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 // Constructors 00019 //----------------------------------------------------------------------------- 00020 template <typename T, typename DATA> 00021 AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle ( 00022 unsigned int size_IPs, // number of interactive points (IPs) on the head needle 00023 unsigned int size_Intp, // number of interpolated points on the head needle's circle path 00024 unsigned int RBDModelAsSutureHeadNeedle, 00025 unsigned int ERModelAsSutureThread 00026 ) 00027 : m_RBDModelAsSutureHeadNeedle( RBDModelAsSutureHeadNeedle ) 00028 , m_ERModelAsSutureThread( ERModelAsSutureThread ) 00029 , m_size_ForIPG( size_IPs ) 00030 , m_NumberOfSutureHeadNeedlePointsPuncturedIn( 0 ) 00031 , m_NumberOfSutureThreadPointsPuncturedIn( 0 ) 00032 00033 // For the head needle's circle path 00034 , m_size_ForCirclePath( size_Intp ) 00035 , m_uiNumOfLockedCirclePaths( 0 ) 00036 { 00037 m_SetOfInteractions_ForIPG.resize( size_IPs ); 00038 m_SetOfInteractionAssociatedModelID_ForIPG.resize( size_IPs, -1 ); 00039 m_PuncturedLocations_ForIPG.resize( size_IPs ); 00040 for ( unsigned int i = 0; i < size_IPs; ++i ) { 00041 m_SetOfInteractions_ForIPG[i].SetVertexIDFromModel_1( i ); 00042 } 00043 00044 // For the head needle's circle path 00045 m_SetOfInteractions_ForCirclePath.resize( size_Intp ); 00046 m_SetOfInteractionAssociatedModelID_ForCirclePath.resize( size_Intp, -1 ); 00047 for ( unsigned int i = 0; i < size_Intp; ++i ) { 00048 m_SetOfInteractions_ForCirclePath[i].SetVertexIDFromModel_1( i ); 00049 } 00050 } 00051 //----------------------------------------------------------------------------- 00052 template <typename T, typename DATA> 00053 AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::~AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle () 00054 { 00055 ResetPunctureController(); 00056 ClearAllConstraints_ForIPG(); 00057 ClearAllConstraints_ForCirclePath(); 00058 } 00059 //----------------------------------------------------------------------------- 00060 template <typename T, typename DATA> 00061 std::string AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::StrInfo () const 00062 { 00063 std::ostringstream ss; 00064 ss << "AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<" << typeid(T).name() << ">"; 00065 ss << " has size of " << m_size_ForIPG << " for IPs on the head needle"; 00066 ss << " and has size of " << m_size_ForCirclePath << " for the interpolated points on the head needle's circle path."; 00067 return ss.str(); 00068 } 00069 //----------------------------------------------------------------------------- 00070 template <typename T, typename DATA> 00071 HEVertex<T> * AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::ProcessPuncturingOfHeadNeedleRepresentedByIPGWithFEMModel ( 00072 unsigned int FEMModel, 00073 typename AdvSimSupport_DATA<T,DATA>::ModelIDs const & ListOfModels, 00074 typename AdvSimSupport_DATA<T,DATA>::ModelLists & ListOfModelObjects, 00075 //std::vector< ModelElasticRod<T> * > & ListOfModelsBasedOnER, //!< list of suture models based on elastic rod (belongs to ER-based model group) 00076 //std::vector< ModelDefBasedOnFEM<T,DATA> * > & ListOfModelsBasedOnFEM, //!< list of deformable models based on FEM (belongs to FEM-based model group) 00077 //std::vector< RigidBodyDynamics<T> * > & ListOfModelsBasedOnRBD, //!< list of models based on rigid body dynamics (belongs to IPG-based model group) 00078 typename AdvSimSupport_DATA<T,DATA>::ListOfInteractions_ERModelVsHEModel & ListOf_ERModelVsHEModelInteractions, 00079 bool & bPunctureIn, 00080 bool & bPunctureOut, 00081 bool & bAborted 00082 ) 00083 { 00084 bPunctureIn = false; 00085 bPunctureOut = false; 00086 bAborted = false; 00087 00088 std::vector< ModelElasticRod<T> * > & ListOfModelsBasedOnER = ListOfModelObjects.BasedOnER; 00089 std::vector< ModelDefBasedOnFEM<T,DATA> * > & ListOfModelsBasedOnFEM = ListOfModelObjects.BasedOnFEM; 00090 std::vector< RigidBodyDynamics<T> * > & ListOfModelsBasedOnRBD = ListOfModelObjects.BasedOnRBD; 00091 00092 m_PunctureController.StrInfo = ""; 00093 00094 unsigned int RBDloc = ListOfModels[m_RBDModelAsSutureHeadNeedle].Slot; 00095 unsigned int FEMloc = ListOfModels[FEMModel].Slot; 00096 00097 ModelDefBasedOnFEMHex<T,DATA> * pFEMHex = dynamic_cast< ModelDefBasedOnFEMHex<T,DATA> * >( ListOfModelsBasedOnFEM[FEMloc] ); 00098 ModelDefBasedOnFEMTet<T,DATA> * pFEMTet = dynamic_cast< ModelDefBasedOnFEMTet<T,DATA> * >( ListOfModelsBasedOnFEM[FEMloc] ); 00099 00100 if ( pFEMHex ) 00101 { 00102 InteractionPointGroup<T> & IPG = ListOfModelsBasedOnRBD[RBDloc]->RefToInteractionPointGroup(); 00103 if ( IPG.GetNumOfInteractionPoints() == 0 ) return NULL; 00104 std::vector< Vector3<T> > IPs = ListOfModelsBasedOnRBD[RBDloc]->GetAllInteractionPointPositionsInWorldSpace(); 00105 //if ( IPs.size() == 0 ) return; 00106 00107 //T distConstraint = 10E5; 00108 T distConstraint = ( IPs[2] - IPs[0] ).Length(); // this length is fixed 00109 00110 // 00111 unsigned int start = 0; 00112 00113 // Check intersection 00114 AdvSimSupport_DATA<T,DATA>::TEMP_ListOfPoints.clear(); 00115 AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEFaces.clear(); 00116 AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEVertices.clear(); 00117 AdvSimSupport_DATA<T,DATA>::TEMP_ListOfRatios.clear(); 00118 AdvSimSupport_DATA<T,DATA>::TEMP_ListOfIntersectionAngles.clear(); 00119 CD::Fn::CD_PolygonalMesh_vs_LineSegment( 00120 &ListOfModelsBasedOnFEM[FEMloc]->RefToSurfaceMesh(), // HE-based mesh model 00121 &IPs[start], &IPs[start+1], // line segment 00122 &AdvSimSupport_DATA<T,DATA>::TEMP_ListOfPoints, // list of intersected points 00123 &AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEFaces, // list of intersected HE-based faces 00124 &AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEVertices, // list of closest HE-based vertices to the intersected points 00125 &AdvSimSupport_DATA<T,DATA>::TEMP_ListOfRatios, // list of ratios of the closest vertices relative to the line segment [0(ptA)-1(ptB)] 00126 &AdvSimSupport_DATA<T,DATA>::TEMP_ListOfIntersectionAngles // list of intersected angles of the needle of Tyco Endo Stitch device with triangle faces 00127 ); 00128 00129 /* 00130 // DEBUG -- print out the intersection angles 00131 for ( unsigned int i = 0; i < AdvSimSupport_DATA<T,DATA>::TEMP_ListOfIntersectionAngles.size(); ++i ) { 00132 std::cout << i << "\tintersection angle\t" << AdvSimSupport_DATA<T,DATA>::TEMP_ListOfIntersectionAngles[i] << "\n"; 00133 } 00134 std::cout << "#of puncture-in pts created: " << m_PunctureController.NumOfPunctureInPointsCreated << "\n"; 00135 std::cout << "#of puncture-Out pts created: " << m_PunctureController.NumOfPunctureOutPointsCreated << "\n"; 00136 //*/ 00137 00138 //------------------------------------------------- 00139 // START: CASE FOR PUNCTURE-IN OR -OUT 00140 if ( AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEVertices.size() > 0 ) { 00141 00142 // (FIRST) PUNCTURE-IN 00143 // Check if the sharp point is not currently punturing ( i.e. < 0 ) 00144 if ( m_SetOfInteractionAssociatedModelID_ForIPG[start] < 0 ) { 00145 00146 if ( m_PunctureController.ReachNumOfPunctureInPointsLimit() ) { 00147 m_PunctureController.StrInfo = "Number of created puncture-in points has reached the limit!"; 00148 return NULL; 00149 } 00150 00151 if ( AdvSimSupport_DATA<T,DATA>::TEMP_ListOfIntersectionAngles[0] > m_PunctureController.PunctureInAngleThreshold ) { 00152 m_PunctureController.StrInfo = "The angle is too small for the head needle to puncture into the wound!"; 00153 return NULL; 00154 } 00155 00156 // DEBUG -- Protection 00157 if ( m_NumberOfSutureHeadNeedlePointsPuncturedIn > 0 ) return NULL; 00158 00159 //std::cout << "NEXT PUNCTURE IN\n"; 00160 00161 unsigned int id = start; 00162 HEVertex<T> * pHEVertex = AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEVertices[0]; 00163 00164 // Set simulation flags of the interaction point at id 00165 IPG.GetInteractionPointAtIndex( id ).GetSimFlags().SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 00166 //IPG.GetInteractionPointAtIndex( id ).GetSimFlags().SetSimulationConstraints( Enum::AddOn::ATTACHED ); 00167 //IPG.GetInteractionPointAtIndex( id ).GetSimFlags().SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 00168 00169 // Set simulation flags of the HEVertex 00170 pHEVertex->SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 00171 00172 // 00173 // Set the constraint number at id 00174 // 00175 m_SetOfInteractionAssociatedModelID_ForIPG[id] = FEMModel; 00176 //m_SetOfInteractions[id].SetVertexIDFromModel_1( id ); // already set and it is invariant 00177 m_SetOfInteractions_ForIPG[id].SetPtrToVertexFromModel_2( pHEVertex ); 00178 m_SetOfInteractions_ForIPG[id].SetForceRatio( AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio ); 00179 m_SetOfInteractions_ForIPG[id].SetForceScaleA( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA ); 00180 m_SetOfInteractions_ForIPG[id].SetForceScaleB( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB ); 00181 m_SetOfInteractions_ForIPG[id].SetForceThresholdA( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA ); 00182 m_SetOfInteractions_ForIPG[id].SetForceThresholdB( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB ); 00183 m_SetOfInteractions_ForIPG[id].SetEnforcePositionStatusA( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA ); 00184 m_SetOfInteractions_ForIPG[id].SetEnforcePositionStatusB( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB ); 00185 00186 00187 ListOfModelsBasedOnRBD[RBDloc]->LockPosition(); // lock the head needle 00188 m_PuncturedLocations_ForIPG[id] = IPs[start]; // save the first punctured location 00189 m_SharpPointPrevPos_ForIPG = IPs[0]; // save the location of the sharp point 00190 ++m_NumberOfSutureHeadNeedlePointsPuncturedIn; 00191 00192 //std::cout << "m_NumberOfSutureHeadNeedlePointsPuncturedIn: " << m_NumberOfSutureHeadNeedlePointsPuncturedIn << "\n"; 00193 00194 bPunctureIn = true; 00195 m_PunctureController.WaitForTransfer = true; 00196 m_PunctureController.IncreaseNumOfPunctureInPointsCreated(); 00197 return pHEVertex; 00198 } 00199 // PUNCTURE-OUT 00200 else { 00201 00202 if ( m_PunctureController.ReachNumOfPunctureOutPointsLimit() ) { 00203 m_PunctureController.StrInfo = "Number of created puncture-out points has reached the limit!"; 00204 return NULL; 00205 } 00206 00207 if ( AdvSimSupport_DATA<T,DATA>::TEMP_ListOfIntersectionAngles[0] < 180 - m_PunctureController.PunctureOutAngleThreshold ) { 00208 m_PunctureController.StrInfo = "The angle is too small for the head needle to puncture out from the wound!"; 00209 return NULL; 00210 } 00211 00212 Matrix4x4<T> TrxFEMModel = ListOfModelsBasedOnFEM[FEMloc]->RefToTransformationSupport().RefToMatrixTransform(); 00213 Vector3<T> pos1 = TrxFEMModel * m_SetOfInteractions_ForIPG[start].GetPtrToVertexFromModel_2()->GetBarycentricPtr()->GetNewVertexLocation(); 00214 00215 T distThreshold = ( IPs[start] - IPs[start+1] ).Length(); 00216 T dist = (IPs[0] - m_SharpPointPrevPos_ForIPG).Length(); 00217 00218 if ( dist >= distThreshold && dist < distConstraint ) { 00219 00220 //std::cout << "NEXT PUNCTURE OUT\n"; 00221 00222 unsigned int next = 1; 00223 for ( ; next < m_size_ForIPG; ++next ) { 00224 if ( m_SetOfInteractionAssociatedModelID_ForIPG[next] < 0 ) break; 00225 } 00226 00227 //------------------------------------- 00228 // Make the next two points puncture in 00229 if ( next < m_size_ForIPG-1 ) { 00230 00231 // Set simulation flags of the HEVertex 00232 HEVertex<T> * pHEVertex = AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEVertices[0]; 00233 pHEVertex->SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 00234 00235 // Set simulation flags of the interaction point at next and next+1 00236 IPG.GetInteractionPointAtIndex( next ).GetSimFlags().SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 00237 //IPG.GetInteractionPointAtIndex( next ).GetSimFlags().SetSimulationConstraints( Enum::AddOn::ATTACHED ); 00238 //IPG.GetInteractionPointAtIndex( next ).GetSimFlags().SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 00239 IPG.GetInteractionPointAtIndex( next+1 ).GetSimFlags().SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 00240 //IPG.GetInteractionPointAtIndex( next+1 ).GetSimFlags().SetSimulationConstraints( Enum::AddOn::ATTACHED ); 00241 //IPG.GetInteractionPointAtIndex( next+1 ).GetSimFlags().SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 00242 00243 // Save the punctured out location 00244 //m_PuncturedLocations_ForIPG[next+1] = IPs[start]; 00245 00246 // Shift the old point back one step, and put the new one at the sharp point. 00247 for ( unsigned int i = next+1; i > 1; --i ) { 00248 // The interaction is shifted by two 00249 m_SetOfInteractionAssociatedModelID_ForIPG[i] = m_SetOfInteractionAssociatedModelID_ForIPG[i-2]; 00250 //m_SetOfInteractions[i].SetVertexIDFromModel_1( i ); // already set and it is invariant 00251 m_SetOfInteractions_ForIPG[i].SetPtrToVertexFromModel_2( m_SetOfInteractions_ForIPG[i-2].GetPtrToVertexFromModel_2() ); 00252 m_SetOfInteractions_ForIPG[i].SetForceRatio( m_SetOfInteractions_ForIPG[i-2].GetForceRatio() ); 00253 m_SetOfInteractions_ForIPG[i].SetForceScaleA( m_SetOfInteractions_ForIPG[i-2].GetForceScaleA() ); 00254 m_SetOfInteractions_ForIPG[i].SetForceScaleB( m_SetOfInteractions_ForIPG[i-2].GetForceScaleB() ); 00255 m_SetOfInteractions_ForIPG[i].SetForceThresholdA( m_SetOfInteractions_ForIPG[i-2].GetForceThresholdA() ); 00256 m_SetOfInteractions_ForIPG[i].SetForceThresholdB( m_SetOfInteractions_ForIPG[i-2].GetForceThresholdB() ); 00257 m_SetOfInteractions_ForIPG[i].SetEnforcePositionStatusA( m_SetOfInteractions_ForIPG[i-2].GetEnforcePositionStatusA() ); 00258 m_SetOfInteractions_ForIPG[i].SetEnforcePositionStatusB( m_SetOfInteractions_ForIPG[i-2].GetEnforcePositionStatusB() ); 00259 00260 // Shift the punctured location (is shifted by two) 00261 m_PuncturedLocations_ForIPG[i] = m_PuncturedLocations_ForIPG[i-2]; 00262 } 00263 00264 // 00265 // Set the constraint number at the 2nd point -- the first point after the sharp point 00266 // 00267 m_SetOfInteractionAssociatedModelID_ForIPG[start+1] = FEMModel; 00268 //m_SetOfInteractions_ForIPG[start].SetVertexIDFromModel_1( start+1 ); // already set and it is invariant 00269 m_SetOfInteractions_ForIPG[start+1].SetPtrToVertexFromModel_2( pHEVertex ); 00270 m_SetOfInteractions_ForIPG[start+1].SetForceRatio( AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio ); 00271 m_SetOfInteractions_ForIPG[start+1].SetForceScaleA( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA ); 00272 m_SetOfInteractions_ForIPG[start+1].SetForceScaleB( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB ); 00273 m_SetOfInteractions_ForIPG[start+1].SetForceThresholdA( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA ); 00274 m_SetOfInteractions_ForIPG[start+1].SetForceThresholdB( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB ); 00275 m_SetOfInteractions_ForIPG[start+1].SetEnforcePositionStatusA( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA ); 00276 m_SetOfInteractions_ForIPG[start+1].SetEnforcePositionStatusB( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB ); 00277 // Save the punctured location(s) 00278 m_PuncturedLocations_ForIPG[start+1] = IPs[start]; 00279 m_PuncturedLocations_ForIPG[start] = IPs[start]; 00280 00281 // 00282 // Clear the sharp point 00283 // 00284 IPG.GetInteractionPointAtIndex( start ).GetSimFlags().ClearSimulationConstraints( Enum::AddOn::SLIDABLE ); 00285 //IPG.GetInteractionPointAtIndex( start ).GetSimFlags().ClearSimulationConstraints( Enum::AddOn::ATTACHED ); 00286 //IPG.GetInteractionPointAtIndex( start ).GetSimFlags().ClearSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 00287 m_SetOfInteractionAssociatedModelID_ForIPG[start] = -1; 00288 00289 m_SharpPointPrevPos_ForIPG = IPs[0]; // save the location of the sharp point 00290 ++m_NumberOfSutureHeadNeedlePointsPuncturedIn; 00291 00292 //std::cout << "m_NumberOfSutureHeadNeedlePointsPuncturedIn: " << m_NumberOfSutureHeadNeedlePointsPuncturedIn << "\n"; 00293 } 00294 //------------------------------------- 00295 // Make the 1st point of suture thread puncture in 00296 else { 00297 00298 // Save the location of the sharp point 00299 //m_SharpPointPrevPos_ForIPG = IPs[0]; 00300 00301 //--m_NumberOfSutureHeadNeedlePointsPuncturedIn; 00302 } 00303 //------------------------------------- 00304 00305 bPunctureOut = true; 00306 //m_PunctureController.WaitForTransfer = true; 00307 m_PunctureController.IncreaseNumOfPunctureOutPointsCreated(); 00308 return AdvSimSupport_DATA<T,DATA>::TEMP_ListOfHEVertices[0]; 00309 } 00310 } 00311 }// END: CASE FOR PUNCTURE-IN OR -OUT 00312 //------------------------------------------------- 00313 // START: CASE FOR STILL PUNCTURING INSIDE, WITH NO PUNCTURE-OUT YET 00314 // The sharp point is currently punturing ( i.e. >= 0 ) 00315 else if ( m_SetOfInteractionAssociatedModelID_ForIPG[start] >= 0 ) { 00316 00317 //std::cout << "m_SetOfInteractionAssociatedModelID[start] >= 0\n"; 00318 00319 unsigned int next = 1; 00320 for ( ; next < m_size_ForIPG; ++next ) { 00321 if ( m_SetOfInteractionAssociatedModelID_ForIPG[next] < 0 ) break; 00322 } 00323 00324 if ( next < m_size_ForIPG ) { 00325 00326 //std::cout << "NEXT: " << next << "\n"; 00327 00328 Matrix4x4<T> TrxFEMModel = ListOfModelsBasedOnFEM[FEMloc]->RefToTransformationSupport().RefToMatrixTransform(); 00329 Vector3<T> pos1 = TrxFEMModel * m_SetOfInteractions_ForIPG[start].GetPtrToVertexFromModel_2()->GetBarycentricPtr()->GetNewVertexLocation(); 00330 00331 T distThreshold = ( IPs[next] - IPs[next-1] ).Length();// * 2; 00332 T dist = (IPs[0] - m_SharpPointPrevPos_ForIPG).Length(); 00333 00334 //* 00335 // Before puncture out, check for abortion of the puncture if the head needle is moved to far from the path 00336 T distAbort = distThreshold * 5; 00337 if ( next < 2 && dist > distAbort ) { // next is the point number in the IPG 00338 bAborted = true; 00339 ClearAllConstraints_ForIPG(); 00340 return NULL; 00341 } 00342 //*/ 00343 00344 if ( dist >= distThreshold && dist < distConstraint ) { 00345 00346 // FIND THE VERTEX AT INTERACTION POINT#0 00347 bool bFound = false; 00348 unsigned int elementID; 00349 //Vector3<T> position = IPs[start]; 00350 Vector3<T> position = pFEMHex->RefToTransformationSupport().GetInverseMatrixTransform() * IPs[start]; // start is 0 00351 Vector3<unsigned int> gridLocations; 00352 Vector3<T> coordinates; 00353 bFound = pFEMHex->FindElementThatContainsPointInLocalSpace( position, elementID, gridLocations, coordinates ); 00354 00355 // Replace the surface HEVertex with a created inner HEVertex inside the FEMModel 00356 if ( bFound ) { 00357 00358 HEVertex<T> * pNewHEVertex = pFEMHex->AddExtraVertexInLocalSpace( 00359 position, elementID, &pFEMHex->RefToFEMMesh().RefToElement( elementID ), gridLocations, coordinates ); 00360 00361 // Set simulation flags of the interaction point at id 00362 IPG.GetInteractionPointAtIndex( next ).GetSimFlags().SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 00363 //IPG.GetInteractionPointAtIndex( next ).GetSimFlags().SetSimulationConstraints( Enum::AddOn::ATTACHED ); 00364 //IPG.GetInteractionPointAtIndex( next ).GetSimFlags().SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 00365 00366 // Set simulation flags of the HEVertex 00367 pNewHEVertex->SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 00368 00369 // Save the punctured location 00370 //PuncturedLocations.push_back( IPs[start] ); 00371 00372 // Shift the old point back one step, and put the new one at the sharp point. 00373 for ( unsigned int i = next; i > 0; --i ) { 00374 m_SetOfInteractionAssociatedModelID_ForIPG[i] = m_SetOfInteractionAssociatedModelID_ForIPG[i-1]; 00375 //m_SetOfInteractions_ForIPG[i].SetVertexIDFromModel_1( i ); // already set and it is invariant 00376 m_SetOfInteractions_ForIPG[i].SetPtrToVertexFromModel_2( m_SetOfInteractions_ForIPG[i-1].GetPtrToVertexFromModel_2() ); 00377 m_SetOfInteractions_ForIPG[i].SetForceRatio( m_SetOfInteractions_ForIPG[i-1].GetForceRatio() ); 00378 m_SetOfInteractions_ForIPG[i].SetForceScaleA( m_SetOfInteractions_ForIPG[i-1].GetForceScaleA() ); 00379 m_SetOfInteractions_ForIPG[i].SetForceScaleB( m_SetOfInteractions_ForIPG[i-1].GetForceScaleB() ); 00380 m_SetOfInteractions_ForIPG[i].SetForceThresholdA( m_SetOfInteractions_ForIPG[i-1].GetForceThresholdA() ); 00381 m_SetOfInteractions_ForIPG[i].SetForceThresholdB( m_SetOfInteractions_ForIPG[i-1].GetForceThresholdB() ); 00382 m_SetOfInteractions_ForIPG[i].SetEnforcePositionStatusA( m_SetOfInteractions_ForIPG[i-1].GetEnforcePositionStatusA() ); 00383 m_SetOfInteractions_ForIPG[i].SetEnforcePositionStatusB( m_SetOfInteractions_ForIPG[i-1].GetEnforcePositionStatusB() ); 00384 // Shift the punctured location 00385 m_PuncturedLocations_ForIPG[i] = m_PuncturedLocations_ForIPG[i-1]; 00386 } 00387 // 00388 // Set the constraint number at the sharp point 00389 // 00390 m_SetOfInteractionAssociatedModelID_ForIPG[start] = FEMModel; 00391 //m_SetOfInteractions_ForIPG[start].SetVertexIDFromModel_1( start ); // already set and it is invariant 00392 m_SetOfInteractions_ForIPG[start].SetPtrToVertexFromModel_2( pNewHEVertex ); 00393 m_SetOfInteractions_ForIPG[start].SetForceRatio( AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio ); 00394 m_SetOfInteractions_ForIPG[start].SetForceScaleA( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA ); 00395 m_SetOfInteractions_ForIPG[start].SetForceScaleB( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB ); 00396 m_SetOfInteractions_ForIPG[start].SetForceThresholdA( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA ); 00397 m_SetOfInteractions_ForIPG[start].SetForceThresholdB( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB ); 00398 m_SetOfInteractions_ForIPG[start].SetEnforcePositionStatusA( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA ); 00399 m_SetOfInteractions_ForIPG[start].SetEnforcePositionStatusB( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB ); 00400 // Save the punctured location 00401 m_PuncturedLocations_ForIPG[start] = IPs[start]; 00402 00403 m_SharpPointPrevPos_ForIPG = IPs[0]; // save the location of the sharp point 00404 ++m_NumberOfSutureHeadNeedlePointsPuncturedIn; 00405 00406 //std::cout << "HEVertex's position: " << pNewHEVertex->GetPosition() << "\n"; 00407 //std::cout << "pConstraint: " << *pConstraint << std::endl; 00408 //std::cout << "pConstraint: " << pConstraint << std::endl; 00409 00410 //std::cout << "m_NumberOfSutureHeadNeedlePointsPuncturedIn: " << m_NumberOfSutureHeadNeedlePointsPuncturedIn << "\n"; 00411 } 00412 } 00413 } 00414 // Case the whole needle stays inside an FEM-based model 00415 else { 00416 } 00417 }//END: CASE FOR STILL PUNCTURING INSIDE 00418 //------------------------------------------------- 00419 // START: CASE FOR THE SHARP POINT HAS PUNCTURED OUT 00420 // The sharp point is currently punturing ( i.e. >= 0 ) 00421 else { 00422 start = 1; 00423 for ( ; start < m_size_ForIPG; ++start ) { 00424 if ( m_SetOfInteractionAssociatedModelID_ForIPG[start] >= 0 ) break; 00425 } 00426 // Case two or more points of head needle still inside an FEM-based model 00427 if ( start < m_size_ForIPG-1 ) { 00428 unsigned int next = start + 1; 00429 for ( ; next < m_size_ForIPG; ++next ) { 00430 if ( m_SetOfInteractionAssociatedModelID_ForIPG[next] < 0 ) break; 00431 } 00432 if ( next < m_size_ForIPG ) { 00433 00434 //std::cout << "next < m_size --> next: " << next << ", start: " << start << std::endl; 00435 00436 Matrix4x4<T> TrxFEMModel = ListOfModelsBasedOnFEM[FEMloc]->RefToTransformationSupport().RefToMatrixTransform(); 00437 Vector3<T> pos1 = TrxFEMModel * m_SetOfInteractions_ForIPG[start].GetPtrToVertexFromModel_2()->GetBarycentricPtr()->GetNewVertexLocation(); 00438 00439 T distThreshold = ( IPs[next] - IPs[next-1] ).Length(); 00440 T dist = (IPs[0] - m_SharpPointPrevPos_ForIPG).Length(); 00441 00442 if ( dist >= distThreshold && dist < distConstraint ) { 00443 // Shift every point by one 00444 for ( unsigned int i = next; i > start; --i ) { 00445 00446 m_SetOfInteractionAssociatedModelID_ForIPG[i] = m_SetOfInteractionAssociatedModelID_ForIPG[i-1]; 00447 //m_SetOfInteractions_ForIPG[i].SetVertexIDFromModel_1( i ); // already set and it is invariant 00448 m_SetOfInteractions_ForIPG[i].SetPtrToVertexFromModel_2( m_SetOfInteractions_ForIPG[i-1].GetPtrToVertexFromModel_2() ); 00449 m_SetOfInteractions_ForIPG[i].SetForceRatio( m_SetOfInteractions_ForIPG[i-1].GetForceRatio() ); 00450 m_SetOfInteractions_ForIPG[i].SetForceScaleA( m_SetOfInteractions_ForIPG[i-1].GetForceScaleA() ); 00451 m_SetOfInteractions_ForIPG[i].SetForceScaleB( m_SetOfInteractions_ForIPG[i-1].GetForceScaleB() ); 00452 m_SetOfInteractions_ForIPG[i].SetForceThresholdA( m_SetOfInteractions_ForIPG[i-1].GetForceThresholdA() ); 00453 m_SetOfInteractions_ForIPG[i].SetForceThresholdB( m_SetOfInteractions_ForIPG[i-1].GetForceThresholdB() ); 00454 m_SetOfInteractions_ForIPG[i].SetEnforcePositionStatusA( m_SetOfInteractions_ForIPG[i-1].GetEnforcePositionStatusA() ); 00455 m_SetOfInteractions_ForIPG[i].SetEnforcePositionStatusB( m_SetOfInteractions_ForIPG[i-1].GetEnforcePositionStatusB() ); 00456 // Shift the punctured location 00457 m_PuncturedLocations_ForIPG[i] = m_PuncturedLocations_ForIPG[i-1]; 00458 } 00459 // 00460 // Clear the start point 00461 // 00462 IPG.GetInteractionPointAtIndex( start ).GetSimFlags().ClearSimulationConstraints( Enum::AddOn::SLIDABLE ); 00463 //IPG.GetInteractionPointAtIndex( start ).GetSimFlags().ClearSimulationConstraints( Enum::AddOn::ATTACHED ); 00464 //IPG.GetInteractionPointAtIndex( start ).GetSimFlags().ClearSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 00465 m_SetOfInteractionAssociatedModelID_ForIPG[start] = -1; 00466 00467 m_PuncturedLocations_ForIPG[start] = IPs[start]; 00468 m_SharpPointPrevPos_ForIPG = IPs[0]; // save the location of the sharp point 00469 //m_NumberOfSutureHeadNeedlePointsPuncturedIn; 00470 00471 //std::cout << "m_NumberOfSutureHeadNeedlePointsPuncturedIn: " << m_NumberOfSutureHeadNeedlePointsPuncturedIn << "\n"; 00472 } 00473 } 00474 else { // CASE: next == m_size 00475 00476 //std::cout << "next >= m_size --> next: " << next << ", start: " << start << std::endl; 00477 --next; 00478 00479 Matrix4x4<T> TrxFEMModel = ListOfModelsBasedOnFEM[FEMloc]->RefToTransformationSupport().RefToMatrixTransform(); 00480 Vector3<T> pos1 = TrxFEMModel * m_SetOfInteractions_ForIPG[start].GetPtrToVertexFromModel_2()->GetBarycentricPtr()->GetNewVertexLocation(); 00481 00482 T distThreshold = ( IPs[next] - IPs[next-1] ).Length(); 00483 T dist = (IPs[0] - m_SharpPointPrevPos_ForIPG).Length(); 00484 00485 if ( dist >= distThreshold && dist < distConstraint ) { 00486 00487 // 00488 // Add the constraint of suture thread 00489 // 00490 { 00491 //std::cout << "ADD SUTURE THREAD" << std::endl; 00492 00493 unsigned int ERloc = ListOfModels[m_ERModelAsSutureThread].Slot; 00494 unsigned int ERModelVertexID = 1; 00495 ModelElasticRod<T> * pSutureThread = ListOfModelsBasedOnER[ERloc]; 00496 pSutureThread->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 00497 pSutureThread->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 00498 pSutureThread->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 00499 // 00500 //unsigned int HEloc = m_SetOfInteractionAssociatedModelID_ForIPG[next]; 00501 HEVertex<T> * pVertexHEModel = m_SetOfInteractions_ForIPG[next].GetPtrToVertexFromModel_2(); 00502 00503 ++m_NumberOfSutureThreadPointsPuncturedIn; 00504 00505 //std::cout << "m_NumberOfSutureThreadPointsPuncturedIn: " << m_NumberOfSutureThreadPointsPuncturedIn << "\n"; 00506 00507 // Shift all suture points by one (FORWARD ATTACHMENT OF SUTURE'S THREAD WITH FEMMODEL) 00508 std::list< InteractionSutureThreadVsFEMModel<T> >::iterator it = m_ListOfSutureThreadVsFEMModels_ForIPG.begin(); 00509 while ( it != m_ListOfSutureThreadVsFEMModels_ForIPG.end() ) { 00510 unsigned int ptID = it->pAdvSimConstraint_VertexIdVsHEVertex->GetVertexIDFromModel_1() + 1; 00511 it->pAdvSimConstraint_VertexIdVsHEVertex->SetVertexIDFromModel_1( ptID ); 00512 pSutureThread->GetListOfNodes()[ptID].SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 00513 pSutureThread->GetListOfNodes()[ptID].SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 00514 pSutureThread->GetListOfNodes()[ptID].SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 00515 ++it; 00516 } 00517 00518 AdvSimConstraint_VertexIdVsHEVertex<T> * pConstraint = 00519 new AdvSimConstraint_VertexIdVsHEVertex<T>( 00520 ERModelVertexID, 00521 pVertexHEModel, 00522 AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio_2, 00523 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA_2, 00524 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB_2, 00525 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA_2, 00526 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB_2, 00527 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA_2, 00528 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB_2 00529 ); 00530 m_ListOfSutureThreadVsFEMModels_ForIPG.push_front( InteractionSutureThreadVsFEMModel<T>( pConstraint, ERloc ) ); 00531 pConstraint->RefToSavedPositionB() = pVertexHEModel->GetPosition(); 00532 00533 //std::cout << "HEVertex's position: " << pVertexHEModel->GetPosition() << "\n"; 00534 //std::cout << "pConstraint: " << pConstraint << std::endl; 00535 //std::cout << "pConstraint: " << *pConstraint << std::endl; 00536 00537 } 00538 00539 // Shift every point by one 00540 for ( unsigned int i = next; i > start; --i ) { 00541 00542 m_SetOfInteractionAssociatedModelID_ForIPG[i] = m_SetOfInteractionAssociatedModelID_ForIPG[i-1]; 00543 //m_SetOfInteractions_ForIPG[i].SetVertexIDFromModel_1( i ); // already set and it is invariant 00544 m_SetOfInteractions_ForIPG[i].SetPtrToVertexFromModel_2( m_SetOfInteractions_ForIPG[i-1].GetPtrToVertexFromModel_2() ); 00545 m_SetOfInteractions_ForIPG[i].SetForceRatio( m_SetOfInteractions_ForIPG[i-1].GetForceRatio() ); 00546 m_SetOfInteractions_ForIPG[i].SetForceScaleA( m_SetOfInteractions_ForIPG[i-1].GetForceScaleA() ); 00547 m_SetOfInteractions_ForIPG[i].SetForceScaleB( m_SetOfInteractions_ForIPG[i-1].GetForceScaleB() ); 00548 m_SetOfInteractions_ForIPG[i].SetForceThresholdA( m_SetOfInteractions_ForIPG[i-1].GetForceThresholdA() ); 00549 m_SetOfInteractions_ForIPG[i].SetForceThresholdB( m_SetOfInteractions_ForIPG[i-1].GetForceThresholdB() ); 00550 m_SetOfInteractions_ForIPG[i].SetEnforcePositionStatusA( m_SetOfInteractions_ForIPG[i-1].GetEnforcePositionStatusA() ); 00551 m_SetOfInteractions_ForIPG[i].SetEnforcePositionStatusB( m_SetOfInteractions_ForIPG[i-1].GetEnforcePositionStatusB() ); 00552 // Shift the punctured location 00553 m_PuncturedLocations_ForIPG[i] = m_PuncturedLocations_ForIPG[i-1]; 00554 } 00555 // 00556 // Clear the start point 00557 // 00558 IPG.GetInteractionPointAtIndex( start ).GetSimFlags().ClearSimulationConstraints( Enum::AddOn::SLIDABLE ); 00559 //IPG.GetInteractionPointAtIndex( start ).GetSimFlags().ClearSimulationConstraints( Enum::AddOn::ATTACHED ); 00560 //IPG.GetInteractionPointAtIndex( start ).GetSimFlags().ClearSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 00561 m_SetOfInteractionAssociatedModelID_ForIPG[start] = -1; 00562 00563 m_PuncturedLocations_ForIPG[start] = IPs[start]; 00564 m_SharpPointPrevPos_ForIPG = IPs[0]; // save the location of the sharp point 00565 --m_NumberOfSutureHeadNeedlePointsPuncturedIn; 00566 00567 //std::cout << "m_NumberOfSutureHeadNeedlePointsPuncturedIn: " << m_NumberOfSutureHeadNeedlePointsPuncturedIn << "\n"; 00568 } 00569 } 00570 } 00571 // Case the last point of head needle still inside an FEM-based model 00572 else if ( start == m_size_ForIPG-1 ) { 00573 00574 //unsigned int next = start + 1; 00575 //std::cout << "next == m_size & start == m_size-1 --> next: " << next << ", start: " << start << ", m_size: " << m_size << std::endl; 00576 00577 Matrix4x4<T> TrxFEMModel = ListOfModelsBasedOnFEM[FEMloc]->RefToTransformationSupport().RefToMatrixTransform(); 00578 Vector3<T> pos1 = TrxFEMModel * m_SetOfInteractions_ForIPG[start].GetPtrToVertexFromModel_2()->GetBarycentricPtr()->GetNewVertexLocation(); 00579 00580 T distThreshold = ( IPs[start] - IPs[start-1] ).Length(); 00581 T dist = (IPs[0] - m_SharpPointPrevPos_ForIPG).Length(); 00582 00583 if ( dist >= distThreshold && dist < distConstraint ) { 00584 00585 // 00586 // Add the constraint of suture thread 00587 // 00588 { 00589 //std::cout << "ADD SUTURE THREAD (last connection)" << std::endl; 00590 00591 unsigned int ERloc = ListOfModels[m_ERModelAsSutureThread].Slot; 00592 unsigned int ERModelVertexID = 1; 00593 ModelElasticRod<T> * pSutureThread = ListOfModelsBasedOnER[ERloc]; 00594 pSutureThread->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 00595 pSutureThread->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 00596 pSutureThread->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 00597 // 00598 //unsigned int HEloc = m_SetOfInteractionAssociatedModelID_ForIPG[start]; 00599 HEVertex<T> * pVertexHEModel = m_SetOfInteractions_ForIPG[start].GetPtrToVertexFromModel_2(); 00600 00601 ++m_NumberOfSutureThreadPointsPuncturedIn; 00602 00603 //std::cout << "m_NumberOfSutureThreadPointsPuncturedIn: " << m_NumberOfSutureThreadPointsPuncturedIn << "\n"; 00604 00605 // Shift all suture points by one (FORWARD ATTACHMENT OF SUTURE'S THREAD WITH FEMMODEL) 00606 std::list< InteractionSutureThreadVsFEMModel<T> >::iterator it = m_ListOfSutureThreadVsFEMModels_ForIPG.begin(); 00607 while ( it != m_ListOfSutureThreadVsFEMModels_ForIPG.end() ) { 00608 unsigned int ptID = it->pAdvSimConstraint_VertexIdVsHEVertex->GetVertexIDFromModel_1() + 1; 00609 it->pAdvSimConstraint_VertexIdVsHEVertex->SetVertexIDFromModel_1( ptID ); 00610 pSutureThread->GetListOfNodes()[ptID].SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 00611 pSutureThread->GetListOfNodes()[ptID].SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 00612 pSutureThread->GetListOfNodes()[ptID].SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 00613 ++it; 00614 } 00615 00616 AdvSimConstraint_VertexIdVsHEVertex<T> * pConstraint = 00617 new AdvSimConstraint_VertexIdVsHEVertex<T>( 00618 ERModelVertexID, 00619 pVertexHEModel, 00620 AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio_2, 00621 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA_2, 00622 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB_2, 00623 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA_2, 00624 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB_2, 00625 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA_2, 00626 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB_2 00627 ); 00628 pConstraint->RefToSavedPositionB() = pVertexHEModel->GetPosition(); 00629 m_ListOfSutureThreadVsFEMModels_ForIPG.push_front( InteractionSutureThreadVsFEMModel<T>( pConstraint, ERloc ) ); 00630 00631 //std::cout << "HEVertex's position: " << pVertexHEModel->GetPosition() << "\n"; 00632 //std::cout << "pConstraint: " << pConstraint << std::endl; 00633 //std::cout << "pConstraint: " << *pConstraint << std::endl; 00634 } 00635 // 00636 // Clear the start point 00637 // 00638 IPG.GetInteractionPointAtIndex( start ).GetSimFlags().ClearSimulationConstraints( Enum::AddOn::SLIDABLE ); 00639 //IPG.GetInteractionPointAtIndex( start ).GetSimFlags().ClearSimulationConstraints( Enum::AddOn::ATTACHED ); 00640 //IPG.GetInteractionPointAtIndex( start ).GetSimFlags().ClearSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 00641 m_SetOfInteractionAssociatedModelID_ForIPG[start] = -1; 00642 00643 ListOfModelsBasedOnRBD[RBDloc]->UnlockPosition(); // lock the head needle 00644 m_PuncturedLocations_ForIPG[start] = IPs[start]; 00645 m_SharpPointPrevPos_ForIPG = IPs[0]; // save the location of the sharp point 00646 --m_NumberOfSutureHeadNeedlePointsPuncturedIn; 00647 00648 //std::cout << "last point out\n"; 00649 //std::cout << "m_NumberOfSutureHeadNeedlePointsPuncturedIn: " << m_NumberOfSutureHeadNeedlePointsPuncturedIn << "\n"; 00650 00651 // Transfer the constraints to the main constraint data structure 00652 { 00653 TransferConstraintsToMainConstraintDS( 00654 m_ListOfSutureThreadVsFEMModels_ForIPG, 00655 ListOf_ERModelVsHEModelInteractions, 00656 ListOfModelObjects, 00657 ListOfModels[m_ERModelAsSutureThread].Slot 00658 ); 00659 00660 m_PunctureController.WaitForTransfer = false; 00661 00662 // Clear 00663 m_NumberOfSutureThreadPointsPuncturedIn = 0; 00664 m_ListOfSutureThreadVsFEMModels_ForIPG.clear(); 00665 } 00666 } 00667 } 00668 }//END: CASE FOR THE SHARP POINT HAS PUNCTURED OUT 00669 } 00670 00671 else if ( pFEMTet ) 00672 { 00673 PrtNotImplementedYet( "ProcessPuncturingOfHeadNeedleRepresentedByIPGWithFEMModel for FEMTet" ); 00674 } 00675 00676 00677 /* 00678 // DEBUG 00679 unsigned int ERloc = ListOfModels[m_ERModelAsSutureThread].Slot; 00680 unsigned int HEloc = 0; 00681 for ( unsigned int i = 0; i < ListOf_ERModelVsHEModelInteractions[ERloc][HEloc].size(); ++i ) { 00682 std::cout << i << ": ER vertID: " << ListOf_ERModelVsHEModelInteractions[ERloc][HEloc][i]->GetVertexIDFromModel_1() << "\n"; 00683 } 00684 //*/ 00685 00686 return NULL; 00687 } 00688 //----------------------------------------------------------------------------- 00689 template <typename T, typename DATA> 00690 HEVertex<T> * AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::ProcessPuncturingOfHeadNeedleUsingCirclePathWithFEMModel ( 00691 typename ModelSurgicalSutureWithHeadNeedle<T>::CirclePath & CirclePathCtrl, 00692 unsigned int FEMModel, 00693 typename AdvSimSupport_DATA<T,DATA>::ModelIDs const & ListOfModels, 00694 typename AdvSimSupport_DATA<T,DATA>::ModelLists & ListOfModelObjects, 00695 typename AdvSimSupport_DATA<T,DATA>::ListOfInteractions_ERModelVsHEModel & ListOf_ERModelVsHEModelInteractions, 00696 bool & bPunctureIn, 00697 bool & bPunctureOut, 00698 bool & bAborted, 00699 bool & bSuccess 00700 ) 00701 { 00702 //return NULL; 00703 00704 // Easy puncturing 00705 00706 00707 bPunctureIn = false; 00708 bPunctureOut = false; 00709 bAborted = false; 00710 bSuccess = false; 00711 00712 std::vector< ModelElasticRod<T> * > & ListOfModelsBasedOnER = ListOfModelObjects.BasedOnER; 00713 std::vector< ModelDefBasedOnFEM<T,DATA> * > & ListOfModelsBasedOnFEM = ListOfModelObjects.BasedOnFEM; 00714 std::vector< RigidBodyDynamics<T> * > & ListOfModelsBasedOnRBD = ListOfModelObjects.BasedOnRBD; 00715 00716 /* 00717 // DEBUG LIMIT THE NUMBER OF LOCKED CIRCLE PATH 00718 if ( m_uiNumOfLockedCirclePaths >= 3 ) { 00719 CirclePathCtrl.pListOfCDNodes.clear(); 00720 ClearAllConstraints_ForCirclePath(); 00721 return NULL; 00722 } 00723 //*/ 00724 00725 //--------------------------------------------------------------- 00726 // Process puncturing of the locked puncture path 00727 if ( m_LockedCirclePath.IsLocked ) { 00728 HEVertex<T> * pVertex = ProcessLockedPuncturePath( 00729 m_LockedCirclePath, 00730 CirclePathCtrl, 00731 ListOfModels, 00732 ListOfModelObjects, 00733 ListOf_ERModelVsHEModelInteractions, 00734 bPunctureIn, 00735 bPunctureOut, 00736 bAborted, 00737 bSuccess 00738 ); 00739 00740 if ( bAborted ) { 00741 ClearAllConstraints_ForCirclePath(); 00742 CirclePathCtrl.pListOfCDNodes.clear(); 00743 return NULL; 00744 } 00745 00746 if ( bPunctureOut ) { 00747 CirclePathCtrl.pListOfCDNodes.clear(); 00748 } 00749 00750 return pVertex; 00751 //*/ 00752 00753 return NULL; 00754 } 00755 00756 //std::cout << "find puncture path" << std::endl; // DEBUG 00757 00758 //--------------------------------------------------------------- 00759 // 00760 // Find puncture path 00761 // 00762 00763 unsigned int RBDloc = ListOfModels[m_RBDModelAsSutureHeadNeedle].Slot; 00764 unsigned int FEMloc = ListOfModels[FEMModel].Slot; 00765 00766 ModelDefBasedOnFEMHex<T,DATA> * pFEMHex = dynamic_cast< ModelDefBasedOnFEMHex<T,DATA> * >( ListOfModelsBasedOnFEM[FEMloc] ); 00767 ModelDefBasedOnFEMTet<T,DATA> * pFEMTet = dynamic_cast< ModelDefBasedOnFEMTet<T,DATA> * >( ListOfModelsBasedOnFEM[FEMloc] ); 00768 00769 RigidBodyDynamics<T> * pHeadNeedle = ListOfModelsBasedOnRBD[RBDloc]; 00770 Matrix4x4<T> TrxRBD( pHeadNeedle->CalMatrixTransform() ); 00771 BVHNodeLeaf<T> cdNode( Enum::BVH_NODE_LEAF_SPHERE ); 00772 cdNode.SetCenter( TrxRBD * CirclePathCtrl.Center ); 00773 cdNode.SetRadius( CirclePathCtrl.Radius ); 00774 00775 Vector3<T> circleCenter = TrxRBD * CirclePathCtrl.Center; 00776 Vector3<T> circleNormalPlane = TrxRBD * (CirclePathCtrl.NormalPlane + CirclePathCtrl.Center) - circleCenter; 00777 00778 if ( pFEMHex ) 00779 { 00780 // Broad-phase collision detection of the model's binary sphere tree with the needle's circle path 00781 //bool bCD = pFEMHex->PtrToBVHTreeOfSurfaceMesh()->TestOverlapWithTillLeafNodes( CirclePathCtrl.pCDSphereNode ); 00782 bool bCD = pFEMHex->PtrToBVHTreeOfSurfaceMesh()->TestIntersectionWithCircleTillLeafNodes( &circleNormalPlane, &circleCenter, CirclePathCtrl.Radius ); 00783 if ( bCD ) { 00784 00785 // Narrow-phase collision detection of the triangles bounded by spheres 00786 // detected from the broad-phase collision detection with the needle's circle path 00787 std::vector< BVHNode<T> * > pBVHNodeList = pFEMHex->PtrToBVHTreeOfSurfaceMesh()->GetListOfCollidedNodes(); 00788 std::vector< BVHNode<T> * >::iterator it = pBVHNodeList.begin(); 00789 00790 Matrix4x4<T> TrxFEM( pFEMHex->PtrToBVHTreeOfSurfaceMesh()->GetTransform().GetMatrixTransform() ); 00791 CirclePathCtrl.Trx = pFEMHex->PtrToBVHTreeOfSurfaceMesh()->GetTransform(); 00792 CirclePathCtrl.pListOfCDNodes.clear(); 00793 00794 // Transform the interpolated points along the circle path to the world space 00795 std::vector< Vector3<T> > points; 00796 unsigned int numOfSegments = CirclePathCtrl.InterpolatedPts.size() - 1; 00797 for ( unsigned int i = 0; i <= numOfSegments; ++i ) { 00798 points.push_back( TrxRBD * CirclePathCtrl.InterpolatedPts[i] ); 00799 } 00800 00801 std::vector< unsigned int > interpolatedPointIDs; 00802 std::vector< HEFace<T> * > triangles; 00803 std::vector< unsigned int > listOfFEMModelIDs; 00804 00805 while ( it != pBVHNodeList.end() ) { 00806 int id = CDCirclePathVsTriangle( points, numOfSegments, (*it)->GetAPrimitiveHalfEdgeFace(), TrxFEM ); 00807 if ( id >= 0 ) { 00808 interpolatedPointIDs.push_back( id ); 00809 triangles.push_back( (*it)->GetAPrimitiveHalfEdgeFace() ); 00810 listOfFEMModelIDs.push_back( FEMModel ); 00811 CirclePathCtrl.pListOfCDNodes.push_back( *it ); 00812 } 00813 ++it; 00814 } 00815 00816 if ( interpolatedPointIDs.size() > 0 ) { 00817 00818 std::vector< unsigned int > interpolatedPointIDs_sorted; 00819 std::vector< HEFace<T> * > triangles_sorted; 00820 std::vector< unsigned int > listOfFEMModelIDs_sorted; 00821 std::vector< bool > checks( interpolatedPointIDs.size(), false ); 00822 00823 // Sort the data 00824 unsigned int minID, address; 00825 for ( unsigned int i = 0; i < interpolatedPointIDs.size(); ++i ) { 00826 minID = INT_MAX; 00827 address = INT_MAX; 00828 for ( unsigned int j = 0; j < interpolatedPointIDs.size(); ++j ) { 00829 if ( !checks[j] && minID > interpolatedPointIDs[j] ) { 00830 minID = interpolatedPointIDs[j]; 00831 address = j; 00832 } 00833 } 00834 if ( minID < INT_MAX ) { 00835 interpolatedPointIDs_sorted.push_back( minID ); 00836 triangles_sorted.push_back( triangles[address] ); 00837 listOfFEMModelIDs_sorted.push_back( listOfFEMModelIDs[address] ); 00838 checks[address] = true; 00839 } 00840 } 00841 00842 //return NULL; 00843 00844 if ( RecordAndLockThePuncturePath( 00845 CirclePathCtrl, 00846 ListOfModels, 00847 ListOfModelsBasedOnER, 00848 ListOfModelsBasedOnFEM, 00849 ListOfModelsBasedOnRBD, 00850 ListOf_ERModelVsHEModelInteractions, 00851 bPunctureIn, 00852 bPunctureOut, 00853 interpolatedPointIDs_sorted, 00854 triangles_sorted, 00855 listOfFEMModelIDs_sorted 00856 ) 00857 ) 00858 { 00859 } 00860 }//END: if ( interpolatedPointIDs.size() > 0 ) 00861 }//END: Narrow-phase CD 00862 }//END: if ( pFEMHex ) 00863 00864 else if ( pFEMTet ) 00865 { 00866 PrtNotImplementedYet( "ProcessPuncturingOfHeadNeedleUsingCirclePathWithFEMModel for FEMTet" ); 00867 }//END: if ( pFEMTet ) 00868 00869 return NULL; 00870 } 00871 //----------------------------------------------------------------------------- 00872 template <typename T, typename DATA> 00873 HEVertex<T> * AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::ProcessPuncturingOfHeadNeedleUsingCirclePathWithFEMModel_MoreAccuButUnfinishYet ( 00874 typename ModelSurgicalSutureWithHeadNeedle<T>::CirclePath & CirclePathCtrl, 00875 unsigned int FEMModel, 00876 typename AdvSimSupport_DATA<T,DATA>::ModelIDs const & ListOfModels, 00877 typename AdvSimSupport_DATA<T,DATA>::ModelLists & ListOfModelObjects, 00878 //std::vector< ModelElasticRod<T> * > & ListOfModelsBasedOnER, //!< list of suture models based on elastic rod (belongs to ER-based model group) 00879 //std::vector< ModelDefBasedOnFEM<T,DATA> * > & ListOfModelsBasedOnFEM, //!< list of deformable models based on FEM (belongs to FEM-based model group) 00880 //std::vector< RigidBodyDynamics<T> * > & ListOfModelsBasedOnRBD, //!< list of models based on rigid body dynamics (belongs to IPG-based model group) 00881 typename AdvSimSupport_DATA<T,DATA>::ListOfInteractions_ERModelVsHEModel & ListOf_ERModelVsHEModelInteractions, 00882 bool & bPunctureIn, 00883 bool & bPunctureOut, 00884 bool & bAborted 00885 ) 00886 { 00887 bPunctureIn = false; 00888 bPunctureOut = false; 00889 bAborted = false; 00890 00891 std::vector< ModelElasticRod<T> * > & ListOfModelsBasedOnER = ListOfModelObjects.BasedOnER; 00892 std::vector< ModelDefBasedOnFEM<T,DATA> * > & ListOfModelsBasedOnFEM = ListOfModelObjects.BasedOnFEM; 00893 std::vector< RigidBodyDynamics<T> * > & ListOfModelsBasedOnRBD = ListOfModelObjects.BasedOnRBD; 00894 00895 /* 00896 // DEBUG LIMIT THE NUMBER OF LOCKED CIRCLE PATH 00897 if ( m_uiNumOfLockedCirclePaths >= 3 ) { 00898 CirclePathCtrl.pListOfCDNodes.clear(); 00899 ClearAllConstraints_ForCirclePath(); 00900 return NULL; 00901 } 00902 //*/ 00903 00904 //--------------------------------------------------------------- 00905 // Process puncturing of the locked puncture path 00906 if ( m_LockedCirclePath.IsLocked ) { 00907 HEVertex<T> * pVertex = ProcessLockedPuncturePath_MoreAccuButUnfinishYet( 00908 m_LockedCirclePath, 00909 CirclePathCtrl, 00910 ListOfModels, 00911 ListOfModelObjects, 00912 ListOf_ERModelVsHEModelInteractions, 00913 bPunctureIn, 00914 bPunctureOut, 00915 bAborted 00916 ); 00917 00918 if ( bAborted ) { 00919 ClearAllConstraints_ForCirclePath(); 00920 CirclePathCtrl.pListOfCDNodes.clear(); 00921 return NULL; 00922 } 00923 00924 if ( bPunctureOut ) { 00925 CirclePathCtrl.pListOfCDNodes.clear(); 00926 } 00927 00928 return pVertex; 00929 } 00930 00931 //std::cout << "find puncture path" << std::endl; // DEBUG 00932 00933 //--------------------------------------------------------------- 00934 // 00935 // Find puncture path 00936 // 00937 00938 unsigned int RBDloc = ListOfModels[m_RBDModelAsSutureHeadNeedle].Slot; 00939 unsigned int FEMloc = ListOfModels[FEMModel].Slot; 00940 00941 ModelDefBasedOnFEMHex<T,DATA> * pFEMHex = dynamic_cast< ModelDefBasedOnFEMHex<T,DATA> * >( ListOfModelsBasedOnFEM[FEMloc] ); 00942 ModelDefBasedOnFEMTet<T,DATA> * pFEMTet = dynamic_cast< ModelDefBasedOnFEMTet<T,DATA> * >( ListOfModelsBasedOnFEM[FEMloc] ); 00943 00944 RigidBodyDynamics<T> * pHeadNeedle = ListOfModelsBasedOnRBD[RBDloc]; 00945 Matrix4x4<T> TrxRBD( pHeadNeedle->CalMatrixTransform() ); 00946 BVHNodeLeaf<T> cdNode( Enum::BVH_NODE_LEAF_SPHERE ); 00947 cdNode.SetCenter( TrxRBD * CirclePathCtrl.Center ); 00948 cdNode.SetRadius( CirclePathCtrl.Radius ); 00949 00950 Vector3<T> circleCenter = TrxRBD * CirclePathCtrl.Center; 00951 Vector3<T> circleNormalPlane = TrxRBD * (CirclePathCtrl.NormalPlane + CirclePathCtrl.Center) - circleCenter; 00952 00953 if ( pFEMHex ) 00954 { 00955 // Broad-phase collision detection of the model's binary sphere tree with the needle's circle path 00956 //bool bCD = pFEMHex->PtrToBVHTreeOfSurfaceMesh()->TestOverlapWithTillLeafNodes( CirclePathCtrl.pCDSphereNode ); 00957 bool bCD = pFEMHex->PtrToBVHTreeOfSurfaceMesh()->TestIntersectionWithCircleTillLeafNodes( &circleNormalPlane, &circleCenter, CirclePathCtrl.Radius ); 00958 if ( bCD ) { 00959 00960 // Narrow-phase collision detection of the triangles bounded by spheres 00961 // detected from the broad-phase collision detection with the needle's circle path 00962 std::vector< BVHNode<T> * > pBVHNodeList = pFEMHex->PtrToBVHTreeOfSurfaceMesh()->GetListOfCollidedNodes(); 00963 std::vector< BVHNode<T> * >::iterator it = pBVHNodeList.begin(); 00964 00965 Matrix4x4<T> TrxFEM( pFEMHex->PtrToBVHTreeOfSurfaceMesh()->GetTransform().GetMatrixTransform() ); 00966 CirclePathCtrl.Trx = pFEMHex->PtrToBVHTreeOfSurfaceMesh()->GetTransform(); 00967 CirclePathCtrl.pListOfCDNodes.clear(); 00968 00969 // Transform the interpolated points along the circle path to the world space 00970 std::vector< Vector3<T> > points; 00971 unsigned int numOfSegments = CirclePathCtrl.InterpolatedPts.size() - 1; 00972 for ( unsigned int i = 0; i <= numOfSegments; ++i ) { 00973 points.push_back( TrxRBD * CirclePathCtrl.InterpolatedPts[i] ); 00974 } 00975 00976 std::vector< unsigned int > interpolatedPointIDs; 00977 std::vector< HEFace<T> * > triangles; 00978 std::vector< unsigned int > listOfFEMModelIDs; 00979 00980 while ( it != pBVHNodeList.end() ) { 00981 int id = CDCirclePathVsTriangle( points, numOfSegments, (*it)->GetAPrimitiveHalfEdgeFace(), TrxFEM ); 00982 if ( id >= 0 ) { 00983 interpolatedPointIDs.push_back( id ); 00984 triangles.push_back( (*it)->GetAPrimitiveHalfEdgeFace() ); 00985 listOfFEMModelIDs.push_back( FEMModel ); 00986 CirclePathCtrl.pListOfCDNodes.push_back( *it ); 00987 } 00988 ++it; 00989 } 00990 00991 if ( interpolatedPointIDs.size() > 0 ) { 00992 00993 std::vector< unsigned int > interpolatedPointIDs_sorted; 00994 std::vector< HEFace<T> * > triangles_sorted; 00995 std::vector< unsigned int > listOfFEMModelIDs_sorted; 00996 std::vector< bool > checks( interpolatedPointIDs.size(), false ); 00997 00998 // Sort the data 00999 unsigned int minID, address; 01000 for ( unsigned int i = 0; i < interpolatedPointIDs.size(); ++i ) { 01001 minID = INT_MAX; 01002 address = INT_MAX; 01003 for ( unsigned int j = 0; j < interpolatedPointIDs.size(); ++j ) { 01004 if ( !checks[j] && minID > interpolatedPointIDs[j] ) { 01005 minID = interpolatedPointIDs[j]; 01006 address = j; 01007 } 01008 } 01009 if ( minID < INT_MAX ) { 01010 interpolatedPointIDs_sorted.push_back( minID ); 01011 triangles_sorted.push_back( triangles[address] ); 01012 listOfFEMModelIDs_sorted.push_back( listOfFEMModelIDs[address] ); 01013 checks[address] = true; 01014 } 01015 } 01016 01017 /* 01018 // DEBUG 01019 std::cout << "Size of unsorted triangles: " << triangles.size() << std::endl; 01020 std::cout << "Size of sorted triangles: " << triangles_sorted.size() << std::endl; 01021 for ( unsigned int i = 0; i < triangles_sorted.size(); ++i ) { 01022 std::cout << i << "\t" << *triangles_sorted[i] << std::endl; 01023 } 01024 //*/ 01025 01026 /* 01027 // DEBUG 01028 std::cout << "IDs\n"; 01029 for ( unsigned int i = 0; i < interpolatedPointIDs_sorted.size(); ++i ) { 01030 std::cout << interpolatedPointIDs_sorted[i] << "\n"; 01031 std::cout << "FEMModel: " << listOfFEMModelIDs_sorted[i] << "\n"; 01032 std::cout << "Triangles: " << triangles_sorted[i] << "\n"; 01033 } 01034 //*/ 01035 01036 // The sorted data are interpolatedPointIDs_sorted and triangles_sorted 01037 // points still contains the interpolated point in world space along the circle path 01038 // point#0 is the end point of the needle 01039 // and point#[CirclePathCtrl.IDOfFirstInterpolatedPtBeforeSharpPt] is the first interpolated point before the needle's sharp point 01040 // 01041 // Record the punctured path 01042 // 01043 //m_LockedCirclePath.Clear(); // m_LockedCirclePath has to be cleared before calling RecordAndLockThePuncturePath_MoreAccuButUnfinishYet fn 01044 // Currently, tt is cleared in ProcessLockedPuncturePath_MoreAccuButUnfinishYet fn 01045 01046 //std::cout << "Try RecordAndLockThePuncturePath_MoreAccuButUnfinishYet" << std::endl; // DEBUG 01047 01048 if ( RecordAndLockThePuncturePath_MoreAccuButUnfinishYet( 01049 CirclePathCtrl, 01050 ListOfModels, 01051 ListOfModelsBasedOnER, 01052 ListOfModelsBasedOnFEM, 01053 ListOfModelsBasedOnRBD, 01054 ListOf_ERModelVsHEModelInteractions, 01055 bPunctureIn, 01056 bPunctureOut, 01057 interpolatedPointIDs_sorted, 01058 triangles_sorted, 01059 listOfFEMModelIDs_sorted 01060 ) 01061 ) 01062 { 01063 //bPunctureIn = true; 01064 //return m_LockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn.back().pHEVertex; 01065 } 01066 }//END: if ( interpolatedPointIDs.size() > 0 ) 01067 }//END: Narrow-phase CD 01068 }//END: if ( pFEMHex ) 01069 01070 else if ( pFEMTet ) 01071 { 01072 PrtNotImplementedYet( "ProcessPuncturingOfHeadNeedleUsingCirclePathWithFEMModel_MoreAccuButUnfinishYet for FEMTet" ); 01073 }//END: if ( pFEMTet ) 01074 01075 return NULL; 01076 } 01077 //----------------------------------------------------------------------------- 01078 template <typename T, typename DATA> 01079 int AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::CDCirclePathVsTriangle ( 01080 std::vector< Vector3<T> > points, 01081 unsigned int numOfSegments, 01082 HEFace<T> const * const pHEFace, 01083 Matrix4x4<T> const & TrxTriangle 01084 ) 01085 { 01086 //std::cout << *pHEFace << "\n"; 01087 01088 bool result; 01089 T ratio; 01090 Vector3<T> projPt; 01091 01092 std::vector< HEVertex<T> const * const > pHEVertices = pHEFace->GetPtrsToVertices(); 01093 for ( unsigned int i = 0; i < numOfSegments; ++i ) { 01094 result = CGMath<T>::FindIntersectionLineSegmentTriangle( 01095 points[i], points[i+1], 01096 TrxTriangle * pHEVertices[0]->GetPosition(), 01097 TrxTriangle * pHEVertices[1]->GetPosition(), 01098 TrxTriangle * pHEVertices[2]->GetPosition(), 01099 ratio, 01100 projPt 01101 ); 01102 if ( result ) { 01103 //std::cout << "InterpolatedPt ID: " << i << "\n"; 01104 return i; 01105 } 01106 } 01107 01108 return -1; 01109 } 01110 //----------------------------------------------------------------------------- 01111 template <typename T, typename DATA> 01112 bool AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::RecordAndLockThePuncturePath ( 01113 typename ModelSurgicalSutureWithHeadNeedle<T>::CirclePath & CirclePathCtrl, 01114 typename AdvSimSupport_DATA<T,DATA>::ModelIDs const & ListOfModels, 01115 std::vector< ModelElasticRod<T> * > & ListOfModelsBasedOnER, 01116 std::vector< ModelDefBasedOnFEM<T,DATA> * > & ListOfModelsBasedOnFEM, 01117 std::vector< RigidBodyDynamics<T> * > & ListOfModelsBasedOnRBD, 01118 typename AdvSimSupport_DATA<T,DATA>::ListOfInteractions_ERModelVsHEModel & ListOf_ERModelVsHEModelInteractions, 01119 bool & bPunctureIn, 01120 bool & bPunctureOut, 01121 std::vector< unsigned int > & interpolatedPointIDs, 01122 std::vector< HEFace<T> * > & triangles, 01123 std::vector< unsigned int > & listOfFEMModelIDs 01124 ) 01125 { 01126 //PunctureInOutList punctureList; 01127 01128 // The head needle's sharp point has to be penetrated the FEM-based model 01129 if ( interpolatedPointIDs[0] > CirclePathCtrl.IDOfFirstInterpolatedPtBeforeSharpPt ) { 01130 return false; 01131 } 01132 01133 //return false; 01134 01135 unsigned int sutureThreadID = 20; 01136 unsigned int numOfPts = interpolatedPointIDs.size(); 01137 01138 01139 // Even number of punctured points 01140 if ( numOfPts >= 2 && numOfPts % 2 == 0 ) { 01141 01142 Vector3<T> position; 01143 Vector3<unsigned int> gridLocations; 01144 Vector3<T> coordinates; 01145 01146 // For all pairs of two points -- puncture out and in 01147 //for ( int i = numOfPts-1; i > 0; i-=2 ) 01148 // Only the first two points -- puncture out and in 01149 int i = 1; 01150 { 01151 01152 // plus two for skipping the first point after the punctured-in point 01153 // because the point can be close to the punctured in point 01154 // and make the suture thread turn sharply. 01155 int outID = interpolatedPointIDs[i]; 01156 int inID = interpolatedPointIDs[i-1] + 1; 01157 //int inID = interpolatedPointIDs[i-1]; 01158 01159 // Locate FEM-based model 01160 unsigned int elementID; 01161 unsigned int FEMModel = listOfFEMModelIDs[i-1]; 01162 unsigned int FEMloc = ListOfModels[FEMModel].Slot; 01163 01164 /* 01165 // DEBUG 01166 std::cout << "outID: " << outID << std::endl; 01167 std::cout << "inID: " << inID << std::endl; 01168 std::cout << "FEMModel = " << FEMModel << std::endl; 01169 std::cout << "FEMloc = " << FEMloc << std::endl; 01170 //*/ 01171 01172 ModelDefBasedOnFEMHex<T,DATA> * pFEMHex = dynamic_cast< ModelDefBasedOnFEMHex<T,DATA> * >( ListOfModelsBasedOnFEM[FEMloc] ); 01173 ModelDefBasedOnFEMTet<T,DATA> * pFEMTet = dynamic_cast< ModelDefBasedOnFEMTet<T,DATA> * >( ListOfModelsBasedOnFEM[FEMloc] ); 01174 unsigned int ERloc = ListOfModels[m_ERModelAsSutureThread].Slot; 01175 unsigned int RBDloc = ListOfModels[m_RBDModelAsSutureHeadNeedle].Slot; 01176 ModelElasticRod<T> * pSutureThread = ListOfModelsBasedOnER[ERloc]; 01177 01178 RigidBodyDynamics<T> * pHeadNeedle = ListOfModelsBasedOnRBD[RBDloc]; 01179 Matrix4x4<T> TrxRBD( pHeadNeedle->CalMatrixTransform() ); 01180 //Vector3<T> circleCenter = TrxRBD * CirclePathCtrl.Center; 01181 //Vector3<T> circleNormalPlane = TrxRBD * (CirclePathCtrl.NormalPlane + CirclePathCtrl.Center) - circleCenter; 01182 01183 // Store the FEM's location id of this punctured pair (of punctured-in and punctured-out points) 01184 m_LockedCirclePath.ListOfFEMModelBasedOnPuncturedPair.push_back( FEMModel ); 01185 01186 //* 01187 // 01188 // Find and store punctured-in vertex 01189 // 01190 //std::cout << "Find and store punctured-in vertex" << std::endl; 01191 { 01192 Vector3<T> position = pFEMHex->RefToTransformationSupport().GetInverseMatrixTransform() * TrxRBD * CirclePathCtrl.InterpolatedPts[i-1]; 01193 std::vector< HEVertex<T> * const > pListOfTriVertices = triangles[i-1]->GetPtrsToVertices(); 01194 01195 /* 01196 // DEBUG 01197 std::cout << "pListOfTriVertices[0]: " << pListOfTriVertices[0] << std::endl; 01198 std::cout << "pListOfTriVertices[1]: " << pListOfTriVertices[1] << std::endl; 01199 std::cout << "pListOfTriVertices[2]: " << pListOfTriVertices[2] << std::endl; 01200 //*/ 01201 01202 T len0 = (position - pListOfTriVertices[0]->GetPosition()).Length(); 01203 T len1 = (position - pListOfTriVertices[1]->GetPosition()).Length(); 01204 T len2 = (position - pListOfTriVertices[2]->GetPosition()).Length(); 01205 if ( len0 < len1 ) { 01206 if ( len0 < len2 ) { // len0 is minimum 01207 m_LockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn.push_back( IData( pListOfTriVertices[0] ) ); 01208 } 01209 else { // len2 is minimum 01210 m_LockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn.push_back( IData( pListOfTriVertices[2] ) ); 01211 } 01212 } 01213 else { 01214 if ( len1 < len2 ) { // len1 is minimum 01215 m_LockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn.push_back( IData( pListOfTriVertices[1] ) ); 01216 } 01217 else { // len2 is minimum 01218 m_LockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn.push_back( IData( pListOfTriVertices[2] ) ); 01219 } 01220 } 01221 } 01222 01223 01224 // 01225 // Find and store punctured-out vertex 01226 // 01227 //std::cout << "Find and store punctured-out vertex" << std::endl; 01228 { 01229 Vector3<T> position = pFEMHex->RefToTransformationSupport().GetInverseMatrixTransform() * TrxRBD * CirclePathCtrl.InterpolatedPts[i]; 01230 std::vector< HEVertex<T> * const > pListOfTriVertices = triangles[i]->GetPtrsToVertices(); 01231 T len0 = (position - pListOfTriVertices[0]->GetPosition()).Length(); 01232 T len1 = (position - pListOfTriVertices[1]->GetPosition()).Length(); 01233 T len2 = (position - pListOfTriVertices[2]->GetPosition()).Length(); 01234 if ( len0 < len1 ) { 01235 if ( len0 < len2 ) { // len0 is minimum 01236 m_LockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut.push_back( IData( pListOfTriVertices[0] ) ); 01237 } 01238 else { // len2 is minimum 01239 m_LockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut.push_back( IData( pListOfTriVertices[2] ) ); 01240 } 01241 } 01242 else { 01243 if ( len1 < len2 ) { // len1 is minimum 01244 m_LockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut.push_back( IData( pListOfTriVertices[1] ) ); 01245 } 01246 else { // len2 is minimum 01247 m_LockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut.push_back( IData( pListOfTriVertices[2] ) ); 01248 } 01249 } 01250 } 01251 //*/ 01252 01253 // From high to low ID numbers 01254 //std::cout << "From high to low ID numbers" << std::endl; 01255 for ( int p = outID; p > inID; --p ) { 01256 if ( pFEMHex ) { 01257 01258 //std::cout << "pFEMHex: " << pFEMHex << std::endl; // DEBUG 01259 01260 position = pFEMHex->RefToTransformationSupport().GetInverseMatrixTransform() * TrxRBD * CirclePathCtrl.InterpolatedPts[p]; 01261 01262 //std::cout << "before FindElementThatContainsPointInLocalSpace" << std::endl; 01263 01264 bool bFound = pFEMHex->FindElementThatContainsPointInLocalSpace( position, elementID, gridLocations, coordinates ); 01265 01266 //std::cout << "p: " << p << std::endl; 01267 01268 if ( bFound ) { 01269 //std::cout << "FOUND element: " << elementID << ",\t" << gridLocations << ",\t" << coordinates << "\n"; // DEBUG 01270 01271 // Create the punctured point inside the found element 01272 HEVertex<T> * pNewHEVertex = pFEMHex->AddExtraVertexInLocalSpace( 01273 position, elementID, &pFEMHex->RefToFEMMesh().RefToElement( elementID ), gridLocations, coordinates ); 01274 01275 // push from high to low ID numbers 01276 m_LockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.push_back( IData( pNewHEVertex ) ); 01277 01278 /* 01279 // ADD CONNECTION RIGHT AWAY 01280 //-------------------------- 01281 // Set simulation flags of the HEVertex 01282 pNewHEVertex->SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 01283 // Create the constraint 01284 AdvSimConstraint_VertexIdVsHEVertex<T> * pConstraint = 01285 new AdvSimConstraint_VertexIdVsHEVertex<T>( 01286 sutureThreadID, 01287 pNewHEVertex, 01288 AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio_2, 01289 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA_2, 01290 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB_2, 01291 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA_2, 01292 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB_2, 01293 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA_2, 01294 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB_2 01295 ); 01296 // Set simulation flags of the suture thread point 01297 pSutureThread->GetListOfNodes()[sutureThreadID].SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 01298 pSutureThread->GetListOfNodes()[sutureThreadID].SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 01299 pSutureThread->GetListOfNodes()[sutureThreadID].SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 01300 // Add the constraint 01301 m_ListOfSutureThreadVsFEMModels_ForCirclePath.push_front( InteractionSutureThreadVsFEMModel<T>( pConstraint, ERloc ) ); 01302 pConstraint->RefToSavedPositionB() = pNewHEVertex->GetPosition(); 01303 // Update the suture thread point id 01304 ++sutureThreadID; 01305 // DEBUG 01306 std::cout << "HEVertex's position: " << pNewHEVertex->GetPosition() << "\n"; 01307 std::cout << "pConstraint: " << *pConstraint << std::endl; 01308 std::cout << "pConstraint: " << pConstraint << std::endl; 01309 //*/ 01310 } 01311 } 01312 else if ( pFEMTet ) 01313 { 01314 PrtNotImplementedYet( "RecordAndLockThePuncturePath for FEMTet" ); 01315 } 01316 } 01317 } 01318 m_pCirclePathCtrl = &CirclePathCtrl; 01319 m_LockedCirclePath.NumOfPointsToBeProcessed = m_LockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size(); 01320 m_LockedCirclePath.IsLocked = true; 01321 ++m_uiNumOfLockedCirclePaths; 01322 01323 //std::cout << "End RecordAndLockThePuncturePath" << std::endl; // DEBUG 01324 01325 return true; 01326 } 01327 01328 // Odd number of punctured points 01329 else { 01330 } 01331 01332 return false; 01333 }//END: RecordAndLockThePuncturePath Function 01334 //----------------------------------------------------------------------------- 01335 template <typename T, typename DATA> 01336 bool AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::RecordAndLockThePuncturePath_MoreAccuButUnfinishYet ( 01337 typename ModelSurgicalSutureWithHeadNeedle<T>::CirclePath & CirclePathCtrl, 01338 typename AdvSimSupport_DATA<T,DATA>::ModelIDs const & ListOfModels, 01339 std::vector< ModelElasticRod<T> * > & ListOfModelsBasedOnER, 01340 std::vector< ModelDefBasedOnFEM<T,DATA> * > & ListOfModelsBasedOnFEM, 01341 std::vector< RigidBodyDynamics<T> * > & ListOfModelsBasedOnRBD, 01342 typename AdvSimSupport_DATA<T,DATA>::ListOfInteractions_ERModelVsHEModel & ListOf_ERModelVsHEModelInteractions, 01343 bool & bPunctureIn, 01344 bool & bPunctureOut, 01345 std::vector< unsigned int > & interpolatedPointIDs, 01346 std::vector< HEFace<T> * > & triangles, 01347 std::vector< unsigned int > & listOfFEMModelIDs 01348 ) 01349 { 01350 //PunctureInOutList punctureList; 01351 01352 // The head needle's sharp point has to be penetrated the FEM-based model 01353 if ( interpolatedPointIDs[0] > CirclePathCtrl.IDOfFirstInterpolatedPtBeforeSharpPt ) { 01354 return false; 01355 } 01356 01357 //return false; 01358 01359 unsigned int sutureThreadID = 20; 01360 unsigned int numOfPts = interpolatedPointIDs.size(); 01361 01362 01363 // Even number of punctured points 01364 if ( numOfPts >= 2 && numOfPts % 2 == 0 ) { 01365 01366 Vector3<T> position; 01367 Vector3<unsigned int> gridLocations; 01368 Vector3<T> coordinates; 01369 01370 // For all pairs of two points -- puncture out and in 01371 //for ( int i = numOfPts-1; i > 0; i-=2 ) 01372 // Only the first two points -- puncture out and in 01373 int i = 1; 01374 { 01375 01376 // plus two for skipping the first point after the punctured-in point 01377 // because the point can be close to the punctured in point 01378 // and make the suture thread turn sharply. 01379 int outID = interpolatedPointIDs[i]; 01380 int inID = interpolatedPointIDs[i-1] + 1; 01381 //int inID = interpolatedPointIDs[i-1]; 01382 01383 // Locate FEM-based model 01384 unsigned int elementID; 01385 unsigned int FEMModel = listOfFEMModelIDs[i-1]; 01386 unsigned int FEMloc = ListOfModels[FEMModel].Slot; 01387 01388 /* 01389 // DEBUG 01390 std::cout << "outID: " << outID << std::endl; 01391 std::cout << "inID: " << inID << std::endl; 01392 std::cout << "FEMModel = " << FEMModel << std::endl; 01393 std::cout << "FEMloc = " << FEMloc << std::endl; 01394 //*/ 01395 01396 ModelDefBasedOnFEMHex<T,DATA> * pFEMHex = dynamic_cast< ModelDefBasedOnFEMHex<T,DATA> * >( ListOfModelsBasedOnFEM[FEMloc] ); 01397 ModelDefBasedOnFEMTet<T,DATA> * pFEMTet = dynamic_cast< ModelDefBasedOnFEMTet<T,DATA> * >( ListOfModelsBasedOnFEM[FEMloc] ); 01398 unsigned int ERloc = ListOfModels[m_ERModelAsSutureThread].Slot; 01399 unsigned int RBDloc = ListOfModels[m_RBDModelAsSutureHeadNeedle].Slot; 01400 ModelElasticRod<T> * pSutureThread = ListOfModelsBasedOnER[ERloc]; 01401 01402 RigidBodyDynamics<T> * pHeadNeedle = ListOfModelsBasedOnRBD[RBDloc]; 01403 Matrix4x4<T> TrxRBD( pHeadNeedle->CalMatrixTransform() ); 01404 //Vector3<T> circleCenter = TrxRBD * CirclePathCtrl.Center; 01405 //Vector3<T> circleNormalPlane = TrxRBD * (CirclePathCtrl.NormalPlane + CirclePathCtrl.Center) - circleCenter; 01406 01407 // Store the FEM's location id of this punctured pair (of punctured-in and punctured-out points) 01408 m_LockedCirclePath.ListOfFEMModelBasedOnPuncturedPair.push_back( FEMModel ); 01409 01410 //* 01411 // 01412 // Find and store punctured-in vertex 01413 // 01414 //std::cout << "Find and store punctured-in vertex" << std::endl; 01415 { 01416 Vector3<T> position = pFEMHex->RefToTransformationSupport().GetInverseMatrixTransform() * TrxRBD * CirclePathCtrl.InterpolatedPts[i-1]; 01417 std::vector< HEVertex<T> * const > pListOfTriVertices = triangles[i-1]->GetPtrsToVertices(); 01418 01419 /* 01420 // DEBUG 01421 std::cout << "pListOfTriVertices[0]: " << pListOfTriVertices[0] << std::endl; 01422 std::cout << "pListOfTriVertices[1]: " << pListOfTriVertices[1] << std::endl; 01423 std::cout << "pListOfTriVertices[2]: " << pListOfTriVertices[2] << std::endl; 01424 //*/ 01425 01426 T len0 = (position - pListOfTriVertices[0]->GetPosition()).Length(); 01427 T len1 = (position - pListOfTriVertices[1]->GetPosition()).Length(); 01428 T len2 = (position - pListOfTriVertices[2]->GetPosition()).Length(); 01429 if ( len0 < len1 ) { 01430 if ( len0 < len2 ) { // len0 is minimum 01431 m_LockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn.push_back( IData( pListOfTriVertices[0] ) ); 01432 } 01433 else { // len2 is minimum 01434 m_LockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn.push_back( IData( pListOfTriVertices[2] ) ); 01435 } 01436 } 01437 else { 01438 if ( len1 < len2 ) { // len1 is minimum 01439 m_LockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn.push_back( IData( pListOfTriVertices[1] ) ); 01440 } 01441 else { // len2 is minimum 01442 m_LockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn.push_back( IData( pListOfTriVertices[2] ) ); 01443 } 01444 } 01445 } 01446 01447 01448 // 01449 // Find and store punctured-out vertex 01450 // 01451 //std::cout << "Find and store punctured-out vertex" << std::endl; 01452 { 01453 Vector3<T> position = pFEMHex->RefToTransformationSupport().GetInverseMatrixTransform() * TrxRBD * CirclePathCtrl.InterpolatedPts[i]; 01454 std::vector< HEVertex<T> * const > pListOfTriVertices = triangles[i]->GetPtrsToVertices(); 01455 T len0 = (position - pListOfTriVertices[0]->GetPosition()).Length(); 01456 T len1 = (position - pListOfTriVertices[1]->GetPosition()).Length(); 01457 T len2 = (position - pListOfTriVertices[2]->GetPosition()).Length(); 01458 if ( len0 < len1 ) { 01459 if ( len0 < len2 ) { // len0 is minimum 01460 m_LockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut.push_back( IData( pListOfTriVertices[0] ) ); 01461 } 01462 else { // len2 is minimum 01463 m_LockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut.push_back( IData( pListOfTriVertices[2] ) ); 01464 } 01465 } 01466 else { 01467 if ( len1 < len2 ) { // len1 is minimum 01468 m_LockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut.push_back( IData( pListOfTriVertices[1] ) ); 01469 } 01470 else { // len2 is minimum 01471 m_LockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut.push_back( IData( pListOfTriVertices[2] ) ); 01472 } 01473 } 01474 } 01475 //*/ 01476 01477 // From high to low ID numbers 01478 //std::cout << "From high to low ID numbers" << std::endl; 01479 for ( int p = outID; p > inID; --p ) { 01480 if ( pFEMHex ) { 01481 01482 //std::cout << "pFEMHex: " << pFEMHex << std::endl; // DEBUG 01483 01484 position = pFEMHex->RefToTransformationSupport().GetInverseMatrixTransform() * TrxRBD * CirclePathCtrl.InterpolatedPts[p]; 01485 01486 //std::cout << "before FindElementThatContainsPointInLocalSpace" << std::endl; 01487 01488 bool bFound = pFEMHex->FindElementThatContainsPointInLocalSpace( position, elementID, gridLocations, coordinates ); 01489 01490 //std::cout << "p: " << p << std::endl; 01491 01492 if ( bFound ) { 01493 //std::cout << "FOUND element: " << elementID << ",\t" << gridLocations << ",\t" << coordinates << "\n"; // DEBUG 01494 01495 // Create the punctured point inside the found element 01496 HEVertex<T> * pNewHEVertex = pFEMHex->AddExtraVertexInLocalSpace( 01497 position, elementID, &pFEMHex->RefToFEMMesh().RefToElement( elementID ), gridLocations, coordinates ); 01498 01499 // push from high to low ID numbers 01500 m_LockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.push_back( IData( pNewHEVertex ) ); 01501 01502 /* 01503 // ADD CONNECTION RIGHT AWAY 01504 //-------------------------- 01505 // Set simulation flags of the HEVertex 01506 pNewHEVertex->SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 01507 // Create the constraint 01508 AdvSimConstraint_VertexIdVsHEVertex<T> * pConstraint = 01509 new AdvSimConstraint_VertexIdVsHEVertex<T>( 01510 sutureThreadID, 01511 pNewHEVertex, 01512 AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio_2, 01513 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA_2, 01514 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB_2, 01515 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA_2, 01516 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB_2, 01517 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA_2, 01518 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB_2 01519 ); 01520 // Set simulation flags of the suture thread point 01521 pSutureThread->GetListOfNodes()[sutureThreadID].SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 01522 pSutureThread->GetListOfNodes()[sutureThreadID].SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 01523 pSutureThread->GetListOfNodes()[sutureThreadID].SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 01524 // Add the constraint 01525 m_ListOfSutureThreadVsFEMModels_ForCirclePath.push_front( InteractionSutureThreadVsFEMModel<T>( pConstraint, ERloc ) ); 01526 pConstraint->RefToSavedPositionB() = pNewHEVertex->GetPosition(); 01527 // Update the suture thread point id 01528 ++sutureThreadID; 01529 // DEBUG 01530 std::cout << "HEVertex's position: " << pNewHEVertex->GetPosition() << "\n"; 01531 std::cout << "pConstraint: " << *pConstraint << std::endl; 01532 std::cout << "pConstraint: " << pConstraint << std::endl; 01533 //*/ 01534 } 01535 } 01536 else if ( pFEMTet ) 01537 { 01538 PrtNotImplementedYet( "RecordAndLockThePuncturePath_MoreAccuButUnfinishYet for FEMTet" ); 01539 } 01540 } 01541 } 01542 m_pCirclePathCtrl = &CirclePathCtrl; 01543 m_LockedCirclePath.NumOfPointsToBeProcessed = m_LockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size(); 01544 m_LockedCirclePath.IsLocked = true; 01545 ++m_uiNumOfLockedCirclePaths; 01546 01547 //std::cout << "End RecordAndLockThePuncturePath_MoreAccuButUnfinishYet" << std::endl; // DEBUG 01548 01549 return true; 01550 } 01551 01552 // Odd number of punctured points 01553 else { 01554 } 01555 01556 return false; 01557 }//END: RecordAndLockThePuncturePath_MoreAccuButUnfinishYet Function 01558 //----------------------------------------------------------------------------- 01559 template <typename T, typename DATA> 01560 HEVertex<T> * AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::ProcessLockedPuncturePath ( 01561 LockedCirclePath & TheLockedCirclePath, 01562 typename ModelSurgicalSutureWithHeadNeedle<T>::CirclePath & CirclePathCtrl, 01563 typename AdvSimSupport_DATA<T,DATA>::ModelIDs const & ListOfModels, 01564 typename AdvSimSupport_DATA<T,DATA>::ModelLists & ListOfModelObjects, 01565 typename AdvSimSupport_DATA<T,DATA>::ListOfInteractions_ERModelVsHEModel & ListOf_ERModelVsHEModelInteractions, 01566 bool & bPunctureIn, 01567 bool & bPunctureOut, 01568 bool & bAborted, 01569 bool & bSuccess 01570 ) 01571 { 01572 /* 01573 // DEBUG 01574 std::cout << "TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size(): " << TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size() << "\n"; 01575 std::cout << "TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn.size(): " << TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn.size() << "\n"; 01576 std::cout << "TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut.size(): " << TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut.size() << "\n"; 01577 std::cout << "TheLockedCirclePath.ListOfFEMModelBasedOnPuncturedPair.size(): " << TheLockedCirclePath.ListOfFEMModelBasedOnPuncturedPair.size() << "\n"; 01578 //*/ 01579 01580 std::vector< ModelElasticRod<T> * > & ListOfModelsBasedOnER = ListOfModelObjects.BasedOnER; 01581 std::vector< ModelDefBasedOnFEM<T,DATA> * > & ListOfModelsBasedOnFEM = ListOfModelObjects.BasedOnFEM; 01582 std::vector< RigidBodyDynamics<T> * > & ListOfModelsBasedOnRBD = ListOfModelObjects.BasedOnRBD; 01583 01584 static unsigned int s_NumberOfPuncturePtsLeft; 01585 static unsigned int s_LeadPtID; 01586 unsigned int ERloc = ListOfModels[m_ERModelAsSutureThread].Slot; 01587 unsigned int RBDloc = ListOfModels[m_RBDModelAsSutureHeadNeedle].Slot; 01588 ModelElasticRod<T> * pSutureThread = ListOfModelsBasedOnER[ERloc]; 01589 RigidBodyDynamics<T> * pHeadNeedle = ListOfModelsBasedOnRBD[RBDloc]; 01590 Matrix4x4<T> TrxRBD( pHeadNeedle->CalMatrixTransform() ); 01591 unsigned int FEMModel = TheLockedCirclePath.ListOfFEMModelBasedOnPuncturedPair[0]; 01592 unsigned int FEMloc = ListOfModels[FEMModel].Slot; 01593 ModelDefBasedOnFEM<T,DATA> * pFEM = ListOfModelsBasedOnFEM[FEMloc]; 01594 Matrix4x4<T> TrxTotal( pFEM->RefToTransformationSupport().GetInverseMatrixTransform() * TrxRBD ); 01595 01596 unsigned int idSharpPt = CirclePathCtrl.IDOfFirstInterpolatedPtBeforeSharpPt + 1; 01597 Vector3<T> beforeSharpPt = CirclePathCtrl.InterpolatedPts[idSharpPt-1]; 01598 bool & bIn = TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn[0].bProcessed; 01599 bool & bOut = TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut[0].bProcessed; 01600 01601 // With a higher value, this one makes it easier to puncture the wound. 01602 int distOffsetSize = 1; 01603 T distConstraint = ( CirclePathCtrl.InterpolatedPts[idSharpPt+distOffsetSize] - CirclePathCtrl.InterpolatedPts[idSharpPt-distOffsetSize] ).Length(); // this length is fixed 01604 distConstraint *= 4; 01605 01606 T distAbort = ( CirclePathCtrl.InterpolatedPts[idSharpPt+3] - CirclePathCtrl.InterpolatedPts[idSharpPt-3] ).Length(); // this length is fixed 01607 01608 if ( !bIn ) { // Puncture In 01609 01610 //std::cout << "Try Puncture In" << std::endl; // DEBUG 01611 01612 Vector3<T> point = TrxTotal * beforeSharpPt; 01613 01614 T len2 = (point - TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn[0].pHEVertex->GetPosition()).Length(); 01615 01616 //std::cout << "len2: " << len2 << std::endl; 01617 //std::cout << "distAbort: " << distAbort << std::endl; 01618 01619 // Before puncture out, check for abortion of the path if the head needle is moved to far from the path 01620 if ( len2 > distAbort ) { 01621 bAborted = true; 01622 return NULL; 01623 } 01624 01625 T len1 = (point - TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut[0].pHEVertex->GetPosition()).Length(); 01626 01627 if ( len2 < len1 && len2 < distConstraint ) { 01628 // Set simulation flags of the HEVertex 01629 HEVertex<T> * pHEVertex = TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn[0].pHEVertex; 01630 pHEVertex->SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 01631 // 01632 m_SetOfInteractionAssociatedModelID_ForCirclePath[idSharpPt] = FEMModel; 01633 m_SetOfInteractions_ForCirclePath[idSharpPt].SetPtrToVertexFromModel_2( pHEVertex ); 01634 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceRatio( AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio ); 01635 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceScaleA( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA ); 01636 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceScaleB( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB ); 01637 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceThresholdA( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA ); 01638 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceThresholdB( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB ); 01639 m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusA( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA ); 01640 m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusB( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB ); 01641 // 01642 pHeadNeedle->LockPosition(); 01643 // Set return data 01644 bIn = bPunctureIn = true; 01645 01646 //std::cout << "Done puncture in" << std::endl; 01647 01648 return TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn[0].pHEVertex; 01649 } 01650 else { 01651 return NULL; 01652 } 01653 } 01654 else { // Already Puncture In 01655 if ( !bOut ) { // Not Puncture Out Yet 01656 01657 // Check for abort 01658 T dist = ( TrxRBD*CirclePathCtrl.Center - pFEM->RefToTransformationSupport().GetMatrixTransform()*TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn[0].pHEVertex->GetPosition() ).Length(); 01659 if ( dist > CirclePathCtrl.Radius * 2 ) { 01660 bAborted = true; 01661 return NULL; 01662 } 01663 01664 if ( TheLockedCirclePath.NumOfPointsToBeProcessed > 0 ) { // Puncture Inside 01665 01666 // The HEVertex pointers stored in TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside 01667 // are associatedly stored from high to low ID numbers of path circle. 01668 unsigned int vertexID = TheLockedCirclePath.NumOfPointsToBeProcessed - 1; 01669 01670 //std::cout << "TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size(): " << TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size() << std::endl; 01671 //std::cout << "TheLockedCirclePath.NumOfPointsToBeProcessed: " << TheLockedCirclePath.NumOfPointsToBeProcessed << std::endl; 01672 //std::cout << "vertexID: " << vertexID << std::endl; 01673 01674 if ( vertexID == TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size() - 1 ) { // the first point after the punctured-in point 01675 Vector3<T> point = TrxTotal * beforeSharpPt; 01676 01677 T len2 = (point - TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[vertexID].pHEVertex->GetPosition()).Length(); 01678 01679 //std::cout << "len2: " << len2 << "\n"; 01680 //std::cout << "distAbort: " << distAbort << "\n"; 01681 // Before puncture out, check for abortion of the path if the head needle is moved to far from the path 01682 if ( len2 > distAbort ) { 01683 bAborted = true; 01684 return NULL; 01685 } 01686 01687 T len1 = (point - TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn[0].pHEVertex->GetPosition()).Length(); 01688 01689 if ( len2 < len1 && len2 < distConstraint ) { 01690 // Transfer the first constraint 01691 unsigned int c = idSharpPt - 1; 01692 01693 //std::cout << "idSharpPt: " << idSharpPt << "\n"; 01694 //std::cout << "c: " << c << "\n"; 01695 01696 m_SetOfInteractionAssociatedModelID_ForCirclePath[c] = FEMModel; 01697 m_SetOfInteractions_ForCirclePath[c].SetPtrToVertexFromModel_2( m_SetOfInteractions_ForCirclePath[idSharpPt].GetPtrToVertexFromModel_2() ); 01698 m_SetOfInteractions_ForCirclePath[c].SetForceRatio( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceRatio() ); 01699 m_SetOfInteractions_ForCirclePath[c].SetForceScaleA( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceScaleA() ); 01700 m_SetOfInteractions_ForCirclePath[c].SetForceScaleB( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceScaleB() ); 01701 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdA( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceThresholdA() ); 01702 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdB( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceThresholdB() ); 01703 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusA( m_SetOfInteractions_ForCirclePath[idSharpPt].GetEnforcePositionStatusA() ); 01704 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusB( m_SetOfInteractions_ForCirclePath[idSharpPt].GetEnforcePositionStatusB() ); 01705 01706 // Set simulation flags of the HEVertex 01707 HEVertex<T> * pHEVertex = TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[vertexID].pHEVertex; 01708 pHEVertex->SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 01709 // Set the next constraint 01710 m_SetOfInteractionAssociatedModelID_ForCirclePath[idSharpPt] = FEMModel; 01711 m_SetOfInteractions_ForCirclePath[idSharpPt].SetPtrToVertexFromModel_2( pHEVertex ); 01712 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceRatio( AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio ); 01713 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceScaleA( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA ); 01714 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceScaleB( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB ); 01715 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceThresholdA( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA ); 01716 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceThresholdB( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB ); 01717 m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusA( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA ); 01718 m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusB( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB ); 01719 01720 //m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusB( false ); 01721 // Set return data 01722 --TheLockedCirclePath.NumOfPointsToBeProcessed; 01723 return TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[vertexID].pHEVertex; 01724 } 01725 } 01726 else { // all the points after the first point after the punctured-in point 01727 Vector3<T> point = TrxTotal * beforeSharpPt; 01728 T len1 = (point - TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[vertexID+1].pHEVertex->GetPosition()).Length(); 01729 T len2 = (point - TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[vertexID].pHEVertex->GetPosition()).Length(); 01730 if ( len2 < len1 && len2 < distConstraint ) { 01731 // Transfer all constraints except the first one 01732 unsigned int numberOfTransferredPoints = TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size() - TheLockedCirclePath.NumOfPointsToBeProcessed; 01733 unsigned int c = idSharpPt - numberOfTransferredPoints - 1; 01734 01735 //std::cout << "numberOfTransferredPoints: " << numberOfTransferredPoints << std::endl; 01736 //std::cout << "idSharpPt: " << idSharpPt << std::endl; 01737 //std::cout << "c: " << c << std::endl; 01738 01739 m_SetOfInteractionAssociatedModelID_ForCirclePath[c] = FEMModel; 01740 for ( unsigned int i = 0; i < numberOfTransferredPoints; ++i, ++c ) { 01741 unsigned int d = c + 1; 01742 01743 //std::cout << "transfer from " << d << " to " << c << std::endl; 01744 01745 m_SetOfInteractions_ForCirclePath[c].SetPtrToVertexFromModel_2( m_SetOfInteractions_ForCirclePath[d].GetPtrToVertexFromModel_2() ); 01746 m_SetOfInteractions_ForCirclePath[c].SetForceRatio( m_SetOfInteractions_ForCirclePath[d].GetForceRatio() ); 01747 m_SetOfInteractions_ForCirclePath[c].SetForceScaleA( m_SetOfInteractions_ForCirclePath[d].GetForceScaleA() ); 01748 m_SetOfInteractions_ForCirclePath[c].SetForceScaleB( m_SetOfInteractions_ForCirclePath[d].GetForceScaleB() ); 01749 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdA( m_SetOfInteractions_ForCirclePath[d].GetForceThresholdA() ); 01750 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdB( m_SetOfInteractions_ForCirclePath[d].GetForceThresholdB() ); 01751 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusA( m_SetOfInteractions_ForCirclePath[d].GetEnforcePositionStatusA() ); 01752 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusB( m_SetOfInteractions_ForCirclePath[d].GetEnforcePositionStatusB() ); 01753 } 01754 // Transfer the first constraint 01755 c = idSharpPt - 1; 01756 01757 //std::cout << "Transfer the first point" << std::endl; 01758 //std::cout << "idSharpPt: " << idSharpPt << std::endl; 01759 //std::cout << "c: " << c << std::endl; 01760 //std::cout << "transfer from " << idSharpPt << " to " << c << std::endl; 01761 01762 m_SetOfInteractionAssociatedModelID_ForCirclePath[c] = FEMModel; 01763 m_SetOfInteractions_ForCirclePath[c].SetPtrToVertexFromModel_2( m_SetOfInteractions_ForCirclePath[idSharpPt].GetPtrToVertexFromModel_2() ); 01764 m_SetOfInteractions_ForCirclePath[c].SetForceRatio( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceRatio() ); 01765 m_SetOfInteractions_ForCirclePath[c].SetForceScaleA( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceScaleA() ); 01766 m_SetOfInteractions_ForCirclePath[c].SetForceScaleB( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceScaleB() ); 01767 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdA( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceThresholdA() ); 01768 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdB( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceThresholdB() ); 01769 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusA( m_SetOfInteractions_ForCirclePath[idSharpPt].GetEnforcePositionStatusA() ); 01770 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusB( m_SetOfInteractions_ForCirclePath[idSharpPt].GetEnforcePositionStatusB() ); 01771 // Set simulation flags of the HEVertex 01772 HEVertex<T> * pHEVertex = TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[vertexID].pHEVertex; 01773 pHEVertex->SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 01774 // Set the next constraint 01775 m_SetOfInteractionAssociatedModelID_ForCirclePath[idSharpPt] = FEMModel; 01776 m_SetOfInteractions_ForCirclePath[idSharpPt].SetPtrToVertexFromModel_2( pHEVertex ); 01777 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceRatio( AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio ); 01778 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceScaleA( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA ); 01779 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceScaleB( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB ); 01780 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceThresholdA( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA ); 01781 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceThresholdB( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB ); 01782 m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusA( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA ); 01783 m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusB( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB ); 01784 //m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusB( false ); 01785 // Set return data 01786 --TheLockedCirclePath.NumOfPointsToBeProcessed; 01787 return TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[vertexID].pHEVertex; 01788 } 01789 } 01790 //unsigned int idPtOnNeedle = idSharpPt - ( TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size() - TheLockedCirclePath.NumOfPointsToBeProcessed ); 01791 } 01792 else 01793 { // Pucture Out 01794 Vector3<T> point = TrxTotal * beforeSharpPt; 01795 T len1 = (point - TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[0].pHEVertex->GetPosition()).Length(); 01796 T len2 = (point - TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut[0].pHEVertex->GetPosition()).Length(); 01797 01798 //if ( false ) { // DEBUG 01799 if ( len2 < len1 && len2 < distConstraint ) { 01800 01801 // Transfer all constraints except the first one 01802 unsigned int numberOfTransferredPoints = TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size() - TheLockedCirclePath.NumOfPointsToBeProcessed; 01803 unsigned int c = idSharpPt - numberOfTransferredPoints - 1; 01804 01805 //std::cout << "numberOfTransferredPoints: " << numberOfTransferredPoints << std::endl; 01806 //std::cout << "idSharpPt: " << idSharpPt << std::endl; 01807 //std::cout << "c: " << c << std::endl; 01808 01809 m_SetOfInteractionAssociatedModelID_ForCirclePath[c] = FEMModel; 01810 for ( unsigned int i = 0; i < numberOfTransferredPoints; ++i, ++c ) { 01811 unsigned int d = c + 1; 01812 01813 //std::cout << "transfer from " << d << " to " << c << std::endl; 01814 01815 m_SetOfInteractions_ForCirclePath[c].SetPtrToVertexFromModel_2( m_SetOfInteractions_ForCirclePath[d].GetPtrToVertexFromModel_2() ); 01816 m_SetOfInteractions_ForCirclePath[c].SetForceRatio( m_SetOfInteractions_ForCirclePath[d].GetForceRatio() ); 01817 m_SetOfInteractions_ForCirclePath[c].SetForceScaleA( m_SetOfInteractions_ForCirclePath[d].GetForceScaleA() ); 01818 m_SetOfInteractions_ForCirclePath[c].SetForceScaleB( m_SetOfInteractions_ForCirclePath[d].GetForceScaleB() ); 01819 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdA( m_SetOfInteractions_ForCirclePath[d].GetForceThresholdA() ); 01820 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdB( m_SetOfInteractions_ForCirclePath[d].GetForceThresholdB() ); 01821 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusA( m_SetOfInteractions_ForCirclePath[d].GetEnforcePositionStatusA() ); 01822 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusB( m_SetOfInteractions_ForCirclePath[d].GetEnforcePositionStatusB() ); 01823 } 01824 // Transfer the first constraint 01825 c = idSharpPt - 1; 01826 01827 //std::cout << "Transfer the first point" << std::endl; 01828 //std::cout << "idSharpPt: " << idSharpPt << std::endl; 01829 //std::cout << "c: " << c << std::endl; 01830 //std::cout << "transfer from " << idSharpPt << " to " << c << std::endl; 01831 01832 m_SetOfInteractionAssociatedModelID_ForCirclePath[c] = FEMModel; 01833 m_SetOfInteractions_ForCirclePath[c].SetPtrToVertexFromModel_2( m_SetOfInteractions_ForCirclePath[idSharpPt].GetPtrToVertexFromModel_2() ); 01834 m_SetOfInteractions_ForCirclePath[c].SetForceRatio( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceRatio() ); 01835 m_SetOfInteractions_ForCirclePath[c].SetForceScaleA( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceScaleA() ); 01836 m_SetOfInteractions_ForCirclePath[c].SetForceScaleB( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceScaleB() ); 01837 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdA( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceThresholdA() ); 01838 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdB( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceThresholdB() ); 01839 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusA( m_SetOfInteractions_ForCirclePath[idSharpPt].GetEnforcePositionStatusA() ); 01840 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusB( m_SetOfInteractions_ForCirclePath[idSharpPt].GetEnforcePositionStatusB() ); 01841 // Set simulation flags of the HEVertex 01842 HEVertex<T> * pHEVertex = TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut[0].pHEVertex; 01843 pHEVertex->SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 01844 // Set the next constraint 01845 m_SetOfInteractionAssociatedModelID_ForCirclePath[idSharpPt] = FEMModel; 01846 m_SetOfInteractions_ForCirclePath[idSharpPt].SetPtrToVertexFromModel_2( pHEVertex ); 01847 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceRatio( AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio ); 01848 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceScaleA( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA ); 01849 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceScaleB( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB ); 01850 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceThresholdA( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA ); 01851 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceThresholdB( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB ); 01852 m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusA( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA ); 01853 m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusB( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB ); 01854 //m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusB( false ); 01855 01856 // Set return data 01857 bOut = bPunctureOut = true; 01858 s_NumberOfPuncturePtsLeft = 2 + TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size(); 01859 s_LeadPtID = idSharpPt; 01860 return TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut[0].pHEVertex; 01861 } 01862 else { 01863 return NULL; 01864 } 01865 } 01866 } 01867 else { // Already Puncture Out 01868 01869 // Check for abort 01870 T dist = ( TrxRBD*CirclePathCtrl.Center - pFEM->RefToTransformationSupport().GetMatrixTransform()*TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn[0].pHEVertex->GetPosition() ).Length(); 01871 if ( dist > CirclePathCtrl.Radius * 2 ) { 01872 bAborted = true; 01873 return NULL; 01874 } 01875 01876 if ( s_NumberOfPuncturePtsLeft > 0 ) { // Head needle sliding forward inside the punctured path 01877 01878 //std::cout << "s_NumberOfPuncturePtsLeft: " << s_NumberOfPuncturePtsLeft << std::endl; 01879 //std::cout << "m_SetOfInteractionAssociatedModelID_ForCirclePath[0]: " << m_SetOfInteractionAssociatedModelID_ForCirclePath[0] << std::endl; 01880 01881 if ( m_SetOfInteractionAssociatedModelID_ForCirclePath[0] < 0 ) { // The end of head needle is still outside the puncture path (not going in the puncture path yet) 01882 01883 Vector3<T> point = TrxTotal * CirclePathCtrl.InterpolatedPts[s_LeadPtID-1]; 01884 T len1 = (point - TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[0].pHEVertex->GetPosition()).Length(); 01885 T len2 = (point - TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut[0].pHEVertex->GetPosition()).Length(); 01886 01887 //if ( false ) { // DEBUG 01888 if ( len2 < len1 && len2 < distConstraint ) { 01889 01890 // Transfer all constraints 01891 unsigned int numberOfTransferredPoints = s_NumberOfPuncturePtsLeft; 01892 unsigned int c = s_LeadPtID - numberOfTransferredPoints; 01893 01894 //std::cout << "numberOfTransferredPoints: " << numberOfTransferredPoints << std::endl; 01895 //std::cout << "idSharpPt: " << idSharpPt << std::endl; 01896 //std::cout << "c: " << c << std::endl; 01897 01898 m_SetOfInteractionAssociatedModelID_ForCirclePath[c] = FEMModel; 01899 for ( unsigned int i = 0; i < numberOfTransferredPoints; ++i, ++c ) { 01900 unsigned int d = c + 1; 01901 01902 //std::cout << "transfer from " << d << " to " << c << std::endl; 01903 01904 //* 01905 m_SetOfInteractions_ForCirclePath[c].SetPtrToVertexFromModel_2( m_SetOfInteractions_ForCirclePath[d].GetPtrToVertexFromModel_2() ); 01906 m_SetOfInteractions_ForCirclePath[c].SetForceRatio( m_SetOfInteractions_ForCirclePath[d].GetForceRatio() ); 01907 m_SetOfInteractions_ForCirclePath[c].SetForceScaleA( m_SetOfInteractions_ForCirclePath[d].GetForceScaleA() ); 01908 m_SetOfInteractions_ForCirclePath[c].SetForceScaleB( m_SetOfInteractions_ForCirclePath[d].GetForceScaleB() ); 01909 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdA( m_SetOfInteractions_ForCirclePath[d].GetForceThresholdA() ); 01910 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdB( m_SetOfInteractions_ForCirclePath[d].GetForceThresholdB() ); 01911 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusA( m_SetOfInteractions_ForCirclePath[d].GetEnforcePositionStatusA() ); 01912 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusB( m_SetOfInteractions_ForCirclePath[d].GetEnforcePositionStatusB() ); 01913 //*/ 01914 } 01915 01916 m_SetOfInteractionAssociatedModelID_ForCirclePath[s_LeadPtID] = -1; // clear the lead point's constraint state 01917 01918 // Set return data 01919 --s_LeadPtID; 01920 return NULL; 01921 } 01922 }//END: The end of head needle is still outside the puncture path (not going in the puncture path yet) 01923 else { // The end of head needle is still inside the puncture path 01924 if ( s_LeadPtID > 0 ) { // not the last point out 01925 01926 //std::cout << "CASE: not the last point out" << std::endl; 01927 01928 Vector3<T> point = TrxTotal * CirclePathCtrl.InterpolatedPts[s_LeadPtID-1]; 01929 T len1 = (point - TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[0].pHEVertex->GetPosition()).Length(); 01930 T len2 = (point - TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut[0].pHEVertex->GetPosition()).Length(); 01931 01932 //if ( false ) { // DEBUG 01933 if ( len2 < len1 && len2 < distConstraint ) { 01934 01935 // Transfer all constraints except the first constraint 01936 unsigned int numberOfTransferredPoints = s_NumberOfPuncturePtsLeft; 01937 unsigned int c = s_LeadPtID - numberOfTransferredPoints + 1; 01938 01939 //* 01940 // 01941 // Transfer the first constraint to the suture's thread 01942 // 01943 // Add the constraint of suture thread 01944 // 01945 { 01946 unsigned int ERloc = ListOfModels[m_ERModelAsSutureThread].Slot; 01947 unsigned int ERModelVertexID = 1; 01948 ModelElasticRod<T> * pSutureThread = ListOfModelsBasedOnER[ERloc]; 01949 pSutureThread->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 01950 pSutureThread->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 01951 pSutureThread->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 01952 // 01953 //unsigned int HEloc = m_SetOfInteractionAssociatedModelID_ForIPG[start]; 01954 HEVertex<T> * pVertexHEModel = m_SetOfInteractions_ForCirclePath[c].GetPtrToVertexFromModel_2(); 01955 01956 ++m_NumberOfSutureThreadPointsPuncturedIn; 01957 01958 //std::cout << "m_NumberOfSutureThreadPointsPuncturedIn: " << m_NumberOfSutureThreadPointsPuncturedIn << "\n"; 01959 01960 // Shift all suture points by one (FORWARD ATTACHMENT OF SUTURE'S THREAD WITH FEMMODEL) 01961 std::list< InteractionSutureThreadVsFEMModel<T> >::iterator it = m_ListOfSutureThreadVsFEMModels_ForCirclePath.begin(); 01962 while ( it != m_ListOfSutureThreadVsFEMModels_ForCirclePath.end() ) { 01963 unsigned int ptID = it->pAdvSimConstraint_VertexIdVsHEVertex->GetVertexIDFromModel_1() + 1; 01964 it->pAdvSimConstraint_VertexIdVsHEVertex->SetVertexIDFromModel_1( ptID ); 01965 pSutureThread->GetListOfNodes()[ptID].SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 01966 pSutureThread->GetListOfNodes()[ptID].SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 01967 pSutureThread->GetListOfNodes()[ptID].SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 01968 ++it; 01969 } 01970 01971 AdvSimConstraint_VertexIdVsHEVertex<T> * pConstraint = 01972 new AdvSimConstraint_VertexIdVsHEVertex<T>( 01973 ERModelVertexID, 01974 pVertexHEModel, 01975 AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio_2, 01976 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA_2, 01977 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB_2, 01978 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA_2, 01979 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB_2, 01980 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA_2, 01981 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB_2 01982 ); 01983 pConstraint->RefToSavedPositionB() = pVertexHEModel->GetPosition(); 01984 m_ListOfSutureThreadVsFEMModels_ForCirclePath.push_front( InteractionSutureThreadVsFEMModel<T>( pConstraint, ERloc ) ); 01985 01986 //std::cout << "HEVertex's position: " << pVertexHEModel->GetPosition() << "\n"; 01987 //std::cout << "pConstraint: " << pConstraint << std::endl; 01988 //std::cout << "pConstraint: " << *pConstraint << std::endl; 01989 } 01990 //*/ 01991 01992 //std::cout << "numberOfTransferredPoints: " << numberOfTransferredPoints << std::endl; 01993 //std::cout << "idSharpPt: " << idSharpPt << std::endl; 01994 //std::cout << "s_LeadPtID: " << s_LeadPtID << std::endl; 01995 //std::cout << "c: " << c << std::endl; 01996 01997 m_SetOfInteractionAssociatedModelID_ForCirclePath[c] = FEMModel; 01998 for ( unsigned int i = 1; i < numberOfTransferredPoints; ++i, ++c ) { 01999 unsigned int d = c + 1; 02000 02001 //std::cout << "transfer from " << d << " to " << c << std::endl; 02002 02003 //* 02004 m_SetOfInteractions_ForCirclePath[c].SetPtrToVertexFromModel_2( m_SetOfInteractions_ForCirclePath[d].GetPtrToVertexFromModel_2() ); 02005 m_SetOfInteractions_ForCirclePath[c].SetForceRatio( m_SetOfInteractions_ForCirclePath[d].GetForceRatio() ); 02006 m_SetOfInteractions_ForCirclePath[c].SetForceScaleA( m_SetOfInteractions_ForCirclePath[d].GetForceScaleA() ); 02007 m_SetOfInteractions_ForCirclePath[c].SetForceScaleB( m_SetOfInteractions_ForCirclePath[d].GetForceScaleB() ); 02008 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdA( m_SetOfInteractions_ForCirclePath[d].GetForceThresholdA() ); 02009 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdB( m_SetOfInteractions_ForCirclePath[d].GetForceThresholdB() ); 02010 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusA( m_SetOfInteractions_ForCirclePath[d].GetEnforcePositionStatusA() ); 02011 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusB( m_SetOfInteractions_ForCirclePath[d].GetEnforcePositionStatusB() ); 02012 //*/ 02013 } 02014 02015 m_SetOfInteractionAssociatedModelID_ForCirclePath[s_LeadPtID] = -1; // clear the lead point's constraint state 02016 02017 // Set return data 02018 --s_NumberOfPuncturePtsLeft; 02019 --s_LeadPtID; 02020 return NULL; 02021 } 02022 } 02023 else { // the last point out 02024 //std::cout << "CASE: the last point out" << std::endl; 02025 02026 Vector3<T> point = TrxTotal * CirclePathCtrl.InterpolatedPts[0]; 02027 T len1 = (point - TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[0].pHEVertex->GetPosition()).Length(); 02028 T len2 = (point - TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut[0].pHEVertex->GetPosition()).Length(); 02029 02030 //if ( false ) { // DEBUG 02031 if ( len2 < len1 && len2 < distConstraint ) { 02032 02033 // Transfer all constraints except the first constraint 02034 //unsigned int numberOfTransferredPoints = s_NumberOfPuncturePtsLeft; 02035 //unsigned int c = s_LeadPtID - numberOfTransferredPoints + 1; // will always be zero 02036 unsigned int c = 0; 02037 02038 //std::cout << "numberOfTransferredPoints: " << numberOfTransferredPoints << "\n"; 02039 //std::cout << "c: " << c << "\n"; 02040 02041 //* 02042 // Transfer the first constraint to the suture's thread 02043 // 02044 // Add the constraint of suture thread 02045 // 02046 { 02047 unsigned int ERloc = ListOfModels[m_ERModelAsSutureThread].Slot; 02048 unsigned int ERModelVertexID = 1; 02049 ModelElasticRod<T> * pSutureThread = ListOfModelsBasedOnER[ERloc]; 02050 pSutureThread->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 02051 pSutureThread->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 02052 pSutureThread->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 02053 // 02054 //unsigned int HEloc = m_SetOfInteractionAssociatedModelID_ForIPG[start]; 02055 HEVertex<T> * pVertexHEModel = m_SetOfInteractions_ForCirclePath[c].GetPtrToVertexFromModel_2(); 02056 02057 ++m_NumberOfSutureThreadPointsPuncturedIn; 02058 02059 //std::cout << "m_NumberOfSutureThreadPointsPuncturedIn: " << m_NumberOfSutureThreadPointsPuncturedIn << "\n"; 02060 02061 // Shift all suture points by one (FORWARD ATTACHMENT OF SUTURE'S THREAD WITH FEMMODEL) 02062 std::list< InteractionSutureThreadVsFEMModel<T> >::iterator it = m_ListOfSutureThreadVsFEMModels_ForCirclePath.begin(); 02063 while ( it != m_ListOfSutureThreadVsFEMModels_ForCirclePath.end() ) { 02064 unsigned int ptID = it->pAdvSimConstraint_VertexIdVsHEVertex->GetVertexIDFromModel_1() + 1; 02065 it->pAdvSimConstraint_VertexIdVsHEVertex->SetVertexIDFromModel_1( ptID ); 02066 pSutureThread->GetListOfNodes()[ptID].SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 02067 pSutureThread->GetListOfNodes()[ptID].SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 02068 pSutureThread->GetListOfNodes()[ptID].SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 02069 ++it; 02070 } 02071 02072 AdvSimConstraint_VertexIdVsHEVertex<T> * pConstraint = 02073 new AdvSimConstraint_VertexIdVsHEVertex<T>( 02074 ERModelVertexID, 02075 pVertexHEModel, 02076 AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio_2, 02077 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA_2, 02078 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB_2, 02079 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA_2, 02080 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB_2, 02081 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA_2, 02082 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB_2 02083 ); 02084 pConstraint->RefToSavedPositionB() = pVertexHEModel->GetPosition(); 02085 m_ListOfSutureThreadVsFEMModels_ForCirclePath.push_front( InteractionSutureThreadVsFEMModel<T>( pConstraint, ERloc ) ); 02086 02087 //std::cout << "HEVertex's position: " << pVertexHEModel->GetPosition() << "\n"; 02088 //std::cout << "pConstraint: " << pConstraint << std::endl; 02089 //std::cout << "pConstraint: " << *pConstraint << std::endl; 02090 } 02091 //*/ 02092 02093 m_SetOfInteractionAssociatedModelID_ForCirclePath[0] = -1; // clear the lead point's constraint state 02094 02095 // Set return data 02096 --s_NumberOfPuncturePtsLeft; 02097 //--s_LeadPtID; 02098 02099 // Transfer the constraints to the main constraint data structure 02100 { 02101 TransferConstraintsToMainConstraintDS( 02102 m_ListOfSutureThreadVsFEMModels_ForCirclePath, 02103 ListOf_ERModelVsHEModelInteractions, 02104 ListOfModelObjects, 02105 ListOfModels[m_ERModelAsSutureThread].Slot 02106 ); 02107 02108 bSuccess = true; 02109 02110 // Clear 02111 m_NumberOfSutureThreadPointsPuncturedIn = 0; 02112 m_ListOfSutureThreadVsFEMModels_ForCirclePath.clear(); 02113 } 02114 02115 return NULL; 02116 } 02117 } 02118 }//END: The end of head needle is still inside the puncture path 02119 } 02120 else { // Head needle is completely out from the puncture path 02121 m_LockedCirclePath.Clear(); 02122 } 02123 } 02124 } 02125 return NULL; 02126 }//END: ProcessLockedPuncturePath Function 02127 //----------------------------------------------------------------------------- 02128 template <typename T, typename DATA> 02129 HEVertex<T> * AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::ProcessLockedPuncturePath_MoreAccuButUnfinishYet ( 02130 LockedCirclePath & TheLockedCirclePath, 02131 typename ModelSurgicalSutureWithHeadNeedle<T>::CirclePath & CirclePathCtrl, 02132 typename AdvSimSupport_DATA<T,DATA>::ModelIDs const & ListOfModels, 02133 typename AdvSimSupport_DATA<T,DATA>::ModelLists & ListOfModelObjects, 02134 //std::vector< ModelElasticRod<T> * > & ListOfModelsBasedOnER, //!< list of suture models based on elastic rod (belongs to ER-based model group) 02135 //std::vector< ModelDefBasedOnFEM<T,DATA> * > & ListOfModelsBasedOnFEM, //!< list of deformable models based on FEM (belongs to FEM-based model group) 02136 //std::vector< RigidBodyDynamics<T> * > & ListOfModelsBasedOnRBD, //!< list of models based on rigid body dynamics (belongs to IPG-based model group) 02137 typename AdvSimSupport_DATA<T,DATA>::ListOfInteractions_ERModelVsHEModel & ListOf_ERModelVsHEModelInteractions, 02138 bool & bPunctureIn, 02139 bool & bPunctureOut, 02140 bool & bAborted 02141 ) 02142 { 02143 /* 02144 // DEBUG 02145 std::cout << "TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size(): " << TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size() << "\n"; 02146 std::cout << "TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn.size(): " << TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn.size() << "\n"; 02147 std::cout << "TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut.size(): " << TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut.size() << "\n"; 02148 std::cout << "TheLockedCirclePath.ListOfFEMModelBasedOnPuncturedPair.size(): " << TheLockedCirclePath.ListOfFEMModelBasedOnPuncturedPair.size() << "\n"; 02149 //*/ 02150 02151 std::vector< ModelElasticRod<T> * > & ListOfModelsBasedOnER = ListOfModelObjects.BasedOnER; 02152 std::vector< ModelDefBasedOnFEM<T,DATA> * > & ListOfModelsBasedOnFEM = ListOfModelObjects.BasedOnFEM; 02153 std::vector< RigidBodyDynamics<T> * > & ListOfModelsBasedOnRBD = ListOfModelObjects.BasedOnRBD; 02154 02155 static unsigned int s_NumberOfPuncturePtsLeft; 02156 static unsigned int s_LeadPtID; 02157 unsigned int ERloc = ListOfModels[m_ERModelAsSutureThread].Slot; 02158 unsigned int RBDloc = ListOfModels[m_RBDModelAsSutureHeadNeedle].Slot; 02159 ModelElasticRod<T> * pSutureThread = ListOfModelsBasedOnER[ERloc]; 02160 RigidBodyDynamics<T> * pHeadNeedle = ListOfModelsBasedOnRBD[RBDloc]; 02161 Matrix4x4<T> TrxRBD( pHeadNeedle->CalMatrixTransform() ); 02162 unsigned int FEMModel = TheLockedCirclePath.ListOfFEMModelBasedOnPuncturedPair[0]; 02163 unsigned int FEMloc = ListOfModels[FEMModel].Slot; 02164 ModelDefBasedOnFEM<T,DATA> * pFEM = ListOfModelsBasedOnFEM[FEMloc]; 02165 Matrix4x4<T> TrxTotal( pFEM->RefToTransformationSupport().GetInverseMatrixTransform() * TrxRBD ); 02166 02167 unsigned int idSharpPt = CirclePathCtrl.IDOfFirstInterpolatedPtBeforeSharpPt + 1; 02168 Vector3<T> beforeSharpPt = CirclePathCtrl.InterpolatedPts[idSharpPt-1]; 02169 bool & bIn = TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn[0].bProcessed; 02170 bool & bOut = TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut[0].bProcessed; 02171 02172 // With a higher value, this one makes it easier to puncture the wound. 02173 int distOffsetSize = 1; 02174 T distConstraint = ( CirclePathCtrl.InterpolatedPts[idSharpPt+distOffsetSize] - CirclePathCtrl.InterpolatedPts[idSharpPt-distOffsetSize] ).Length(); // this length is fixed 02175 //distConstraint *= 2; 02176 02177 T distAbort = ( CirclePathCtrl.InterpolatedPts[idSharpPt+3] - CirclePathCtrl.InterpolatedPts[idSharpPt-3] ).Length(); // this length is fixed 02178 02179 if ( !bIn ) { // Puncture In 02180 02181 //std::cout << "Try Puncture In" << std::endl; // DEBUG 02182 02183 Vector3<T> point = TrxTotal * beforeSharpPt; 02184 02185 T len2 = (point - TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn[0].pHEVertex->GetPosition()).Length(); 02186 02187 //std::cout << "len2: " << len2 << std::endl; 02188 //std::cout << "distAbort: " << distAbort << std::endl; 02189 02190 // Before puncture out, check for abortion of the path if the head needle is moved to far from the path 02191 if ( len2 > distAbort ) { 02192 bAborted = true; 02193 return NULL; 02194 } 02195 02196 T len1 = (point - TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut[0].pHEVertex->GetPosition()).Length(); 02197 02198 if ( len2 < len1 && len2 < distConstraint ) { 02199 // Set simulation flags of the HEVertex 02200 HEVertex<T> * pHEVertex = TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn[0].pHEVertex; 02201 pHEVertex->SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 02202 // 02203 m_SetOfInteractionAssociatedModelID_ForCirclePath[idSharpPt] = FEMModel; 02204 m_SetOfInteractions_ForCirclePath[idSharpPt].SetPtrToVertexFromModel_2( pHEVertex ); 02205 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceRatio( AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio ); 02206 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceScaleA( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA ); 02207 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceScaleB( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB ); 02208 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceThresholdA( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA ); 02209 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceThresholdB( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB ); 02210 m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusA( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA ); 02211 m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusB( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB ); 02212 // 02213 pHeadNeedle->LockPosition(); 02214 // Set return data 02215 bIn = bPunctureIn = true; 02216 02217 //std::cout << "Done puncture in" << std::endl; 02218 02219 return TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn[0].pHEVertex; 02220 } 02221 else { 02222 return NULL; 02223 } 02224 } 02225 else { // Already Puncture In 02226 if ( !bOut ) { // Not Puncture Out Yet 02227 if ( TheLockedCirclePath.NumOfPointsToBeProcessed > 0 ) { // Puncture Inside 02228 02229 // The HEVertex pointers stored in TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside 02230 // are associatedly stored from high to low ID numbers of path circle. 02231 unsigned int vertexID = TheLockedCirclePath.NumOfPointsToBeProcessed - 1; 02232 02233 //std::cout << "TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size(): " << TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size() << std::endl; 02234 //std::cout << "TheLockedCirclePath.NumOfPointsToBeProcessed: " << TheLockedCirclePath.NumOfPointsToBeProcessed << std::endl; 02235 //std::cout << "vertexID: " << vertexID << std::endl; 02236 02237 if ( vertexID == TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size() - 1 ) { // the first point after the punctured-in point 02238 Vector3<T> point = TrxTotal * beforeSharpPt; 02239 02240 T len2 = (point - TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[vertexID].pHEVertex->GetPosition()).Length(); 02241 02242 //std::cout << "len2: " << len2 << "\n"; 02243 //std::cout << "distAbort: " << distAbort << "\n"; 02244 // Before puncture out, check for abortion of the path if the head needle is moved to far from the path 02245 if ( len2 > distAbort ) { 02246 bAborted = true; 02247 return NULL; 02248 } 02249 02250 T len1 = (point - TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedIn[0].pHEVertex->GetPosition()).Length(); 02251 02252 if ( len2 < len1 && len2 < distConstraint ) { 02253 // Transfer the first constraint 02254 unsigned int c = idSharpPt - 1; 02255 02256 //std::cout << "idSharpPt: " << idSharpPt << "\n"; 02257 //std::cout << "c: " << c << "\n"; 02258 02259 m_SetOfInteractionAssociatedModelID_ForCirclePath[c] = FEMModel; 02260 m_SetOfInteractions_ForCirclePath[c].SetPtrToVertexFromModel_2( m_SetOfInteractions_ForCirclePath[idSharpPt].GetPtrToVertexFromModel_2() ); 02261 m_SetOfInteractions_ForCirclePath[c].SetForceRatio( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceRatio() ); 02262 m_SetOfInteractions_ForCirclePath[c].SetForceScaleA( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceScaleA() ); 02263 m_SetOfInteractions_ForCirclePath[c].SetForceScaleB( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceScaleB() ); 02264 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdA( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceThresholdA() ); 02265 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdB( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceThresholdB() ); 02266 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusA( m_SetOfInteractions_ForCirclePath[idSharpPt].GetEnforcePositionStatusA() ); 02267 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusB( m_SetOfInteractions_ForCirclePath[idSharpPt].GetEnforcePositionStatusB() ); 02268 02269 // Set simulation flags of the HEVertex 02270 HEVertex<T> * pHEVertex = TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[vertexID].pHEVertex; 02271 pHEVertex->SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 02272 // Set the next constraint 02273 m_SetOfInteractionAssociatedModelID_ForCirclePath[idSharpPt] = FEMModel; 02274 m_SetOfInteractions_ForCirclePath[idSharpPt].SetPtrToVertexFromModel_2( pHEVertex ); 02275 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceRatio( AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio ); 02276 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceScaleA( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA ); 02277 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceScaleB( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB ); 02278 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceThresholdA( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA ); 02279 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceThresholdB( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB ); 02280 m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusA( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA ); 02281 m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusB( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB ); 02282 02283 //m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusB( false ); 02284 // Set return data 02285 --TheLockedCirclePath.NumOfPointsToBeProcessed; 02286 return TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[vertexID].pHEVertex; 02287 } 02288 } 02289 else { // all the points after the first point after the punctured-in point 02290 Vector3<T> point = TrxTotal * beforeSharpPt; 02291 T len1 = (point - TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[vertexID+1].pHEVertex->GetPosition()).Length(); 02292 T len2 = (point - TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[vertexID].pHEVertex->GetPosition()).Length(); 02293 if ( len2 < len1 && len2 < distConstraint ) { 02294 // Transfer all constraints except the first one 02295 unsigned int numberOfTransferredPoints = TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size() - TheLockedCirclePath.NumOfPointsToBeProcessed; 02296 unsigned int c = idSharpPt - numberOfTransferredPoints - 1; 02297 02298 //std::cout << "numberOfTransferredPoints: " << numberOfTransferredPoints << std::endl; 02299 //std::cout << "idSharpPt: " << idSharpPt << std::endl; 02300 //std::cout << "c: " << c << std::endl; 02301 02302 m_SetOfInteractionAssociatedModelID_ForCirclePath[c] = FEMModel; 02303 for ( unsigned int i = 0; i < numberOfTransferredPoints; ++i, ++c ) { 02304 unsigned int d = c + 1; 02305 02306 //std::cout << "transfer from " << d << " to " << c << std::endl; 02307 02308 m_SetOfInteractions_ForCirclePath[c].SetPtrToVertexFromModel_2( m_SetOfInteractions_ForCirclePath[d].GetPtrToVertexFromModel_2() ); 02309 m_SetOfInteractions_ForCirclePath[c].SetForceRatio( m_SetOfInteractions_ForCirclePath[d].GetForceRatio() ); 02310 m_SetOfInteractions_ForCirclePath[c].SetForceScaleA( m_SetOfInteractions_ForCirclePath[d].GetForceScaleA() ); 02311 m_SetOfInteractions_ForCirclePath[c].SetForceScaleB( m_SetOfInteractions_ForCirclePath[d].GetForceScaleB() ); 02312 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdA( m_SetOfInteractions_ForCirclePath[d].GetForceThresholdA() ); 02313 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdB( m_SetOfInteractions_ForCirclePath[d].GetForceThresholdB() ); 02314 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusA( m_SetOfInteractions_ForCirclePath[d].GetEnforcePositionStatusA() ); 02315 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusB( m_SetOfInteractions_ForCirclePath[d].GetEnforcePositionStatusB() ); 02316 } 02317 // Transfer the first constraint 02318 c = idSharpPt - 1; 02319 02320 //std::cout << "Transfer the first point" << std::endl; 02321 //std::cout << "idSharpPt: " << idSharpPt << std::endl; 02322 //std::cout << "c: " << c << std::endl; 02323 //std::cout << "transfer from " << idSharpPt << " to " << c << std::endl; 02324 02325 m_SetOfInteractionAssociatedModelID_ForCirclePath[c] = FEMModel; 02326 m_SetOfInteractions_ForCirclePath[c].SetPtrToVertexFromModel_2( m_SetOfInteractions_ForCirclePath[idSharpPt].GetPtrToVertexFromModel_2() ); 02327 m_SetOfInteractions_ForCirclePath[c].SetForceRatio( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceRatio() ); 02328 m_SetOfInteractions_ForCirclePath[c].SetForceScaleA( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceScaleA() ); 02329 m_SetOfInteractions_ForCirclePath[c].SetForceScaleB( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceScaleB() ); 02330 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdA( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceThresholdA() ); 02331 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdB( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceThresholdB() ); 02332 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusA( m_SetOfInteractions_ForCirclePath[idSharpPt].GetEnforcePositionStatusA() ); 02333 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusB( m_SetOfInteractions_ForCirclePath[idSharpPt].GetEnforcePositionStatusB() ); 02334 // Set simulation flags of the HEVertex 02335 HEVertex<T> * pHEVertex = TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[vertexID].pHEVertex; 02336 pHEVertex->SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 02337 // Set the next constraint 02338 m_SetOfInteractionAssociatedModelID_ForCirclePath[idSharpPt] = FEMModel; 02339 m_SetOfInteractions_ForCirclePath[idSharpPt].SetPtrToVertexFromModel_2( pHEVertex ); 02340 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceRatio( AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio ); 02341 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceScaleA( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA ); 02342 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceScaleB( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB ); 02343 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceThresholdA( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA ); 02344 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceThresholdB( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB ); 02345 m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusA( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA ); 02346 m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusB( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB ); 02347 //m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusB( false ); 02348 // Set return data 02349 --TheLockedCirclePath.NumOfPointsToBeProcessed; 02350 return TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[vertexID].pHEVertex; 02351 } 02352 } 02353 //unsigned int idPtOnNeedle = idSharpPt - ( TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size() - TheLockedCirclePath.NumOfPointsToBeProcessed ); 02354 } 02355 else 02356 { // Pucture Out 02357 Vector3<T> point = TrxTotal * beforeSharpPt; 02358 T len1 = (point - TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[0].pHEVertex->GetPosition()).Length(); 02359 T len2 = (point - TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut[0].pHEVertex->GetPosition()).Length(); 02360 02361 //if ( false ) { // DEBUG 02362 if ( len2 < len1 && len2 < distConstraint ) { 02363 02364 // Transfer all constraints except the first one 02365 unsigned int numberOfTransferredPoints = TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size() - TheLockedCirclePath.NumOfPointsToBeProcessed; 02366 unsigned int c = idSharpPt - numberOfTransferredPoints - 1; 02367 02368 //std::cout << "numberOfTransferredPoints: " << numberOfTransferredPoints << std::endl; 02369 //std::cout << "idSharpPt: " << idSharpPt << std::endl; 02370 //std::cout << "c: " << c << std::endl; 02371 02372 m_SetOfInteractionAssociatedModelID_ForCirclePath[c] = FEMModel; 02373 for ( unsigned int i = 0; i < numberOfTransferredPoints; ++i, ++c ) { 02374 unsigned int d = c + 1; 02375 02376 //std::cout << "transfer from " << d << " to " << c << std::endl; 02377 02378 m_SetOfInteractions_ForCirclePath[c].SetPtrToVertexFromModel_2( m_SetOfInteractions_ForCirclePath[d].GetPtrToVertexFromModel_2() ); 02379 m_SetOfInteractions_ForCirclePath[c].SetForceRatio( m_SetOfInteractions_ForCirclePath[d].GetForceRatio() ); 02380 m_SetOfInteractions_ForCirclePath[c].SetForceScaleA( m_SetOfInteractions_ForCirclePath[d].GetForceScaleA() ); 02381 m_SetOfInteractions_ForCirclePath[c].SetForceScaleB( m_SetOfInteractions_ForCirclePath[d].GetForceScaleB() ); 02382 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdA( m_SetOfInteractions_ForCirclePath[d].GetForceThresholdA() ); 02383 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdB( m_SetOfInteractions_ForCirclePath[d].GetForceThresholdB() ); 02384 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusA( m_SetOfInteractions_ForCirclePath[d].GetEnforcePositionStatusA() ); 02385 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusB( m_SetOfInteractions_ForCirclePath[d].GetEnforcePositionStatusB() ); 02386 } 02387 // Transfer the first constraint 02388 c = idSharpPt - 1; 02389 02390 //std::cout << "Transfer the first point" << std::endl; 02391 //std::cout << "idSharpPt: " << idSharpPt << std::endl; 02392 //std::cout << "c: " << c << std::endl; 02393 //std::cout << "transfer from " << idSharpPt << " to " << c << std::endl; 02394 02395 m_SetOfInteractionAssociatedModelID_ForCirclePath[c] = FEMModel; 02396 m_SetOfInteractions_ForCirclePath[c].SetPtrToVertexFromModel_2( m_SetOfInteractions_ForCirclePath[idSharpPt].GetPtrToVertexFromModel_2() ); 02397 m_SetOfInteractions_ForCirclePath[c].SetForceRatio( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceRatio() ); 02398 m_SetOfInteractions_ForCirclePath[c].SetForceScaleA( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceScaleA() ); 02399 m_SetOfInteractions_ForCirclePath[c].SetForceScaleB( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceScaleB() ); 02400 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdA( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceThresholdA() ); 02401 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdB( m_SetOfInteractions_ForCirclePath[idSharpPt].GetForceThresholdB() ); 02402 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusA( m_SetOfInteractions_ForCirclePath[idSharpPt].GetEnforcePositionStatusA() ); 02403 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusB( m_SetOfInteractions_ForCirclePath[idSharpPt].GetEnforcePositionStatusB() ); 02404 // Set simulation flags of the HEVertex 02405 HEVertex<T> * pHEVertex = TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut[0].pHEVertex; 02406 pHEVertex->SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 02407 // Set the next constraint 02408 m_SetOfInteractionAssociatedModelID_ForCirclePath[idSharpPt] = FEMModel; 02409 m_SetOfInteractions_ForCirclePath[idSharpPt].SetPtrToVertexFromModel_2( pHEVertex ); 02410 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceRatio( AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio ); 02411 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceScaleA( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA ); 02412 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceScaleB( AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB ); 02413 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceThresholdA( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA ); 02414 m_SetOfInteractions_ForCirclePath[idSharpPt].SetForceThresholdB( AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB ); 02415 m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusA( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA ); 02416 m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusB( AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB ); 02417 //m_SetOfInteractions_ForCirclePath[idSharpPt].SetEnforcePositionStatusB( false ); 02418 02419 // Set return data 02420 bOut = bPunctureOut = true; 02421 s_NumberOfPuncturePtsLeft = 2 + TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside.size(); 02422 s_LeadPtID = idSharpPt; 02423 return TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut[0].pHEVertex; 02424 } 02425 else { 02426 return NULL; 02427 } 02428 } 02429 } 02430 else { // Already Puncture Out 02431 if ( s_NumberOfPuncturePtsLeft > 0 ) { // Head needle sliding forward inside the punctured path 02432 02433 //std::cout << "s_NumberOfPuncturePtsLeft: " << s_NumberOfPuncturePtsLeft << std::endl; 02434 //std::cout << "m_SetOfInteractionAssociatedModelID_ForCirclePath[0]: " << m_SetOfInteractionAssociatedModelID_ForCirclePath[0] << std::endl; 02435 02436 if ( m_SetOfInteractionAssociatedModelID_ForCirclePath[0] < 0 ) { // The end of head needle is still outside the puncture path (not going in the puncture path yet) 02437 02438 Vector3<T> point = TrxTotal * CirclePathCtrl.InterpolatedPts[s_LeadPtID-1]; 02439 T len1 = (point - TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[0].pHEVertex->GetPosition()).Length(); 02440 T len2 = (point - TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut[0].pHEVertex->GetPosition()).Length(); 02441 02442 //if ( false ) { // DEBUG 02443 if ( len2 < len1 && len2 < distConstraint ) { 02444 02445 // Transfer all constraints 02446 unsigned int numberOfTransferredPoints = s_NumberOfPuncturePtsLeft; 02447 unsigned int c = s_LeadPtID - numberOfTransferredPoints; 02448 02449 //std::cout << "numberOfTransferredPoints: " << numberOfTransferredPoints << std::endl; 02450 //std::cout << "idSharpPt: " << idSharpPt << std::endl; 02451 //std::cout << "c: " << c << std::endl; 02452 02453 m_SetOfInteractionAssociatedModelID_ForCirclePath[c] = FEMModel; 02454 for ( unsigned int i = 0; i < numberOfTransferredPoints; ++i, ++c ) { 02455 unsigned int d = c + 1; 02456 02457 //std::cout << "transfer from " << d << " to " << c << std::endl; 02458 02459 //* 02460 m_SetOfInteractions_ForCirclePath[c].SetPtrToVertexFromModel_2( m_SetOfInteractions_ForCirclePath[d].GetPtrToVertexFromModel_2() ); 02461 m_SetOfInteractions_ForCirclePath[c].SetForceRatio( m_SetOfInteractions_ForCirclePath[d].GetForceRatio() ); 02462 m_SetOfInteractions_ForCirclePath[c].SetForceScaleA( m_SetOfInteractions_ForCirclePath[d].GetForceScaleA() ); 02463 m_SetOfInteractions_ForCirclePath[c].SetForceScaleB( m_SetOfInteractions_ForCirclePath[d].GetForceScaleB() ); 02464 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdA( m_SetOfInteractions_ForCirclePath[d].GetForceThresholdA() ); 02465 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdB( m_SetOfInteractions_ForCirclePath[d].GetForceThresholdB() ); 02466 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusA( m_SetOfInteractions_ForCirclePath[d].GetEnforcePositionStatusA() ); 02467 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusB( m_SetOfInteractions_ForCirclePath[d].GetEnforcePositionStatusB() ); 02468 //*/ 02469 } 02470 02471 m_SetOfInteractionAssociatedModelID_ForCirclePath[s_LeadPtID] = -1; // clear the lead point's constraint state 02472 02473 // Set return data 02474 --s_LeadPtID; 02475 return NULL; 02476 } 02477 }//END: The end of head needle is still outside the puncture path (not going in the puncture path yet) 02478 else { // The end of head needle is still inside the puncture path 02479 if ( s_LeadPtID > 0 ) { // not the last point out 02480 02481 //std::cout << "CASE: not the last point out" << std::endl; 02482 02483 Vector3<T> point = TrxTotal * CirclePathCtrl.InterpolatedPts[s_LeadPtID-1]; 02484 T len1 = (point - TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[0].pHEVertex->GetPosition()).Length(); 02485 T len2 = (point - TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut[0].pHEVertex->GetPosition()).Length(); 02486 02487 //if ( false ) { // DEBUG 02488 if ( len2 < len1 && len2 < distConstraint ) { 02489 02490 // Transfer all constraints except the first constraint 02491 unsigned int numberOfTransferredPoints = s_NumberOfPuncturePtsLeft; 02492 unsigned int c = s_LeadPtID - numberOfTransferredPoints + 1; 02493 02494 //* 02495 // 02496 // Transfer the first constraint to the suture's thread 02497 // 02498 // Add the constraint of suture thread 02499 // 02500 { 02501 unsigned int ERloc = ListOfModels[m_ERModelAsSutureThread].Slot; 02502 unsigned int ERModelVertexID = 1; 02503 ModelElasticRod<T> * pSutureThread = ListOfModelsBasedOnER[ERloc]; 02504 pSutureThread->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 02505 pSutureThread->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 02506 pSutureThread->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 02507 // 02508 //unsigned int HEloc = m_SetOfInteractionAssociatedModelID_ForIPG[start]; 02509 HEVertex<T> * pVertexHEModel = m_SetOfInteractions_ForCirclePath[c].GetPtrToVertexFromModel_2(); 02510 02511 ++m_NumberOfSutureThreadPointsPuncturedIn; 02512 02513 //std::cout << "m_NumberOfSutureThreadPointsPuncturedIn: " << m_NumberOfSutureThreadPointsPuncturedIn << "\n"; 02514 02515 // Shift all suture points by one (FORWARD ATTACHMENT OF SUTURE'S THREAD WITH FEMMODEL) 02516 std::list< InteractionSutureThreadVsFEMModel<T> >::iterator it = m_ListOfSutureThreadVsFEMModels_ForCirclePath.begin(); 02517 while ( it != m_ListOfSutureThreadVsFEMModels_ForCirclePath.end() ) { 02518 unsigned int ptID = it->pAdvSimConstraint_VertexIdVsHEVertex->GetVertexIDFromModel_1() + 1; 02519 it->pAdvSimConstraint_VertexIdVsHEVertex->SetVertexIDFromModel_1( ptID ); 02520 pSutureThread->GetListOfNodes()[ptID].SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 02521 pSutureThread->GetListOfNodes()[ptID].SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 02522 pSutureThread->GetListOfNodes()[ptID].SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 02523 ++it; 02524 } 02525 02526 AdvSimConstraint_VertexIdVsHEVertex<T> * pConstraint = 02527 new AdvSimConstraint_VertexIdVsHEVertex<T>( 02528 ERModelVertexID, 02529 pVertexHEModel, 02530 AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio_2, 02531 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA_2, 02532 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB_2, 02533 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA_2, 02534 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB_2, 02535 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA_2, 02536 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB_2 02537 ); 02538 pConstraint->RefToSavedPositionB() = pVertexHEModel->GetPosition(); 02539 m_ListOfSutureThreadVsFEMModels_ForCirclePath.push_front( InteractionSutureThreadVsFEMModel<T>( pConstraint, ERloc ) ); 02540 02541 //std::cout << "HEVertex's position: " << pVertexHEModel->GetPosition() << "\n"; 02542 //std::cout << "pConstraint: " << pConstraint << std::endl; 02543 //std::cout << "pConstraint: " << *pConstraint << std::endl; 02544 } 02545 //*/ 02546 02547 //std::cout << "numberOfTransferredPoints: " << numberOfTransferredPoints << std::endl; 02548 //std::cout << "idSharpPt: " << idSharpPt << std::endl; 02549 //std::cout << "s_LeadPtID: " << s_LeadPtID << std::endl; 02550 //std::cout << "c: " << c << std::endl; 02551 02552 m_SetOfInteractionAssociatedModelID_ForCirclePath[c] = FEMModel; 02553 for ( unsigned int i = 1; i < numberOfTransferredPoints; ++i, ++c ) { 02554 unsigned int d = c + 1; 02555 02556 //std::cout << "transfer from " << d << " to " << c << std::endl; 02557 02558 //* 02559 m_SetOfInteractions_ForCirclePath[c].SetPtrToVertexFromModel_2( m_SetOfInteractions_ForCirclePath[d].GetPtrToVertexFromModel_2() ); 02560 m_SetOfInteractions_ForCirclePath[c].SetForceRatio( m_SetOfInteractions_ForCirclePath[d].GetForceRatio() ); 02561 m_SetOfInteractions_ForCirclePath[c].SetForceScaleA( m_SetOfInteractions_ForCirclePath[d].GetForceScaleA() ); 02562 m_SetOfInteractions_ForCirclePath[c].SetForceScaleB( m_SetOfInteractions_ForCirclePath[d].GetForceScaleB() ); 02563 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdA( m_SetOfInteractions_ForCirclePath[d].GetForceThresholdA() ); 02564 m_SetOfInteractions_ForCirclePath[c].SetForceThresholdB( m_SetOfInteractions_ForCirclePath[d].GetForceThresholdB() ); 02565 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusA( m_SetOfInteractions_ForCirclePath[d].GetEnforcePositionStatusA() ); 02566 m_SetOfInteractions_ForCirclePath[c].SetEnforcePositionStatusB( m_SetOfInteractions_ForCirclePath[d].GetEnforcePositionStatusB() ); 02567 //*/ 02568 } 02569 02570 m_SetOfInteractionAssociatedModelID_ForCirclePath[s_LeadPtID] = -1; // clear the lead point's constraint state 02571 02572 // Set return data 02573 --s_NumberOfPuncturePtsLeft; 02574 --s_LeadPtID; 02575 return NULL; 02576 } 02577 } 02578 else { // the last point out 02579 //std::cout << "CASE: the last point out" << std::endl; 02580 02581 Vector3<T> point = TrxTotal * CirclePathCtrl.InterpolatedPts[0]; 02582 T len1 = (point - TheLockedCirclePath.ListOfFEMMeshPuncturedVerticesInside[0].pHEVertex->GetPosition()).Length(); 02583 T len2 = (point - TheLockedCirclePath.ListOfFEMMeshSurfaceVertexPuncturedOut[0].pHEVertex->GetPosition()).Length(); 02584 02585 //if ( false ) { // DEBUG 02586 if ( len2 < len1 && len2 < distConstraint ) { 02587 02588 // Transfer all constraints except the first constraint 02589 //unsigned int numberOfTransferredPoints = s_NumberOfPuncturePtsLeft; 02590 //unsigned int c = s_LeadPtID - numberOfTransferredPoints + 1; // will always be zero 02591 unsigned int c = 0; 02592 02593 //std::cout << "numberOfTransferredPoints: " << numberOfTransferredPoints << "\n"; 02594 //std::cout << "c: " << c << "\n"; 02595 02596 //* 02597 // Transfer the first constraint to the suture's thread 02598 // 02599 // Add the constraint of suture thread 02600 // 02601 { 02602 unsigned int ERloc = ListOfModels[m_ERModelAsSutureThread].Slot; 02603 unsigned int ERModelVertexID = 1; 02604 ModelElasticRod<T> * pSutureThread = ListOfModelsBasedOnER[ERloc]; 02605 pSutureThread->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 02606 pSutureThread->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 02607 pSutureThread->GetListOfNodes()[ERModelVertexID].SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 02608 // 02609 //unsigned int HEloc = m_SetOfInteractionAssociatedModelID_ForIPG[start]; 02610 HEVertex<T> * pVertexHEModel = m_SetOfInteractions_ForCirclePath[c].GetPtrToVertexFromModel_2(); 02611 02612 ++m_NumberOfSutureThreadPointsPuncturedIn; 02613 02614 //std::cout << "m_NumberOfSutureThreadPointsPuncturedIn: " << m_NumberOfSutureThreadPointsPuncturedIn << "\n"; 02615 02616 // Shift all suture points by one (FORWARD ATTACHMENT OF SUTURE'S THREAD WITH FEMMODEL) 02617 std::list< InteractionSutureThreadVsFEMModel<T> >::iterator it = m_ListOfSutureThreadVsFEMModels_ForCirclePath.begin(); 02618 while ( it != m_ListOfSutureThreadVsFEMModels_ForCirclePath.end() ) { 02619 unsigned int ptID = it->pAdvSimConstraint_VertexIdVsHEVertex->GetVertexIDFromModel_1() + 1; 02620 it->pAdvSimConstraint_VertexIdVsHEVertex->SetVertexIDFromModel_1( ptID ); 02621 pSutureThread->GetListOfNodes()[ptID].SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE ); 02622 pSutureThread->GetListOfNodes()[ptID].SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED ); 02623 pSutureThread->GetListOfNodes()[ptID].SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ); 02624 ++it; 02625 } 02626 02627 AdvSimConstraint_VertexIdVsHEVertex<T> * pConstraint = 02628 new AdvSimConstraint_VertexIdVsHEVertex<T>( 02629 ERModelVertexID, 02630 pVertexHEModel, 02631 AdvSimSupport_DATA<T,DATA>::STATE_ForceRatio_2, 02632 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleA_2, 02633 AdvSimSupport_DATA<T,DATA>::STATE_ForceScaleB_2, 02634 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdA_2, 02635 AdvSimSupport_DATA<T,DATA>::STATE_ForceThresholdB_2, 02636 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionA_2, 02637 AdvSimSupport_DATA<T,DATA>::STATE_EnforcePositionB_2 02638 ); 02639 pConstraint->RefToSavedPositionB() = pVertexHEModel->GetPosition(); 02640 m_ListOfSutureThreadVsFEMModels_ForCirclePath.push_front( InteractionSutureThreadVsFEMModel<T>( pConstraint, ERloc ) ); 02641 02642 //std::cout << "HEVertex's position: " << pVertexHEModel->GetPosition() << "\n"; 02643 //std::cout << "pConstraint: " << pConstraint << std::endl; 02644 //std::cout << "pConstraint: " << *pConstraint << std::endl; 02645 } 02646 //*/ 02647 02648 m_SetOfInteractionAssociatedModelID_ForCirclePath[0] = -1; // clear the lead point's constraint state 02649 02650 // Set return data 02651 --s_NumberOfPuncturePtsLeft; 02652 //--s_LeadPtID; 02653 02654 // Transfer the constraints to the main constraint data structure 02655 { 02656 TransferConstraintsToMainConstraintDS( 02657 m_ListOfSutureThreadVsFEMModels_ForCirclePath, 02658 ListOf_ERModelVsHEModelInteractions, 02659 ListOfModelObjects, 02660 ListOfModels[m_ERModelAsSutureThread].Slot 02661 ); 02662 02663 // Clear 02664 m_NumberOfSutureThreadPointsPuncturedIn = 0; 02665 m_ListOfSutureThreadVsFEMModels_ForCirclePath.clear(); 02666 } 02667 02668 return NULL; 02669 } 02670 } 02671 }//END: The end of head needle is still inside the puncture path 02672 } 02673 else { // Head needle is completely out from the puncture path 02674 m_LockedCirclePath.Clear(); 02675 } 02676 } 02677 } 02678 return NULL; 02679 }//END: ProcessLockedPuncturePath_MoreAccuButUnfinishYet Function 02680 //----------------------------------------------------------------------------- 02681 02682 02683 //----------------------------------------------------------------------------- 02684 template <typename T, typename DATA> 02685 void AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::TransferConstraintsToMainConstraintDS ( 02686 std::list< InteractionSutureThreadVsFEMModel<T> > & listOfConstraints, 02687 typename AdvSimSupport_DATA<T,DATA>::ListOfInteractions_ERModelVsHEModel & ListOf_ERModelVsHEModelInteractions, 02688 typename AdvSimSupport_DATA<T,DATA>::ModelLists & ListOfModelObjects, 02689 unsigned int ERloc 02690 ) 02691 { 02692 std::list< InteractionSutureThreadVsFEMModel<T> >::iterator it = listOfConstraints.begin(); 02693 while ( it != listOfConstraints.end() ) { 02694 02695 // For controlling forward/backward movement of suture 02696 unsigned int ERModelVertexID = it->pAdvSimConstraint_VertexIdVsHEVertex->GetVertexIDFromModel_1(); 02697 ListOfModelObjects.AllERNodePropsList[ERloc][ERModelVertexID].IsConstrained = true; 02698 02699 //ListOf_ERModelVsHEModelInteractions[ERloc][it->FEMloc].push_back( it->pAdvSimConstraint_VertexIdVsHEVertex ); 02700 02701 // Insert to the list (sorted from low to high) 02702 unsigned int size = ListOf_ERModelVsHEModelInteractions[ERloc][it->FEMloc].Constraints.size(); 02703 if ( size == 0 ) { 02704 ListOf_ERModelVsHEModelInteractions[ERloc][it->FEMloc].Constraints.push_back( it->pAdvSimConstraint_VertexIdVsHEVertex ); 02705 } 02706 else { 02707 std::vector< AdvSimConstraint_VertexIdVsHEVertex<T> * > & List = ListOf_ERModelVsHEModelInteractions[ERloc][it->FEMloc].Constraints; 02708 std::vector< AdvSimConstraint_VertexIdVsHEVertex<T> * >::iterator itMDS = List.begin(); 02709 02710 //----------------------------------------------------------- 02711 // (binary) insert from low to high 02712 unsigned int start = 0, middle = size/2, end = size; 02713 while ( end - start > 0 ) { 02714 02715 if ( List[middle]->GetVertexIDFromModel_1() > ERModelVertexID ) { 02716 end = middle; 02717 middle = (end - start)/2 + start; 02718 } 02719 else if ( List[middle]->GetVertexIDFromModel_1() < ERModelVertexID ) { 02720 start = middle+1; 02721 middle = (end - start)/2 + start; 02722 } 02723 else { 02724 break; 02725 } 02726 } 02727 List.insert( itMDS + middle, it->pAdvSimConstraint_VertexIdVsHEVertex ); 02728 //----------------------------------------------------------- 02729 02730 /* 02731 // DEBUG 02732 std::cout << "Sorted List:"; 02733 for ( itMDS = List.begin(); itMDS != List.end(); ++itMDS ) { 02734 std::cout << "\t" << (*itMDS)->GetVertexIDFromModel_1(); 02735 } 02736 std::cout << "\n"; 02737 //*/ 02738 } 02739 02740 it->pAdvSimConstraint_VertexIdVsHEVertex->RefToSavedPositionB() = it->pAdvSimConstraint_VertexIdVsHEVertex->GetPtrToVertexFromModel_2()->GetPosition(); 02741 ++it; 02742 } 02743 } 02744 //----------------------------------------------------------------------------- 02745 02746 02747 //============================================================================= 02748 // FOR IPG 02749 //----------------------------------------------------------------------------- 02750 template <typename T, typename DATA> 02751 void AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::EnforceAllConstraints_ForIPG ( 02752 typename AdvSimSupport_DATA<T,DATA>::ModelIDs const & ListOfModels, 02753 std::vector< ModelElasticRod<T> * > & ListOfModelsBasedOnER, 02754 std::vector< ModelDefBasedOnFEM<T,DATA> * > & ListOfModelsBasedOnFEM, 02755 std::vector< RigidBodyDynamics<T> * > & ListOfModelsBasedOnRBD, 02756 typename AdvSimSupport_DATA<T,DATA>::ListOfPointForces & ListOfPointForces 02757 ) 02758 { 02759 // The head needle model type must be MODEL_BASED_ON_IPG_RBD 02760 Enum::ModelType HeadNeedleModelType = ListOfModels[m_RBDModelAsSutureHeadNeedle].Type; 02761 if ( HeadNeedleModelType != Enum::MODEL_BASED_ON_IPG_RBD ) return; 02762 02763 // Get the head needle's transformation matrix and its inverse 02764 unsigned int locRBD = ListOfModels[m_RBDModelAsSutureHeadNeedle].Slot; 02765 RigidBodyDynamics<T> * pRBDModel = ListOfModelsBasedOnRBD[locRBD]; 02766 Matrix4x4<T> TrxModelRBD = pRBDModel->CalMatrixTransform(); 02767 Matrix4x4<T> InvTrxModelRBD = TrxModelRBD.GetInverse(); 02768 02769 // Suture's head needle with FEM-based models 02770 for ( unsigned int i = 0; i < m_size_ForIPG; ++i ) { 02771 int modelID = m_SetOfInteractionAssociatedModelID_ForIPG[i]; 02772 // A model is linked to the i(th) constraint 02773 if ( modelID >= 0 ) { 02774 Enum::ModelType modelType = ListOfModels[modelID].Type; 02775 if ( modelType == Enum::MODEL_BASED_ON_FEM ) { 02776 // MODEL_BASED_ON_IPG_RBD vs MODEL_BASED_ON_FEM at the i(th) constraint 02777 EnforceAConstraintOfSutureHeadNeedleTiedWithFEMModel_ForIPG( 02778 i, TrxModelRBD, InvTrxModelRBD, pRBDModel, 02779 ListOfModelsBasedOnFEM[ListOfModels[modelID].Slot], 02780 ListOfPointForces 02781 ); 02782 } 02783 } 02784 } 02785 02786 // Suture's thread with FEM-based models 02787 std::list< InteractionSutureThreadVsFEMModel<T> >::iterator it = m_ListOfSutureThreadVsFEMModels_ForIPG.begin(); 02788 while ( it != m_ListOfSutureThreadVsFEMModels_ForIPG.end() ) { 02789 EnforceAConstraintOfSutureThreadTiedWithFEMModel_ForIPG( 02790 it->pAdvSimConstraint_VertexIdVsHEVertex, 02791 ListOfModelsBasedOnER[ListOfModels[m_ERModelAsSutureThread].Slot], 02792 ListOfModelsBasedOnFEM[it->FEMloc], 02793 ListOfPointForces 02794 ); 02795 ++it; 02796 } 02797 } 02798 //----------------------------------------------------------------------------- 02799 template <typename T, typename DATA> 02800 void AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::ResetPunctureController () 02801 { 02802 m_PunctureController.Reset(); 02803 } 02804 //----------------------------------------------------------------------------- 02805 template <typename T, typename DATA> 02806 std::string AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::GetPunctureControllerCurrentInfo () 02807 { 02808 return m_PunctureController.StrInfo; 02809 } 02810 //----------------------------------------------------------------------------- 02811 template <typename T, typename DATA> 02812 void AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::ClearAllConstraints_ForIPG () 02813 { 02814 // Since all values of each constraint will be set explicitly, except VertexId with invariant. 02815 // And m_SetOfInteractionAssociatedModelID less than zero signifies that the constraint is voided or cancelled. 02816 // Therefore, only m_SetOfInteractionAssociatedModelID needs to be cleared. 02817 for ( unsigned int i = 0; i < m_size_ForIPG; ++i ) { 02818 m_SetOfInteractionAssociatedModelID_ForIPG[i] = -1; 02819 } 02820 02821 // Unlock the head needle 02822 //ListOfModelsBasedOnRBD[RBDloc].UnlockHeadNeedlePosition(); 02823 02824 m_NumberOfSutureThreadPointsPuncturedIn = 0; 02825 m_NumberOfSutureHeadNeedlePointsPuncturedIn = 0; 02826 } 02827 //----------------------------------------------------------------------------- 02828 template <typename T, typename DATA> 02829 void AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::EnforceAllConsistentPositions_ForIPG ( 02830 typename AdvSimSupport_DATA<T,DATA>::ModelIDs const & ListOfModels, 02831 std::vector< ModelElasticRod<T> * > & ListOfModelsBasedOnER, 02832 std::vector< ModelDefBasedOnFEM<T,DATA> * > & ListOfModelsBasedOnFEM, 02833 std::vector< RigidBodyDynamics<T> * > & ListOfModelsBasedOnRBD 02834 ) 02835 { 02836 // Suture's head needle with FEM-based models 02837 SetOfInteractions::iterator it = m_SetOfInteractions_ForIPG.begin(); 02838 int i = 0; 02839 while ( it != m_SetOfInteractions_ForIPG.end() ) { 02840 if ( m_SetOfInteractionAssociatedModelID_ForIPG[i] >= 0 ) { 02841 if ( it->GetEnforcePositionStatusB() ) { 02842 //it->RefToSavedPositionA() = it->GetPtrToVertexFromModel_1()->GetProtectedPosition(); 02843 it->RefToSavedPositionB() = it->GetPtrToVertexFromModel_2()->GetProtectedPosition(); 02844 //it->GetPtrToVertexFromModel_1()->GetProtectedPosition() = it->RefToTargetPositionA(); 02845 02846 #ifdef TAPs_USE_VERTEX_POSITION_AVERAGING 02847 std::vector< HEVertex<T> * > firstRing = it->GetPtrToVertexFromModel_2()->FindTheFirstRingOfVertices(); 02848 unsigned int size = firstRing.size(); 02849 if ( size > 0 ) { 02850 Vector3<T> avgPos; 02851 for ( unsigned int i = 0; i < size; ++i ) { 02852 avgPos += firstRing[i]->GetPosition(); 02853 } 02854 T scale = size / T(2); 02855 avgPos = ( avgPos + it->RefToTargetPositionB()*scale ) / ( size + scale ); 02856 it->GetPtrToVertexFromModel_2()->GetProtectedPosition() = avgPos; 02857 for ( unsigned int i = 0; i < size; ++i ) { 02858 firstRing[i]->GetProtectedPosition() = ( firstRing[i]->GetPosition() + avgPos ) / 2; 02859 } 02860 } 02861 else { 02862 it->GetPtrToVertexFromModel_2()->GetProtectedPosition() = it->RefToTargetPositionB(); 02863 } 02864 #else //TAPs_USE_VERTEX_POSITION_AVERAGING 02865 it->GetPtrToVertexFromModel_2()->GetProtectedPosition() = it->RefToTargetPositionB(); 02866 #endif//TAPs_USE_VERTEX_POSITION_AVERAGING 02867 } 02868 } 02869 ++it; 02870 ++i; 02871 } 02872 02873 // Suture's thread with FEM-based models 02874 std::list< InteractionSutureThreadVsFEMModel<T> >::iterator itc = m_ListOfSutureThreadVsFEMModels_ForIPG.begin(); 02875 while ( itc != m_ListOfSutureThreadVsFEMModels_ForIPG.end() ) { 02876 EnforceAConsistentPositionSutureThreadTiedWithFEMModel_ForIPG( 02877 itc->pAdvSimConstraint_VertexIdVsHEVertex, 02878 ListOfModelsBasedOnER[ListOfModels[m_ERModelAsSutureThread].Slot], 02879 ListOfModelsBasedOnFEM[itc->FEMloc] 02880 ); 02881 ++itc; 02882 } 02883 } 02884 //----------------------------------------------------------------------------- 02885 template <typename T, typename DATA> 02886 void AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::EnforceAConstraintOfSutureHeadNeedleTiedWithFEMModel_ForIPG ( 02887 unsigned int constraintNumber, 02888 Matrix4x4<T> const & HeadNeedleTrx, 02889 Matrix4x4<T> const & HeadNeedleTrxInv, 02890 RigidBodyDynamics<T> * pRBDModel, 02891 ModelDefBasedOnFEM<T,DATA> * pFEMModel, 02892 typename AdvSimSupport_DATA<T,DATA>::ListOfPointForces & ListOfPointForces 02893 ) 02894 { 02895 Matrix4x4<T> TrxModelFEM = pFEMModel->RefToTransformationSupport().RefToMatrixTransform(); 02896 Matrix4x4<T> InvTrxModelFEM = TrxModelFEM.GetInverse(); 02897 02898 AdvSimConstraint_VertexIdVsHEVertex<T> & Constraint = m_SetOfInteractions_ForIPG[constraintNumber]; 02899 02900 unsigned int vertID = Constraint.GetVertexIDFromModel_1(); 02901 HEVertex<T> * pHEvert = Constraint.GetPtrToVertexFromModel_2(); 02902 Vector3<T> V1 = pRBDModel->RefToInteractionPointGroup().GetInteractionPointAtIndex( vertID ).GetPosition(); 02903 Vector3<T> V2 = pHEvert->GetProtectedPosition() = Constraint.RefToSavedPositionB(); 02904 V1 = HeadNeedleTrx * V1; 02905 V2 = TrxModelFEM * V2; 02906 T ratio = Constraint.GetForceRatio(); 02907 Constraint.RefToTargetPositionA() = Constraint.RefToTargetPositionB() = ratio*V1 + (1-ratio)*V2; 02908 V1 = HeadNeedleTrxInv * Constraint.RefToTargetPositionA(); 02909 V2 = InvTrxModelFEM * Constraint.RefToTargetPositionB(); 02910 Constraint.RefToTargetPositionA() = V1; 02911 Constraint.RefToTargetPositionB() = V2; 02912 02913 // RBD-based, add the connection as a point force. 02914 { 02915 } 02916 // FEM-based, add the connection as a point force. 02917 { 02918 Vector3<T> Fb = V2 - pHEvert->GetProtectedPosition(); 02919 PointForce<T> * pF = new PointForce<T>(); 02920 ListOfPointForces.push_back( pF ); 02921 02922 // Set connection (i.e. point) force 02923 pF->SetID( pHEvert->GetID() ); 02924 pF->RefToPosition() = pHEvert->GetBarycentricPtr()->RetGenBaryCoords(); 02925 pF->RefToForce() = Fb * Constraint.GetForceScaleB(); 02926 02927 pFEMModel->AddConnectionForceInLocalCoordinates( ListOfPointForces.back() ); 02928 } 02929 } 02930 //----------------------------------------------------------------------------- 02931 template <typename T, typename DATA> 02932 void AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::EnforceAConstraintOfSutureThreadTiedWithFEMModel_ForIPG ( 02933 AdvSimConstraint_VertexIdVsHEVertex<T> * pConstraint, 02934 ModelElasticRod<T> * pERModel, 02935 ModelDefBasedOnFEM<T,DATA> * pFEMModel, 02936 typename AdvSimSupport_DATA<T,DATA>::ListOfPointForces & ListOfPointForces 02937 ) 02938 { 02939 int ERModelPt = pConstraint->GetVertexIDFromModel_1(); 02940 Matrix4x4<T> TrxFEMModel = pFEMModel->RefToTransformationSupport().RefToMatrixTransform(); 02941 Matrix4x4<T> InvTrxFEMModel = TrxFEMModel.GetInverse(); 02942 02943 Vector3<T> & S = pERModel->RefToCurrentVertexPositionOfNodeNumber( ERModelPt ); 02944 HEVertex<T> * he = pConstraint->GetPtrToVertexFromModel_2(); 02945 Vector3<T> V = he->GetProtectedPosition() = pConstraint->RefToSavedPositionB(); 02946 V = TrxFEMModel * V; 02947 T ratio = pConstraint->GetForceRatio(); 02948 pConstraint->RefToTargetPositionA() = 02949 pConstraint->RefToTargetPositionB() = ratio*S + (1-ratio)*V; 02950 //S = pConstraint->RefToTargetPositionA(); // ERModel's point's current position 02951 V = InvTrxFEMModel * pConstraint->RefToTargetPositionB(); 02952 pConstraint->RefToTargetPositionB() = V; 02953 02954 // FEM-based model, add the connection as a point force. 02955 { 02956 Vector3<T> F = V - he->GetProtectedPosition(); 02957 PointForce<T> * pF = new PointForce<T>(); 02958 ListOfPointForces.push_back( pF ); 02959 02960 pF->SetID( he->GetID() ); 02961 pF->RefToPosition() = he->GetBarycentricPtr()->RetGenBaryCoords(); 02962 // Scale pF by the timestep 02963 pF->RefToForce() = F * pConstraint->GetForceScaleB() / pFEMModel->GetTimeStep(); 02964 pFEMModel->AddConnectionForceInLocalCoordinates( ListOfPointForces.back() ); 02965 } 02966 02967 // Calculate the external force apply to Suture 02968 Vector3<T> Fs = pConstraint->RefToTargetPositionA() - S; 02969 02970 bool bLinearForce = true; 02971 T scaleNonlinearForce = 0.5; 02972 02973 if ( bLinearForce ) 02974 { 02975 // LINEAR FORCE 02976 Fs *= pConstraint->GetForceScaleA(); 02977 } 02978 else 02979 { 02980 // NONLINEAR FORCE 02981 T ERModel_link_length = pERModel->GetLinkLength(); 02982 T magnitude = Fs.Magnitude(); 02983 T scale = pow( pConstraint->GetForceScaleA(), magnitude/ERModel_link_length ); 02984 if ( scale*magnitude <= pERModel->GetStretchModulus()*scaleNonlinearForce ) { 02985 Fs *= scale; 02986 } 02987 else { 02988 Fs *= pERModel->GetStretchModulus(); 02989 } 02990 } 02991 02992 // Scale Fs by the timestep, iteration times, and mass 02993 pERModel->GetListOfNodes()[ERModelPt].ExternalForce_ForAdvSimCtrl = Fs / pERModel->GetSimTimeStep() / pERModel->GetSimIterationTimes() / pERModel->GetListOfNodes()[ERModelPt].GetMass(); 02994 } 02995 //----------------------------------------------------------------------------- 02996 template <typename T, typename DATA> 02997 void AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::EnforceAConsistentPositionSutureThreadTiedWithFEMModel_ForIPG ( 02998 AdvSimConstraint_VertexIdVsHEVertex<T> * pConstraint, 02999 ModelElasticRod<T> * pERModel, 03000 ModelDefBasedOnFEM<T,DATA> * pFEMModel 03001 ) 03002 { 03003 int ERModelPt = pConstraint->GetVertexIDFromModel_1(); 03004 Matrix4x4<T> TrxFEMModel = pFEMModel->RefToTransformationSupport().RefToMatrixTransform(); 03005 Matrix4x4<T> InvTrxFEMModel = TrxFEMModel.GetInverse(); 03006 03007 unsigned int nodeID = pConstraint->GetVertexIDFromModel_1(); 03008 03009 // Enforce ER-based model's Position 03010 if ( pConstraint->GetEnforcePositionStatusA() ) { 03011 pERModel->GetListOfNodes()[nodeID].UseEnforcedPosition = true; 03012 pERModel->GetListOfNodes()[nodeID].EnforcedPosition = pConstraint->RefToTargetPositionA(); 03013 } 03014 else { 03015 } 03016 03017 pConstraint->RefToSavedPositionB() = pConstraint->GetPtrToVertexFromModel_2()->GetProtectedPosition(); 03018 03019 // Move the model's constrained point to the target point 03020 if ( false ) 03021 { 03022 // TO THE TARGET POSITION B 03023 if ( pConstraint->GetEnforcePositionStatusB() ) { 03024 pConstraint->GetPtrToVertexFromModel_2()->GetProtectedPosition() = pConstraint->RefToTargetPositionB(); 03025 } 03026 } 03027 else { 03028 // TO THE SUTURE POINT 03029 if ( pConstraint->GetEnforcePositionStatusB() ) { 03030 #ifdef TAPs_USE_VERTEX_POSITION_AVERAGING 03031 std::vector< HEVertex<T> * > firstRing = pConstraint->GetPtrToVertexFromModel_2()->FindTheFirstRingOfVertices(); 03032 unsigned int size = firstRing.size(); 03033 Vector3<T> targetPos = InvTrxFEMModel * pERModel->RefToCurrentVertexPositionOfNodeNumber( pConstraint->GetVertexIDFromModel_1() ); 03034 if ( size > 0 ) { 03035 Vector3<T> avgPos; 03036 for ( unsigned int i = 0; i < size; ++i ) { 03037 avgPos += firstRing[i]->GetPosition(); 03038 } 03039 T scale = size / T(2); 03040 avgPos = ( avgPos + targetPos*scale ) / ( size + scale ); 03041 pConstraint->GetPtrToVertexFromModel_2()->GetProtectedPosition() = avgPos; 03042 for ( unsigned int i = 0; i < size; ++i ) { 03043 firstRing[i]->GetProtectedPosition() = ( firstRing[i]->GetPosition() + avgPos ) / 2; 03044 } 03045 } 03046 else { 03047 pConstraint->GetPtrToVertexFromModel_2()->GetProtectedPosition() = targetPos; 03048 } 03049 #else //TAPs_USE_VERTEX_POSITION_AVERAGING 03050 pConstraint->GetPtrToVertexFromModel_2()->GetProtectedPosition() = InvTrxFEMModel * 03051 pERModel->RefToCurrentVertexPositionOfNodeNumber( 03052 pConstraint->GetVertexIDFromModel_1() ); 03053 #endif//TAPs_USE_VERTEX_POSITION_AVERAGING 03054 } 03055 } 03056 } 03057 //----------------------------------------------------------------------------- 03058 // FOR IPG 03059 //============================================================================= 03060 03061 03062 03063 03064 //============================================================================= 03065 // FOR CIRCLE PATH 03066 //----------------------------------------------------------------------------- 03067 template <typename T, typename DATA> 03068 void AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::EnforceAllConstraints_ForCirclePath ( 03069 typename AdvSimSupport_DATA<T,DATA>::ModelIDs const & ListOfModels, 03070 std::vector< ModelElasticRod<T> * > & ListOfModelsBasedOnER, 03071 std::vector< ModelDefBasedOnFEM<T,DATA> * > & ListOfModelsBasedOnFEM, 03072 std::vector< RigidBodyDynamics<T> * > & ListOfModelsBasedOnRBD, 03073 typename AdvSimSupport_DATA<T,DATA>::ListOfPointForces & ListOfPointForces 03074 ) 03075 { 03076 // The head needle model type must be MODEL_BASED_ON_IPG_RBD 03077 Enum::ModelType HeadNeedleModelType = ListOfModels[m_RBDModelAsSutureHeadNeedle].Type; 03078 if ( HeadNeedleModelType != Enum::MODEL_BASED_ON_IPG_RBD ) return; 03079 03080 // Get the head needle's transformation matrix and its inverse 03081 unsigned int locRBD = ListOfModels[m_RBDModelAsSutureHeadNeedle].Slot; 03082 RigidBodyDynamics<T> * pRBDModel = ListOfModelsBasedOnRBD[locRBD]; 03083 Matrix4x4<T> TrxModelRBD = pRBDModel->CalMatrixTransform(); 03084 Matrix4x4<T> InvTrxModelRBD = TrxModelRBD.GetInverse(); 03085 03086 // Suture's head needle with FEM-based models 03087 for ( unsigned int i = 0; i < m_size_ForCirclePath; ++i ) { 03088 03089 int modelID = m_SetOfInteractionAssociatedModelID_ForCirclePath[i]; 03090 03091 // A model is linked to the i(th) constraint 03092 if ( modelID >= 0 ) { 03093 Enum::ModelType modelType = ListOfModels[modelID].Type; 03094 if ( modelType == Enum::MODEL_BASED_ON_FEM ) { 03095 // MODEL_BASED_ON_IPG_RBD vs MODEL_BASED_ON_FEM at the i(th) constraint 03096 EnforceAConstraintOfSutureHeadNeedleTiedWithFEMModel_ForCirclePath( 03097 i, TrxModelRBD, InvTrxModelRBD, pRBDModel, 03098 ListOfModelsBasedOnFEM[ListOfModels[modelID].Slot], 03099 ListOfPointForces 03100 ); 03101 } 03102 } 03103 } 03104 03105 // Suture's thread with FEM-based models 03106 std::list< InteractionSutureThreadVsFEMModel<T> >::iterator it = m_ListOfSutureThreadVsFEMModels_ForCirclePath.begin(); 03107 while ( it != m_ListOfSutureThreadVsFEMModels_ForCirclePath.end() ) { 03108 EnforceAConstraintOfSutureThreadTiedWithFEMModel_ForCirclePath( 03109 it->pAdvSimConstraint_VertexIdVsHEVertex, 03110 ListOfModelsBasedOnER[ListOfModels[m_ERModelAsSutureThread].Slot], 03111 ListOfModelsBasedOnFEM[it->FEMloc], 03112 ListOfPointForces 03113 ); 03114 ++it; 03115 } 03116 } 03117 //----------------------------------------------------------------------------- 03118 template <typename T, typename DATA> 03119 void AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::ClearAllConstraints_ForCirclePath () 03120 { 03121 // Since all values of each constraint will be set explicitly, except VertexId with invariant. 03122 // And m_SetOfInteractionAssociatedModelID less than zero signifies that the constraint is voided or cancelled. 03123 // Therefore, only m_SetOfInteractionAssociatedModelID needs to be cleared. 03124 for ( unsigned int i = 0; i < m_size_ForCirclePath; ++i ) { 03125 m_SetOfInteractionAssociatedModelID_ForCirclePath[i] = -1; 03126 } 03127 03128 m_ListOfSutureThreadVsFEMModels_ForCirclePath.clear(); 03129 03130 m_LockedCirclePath.Clear(); 03131 03132 m_uiNumOfLockedCirclePaths = 0; 03133 03134 // Unlock the head needle 03135 //ListOfModelsBasedOnRBD[RBDloc].UnlockHeadNeedlePosition(); 03136 03137 m_NumberOfSutureThreadPointsPuncturedIn = 0; 03138 m_NumberOfSutureHeadNeedlePointsPuncturedIn = 0; 03139 } 03140 //----------------------------------------------------------------------------- 03141 template <typename T, typename DATA> 03142 void AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::EnforceAllConsistentPositions_ForCirclePath ( 03143 typename AdvSimSupport_DATA<T,DATA>::ModelIDs const & ListOfModels, 03144 std::vector< ModelElasticRod<T> * > & ListOfModelsBasedOnER, 03145 std::vector< ModelDefBasedOnFEM<T,DATA> * > & ListOfModelsBasedOnFEM, 03146 std::vector< RigidBodyDynamics<T> * > & ListOfModelsBasedOnRBD 03147 ) 03148 { 03149 // Suture's head needle with FEM-based models 03150 SetOfInteractions::iterator it = m_SetOfInteractions_ForCirclePath.begin(); 03151 int i = 0; 03152 while ( it != m_SetOfInteractions_ForCirclePath.end() ) { 03153 if ( m_SetOfInteractionAssociatedModelID_ForCirclePath[i] >= 0 ) { 03154 if ( it->GetEnforcePositionStatusB() ) { 03155 //it->RefToSavedPositionA() = it->GetPtrToVertexFromModel_1()->GetProtectedPosition(); 03156 it->RefToSavedPositionB() = it->GetPtrToVertexFromModel_2()->GetProtectedPosition(); 03157 //it->GetPtrToVertexFromModel_1()->GetProtectedPosition() = it->RefToTargetPositionA(); 03158 03159 #ifdef TAPs_USE_VERTEX_POSITION_AVERAGING 03160 std::vector< HEVertex<T> * > firstRing = it->GetPtrToVertexFromModel_2()->FindTheFirstRingOfVertices(); 03161 unsigned int size = firstRing.size(); 03162 if ( size > 0 ) { 03163 Vector3<T> avgPos; 03164 for ( unsigned int i = 0; i < size; ++i ) { 03165 avgPos += firstRing[i]->GetPosition(); 03166 } 03167 T scale = size;// / T(2); 03168 avgPos = ( avgPos + it->RefToTargetPositionB()*scale ) / ( size + scale ); 03169 it->GetPtrToVertexFromModel_2()->GetProtectedPosition() = avgPos; 03170 for ( unsigned int i = 0; i < size; ++i ) { 03171 firstRing[i]->GetProtectedPosition() = ( firstRing[i]->GetPosition() + avgPos ) / 2; 03172 } 03173 } 03174 else { 03175 it->GetPtrToVertexFromModel_2()->GetProtectedPosition() = it->RefToTargetPositionB(); 03176 } 03177 #else //TAPs_USE_VERTEX_POSITION_AVERAGING 03178 it->GetPtrToVertexFromModel_2()->GetProtectedPosition() = it->RefToTargetPositionB(); 03179 #endif//TAPs_USE_VERTEX_POSITION_AVERAGING 03180 } 03181 } 03182 ++it; 03183 ++i; 03184 } 03185 03186 // Suture's thread with FEM-based models 03187 std::list< InteractionSutureThreadVsFEMModel<T> >::iterator itc = m_ListOfSutureThreadVsFEMModels_ForCirclePath.begin(); 03188 while ( itc != m_ListOfSutureThreadVsFEMModels_ForCirclePath.end() ) { 03189 EnforceAConsistentPositionSutureThreadTiedWithFEMModel_ForCirclePath( 03190 itc->pAdvSimConstraint_VertexIdVsHEVertex, 03191 ListOfModelsBasedOnER[ListOfModels[m_ERModelAsSutureThread].Slot], 03192 ListOfModelsBasedOnFEM[itc->FEMloc] 03193 ); 03194 ++itc; 03195 } 03196 } 03197 //----------------------------------------------------------------------------- 03198 template <typename T, typename DATA> 03199 void AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::EnforceAConstraintOfSutureHeadNeedleTiedWithFEMModel_ForCirclePath ( 03200 unsigned int constraintNumber, 03201 Matrix4x4<T> const & HeadNeedleTrx, 03202 Matrix4x4<T> const & HeadNeedleTrxInv, 03203 RigidBodyDynamics<T> * pRBDModel, 03204 ModelDefBasedOnFEM<T,DATA> * pFEMModel, 03205 typename AdvSimSupport_DATA<T,DATA>::ListOfPointForces & ListOfPointForces 03206 ) 03207 { 03208 Matrix4x4<T> TrxModelFEM = pFEMModel->RefToTransformationSupport().RefToMatrixTransform(); 03209 Matrix4x4<T> InvTrxModelFEM = TrxModelFEM.GetInverse(); 03210 03211 AdvSimConstraint_VertexIdVsHEVertex<T> & Constraint = m_SetOfInteractions_ForCirclePath[constraintNumber]; 03212 03213 unsigned int vertID = Constraint.GetVertexIDFromModel_1(); 03214 HEVertex<T> * pHEvert = Constraint.GetPtrToVertexFromModel_2(); 03215 //Vector3<T> V1 = pRBDModel->RefToInteractionPointGroup().GetInteractionPointAtIndex( vertID ).GetPosition(); 03216 Vector3<T> V1 = m_pCirclePathCtrl->InterpolatedPts[vertID]; 03217 Vector3<T> V2 = pHEvert->GetProtectedPosition() = Constraint.RefToSavedPositionB(); 03218 V1 = HeadNeedleTrx * V1; 03219 V2 = TrxModelFEM * V2; 03220 T ratio = Constraint.GetForceRatio(); 03221 Constraint.RefToTargetPositionA() = Constraint.RefToTargetPositionB() = ratio*V1 + (1-ratio)*V2; 03222 V1 = HeadNeedleTrxInv * Constraint.RefToTargetPositionA(); 03223 V2 = InvTrxModelFEM * Constraint.RefToTargetPositionB(); 03224 Constraint.RefToTargetPositionA() = V1; 03225 Constraint.RefToTargetPositionB() = V2; 03226 03227 // RBD-based, add the connection as a point force. 03228 { 03229 } 03230 // FEM-based, add the connection as a point force. 03231 { 03232 Vector3<T> Fb = V2 - pHEvert->GetProtectedPosition(); 03233 PointForce<T> * pF = new PointForce<T>(); 03234 ListOfPointForces.push_back( pF ); 03235 03236 // Set connection (i.e. point) force 03237 pF->SetID( pHEvert->GetID() ); 03238 pF->RefToPosition() = pHEvert->GetBarycentricPtr()->RetGenBaryCoords(); 03239 pF->RefToForce() = Fb * Constraint.GetForceScaleB(); 03240 pFEMModel->AddConnectionForceInLocalCoordinates( ListOfPointForces.back() ); 03241 } 03242 } 03243 //----------------------------------------------------------------------------- 03244 template <typename T, typename DATA> 03245 void AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::EnforceAConstraintOfSutureThreadTiedWithFEMModel_ForCirclePath ( 03246 AdvSimConstraint_VertexIdVsHEVertex<T> * pConstraint, 03247 ModelElasticRod<T> * pERModel, 03248 ModelDefBasedOnFEM<T,DATA> * pFEMModel, 03249 typename AdvSimSupport_DATA<T,DATA>::ListOfPointForces & ListOfPointForces 03250 ) 03251 { 03252 int ERModelPt = pConstraint->GetVertexIDFromModel_1(); 03253 Matrix4x4<T> TrxFEMModel = pFEMModel->RefToTransformationSupport().RefToMatrixTransform(); 03254 Matrix4x4<T> InvTrxFEMModel = TrxFEMModel.GetInverse(); 03255 03256 Vector3<T> & S = pERModel->RefToCurrentVertexPositionOfNodeNumber( ERModelPt ); 03257 HEVertex<T> * he = pConstraint->GetPtrToVertexFromModel_2(); 03258 Vector3<T> V = he->GetProtectedPosition() = pConstraint->RefToSavedPositionB(); 03259 V = TrxFEMModel * V; 03260 T ratio = pConstraint->GetForceRatio(); 03261 pConstraint->RefToTargetPositionA() = 03262 pConstraint->RefToTargetPositionB() = ratio*S + (1-ratio)*V; 03263 //S = pConstraint->RefToTargetPositionA(); // ERModel's point's current position 03264 V = InvTrxFEMModel * pConstraint->RefToTargetPositionB(); 03265 pConstraint->RefToTargetPositionB() = V; 03266 03267 // FEM-based model, add the connection as a point force. 03268 { 03269 Vector3<T> F = V - he->GetProtectedPosition(); 03270 PointForce<T> * pF = new PointForce<T>(); 03271 ListOfPointForces.push_back( pF ); 03272 03273 pF->SetID( he->GetID() ); 03274 pF->RefToPosition() = he->GetBarycentricPtr()->RetGenBaryCoords(); 03275 pF->RefToForce() = F * pConstraint->GetForceScaleB(); 03276 pFEMModel->AddConnectionForceInLocalCoordinates( ListOfPointForces.back() ); 03277 } 03278 03279 // Calculate the external force apply to Suture 03280 Vector3<T> Fs = pConstraint->RefToTargetPositionA() - S; 03281 03282 bool bLinearForce = true; 03283 T scaleNonlinearForce = 0.5; 03284 03285 if ( bLinearForce ) 03286 { 03287 // LINEAR FORCE 03288 Fs *= pConstraint->GetForceScaleA(); 03289 } 03290 else 03291 { 03292 // NONLINEAR FORCE 03293 T ERModel_link_length = pERModel->GetLinkLength(); 03294 T magnitude = Fs.Magnitude(); 03295 T scale = pow( pConstraint->GetForceScaleA(), magnitude/ERModel_link_length ); 03296 if ( scale*magnitude <= pERModel->GetStretchModulus()*scaleNonlinearForce ) { 03297 Fs *= scale; 03298 } 03299 else { 03300 Fs *= pERModel->GetStretchModulus(); 03301 } 03302 } 03303 03304 pERModel->GetListOfNodes()[ERModelPt].ExternalForce_ForAdvSimCtrl = Fs; 03305 } 03306 //----------------------------------------------------------------------------- 03307 template <typename T, typename DATA> 03308 void AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::EnforceAConsistentPositionSutureThreadTiedWithFEMModel_ForCirclePath ( 03309 AdvSimConstraint_VertexIdVsHEVertex<T> * pConstraint, 03310 ModelElasticRod<T> * pERModel, 03311 ModelDefBasedOnFEM<T,DATA> * pFEMModel 03312 ) 03313 { 03314 int ERModelPt = pConstraint->GetVertexIDFromModel_1(); 03315 Matrix4x4<T> TrxFEMModel = pFEMModel->RefToTransformationSupport().RefToMatrixTransform(); 03316 Matrix4x4<T> InvTrxFEMModel = TrxFEMModel.GetInverse(); 03317 03318 unsigned int nodeID = pConstraint->GetVertexIDFromModel_1(); 03319 03320 // Enforce ER-based model's Position 03321 if ( pConstraint->GetEnforcePositionStatusA() ) { 03322 pERModel->GetListOfNodes()[nodeID].UseEnforcedPosition = true; 03323 pERModel->GetListOfNodes()[nodeID].EnforcedPosition = pConstraint->RefToTargetPositionA(); 03324 } 03325 else { 03326 } 03327 03328 pConstraint->RefToSavedPositionB() = pConstraint->GetPtrToVertexFromModel_2()->GetProtectedPosition(); 03329 03330 // Move the model's constrained point to the target point 03331 if ( false ) 03332 { 03333 // TO THE TARGET POSITION B 03334 if ( pConstraint->GetEnforcePositionStatusB() ) { 03335 pConstraint->GetPtrToVertexFromModel_2()->GetProtectedPosition() = pConstraint->RefToTargetPositionB(); 03336 } 03337 } 03338 else { 03339 // TO THE SUTURE POINT 03340 if ( pConstraint->GetEnforcePositionStatusB() ) { 03341 #ifdef TAPs_USE_VERTEX_POSITION_AVERAGING 03342 std::vector< HEVertex<T> * > firstRing = pConstraint->GetPtrToVertexFromModel_2()->FindTheFirstRingOfVertices(); 03343 unsigned int size = firstRing.size(); 03344 Vector3<T> targetPos = InvTrxFEMModel * pERModel->RefToCurrentVertexPositionOfNodeNumber( pConstraint->GetVertexIDFromModel_1() ); 03345 if ( size > 0 ) { 03346 Vector3<T> avgPos; 03347 for ( unsigned int i = 0; i < size; ++i ) { 03348 avgPos += firstRing[i]->GetPosition(); 03349 //firstRing[i]->GetProtectedPosition() = ( firstRing[i]->GetPosition() + targetPos ) / 2; 03350 } 03351 T scale = size;// / T(2); 03352 avgPos = ( avgPos + targetPos*scale ) / ( size + scale ); 03353 pConstraint->GetPtrToVertexFromModel_2()->GetProtectedPosition() = avgPos; 03354 } 03355 else { 03356 pConstraint->GetPtrToVertexFromModel_2()->GetProtectedPosition() = targetPos; 03357 } 03358 #else //TAPs_USE_VERTEX_POSITION_AVERAGING 03359 pConstraint->GetPtrToVertexFromModel_2()->GetProtectedPosition() = InvTrxFEMModel * 03360 pERModel->RefToCurrentVertexPositionOfNodeNumber( 03361 pConstraint->GetVertexIDFromModel_1() ); 03362 #endif//TAPs_USE_VERTEX_POSITION_AVERAGING 03363 } 03364 } 03365 } 03366 //----------------------------------------------------------------------------- 03367 // FOR CIRCLE PATH 03368 //============================================================================= 03369 03370 03371 03372 03373 //============================================================================= 03374 #if defined(__gl_h_) || defined(__GL_H__) 03375 //----------------------------------------------------------------------------- 03376 template <typename T, typename DATA> 03377 void AdvSimConstraint_ForModelSurgicalSutureWithHeadNeedle<T,DATA>::Draw () const 03378 { 03379 for ( unsigned int i = 0; i < m_size_ForIPG; ++i ) { 03380 if ( m_SetOfInteractionAssociatedModelID_ForIPG[i] >= 0 ) { 03381 // ... 03382 //DRAW SOMETHING IN HERE 03383 m_SetOfInteractions_ForIPG[i].Draw(); 03384 03385 //std::cout << 03386 //... 03387 } 03388 } 03389 03390 // Draw saved punctured locations 03391 glPushAttrib( GL_ALL_ATTRIB_BITS ); 03392 glDisable( GL_LIGHTING ); 03393 glPointSize( 25 ); 03394 glBegin( GL_POINTS ); 03395 glColor3f( 1, 1, 1 ); 03396 for ( unsigned int i = 0; i < m_size_ForIPG; ++i ) { 03397 if ( m_SetOfInteractionAssociatedModelID_ForIPG[i] >= 0 ) { 03398 glVertex3fv( m_PuncturedLocations_ForIPG[i].GetDataFloat() ); 03399 } 03400 } 03401 glEnd(); 03402 glPopAttrib(); 03403 03404 /* 03405 // Draw suture's punctured thread points 03406 glPushAttrib( GL_ALL_ATTRIB_BITS ); 03407 std::list< InteractionSutureThreadVsFEMModel<T> >::const_iterator it = m_ListOfSutureThreadVsFEMModels.begin(); 03408 int count = 0; 03409 while ( it != m_ListOfSutureThreadVsFEMModels.end() ) { 03410 std::cout << ++count << ": " << *it->pAdvSimConstraint_VertexIdVsHEVertex << "\n"; 03411 03412 glBegin( GL_POINTS ); 03413 glEnd(); 03414 ++it; 03415 } 03416 glPopAttrib(); 03417 //*/ 03418 03419 } 03420 03421 //----------------------------------------------------------------------------- 03422 #endif // #if defined(__gl_h_) || defined(__GL_H__) 03423 //============================================================================= 03424 03425 03426 //============================================================================= 03427 END_NAMESPACE_TAPs 03428 //34567890123456789012345678901234567890123456789012345678901234567890123456789 03429 //--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----
1.7.4 
