Representations/Cognition/ObstaclesModel.cpp

Go to the documentation of this file.

/**
 * @file ObstaclesModel.cpp
 *
 * Implementation of class ObstaclesModel. 
 * @author <a href="mailto:juengel@informatik.hu-berlin.de">Matthias Juengel</a>
 */

#include "ObstaclesModel.h"
#include "Tools/Math/Geometry.h"
#include "Tools/Debugging/DebugDrawings.h"
#include "Tools/RingBufferWithSum.h"
#include "Tools/FieldDimensions.h"



ObstaclesModel::ObstaclesModel()
{
  for(int i = 0; i < numOfSectors; i++)
  {
    distance[i] = maxDistance;
  }
  corridorInFront = maxDistance;
  lastTimeFreePartOfGoalWasDetermined[0] = 0;
  lastTimeFreePartOfGoalWasDetermined[1] = 0;
  angleToFreePartOfGoalWasDetermined[0] = false;
  angleToFreePartOfGoalWasDetermined[1] = false;
  angleToNextFreeTeammateWasDetermined = false;
  currentlySeenStartSector = numOfSectors;
  currentlySeenEndSector = 0;
  bodyPSD = 0;

  trustSeenGoal[0] = false;
  trustSeenGoal[1] = false;
}

ObstaclesModel::~ObstaclesModel()
{
}

int ObstaclesModel::getDistanceInMajorDirection(Directions direction) const
{
  int numberOfMicrosPerMacro = numOfSectors/numberOfDirections;
  int macroSector = (int)direction;
  int sector;
  int endSector = ((macroSector+1)*numberOfMicrosPerMacro-numberOfMicrosPerMacro/2)%numOfSectors;

  int minDistance = maxDistance;

  for(
    sector = macroSector*numberOfMicrosPerMacro-numberOfMicrosPerMacro/2; 
    sector != endSector; 
    sector = (sector + 1) % numOfSectors)
    if(distance[sector] < minDistance) 
      minDistance = distance[sector];

  return minDistance;
}



int ObstaclesModel::getDistanceInDirection(double direction, double openingAngle) const
{
  int sector;
  int minDistance = 2 * xPosOpponentGroundline;
  int endSector = getSectorFromAngle(direction + openingAngle);
  int startSector = getSectorFromAngle(direction - openingAngle);

  if (endSector == startSector && distance[startSector] < minDistance)
    return distance[startSector];

  for (
    sector = startSector;
    sector != (endSector + 1) % numOfSectors;
    sector = (sector + 1) % numOfSectors)
    if (distance[sector] < minDistance)
      minDistance = distance[sector];

  return minDistance;
}

int ObstaclesModel::getSectorDistanceInDirection(double direction) const
{
    return distance[getSectorFromAngle(direction)];
}



double ObstaclesModel::getTotalFreeSpaceInSector(double direction, double openingAngle, double maxDist) const
{
  int sector;
  int endSector = getSectorFromAngle(direction + openingAngle/2);
  double sum = 0.0;

  for (sector = getSectorFromAngle(direction - openingAngle/2); 
    sector != endSector; sector = (sector + 1) % numOfSectors)
    {
    if (distance[sector] < maxDist)
      sum += distance[sector];
    else
      sum += maxDist;
    }

  return sum;
}


double ObstaclesModel::getDistanceInCorridor(double angle, double width) const
{
  double minDistance = maxDistance;

  int minSector = ObstaclesModel::getSectorFromAngle(angle-pi_2);
  int maxSector = ObstaclesModel::getSectorFromAngle(angle+pi_2);

  Geometry::Line centerLine, leftLine, rightLine;
  leftLine.direction.x = cos(angle);
  leftLine.direction.y = sin(angle);

  leftLine.base.y = leftLine.direction.x * width / 2;
  leftLine.base.x = -leftLine.direction.y * width / 2;

  rightLine.direction = leftLine.direction;
  rightLine.base = -leftLine.base;

  centerLine.base.x = 0; centerLine.base.y = 0;
  centerLine.direction = leftLine.direction;

  for(int i = minSector; i < maxSector; i++)
  {
    int currentSector = i%numOfSectors;
    Vector2<double> point; 
    point.x = cos(getAngleOfSector(currentSector)) * distance[currentSector];
    point.y = sin(getAngleOfSector(currentSector)) * distance[currentSector];
    if(
      distance[currentSector] < minDistance && 
      distance[currentSector] > 10 &&
      abs((int)Geometry::getDistanceToLine(centerLine, point)) < width / 2.0
      )
      minDistance = distance[currentSector];
  }

  LINE(models_corridorsRadar, 
    leftLine.base.x, leftLine.base.y, 
    leftLine.base.x + leftLine.direction.x * minDistance,
    leftLine.base.y + leftLine.direction.y * minDistance,
    2, Drawings::ps_solid, Drawings::red
    );
  LINE(models_corridorsRadar, 
    rightLine.base.x, rightLine.base.y, 
    rightLine.base.x + rightLine.direction.x * minDistance,
    rightLine.base.y + rightLine.direction.y * minDistance,
    2, Drawings::ps_solid, Drawings::green
    );
  LINE(models_corridorsRadar, 
    leftLine.base.x + leftLine.direction.x * minDistance,
    leftLine.base.y + leftLine.direction.y * minDistance,
    rightLine.base.x + rightLine.direction.x * minDistance,
    rightLine.base.y + rightLine.direction.y * minDistance,
    2, Drawings::ps_solid, Drawings::blue
    );
  return minDistance;
}

