Tools/Math/Geometry.cpp

Go to the documentation of this file.

/**
* @file Math/Geometry.cpp
* Implemets class Geometry
*
* @author <A href=mailto:juengel@informatik.hu-berlin.de>Matthias Jüngel</A>
* @author <a href="mailto:walter.nistico@uni-dortmund.de">Walter Nistico</a>
*/

#include "Geometry.h"
#include "Tools/FieldDimensions.h"
#include "Tools/Debugging/Debugging.h"
#include "Tools/RobotConfiguration.h"

Geometry::Circle::Circle() : radius(0) 
{
}

double Geometry::angleTo(const Pose2D& from, 
                                const Vector2<double>& to)
{
  Pose2D relPos = Pose2D(to) - from;
  return atan2(relPos.translation.y,relPos.translation.x);
}

double Geometry::distanceTo(const Pose2D& from, 
                                   const Vector2<double>& to)
{
  return (Pose2D(to) - from).translation.abs();
}

Vector2<double> Geometry::vectorTo(const Pose2D& from, const Vector2<double>& to)
{
  return (Pose2D(to) - from).translation;
}

void Geometry::Line::normalizeDirection()
{
  double distance = 
    sqrt(sqr(direction.x) + sqr(direction.y));
  direction.x = direction.x / distance;
  direction.y = direction.y / distance;
}

Geometry::Circle Geometry::getCircle
(
 const Vector2<int> point1,
 const Vector2<int> point2,
 const Vector2<int> point3
 )
{
  double x1 = point1.x;
  double y1 = point1.y;
  double x2 = point2.x;
  double y2 = point2.y;
  double x3 = point3.x;
  double y3 = point3.y;
  
  Circle circle;
  
  if((x2*y1 - x3*y1 - x1*y2 + x3*y2 + x1*y3 - x2*y3 == 0) )
  {
    circle.radius = 0;
  }
  else
  {
    circle.radius =
      0.5 * 
      sqrt(
      ( (sqr(x1 - x2) + sqr(y1 - y2) ) *
      (sqr(x1 - x3) + sqr(y1 - y3) ) *
      (sqr(x2 - x3) + sqr(y2 - y3) )
      )
      /
      sqr(x2*y1 - x3*y1 - x1*y2 + x3*y2 + x1*y3 - x2*y3)
      );
  }
  
  if( (2 * (-x2*y1 + x3*y1 + x1*y2 - x3*y2 - x1*y3 + x2*y3) == 0) )
  {
    circle.center.x = 0;
  }
  else
  {
    circle.center.x =
      (
      sqr(x3) * (y1 - y2) + 
      (sqr(x1) + (y1 - y2) * (y1 - y3)) * (y2 - y3) +
      sqr(x2) * (-y1 + y3)
      )
      /
      (2 * (-x2*y1 + x3*y1 + x1*y2 - x3*y2 - x1*y3 + x2*y3) );
  }   
  
  if((2 * (x2*y1 - x3*y1 - x1*y2 + x3*y2 + x1*y3 - x2*y3) == 0) )
  {
    circle.center.y = 0;
  }
  else
  {
    circle.center.y =
      (
      sqr(x1) * (x2 - x3) + 
      sqr(x2) * x3 + 
      x3*(-sqr(y1) + sqr(y2) ) - 
      x2*(+sqr(x3) - sqr(y1) + sqr(y3) ) + 
      x1*(-sqr(x2) + sqr(x3) - sqr(y2) + sqr(y3) )
      )
      /
      (2 * (x2*y1 - x3*y1 - x1*y2 + x3*y2 + x1*y3 - x2*y3) );
  } 
  return circle;
}

bool Geometry::getIntersectionOfLines
(
 const Line line1,
 const Line line2,
 Vector2<int>& intersection
 )
{
 Vector2<double> intersectionDouble;
 bool toReturn = getIntersectionOfLines(line1,line2,intersectionDouble);
 intersection.x = (int)intersectionDouble.x;
 intersection.y = (int)intersectionDouble.y;
 return toReturn;
}

