| 2324 | } |
| 2325 | |
| 2326 | void ODRoad::roadSlope(std::vector<double>& startx, std::vector<double>& length, std::vector<double>& slope) { |
| 2327 | for (size_t i = 0; i < elevation.size(); i++) { |
| 2328 | Polynomial3D& eleItr = elevation.at(i); |
| 2329 | double _a, _b, _c, _d, _s; |
| 2330 | eleItr.getParam(_a, _b, _c, _d, _s); |
| 2331 | double _length = 0; |
| 2332 | if (i != elevation.size() - 1) { |
| 2333 | double _aa, _bb, _cc, _dd, _ss; |
| 2334 | elevation.at(i + 1).getParam(_aa, _bb, _cc, _dd, _ss); |
| 2335 | _length = _ss - _s; |
| 2336 | } else { |
| 2337 | _length = this->length() - _s; |
| 2338 | } |
| 2339 | if (std::abs(_c) < 1e-6 && std::abs(_d) < 1e-6) { |
| 2340 | startx.push_back(_s); |
| 2341 | length.push_back(_length); |
| 2342 | slope.push_back(_b); |
| 2343 | continue; |
| 2344 | } else { |
| 2345 | const double SampleDistance = 30.0; |
| 2346 | int i = 0; |
| 2347 | double startx_tmp = 0.0; |
| 2348 | double length_tmp = 0.0; |
| 2349 | while (SampleDistance * i < _length) { |
| 2350 | std::vector<double> dis; |
| 2351 | startx_tmp = SampleDistance * i; |
| 2352 | length_tmp = SampleDistance; |
| 2353 | if (SampleDistance * i > _length - SampleDistance) { |
| 2354 | length_tmp = _length - SampleDistance * i; |
| 2355 | } |
| 2356 | dis.push_back(startx_tmp); |
| 2357 | dis.push_back(startx_tmp + length_tmp); |
| 2358 | |
| 2359 | std::vector<double> value; |
| 2360 | eleItr.sampleValue(dis, value); |
| 2361 | double slope_tmp = (value[1] - value[0]) / length_tmp; |
| 2362 | startx.push_back(_s + startx_tmp); |
| 2363 | length.push_back(length_tmp); |
| 2364 | slope.push_back(slope_tmp); |
| 2365 | i++; |
| 2366 | } |
| 2367 | } |
| 2368 | } |
| 2369 | } |
| 2370 | |
| 2371 | void ODRoad::roadControlPoint(std::string& controltype, std::vector<std::vector<double>>& points) { |
| 2372 | for (ODGeomPtr& _geom_ptr : ref_line) { |