| 2369 | } |
| 2370 | |
| 2371 | void ODRoad::roadControlPoint(std::string& controltype, std::vector<std::vector<double>>& points) { |
| 2372 | for (ODGeomPtr& _geom_ptr : ref_line) { |
| 2373 | if (_geom_ptr->getName() == "line") { |
| 2374 | std::vector<double> pt1; |
| 2375 | pt1.push_back(_geom_ptr->startX()); |
| 2376 | pt1.push_back(_geom_ptr->startY()); |
| 2377 | pt1.push_back(_geom_ptr->getHeading(0)); |
| 2378 | points.push_back(pt1); |
| 2379 | std::vector<double> pt2; |
| 2380 | double x; |
| 2381 | double y; |
| 2382 | _geom_ptr->getGeom(_geom_ptr->length(), x, y); |
| 2383 | pt2.push_back(x); |
| 2384 | pt2.push_back(y); |
| 2385 | pt2.push_back(_geom_ptr->getHeading(_geom_ptr->length())); |
| 2386 | points.push_back(pt2); |
| 2387 | controltype = "line"; |
| 2388 | } else if (_geom_ptr->getName() == "arc") { |
| 2389 | std::vector<double> pt1; |
| 2390 | pt1.push_back(_geom_ptr->startX()); |
| 2391 | pt1.push_back(_geom_ptr->startY()); |
| 2392 | pt1.push_back(_geom_ptr->getHeading(0)); |
| 2393 | points.push_back(pt1); |
| 2394 | std::vector<double> pt2; |
| 2395 | double x; |
| 2396 | double y; |
| 2397 | double s = _geom_ptr->length() + _geom_ptr->offset(); |
| 2398 | _geom_ptr->getGeom(s, x, y); |
| 2399 | pt2.push_back(x); |
| 2400 | pt2.push_back(y); |
| 2401 | pt2.push_back(_geom_ptr->getHeading(s)); |
| 2402 | points.push_back(pt2); |
| 2403 | controltype = "arc"; |
| 2404 | } else if (_geom_ptr->getName() == "paramPoly3") { |
| 2405 | if (points.size() == 0) { |
| 2406 | std::vector<double> pt1; |
| 2407 | pt1.push_back(_geom_ptr->startX()); |
| 2408 | pt1.push_back(_geom_ptr->startY()); |
| 2409 | pt1.push_back(_geom_ptr->getHeading(0)); |
| 2410 | points.push_back(pt1); |
| 2411 | } |
| 2412 | std::vector<double> pt2; |
| 2413 | double x; |
| 2414 | double y; |
| 2415 | double s = _geom_ptr->length() + _geom_ptr->offset(); |
| 2416 | _geom_ptr->getGeom(s, x, y); |
| 2417 | pt2.push_back(x); |
| 2418 | pt2.push_back(y); |
| 2419 | pt2.push_back(_geom_ptr->getHeading(s)); |
| 2420 | points.push_back(pt2); |
| 2421 | controltype = "catmullrom"; |
| 2422 | } else { |
| 2423 | controltype = "none"; |
| 2424 | } |
| 2425 | } |
| 2426 | if (controltype == "catmullrom") { |
| 2427 | // |
| 2428 | std::vector<double> pt1; |