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 <bio/comemolecule.h>
00033 #include <bio/comemoleculescartilage.h>
00034 #include <general/comescenario.h>
00035 #include <math.h>
00036 #include <algebra/quaternion.h>
00037 
00041 
00042 COME_Molecule::COME_Molecule()
00043 {
00044 
00045         connectionList = new list<COME_MoleculeLink*>;
00046         parent = NULL;
00047         localFrame = NULL;
00048         fixed = false;
00049         liquidSharingArray = NULL;
00050         intersecClamp = false;
00051         collide = false;
00052         updated = true;
00053         stress = 0.0;
00054 }
00055 
00059 
00060 COME_Molecule::COME_Molecule( COME_Point3D positionN, double radiusN, double frictionConstN,
00061                                                           COME_Material *materialN, COME_Vector3D velocityN, COME_BioStructure *parentN ){
00062 
00063         nominalRadius = radius = radiusN;
00064         frictionConst = frictionConstN;
00065         parent = parentN;
00066         fixed = false;
00067         onSurface = false;
00068         localFrame = NULL;
00069         initPosition = positionN;
00070 
00071         setPosition( positionN );
00072         setVelocity( velocityN );
00073         
00074         if( materialN ){
00075                 //setVolume( 4 * M_PI * pow( radius, 3 ) / 3 );
00076                 setMass( getVolume() * materialN->getDensity() );
00077                 setMaterial( materialN );
00078                 liquidFraction = materialN->getLiquidFraction();
00079                 permeability = materialN->getPermeability();
00080         } else {
00081                 setMass( 0.0 );
00082                 setMaterial( NULL );
00083                 liquidFraction = 0.0;
00084                 permeability = 0.0;
00085         }
00086 
00087         connectionList = new list<COME_MoleculeLink*>;
00088 
00089         liquidSharingArray = NULL;
00090         intersecClamp = false;
00091         collide = false;
00092         updated = true;
00093         stress = 0.0;
00094 }
00095 
00096 COME_Molecule::COME_Molecule( COME_Point3D &positionN, double &radiusN, double &frictionConstN, COME_Material *materialN, COME_Vector3D &velocityN, COME_BioStructure *parentN ){
00097                                    
00098         //COME_Molecule( positionN, radiusN, frictionConstN, materialN, velocityN, parentN );
00099         nominalRadius = radius = radiusN;
00100         frictionConst = frictionConstN;
00101         parent = parentN;
00102         fixed = false;
00103         onSurface = false;
00104         localFrame = NULL;
00105         initPosition = positionN;
00106 
00107         setPosition( positionN );
00108         setVelocity( velocityN );
00109         
00110         if( materialN ){
00111                 //setVolume( 4 * M_PI * pow( radius, 3 ) / 3 );
00112                 setMass( getVolume() * materialN->getDensity() );
00113                 setMaterial( materialN );
00114                 liquidFraction = materialN->getLiquidFraction();
00115                 permeability = materialN->getPermeability();
00116         } else {
00117                 setMass( 0.0 );
00118                 setMaterial( NULL );
00119                 liquidFraction = 0.0;
00120                 permeability = 0.0;
00121         }
00122 
00123         connectionList = new list<COME_MoleculeLink*>;
00124 
00125         liquidSharingArray = NULL;
00126         intersecClamp = false;
00127         collide = false;
00128         updated = true;
00129         stress = 0.0;
00130 }
00131 
00132 
00133 COME_Molecule::~COME_Molecule()
00134 {
00135         if( connectionList ){
00136         
00137                 list<COME_MoleculeLink*>::iterator iterThis;
00138                 for (iterThis = connectionList->begin(); !( iterThis == connectionList->end() ); iterThis++){
00139                 
00140                         if( (*iterThis) ){
00141                                 list<COME_MoleculeLink*>::iterator iterOther;
00142                                 for (iterOther = (*iterThis)->getOtherElement(this)->getConnectionList()->begin(); !( iterOther == (*iterThis)->getOtherElement(this)->getConnectionList()->end() ); iterOther++){
00143                                         if( (*iterThis) == (*iterOther) ){
00144 
00145                                                 (*iterThis)->getOtherElement(this)->getConnectionList()->erase( iterOther++ );
00146                                         }
00147                                 }
00148                                 delete (*iterThis);
00149                         }
00150                 }
00151                 delete connectionList;
00152         }
00153 }
00154 
00158 void
00159 COME_Molecule::setRadius( double radiusN ){
00160 
00161         radius = radiusN;
00162 }
00163 
00164 
00165 void
00166 COME_Molecule::setFrictionConst( double frictionConstN ){
00167 
00168         frictionConst = frictionConstN;
00169 }
00170 
00171 void
00172 COME_Molecule::setConnectionList( list<COME_MoleculeLink*> *listN ){
00173 
00174         connectionList = listN;
00175 }
00176 
00177 void
00178 COME_Molecule::setExternalForces( vector<COME_TimeForce*> listN ){
00179 
00180         externalForces = listN;
00181 }
00182 
00183 
00184 
00185 void
00186 COME_Molecule::setOnSurface( bool flag ){
00187 
00188         onSurface = flag;
00189 }
00190 
00191 void
00192 COME_Molecule::setCollisionForce( COME_Force forceN ){
00193 
00194         collisionForce = forceN;
00195 }
00196 
00197 void
00198 COME_Molecule::setIfGreaterCollisionForce( COME_Force forceN ){
00199 
00200         if( collisionForce.getIntensity() < forceN.getIntensity() )
00201                 collisionForce = forceN;
00202 }
00203         
00204 
00205 void
00206 COME_Molecule::setIntersectClamp( int type ){
00207 
00208         intersecClamp = type;
00209 }
00210 
00211 void
00212 COME_Molecule::setCollide( bool yesno ){
00213 
00214         collide = yesno;
00215 }
00216 
00217 void
00218 COME_Molecule::setUpdated( bool yesno ){
00219 
00220         updated = yesno;
00221 }
00222 
00223 
00227 double
00228 COME_Molecule::getRadius() const{
00229 
00230         return radius;
00231 }
00232 
00233 double
00234 COME_Molecule::getFrictionConst() const{
00235 
00236         return frictionConst;
00237 }
00238 
00239 list<COME_MoleculeLink*>*
00240 COME_Molecule::getConnectionList() const{
00241 
00242         return connectionList;
00243 }
00244 
00245 vector<COME_TimeForce*>
00246 COME_Molecule::getExternalForces() const{
00247 
00248         return externalForces;
00249 }
00250 
00251 vector<COME_Vertex3D*>*
00252 COME_Molecule::getBuois() {
00253 
00254         return &buois;
00255 }
00256 
00257 vector<int>*
00258 COME_Molecule::getBuoisIndices() {
00259 
00260         return &buoisIndices;
00261 }
00262 
00263 bool
00264 COME_Molecule::isOnSurface() const{
00265 
00266         return onSurface;
00267 }
00268 
00269 bool
00270 COME_Molecule::isCollide() const{
00271 
00272         return collide;
00273 }
00274 
00275 bool
00276 COME_Molecule::isUpdated() const{
00277 
00278         return updated;
00279 }
00280 
00281 bool
00282 COME_Molecule::intersectClamp() const{
00283 
00284         if( intersecClamp == FREE )
00285                 return false;
00286         return true;
00287 }
00288 
00289 bool
00290 COME_Molecule::intersectClampMobile() const{
00291 
00292         if( intersecClamp == CLAMPMOBILE )
00293                 return true;
00294         return false;
00295 }
00296 
00297 bool
00298 COME_Molecule::intersectClampFix() const{
00299 
00300         if( intersecClamp == CLAMPFIX )
00301                 return true;
00302         return false;
00303 }
00304 
00305 COME_Force
00306 COME_Molecule::getCollisionForce() const{
00307 
00308         return collisionForce;
00309 }
00310 
00311 COME_Vector3D
00312 COME_Molecule::getLastDisplacement() const{
00313 
00314         return lastDisplacement;
00315 }
00316 
00317 vector<COME_Molecule*>&
00318 COME_Molecule::getOrthogonalNeighbors(){
00319 
00320         return orthogonalNeighbors;
00321 }
00322 
00323 COME_Matrix*
00324 COME_Molecule::getLocalFrame(){
00325 
00326         return localFrame;
00327 }
00328 
00329 double
00330 COME_Molecule::getVolume() const {
00331 
00332         return 4.0 * M_PI * pow( radius, 3 ) / 3.0;
00333 }
00334 
00335 double
00336 COME_Molecule::getPressureArea() const {
00337 
00338         return M_PI * radius * radius;
00339 }
00340 
00341 double
00342 COME_Molecule::getLiquidLevel() const{
00343 
00344         double solidVolume = ( ( 4.0 * M_PI * pow( nominalRadius, 3) ) / 3.0 ) * (1 - liquidFraction );
00345 
00346         if( liquidFraction == 0.0 ) return 0.0;
00347 
00348         return ( getVolume() - solidVolume ) / ( ( 4.0 * M_PI * pow( nominalRadius, 3) ) / 3.0 * liquidFraction );
00349 }
00350 
00351 double
00352 COME_Molecule::getLiquidFraction() const{
00353 
00354         return liquidFraction;
00355 
00356 }
00357 double
00358 COME_Molecule::getNominalRadius() const{
00359 
00360         return nominalRadius;
00361 }
00362 
00363 double
00364 COME_Molecule::getPermeability() const{
00365 
00366         return permeability;
00367 }
00368 
00369 
00373 void
00374 COME_Molecule::addLink( COME_MoleculeLink *linkN ){
00375 
00376         connectionList->push_back( linkN );
00377 }
00378 
00382 void
00383 COME_Molecule::addBuoy( COME_Vertex3D *buoyN, int index ){
00384 
00385         buois.push_back( buoyN );
00386         buoisIndices.push_back( index );
00387 }
00388 
00389 
00393 void
00394 COME_Molecule::addOrthogonalNeighbor( COME_Molecule *newNeighbor ){
00395 
00396         orthogonalNeighbors.push_back( newNeighbor );
00397 }
00398 
00402 void
00403 COME_Molecule::addExternalForce( COME_TimeForce *forceN ){
00404 
00405         externalForces.push_back( forceN );
00406 }
00407 
00411 void
00412 COME_Molecule::clearExternalForces(){
00413 
00414         for( int i = 0; i < externalForces.size(); i++ ){
00415                 delete externalForces[i];
00416         }
00417         externalForces.clear();
00418 }
00419 
00421 void
00422 COME_Molecule::addCollisionForce( COME_Force forceN ){
00423 
00424         collisionForce = collisionForce + forceN;
00425 }
00426 
00433 void
00434 COME_Molecule::createLink( COME_Molecule *moleculeN ){
00435 
00436         // Create connection
00437         COME_MoleculeLink *newLink = new COME_MoleculeLink( this, moleculeN );
00438         
00439         // Set connection attributes
00440         newLink->setDampingConst( (getMaterial()->getDamping() + moleculeN->getMaterial()->getDamping())/2.0 );
00441         newLink->setFrictionConst( (getFrictionConst() + moleculeN->getFrictionConst())/2.0 );
00442         
00443         // Calculate nominal distance
00444         newLink->setNominalDist( getPosition().vpDistance( moleculeN->getPosition() ) );
00445 
00446         // Calculate Hooke's constant
00447         double area1 = pow(getRadius()*2, 2);
00448         double area2 = pow( moleculeN->getRadius()*2, 2);
00449         double hookeConstant = ( ( ( getMaterial()->getYoungsModulus()* area1 ) + ( moleculeN->getMaterial()->getYoungsModulus()* area2 ) ) / 2.0 ) / newLink->getNominalDist();
00450         newLink->setHookeConst( hookeConstant );
00451 
00452         printf( "Link's nominal dist: %f hooke's: %f \n", newLink->getNominalDist(), hookeConstant );
00453         
00454         // Add connection reference to each of its molecules
00455         addLink( newLink );
00456         moleculeN->addLink( newLink );
00457 }
00458 
00465 void
00466 COME_Molecule::createLink( COME_Molecule *moleculeN, double globalHooke ){
00467 
00468         // Create connection
00469         COME_MoleculeLink *newLink = new COME_MoleculeLink( this, moleculeN );
00470         
00471         // Set connection attributes
00472         newLink->setDampingConst( (getMaterial()->getDamping() + moleculeN->getMaterial()->getDamping())/2.0 );
00473         newLink->setFrictionConst( (getFrictionConst() + moleculeN->getFrictionConst())/2.0 );
00474         
00475         // Calculate nominal distance
00476         newLink->setNominalDist( getPosition().vpDistance( moleculeN->getPosition() ) );
00477 
00478         // Calculate Hooke's constant
00479         COME_Vector3D aniso = material->getAnisotropyVector();
00480         COME_Vector3D kVec = position - moleculeN->getPosition();
00481         kVec.vpNormalize();
00482         double kThisLink = ( ( fabs(kVec.getX()) * aniso.getX() ) + ( fabs(kVec.getY()) * aniso.getY() ) + ( fabs(kVec.getZ()) * aniso.getZ() ) )* globalHooke;
00483         newLink->setHookeConst( kThisLink );
00484         
00485         // Add connection reference to each of its molecules
00486         addLink( newLink );
00487         moleculeN->addLink( newLink );
00488 }
00489 
00495 void
00496 COME_Molecule::removeLink( COME_Molecule *moleculeN ){
00497 
00498         list<COME_MoleculeLink*>::iterator iterThis;
00499         for (iterThis = connectionList->begin(); !( iterThis == connectionList->end() ); iterThis++){
00500                 if( ( (*iterThis)->getElement(0) == moleculeN ) || ( (*iterThis)->getElement(1) == moleculeN ) ){
00501                         break;
00502                 }
00503         }
00504         list<COME_MoleculeLink*>::iterator iterNew;
00505         for (iterNew = moleculeN->getConnectionList()->begin(); !( iterNew == moleculeN->getConnectionList()->end() ); iterNew++){
00506                 if( ( (*iterNew)->getElement(0) == this ) || ( (*iterNew)->getElement(1) == this ) ){
00507                         break;
00508                 }
00509         }
00510         delete (*iterThis);
00511         moleculeN->getConnectionList()->erase( iterNew );
00512         connectionList->erase( iterThis );
00513 }
00514 
00520 COME_Force
00521 COME_Molecule::calculateForce( double currTime, COME_Point3D *positionsIN, COME_Vector3D *velocitiesIN, int currIndex ){
00522 
00523         double stressForce = 0.0;
00524 
00525         // Calculate local force (produced by the current amount of movement of the molecule) 
00526         COME_Vector3D velocNormalized;
00527         if( velocitiesIN[ currIndex ].vpModule() == 0 ){
00528                 velocNormalized = COME_Vector3D( 0,0,0 );
00529         } else {
00530                 velocNormalized = velocitiesIN[ currIndex ] / velocitiesIN[ currIndex ].vpModule();
00531         }
00532         fLocal = COME_Force( (velocNormalized)*(pow(velocitiesIN[ currIndex ].vpModule(), 2) )*(-M_PI)*(radius*radius)*material->getMediumDensity() );
00533         
00535         COME_Vector3D accG = ((COME_Scenario*)(parent->getParent()->getParent()))->getGravity();
00536         fGravity = accG * mass;
00537 
00539         fExternal = externalForceTo( currTime );
00540 
00541         // Calculate connections' force (produced by all connections existent with this molecule)
00542         // It is the addition of friction, damping and original forces of the connections.
00543         double strainTotal = 0.0;
00544         fOriginal = fDamping = fFriction = COME_Force();
00545         list<COME_MoleculeLink*>::iterator iterO;
00546         for (iterO = connectionList->begin(); iterO != connectionList->end(); iterO++ ){
00547                 
00548                 COME_Molecule *temp;
00549                 if( (*iterO)->getElement(0) == this ){
00550                         temp = (*iterO)->getElement(1);
00551                 } else {
00552                         temp = (*iterO)->getElement(0);
00553                 }
00554 
00555                 // Find index of molecule temp to get respectives position and velocity 
00556                 int i = temp->getIndex();
00557 
00558                 COME_Vector3D tempVelocity = velocitiesIN[ i ]; 
00559                 COME_Point3D tempPosition = positionsIN[ i ];
00560 
00561 
00562                 double dist = positionsIN[ currIndex ].vpDistance( tempPosition ) ;
00563 
00564                 strainTotal = ( (*iterO)->getNominalDist() - dist ) / (*iterO)->getNominalDist();
00565 
00566                 COME_Vector3D parallelVelocity;
00567                 if( dist == 0 ){
00568 
00569                         //printf( "DISTANCE: %f \n", dist );
00570                 } else {
00571 
00572                         //printf( "HOOKE: %f", (*iterO)->getHookeConst() );
00573                         // Calculate original elasticity force
00574                         COME_Force currentOriginal = COME_Force( ( ( positionsIN[ currIndex ] - tempPosition ) * (1/dist) ) * ( -(*iterO)->getHookeConst()*( dist - (*iterO)->getNominalDist() ) ) );
00575                         fOriginal = COME_Force( fOriginal + ( currentOriginal /*/ 3.75*/ ) );
00576                         stressForce += currentOriginal.getIntensity();
00577                         
00578                         // Calculate internal damping force
00579                         COME_Vector3D P; P.setVector3D( positionsIN[ currIndex ] - tempPosition );
00580                         COME_Vector3D V = COME_Vector3D( velocitiesIN[ currIndex ] - tempVelocity );
00581                         parallelVelocity = COME_Vector3D( P * ( V.vpDotProduct( P ) / (dist * dist) ) );
00582 
00583                         COME_Force currentDamping = COME_Force( parallelVelocity * ( -(*iterO)->getDampingConst() ) );
00584                         fDamping = COME_Force( fDamping + currentDamping );
00585                 }
00586 
00587                 // Calculate friction force
00588                 COME_Force fNormal = COME_Force( fOriginal + fDamping );
00589                 COME_Vector3D orthogonalVelocity = COME_Vector3D( ( velocitiesIN[ currIndex ] - tempVelocity ) - parallelVelocity );
00590                 COME_Vector3D miNormal = COME_Vector3D( fNormal.getForceVector() );
00591                 miNormal = miNormal * frictionConst;
00592                 COME_Vector3D orthogVelocityNormalized;
00593                 if( orthogonalVelocity.vpModule() == 0 ){
00594                         orthogVelocityNormalized = COME_Vector3D( 0,0,0 );
00595                 } else {
00596                         orthogVelocityNormalized = orthogonalVelocity / orthogonalVelocity.vpModule();
00597                 }
00598                 COME_Force currentFriction = COME_Force( orthogVelocityNormalized * ( -miNormal.vpModule() ) );
00599                 fFriction = COME_Force( fFriction + currentFriction );
00600                 stressForce += currentFriction.getIntensity();
00601 
00602         }
00603 
00605         COME_Force fConnection = COME_Force( fOriginal + fDamping + fFriction );
00606 
00608         COME_Vector3D direction = COME_Vector3D( velocitiesIN[ currIndex ] );
00609         double coeff = 0.2;
00610         COME_Force fExtFriction = direction.vpNormalize() * ( collisionForce.getIntensity() * -coeff );
00611         stressForce += fExtFriction.getIntensity();
00612         
00614         COME_Force resultant = COME_Force( fLocal + fConnection + fGravity + fExternal + collisionForce + fExtFriction );
00615         stressForce += fGravity.getIntensity() + fExternal.getIntensity();
00616 
00618         stress = stressForce / ( M_PI * radius * radius );
00619         strain = strainTotal / connectionList->size();
00620 
00621         // Reset collision force
00622         fCollision = collisionForce; //keep thid force to chart plot
00623         collisionForce = COME_Force();
00624 
00625         return resultant;
00626 }
00627 
00635 void
00636 COME_Molecule::derivs( double currTime, double timestep, COME_Point3D *positionsIN, COME_Vector3D *velocitiesIN, COME_Vector3D *velocitiesOUT, COME_Vector3D *accelerationsOUT, int currIndex ){
00637         
00638         if( !isFixed() ){
00639                 updateLinks();
00640                 #ifndef GPU_FORCES
00641                         COME_Force force = calculateForce( currTime, positionsIN, velocitiesIN, currIndex );
00642                 #else
00643                         COME_Force force = ((COME_MoleculesTissue*)parent)->getGPUForce(currIndex);
00644                 #endif
00645                 acceleration = accelerationsOUT[ currIndex ] = COME_Vector3D( force.getForceVector() / mass );
00646                 velocitiesOUT[ currIndex ] = COME_Vector3D( velocitiesIN[ currIndex ] + ( accelerationsOUT[ currIndex ] * timestep * 0.5 ) );
00647                 // divide increment by 2 above to consider avg velocity
00648         }
00649 }
00650 
00651 
00658 int
00659 COME_Molecule::updateLinks(){
00660 
00661         int removed = 0;
00662 /*      list<COME_MoleculeLink*>::iterator iterO;
00663         for (iterO = connectionList->begin(); iterO != connectionList->end(); iterO++){
00664                 
00665                 COME_Molecule *temp;
00666                 if( (*iterO)->getElement(0) == this ){
00667                         temp = (*iterO)->getElement(1);
00668                 } else {
00669                         temp = (*iterO)->getElement(0);
00670                 }
00671                 double dist = position.vpDistance( temp->getPosition() ) ;
00672                 if( dist > (*iterO)->getFractureDist() ){
00673                         iterO++;
00674                         removeLink( temp );
00675                         removed++;
00676                 }
00677                 
00678         }
00679         if( removed ){
00680                 COME_Point3D pos = getPosition();
00681                 printf( "remotion - molecule: %f %f %f - %d connections.\n", pos.getX(), pos.getY(), pos.getZ(), removed );
00682         }
00683 */      return removed;
00684 }
00685 
00690 COME_Force
00691 COME_Molecule::externalForceTo( double currTime ){
00692 
00693         if( externalForces.size() == 0 ){
00694                 
00695                 return COME_Force();
00696 
00697         } else {
00698                 double iT = externalForces[0]->getTime();
00699                 double fT = externalForces[externalForces.size()-1]->getTime();
00700                 COME_Vector3D fF, iF;
00701 
00702                 for( int i = 1; i < externalForces.size(); i++ ){
00703 
00704                         if( currTime < externalForces[i]->getTime() ){
00705                                 fT = externalForces[i]->getTime();
00706                                 fF = externalForces[i]->getForceVector();
00707                                 iT = externalForces[i-1]->getTime();
00708                                 iF = externalForces[i-1]->getForceVector();
00709                                 break;
00710                         }
00711                 }
00712 
00713                 COME_Force currForce = COME_Force( iF + ( ( fF - iF )*( ( currTime - iT ) / ( fT - iT ) ) ) );
00714 
00715                 return currForce;
00716         }
00717 }
00718 
00726 void
00727 COME_Molecule::setPosition( COME_Point3D &positionN ) {
00728 
00729         // When a molecule is just created its position is (0,0,0), so the first set
00730         // will cause an incorrect large displacement.
00731         // This test avoid creation of a displacement when this molecule is brand new.
00732         if( position.vpDistance( COME_Point3D( 0, 0, 0 ) ) != 0.0 ){
00733                 lastDisplacement.setVector3D( positionN - position );
00734         }
00735         position = COME_Point3D( positionN );
00736 }
00737 
00742 void
00743 COME_Molecule::resetPosition() {
00744 
00745         position = initPosition;
00746 }
00747 
00752 void
00753 COME_Molecule::checkPosition() {
00754 
00755         initPosition = position;
00756 }
00757 
00758 
00762 void
00763 COME_Molecule::setINITPosition( COME_Point3D &newPos ){ //TEMP for testing RESPONSE
00764 
00765         initPosition = newPos;
00766 }
00767 
00768 COME_Point3D
00769 COME_Molecule::getINITPositionGlobal(){ //TEMP for testing RESPONSE
00770 
00771         COME_Matrix localGIM = ((COME_BioStructure*)parent)->getGIM();
00772         if( description == "origin" )
00773                 localGIM = COME_Matrix();
00774         //printf( "CHECK: Line commented because of getGIM() link error on discretizer \n" );
00775         if( !isFixed() )
00776                 return initPosition;
00777         return localGIM * initPosition;
00778 }
00779         
00784 void
00785 COME_Molecule::initializeOrthogonalNeighbors(){
00786 
00787         //Test if by the way this molecule is not connected to 3 others
00788         if( connectionList->size() < 3 ){
00789                 printf( "Surface molecule found with less than 3 neighbors.\n" );
00790                 exit(666);
00791                 //return;
00792         }
00793         
00794         // Test if orthogonalNeighbor is already initialized
00795         // (it happens when a molecule is anchor of more than one vertex.)
00796         if( orthogonalNeighbors.size() == 0 ){
00797 
00798                 //Choose the first neighbor (randomly) from the connection List
00799                 list<COME_MoleculeLink*>::iterator iterO = connectionList->begin();
00800                 COME_Molecule *temp;
00801                 if( (*iterO)->getElement(0) == this ){
00802                         temp = (*iterO)->getElement(1);
00803                 } else {
00804                         temp = (*iterO)->getElement(0);
00805                 }
00806                 orthogonalNeighbors.push_back( temp );
00807 
00808                 COME_Vector3D vI( orthogonalNeighbors[0]->getGlobalPosition() - this->getGlobalPosition() ); vI.vpNormalize();
00809                 COME_Vector3D vJ;
00810                 COME_Vector3D vK;
00811                 COME_Vector3D normal;
00812 
00813 
00814                 //Find the second neighbour by choosing the neighbour witch forms 
00815                 //the closest angle to 90° with the first neighbour
00816                 double localAngle = M_PI;
00817                 COME_Molecule* localMolecule;
00818                 for (iterO = connectionList->begin(), iterO++; iterO != connectionList->end(); iterO++ ){
00819                         
00820                         if( (*iterO)->getElement(0) == this ){
00821                                 temp = (*iterO)->getElement(1);
00822                         } else {
00823                                 temp = (*iterO)->getElement(0);
00824                         }
00825 
00826                         vJ =  temp->getGlobalPosition() - this->getGlobalPosition() ; vJ.vpNormalize();
00827                         if( fabs( vI.getAngle( vJ ) - ( M_PI / 2.0 ) ) < localAngle ){
00828                         
00829                                 localMolecule = temp;
00830                                 localAngle = fabs (vI.getAngle( vJ ) - ( M_PI / 2.0 ));
00831                         }
00832                 }
00833                 orthogonalNeighbors.push_back( localMolecule );
00834                 
00835                 
00836                 //Find the third neighbour by choosing the neighbour witch forms 
00837                 //the closest angle to 90° with the first and second neighbour
00838                 vJ = orthogonalNeighbors[1]->getGlobalPosition() - this->getGlobalPosition(); vJ.vpNormalize();
00839                 normal = vI.vpCrossProduct(vJ); normal.vpNormalize();
00840                 localAngle = M_PI;
00841                 double AngleFromNormal;
00842                 for (iterO = connectionList->begin(), iterO++; iterO != connectionList->end(); iterO++ ){
00843                         
00844                         if( (*iterO)->getElement(0) == this ){
00845                                 temp = (*iterO)->getElement(1);
00846                         } else {
00847                                 temp = (*iterO)->getElement(0);
00848                         }
00849 
00850                         vK= temp->getGlobalPosition() - this->getGlobalPosition(); vK.vpNormalize();
00851                         
00852                         AngleFromNormal = normal.getAngle(vK);
00853                         if (AngleFromNormal > (M_PI / 2.0)) AngleFromNormal = M_PI-AngleFromNormal; // it never happens anyway
00854                         if (fabs(AngleFromNormal) < localAngle){
00855                                 localMolecule = temp;
00856                                 localAngle = fabs(AngleFromNormal);
00857                         }
00858                 }
00859                 //cout << "Angle of third coordinate Axis = " << localAngle*180/M_PI << " \n";  
00860                 orthogonalNeighbors.push_back( localMolecule );
00861                 
00862                 //create the original frame
00863                 makeOriginalFrame();
00864                 // Create matrix to represent this Local Frame
00865                 makeLocalFrame();
00866         }
00867         
00868 }
00869 
00870 void
00871 COME_Molecule::makeOriginalFrame(){
00872 
00873         if( orthogonalNeighbors.size() > 0 ){
00874 
00875                 if( localFrame == NULL ){
00876                         localFrame = new COME_Matrix();
00877                 }
00878                 
00879                 COME_Vector3D vI( orthogonalNeighbors[0]->getGlobalPosition() - this->getGlobalPosition() ); vI.vpNormalize();
00880                 COME_Vector3D vJ( orthogonalNeighbors[1]->getGlobalPosition() - this->getGlobalPosition() ); vJ.vpNormalize();
00881                 COME_Vector3D vK( orthogonalNeighbors[2]->getGlobalPosition() - this->getGlobalPosition() ); vK.vpNormalize();
00882 
00884                 
00885                 //COME_Vector3D vI( 1, 0, 0 );
00886                 //COME_Vector3D vJ( 0, 1, 0 );
00887                 //COME_Vector3D vK( 0, 0, 1 );
00888 /*
00889                 printf( "I: %f %f %f O: %f %f %f \n", orthogonalNeighbors[0]->getPosition().x, orthogonalNeighbors[0]->getPosition().y, orthogonalNeighbors[0]->getPosition().z, this->getPosition().x, this->getPosition().y, this->getPosition().z );
00890                 printf( "J: %f %f %f O: %f %f %f \n", orthogonalNeighbors[1]->getPosition().x, orthogonalNeighbors[1]->getPosition().y, orthogonalNeighbors[1]->getPosition().z, this->getPosition().x, this->getPosition().y, this->getPosition().z );
00891                 printf( "K: %f %f %f O: %f %f %f \n", orthogonalNeighbors[2]->getPosition().x, orthogonalNeighbors[2]->getPosition().y, orthogonalNeighbors[2]->getPosition().z, this->getPosition().x, this->getPosition().y, this->getPosition().z );
00892 */
00893 
00894 
00895 /*
00896                 COME_Vector3D vI( orthogonalNeighbors[0]->getGlobalPosition() - this->getGlobalPosition() );
00897                 COME_Vector3D vJ( orthogonalNeighbors[1]->getGlobalPosition() - this->getGlobalPosition() );
00898                 COME_Vector3D vK( orthogonalNeighbors[2]->getGlobalPosition() - this->getGlobalPosition() );
00899                 */
00900                 
00901                 originalLocalFrame.push_back(vI);
00902                 originalLocalFrame.push_back(vJ);
00903                 originalLocalFrame.push_back(vK);
00904         }
00905 }
00906 
00911 void
00912 COME_Molecule::makeLocalFrame(){
00913         
00914         if( orthogonalNeighbors.size() > 0 ){
00915 
00916                 if( localFrame == NULL ){
00917 
00918                         localFrame = new COME_Matrix();
00919                 }
00920 
00921                 
00922                 COME_Vector3D vI( orthogonalNeighbors[0]->getGlobalPosition() - this->getGlobalPosition() ); vI.vpNormalize();
00923                 COME_Vector3D vJ( orthogonalNeighbors[1]->getGlobalPosition() - this->getGlobalPosition() ); vJ.vpNormalize();
00924                 COME_Vector3D vK( orthogonalNeighbors[2]->getGlobalPosition() - this->getGlobalPosition() ); vK.vpNormalize();
00925                 
00926                 //printf( "I: %f %f %f O: %f %f %f \n", orthogonalNeighbors[0]->getGlobalPosition().x, orthogonalNeighbors[0]->getGlobalPosition().y, orthogonalNeighbors[0]->getGlobalPosition().z, vI.x, vI.y, vI.z );
00927                 //printf( "J: %f %f %f O: %f %f %f \n", orthogonalNeighbors[1]->getGlobalPosition().x, orthogonalNeighbors[1]->getGlobalPosition().y, orthogonalNeighbors[1]->getGlobalPosition().z, vJ.x, vJ.y, vJ.z );
00928                 //printf( "K: %f %f %f O: %f %f %f \n", orthogonalNeighbors[2]->getGlobalPosition().x, orthogonalNeighbors[2]->getGlobalPosition().y, orthogonalNeighbors[2]->getGlobalPosition().z, vK.x, vK.y, vK.z );
00929 
00931                 /*COME_Vector3D vI( 1, 0, 0 );
00932                 COME_Vector3D vJ( 0, 1, 0 );
00933                 COME_Vector3D vK( 0, 0, 1 );
00934                 */
00935                 
00936                 COME_Vector3D x(1,0,0); 
00937                 COME_Vector3D y(1,0,0); 
00938                 
00939                 Quaternion rotation(0,0,0,1);           
00940                                 
00941                 double angle = vI.getAngle(originalLocalFrame[0]);
00942                 x = originalLocalFrame[0];
00943                 y = vI;
00944                 double angle2 = vJ.getAngle(originalLocalFrame[1]);
00945                 double angle3 = vK.getAngle(originalLocalFrame[2]);
00946                 
00947                 if (angle2 > angle) {
00948                         x = originalLocalFrame[1];
00949                         y= vJ;
00950                         angle = angle2;
00951                 }
00952                 if (angle3 > angle){
00953                         x = originalLocalFrame[2];
00954                         y = vK;
00955                 }
00956                                 
00957                 rotation.makeFromVecs(x, y);
00958                 double RotationMatrix[3][3];
00959                 rotation.ToMatrix(RotationMatrix);
00960                                 
00961                 localFrame->setValueAt( 0, 0, RotationMatrix[0][0]);
00962                 localFrame->setValueAt( 1, 0, RotationMatrix[1][0]);
00963                 localFrame->setValueAt( 2, 0, RotationMatrix[2][0]);
00964                 localFrame->setValueAt( 3, 0, this->getPosition().getX() );
00965                 
00966                 localFrame->setValueAt( 0, 1, RotationMatrix[0][1] );
00967                 localFrame->setValueAt( 1, 1, RotationMatrix[1][1] );
00968                 localFrame->setValueAt( 2, 1, RotationMatrix[2][1] );
00969                 localFrame->setValueAt( 3, 1, this->getPosition().getY() );
00970 
00971                 localFrame->setValueAt( 0, 2, RotationMatrix[0][2]);
00972                 localFrame->setValueAt( 1, 2, RotationMatrix[1][2]);
00973                 localFrame->setValueAt( 2, 2, RotationMatrix[2][2]);
00974                 localFrame->setValueAt( 3, 2, this->getPosition().getZ() );
00975 
00976                 localFrame->setValueAt( 0, 3, 0.0 );
00977                 localFrame->setValueAt( 1, 3, 0.0 );
00978                 localFrame->setValueAt( 2, 3, 0.0 );
00979                 localFrame->setValueAt( 3, 3, 1.0 );    
00980         }
00981 }
comemolecule.cpp
