00001
00002 #include <iostream>
00003
00004 #include "pi2d14/pi2d14_molecule_atome.h"
00005
00006 #include "Atom.h"
00007 #include "ConfigReader.h"
00008 #include "Math.h"
00009 #include <GL/gl.h>
00010 #include <iostream>
00011
00012
00013 ALGOATOMD::MoleculeAtome::MoleculeAtome(int buildParam, int typeDep)
00014 {
00015 size = 2 + buildParam;
00016
00017 atoms = new API::Atom();
00018 atoms->util->setCartesianPosition(new API::CartesianPosition(0, 0, atomSize));
00019
00020 m1=false;
00021 m2=false;
00022 m3=false;
00023 indexInit=-1;
00024 index1=-1;
00025 index2=-1;
00026 index3=-1;
00027
00028 marqueur = 0;
00029
00030 typeDeplacement = typeDep;
00031
00032
00033 switch(typeDeplacement)
00034 {
00035 case 1:
00036 initDep1();
00037 break;
00038 case 2:
00039 initDep2();
00040 break;
00041 case 3:
00042 initDep3();
00043 break;
00044 default:
00045
00046 break;
00047 }
00048 }
00049
00050 ALGOATOMD::MoleculeAtome::~MoleculeAtome()
00051 {
00052 delete atoms;
00053 }
00054
00055
00056 void ALGOATOMD::MoleculeAtome::processMovement()
00057 {
00058 switch(typeDeplacement)
00059 {
00060 case 1:
00061 algoDep1();
00062 break;
00063 case 2:
00064 algoDep2();
00065 break;
00066 case 3:
00067 algoDep3();
00068 break;
00069 default:
00070
00071 break;
00072 }
00073 }
00074
00075 void ALGOATOMD::MoleculeAtome::renderMoleculeInfo()
00076 {
00077 switch(typeDeplacement)
00078 {
00079 case 1:
00080 break;
00081 case 2:
00082 debugAlgo2();
00083 break;
00084 case 3:
00085 break;
00086 default:
00087
00088 break;
00089 }
00090 }
00091
00092
00093
00094
00095
00096
00097 void ALGOATOMD::MoleculeAtome::initDep1()
00098 {
00099
00100 indexInit = atoms->getMolecule()->getNextSyncIndex();
00101
00102 atoms->getLeg(2)->getServoMotor(0)->setTargetPosition(1,REPLACE);
00103 atoms->getLeg(2)->getServoMotor(1)->setTargetPosition(1,REPLACE);
00104 atoms->getLeg(2)->getServoMotor(0)->setToHalfPosition(KEEP,0,indexInit);
00105 atoms->getLeg(2)->getServoMotor(1)->setToHalfPosition(KEEP,0,indexInit);
00106 }
00107
00108 void ALGOATOMD::MoleculeAtome::algoDep1()
00109 {
00110
00111 if(!m1 && !m2 && !m3)
00112 {
00113
00114 if(atoms->getMolecule()->isFinished(indexInit))
00115 {
00116 printf("11\n");
00117
00118 index1 = atoms->getMolecule()->getNextSyncIndex();
00119
00120
00121 atoms->getLeg(0)->getServoMotor(0)->setTargetPosition(249,KEEP,0,index1);
00122 atoms->getLeg(0)->getServoMotor(1)->setTargetPosition(1,KEEP,0,index1);
00123
00124 m1=true;
00125 m2=false;
00126 m3=false;
00127
00128
00129 atoms->getMolecule()->releaseSyncIndex(indexInit);
00130 }
00131 }
00132
00133
00134 if(m1 && !m2 && !m3)
00135 {
00136 if(atoms->getMolecule()->isFinished(index1))
00137 {
00138 printf("22\n");
00139 index2 = atoms->getMolecule()->getNextSyncIndex();
00140
00141 atoms->getLeg(2)->getServoMotor(0)->setTargetPosition(1,KEEP,0,index2);
00142 atoms->getLeg(2)->getServoMotor(1)->setTargetPosition(249,KEEP,0,index2);
00143
00144 m1=false;
00145 m2=true;
00146 m3=false;
00147 atoms->getMolecule()->releaseSyncIndex(index1);
00148 }
00149 }
00150
00151
00152 if(!m1 && m2 && !m3)
00153 {
00154 if(atoms->getMolecule()->isFinished(index2))
00155 {
00156 printf("33\n");
00157 index3 = atoms->getMolecule()->getNextSyncIndex();
00158
00159 atoms->getLeg(5)->getServoMotor(0)->setTargetPosition(249,KEEP,0,index3);
00160 atoms->getLeg(5)->getServoMotor(1)->setTargetPosition(1,KEEP,0,index3);
00161
00162 m1=false;
00163 m2=false;
00164 m3=true;
00165 atoms->getMolecule()->releaseSyncIndex(index2);
00166 }
00167 }
00168
00169
00170 if(!m1 && !m2 && m3)
00171 {
00172 if(atoms->getMolecule()->isFinished(index3))
00173 {
00174 printf("ii\n");
00175 indexInit = atoms->getMolecule()->getNextSyncIndex();
00176
00177 atoms->getLeg(2)->getServoMotor(0)->setToHalfPosition (KEEP,0,indexInit);
00178 atoms->getLeg(2)->getServoMotor(1)->setToHalfPosition (KEEP,0,indexInit);
00179 atoms->getLeg(5)->getServoMotor(0)->setToHalfPosition (KEEP,0,indexInit);
00180 atoms->getLeg(5)->getServoMotor(1)->setToHalfPosition (KEEP,0,indexInit);
00181 atoms->getLeg(0)->getServoMotor(0)->setToHalfPosition (KEEP,0,indexInit);
00182 atoms->getLeg(0)->getServoMotor(1)->setToHalfPosition (KEEP,0,indexInit);
00183
00184 m1=false;
00185 m2=false;
00186 m3=false;
00187 atoms->getMolecule()->releaseSyncIndex(index3);
00188 }
00189 }
00190 }
00191
00192
00193 void ALGOATOMD::MoleculeAtome::initDep2()
00194 {
00195 indexInit = atoms->getMolecule()->getNextSyncIndex();
00196
00197 atoms->getLeg(2)->getServoMotor(0)->setTargetPosition(1,REPLACE);
00198 atoms->getLeg(2)->getServoMotor(1)->setTargetPosition(1,REPLACE);
00199 atoms->getLeg(2)->getServoMotor(0)->setToHalfPosition (KEEP,0,indexInit);
00200 atoms->getLeg(2)->getServoMotor(1)->setToHalfPosition (KEEP,0,indexInit);
00201 }
00202
00203 void ALGOATOMD::MoleculeAtome::algoDep2()
00204 {
00205
00206 if(!m1 && !m2 && !m3 && needMoreMouvement())
00207 {
00208 if(atoms->getMolecule()->isFinished(indexInit))
00209 {
00210 printf("11\n");
00211 calcZone();
00212
00213 index1 = atoms->getMolecule()->getNextSyncIndex();
00214
00215 switch(currentZone)
00216 {
00217 case 1:
00218 atoms->getLeg(2)->getServoMotor(0)->setTargetPosition(1,KEEP,0,index1);
00219 atoms->getLeg(2)->getServoMotor(1)->setTargetPosition(249,KEEP,0,index1);
00220 break;
00221 case 2:
00222 atoms->getLeg(5)->getServoMotor(0)->setTargetPosition(1,KEEP,0,index1);
00223 atoms->getLeg(5)->getServoMotor(1)->setTargetPosition(249,KEEP,0,index1);
00224 break;
00225 case 3:
00226 atoms->getLeg(0)->getServoMotor(0)->setTargetPosition(249,KEEP,0,index1);
00227 atoms->getLeg(0)->getServoMotor(1)->setTargetPosition(249,KEEP,0,index1);
00228 break;
00229 }
00230
00231
00232 m1=true;
00233 m2=false;
00234 m3=false;
00235 atoms->getMolecule()->releaseSyncIndex(indexInit);
00236 }
00237 }
00238
00239
00240 if(m1 && !m2 && !m3)
00241 {
00242 if(atoms->getMolecule()->isFinished(index1))
00243 {
00244 printf("22\n");
00245 index2 = atoms->getMolecule()->getNextSyncIndex();
00246
00247 switch(currentZone)
00248 {
00249 case 1:
00250 atoms->getLeg(0)->getServoMotor(0)->setTargetPosition(1,KEEP,0,index2);
00251 atoms->getLeg(0)->getServoMotor(1)->setTargetPosition(1,KEEP,0,index2);
00252 break;
00253 case 2:
00254 atoms->getLeg(2)->getServoMotor(0)->setTargetPosition(249,KEEP,0,index2);
00255 atoms->getLeg(2)->getServoMotor(1)->setTargetPosition(1,KEEP,0,index2);
00256 break;
00257 case 3:
00258 atoms->getLeg(5)->getServoMotor(0)->setTargetPosition(249,KEEP,0,index2);
00259 atoms->getLeg(5)->getServoMotor(1)->setTargetPosition(1,KEEP,0,index2);
00260 break;
00261 }
00262
00263 m1=false;
00264 m2=true;
00265 m3=false;
00266 atoms->getMolecule()->releaseSyncIndex(index1);
00267 }
00268 }
00269
00270
00271 if(!m1 && m2 && !m3)
00272 {
00273 if(atoms->getMolecule()->isFinished(index2))
00274 {
00275 printf("33\n");
00276 index3 = atoms->getMolecule()->getNextSyncIndex();
00277
00278 switch(currentZone)
00279 {
00280 case 1:
00281 atoms->getLeg(5)->getServoMotor(1)->setTargetPosition(1,KEEP,0,index3);
00282 atoms->getLeg(5)->getServoMotor(0)->setTargetPosition(1,KEEP,0,index3);
00283 break;
00284 case 2:
00285 atoms->getLeg(0)->getServoMotor(0)->setTargetPosition(1,KEEP,0,index3);
00286 atoms->getLeg(0)->getServoMotor(1)->setTargetPosition(249,KEEP,0,index3);
00287 break;
00288 case 3:
00289 atoms->getLeg(2)->getServoMotor(0)->setTargetPosition(249,KEEP,0,index3);
00290 atoms->getLeg(2)->getServoMotor(1)->setTargetPosition(249,KEEP,0,index3);
00291 break;
00292 }
00293
00294
00295 m1=false;
00296 m2=false;
00297 m3=true;
00298
00299 atoms->getMolecule()->releaseSyncIndex(index2);
00300 }
00301 }
00302
00303
00304 if(!m1 && !m2 && m3)
00305 {
00306 if(atoms->getMolecule()->isFinished(index3))
00307 {
00308 printf("ii\n");
00309 indexInit = atoms->getMolecule()->getNextSyncIndex();
00310 atoms->getLeg(2)->getServoMotor(0)->setToHalfPosition (KEEP,0,indexInit);
00311 atoms->getLeg(2)->getServoMotor(1)->setToHalfPosition (KEEP,0,indexInit);
00312 atoms->getLeg(5)->getServoMotor(0)->setToHalfPosition (KEEP,0,indexInit);
00313 atoms->getLeg(5)->getServoMotor(1)->setToHalfPosition (KEEP,0,indexInit);
00314 atoms->getLeg(0)->getServoMotor(0)->setToHalfPosition (KEEP,0,indexInit);
00315 atoms->getLeg(0)->getServoMotor(1)->setToHalfPosition (KEEP,0,indexInit);
00316
00317 m1=false;
00318 m2=false;
00319 m3=false;
00320 atoms->getMolecule()->releaseSyncIndex(index3);
00321 }
00322 }
00323 }
00324
00325 void ALGOATOMD::MoleculeAtome::calcZone()
00326 {
00327 double longueur, angle, angleBis;
00328
00329 axe0 = atoms->getLeg(0)->util->getSimulatedObject()->getAbsPosEnd();
00330 axe2 = atoms->getLeg(2)->util->getSimulatedObject()->getAbsPosEnd();
00331 axe5 = atoms->getLeg(5)->util->getSimulatedObject()->getAbsPosEnd();
00332
00333 API::CartesianPosition aPosition = atoms->util->getCartesianPosition();
00334 API::CartesianPosition destination = *this->getDestination();
00335 API::CartesianPosition direction(aPosition.getX() - destination.getX(), aPosition.getY() - destination.getY(), 0);
00336
00337 longueur = sqrt(direction.getX()*direction.getX() + direction.getY()*direction.getY() );
00338 direction.setX( direction.getX()/longueur );
00339 direction.setY( direction.getY()/longueur );
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356 angle= acos(direction.getX()*axe0.getX() + direction.getY()*axe0.getY());
00357 currentZone = 2;
00358
00359
00360 angleBis = acos(direction.getX()*axe2.getX() + direction.getY()*axe2.getY());
00361 if(angle>angleBis)
00362 {
00363 angle = angleBis;
00364 currentZone = 3;
00365 }
00366
00367
00368 angleBis = acos(direction.getX()*axe5.getX() + direction.getY()*axe5.getY());
00369 if(angle>angleBis)
00370 {
00371 currentZone = 1;
00372 }
00373
00374 std::cout<<"la zone selectionner est : "<<currentZone<<std::endl;
00375 }
00376
00377 void ALGOATOMD::MoleculeAtome::debugAlgo2()
00378 {
00379
00380 API::CartesianPosition p = atoms->util->getCartesianPosition();
00381 glDisable(GL_TEXTURE_2D);
00382 glDisable(GL_LIGHTING);
00383 glLineWidth(8);
00384 glBegin(GL_LINES);
00385 glColor3f(0.3,0.3,0.3);
00386 glVertex3f(p.getX(), p.getY(), p.getZ());
00387 glVertex3f(p.getX()+ this->getDestination()->getX() , p.getX()+ this->getDestination()->getY() , p.getZ());
00388 glColor3f(1,0.3,0.3);
00389 glVertex3f(p.getX(), p.getY(), p.getZ());
00390 glVertex3f(p.getX()+ 10*axe0.getX() , p.getX()+ 10*axe0.getY() , p.getZ());
00391 glColor3f(0.3,0.3,1);
00392 glVertex3f(p.getX(), p.getY(), p.getZ());
00393 glVertex3f(p.getX()+ 10*axe2.getX() , p.getX()+ 10*axe2.getY() , p.getZ());
00394 glColor3f(1,0.3,1);
00395 glVertex3f(p.getX(), p.getY(), p.getZ());
00396 glVertex3f(p.getX()+ 10*axe5.getX() , p.getX()+ 10*axe5.getY() , p.getZ());
00397 glEnd();
00398 glEnable(GL_TEXTURE_2D);
00399 glEnable(GL_LIGHTING);
00400 glLineWidth(1);
00401 }
00402
00403 void ALGOATOMD::MoleculeAtome::initDep3()
00404 {
00405 indexInit = atoms->getMolecule()->getNextSyncIndex();
00406
00407 atoms->getLeg(2)->getServoMotor(0)->setTargetPosition(1,REPLACE);
00408 atoms->getLeg(2)->getServoMotor(1)->setTargetPosition(1,REPLACE);
00409 atoms->getLeg(2)->getServoMotor(0)->setToHalfPosition (KEEP,0,indexInit);
00410 atoms->getLeg(2)->getServoMotor(1)->setToHalfPosition (KEEP,0,indexInit);
00411 }
00412
00413 void ALGOATOMD::MoleculeAtome::algoDep3()
00414 {
00415
00416 if(!m1 && !m2)
00417 {
00418
00419 if(atoms->getMolecule()->isFinished(indexInit))
00420 {
00421
00422 printf("11\n");
00423
00424 index1 = atoms->getMolecule()->getNextSyncIndex();
00425
00426 switch(marqueur)
00427 {
00428 case 0:
00429 {
00430 atoms->getLeg(0)->getServoMotor(0)->setTargetPosition(0,KEEP,1,index1);
00431 atoms->getLeg(0)->getServoMotor(1)->setTargetPosition(250,KEEP,1,index1);
00432 }
00433 break;
00434 case 1:
00435 {
00436 atoms->getLeg(2)->getServoMotor(0)->setTargetPosition(250,KEEP,1,index1);
00437 atoms->getLeg(2)->getServoMotor(1)->setTargetPosition(0,KEEP,1,index1);
00438 }
00439 break;
00440 case 2:
00441 {
00442 atoms->getLeg(5)->getServoMotor(0)->setTargetPosition(250,KEEP,1,index1);
00443 atoms->getLeg(5)->getServoMotor(1)->setTargetPosition(250,KEEP,1,index1);
00444 }
00445 break;
00446 case 3:
00447 {
00448 atoms->getLeg(1)->getServoMotor(0)->setTargetPosition(0,KEEP,1,index1);
00449 atoms->getLeg(1)->getServoMotor(1)->setTargetPosition(250,KEEP,1,index1);
00450 }
00451 break;
00452 case 4:
00453 {
00454 atoms->getLeg(3)->getServoMotor(0)->setTargetPosition(250,KEEP,1,index1);
00455 atoms->getLeg(3)->getServoMotor(1)->setTargetPosition(0,KEEP,1,index1);
00456 }
00457 break;
00458 case 5:
00459 {
00460 atoms->getLeg(4)->getServoMotor(0)->setTargetPosition(250,KEEP,1,index1);
00461 atoms->getLeg(4)->getServoMotor(1)->setTargetPosition(250,KEEP,1,index1);
00462 }
00463 }
00464
00465
00466 m1=true;
00467 m2=false;
00468
00469
00470
00471 atoms->getMolecule()->releaseSyncIndex(indexInit);
00472 marqueur++;
00473 }
00474 }
00475
00476 if(m1 && !m2)
00477 {
00478 if(atoms->getMolecule()->isFinished(index1))
00479 {
00480 printf("22\n");
00481 index2 = atoms->getMolecule()->getNextSyncIndex();
00482
00483 switch(marqueur)
00484 {
00485 case 1:
00486 {
00487
00488 atoms->getLeg(5)->getServoMotor(0)->setTargetPosition(250,KEEP,0,index2);
00489 atoms->getLeg(2)->getServoMotor(0)->setTargetPosition(0,KEEP,0,index2);
00490 }
00491 break;
00492
00493 case 2:
00494 {
00495
00496 atoms->getLeg(1)->getServoMotor(0)->setTargetPosition(250,KEEP,0,index2);
00497 atoms->getLeg(5)->getServoMotor(0)->setTargetPosition(0,KEEP,0,index2);
00498 }
00499 break;
00500
00501 case 3:
00502 {
00503
00504 atoms->getLeg(1)->getServoMotor(0)->setTargetPosition(250,KEEP,0,index2);
00505 atoms->getLeg(3)->getServoMotor(0)->setTargetPosition(0,KEEP,0,index2);
00506 }
00507 break;
00508
00509 case 4:
00510 {
00511
00512 atoms->getLeg(4)->getServoMotor(0)->setTargetPosition(250,KEEP,0,index2);
00513 atoms->getLeg(3)->getServoMotor(0)->setTargetPosition(0,KEEP,0,index2);
00514 }
00515 break;
00516
00517 case 5:
00518 {
00519
00520 atoms->getLeg(0)->getServoMotor(0)->setTargetPosition(250,KEEP,0,index2);
00521 atoms->getLeg(4)->getServoMotor(0)->setTargetPosition(0,KEEP,0,index2);
00522 }
00523 break;
00524
00525 case 6:
00526 {
00527
00528 atoms->getLeg(0)->getServoMotor(0)->setTargetPosition(250,KEEP,0,index2);
00529 atoms->getLeg(2)->getServoMotor(0)->setTargetPosition(0,KEEP,0,index2);
00530 marqueur=0;
00531 }
00532 }
00533 m1=false;
00534 m2=true;
00535 atoms->getMolecule()->releaseSyncIndex(index1);
00536 }
00537 }
00538
00539 if(!m1 && m2)
00540 {
00541 if(atoms->getMolecule()->isFinished(index2))
00542 {
00543 printf("ii\n");
00544 indexInit = atoms->getMolecule()->getNextSyncIndex();
00545
00546
00547
00548 atoms->getLeg(2)->getServoMotor(0)->setToHalfPosition (KEEP,0,indexInit);
00549 atoms->getLeg(2)->getServoMotor(1)->setToHalfPosition (KEEP,0,indexInit);
00550 atoms->getLeg(5)->getServoMotor(0)->setToHalfPosition (KEEP,0,indexInit);
00551 atoms->getLeg(5)->getServoMotor(1)->setToHalfPosition (KEEP,0,indexInit);
00552 atoms->getLeg(1)->getServoMotor(0)->setToHalfPosition (KEEP,0,indexInit);
00553 atoms->getLeg(1)->getServoMotor(1)->setToHalfPosition (KEEP,0,indexInit);
00554 atoms->getLeg(0)->getServoMotor(0)->setToHalfPosition (KEEP,0,indexInit);
00555 atoms->getLeg(0)->getServoMotor(1)->setToHalfPosition (KEEP,0,indexInit);
00556 atoms->getLeg(3)->getServoMotor(0)->setToHalfPosition (KEEP,0,indexInit);
00557 atoms->getLeg(3)->getServoMotor(1)->setToHalfPosition (KEEP,0,indexInit);
00558 atoms->getLeg(4)->getServoMotor(0)->setToHalfPosition (KEEP,0,indexInit);
00559 atoms->getLeg(4)->getServoMotor(1)->setToHalfPosition (KEEP,0,indexInit);
00560
00561 m1=false;
00562 m2=false;
00563 atoms->getMolecule()->releaseSyncIndex(index2);
00564 }
00565 }
00566 }
00567
00568
00569 bool ALGOATOMD::MoleculeAtome::needMoreMouvement()
00570 {
00571 bool ret = false;
00572 API::CartesianPosition destination = *this->getDestination();
00573 API::CartesianPosition aPosition = atoms->util->getCartesianPosition();
00574 API::CartesianPosition v(destination.getX()-aPosition.getX(),destination.getY()-aPosition.getY(),destination.getZ()-aPosition.getZ() );
00575 double longueur = sqrt(v.getX()*v.getX() + v.getY()*v.getY() + v.getZ()*v.getZ() );
00576
00577 if(longueur>atomSize)
00578 {
00579 ret = true;
00580 }
00581
00582 return ret;
00583 }
00584