Tools/FieldDimensions.cpp

Go to the documentation of this file.

/**
* @file FieldDimensions.cpp
*
* Some useful functions regarding field dimensions.
*
* @author Max Risler
*/

#include "FieldDimensions.h"

double FieldDimensions::distanceToOpponentPenaltyArea(const Vector2<double> &p)
{
  double distanceX, distanceY;
  double minDistance = 0;
  
  if ((p.x <= xPosOpponentPenaltyArea) ||
    (p.y < yPosRightPenaltyArea) ||
    (p.y > yPosLeftPenaltyArea))
  {
    if (p.x > xPosOpponentPenaltyArea)
      distanceX = 0;
    else
      distanceX = xPosOpponentPenaltyArea - p.x;
    
    if (p.y < yPosRightPenaltyArea)
      distanceY = yPosRightPenaltyArea - p.y;
    else if (p.y > yPosLeftPenaltyArea)
      distanceY = p.y - yPosLeftPenaltyArea;
    else
      distanceY = 0;
    return sqrt(distanceX*distanceX+distanceY*distanceY);
  }
  else
  {
    if ((yPosRightPenaltyArea - p.y) > (p.y - yPosLeftPenaltyArea))
    {
      minDistance = yPosRightPenaltyArea - p.y;
    }
    else
    {
      minDistance = p.y - yPosLeftPenaltyArea;
    }
    if ((xPosOpponentPenaltyArea - p.x) > minDistance)
    {
      minDistance = xPosOpponentPenaltyArea - p.x;
    }
    return minDistance;
  }
}

double FieldDimensions::distanceToOwnPenaltyArea(const Vector2<double> &p)
{
  Pose2D pose(p);
  return distanceToOwnPenaltyArea(pose);
}

double FieldDimensions::distanceToOwnPenaltyArea(const Pose2D &p)
{
  double distanceX, distanceY;
  double minDistance = 0;
  
  
  if ((p.translation.x >= xPosOwnPenaltyArea) ||
    (p.translation.y < yPosRightPenaltyArea) ||
    (p.translation.y > yPosLeftPenaltyArea))
  {
    if (p.translation.x < xPosOwnPenaltyArea)
      distanceX = 0;
    else
      distanceX = p.translation.x - xPosOwnPenaltyArea;
    
    if (p.translation.y < yPosRightPenaltyArea)
      distanceY = yPosRightPenaltyArea - p.translation.y;
    else if (p.translation.y > yPosLeftPenaltyArea)
      distanceY = p.translation.y - yPosLeftPenaltyArea;
    else
      distanceY = 0;
    
    return sqrt(distanceX*distanceX+distanceY*distanceY);
  }
  else
  {
    if ((yPosRightPenaltyArea - p.translation.y) >
      (p.translation.y - yPosLeftPenaltyArea))
    {
      minDistance = yPosRightPenaltyArea - p.translation.y;
    }
    else
    {
      minDistance = p.translation.y - yPosLeftPenaltyArea;
    }
    if ((p.translation.x - xPosOwnPenaltyArea) > minDistance)
    {
      minDistance = p.translation.x - xPosOwnPenaltyArea;
    }
    return minDistance;
  }
}


bool FieldDimensions::isInsideField(const Vector2<int> &p) 
{
  if (
    p.x > xPosOwnGoal && p.x < xPosOpponentGoal &&
    p.y > yPosRightGoal && p.y < yPosLeftGoal)
    //inside rectangle between goals
    return true;
  
  if (
    p.x <= xPosOwnGroundline || p.x >= xPosOpponentGroundline ||
    p.y <= yPosRightSideline || p.y >= yPosLeftSideline)
    // outside the field rectancle
    return false;
  
  // check corners
  if (Geometry::getDistanceToLine(ownLeftCorner,Vector2<double>(p.x, p.y)) <= 0) return false;
  if (Geometry::getDistanceToLine(opponentLeftCorner,Vector2<double>(p.x, p.y)) <= 0) return false;
  if (Geometry::getDistanceToLine(ownRightCorner,Vector2<double>(p.x, p.y)) <= 0) return false;
  if (Geometry::getDistanceToLine(opponentRightCorner,Vector2<double>(p.x, p.y)) <= 0) return false;
  
  return true;
}

bool FieldDimensions::isInsideGoal(const Vector2<int> &p)
{
  return (
    p.y > yPosRightGoal && p.y < yPosLeftGoal &&
    (
    p.x > xPosOwnGoal && p.x < xPosOwnGroundline 
    ||
    p.x > xPosOpponentGoal && p.x < xPosOpponentGroundline 
    )
    );
}

