 |
TAPs 0.7.7.3
|
|
TAPs 0.7.7.3
|
00001 /****************************************************************************** 00002 TAPsModelDefBasedOnFEM_CD.cpp 00003 ******************************************************************************/ 00007 /****************************************************************************** 00008 SUKITTI PUNAK (04/15/2010) 00009 UPDATE (12/05/2010) 00010 ******************************************************************************/ 00011 #include "TAPsModelDefBasedOnFEM_CD.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__FEM 00017 //============================================================================= 00018 //template <typename T> int ModelDefBasedOnFEM_CD<T>::MAX_NUMBER_OF_GRABBED_PARTS = 32; 00019 //----------------------------------------------------------------------------- 00020 template <typename T> 00021 ModelDefBasedOnFEM_CD<T>::ModelDefBasedOnFEM_CD () 00022 : m_pHEModel( NULL ) 00023 , m_bCollisionStatus( false ) 00024 , m_tCDForceScale( 1 ) 00025 , m_tGrabbedForceScale( 1 ) 00026 { 00027 for ( int i = 0; i < MAX_NUMBER_OF_GRABBED_PARTS; ++i ) { 00028 m_ListOfGrabbedParts_IsGrabbedStatus[i] = false; 00029 } 00030 } 00031 //----------------------------------------------------------------------------- 00032 //template <typename T> 00033 //ModelDefBasedOnFEM_CD<T>::ModelDefBasedOnFEM_CD ( ModelDefBasedOnFEM_CD<T> const &orig ) 00034 // : 00035 //{} 00036 //----------------------------------------------------------------------------- 00037 template <typename T> 00038 ModelDefBasedOnFEM_CD<T>::~ModelDefBasedOnFEM_CD () 00039 {} 00040 //----------------------------------------------------------------------------- 00041 template <typename T> 00042 std::string ModelDefBasedOnFEM_CD<T>::StrInfo () const 00043 { 00044 std::ostringstream ss; 00045 ss << "ModelDefBasedOnFEM_CD<" << typeid(T).name() << ">"; 00046 ss << "\n"; 00047 return ss.str(); 00048 } 00049 //----------------------------------------------------------------------------- 00050 //============================================================================= 00051 // Assignment Operator 00052 //----------------------------------------------------------------------------- 00053 //template <typename T> 00054 //inline ModelDefBasedOnFEM_CD<T> & ModelDefBasedOnFEM_CD<T>::operator= ( ModelDefBasedOnFEM_CD<T> const &orig ) 00055 //{ 00056 // return *this; 00057 //} 00058 //----------------------------------------------------------------------------- 00059 //============================================================================= 00060 00061 //----------------------------------------------------------------------------- 00062 template <typename T> 00063 void ModelDefBasedOnFEM_CD<T>::Reset () 00064 { 00065 m_ListOfCDObjs.clear(); 00066 m_ListOfGrabbedParts.clear(); 00067 } 00068 //----------------------------------------------------------------------------- 00069 template <typename T> 00070 bool ModelDefBasedOnFEM_CD<T>::Init ( 00071 OpenGL::OpenGLHalfEdgeTrigonalModel<T> * pHEModel 00072 ) 00073 { 00074 m_pHEModel = pHEModel; 00075 return true; 00076 } 00077 //----------------------------------------------------------------------------- 00078 template <typename T> 00079 bool ModelDefBasedOnFEM_CD<T>::CORE_CDwith ( 00080 MultiBoundingVolume<T> * const pMBVObj, 00081 std::vector< CDObj<T> > * pListOfCDObj 00082 ) 00083 { 00084 // Clear the mark status of all vertices 00085 HEVertexList<T> * pVertexList = m_pHEModel->GetVertexList(); 00086 HEVertex<T> * pVertex = pVertexList->Head(); // Set to head 00087 while ( pVertexList->Current() != NULL ) { 00088 pVertexList->Current()->SetMarkedStatus( false ); 00089 pVertexList->NextCurrent(); 00090 } 00091 00092 #ifdef TAPs_DEBUG_COLLISION_DETECTION 00093 pPolygonalMeshObj->GetBVHTree()->ClearFlags(); 00094 #endif//TAPs_DEBUG_COLLISION_DETECTION 00095 00096 //#ifdef TAPs_DEBUG_MODE 00097 //std::cout << "CDR_PolygonalMesh_vs_MBV( HalfEdgeModel<T>, MultiBoundingVolume<T> )\n"; 00098 //#endif//TAPs_DEBUG_MODE 00099 00100 //std::cout << "BVHTree: " << *(pPolygonalMeshObj->GetBVHTree()) << "\n"; 00101 00102 bool bResult = false; 00103 00104 // Collision Detection 00105 bResult = m_pHEModel->GetBVHTree()->TestOverlapWithTillLeafNodes( pMBVObj ); 00106 00107 //return bResult; 00108 00109 //--------------------------------------------------------------- 00110 // Collision Response 00111 if ( bResult ) { 00112 00113 // Reset the result for primitive-primitive test, 00114 // because the previous result is only from bounding volume test. 00115 bResult = false; 00116 00117 //------------------------------------------------- 00118 //T separateDistanceConstraint; 00119 Vector3<T> N, centerA; 00120 //T separateDistance; 00121 HEFace<T> * primB; 00122 std::vector< HEVertex<T> * const > vertexList; 00123 int noOfVertices; 00124 //------------------------------------------------- 00125 TransformationSupport<T> trxMBV; // default constructor is an identity transformation 00126 00127 trxMBV = pMBVObj->GetTransform(); 00128 //------------------------------------------------- 00129 std::set< BVHNode<T> * > defPolyMesh_NodeSet; 00130 00131 // Get the list of collided nodes and bounding volumes 00132 std::vector< BVsAndNodesList<T> > * bvList = &m_pHEModel->GetBVHTree()->GetListOfCollidedBoundingVolumesAndNodes(); 00133 00134 /* 00135 // DEBUG 00136 std::cout << "bvList.size(): " << bvList->size() << "\n"; 00137 for ( unsigned int i = 0; i < bvList->size(); ++i ) { 00138 std::cout << "bvList["<<i<<"].NodeList.size(): " << (*bvList)[i].NodeList.size() << "\n"; 00139 for ( unsigned int j = 0; j < (*bvList)[i].NodeList.size(); ++j ) { 00140 std::cout << j << "\t" << (*bvList)[i].NodeList[j] << "\n"; 00141 } 00142 } 00143 //*/ 00144 00145 //------------------------------------------------- 00146 // Traverse the list of bounding volumes 00147 std::vector< BVsAndNodesList<T> >::const_iterator pos = bvList->begin(); 00148 00149 //------------------------------------------------- 00150 // Mesh's inverse transformation 00151 Matrix4x4<T> trxInvMesh = m_pHEModel->GetTransform().RefToMatrixTransform(); 00152 trxInvMesh.Inversed(); 00153 Vector3<T> vOrigin( (m_pHEModel->GetTransform().RefToMatrixTransform() * Vector4<T>(0,0,0,1)).GetVector3() ); 00154 00155 while ( pos != bvList->end() ) { 00156 00157 // Store the transformation of the bounding volume including the mesh's inverse transformation 00158 TransformationSupport<T> trx = (*pos).BV->GetTransform(); 00159 (*pos).BV->GetTransform().SetMatrixTransform( trxInvMesh * trxMBV.RefToMatrixTransform() * trx.RefToMatrixTransform() ); 00160 00161 //----------------------------------------- 00162 // Deform the deformable polygonal mesh away from the rigid polygonal mesh 00163 switch ( (*pos).BV->GetType() ) { 00164 00165 case TAPs::Enum::BOUNDING_SPHERE: 00166 { 00167 00168 //std::cout << "BOUNDING_SPHERE: " << (*pos).BV->GetName() << std::endl; 00169 00170 // For Test against triangle 00171 //--------------------------------------------- 00172 // Transform the BV due to its transformation, center, and radius 00173 T radiusOfBV = (*pos).BV->GetRadius(); 00174 Vector3<T> C( (*pos).BV->GetCenter() ); 00175 Matrix4x4<T> translation( 1, 0, 0, C[0], 00176 0, 1, 0, C[1], 00177 0, 0, 1, C[2], 00178 0, 0, 0, 1 ); 00179 Vector3<T> S( radiusOfBV, radiusOfBV, radiusOfBV ); 00180 Matrix4x4<T> scale( S[0], 0, 0, 0, 00181 0, S[1], 0, 0, 00182 0, 0, S[2], 0, 00183 0, 0, 0, 1 ); 00184 // The BV's current transformation includes the mesh's inverse transformation (see the code above) 00185 Matrix4x4<T> mat = (*pos).BV->GetTransform().RefToMatrixTransform() * translation * scale; 00186 00187 // BV Sphere's center and radius after the whole transformation from 00188 // BV's transformation to mesh's inverse transformation. 00189 // I.e., transforming the BV to mesh's coordinate 00190 Vector3<T> centerOfBV_inMeshCoord = (mat * Vector4<T>(0,0,0,1)).GetVector3(); 00191 T radiusOfBV_inMeshCoord = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV_inMeshCoord ).Length(); 00192 00193 // Inverse the mesh's transformation to the BV's pure transformation 00194 mat = m_pHEModel->GetTransform().RefToMatrixTransform() * mat; 00195 00196 // BV Sphere's center and radius after the BV's pure transformation 00197 Vector3<T> centerOfBV( (mat * Vector4<T>(0,0,0,1)).GetVector3() ); 00198 radiusOfBV = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV ).Length(); 00199 //--------------------------------------------- 00200 00201 //count = 0; 00202 for ( unsigned int i = 0; i < (*pos).NodeList.size(); ++i ) { 00203 00204 bool testResult = false; 00205 00206 // Assume triangle is in Sphere Node 00207 // Test the triangle in sphere node with BVSphere 00208 primB = (*pos).NodeList[i]->GetAPrimitiveHalfEdgeFace(); 00209 vertexList = primB->GetPtrsToVerticesAndNumberOfVertices( noOfVertices ); 00210 Vector3<T> cdrVec[3]; 00211 00212 //* 00213 // Test against each vertex in the triangle 00214 //----------------------------------- 00215 //for ( int j = 0; j < noOfVertices; ++j ) { 00216 for ( int j = 0; j < 3; ++j ) { 00217 if ( !vertexList[j]->GetMarkedStatus() ) { 00218 if( (*pos).BV->TestPointLocation( vertexList[j]->GetPosition(), &(cdrVec[j]) ) ) 00219 { 00220 //vertexList[j]->SetPosition( vertexList[j]->GetPosition() + cdrVec[j] ); 00221 //Vector3<T> force( (m_pHEModel->GetTransform().GetMatrixTransform() * Vector4<T>(cdrVec[j])).GetVector3() - vOrigin ); 00222 00223 //TAPs::CD::Fn::ListOfContactPts.push_back( Vector3<GLfloat>( centerOfBV[0], centerOfBV[1], centerOfBV[2] ) ); 00224 //TAPs::CD::Fn::ListOfForces.push_back( Vector3<GLfloat>( force[0], force[1], force[2] ) ); 00225 00226 vertexList[j]->SetMarkedStatus( true ); 00227 //pListOfCDObj->push_back( CDObj<T>( vertexList[j], vertexList[j]->GetPosition() + cdrVec[j] ) ); // create CDObj with the three collided vertices and the absolute position for resolving collision 00228 pListOfCDObj->push_back( CDObj<T>( vertexList[j], cdrVec[j] ) ); // create CDObj with the three collided vertices and the three distances for resolving collision 00229 testResult = true; 00230 } 00231 } 00232 } 00233 //----------------------------------- 00234 //*/ 00235 00236 /* 00237 // Test against triangle 00238 //----------------------------------- 00239 //Vector3<T> contactPt; 00240 if ( TAPs::CGMath<T>::FindIntersectionSphereTriangle( 00241 centerOfBV_inMeshCoord, radiusOfBV_inMeshCoord, // BVSphere properties 00242 vertexList[0]->GetPosition(), vertexList[1]->GetPosition(), vertexList[2]->GetPosition(), // triangle vertices 00243 cdrVec[0], cdrVec[1], cdrVec[2] // out vectors 00244 //, &contactPt 00245 ) ) { 00246 //Vector3<T> force_1( cdrVec[0] * TAPs::CD::Fn::CDScaleForSphereBVWithTriangle ); 00247 //Vector3<T> force_2( cdrVec[1] * TAPs::CD::Fn::CDScaleForSphereBVWithTriangle ); 00248 //Vector3<T> force_3( cdrVec[2] * TAPs::CD::Fn::CDScaleForSphereBVWithTriangle ); 00249 00250 //vertexList[0]->SetPosition( vertexList[0]->GetPosition() + force_1 ); 00251 //vertexList[1]->SetPosition( vertexList[1]->GetPosition() + force_2 ); 00252 //vertexList[2]->SetPosition( vertexList[2]->GetPosition() + force_3 ); 00253 00254 //force_1 = (m_pHEModel->GetTransform().GetMatrixTransform() * Vector4<T>(force_1)).GetVector3() - vOrigin; 00255 //force_2 = (m_pHEModel->GetTransform().GetMatrixTransform() * Vector4<T>(force_2)).GetVector3() - vOrigin; 00256 //force_3 = (m_pHEModel->GetTransform().GetMatrixTransform() * Vector4<T>(force_3)).GetVector3() - vOrigin; 00257 00258 //TAPs::CD::Fn::ListOfContactPts.push_back( Vector3<GLfloat>( centerOfBV[0], centerOfBV[1], centerOfBV[2] ) ); 00259 //Vector3<T> force( ( force_1 + force_2 + force_3 )/3 ); 00260 //TAPs::CD::Fn::ListOfForces.push_back( Vector3<GLfloat>( force[0], force[1], force[2] ) ); 00261 00262 pListOfCDObj->push_back( CDObj<T>( vertexList[j], cdrVec[0] ) ); 00263 pListOfCDObj->push_back( CDObj<T>( vertexList[j], cdrVec[1] ) ); 00264 pListOfCDObj->push_back( CDObj<T>( vertexList[j], cdrVec[2] ) ); 00265 00266 testResult = true; 00267 } 00268 //----------------------------------- 00269 //*/ 00270 00271 00272 //std::cout << "\t\tCount#" << ++count << ": " << (*pos).NodeList[i] << "\n"; 00273 00274 // List of BVNodes to be updated 00275 if ( testResult ) { 00276 defPolyMesh_NodeSet.insert( (*pos).NodeList[i] ); 00277 bResult = true; 00278 } 00279 } 00280 } 00281 break; 00282 00283 case TAPs::Enum::BOUNDING_CYLINDER: 00284 { 00285 //std::cout << "BOUNDING_CYLINDER: " << (*pos).BV->GetName() << "\n"; 00286 00287 // For Test against triangle 00288 //--------------------------------------------- 00289 // Transform the BV due to its transformation, center, and radius 00290 T radiusOfBV = (*pos).BV->GetRadius(); 00291 Vector3<T> C( (*pos).BV->GetCenter() ); 00292 Matrix4x4<T> translation( 1, 0, 0, C[0], 00293 0, 1, 0, C[1], 00294 0, 0, 1, C[2], 00295 0, 0, 0, 1 ); 00296 Vector3<T> S( radiusOfBV, radiusOfBV, radiusOfBV ); 00297 Matrix4x4<T> scale( S[0], 0, 0, 0, 00298 0, S[1], 0, 0, 00299 0, 0, S[2], 0, 00300 0, 0, 0, 1 ); 00301 // The BV's current transformation includes the mesh's inverse transformation (see the code above) 00302 Matrix4x4<T> mat = (*pos).BV->GetTransform().RefToMatrixTransform() * translation * scale; 00303 00304 // BV Sphere's center and radius after the whole transformation from 00305 // BV's transformation to mesh's inverse transformation. 00306 // I.e., transforming the BV to mesh's coordinate 00307 T halfHeightOfBV = (*pos).BV->GetHeight() / 2.0; 00308 Vector3<T> centerOfBV_inMeshCoord = (mat * Vector4<T>(0,0,0,1)).GetVector3(); 00309 T radiusOfBV_inMeshCoord = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV_inMeshCoord ).Length(); 00310 Vector3<T> topPtOfBV_inMeshCoord( (mat * Vector4<T>(0,0,halfHeightOfBV,1)).GetVector3() ); 00311 Vector3<T> bottomPtOfBV_inMeshCoord( (mat * Vector4<T>(0,0,-halfHeightOfBV,1)).GetVector3() ); 00312 00313 // Inverse the mesh's transformation to the BV's pure transformation 00314 mat = m_pHEModel->GetTransform().GetMatrixTransform() * mat; 00315 00316 // BV Sphere's center and radius after the BV's pure transformation 00317 Vector3<T> centerOfBV( (mat * Vector4<T>(0,0,0,1)).GetVector3() ); 00318 radiusOfBV = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV ).Length(); 00319 Vector3<T> topPtOfBV( (mat * Vector4<T>(0,0,halfHeightOfBV,1)).GetVector3() ); 00320 Vector3<T> bottomPtOfBV( (mat * Vector4<T>(0,0,-halfHeightOfBV,1)).GetVector3() ); 00321 //--------------------------------------------- 00322 00323 //count = 0; 00324 for ( int i = 0; i < static_cast<int>( (*pos).NodeList.size() ); ++i ) { 00325 00326 bool testResult = false; 00327 00328 // Assume triangle is in Sphere Node 00329 // Test the triangle in sphere node with BVCylinder 00330 primB = (*pos).NodeList[i]->GetAPrimitiveHalfEdgeFace(); 00331 vertexList = primB->GetPtrsToVerticesAndNumberOfVertices( noOfVertices ); 00332 Vector3<T> cdrVec[3]; 00333 00334 //* 00335 // Test against each vertex in the triangle 00336 //----------------------------------- 00337 //for ( int j = 0; j < noOfVertices; ++j ) { 00338 for ( int j = 0; j < 3; ++j ) { 00339 if ( !vertexList[j]->GetMarkedStatus() ) { 00340 if( (*pos).BV->TestPointLocation( vertexList[j]->GetPosition(), &(cdrVec[j]) ) ) 00341 { 00342 //vertexList[j]->SetPosition( vertexList[j]->GetPosition() + cdrVec[j] ); 00343 //Vector3<T> force( (m_pHEModel->GetTransform().GetMatrixTransform() * Vector4<T>(cdrVec[j])).GetVector3() - vOrigin ); 00344 00345 //TAPs::CD::Fn::ListOfContactPts.push_back( Vector3<GLfloat>( centerOfBV[0], centerOfBV[1], centerOfBV[2] ) ); 00346 //TAPs::CD::Fn::ListOfForces.push_back( Vector3<GLfloat>( force[0], force[1], force[2] ) ); 00347 00348 vertexList[j]->SetMarkedStatus( true ); 00349 //pListOfCDObj->push_back( CDObj<T>( vertexList[j], vertexList[j]->GetPosition() + cdrVec[j] ) ); // create CDObj with the three collided vertices and the absolute position for resolving collision 00350 pListOfCDObj->push_back( CDObj<T>( vertexList[j], cdrVec[j] ) ); // create CDObj with the three collided vertices and the three distances for resolving collision 00351 testResult = true; 00352 } 00353 } 00354 } 00355 //----------------------------------- 00356 //*/ 00357 00358 /* 00359 // Test against triangle 00360 //----------------------------------- 00361 Vector3<T> contactPt; 00362 if ( TAPs::CGMath<T>::FindIntersectionCylinderTriangle( 00363 topPtOfBV_inMeshCoord, bottomPtOfBV_inMeshCoord, radiusOfBV_inMeshCoord, // BVCylinder properties 00364 vertexList[0]->GetPosition(), vertexList[1]->GetPosition(), vertexList[2]->GetPosition(), // triangle vertices 00365 cdrVec[0], cdrVec[1], cdrVec[2] // out vectors 00366 , &contactPt 00367 ) ) { 00368 //Vector3<T> force_1( cdrVec[0] * TAPs::CD::Fn::CDScaleForSphereBVWithTriangle ); 00369 //Vector3<T> force_2( cdrVec[1] * TAPs::CD::Fn::CDScaleForSphereBVWithTriangle ); 00370 //Vector3<T> force_3( cdrVec[2] * TAPs::CD::Fn::CDScaleForSphereBVWithTriangle ); 00371 00372 //vertexList[0]->SetPosition( vertexList[0]->GetPosition() + force_1 ); 00373 //vertexList[1]->SetPosition( vertexList[1]->GetPosition() + force_2 ); 00374 //vertexList[2]->SetPosition( vertexList[2]->GetPosition() + force_3 ); 00375 00376 //force_1 = (m_pHEModel->GetTransform().GetMatrixTransform() * Vector4<T>(force_1)).GetVector3() - vOrigin; 00377 //force_2 = (m_pHEModel->GetTransform().GetMatrixTransform() * Vector4<T>(force_2)).GetVector3() - vOrigin; 00378 //force_3 = (m_pHEModel->GetTransform().GetMatrixTransform() * Vector4<T>(force_3)).GetVector3() - vOrigin; 00379 00380 //contactPt = ( (m_pHEModel->GetTransform().GetMatrixTransform() * Vector4<T>(contactPt)).GetVector3() ); 00381 00382 //TAPs::CD::Fn::ListOfContactPts.push_back( Vector3<GLfloat>( contactPt[0], contactPt[1], contactPt[2] ) ); 00383 //Vector3<T> force( ( force_1 + force_2 + force_3 )/3 ); 00384 //TAPs::CD::Fn::ListOfForces.push_back( Vector3<GLfloat>( force[0], force[1], force[2] ) ); 00385 00386 pListOfCDObj->push_back( CDObj<T>( vertexList[j], cdrVec[0] ) ); 00387 pListOfCDObj->push_back( CDObj<T>( vertexList[j], cdrVec[1] ) ); 00388 pListOfCDObj->push_back( CDObj<T>( vertexList[j], cdrVec[2] ) ); 00389 00390 testResult = true; 00391 } 00392 //----------------------------------- 00393 //*/ 00394 00395 //std::cout << "\t\tCount#" << ++count << ": " << (*pos).NodeList[i] << "\n"; 00396 00397 // List of BVNodes to be updated 00398 if ( testResult ) { 00399 defPolyMesh_NodeSet.insert( (*pos).NodeList[i] ); 00400 bResult = true; 00401 } 00402 } 00403 } 00404 break; 00405 00406 default: 00407 std::cout << "CDR of BV Type (" << (*pos).BV->GetType() << ") doesn't support!\n"; 00408 break; 00409 } 00410 00411 // Restore the transformation of the bounding volume 00412 //(*pos).BV->GetTransform().SetMatrixTransform( trx.GetMatrixTransform() ); 00413 (*pos).BV->SetTransform( trx ); 00414 // Next Bounding Volume 00415 ++pos; 00416 } 00417 //------------------------------------------------- 00418 00419 //------------------------------------------------- 00420 // Update Normals of Deformable Polygonal Mesh 00421 //pPolygonalMeshObj->CalAndSetNormals(); // commented out to rely on FEM's AdvanceSimulation fn to update the normals 00422 //------------------------------------------------- 00423 // Update Bounding Volume Hierarchy of Deformable Polygonal Mesh 00424 //m_pHEModel->GetBVHTree()->Update( defPolyMesh_NodeSet ); // commputed out to rely on FEM's AdvanceSimulation fn to update the BVH tree 00425 SetCollisionStatus( true ); // collision status is set to tell FEM's AdvanceSimulation fn to update the BVH tree 00426 } 00427 //--------------------------------------------------------------- 00428 // if ( totalForce ) { 00429 // totalForce->SetXYZ( 0, 0, 0 ); 00430 // Vector3<float> force( 0, 0, 0 ); 00431 // for ( int i = 0; i < static_cast<int>( ListOfForces.size() ); ++i ) { 00432 // force += ListOfForces[i]; 00433 // } 00434 // force *= gain; 00435 // (*totalForce).SetXYZ( force[0], force[1], force[2] ); 00436 // } 00437 00438 //std::cout << "END: CDR(...) for HalfEdgeTrigonalModel" << std::endl; 00439 00440 return bResult; 00441 //*/ 00442 } 00443 00444 00445 00446 00447 //============================================================================= 00448 // Grabbing 00449 //----------------------------------------------------------------------------- 00450 template <typename T> 00451 bool ModelDefBasedOnFEM_CD<T>::GrabbedBy ( 00452 MultiBoundingVolume<T> * const pMBVObj, 00453 std::vector< CDObj<T> > * pListOfCDObj 00454 ) 00455 { 00456 // Clear the mark status of all vertices 00457 HEVertexList<T> * pVertexList = m_pHEModel->GetVertexList(); 00458 HEVertex<T> * pVertex = pVertexList->Head(); // Set to head 00459 while ( pVertexList->Current() != NULL ) { 00460 pVertexList->Current()->SetMarkedStatus( false ); 00461 pVertexList->NextCurrent(); 00462 } 00463 00464 #ifdef TAPs_DEBUG_COLLISION_DETECTION 00465 pPolygonalMeshObj->GetBVHTree()->ClearFlags(); 00466 #endif//TAPs_DEBUG_COLLISION_DETECTION 00467 00468 bool bResult = false; 00469 00470 // Collision Detection 00471 bResult = m_pHEModel->GetBVHTree()->TestOverlapWithTillLeafNodes( pMBVObj ); 00472 00473 //--------------------------------------------------------------- 00474 // Collision Response 00475 if ( bResult ) { 00476 00477 // Reset the result for primitive-primitive test, 00478 // because the previous result is only from bounding volume test. 00479 bResult = false; 00480 00481 //------------------------------------------------- 00482 //T separateDistanceConstraint; 00483 Vector3<T> N, centerA; 00484 //T separateDistance; 00485 HEFace<T> * primB; 00486 std::vector< HEVertex<T> * const > vertexList; 00487 int noOfVertices; 00488 //------------------------------------------------- 00489 TransformationSupport<T> trxMBV; // default constructor is an identity transformation 00490 00491 trxMBV = pMBVObj->GetTransform(); 00492 //------------------------------------------------- 00493 std::set< BVHNode<T> * > defPolyMesh_NodeSet; 00494 00495 // Get the list of collided nodes and bounding volumes 00496 std::vector< BVsAndNodesList<T> > * bvList = &m_pHEModel->GetBVHTree()->GetListOfCollidedBoundingVolumesAndNodes(); 00497 00498 //------------------------------------------------- 00499 // Traverse the list of bounding volumes 00500 std::vector< BVsAndNodesList<T> >::const_iterator pos = bvList->begin(); 00501 00502 //------------------------------------------------- 00503 // Mesh's inverse transformation 00504 Matrix4x4<T> trxInvMesh = m_pHEModel->GetTransform().RefToMatrixTransform(); 00505 trxInvMesh.Inversed(); 00506 Vector3<T> vOrigin( (m_pHEModel->GetTransform().RefToMatrixTransform() * Vector4<T>(0,0,0,1)).GetVector3() ); 00507 00508 while ( pos != bvList->end() ) { 00509 00510 // Store the transformation of the bounding volume including the mesh's inverse transformation 00511 TransformationSupport<T> trx = (*pos).BV->GetTransform(); 00512 (*pos).BV->GetTransform().SetMatrixTransform( trxInvMesh * trxMBV.RefToMatrixTransform() * trx.RefToMatrixTransform() ); 00513 00514 //----------------------------------------- 00515 // Deform the deformable polygonal mesh away from the rigid polygonal mesh 00516 switch ( (*pos).BV->GetType() ) { 00517 00518 case TAPs::Enum::BOUNDING_SPHERE: 00519 { 00520 // For Test against triangle 00521 //--------------------------------------------- 00522 // Transform the BV due to its transformation, center, and radius 00523 T radiusOfBV = (*pos).BV->GetRadius(); 00524 Vector3<T> C( (*pos).BV->GetCenter() ); 00525 Matrix4x4<T> translation( 1, 0, 0, C[0], 00526 0, 1, 0, C[1], 00527 0, 0, 1, C[2], 00528 0, 0, 0, 1 ); 00529 Vector3<T> S( radiusOfBV, radiusOfBV, radiusOfBV ); 00530 Matrix4x4<T> scale( S[0], 0, 0, 0, 00531 0, S[1], 0, 0, 00532 0, 0, S[2], 0, 00533 0, 0, 0, 1 ); 00534 // The BV's current transformation includes the mesh's inverse transformation (see the code above) 00535 Matrix4x4<T> mat = (*pos).BV->GetTransform().RefToMatrixTransform() * translation * scale; 00536 00537 // BV Sphere's center and radius after the whole transformation from 00538 // BV's transformation to mesh's inverse transformation. 00539 // I.e., transforming the BV to mesh's coordinate 00540 Vector3<T> centerOfBV_inMeshCoord = (mat * Vector4<T>(0,0,0,1)).GetVector3(); 00541 T radiusOfBV_inMeshCoord = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV_inMeshCoord ).Length(); 00542 00543 // Inverse the mesh's transformation to the BV's pure transformation 00544 mat = m_pHEModel->GetTransform().RefToMatrixTransform() * mat; 00545 00546 // BV Sphere's center and radius after the BV's pure transformation 00547 Vector3<T> centerOfBV( (mat * Vector4<T>(0,0,0,1)).GetVector3() ); 00548 radiusOfBV = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV ).Length(); 00549 //--------------------------------------------- 00550 00551 //count = 0; 00552 for ( unsigned int i = 0; i < (*pos).NodeList.size(); ++i ) { 00553 00554 bool testResult = false; 00555 00556 // Assume triangle is in Sphere Node 00557 // Test the triangle in sphere node with BVSphere 00558 primB = (*pos).NodeList[i]->GetAPrimitiveHalfEdgeFace(); 00559 vertexList = primB->GetPtrsToVerticesAndNumberOfVertices( noOfVertices ); 00560 Vector3<T> cdrVec[3]; 00561 00562 // Test against each vertex in the triangle 00563 //----------------------------------- 00564 //for ( int j = 0; j < noOfVertices; ++j ) { 00565 for ( int j = 0; j < 3; ++j ) { 00566 if ( !vertexList[j]->GetMarkedStatus() ) { 00567 if( (*pos).BV->TestPointLocation( vertexList[j]->GetPosition(), &(cdrVec[j]) ) ) 00568 { 00569 vertexList[j]->SetMarkedStatus( true ); 00570 pListOfCDObj->push_back( CDObj<T>( vertexList[j], cdrVec[j] ) ); // create CDObj with the three collided vertices and the three distances for resolving collision 00571 testResult = true; 00572 } 00573 } 00574 } 00575 //----------------------------------- 00576 // List of BVNodes to be updated 00577 if ( testResult ) { 00578 defPolyMesh_NodeSet.insert( (*pos).NodeList[i] ); 00579 bResult = true; 00580 } 00581 } 00582 } 00583 break; 00584 00585 case TAPs::Enum::BOUNDING_CYLINDER: 00586 { 00587 // For Test against triangle 00588 //--------------------------------------------- 00589 // Transform the BV due to its transformation, center, and radius 00590 T radiusOfBV = (*pos).BV->GetRadius(); 00591 Vector3<T> C( (*pos).BV->GetCenter() ); 00592 Matrix4x4<T> translation( 1, 0, 0, C[0], 00593 0, 1, 0, C[1], 00594 0, 0, 1, C[2], 00595 0, 0, 0, 1 ); 00596 Vector3<T> S( radiusOfBV, radiusOfBV, radiusOfBV ); 00597 Matrix4x4<T> scale( S[0], 0, 0, 0, 00598 0, S[1], 0, 0, 00599 0, 0, S[2], 0, 00600 0, 0, 0, 1 ); 00601 // The BV's current transformation includes the mesh's inverse transformation (see the code above) 00602 Matrix4x4<T> mat = (*pos).BV->GetTransform().RefToMatrixTransform() * translation * scale; 00603 00604 // BV Sphere's center and radius after the whole transformation from 00605 // BV's transformation to mesh's inverse transformation. 00606 // I.e., transforming the BV to mesh's coordinate 00607 T halfHeightOfBV = (*pos).BV->GetHeight() / 2.0; 00608 Vector3<T> centerOfBV_inMeshCoord = (mat * Vector4<T>(0,0,0,1)).GetVector3(); 00609 T radiusOfBV_inMeshCoord = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV_inMeshCoord ).Length(); 00610 Vector3<T> topPtOfBV_inMeshCoord( (mat * Vector4<T>(0,0,halfHeightOfBV,1)).GetVector3() ); 00611 Vector3<T> bottomPtOfBV_inMeshCoord( (mat * Vector4<T>(0,0,-halfHeightOfBV,1)).GetVector3() ); 00612 00613 // Inverse the mesh's transformation to the BV's pure transformation 00614 mat = m_pHEModel->GetTransform().GetMatrixTransform() * mat; 00615 00616 // BV Sphere's center and radius after the BV's pure transformation 00617 Vector3<T> centerOfBV( (mat * Vector4<T>(0,0,0,1)).GetVector3() ); 00618 radiusOfBV = ( (mat * Vector4<T>(1,0,0,1)) - centerOfBV ).Length(); 00619 Vector3<T> topPtOfBV( (mat * Vector4<T>(0,0,halfHeightOfBV,1)).GetVector3() ); 00620 Vector3<T> bottomPtOfBV( (mat * Vector4<T>(0,0,-halfHeightOfBV,1)).GetVector3() ); 00621 //--------------------------------------------- 00622 00623 //count = 0; 00624 for ( int i = 0; i < static_cast<int>( (*pos).NodeList.size() ); ++i ) { 00625 00626 bool testResult = false; 00627 00628 // Assume triangle is in Sphere Node 00629 // Test the triangle in sphere node with BVCylinder 00630 primB = (*pos).NodeList[i]->GetAPrimitiveHalfEdgeFace(); 00631 vertexList = primB->GetPtrsToVerticesAndNumberOfVertices( noOfVertices ); 00632 Vector3<T> cdrVec[3]; 00633 00634 // Test against each vertex in the triangle 00635 //----------------------------------- 00636 //for ( int j = 0; j < noOfVertices; ++j ) { 00637 for ( int j = 0; j < 3; ++j ) { 00638 if ( !vertexList[j]->GetMarkedStatus() ) { 00639 if( (*pos).BV->TestPointLocation( vertexList[j]->GetPosition(), &(cdrVec[j]) ) ) 00640 { 00641 vertexList[j]->SetMarkedStatus( true ); 00642 pListOfCDObj->push_back( CDObj<T>( vertexList[j], cdrVec[j] ) ); // create CDObj with the three collided vertices and the three distances for resolving collision 00643 testResult = true; 00644 } 00645 } 00646 } 00647 //----------------------------------- 00648 // List of BVNodes to be updated 00649 if ( testResult ) { 00650 defPolyMesh_NodeSet.insert( (*pos).NodeList[i] ); 00651 bResult = true; 00652 } 00653 } 00654 } 00655 break; 00656 00657 default: 00658 std::cout << "CDR of BV Type (" << (*pos).BV->GetType() << ") doesn't support!\n"; 00659 break; 00660 } 00661 00662 // Restore the transformation of the bounding volume 00663 (*pos).BV->SetTransform( trx ); 00664 // Next Bounding Volume 00665 ++pos; 00666 } 00667 //------------------------------------------------- 00668 SetCollisionStatus( true ); // collision status is set to tell FEM's AdvanceSimulation fn to update the BVH tree 00669 } 00670 return bResult; 00671 }//----------------------------------------------------------------------------- 00672 template <typename T> 00673 int ModelDefBasedOnFEM_CD<T>::GrabbedBy ( 00674 Vector3<T> * pSpherePos, 00675 T sphereRadius 00676 ) 00677 { 00678 // Check against the limit 00679 if ( m_ListOfGrabbedParts.size() >= MAX_NUMBER_OF_GRABBED_PARTS ) return -1; 00680 00681 // 00682 // Create a MBV for the sphere 00683 // 00684 MultiBoundingVolume<T> sphereMBV; 00685 BoundingVolume<T> * pSphereBV = new BoundingSphere<T>(); 00686 pSphereBV->SetRadius( sphereRadius ); 00687 pSphereBV->SetCenter( *pSpherePos ); 00688 sphereMBV.GetBoundingVolumeList().push_back( pSphereBV ); 00689 00690 //std::cout << "sphereMBV: " << sphereMBV << "\n"; 00691 //std::cout << "*pSphereBV: " << *pSphereBV << "\n"; 00692 //std::cout << "trx: " << sphereMBV.GetTransform().RefToMatrixTransform() << "\n"; 00693 00694 int id = -1; 00695 m_ListOfGrabbedParts.push_back( GrabbedPartData() ); 00696 00697 bool bResult = ModelDefBasedOnFEM_CD<T>::GrabbedBy( &sphereMBV, &m_ListOfGrabbedParts.back().GrabbedVertices ); 00698 if ( bResult ) { 00699 for ( int i = 0; i < MAX_NUMBER_OF_GRABBED_PARTS; ++i ) { 00700 if ( m_ListOfGrabbedParts_IsGrabbedStatus[i] == false ) { 00701 m_ListOfGrabbedParts_IsGrabbedStatus[i] = true; 00702 id = i; 00703 m_ListOfGrabbedParts.back().ID = i; 00704 m_ListOfGrabbedParts.back().pGrabbingPosition = pSpherePos; 00705 break; 00706 } 00707 } 00708 } 00709 else { 00710 m_ListOfGrabbedParts.back().Clear(); 00711 m_ListOfGrabbedParts.erase( m_ListOfGrabbedParts.end() - 1 ); 00712 } 00713 00714 return id; 00715 } 00717 //template <typename T> 00718 //int ModelDefBasedOnFEM_CD<T>::GrabbedBy ( 00719 // MultiBoundingVolume<T> * const pMBVObj //!< a multi bounding volume object 00720 //) 00721 //{ 00722 // // Check against the limit 00723 // if ( m_ListOfGrabbedParts.size() >= MAX_NUMBER_OF_GRABBED_PARTS ) return -1; 00724 // 00725 // int id = -1; 00726 // m_ListOfGrabbedParts.push_back( GrabbedPartData() ); 00727 // bool bResult = ModelDefBasedOnFEM_CD<T>::CORE_CDwith( pMBVObj, &m_ListOfGrabbedParts.back().GrabbedVertices ); 00728 // if ( bResult ) { 00729 // for ( int i = 0; i < MAX_NUMBER_OF_GRABBED_PARTS; ++i ) { 00730 // if ( m_ListOfGrabbedParts_IsGrabbedStatus[i] == false ) { 00731 // m_ListOfGrabbedParts_IsGrabbedStatus[i] = true; 00732 // id = i; 00733 // m_ListOfGrabbedParts.back().ID = i; 00734 // break; 00735 // } 00736 // } 00737 // } 00738 // else { 00739 // m_ListOfGrabbedParts.back().Clear(); 00740 // m_ListOfGrabbedParts.erase( m_ListOfGrabbedParts.end() - 1 ); 00741 // } 00742 // 00743 // return id; 00744 //} 00746 //template <typename T> 00747 //int ModelDefBasedOnFEM_CD<T>::GrabbedBy ( 00748 // MultiBoundingVolume<T> * const pMBVObj, //!< a multi bounding volume object 00749 // TransformationSupport<T> * const pTransform //!< an extra transformation for the MBV object 00750 //) 00751 //{ 00752 // // Check against the limit 00753 // if ( m_ListOfGrabbedParts.size() >= MAX_NUMBER_OF_GRABBED_PARTS ) return -1; 00754 // 00755 // int id = -1; 00756 // // Save MBV's transformation 00757 // TransformationSupport<T> trx = pMBVObj->GetTransform(); 00758 // // Add the extra transformation 00759 // pMBVObj->GetTransform().RefToMatrixTransform() = pTransform->RefToMatrixTransform() * pMBVObj->GetTransform().RefToMatrixTransform(); 00760 // m_ListOfGrabbedParts.push_back( GrabbedPartData() ); 00761 // bool bResult = ModelDefBasedOnFEM_CD<T>::CORE_CDwith( pMBVObj, &m_ListOfGrabbedParts.back().GrabbedVertices ); 00762 // if ( bResult ) { 00763 // for ( int i = 0; i < MAX_NUMBER_OF_GRABBED_PARTS; ++i ) { 00764 // if ( m_ListOfGrabbedParts_IsGrabbedStatus[i] == false ) { 00765 // m_ListOfGrabbedParts_IsGrabbedStatus[i] = true; 00766 // id = i; 00767 // m_ListOfGrabbedParts.back().ID = i; 00768 // break; 00769 // } 00770 // } 00771 // } 00772 // else { 00773 // m_ListOfGrabbedParts.back().Clear(); 00774 // m_ListOfGrabbedParts.erase( m_ListOfGrabbedParts.end() - 1 ); 00775 // } 00776 // pMBVObj->GetTransform() = trx; // restore MBV's transformation 00777 // 00778 // return id; 00779 //} 00780 //----------------------------------------------------------------------------- 00781 template <typename T> 00782 void ModelDefBasedOnFEM_CD<T>::ReleaseGrabbedPart ( int grabbedPartID ) 00783 { 00784 // Check against invalid range 00785 if ( grabbedPartID < 0 || grabbedPartID >= MAX_NUMBER_OF_GRABBED_PARTS ) return; 00786 00787 // Release the grabbed part 00788 if ( m_ListOfGrabbedParts_IsGrabbedStatus[ grabbedPartID ] ) { 00789 for ( std::vector< GrabbedPartData >::iterator it = m_ListOfGrabbedParts.begin(); it != m_ListOfGrabbedParts.end(); ++it ) { 00790 if ( grabbedPartID == it->ID ) { 00791 it->Clear(); 00792 m_ListOfGrabbedParts.erase( it ); 00793 m_ListOfGrabbedParts_IsGrabbedStatus[ grabbedPartID ] = false; 00794 break; 00795 } 00796 } 00797 } 00798 } 00799 //----------------------------------------------------------------------------- 00800 // Grabbing 00801 //============================================================================= 00802 00803 00804 00805 00806 //============================================================================= 00807 #if defined(__gl_h_) || defined(__GL_H__) 00808 //----------------------------------------------------------------------------- 00809 //----------------------------------------------------------------------------- 00810 #endif // #if defined(__gl_h_) || defined(__GL_H__) 00811 //============================================================================= 00812 00813 //============================================================================= 00814 END_NAMESPACE_TAPs__FEM 00815 //34567890123456789012345678901234567890123456789012345678901234567890123456789 00816 //--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----
1.7.4 