bool Geometry::getIntersectionOfLines
(
 const Line line1,
 const Line line2,
 Vector2<double>& intersection
 )
{
  if(line1.direction.y * line2.direction.x == line1.direction.x * line2.direction.y)
  {
    return false;
  }
  
  intersection.x = 
    line1.base.x +
    line1.direction.x * 
    (
    line1.base.y * line2.direction.x - 
    line2.base.y * line2.direction.x + 
    (-line1.base.x + line2.base.x) * line2.direction.y
    )
    /
    ( (-line1.direction.y) * line2.direction.x + line1.direction.x * line2.direction.y );
  
  intersection.y = 
    line1.base.y + 
    line1.direction.y *
    (
    -line1.base.y * line2.direction.x + 
    line2.base.y * line2.direction.x + 
    (line1.base.x - line2.base.x) * line2.direction.y
    )
    /
    (line1.direction.y * line2.direction.x - line1.direction.x * line2.direction.y);
  
  return true;
}

bool Geometry::getIntersectionOfRaysFactor
(
 const Line ray1,
 const Line ray2,
 double& factor
 )
{
  double divisor = ray2.direction.x * ray1.direction.y - ray1.direction.x * ray2.direction.y;
  if(divisor==0)
  {
    return false;
  }
  double k=(ray2.direction.y*ray1.base.x - ray2.direction.y*ray2.base.x - ray2.direction.x*ray1.base.y + ray2.direction.x*ray2.base.y)/divisor;
  double l=(ray1.direction.y*ray1.base.x - ray1.direction.y*ray2.base.x - ray1.direction.x*ray1.base.y + ray1.direction.x*ray2.base.y)/divisor;
  if ((k>=0)&&(l>=0)&&(k<=1)&&(l<=1))
  {
    //intersection=ray1.base + ray1.direction*k;
    factor=k;
    return true;
  }
  return false;
}

double Geometry::getDistanceToLine
(
 const Line line,
 const Vector2<double>& point
 )
{
  if (line.direction.x == 0 && line.direction.y == 0) 
    return distance(point, line.base);

  Vector2<double> normal;
  normal.x = line.direction.y;
  normal.y = -line.direction.x;
  normal.normalize();

  double c = normal * line.base;

  return normal * point - c;
}

double Geometry::getDistanceToEdge
(
 const Line line,
 const Vector2<double>& point
 )
{
  if (line.direction.x == 0 && line.direction.y == 0) 
    return distance(point, line.base);

  double c = line.direction * line.base;

  double d = (line.direction * point - c) / (line.direction * line.direction);

  if (d < 0) 
    return distance(point, line.base);
  else if (d > 1.0)
    return distance(point, line.base + line.direction);
  else
    return fabs(getDistanceToLine(line, point));
}


double Geometry::distance
(
 const Vector2<double>& point1,
 const Vector2<double>& point2
 )
{
  return (point2 - point1).abs();
}

double Geometry::distance
(
 const Vector2<int>& point1,
 const Vector2<int>& point2
 )
{
  return (point2 - point1).abs();
}

double Geometry::calculateScaleFactor(
  int y,
  unsigned long frameNumber,
  unsigned long prevFrameNumber,
  const CameraInfo& cameraInfo
  )
{
  if(frameNumber != prevFrameNumber)
  {
    const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
    double timeDiff = (frameNumber - prevFrameNumber) * r.motionCycleTime; // in seconds
    double imageDiff = timeDiff * r.imagesPerSecond; // how many images are between the old and the current one (should be 1)?
    return (1 - double(1 + y) / cameraInfo.resolutionHeight) / imageDiff * 0.9; // interpolate, 10% less
  }
  else
    return 0;
}

Vector3<double> Geometry::rayFromCamera(
  int y,
  const CameraMatrix& cameraMatrix, 
  const CameraMatrix& prevCameraMatrix,
  const Vector3<double> vector,
  const CameraInfo& cameraInfo
  )
{
  Vector3<double> v1 = cameraMatrix.rotation * vector;
  if(!cameraInfo.simulated)
  {
    Vector3<double> v2 = prevCameraMatrix.rotation * vector;
    // shift current percept towards old percept based on image row
    v1 += (v2 - v1) * calculateScaleFactor(y, cameraMatrix.frameNumber, prevCameraMatrix.frameNumber, cameraInfo);
  }
  return v1;
}