bool FieldDimensions::isInsideOwnGoal(const Vector2<int> &p)
{
  return (
    p.y > yPosRightGoal && p.y < yPosLeftGoal &&
    p.x > xPosOwnGoal && p.x < xPosOwnGroundline 
    );
}

bool FieldDimensions::isInsideOpponentGoal(const Vector2<int> &p)
{
  return (
    p.y > yPosRightGoal && p.y < yPosLeftGoal &&
    p.x < xPosOpponentGoal && p.x > xPosOpponentGroundline 
    );
}


double FieldDimensions::distanceToBorder(const Vector2<double> &p)
{
  if (!isInsideField(Vector2<int>((int)p.x, (int)p.y))) return 0.0;
  
  double distance;
  Vector2<double> pAbs(fabs(p.x), fabs(p.y));
  
  distance = yPosLeftSideline - pAbs.y;
  distance = min(distance, Geometry::getDistanceToLine(opponentLeftCorner, pAbs));
  distance = min(distance, Geometry::getDistanceToEdge(opponentLeftGroundline, pAbs));
  distance = min(distance, Geometry::getDistanceToEdge(opponentGoalLeftWall, pAbs));
  distance = min(distance, xPosOpponentGoal - pAbs.x);
  
  return distance;
}

void FieldDimensions::vectorToBorder(const Vector2<double> &p, Vector2<double> &result)
{
  if (!isInsideField(Vector2<int>((int)p.x, (int)p.y))) return;
  
  double minDistance;
  double distance;
  const Geometry::Line *pNearestBorder;
  
  minDistance = Geometry::getDistanceToLine(leftWall, p);
  pNearestBorder = &leftWall;
  
  distance = Geometry::getDistanceToLine(opponentLeftCorner, p);
  if ( distance < minDistance )
  {
    minDistance = distance;
    pNearestBorder = &opponentLeftCorner;
  }
  
  distance = Geometry::getDistanceToLine(opponentLeftGroundline, p);
  if ( distance < minDistance )
  {
    minDistance = distance;
    pNearestBorder = &opponentLeftGroundline;
  }
  
  distance = Geometry::getDistanceToLine(opponentRightCorner, p);
  if ( distance < minDistance )
  {
    minDistance = distance;
    pNearestBorder = &opponentRightCorner;
  }
  
  distance = Geometry::getDistanceToLine(rightWall, p);
  if ( distance < minDistance )
  {
    minDistance = distance;
    pNearestBorder = &rightWall;
  }
  
  distance = Geometry::getDistanceToLine(ownRightCorner, p);
  if ( distance < minDistance )
  {
    minDistance = distance;
    pNearestBorder = &ownRightCorner;
  }
  
  distance = Geometry::getDistanceToLine(ownLeftGroundline, p);
  if ( distance < minDistance )
  {
    minDistance = distance;
    pNearestBorder = &ownLeftGroundline;
  }
  
  distance = Geometry::getDistanceToLine(ownLeftCorner, p);
  if ( distance < minDistance )
  {
    minDistance = distance;
    pNearestBorder = &ownLeftCorner;
  }
  
  result.x = -pNearestBorder->direction.y;
  result.y = pNearestBorder->direction.x;
  result.normalize();
  result *= minDistance;
  
  return;
}