double ObstaclesModel::getAngleOfNextFreeSectorLeft(double sizeOfGap, double angle, int minDistance) const
{
  double direction = 0;
  int numberOfSectorsOfGap = (int)(sizeOfGap * numOfSectors / pi2);

  int startSector = getSectorFromAngle(angle);
  int leftCounter = 0;
 
  for(int i = -numberOfSectorsOfGap / 2; i < numOfSectors / 2; i++)
  {
    int leftSector = (startSector+i)%numOfSectors;
//    ASSERT(leftSector < numOfSectors && leftSector >=0);
    if(distance[leftSector] > minDistance)
      leftCounter++;
    else leftCounter = 0;
    
    if(leftCounter == numberOfSectorsOfGap)
    {
      direction = getAngleOfSector((startSector+i-numberOfSectorsOfGap/2)%numOfSectors);
      break;
    }

  }
  /*
  LINE(models_corridorsRadar, 
    0, 0, 
    cos(direction) * 1000,
    sin(direction) * 1000,
    20, Drawings::ps_solid, Drawings::red
    );

  LINE(models_corridorsRadar, 
    0, 0, 
    cos(angle) * 1000,
    sin(angle) * 1000,
    15, Drawings::ps_dash, Drawings::blue
    );

  CIRCLE(models_corridorsRadar, 
    0, 0, minDistance,
    3, Drawings::ps_solid, Drawings::blue);
*/
  // Left and Rightside is calculated, check teammates

  return direction;
}


double ObstaclesModel::getAngleOfNextFreeSectorRight(double sizeOfGap, double angle, int minDistance) const
{
  double direction = 0;
  int numberOfSectorsOfGap = (int)(sizeOfGap * numOfSectors / pi2);

  int startSector = getSectorFromAngle(angle);
  int rightCounter = 0;

  for(int i = -numberOfSectorsOfGap / 2; i < numOfSectors / 2; i++)
  {    
    int rightSector = (startSector-i)%numOfSectors;
//    ASSERT(rightSector < numOfSectors && rightSector >=0);
    if(distance[rightSector] > minDistance)
      rightCounter++;
    else rightCounter = 0;

    if(rightCounter == numberOfSectorsOfGap)
    {
      direction = getAngleOfSector((startSector-i+numberOfSectorsOfGap/2)%numOfSectors);
      break;
    }

  }
  /*
  LINE(models_corridorsRadar, 
    0, 0, 
    cos(direction) * 1000,
    sin(direction) * 1000,
    20, Drawings::ps_solid, Drawings::red
    );

  LINE(models_corridorsRadar, 
    0, 0, 
    cos(angle) * 1000,
    sin(angle) * 1000,
    15, Drawings::ps_dash, Drawings::blue
    );

  CIRCLE(models_corridorsRadar, 
    0, 0, minDistance,
    3, Drawings::ps_solid, Drawings::blue);
*/
  // Left and Rightside is calculated, check teammates

  return direction;
}
double ObstaclesModel::getAngleOfNextFreeSector(double sizeOfGap, double angle, int minDistance) const
{
  double direction = 0;
  int numberOfSectorsOfGap = (int)(sizeOfGap * numOfSectors / pi2);

  int startSector = getSectorFromAngle(angle);
  int leftCounter = 0;
  int rightCounter = 0;

  for(int i = -numberOfSectorsOfGap / 2; i < numOfSectors / 2; i++)
  {
    int leftSector = (startSector+i)%numOfSectors;
//    ASSERT(leftSector < numOfSectors && leftSector >=0);
    if(distance[leftSector] > minDistance)
      leftCounter++;
    else leftCounter = 0;
    
    int rightSector = (startSector-i)%numOfSectors;
//    ASSERT(rightSector < numOfSectors && rightSector >=0);
    if(distance[rightSector] > minDistance)
      rightCounter++;
    else rightCounter = 0;

    if(leftCounter == numberOfSectorsOfGap)
    {
      direction = getAngleOfSector((startSector+i-numberOfSectorsOfGap/2)%numOfSectors);
      break;
    }

    if(rightCounter == numberOfSectorsOfGap)
    {
      direction = getAngleOfSector((startSector-i+numberOfSectorsOfGap/2)%numOfSectors);
      break;
    }
  }
  /*
  LINE(models_corridorsRadar, 
    0, 0, 
    cos(direction) * 1000,
    sin(direction) * 1000,
    20, Drawings::ps_solid, Drawings::red
    );

  LINE(models_corridorsRadar, 
    0, 0, 
    cos(angle) * 1000,
    sin(angle) * 1000,
    15, Drawings::ps_dash, Drawings::blue
    );

  CIRCLE(models_corridorsRadar, 
    0, 0, minDistance,
    3, Drawings::ps_solid, Drawings::blue);
*/
  return direction;
}


