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"

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

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 = maxDistance;
  int endSector = getSectorFromAngle(direction + openingAngle);

  for (
    sector = getSectorFromAngle(direction - openingAngle);
    sector != endSector;
    sector = (sector + 1) % numOfSectors)
    if (distance[sector] < minDistance)
      minDistance = distance[sector];

  return minDistance;
}


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;
}

/*
 * Change log :
 * 
 * $Log: ObstaclesModel.cpp,v $
 * Revision 1.2  2004/06/16 14:43:51  risler
 * added obstacles symbols
 * added symbol obstacles.opponent-close-to-ball
 *
 * Revision 1.1.1.1  2004/05/22 17:25:15  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.5  2004/03/16 14:00:21  juengel
 * Integrated Improvments from "Günne"
 * -ATH2004ERS7Behavior
 * -ATHHeadControl
 * -KickSelectionTable
 * -KickEditor
 *
 * Revision 1.4  2004/03/10 14:16:33  risler
 * body psd value added to PSDPercept and ObstaclesModel
 *
 * Revision 1.2  2004/03/15 17:11:40  hoffmann
 * - added ATH2004HeadControl
 * - added ATH2004LEDControl
 * - headmotiontester shows "tilt2"
 * - motion process updates odometry while no new robotPose is received, added to motion request
 * - some ui adjustments
 * - added member function to "field" to find out if robot is in own penalty area for use in the obstacles locator
 *
 * Revision 1.3  2004/02/28 13:57:34  juengel
 * Added ObstacleType.
 *
 * Revision 1.2  2003/11/14 19:02:25  goehring
 * frameNumber added
 *
 * Revision 1.1  2003/10/07 10:07:01  cvsadm
 * Created GT2004 (M.J.)
 *
 * Revision 1.3  2003/09/01 10:23:14  juengel
 * DebugDrawings clean-up 2
 * DebugImages clean-up
 * MessageIDs clean-up
 * Stopwatch clean-up
 *
 * Revision 1.2  2003/07/03 18:13:35  dassler
 * Adding two Methods:
 * getAngleOfNextFreeSectorLeft
 * getAngleOfNextFreeSectorRight
 *
 * Copy of getAngleOfNextFreeSector
 *
 * Revision 1.1.1.1  2003/07/02 09:40:22  cvsadm
 * created new repository for the competitions in Padova from the 
 * tamara CVS (Tuesday 2:00 pm)
 *
 * removed unused solutions
 *
 * Revision 1.20  2003/06/27 15:10:15  juengel
 * Added getAngleOfLargeGapInRange2 which uses minimum instead of sum.
 *
 * Revision 1.19  2003/06/27 13:17:57  jhoffman
 * work on obstacle avoider challenge,
 * added headcontrolmode,
 * added method needed to determine empty space,
 * updatet drawing method to reflect actual corridor size
 *
 * Revision 1.18  2003/06/26 12:35:25  juengel
 * getAngleOfLargeGapInRange has search direction.
 *
 * Revision 1.17  2003/06/21 15:26:01  juengel
 * Fixed bug in getPercentageOfLowDistanceObstaclesInRange.
 *
 * Revision 1.16  2003/06/21 12:56:26  juengel
 * Added getPercentageOfLowDistanceObstaclesInRange.
 *
 * Revision 1.15  2003/06/21 10:13:59  dueffert
 * warnings removed
 *
 * Revision 1.14  2003/06/20 20:13:01  juengel
 * Renamed some methods.
 *
 * Revision 1.13  2003/06/20 13:59:17  juengel
 * Added minimalDistanceInRange.
 *
 * Revision 1.12  2003/06/19 19:51:56  juengel
 * Renamed some variables.
 *
 * Revision 1.11  2003/06/19 12:10:43  juengel
 * Added getBestAngle
 *
 * Revision 1.10  2003/06/18 18:31:15  juengel
 * DebugDrawings removed.
 *
 * Revision 1.9  2003/06/17 20:01:22  juengel
 * Renamed getNextFreeSector to getAngleOfNextFreeSector.
 *
 * Revision 1.8  2003/06/05 08:33:27  juengel
 * warning removed
 *
 * Revision 1.7  2003/06/05 08:14:28  juengel
 * Added getDistanceInCorridor and getNextFreeSector.
 *
 * Revision 1.6  2003/06/03 15:50:04  risler
 * added getDistanceInDirection
 * minor optimizations
 *
 * Revision 1.5  2003/05/28 17:46:48  loetzsch
 * some initialitzations
 *
 * Revision 1.4  2003/05/14 13:08:38  risler
 * removed DefaultObstaclesLocator
 * renamed MicroSectorsObstaclesLocator to DefaultObstaclesLocator
 * ObstaclesModel contains increased number of sectors
 * DefaultObstaclesLocator clean up
 *
 * Revision 1.3  2003/04/14 16:01:41  loetzsch
 * ATH after GermanOpen CVS merge:
 * added corridor to obstacle model
 *
 * Revision 1.2  2003/04/10 14:14:47  Jan Hoffmann
 * added corridor to obstacle model
 *
 * Revision 1.1.1.1  2003/04/09 14:22:16  loetzsch
 * started Aibo Team Humboldt's GermanOpen CVS
 *
 * Revision 1.2  2003/03/11 11:29:20  juengel
 * Added data and constructor
 *
 * Revision 1.1  2003/03/10 13:52:30  juengel
 * Added ObstaclesModel
 *
 */

---