Page principale | Liste des namespaces | Hiérarchie des classes | Liste par ordre alphabétique | Liste des composants | Liste des fichiers | Composants | Déclarations

pi2d14_molecule_tapis.cpp

Aller à la documentation de ce fichier.
00001 
00002 #include <iostream>
00003 
00004 #include "pi2d14/pi2d14_molecule_tapis.h"
00005 
00006 #include "Atom.h"
00007 #include "ConfigReader.h"
00008 #include "Math.h"
00009 #include "GL/gl.h"
00010 
00011 
00012 ALGOATOMD::MoleculeTapis::MoleculeTapis(int buildParam, int typeDep)
00013 {
00014         size = 1 + buildParam;          //calcul de la taille demandee
00015         //pour le tapis, size est le nombre d'atome sur le coté
00016         
00017         //construction de la molecule
00018         atoms = new API::Atom[size*size];
00019         for(int x=0;x<size;x++)
00020         {
00021                 for(int y=0;y<size;y++)
00022                 {
00023                         int num = x + y*size;
00024                         atoms[num].util->setCartesianPosition(new API::CartesianPosition( atomSize+2*x*atomSize, atomSize+2*y*atomSize, atomSize) );
00025                 }
00026         }
00027         
00028         linkAllLegs();  // TODO faire un linkage plus otpimis�
00029         
00030         m1=false;
00031         m2=false;
00032         m3=false;
00033         indexInit=-1;
00034         index1=-1;
00035         index2=-1;
00036         index3=-1;
00037         
00038         typeDeplacement = typeDep;
00039         
00040         switch(typeDeplacement)
00041         {
00042                 case 1:
00043                         initDep1();
00044                         break;
00045                         case 2:
00046                                 initDep2();
00047                                 break;
00048                                 default:
00049                                         //pas de d�placement
00050                                         break;
00051         }
00052 }
00053 
00054 ALGOATOMD::MoleculeTapis::~MoleculeTapis()
00055 {
00056         delete [] atoms;
00057 }
00058 
00059 void ALGOATOMD::MoleculeTapis::linkAllLegs()
00060 {
00061         //les pointeur sur les deux atom courant
00062         API::Atom *atom1;
00063         API::Atom *atom2;
00064         
00065         for(int a1=0;a1<size*size;a1++)
00066         {
00067                 for(int a2=0;a2<size*size;a2++)
00068                 {
00069                         //on v�rifie qu'on est pas 2 fois sur le meme atome
00070                         if(a1!=a2)
00071                         {
00072                                 atom1 = &atoms[a1];
00073                                 atom2 = &atoms[a2];
00074                                 for(int p1=0;p1<6;p1++)
00075                                 {
00076                                         if(!atom1->getLeg(p1)->isLinked())
00077                                         {
00078                                                 for(int p2=0;p2<6;p2++)
00079                                                 {
00080                                                         //on v�rifie qu'on est pas 2 fois sur les meme pattes=> annul� : id�e � conne qui sert a rien
00081                                                         //on v�rifie que les deux pattes ne sont pas d�j� connect�
00082                                                         if( !atom2->getLeg(p2)->isLinked() )
00083                                                         {
00084                                                                 if( atom2->getLeg(p2)->util->isConnectable(atom1->getLeg(p1)) )
00085                                                                 {
00086                                                                         atom2->getLeg(p2)->setLink(atom1->getLeg(p1));
00087                                                                 }
00088                                                         }
00089                                                 }
00090                                         }
00091                                 }
00092                         }
00093                 }
00094         }
00095 }
00096 
00097 void ALGOATOMD::MoleculeTapis::processMovement()
00098 {
00099         switch(typeDeplacement)
00100         {
00101                 case 1:
00102                         algoDep1();
00103                         break;
00104                         case 2:
00105                                 algoDep2();
00106                                 break;
00107                                 default:
00108                                         //pas de d�placement
00109                                         break;
00110         }
00111 }
00112 
00113 void ALGOATOMD::MoleculeTapis::renderMoleculeInfo()
00114 {
00115         switch(typeDeplacement)
00116         {
00117                 case 1:
00118                         break;
00119                         case 2:
00120                                 debugAlgo2();
00121                                 break;
00122                                 default:
00123                                         //pas de d�placement
00124                                         break;
00125         }
00126 }
00127 
00128 /*int MoleculeAtome::getNbAlgo()
00129 {
00130         return 1;               //le nombre d'algo de dep TODO : si on ajoute un nouvel algo, l'incr�ment� de 1.
00131 }*/
00132 
00133 void ALGOATOMD::MoleculeTapis::initDep1()
00134 {
00135 }
00136 
00137 void ALGOATOMD::MoleculeTapis::algoDep1()
00138 {
00139         if(!m1 && !m2&& !m3)
00140         {
00141                 //On regarde si les mouvements ind�x� par indexInit sont termin�s
00142                 if(atoms[0].getMolecule()->isFinished(indexInit))
00143                 {
00144                         printf("11\n");
00145                         //On r�cup�re un nouvel index dans index1
00146                         index1 = atoms[0].getMolecule()->getNextSyncIndex();
00147                         
00148                         
00149                         //pour chaque ligne impair
00150                         for(int x=1;x<size;x+=2)
00151                         {
00152                                 //pour chaque colone impair
00153                                 for(int y=1;y<size;y+=2)
00154                                 {
00155                                         int num = x + y*size;
00156                                         atoms[num].getLeg(2)->getServoMotor(0)->setTargetPosition(1,REPLACE,0,index1);
00157                                 }
00158                         }
00159                         
00160                         //pour chaque ligne pair
00161                         for(int x=0;x<size;x+=2)
00162                         {
00163                                 //pour chaque colone pair
00164                                 for(int y=0;y<size;y+=2)
00165                                 {
00166                                         int num = x + y*size;
00167                                         atoms[num].getLeg(2)->getServoMotor(0)->setTargetPosition(1,REPLACE,0,index1);
00168                                 }
00169                         }
00170                         
00171                         
00172                         m1=true;        //On indique que le mouvement 1 est parti
00173                         m2=false;
00174                         m3=false;
00175                         
00176                         //On lib�re l'index indexInit
00177                         atoms[0].getMolecule()->releaseSyncIndex(indexInit);
00178                 }
00179         }
00180         
00181         if(m1 && !m2 && !m3)
00182         {
00183                 if(atoms[0].getMolecule()->isFinished(index1))
00184                 {
00185                         printf("22\n");
00186                         index2 = atoms[0].getMolecule()->getNextSyncIndex();
00187                         
00188                         
00189                         //pour chaque ligne impair
00190                         for(int x=1;x<size;x+=2)
00191                         {
00192                                 //pour chaque colone pair
00193                                 for(int y=0;y<size;y+=2)
00194                                 {
00195                                         int num = x + y*size;
00196                                         atoms[num].getLeg(2)->getServoMotor(0)->setTargetPosition(249,REPLACE,0,index2);
00197                                 }
00198                         }
00199                         
00200                         //pour chaque ligne pair
00201                         for(int x=0;x<size;x+=2)
00202                         {
00203                                 //pour chaque colone impair
00204                                 for(int y=1;y<size;y+=2)
00205                                 {
00206                                         int num = x + y*size;
00207                                         atoms[num].getLeg(2)->getServoMotor(0)->setTargetPosition(249,REPLACE,0,index2);
00208                                 }
00209                         }
00210                         
00211                         
00212                         m1=false;
00213                         m2=true;
00214                         m3=false;
00215                         
00216                         atoms[0].getMolecule()->releaseSyncIndex(index1);
00217                 }
00218         }
00219         
00220         if(!m1 && m2 && !m3)
00221         {
00222                 if(atoms[0].getMolecule()->isFinished(index2))
00223                 {
00224                         printf("33\n");
00225                         index3 = atoms[0].getMolecule()->getNextSyncIndex();
00226                         
00227                         
00228                         //pour chaque ligne impair
00229                         for(int x=1;x<size;x+=2)
00230                         {
00231                                 //pour chaque colone impair
00232                                 for(int y=1;y<size;y+=2)
00233                                 {
00234                                         int num = x + y*size;
00235                                         atoms[num].getLeg(2)->getServoMotor(0)->setToHalfPosition(REPLACE,0,index3);
00236                                 }
00237                         }
00238                         
00239                         //pour chaque ligne pair
00240                         for(int x=0;x<size;x+=2)
00241                         {
00242                                 //pour chaque colone pair
00243                                 for(int y=0;y<size;y+=2)
00244                                 {
00245                                         int num = x + y*size;
00246                                         atoms[num].getLeg(2)->getServoMotor(0)->setToHalfPosition(REPLACE,0,index3);
00247                                 }
00248                         }
00249                         
00250                         
00251                         m1=false;
00252                         m2=false;
00253                         m3=true;
00254                         
00255                         atoms[0].getMolecule()->releaseSyncIndex(index2);
00256                 }
00257         }
00258         
00259         if(!m1 && !m2 && m3)
00260         {
00261                 if(atoms[0].getMolecule()->isFinished(index3))
00262                 {
00263                         printf("ii\n");
00264                         indexInit = atoms[0].getMolecule()->getNextSyncIndex();
00265                         
00266                         
00267                         //pour chaque ligne impair
00268                         for(int x=1;x<size;x+=2)
00269                         {
00270                                 //pour chaque colone pair
00271                                 for(int y=0;y<size;y+=2)
00272                                 {
00273                                         int num = x + y*size;
00274                                         atoms[num].getLeg(2)->getServoMotor(0)->setToHalfPosition(REPLACE,0,index2);
00275                                 }
00276                         }
00277                         
00278                         //pour chaque ligne pair
00279                         for(int x=0;x<size;x+=2)
00280                         {
00281                                 //pour chaque colone impair
00282                                 for(int y=1;y<size;y+=2)
00283                                 {
00284                                         int num = x + y*size;
00285                                         atoms[num].getLeg(2)->getServoMotor(0)->setToHalfPosition(REPLACE,0,index2);
00286                                 }
00287                         }
00288                         
00289                         m1=false;
00290                         m2=false;
00291                         m3=false;
00292                         
00293                         atoms[0].getMolecule()->releaseSyncIndex(index3);
00294                 }
00295         }
00296 }
00297 
00298 
00299 void ALGOATOMD::MoleculeTapis::initDep2()
00300 {
00301 }
00302 
00303 void ALGOATOMD::MoleculeTapis::algoDep2()
00304 {
00305         //On regarde que auncun mouvement soit en cours
00306         if(!m1 && !m2&& !m3)
00307         {
00308                 //On regarde si les mouvements ind�x� par indexInit sont termin�s
00309                 if(atoms[0].getMolecule()->isFinished(indexInit))
00310                 {
00311                         calcAngle();
00312                         printf("11\n");
00313                         //On r�cup�re un nouvel index dans index1
00314                         index1 = atoms[0].getMolecule()->getNextSyncIndex();
00315                         
00316                         
00317                         //pour chaque ligne impair
00318                         for(int x=1;x<size;x+=2)
00319                         {
00320                                 //pour chaque colone impair
00321                                 for(int y=1;y<size;y+=2)
00322                                 {
00323                                         int num = x + y*size;
00324                                         atoms[num].getLeg(2)->getServoMotor(0)->setTargetPosition(125 + (int)(124*cos(pi/2 + angle)), REPLACE, 0, index1);
00325                                         atoms[num].getLeg(2)->getServoMotor(1)->setTargetPosition(125 + (int)(124*sin(pi/2 + angle)), REPLACE, 0, index1);
00326                                 }
00327                         }
00328                         
00329                         //pour chaque ligne pair
00330                         for(int x=0;x<size;x+=2)
00331                         {
00332                                 //pour chaque colone pair
00333                                 for(int y=0;y<size;y+=2)
00334                                 {
00335                                         int num = x + y*size;
00336                                         atoms[num].getLeg(2)->getServoMotor(0)->setTargetPosition(125 + (int)(124*cos(pi/2 + angle)), REPLACE, 0, index1);
00337                                         atoms[num].getLeg(2)->getServoMotor(1)->setTargetPosition(125 + (int)(124*sin(pi/2 + angle)), REPLACE, 0, index1);
00338                                 }
00339                         }
00340                         
00341                         
00342                         m1=true;        //On indique que le mouvement 1 est parti
00343                         m2=false;
00344                         m3=false;
00345                         
00346                         //On lib�re l'index indexInit
00347                         atoms[0].getMolecule()->releaseSyncIndex(indexInit);
00348                 }
00349         }
00350         
00351         if(m1 && !m2 && !m3)
00352         {
00353                 if(atoms[0].getMolecule()->isFinished(index1))
00354                 {
00355                         printf("22\n");
00356                         index2 = atoms[0].getMolecule()->getNextSyncIndex();
00357                         
00358                         
00359                         //pour chaque ligne impair
00360                         for(int x=1;x<size;x+=2)
00361                         {
00362                                 //pour chaque colone pair
00363                                 for(int y=0;y<size;y+=2)
00364                                 {
00365                                         int num = x + y*size;
00366                                         atoms[num].getLeg(2)->getServoMotor(0)->setTargetPosition(125 - (int)(124*cos(pi/2 +angle )), REPLACE, 0, index2);
00367                                         atoms[num].getLeg(2)->getServoMotor(1)->setTargetPosition(125 - (int)(124*sin(pi/2 +angle )), REPLACE, 0, index2);
00368                                 }
00369                         }
00370                         
00371                         //pour chaque ligne pair
00372                         for(int x=0;x<size;x+=2)
00373                         {
00374                                 //pour chaque colone impair
00375                                 for(int y=1;y<size;y+=2)
00376                                 {
00377                                         int num = x + y*size;
00378                                         atoms[num].getLeg(2)->getServoMotor(0)->setTargetPosition(125 - (int)(124*cos(pi/2 +angle )), REPLACE, 0, index2);
00379                                         atoms[num].getLeg(2)->getServoMotor(1)->setTargetPosition(125 - (int)(124*sin(pi/2 +angle )), REPLACE, 0, index2);
00380                                 }
00381                         }
00382                         
00383                         
00384                         m1=false;
00385                         m2=true;
00386                         m3=false;
00387                         
00388                         atoms[0].getMolecule()->releaseSyncIndex(index1);
00389                 }
00390         }
00391         
00392         if(!m1 && m2 && !m3)
00393         {
00394                 if(atoms[0].getMolecule()->isFinished(index2))
00395                 {
00396                         printf("33\n");
00397                         index3 = atoms[0].getMolecule()->getNextSyncIndex();
00398                         
00399                         
00400                         //pour chaque ligne impair
00401                         for(int x=1;x<size;x+=2)
00402                         {
00403                                 //pour chaque colone impair
00404                                 for(int y=1;y<size;y+=2)
00405                                 {
00406                                         int num = x + y*size;
00407                                         atoms[num].getLeg(2)->getServoMotor(0)->setToHalfPosition(REPLACE,0,index3);
00408                                         atoms[num].getLeg(2)->getServoMotor(1)->setToHalfPosition(REPLACE,0,index3);
00409                                 }
00410                         }
00411                         
00412                         //pour chaque ligne pair
00413                         for(int x=0;x<size;x+=2)
00414                         {
00415                                 //pour chaque colone pair
00416                                 for(int y=0;y<size;y+=2)
00417                                 {
00418                                         int num = x + y*size;
00419                                         atoms[num].getLeg(2)->getServoMotor(0)->setToHalfPosition(REPLACE,0,index3);
00420                                         atoms[num].getLeg(2)->getServoMotor(1)->setToHalfPosition(REPLACE,0,index3);
00421                                 }
00422                         }
00423                         
00424                         
00425                         m1=false;
00426                         m2=false;
00427                         m3=true;
00428                         
00429                         atoms[0].getMolecule()->releaseSyncIndex(index2);
00430                 }
00431         }
00432         
00433         if(!m1 && !m2 && m3)
00434         {
00435                 if(atoms[0].getMolecule()->isFinished(index3))
00436                 {
00437                         printf("ii\n");
00438                         calcAngle();
00439                         indexInit = atoms[0].getMolecule()->getNextSyncIndex();
00440                         
00441                         
00442                         //pour chaque ligne impair
00443                         for(int x=1;x<size;x+=2)
00444                         {
00445                                 //pour chaque colone pair
00446                                 for(int y=0;y<size;y+=2)
00447                                 {
00448                                         int num = x + y*size;
00449                                         atoms[num].getLeg(2)->getServoMotor(0)->setToHalfPosition(REPLACE,0,index2);
00450                                         atoms[num].getLeg(2)->getServoMotor(1)->setToHalfPosition(REPLACE,0,index2);
00451                                 }
00452                         }
00453                         
00454                         //pour chaque ligne pair
00455                         for(int x=0;x<size;x+=2)
00456                         {
00457                                 //pour chaque colone impair
00458                                 for(int y=1;y<size;y+=2)
00459                                 {
00460                                         int num = x + y*size;
00461                                         atoms[num].getLeg(2)->getServoMotor(0)->setToHalfPosition(REPLACE,0,index2);
00462                                         atoms[num].getLeg(2)->getServoMotor(1)->setToHalfPosition(REPLACE,0,index2);
00463                                 }
00464                         }
00465                         
00466                         m1=false;
00467                         m2=false;
00468                         m3=false;
00469                         
00470                         atoms[0].getMolecule()->releaseSyncIndex(index3);
00471                 }
00472         }
00473 }
00474 
00475 
00476 bool ALGOATOMD::MoleculeTapis::needMoreMouvement()
00477 {
00478         bool ret = true;
00479         API::CartesianPosition destination = *this->getDestination();                   //position point final
00480         
00481         //le centre de gravite du tapis
00482         double x1 = atoms[0].util->getCartesianPosition().getX();
00483         double y1 = atoms[0].util->getCartesianPosition().getY();
00484         double x2 = atoms[size*size-1].util->getCartesianPosition().getX();
00485         double y2 = atoms[size*size-1].util->getCartesianPosition().getY();
00486         xg = x1 + (x2-x1)/2;
00487         yg = y1 + (y2-y1)/2;
00488         
00489         double demiCote = size*atomSize;        //bn atom * demilargeur d'un atom = demilargeur de tapis
00490         
00491         if( (xg+demiCote)>destination.getX() && (xg-demiCote)<destination.getX() && (yg-demiCote)<destination.getY() && (yg+demiCote)>destination.getY() )
00492         {
00493                 ret = false;
00494         }
00495         
00496         return ret;
00497 }
00498 
00499 void ALGOATOMD::MoleculeTapis::debugAlgo2()
00500 {
00501         glDisable(GL_TEXTURE_2D);
00502         glDisable(GL_LIGHTING);
00503         glLineWidth(8);
00504         glBegin(GL_LINES);
00505         glColor3f(0.3,0.3,0.3);
00506         glVertex3f(xg, yg, 0.5);
00507         glVertex3f(xg+ 10*xf , yg+ 10*yf , 0.5);
00508         glColor3f(1,0.3,0.3);
00509         glVertex3f(xg, yg, 0.5);
00510         glVertex3f(xg+ 10*xrepX , yg+ 10*yrepX , 0.5);
00511         glColor3f(0.3,0.3,1);
00512         glVertex3f(xg, yg, 0.5);
00513         glVertex3f(xg+ 10*xrepY , yg+ 10*yrepY , 0.5);
00514         glEnd();
00515         glEnable(GL_TEXTURE_2D);
00516         glEnable(GL_LIGHTING);
00517         glLineWidth(1);
00518 }
00519 
00520 void ALGOATOMD::MoleculeTapis::calcAngle()
00521 {
00522         double longueur;
00523         double x1 = atoms[0].util->getCartesianPosition().getX();
00524         double y1 = atoms[0].util->getCartesianPosition().getY();
00525         double x2 = atoms[1].util->getCartesianPosition().getX();
00526         double y2 = atoms[1].util->getCartesianPosition().getY();
00527         
00528         //calcul de l'axe X du repere du tapis
00529         xrepX=x2-x1;
00530         yrepX=y2-y1;
00531         longueur = sqrt(xrepX*xrepX + yrepX*yrepX );
00532         xrepX/=longueur;
00533         yrepX/=longueur;
00534         
00535         //calcul de l'axe Y du repere du tapis
00536         xrepY=-yrepX;
00537         yrepY= xrepX;
00538         
00539         //le centre de gravite du tapis
00540         x1 = atoms[0].util->getCartesianPosition().getX();
00541         y1 = atoms[0].util->getCartesianPosition().getY();
00542         x2 = atoms[size*size-1].util->getCartesianPosition().getX();
00543         y2 = atoms[size*size-1].util->getCartesianPosition().getY();
00544         xg = x1 + (x2-x1)/2;
00545         yg = y1 + (y2-y1)/2;
00546         
00547         //le vecteur allant du centre du tapis au point final
00548         xf =  this->getDestination()->getX() - xg;
00549         yf =  this->getDestination()->getY() - yg;
00550         longueur = sqrt(xf*xf + yf*yf );
00551         xf/=longueur;
00552         yf/=longueur;
00553         
00554         angle = acos(xrepX*xf + yrepX*yf);
00555         if(asin(xrepY*xf + yrepY*yf)<0)
00556         {
00557                 angle = -angle;
00558         }
00559         std::cout<<" angle = "<<angle<<std::endl;
00560 }
00561 
00562 

Généré le Fri Mar 26 13:02:03 2004 pour AlgoAtomD par doxygen 1.3.5