00001
00002
00003
00004 #include "REnemySpecialist.h"
00005 #include <algorithm>
00006 #include <stdlib.h>
00007 #include <time.h>
00008 #include <list>
00009 #include <bitset>
00010 #include "Tools/RingBuffer.h"
00011 using namespace std;
00012
00013 typedef list<list<RasterSpecialist::LinePair> > SegmentList;
00014
00015
00016
00017
00018
00019 REnemySpecialist::REnemySpecialist(RasterImageProcessor &processor,RasterStrategy &strat):
00020 RasterSpecialist(processor),lines(200),columns(30)
00021 {
00022 strategy = &strat;
00023 preScanNeeded = true;
00024 postScanNeeded = true;
00025 }
00026
00027 REnemySpecialist::~REnemySpecialist()
00028 {
00029 }
00030
00031 void REnemySpecialist::executePostProcessing()
00032 {
00033 if (lines.size()==0)
00034 return;
00035
00036
00037
00038 vector<list<LinePair> > v;
00039 list<LinePair> linesList;
00040
00041 vector<LinePair>::iterator ite = lines.begin();
00042 while(ite != lines.end()){
00043 linesList.push_back(*ite);
00044 ite++;
00045 }
00046
00047
00048 createSegmentsFromLines2(linesList,v,-2,6);
00049
00050
00051 for (unsigned s = 0; s < v.size();s++) {
00052
00053
00054 int counterRed=0;
00055 int counterBlue=0;
00056 int counterGray=0;
00057 int counterOrange=0;
00058
00059 list <LinePair>::iterator it = v[s].begin();
00060
00061
00062
00063 while(it != v[s].end())
00064 {
00065 LinePair b = *it;
00066 for (unsigned i=1;i<10;i++) {
00067 if (getColor(b.v1.x+((b.v2.x-b.v1.x)*i)/10,b.v1.y)==red)
00068 {
00069
00070 counterRed++;
00071 }
00072 else
00073 if (getColor(b.v1.x+((b.v2.x-b.v1.x)*i)/10,b.v1.y)==gray)
00074 {
00075 counterGray++;
00076 }
00077 else
00078 if (getColor(b.v1.x+((b.v2.x-b.v1.x)*i)/10,b.v1.y)==blue)
00079 {
00080
00081 counterBlue++;
00082 }
00083 else
00084 if (getColor(b.v1.x+((b.v2.x-b.v1.x)*i)/10,b.v1.y)==orange)
00085 {
00086 counterOrange++;
00087 }
00088 }
00089 it++;
00090 }
00091
00092 if (((counterOrange<counterBlue)||(counterOrange<counterRed))&&((counterRed>=2)||(counterBlue>=2))&&(v[s].size()>3))
00093 {
00094
00095 Vector2<int> pointOnField=calculateFarestPointFastCOG(v[s]);
00096
00097
00098 Vector2<int> np;
00099 np.x=0;
00100 np.y=0;
00101 SinglePlayerPercept percept;
00102 percept.offset.x=pointOnField.x;
00103 percept.offset.y=pointOnField.y;
00104 double enemyValidity;
00105 double enemyValidity2;
00106 if (counterRed>counterBlue)
00107 {
00108 enemyValidity=(double)(counterRed+counterGray)/((v[s].size()*9)+counterBlue);
00109 if ((Geometry::distance(np,pointOnField)>2000)&&(Geometry::distance(np,pointOnField)<5500))
00110 {
00111 enemyValidity2=(double)((-1)*(Geometry::distance(np,pointOnField)-2000)/5500+0.64);
00112 enemyValidity=(double)enemyValidity*enemyValidity2*enemyValidity2;
00113
00114 }
00115 else
00116 if (Geometry::distance(np,pointOnField)<=2000)
00117 enemyValidity=(double)(enemyValidity*((-1)*(Geometry::distance(np,pointOnField))/8000+1));
00118 else
00119 if (Geometry::distance(np,pointOnField)>=5500)
00120 enemyValidity=(double)(enemyValidity/100);
00121
00122
00123 percept.validity=enemyValidity;
00124
00125
00126
00127
00128
00129
00130
00131 rip->playersPercept.addRedPlayer(percept);
00132
00133 }
00134 else
00135 if (counterBlue>counterRed)
00136 {
00137 enemyValidity=(double)(counterBlue+counterGray)/((v[s].size()*9)+counterRed);
00138 if ((Geometry::distance(np,pointOnField)>2000)&&(Geometry::distance(np,pointOnField)<5500))
00139 {
00140 enemyValidity2=(double)((-1)*(Geometry::distance(np,pointOnField)-2000)/5500+0.64);
00141 enemyValidity=(double)enemyValidity*enemyValidity2*enemyValidity2;
00142
00143 }
00144 else
00145 if (Geometry::distance(np,pointOnField)<=2000)
00146 enemyValidity=(double)(enemyValidity*((-1)*(Geometry::distance(np,pointOnField))/8000+1));
00147 else
00148 if (Geometry::distance(np,pointOnField)>=5500)
00149 enemyValidity=(double)(enemyValidity/100);
00150
00151 percept.validity=enemyValidity;
00152
00153
00154
00155
00156
00157
00158
00159 rip->playersPercept.addBluePlayer(percept);
00160 }
00161 else
00162 if (counterBlue==counterRed)
00163 {
00164
00165 }
00166 }
00167 }
00168
00169 }
00170
00171
00172 void REnemySpecialist::invokeOnPreScan(int x,int y){
00173 if (!strategy->insidePlayer) tempP = Vector2<int>(x,y);
00174 else{
00175 LinePair lp(tempP,Vector2<int>(x,y));
00176
00177 if ((lp.v2.x - lp.v1.x) < 5)
00178 return;
00179
00180 lines.push_back(lp);
00181 LINE(imageProcessor_obstacles,
00182 lp.v1.x,lp.v1.y, lp.v2.x,lp.v2.y,
00183 0.5, Drawings::ps_solid, Drawings::white);
00184 DOT(imageProcessor_obstacles, lp.v2.x, lp.v2.y,
00185 Drawings::pink, Drawings::green);
00186 }
00187 }
00188
00189
00190 void REnemySpecialist::invokeOnPostScan(int x,int y)
00191 {
00192 if (!strategy->insidePlayer) tempP = Vector2<int>(x,y);
00193 else{
00194 LinePair lp(tempP,Vector2<int>(x,y));
00195 lines.push_back(lp);
00196
00197 LINE(imageProcessor_obstacles,
00198 lp.v1.x,lp.v1.y, lp.v2.x,lp.v2.y,
00199 0.5, Drawings::ps_solid, Drawings::white);
00200 DOT(imageProcessor_obstacles, lp.v2.x, lp.v2.y,
00201 Drawings::pink, Drawings::green);
00202 }
00203 }
00204
00205 int REnemySpecialist::getType()
00206 {
00207 return __REnemySpecialist;
00208 }
00209
00210 Vector2<int> REnemySpecialist::calculateFarestPoint(std::list<LinePair> enemyLines)
00211 {
00212 Vector2<int> pointOnField;
00213 horizon = rip->getHorizon();
00214 Vector2<double> iPoint, maxPoint;
00215 list <LinePair>::iterator itmax = enemyLines.begin();
00216 list <LinePair>::iterator it1 = enemyLines.begin();
00217 LinePair b=*it1;
00218 LinePair bmax=*itmax;
00219 if (horizon.direction.y>0)
00220 {
00221
00222 while(it1 != enemyLines.end())
00223 {
00224 b=*it1;
00225 bmax=*itmax;
00226 iPoint.x=b.v1.x;
00227 iPoint.y=b.v1.y;
00228 maxPoint.x=bmax.v1.x;
00229 maxPoint.y=bmax.v1.y;
00230 if (Geometry::getDistanceToLine(horizon,iPoint)<Geometry::getDistanceToLine(horizon, maxPoint))
00231 {
00232 itmax=it1;
00233 bmax=b;
00234 }
00235 it1++;
00236 }
00237 DOT(imageProcessor_obstacles, bmax.v1.x,bmax.v1.y,Drawings::ps_solid, Drawings::yellow);
00238 Geometry::calculatePointOnField(bmax.v1.x,bmax.v1.y,(rip->cameraMatrix),rip->image.cameraInfo,pointOnField);
00239 }
00240 else
00241 {
00242
00243 while(it1 != enemyLines.end())
00244 {
00245 b=*it1;
00246 bmax=*itmax;
00247 iPoint.x=b.v2.x;
00248 iPoint.y=b.v2.y;
00249 maxPoint.x=bmax.v2.x;
00250 maxPoint.y=bmax.v2.y;
00251 if (Geometry::getDistanceToLine(horizon,iPoint)<Geometry::getDistanceToLine(horizon, maxPoint))
00252 {
00253 itmax=it1;
00254 bmax=b;
00255 }
00256 it1++;
00257 }
00258 DOT(imageProcessor_obstacles, bmax.v2.x,bmax.v2.y,Drawings::ps_solid, Drawings::yellow);
00259 Geometry::calculatePointOnField(bmax.v2.x,bmax.v2.y,(rip->cameraMatrix),rip->image.cameraInfo,pointOnField);
00260 }
00261
00262
00263 return (pointOnField);
00264 }
00265
00266 void REnemySpecialist::sort(int footPointDistance[], Vector2<double> footPoint[], const int& start, const int& end)
00267 {
00268 if (start == end)
00269 {
00270 return;
00271 }
00272 int pivot = footPointDistance[(start+end)/2];
00273 int left = start;
00274 int right = end;
00275 do
00276 {
00277 while (footPointDistance[left] < pivot)
00278 {
00279 left++;
00280 }
00281 while (footPointDistance[right] > pivot)
00282 {
00283 right--;
00284 }
00285 if (left < right)
00286 {
00287 int tmp = footPointDistance[left];
00288 Vector2<double> tmp2 = footPoint[left];
00289 footPointDistance[left] = footPointDistance[right];
00290 footPoint[left] = footPoint[right];
00291 footPointDistance[right] = tmp;
00292 footPoint[right] = tmp2;
00293 }
00294 if (left <= right)
00295 {
00296 left++;
00297 right--;
00298 }
00299 } while (left <= right);
00300 if (start < right)
00301 {
00302 sort(footPointDistance, footPoint, start, right);
00303 }
00304 if (end > left)
00305 {
00306 sort(footPointDistance, footPoint, left, end);
00307 }
00308 }
00309
00310
00311 Vector2<int> REnemySpecialist::calculateFarestPointFastCOG(std::list<LinePair> enemyLines)
00312 {
00313 int numberOfFootPoints=2;
00314 int footPointDistance [500];
00315 Vector2<double> footPoint[500];
00316 Vector2<int> pointsOnField[2];
00317 Vector2<int> pointOnField;
00318 horizon = rip->getHorizon();
00319 Vector2<double> point1,point2,maxPoint;
00320 list <LinePair>::iterator itmax = enemyLines.begin();
00321 list <LinePair>::iterator it1 = enemyLines.begin();
00322 LinePair b=*it1;
00323 LinePair bmax=*itmax;
00324 int i=0;
00325
00326 while(it1 != enemyLines.end())
00327 {
00328 b=*it1;
00329 point1.x=b.v1.x;
00330 point1.y=b.v1.y;
00331 point2.x=b.v2.x;
00332 point2.y=b.v2.y;
00333 footPoint[i]=point1;
00334 footPointDistance[i]=(int)Geometry::getDistanceToLine(horizon,footPoint[i]);
00335 i++;
00336 footPoint[i]=point2;
00337 footPointDistance[i]=(int)Geometry::getDistanceToLine(horizon,footPoint[i]);
00338 i++;
00339 it1++;
00340 }
00341
00342 for (unsigned l = 0; l < (unsigned)numberOfFootPoints;l++)
00343 {
00344 for (unsigned m = l+1; m < (unsigned)(enemyLines.size()*2-1);m++)
00345 {
00346 if (footPointDistance[m]<footPointDistance[l])
00347 {
00348
00349 int tmp = footPointDistance[m];
00350 Vector2<double> tmp2 = footPoint[m];
00351 footPointDistance[m] = footPointDistance[l];
00352 footPoint[m] = footPoint[l];
00353 footPointDistance[l] = tmp;
00354 footPoint[l] = tmp2;
00355 }
00356 }
00357 }
00358
00359 for (unsigned g = 0; g < (unsigned)numberOfFootPoints;g++)
00360 {
00361 DOT(imageProcessor_obstacles, footPoint[g].x,footPoint[g].y,Drawings::ps_solid, Drawings::yellow);
00362 Geometry::calculatePointOnField((int)footPoint[g].x,(int)footPoint[g].y,(rip->cameraMatrix),rip->image.cameraInfo,pointOnField);
00363 pointsOnField[g]=pointOnField;
00364 }
00365
00366
00367 pointOnField.x=0;
00368 pointOnField.y=0;
00369 for (unsigned f = 0; f < (unsigned)(numberOfFootPoints);f++)
00370 {
00371 pointOnField.x=pointOnField.x+pointsOnField[f].x;
00372 pointOnField.y=pointOnField.y+pointsOnField[f].y;
00373 }
00374 pointOnField.x=pointOnField.x/numberOfFootPoints;
00375 pointOnField.y=pointOnField.y/numberOfFootPoints;
00376
00377
00378 return (pointOnField);
00379 }
00380
00381 Vector2<int> REnemySpecialist::calculateFarestPointCOG(int numberOfFootPoints, std::list<LinePair> enemyLines)
00382 {
00383 if (enemyLines.size()<4)
00384 {
00385 numberOfFootPoints=1;
00386
00387 }
00388 if ((unsigned)numberOfFootPoints>=enemyLines.size())
00389 {
00390 numberOfFootPoints=enemyLines.size();
00391
00392 }
00393 int footPointDistance [500];
00394 Vector2<double> footPoint[500];
00395 Vector2<int> pointsOnField[20];
00396 Vector2<int> pointOnField;
00397 horizon = rip->getHorizon();
00398 Vector2<double> point1,point2,maxPoint;
00399 list <LinePair>::iterator itmax = enemyLines.begin();
00400 list <LinePair>::iterator it1 = enemyLines.begin();
00401 LinePair b=*it1;
00402 LinePair bmax=*itmax;
00403 int i=0;
00404
00405 while(it1 != enemyLines.end())
00406 {
00407 b=*it1;
00408 point1.x=b.v1.x;
00409 point1.y=b.v1.y;
00410 point2.x=b.v2.x;
00411 point2.y=b.v2.y;
00412 footPoint[i]=point1;
00413 footPointDistance[i]=(int)Geometry::getDistanceToLine(horizon,footPoint[i]);
00414 i++;
00415 footPoint[i]=point2;
00416 footPointDistance[i]=(int)Geometry::getDistanceToLine(horizon,footPoint[i]);
00417 i++;
00418 it1++;
00419 }
00420
00421 sort(footPointDistance,footPoint,0,enemyLines.size()*2-1);
00422
00423 for (unsigned g = 0; g < (unsigned)numberOfFootPoints;g++)
00424 {
00425 DOT(imageProcessor_obstacles, footPoint[g].x,footPoint[g].y,Drawings::ps_solid, Drawings::yellow);
00426 Geometry::calculatePointOnField((int)footPoint[g].x,(int)footPoint[g].y,(rip->cameraMatrix),rip->image.cameraInfo,pointOnField);
00427 pointsOnField[g]=pointOnField;
00428 }
00429
00430
00431 pointOnField.x=0;
00432 pointOnField.y=0;
00433 for (unsigned f = 0; f < (unsigned)(numberOfFootPoints);f++)
00434 {
00435 pointOnField.x=pointOnField.x+pointsOnField[f].x;
00436 pointOnField.y=pointOnField.y+pointsOnField[f].y;
00437 }
00438 pointOnField.x=pointOnField.x/numberOfFootPoints;
00439 pointOnField.y=pointOnField.y/numberOfFootPoints;
00440
00441
00442 return (pointOnField);
00443 }
00444
00445
00446 Vector2<int> REnemySpecialist::calculatePointOnFieldFromSegment(std::list<LinePair> enemyLines)
00447 {
00448 Vector2<int> pointOnField;
00449 double maxY,maxY2,leftX,rightX,pointOnPictureX,pointOnPictureY;
00450 list <LinePair>::iterator it1 = enemyLines.begin();
00451 LinePair b=*it1;
00452
00453
00454 b=*it1;
00455 leftX=b.v1.x;
00456 rightX=b.v2.x;
00457 maxY=b.v1.y;
00458 maxY2=b.v1.y;
00459 while(it1 != enemyLines.end())
00460 {
00461 b=*it1;
00462 if (leftX>b.v1.x)
00463 {
00464 leftX=b.v1.x;
00465 }
00466 if (rightX<b.v2.x)
00467 {
00468 rightX=b.v2.x;
00469 }
00470 if (maxY<b.v1.y)
00471 {
00472 maxY2=maxY;
00473 maxY=b.v1.y;
00474 }
00475 it1++;
00476 }
00477
00478 pointOnPictureX=(leftX+rightX)/2;
00479
00480 pointOnPictureY=maxY;
00481 DOT(imageProcessor_obstacles, pointOnPictureX,pointOnPictureY,Drawings::ps_solid, Drawings::yellow);
00482
00483 Geometry::calculatePointOnField((int)pointOnPictureX,(int)pointOnPictureY,(rip->cameraMatrix),rip->image.cameraInfo,pointOnField);
00484
00485
00486 return (pointOnField);
00487 }
00488
00489 void REnemySpecialist::init()
00490 {
00491 postScanNeeded = false;
00492 preScanNeeded = true;
00493 if (!columns.empty())columns.clear();
00494 if (!lines.empty())lines.clear();
00495 }
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00506
00507
00508
00509
00510
00511
00512
00513
00514
00515
00516
00517
00518
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678