void Geometry::calculateAnglesForPoint
(
 const Vector2<int>& point, 
 const CameraMatrix& cameraMatrix, 
 const CameraInfo& cameraInfo, 
 Vector2<double>& angles
 )
{
  double factor = cameraInfo.focalLength;
  
  Vector3<double> vectorToPoint(
    factor,
    cameraInfo.opticalCenter.x - point.x,
    cameraInfo.opticalCenter.y - point.y);
  
  Vector3<double> vectorToPointWorld = 
    cameraMatrix.rotation * vectorToPoint;
  
  angles.x =
    atan2(vectorToPointWorld.y,vectorToPointWorld.x);
  
  angles.y =
    atan2(
    vectorToPointWorld.z,
    sqrt(sqr(vectorToPointWorld.x) + sqr(vectorToPointWorld.y)) 
    );
}

void Geometry::calculateAnglesForPoint
(
 const Vector2<int>& point, 
 const CameraMatrix& cameraMatrix, 
 const CameraMatrix& prevCameraMatrix, 
 const CameraInfo& cameraInfo, 
 Vector2<double>& angles
 )
{
  calculateAnglesForPoint(point, cameraMatrix, cameraInfo, angles);
  if(!cameraInfo.simulated)
  {
    Vector2<double> angles2;
    calculateAnglesForPoint(point, prevCameraMatrix, cameraInfo, angles2);
    // shift current percept towards old percept based on image row
    angles += (angles2 - angles) * calculateScaleFactor(point.y, cameraMatrix.frameNumber, prevCameraMatrix.frameNumber, cameraInfo);
  }
}
  
void Geometry::calculatePointByAngles
(
 const Vector2<double>& angles,
 const CameraMatrix& cameraMatrix, 
 const CameraInfo& cameraInfo, 
 Vector2<int>& point
 )
{
  Vector3<double> vectorToPointWorld, vectorToPoint;
  vectorToPointWorld.x = cos(angles.x);
  vectorToPointWorld.y = sin(angles.x);
  vectorToPointWorld.z = tan(angles.y);
  
  RotationMatrix rotationMatrix = cameraMatrix.rotation;
  vectorToPoint = rotationMatrix.invert() * vectorToPointWorld;
  
  double factor = cameraInfo.focalLength;
  
  double scale = factor / vectorToPoint.x;
  
  point.x = (int)(0.5 + cameraInfo.opticalCenter.x - vectorToPoint.y * scale);
  point.y = (int)(0.5 + cameraInfo.opticalCenter.y  - vectorToPoint.z * scale);
}


bool Geometry::clipLineWithQuadrangle
(
 const Line lineToClip,
 const Vector2<double>& corner0, 
 const Vector2<double>& corner1,
 const Vector2<double>& corner2, 
 const Vector2<double>& corner3,
 Vector2<int>& clipPoint1, 
 Vector2<int>& clipPoint2
 )
{
  Vector2<double> point1, point2;
  bool toReturn = clipLineWithQuadrangle(
   lineToClip,
   corner0, corner1, corner2, corner3,
   point1, point2);
   clipPoint1.x = (int)point1.x;
   clipPoint1.y = (int)point1.y;
   clipPoint2.x = (int)point2.x;
   clipPoint2.y = (int)point2.y;
   return toReturn;
}

