Modules/PlayersLocator/VAPoints.cpp

Go to the documentation of this file.

/**
 *  @file VAPoints.cpp
 *
 *  Represents a set of points on the field.
 *
 *  @author <A href=mailto:mkunz@sim.tu-darmstadt.de.de>Michael Kunz</A>
 */ 


#include "VAPoints.h"

#include "Tools/Debugging/Debugging.h"

/*const double GAUSS0 = 1.0, GAUSS1 = .85, GAUSS2 = .51, GAUSS3 = .22, GAUSS4 = .72,
             GAUSS5 = .43, GAUSS6 = .19, GAUSS7 = .26, GAUSS8 = .11;*/

const double GAUSS0 = 1.0, GAUSS1 = .63, GAUSS2 = .16, GAUSS4 = .40, GAUSS5 = .10;


VAPoints::VAPoints(int numberOfVAPoints, int numberOfMaxima)
{
  this->numberOfVAPoints = numberOfVAPoints;
  this->numberOfMaxima = numberOfMaxima;

  points = new VAPoint[numberOfVAPoints];
  maxima = new Maximum[numberOfMaxima];
  
  for(int i = 0; i < numberOfVAPoints; i++)
  {
    points[i].relevance = 0;
  }
  
  int gridX = 0, gridY = 0;
  
  for (gridX = 0; gridX < NUMBER_OF_GRID_POINTS_X; gridX++)
  {
    for (gridY = 0; gridY < NUMBER_OF_GRID_POINTS_Y; gridY++)
    {
      field[gridX][gridY] = 0;    
    }
  }
  
  timeOfLastMaximaSearch = SystemCall::getCurrentSystemTime();
}

VAPoints::~VAPoints()
{
  delete points;
  delete maxima;
}

void VAPoints::addPoint(int x, int y, double validity, unsigned long timestamp)
{
  double relevance;
  double age = SystemCall::getTimeSince(timestamp);
  if (age < validity * FADE_OUT_TIME2)
  {
    relevance = validity - (age / FADE_OUT_TIME2);
  }
  else
  {
    relevance = 0;
  }

  updateGridByPoint(x, y, relevance);
  
  /*  int insertPos = findInsertPos();
  points[insertPos].x = x;
  points[insertPos].y = y;
  points[insertPos].validity = validity;
  points[insertPos].timestamp = timestamp;
  points[insertPos].updateRelevance();*/
}

int VAPoints::findInsertPos()
{
  double minRelevance = 10;
  int insertPos = 0;
  double currRelevance;
  for(int i = 0; i < numberOfVAPoints; i++)
  {
    currRelevance = points[i].getRelevance();
    if (currRelevance < minRelevance)
    {
      minRelevance = currRelevance;
      insertPos = i;
      if (minRelevance == 0) break;
    }
  }
  return insertPos;
}

void VAPoints::searchMaxima()
{
  int gridX = 0, gridY = 0, maximum1 = 0, maximum2 = 0, temp = 0;
  bool isNear = false;

  for (int i=0; i < this->numberOfMaxima; i++)
  {
    maxima[i].x = -10;
    maxima[i].y = -10;
    maxima[i].height = 0;
  }
  
  double timeSinceLastSearch = SystemCall::getTimeSince(timeOfLastMaximaSearch);
  timeOfLastMaximaSearch = SystemCall::getCurrentSystemTime();

  double decrement = MIN_HEIGHT_FOR_MAX_VALIDITY * (timeSinceLastSearch / FADE_OUT_TIME2);

  for (gridX = 0; gridX < NUMBER_OF_GRID_POINTS_X; gridX++)
  {
    for (gridY = 0; gridY < NUMBER_OF_GRID_POINTS_Y; gridY++)
    {
      if (field[gridX][gridY] > MIN_HEIGHT_FOR_MAX_VALIDITY)
      {
        field[gridX][gridY] -= 2 * decrement;
      }
      else
      {
        if (field[gridX][gridY] > decrement)
        {
          field[gridX][gridY] -= decrement;
        }
        else
        {
          field[gridX][gridY] = 0;
        }
      }
    }
  }

  
  //pointsToField();
  
  for (gridX = 0; gridX < NUMBER_OF_GRID_POINTS_X; gridX++)
  {
    for (gridY = 0; gridY < NUMBER_OF_GRID_POINTS_Y; gridY++)
    {
      for (maximum1 = 0; maximum1 < this->numberOfMaxima; maximum1++)
      {
        if (field[gridX][gridY] > maxima[maximum1].height) if (isLokMax(gridX, gridY))
        {
          isNear = false;
          for (maximum2 = 0; maximum2 < this->numberOfMaxima; maximum2++)
          {
            if ((abs(gridX - maxima[maximum2].x) <= 2) && (abs(gridY - maxima[maximum2].y) <= 2))
            {
              isNear = true;
              if (field[gridX][gridY] > maxima[maximum2].height)
              {
                for (temp = maximum2; temp > maximum1; temp--)
                {
                  maxima[temp] = maxima[temp - 1];
                }
                maxima[maximum1].x = gridX;
                maxima[maximum1].y = gridY;
                maxima[maximum1].height = field[gridX][gridY];
              }
              break;
            }
          }
          
          if (isNear) break;

          for (temp = this->numberOfMaxima - 1; temp > maximum1; temp--)
          {
            maxima[temp] = maxima[temp - 1];
          }
          maxima[maximum1].x = gridX;
          maxima[maximum1].y = gridY;
          maxima[maximum1].height = field[gridX][gridY];
          break;
        }
      }
    }
  }  
}

