2 * SPDX-FileCopyrightText: 2022 Jiri Vlasak
4 * SPDX-License-Identifier: GPL-3.0-only
13 RRTExt20::collide_steered()
15 assert(this->_points_to_check != nullptr);
16 std::vector<bcar::Point> points_to_check(*this->_points_to_check);
18 for (auto &n: this->steered_) {
19 for (auto &p: points_to_check) {
21 CarSize const n_cs(this->bc());
22 if (p.inside_of(n_p, n_cs.edist_to_rr())) {
25 if (p.edist(n_p) < n_cs.edist_to_lf()
26 && p.edist(n_p) > n_cs.edist_to_rr()) {
27 BicycleCar n_bc(this->bc());
31 std::vector<Point> poly;
32 poly.push_back(n_bc.lf());
33 poly.push_back(n_bc.lr());
34 poly.push_back(n_bc.rr());
35 poly.push_back(n_bc.rf());
36 poly.push_back(n_bc.lf());
37 if (p.inside_of(poly)) {
45 this->steered_.erase(this->steered_.begin() + i, this->steered_.end());
46 return this->steered_.size() == 0;
50 RRTExt20::set_points_to_check(std::vector<bcar::Point> const *p)
52 this->_points_to_check = p;
55 } /* namespace rrts */