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;
00013
00014
00015
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();
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
00065 break;
00066 }
00067 }
00068
00069 ALGOATOMD::MoleculeRoue::~MoleculeRoue()
00070 {
00071 delete [] atoms;
00072 }
00073
00074 void ALGOATOMD::MoleculeRoue::linkAllLegs()
00075 {
00076
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
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
00096
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
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();
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