bool VAPoints::getMaximum(int index, int& x_position, int& y_position, double& validity)
{
  if((index < 0) || (index > this->numberOfMaxima) || (maxima[index].height == 0)) return false;
  
  int fieldX = 0, fieldY = 0;
  getFieldPoints(maxima[index].x, maxima[index].y, fieldX, fieldY);
  
  x_position = fieldX;
  y_position = fieldY;

  if (maxima[index].height > MIN_HEIGHT_FOR_MAX_VALIDITY)
    validity = 1.0;
  else
    validity = maxima[index].height / MIN_HEIGHT_FOR_MAX_VALIDITY;

/*  if (index == 0)
  {
    OUTPUT(idText,text, "Gipfelhöhe:" << maxima[index].height << "validity: " << validity);
  }*/

  return true;
}

void VAPoints::pointsToField()
{

  int gridX = 0, gridY = 0;

  for (gridX = 0; gridX < NUMBER_OF_GRID_POINTS_X; gridX++)
  {
    for (gridY = 0; gridY < NUMBER_OF_GRID_POINTS_Y; gridY++)
    {
      field[gridX][gridY] = 0;    
    }
  }
  
  for (int i = 0; i < numberOfVAPoints; i++)
  {
    updateGridByPoint(points[i].x, points[i].y,points[i].getRelevance());
  }
}

