Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/ImageProcessor/ImageProcessorTools/CircleCalculation.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file CircleCalculation.cpp
    00003 * Implementation of class CircleCalculation.
    00004 *
    00005 * @author <A href=mailto:juengel@informatik.hu-berlin.de>Matthias Jüngel</A>
    00006 */
    00007
    00008 #include "CircleCalculation.h"
    00009 #include "Tools/Debugging/DebugDrawings.h"
    00010
    00011 #include <limits.h>
    00012
    00013 void CircleCalculation::init()
    00014 {
    00015   for(int i = 0; i < numberOfBallPointSets; i++)
    00016   {
    00017     numberOfBallPoints[i] = 0;
    00018   }
    00019 }
    00020
    00021 void CircleCalculation::addBallPoint(int set, int x, int y, bool isBottom)
    00022 {
    00023   ballPoints[numberOfBallPoints[set]][set].x = x;
    00024   ballPoints[numberOfBallPoints[set]][set].y = y;
    00025   ballPoints[numberOfBallPoints[set]][set].isBottom = isBottom;
    00026   numberOfBallPoints[set]++;
    00027 }
    00028
    00029 int CircleCalculation::getNumberOfBallPoints(int set)
    00030 {
    00031   return numberOfBallPoints[set];
    00032 }
    00033 bool CircleCalculation::createCircle(int set, Geometry::Circle& circle)
    00034 {
    00035   int point1, point2, point3;
    00036   //Determine 3 points out of all ballPoints ...
    00037   if(select3Points(point1, point2, point3, set))
    00038   {
    00039     CIRCLE(imageProcessor_ball3, ballPoints[point1][set].x, ballPoints[point1][set].y, 6, 2, Drawings::ps_solid, Drawings::red);
    00040     CIRCLE(imageProcessor_ball3, ballPoints[point2][set].x, ballPoints[point2][set].y, 6, 2, Drawings::ps_solid, Drawings::red);
    00041     CIRCLE(imageProcessor_ball3, ballPoints[point3][set].x, ballPoints[point3][set].y, 6, 2, Drawings::ps_solid, Drawings::red);
    00042
    00043     //... and determine a circle.
    00044     Vector2<int> center = cutMiddlePerpendiculars(ballPoints[point1][set], ballPoints[point2][set], ballPoints[point3][set]);
    00045     double ballRadiusInImage = (center - ballPoints[point1][set]).abs();
    00046
    00047     circle.center.x = center.x;
    00048     circle.center.y = center.y;
    00049     circle.radius = ballRadiusInImage;
    00050
    00051     CIRCLE(imageProcessor_ball4, circle.center.x, circle.center.y, circle.radius, 3, Drawings::ps_solid, Drawings::red);
    00052
    00053     return true;
    00054   }
    00055   return false;
    00056 }
    00057
    00058 bool CircleCalculation::getBoundary (Boundary<int>& ballBoundary, int set)
    00059 {
    00060   if(numberOfBallPoints[set] < 1) return false;
    00061   else
    00062   {
    00063     ballBoundary.x.min = INT_MAX;
    00064     ballBoundary.y.min = INT_MAX;
    00065     ballBoundary.x.max = 0;
    00066     ballBoundary.y.max = 0;
    00067     for(int i = 0; i < numberOfBallPoints[set]; i++)
    00068     {
    00069       ballBoundary.add(ballPoints[i][set]);
    00070     }
    00071 /*
    00072 for(int i = 0; i < numberOfBallPoints[set]; i++)
    00073     {
    00074       if(ballPoints[i][set].x > bottomRight.x) bottomRight.x = ballPoints[i][set].x;
    00075       if(ballPoints[i][set].y > bottomRight.y) bottomRight.y = ballPoints[i][set].y;
    00076       if(ballPoints[i][set].x < topLeft.x) topLeft.x = ballPoints[i][set].x;
    00077       if(ballPoints[i][set].y < topLeft.y) topLeft.y = ballPoints[i][set].y;
    00078     }*/
    00079     return true;
    00080   }
    00081 }
    00082 bool CircleCalculation::select3Points(int& point1, int& point2, int& point3, int set)
    00083 {
    00084   double maxDist = 0;
    00085   int i;
    00086
    00087   // first select the two points with largest distance from each other
    00088   for(i = 0; i < numberOfBallPoints[set] - 1; ++i)
    00089   {
    00090     for(int j = i + 1; j < numberOfBallPoints[set] ; ++j)
    00091     {
    00092       double dist = Vector2<double>(ballPoints[i][set].x - ballPoints[j][set].x,
    00093         ballPoints[i][set].y - ballPoints[j][set].y).abs();
    00094       if(dist > maxDist)
    00095       {
    00096         maxDist = dist;
    00097         point1 = i;
    00098         point2 = j;
    00099       }
    00100     }
    00101   }
    00102   if(maxDist > 0)
    00103   {
    00104     // then select a third point that is farest away from the other two.
    00105     // point 3 must form a curve with points 1 and 2, so the sum of both distances
    00106     // must be larger then the distance between the first two points.
    00107     maxDist += 0.1;
    00108     point3 = -1;
    00109     for(i = 0; i < numberOfBallPoints[set]; ++i)
    00110     {
    00111       if(i != point1 && i != point2)
    00112       {
    00113         double dist = Vector2<double>(ballPoints[i][set].x - ballPoints[point1][set].x,
    00114           ballPoints[i][set].y - ballPoints[point1][set].y).abs() +
    00115           Vector2<double>(ballPoints[i][set].x - ballPoints[point2][set].x,
    00116           ballPoints[i][set].y - ballPoints[point2][set].y).abs();
    00117         if(dist > maxDist)
    00118         {
    00119           maxDist = dist;
    00120           point3 = i;
    00121         }
    00122       }
    00123     }
    00124     return point3 != -1;
    00125   }
    00126   else
    00127     return false;
    00128 }
    00129
    00130 Vector2<int> CircleCalculation::cutMiddlePerpendiculars(
    00131   Vector2<int>& v1,
    00132   Vector2<int>& v2,
    00133   Vector2<int>& v3) const
    00134 {
    00135   Pose2D p1(atan2((double)(v1.y - v3.y), (double)(v1.x - v3.x)) + pi_2,
    00136     Vector2<double>((v1.x + v3.x) / 2, (v1.y + v3.y) / 2)),
    00137     p2(atan2((double)(v2.y - v3.y), (double)(v2.x - v3.x)) + pi_2,
    00138     Vector2<double>((v2.x + v3.x) / 2, (v2.y + v3.y) / 2)),
    00139     p = p2 - p1;
    00140   double sinAngle = sin(p.getAngle());
    00141   if(sinAngle)
    00142   {
    00143     p = p1 + Pose2D(Vector2<double>(p.translation.x - p.translation.y * cos(p.getAngle()) / sinAngle, 0));
    00144     return Vector2<int>(int(p.translation.x), int(p.translation.y));
    00145   }
    00146   else
    00147     return v1;
    00148 }
    00149
    00150 int CircleCalculation::paintBallPoints
    00151 (
    00152  int set,
    00153  int minIndex,
    00154  int drawingID,
    00155  Drawings::Color color,
    00156  int size)
    00157 {
    00158   switch(drawingID)
    00159   {
    00160   case 1:
    00161     COMPLEX_DRAWING(imageProcessor_ball1,
    00162       for(int i = minIndex; i < numberOfBallPoints[set]; i++)
    00163       {
    00164         CIRCLE(imageProcessor_ball1, ballPoints[i][set].x, ballPoints[i][set].y, size, 2, Drawings::ps_solid, color);
    00165       }
    00166       );
    00167       break;
    00168   case 2:
    00169     COMPLEX_DRAWING(imageProcessor_ball2,
    00170       for(int i = minIndex; i < numberOfBallPoints[set]; i++)
    00171       {
    00172         CIRCLE(imageProcessor_ball2, ballPoints[i][set].x, ballPoints[i][set].y, size, 2, Drawings::ps_solid, color);
    00173       }
    00174       );
    00175       break;
    00176   case 3:
    00177     COMPLEX_DRAWING(imageProcessor_ball3,
    00178       for(int i = minIndex; i < numberOfBallPoints[set]; i++)
    00179       {
    00180         CIRCLE(imageProcessor_ball3, ballPoints[i][set].x, ballPoints[i][set].y, size, 2, Drawings::ps_solid, color);
    00181       }
    00182       );
    00183       break;
    00184   }
    00185
    00186   return numberOfBallPoints[set];
    00187 }