| 2100 | } |
| 2101 | |
| 2102 | void ODRoad::roadTangent(std::vector<std::pair<double, double>>& tangent, const std::vector<double>* interval) const { |
| 2103 | tangent.clear(); |
| 2104 | if (interval && !interval->empty()) { |
| 2105 | tangent.resize(interval->size()); |
| 2106 | size_t index = 0; |
| 2107 | for (size_t i = 0; i < interval->size();) { |
| 2108 | if (index + 1 < ref_line.size() && interval->at(i) > ref_line[index + 1]->offset()) { |
| 2109 | index++; |
| 2110 | } else { |
| 2111 | double x = 0, y = 0; |
| 2112 | ref_line[index]->getTangent(interval->at(i), x, y); |
| 2113 | tangent[i] = std::make_pair(x, y); |
| 2114 | i++; |
| 2115 | } |
| 2116 | } |
| 2117 | } else { |
| 2118 | for (auto& _geom_ptr : ref_line) { |
| 2119 | std::vector<std::pair<double, double>> _cur_tangent; |
| 2120 | _geom_ptr->getTangent(_cur_tangent); |
| 2121 | if (_cur_tangent.empty()) { |
| 2122 | continue; |
| 2123 | } |
| 2124 | if (tangent.empty()) { |
| 2125 | tangent.assign(_cur_tangent.begin(), _cur_tangent.end()); |
| 2126 | } else { |
| 2127 | tangent.insert(tangent.end(), _cur_tangent.begin() + 1, _cur_tangent.end()); |
| 2128 | } |
| 2129 | } |
| 2130 | } |
| 2131 | } |
| 2132 | |
| 2133 | void ODRoad::roadNormal(std::vector<std::pair<double, double>>& normal, const std::vector<double>* interval) const { |
| 2134 | normal.clear(); |