Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Representations/Cognition/ObstaclesModel.cpp

Go to the documentation of this file.

    00001 /**
    00002  * @file ObstaclesModel.cpp
    00003  *
    00004  * Implementation of class ObstaclesModel.
    00005  * @author <a href="mailto:juengel@informatik.hu-berlin.de">Matthias Juengel</a>
    00006  */
    00007
    00008 #include "ObstaclesModel.h"
    00009 #include "Tools/Math/Geometry.h"
    00010 #include "Tools/Debugging/DebugDrawings.h"
    00011 #include "Tools/RingBufferWithSum.h"
    00012 #include "Tools/FieldDimensions.h"
    00013
    00014
    00015
    00016 ObstaclesModel::ObstaclesModel()
    00017 {
    00018   for(int i = 0; i < numOfSectors; i++)
    00019   {
    00020     distance[i] = maxDistance;
    00021   }
    00022   corridorInFront = maxDistance;
    00023   lastTimeFreePartOfGoalWasDetermined[0] = 0;
    00024   lastTimeFreePartOfGoalWasDetermined[1] = 0;
    00025   angleToFreePartOfGoalWasDetermined[0] = false;
    00026   angleToFreePartOfGoalWasDetermined[1] = false;
    00027   angleToNextFreeTeammateWasDetermined = false;
    00028   currentlySeenStartSector = numOfSectors;
    00029   currentlySeenEndSector = 0;
    00030   bodyPSD = 0;
    00031
    00032   trustSeenGoal[0] = false;
    00033   trustSeenGoal[1] = false;
    00034 }
    00035
    00036 ObstaclesModel::~ObstaclesModel()
    00037 {
    00038 }
    00039
    00040 int ObstaclesModel::getDistanceInMajorDirection(Directions direction) const
    00041 {
    00042   int numberOfMicrosPerMacro = numOfSectors/numberOfDirections;
    00043   int macroSector = (int)direction;
    00044   int sector;
    00045   int endSector = ((macroSector+1)*numberOfMicrosPerMacro-numberOfMicrosPerMacro/2)%numOfSectors;
    00046
    00047   int minDistance = maxDistance;
    00048
    00049   for(
    00050     sector = macroSector*numberOfMicrosPerMacro-numberOfMicrosPerMacro/2;
    00051     sector != endSector;
    00052     sector = (sector + 1) % numOfSectors)
    00053     if(distance[sector] < minDistance)
    00054       minDistance = distance[sector];
    00055
    00056   return minDistance;
    00057 }
    00058
    00059
    00060
    00061 int ObstaclesModel::getDistanceInDirection(double direction, double openingAngle) const
    00062 {
    00063   int sector;
    00064   int minDistance = 2 * xPosOpponentGroundline;
    00065   int endSector = getSectorFromAngle(direction + openingAngle);
    00066   int startSector = getSectorFromAngle(direction - openingAngle);
    00067
    00068   if (endSector == startSector && distance[startSector] < minDistance)
    00069     return distance[startSector];
    00070
    00071   for (
    00072     sector = startSector;
    00073     sector != (endSector + 1) % numOfSectors;
    00074     sector = (sector + 1) % numOfSectors)
    00075     if (distance[sector] < minDistance)
    00076       minDistance = distance[sector];
    00077
    00078   return minDistance;
    00079 }
    00080
    00081 int ObstaclesModel::getSectorDistanceInDirection(double direction) const
    00082 {
    00083     return distance[getSectorFromAngle(direction)];
    00084 }
    00085
    00086
    00087
    00088 double ObstaclesModel::getTotalFreeSpaceInSector(double direction, double openingAngle, double maxDist) const
    00089 {
    00090   int sector;
    00091   int endSector = getSectorFromAngle(direction + openingAngle/2);
    00092   double sum = 0.0;
    00093
    00094   for (sector = getSectorFromAngle(direction - openingAngle/2);
    00095     sector != endSector; sector = (sector + 1) % numOfSectors)
    00096     {
    00097     if (distance[sector] < maxDist)
    00098       sum += distance[sector];
    00099     else
    00100       sum += maxDist;
    00101     }
    00102
    00103   return sum;
    00104 }
    00105
    00106
    00107 double ObstaclesModel::getDistanceInCorridor(double angle, double width) const
    00108 {
    00109   double minDistance = maxDistance;
    00110
    00111   int minSector = ObstaclesModel::getSectorFromAngle(angle-pi_2);
    00112   int maxSector = ObstaclesModel::getSectorFromAngle(angle+pi_2);
    00113
    00114   Geometry::Line centerLine, leftLine, rightLine;
    00115   leftLine.direction.x = cos(angle);
    00116   leftLine.direction.y = sin(angle);
    00117
    00118   leftLine.base.y = leftLine.direction.x * width / 2;
    00119   leftLine.base.x = -leftLine.direction.y * width / 2;
    00120
    00121   rightLine.direction = leftLine.direction;
    00122   rightLine.base = -leftLine.base;
    00123
    00124   centerLine.base.x = 0; centerLine.base.y = 0;
    00125   centerLine.direction = leftLine.direction;
    00126
    00127   for(int i = minSector; i < maxSector; i++)
    00128   {
    00129     int currentSector = i%numOfSectors;
    00130     Vector2<double> point;
    00131     point.x = cos(getAngleOfSector(currentSector)) * distance[currentSector];
    00132     point.y = sin(getAngleOfSector(currentSector)) * distance[currentSector];
    00133     if(
    00134       distance[currentSector] < minDistance &&
    00135       distance[currentSector] > 10 &&
    00136       abs((int)Geometry::getDistanceToLine(centerLine, point)) < width / 2.0
    00137       )
    00138       minDistance = distance[currentSector];
    00139   }
    00140
    00141   LINE(models_corridorsRadar,
    00142     leftLine.base.x, leftLine.base.y,
    00143     leftLine.base.x + leftLine.direction.x * minDistance,
    00144     leftLine.base.y + leftLine.direction.y * minDistance,
    00145     2, Drawings::ps_solid, Drawings::red
    00146     );
    00147   LINE(models_corridorsRadar,
    00148     rightLine.base.x, rightLine.base.y,
    00149     rightLine.base.x + rightLine.direction.x * minDistance,
    00150     rightLine.base.y + rightLine.direction.y * minDistance,
    00151     2, Drawings::ps_solid, Drawings::green
    00152     );
    00153   LINE(models_corridorsRadar,
    00154     leftLine.base.x + leftLine.direction.x * minDistance,
    00155     leftLine.base.y + leftLine.direction.y * minDistance,
    00156     rightLine.base.x + rightLine.direction.x * minDistance,
    00157     rightLine.base.y + rightLine.direction.y * minDistance,
    00158     2, Drawings::ps_solid, Drawings::blue
    00159     );
    00160   return minDistance;
    00161 }
    00162
    00163 double ObstaclesModel::getAngleOfNextFreeSectorLeft(double sizeOfGap, double angle, int minDistance) const
    00164 {
    00165   double direction = 0;
    00166   int numberOfSectorsOfGap = (int)(sizeOfGap * numOfSectors / pi2);
    00167
    00168   int startSector = getSectorFromAngle(angle);
    00169   int leftCounter = 0;
    00170
    00171   for(int i = -numberOfSectorsOfGap / 2; i < numOfSectors / 2; i++)
    00172   {
    00173     int leftSector = (startSector+i)%numOfSectors;
    00174 //    ASSERT(leftSector < numOfSectors && leftSector >=0);
    00175     if(distance[leftSector] > minDistance)
    00176       leftCounter++;
    00177     else leftCounter = 0;
    00178
    00179     if(leftCounter == numberOfSectorsOfGap)
    00180     {
    00181       direction = getAngleOfSector((startSector+i-numberOfSectorsOfGap/2)%numOfSectors);
    00182       break;
    00183     }
    00184
    00185   }
    00186   /*
    00187   LINE(models_corridorsRadar,
    00188     0, 0,
    00189     cos(direction) * 1000,
    00190     sin(direction) * 1000,
    00191     20, Drawings::ps_solid, Drawings::red
    00192     );
    00193
    00194   LINE(models_corridorsRadar,
    00195     0, 0,
    00196     cos(angle) * 1000,
    00197     sin(angle) * 1000,
    00198     15, Drawings::ps_dash, Drawings::blue
    00199     );
    00200
    00201   CIRCLE(models_corridorsRadar,
    00202     0, 0, minDistance,
    00203     3, Drawings::ps_solid, Drawings::blue);
    00204 */
    00205   // Left and Rightside is calculated, check teammates
    00206
    00207   return direction;
    00208 }
    00209
    00210
    00211 double ObstaclesModel::getAngleOfNextFreeSectorRight(double sizeOfGap, double angle, int minDistance) const
    00212 {
    00213   double direction = 0;
    00214   int numberOfSectorsOfGap = (int)(sizeOfGap * numOfSectors / pi2);
    00215
    00216   int startSector = getSectorFromAngle(angle);
    00217   int rightCounter = 0;
    00218
    00219   for(int i = -numberOfSectorsOfGap / 2; i < numOfSectors / 2; i++)
    00220   {
    00221     int rightSector = (startSector-i)%numOfSectors;
    00222 //    ASSERT(rightSector < numOfSectors && rightSector >=0);
    00223     if(distance[rightSector] > minDistance)
    00224       rightCounter++;
    00225     else rightCounter = 0;
    00226
    00227     if(rightCounter == numberOfSectorsOfGap)
    00228     {
    00229       direction = getAngleOfSector((startSector-i+numberOfSectorsOfGap/2)%numOfSectors);
    00230       break;
    00231     }
    00232
    00233   }
    00234   /*
    00235   LINE(models_corridorsRadar,
    00236     0, 0,
    00237     cos(direction) * 1000,
    00238     sin(direction) * 1000,
    00239     20, Drawings::ps_solid, Drawings::red
    00240     );
    00241
    00242   LINE(models_corridorsRadar,
    00243     0, 0,
    00244     cos(angle) * 1000,
    00245     sin(angle) * 1000,
    00246     15, Drawings::ps_dash, Drawings::blue
    00247     );
    00248
    00249   CIRCLE(models_corridorsRadar,
    00250     0, 0, minDistance,
    00251     3, Drawings::ps_solid, Drawings::blue);
    00252 */
    00253   // Left and Rightside is calculated, check teammates
    00254
    00255   return direction;
    00256 }
    00257 double ObstaclesModel::getAngleOfNextFreeSector(double sizeOfGap, double angle, int minDistance) const
    00258 {
    00259   double direction = 0;
    00260   int numberOfSectorsOfGap = (int)(sizeOfGap * numOfSectors / pi2);
    00261
    00262   int startSector = getSectorFromAngle(angle);
    00263   int leftCounter = 0;
    00264   int rightCounter = 0;
    00265
    00266   for(int i = -numberOfSectorsOfGap / 2; i < numOfSectors / 2; i++)
    00267   {
    00268     int leftSector = (startSector+i)%numOfSectors;
    00269 //    ASSERT(leftSector < numOfSectors && leftSector >=0);
    00270     if(distance[leftSector] > minDistance)
    00271       leftCounter++;
    00272     else leftCounter = 0;
    00273
    00274     int rightSector = (startSector-i)%numOfSectors;
    00275 //    ASSERT(rightSector < numOfSectors && rightSector >=0);
    00276     if(distance[rightSector] > minDistance)
    00277       rightCounter++;
    00278     else rightCounter = 0;
    00279
    00280     if(leftCounter == numberOfSectorsOfGap)
    00281     {
    00282       direction = getAngleOfSector((startSector+i-numberOfSectorsOfGap/2)%numOfSectors);
    00283       break;
    00284     }
    00285
    00286     if(rightCounter == numberOfSectorsOfGap)
    00287     {
    00288       direction = getAngleOfSector((startSector-i+numberOfSectorsOfGap/2)%numOfSectors);
    00289       break;
    00290     }
    00291   }
    00292   /*
    00293   LINE(models_corridorsRadar,
    00294     0, 0,
    00295     cos(direction) * 1000,
    00296     sin(direction) * 1000,
    00297     20, Drawings::ps_solid, Drawings::red
    00298     );
    00299
    00300   LINE(models_corridorsRadar,
    00301     0, 0,
    00302     cos(angle) * 1000,
    00303     sin(angle) * 1000,
    00304     15, Drawings::ps_dash, Drawings::blue
    00305     );
    00306
    00307   CIRCLE(models_corridorsRadar,
    00308     0, 0, minDistance,
    00309     3, Drawings::ps_solid, Drawings::blue);
    00310 */
    00311   return direction;
    00312 }
    00313
    00314
    00315 int ObstaclesModel::getMinimalDistanceInRange
    00316 (
    00317  double centerAngle,
    00318  double openingAngle,
    00319  double& angleWithMinimalDistance
    00320  ) const
    00321 {
    00322   int centerSector = getSectorFromAngle(centerAngle);
    00323   int endSector = getSectorFromAngle(centerAngle + openingAngle / 2.0);
    00324
    00325   int minDistance = 10000;
    00326   int sectorWithMinimalDistance = centerSector;
    00327
    00328   for(int i = 0; centerSector + i < endSector; i++)
    00329   {
    00330     if(distance[centerSector + i] < minDistance)
    00331     {
    00332       minDistance = distance[centerSector + i];
    00333       sectorWithMinimalDistance = centerSector + i;
    00334     }
    00335     if(distance[centerSector - i] < minDistance)
    00336     {
    00337       minDistance = distance[centerSector - i];
    00338       sectorWithMinimalDistance = centerSector - i;
    00339     }
    00340   }
    00341   angleWithMinimalDistance = getAngleOfSector(sectorWithMinimalDistance);
    00342   return minDistance;
    00343 }
    00344
    00345 int ObstaclesModel::getMinimalDistanceInRange
    00346 (
    00347  double centerAngle,
    00348  double openingAngle,
    00349  double& angleWithMinimalDistance,
    00350  ObstaclesPercept::ObstacleType obstacleType
    00351  ) const
    00352 {
    00353   int centerSector = getSectorFromAngle(centerAngle);
    00354   int endSector = getSectorFromAngle(centerAngle + openingAngle / 2.0);
    00355
    00356   int minDistance = 10000;
    00357   int sectorWithMinimalDistance = centerSector;
    00358
    00359   for(int i = 0; centerSector + i < endSector; i++)
    00360   {
    00361     if(this->obstacleType[centerSector + i] == obstacleType && distance[centerSector + i] < minDistance)
    00362     {
    00363       minDistance = distance[centerSector + i];
    00364       sectorWithMinimalDistance = centerSector + i;
    00365     }
    00366     if(this->obstacleType[centerSector - i] == obstacleType && distance[centerSector - i] < minDistance)
    00367     {
    00368       minDistance = distance[centerSector - i];
    00369       sectorWithMinimalDistance = centerSector - i;
    00370     }
    00371   }
    00372   angleWithMinimalDistance = getAngleOfSector(sectorWithMinimalDistance);
    00373   return minDistance;
    00374 }
    00375
    00376 double ObstaclesModel::getPercentageOfLowDistanceObstaclesInRange
    00377 (
    00378  double centerAngle,
    00379  double openingAngle,
    00380  int maxDistance
    00381  ) const
    00382 {
    00383   int beginSector = getSectorFromAngle(centerAngle - openingAngle / 2.0);
    00384   int endSector = getSectorFromAngle(centerAngle + openingAngle / 2.0);
    00385
    00386   int numberOfSectorsWithCloseObstacle = 0;
    00387
    00388   for(int i = beginSector; i <= endSector; i++)
    00389   {
    00390     if(distance[i] < maxDistance)
    00391     {
    00392       numberOfSectorsWithCloseObstacle++;
    00393     }
    00394   }
    00395   return (double)(numberOfSectorsWithCloseObstacle) / (double)(endSector - beginSector + 1);
    00396 }
    00397
    00398
    00399 double ObstaclesModel::getAngleOfLargeGapInRange(double centerAngle, double openingAngle, SearchDirections searchDirection) const
    00400 {
    00401   int centerSector = getSectorFromAngle(centerAngle);
    00402   int endSectorLeft = getSectorFromAngle(centerAngle + openingAngle / 2.0);
    00403
    00404   int numberOfSectorsOnOneSide = endSectorLeft - centerSector + 1;
    00405
    00406   int bestSector;
    00407   //int sumOfDistances = 0;
    00408   int maxSumOfDistances = 0;
    00409   enum {numberOfSectorsOfGap = 7};
    00410
    00411   RingBufferWithSum<numberOfSectorsOfGap> bufferMovingRight;
    00412   RingBufferWithSum<numberOfSectorsOfGap> bufferMovingLeft;
    00413
    00414   int i;
    00415   for(i = 0; i < numberOfSectorsOfGap; i++)
    00416   {
    00417     bufferMovingLeft.add(min(1000, distance[i + centerSector - numberOfSectorsOfGap / 2]) );
    00418     bufferMovingRight.add(min(1000, distance[-i + centerSector + numberOfSectorsOfGap / 2]) );
    00419   }
    00420   maxSumOfDistances = bufferMovingLeft.getSum();
    00421   bestSector = centerSector;
    00422
    00423   for(i = numberOfSectorsOfGap / 2 + 1; i < numberOfSectorsOnOneSide; i++)
    00424   {
    00425     bufferMovingLeft.add(min(1000, distance[centerSector + i]) );
    00426     bufferMovingRight.add(min(1000, distance[centerSector - i]) );
    00427
    00428     int newSum;
    00429     int newSector;
    00430     if(
    00431       (
    00432       bufferMovingLeft.getSum() > bufferMovingRight.getSum() &&
    00433       searchDirection != searchRight)
    00434       ||
    00435       searchDirection == searchLeft
    00436       )
    00437     {
    00438       newSum = bufferMovingLeft.getSum();
    00439       newSector = centerSector + i - (numberOfSectorsOfGap / 2);
    00440     }
    00441     else
    00442     {
    00443       newSum = bufferMovingRight.getSum();
    00444       newSector = centerSector - i + (numberOfSectorsOfGap / 2);
    00445     }
    00446
    00447     if(newSum > maxSumOfDistances + 80 * numberOfSectorsOfGap)
    00448     {
    00449       maxSumOfDistances = newSum;
    00450       bestSector = newSector;
    00451     }
    00452   }
    00453   return getAngleOfSector(bestSector);
    00454 }
    00455
    00456 double ObstaclesModel::getAngleOfLargeGapInRange2(double centerAngle, double openingAngle, SearchDirections searchDirection) const
    00457 {
    00458   int centerSector = getSectorFromAngle(centerAngle);
    00459   int endSectorLeft = getSectorFromAngle(centerAngle + openingAngle / 2.0);
    00460
    00461   int numberOfSectorsOnOneSide = endSectorLeft - centerSector + 1;
    00462
    00463   int bestSector;
    00464   //int sumOfDistances = 0;
    00465   int maxMinOfDistances = 0;
    00466   enum {numberOfSectorsOfGap = 7};
    00467
    00468   RingBufferWithSum<numberOfSectorsOfGap> bufferMovingRight;
    00469   RingBufferWithSum<numberOfSectorsOfGap> bufferMovingLeft;
    00470
    00471   int i;
    00472   for(i = 0; i < numberOfSectorsOfGap; i++)
    00473   {
    00474     bufferMovingLeft.add(min(1000, distance[i + centerSector - numberOfSectorsOfGap / 2]) );
    00475     bufferMovingRight.add(min(1000, distance[-i + centerSector + numberOfSectorsOfGap / 2]) );
    00476   }
    00477   maxMinOfDistances = bufferMovingLeft.getMinimum();
    00478   bestSector = centerSector;
    00479
    00480   for(i = numberOfSectorsOfGap / 2 + 1; i < numberOfSectorsOnOneSide; i++)
    00481   {
    00482     bufferMovingLeft.add(min(1000, distance[centerSector + i]) );
    00483     bufferMovingRight.add(min(1000, distance[centerSector - i]) );
    00484
    00485     int newMin;
    00486     int newSector;
    00487     if(
    00488       (
    00489       bufferMovingLeft.getSum() > bufferMovingRight.getSum() &&
    00490       searchDirection != searchRight)
    00491       ||
    00492       searchDirection == searchLeft
    00493       )
    00494     {
    00495       newMin = bufferMovingLeft.getMinimum();
    00496       newSector = centerSector + i - (numberOfSectorsOfGap / 2);
    00497     }
    00498     else
    00499     {
    00500       newMin = bufferMovingRight.getMinimum();
    00501       newSector = centerSector - i + (numberOfSectorsOfGap / 2);
    00502     }
    00503
    00504     if(newMin > maxMinOfDistances + 80)
    00505     {
    00506       maxMinOfDistances = newMin;
    00507       bestSector = newSector;
    00508     }
    00509   }
    00510   return getAngleOfSector(bestSector);
    00511 }
    00512
    00513
    00514 void ObstaclesModel::operator=(const ObstaclesModel& other)
    00515 {
    00516   for(int i = 0; i < numOfSectors; i++)
    00517   {
    00518     distance[i] = other.distance[i];
    00519     obstacleType[i] = other.obstacleType[i];
    00520   }
    00521   corridorInFront = other.corridorInFront;
    00522 }
    00523
    00524 In& operator>>(In& stream,ObstaclesModel& obstaclesModel)
    00525 {
    00526   stream >> obstaclesModel.frameNumber;
    00527   stream.read(&obstaclesModel,sizeof(ObstaclesModel));
    00528   return stream;
    00529 }
    00530
    00531 Out& operator<<(Out& stream, const ObstaclesModel& obstaclesModel)
    00532 {
    00533   stream << obstaclesModel.frameNumber;
    00534   stream.write(&obstaclesModel,sizeof(ObstaclesModel));
    00535   return stream;
    00536 }