void FieldDimensions::vectorToBorderIncludingGoals(const Vector2<double> &p, Vector2<double> &result)
{
  if (!isInsideField(Vector2<int>((int)p.x, (int)p.y))) return;
  
  double minDistance;
  double distance;
  const Geometry::Line *pNearestBorder;
  
  minDistance = Geometry::getDistanceToLine(leftWall, p);
  pNearestBorder = &leftWall;
  
  distance = Geometry::getDistanceToLine(opponentLeftCorner, p);
  if ( distance < minDistance )
  {
    minDistance = distance;
    pNearestBorder = &opponentLeftCorner;
  }
  
  distance = Geometry::getDistanceToLine(opponentLeftGroundline, p);
  if ( distance < minDistance )
  {
    minDistance = distance;
    pNearestBorder = &opponentLeftGroundline;
  }
  
  distance = Geometry::getDistanceToLine(opponentRightCorner, p);
  if ( distance < minDistance )
  {
    minDistance = distance;
    pNearestBorder = &opponentRightCorner;
  }
  
  distance = Geometry::getDistanceToLine(rightWall, p);
  if ( distance < minDistance )
  {
    minDistance = distance;
    pNearestBorder = &rightWall;
  }
  
  distance = Geometry::getDistanceToLine(ownRightCorner, p);
  if ( distance < minDistance )
  {
    minDistance = distance;
    pNearestBorder = &ownRightCorner;
  }
  
  distance = Geometry::getDistanceToLine(ownLeftGroundline, p);
  if ( distance < minDistance )
  {
    minDistance = distance;
    pNearestBorder = &ownLeftGroundline;
  }
  
  distance = Geometry::getDistanceToLine(ownLeftCorner, p);
  if ( distance < minDistance )
  {
    minDistance = distance;
    pNearestBorder = &ownLeftCorner;
  }
  
  if(isInsideOwnGoal(Vector2<int>((int)p.x, (int)p.y)))
  {
    minDistance = 2000;
    distance = Geometry::getDistanceToLine(ownGoalRightWall, p);
    if ( distance < minDistance )
    {
      minDistance = distance;
      pNearestBorder = &ownGoalRightWall;
    }
    
    distance = Geometry::getDistanceToLine(ownGoalRearWall, p);
    if ( distance < minDistance )
    {
      minDistance = distance;
      pNearestBorder = &ownGoalRearWall;
    }
    
    distance = Geometry::getDistanceToLine(ownGoalLeftWall, p);
    if ( distance < minDistance )
    {
      minDistance = distance;
      pNearestBorder = &ownGoalLeftWall;
    }
  }
  if(isInsideOpponentGoal(Vector2<int>((int)p.x, (int)p.y)))
  {
    minDistance = 2000;
    distance = Geometry::getDistanceToLine(opponentGoalRightWall, p);
    if ( distance < minDistance )
    {
      minDistance = distance;
      pNearestBorder = &opponentGoalRightWall;
    }
    
    distance = Geometry::getDistanceToLine(opponentGoalRearWall, p);
    if ( distance < minDistance )
    {
      minDistance = distance;
      pNearestBorder = &opponentGoalRearWall;
    }
    
    distance = Geometry::getDistanceToLine(opponentGoalLeftWall, p);
    if ( distance < minDistance )
    {
      minDistance = distance;
      pNearestBorder = &opponentGoalLeftWall;
    }
  }
  result.x = -pNearestBorder->direction.y;
  result.y = pNearestBorder->direction.x;
  result.normalize();
  result *= minDistance;
  
  return;
}


bool FieldDimensions::isOnOwnGoalGroundline(const Vector2<int> &p)
{
  return (
    p.x == xPosOwnGroundline && 
    p.y < yPosLeftGoal && 
    p.y > yPosRightGoal 
    );
}

bool FieldDimensions::isOnOpponentGoalGroundline(const Vector2<int> &p)
{
  return (
    p.x == xPosOpponentGroundline && 
    p.y < yPosLeftGoal && 
    p.y > yPosRightGoal 
    );
}

bool FieldDimensions::clipLineWithField
(
 Vector2<int>& point1,
 Vector2<int>& point2
 )
{
  bool clipped;
  
  Vector2<int> newPoint1 = point1;
  Vector2<int> newPoint2 = point2;
  
  Vector2<int> bottomLeftField(xPosOwnGroundline, yPosRightSideline);
  Vector2<int> topRightField(xPosOpponentGroundline, yPosLeftSideline);
  
  clipped = Geometry::clipLineWithRectangleCohenSutherland(bottomLeftField, topRightField, newPoint1, newPoint2);
  
  Geometry::Line lineToClip(point1, point2 - point1);
  /*  if () return false;
  if (Geometry::getDistanceToLine(opponentLeftCorner,Vector2<double>(p.x, p.y)) <= 0) return false;
  if (Geometry::getDistanceToLine(ownRightCorner,Vector2<double>(p.x, p.y)) <= 0) return false;
  if (Geometry::getDistanceToLine(opponentRightCorner,Vector2<double>(p.x, p.y)) <= 0) return false;
  */
  //point1 inside own corners
  //right
  if(Geometry::getDistanceToLine(ownRightCorner,Vector2<double>(newPoint1.x, newPoint1.y)) <= 0)
  {
    Geometry::getIntersectionOfLines(lineToClip, ownRightCorner, newPoint1);
    clipped = true;
  }
  //left
  else if(Geometry::getDistanceToLine(ownLeftCorner,Vector2<double>(newPoint1.x, newPoint1.y)) <= 0)
  {
    Geometry::getIntersectionOfLines(lineToClip, ownLeftCorner, newPoint1);
    clipped = true;
  }
  //point inside opponent corners
  //right
  else if(Geometry::getDistanceToLine(opponentRightCorner,Vector2<double>(newPoint1.x, newPoint1.y)) <= 0)
  {
    Geometry::getIntersectionOfLines(lineToClip, opponentRightCorner, newPoint1);
    clipped = true;
  }
  //left
  else if(Geometry::getDistanceToLine(opponentLeftCorner,Vector2<double>(newPoint1.x, newPoint1.y)) <= 0)
  {
    Geometry::getIntersectionOfLines(lineToClip, opponentLeftCorner, newPoint1);
    clipped = true;
  }
  
  //point2  inside own corners
  //right
  if(Geometry::getDistanceToLine(ownRightCorner,Vector2<double>(newPoint2.x, newPoint2.y)) <= 0)
  {
    Geometry::getIntersectionOfLines(lineToClip, ownRightCorner, newPoint2);
    clipped = true;
  }
  //left
  else if(Geometry::getDistanceToLine(ownLeftCorner,Vector2<double>(newPoint2.x, newPoint2.y)) <= 0)
  {
    Geometry::getIntersectionOfLines(lineToClip, ownLeftCorner, newPoint2);
    clipped = true;
  }
  
  //point inside opponent corners
  //right
  else if(Geometry::getDistanceToLine(opponentRightCorner,Vector2<double>(newPoint2.x, newPoint2.y)) <= 0)
  {
    Geometry::getIntersectionOfLines(lineToClip, opponentRightCorner, newPoint2);
    clipped = true;
  }
  //left
  else if(Geometry::getDistanceToLine(opponentLeftCorner,Vector2<double>(newPoint2.x, newPoint2.y)) <= 0)
  {
    Geometry::getIntersectionOfLines(lineToClip, opponentLeftCorner, newPoint2);
    clipped = true;
  }
  
  point1 = newPoint1;
  point2 = newPoint2;
  return clipped;
}