void VAPoints::updateGridByPoint(int xPos, int yPos, double relevance)
{
  int gridX = 0, gridY = 0;
  
  if (relevance > 0)
  {
    getGridIndices(xPos, yPos, gridX, gridY);
    
    /*incrementGridPoint(gridX-2, gridY-3, GAUSS8 * relevance);
    incrementGridPoint(gridX-1, gridY-3, GAUSS6 * relevance);
    incrementGridPoint(gridX, gridY-3, GAUSS3 * relevance);
    incrementGridPoint(gridX+1, gridY-3, GAUSS6 * relevance);
    incrementGridPoint(gridX+2, gridY-3, GAUSS8 * relevance);
    
    incrementGridPoint(gridX-3, gridY-2, GAUSS8 * relevance);
    incrementGridPoint(gridX-2, gridY-2, GAUSS7 * relevance);
    incrementGridPoint(gridX-1, gridY-2, GAUSS5 * relevance);
    incrementGridPoint(gridX, gridY-2, GAUSS2 * relevance);
    incrementGridPoint(gridX+1, gridY-2, GAUSS5 * relevance);
    incrementGridPoint(gridX+2, gridY-2, GAUSS7 * relevance);
    incrementGridPoint(gridX+3, gridY-2, GAUSS8 * relevance);
    
    incrementGridPoint(gridX-3, gridY-1, GAUSS6 * relevance);
    incrementGridPoint(gridX-2, gridY-1, GAUSS5 * relevance);
    incrementGridPoint(gridX-1, gridY-1, GAUSS4 * relevance);
    incrementGridPoint(gridX, gridY-1, GAUSS1 * relevance);
    incrementGridPoint(gridX+1, gridY-1, GAUSS4 * relevance);
    incrementGridPoint(gridX+2, gridY-1, GAUSS5 * relevance);
    incrementGridPoint(gridX+3, gridY-1, GAUSS6 * relevance);
    
    incrementGridPoint(gridX-3, gridY, GAUSS3 * relevance);
    incrementGridPoint(gridX-2, gridY, GAUSS2 * relevance);
    incrementGridPoint(gridX-1, gridY, GAUSS1 * relevance);
    incrementGridPoint(gridX, gridY, GAUSS0 * relevance);
    incrementGridPoint(gridX+1, gridY, GAUSS1 * relevance);
    incrementGridPoint(gridX+2, gridY, GAUSS2 * relevance);
    incrementGridPoint(gridX+3, gridY, GAUSS3 * relevance);
    
    incrementGridPoint(gridX-3, gridY+1, GAUSS6 * relevance);
    incrementGridPoint(gridX-2, gridY+1, GAUSS5 * relevance);
    incrementGridPoint(gridX-1, gridY+1, GAUSS4 * relevance);
    incrementGridPoint(gridX, gridY+1, GAUSS1 * relevance);
    incrementGridPoint(gridX+1, gridY+1, GAUSS4 * relevance);
    incrementGridPoint(gridX+2, gridY+1, GAUSS5 * relevance);
    incrementGridPoint(gridX+3, gridY+1, GAUSS6 * relevance);
    
    incrementGridPoint(gridX-3, gridY+2, GAUSS8 * relevance);
    incrementGridPoint(gridX-2, gridY+2, GAUSS7 * relevance);
    incrementGridPoint(gridX-1, gridY+2, GAUSS5 * relevance);
    incrementGridPoint(gridX, gridY+2, GAUSS2 * relevance);
    incrementGridPoint(gridX+1, gridY+2, GAUSS5 * relevance);
    incrementGridPoint(gridX+2, gridY+2, GAUSS7 * relevance);
    incrementGridPoint(gridX+3, gridY+2, GAUSS8 * relevance);
    
    incrementGridPoint(gridX-2, gridY+3, GAUSS8 * relevance);
    incrementGridPoint(gridX-1, gridY+3, GAUSS6 * relevance);
    incrementGridPoint(gridX, gridY+3, GAUSS3 * relevance);
    incrementGridPoint(gridX+1, gridY+3, GAUSS6 * relevance);
    incrementGridPoint(gridX+2, gridY+3, GAUSS8 * relevance);*/   
    
    
    incrementGridPoint(gridX-1, gridY-2, GAUSS5 * relevance);
    incrementGridPoint(gridX, gridY-2, GAUSS2 * relevance);
    incrementGridPoint(gridX+1, gridY-2, GAUSS5 * relevance);
    
    incrementGridPoint(gridX-2, gridY-1, GAUSS5 * relevance);
    incrementGridPoint(gridX-1, gridY-1, GAUSS4 * relevance);
    incrementGridPoint(gridX, gridY-1, GAUSS1 * relevance);
    incrementGridPoint(gridX+1, gridY-1, GAUSS4 * relevance);
    incrementGridPoint(gridX+2, gridY-1, GAUSS5 * relevance);
    
    incrementGridPoint(gridX-2, gridY, GAUSS2 * relevance);
    incrementGridPoint(gridX-1, gridY, GAUSS1 * relevance);
    incrementGridPoint(gridX, gridY, GAUSS0 * relevance);
    incrementGridPoint(gridX+1, gridY, GAUSS1 * relevance);
    incrementGridPoint(gridX+2, gridY, GAUSS2 * relevance);
    
    incrementGridPoint(gridX-2, gridY+1, GAUSS5 * relevance);
    incrementGridPoint(gridX-1, gridY+1, GAUSS4 * relevance);
    incrementGridPoint(gridX, gridY+1, GAUSS1 * relevance);
    incrementGridPoint(gridX+1, gridY+1, GAUSS4 * relevance);
    incrementGridPoint(gridX+2, gridY+1, GAUSS5 * relevance);
    
    incrementGridPoint(gridX-1, gridY+2, GAUSS5 * relevance);
    incrementGridPoint(gridX, gridY+2, GAUSS2 * relevance);
    incrementGridPoint(gridX+1, gridY+2, GAUSS5 * relevance);
  }
}

