2 This file is part of I am car.
4 I am car is free software: you can redistribute it and/or modify
5 it under the terms of the GNU General Public License as published by
6 the Free Software Foundation, either version 3 of the License, or
7 (at your option) any later version.
9 I am car is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU General Public License for more details.
14 You should have received a copy of the GNU General Public License
15 along with I am car. If not, see <http://www.gnu.org/licenses/>.
22 #include "reeds_shepp.h"
25 float co1(RRTNode *init, RRTNode *goal)
27 float dx = goal->x() - init->x();
28 float dy = goal->y() - init->y();
29 return pow(pow(dx, 2) + pow(dy, 2), 0.5);
32 float cco1(RRTNode *init, RRTNode *goal)
34 return init->ccost() + co1(init, goal);
37 float co2(RRTNode *init, RRTNode *goal)
39 double q0[] = {init->x(), init->y(), init->h()};
40 double q1[] = {goal->x(), goal->y(), goal->h()};
41 ReedsSheppStateSpace rsss(10.82); // TODO const param
42 return static_cast<float>(rsss.distance(q0, q1));
45 float cco2(RRTNode *init, RRTNode *goal)
47 return init->ccost() + co2(init, goal);
50 float co3(RRTNode *init, RRTNode *goal)
52 // The basic values from co1:
53 // edist is euclidean distance
54 // dh is difference between headings
55 // dt is difference between times
57 // The basic values from co2:
58 // rsdist is Reeds & Shepp distance
59 // cnt is number of cusps (direction changes)
61 // There is also available:
62 // node->ccost() -- cumulative cost from root
63 // node->dcost() -- direct cost to parent
64 // node->ocost() -- distance to the nearest obstacle
66 float dx = goal->x() - init->x();
67 float dy = goal->y() - init->y();
68 float edist = pow(pow(dx, 2) + pow(dy, 2), 0.5);
69 float dh = std::abs(goal->h() - init->h());
70 float dt = std::abs(goal->t() - init->t());
71 if (edist < 0.2 && dh < M_PI / 32)
74 double q0[] = {init->x(), init->y(), init->h()};
75 double q1[] = {goal->x(), goal->y(), goal->h()};
76 ReedsSheppStateSpace rsss(10.82); // TODO const param
77 float rsdist = static_cast<float>(rsss.distance(q0, q1));
80 if (rsss.reedsShepp(q0, q1).length_[0] > 0)
82 int cnt = 0; // #cusps
83 for (int i = 0; i < 5; i++) {
84 if (rsss.reedsShepp(q0, q1).length_[i] == 0)
86 if (rsss.reedsShepp(q0, q1).length_[i] > 0)
94 return rsdist + edist * cnt + 1 / (1 + init->ocost());
97 float cco3(RRTNode *init, RRTNode *goal)
99 return init->ccost() + co3(init, goal);
102 float co4(RRTNode *init, RRTNode *goal)
104 float dx = goal->x() - init->x();
105 float dy = goal->y() - init->y();
106 float edist = pow(pow(dx, 2) + pow(dy, 2), 0.5);
107 float heur = BCAR_TURNING_RADIUS * MIN(
108 abs(goal->h() - init->h()),
109 2 * M_PI - abs(goal->h() - init->h())
111 return MAX(edist, heur);
113 float cco4(RRTNode *init, RRTNode *goal)
115 return init->ccost() + co4(init, goal);