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;
00015
00016
00017
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();
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
00050 break;
00051 }
00052 }
00053
00054 ALGOATOMD::MoleculeTapis::~MoleculeTapis()
00055 {
00056 delete [] atoms;
00057 }
00058
00059 void ALGOATOMD::MoleculeTapis::linkAllLegs()
00060 {
00061
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
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
00081
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
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
00124 break;
00125 }
00126 }
00127
00128
00129
00130
00131
00132
00133 void ALGOATOMD::MoleculeTapis::initDep1()
00134 {
00135 }
00136
00137 void ALGOATOMD::MoleculeTapis::algoDep1()
00138 {
00139 if(!m1 && !m2&& !m3)
00140 {
00141
00142 if(atoms[0].getMolecule()->isFinished(indexInit))
00143 {
00144 printf("11\n");
00145
00146 index1 = atoms[0].getMolecule()->getNextSyncIndex();
00147
00148
00149
00150 for(int x=1;x<size;x+=2)
00151 {
00152
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
00161 for(int x=0;x<size;x+=2)
00162 {
00163
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;
00173 m2=false;
00174 m3=false;
00175
00176
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
00190 for(int x=1;x<size;x+=2)
00191 {
00192
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
00201 for(int x=0;x<size;x+=2)
00202 {
00203
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
00229 for(int x=1;x<size;x+=2)
00230 {
00231
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
00240 for(int x=0;x<size;x+=2)
00241 {
00242
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
00268 for(int x=1;x<size;x+=2)
00269 {
00270
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
00279 for(int x=0;x<size;x+=2)
00280 {
00281
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
00306 if(!m1 && !m2&& !m3)
00307 {
00308
00309 if(atoms[0].getMolecule()->isFinished(indexInit))
00310 {
00311 calcAngle();
00312 printf("11\n");
00313
00314 index1 = atoms[0].getMolecule()->getNextSyncIndex();
00315
00316
00317
00318 for(int x=1;x<size;x+=2)
00319 {
00320
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
00330 for(int x=0;x<size;x+=2)
00331 {
00332
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;
00343 m2=false;
00344 m3=false;
00345
00346
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
00360 for(int x=1;x<size;x+=2)
00361 {
00362
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
00372 for(int x=0;x<size;x+=2)
00373 {
00374
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
00401 for(int x=1;x<size;x+=2)
00402 {
00403
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
00413 for(int x=0;x<size;x+=2)
00414 {
00415
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
00443 for(int x=1;x<size;x+=2)
00444 {
00445
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
00455 for(int x=0;x<size;x+=2)
00456 {
00457
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();
00480
00481
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;
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
00529 xrepX=x2-x1;
00530 yrepX=y2-y1;
00531 longueur = sqrt(xrepX*xrepX + yrepX*yrepX );
00532 xrepX/=longueur;
00533 yrepX/=longueur;
00534
00535
00536 xrepY=-yrepX;
00537 yrepY= xrepX;
00538
00539
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
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