| 2064 | int ODRoad::speedlimit() const { return speed_limit; } |
| 2065 | |
| 2066 | void ODRoad::roadGeom(std::vector<std::pair<double, double>>& geom, const std::vector<double>* interval) const { |
| 2067 | geom.clear(); |
| 2068 | if (interval && !interval->empty()) { |
| 2069 | geom.resize(interval->size()); |
| 2070 | size_t index = 0; |
| 2071 | for (size_t i = 0; i < interval->size();) { |
| 2072 | if (index + 1 < ref_line.size() && interval->at(i) > ref_line[index + 1]->offset()) { |
| 2073 | index++; |
| 2074 | } else { |
| 2075 | double x = 0, y = 0; |
| 2076 | ref_line[index]->getGeom(interval->at(i), x, y); |
| 2077 | assert(!std::isnan(x)); |
| 2078 | assert(!std::isnan(y)); |
| 2079 | geom[i] = std::make_pair(x, y); |
| 2080 | i++; |
| 2081 | } |
| 2082 | } |
| 2083 | } else { |
| 2084 | for (auto& _geom_ptr : ref_line) { |
| 2085 | double _x = _geom_ptr->startX(); |
| 2086 | double _y = _geom_ptr->startY(); |
| 2087 | std::vector<std::pair<double, double>> _cur_geom; |
| 2088 | _geom_ptr->getGeom(_cur_geom); |
| 2089 | if (_cur_geom.empty()) { |
| 2090 | continue; |
| 2091 | } |
| 2092 | for (auto& _point : _cur_geom) _point.first += _x, _point.second += _y; |
| 2093 | if (geom.empty()) { |
| 2094 | geom.assign(_cur_geom.begin(), _cur_geom.end()); |
| 2095 | } else { |
| 2096 | geom.insert(geom.end(), _cur_geom.begin() + 1, _cur_geom.end()); |
| 2097 | } |
| 2098 | } |
| 2099 | } |
| 2100 | } |
| 2101 | |
| 2102 | void ODRoad::roadTangent(std::vector<std::pair<double, double>>& tangent, const std::vector<double>* interval) const { |
| 2103 | tangent.clear(); |