bool Geometry::clipLineWithQuadrangle
(
 const Line lineToClip,
 const Vector2<double>& corner0, 
 const Vector2<double>& corner1,
 const Vector2<double>& corner2, 
 const Vector2<double>& corner3,
 Vector2<double>& clipPoint1, 
 Vector2<double>& clipPoint2
 )
{
  Geometry::Line side[4] , verticalLine;

  verticalLine.base = lineToClip.base;

  verticalLine.direction.x = -lineToClip.direction.y;
  verticalLine.direction.y = lineToClip.direction.x;
  
  Vector2<double> corner[4];
  corner[0] = corner0;
  corner[1] = corner1;
  corner[2] = corner2;
  corner[3] = corner3;

  side[0].base = corner0;
  side[0].direction = corner1;
  
  side[1].base = corner1;
  side[1].direction = corner3;
  
  side[2].base = corner2;
  side[2].direction = corner1;
  
  side[3].base = corner3;
  side[3].direction = corner3;
  
  Vector2<double> point1, point2, point;
  bool nextIsPoint1 = true;
  /*
  for(int i = 0; i < 4; i++)
  {
    if(Geometry::getIntersectionOfLines(side[i], lineToClip, point))
    {
      if(nextIsPoint1 && sign(point.x - corner[i]) != sign(point.x - corner[(i+1)%4])
        )
      {
        point1 = point;
        nextIsPoint1 = false;
      }
      else
        point2 = point;
    };
  }*/

  if(Geometry::getIntersectionOfLines(side[0], lineToClip, point))
  {
    if(corner[0].x < point.x && point.x < corner[1].x)
    {
      if(nextIsPoint1)
      {
        point1 = point;
        nextIsPoint1 = false;
      }
    }
  }

  if(Geometry::getIntersectionOfLines(side[1], lineToClip, point))
  {
    if(corner[1].y < point.y && point.y < corner[2].y)
    {
      if(nextIsPoint1)
      {
        point1 = point;
        nextIsPoint1 = false;
      }
      else
      point2 = point;
    }
  }

  if(Geometry::getIntersectionOfLines(side[2], lineToClip, point))
  {
    if(corner[2].x > point.x && point.x > corner[3].x)
    {
      if(nextIsPoint1)
      {
        point1 = point;
        nextIsPoint1 = false;
      }
      else
      point2 = point;
    }
  }

  if(Geometry::getIntersectionOfLines(side[3], lineToClip, point))
  {
    if(corner[3].y > point.y && point.y > corner[0].y)
    {
      if(nextIsPoint1)
      {
        point1 = point;
        nextIsPoint1 = false;
      }
      else
      point2 = point;
    }
  }

  if(nextIsPoint1)
    return false;

  if(getDistanceToLine(verticalLine, point1) < getDistanceToLine(verticalLine, point2) )
  {
    clipPoint1 = point1;
    clipPoint2 = point2;
  }
  else
  {
    clipPoint1 = point2;
    clipPoint2 = point1;
  }
  return true;
}

bool Geometry::isPointInsideRectangle
(
 const Vector2<double>& bottomLeftCorner, 
 const Vector2<double>& topRightCorner,
 const Vector2<double>& point
)
{
  return(
    bottomLeftCorner.x <= point.x && point.x <= topRightCorner.x &&
    bottomLeftCorner.y <= point.y && point.y <= topRightCorner.y
    );
}

bool Geometry::isPointInsideRectangle
(
 const Vector2<int>& bottomLeftCorner, 
 const Vector2<int>& topRightCorner,
 const Vector2<int>& point
)
{
  return(
    bottomLeftCorner.x <= point.x && point.x <= topRightCorner.x &&
    bottomLeftCorner.y <= point.y && point.y <= topRightCorner.y
    );
}

bool Geometry::clipPointInsideRectange(
  const Vector2<int>& bottomLeftCorner, 
  const Vector2<int>& topRightCorner,
  Vector2<int>& point
)
{
  bool clipped = false;
  if (point.x < bottomLeftCorner.x)
  {
    point.x = bottomLeftCorner.x;
    clipped = true;
  }
  if (point.x > topRightCorner.x)
  {
    point.x = topRightCorner.x;
    clipped = true;
  }
  if (point.y < bottomLeftCorner.y)
  {
    point.y = bottomLeftCorner.y;
    clipped = true;
  }
  if (point.y > topRightCorner.y)
  {
    point.y = topRightCorner.y;
    clipped = true;
  }
  return clipped;
}

bool Geometry::clipPointInsideRectange(
  const Vector2<int>& bottomLeftCorner, 
  const Vector2<int>& topRightCorner,
  Vector2<double>& point
)
{
  bool clipped = false;
  if (point.x < bottomLeftCorner.x)
  {
    point.x = (double)bottomLeftCorner.x;
    clipped = true;
  }
  if (point.x > topRightCorner.x)
  {
    point.x = (double)topRightCorner.x;
    clipped = true;
  }
  if (point.y < bottomLeftCorner.y)
  {
    point.y = (double)bottomLeftCorner.y;
    clipped = true;
  }
  if (point.y > topRightCorner.y)
  {
    point.y = (double)topRightCorner.y;
    clipped = true;
  }
  return clipped;
}

