+++ /dev/null
-/*
-This file is part of I am car.
-
-I am car is free software: you can redistribute it and/or modify
-it under the terms of the GNU General Public License as published by
-the Free Software Foundation, either version 3 of the License, or
-(at your option) any later version.
-
-I am car is distributed in the hope that it will be useful,
-but WITHOUT ANY WARRANTY; without even the implied warranty of
-MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-GNU General Public License for more details.
-
-You should have received a copy of the GNU General Public License
-along with I am car. If not, see <http://www.gnu.org/licenses/>.
-*/
-
-#include <omp.h>
-#include <vector>
-#include "cost.h"
-#include "nn.h"
-#include "rrtbase.h"
-
-RRTNode *nn1(
- std::vector<RRTNode *> &nodes,
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *))
-{
- RRTNode *root = nodes[0];
- std::vector<RRTNode *> s; // DFS stack
- std::vector<RRTNode *> r; // reset visited_
- RRTNode *tmp;
- RRTNode *nn = root;
- float mcost = (*cost)(root, node);
-
- s.push_back(root);
- while (s.size() > 0) {
- tmp = s.back();
- s.pop_back();
- if (!tmp->visit()) {
- r.push_back(tmp);
- if ((*cost)(tmp, node) < mcost) {
- nn = tmp;
- mcost = (*cost)(tmp, node);
- }
- for (auto ch: tmp->children()) {
- s.push_back(ch);
- }
- }
- }
- for (auto n: r) {
- n->visit(false);
- }
- return nn;
-}
-
-RRTNode *nn2(
- std::vector<RRTNode *> &nodes,
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *))
-{
- RRTNode *nn = nodes[0];
- float mcost = EDIST(nn, node);
- unsigned int i;
- // TODO fix see, user-defined reductions
- #pragma omp parallel for
- for (i = 0; i < nodes.size(); i++) {
- if (EDIST(nodes[i], node) < mcost) {
- nn = nodes[i];
- mcost = EDIST(nodes[i], node);
- }
- }
- return nn;
-}
-
-RRTNode *nn3(
- std::vector<RRTNode *> (&nodes)[IYSIZE],
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *))
-{
- int iy = IYI(node->y());
- struct mcnn nn;
- nn.nn = nullptr;
- nn.mc = 9999;
- unsigned int i = 0; // vector step
- unsigned int j = 0; // array step
- int iyj = 0;
- float oh = node->h();
- while (nn.mc > j * IYSTEP) {
- iyj = (int) (iy + j);
- if (iyj >= IYSIZE)
- iyj = IYSIZE - 1;
- #pragma omp parallel for reduction(minn: nn)
- for (i = 0; i < nodes[iyj].size(); i++) {
- node->h(nodes[iyj][i]->h());
- if (co2(nodes[iyj][i], node) < nn.mc) {
- nn.mc = co2(nodes[iyj][i], node);
- nn.nn = nodes[iyj][i];
- }
- }
- if (j > 0) {
- iyj = (int) (iy - j);
- if (iyj < 0)
- iyj = 0;
- #pragma omp parallel for reduction(minn: nn)
- for (i = 0; i < nodes[iyj].size(); i++) {
- node->h(nodes[iyj][i]->h());
- if (co2(nodes[iyj][i], node) < nn.mc) {
- nn.mc = co2(nodes[iyj][i], node);
- nn.nn = nodes[iyj][i];
- }
- }
- }
- j++;
- }
- node->h(oh);
- return nn.nn;
-}
-
-RRTNode *nn4(
- std::array<std::vector<RRTNode *>, IYSIZE> &nodes,
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *))
-{
- int iy = IYI(node->y());
- struct mcnn nn;
- nn.nn = nullptr;
- nn.mc = 9999;
- unsigned int i = 0; // vector step
- unsigned int j = 0; // array step
- int iyj = 0;
- while (nn.mc > j * IYSTEP) {
- iyj = (int) (iy + j);
- if (iyj >= IYSIZE)
- iyj = IYSIZE - 1;
- #pragma omp parallel for reduction(minn: nn)
- for (i = 0; i < nodes[iyj].size(); i++) {
- if (EDIST(nodes[iyj][i], node) < nn.mc) {
- nn.mc = EDIST(nodes[iyj][i], node);
- nn.nn = nodes[iyj][i];
- }
- }
- if (j > 0) {
- iyj = (int) (iy - j);
- if (iyj < 0)
- iyj = 0;
- #pragma omp parallel for reduction(minn: nn)
- for (i = 0; i < nodes[iyj].size(); i++) {
- if (EDIST(nodes[iyj][i], node) < nn.mc) {
- nn.mc = EDIST(nodes[iyj][i], node);
- nn.nn = nodes[iyj][i];
- }
- }
- }
- j++;
- }
- return nn.nn;
-}
-
-RRTNode *nn5(
- std::vector<RRTNode *> (&nodes)[IYSIZE],
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *),
- char tree)
-{
- int iy = IYI(node->y());
- struct mcnn nn;
- nn.nn = nullptr;
- nn.mc = 9999;
- unsigned int i = 0; // vector step
- unsigned int j = 0; // array step
- int iyj = 0;
- while (nn.mc > j * IYSTEP) {
- iyj = (int) (iy + j);
- if (iyj >= IYSIZE)
- iyj = IYSIZE - 1;
- #pragma omp parallel for reduction(minn: nn)
- for (i = 0; i < nodes[iyj].size(); i++) {
- if (EDIST(nodes[iyj][i], node) < nn.mc &&
- tree != '0' &&
- nodes[iyj][i]->tree() == tree) {
- nn.mc = EDIST(nodes[iyj][i], node);
- nn.nn = nodes[iyj][i];
- }
- }
- if (j > 0) {
- iyj = (int) (iy - j);
- if (iyj < 0)
- iyj = 0;
- #pragma omp parallel for reduction(minn: nn)
- for (i = 0; i < nodes[iyj].size(); i++) {
- if (EDIST(nodes[iyj][i], node) < nn.mc &&
- tree != '0' &&
- nodes[iyj][i]->tree() == tree) {
- nn.mc = EDIST(nodes[iyj][i], node);
- nn.nn = nodes[iyj][i];
- }
- }
- }
- j++;
- }
- return nn.nn;
-}
-
-RRTNode *nn6(
- std::vector<RRTNode *> (&nodes)[IYSIZE],
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *))
-{
- int iy = IYI(node->y());
- struct mcnn nn;
- nn.nn = nullptr;
- nn.mc = 9999;
- unsigned int i = 0; // vector step
- unsigned int j = 0; // array step
- int iyj = 0;
- while (nn.mc > j * IYSTEP) {
- iyj = (int) (iy + j);
- if (iyj >= IYSIZE)
- iyj = IYSIZE - 1;
- #pragma omp parallel for reduction(minn: nn)
- for (i = 0; i < nodes[iyj].size(); i++) {
- if (co4(nodes[iyj][i], node) < nn.mc) {
- nn.mc = co4(nodes[iyj][i], node);
- nn.nn = nodes[iyj][i];
- }
- }
- if (j > 0) {
- iyj = (int) (iy - j);
- if (iyj < 0)
- iyj = 0;
- #pragma omp parallel for reduction(minn: nn)
- for (i = 0; i < nodes[iyj].size(); i++) {
- if (co4(nodes[iyj][i], node) < nn.mc) {
- nn.mc = co4(nodes[iyj][i], node);
- nn.nn = nodes[iyj][i];
- }
- }
- }
- j++;
- }
- return nn.nn;
-}
+++ /dev/null
-/*
-This file is part of I am car.
-
-I am car is free software: you can redistribute it and/or modify
-it under the terms of the GNU General Public License as published by
-the Free Software Foundation, either version 3 of the License, or
-(at your option) any later version.
-
-I am car is distributed in the hope that it will be useful,
-but WITHOUT ANY WARRANTY; without even the implied warranty of
-MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-GNU General Public License for more details.
-
-You should have received a copy of the GNU General Public License
-along with I am car. If not, see <http://www.gnu.org/licenses/>.
-*/
-
-#include <omp.h>
-#include <vector>
-#include "nn.h"
-#include "nv.h"
-
-std::vector<RRTNode *> nv1(
- RRTNode *root,
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *),
- float dist)
-{
- std::vector<RRTNode *> nodes;
- std::vector<RRTNode *> s; // DFS stack
- std::vector<RRTNode *> r; // reset visited_
- RRTNode *tmp;
-
- s.push_back(root);
- while (s.size() > 0) {
- tmp = s.back();
- s.pop_back();
- if (!tmp->visit()) {
- r.push_back(tmp);
- if ((*cost)(tmp, node) < dist) {
- nodes.push_back(tmp);
- }
- for (auto ch: tmp->children()) {
- s.push_back(ch);
- }
- }
- }
- for (auto n: r) {
- n->visit(false);
- }
- return nodes;
-}
-
-std::vector<RRTNode *> nv2(
- std::vector<RRTNode *> (&nodes)[IYSIZE],
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *),
- float dist)
-{
- std::vector<RRTNode *> nvs;
- unsigned int iy = IYI(node->y());
- unsigned int iy_dist = floor(dist / IYSTEP) + 1;
- unsigned int i = 0; // vector index
- unsigned int j = 0; // array index
- unsigned int jmin = 0; // minimal j index
- unsigned int jmax = 0; // maximal j index
- jmin = iy - iy_dist;
- jmin = (jmin > 0) ? jmin : 0;
- jmax = iy + iy_dist + 1;
- jmax = (jmax < IYSIZE) ? jmax : IYSIZE;
- #pragma omp parallel for reduction(merge: nvs)
- for (j = jmin; j < jmax; j++) {
- #pragma omp parallel for reduction(merge: nvs)
- for (i = 0; i < nodes[j].size(); i++) {
- if ((*cost)(nodes[j][i], node) < dist) {
- nvs.push_back(nodes[j][i]);
- }
- }
- }
- return nvs;
-}
-
-std::vector<RRTNode *> nv3(
- std::vector<RRTNode *> (&nodes)[IYSIZE],
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *),
- float dist)
-{
- std::vector<RRTNode *> nvs;
- unsigned int iy = IYI(node->y());
- unsigned int iy_dist = floor(dist / IYSTEP) + 1;
- unsigned int i = 0; // vector index
- unsigned int j = 0; // array index
- unsigned int jmin = 0; // minimal j index
- unsigned int jmax = 0; // maximal j index
- jmin = iy - iy_dist;
- jmin = (jmin > 0) ? jmin : 0;
- jmax = iy + iy_dist + 1;
- jmax = (jmax < IYSIZE) ? jmax : IYSIZE;
- #pragma omp parallel for reduction(merge: nvs)
- for (j = jmin; j < jmax; j++) {
- #pragma omp parallel for reduction(merge: nvs)
- for (i = 0; i < nodes[j].size(); i++) {
- if (EDIST(nodes[j][i], node) < dist) {
- nvs.push_back(nodes[j][i]);
- }
- }
- }
- return nvs;
-}
-
-std::vector<RRTNode *> nv4(
- std::vector<RRTNode *> (&nodes)[IYSIZE],
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *),
- float dist,
- char tree)
-{
- std::vector<RRTNode *> nvs;
- unsigned int iy = IYI(node->y());
- unsigned int iy_dist = floor(dist / IYSTEP) + 1;
- unsigned int i = 0; // vector index
- unsigned int j = 0; // array index
- unsigned int jmin = 0; // minimal j index
- unsigned int jmax = 0; // maximal j index
- jmin = iy - iy_dist;
- jmin = (jmin > 0) ? jmin : 0;
- jmax = iy + iy_dist + 1;
- jmax = (jmax < IYSIZE) ? jmax : IYSIZE;
- #pragma omp parallel for reduction(merge: nvs)
- for (j = jmin; j < jmax; j++) {
- #pragma omp parallel for reduction(merge: nvs)
- for (i = 0; i < nodes[j].size(); i++) {
- if ((*cost)(nodes[j][i], node) < dist &&
- tree != '0' &&
- nodes[j][i]->tree() == tree) {
- nvs.push_back(nodes[j][i]);
- }
- }
- }
- return nvs;
-}
#endif
// RRT
-#include "sample.h"
#include "cost.h"
#include "steer.h"
-#include "nn.h"
-#include "nv.h"
#if USE_GL > 0
extern SDL_Window* gw;
+++ /dev/null
-/*
-This file is part of I am car.
-
-I am car is free software: you can redistribute it and/or modify
-it under the terms of the GNU General Public License as published by
-the Free Software Foundation, either version 3 of the License, or
-(at your option) any later version.
-
-I am car is distributed in the hope that it will be useful,
-but WITHOUT ANY WARRANTY; without even the implied warranty of
-MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-GNU General Public License for more details.
-
-You should have received a copy of the GNU General Public License
-along with I am car. If not, see <http://www.gnu.org/licenses/>.
-*/
-
-#include <cmath>
-#include <cstdlib>
-#include "sample.h"
-
-RRTNode *sa1()
-{
- float new_x = VMIN + static_cast<float>(rand()) /
- static_cast<float>(RAND_MAX / (VMAX - VMIN));
- float new_y = HMIN + static_cast<float>(rand()) /
- static_cast<float>(RAND_MAX / (HMAX - HMIN));
- float new_h = static_cast<float>(rand()) /
- static_cast<float>(RAND_MAX / (2 * M_PI));
- return new RRTNode(new_x, new_y, new_h);
-}
-
-RRTNode *sa2(float bx, float by, float br)
-{
- float r = static_cast<float>(rand());
- r /= static_cast<float>(RAND_MAX / br);
- float sh = static_cast<float>(rand());
- sh /= static_cast<float>(RAND_MAX / (2 * M_PI));
- float h = static_cast<float>(rand());
- h /= static_cast<float>(RAND_MAX / (2 * M_PI));
- return new RRTNode(bx + r * cos(sh), by + r * sin(sh), h);
-}
#include <cstdlib>
#include <ctime>
#include "compile.h"
-#include "nn.h"
-#include "nv.h"
-#include "sample.h"
#include "steer.h"
#include "rrtplanner.h"
#include "cost.h"
+++ /dev/null
-/*
-This file is part of I am car.
-
-I am car is free software: you can redistribute it and/or modify
-it under the terms of the GNU General Public License as published by
-the Free Software Foundation, either version 3 of the License, or
-(at your option) any later version.
-
-I am car is distributed in the hope that it will be useful,
-but WITHOUT ANY WARRANTY; without even the implied warranty of
-MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-GNU General Public License for more details.
-
-You should have received a copy of the GNU General Public License
-along with I am car. If not, see <http://www.gnu.org/licenses/>.
-*/
-
-#ifndef NEARESTNEIGHBOUR_H
-#define NEARESTNEIGHBOUR_H
-
-#include <array>
-#include "aux.h"
-#include "rrtbase.h"
-#include "rrtnode.h"
-
-RRTNode *nn1(
- std::vector<RRTNode *> &nodes,
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *));
-RRTNode *nn2(
- std::vector<RRTNode *> &nodes,
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *));
-RRTNode *nn3(
- std::vector<RRTNode *> (&nodes)[IYSIZE],
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *));
-RRTNode *nn4(
- std::array<std::vector<RRTNode *>, IYSIZE> &nodes,
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *));
-RRTNode *nn5(
- std::vector<RRTNode *> (&nodes)[IYSIZE],
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *),
- char tree);
-RRTNode *nn6(
- std::vector<RRTNode *> (&nodes)[IYSIZE],
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *));
-
-#endif
+++ /dev/null
-/*
-This file is part of I am car.
-
-I am car is free software: you can redistribute it and/or modify
-it under the terms of the GNU General Public License as published by
-the Free Software Foundation, either version 3 of the License, or
-(at your option) any later version.
-
-I am car is distributed in the hope that it will be useful,
-but WITHOUT ANY WARRANTY; without even the implied warranty of
-MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-GNU General Public License for more details.
-
-You should have received a copy of the GNU General Public License
-along with I am car. If not, see <http://www.gnu.org/licenses/>.
-*/
-
-#ifndef NEARVERTICES_H
-#define NEARVERTICES_H
-
-#include <vector>
-#include "aux.h"
-#include "rrtbase.h"
-#include "rrtnode.h"
-
-std::vector<RRTNode *> nv1(
- RRTNode *root,
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *),
- float dist);
-std::vector<RRTNode *> nv2(
- std::vector<RRTNode *> (&nodes)[IYSIZE],
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *),
- float dist);
-std::vector<RRTNode *> nv3(
- std::vector<RRTNode *> (&nodes)[IYSIZE],
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *),
- float dist);
-std::vector<RRTNode *> nv4(
- std::vector<RRTNode *> (&nodes)[IYSIZE],
- RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *),
- float dist,
- char tree);
-
-#endif
#include <vector>
#include "obstacle.h"
#include "rrtnode.h"
-#include "sample.h"
#include "slotplanner.h"
#define NOFNODES 20000
+++ /dev/null
-/*
-This file is part of I am car.
-
-I am car is free software: you can redistribute it and/or modify
-it under the terms of the GNU General Public License as published by
-the Free Software Foundation, either version 3 of the License, or
-(at your option) any later version.
-
-I am car is distributed in the hope that it will be useful,
-but WITHOUT ANY WARRANTY; without even the implied warranty of
-MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-GNU General Public License for more details.
-
-You should have received a copy of the GNU General Public License
-along with I am car. If not, see <http://www.gnu.org/licenses/>.
-*/
-
-#ifndef SAMPLE_H
-#define SAMPLE_H
-
-#include "rrtnode.h"
-
-RRTNode *sa1();
-RRTNode *sa2(float bx, float by, float br);
-
-#endif
projects / hubacji1 / iamcar.git / commitdiff