| 2131 | } |
| 2132 | |
| 2133 | void ODRoad::roadNormal(std::vector<std::pair<double, double>>& normal, const std::vector<double>* interval) const { |
| 2134 | normal.clear(); |
| 2135 | if (interval && !interval->empty()) { |
| 2136 | normal.resize(interval->size()); |
| 2137 | size_t index = 0; |
| 2138 | for (size_t i = 0; i < interval->size();) { |
| 2139 | if (index + 1 < ref_line.size() && interval->at(i) >= ref_line[index + 1]->offset()) { |
| 2140 | index++; |
| 2141 | } else { |
| 2142 | double x = 0, y = 0; |
| 2143 | ref_line[index]->getNormal(interval->at(i), x, y); |
| 2144 | normal[i] = std::make_pair(x, y); |
| 2145 | i++; |
| 2146 | } |
| 2147 | } |
| 2148 | } else { |
| 2149 | for (auto& _geom_ptr : ref_line) { |
| 2150 | std::vector<std::pair<double, double>> _cur_normal; |
| 2151 | _geom_ptr->getNormal(_cur_normal); |
| 2152 | if (_cur_normal.empty()) { |
| 2153 | continue; |
| 2154 | } |
| 2155 | if (normal.empty()) { |
| 2156 | normal.assign(_cur_normal.begin(), _cur_normal.end()); |
| 2157 | } else { |
| 2158 | normal.insert(normal.end(), _cur_normal.begin() + 1, _cur_normal.end()); |
| 2159 | } |
| 2160 | } |
| 2161 | } |
| 2162 | } |
| 2163 | |
| 2164 | void ODRoad::roadInterval(std::vector<double>& interval) const { |
| 2165 | interval.clear(); |