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 "cost.h"
  21 #include "nn.h"
  22 #include "rrtbase.h"
  23
  24 RRTNode *nn1(
  25                 std::vector<RRTNode *> &nodes,
  26                 RRTNode *node,
  27                 float (*cost)(RRTNode *, RRTNode *))
  28 {
  29         RRTNode *root = nodes[0];
  30         std::vector<RRTNode *> s; // DFS stack
  31         std::vector<RRTNode *> r; // reset visited_
  32         RRTNode *tmp;
  33         RRTNode *nn = root;
  34         float mcost = (*cost)(root, node);
  35
  36         s.push_back(root);
  37         while (s.size() > 0) {
  38                 tmp = s.back();
  39                 s.pop_back();
  40                 if (!tmp->visit()) {
  41                         r.push_back(tmp);
  42                         if ((*cost)(tmp, node) < mcost) {
  43                                 nn = tmp;
  44                                 mcost = (*cost)(tmp, node);
  45                         }
  46                         for (auto ch: tmp->children()) {
  47                                 s.push_back(ch);
  48                         }
  49                 }
  50         }
  51         for (auto n: r) {
  52                 n->visit(false);
  53         }
  54         return nn;
  55 }
  56
  57 RRTNode *nn2(
  58                 std::vector<RRTNode *> &nodes,
  59                 RRTNode *node,
  60                 float (*cost)(RRTNode *, RRTNode *))
  61 {
  62         RRTNode *nn = nodes[0];
  63         float mcost = EDIST(nn, node);
  64         unsigned int i;
  65         // TODO fix see, user-defined reductions
  66         #pragma omp parallel for
  67         for (i = 0; i < nodes.size(); i++) {
  68                 if (EDIST(nodes[i], node) < mcost) {
  69                         nn = nodes[i];
  70                         mcost = EDIST(nodes[i], node);
  71                 }
  72         }
  73         return nn;
  74 }
  75
  76 RRTNode *nn3(
  77                 std::vector<RRTNode *> (&nodes)[IYSIZE],
  78                 RRTNode *node,
  79                 float (*cost)(RRTNode *, RRTNode *))
  80 {
  81         int iy = IYI(node->y());
  82         struct mcnn nn;
  83         nn.nn = nullptr;
  84         nn.mc = 9999;
  85         unsigned int i = 0; // vector step
  86         unsigned int j = 0; // array step
  87         int iyj = 0;
  88         float oh = node->h();
  89         while (nn.mc > j * IYSTEP) {
  90                 iyj = (int) (iy + j);
  91                 if (iyj >= IYSIZE)
  92                         iyj = IYSIZE - 1;
  93                 #pragma omp parallel for reduction(minn: nn)
  94                 for (i = 0; i < nodes[iyj].size(); i++) {
  95                         node->h(nodes[iyj][i]->h());
  96                         if (co2(nodes[iyj][i], node) < nn.mc) {
  97                                 nn.mc = co2(nodes[iyj][i], node);
  98                                 nn.nn = nodes[iyj][i];
  99                         }
 100                 }
 101                 if (j > 0) {
 102                         iyj = (int) (iy - j);
 103                         if (iyj < 0)
 104                                 iyj = 0;
 105                         #pragma omp parallel for reduction(minn: nn)
 106                         for (i = 0; i < nodes[iyj].size(); i++) {
 107                                 node->h(nodes[iyj][i]->h());
 108                                 if (co2(nodes[iyj][i], node) < nn.mc) {
 109                                         nn.mc = co2(nodes[iyj][i], node);
 110                                         nn.nn = nodes[iyj][i];
 111                                 }
 112                         }
 113                 }
 114                 j++;
 115         }
 116         node->h(oh);
 117         return nn.nn;
 118 }
 119
 120 RRTNode *nn4(
 121                 std::vector<RRTNode *> (&nodes)[IYSIZE],
 122                 RRTNode *node,
 123                 float (*cost)(RRTNode *, RRTNode *))
 124 {
 125         int iy = IYI(node->y());
 126         struct mcnn nn;
 127         nn.nn = nullptr;
 128         nn.mc = 9999;
 129         unsigned int i = 0; // vector step
 130         unsigned int j = 0; // array step
 131         int iyj = 0;
 132         while (nn.mc > j * IYSTEP) {
 133                 iyj = (int) (iy + j);
 134                 if (iyj >= IYSIZE)
 135                         iyj = IYSIZE - 1;
 136                 #pragma omp parallel for reduction(minn: nn)
 137                 for (i = 0; i < nodes[iyj].size(); i++) {
 138                         if (EDIST(nodes[iyj][i], node) < nn.mc) {
 139                                 nn.mc = EDIST(nodes[iyj][i], node);
 140                                 nn.nn = nodes[iyj][i];
 141                         }
 142                 }
 143                 if (j > 0) {
 144                         iyj = (int) (iy - j);
 145                         if (iyj < 0)
 146                                 iyj = 0;
 147                         #pragma omp parallel for reduction(minn: nn)
 148                         for (i = 0; i < nodes[iyj].size(); i++) {
 149                                 if (EDIST(nodes[iyj][i], node) < nn.mc) {
 150                                         nn.mc = EDIST(nodes[iyj][i], node);
 151                                         nn.nn = nodes[iyj][i];
 152                                 }
 153                         }
 154                 }
 155                 j++;
 156         }
 157         return nn.nn;
 158 }
 159
 160 RRTNode *nn5(
 161                 std::vector<RRTNode *> (&nodes)[IYSIZE],
 162                 RRTNode *node,
 163                 float (*cost)(RRTNode *, RRTNode *),
 164                 char tree)
 165 {
 166         int iy = IYI(node->y());
 167         struct mcnn nn;
 168         nn.nn = nullptr;
 169         nn.mc = 9999;
 170         unsigned int i = 0; // vector step
 171         unsigned int j = 0; // array step
 172         int iyj = 0;
 173         while (nn.mc > j * IYSTEP) {
 174                 iyj = (int) (iy + j);
 175                 if (iyj >= IYSIZE)
 176                         iyj = IYSIZE - 1;
 177                 #pragma omp parallel for reduction(minn: nn)
 178                 for (i = 0; i < nodes[iyj].size(); i++) {
 179                         if (EDIST(nodes[iyj][i], node) < nn.mc &&
 180                                         tree != '0' &&
 181                                         nodes[iyj][i]->tree() == tree) {
 182                                 nn.mc = EDIST(nodes[iyj][i], node);
 183                                 nn.nn = nodes[iyj][i];
 184                         }
 185                 }
 186                 if (j > 0) {
 187                         iyj = (int) (iy - j);
 188                         if (iyj < 0)
 189                                 iyj = 0;
 190                         #pragma omp parallel for reduction(minn: nn)
 191                         for (i = 0; i < nodes[iyj].size(); i++) {
 192                                 if (EDIST(nodes[iyj][i], node) < nn.mc &&
 193                                                 tree != '0' &&
 194                                                 nodes[iyj][i]->tree() == tree) {
 195                                         nn.mc = EDIST(nodes[iyj][i], node);
 196                                         nn.nn = nodes[iyj][i];
 197                                 }
 198                         }
 199                 }
 200                 j++;
 201         }
 202         return nn.nn;
 203 }