00001
00002
00003
00004
00005
00006
00007
00008
00009 #include "RDefaultStrategy.h"
00010 #include "Tools/Location.h"
00011
00012 typedef Geometry::Line Line;
00013
00014 RDefaultStrategy::RDefaultStrategy(RasterImageProcessor &processor):
00015 RasterStrategy(processor),
00016 ballFactor(2),
00017 fieldFactor(4),
00018 playerFactor(2),
00019 goalFactor(2),
00020 lmFactor(2),
00021 edgeScanner(processor),
00022 fieldDSA()
00023 {
00024
00025 InBinaryFile stream(getLocation().getFilename("active.rip"));
00026
00027 if(stream.exists()){
00028 for (int i = 0;i<RasterStrategy::numberOfActivationKeys;i++)
00029 stream.read(&active[i],1);
00030 }
00031 else{
00032 active[RasterStrategy::preScan] = true;
00033 active[RasterStrategy::postScan] = true;
00034 active[RasterStrategy::ball] = true;
00035 active[RasterStrategy::flag] = true;
00036 active[RasterStrategy::goal] = true;
00037 active[RasterStrategy::lines] = true;
00038 active[RasterStrategy::foes] = true;
00039 };
00040
00041 InBinaryFile stream2(getLocation().getFilename("ps.thr"));
00042
00043 if(stream2.exists()) stream2 >> postThreshold;
00044 else postThreshold = 30;
00045
00046 OUTPUT(idText,text,"RIP Activated");
00047 OUTPUT(idText,text,"preScan: " << active[RasterStrategy::preScan]);
00048 OUTPUT(idText,text,"postScan: " << active[RasterStrategy::postScan]);
00049 OUTPUT(idText,text,"ball: " << active[RasterStrategy::ball]);
00050 OUTPUT(idText,text,"flags: " << active[RasterStrategy::flag]);
00051 OUTPUT(idText,text,"goals: " << active[RasterStrategy::goal]);
00052 OUTPUT(idText,text,"lines: " << active[RasterStrategy::lines]);
00053 OUTPUT(idText,text,"foes: " << active[RasterStrategy::foes]);
00054 OUTPUT(idText,text,"ps-thr: " << postThreshold);
00055
00056 edgeScanner.threshold = postThreshold;
00057
00058
00059 rip->marginX = 2;
00060 rip->marginY = 2;
00061
00062 ballSpecialist = new RBallSpecialist2(processor,*this);
00063 rip->setSpecialist(ballSpecialist);
00064
00065 enemySpecialist = new REnemySpecialist(processor,*this);
00066 rip->setSpecialist(enemySpecialist);
00067
00068 fieldSpecialist = new RFieldSpecialist(processor,*this);
00069 rip->setSpecialist(fieldSpecialist);
00070
00071 boxSpecialist = new BoxSpecialist(processor,*this);
00072 rip->setSpecialist(boxSpecialist);
00073
00074 bridgeSpecialist = new RBridgeSpecialist(processor,*this);
00075 rip->setSpecialist(bridgeSpecialist);
00076
00077 for (int x=0;x<width;x++)
00078 for (int y=0;y<height;y++)
00079 {
00080
00081
00082 }
00083
00084
00085 }
00086
00087 RDefaultStrategy::~RDefaultStrategy()
00088 {
00089 delete ballSpecialist;
00090 delete fieldSpecialist;
00091 delete enemySpecialist;
00092 delete boxSpecialist;
00093 delete bridgeSpecialist;
00094 }
00095
00096 void RDefaultStrategy::init()
00097 {
00098 ballSpecialist->init();
00099 fieldSpecialist->init();
00100 boxSpecialist->init();
00101 enemySpecialist->init();
00102 bridgeSpecialist->init();
00103
00104 ballSpecialist->preScanNeeded = active[RasterStrategy::ball];
00105 enemySpecialist->preScanNeeded = active[RasterStrategy::foes];
00106 boxSpecialist->preScanNeeded = active[RasterStrategy::goal];
00107 fieldSpecialist->preScanNeeded = true;
00108 bridgeSpecialist->preScanNeeded = true;
00109
00110 if (!rip->cameraMatrix.isValid)
00111 {
00112 enemySpecialist->preScanNeeded = false;
00113 }
00114
00115
00116 width = rip->image.cameraInfo.resolutionWidth;
00117 height = rip->image.cameraInfo.resolutionHeight;
00118 }
00119
00120
00121
00122 void RDefaultStrategy::postProcessing(){
00123 if (active[RasterStrategy::ball]) ballSpecialist->executePostProcessing();
00124
00125 if (active[RasterStrategy::foes]) enemySpecialist->executePostProcessing();
00126
00127 if (active[RasterStrategy::goal]) boxSpecialist->executePostProcessing();
00128 bridgeSpecialist->executePostProcessing();
00129 fieldSpecialist->executePostProcessing();
00130 }
00131
00132 void RDefaultStrategy::execute()
00133 {
00134
00135 if (active[RasterStrategy::preScan])preScan();
00136
00137 postProcessing();
00138
00139 }
00140
00141
00142 void RDefaultStrategy::preScan()
00143 {
00144 const Geometry::Line& horizon = rip->horizon;
00145 Vector2<double> direction = horizon.direction;
00146 Vector2<double> base = horizon.base;
00147
00148 bool beforeHorizon = (direction.y > 0);
00149 Vector2<double> pDirection = direction;
00150 pDirection.rotateLeft();
00151 pDirection *= 20;
00152
00153
00154 if (0 <= direction.y && direction.y < 0.001 ) direction.y = 0.001;
00155 if (0 > direction.y && direction.y > -0.001 ) direction.y = -0.001;
00156 if (0 <= direction.x && direction.x < 0.001 ) direction.x = 0.001;
00157 if (0 > direction.x && direction.x > -0.001 ) direction.x = -0.001;
00158
00159 Vector2<double> upperBase = base + pDirection;
00160 Vector2<double> lowerBase = base - pDirection;
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170 const int xMax = width - rip->marginX;
00171 const int yMax = height - rip->marginY;
00172 const int my2 = rip->marginY*2;
00173 for (int y=0;y<yMax;y+=rip->marginY)
00174 {
00175 lastColor = noColor;
00176 currentColor = noColor;
00177 insideBall = false;
00178 insideBox = false;
00179 insideField = false;
00180 insideLM = false;
00181 insidePlayer = false;
00182 insideBridge = false;
00183
00184 BoxColor = noColor;
00185 bridgeColor = noColor;
00186
00187 int cross = 0;
00188 int upperCross = 0;
00189 int lowerCross = 0;
00190
00191 double t = 0;
00192
00193 t = ((double)y - base.y)/direction.y;
00194 cross = (int)(base.x + t*direction.x);
00195
00196 t = ((double)y - upperBase.y)/direction.y;
00197 upperCross = (int)(upperBase.x + t*direction.x);
00198
00199 t = ((double)y - lowerBase.y)/direction.y;
00200 lowerCross = (int)(lowerBase.x + t*direction.x);
00201
00202
00203 if (cross < 0) cross = 0;
00204 else if (cross > xMax) cross = xMax;
00205
00206 if (upperCross < 0) upperCross = 0;
00207 else if (upperCross > xMax) upperCross = xMax;
00208
00209 if (lowerCross < 0) lowerCross = 0;
00210 else if (lowerCross > xMax) lowerCross = xMax;
00211
00212 if (lowerCross > upperCross){
00213 int temp = lowerCross;
00214 lowerCross = upperCross;
00215 upperCross = temp;
00216 }
00217
00218 int x=0;
00219 if (y%my2 == 0){
00220 for(;x<lowerCross;x+=rip->marginX)
00221 {
00222 currentColor = getColor(x,y);
00223 checkBridge(x,y);
00224 checkRedLine(x,y);
00225 if (beforeHorizon){
00226 if (ballSpecialist->preScanNeeded)
00227 checkBall(x,y,0);
00228
00229 if (enemySpecialist->preScanNeeded)
00230 checkPlayer(x,y,0);
00231 }
00232 else{
00233 checkBox(x,y,0);
00234 }
00235
00236 lastColor = currentColor;
00237 }
00238 }
00239 else x=lowerCross-(lowerCross%rip->marginX);
00240
00241 for(;x<cross;x+=rip->marginX)
00242 {
00243 currentColor = getColor(x,y);
00244 checkBridge(x,y);
00245 checkRedLine(x,y);
00246 checkBox(x,y,0);
00247 if (beforeHorizon){
00248 if (ballSpecialist->preScanNeeded)
00249 checkBall(x,y,0);
00250
00251 if (enemySpecialist->preScanNeeded)
00252 checkPlayer(x,y,0);
00253 }
00254 else{
00255
00256 }
00257 lastColor = currentColor;
00258 }
00259
00260 if (beforeHorizon){
00261 ballSpecialist->invokeOnPreScan(x,y);
00262 enemySpecialist->invokeOnPreScan(x,y);
00263 }
00264
00265 for(;x<upperCross;x+=rip->marginX)
00266 {
00267 currentColor = getColor(x,y);
00268 checkBox(x,y,0);
00269 checkBridge(x,y);
00270 checkRedLine(x,y);
00271 if (!beforeHorizon){
00272 if (ballSpecialist->preScanNeeded)
00273 checkBall(x,y,0);
00274
00275 if (enemySpecialist->preScanNeeded)
00276 checkPlayer(x,y,0);
00277 }
00278 else{
00279
00280 }
00281 lastColor = currentColor;
00282 }
00283
00284 if (!beforeHorizon){
00285 boxSpecialist->invokeOnPreScan(x,y);
00286 }
00287
00288 if(y%my2 == 0){
00289 for(;x<xMax;x+=rip->marginX)
00290 {
00291 currentColor = getColor(x,y);
00292 checkBridge(x,y);
00293 checkRedLine(x,y);
00294 if (!beforeHorizon){
00295 if (ballSpecialist->preScanNeeded)
00296 checkBall(x,y,0);
00297
00298 if (enemySpecialist->preScanNeeded)
00299 checkPlayer(x,y,0);
00300 }
00301 else{
00302 checkBox(x,y,0);
00303 }
00304 lastColor = currentColor;
00305 }
00306 }
00307
00308
00309 if (!beforeHorizon){
00310 ballSpecialist->invokeOnPreScan(x,y);
00311 enemySpecialist->invokeOnPreScan(x,y);
00312 }
00313 else boxSpecialist->invokeOnPreScan(x,y);
00314 bridgeSpecialist->invokeOnPreScan(x,y);
00315 fieldSpecialist->invokeOnPreScan(x,y);
00316
00317 }
00318 }
00319
00320 void RDefaultStrategy::postScan(){
00321
00322
00323 const Geometry::Line& horizon = rip->horizon;
00324 Geometry::Line cHorizon;
00325 cHorizon.direction.x = horizon.direction.y;
00326 cHorizon.direction.y = -horizon.direction.x;
00327
00328 cHorizon.base.x = (rip->image.cameraInfo.resolutionWidth - 1)/2;
00329 cHorizon.base.y = (rip->image.cameraInfo.resolutionHeight -1)/2;
00330 Vector2<double>horCenter;
00331 Geometry::getIntersectionOfLines(horizon,cHorizon,horCenter);
00332
00333 int margin = 3;
00334 Vector2<int> leftBorder(
00335 (int)(horCenter.x - horizon.direction.x * 500),
00336 (int)(horCenter.y - horizon.direction.y * 500));
00337 Vector2<int> rightBorder(
00338 (int) (horCenter.x + horizon.direction.x * 500),
00339 (int) (horCenter.y + horizon.direction.y * 500));
00340 Vector2<int> topRight(
00341 rip->image.cameraInfo.resolutionWidth-2,
00342 rip->image.cameraInfo.resolutionHeight-2);
00343 Vector2<int> bottomLeft(1,1);
00344
00345 Geometry::clipLineWithRectangleCohenSutherland(
00346 bottomLeft,topRight,leftBorder,rightBorder);
00347 int horizonWidth = (leftBorder - rightBorder).abs();
00348 int steps = (int)((horizonWidth/(double)margin)*1.1);
00349
00350
00351 Vector2<int> leftToRight = rightBorder - leftBorder;
00352 Vector2<double> south = horizon.direction;
00353 south.rotateLeft();
00354 Vector2<double> step = horizon.direction*margin;
00355 Vector2<double> temp;
00356 Vector2<int> scan;
00357 Vector2<int> hScan;
00358 bool found = true;
00359
00360 hScan.x = (int)((horCenter.x - step.x*(steps/2)) + 0.5);
00361 hScan.y = (int)((horCenter.y - step.y*(steps/2)) + 0.5);
00362 DOT(imageProcessor_general, hScan.x,hScan.y,
00363 Drawings::red, Drawings::red);
00364
00365
00366
00367
00368
00369
00370
00371 edgeScanner.threshold = 50;
00372 Vector2<double> dir = south;
00373
00374
00375 Vector2<int> edge;
00376
00377 for (int i=0;i<steps;i++)
00378 {
00379 scan.x = (int)((double)hScan.x + (double)i*step.x + 0.5);
00380 scan.y = (int)((double)hScan.y + (double)i*step.y + 0.5);
00381 if (scan.x < 1 || scan.x > rip->image.cameraInfo.resolutionWidth-2
00382 ||scan.y < 1 || scan.y > rip->image.cameraInfo.resolutionHeight-2)
00383 {
00384
00385 if(!edgeScanner.findStart(scan,dir))
00386 continue;
00387 }
00388
00389 edgeScanner.setDirection(dir);
00390 fieldDSA.reset(scan.x,scan.y);
00391
00392 do{
00393 found = edgeScanner.bufferedScan(scan.x,scan.y);
00394 fieldDSA.update(scan.x,scan.y,edgeScanner);
00395 }while (found);
00396
00397 if (fieldDSA.foundObstacle())
00398 {
00399 rip->addObstaclePoints(
00400 fieldDSA.nearPoint(),
00401 fieldDSA.farPoint(),
00402 fieldDSA.getObstacleType());
00403 }
00404
00405 for (int j = 0;j<fieldDSA.size();j++)
00406 {
00407 Vector2<int> point = fieldDSA.getEdge(j);
00408
00409
00410 switch(fieldDSA.getType(j)){
00411 case RFieldStateMachine::line:
00412 rip->addFieldPoint(point.x,point.y,LinesPercept::field);
00413
00414 DOT(imageProcessor_general, point.x,point.y,
00415 Drawings::white, Drawings::skyblue);
00416 break;
00417 case RFieldStateMachine::border:
00418 rip->addFieldPoint(point.x,point.y,LinesPercept::border);
00419
00420 DOT(imageProcessor_general, point.x,point.y,
00421 Drawings::green, Drawings::skyblue);
00422 break;
00423 case RFieldStateMachine::yellowGoal:
00424 rip->addFieldPoint(point.x,point.y,LinesPercept::yellowGoal);
00425
00426 if (active[RasterStrategy::goal])
00427 boxSpecialist->invokeOnPostScan(point.x,point.y);
00428 DOT(imageProcessor_general, point.x,point.y,
00429 Drawings::yellow, Drawings::skyblue);
00430 break;
00431 case RFieldStateMachine::skyblueGoal:
00432 rip->addFieldPoint(point.x,point.y,LinesPercept::skyblueGoal);
00433
00434 if (active[RasterStrategy::goal])
00435 boxSpecialist->invokeOnPostScan(point.x,point.y);
00436 DOT(imageProcessor_general, point.x,point.y,
00437 Drawings::skyblue, Drawings::skyblue);
00438 break;
00439 case RFieldStateMachine::redRobot:
00440 DOT(imageProcessor_general, point.x,point.y,
00441 Drawings::skyblue, Drawings::red);
00442 break;
00443 case RFieldStateMachine::blueRobot:
00444 DOT(imageProcessor_general, point.x,point.y,
00445 Drawings::skyblue, Drawings::blue);
00446 break;
00447 case RFieldStateMachine::ball:
00448 DOT(imageProcessor_general, point.x,point.y,
00449 Drawings::orange, Drawings::orange);
00450 break;
00451 default:
00452 DOT(imageProcessor_general, point.x,point.y,
00453 Drawings::black, Drawings::skyblue);
00454 break;
00455 }
00456 }
00457 }
00458 }
00459
00460 void RDefaultStrategy::checkPlayer(int x,int y,int prePost){
00461
00462 bool isNewPoint = (currentColor == red || blue || gray || black);
00463 bool isClosingPoint = (currentColor == green || currentColor == white);
00464
00465 if (!insidePlayer){
00466 if (isNewPoint){
00467 enemySpecialist->invokeOnPreScan(x,y);
00468 insidePlayer = true;
00469 }
00470 }
00471 else if (isClosingPoint){
00472 enemySpecialist->invokeOnPreScan(x-rip->marginX,y);
00473 insidePlayer = false;
00474 }
00475 }
00476
00477 void RDefaultStrategy::checkBall(int x,int y,int prePost){
00478 bool isNewPoint = currentColor == orange;
00479 if (!insideBall){
00480 if (isNewPoint){
00481 ballSpecialist->invokeOnPreScan(x,y);
00482 insideBall = true;
00483 }
00484 }
00485 else if (!isNewPoint){
00486 ballSpecialist->invokeOnPreScan(x-rip->marginX,y);
00487 insideBall = false;
00488 }
00489 }
00490
00491 void RDefaultStrategy::checkField(int x,int y,int prePost){
00492
00493 bool isNewPoint;
00494
00495 isNewPoint = (currentColor == white);
00496
00497 if (!insideField){
00498 if (isNewPoint){
00499 if (prePost == 0) fieldSpecialist->invokeOnPreScan(x,y);
00500 else fieldSpecialist->invokeOnPostScan(x,y);
00501 insideField = true;
00502 }
00503 }
00504 else if(!isNewPoint){
00505 if (prePost == 0) fieldSpecialist->invokeOnPreScan(x-fieldFactor,y);
00506 else fieldSpecialist->invokeOnPostScan(x,y+fieldFactor);
00507 insideField = false;
00508
00509 }
00510 }
00511
00512
00513 void RDefaultStrategy::checkBox(int x, int y,int prePost){
00514
00515 bool newBoxPoint;
00516
00517
00518 newBoxPoint = (currentColor == yellow || currentColor == skyblue || currentColor == pink);
00519
00520
00521 if(!insideBox) {
00522 if (newBoxPoint) {
00523
00524 boxSpecialist->invokeOnPreScan(x,y);
00525 BoxColor = currentColor;
00526 insideBox = true;
00527 }
00528 }
00529 else if(currentColor!=BoxColor){
00530
00531 boxSpecialist->invokeOnPreScan(x-rip->marginX,y);
00532 BoxColor = noColor;
00533 insideBox = false;
00534 }
00535
00536 }
00537
00538 void RDefaultStrategy::checkRedLine(int x,int y){
00539 bool newRedPoint = currentColor == red;
00540
00541 if (!insideField) {
00542 if (newRedPoint){
00543 fieldSpecialist->invokeOnPreScan(x,y);
00544 insideField = true;
00545 }
00546 }
00547 else if (!newRedPoint){
00548 fieldSpecialist->invokeOnPreScan(x,y);
00549 insideField = false;
00550 }
00551 }
00552
00553 void RDefaultStrategy::checkBridge(int x, int y){
00554
00555 bool newBoxPoint =
00556 (currentColor == yellow || currentColor == skyblue || currentColor == orange);
00557
00558 if(!insideBridge) {
00559 if (newBoxPoint) {
00560
00561 bridgeSpecialist->invokeOnPreScan(x,y);
00562 bridgeColor = currentColor;
00563 insideBridge = true;
00564 }
00565 }
00566 else if(currentColor!=bridgeColor){
00567
00568 bridgeSpecialist->invokeOnPreScan(x-rip->marginX,y);
00569 bridgeColor = noColor;
00570 insideBridge = false;
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
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695
00696
00697
00698
00699
00700
00701
00702
00703
00704
00705
00706
00707
00708
00709
00710
00711
00712
00713
00714
00715
00716
00717
00718
00719
00720
00721
00722
00723
00724
00725
00726
00727
00728
00729
00730
00731
00732
00733
00734
00735
00736
00737
00738
00739
00740
00741
00742
00743
00744
00745
00746
00747
00748
00749
00750
00751
00752
00753
00754
00755
00756
00757
00758
00759
00760
00761
00762
00763
00764
00765
00766
00767
00768
00769
00770
00771
00772
00773
00774
00775
00776
00777
00778
00779
00780
00781
00782
00783
00784
00785
00786
00787
00788
00789
00790
00791
00792
00793
00794
00795
00796
00797
00798
00799
00800
00801
00802
00803
00804
00805
00806
00807
00808
00809
00810
00811
00812
00813
00814
00815
00816
00817
00818
00819
00820
00821
00822
00823
00824
00825
00826