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 <vector>
  19 # include "nn.h"
  20
  21 RRTNode *nn1(
  22                 std::vector<RRTNode *> &nodes,
  23                 RRTNode *node,
  24                 float (*cost)(RRTNode *, RRTNode *))
  25 {
  26         RRTNode *root = nodes[0];
  27         std::vector<RRTNode *> s; // DFS stack
  28         std::vector<RRTNode *> r; // reset visited_
  29         RRTNode *tmp;
  30         RRTNode *nn = root;
  31         float mcost = (*cost)(root, node);
  32
  33         s.push_back(root);
  34         while (s.size() > 0) {
  35                 tmp = s.back();
  36                 s.pop_back();
  37                 if (!tmp->visit()) {
  38                         r.push_back(tmp);
  39                         if ((*cost)(tmp, node) < mcost) {
  40                                 nn = tmp;
  41                                 mcost = (*cost)(tmp, node);
  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 nn;
  52 }
  53
  54 RRTNode *nn2(
  55                 std::vector<RRTNode *> &nodes,
  56                 RRTNode *node,
  57                 float (*cost)(RRTNode *, RRTNode *))
  58 {
  59         RRTNode *nn = nodes[0];
  60         float mcost = (*cost)(nn, node);
  61         unsigned int i;
  62         for (i = 0; i < nodes.size(); i++) {
  63                 if ((*cost)(nodes[i], node) < mcost) {
  64                         nn = nodes[i];
  65                         mcost = (*cost)(nodes[i], node);
  66                 }
  67         }
  68         return nn;
  69 }