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_roue.cpp

Aller à la documentation de ce fichier.
00001 
00002 #include <iostream>
00003 
00004 #include "pi2d14/pi2d14_molecule_roue.h"
00005 
00006 #include "Atom.h"
00007 #include "ConfigReader.h"
00008 #include "Math.h"
00009 
00010 ALGOATOMD::MoleculeRoue::MoleculeRoue(int buildParam, int typeDep)
00011 {
00012         size = 16;              //calcul de la taille demandee
00013                                         //pour le cube, size est le nombre d'atome sur le coté
00014         
00015         //construction de la molecule
00016         atoms = new API::Atom[size];
00017         atomsFixedWorld = new dJointID[size];
00018         
00019         double l1 = 2 * getAtomSize();
00020         double l2 = 2 * getLegSize() + 0.017;
00021         double l3 = getAtomSize();
00022 
00023         printf("Début création...  ");
00024         atoms[0].util->setCartesianPosition(new API::CartesianPosition(0, 0, l3));
00025         atoms[1].util->setCartesianPosition(new API::CartesianPosition(0, l1, l3));
00026         atoms[2].util->setCartesianPosition(new API::CartesianPosition(l1, 0, l3));
00027         atoms[3].util->setCartesianPosition(new API::CartesianPosition(l1, l1, l3));
00028 
00029         atoms[4].util->setCartesianPosition(new API::CartesianPosition(l1 + l2, 0, l3 + l2));
00030         atoms[5].util->setCartesianPosition(new API::CartesianPosition(l1 + l2, l1, l3 + l2));
00031         atoms[6].util->setCartesianPosition(new API::CartesianPosition(l1 + l2, 0, l3 + l2 + l1));
00032         atoms[7].util->setCartesianPosition(new API::CartesianPosition(l1 + l2, l1, l3 + l2 + l1));
00033 
00034 
00035         atoms[8].util->setCartesianPosition(new API::CartesianPosition(0, 0, l3 + l2 + l1 + l2));
00036         atoms[9].util->setCartesianPosition(new API::CartesianPosition(0, l1, l3 + l2 + l1 + l2));
00037         atoms[10].util->setCartesianPosition(new API::CartesianPosition(l1, 0, l3 + l2 + l1 + l2));
00038         atoms[11].util->setCartesianPosition(new API::CartesianPosition(l1, l1, l3 + l2 + l1 + l2));
00039 
00040         atoms[12].util->setCartesianPosition(new API::CartesianPosition(-l2, 0, l3 + l2 + l1));
00041         atoms[13].util->setCartesianPosition(new API::CartesianPosition(-l2, l1, l3 + l2 + l1));
00042         atoms[14].util->setCartesianPosition(new API::CartesianPosition(-l2, 0, l3 + l2));
00043         atoms[15].util->setCartesianPosition(new API::CartesianPosition(-l2, l1, l3 + l2));
00044         printf("Fin.\n");
00045         linkAllLegs();  // TODO faire un linkage plus otpimis�
00046         
00047         m1=false;
00048         m2=false;
00049         m3=false;
00050         init=true;
00051         indexInit=-1;
00052         index1=-1;
00053         index2=-1;
00054         index3=-1;
00055         
00056         typeDeplacement = typeDep;
00057         
00058         switch(typeDeplacement)
00059         {
00060         case 1:
00061                 initDep1();
00062                 break;
00063         default:
00064                 //pas de d�placement
00065                 break;
00066         }
00067 }
00068 
00069 ALGOATOMD::MoleculeRoue::~MoleculeRoue()
00070 {
00071         delete [] atoms;
00072 }
00073 
00074 void ALGOATOMD::MoleculeRoue::linkAllLegs()
00075 {
00076         //les pointeur sur les deux atom courant
00077         API::Atom *atom1;
00078         API::Atom *atom2;
00079 
00080         for(int a1=0;a1<size;a1++)
00081         {
00082                 for(int a2=0;a2<size;a2++)
00083                 {
00084                         //on v�rifie qu'on est pas 2 fois sur le meme atome
00085                         if(a1!=a2)
00086                         {
00087                                 atom1 = &atoms[a1];
00088                                 atom2 = &atoms[a2];
00089                                 for(int p1=0;p1<6;p1++)
00090                                 {
00091                                         if(!atom1->getLeg(p1)->isLinked())
00092                                         {
00093                                                 for(int p2=0;p2<6;p2++)
00094                                                 {
00095                                                         //on v�rifie qu'on est pas 2 fois sur les meme pattes=> annul� : id�e � conne qui sert a rien
00096                                                         //on v�rifie que les deux pattes ne sont pas d�j� connect�
00097                                                         if( !atom2->getLeg(p2)->isLinked() )
00098                                                         {
00099                                                                 if( atom2->getLeg(p2)->util->isConnectable(atom1->getLeg(p1)) )
00100                                                                 {
00101                                                                         atom2->getLeg(p2)->setLink(atom1->getLeg(p1));
00102                                                                 }
00103                                                         }
00104                                                 }
00105                                         }
00106                                 }
00107                         }
00108                 }
00109         }
00110 }
00111 
00112 void ALGOATOMD::MoleculeRoue::processMovement()
00113 {
00114         switch(typeDeplacement)
00115         {
00116         case 1:
00117                 algoDep1();
00118                 break;
00119         default:
00120                 //pas de d�placement
00121                 break;
00122         }
00123 }
00124 
00125 void ALGOATOMD::MoleculeRoue::renderMoleculeInfo()
00126 {
00127 }
00128 
00129 void ALGOATOMD::MoleculeRoue::initDep1()
00130 {
00131         indexInit = atoms[0].getMolecule()->getNextSyncIndex();
00132         
00133         accroche(true);
00134         
00135         atoms[0].getLeg(1)->getServoMotor(0)->setTargetPosition(0,KEEP,0,indexInit);
00136         atoms[1].getLeg(1)->getServoMotor(0)->setTargetPosition(0,KEEP,0,indexInit);
00137         atoms[2].getLeg(0)->getServoMotor(0)->setTargetPosition(255,KEEP,0,indexInit);
00138         atoms[3].getLeg(0)->getServoMotor(0)->setTargetPosition(255,KEEP,0,indexInit);
00139 
00140         atoms[4].getLeg(2)->getServoMotor(1)->setTargetPosition(0,KEEP,0,indexInit);
00141         atoms[5].getLeg(2)->getServoMotor(1)->setTargetPosition(0,KEEP,0,indexInit);
00142         atoms[6].getLeg(3)->getServoMotor(1)->setTargetPosition(255,KEEP,0,indexInit);
00143         atoms[7].getLeg(3)->getServoMotor(1)->setTargetPosition(255,KEEP,0,indexInit);
00144 
00145         atoms[8].getLeg(1)->getServoMotor(0)->setTargetPosition(255,KEEP,0,indexInit);
00146         atoms[9].getLeg(1)->getServoMotor(0)->setTargetPosition(255,KEEP,0,indexInit);
00147         atoms[10].getLeg(0)->getServoMotor(0)->setTargetPosition(0,KEEP,0,indexInit);
00148         atoms[11].getLeg(0)->getServoMotor(0)->setTargetPosition(0,KEEP,0,indexInit);
00149 
00150         atoms[12].getLeg(3)->getServoMotor(1)->setTargetPosition(0,KEEP,0,indexInit);
00151         atoms[13].getLeg(3)->getServoMotor(1)->setTargetPosition(0,KEEP,0,indexInit);
00152         atoms[14].getLeg(2)->getServoMotor(1)->setTargetPosition(255,KEEP,0,indexInit);
00153         atoms[15].getLeg(2)->getServoMotor(1)->setTargetPosition(255,KEEP,0,indexInit);
00154 }
00155 
00156 void ALGOATOMD::MoleculeRoue::algoDep1()
00157 {
00158         if(!m1)
00159         {
00160                 if(atoms[0].getMolecule()->isFinished(indexInit))
00161                 {
00162                         if(init)
00163                         {
00164                                 linkAllLegs();
00165                                 accroche(false);
00166                                 init = false;
00167                         }
00168                         else
00169                         {
00170                                 index1 = atoms[0].getMolecule()->getNextSyncIndex();
00171                         
00172                                 atoms[0].getLeg(2)->getServoMotor(1)->setTargetPosition(255,KEEP,0,index1);
00173                                 atoms[1].getLeg(2)->getServoMotor(1)->setTargetPosition(255,KEEP,0,index1);
00174                         
00175                                 m1 = true;
00176                         
00177                                 atoms[0].getMolecule()->releaseSyncIndex(indexInit);
00178                         }
00179                 }
00180         }
00181         if(m1 && !init)
00182         {
00183                 if(atoms[0].getMolecule()->isFinished(index1))
00184                 {
00185                         indexInit = atoms[0].getMolecule()->getNextSyncIndex();
00186                         
00187                         atoms[0].getLeg(2)->getServoMotor(1)->setToHalfPosition(KEEP,0,indexInit);
00188                         atoms[1].getLeg(2)->getServoMotor(1)->setToHalfPosition(KEEP,0,indexInit);
00189                         
00190                         m1 = false;
00191                 }
00192         }
00193 }
00194 
00195 
00196 bool ALGOATOMD::MoleculeRoue::needMoreMouvement()
00197 {
00198         bool ret = true;
00199         API::CartesianPosition destination = *this->getDestination();                   //position point final
00200         
00201         return ret;
00202 }
00203 
00204 void ALGOATOMD::MoleculeRoue::accroche(bool flag)
00205 {
00206         if(flag)
00207         {
00208                 for(int i = 0; i < size; i++)
00209                 {
00210                         atomsFixedWorld[i]= dJointCreateFixed (Simulator::getInstance ()->getWorld (), 0);
00211                         dJointAttach (atomsFixedWorld[i],0, atoms[i].util->getSimulatedObject()->getKernel()->getBody());
00212                         dJointSetFixed (atomsFixedWorld[i]);
00213                         printf("**Atoms[%i] ne subit plus l'effet de la gravite**\n",i);
00214                 }
00215         }
00216         else
00217         {
00218                 for(int i = 0; i < size; i++)
00219                 {
00220                         dJointDestroy(atomsFixedWorld[i]);
00221                         printf("**Atoms[%i] subit l'effet de la gravit�**\n",i);
00222                 }
00223         }
00224 }
00225 

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