int ObstaclesModel::getMinimalDistanceInRange
(
 double centerAngle, 
 double openingAngle, 
 double& angleWithMinimalDistance
 ) const
{
  int centerSector = getSectorFromAngle(centerAngle); 
  int endSector = getSectorFromAngle(centerAngle + openingAngle / 2.0);

  int minDistance = 10000;
  int sectorWithMinimalDistance = centerSector;

  for(int i = 0; centerSector + i < endSector; i++)
  {
    if(distance[centerSector + i] < minDistance) 
    {
      minDistance = distance[centerSector + i];
      sectorWithMinimalDistance = centerSector + i;
    }
    if(distance[centerSector - i] < minDistance) 
    {
      minDistance = distance[centerSector - i];
      sectorWithMinimalDistance = centerSector - i;
    }
  }
  angleWithMinimalDistance = getAngleOfSector(sectorWithMinimalDistance);
  return minDistance;
}

int ObstaclesModel::getMinimalDistanceInRange
(
 double centerAngle, 
 double openingAngle, 
 double& angleWithMinimalDistance, 
 ObstaclesPercept::ObstacleType obstacleType
 ) const
{
  int centerSector = getSectorFromAngle(centerAngle); 
  int endSector = getSectorFromAngle(centerAngle + openingAngle / 2.0);

  int minDistance = 10000;
  int sectorWithMinimalDistance = centerSector;

  for(int i = 0; centerSector + i < endSector; i++)
  {
    if(this->obstacleType[centerSector + i] == obstacleType && distance[centerSector + i] < minDistance) 
    {
      minDistance = distance[centerSector + i];
      sectorWithMinimalDistance = centerSector + i;
    }
    if(this->obstacleType[centerSector - i] == obstacleType && distance[centerSector - i] < minDistance) 
    {
      minDistance = distance[centerSector - i];
      sectorWithMinimalDistance = centerSector - i;
    }
  }
  angleWithMinimalDistance = getAngleOfSector(sectorWithMinimalDistance);
  return minDistance;
}

double ObstaclesModel::getPercentageOfLowDistanceObstaclesInRange
(
 double centerAngle, 
 double openingAngle, 
 int maxDistance
 ) const
{
  int beginSector = getSectorFromAngle(centerAngle - openingAngle / 2.0);
  int endSector = getSectorFromAngle(centerAngle + openingAngle / 2.0);
  
  int numberOfSectorsWithCloseObstacle = 0;

  for(int i = beginSector; i <= endSector; i++)
  {
    if(distance[i] < maxDistance) 
    {
      numberOfSectorsWithCloseObstacle++;
    }
  }
  return (double)(numberOfSectorsWithCloseObstacle) / (double)(endSector - beginSector + 1);
}


