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::calculatePointOnField
(
 const int x,
 const int y,
 const CameraMatrix& cameraMatrix,
 const CameraInfo& cameraInfo,
 Vector2<int>& 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 = int((a1 * b3 - a3 * b1) / b3 + 0.5);
  pointOnField.y = int((a2 * b3 - a3 * b2) / b3 + 0.5);

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

bool Geometry::calculatePointOnField
(
 const int x,
 const int y,
 const CameraMatrix& cameraMatrix,
 const CameraMatrix& prevCameraMatrix,
 const CameraInfo& cameraInfo,
 Vector2<int>& pointOnField
 )
{
  if(cameraInfo.simulated)
    return calculatePointOnField(x, y, cameraMatrix, cameraInfo, pointOnField);
  else
  {
    Vector2<int> 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 += int((pointOnField2.x - pointOnField.x) * scaleFactor);
      pointOnField.y += int((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);
  relativeCoord.x = distance * cos(Geometry::angleTo(robotPose.getPose(), fieldCoord));
  relativeCoord.y = distance * sin(Geometry::angleTo(robotPose.getPose(), fieldCoord));
  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.0001);
}

double Geometry::getDistanceBySize
(
 const CameraInfo& cameraInfo,
 double sizeInReality,
 double sizeInPixels,
 int centerX,
 int 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.0001);
}

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

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

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

/*
* Change log :
* 
* $Log: Geometry.cpp,v $
* Revision 1.11  2004/06/16 13:39:14  thomas
* update edge-specialist
*
* Revision 1.10  2004/06/14 23:20:08  spranger
* -changed some functions in Geometry from int to double including ballradius(fieldimensions)
* -maybe all Geometric-functions in Geometry should be as precise as possible, avoiding int to double conversion errors
*
* Revision 1.9  2004/06/13 21:22:14  nistico
* Minor bug fixes
*
* Revision 1.8  2004/06/05 18:36:04  nistico
* Cleanup
*
* Revision 1.7  2004/06/05 07:58:22  roefer
* Compensation for motion distortions of images
*
* Revision 1.6  2004/06/04 21:47:13  juengel
* Removed unused stuff.
*
* Revision 1.5  2004/06/04 21:35:10  juengel
* Added getBallDistanceByAngleSize.
*
* Revision 1.4  2004/06/04 08:45:59  juengel
* Added fieldCoord2Relative.
*
* Revision 1.3  2004/05/26 15:54:26  dueffert
* camera matrix calculation cleaned up
*
* Revision 1.2  2004/05/22 22:52:03  juengel
* Renamed ballP_osition to ballModel.
*
* Revision 1.1.1.1  2004/05/22 17:37:07  cvsadm
* created new repository GT2004_WM
*
* Revision 1.20  2004/05/12 11:38:22  nistico
* RadialDistortionCorrectionFast is now static
*
* Revision 1.19  2004/05/07 15:16:25  nistico
* All geometry calculations are making use of intrinsic functions.
* I updated most of the images processors to make correct use of this.
* Please test if there are any problems, because i'm going to remove the
* old code soon.
*
* Revision 1.18  2004/04/18 18:14:53  nistico
* USE_INTRINSIC layout removed.
* All functions properly replicated in intrinsic version.
* However, image processor (MSH2004) making use of them get distorted visualization
* of percepts, because drawing functions use the old parameters.
* It has to be decided wheter to fully move to intrinsic, or discard it.
*
* Revision 1.17  2004/04/08 15:33:08  wachter
* GT04 checkin of Microsoft-Hellounds
*
* Revision 1.17  2004/03/28 18:35:19  nistico
* Temporarily created MSH2004ImageProcessor2, it has some important advancements compared to
* the MSH2004ImageProcessor1 but it is untested on real robots yet, hence the creation of the
* new module.
* It will be removed soon, don't use it!
*
* Revision 1.16  2004/03/27 16:20:23  nistico
* Little bug fixed (unrelated to ball measurement though)
*
* Revision 1.15  2004/03/25 15:13:00  nistico
* Ball detection improved in MSH2004ImageProcessor
* Ball distance measurement stabilized in MSH2004BallLocator (NOTE: to work with RIP, a small change has to be done in RIP code, just ask me...)
* Fixed bugs with visualization of percepts (using intrinsic camera parameter based measurements)
*
* Revision 1.14  2004/03/23 12:41:36  nistico
* MSH2004ImageProcessor improvements, distances now measured with intrinsic camera parameters
*
* Revision 1.16  2004/04/07 13:00:47  risler
* ddd checkin after go04 - second part
*
* Revision 1.2  2004/03/29 10:15:21  risler
* no message
*
* Revision 1.1.1.1  2004/03/29 08:28:42  Administrator
* initial transfer from tamara
*
* Revision 1.14  2004/03/29 06:28:18  schmitt
* Improved horizonCalculation
*
* Revision 1.15  2004/03/29 09:38:25  tim
* did not compile with gcc
*
* Revision 1.14  2004/03/29 06:28:18  schmitt
* Improved horizonCalculation
*
* Revision 1.13  2004/03/12 13:43:29  nistico
* - Bugs fixed in USE_INTRINSIC mode
*
* Revision 1.12  2004/03/09 11:33:21  nistico
* - Intrinsic parameters based measurements can now be triggered through a single conditional compilation
* switch located in CameraInfo.h
* - Implemented fast (look-up table based) radial distortion correction
*
* Revision 1.11  2004/02/10 10:48:05  nistico
* Introduced Intrinsic camera parameters to perform geometric calculations (distance, angle, size...) without opening angle
* Implemented radial distortion correction function
* Implemented ball distance calculation based on size and intrinsic params (potentially more stable)
* To Be Done: calculate intrinsic params for ERS7, as soon as we get our puppies back
*
* Revision 1.10  2004/01/28 18:40:43  mkunz
* bug fixed in precalculated camera matrix
*
* Revision 1.9  2004/01/27 21:23:40  roefer
* Camera matrix for ERS-7 is wrong. Reactivated simple version.
*
* Revision 1.8  2004/01/27 15:24:16  roefer
* Sorry, checked in too much
*
* Revision 1.7  2004/01/23 00:13:24  roefer
* ERS-7 simulation, first draft
*
* Revision 1.6  2004/01/08 18:20:54  mkunz
* new precalculated cameramatrix for ERS7
*
* Revision 1.4  2004/01/01 10:58:52  roefer
* RobotDimensions are in a class now
*
* Revision 1.3  2003/12/15 11:47:00  juengel
* Introduced CameraInfo
*
* Revision 1.2  2003/11/15 17:12:13  juengel
* Removed distancePanCenterToCamera from getDistanceBySize
*
* Revision 1.1  2003/10/07 10:13:24  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.4  2003/09/26 16:06:40  mkunz
* old comment for CameraMatrix calculation restored
*
* Revision 1.3  2003/08/18 11:50:21  juengel
* Added calculateLineSize.
*
* Revision 1.2  2003/07/29 12:40:00  juengel
* Added calculateHorizon
*
* 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.22  2003/06/23 12:10:40  dueffert
* getIntersectionPoints...() completed
*
* Revision 1.21  2003/06/22 17:39:05  dueffert
* getIntersectionPointsOfLineAndRectangle nearly complete
*
* Revision 1.20  2003/06/22 17:25:57  dueffert
* getIntersectionPointsOfLineAndRectangle added
*
* Revision 1.19  2003/06/22 17:09:58  juengel
* Added int version of function "distance".
*
* Revision 1.18  2003/06/17 13:43:56  dueffert
* numbers replaced by constants
*
* Revision 1.17  2003/06/15 14:26:44  jhoffman
* + moved "relative2FieldCoord" to Geometry
* + added member function to ballp_osition to calculate the propagated position and speed for a given time
* + propagated speed and time calculation using exponential decay instead of using an iterative calculation
* + in motion you can now use propageted ball pos at 125 Hz rather then the framerate determined by cognition
*
* Revision 1.16  2003/06/12 16:54:09  juengel
* Added getDistanceBySize and getSizeByDistance.
*
* Revision 1.15  2003/06/10 17:58:35  juengel
* Fixed bug in Line Clipping.
*
* Revision 1.14  2003/04/16 07:00:17  roefer
* Bremen GO checkin
*
* Revision 1.13  2003/04/15 15:52:07  risler
* DDD GO 2003 code integrated
*
* Revision 1.12  2003/03/26 17:46:00  max
* added getDistanceToEdge
* optimized getDistanceToLine
*
* Revision 1.12  2003/04/01 16:51:09  roefer
* CameraMatrix contains validity and is calculated in Geometry
*
* Revision 1.11  2003/03/18 13:48:18  roefer
* Results of calculatePointOnField limited
*
* Revision 1.10  2003/01/19 11:38:08  juengel
* int versions of clipLineWithQuadrangle and isPointInsideRectangle
*
* Revision 1.9  2003/01/15 08:19:48  juengel
* Added "int" - version of getIntersectionOfLines.
*
* Revision 1.8  2002/12/14 19:45:54  roefer
* paintLinesPerceptToImageView added
*
* Revision 1.7  2002/12/04 12:24:35  juengel
* Changed parameter "pointOnField" of method calculatePointOnField() from Vector2<double> to Vector2<int>.
*
* Revision 1.6  2002/12/02 12:02:10  juengel
* Finished cohen sutherland line clipping.
*
* Revision 1.5  2002/12/01 17:37:45  juengel
* Worked at clipping functions.
*
* Revision 1.4  2002/11/28 18:53:53  juengel
* RadarViewer3D shows images projected on ground.
*
* Revision 1.3  2002/11/19 15:43:04  dueffert
* doxygen comments corrected
*
* Revision 1.2  2002/09/22 18:40:52  risler
* added new math functions, removed GTMath library
*
* Revision 1.1  2002/09/22 09:15:34  risler
* Geometry.h moved to directory Math
*
* Revision 1.1  2002/09/10 15:53:58  cvsadm
* Created new project GT2003 (M.L.)
* - Cleaned up the /Src/DataTypes directory
* - Removed challenge related source code
* - Removed processing of incoming audio data
* - Renamed AcousticMessage to SoundRequest
*
* Revision 1.3  2002/06/13 18:22:10  dueffert
* ray intersection added
*
* Revision 1.2  2002/05/29 16:06:05  dueffert
* warning removed
*
* Revision 1.1.1.1  2002/05/10 12:40:33  cvsadm
* Moved GT2002 Project from ute to tamara.
*
* Revision 1.12  2002/04/25 14:50:37  kallnik
* changed double/float to double
* added several #include GTMath
*
* PLEASE use double
*
* Revision 1.11  2002/04/16 15:44:04  dueffert
* vectorTo(Pose, Vector) added
*
* Revision 1.2  2002/04/11 14:20:04  dueffert
* dribble skill filled
*
* Revision 1.10  2002/04/08 19:53:14  juengel
* Drawing of percept collections in images added.
*
* Revision 1.9  2002/04/06 02:30:55  loetzsch
* added angleTo and distanceTo functions
*
* Revision 1.8  2002/04/04 18:43:31  juengel
* Disatance and distance from point to line added.
*
* Revision 1.7  2002/04/02 10:30:34  juengel
* GridImageProcessor enhanced.
*
* Revision 1.6  2002/03/29 14:55:48  juengel
* "horizon aligned grid" started.
*
* Revision 1.5  2002/03/18 09:45:48  kallnik
* GTMathTable updated
* GTMathConfig updated
* several doubles changed in GTMathValue
*
* Revision 1.4  2002/02/12 22:42:42  juengel
* ImageProcessorTester improved.
*
* Revision 1.3  2002/02/11 00:56:36  loetzsch
* added a constructor for class Circle
*
* Revision 1.2  2002/01/23 12:34:18  juengel
* Kommentare eingefügt
*
* Revision 1.1  2002/01/22 14:54:47  juengel
* Geometry eingeführt
*
*
*/

---