base/nv.cc

   1 /*
   2 This file is part of I am car.
   3
   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.
   8
   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.
  13
  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/>.
  16 */
  17
  18 #include <omp.h>
  19 #include <vector>
  20 #include "nn.h"
  21 #include "nv.h"
  22
  23 std::vector<RRTNode *> nv1(
  24                 RRTNode *root,
  25                 RRTNode *node,
  26                 float (*cost)(RRTNode *, RRTNode *),
  27                 float dist)
  28 {
  29         std::vector<RRTNode *> nodes;
  30         std::vector<RRTNode *> s; // DFS stack
  31         std::vector<RRTNode *> r; // reset visited_
  32         RRTNode *tmp;
  33
  34         s.push_back(root);
  35         while (s.size() > 0) {
  36                 tmp = s.back();
  37                 s.pop_back();
  38                 if (!tmp->visit()) {
  39                         r.push_back(tmp);
  40                         if ((*cost)(tmp, node) < dist) {
  41                                 nodes.push_back(tmp);
  42                         }
  43                         for (auto ch: tmp->children()) {
  44                                 s.push_back(ch);
  45                         }
  46                 }
  47         }
  48         for (auto n: r) {
  49                 n->visit(false);
  50         }
  51         return nodes;
  52 }
  53
  54 std::vector<RRTNode *> nv2(
  55                 std::vector<RRTNode *> (&nodes)[IYSIZE],
  56                 RRTNode *node,
  57                 float (*cost)(RRTNode *, RRTNode *),
  58                 float dist)
  59 {
  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
  67         jmin = iy - iy_dist;
  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]);
  77                         }
  78                 }
  79         }
  80         return nvs;
  81 }
  82
  83 std::vector<RRTNode *> nv3(
  84                 std::vector<RRTNode *> (&nodes)[IYSIZE],
  85                 RRTNode *node,
  86                 float (*cost)(RRTNode *, RRTNode *),
  87                 float dist)
  88 {
  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
  96         jmin = iy - iy_dist;
  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]);
 106                         }
 107                 }
 108         }
 109         return nvs;
 110 }
 111
 112 std::vector<RRTNode *> nv4(
 113                 std::vector<RRTNode *> (&nodes)[IYSIZE],
 114                 RRTNode *node,
 115                 float (*cost)(RRTNode *, RRTNode *),
 116                 float dist,
 117                 char tree)
 118 {
 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
 126         jmin = iy - iy_dist;
 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 &&
 135                                         tree != '0' &&
 136                                         nodes[j][i]->tree() == tree) {
 137                                 nvs.push_back(nodes[j][i]);
 138                         }
 139                 }
 140         }
 141         return nvs;
 142 }