- // Ax+By+C=0
- // normal vector: n[n_x, -n_y]
- float n_x = cartes[begin].y - cartes[end].y;
- float n_y = cartes[begin].x - cartes[end].x;
-
- float A = n_x;
- float B = -n_y;
- float C = n_y*cartes[end].y - n_x*cartes[end].x;
-
- int line_break_point = 0;
- float dist, dist_max = 0;
-
- for (int i = begin; i < end; i++) {
- // distance point from the line
- dist = fabs( (cartes[i].x*A + cartes[i].y*B + C) / sqrt(A*A + B*B));
-
- if (dist > dist_max) {
- dist_max = dist;
- line_break_point = i;
- }
- }
-
- if (dist_max > LINE_ERROR_THRESHOLD) {
- line_fitting(begin, line_break_point);
- line_fitting(line_break_point, end);
- } else {
-
- lines.push_back(cartes[begin]);
- lines.push_back(cartes[end]);
- cout << endl << "begin X: " << cartes[begin].x << " Y: " << cartes[begin].y << endl << flush;
- cout << "end X: " << cartes[end].x << " Y: " << cartes[end].y << endl << endl << flush;
-
- }
+ if (line_break_point == begin)
+ line_break_point++;
+ else if (line_break_point == end)
+ line_break_point--;
+ line_fitting(begin, line_break_point);
+ line_fitting(line_break_point, end);