bool FieldDimensions::clipLineWithFieldAndGoalAreas
(
 Vector2<int>& point1,
 Vector2<int>& point2
 )
{
  bool clipped = false;
  
  Vector2<int> newPoint1 = point1;
  Vector2<int> newPoint2 = point2;
  
  
  // point 1 is inside a goal
  if(isInsideOwnGoal(point1))
  {
    Geometry::clipLineWithRectangleCohenSutherland(ownGoalRightRearPanel, ownGoalLeftPost, newPoint1, newPoint2);
    if(isOnOwnGoalGroundline(newPoint2))
    {
      Vector2<int> point2InField = point2;
      clipLineWithField(newPoint2, point2InField);
      if(isOnOpponentGoalGroundline(point2InField))
      {
        Vector2<int> point2InGoal = point2;
        Geometry::clipLineWithRectangleCohenSutherland(opponentGoalRightPost, opponentGoalLeftRearPanel, point2InField, point2InGoal);
        newPoint2 = point2InGoal;
      }
      else
      {
        newPoint2 = point2InField;
      }
    }
    clipped = true;
  }
  else if(isInsideOpponentGoal(point1))
  {
    Geometry::clipLineWithRectangleCohenSutherland(opponentGoalRightPost, opponentGoalLeftRearPanel, newPoint1, newPoint2);
    if(isOnOpponentGoalGroundline(newPoint2))
    {
      Vector2<int> point2InField = point2;
      clipLineWithField(newPoint2, point2InField);
      if(isOnOwnGoalGroundline(point2InField))
      {
        Vector2<int> point2InGoal = point2;
        Geometry::clipLineWithRectangleCohenSutherland(ownGoalRightRearPanel, ownGoalLeftPost, point2InField, point2InGoal);
        newPoint2 = point2InGoal;
      }
      else
      {
        newPoint2 = point2InField;
      }
    }
    clipped = true;
  }
  
  // point 2 is inside a goal
  if(isInsideOwnGoal(point2))
  {
    Geometry::clipLineWithRectangleCohenSutherland(ownGoalRightRearPanel, ownGoalLeftPost, newPoint1, newPoint2);
    if(isOnOwnGoalGroundline(newPoint1))
    {
      Vector2<int> point1InField = point1;
      clipLineWithField(newPoint1, point1InField);
      if(isOnOpponentGoalGroundline(point1InField))
      {
        Vector2<int> point1InGoal = point1;
        Geometry::clipLineWithRectangleCohenSutherland(opponentGoalRightPost, opponentGoalLeftRearPanel, point1InField, point1InGoal);
        newPoint1 = point1InGoal;
      }
      else
      {
        newPoint1 = point1InField;
      }
    }
    clipped = true;
  }
  else if(isInsideOpponentGoal(point2))
  {
    Geometry::clipLineWithRectangleCohenSutherland(opponentGoalRightPost, opponentGoalLeftRearPanel, newPoint1, newPoint2);
    if(isOnOpponentGoalGroundline(newPoint1))
    {
      Vector2<int> point1InField = point1;
      clipLineWithField(newPoint1, point1InField);
      if(isOnOwnGoalGroundline(point1InField))
      {
        Vector2<int> point1InGoal = point1;
        Geometry::clipLineWithRectangleCohenSutherland(ownGoalRightRearPanel, ownGoalLeftPost, point1InField, point1InGoal);
        newPoint1 = point1InGoal;
      }
      else
      {
        newPoint1 = point1InField;
      }
    }
    clipped = true;
  }
  
  if(!clipped)
  {
    Vector2<int> bottomLeftField(xPosOwnGroundline, yPosRightSideline);
    Vector2<int> topRightField(xPosOpponentGroundline, yPosLeftSideline);
    
    clipped = clipLineWithField(newPoint1, newPoint2);
    
    // Line intersects groudline at own goal
    if(newPoint1.x == xPosOwnGroundline && newPoint1.y < yPosLeftGoal && newPoint1.y > yPosRightGoal)
    {
      Geometry::clipLineWithRectangleCohenSutherland(ownGoalRightRearPanel, ownGoalLeftPost, newPoint1, point1);
      newPoint1 = point1;
      clipped = true;
    }
    if(newPoint2.x == xPosOwnGroundline && newPoint2.y < yPosLeftGoal && newPoint2.y > yPosRightGoal )
    {
      Geometry::clipLineWithRectangleCohenSutherland(ownGoalRightRearPanel, ownGoalLeftPost, newPoint2, point2);
      newPoint2 = point2;
      clipped = true;
    }
    // Line intersects groudline at opponent goal
    if(newPoint1.x == xPosOpponentGroundline && newPoint1.y < yPosLeftGoal && newPoint1.y > yPosRightGoal)
    {
      Geometry::clipLineWithRectangleCohenSutherland(opponentGoalRightPost, opponentGoalLeftRearPanel, newPoint1, point1);
      newPoint1 = point1;
      clipped = true;
    }
    if(isOnOpponentGoalGroundline(newPoint2))
    {
      Geometry::clipLineWithRectangleCohenSutherland(opponentGoalRightPost, opponentGoalLeftRearPanel, newPoint2, point2);
      newPoint2 = point2;
      clipped = true;
    }
    
  }
  
  point1 = newPoint1;
  point2 = newPoint2;
  return clipped;
}