bool Geometry::calculatePointOnField
(
 const int x,
 const int y,
 const CameraMatrix& cameraMatrix,
 const CameraInfo& cameraInfo,
 Vector2<double>& pointOnField
 )
{
#ifndef _WIN32 // don't recalculate on real robot
  static 
#endif
  double xFactor = cameraInfo.focalLengthInv,
         yFactor = cameraInfo.focalLengthInv;

  Vector3<double> 
    vectorToCenter(1, (cameraInfo.opticalCenter.x - x) * xFactor, (cameraInfo.opticalCenter.y - y) * yFactor);
  
  Vector3<double> 
    vectorToCenterWorld = cameraMatrix.rotation * vectorToCenter;

  //Is the point above the horizon ? - return
  if(vectorToCenterWorld.z > -5 * yFactor) return false;
  
  double a1 = cameraMatrix.translation.x,
         a2 = cameraMatrix.translation.y,
         a3 = cameraMatrix.translation.z,
         b1 = vectorToCenterWorld.x,
         b2 = vectorToCenterWorld.y,
         b3 = vectorToCenterWorld.z;
  
  pointOnField.x = (a1 * b3 - a3 * b1) / b3 + 0.5;
  pointOnField.y = (a2 * b3 - a3 * b2) / b3 + 0.5;

  return fabs(pointOnField.x) < 10000 && fabs(pointOnField.y) < 10000;
}

bool Geometry::calculatePointOnField
(
 const int x,
 const int y,
 const CameraMatrix& cameraMatrix,
 const CameraMatrix& prevCameraMatrix,
 const CameraInfo& cameraInfo,
 Vector2<double>& pointOnField
 )
{
  if(cameraInfo.simulated)
    return calculatePointOnField(x, y, cameraMatrix, cameraInfo, pointOnField);
  else
  {
    Vector2<double> pointOnField2;
    if(calculatePointOnField(x, y, cameraMatrix, cameraInfo, pointOnField) &&
       calculatePointOnField(x, y, prevCameraMatrix, cameraInfo, pointOnField2))
    {
      double scaleFactor = calculateScaleFactor(y, cameraMatrix.frameNumber, prevCameraMatrix.frameNumber, cameraInfo);
      // shift current percept towards old percept based on image row
      pointOnField.x += ((pointOnField2.x - pointOnField.x) * scaleFactor);
      pointOnField.y += ((pointOnField2.y - pointOnField.y) * scaleFactor);
      return true;
    }
    else
      return false;
  }
}

void Geometry::calculatePointInImage
( 
 const Vector2<int>& point,
 const CameraMatrix& cameraMatrix,
 const CameraInfo& cameraInfo,
 Vector2<int>& pointInImage
 )
{
  Vector2<double> offset(point.x - cameraMatrix.translation.x,
                         point.y - cameraMatrix.translation.y);
  calculatePointByAngles(
    Vector2<double>(atan2(offset.y,offset.x),
                    -atan2(cameraMatrix.translation.z,offset.abs())),
    cameraMatrix, cameraInfo, 
    pointInImage
  );
}

/*
not tested
bool Geometry::ExpandOrClipLineWithRectangleCohenSutherland
(
 const Vector2<int>& bottomLeft, 
 const Vector2<int>& topRight,
 Vector2<int>& point1, 
 Vector2<int>& point2
 )
{
  while(!isPointInsideRectangle(bottomLeft, topRight, point1) )
  {
    point1 -= (point2 - point1);
  }
  while(!isPointInsideRectangle(bottomLeft, topRight, point2) )
  {
    point2 -= (point1 - point2);
  }
  return(clipLineWithRectangleCohenSutherland(bottomLeft, topRight, point1, point2));
}
*/