double ObstaclesModel::getAngleOfLargeGapInRange(double centerAngle, double openingAngle, SearchDirections searchDirection) const
{
  int centerSector = getSectorFromAngle(centerAngle); 
  int endSectorLeft = getSectorFromAngle(centerAngle + openingAngle / 2.0);

  int numberOfSectorsOnOneSide = endSectorLeft - centerSector + 1;

  int bestSector;
  //int sumOfDistances = 0;
  int maxSumOfDistances = 0;
  enum {numberOfSectorsOfGap = 7};

  RingBufferWithSum<numberOfSectorsOfGap> bufferMovingRight;
  RingBufferWithSum<numberOfSectorsOfGap> bufferMovingLeft;

  int i;
  for(i = 0; i < numberOfSectorsOfGap; i++)
  {
    bufferMovingLeft.add(min(1000, distance[i + centerSector - numberOfSectorsOfGap / 2]) );
    bufferMovingRight.add(min(1000, distance[-i + centerSector + numberOfSectorsOfGap / 2]) );
  }
  maxSumOfDistances = bufferMovingLeft.getSum();
  bestSector = centerSector;

  for(i = numberOfSectorsOfGap / 2 + 1; i < numberOfSectorsOnOneSide; i++)
  {
    bufferMovingLeft.add(min(1000, distance[centerSector + i]) );
    bufferMovingRight.add(min(1000, distance[centerSector - i]) );

    int newSum;
    int newSector;
    if(
      (
      bufferMovingLeft.getSum() > bufferMovingRight.getSum() && 
      searchDirection != searchRight)
      ||
      searchDirection == searchLeft
      )
    {
      newSum = bufferMovingLeft.getSum();
      newSector = centerSector + i - (numberOfSectorsOfGap / 2);
    }
    else
    {
      newSum = bufferMovingRight.getSum();
      newSector = centerSector - i + (numberOfSectorsOfGap / 2);
    }

    if(newSum > maxSumOfDistances + 80 * numberOfSectorsOfGap)
    {
      maxSumOfDistances = newSum;
      bestSector = newSector;
    }
  }
  return getAngleOfSector(bestSector);
}

double ObstaclesModel::getAngleOfLargeGapInRange2(double centerAngle, double openingAngle, SearchDirections searchDirection) const
{
  int centerSector = getSectorFromAngle(centerAngle); 
  int endSectorLeft = getSectorFromAngle(centerAngle + openingAngle / 2.0);

  int numberOfSectorsOnOneSide = endSectorLeft - centerSector + 1;

  int bestSector;
  //int sumOfDistances = 0;
  int maxMinOfDistances = 0;
  enum {numberOfSectorsOfGap = 7};

  RingBufferWithSum<numberOfSectorsOfGap> bufferMovingRight;
  RingBufferWithSum<numberOfSectorsOfGap> bufferMovingLeft;

  int i;
  for(i = 0; i < numberOfSectorsOfGap; i++)
  {
    bufferMovingLeft.add(min(1000, distance[i + centerSector - numberOfSectorsOfGap / 2]) );
    bufferMovingRight.add(min(1000, distance[-i + centerSector + numberOfSectorsOfGap / 2]) );
  }
  maxMinOfDistances = bufferMovingLeft.getMinimum();
  bestSector = centerSector;

  for(i = numberOfSectorsOfGap / 2 + 1; i < numberOfSectorsOnOneSide; i++)
  {
    bufferMovingLeft.add(min(1000, distance[centerSector + i]) );
    bufferMovingRight.add(min(1000, distance[centerSector - i]) );

    int newMin;
    int newSector;
    if(
      (
      bufferMovingLeft.getSum() > bufferMovingRight.getSum() && 
      searchDirection != searchRight)
      ||
      searchDirection == searchLeft
      )
    {
      newMin = bufferMovingLeft.getMinimum();
      newSector = centerSector + i - (numberOfSectorsOfGap / 2);
    }
    else
    {
      newMin = bufferMovingRight.getMinimum();
      newSector = centerSector - i + (numberOfSectorsOfGap / 2);
    }

    if(newMin > maxMinOfDistances + 80)
    {
      maxMinOfDistances = newMin;
      bestSector = newSector;
    }
  }
  return getAngleOfSector(bestSector);
}


void ObstaclesModel::operator=(const ObstaclesModel& other)
{
  for(int i = 0; i < numOfSectors; i++)
  {
    distance[i] = other.distance[i];
    obstacleType[i] = other.obstacleType[i];
  }
  corridorInFront = other.corridorInFront;
}

In& operator>>(In& stream,ObstaclesModel& obstaclesModel)
{
  stream >> obstaclesModel.frameNumber;
  stream.read(&obstaclesModel,sizeof(ObstaclesModel));
  return stream;
}

Out& operator<<(Out& stream, const ObstaclesModel& obstaclesModel)
{
  stream << obstaclesModel.frameNumber;
  stream.write(&obstaclesModel,sizeof(ObstaclesModel));
  return stream;
}

---