Modules/SelfLocator/SlamSelfLocator/SlamLineCrossingsTable.cpp

Go to the documentation of this file.

#include "SlamLineCrossingsTable.h"

int SlamLineCrossingsTable::NO_POINT_DISTANCE   = 100000;

double SlamLineCrossingsTable::crossingsX[] = { 
    xPosOwnGroundline,
    xPosOwnGroundline,
    xPosOwnGroundline,
    xPosOwnGroundline,

    xPosOwnPenaltyArea,
    xPosOwnPenaltyArea,
    xPosOwnPenaltyArea,
    xPosOwnPenaltyArea,

    xPosHalfWayLine,
    xPosHalfWayLine,
    xPosHalfWayLine,
    xPosHalfWayLine,

    xPosOpponentPenaltyArea,
    xPosOpponentPenaltyArea,
    xPosOpponentPenaltyArea,
    xPosOpponentPenaltyArea,

    xPosOpponentGroundline,
    xPosOpponentGroundline,
    xPosOpponentGroundline,
    xPosOpponentGroundline

    //xPosHalfWayLine         // for middle circle
  };
double SlamLineCrossingsTable::crossingsY[]={
    yPosLeftGroundline,
    yPosLeftPenaltyArea,
    yPosRightPenaltyArea,
    yPosRightGroundline,

    yPosLeftGroundline,
    yPosLeftPenaltyArea,
    yPosRightPenaltyArea,
    yPosRightGroundline,

    yPosLeftGroundline,
    yPosLeftPenaltyArea,
    yPosRightPenaltyArea,
    yPosRightGroundline,

    yPosLeftGroundline,
    yPosLeftPenaltyArea,
    yPosRightPenaltyArea,
    yPosRightGroundline,

    yPosLeftGroundline,
    yPosLeftPenaltyArea,
    yPosRightPenaltyArea,
    yPosRightGroundline

    // yPosCenterGoal        for middle circle
  };

SlamLineCrossingsTable::SlamLineCrossingsTable()
{
  initAllCrossings();
  initVirtualCrossings();
  initTCrossings();
  initLCrossings();
  initFalseCrossings();
}


Vector2<double> SlamLineCrossingsTable::getClosestPoint(const Vector2<double>& point, CrossingType type)
{
  Vector2<double> minimum(NO_POINT_DISTANCE,NO_POINT_DISTANCE);
  double minDist = NO_POINT_DISTANCE;

  for(int i = 0; i < NUMBER_OF_CROSSINGS; i++)
  {
    Vector2<double> crossing(crossingsX[i],crossingsY[i]);
    Vector2<double> diff = point - crossing;
    if (diff.abs()<minDist)
    {
      minDist = diff.abs();
      minimum = crossing;
    }
  }
  return minimum;
}

Vector2<double> SlamLineCrossingsTable::getClassifiedClosestPoint(const Vector2<double>& point, CrossingClass crossingClass)
{
  int number = 0;
  Vector2<double>* crossingsArray;
  Vector2<double> minimum(NO_POINT_DISTANCE,NO_POINT_DISTANCE);
  double minDist = NO_POINT_DISTANCE;

  switch(crossingClass)
  {
    case unknownCrossing:
      number = NUMBER_OF_CLASSIFIED_CROSSINGS;
      crossingsArray = allCrossings;
      break;
    case virtualCrossing:
      number = NUMBER_OF_VIRTUAL_CROSSINGS;
      crossingsArray = virtualCrossings;
      break;
    case lCrossing:
      number = NUMBER_OF_L_CROSSINGS;
      crossingsArray = lCrossings;
      break;
    case tCrossing:
      number = NUMBER_OF_T_CROSSINGS;
      crossingsArray = tCrossings;
      break;
    case falseCrossing:
      number = NUMBER_OF_FALSE_CROSSINGS;
      crossingsArray = falseCrossings;
      break;
    default:
      number = NUMBER_OF_CLASSIFIED_CROSSINGS;
      crossingsArray = allCrossings;
  };

  for(int i = 0; i < number; i++)
  {
    Vector2<double> diff = point - crossingsArray[i];
    if (diff.abs()<minDist)
    {
      minDist = diff.abs();
      minimum = crossingsArray[i];
    }
  }
  return minimum;

}

