FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

added several grid-walks

Browse Source 
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...
This commit is contained in:
FrankE
2016-01-24 18:59:06 +01:00
parent cdf97322f8
commit 9947dced15
30 changed files with 1406 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -62,6 +62,8 @@ ADD_DEFINITIONS(
-Wextra
-Wpedantic
-fstack-protector-all
-g3
-O0

10
geo/Angle.h
View File

@@ -4,7 +4,8 @@
#include <cmath>
#include <KLib/Assertions.h>
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;
}

63
geo/Heading.h Normal file
View File

@@ -0,0 +1,63 @@
#ifndef HEADING_H
#define HEADING_H
#include <cmath>
#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

10
geo/Point3.h
View File

@@ -1,6 +1,8 @@
#ifndef POINT3_H
#define POINT3_H
#include <KLib/Assertions.h>
/**
* 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 {

19
grid/Grid.h
View File

@@ -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 <int gridSize_cm, typename T> 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)

8
grid/GridNeighborIterator.h
View File

@@ -8,10 +8,10 @@ class NeighborIter : std::iterator<std::input_iterator_tag, int> {
private:
/** the grid the src-node belongs to */
const Grid<gridSize_cm, T>& grid;
Grid<gridSize_cm, T>* 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<gridSize_cm, T>& grid, const int srcNodeIdx, const int nIdx) :
grid(grid), srcNodeIdx(srcNodeIdx), nIdx(nIdx) {;}
grid((Grid<gridSize_cm, T>*)&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);}
};

9
grid/GridPoint.h
View File

@@ -4,6 +4,8 @@
#include <cmath>
#include "../geo/Point3.h"
#include <KLib/Assertions.h>
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;}
}
};

4
grid/factory/GridFactory.h
View File

@@ -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));

49
grid/factory/GridImportance.h
View File

