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