/*
* Change log :
* 
* $Log: FieldDimensions.cpp,v $
* Revision 1.4  2004/06/24 12:49:29  goehring
* distanceTo OpponentPenaltyArea can be negative when ball is in area now
*
* Revision 1.3  2004/06/24 12:07:06  goehring
* unused variables removed
*
* Revision 1.2  2004/06/24 12:02:28  goehring
* distanceToOwnPenaltyArea now negative when ball is in p-area
*
* Revision 1.1.1.1  2004/05/22 17:35:50  cvsadm
* created new repository GT2004_WM
*
* Revision 1.2  2004/03/16 14:00:23  juengel
* Integrated Improvments from "Günne"
* -ATH2004ERS7Behavior
* -ATHHeadControl
* -KickSelectionTable
* -KickEditor
*
* Revision 1.2  2004/03/15 17:11:41  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.1  2003/10/07 10:13:21  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.1.1.1  2003/07/02 09:40:28  cvsadm
* created new repository for the competitions in Padova from the 
* tamara CVS (Tuesday 2:00 pm)
*
* removed unused solutions
*
* Revision 1.5  2003/06/12 16:53:09  juengel
* no message
*
* Revision 1.4  2003/06/10 18:07:25  juengel
* Added methods: isInsideGoal, isInsideOwnGoal, isInsideOpponentGoal, isOnOwnGoalGroundline, isOnOpponentGoalGroundline, vectorToBorderIncludingGoals, clipLineWithField, clipLineWithFieldAndGoalAreas.
*
* Revision 1.3  2003/05/20 14:28:07  risler
* distanceToBorder optimized
*
* Revision 1.2  2003/05/05 14:52:17  dueffert
* warnings removed
*
* Revision 1.1  2003/04/15 15:52:07  risler
* DDD GO 2003 code integrated
*
* Revision 1.3  2003/04/07 17:08:04  dthomas
* added gobehindballorthogonaltoborder, gotoball
*
* Revision 1.2  2003/04/05 12:57:34  max
* distancetoownpenaltyarea bug fixed
*
* Revision 1.1  2003/04/04 11:48:52  max
* moved field dimensions function in separate class
*
*
*/

---