bool Geometry::getIntersectionPointsOfLineAndRectangle(
 const Vector2<int>& bottomLeft, 
 const Vector2<int>& topRight,
 const Geometry::Line line,
 Vector2<int>& point1, 
 Vector2<int>& point2
)
{
  int foundPoints=0;
  Vector2<double> point[2];
  if (line.direction.x!=0)
  {
    double y1=line.base.y+(bottomLeft.x-line.base.x)*line.direction.y/line.direction.x;
    if ((y1>=bottomLeft.y)&&(y1<=topRight.y))
    {
      point[foundPoints].x=bottomLeft.x;
      point[foundPoints++].y=y1;
    }
    double y2=line.base.y+(topRight.x-line.base.x)*line.direction.y/line.direction.x;
    if ((y2>=bottomLeft.y)&&(y2<=topRight.y))
    {
      point[foundPoints].x=topRight.x;
      point[foundPoints++].y=y2;
    }
  }
  if (line.direction.y!=0)
  {
    double x1=line.base.x+(bottomLeft.y-line.base.y)*line.direction.x/line.direction.y;
    if ((x1>=bottomLeft.x)&&(x1<=topRight.x)&&(foundPoints<2))
    {
      point[foundPoints].x=x1;
      point[foundPoints].y=bottomLeft.y;
      if ((foundPoints==0)||((point[0]-point[1]).abs()>0.1))
      {
        foundPoints++;
      }
    }
    double x2=line.base.x+(topRight.y-line.base.y)*line.direction.x/line.direction.y;
    if ((x2>=bottomLeft.x)&&(x2<=topRight.x)&&(foundPoints<2))
    {
      point[foundPoints].x=x2;
      point[foundPoints].y=topRight.y;
      if ((foundPoints==0)||((point[0]-point[1]).abs()>0.1))
      {
        foundPoints++;
      }
    }
  }
  switch (foundPoints)
  {
  case 1:
    point1.x=(int)point[0].x;
    point2.x=point1.x;
    point1.y=(int)point[0].y;
    point2.y=point1.y;
    foundPoints++;
    return true;
  case 2:
    if ((point[1]-point[0])*line.direction >0)
    {
      point1.x=(int)point[0].x;
      point1.y=(int)point[0].y;
      point2.x=(int)point[1].x;
      point2.y=(int)point[1].y;
    }
    else
    {
      point1.x=(int)point[1].x;
      point1.y=(int)point[1].y;
      point2.x=(int)point[0].x;
      point2.y=(int)point[0].y;
    }
    return true;
  default:
    return false;
  }
}

bool Geometry::clipLineWithRectangleCohenSutherland
(
 const Vector2<int>& bottomLeft, 
 const Vector2<int>& topRight,
 Vector2<int>& point1, 
 Vector2<int>& point2
 )
{
//  bool point2WasRight = point2.x > point1.x;
//  bool point2WasTop = point2.y > point1.y;

  int swap = 0;

  Vector2<int> p;
  int c1 = cohenSutherlandOutCode(bottomLeft, topRight, point1);
  int c2 = cohenSutherlandOutCode(bottomLeft, topRight, point2);
  
  if(c1 == 0 && c2 == 0) return false;

  while ( ( !(c1 == 0) || !(c2 == 0) ) && ( (c1&c2) == 0 ) ) 
  {
    if ( c1 == 0 ) 
    {
      p = point1;
      point1 = point2;
      point2 = p;
      c1 = c2; 
      swap++;
    }
    if (c1 & 1) 
    {
      point1.y = intersection(point1.x,point1.y,point2.x,point2.y,bottomLeft.x) ;
      point1.x = bottomLeft.x ; 
    }
    else if (c1&2)
    {
      point1.y = intersection(point1.x,point1.y,point2.x,point2.y,topRight.x) ;
      point1.x = topRight.x ; 
    }
    else if (c1&4)
    {
      point1.x = intersection(point1.y,point1.x,point2.y,point2.x,bottomLeft.y) ;
      point1.y = bottomLeft.y ; 
    }
    else if (c1&8)
    {
      point1.x = intersection(point1.y,point1.x,point2.y,point2.x,topRight.y) ;
      point1.y = topRight.y ; 
    }
    c1 = cohenSutherlandOutCode(bottomLeft, topRight, point1);
    c2 = cohenSutherlandOutCode(bottomLeft, topRight, point2);
  }
  if ( c1 == 0 && c2 == 0 )
  {
/*    if(
      (point2WasTop && point2.y <= point1.y) ||
      (!point2WasTop && point2.y > point1.y) ||
      (point2WasRight && point2.x <= point1.x) ||
      (!point2WasRight && point2.x > point1.x) 
      )
*/
    if(swap % 2 == 1)
    {
      p = point1 ;
      point1 = point2;
      point2 = p ;
    }
    return true;
  }
  else
    return false;
}

