| 2218 | } |
| 2219 | |
| 2220 | void ODRoad::roadRef(std::vector<std::pair<double, double>>& ref, const std::vector<double>* intervals) const { |
| 2221 | ref.clear(); |
| 2222 | |
| 2223 | if (lane_offset.empty()) { |
| 2224 | roadGeom(ref, intervals); |
| 2225 | return; |
| 2226 | } |
| 2227 | std::vector<double> interval; |
| 2228 | if (intervals && !intervals->empty()) { |
| 2229 | interval = *intervals; |
| 2230 | } else { |
| 2231 | roadInterval(interval); |
| 2232 | } |
| 2233 | |
| 2234 | std::vector<double> offset; |
| 2235 | size_t index = 0; |
| 2236 | for (size_t i = 0; i < interval.size();) { |
| 2237 | if (index + 1 < lane_offset.size() && interval[i] > lane_offset[index + 1].offset()) { |
| 2238 | index++; |
| 2239 | } else { |
| 2240 | double dis = interval[i] - lane_offset[index].offset(); |
| 2241 | offset.push_back(lane_offset[index].value(dis)); |
| 2242 | i++; |
| 2243 | } |
| 2244 | } |
| 2245 | |
| 2246 | std::vector<std::pair<double, double>> geom, normal; |
| 2247 | roadGeom(geom, &interval); |
| 2248 | roadNormal(normal, &interval); |
| 2249 | |
| 2250 | ref.resize(geom.size()); |
| 2251 | for (size_t i = 0; i < geom.size(); ++i) { |
| 2252 | ref[i].first = geom[i].first + offset[i] * normal[i].first; |
| 2253 | ref[i].second = geom[i].second + offset[i] * normal[i].second; |
| 2254 | } |
| 2255 | } |
| 2256 | |
| 2257 | void ODRoad::roadEle(std::vector<double>& ele, const std::vector<double>* intervals) const { |
| 2258 | ele.clear(); |