COME - Biomodel

00001 /***************************************************************************
00002  *   Copyright (C) 2004 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  
00021 #include        <physics/comeproximity.h>
00022 #include        <bio/comemolecule.h>
00023 
00024 #include        <math.h>
00025 
00026 COME_Proximity::COME_Proximity( COME_Vertex3D *oA, COME_Vertex3D * oB ){
00027  
00028         //objA = oA;
00029         //objB = oB;
00030         
00031 }
00032 
00033 COME_Proximity::COME_Proximity( COME_Vertex3D *oA, COME_Face * fB, double hardSoftP ){
00034  
00035         objA = oA;
00036         faceB = fB;
00037         hardSoft = hardSoftP;
00038         
00039         // Calculate point on B
00040         //pointOnB = faceB->getCenterGlobalPosition();
00041         pointOnB = faceB->getClosestPointOnFaceToPoint( COME_Point3D( objA->x, objA->y, objA->z ) );
00042         //COME_Vertex3D *vvP = faceB->getClosestVertexPt( COME_Point3D( objA->x, objA->y, objA->z ) );
00043         //pointOnB =COME_Point3D( vvP->x, vvP->y, vvP->z );
00044 }
00045  
00048 void
00049 COME_Proximity::updatePointOnFace(){
00050  
00051         //pointOnB = faceB->getCenterGlobalPosition();
00052         pointOnB = faceB->getClosestPointOnFaceToPoint( COME_Point3D( objA->x, objA->y, objA->z ) );
00053         
00054  }
00055  
00058 double
00059 COME_Proximity::signedDistance(){
00060  
00061         COME_Vector3D p_a = objA->getVertexAsPoint3D() - COME_Vector3D( faceB->getVertexGlobalPositionPt( 0 )->x, faceB->getVertexGlobalPositionPt( 0 )->y, faceB->getVertexGlobalPositionPt( 0 )->z );
00062         double dist = pointOnB.vpDistance( objA->getVertexAsPoint3D() );
00063         
00064         /*double signal = faceB->getNormalGlobalPosition().vpDotProduct( p_a );
00065         if( signal < 0.0 ) {
00066                 dist = -dist;
00067         }*/
00068         
00069         if( ( faceB->getNormalGlobalPositionPt()->vpDotProduct( objA->getNormalGlobalPosition() ) < 0.0 ) && ( faceB->isInsideGlobalGOOD( objA->getVertexAsPoint3D() ) ) )
00070                 dist = -dist;
00071         
00072         return dist;
00073         
00074  }
00075  
00078 double
00079 COME_Proximity::signedDistance( COME_Point3D p ){
00080  
00081         COME_Vector3D p_a = p - COME_Vector3D( faceB->getVertexGlobalPositionPt( 0 )->x, faceB->getVertexGlobalPositionPt( 0 )->y, faceB->getVertexGlobalPositionPt( 0 )->z );
00082         double dist = pointOnB.vpDistance( p );
00083         
00084         
00085         /*double signal = faceB->getNormalGlobalPosition().vpDotProduct( p_a );
00086         if( signal < 0.0 ) {
00087                 dist = -dist;
00088         }*/
00089         
00090         if( ( faceB->getNormalGlobalPositionPt()->vpDotProduct( objA->getNormalGlobalPosition() ) < 0.0 ) && ( faceB->isInsideGlobalGOOD( p ) ) )
00091                 dist = -dist;
00092         
00093         return dist;
00094  }
00095 
00099 void
00100 COME_Proximity::updatePosition(){
00101 
00102         COME_Vector3D posCorrection = pointOnB - objA->getVertexAsPoint3D();
00103         
00104         // update objA
00105         objA->addCollisionDisp( posCorrection/hardSoft );
00106         
00107         // update faceB
00108         COME_Vertex3D *A =  faceB->getVertexGlobalPositionPt( 0 );
00109         COME_Vertex3D *B =  faceB->getVertexGlobalPositionPt( 1 );
00110         COME_Vertex3D *C =  faceB->getVertexGlobalPositionPt( 2 );
00111         
00112         A->addCollisionDisp( -posCorrection/hardSoft );
00113         B->addCollisionDisp( -posCorrection/hardSoft );
00114         C->addCollisionDisp( -posCorrection/hardSoft );
00115 
00116 }
00117 
00120 void
00121 COME_Proximity::updateVelocity(){
00122 
00123         COME_Vector3D velocDir;
00124         if( !(signedDistance() < 0) ){
00125                 velocDir = objA->getVertexAsPoint3D() - pointOnB; velocDir.vpNormalize();
00126         
00127                 COME_Vector3D velocA = objA->getBlendedVelocity();
00128                 COME_Vector3D velocB = faceB->getBlendedVelocity( pointOnB );
00129                 COME_Vector3D velocRelativeA = velocDir * COME_Vector3D(velocA-velocB).vpDotProduct(velocDir);
00130                 
00131                 COME_Point3D posObjATimePlusOne = objA->getVertexAsPoint3D() + ( velocRelativeA * COME::timestepGlobal * 1.0 );
00132                 
00133                 if( signedDistance( posObjATimePlusOne ) < 0.0 ){
00134                         
00135                         COME_Vector3D velocCorrection = velocRelativeA - ( ( pointOnB - objA->getVertexAsPoint3D() ) * ( 1.0 / ( COME::timestepGlobal * 1.0 ) ) );
00136                         
00137                         // update objA
00138                         objA->addVelocityDisp( -velocCorrection/hardSoft );
00139                         
00140                         // update faceB
00141                         COME_Vertex3D *A =  faceB->getVertexGlobalPositionPt( 0 );
00142                         COME_Vertex3D *B =  faceB->getVertexGlobalPositionPt( 1 );
00143                         COME_Vertex3D *C =  faceB->getVertexGlobalPositionPt( 2 );
00144                         
00145                         A->addVelocityDisp( velocCorrection/hardSoft );
00146                         B->addVelocityDisp( velocCorrection/hardSoft );
00147                         C->addVelocityDisp( velocCorrection/hardSoft );
00148                 }
00149         } else {
00150                 updatePosition();
00151                 objA->addVelocityDisp( -objA->getBlendedVelocity() );
00152                 faceB->getVertexGlobalPositionPt( 0 )->addVelocityDisp( -faceB->getVertexGlobalPositionPt( 0 )->getBlendedVelocity() );
00153                 faceB->getVertexGlobalPositionPt( 1 )->addVelocityDisp( -faceB->getVertexGlobalPositionPt( 1 )->getBlendedVelocity() );
00154                 faceB->getVertexGlobalPositionPt( 2 )->addVelocityDisp( -faceB->getVertexGlobalPositionPt( 2 )->getBlendedVelocity() );
00155         }
00156         
00157 }
00158 
00161 void
00162 COME_Proximity::resetDisplacements(){
00163 
00164         // reset position displacements
00165         objA->setCollisionDisp( COME_Vector3D() );
00166         faceB->getVertexGlobalPositionPt( 0 )->setCollisionDisp( COME_Vector3D() );
00167         faceB->getVertexGlobalPositionPt( 1 )->setCollisionDisp( COME_Vector3D() );
00168         faceB->getVertexGlobalPositionPt( 2 )->setCollisionDisp( COME_Vector3D() );
00169         
00170         // reset velocity displacements
00171         objA->setVelocityDisp( COME_Vector3D() );
00172         faceB->getVertexGlobalPositionPt( 0 )->setVelocityDisp( COME_Vector3D() );
00173         faceB->getVertexGlobalPositionPt( 1 )->setVelocityDisp( COME_Vector3D() );
00174         faceB->getVertexGlobalPositionPt( 2 )->setVelocityDisp( COME_Vector3D() );
00175         
00176         // reset anchors 
00177         int j;
00178         vector<COME_Molecule*> localAnchors = objA->getAnchors();
00179         for( j = 0; j < localAnchors.size(); j++ ){
00180                 localAnchors[j]->setUpdated(false);
00181         }
00182         localAnchors = faceB->getVertexGlobalPositionPt( 0 )->getAnchors();
00183         for( j = 0; j < localAnchors.size(); j++ ){
00184                 localAnchors[j]->setUpdated(false);
00185         }
00186         localAnchors = faceB->getVertexGlobalPositionPt( 1 )->getAnchors();
00187         for( j = 0; j < localAnchors.size(); j++ ){
00188                 localAnchors[j]->setUpdated(false);
00189         }
00190         localAnchors = faceB->getVertexGlobalPositionPt( 2 )->getAnchors();
00191         for( j = 0; j < localAnchors.size(); j++ ){
00192                 localAnchors[j]->setUpdated(false);
00193         }
00194 }
00195 
00200 void
00201 COME_Proximity::updateAnchors(){ 
00202 
00203         // Update anchors of objA
00204         vector<COME_Molecule*> localAnchors = objA->getAnchors();
00205         for( int j = 0; j < localAnchors.size(); j++ ){
00206 
00207                 if( ( ! localAnchors[j]->isFixed() ) && ( ! localAnchors[j]->isUpdated() ) ){
00208                 
00209                         vector<COME_Vertex3D*> *vertices = localAnchors[j]->getBuois();
00210                         int i;
00211                         double sumDists = 0.0;
00212                         int count = 1;
00213                         for( i = 0; i < vertices->size(); i++ ){
00214                                 sumDists += pow( (*vertices)[i]->vpDistance( localAnchors[j]->getGlobalPosition() ), 2.0 );
00215                         }
00216                         
00217                         count = i-1;
00218                         if( count == 0 ) count = 1;
00219                         
00220                         COME_Vector3D dispPosition;
00221                         COME_Vector3D dispVelocity;
00222                         for( i = 0; i < vertices->size(); i++ ){
00223                                 //double w = ( 1.0 - ( pow( (*vertices)[i]->vpDistance( localAnchors[j]->getGlobalPosition() ), 2.0 ) / sumDists ) ) / (count*2.0);
00224                                 //dispPosition =  dispPosition + ( (*vertices)[i]->getCollisionDispAvg() * w );
00225                                 //dispVelocity =  dispVelocity + ( (*vertices)[i]->getVelocityDispAvg() * w );
00226                                 if( dispPosition.vpModule() < ( (*vertices)[i]->getCollisionDispAvg().vpModule()  ) )
00227                                         dispPosition = ( (*vertices)[i]->getCollisionDispAvg()  );
00228                                 if( dispVelocity.vpModule() < ( (*vertices)[i]->getVelocityDispAvg().vpModule() ) )
00229                                         dispVelocity = ( (*vertices)[i]->getVelocityDispAvg() );
00230                         }
00231                         
00232                         COME_Point3D newPos = localAnchors[j]->getPosition() + dispPosition;
00233                         COME_Vector3D newVeloc = localAnchors[j]->getVelocity() + dispVelocity;
00234                         //localAnchors[j]->setPosition( newPos );
00235                                 
00236                         //printf( "dispVelocityonAnchor: %f %f %f newVelocityonAnchor: %f %f %f \n", dispVelocity.x, dispVelocity.y, dispVelocity.z, newVeloc.x, newVeloc.y, newVeloc.z );
00237                         //localAnchors[j]->setVelocity( newVeloc );
00238                         localAnchors[j]->setUpdated(true);
00239                 }
00240         }
00241         
00242         // Update anchors of objB
00243         for( int oBi = 0; oBi < 3; oBi++ ){
00244                 localAnchors = faceB->getVertexGlobalPositionPt( oBi )->getAnchors();
00245                 for( int jB = 0; jB < localAnchors.size(); jB++ ){
00246         
00247                         if( ( ! localAnchors[jB]->isFixed() ) && ( ! localAnchors[jB]->isUpdated() ) ){
00248                         
00249                                 vector<COME_Vertex3D*> *vertices = localAnchors[jB]->getBuois();
00250                                 int i;
00251                                 double sumDists = 0.0;
00252                                 int count = 1;
00253                                 for( i = 0; i < vertices->size(); i++ ){
00254                                         sumDists += pow( (*vertices)[i]->vpDistance( localAnchors[jB]->getGlobalPosition() ), 2.0 );
00255                                 }
00256                                 
00257                                 count = i-1;
00258                                 if( count == 0 ) count = 1;
00259                                 
00260                                 COME_Vector3D dispPosition;
00261                                 COME_Vector3D dispVelocity;
00262                                 for( i = 0; i < vertices->size(); i++ ){
00263                                         //double w = ( 1.0 - ( pow( (*vertices)[i]->vpDistance( localAnchors[jB]->getGlobalPosition() ), 2.0 ) / sumDists ) ) / (count*2.0);
00264                                         //dispPosition =  dispPosition +  ( (*vertices)[i]->getCollisionDispAvg() * w );
00265                                         //dispVelocity =  dispVelocity +   ( (*vertices)[i]->getVelocityDispAvg() * w );
00266                                         if( dispPosition.vpModule() < ( (*vertices)[i]->getCollisionDispAvg().vpModule()  ) )
00267                                                 dispPosition = ( (*vertices)[i]->getCollisionDispAvg()  );
00268                                         if( dispVelocity.vpModule() < ( (*vertices)[i]->getVelocityDispAvg().vpModule() ) )
00269                                                 dispVelocity = ( (*vertices)[i]->getVelocityDispAvg() );
00270                                 }
00271                                 COME_Point3D newPos = localAnchors[jB]->getPosition() + dispPosition;
00272                                 COME_Vector3D newVeloc = localAnchors[jB]->getVelocity() + dispVelocity;
00273                                 //localAnchors[jB]->setPosition( newPos );
00274                                 //printf( "dispPos2onAnchor: %f %f %f newPos2onAnchor: %f %f %f \n", dispPosition.x, dispPosition.y, dispPosition.z, newPos.x, newPos.y, newPos.z );
00275                                 //localAnchors[jB]->setVelocity( newVeloc );
00276                                 localAnchors[jB]->setUpdated(true);
00277                         }
00278                 }
00279         }
00280 }
00281 
00285 void
00286 COME_Proximity::updateAnchorsOptimization(){
00287 
00288         // fill big arrays of positions and velocities
00289         // multiply J by the arrays
00290         // apply result onto molecules.
00291 
00292 }
comeproximity.cpp