int Geometry::intersection(int a1, int b1, int a2, int b2, int value)
{
  int result = 0 ;
  if ( a2 - a1 != 0 )
    result = (int) (b1 +(double) (value-a1) / (a2-a1) * (b2-b1));
  else
    result = 32767;
  return(result);
}

Vector2<double> Geometry::relative2FieldCoord(RobotPose rp, double x, double y)
{
  Vector3<double> relativePos(x, y, 0.0);
  Vector2<double> fieldPos(0.0, 0.0);
  RotationMatrix R;          

  R.fromKardanRPY(0,0, -rp.rotation);
  relativePos = R * relativePos;

  fieldPos.x = relativePos.x + rp.translation.x;
  fieldPos.y = relativePos.y + rp.translation.y;
  
  return fieldPos;
}

Vector2<double> Geometry::fieldCoord2Relative(RobotPose robotPose, Vector2<double> fieldCoord)
{
  Vector2<double> relativeCoord;
  double distance = Geometry::distanceTo(robotPose.getPose(), fieldCoord);
  double angle = Geometry::angleTo(robotPose.getPose(), fieldCoord);
  relativeCoord.x = distance * cos(angle);
  relativeCoord.y = distance * sin(angle);
  return relativeCoord;
}


bool Geometry::calculateBallInImage(const Vector2<double>& ballOffset,
                                    const CameraMatrix& cameraMatrix, const CameraInfo& cameraInfo, Circle& circle)
{
  Vector2<double> offset(ballOffset.x - cameraMatrix.translation.x,
                         ballOffset.y - cameraMatrix.translation.y);
  double distance = offset.abs(),
         height = cameraMatrix.translation.z - ballRadius,
         cameraDistance = sqrt(sqr(distance) + sqr(height));
  circle.center = Vector2<double>(atan2(offset.y,offset.x),-atan2(height,distance));
  if(cameraDistance >= ballRadius)
  {
    double alpha = pi_2 - circle.center.y - acos(ballRadius / cameraDistance),
           yBottom = -atan2(height + cos(alpha) * ballRadius,
                            distance - sin(alpha) * ballRadius),
           beta = pi_2 - circle.center.y + acos(ballRadius / cameraDistance),
           yTop = -atan2(height + cos(beta) * ballRadius,
                         distance - sin(beta) * ballRadius);
    Vector2<int> top,
                 bottom;
    calculatePointByAngles(Vector2<double>(circle.center.x,yTop),cameraMatrix, cameraInfo, top);
    calculatePointByAngles(Vector2<double>(circle.center.x,yBottom),cameraMatrix, cameraInfo, bottom);
    circle.center.x = (top.x + bottom.x) / 2.0;
    circle.center.y = (top.y + bottom.y) / 2.0;
    circle.radius = (top - bottom).abs() / 2.0;
    return true;
  }
  else
    return false;
}

double Geometry::angleSizeToPixelSize(double angleSize, const CameraInfo& cameraInfo)
{
  return cameraInfo.focalLength * tan(angleSize);
}

double Geometry::pixelSizeToAngleSize(double pixelSize, const CameraInfo& cameraInfo)
{
  return atan(pixelSize * cameraInfo.focalLengthInv);  
}

double Geometry::getDistanceBySize
(
 const CameraInfo& cameraInfo,
 double sizeInReality,
 double sizeInPixels
 )
{
  double xFactor = cameraInfo.focalLength;
  return sizeInReality * xFactor / (sizeInPixels + 0.00000001);
}

double Geometry::getDistanceBySize
(
 const CameraInfo& cameraInfo,
 double sizeInReality,
 double sizeInPixels,
 double centerX,
 double centerY
 )
{
  double mx = centerX;
  double my = centerY;
  double cx = cameraInfo.opticalCenter.x;
  double cy = cameraInfo.opticalCenter.y;
  double focalLenPow2 = cameraInfo.focalLenPow2;
  double sqrImgRadius = (mx-cx)*(mx-cx) + (my-cy)*(my-cy);
  double imgDistance = sqrt(focalLenPow2 + sqrImgRadius);
  return imgDistance*sizeInReality/(sizeInPixels + 0.00000001);
}

