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