2 * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
4 * SPDX-License-Identifier: GPL-3.0-only
8 #include "reeds_shepp.h"
13 RRTExt10::cost_build(RRTNode const& f, RRTNode const& t) const
15 double q0[] = {f.x(), f.y(), f.h()};
16 double q1[] = {t.x(), t.y(), t.h()};
17 ReedsSheppStateSpace rsss(this->bc_.mtr());
18 return rsss.distance(q0, q1);
22 RRTExt10::cost_search(RRTNode const& f, RRTNode const& t) const
24 double cost = f.edist(t);
25 double heur = std::min(std::abs(t.h() - f.h()),
26 2 * M_PI - std::abs(t.h() - f.h()));
27 heur *= this->bc_.mtr();
28 cost = std::max(cost, heur);
29 return cost + f.cusp() * 0.1;