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/>.
23 std::vector<RRTNode *> nv1(
26 float (*cost)(RRTNode *, RRTNode *),
29 std::vector<RRTNode *> nodes;
30 std::vector<RRTNode *> s; // DFS stack
31 std::vector<RRTNode *> r; // reset visited_
35 while (s.size() > 0) {
40 if ((*cost)(tmp, node) < dist) {
43 for (auto ch: tmp->children()) {
54 std::vector<RRTNode *> nv2(
55 std::vector<RRTNode *> (&nodes)[IYSIZE],
57 float (*cost)(RRTNode *, RRTNode *),
60 std::vector<RRTNode *> nvs;
61 unsigned int iy = IYI(node->y());
62 unsigned int iy_dist = floor(dist / IYSTEP) + 1;
63 unsigned int i = 0; // vector index
64 unsigned int j = 0; // array index
65 unsigned int jmin = 0; // minimal j index
66 unsigned int jmax = 0; // maximal j index
68 jmin = (jmin > 0) ? jmin : 0;
69 jmax = iy + iy_dist + 1;
70 jmax = (jmax < IYSIZE) ? jmax : IYSIZE;
71 #pragma omp parallel for reduction(merge: nvs)
72 for (j = jmin; j < jmax; j++) {
73 #pragma omp parallel for reduction(merge: nvs)
74 for (i = 0; i < nodes[j].size(); i++) {
75 if ((*cost)(nodes[j][i], node) < dist) {
76 nvs.push_back(nodes[j][i]);
83 std::vector<RRTNode *> nv3(
84 std::vector<RRTNode *> (&nodes)[IYSIZE],
86 float (*cost)(RRTNode *, RRTNode *),
89 std::vector<RRTNode *> nvs;
90 unsigned int iy = IYI(node->y());
91 unsigned int iy_dist = floor(dist / IYSTEP) + 1;
92 unsigned int i = 0; // vector index
93 unsigned int j = 0; // array index
94 unsigned int jmin = 0; // minimal j index
95 unsigned int jmax = 0; // maximal j index
97 jmin = (jmin > 0) ? jmin : 0;
98 jmax = iy + iy_dist + 1;
99 jmax = (jmax < IYSIZE) ? jmax : IYSIZE;
100 #pragma omp parallel for reduction(merge: nvs)
101 for (j = jmin; j < jmax; j++) {
102 #pragma omp parallel for reduction(merge: nvs)
103 for (i = 0; i < nodes[j].size(); i++) {
104 if (EDIST(nodes[j][i], node) < dist) {
105 nvs.push_back(nodes[j][i]);
112 std::vector<RRTNode *> nv4(
113 std::vector<RRTNode *> (&nodes)[IYSIZE],
115 float (*cost)(RRTNode *, RRTNode *),
119 std::vector<RRTNode *> nvs;
120 unsigned int iy = IYI(node->y());
121 unsigned int iy_dist = floor(dist / IYSTEP) + 1;
122 unsigned int i = 0; // vector index
123 unsigned int j = 0; // array index
124 unsigned int jmin = 0; // minimal j index
125 unsigned int jmax = 0; // maximal j index
127 jmin = (jmin > 0) ? jmin : 0;
128 jmax = iy + iy_dist + 1;
129 jmax = (jmax < IYSIZE) ? jmax : IYSIZE;
130 #pragma omp parallel for reduction(merge: nvs)
131 for (j = jmin; j < jmax; j++) {
132 #pragma omp parallel for reduction(merge: nvs)
133 for (i = 0; i < nodes[j].size(); i++) {
134 if ((*cost)(nodes[j][i], node) < dist &&
136 nodes[j][i]->tree() == tree) {
137 nvs.push_back(nodes[j][i]);