void SlamLineCrossingsTable::initAllCrossings()
{
  allCrossings[0].x = xPosOwnGroundline;
  allCrossings[0].y = yPosLeftGroundline;

  allCrossings[1].x = xPosOwnGroundline;
  allCrossings[1].y = yPosLeftPenaltyArea;

  allCrossings[2].x = xPosOwnGroundline;
  allCrossings[2].y = yPosRightPenaltyArea;

  allCrossings[3].x = xPosOwnGroundline;
  allCrossings[3].y = yPosRightGroundline;

  allCrossings[4].x = xPosOwnPenaltyArea;
  allCrossings[4].y = yPosLeftGroundline;

  allCrossings[5].x = xPosOwnPenaltyArea;
  allCrossings[5].y = yPosLeftPenaltyArea;

  allCrossings[6].x = xPosOwnPenaltyArea;
  allCrossings[6].y = yPosRightPenaltyArea;

  allCrossings[7].x = xPosOwnPenaltyArea;
  allCrossings[7].y = yPosRightGroundline;

  allCrossings[8].x = xPosHalfWayLine;
  allCrossings[8].y = yPosLeftGroundline;

  // allCrossings[9].x = xPosHalfWayLine;
  // allCrossings[9].y = yPosLeftPenaltyArea;

  // allCrossings[10].x = xPosHalfWayLine;
  // allCrossings[10].y = yPosRightPenaltyArea;

  allCrossings[9].x = xPosHalfWayLine;
  allCrossings[9].y = yPosRightGroundline;

  allCrossings[10].x = xPosOpponentPenaltyArea;
  allCrossings[10].y = yPosLeftGroundline;

  allCrossings[11].x = xPosOpponentPenaltyArea;
  allCrossings[11].y = yPosLeftPenaltyArea;

  allCrossings[12].x = xPosOpponentPenaltyArea;
  allCrossings[12].y = yPosRightPenaltyArea;

  allCrossings[13].x = xPosOpponentPenaltyArea;
  allCrossings[13].y = yPosRightGroundline;

  allCrossings[14].x = xPosOpponentGroundline;
  allCrossings[14].y = yPosLeftGroundline;

  allCrossings[15].x = xPosOpponentGroundline;
  allCrossings[15].y = yPosLeftPenaltyArea;

  allCrossings[16].x = xPosOpponentGroundline;
  allCrossings[16].y = yPosRightPenaltyArea;

  allCrossings[17].x = xPosOpponentGroundline;
  allCrossings[17].y = yPosRightGroundline;
}

void SlamLineCrossingsTable::initVirtualCrossings()
{
  virtualCrossings[0].x = xPosOwnPenaltyArea;
  virtualCrossings[0].y = yPosLeftGroundline;

  virtualCrossings[1].x = xPosOwnPenaltyArea;
  virtualCrossings[1].y = yPosRightGroundline;

//   virtualCrossings[2].x = xPosHalfWayLine;
//  virtualCrossings[2].y = yPosLeftPenaltyArea;

//  virtualCrossings[3].x = xPosHalfWayLine;
//  virtualCrossings[3].y = yPosRightPenaltyArea;

  virtualCrossings[2].x = xPosOpponentPenaltyArea;
  virtualCrossings[2].y = yPosLeftGroundline;

  virtualCrossings[3].x = xPosOpponentPenaltyArea;
  virtualCrossings[3].y = yPosRightGroundline;
}

void SlamLineCrossingsTable::initTCrossings()
{
  tCrossings[0].x = xPosOwnGroundline;
  tCrossings[0].y = yPosLeftPenaltyArea;

  tCrossings[1].x = xPosOwnGroundline;
  tCrossings[1].y = yPosRightPenaltyArea;

  tCrossings[2].x = xPosHalfWayLine;
  tCrossings[2].y = yPosLeftGroundline;

  tCrossings[3].x = xPosHalfWayLine;
  tCrossings[3].y = yPosRightGroundline;

  tCrossings[4].x = xPosOpponentGroundline;
  tCrossings[4].y = yPosLeftPenaltyArea;

  tCrossings[5].x = xPosOpponentGroundline;
  tCrossings[5].y = yPosRightPenaltyArea;
}

void SlamLineCrossingsTable::initLCrossings()
{
  lCrossings[0].x = xPosOwnGroundline;
  lCrossings[0].y = yPosLeftGroundline;

  lCrossings[1].x = xPosOwnGroundline;
  lCrossings[1].y = yPosRightGroundline;

  lCrossings[2].x = xPosOwnPenaltyArea;
  lCrossings[2].y = yPosLeftPenaltyArea;

  lCrossings[3].x = xPosOwnPenaltyArea;
  lCrossings[3].y = yPosRightPenaltyArea;

  lCrossings[4].x = xPosOpponentPenaltyArea;
  lCrossings[4].y = yPosLeftPenaltyArea;

  lCrossings[5].x = xPosOpponentPenaltyArea;
  lCrossings[5].y = yPosRightPenaltyArea;

  lCrossings[6].x = xPosOpponentGroundline;
  lCrossings[6].y = yPosLeftGroundline;

  lCrossings[7].x = xPosOpponentGroundline;
  lCrossings[7].y = yPosRightGroundline;

}


void SlamLineCrossingsTable::initFalseCrossings()
{
  lCrossings[0].x = xPosHalfWayLine;
  lCrossings[0].y = yPosLeftPenaltyArea;

  lCrossings[1].x = xPosHalfWayLine;
  lCrossings[1].y = yPosRightPenaltyArea;
}

---