void VAPoints::incrementGridPoint(int xIndex, int yIndex, double increment)
{
  if ((xIndex >= 1) && (xIndex <= NUMBER_OF_GRID_POINTS_X - 1) && (yIndex >= 1) && (yIndex <= NUMBER_OF_GRID_POINTS_Y - 1))
  {
    double fieldHeight = field[xIndex][yIndex];
    double realHeight = (MIN_HEIGHT_FOR_MAX_VALIDITY * fieldHeight) / ((2 * MIN_HEIGHT_FOR_MAX_VALIDITY) - fieldHeight);

    field[xIndex][yIndex] = (2 * MIN_HEIGHT_FOR_MAX_VALIDITY) - ((2 * MIN_HEIGHT_FOR_MAX_VALIDITY * MIN_HEIGHT_FOR_MAX_VALIDITY) / (realHeight + increment +  MIN_HEIGHT_FOR_MAX_VALIDITY));
    //field[xIndex][yIndex] += increment;
  }
}

void VAPoints::getGridIndices(int xPos, int yPos, int& xIndex, int& yIndex)
{
  xIndex = (int)((xPos - xPosBackFlags) / GRID_SPACING);
  
  if(xIndex < 1)
    xIndex = 1;
  if(xIndex >= NUMBER_OF_GRID_POINTS_X - 1)
    xIndex = NUMBER_OF_GRID_POINTS_X - 1;
  
  yIndex = (int)((yPos - yPosRightFlags) / GRID_SPACING);
  
  if(yIndex < 1)
    yIndex = 1;
  if(yIndex >= NUMBER_OF_GRID_POINTS_Y - 1)
    yIndex = NUMBER_OF_GRID_POINTS_Y - 1;  
}

void VAPoints::getFieldPoints(int xIndex, int yIndex, int& xPos, int& yPos)
{
  xPos = xIndex * GRID_SPACING + xPosBackFlags;
  yPos = yIndex * GRID_SPACING + yPosRightFlags;
}

bool VAPoints::isLokMax(int xIndex, int yIndex)
{
  bool result = true;

  for (int gridX = xIndex-1; gridX <= xIndex+1; gridX++)
  {
    for (int gridY = yIndex-1; gridY <= yIndex+1; gridY++)
    {
      if ((gridX == xIndex) && (gridY == yIndex)) break;
      if ((xIndex >= 2) && (xIndex <= NUMBER_OF_GRID_POINTS_X - 2) && (yIndex >= 2) && (yIndex <= NUMBER_OF_GRID_POINTS_Y - 2))
        result &= (field[xIndex][yIndex] > field[gridX][gridY]);
    }
  }
  return result;
}


// Streaming operators
In& operator>>(In& stream,VAPoints& VAPoints)
{
  stream >> VAPoints.numberOfVAPoints;
  stream.read(&VAPoints.points,sizeof(VAPoints.points));
  return stream;
}

Out& operator<<(Out& stream, const VAPoints& vaPoints)
{
  stream << vaPoints.numberOfVAPoints;
  stream.write(&vaPoints.points, sizeof(vaPoints.points));
  return stream;
}


/*
 *
 * $Log: VAPoints.cpp,v $
 * Revision 1.1.1.1  2004/05/22 17:20:40  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.1  2003/10/06 14:10:15  cvsadm
 * Created GT2004 (M.J.)
 *
 * Revision 1.1  2003/09/26 11:38:52  juengel
 * - sorted tools
 * - clean-up in DataTypes
 *
 * 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.10  2003/05/27 09:05:50  mkunz
 * more flexible number of maxima
 * restricted height in field
 * some performance hacks
 *
 * Revision 1.9  2003/05/20 12:39:08  mkunz
 * debug messages removed
 *
 * Revision 1.8  2003/05/16 14:49:26  mkunz
 * enlarged gridsize
 *
 * Revision 1.7  2003/05/14 13:21:09  mkunz
 * flexible array size
 *
 * Revision 1.6  2003/05/12 10:29:37  dueffert
 * == bug fixed
 *
 * Revision 1.5  2003/05/09 09:54:27  mkunz
 * some finetuning
 *
 * Revision 1.4  2003/05/08 19:49:00  mkunz
 * first runnig version
 *
 * Revision 1.3  2003/05/08 18:19:39  mkunz
 * little dirty workaround-bugfix
 *
 * Revision 1.2  2003/05/08 18:05:35  mkunz
 * methods completed
 *
 * Revision 1.1  2003/05/08 16:49:39  mkunz
 * class VAPoints added.
 * similar to PointsWithValidityAndAge but with validity and age
 *
 */

---