Modules/ImageProcessor/ImageProcessorTools/CircleCalculation.cpp

Go to the documentation of this file.

/** 
* @file CircleCalculation.cpp
* Implementation of class CircleCalculation.
*
* @author <A href=mailto:juengel@informatik.hu-berlin.de>Matthias Jüngel</A>
*/

#include "CircleCalculation.h"
#include "Tools/Debugging/DebugDrawings.h"

#include <limits.h>

void CircleCalculation::init()
{
  for(int i = 0; i < numberOfBallPointSets; i++)
  {
    numberOfBallPoints[i] = 0;
  }
}

void CircleCalculation::addBallPoint(int set, int x, int y, bool isBottom)
{
  ballPoints[numberOfBallPoints[set]][set].x = x;
  ballPoints[numberOfBallPoints[set]][set].y = y;
  ballPoints[numberOfBallPoints[set]][set].isBottom = isBottom;
  numberOfBallPoints[set]++;
}

int CircleCalculation::getNumberOfBallPoints(int set)
{
  return numberOfBallPoints[set];
}
bool CircleCalculation::createCircle(int set, Geometry::Circle& circle)
{
  int point1, point2, point3;
  //Determine 3 points out of all ballPoints ...
  if(select3Points(point1, point2, point3, set))
  {
    CIRCLE(imageProcessor_ball3, ballPoints[point1][set].x, ballPoints[point1][set].y, 6, 2, Drawings::ps_solid, Drawings::red);
    CIRCLE(imageProcessor_ball3, ballPoints[point2][set].x, ballPoints[point2][set].y, 6, 2, Drawings::ps_solid, Drawings::red);
    CIRCLE(imageProcessor_ball3, ballPoints[point3][set].x, ballPoints[point3][set].y, 6, 2, Drawings::ps_solid, Drawings::red);

    //... and determine a circle.
    Vector2<int> center = cutMiddlePerpendiculars(ballPoints[point1][set], ballPoints[point2][set], ballPoints[point3][set]);
    double ballRadiusInImage = (center - ballPoints[point1][set]).abs();

    circle.center.x = center.x;
    circle.center.y = center.y;
    circle.radius = ballRadiusInImage;

    CIRCLE(imageProcessor_ball4, circle.center.x, circle.center.y, circle.radius, 3, Drawings::ps_solid, Drawings::red);
    
    return true;
  }
  return false;
}

bool CircleCalculation::getBoundary (Boundary<int>& ballBoundary, int set)
{
  if(numberOfBallPoints[set] < 1) return false;
  else
  {
    ballBoundary.x.min = INT_MAX;
    ballBoundary.y.min = INT_MAX;
    ballBoundary.x.max = 0;
    ballBoundary.y.max = 0;
    for(int i = 0; i < numberOfBallPoints[set]; i++)
    {
      ballBoundary.add(ballPoints[i][set]);
    }
/*
for(int i = 0; i < numberOfBallPoints[set]; i++)
    {
      if(ballPoints[i][set].x > bottomRight.x) bottomRight.x = ballPoints[i][set].x;
      if(ballPoints[i][set].y > bottomRight.y) bottomRight.y = ballPoints[i][set].y;
      if(ballPoints[i][set].x < topLeft.x) topLeft.x = ballPoints[i][set].x;
      if(ballPoints[i][set].y < topLeft.y) topLeft.y = ballPoints[i][set].y;
    }*/
    return true;
  }
}
bool CircleCalculation::select3Points(int& point1, int& point2, int& point3, int set)
{
  double maxDist = 0;
  int i;
  
  // first select the two points with largest distance from each other
  for(i = 0; i < numberOfBallPoints[set] - 1; ++i)
  {
    for(int j = i + 1; j < numberOfBallPoints[set] ; ++j)
    {
      double dist = Vector2<double>(ballPoints[i][set].x - ballPoints[j][set].x,
        ballPoints[i][set].y - ballPoints[j][set].y).abs();
      if(dist > maxDist)
      {
        maxDist = dist;
        point1 = i;
        point2 = j;
      }
    }
  }
  if(maxDist > 0)
  {
    // then select a third point that is farest away from the other two.
    // point 3 must form a curve with points 1 and 2, so the sum of both distances
    // must be larger then the distance between the first two points.
    maxDist += 0.1;
    point3 = -1;
    for(i = 0; i < numberOfBallPoints[set]; ++i)
    {
      if(i != point1 && i != point2)
      {
        double dist = Vector2<double>(ballPoints[i][set].x - ballPoints[point1][set].x,
          ballPoints[i][set].y - ballPoints[point1][set].y).abs() +
          Vector2<double>(ballPoints[i][set].x - ballPoints[point2][set].x,
          ballPoints[i][set].y - ballPoints[point2][set].y).abs();
        if(dist > maxDist)
        {
          maxDist = dist;
          point3 = i;
        }
      }
    }
    return point3 != -1;
  }
  else
    return false;
}

Vector2<int> CircleCalculation::cutMiddlePerpendiculars(
  Vector2<int>& v1,
  Vector2<int>& v2,
  Vector2<int>& v3) const
{
  Pose2D p1(atan2((double)(v1.y - v3.y), (double)(v1.x - v3.x)) + pi_2,
    Vector2<double>((v1.x + v3.x) / 2, (v1.y + v3.y) / 2)),
    p2(atan2((double)(v2.y - v3.y), (double)(v2.x - v3.x)) + pi_2,
    Vector2<double>((v2.x + v3.x) / 2, (v2.y + v3.y) / 2)),
    p = p2 - p1;
  double sinAngle = sin(p.getAngle()); 
  if(sinAngle)
  {
    p = p1 + Pose2D(Vector2<double>(p.translation.x - p.translation.y * cos(p.getAngle()) / sinAngle, 0));
    return Vector2<int>(int(p.translation.x), int(p.translation.y));
  }
  else
    return v1;
}

int CircleCalculation::paintBallPoints
(
 int set, 
 int minIndex, 
 int drawingID,
 Drawings::Color color, 
 int size)
{
  switch(drawingID)
  {
  case 1:
    COMPLEX_DRAWING(imageProcessor_ball1,
      for(int i = minIndex; i < numberOfBallPoints[set]; i++)
      {
        CIRCLE(imageProcessor_ball1, ballPoints[i][set].x, ballPoints[i][set].y, size, 2, Drawings::ps_solid, color);
      }
      );
      break;
  case 2:
    COMPLEX_DRAWING(imageProcessor_ball2,
      for(int i = minIndex; i < numberOfBallPoints[set]; i++)
      {
        CIRCLE(imageProcessor_ball2, ballPoints[i][set].x, ballPoints[i][set].y, size, 2, Drawings::ps_solid, color);
      }
      );
      break;
  case 3:
    COMPLEX_DRAWING(imageProcessor_ball3,
      for(int i = minIndex; i < numberOfBallPoints[set]; i++)
      {
        CIRCLE(imageProcessor_ball3, ballPoints[i][set].x, ballPoints[i][set].y, size, 2, Drawings::ps_solid, color);
      }
      );
      break;
  }

  return numberOfBallPoints[set];
}

/*
 * Change log :
 * 
 * $Log: CircleCalculation.cpp,v $
 * Revision 1.1.1.1  2004/05/22 17:19:46  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.3  2004/02/12 14:40:35  juengel
 * changed visualzation.
 *
 * Revision 1.2  2003/12/15 11:46:14  juengel
 * Introduced CameraInfo
 *
 * Revision 1.1  2003/12/04 09:44:23  juengel
 * Added CircleCalculation
 *
 */

---