double Geometry::getDistanceByAngleSize
(
 double sizeInReality,
 double sizeAsAngle
 )
{
  return (sizeInReality / 2.0) / tan(sizeAsAngle / 2.0 + 0.00000001);
}

double Geometry::getBallDistanceByAngleSize
(
 double sizeInReality,
 double sizeAsAngle
 )
{
  return (sizeInReality / 2.0) / sin(sizeAsAngle / 2.0 + 0.00000001);
}

double Geometry::getSizeByDistance
(
 const CameraInfo& cameraInfo,
 double sizeInReality,
 double distance
)
{
  double xFactor = cameraInfo.focalLength;
  return sizeInReality / distance * xFactor;
}


double Geometry::getSizeByDistance
(
 double sizeInReality,
 double distance,
 double imageWidthPixels,
 double imageWidthAngle
)
{
  double xFactor = imageWidthPixels / tan(imageWidthAngle / 2.0) / 2.0;
//  return int(sizeInReality / double(distance - distancePanCenterToCamera) * xFactor);
  return sizeInReality / distance * xFactor;
}

Geometry::Line Geometry::calculateHorizon
(
const CameraMatrix& cameraMatrix,
const CameraInfo& cameraInfo
)
{
  Line horizon;
  double r31 = cameraMatrix.rotation.c[0].z;
  double r32 = cameraMatrix.rotation.c[1].z;
  double r33 = cameraMatrix.rotation.c[2].z;
  
  if(r33 == 0) 
    r33 = 0.00001;
  
  double x1 = 0,
         x2 = cameraInfo.resolutionWidth - 1,
         v1 = cameraInfo.focalLength,
         v2 = cameraInfo.opticalCenter.x,
         v3 = cameraInfo.opticalCenter.y,
         y1 = (v3 * r33 + r31 * v1 + r32 * v2) / r33,
         y2 = (v3 * r33 + r31 * v1 - r32 * v2) / r33;
  
  // Mirror ends of horizon if Camera rotated to the left  
  if((cameraMatrix.rotation * Vector3<double>(0,0,1)).z < 0)
  {
    double t = x1;
    x1 = x2;
    x2 = t;
    t = y1;
    y1 = y2;
    y2 = t;
  }

  horizon.base.x = (x1 + x2) / 2.0;
  horizon.base.y = (y1 + y2) / 2.0;
  horizon.direction.x = x2 - x1;
  horizon.direction.y = y2 - y1;
  horizon.normalizeDirection();
  return horizon;
}


int Geometry::calculateLineSize
(
 const Vector2<int>& pointInImage, 
 const CameraMatrix& cameraMatrix,
 const CameraInfo& cameraInfo
 )
{
  Vector2<int> pointOnField; //position on field, relative to robot
  if(Geometry::calculatePointOnField(pointInImage.x, pointInImage.y, cameraMatrix, cameraInfo, pointOnField))
  {
    int distance = (int) sqrt(sqr(cameraMatrix.translation.z) + sqr(pointOnField.abs()));
    return (int)Geometry::getSizeByDistance(cameraInfo, 25, distance);
  }
  else
  {
    return 0;
  }
}

Geometry::CorrectedCoords Geometry::radialCorrectionLUT[cameraResolutionHeight_ERS7][cameraResolutionWidth_ERS7];

void Geometry::setupRadialCorrection(const CameraInfo& cameraInfo)
{
  double corrX, corrY;
  unsigned char cX, cY;
  for(int x=0; x<cameraInfo.resolutionWidth; x++)
    for(int y=0; y<cameraInfo.resolutionHeight; y++)
    {
      radialDistortionCorrection(cameraInfo, x, y, corrX, corrY);
      cX = (unsigned char) (corrX+0.5);
      cY = (unsigned char) (corrY+0.5);
      radialCorrectionLUT[y][x].x = cX;
      radialCorrectionLUT[y][x].y = cY;
    }
}

---