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 }
 114
 115 RRTNode *nn4(
 116                 std::vector<RRTNode *> (&nodes)[IYSIZE],
 117                 RRTNode *node,
 118                 float (*cost)(RRTNode *, RRTNode *))
 119 {
 120         int iy = IYI(node->y());
 121         struct mcnn nn;
 122         nn.nn = nullptr;
 123         nn.mc = 9999;
 124         unsigned int i = 0; // vector step
 125         unsigned int j = 0; // array step
 126         int iyj = 0;
 127         while (nn.mc > j * IYSTEP) {
 128                 iyj = (int) (iy + j);
 129                 if (iyj >= IYSIZE)
 130                         iyj = IYSIZE - 1;
 131                 #pragma omp parallel for reduction(minn: nn)
 132                 for (i = 0; i < nodes[iyj].size(); i++) {
 133                         if (EDIST(nodes[iyj][i], node) < nn.mc) {
 134                                 nn.mc = EDIST(nodes[iyj][i], node);
 135                                 nn.nn = nodes[iyj][i];
 136                         }
 137                 }
 138                 if (j > 0) {
 139                         iyj = (int) (iy - j);
 140                         if (iyj < 0)
 141                                 iyj = 0;
 142                         #pragma omp parallel for reduction(minn: nn)
 143                         for (i = 0; i < nodes[iyj].size(); i++) {
 144                                 if (EDIST(nodes[iyj][i], node) < nn.mc) {
 145                                         nn.mc = EDIST(nodes[iyj][i], node);
 146                                         nn.nn = nodes[iyj][i];
 147                                 }
 148                         }
 149                 }
 150                 j++;
 151         }
 152         return nn.nn;
 153 }
 154
 155 RRTNode *nn5(
 156                 std::vector<RRTNode *> (&nodes)[IYSIZE],
 157                 RRTNode *node,
 158                 float (*cost)(RRTNode *, RRTNode *),
 159                 char tree)
 160 {
 161         int iy = IYI(node->y());
 162         struct mcnn nn;
 163         nn.nn = nullptr;
 164         nn.mc = 9999;
 165         unsigned int i = 0; // vector step
 166         unsigned int j = 0; // array step
 167         int iyj = 0;
 168         while (nn.mc > j * IYSTEP) {
 169                 iyj = (int) (iy + j);
 170                 if (iyj >= IYSIZE)
 171                         iyj = IYSIZE - 1;
 172                 #pragma omp parallel for reduction(minn: nn)
 173                 for (i = 0; i < nodes[iyj].size(); i++) {
 174                         if (EDIST(nodes[iyj][i], node) < nn.mc &&
 175                                         tree != '0' &&
 176                                         nodes[iyj][i]->tree() == tree) {
 177                                 nn.mc = EDIST(nodes[iyj][i], node);
 178                                 nn.nn = nodes[iyj][i];
 179                         }
 180                 }
 181                 if (j > 0) {
 182                         iyj = (int) (iy - j);
 183                         if (iyj < 0)
 184                                 iyj = 0;
 185                         #pragma omp parallel for reduction(minn: nn)
 186                         for (i = 0; i < nodes[iyj].size(); i++) {
 187                                 if (EDIST(nodes[iyj][i], node) < nn.mc &&
 188                                                 tree != '0' &&
 189                                                 nodes[iyj][i]->tree() == tree) {
 190                                         nn.mc = EDIST(nodes[iyj][i], node);
 191                                         nn.nn = nodes[iyj][i];
 192                                 }
 193                         }
 194                 }
 195                 j++;
 196         }
 197         return nn.nn;
 198 }