From 9947dced15d5fcfbc700afcd6cdaafb94ec8f9dd Mon Sep 17 00:00:00 2001 From: kazu Date: Sun, 24 Jan 2016 18:59:06 +0100 Subject: [PATCH] added several grid-walks added new helper methods/classes (e.g. for heading) new test cases optimize the dijkstra cleanups/refactoring added timed-benchmarks to the log many more... --- CMakeLists.txt | 2 + geo/Angle.h | 10 +- geo/Heading.h | 63 ++++++++ geo/Point3.h | 10 ++ grid/Grid.h | 19 ++- grid/GridNeighborIterator.h | 8 +- grid/GridPoint.h | 9 ++ grid/factory/GridFactory.h | 4 +- grid/factory/GridImportance.h | 49 +++++- grid/walk/GridWalkHelper.h | 17 ++ grid/walk/GridWalkLightAtTheEndOfTheTunnel.h | 160 +++++++++++++++++++ grid/walk/GridWalkState.h | 21 +++ grid/walk/GridWalkWeighted.h | 141 ++++++++++++++++ grid/walk/GridWalkWeighted2.h | 127 +++++++++++++++ grid/walk/TestWalkWeighted3.h | 139 ++++++++++++++++ main.cpp | 2 +- math/DrawList.h | 82 ++++++++++ misc/Debug.h | 18 ++- misc/KNN.h | 46 +++++- misc/KNNArray.h | 59 +++++++ misc/Time.h | 21 +++ nav/dijkstra/Dijkstra.h | 62 +++---- nav/dijkstra/DijkstraPath.h | 58 +++++++ nav/dijkstra/DijkstraStructs.h | 27 +--- tests/data/fp1.svg | 32 ++-- tests/geo/TestHeading.cpp | 29 ++++ tests/grid/Plot.h | 85 +++++++++- tests/grid/TestAll.cpp | 30 +++- tests/grid/TestWalk.cpp | 127 +++++++++++++++ tests/misc/TestKNN.cpp | 43 +++++ 30 files changed, 1406 insertions(+), 94 deletions(-) create mode 100644 geo/Heading.h create mode 100644 grid/walk/GridWalkHelper.h create mode 100644 grid/walk/GridWalkLightAtTheEndOfTheTunnel.h create mode 100644 grid/walk/GridWalkState.h create mode 100644 grid/walk/GridWalkWeighted.h create mode 100644 grid/walk/GridWalkWeighted2.h create mode 100644 grid/walk/TestWalkWeighted3.h create mode 100644 math/DrawList.h create mode 100644 misc/KNNArray.h create mode 100644 misc/Time.h create mode 100644 nav/dijkstra/DijkstraPath.h mode change 100644 => 100755 tests/data/fp1.svg create mode 100644 tests/geo/TestHeading.cpp create mode 100644 tests/grid/TestWalk.cpp create mode 100644 tests/misc/TestKNN.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a65b2c1..6ecdbd1 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,6 +62,8 @@ ADD_DEFINITIONS( -Wextra -Wpedantic + -fstack-protector-all + -g3 -O0 diff --git a/geo/Angle.h b/geo/Angle.h index f177cab..f5905c6 100755 --- a/geo/Angle.h +++ b/geo/Angle.h @@ -4,7 +4,8 @@ #include #include -class Angle { +struct Angle { + public: @@ -28,12 +29,15 @@ public: static float getDiffRAD_2PI_PI(const float r1, const float r2) { _assertBetween(r1, 0, 2*M_PI, "r1 out of bounds"); _assertBetween(r2, 0, 2*M_PI, "r2 out of bounds"); - return fmod(std::abs(r2 - r1), M_PI); + float tmp = std::abs(r1-r2); + return (tmp <= M_PI) ? (tmp) : (2*M_PI-tmp); + //float tmp2 = fmod(tmp, M_PI); + //return fmod(std::abs(r2 - r1), M_PI); } /** convert degrees to radians */ - static float degToRad(const float deg) { + static constexpr float degToRad(const float deg) { return deg / 180 * M_PI; } diff --git a/geo/Heading.h b/geo/Heading.h new file mode 100644 index 0000000..b00c028 --- /dev/null +++ b/geo/Heading.h @@ -0,0 +1,63 @@ +#ifndef HEADING_H +#define HEADING_H + +#include + +#include "Angle.h" + +struct Heading { + +#define _2PI (2*M_PI) + +private: + + /** heading in radians. 0 = to-the-right */ + float rad; + +public: + + /** ctor with radians */ + Heading(const float rad) : rad(rad) { + _assertBetween(rad, 0, _2PI, "radians out of bounds"); + } + + /** ctor from(x1,y1) to(x2,y2) */ + Heading(const float x1, const float y1, const float x2, const float y2) : rad(Angle::getRAD_2PI(x1,y1,x2,y2)) { + _assertBetween(rad, 0, _2PI, "radians out of bounds"); + } + + /** angular difference [0:PI] */ + float getDiffHalfRAD(const Heading other) const { + return Angle::getDiffRAD_2PI_PI(rad, other.rad); + } + + /** update the angle but ensure we stay within [0:2PI] */ + Heading& operator += (const float _rad) { + _assertBetween(_rad, -_2PI*0.99, +_2PI*0.99, "radians out of bounds"); + rad += _rad; + if (rad > _2PI) {rad -= _2PI;} + if (rad < 0) {rad += _2PI;} + return *this; + } + + /** get an inverted version of this heading (upwards -> downwards, left -> right, ...) */ + Heading getInverted() const { + Heading out(rad); + out += M_PI; + return out; + } + + float getRAD() const {return rad;} + +#undef _2PI + +}; + +namespace Headings { + static const Heading RIGHT = Heading(M_PI*0/2); + static const Heading UP = Heading(M_PI*1/2); + static const Heading LEFT = Heading(M_PI*2/2); + static const Heading DOWN = Heading(M_PI*3/2); +} + +#endif // HEADING_H diff --git a/geo/Point3.h b/geo/Point3.h index 96ec9bc..be11516 100644 --- a/geo/Point3.h +++ b/geo/Point3.h @@ -1,6 +1,8 @@ #ifndef POINT3_H #define POINT3_H +#include + /** * 3D Point */ @@ -26,6 +28,14 @@ struct Point3 { Point3& operator /= (const float v) {x/=v; y/=v; z/=v; return *this;} + /** read-only array access */ + float operator [] (const int idx) const { + _assertBetween(idx, 0, 2, "index out of bounds"); + if (0 == idx) {return x;} + if (1 == idx) {return y;} + return z; + } + float length() const {return std::sqrt(x*x + y*y + z*z);} float length(const float norm) const { diff --git a/grid/Grid.h b/grid/Grid.h index f6c930e..4b9a9ed 100755 --- a/grid/Grid.h +++ b/grid/Grid.h @@ -15,7 +15,12 @@ /** * grid of the given grid-size, storing some value which - * extends GridPoint + * extends GridPoint and GridNode + * + * Usage: + * for (Node& n : grid) {...} + * for (Node& n2 : grid.neighbors(n)) {...} + * */ template class Grid { @@ -47,6 +52,12 @@ public: /** no-assign */ void operator = (const Grid& o) = delete; + /** allows for-each iteration over all included nodes */ + decltype(nodes.begin()) begin() {return nodes.begin();} + + /** allows for-each iteration over all included nodes */ + decltype(nodes.end()) end() {return nodes.end();} + /** * add the given element to the grid. @@ -235,14 +246,14 @@ public: Log::add(name, "running grid cleanup"); // check every single node - for (int i = (int)nodes.size() - 1; i >= 0; --i) { + for (size_t i = 0; i < nodes.size(); ++i) { // is this node marked as "deleted"? (idx == -1) if (nodes[i]._idx == -1) { // remove this node deleteNode(i); - ++i; + --i; } } @@ -261,6 +272,8 @@ private: /** hard-delete the given node */ void deleteNode(const int idx) { + _assertBetween(idx, 0, nodes.size()-1, "index out of bounds"); + // COMPLEX AND SLOW AS HELL.. BUT UGLY TO REWIRTE TO BE CORRECT // remove him from the node list (reclaim its memory and its index) diff --git a/grid/GridNeighborIterator.h b/grid/GridNeighborIterator.h index 6e3c781..a3ffdf4 100644 --- a/grid/GridNeighborIterator.h +++ b/grid/GridNeighborIterator.h @@ -8,10 +8,10 @@ class NeighborIter : std::iterator { private: /** the grid the src-node belongs to */ - const Grid& grid; + Grid* grid; /** index of the source-node within its grid */ - const int srcNodeIdx; + int srcNodeIdx; /** index of the current neighbor [0:10] */ int nIdx; @@ -20,7 +20,7 @@ public: /** ctor */ NeighborIter(const Grid& grid, const int srcNodeIdx, const int nIdx) : - grid(grid), srcNodeIdx(srcNodeIdx), nIdx(nIdx) {;} + grid((Grid*)&grid), srcNodeIdx(srcNodeIdx), nIdx(nIdx) {;} /** next neighbor */ NeighborIter& operator++() {++nIdx; return *this;} @@ -35,7 +35,7 @@ public: bool operator!=(const NeighborIter& rhs) {return srcNodeIdx != rhs.srcNodeIdx || nIdx != rhs.nIdx;} /** get the neighbor the iterator currently points to */ - T& operator*() {return (T&) grid.getNeighbor(srcNodeIdx, nIdx);} + T& operator*() {return (T&) grid->getNeighbor(srcNodeIdx, nIdx);} }; diff --git a/grid/GridPoint.h b/grid/GridPoint.h index a1e7dff..f920e8d 100755 --- a/grid/GridPoint.h +++ b/grid/GridPoint.h @@ -4,6 +4,8 @@ #include #include "../geo/Point3.h" +#include + struct GridPoint { /** x-position (in centimeter) */ @@ -42,6 +44,13 @@ struct GridPoint { /** cast to string */ operator std::string() const {return "(" + std::to_string(x_cm) + "," + std::to_string(y_cm) + "," + std::to_string(z_cm) + ")";} + /** read-only array access */ + float operator [] (const int idx) const { + _assertBetween(idx, 0, 2, "index out of bounds"); + if (0 == idx) {return x_cm;} + if (1 == idx) {return y_cm;} + {return z_cm;} + } }; diff --git a/grid/factory/GridFactory.h b/grid/factory/GridFactory.h index b044b29..92bc4aa 100755 --- a/grid/factory/GridFactory.h +++ b/grid/factory/GridFactory.h @@ -143,8 +143,8 @@ public: int y = n1.y_cm + yDiff * percent; // snap (x,y) to the grid??? - //x = std::round(x / gridSize_cm) * gridSize_cm; - //y = std::round(y / gridSize_cm) * gridSize_cm; + x = std::round(x / gridSize_cm) * gridSize_cm; + y = std::round(y / gridSize_cm) * gridSize_cm; // create a new node add it to the grid, and connect it with the previous one idx2 = grid.addUnaligned(T(x,y,z)); diff --git a/grid/factory/GridImportance.h b/grid/factory/GridImportance.h index 127a41c..0fccf33 100644 --- a/grid/factory/GridImportance.h +++ b/grid/factory/GridImportance.h @@ -4,12 +4,18 @@ #include "../Grid.h" #include "GridFactory.h" #include "../../misc/KNN.h" +#include "../../misc/KNNArray.h" #include "../../math/MiniMat2.h" #include "../../misc/Debug.h" +#include "../../nav/dijkstra/Dijkstra.h" +#include "../../nav/dijkstra/DijkstraPath.h" #include + + + /** * add an importance factor to each node within the grid. * the importance is calculated based on several facts: @@ -35,7 +41,7 @@ public: fac.addInverted(g, z_cm); // construct KNN search - KNN, T, 3> knn(inv); + KNN, 3> knn(inv); // the number of neighbors to use static constexpr int numNeighbors = 8; @@ -62,6 +68,45 @@ public: } + } + + /** attach importance-factors to the grid */ + template void addDistanceToTarget(Grid& g, Dijkstra& d) { + + //Log::add(name, "adding importance information to all nodes at height " + std::to_string(z_cm)); + + for (T& node : g) { + + DijkstraNode* dn = d.getNode(node); + if (dn != nullptr) { + node.distToTarget = dn->cumWeight / 2000; + } + + } + + + } + + template void addImportance(Grid& g, DijkstraNode* start, DijkstraNode* end) { + + // routing path + DijkstraPath path(end, start); + + // knn search within the path + KNN, 3> knn(path); + + // update each node from the grid using its distance to the path + for (T& n : g) { + + //const int idx = knn.getNearestIndex( {n.x_cm, n.y_cm, n.z_cm} ); + //T& node = g[idx]; + const float dist_cm = knn.getNearestDistance( {n.x_cm, n.y_cm, n.z_cm} ); + const float dist_m = Units::cmToM(dist_cm); + n.impPath = 1.0 + K::NormalDistribution::getProbability(0, 1.0, dist_m) * 0.8; + + + } + } @@ -102,7 +147,7 @@ public: if (ev.e1 < ev.e2) {std::swap(ev.e1, ev.e2);} // door? - if ((ev.e2/ev.e1) < 0.15) { nSrc.imp *= 1.2; } + if ((ev.e2/ev.e1) < 0.15) { nSrc.imp *= 1.3; } } diff --git a/grid/walk/GridWalkHelper.h b/grid/walk/GridWalkHelper.h new file mode 100644 index 0000000..3bd717b --- /dev/null +++ b/grid/walk/GridWalkHelper.h @@ -0,0 +1,17 @@ +#ifndef GRIDWALKHELPER_H +#define GRIDWALKHELPER_H + +#include "../../geo/Heading.h" + +class GridWalkHelper { + +public: + + /** get the heading-change between the two given locations */ + template static Heading getHeading(const T& from, const T& to) { + return Heading(from.x_cm, from.y_cm, to.x_cm, to.y_cm); + } + +}; + +#endif // GRIDWALKHELPER_H diff --git a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h new file mode 100644 index 0000000..56b33b2 --- /dev/null +++ b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h @@ -0,0 +1,160 @@ +#ifndef GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H +#define GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H + +#include "../../geo/Heading.h" +#include "../Grid.h" + +#include "../../math/DrawList.h" +#include + +#include "../../nav/dijkstra/Dijkstra.h" + +#include "GridWalkState.h" +#include "GridWalkHelper.h" + +/** + * perform walks on the grid based on some sort of weighting + * and drawing from the weighted elements + */ +template class GridWalkLightAtTheEndOfTheTunnel { + +private: + + /** per-edge: change heading with this sigma */ + static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(3); + + /** per-edge: allowed heading difference */ + static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(30); + + + /** allows drawing elements according to their probability */ + DrawList drawer; + + /** fast random-number-generator */ + std::minstd_rand gen; + + /** 0-mean normal distribution */ + std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); + + Dijkstra dijkstra; + +public: + + /** ctor with the target you want to reach */ + template GridWalkLightAtTheEndOfTheTunnel(Grid& grid, const Access& acc, const T& target) { + + // build all shortest path to reach th target + dijkstra.build(target, target, acc); + + // attach a corresponding weight-information to each user-grid-node + for (T& node : grid) { + + const DijkstraNode* dn = dijkstra.getNode(node); + + // should never be null as all nodes were evaluated + if (dn != nullptr) { + node.distToTarget = dn->cumWeight/2000; + } + + } + + } + + template GridWalkState getDestination(Grid& grid, GridWalkState start, float distance_m) { + + int retries = 2; + GridWalkState res; + + // try to walk the given distance from the start + // if this fails (reached a dead end) -> restart (maybe the next try finds a better path) + do { + res = walk(grid, start, distance_m); + } while (res.node == nullptr && --retries); + + // still reaching a dead end? + // -> try a walk in the opposite direction instead + if (res.node == nullptr) { + res = walk(grid, GridWalkState(start.node, start.heading.getInverted()), distance_m); + } + + // still nothing found? -> keep the start as-is + return (res.node == nullptr) ? (start) : (res); + + } + +private: + + template GridWalkState walk(Grid& grid, GridWalkState cur, float distRest_m) { + + drawer.reset();; + + // calculate the weight for all possible destinations from "cur" + for (T& neighbor : grid.neighbors(*cur.node)) { + + // heading when walking from cur to neighbor + const Heading potentialHeading = GridWalkHelper::getHeading(*cur.node, neighbor); + + // angular difference + const float diff = cur.heading.getDiffHalfRAD(potentialHeading); + + // probability for this direction change? + double prob = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff); + + // perfer locations reaching the target + const double shortening = cur.node->distToTarget - neighbor.distToTarget; + if (shortening > 0) {prob *= 30;} // << importance factor!! + + drawer.add(neighbor, prob); + + } + + GridWalkState next(nullptr, cur.heading); + + // pick a random destination + T& nDir = drawer.get(); + const Heading hDir = GridWalkHelper::getHeading(*cur.node, nDir); + //next.heading += (cur.heading.getRAD() - hDir.getRAD()) * -0.5; + next.heading = hDir; + next.heading += headingChangeDist(gen); + + // compare two neighbors according to their implied heading change + auto compp = [&] (const T& n1, const T& n2) { + Heading h1 = GridWalkHelper::getHeading(*cur.node, n1); + Heading h2 = GridWalkHelper::getHeading(*cur.node, n2); + const float d1 = next.heading.getDiffHalfRAD(h1); + const float d2 = next.heading.getDiffHalfRAD(h2); + // same heading -> prefer nodes nearer to the target. needed for stairs!!! + // BAD: leads to straight lines in some palces. see solution B (below) + //return (d1 < d2) && (n1.distToTarget < n2.distToTarget); + + // VERY IMPORTANT! + // pick the node with the smallest heading change. + // if the heading change is the same for two nodes, pick a random one! + return (d1 == d2) ? (rand() < RAND_MAX/2) : (d1 < d2); + }; + + // pick the neighbor best matching the new heading + auto it = grid.neighbors(*cur.node); + T& nn = *std::min_element(it.begin(), it.end(), compp); + next.node = &nn; + +// // pervent dramatic heading changes. instead: try again +// if (cur.heading.getDiffHalfRAD(getHeading(*cur.node, nn)) > Angle::degToRad(60)) { +// return State(nullptr, 0); +// } + + // get the distance up to this neighbor + distRest_m -= next.node->getDistanceInMeter(*cur.node); + + // done? + if (distRest_m <= 0) {return next;} + + // another round.. + return walk(grid, next, distRest_m); + + } + +}; + + +#endif // GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H diff --git a/grid/walk/GridWalkState.h b/grid/walk/GridWalkState.h new file mode 100644 index 0000000..ae48679 --- /dev/null +++ b/grid/walk/GridWalkState.h @@ -0,0 +1,21 @@ +#ifndef GRIDWALKSTATE_H +#define GRIDWALKSTATE_H + +#include "../../geo/Heading.h" + +template struct GridWalkState { + + /** the user-node this state resides at */ + const T* node; + + /** the current heading */ + Heading heading; + + /** empty ctor */ + GridWalkState() : node(nullptr), heading(0) {;} + + /** ctor with user-node and heading */ + GridWalkState(const T* node, const Heading heading) : node(node), heading(heading) {;} + +}; +#endif // GRIDWALKSTATE_H diff --git a/grid/walk/GridWalkWeighted.h b/grid/walk/GridWalkWeighted.h new file mode 100644 index 0000000..f38d0a8 --- /dev/null +++ b/grid/walk/GridWalkWeighted.h @@ -0,0 +1,141 @@ +#ifndef GRIDWALKWEIGHTED_H +#define GRIDWALKWEIGHTED_H + +#include "../../geo/Heading.h" +#include "../Grid.h" + +#include "../../math/DrawList.h" +#include + +/** + * perform walks on the grid based on some sort of weighting + * and drawing from the weighted elements + */ +template class GridWalkWeighted { + +public: + + struct State { + const T* node; + Heading heading; + + State() : node(nullptr), heading(0) {;} + State(const T* node, Heading heading) : node(node), heading(heading) {;} + + }; + + +private: + + /** per-edge: change heading with this sigma */ + static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(3); + + /** per-edge: allowed heading difference */ + static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(30); + + + /** allows drawing elements according to their probability */ + DrawList drawer; + + /** fast random-number-generator */ + std::minstd_rand gen; + + /** 0-mean normal distribution */ + std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); + +public: + + template State getDestination(Grid& grid, State start, float distance_m) { + + int retries = 2; + State res; + + // try to walk the given distance from the start + // if this fails (reached a dead end) -> restart (maybe the next try finds a better path) + do { + res = walk(grid, start, distance_m); + } while (res.node == nullptr && --retries); + + // still reaching a dead end? + // -> try a walk in the opposite direction instead + if (res.node == nullptr) { + res = walk(grid, State(start.node, start.heading.getInverted()), distance_m); + } + + // still nothing found? -> keep the start as-is + return (res.node == nullptr) ? (start) : (res); + + } + +private: + + static Heading getHeading(const T& from, const T& to) { + return Heading(from.x_cm, from.y_cm, to.x_cm, to.y_cm); + } + + template State walk(Grid& grid, State cur, float distRest_m) { + + drawer.reset();; + + // calculate the weight for all possible destinations from "cur" + for (T& neighbor : grid.neighbors(*cur.node)) { + + // heading when walking from cur to neighbor + const Heading potentialHeading = getHeading(*cur.node, neighbor); + + // angular difference + const float diff = cur.heading.getDiffHalfRAD(potentialHeading); + + // probability for this direction change? + double prob = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff); + + // perfer locations reaching the target + const double shortening = cur.node->distToTarget - neighbor.distToTarget; + if (shortening > 0) {prob *= 5;} + + drawer.add(neighbor, prob); + + } + + State next(nullptr, cur.heading); + + // pick a random destination + T& nDir = drawer.get(); + const Heading hDir = getHeading(*cur.node, nDir); + //next.heading += (cur.heading.getRAD() - hDir.getRAD()) * -0.5; + next.heading = hDir; + next.heading += headingChangeDist(gen); + + // compare two neighbors according to their implied heading change + auto compp = [&] (const T& n1, const T& n2) { + Heading h1 = getHeading(*cur.node, n1); + Heading h2 = getHeading(*cur.node, n2); + const float d1 = next.heading.getDiffHalfRAD(h1); + const float d2 = next.heading.getDiffHalfRAD(h2); + return d1 < d2; + }; + + // pick the neighbor best matching the new heading + auto it = grid.neighbors(*cur.node); + T& nn = *std::min_element(it.begin(), it.end(), compp); + next.node = &nn; + +// // pervent dramatic heading changes. instead: try again +// if (cur.heading.getDiffHalfRAD(getHeading(*cur.node, nn)) > Angle::degToRad(60)) { +// return State(nullptr, 0); +// } + + // get the distance up to this neighbor + distRest_m -= next.node->getDistanceInMeter(*cur.node); + + // done? + if (distRest_m <= 0) {return next;} + + // another round.. + return walk(grid, next, distRest_m); + + } + +}; + +#endif // GRIDWALKWEIGHTED_H diff --git a/grid/walk/GridWalkWeighted2.h b/grid/walk/GridWalkWeighted2.h new file mode 100644 index 0000000..04cb09f --- /dev/null +++ b/grid/walk/GridWalkWeighted2.h @@ -0,0 +1,127 @@ +#ifndef GRIDWALKWEIGHTED2_H +#define GRIDWALKWEIGHTED2_H + +#include "../../geo/Heading.h" +#include "../Grid.h" + +#include "../../math/DrawList.h" +#include + +/** + * perform walks on the grid based on some sort of weighting + * and drawing from the weighted elements + */ +template class GridWalkWeighted { + +public: + + struct State { + const T* node; + Heading heading; + + State() : node(nullptr), heading(0) {;} + State(const T* node, Heading heading) : node(node), heading(heading) {;} + + }; + + +private: + + /** per-edge: change heading with this sigma */ + static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(5); + + /** per-edge: allowed heading difference */ + static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(20); + + + /** allows drawing elements according to their probability */ + DrawList drawer; + + /** fast random-number-generator */ + std::minstd_rand gen; + + /** 0-mean normal distribution */ + std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); + +public: + + template State getDestination(Grid& grid, State start, float distance_m) { + + int retries = 2; + State res; + + // try to walk the given distance from the start + // if this fails (reached a dead end) -> restart (maybe the next try finds a better path) + do { + res = walk(grid, start, distance_m); + } while (res.node == nullptr && --retries); + + // still reaching a dead end? + // -> try a walk in the opposite direction instead + if (res.node == nullptr) { + res = walk(grid, State(start.node, start.heading.getInverted()), distance_m); + } + + // still nothing found? -> keep the start as-is + return (res.node == nullptr) ? (start) : (res); + + } + +private: + + static Heading getHeading(const T& from, const T& to) { + return Heading(from.x_cm, from.y_cm, to.x_cm, to.y_cm); + } + + template State walk(Grid& grid, State cur, float distRest_m) { + + // make the draw-list empty (faster than allocating a new one every time?) + drawer.reset(); + + // calculate probabilites for the neighboring nodes + for (T& neighbor : grid.neighbors(*cur.node)) { + + // heading when walking from cur to neighbor + Heading potentialHeading(cur.node->x_cm, cur.node->y_cm, neighbor.x_cm, neighbor.y_cm); + + // angular difference + const float diff = cur.heading.getDiffHalfRAD(potentialHeading); + + // probability for this change? + double prob = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff); + + // adjust by the neighbors importance factor + //prob += std::pow(neighbor.impPath, 2); + prob *= neighbor.imp; + //prob *= K::NormalDistribution::getProbability(1.4, 0.2, neighbor.imp); + + drawer.add(neighbor, prob); + + } + + // if there is no likely neighbor at all, we reached a dead end + // -> start over! + if (drawer.getCumProbability() < 0.01) {return State(nullptr, 0);} + + // get the next node + T* nn = &drawer.get(); + + //Heading potentialHeading(cur.node->x_cm, cur.node->y_cm, nn->x_cm, nn->y_cm); + //State next(nn, potentialHeading); + State next(&nn, cur.heading); + next.heading += headingChangeDist(gen); + + // get the distance up to this neighbor + distRest_m -= next.node->getDistanceInMeter(*cur.node); + + // done? + if (distRest_m <= 0) {return next;} + + // another round.. + return walk(grid, next, distRest_m); + + } + +}; + +#endif // GRIDWALKWEIGHTED2_H diff --git a/grid/walk/TestWalkWeighted3.h b/grid/walk/TestWalkWeighted3.h new file mode 100644 index 0000000..84f98dc --- /dev/null +++ b/grid/walk/TestWalkWeighted3.h @@ -0,0 +1,139 @@ +#ifndef GRIDWALKWEIGHTED3_H +#define GRIDWALKWEIGHTED3_H + +#include "../../geo/Heading.h" +#include "../Grid.h" + +#include "../../math/DrawList.h" +#include + +/** + * perform walks on the grid based on some sort of weighting + * and drawing from the weighted elements + */ +template class GridWalkWeighted { + +public: + + struct State { + const T* node; + Heading heading; + + State() : node(nullptr), heading(0) {;} + State(const T* node, Heading heading) : node(node), heading(heading) {;} + + }; + + +private: + + /** per-edge: change heading with this sigma */ + static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(3); + + /** per-edge: allowed heading difference */ + static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(30); + + + /** allows drawing elements according to their probability */ + DrawList drawer; + + /** fast random-number-generator */ + std::minstd_rand gen; + + /** 0-mean normal distribution */ + std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); + +public: + + template State getDestination(Grid& grid, State start, float distance_m) { + + int retries = 2; + State res; + + // try to walk the given distance from the start + // if this fails (reached a dead end) -> restart (maybe the next try finds a better path) + do { + res = walk(grid, start, distance_m); + } while (res.node == nullptr && --retries); + + // still reaching a dead end? + // -> try a walk in the opposite direction instead + if (res.node == nullptr) { + res = walk(grid, State(start.node, start.heading.getInverted()), distance_m); + } + + // still nothing found? -> keep the start as-is + return (res.node == nullptr) ? (start) : (res); + + } + +private: + + static Heading getHeading(const T& from, const T& to) { + return Heading(from.x_cm, from.y_cm, to.x_cm, to.y_cm); + } + + template State walk(Grid& grid, State cur, float distRest_m) { + + drawer.reset();; + + // calculate the weight for all possible destinations from "cur" + for (T& neighbor : grid.neighbors(*cur.node)) { + + // heading when walking from cur to neighbor + const Heading potentialHeading = getHeading(*cur.node, neighbor); + + // angular difference + const float diff = cur.heading.getDiffHalfRAD(potentialHeading); + + // probability for this change? + double prob = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff); + + prob *= std::pow(neighbor.impPath, 5); + + drawer.add(neighbor, prob); + + } + + State next(nullptr, cur.heading); + + // pick a random destination + T& nDir = drawer.get(); + const Heading hDir = getHeading(*cur.node, nDir); + //next.heading += (cur.heading.getRAD() - hDir.getRAD()) * -0.95; + next.heading = hDir; + next.heading += headingChangeDist(gen); + + // compare two neighbors according to their implied heading change + auto compp = [&] (const T& n1, const T& n2) { + Heading h1 = getHeading(*cur.node, n1); + Heading h2 = getHeading(*cur.node, n2); + const float d1 = next.heading.getDiffHalfRAD(h1); + const float d2 = next.heading.getDiffHalfRAD(h2); + return d1 < d2; + }; + + // pick the neighbor best matching the new heading + auto it = grid.neighbors(*cur.node); + T& nn = *std::min_element(it.begin(), it.end(), compp); + next.node = &nn; + +// // pervent dramatic heading changes. instead: try again +// if (cur.heading.getDiffHalfRAD(getHeading(*cur.node, nn)) > Angle::degToRad(60)) { +// return State(nullptr, 0); +// } + + // get the distance up to this neighbor + distRest_m -= next.node->getDistanceInMeter(*cur.node); + + // done? + if (distRest_m <= 0) {return next;} + + // another round.. + return walk(grid, next, distRest_m); + + } + +}; + +#endif // GRIDWALKWEIGHTED3_H diff --git a/main.cpp b/main.cpp index ed13c87..1a20c74 100755 --- a/main.cpp +++ b/main.cpp @@ -15,7 +15,7 @@ int main(int argc, char** argv) { #ifdef WITH_TESTS ::testing::InitGoogleTest(&argc, argv); - ::testing::GTEST_FLAG(filter) = "*TestAll*"; + ::testing::GTEST_FLAG(filter) = "*Walk*"; return RUN_ALL_TESTS(); #endif diff --git a/math/DrawList.h b/math/DrawList.h new file mode 100644 index 0000000..a74e729 --- /dev/null +++ b/math/DrawList.h @@ -0,0 +1,82 @@ +#ifndef DRAWLIST_H +#define DRAWLIST_H + +#include +#include + +/** + * add elements of a certain probability + * and randomly draw from them + */ +template class DrawList { + + /** one entry */ + struct Entry { + + /** the user element */ + T element; + + /** the cumulative probability up to this element */ + double cumProbability; + + /** ctor */ + Entry(T element, const double cumProbability) : element(element), cumProbability(cumProbability) {;} + + /** compare for searches */ + bool operator < (const double val) const {return cumProbability < val;} + + }; + +private: + + /** current cumulative probability */ + double cumProbability; + + /** all contained elements */ + std::vector elements; + + /** random number generator */ + std::minstd_rand gen; + +public: + + /** ctor */ + DrawList() : cumProbability(0) { + ; + } + + /** reset */ + void reset() { + cumProbability = 0; + elements.clear(); + } + + /** add a new user-element and its probability */ + void add(T element, const double probability) { + cumProbability += probability; + elements.push_back(Entry(element, cumProbability)); + } + + /** get a random element based on its probability */ + T get() { + + // generate random number between [0:cumProbability] + std::uniform_real_distribution<> dist(0, cumProbability); + + // get a random value + const double rndVal = dist(gen); + + // binary search for the matching entry O(log(n)) + const auto tmp = std::lower_bound(elements.begin(), elements.end(), rndVal); + return (*tmp).element; + + } + + /** get the current, cumulative probability */ + double getCumProbability() const { + return cumProbability; + } + +}; + +#endif // DRAWLIST_H diff --git a/misc/Debug.h b/misc/Debug.h index 7506084..7f543f7 100644 --- a/misc/Debug.h +++ b/misc/Debug.h @@ -4,23 +4,37 @@ #include #include #include +#include "Time.h" class Log { public: + + static void add(const char* comp, const std::string what) { addComp(comp); - std::cout << what << std::endl; + std::cout << what; + addTime(); + std::cout << std::endl; } static void add(const std::string& component, const std::string what) { addComp(component.c_str()); - std::cout << what << std::endl; + std::cout << what; + addTime(); + std::cout << std::endl; } private: + static void addTime() { + static auto last = Time::tick(); + const auto cur = Time::tick(); + std::cout << " (+" << Time::diffMS(last, cur) << "ms)"; + last = cur; + } + static void addComp(const char* component) { std::cout << "[" << std::setw(12) << std::setfill(' ') << component << "] "; } diff --git a/misc/KNN.h b/misc/KNN.h index 7c941b9..503fb78 100644 --- a/misc/KNN.h +++ b/misc/KNN.h @@ -2,6 +2,7 @@ #define KNN_H #include "../lib/nanoflann/nanoflann.hpp" +#include "Debug.h" /** * helper class to extract k-nearest-neighbors @@ -9,14 +10,16 @@ * uses nanoflann * * usage: - * KNN, T, 3> knn(theGrid); - * float search[] = {0,0,0}; - * std::vector elems = knn.get(search, 3); + * Grid<30, T> theGrid; + * KNN, 3, float> knn(theGrid); + * std::vector elems = knn.get({0,0,0}, 10); */ -template class KNN { +template class KNN { private: + static constexpr const char* name = "KNN"; + /** type-definition for the nanoflann KD-Tree used for searching */ typedef nanoflann::KDTreeSingleIndexAdaptor, DataStructure, dim> Tree; @@ -33,11 +36,15 @@ public: /** ctor */ KNN(DataStructure& data) : tree(dim, data, nanoflann::KDTreeSingleIndexAdaptorParams(maxLeafs)), data(data) { + + Log::add(name, "building kd-tree for " + std::to_string(data.kdtree_get_point_count()) + " elements"); tree.buildIndex(); + Log::add(name, "done"); + } /** get the k-nearest-neighbors for the given input point */ - std::vector get(const Scalar* point, const int numNeighbors, const float maxDistSquared = 99999) const { + template std::vector get(const Scalar* point, const int numNeighbors, const float maxDistSquared = 99999) const { // buffer for to-be-fetched neighbors size_t indices[numNeighbors]; @@ -56,6 +63,11 @@ public: } + /** get the k-nearest-neighbors for the given input point */ + template std::vector get(std::initializer_list point, const int numNeighbors, const float maxDistSquared = 99999) const { + return get(point.begin(), numNeighbors, maxDistSquared); + } + /** get the nearest neighbor and its distance */ void getNearest(const Scalar* point, size_t& idx, float& distSquared) { @@ -64,6 +76,30 @@ public: } + /** get the index of the element nearest to the given point */ + size_t getNearestIndex(const Scalar* point) { + size_t idx; + float distSquared; + tree.knnSearch(point, 1, &idx, &distSquared); + return idx; + } + + /** get the index of the element nearest to the given point */ + size_t getNearestIndex(const std::initializer_list lst) { + size_t idx; + float distSquared; + tree.knnSearch(lst.begin(), 1, &idx, &distSquared); + return idx; + } + + /** get the distance to the element nearest to the given point */ + float getNearestDistance(const std::initializer_list lst) { + size_t idx; + float distSquared; + tree.knnSearch(lst.begin(), 1, &idx, &distSquared); + return std::sqrt(distSquared); + } + void get(const Scalar* point, const int numNeighbors, size_t* indices, float* squaredDist) { // find k-nearest-neighbors diff --git a/misc/KNNArray.h b/misc/KNNArray.h new file mode 100644 index 0000000..10b675d --- /dev/null +++ b/misc/KNNArray.h @@ -0,0 +1,59 @@ +#ifndef KNNARRAY_H +#define KNNARRAY_H + +/** + * this wrapper class provides all methods needed for nanoflanns KNN-search. + * in order for this wrapper class to work, your data-structure must provide + * the following methods: + * + * PointList: + * size() - return the number of contained points + * operator [] - access points via their index + * Point + * operator [] - access each dimension via its index + * + * example: + * std::vector points; + * KNNArray> arr(points); + * KNN>, 3> knn(arr); + */ +template class KNNArray { + +private: + + /** the underlying data structure */ + const T& elem; + +public: + + /** ctor with the underlying data structure */ + KNNArray(const T& elem) : elem(elem) { + ; + } + + /** get the number of elements to search throrugh */ + inline int kdtree_get_point_count() const { + return elem.size(); + } + + /** use nanoflanns default bbox */ + template inline bool kdtree_get_bbox(BBOX& bb) const { + (void) bb; return false; + } + + /** get the idx-th element's dim-th coordinate */ + inline float kdtree_get_pt(const size_t idx, const int dim) const { + return elem[idx][dim]; + } + + /** get the SQUARED distance between the given coordinates and the provided element */ + inline float kdtree_distance(const float* p1, const size_t idx_p2, size_t) const { + const float d0 = p1[0] - elem[idx_p2][0]; + const float d1 = p1[1] - elem[idx_p2][1]; + const float d2 = p1[2] - elem[idx_p2][2]; + return (d0*d0) + (d1*d1) + (d2*d2); + } + +}; + +#endif // KNNARRAY_H diff --git a/misc/Time.h b/misc/Time.h new file mode 100644 index 0000000..d6fa4b3 --- /dev/null +++ b/misc/Time.h @@ -0,0 +1,21 @@ +#ifndef TIME_H +#define TIME_H + +#include + +class Time { + +public: + + static std::chrono::system_clock::time_point tick() { + return std::chrono::system_clock::now(); + + } + + static int diffMS(std::chrono::system_clock::time_point tick1, std::chrono::system_clock::time_point tick2) { + return std::chrono::duration_cast(tick2 - tick1).count(); + } + +}; + +#endif // TIME_H diff --git a/nav/dijkstra/Dijkstra.h b/nav/dijkstra/Dijkstra.h index e82fb2f..c19858e 100644 --- a/nav/dijkstra/Dijkstra.h +++ b/nav/dijkstra/Dijkstra.h @@ -5,9 +5,11 @@ #include #include #include +#include #include "DijkstraStructs.h" #include "../../misc/Debug.h" +#include "../../misc/Time.h" #include @@ -19,26 +21,11 @@ template class Dijkstra { /** all already processed edges */ std::unordered_set> usedEdges; - /** to-be-processed nodes (USE LINKED LIST INSTEAD?!) */ + /** to-be-processed nodes (NOTE: using std::list here was SLOWER!) */ std::vector*> toBeProcessedNodes; - public: - /** get (or create) a new node for the given user-node */ - DijkstraNode* getNode(const T* userNode) { - if (nodes.find(userNode) == nodes.end()) { - DijkstraNode* dn = new DijkstraNode(userNode); - nodes[userNode] = dn; - } - return nodes[userNode]; - } - - /** get the edge (bi-directional) between the two given nodes */ - DijkstraEdge getEdge(const DijkstraNode* n1, const DijkstraNode* n2) { - return DijkstraEdge(n1, n2); - } - /** get the dijkstra-pendant for the given user-node */ DijkstraNode* getNode(const T& userNode) { return nodes[&userNode]; @@ -50,7 +37,10 @@ public: // NOTE: end is currently ignored! // runs until all nodes were evaluated - Log::add("Dijkstra", "calculating dijkstra from " + (std::string)start); + // compare two nodes by their distance from the start + static auto comp = [] (const DijkstraNode* n1, const DijkstraNode* n2) {return n1->cumWeight < n2->cumWeight;}; + + Log::add("Dijkstra", "calculating dijkstra from " + (std::string)start + " to ALL OTHER nodes"); // cleanup toBeProcessedNodes.clear(); @@ -71,10 +61,14 @@ public: while(!toBeProcessedNodes.empty()) { // get the next to-be-processed node - DijkstraNode* dnSrc = toBeProcessedNodes[0]; + const auto min = std::min_element(toBeProcessedNodes.begin(), toBeProcessedNodes.end(), comp); + DijkstraNode* dnSrc = *min; + + // stop when end was reached?? + //if (dnSrc->element == &end) {break;} // and remove him from the list - toBeProcessedNodes.erase(toBeProcessedNodes.begin()); + toBeProcessedNodes.erase(min); // process each neighbor of the current element for (int i = 0; i < acc.getNumNeighbors(*dnSrc->element); ++i) { @@ -90,7 +84,7 @@ public: DijkstraNode* dnDst = getNode(dst); // get-or-create the edge describing the connection - DijkstraEdge edge = getEdge(dnSrc, dnDst); + const DijkstraEdge edge = getEdge(dnSrc, dnDst); // was this edge already processed? -> skip it if (usedEdges.find(edge) != usedEdges.end()) {continue;} @@ -98,6 +92,8 @@ public: // otherwise: remember it usedEdges.insert(edge); + + // and add the node for later processing toBeProcessedNodes.push_back(dnDst); @@ -110,18 +106,30 @@ public: } - // sort the nodes by distance-from-start (shortest first) - auto comp = [] (const DijkstraNode* n1, const DijkstraNode* n2) {return n1->cumWeight < n2->cumWeight;}; - std::sort(toBeProcessedNodes.begin(), toBeProcessedNodes.end(), comp); - } - Log::add("Dijkstra", "processed " + std::to_string(nodes.size()) + " nodes"); - - // cleanup + // reclaim temporal memory toBeProcessedNodes.clear(); usedEdges.clear(); + Log::add("Dijkstra", "processed " + std::to_string(nodes.size()) + " nodes"); + + } + +private: + + /** get (or create) a new node for the given user-node */ + inline DijkstraNode* getNode(const T* userNode) { + if (nodes.find(userNode) == nodes.end()) { + DijkstraNode* dn = new DijkstraNode(userNode); + nodes[userNode] = dn; + } + return nodes[userNode]; + } + + /** get the edge (bi-directional) between the two given nodes */ + inline DijkstraEdge getEdge(const DijkstraNode* n1, const DijkstraNode* n2) const { + return DijkstraEdge(n1, n2); } }; diff --git a/nav/dijkstra/DijkstraPath.h b/nav/dijkstra/DijkstraPath.h new file mode 100644 index 0000000..76ef80f --- /dev/null +++ b/nav/dijkstra/DijkstraPath.h @@ -0,0 +1,58 @@ +#ifndef DIJKSTRAPATH_H +#define DIJKSTRAPATH_H + +#include "DijkstraStructs.h" + +/** + * describes a dijkstra-generated path between end and start. + * allows KNN searches for points within this path. + * + */ +template class DijkstraPath { + +private: + + /** the constructed path */ + std::vector*> path; + +public: + + /** ctor from end- to start-node */ + DijkstraPath(DijkstraNode* end, DijkstraNode* start) { + + // follow the path from the end to the start + DijkstraNode* curNode = end; + while (curNode != start) { + path.push_back(curNode); + curNode = curNode->previous; + } + + } + + /** NANOFLANN: number of elements in the path */ + inline int kdtree_get_point_count() const { + return path.size(); + } + + /** NANOFLANN: use default bbox */ + template inline bool kdtree_get_bbox(BBOX& bb) const { + (void) bb; return false; + } + + /** NANOFLANN: get the idx-th elements dim-th dimension */ + inline float kdtree_get_pt(const size_t idx, const int dim) const { + return (*(path[idx]->element))[dim]; + } + + /** NANOFLANN: get the distance between the given point and the idx-th element */ + inline float kdtree_distance(const float* p1, const size_t idx_p2, size_t) const { + const DijkstraNode* n = path[idx_p2]; + const float d0 = p1[0] - (*(n->element))[0]; + const float d1 = p1[1] - (*(n->element))[1]; + const float d2 = p1[2] - (*(n->element))[2]; + return (d0*d0) + (d1*d1) + (d2*d2); + } + +}; + +#endif // DIJKSTRAPATH_H diff --git a/nav/dijkstra/DijkstraStructs.h b/nav/dijkstra/DijkstraStructs.h index 8984832..3daa1e6 100644 --- a/nav/dijkstra/DijkstraStructs.h +++ b/nav/dijkstra/DijkstraStructs.h @@ -20,23 +20,20 @@ template struct DijkstraNode { float cumWeight; -// /** ctor */ -// DijkstraNode() : element(nullptr), previous(), cumWeight(INF) {;} - /** ctor */ DijkstraNode(const T* element) : element(element), previous(), cumWeight(INF) {;} - /** equal? (bi-dir) */ - bool operator == (const DijkstraNode& other) { - return element == other.element; - } +// /** equal? (bi-dir) */ +// bool operator == (const DijkstraNode& other) const { +// return element == other.element; +// } }; /** - * data structure describing the connection between two nodes - * only used to track already processed connections! + * data structure describing the connection between two nodes. + * NOTE: only used to track already processed connections! */ template struct DijkstraEdge { @@ -49,7 +46,7 @@ template struct DijkstraEdge { /** ctor */ DijkstraEdge(const DijkstraNode* src, const DijkstraNode* dst) : src(src), dst(dst) {;} - /** equal? (bi-dir) */ + /** equal? (bi-directional! edge direction does NOT matter) */ bool operator == (const DijkstraEdge& other) const { return ((dst == other.dst) && (src == other.src)) || ((src == other.dst) && (dst == other.src)); @@ -57,15 +54,7 @@ template struct DijkstraEdge { }; -//template struct DijkstraEdgeWeighted : public DijkstraEdge { - -// /** the edge's weight */ -// float weight; - -// DijkstraEdgeWeighted(const DijkstraNode* src, const DijkstraNode* dst, const float weight) : DijkstraEdge(src,dst), weight(weight) {;} - -//}; - +/** allows adding DijkstraEdge to hash-maps */ namespace std { template struct hash>{ size_t operator()(const DijkstraEdge& e) const { diff --git a/tests/data/fp1.svg b/tests/data/fp1.svg old mode 100644 new mode 100755 index 9392a34..1c28b56 --- a/tests/data/fp1.svg +++ b/tests/data/fp1.svg @@ -26,17 +26,17 @@ inkscape:pageopacity="0.0" inkscape:pageshadow="2" inkscape:zoom="5.6" - inkscape:cx="73.937318" - inkscape:cy="144.77356" + inkscape:cx="124.63903" + inkscape:cy="144.84968" inkscape:document-units="px" - inkscape:current-layer="layer3" + inkscape:current-layer="layer1" showgrid="true" units="px" inkscape:object-nodes="true" - inkscape:window-width="1920" - inkscape:window-height="1021" - inkscape:window-x="0" - inkscape:window-y="0" + inkscape:window-width="1600" + inkscape:window-height="837" + inkscape:window-x="-8" + inkscape:window-y="-8" inkscape:window-maximized="1"> image/svg+xml - + @@ -60,8 +60,7 @@ inkscape:groupmode="layer" id="layer1" transform="translate(0,-796.36219)" - style="display:none" - sodipodi:insensitive="true"> + style="display:inline"> + + style="display:none"> + diff --git a/tests/geo/TestHeading.cpp b/tests/geo/TestHeading.cpp new file mode 100644 index 0000000..0fc226b --- /dev/null +++ b/tests/geo/TestHeading.cpp @@ -0,0 +1,29 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" +#include "../../geo/Heading.h" + +TEST(Heading, diff) { + + // 180 degree turn + { +// Heading h1(0,0, 0,+1); +// Heading h2(0,0, 0,-1); +// ASSERT_NEAR(h1.getDiffHalfRAD(h2), M_PI, 0.001); +// ASSERT_NEAR(h2.getDiffHalfRAD(h1), M_PI, 0.001); + } + + // ~180 degree turn + { + Heading h1(0,0, 0.00,+1); + Heading h2(0,1, -0.01,-1); + Heading h3(0,1, +0.01,-1); + ASSERT_NEAR(h1.getDiffHalfRAD(h2), M_PI, 0.01); + ASSERT_NEAR(h2.getDiffHalfRAD(h1), M_PI, 0.01); + ASSERT_NEAR(h1.getDiffHalfRAD(h3), M_PI, 0.01); + ASSERT_NEAR(h3.getDiffHalfRAD(h1), M_PI, 0.01); + } + +} + +#endif diff --git a/tests/grid/Plot.h b/tests/grid/Plot.h index f9da3c5..6d29a8a 100644 --- a/tests/grid/Plot.h +++ b/tests/grid/Plot.h @@ -10,14 +10,16 @@ #include #include +#include "../../floorplan/Floor.h" class GP : public GridNode, public GridPoint { public: float imp = 1.0f; + float impPath = 1.0f; + float distToTarget = 1.0; public: GP() : GridNode(), GridPoint() {;} GP(int x, int y, int z) : GridNode(), GridPoint(x,y,z) {;} - }; class Plot { @@ -27,22 +29,76 @@ public: K::Gnuplot gp; K::GnuplotSplot splot; - K::GnuplotSplotElementColorPoints points; + K::GnuplotSplotElementColorPoints nodes; K::GnuplotSplotElementLines lines; + K::GnuplotSplotElementLines floors; - template Plot& build(Grid& g) { + /** ctor */ + Plot() { gp << "set ticslevel 0\n"; gp << "set view equal xyz\n"; gp << "set cbrange[0.5:1.5]\n"; gp << "set palette gray negative\n"; + //gp << "set hidden3d front\n"; + + splot.add(&nodes); + + splot.add(&floors); + floors.setLineWidth(2); + floors.setColorHex("#008800"); + + } + + template Plot& showGrid(Grid& g) { + addEdges(g); + addNodes(g); + return *this; + } + + template Plot& addEdges(Grid& g) { + + // prevent adding edges twice + std::set done; + + for (GP& n1 : g) { + for (const T& n2 : g.neighbors(n1)) { + size_t edge = g.getUID(n1) ^ g.getUID(n2); + if (done.find(edge) == done.end()) { + K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm); + K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm); + lines.addSegment(p1, p2); + done.insert(edge); + } + } + } + + return *this; + + } + + template Plot& addNodes(Grid& g) { + + for (GP& n1 : g) { + K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm); + nodes.add(p1, 1.0); + } + + return *this; + + } + + template Plot& build(Grid& g) { + std::set done; int cnt = 0; for (int i = 0; i < g.getNumNodes(); ++i) { const GP& n1 = g[i]; - points.add(K::GnuplotPoint3(n1.x_cm, n1.y_cm, n1.z_cm), n1.imp); + //const float color = std::pow(n1.distToTarget,0.1); + const float color = n1.imp * n1.impPath; + nodes.add(K::GnuplotPoint3(n1.x_cm, n1.y_cm, n1.z_cm), color); for (const T& n2 : g.neighbors(n1)) { //for (int n = 0; n < n1.getNumNeighbors(); ++n) { @@ -59,15 +115,28 @@ public: } - points.setPointSize(1); - //splot.add(&lines); - splot.add(&points); - + nodes.setPointSize(1.0); return *this; } + /** remove all floors from the plot */ + Plot& resetFloors() { + floors.clear(); + return *this; + } + + /** add the given floor to the plot */ + Plot& addFloor(Floor& f, const float z_cm) { + for (const Line2& l : f.getObstacles()) { + K::GnuplotPoint3 p1(l.p1.x, l.p1.y, z_cm+10); + K::GnuplotPoint3 p2(l.p2.x, l.p2.y, z_cm+10); + floors.addSegment(p1, p2); + } + return *this; + } + Plot& fire() { gp.draw(splot); gp.flush(); diff --git a/tests/grid/TestAll.cpp b/tests/grid/TestAll.cpp index cd6857c..ddea5ec 100644 --- a/tests/grid/TestAll.cpp +++ b/tests/grid/TestAll.cpp @@ -6,6 +6,7 @@ #include "../../grid/factory/GridFactory.h" #include "../../floorplan/FloorplanFactorySVG.h" #include "../../nav/dijkstra/Dijkstra.h" +#include "../../grid/walk/GridWalkWeighted.h" #include "Plot.h" @@ -29,38 +30,55 @@ TEST(TestAll, Nav) { } tmp(g); GridFactory<20, GP> gf(g); - FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6); + // load floorplan + FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6); Floor f1 = fpf.getFloor("1"); Floor f2 = fpf.getFloor("2"); Stairs s1_2 = fpf.getStairs("1_2"); + // build the grid gf.addFloor(f1, 20); gf.addFloor(f2, 340); gf.addStairs(s1_2, 20, 340); gf.removeIsolated(); - + // calculate node importance based on the floorplan (walls, ...) GridImportance gi; - gi.addImportance(g, 20); - gi.addImportance(g, 340); + //gi.addImportance(g, 20); + //gi.addImportance(g, 340); + // calculate dijkstra using aforementioned importance Dijkstra d; const GP& start = g.getNodeFor(GridPoint(500,200,20)); - //const GP& end = g.getNodeFor(GridPoint(1400,1400,20)); const GP& end = g.getNodeFor(GridPoint(1200,200,340)); + //const GP& end = g.getNodeFor(GridPoint(1300,1300,20)); d.build(start, end, tmp); + // add the path's importance to the grid + gi.addImportance(g, d.getNode(start), d.getNode(end)); + // plot path K::GnuplotSplotElementLines path; path.setColorHex("#0000ff"); path.setLineWidth(2); DijkstraNode* dn = d.getNode(end); while (dn->previous != nullptr) { - path.add(K::GnuplotPoint3(dn->element->x_cm, dn->element->y_cm, dn->element->z_cm)); + path.add(K::GnuplotPoint3(dn->element->x_cm, dn->element->y_cm, dn->element->z_cm+50)); dn = dn->previous; } +// // walk +// GridWalkWeighted walk; +// Log::add("test", "walking"); +// for (int i = 0; i < 5000; ++i) { +// walk.getDestination(g, &start, 1.0, Headings::UP); +// } +// Log::add("test", "done"); + Plot p; p.build(g); + //p.showGrid(g); + p.addFloor(f1, 20); + p.addFloor(f2, 340); p.splot.add(&path); p.fire(); diff --git a/tests/grid/TestWalk.cpp b/tests/grid/TestWalk.cpp new file mode 100644 index 0000000..19e7455 --- /dev/null +++ b/tests/grid/TestWalk.cpp @@ -0,0 +1,127 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" + +#include "Plot.h" +#include "../../grid/factory/GridImportance.h" +#include "../../grid/factory/GridFactory.h" +#include "../../floorplan/FloorplanFactorySVG.h" +#include "../../nav/dijkstra/Dijkstra.h" + +#include "../../grid/walk/GridWalkState.h" +#include "../../grid/walk/GridWalkWeighted.h" +#include "../../grid/walk/GridWalkLightAtTheEndOfTheTunnel.h" + +TEST(Walk, plot) { + + Grid<20, GP> g; + GridFactory<20, GP> gf(g); + + bool use3D = true; + + // load floorplan + FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6); + Floor f1 = fpf.getFloor("1"); + Floor f2 = fpf.getFloor("2"); + Stairs s1_2 = fpf.getStairs("1_2"); + + // build the grid + gf.addFloor(f1, 20); + if (use3D) { + gf.addFloor(f2, 340); + gf.addStairs(s1_2, 20, 340); + } + gf.removeIsolated(); + + // calculate node importance based on the floorplan (walls, ...) + GridImportance gi; + gi.addImportance(g, 20); + gi.addImportance(g, 340); + + // dijkstra mapper + class TMP { + Grid<20, GP>& grid; + public: + TMP(Grid<20, GP>& grid) : grid(grid) {;} + int getNumNeighbors(const GP& node) const {return node.getNumNeighbors();} + const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);} + float getWeightBetween(const GP& n1, const GP& n2) const { + float d = ((Point3)n1 - (Point3)n2).length(2.0); + //if (d > 20) {d*= 1.30;} + return d / std::pow(n2.imp, 3); + } + } tmp(g); + + + +// // calculate shortest path +// Dijkstra d; + const GP* start = &g.getNodeFor(GridPoint(700,200,20)); + const GP* end; + if (use3D) { + end = &g.getNodeFor(GridPoint(1200,200,340)); + } else { + end = &g.getNodeFor(GridPoint(1300,1300,20)); + } +// d.build(end, start, tmp); + +// gi.addDistanceToTarget(g, d); + +// // add the path's importance to the grid +// gi.addImportance(g, d.getNode(start), d.getNode(end)); + +// // plot path +// K::GnuplotSplotElementLines path; path.setColorHex("#0000ff"); path.setLineWidth(2); +// DijkstraNode* dn = d.getNode(start); +// while (dn->previous != nullptr) { +// path.add(K::GnuplotPoint3(dn->element->x_cm, dn->element->y_cm, dn->element->z_cm+50)); +// dn = dn->previous; +// } + + + + // walk + GridWalkLightAtTheEndOfTheTunnel walk(g, tmp, *end); + + std::minstd_rand gen; + std::normal_distribution dist(0.3, 0.3); + + K::GnuplotSplotElementPoints pStates; pStates.setColorHex("#880000"); + + // setup starting states + std::vector> states; + for (int i = 0; i < 1000; ++i) { + states.push_back( GridWalkState( start, Headings::UP ) ); + } + + Plot p; + //p.build(g); + p.addFloor(f1, 20); + if (use3D) { + p.addFloor(f2, 340); + } + //p.splot.add(&path); + p.splot.add(&pStates); + p.nodes.setPointSize(0.2); + + if (!use3D) { + p.gp << "set view 0,0\n"; + } + p.gp << "set view equal xy\n"; + + // walk random distances + for (int i = 0; i < 5000; ++i) { + pStates.clear(); + for (GridWalkState& state : states) { + state = walk.getDestination(g, state, std::abs(dist(gen))); + pStates.add(K::GnuplotPoint3(state.node->x_cm, state.node->y_cm, state.node->z_cm+10)); + } + p.gp.draw(p.splot); + p.gp.flush(); + usleep(1000*80); + } + + +} + +#endif diff --git a/tests/misc/TestKNN.cpp b/tests/misc/TestKNN.cpp new file mode 100644 index 0000000..e4a5a28 --- /dev/null +++ b/tests/misc/TestKNN.cpp @@ -0,0 +1,43 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" + +#include +#include "../../misc/KNN.h" +#include "../../misc/KNNArray.h" +#include "../../geo/Point3.h" + +/** test knn-search using the array-wrapper */ +TEST(KNN, array) { + + // construct 3D cross + std::vector points; + + for (int x = -10; x <= 10; ++x) { if (0 == x) {continue;} points.push_back(Point3(x,0,0)); } // idx 0 - 19 + for (int y = -10; y <= 10; ++y) { if (0 == y) {continue;} points.push_back(Point3(0,y,0)); } // idx 20 - 39 + for (int z = -10; z <= 10; ++z) { if (0 == z) {continue;} points.push_back(Point3(0,0,z)); } // idx 40 - 59 + points.push_back(Point3(0,0,0)); // idx 60 + + // construct KNN search + KNNArray> arr(points); + KNN>, 3> knn(arr); + + // test center + ASSERT_EQ(60, knn.getNearestIndex({0,0,0}) ); + + // test X min/max + ASSERT_EQ(0, knn.getNearestIndex({-20,0,0}) ); + ASSERT_EQ(19, knn.getNearestIndex({20,0,0}) ); + + // test Y min/max + ASSERT_EQ(20, knn.getNearestIndex({0,-20,0}) ); + ASSERT_EQ(39, knn.getNearestIndex({0,20,0}) ); + + // test Z min/max + ASSERT_EQ(40, knn.getNearestIndex({0,0,-20}) ); + ASSERT_EQ(59, knn.getNearestIndex({0,0,20}) ); + +} + + +#endif