@@ -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 <KLib/math/distribution/Normal.h>
/**
* 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<float, Grid<gridSize_cm, T>, T, 3> knn(inv);
KNN<Grid<gridSize_cm, T>, 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 <int gridSize_cm, typename T> void addDistanceToTarget(Grid<gridSize_cm, T>& g, Dijkstra<T>& d) {
//Log::add(name, "adding importance information to all nodes at height " + std::to_string(z_cm));
for (T& node : g) {
DijkstraNode<T>* dn = d.getNode(node);
if (dn != nullptr) {
node.distToTarget = dn->cumWeight / 2000;
}
}
}
template <int gridSize_cm, typename T> void addImportance(Grid<gridSize_cm, T>& g, DijkstraNode<T>* start, DijkstraNode<T>* end) {
// routing path
DijkstraPath<T> path(end, start);
// knn search within the path
KNN<DijkstraPath<T>, 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; }
}

17
grid/walk/GridWalkHelper.h Normal file
View File

@@ -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 <typename T> 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

160
grid/walk/GridWalkLightAtTheEndOfTheTunnel.h Normal file
View File

@@ -0,0 +1,160 @@
#ifndef GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H
#define GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H
#include "../../geo/Heading.h"
#include "../Grid.h"
#include "../../math/DrawList.h"
#include <KLib/math/distribution/Normal.h>
#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 <typename T> 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<T&> drawer;
/** fast random-number-generator */
std::minstd_rand gen;
/** 0-mean normal distribution */
std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);
Dijkstra<T> dijkstra;
public:
/** ctor with the target you want to reach */
template <int gridSize_cm, typename Access> GridWalkLightAtTheEndOfTheTunnel(Grid<gridSize_cm, T>& 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<T>* dn = dijkstra.getNode(node);
// should never be null as all nodes were evaluated
if (dn != nullptr) {
node.distToTarget = dn->cumWeight/2000;
}
}
}
template <int gridSize_cm> GridWalkState<T> getDestination(Grid<gridSize_cm, T>& grid, GridWalkState<T> start, float distance_m) {
int retries = 2;
GridWalkState<T> 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<T>(start.node, start.heading.getInverted()), distance_m);
}
// still nothing found? -> keep the start as-is
return (res.node == nullptr) ? (start) : (res);
}
private:
template <int gridSize_cm> GridWalkState<T> walk(Grid<gridSize_cm, T>& grid, GridWalkState<T> 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<T> 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

21
grid/walk/GridWalkState.h Normal file
View File

@@ -0,0 +1,21 @@
#ifndef GRIDWALKSTATE_H
#define GRIDWALKSTATE_H
#include "../../geo/Heading.h"
template <typename T> 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

141
grid/walk/GridWalkWeighted.h Normal file
View File

@@ -0,0 +1,141 @@
#ifndef GRIDWALKWEIGHTED_H
#define GRIDWALKWEIGHTED_H
#include "../../geo/Heading.h"
#include "../Grid.h"
#include "../../math/DrawList.h"
#include <KLib/math/distribution/Normal.h>
/**
* perform walks on the grid based on some sort of weighting
* and drawing from the weighted elements
*/
template <typename T> 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<T&> drawer;
/** fast random-number-generator */
std::minstd_rand gen;
/** 0-mean normal distribution */
std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);
public:
template <int gridSize_cm> State getDestination(Grid<gridSize_cm, T>& 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 <int gridSize_cm> State walk(Grid<gridSize_cm, T>& 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

127
grid/walk/GridWalkWeighted2.h Normal file
View File

@@ -0,0 +1,127 @@
#ifndef GRIDWALKWEIGHTED2_H
#define GRIDWALKWEIGHTED2_H
#include "../../geo/Heading.h"
#include "../Grid.h"
#include "../../math/DrawList.h"
#include <KLib/math/distribution/Normal.h>
/**
* perform walks on the grid based on some sort of weighting
* and drawing from the weighted elements
*/
template <typename T> 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<T&> drawer;
/** fast random-number-generator */
std::minstd_rand gen;
/** 0-mean normal distribution */
std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);
public:
template <int gridSize_cm> State getDestination(Grid<gridSize_cm, T>& 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 <int gridSize_cm> State walk(Grid<gridSize_cm, T>& 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

139
grid/walk/TestWalkWeighted3.h Normal file
View File

@@ -0,0 +1,139 @@
#ifndef GRIDWALKWEIGHTED3_H
#define GRIDWALKWEIGHTED3_H
#include "../../geo/Heading.h"
#include "../Grid.h"
#include "../../math/DrawList.h"
#include <KLib/math/distribution/Normal.h>
/**
* perform walks on the grid based on some sort of weighting
* and drawing from the weighted elements
*/
template <typename T> 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<T&> drawer;
/** fast random-number-generator */
std::minstd_rand gen;
/** 0-mean normal distribution */
std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);
public:
template <int gridSize_cm> State getDestination(Grid<gridSize_cm, T>& 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 <int gridSize_cm> State walk(Grid<gridSize_cm, T>& 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

2
main.cpp
View File

@@ -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

82
math/DrawList.h Normal file
View File

@@ -0,0 +1,82 @@
#ifndef DRAWLIST_H
#define DRAWLIST_H
#include <vector>
#include <random>
/**
* add elements of a certain probability
* and randomly draw from them
*/
template <typename T> 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<Entry> 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

18
misc/Debug.h
View File

@@ -4,23 +4,37 @@
#include <string>
#include <iostream>
#include <iomanip>
#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 << "] ";
}

46
misc/KNN.h
View File

@@ -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<float, Grid<20, T>, T, 3> knn(theGrid);
* float search[] = {0,0,0};
* std::vector<T> elems = knn.get(search, 3);
* Grid<30, T> theGrid;
* KNN<Grid<20, T>, 3, float> knn(theGrid);
* std::vector<T> elems = knn.get({0,0,0}, 10);
*/
template <typename Scalar, typename DataStructure, typename Element, int dim> class KNN {
template <typename DataStructure, int dim, typename Scalar = float> class KNN {
private:
static constexpr const char* name = "KNN";
/** type-definition for the nanoflann KD-Tree used for searching */
typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<Scalar, DataStructure>, 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<Element> get(const Scalar* point, const int numNeighbors, const float maxDistSquared = 99999) const {
template <typename Element> std::vector<Element> 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 <typename Element> std::vector<Element> get(std::initializer_list<Scalar> 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<Scalar> 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<Scalar> 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

59
misc/KNNArray.h Normal file
View File

@@ -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<Point3> points;
* KNNArray<std::vector<Point3>> arr(points);
* KNN<KNNArray<std::vector<Point3>>, 3> knn(arr);
*/
template <typename T> 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 <class BBOX> 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

21
misc/Time.h Normal file
View File

@@ -0,0 +1,21 @@
#ifndef TIME_H
#define TIME_H
#include <chrono>
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<std::chrono::milliseconds>(tick2 - tick1).count();
}
};
#endif // TIME_H

62
nav/dijkstra/Dijkstra.h
View File

@@ -5,9 +5,11 @@
#include <vector>
#include <algorithm>
#include <unordered_set>
#include <list>
#include "DijkstraStructs.h"
#include "../../misc/Debug.h"
#include "../../misc/Time.h"
#include <KLib/Assertions.h>
@@ -19,26 +21,11 @@ template <typename T> class Dijkstra {
/** all already processed edges */
std::unordered_set<DijkstraEdge<T>> usedEdges;
/** to-be-processed nodes (USE LINKED LIST INSTEAD?!) */
/** to-be-processed nodes (NOTE: using std::list here was SLOWER!) */
std::vector<DijkstraNode<T>*> toBeProcessedNodes;
public:
/** get (or create) a new node for the given user-node */
DijkstraNode<T>* getNode(const T* userNode) {
if (nodes.find(userNode) == nodes.end()) {
DijkstraNode<T>* dn = new DijkstraNode<T>(userNode);
nodes[userNode] = dn;
}
return nodes[userNode];
}
/** get the edge (bi-directional) between the two given nodes */
DijkstraEdge<T> getEdge(const DijkstraNode<T>* n1, const DijkstraNode<T>* n2) {
return DijkstraEdge<T>(n1, n2);
}
/** get the dijkstra-pendant for the given user-node */
DijkstraNode<T>* 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<T>* n1, const DijkstraNode<T>* 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<T>* dnSrc = toBeProcessedNodes[0];
const auto min = std::min_element(toBeProcessedNodes.begin(), toBeProcessedNodes.end(), comp);
DijkstraNode<T>* 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<T>* dnDst = getNode(dst);
// get-or-create the edge describing the connection
DijkstraEdge<T> edge = getEdge(dnSrc, dnDst);
const DijkstraEdge<T> 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<T>* n1, const DijkstraNode<T>* 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<T>* getNode(const T* userNode) {
if (nodes.find(userNode) == nodes.end()) {
DijkstraNode<T>* dn = new DijkstraNode<T>(userNode);
nodes[userNode] = dn;
}
return nodes[userNode];
}
/** get the edge (bi-directional) between the two given nodes */
inline DijkstraEdge<T> getEdge(const DijkstraNode<T>* n1, const DijkstraNode<T>* n2) const {
return DijkstraEdge<T>(n1, n2);
}
};

58
nav/dijkstra/DijkstraPath.h Normal file
View File

@@ -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 <typename T> class DijkstraPath {
private:
/** the constructed path */
std::vector<DijkstraNode<T>*> path;
public:
/** ctor from end- to start-node */
DijkstraPath(DijkstraNode<T>* end, DijkstraNode<T>* start) {
// follow the path from the end to the start
DijkstraNode<T>* 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 <class BBOX> 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<T>* 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

27
nav/dijkstra/DijkstraStructs.h
View File

@@ -20,23 +20,20 @@ template <typename T> 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<T>& other) {
return element == other.element;
}
// /** equal? (bi-dir) */
// bool operator == (const DijkstraNode<T>& 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 <typename T> struct DijkstraEdge {
@@ -49,7 +46,7 @@ template <typename T> struct DijkstraEdge {
/** ctor */
DijkstraEdge(const DijkstraNode<T>* src, const DijkstraNode<T>* 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 <typename T> struct DijkstraEdge {
};
//template <typename T> struct DijkstraEdgeWeighted : public DijkstraEdge<T> {
// /** the edge's weight */
// float weight;
// DijkstraEdgeWeighted(const DijkstraNode<T>* src, const DijkstraNode<T>* dst, const float weight) : DijkstraEdge<T>(src,dst), weight(weight) {;}
//};
/** allows adding DijkstraEdge<T> to hash-maps */
namespace std {
template <typename T> struct hash<DijkstraEdge<T>>{
size_t operator()(const DijkstraEdge<T>& e) const {

32
tests/data/fp1.svg Normal file → Executable file
View File

@@ -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">
<inkscape:grid
type="xygrid"
@@ -51,7 +51,7 @@
<dc:format>image/svg+xml</dc:format>
<dc:type
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
<dc:title></dc:title>
<dc:title />
</cc:Work>
</rdf:RDF>
</metadata>
@@ -60,8 +60,7 @@
inkscape:groupmode="layer"
id="layer1"
transform="translate(0,-796.36219)"
style="display:none"
sodipodi:insensitive="true">
style="display:inline">
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 8,1044.3622 0,-240.00001 240,0 0,168 -128,0 0,72.00001 -112,0"
@@ -151,6 +150,11 @@
id="path3389"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cccc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 160,876.36219 0,24"
id="path3381"
inkscape:connector-curvature="0" />
</g>
<g
inkscape:groupmode="layer"
@@ -168,8 +172,7 @@
inkscape:groupmode="layer"
id="layer2"
inkscape:label="2"
style="display:inline"
sodipodi:insensitive="true">
style="display:none">
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 112,104 40,0 0,-24 -40,0"
@@ -265,5 +268,10 @@
d="m 72,136 144,0"
id="path3438"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 120,80 0,24"
id="path3379"
inkscape:connector-curvature="0" />
</g>
</svg>
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 11 KiB

After

Width:  |  Height:  |  Size: 11 KiB

29
tests/geo/TestHeading.cpp Normal file
View File

@@ -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

85
tests/grid/Plot.h
View File

@@ -10,14 +10,16 @@
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#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 <int gridSize_cm, typename T> Plot& build(Grid<gridSize_cm, T>& 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 <int gridSize_cm, typename T> Plot& showGrid(Grid<gridSize_cm, T>& g) {
addEdges(g);
addNodes(g);
return *this;
}
template <int gridSize_cm, typename T> Plot& addEdges(Grid<gridSize_cm, T>& g) {
// prevent adding edges twice
std::set<size_t> 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 <int gridSize_cm, typename T> Plot& addNodes(Grid<gridSize_cm, T>& 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 <int gridSize_cm, typename T> Plot& build(Grid<gridSize_cm, T>& g) {
std::set<uint64_t> 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();

30
tests/grid/TestAll.cpp
View File

@@ -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<GP> 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<GP>* 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<GP> 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();

127
tests/grid/TestWalk.cpp Normal file
View File

@@ -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<GP> 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<GP>* 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<GP> walk(g, tmp, *end);
std::minstd_rand gen;
std::normal_distribution<float> dist(0.3, 0.3);
K::GnuplotSplotElementPoints pStates; pStates.setColorHex("#880000");
// setup starting states
std::vector<GridWalkState<GP>> states;
for (int i = 0; i < 1000; ++i) {
states.push_back( GridWalkState<GP>( 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<GP>& 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

43
tests/misc/TestKNN.cpp Normal file
View File

@@ -0,0 +1,43 @@
#ifdef WITH_TESTS
#include "../Tests.h"
#include <vector>
#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<Point3> 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<std::vector<Point3>> arr(points);
KNN<KNNArray<std::vector<Point3>>, 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