hokuyoscan/shape_detect: Add drawing arc center (green point).

[eurobot/public.git] / src / hokuyo / shape-detect / shape_detect.cc

diff --git a/src/hokuyo/shape-detect/shape_detect.cc b/src/hokuyo/shape-detect/shape_detect.cc
index 0337bdf1bb7474b5edbdb04eeefd27e6f89ffe24..34be388c4d1d94213561e91f20c0901091feb6af 100644 (file)
--- a/src/hokuyo/shape-detect/shape_detect.cc
+++ b/src/hokuyo/shape-detect/shape_detect.cc
@@ -125,8 +125,7 @@ bool Shape_detect::fit_arc(int begin, int end, std::vector<Shape_detect::Arc> &a
                return false;
        }
 
-       int idx_middle_point = (end + begin) / 2;
-       Point rotated_point = cartes[idx_middle_point];
+       Point rotated_point = cartes[(end + begin) / 2];
        Point right = cartes[begin];
        Point left = cartes[end];
        rotated_point.x = rotated_point.x - left.x;
@@ -138,12 +137,14 @@ bool Shape_detect::fit_arc(int begin, int end, std::vector<Shape_detect::Arc> &a
 
        if (-rotated_point.x > .1 * Shape_detect::point_distance(left, right) &&
                -rotated_point.x < Shape_detect::point_distance(left, right)) {
+/*
                if (end - begin > 20) {
                        end--;
                        begin++;
                        right = cartes[begin];
                        left = cartes[end];
                }
+*/
        } else {
                //std::cout << "rotace bodu" << std::endl;
                return false;
@@ -162,11 +163,6 @@ bool Shape_detect::fit_arc(int begin, int end, std::vector<Shape_detect::Arc> &a
                slopes[i] = ma - mb;
        }
 
-       if (ma == 0)
-               ma = 1e-8;
-       if (mb == 0)
-               mb = 1e-8;
-
        float sum = 0;
        for (int i = 0; i < slopes_size; i++)
                sum = sum + slopes[i];
@@ -190,11 +186,12 @@ bool Shape_detect::fit_arc(int begin, int end, std::vector<Shape_detect::Arc> &a
                tmp.x = right.x - left.x;
                tmp.y = right.y - left.y;
                
+               angle_to_rotate = atan2(tmp.y, tmp.x);
+
                tmp = Shape_detect::rotate(tmp, -angle_to_rotate);
                
                float middle = tmp.x / 2.0;
-               float q = average - M_PI / 2.0;
-               float height = middle * tan(q);
+               float height = middle * tan(average - M_PI / 2.0);
 
                Shape_detect::Point center;
                center.x = middle;
@@ -212,6 +209,8 @@ bool Shape_detect::fit_arc(int begin, int end, std::vector<Shape_detect::Arc> &a
 
                arcs.push_back(arc);
 
+               //std::cout << std::endl << std::endl << "tmp: " << tmp.x << " , " << tmp.y << "; center: " << center.x << " , " << center.y << std::endl;
+
                //std::cout << "Detekovan" << std::endl;
                return 1;
        }