base/nn.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
  22 RRTNode *nn1(
  23                 std::vector<RRTNode *> &nodes,
  24                 RRTNode *node,
  25                 float (*cost)(RRTNode *, RRTNode *))
  26 {
  27         RRTNode *root = nodes[0];
  28         std::vector<RRTNode *> s; // DFS stack
  29         std::vector<RRTNode *> r; // reset visited_
  30         RRTNode *tmp;
  31         RRTNode *nn = root;
  32         float mcost = (*cost)(root, node);
  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) < mcost) {
  41                                 nn = tmp;
  42                                 mcost = (*cost)(tmp, node);
  43                         }
  44                         for (auto ch: tmp->children()) {
  45                                 s.push_back(ch);
  46                         }
  47                 }
  48         }
  49         for (auto n: r) {
  50                 n->visit(false);
  51         }
  52         return nn;
  53 }
  54
  55 RRTNode *nn2(
  56                 std::vector<RRTNode *> &nodes,
  57                 RRTNode *node,
  58                 float (*cost)(RRTNode *, RRTNode *))
  59 {
  60         RRTNode *nn = nodes[0];
  61         float mcost = (*cost)(nn, node);
  62         unsigned int i;
  63         #pragma omp parallel for
  64         for (i = 0; i < nodes.size(); i++) {
  65                 if ((*cost)(nodes[i], node) < mcost) {
  66                         nn = nodes[i];
  67                         mcost = (*cost)(nodes[i], node);
  68                 }
  69         }
  70         return nn;
  71 }
  72
  73 RRTNode *nn3(
  74                 std::vector<RRTNode *> (&nodes)[IYSIZE],
  75                 RRTNode *node,
  76                 float (*cost)(RRTNode *, RRTNode *))
  77 {
  78         int iy = floor(node->y() / IYSTEP) + floor(IYSIZE / 2);
  79         RRTNode *nn = nullptr;
  80         float mc = 9999;
  81         unsigned int i = 0; // vector step
  82         unsigned int j = 0; // array step
  83         int iyj = 0;
  84         while (mc > j * IYSTEP) {
  85                 iyj = (int) (iy + j);
  86                 if (iyj >= IYSIZE)
  87                         iyj = IYSIZE - 1;
  88                 #pragma omp parallel for
  89                 for (i = 0; i < nodes[iyj].size(); i++) {
  90                         if ((*cost)(nodes[iyj][i], node) < mc) {
  91                                 mc = (*cost)(nodes[iyj][i], node);
  92                                 nn = nodes[iyj][i];
  93                         }
  94                 }
  95                 if (j > 0) {
  96                         iyj = (int) (iy - j);
  97                         if (iyj < 0)
  98                                 iyj = 0;
  99                         #pragma omp parallel for
 100                         for (i = 0; i < nodes[iyj].size(); i++) {
 101                                 if ((*cost)(nodes[iyj][i], node) < mc) {
 102                                         mc = (*cost)(nodes[iyj][i], node);
 103                                         nn = nodes[iyj][i];
 104                                 }
 105                         }
 106                 }
 107                 j++;
 108         }
 109         return nn;
 110 }