| 69 | return res; |
| 70 | } |
| 71 | vector<Point2f> Spline::splineInterpStep(vector<Point2f> tmp_line, double step) { |
| 72 | vector<Point2f> res; |
| 73 | /* |
| 74 | if (tmp_line.size() == 2) { |
| 75 | double x1 = tmp_line[0].x; |
| 76 | double y1 = tmp_line[0].y; |
| 77 | double x2 = tmp_line[1].x; |
| 78 | double y2 = tmp_line[1].y; |
| 79 | |
| 80 | for (double yi = std::min(y1, y2); yi < std::max(y1, y2); yi += step) { |
| 81 | double xi; |
| 82 | if (yi == y1) xi = x1; |
| 83 | else xi = (x2 - x1) / (y2 - y1) * (yi - y1) + x1; |
| 84 | res.push_back(Point2f(xi, yi)); |
| 85 | } |
| 86 | }*/ |
| 87 | if (tmp_line.size() == 2) { |
| 88 | double x1 = tmp_line[0].x; |
| 89 | double y1 = tmp_line[0].y; |
| 90 | double x2 = tmp_line[1].x; |
| 91 | double y2 = tmp_line[1].y; |
| 92 | tmp_line[1].x = (x1 + x2) / 2; |
| 93 | tmp_line[1].y = (y1 + y2) / 2; |
| 94 | tmp_line.push_back(Point2f(x2, y2)); |
| 95 | } |
| 96 | if (tmp_line.size() > 2) { |
| 97 | vector<Func> tmp_func; |
| 98 | tmp_func = this->cal_fun(tmp_line); |
| 99 | double ystart = tmp_line[0].y; |
| 100 | double yend = tmp_line[tmp_line.size() - 1].y; |
| 101 | bool down; |
| 102 | if (ystart < yend) down = 1; |
| 103 | else down = 0; |
| 104 | if (tmp_func.empty()) { |
| 105 | cerr << "in splineInterpStep: cal_fun failed" << endl; |
| 106 | } |
| 107 | |
| 108 | for(int j = 0; j < tmp_func.size(); j++) |
| 109 | { |
| 110 | for(double t1 = 0; t1 < tmp_func[j].h; t1 += step) |
| 111 | { |
| 112 | double x1 = tmp_func[j].a_x + tmp_func[j].b_x*t1 + tmp_func[j].c_x*pow(t1,2) + tmp_func[j].d_x*pow(t1,3); |
| 113 | double y1 = tmp_func[j].a_y + tmp_func[j].b_y*t1 + tmp_func[j].c_y*pow(t1,2) + tmp_func[j].d_y*pow(t1,3); |
| 114 | res.push_back(Point2f(x1, y1)); |
| 115 | } |
| 116 | } |
| 117 | res.push_back(tmp_line[tmp_line.size() - 1]); |
| 118 | } |
| 119 | else { |
| 120 | cerr << "in splineInterpStep: not enough points" << endl; |
| 121 | } |
| 122 | return res; |
| 123 | } |
| 124 | |
| 125 | vector<Func> Spline::cal_fun(const vector<Point2f> &point_v) |
| 126 | { |