COME - Biomodel

00001 /***************************************************************************
00002  *   Copyright (C) 2002 by Anderson Maciel                                 *
00003  *   andi.maciel@gmail.com                                                 *
00004  *                                                                         *
00005  *   This program is free software; you can redistribute it and/or modify  *
00006  *   it under the terms of the GNU General Public License as published by  *
00007  *   the Free Software Foundation; either version 2 of the License, or     *
00008  *   (at your option) any later version.                                   *
00009  *                                                                         *
00010  *   This program is distributed in the hope that it will be useful,       *
00011  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00012  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
00013  *   GNU General Public License for more details.                          *
00014  *                                                                         *
00015  *   You should have received a copy of the GNU General Public License     *
00016  *   along with this program; if not, write to the                         *
00017  *   Free Software Foundation, Inc.,                                       *
00018  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
00019  **************************************************************************/
00020 
00031 
00032 #include        <physics/comecollide.h>
00033 #include        <algebra/comemesh.h>
00034 #include        <algebra/comevertex3d.h>
00035 #include        <bio/comebiostructure.h>
00036 #include        <bio/comemoleculescartilage.h>
00037 #include        <bio/comemolecule.h>
00038 #include        <general/comescenario.h>
00039 #include        <RAPID/RAPID.H>
00040 #include        <VCollide.H>
00041 
00042 #include        <time.h>
00043 #include        <math.h>
00044 
00048 COME_Collide :: COME_Collide( COME_Scenario* scene ){
00049 
00050         parent = scene;
00051         collisions1stLevel = ENVELOPE;
00052         if( COME::flagCollisionTreatment == NEIGHBORS ){
00053                 initializeProximityStructure();
00054         }
00055         softhard = SOFT;
00056         CONTADIST = 0;
00057 }
00058 
00059 
00060 COME_Collide :: ~COME_Collide(){
00061 
00062         collisions.clear();
00063         
00064 }
00065 
00069 
00070 void
00071 COME_Collide :: setCollisions1stLevel( int detectionType ) {
00072 
00073         collisions1stLevel = detectionType;
00074 }
00075 
00079 
00080 int             
00081 COME_Collide :: getNumberOfCollisions() const{
00082 
00083         return collisions.size();
00084 
00085 }
00086 
00087 vector<COME_Collision>  
00088 COME_Collide :: getCollisions() const{
00089 
00090         return collisions;
00091 
00092 }
00093 
00094 int             
00095 COME_Collide :: getCollisions1stLevel() const{
00096 
00097         return collisions1stLevel;
00098 
00099 }
00100 
00101 
00105 void    
00106 COME_Collide :: resetCollisions(){
00107 
00108         collisions.clear();
00109 
00110 }
00111 
00112 
00113 
00119 void            
00120 COME_Collide :: treatCollisions() {
00121 
00122 
00123         COME_Patient *activePatient = ((COME_Scenario*)parent)->getPatient(((COME_Scenario*)parent)->getPatientList()->size()-1);
00124         // If there's only one object, doesn't detect collisions
00125         if( activePatient->getPtOrganList()->size() > 1 ){
00126 
00127                 // Take the organs of the first patient (there is only one patientin this phase)
00128                 list<COME_MoleculesCartilage*>::const_iterator iterOrgans1;
00129                 for ( iterOrgans1 =((list<COME_MoleculesCartilage*>*)activePatient->getPtOrganList())->begin(); iterOrgans1 != ((list<COME_MoleculesCartilage*>*)activePatient->getPtOrganList())->end(); iterOrgans1++  ){
00130                         
00131                         COME_Point3D mins1, maxs1;
00132                         (*iterOrgans1)->getEnvelop( mins1, maxs1 );
00133                         
00134                         list<COME_MoleculesCartilage*>::const_iterator iterOrgans2;
00135                         for ( iterOrgans1++, iterOrgans2 = iterOrgans1, iterOrgans1--; iterOrgans2 != ((list<COME_MoleculesCartilage*>*)activePatient->getPtOrganList())->end(); iterOrgans2++  ){
00136                                 
00137                                 if( (*iterOrgans1) != (*iterOrgans2) ){
00138                                         COME_Point3D mins2, maxs2;
00139                                         (*iterOrgans2)->getEnvelop( mins2, maxs2 );
00140 
00141                                         if( ( ( mins1.getX() > maxs2.getX() ) || ( maxs1.getX() < mins2.getX() ) ) ||
00142                                                 ( ( mins1.getY() > maxs2.getY() ) || ( maxs1.getY() < mins2.getY() ) ) ||
00143                                                 ( ( mins1.getZ() > maxs2.getZ() ) || ( maxs1.getZ() < mins2.getZ() ) ) ){
00144 
00145                                                 // do not collide
00146                                         } else {
00147 
00148                                                 // test spheres
00149                                                 list<COME_Molecule*>::const_iterator iterMolec1;
00150                                                 for ( iterMolec1 = (*iterOrgans1)->getTissue()->getShape()->begin(); iterMolec1 != (*iterOrgans1)->getTissue()->getShape()->end(); iterMolec1++  ){
00151 
00152                                                         list<COME_Molecule*>::const_iterator iterMolec2;
00153                                                         for ( iterMolec2 = (*iterOrgans2)->getTissue()->getShape()->begin(); iterMolec2 != (*iterOrgans2)->getTissue()->getShape()->end(); iterMolec2++  ){
00154 
00155                                                                 double actualDistance = (*iterMolec1)->getGlobalPosition().vpDistance( (*iterMolec2)->getGlobalPosition() );
00156                                                                 double allowedDistance = ( (*iterMolec1)->getRadius() + (*iterMolec2)->getRadius() );
00157                                                                 if( actualDistance < allowedDistance ){
00158                                                                         // there is collision
00159                                                                         
00160                                                                         double penetrationDistance = allowedDistance - actualDistance;
00161 
00162                                                                         double springConstant = ( ( (*iterMolec1)->getMaterial()->getYoungsModulus() + (*iterMolec2)->getMaterial()->getYoungsModulus() ) / 2.0 ) * allowedDistance;
00163 
00164                                                                         COME_Vector3D forceDirection; forceDirection.setVector3D( (*iterMolec1)->getGlobalPosition() - (*iterMolec2)->getGlobalPosition() );
00165                                                                         forceDirection.vpNormalize();
00166 
00167                                                                         (*iterMolec1)->addCollisionForce( ( forceDirection * penetrationDistance ) * springConstant );
00168                                                                         (*iterMolec2)->addCollisionForce( ( forceDirection * penetrationDistance ) * -springConstant );
00169 
00170                                                                 }
00171                                                         }
00172                                                 }
00173                                         }
00174                                 }
00175                         }
00176                 }
00177         }
00178 }
00179 
00180 
00184 void            
00185 COME_Collide :: detectContacts() {
00186 
00187 
00188         resetCollisions();
00189         
00190         COME_Patient *activePatient = ((COME_Scenario*)parent)->getPatient(((COME_Scenario*)parent)->getPatientList()->size()-1);
00191         
00192         VCollide        vc;  //V-Collide collision detection library
00193         int*            objects = new int[((COME_Scenario*) parent)->getPatient(0)->getPtOrganList()->size()];
00194         int                     iPosObject = 0;
00195 
00196         // If there's only one object, doesn't detect collisions
00197         if( activePatient->getPtOrganList()->size() > 2 ){
00198 
00199                 
00200                 if( collisions1stLevel == ENVELOPE ){
00201                         // Test envelope.
00202                         // Take the organs of the first patient (there is only one patientin this phase)
00203                         list<COME_MoleculesCartilage*>::const_iterator iterOrgans1;
00204                         for ( iterOrgans1 =((list<COME_MoleculesCartilage*>*)activePatient->getPtOrganList())->begin(); iterOrgans1 != ((list<COME_MoleculesCartilage*>*)activePatient->getPtOrganList())->end(); iterOrgans1++  ){
00205                                 
00206                                 COME_Point3D mins1, maxs1;
00207                                 (*iterOrgans1)->getEnvelop( mins1, maxs1 );
00208                                 
00209                                 list<COME_MoleculesCartilage*>::const_iterator iterOrgans2;
00210                                 for ( iterOrgans1++, iterOrgans2 = iterOrgans1, iterOrgans1--; iterOrgans2 != ((list<COME_MoleculesCartilage*>*)activePatient->getPtOrganList())->end(); iterOrgans2++  ){
00211                                         
00213                                         if( ( (*iterOrgans1)->isFixed() ) && ( (*iterOrgans2)->isFixed() ) ) continue;
00214                                         if( (*iterOrgans1) != (*iterOrgans2) ){
00215                                                 COME_Point3D mins2, maxs2;
00216                                                 (*iterOrgans2)->getEnvelop( mins2, maxs2 );
00217 
00218                                                 if( ( ( mins1.getX() > maxs2.getX() ) || ( maxs1.getX() < mins2.getX() ) ) ||
00219                                                         ( ( mins1.getY() > maxs2.getY() ) || ( maxs1.getY() < mins2.getY() ) ) ||
00220                                                         ( ( mins1.getZ() > maxs2.getZ() ) || ( maxs1.getZ() < mins2.getZ() ) ) ){
00221 
00222                                                         // do not collide
00223                                                 } else {
00224 
00225                                                         //call RAPID collision detection routine
00226                                                         if (COME::flagCollisionTreatment == HYBRID ){
00227                                                                 detectContactsHybrid( (*iterOrgans1), (*iterOrgans2) );
00228                                                         } else {
00229                                                                 detectContacts( (*iterOrgans1), (*iterOrgans2) );
00230                                                         }
00231                                                 }
00232                                         }
00233                                 }
00234                         }
00235                 } else { // verify collision pairs by V-Collide
00236                 
00237                         // Take the organs of the first patient (there is only one patientin this phase)
00238                         list<COME_BioStructure*>::const_iterator iterOrgans;
00239 
00240                         for ( iterOrgans =((list<COME_BioStructure*>*)activePatient->getPtOrganList())->begin(); iterOrgans != ((list<COME_BioStructure*>*)activePatient->getPtOrganList())->end(); iterOrgans++  ){
00241                                 
00242                                 //(*iterOrgans)->updateSurface(DEFORM); ////
00243                                 COME_Mesh *mesh = (*iterOrgans)->getSurface();
00244 
00245                                 //add the objects to the library                                
00246                                 vc.NewObject( &( objects[ iPosObject ] ) );
00247                                 
00248                                 //add triangles
00249                                 for ( int face = 0; face < mesh->getNumberFaces(); face++ ){
00250                                                         
00251                                         //read the object of the Mesh Class 
00252                                         vector<COME_Vertex3D> vertices;
00253                                         vertices = mesh->getVerticesFace( face );
00254 
00255                                         double v1[ 3 ], v2[ 3 ], v3[ 3 ];
00256                                         v1[ 0 ] = vertices[ 0 ].getX();
00257                                         v1[ 1 ] = vertices[ 0 ].getY();
00258                                         v1[ 2 ] = vertices[ 0 ].getZ();
00259 
00260                                         v2[ 0 ] = vertices[ 1 ].getX();
00261                                         v2[ 1 ] = vertices[ 1 ].getY();
00262                                         v2[ 2 ] = vertices[ 1 ].getZ();
00263 
00264                                         v3[ 0 ] = vertices[ 2 ].getX();
00265                                         v3[ 1 ] = vertices[ 2 ].getY();
00266                                         v3[ 2 ] = vertices[ 2 ].getZ();
00267 
00268                                         vc.AddTri( v1, v2, v3 );
00269                                 }
00270       
00271                         vc.EndObject();           
00272                                 iPosObject++;
00273                         }
00274                         
00275                 /*      //transformation matrices.
00276                         double R1[4][4]; 
00277                         R1[0][0] = R1[1][1] = R1[2][2] = R1[3][3] = 1.0;
00278                         R1[0][1] = R1[1][0] = R1[2][0] = R1[3][0] = 0.0;
00279                         R1[0][2] = R1[1][2] = R1[2][1] = R1[3][1] = 0.0;
00280                         R1[0][3] = R1[1][3] = R1[2][3] = R1[3][2] = 0.0;
00281                         vc.UpdateTrans( objects[ 0 ], R1 );    
00282                         vc.UpdateTrans( objects[ 1 ], R1 );    */
00283 
00284                         vc.Collide();  //perform collision test
00285       
00286                         //report the results.
00287                         int VCReportSize = iPosObject;  //size of report
00288                         VCReportType *vcReport = new VCReportType[ VCReportSize ];
00289       
00290                         int numberOfCollidingPairs = vc.Report( VCReportSize, vcReport );
00291       
00292                         if ( numberOfCollidingPairs > VCReportSize ){
00293                                   
00294                                 VCReportSize = numberOfCollidingPairs;
00295                                 delete vcReport;
00296                                 vcReport = new VCReportType[ VCReportSize ];
00297                                 numberOfCollidingPairs = vc.Report( VCReportSize, vcReport );
00298                         }
00299       
00300                         for ( int j = 0; j < numberOfCollidingPairs; j++ ){
00301 
00302                                 printf( "Detected collision between objects %d and %d \n", vcReport[ j ].id1, vcReport[ j ].id2 );
00303 
00304                                 //call RAPID collision detection routine
00305                                 detectContacts( activePatient->getOrgan( vcReport[ j ].id1 ), activePatient->getOrgan( vcReport[ j ].id2 ) );
00306                         }
00307                 }
00308         } else if( activePatient->getPtOrganList()->size() == 2 ){
00309 
00310                         detectContacts( activePatient->getOrgan( 0 ), activePatient->getOrgan( 1 ) );
00311                         //printf( "collision 2 obj" );
00312         }
00313 
00314 }
00315          
00319 void            
00320 COME_Collide :: detectContacts( COME_BioStructure* organ1, COME_BioStructure* organ2 ) {  
00321 
00322         RAPID_model *object1    = new RAPID_model;
00323         RAPID_model *object2    = new RAPID_model;
00324         
00325         double cCollided[] = {1.0, 0.4, 0.4, 1.0};
00326         double cNOCollided[] = {0.4, 1.0, 1.0, 1.0};
00327                                 
00328         int face;
00329         
00330         // Take the organs of the first patient (there is only one patientin this phase)
00331 
00332         COME_Mesh *meshOrgan1 = organ1->getSurface();
00333 
00334         int iId = 0;
00335                 
00336         //add the first organ to the library                            
00337         object1->BeginModel();
00338                         
00339         //add triangles   
00340         for ( face = 0; face < meshOrgan1->getNumberFaces(); face++ ){
00341                                                 
00342                 //read each face of the Mesh Class 
00343                 vector<COME_Vertex3D> vertices;
00344                 vertices = meshOrgan1->getVerticesFaceGlobalPosition( face );
00345 
00346                 double v1[ 3 ], v2[ 3 ], v3[ 3 ];
00347                 v1[ 0 ] = vertices[ 0 ].getX();
00348                 v1[ 1 ] = vertices[ 0 ].getY();
00349                 v1[ 2 ] = vertices[ 0 ].getZ();
00350 
00351                 v2[ 0 ] = vertices[ 1 ].getX();
00352                 v2[ 1 ] = vertices[ 1 ].getY();
00353                 v2[ 2 ] = vertices[ 1 ].getZ();
00354 
00355                 v3[ 0 ] = vertices[ 2 ].getX();
00356                 v3[ 1 ] = vertices[ 2 ].getY();
00357                 v3[ 2 ] = vertices[ 2 ].getZ();
00358 
00359                 object1->AddTri( v1, v2, v3, iId );
00360                 
00361                 meshOrgan1->getAFacePt( face )->getVertexGlobalPositionPt(0)->setColor( cNOCollided );
00362                 meshOrgan1->getAFacePt( face )->getVertexGlobalPositionPt(1)->setColor( cNOCollided );
00363                 meshOrgan1->getAFacePt( face )->getVertexGlobalPositionPt(2)->setColor( cNOCollided );
00364                 
00365                 iId++;
00366         }
00367                 
00368      
00369         object1->EndModel();
00370                 
00371         COME_Mesh *meshOrgan2 = organ2->getSurface();
00372         iId             = 0;
00373         
00374         object2->BeginModel();
00375                         
00376         //add triangles   
00377         for ( face = 0; face < meshOrgan2->getNumberFaces(); face++ ){
00378                                                 
00379                 //read the object of the Mesh Class 
00380                 vector<COME_Vertex3D> vertices;
00381                 vertices = meshOrgan2->getVerticesFaceGlobalPosition( face );
00382 
00383                 double v1[ 3 ], v2[ 3 ], v3[ 3 ];
00384                 v1[ 0 ] = vertices[ 0 ].getX();
00385                 v1[ 1 ] = vertices[ 0 ].getY();
00386                 v1[ 2 ] = vertices[ 0 ].getZ();
00387 
00388                 v2[ 0 ] = vertices[ 1 ].getX();
00389                 v2[ 1 ] = vertices[ 1 ].getY();
00390                 v2[ 2 ] = vertices[ 1 ].getZ();
00391 
00392                 v3[ 0 ] = vertices[ 2 ].getX();
00393                 v3[ 1 ] = vertices[ 2 ].getY();
00394                 v3[ 2 ] = vertices[ 2 ].getZ();
00395 
00396                 object2->AddTri( v1, v2, v3, iId );
00397                 
00398                 meshOrgan2->getAFacePt( face )->getVertexGlobalPositionPt(0)->setColor( cNOCollided );
00399                 meshOrgan2->getAFacePt( face )->getVertexGlobalPositionPt(1)->setColor( cNOCollided );
00400                 meshOrgan2->getAFacePt( face )->getVertexGlobalPositionPt(2)->setColor( cNOCollided );
00401                 
00402                 iId++;
00403                 
00404         }
00405       
00406         object2->EndModel();          
00407         
00408         
00409         //set up the rotation and translation matrices as
00410         //required by RAPID for the objects.
00411         double R[3][3], T[3];
00412 
00413         R[0][0] = R[1][1] = R[2][2] = 1.0;
00414         R[0][1] = R[1][0] = R[2][0] = 0.0;
00415         R[0][2] = R[1][2] = R[2][1] = 0.0;
00416 
00417         T[0] = 0.0; T[1] = 0.0; T[2] = 0.0;
00418 
00419         //call the RAPID collision detection routine.
00420         RAPID_Collide( R, T, object1, R, T, object2, RAPID_ALL_CONTACTS );
00421                 
00422         //report the results of collision detection.
00423         //printf ( "Num contact pairs %d\n", RAPID_num_contacts  );
00424         for ( int i = 0; i < RAPID_num_contacts; i++ ){
00425 
00426                 //printf( "\t contact %4d: tri %4d and %4d\n ", i, RAPID_contact[i].id1, RAPID_contact[i].id2 );
00427                 COME_Collision collision = COME_Collision( organ1, RAPID_contact[ i ].id1, organ2, RAPID_contact[ i ].id2 );
00428         
00429                 collisions.push_back( collision );  
00430                 meshOrgan1->getAFacePt( RAPID_contact[ i ].id1 )->getVertexGlobalPositionPt(0)->setColor( cCollided );
00431                 meshOrgan1->getAFacePt( RAPID_contact[ i ].id1 )->getVertexGlobalPositionPt(1)->setColor( cCollided );
00432                 meshOrgan1->getAFacePt( RAPID_contact[ i ].id1 )->getVertexGlobalPositionPt(2)->setColor( cCollided );
00433                 meshOrgan2->getAFacePt( RAPID_contact[ i ].id2 )->getVertexGlobalPositionPt(0)->setColor( cCollided );
00434                 meshOrgan2->getAFacePt( RAPID_contact[ i ].id2 )->getVertexGlobalPositionPt(1)->setColor( cCollided );
00435                 meshOrgan2->getAFacePt( RAPID_contact[ i ].id2 )->getVertexGlobalPositionPt(2)->setColor( cCollided );
00436 
00437         }
00438 
00439         delete object1;
00440         delete object2;
00441 }
00442 
00448 void            
00449 COME_Collide :: detectContactsHybrid( COME_BioStructure* organ1, COME_BioStructure* organ2 ) {  
00450 
00451         RAPID_model *object1    = new RAPID_model;
00452         RAPID_model *object2    = new RAPID_model;
00453         
00454         //int   objects[2];
00455         int face;
00456         
00457         // test spheres (anchors)
00458         list<COME_Molecule*>::const_iterator iterMolec1;
00459         for ( iterMolec1 = organ1->getTissue()->getShape()->begin(); iterMolec1 != organ1->getTissue()->getShape()->end(); iterMolec1++  ){
00460 
00461                 list<COME_Molecule*>::const_iterator iterMolec2;
00462                 for ( iterMolec2 = organ2->getTissue()->getShape()->begin(); iterMolec2 != organ2->getTissue()->getShape()->end(); iterMolec2++  ){
00463 
00464                         double actualDistance = (*iterMolec1)->getGlobalPosition().vpDistance( (*iterMolec2)->getGlobalPosition() );
00465                         double allowedDistance = ( (*iterMolec1)->getRadius() + (*iterMolec2)->getRadius() );
00466                         if( actualDistance < allowedDistance ){
00467                                 // there is collision
00468                                 (*iterMolec1)->setCollide( true );
00469                                 (*iterMolec2)->setCollide( true );
00470                         }
00471                 }
00472         }
00473         
00474         // Take the organs of the first patient (there is only one patientin this phase)
00475 
00476         COME_Mesh *meshOrgan1 = organ1->getSurface();
00477 
00478         int iId = 0;
00479                 
00480         //add the first organ to the library                            
00481         object1->BeginModel();
00482                         
00483         //add triangles   
00484         for ( face = 0; face < meshOrgan1->getNumberFaces(); face++ ){
00485                                                 
00486                 //read each face of the Mesh Class 
00487                 vector<COME_Vertex3D> vertices;
00488                 vertices = meshOrgan1->getVerticesFaceGlobalPosition( face );
00489 
00490                 if( ( vertices[0].anchorsCollide() ) ||
00491                     ( vertices[1].anchorsCollide() ) ||
00492                     ( vertices[2].anchorsCollide() ) ){
00493                         double v1[ 3 ], v2[ 3 ], v3[ 3 ];
00494                         v1[ 0 ] = vertices[ 0 ].getX();
00495                         v1[ 1 ] = vertices[ 0 ].getY();
00496                         v1[ 2 ] = vertices[ 0 ].getZ();
00497         
00498                         v2[ 0 ] = vertices[ 1 ].getX();
00499                         v2[ 1 ] = vertices[ 1 ].getY();
00500                         v2[ 2 ] = vertices[ 1 ].getZ();
00501         
00502                         v3[ 0 ] = vertices[ 2 ].getX();
00503                         v3[ 1 ] = vertices[ 2 ].getY();
00504                         v3[ 2 ] = vertices[ 2 ].getZ();
00505         
00506                         object1->AddTri( v1, v2, v3, iId );
00507                         iId++;
00508                 }
00509         }
00510                 
00511      
00512         object1->EndModel();
00513                 
00514         COME_Mesh *meshOrgan2 = organ2->getSurface();
00515         iId             = 0;
00516         
00517         object2->BeginModel();
00518                         
00519         //add triangles   
00520         for ( face = 0; face < meshOrgan2->getNumberFaces(); face++ ){
00521                                                 
00522                 //read the object of the Mesh Class 
00523                 vector<COME_Vertex3D> vertices;
00524                 vertices = meshOrgan2->getVerticesFaceGlobalPosition( face );
00525 
00526                 if( ( vertices[0].anchorsCollide() ) ||
00527                     ( vertices[1].anchorsCollide() ) ||
00528                     ( vertices[2].anchorsCollide() ) ){
00529                         double v1[ 3 ], v2[ 3 ], v3[ 3 ];
00530                         v1[ 0 ] = vertices[ 0 ].getX();
00531                         v1[ 1 ] = vertices[ 0 ].getY();
00532                         v1[ 2 ] = vertices[ 0 ].getZ();
00533         
00534                         v2[ 0 ] = vertices[ 1 ].getX();
00535                         v2[ 1 ] = vertices[ 1 ].getY();
00536                         v2[ 2 ] = vertices[ 1 ].getZ();
00537         
00538                         v3[ 0 ] = vertices[ 2 ].getX();
00539                         v3[ 1 ] = vertices[ 2 ].getY();
00540                         v3[ 2 ] = vertices[ 2 ].getZ();
00541         
00542                         object2->AddTri( v1, v2, v3, iId );
00543                         iId++;
00544                 }
00545                 
00546         }
00547       
00548         object2->EndModel();          
00549         
00550         
00551         //set up the rotation and translation matrices as
00552         //required by RAPID for the objects.
00553         double R[3][3], T[3];
00554 
00555         R[0][0] = R[1][1] = R[2][2] = 1.0;
00556         R[0][1] = R[1][0] = R[2][0] = 0.0;
00557         R[0][2] = R[1][2] = R[2][1] = 0.0;
00558 
00559         T[0] = 0.0; T[1] = 0.0; T[2] = 0.0;
00560 
00561         //call the RAPID collision detection routine.
00562         RAPID_Collide( R, T, object1, R, T, object2, RAPID_ALL_CONTACTS );
00563       
00564         
00565         //report the results of collision detection.
00566         //printf ( "Num contact pairs %d\n", RAPID_num_contacts  );
00567         for ( int i = 0; i < RAPID_num_contacts; i++ ){
00568 
00569                 //printf( "\t contact %4d: tri %4d and %4d\n ", i, RAPID_contact[i].id1, RAPID_contact[i].id2 );
00570                 COME_Collision collision = COME_Collision( organ1, RAPID_contact[ i ].id1, organ2, RAPID_contact[ i ].id2 );
00571         
00572                 collisions.push_back( collision );  
00573 
00574         }
00575         
00576         // reset spheres (anchors)
00577         list<COME_Molecule*>::const_iterator iterMolec;
00578         for ( iterMolec = organ1->getTissue()->getShape()->begin(); iterMolec != organ1->getTissue()->getShape()->end(); iterMolec++  ){
00579                 (*iterMolec)->setCollide( false );
00580         }
00581         for ( iterMolec = organ2->getTissue()->getShape()->begin(); iterMolec != organ2->getTissue()->getShape()->end(); iterMolec++  ){
00582                 (*iterMolec)->setCollide( false );
00583         }
00584 
00585         delete object1;
00586         delete object2;
00587 }
00588 
00589 
00593 bool            
00594 COME_Collide :: detectContactsDisplacement() {
00595 
00596 //printf( "Entrou detectContactsDisp01 \n" );
00597 
00598         bool collided = false;
00599         COME_Patient *activePatient = ((COME_Scenario*)parent)->getPatient(((COME_Scenario*)parent)->getPatientList()->size()-1);
00600         
00601         
00602         // reset Collision Vectors
00603         list<COME_MoleculesCartilage*>::const_iterator iterOrgansReset;
00604         for ( iterOrgansReset =((list<COME_MoleculesCartilage*>*)activePatient->getPtOrganList())->begin(); iterOrgansReset != ((list<COME_MoleculesCartilage*>*)activePatient->getPtOrganList())->end(); iterOrgansReset++  ){
00605                 (*iterOrgansReset)->getSurface()->resetCollisionDisps();
00606         }
00607         
00608         // If there's only one object, doesn't detect collisions
00609         if( activePatient->getPtOrganList()->size() > 2 ){
00610 
00611                 
00612                 //if( collisions1stLevel == ENVELOPE ){
00613                         // Test envelope.
00614                         // Take the organs of the first patient (there is only one patientin this phase)
00615                         list<COME_MoleculesCartilage*>::const_iterator iterOrgans1;
00616                         for ( iterOrgans1 =((list<COME_MoleculesCartilage*>*)activePatient->getPtOrganList())->begin(); iterOrgans1 != ((list<COME_MoleculesCartilage*>*)activePatient->getPtOrganList())->end(); iterOrgans1++  ){
00617                                 
00618                                 //COME_Point3D mins1, maxs1;
00619                                 //(*iterOrgans1)->getEnvelop( mins1, maxs1 );
00620                                 
00621                                 list<COME_MoleculesCartilage*>::const_iterator iterOrgans2;
00622                                 for ( iterOrgans1++, iterOrgans2 = iterOrgans1, iterOrgans1--; iterOrgans2 != ((list<COME_MoleculesCartilage*>*)activePatient->getPtOrganList())->end(); iterOrgans2++  ){
00623                                         
00625                                         if( ( (*iterOrgans1)->isFixed() ) && ( (*iterOrgans2)->isFixed() ) ) continue;
00626                                         if( (*iterOrgans1) != (*iterOrgans2) ){
00627                                                 //COME_Point3D mins2, maxs2;
00628                                                 //(*iterOrgans2)->getEnvelop( mins2, maxs2 );
00629 
00630                                                 /*if( ( ( mins1.getX() > maxs2.getX() ) || ( maxs1.getX() < mins2.getX() ) ) ||
00631                                                         ( ( mins1.getY() > maxs2.getY() ) || ( maxs1.getY() < mins2.getY() ) ) ||
00632                                                         ( ( mins1.getZ() > maxs2.getZ() ) || ( maxs1.getZ() < mins2.getZ() ) ) ){
00633 
00634                                                         // do not collide
00635                                                 } else {*/
00636                                                         if(
00637                                                         ( ( (*iterOrgans1)->getDescription() == "labrum" ) || ( (*iterOrgans2)->getDescription() == "labrum" ) )
00638                                                         ||
00639                                                         ( ( ( (*iterOrgans1)->getDescription() == "ischiofemoral" ) && ( (*iterOrgans2)->getDescription() == "labrum" ) )
00640                                                         || ( ( (*iterOrgans2)->getDescription() == "ischiofemoral" ) && ( (*iterOrgans1)->getDescription() == "labrum" ) ) )
00641                                                         ||
00642                                                         ( ( ( (*iterOrgans1)->getDescription() == "ischiofemoral" ) && ( (*iterOrgans2)->getDescription() == "pelvisbone" ) )
00643                                                         || ( ( (*iterOrgans2)->getDescription() == "ischiofemoral" ) && ( (*iterOrgans1)->getDescription() == "pelvisbone" ) ) )
00644                                                         ){
00645                                                         // do nothing
00646                                                         } else {
00647                                                                 if( detectContactsDisplacement( (*iterOrgans1), (*iterOrgans2) ) )
00648                                                                         collided = true;
00649                                                         }
00650                                                 //}
00651                                         }
00652                                 }
00653                         }
00654                 //}
00655         } else if( activePatient->getPtOrganList()->size() == 2 ){
00656 
00657                 if( detectContactsDisplacement( activePatient->getOrgan( 0 ), activePatient->getOrgan( 1 ) ) )
00658                         collided = true;
00659         }
00660         
00661         return collided;
00662 
00663 }
00664 
00669 bool
00670 COME_Collide :: detectContactsDisplacement( COME_BioStructure* organ1, COME_BioStructure* organ2 ) {  
00671 
00672 //printf( "Entrou detectContactsDisp02 \n" );
00673         
00674         RAPID_model *object1    = new RAPID_model;
00675         RAPID_model *object2    = new RAPID_model;
00676         
00677         double cCollided[] = {1.0, 0.4, 0.4, 1.0};
00678         double cCollidedTotal[] = {0.1, 0.1, 1.0, 1.0};
00679         double cNOCollided[] = {0.4, 1.0, 1.0, 1.0};
00680         double cNOTenabled[] = {1.0, 1.0, 1.0, 1.0};
00681         
00682         //int   objects[2];
00683         bool collided = false;
00684         int face;
00685         
00686         // Take the organs of the first patient (there is only one patientin this phase)
00687 
00688         COME_Mesh *meshOrgan1 = organ1->getSurface();
00689 
00690         int iId = 0;
00691                 
00692         //add the first organ to the library                            
00693         object1->BeginModel();
00694                         
00695         //add triangles   
00696         for ( face = 0; face < meshOrgan1->getNumberFaces(); face++ ){
00697                                                 
00698                 meshOrgan1->getAFacePt( face )->getVertexGlobalPositionPt(0)->setColor( cNOTenabled );
00699                 meshOrgan1->getAFacePt( face )->getVertexGlobalPositionPt(1)->setColor( cNOTenabled );
00700                 meshOrgan1->getAFacePt( face )->getVertexGlobalPositionPt(2)->setColor( cNOTenabled );
00701                         
00702                 if( meshOrgan1->getAFacePt(face)->getCollisionDetectable() ){
00703                         //read each face of the Mesh Class 
00704                         vector<COME_Vertex3D> vertices;
00705                         vertices = meshOrgan1->getVerticesFaceGlobalPosition( face );
00706         
00707                         double v1[ 3 ], v2[ 3 ], v3[ 3 ];
00708                         v1[ 0 ] = vertices[ 0 ].getX();
00709                         v1[ 1 ] = vertices[ 0 ].getY();
00710                         v1[ 2 ] = vertices[ 0 ].getZ();
00711         
00712                         v2[ 0 ] = vertices[ 1 ].getX();
00713                         v2[ 1 ] = vertices[ 1 ].getY();
00714                         v2[ 2 ] = vertices[ 1 ].getZ();
00715         
00716                         v3[ 0 ] = vertices[ 2 ].getX();
00717                         v3[ 1 ] = vertices[ 2 ].getY();
00718                         v3[ 2 ] = vertices[ 2 ].getZ();
00719         
00720                         object1->AddTri( v1, v2, v3, iId );
00721                         
00722                         meshOrgan1->getAFacePt( face )->getVertexGlobalPositionPt(0)->setColor( cNOCollided );
00723                         meshOrgan1->getAFacePt( face )->getVertexGlobalPositionPt(1)->setColor( cNOCollided );
00724                         meshOrgan1->getAFacePt( face )->getVertexGlobalPositionPt(2)->setColor( cNOCollided );
00725                 }
00726                 iId++;
00727         }
00728                 
00729      
00730         object1->EndModel();
00731                 
00732         COME_Mesh *meshOrgan2 = organ2->getSurface();
00733         iId             = 0;
00734         
00735         object2->BeginModel();
00736                         
00737         //add triangles   
00738         for ( face = 0; face < meshOrgan2->getNumberFaces(); face++ ){
00739                                                 
00740                 meshOrgan2->getAFacePt( face )->getVertexGlobalPositionPt(0)->setColor( cNOTenabled );
00741                 meshOrgan2->getAFacePt( face )->getVertexGlobalPositionPt(1)->setColor( cNOTenabled );
00742                 meshOrgan2->getAFacePt( face )->getVertexGlobalPositionPt(2)->setColor( cNOTenabled );
00743                 if( meshOrgan2->getAFacePt(face)->getCollisionDetectable() ){
00744                         //read the object of the Mesh Class 
00745                         vector<COME_Vertex3D> vertices;
00746                         vertices = meshOrgan2->getVerticesFaceGlobalPosition( face );
00747         
00748                         double v1[ 3 ], v2[ 3 ], v3[ 3 ];
00749                         v1[ 0 ] = vertices[ 0 ].getX();
00750                         v1[ 1 ] = vertices[ 0 ].getY();
00751                         v1[ 2 ] = vertices[ 0 ].getZ();
00752         
00753                         v2[ 0 ] = vertices[ 1 ].getX();
00754                         v2[ 1 ] = vertices[ 1 ].getY();
00755                         v2[ 2 ] = vertices[ 1 ].getZ();
00756         
00757                         v3[ 0 ] = vertices[ 2 ].getX();
00758                         v3[ 1 ] = vertices[ 2 ].getY();
00759                         v3[ 2 ] = vertices[ 2 ].getZ();
00760         
00761                         object2->AddTri( v1, v2, v3, iId );
00762                         
00763                         meshOrgan2->getAFacePt( face )->getVertexGlobalPositionPt(0)->setColor( cNOCollided );
00764                         meshOrgan2->getAFacePt( face )->getVertexGlobalPositionPt(1)->setColor( cNOCollided );
00765                         meshOrgan2->getAFacePt( face )->getVertexGlobalPositionPt(2)->setColor( cNOCollided );
00766                 }
00767                 iId++;
00768         }
00769       
00770         object2->EndModel();          
00771         
00772         
00773         //set up the rotation and translation matrices as
00774         //required by RAPID for the objects.
00775         double R[3][3], T[3];
00776 
00777         R[0][0] = R[1][1] = R[2][2] = 1.0;
00778         R[0][1] = R[1][0] = R[2][0] = 0.0;
00779         R[0][2] = R[1][2] = R[2][1] = 0.0;
00780 
00781         T[0] = 0.0; T[1] = 0.0; T[2] = 0.0;
00782 
00783         //call the RAPID collision detection routine.
00784         RAPID_Collide( R, T, object1, R, T, object2, RAPID_ALL_CONTACTS );
00785                 
00786         //report the results of collision detection.
00787         //printf ( "Num contact pairs %d\n", RAPID_num_contacts  );
00788         
00789         vector2ndPass.clear();
00790         for ( int i = 0; i < RAPID_num_contacts; i++ ){
00791 
00792                 // Here we have the two triangles in contact
00793                 COME_Mesh *meshOrgan1 = organ1->getSurface();
00794                 COME_Mesh *meshOrgan2 = organ2->getSurface();
00795                 
00796                 vector<COME_Vertex3D> vertices1;
00797                 vertices1 = meshOrgan1->getVerticesFaceGlobalPosition( RAPID_contact[ i ].id1 );
00798                 vector<COME_Vertex3D> vertices2;
00799                 vertices2 = meshOrgan2->getVerticesFaceGlobalPosition( RAPID_contact[ i ].id2 );
00800                 
00801                 // for each vertex of one face 
00802                 if( !organ1->isFixed() ){
00803                         for( int v1 = 0; v1 < vertices1.size(); v1++ ){
00804                 
00805                                 if( !meshOrgan1->getAFacePt( RAPID_contact[ i ].id1 )->getVertexGlobalPositionPt(v1)->hasFixedAnchor() )
00806                                 if( meshOrgan2->getAFacePt( RAPID_contact[ i ].id2 )->isInsideGlobalPosition( vertices1[v1] ) ){
00807                                         // get distance vertex v1 to face 2
00808                                         COME_Point3D pointOnPlane;
00809                                         double dist1to2 = meshOrgan2->getAFacePt( RAPID_contact[ i ].id2 )->distanceVertexPlaneGlobalPosition( vertices1[v1] );
00810 
00811                                         if( !organ2->isFixed() )
00812                                                 dist1to2/=2.0;
00813                                         
00814                                         //set displacemet for vert i1
00815                                         meshOrgan1->getAFacePt( RAPID_contact[ i ].id1 )->getVertexGlobalPositionPt(v1)->addCollisionDisp( meshOrgan2->getAFacePt(RAPID_contact[ i ].id2)->getNormalGlobalPosition() * dist1to2 );
00816                                         
00818                                         meshOrgan1->getAFacePt( RAPID_contact[ i ].id1 )->getVertexGlobalPositionPt(v1)->setColor( cCollided );
00819                                         
00820                                         // Add inner adjacencies for second pass test
00821                                         if( flagCollision2ndPass ){
00822                                                 vector<int> neighborFacesInds = meshOrgan1->getAFacePt( RAPID_contact[ i ].id1 )->getVertexGlobalPositionPt(v1)->getNeighbourFaces();
00823                                                 for( int neighborFaces = 0; neighborFaces < neighborFacesInds.size(); neighborFaces++ ){
00824                                                         for( int vertInd = 0; vertInd < meshOrgan1->getAFacePt( neighborFacesInds[neighborFaces] )->getNumberVertices(); vertInd++ ){
00825                                                                 if( meshOrgan2->getAFacePt( RAPID_contact[ i ].id2 )->isInsideGlobalPosition( meshOrgan1->getAFacePt( neighborFacesInds[neighborFaces] )->getVertexGlobalPosition( vertInd ) ) ){
00826                                                                         COME_2ndPassItem aCollision;
00827                                                                         aCollision.innerV = meshOrgan1->getAFacePt( neighborFacesInds[neighborFaces] )->getVertexGlobalPositionPt( vertInd );
00828                                                                         aCollision.otherF = meshOrgan2->getAFacePt( RAPID_contact[ i ].id2 );
00829                                                                         vector2ndPass.push_back( aCollision );
00830                                                                 }
00831                                                         }
00832                                                 }
00833                                         }
00834                                 }
00835                         } 
00836                 }
00837                 
00838                 // for each vertex of the other face 
00839                 if( !organ2->isFixed() ){
00840                         for( int v2 = 0; v2 < vertices2.size(); v2++ ){
00841                 
00842                                 if( !meshOrgan2->getAFacePt( RAPID_contact[ i ].id2 )->getVertexGlobalPositionPt(v2)->hasFixedAnchor() )
00843                                 if( meshOrgan1->getAFacePt( RAPID_contact[ i ].id1 )->isInsideGlobalPosition( vertices2[v2] ) ){
00844                                         // get distance vertex v2 to face 1
00845                                         double dist2to1 = meshOrgan1->getAFacePt( RAPID_contact[ i ].id1 )->distanceVertexPlaneGlobalPosition( vertices2[v2] );
00846                                         
00847                                         if( !organ1->isFixed() )
00848                                                 dist2to1/=2.0;
00849                                         
00850                                         //set displacemet for vert i2
00851                                         meshOrgan2->getAFacePt( RAPID_contact[ i ].id2 )->getVertexGlobalPositionPt(v2)->addCollisionDisp( meshOrgan1->getAFacePt(RAPID_contact[ i ].id1)->getNormalGlobalPosition() * dist2to1 );
00852                                         
00854                                         meshOrgan2->getAFacePt( RAPID_contact[ i ].id2 )->getVertexGlobalPositionPt(v2)->setColor( cCollided );
00855                                         
00856                                         // Add inner adjacencies for second pass test
00857                                         if( flagCollision2ndPass ){
00858                                                 vector<int> neighborFacesInds = meshOrgan2->getAFacePt( RAPID_contact[ i ].id2 )->getVertexGlobalPositionPt(v2)->getNeighbourFaces();
00859                                                 for( int neighborFaces = 0; neighborFaces < neighborFacesInds.size(); neighborFaces++ ){
00860                                                         for( int vertInd = 0; vertInd < meshOrgan2->getAFacePt( neighborFacesInds[neighborFaces] )->getNumberVertices(); vertInd++ ){
00861                                                                 if( meshOrgan1->getAFacePt( RAPID_contact[ i ].id1 )->isInsideGlobalPosition( meshOrgan2->getAFacePt( neighborFacesInds[neighborFaces] )->getVertexGlobalPosition( vertInd ) ) ){
00862                                                                         COME_2ndPassItem aCollision;
00863                                                                         aCollision.innerV = meshOrgan2->getAFacePt( neighborFacesInds[neighborFaces] )->getVertexGlobalPositionPt( vertInd );
00864                                                                         aCollision.otherF = meshOrgan1->getAFacePt( RAPID_contact[ i ].id1 );
00865                                                                         vector2ndPass.push_back( aCollision );
00866                                                                 }
00867                                                         }
00868                                                 }
00869                                         }
00870                                 }
00871                         } 
00872                 }
00873                 collided = true;
00874         }
00875 
00876         // Perform 2nd pass
00877         if( flagCollision2ndPass ){
00878                 for( int ind2ndpass = 0; ind2ndpass < vector2ndPass.size(); ind2ndpass++ ){
00879                 
00880                         if( vector2ndPass[ind2ndpass].innerV->getCollisionDisp().vpModule() == 0 ){
00881                                 double dist = vector2ndPass[ind2ndpass].otherF->distanceVertexPlaneGlobalPosition( vector2ndPass[ind2ndpass].innerV );
00882                                 
00883                                 if( ( !organ1->isFixed() ) && ( !organ2->isFixed() ) )
00884                                         dist/=2.0;
00885                                 
00886                                 //set displacemet for vert
00887                                 vector2ndPass[ind2ndpass].innerV->addCollisionDisp( vector2ndPass[ind2ndpass].otherF->getNormalGlobalPosition() * dist );
00888                                 vector2ndPass[ind2ndpass].innerV->setColor( cCollidedTotal );
00889                         }
00890                 }
00891         }
00892         
00893         delete object1;
00894         delete object2;
00895         
00896         return collided;
00897 }
00898 
00899 
00900 //Checks if the integer given as argument is already in the vector group
00901 bool
00902 COME_Collide::isNotAlreadyIn(vector<int> group, int part){
00903 
00904         for (int i = 0; i < group.size(); i++){
00905                 if (group[i] == part) return false;
00906         }
00907         return true;
00908 }
00909 
00910 
00911 //Projects all vertices of the faces given as argument on the the dir Vector
00912 //and returns the one with the biggest value 
00913 double
00914 COME_Collide::findMaxProjection(COME_Vector3D dir, COME_Vertex3D base, COME_Molecule* nearestMol, vector<int> faces, COME_Mesh* mesh){
00915 
00916         double max = -2000000;
00917         dir = dir.vpNormalize();
00918         for (int i =0; i < faces.size(); i++){
00919                 if (mesh->getAFacePt(faces[i])->getNearestMolecule() == nearestMol){
00920                         vector<COME_Vertex3D> vertices = mesh->getAFacePt(faces[i])->getVerticesGlobalPosition();
00921                         for (int a = 0; a < vertices.size(); a++){
00922                                 COME_Vector3D vect = vertices[a] - base;
00923                                 double distance = dir.vpDotProduct(vect);
00924                                 if (distance > max) {
00925                                 //      cout << " max = " << max << "\n";
00926                                         max=distance;
00927                                 }
00928                         }
00929                 }
00930         }
00931                 
00932         return max;
00933 }
00934 
00935 
00936 //Projects all vertices of the faces given as argument on the the dir Vector
00937 //and returns the one with the smallest value 
00938 double
00939 COME_Collide::findMinProjection(COME_Vector3D dir, COME_Vertex3D base, COME_Molecule* nearestMol, vector<int> faces, COME_Mesh* mesh){
00940 
00941         double min = 2000000;
00942         dir = dir.vpNormalize();
00943         for (int i =0; i < faces.size(); i++){
00944                 if (mesh->getAFacePt(faces[i])->getNearestMolecule() == nearestMol){
00945                         vector<COME_Vertex3D> vertices = mesh->getAFacePt(faces[i])->getVerticesGlobalPosition();
00946                         for (int a = 0; a < vertices.size(); a++){
00947                                 COME_Vector3D vect = vertices[a] - base;
00948                                 double distance = dir.vpDotProduct(vect);
00949                                 if (distance < min) {
00950                                         //cout << " min = " << min << "\n";
00951                                         min=distance;
00952                                 }
00953                         }
00954                 }
00955         }
00956                 
00957         return min;
00958                 
00959 }
00960 
00961 //Checks if the face given as parameter is inside the acceptable range defined
00962 //by the min and max values. Uses the projection of the vertices of the face on to the
00963 //two direction vectors to compare with min and max values.
00964 bool
00965 COME_Collide::IsInAcceptableRange(COME_Vector3D dir1,COME_Vector3D dir2,COME_Vertex3D base, double maxDir1,double minDir1,double maxDir2,double minDir2, COME_Face* face){
00966 
00967         vector<COME_Vertex3D> vertices = face->getVertices();
00968         for (int a = 0; a < vertices.size(); a++){
00969                 COME_Vector3D vect = vertices[a] - base;
00970                 double distance1 = dir1.vpDotProduct(vect);
00971                 if ( distance1 > maxDir1 || distance1 < minDir1){
00972                         //cout << "Face is not in acceptable range dir1!!\n";
00973                         return false;                   
00974                 }
00975                 double distance2 = dir2.vpDotProduct(vect);
00976                 if (distance2 > maxDir2 || distance2 < minDir2) {
00977                         //cout << "Face is not in acceptable range dir2!!\n";
00978                         return false;                   
00979                 }
00980                 
00981         }
00982                 
00983         return true;
00984 }
00985 
00988 void
00989 COME_Collide::initializeProximityStructure(){
00990 
00991         clock_t start,end;
00992         start = clock ();
00993         proximityStructure.clear();
00994         COME_Patient *activePatient = ((COME_Scenario*)parent)->getPatient(((COME_Scenario*)parent)->getPatientList()->size()-1);
00995         // If there's only one object, doesn't detect collisions
00996         if( activePatient->getPtOrganList()->size() > 1 ){
00997 
00998                 // Take the organs of the first patient (there is only one patientin this phase)
00999                 list<COME_MoleculesCartilage*>::const_iterator iterOrgans1;
01000                 for ( iterOrgans1 =((list<COME_MoleculesCartilage*>*)activePatient->getPtOrganList())->begin(); iterOrgans1 != ((list<COME_MoleculesCartilage*>*)activePatient->getPtOrganList())->end(); iterOrgans1++  ){
01001                         
01002                         list<COME_MoleculesCartilage*>::const_iterator iterOrgans2;
01003                         for ( iterOrgans2 = ((list<COME_MoleculesCartilage*>*)activePatient->getPtOrganList())->begin(); iterOrgans2 != ((list<COME_MoleculesCartilage*>*)activePatient->getPtOrganList())->end(); iterOrgans2++  ){
01004                                 
01005                                 double softhard = SOFT;
01006                                 if( (*iterOrgans1)->isFixed() ||  (*iterOrgans2)->isFixed() ){
01007                                         softhard = HARD;
01008                                 }
01009                                 
01010                                 if( (*iterOrgans1) != (*iterOrgans2) ){
01011                                         if(
01012                                         ( ( (*iterOrgans1)->getDescription() == "femurcart" ) && ( (*iterOrgans2)->getDescription() == "pelviscart" ) ) || 
01013                                         ( ( (*iterOrgans1)->getDescription() == "femurcart1" ) && ( (*iterOrgans2)->getDescription() == "pelviscart1" ) ) || 
01014                                         ( ( (*iterOrgans1)->getDescription() == "sphere" ) && ( (*iterOrgans2)->getDescription() == "pelviscart1" ) ) || 
01015                                         ( ( (*iterOrgans1)->getDescription() == "femurbone" ) && ( (*iterOrgans2)->getDescription() == "ischiofemoral" ) ) || 
01016                                         ( ( (*iterOrgans1)->getDescription() == "femurcart" ) && ( (*iterOrgans2)->getDescription() == "ischiofemoral" ) ) 
01017                                         ){
01018                                                 addProximities( (*iterOrgans1)->getSurface(), (*iterOrgans2)->getSurface(), softhard );
01019                                         }
01020                                 }
01021                         }
01022                 }
01023         }
01024         printf( "Proximities initilized. %d pairs. \n", proximityStructure.size() );
01025         end = clock ();
01026         double dif = end - start;
01027         printf ("Init proximities for collision time was %.8f seconds.\n", dif / (double) CLOCKS_PER_SEC );
01028 }
01029 
01035 int
01036 COME_Collide::addProximities( COME_Mesh *mA, COME_Mesh * mB, double hardSoft ){
01037 
01038         int iB;
01039         for( int iA = 0; iA < mA->getVerticesGlobalPt()->size(); iA++ ){
01040                 
01041                 COME_Vertex3D* vertexLocal = &((*(mA->getVerticesPt()))[iA]);
01042                 if( vertexLocal->vpDistance( COME_Point3D() ) >= ((COME_BioStructure*)(mA->getParent()))->getCollisionRadius() ){
01043                                 continue;
01044                 }
01045                 COME_Vertex3D* vertexInA = &((*(mA->getVerticesGlobalPt()))[iA]);
01046                 iB = mB->getClosestFaceIndex( COME_Point3D(vertexInA->x, vertexInA->y, vertexInA->z ) );
01047                 int iBV = mB->getClosestVertexIndex( COME_Point3D(vertexInA->x, vertexInA->y, vertexInA->z ) );
01048                 COME_Face* closestFaceInB = mB->getAFacePt(iB);
01049                 COME_Vertex3D* closestInB = &((*(mB->getVerticesGlobalPt()))[iBV]);
01050                 
01051                 COME_Vector3D tempVec = (*closestInB);
01052                 tempVec.vpNormalize();
01053                 if( ( closestFaceInB->getNormalGlobalPosition().vpDotProduct( tempVec ) < 0.0 )
01054                 && ( vertexInA->getNormalGlobalPosition().vpDotProduct( tempVec ) >= 0.0 ) ) { 
01055                 
01056                         proximityStructure.push_back( COME_Proximity( vertexInA, closestFaceInB, hardSoft ) );
01057                         vertexInA->setCollide( true );
01058                 } else {
01059                         vertexInA->setCollide( false );
01060                 }
01061                 
01063                 //if( proximityStructure.size() > 0 ) return proximityStructure.size();
01064         }
01065         
01066         return proximityStructure.size();
01067 }
01068 
01069 void
01070 COME_Collide::updateProximityStructure(){
01071 
01072         for( int iPS = 0; iPS < proximityStructure.size(); iPS++ ){
01073         
01074                 proximityStructure[iPS].faceB = proximityStructure[iPS].faceB->getNeighborClosestTo( proximityStructure[iPS].objA );
01075                 proximityStructure[iPS].updatePointOnFace();
01076                 double lacor[4]; lacor[0]=1; lacor[1]=1; lacor[2]=1; lacor[3]=1;
01077                 proximityStructure[iPS].faceB->getVertexGlobalPositionPt(0)->setColor( lacor );
01078                 proximityStructure[iPS].faceB->getVertexGlobalPositionPt(1)->setColor( lacor );
01079                 proximityStructure[iPS].faceB->getVertexGlobalPositionPt(2)->setColor( lacor );
01080         }
01081 }
01082 
01083 vector<COME_Proximity>*
01084 COME_Collide::getProximityStructurePt(){
01085 
01086         return &proximityStructure;
01087 }
01088 
01089 void
01090 COME_Collide::geometricResponse(){
01091 
01092         // rebuild all Collision Vectors
01093         for( int iPS = 0; iPS < proximityStructure.size(); iPS++ ){
01094                 proximityStructure[iPS].updateVelocity();
01095         }
01096 }
01097 
01098 void
01099 COME_Collide::resetProximitiesDisps(){
01100 
01101         // rebuild all Collision Vectors
01102         for( int iPS = 0; iPS < proximityStructure.size(); iPS++ ){
01103         
01104                 proximityStructure[iPS].resetDisplacements();
01105         }
01106 }
01107 
01110 
01111 
01112 void
01113 COME_Collide::sphericalSlidingResponse(){
01114 
01115         COME_Patient *activePatient = ((COME_Scenario*)parent)->getPatient(((COME_Scenario*)parent)->getPatientList()->size()-1);
01116         // Reset all collision vectors
01117         list<COME_MoleculesCartilage*>::const_iterator iterOrgansReset;
01118         for ( iterOrgansReset =((list<COME_MoleculesCartilage*>*)activePatient->getPtOrganList())->begin(); iterOrgansReset != ((list<COME_MoleculesCartilage*>*)activePatient->getPtOrganList())->end(); iterOrgansReset++  ){
01119                 //if( (*iterOrgansReset)->internals.size() > 0 ){
01120                         (*iterOrgansReset)->getSurface()->resetCollisionDisps();
01121                 //}
01122         }
01123         
01124         // rebuild all Collision Vectors
01125         // run all vertices of the internal mesh and construct ephemerous proximities
01126         
01127         // first clean ephemerous proximities for test
01128         proximityStructure.clear();
01129         
01130         list<COME_MoleculesCartilage*>::const_iterator iterOrgans;
01131         for ( iterOrgans =((list<COME_MoleculesCartilage*>*)activePatient->getPtOrganList())->begin(); iterOrgans != ((list<COME_MoleculesCartilage*>*)activePatient->getPtOrganList())->end(); iterOrgans++  ){
01132         
01133                 for( int iI = 0; iI < (*iterOrgans)->internals.size(); iI++ ){
01134                 
01135                         softhard = SOFT;
01136                         if( (*iterOrgans)->isFixed() ||  (*iterOrgans)->internals[iI]->isFixed() ){
01137                                 softhard = HARD;
01138                         }
01139                         COME_Mesh *mA = (*iterOrgans)->internals[iI]->getSurface();
01140                         for( int iA = 0; iA < mA->getVerticesGlobalPt()->size(); iA++ ){
01141                                 COME_Vertex3D* vertexInA = &((*(mA->getVerticesGlobalPt()))[iA]);
01142                                 if( vertexInA->getCollide() ){
01143                                 
01144                                         // calculate spherical coord for A
01145                                         COME_Vector3D aNormal; aNormal.setVector3D( vertexInA->getVertexAsPoint3D() );
01146                                         aNormal.vpNormalize();
01147                                         COME_Point2D aSpherical = COME_Point2D( atan2( aNormal.y, aNormal.x ), acos( aNormal.z ) ) - (*iterOrgans)->sSpherical;
01148                                         COME_Face *theFace = (*iterOrgans)->getHashElement(aSpherical).face;
01149                                         if( theFace ){
01150                                                 //printf( "hit\n" );
01151                                                 updateVelocity( theFace, vertexInA );
01152                                                 proximityStructure.push_back( COME_Proximity( vertexInA, theFace, softhard ) );
01153                                         }
01154                                 }
01155                         }
01156                 }
01157         }
01158 }
01159 
01162 void
01163 COME_Collide::updateVelocity( COME_Face *faceB, COME_Vertex3D *objA ){
01164 
01165         COME_Point3D    pointOnB = faceB->getClosestPointOnFaceToPoint( COME_Point3D( objA->x, objA->y, objA->z ) );
01166 
01167         COME_Vector3D velocDir;
01168         if( !( signedDistance(faceB, objA->getVertexAsPoint3D(), pointOnB, objA->getNormalGlobalPosition() ) < 0) ){
01169                 velocDir = objA->getVertexAsPoint3D() - pointOnB; velocDir.vpNormalize();
01170         
01171                 COME_Vector3D velocA = objA->getBlendedVelocity();
01172                 COME_Vector3D velocB = faceB->getBlendedVelocity( pointOnB );
01173                 COME_Vector3D velocRelativeA = velocDir * COME_Vector3D(velocA-velocB).vpDotProduct(velocDir);
01174                 
01175                 COME_Point3D posObjATimePlusOne = objA->getVertexAsPoint3D() + ( velocRelativeA * COME::timestepGlobal * 1.0 );
01176                 
01177                 if( signedDistance( faceB, posObjATimePlusOne, pointOnB, objA->getNormalGlobalPosition() ) < 0.0 ){
01178                         
01179                         COME_Vector3D velocCorrection = velocRelativeA - ( ( pointOnB - objA->getVertexAsPoint3D() ) * ( 1.0 / ( COME::timestepGlobal * 1.0 ) ) );
01180                         
01181                         // update objA
01182                         objA->addVelocityDisp( -velocCorrection/softhard );
01183                         
01184                         // update faceB
01185                         COME_Vertex3D *A =  faceB->getVertexGlobalPositionPt( 0 );
01186                         COME_Vertex3D *B =  faceB->getVertexGlobalPositionPt( 1 );
01187                         COME_Vertex3D *C =  faceB->getVertexGlobalPositionPt( 2 );
01188                         
01189                         A->addVelocityDisp( velocCorrection/softhard );
01190                         B->addVelocityDisp( velocCorrection/softhard );
01191                         C->addVelocityDisp( velocCorrection/softhard );
01192                 }
01193         } else {
01194                 updatePosition( faceB, objA, pointOnB );
01195                 objA->addVelocityDisp( -objA->getBlendedVelocity() );
01196                 faceB->getVertexGlobalPositionPt( 0 )->addVelocityDisp( -faceB->getVertexGlobalPositionPt( 0 )->getBlendedVelocity() );
01197                 faceB->getVertexGlobalPositionPt( 1 )->addVelocityDisp( -faceB->getVertexGlobalPositionPt( 1 )->getBlendedVelocity() );
01198                 faceB->getVertexGlobalPositionPt( 2 )->addVelocityDisp( -faceB->getVertexGlobalPositionPt( 2 )->getBlendedVelocity() );
01199         }
01200         
01201 }
01202 
01205 double
01206 COME_Collide::signedDistance( COME_Face *faceB, COME_Point3D p, COME_Point3D pointOnB, COME_Vector3D normal ){
01207  
01208         double dist = pointOnB.vpDistance( p );
01209 
01210         if( ( faceB->getNormalGlobalPositionPt()->vpDotProduct( normal ) < 0.0 ) && ( faceB->isInsideGlobalGOOD( p ) ) )
01211                 dist = -dist;
01212         
01213         CONTADIST++;
01214         return dist;
01215  }
01216 
01219 void
01220 COME_Collide::updatePosition( COME_Face *faceB, COME_Vertex3D *objA, COME_Point3D pointOnB ){
01221 
01222         COME_Vector3D posCorrection = pointOnB - objA->getVertexAsPoint3D();
01223         
01224         // update objA
01225         objA->addCollisionDisp( posCorrection/softhard );
01226         
01227         // update faceB
01228         COME_Vertex3D *A =  faceB->getVertexGlobalPositionPt( 0 );
01229         COME_Vertex3D *B =  faceB->getVertexGlobalPositionPt( 1 );
01230         COME_Vertex3D *C =  faceB->getVertexGlobalPositionPt( 2 );
01231         
01232         A->addCollisionDisp( -posCorrection/softhard );
01233         B->addCollisionDisp( -posCorrection/softhard );
01234         C->addCollisionDisp( -posCorrection/softhard );
01235 
01236 }
comecollide.cpp
