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

added interface for walkers

Browse Source 
some new helper methods
added interpolater for paths
This commit is contained in:
FrankE
2016-01-28 21:48:04 +01:00
parent c4ea811342
commit da0bd43fe0
10 changed files with 92 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
floorplan/Floor.h
View File

@@ -24,6 +24,9 @@ private:
public:
/** empty ctor */
Floor() {;}
/** ctor */
Floor(const float width_cm, const float depth_cm) : width_cm(width_cm), depth_cm(depth_cm) {;}

4
geo/Point3.h
View File

@@ -27,6 +27,10 @@ struct Point3 {
Point3 operator * (const float v) const {return Point3(v*x, v*y, v*z);}
Point3 operator / (const float v) const {return Point3(x/v, y/v, z/v);}
Point3& operator *= (const float v) {x*=v; y*=v; z*=v; return *this;}
Point3& operator /= (const float v) {x/=v; y/=v; z/=v; return *this;}
Point3& operator += (const Point3& o) {x+=o.x; y+=o.y; z+=o.z; return *this;}

15
grid/walk/GridWalk.h Normal file
View File

@@ -0,0 +1,15 @@
#ifndef GRIDWALK_H
#define GRIDWALK_H
#include "GridWalkState.h"
#include "../Grid.h"
template <typename T> class GridWalk {
public:
virtual GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) = 0;
};
#endif // GRIDWALK_H

2
grid/walk/GridWalkHelper.h
View File

@@ -2,6 +2,7 @@
#define GRIDWALKHELPER_H
#include "../../geo/Heading.h"
#include "GridWalkState.h"
class GridWalkHelper {
@@ -41,6 +42,7 @@ public:
template <typename T, typename Walker> static GridWalkState<T> retryOrInvert(Walker& w, const int numRetries, Grid<T>& grid, GridWalkState<T> start, float distance_m) {
Assert::isTrue(distance_m >= 0, "distance must not be negative!");
Assert::isNotNull(start.node, "starting node must not be null");
GridWalkState<T> res;

7
grid/walk/GridWalkLightAtTheEndOfTheTunnel.h
View File

@@ -9,6 +9,7 @@
#include "../../nav/dijkstra/Dijkstra.h"
#include "GridWalk.h"
#include "GridWalkState.h"
#include "GridWalkHelper.h"
@@ -16,7 +17,7 @@
* perform walks on the grid based on some sort of weighting
* and drawing from the weighted elements
*/
template <typename T> class GridWalkLightAtTheEndOfTheTunnel {
template <typename T> class GridWalkLightAtTheEndOfTheTunnel : public GridWalk<T> {
friend class GridWalkHelper;
@@ -62,7 +63,7 @@ public:
}
GridWalkState<T> getDestination(Grid<T>& grid, GridWalkState<T> start, float distance_m) {
GridWalkState<T> getDestination(Grid<T>& grid, GridWalkState<T> start, float distance_m) override {
return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);
@@ -72,7 +73,7 @@ private:
GridWalkState<T> walk(Grid<T>& grid, GridWalkState<T> cur, float distRest_m) {
drawer.reset();;
drawer.reset();
// calculate the weight for all possible destinations from "cur"
for (T& neighbor : grid.neighbors(*cur.node)) {

5
grid/walk/GridWalkPushForward.h
View File

@@ -12,13 +12,14 @@
#include "GridWalkState.h"
#include "GridWalkHelper.h"
#include "GridWalk.h"
/**
* keeps something like an "average position within the last X steps"
* and tries to move away from this point as fast as possible
*
*/
template <typename T> class GridWalkPushForward {
template <typename T> class GridWalkPushForward : public GridWalk<T> {
friend class GridWalkHelper;
@@ -42,7 +43,7 @@ public:
;
}
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) {
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) override {
return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);

9
grid/walk/GridWalkRandomHeadingUpdate.h
View File

@@ -6,6 +6,7 @@
#include "../../nav/dijkstra/Dijkstra.h"
#include "GridWalk.h"
#include "GridWalkState.h"
#include "GridWalkHelper.h"
@@ -22,14 +23,14 @@
* to stay within the room..
*
*/
template <typename T> class GridWalkRandomHeadingUpdate {
template <typename T> class GridWalkRandomHeadingUpdate : public GridWalk<T> {
friend class GridWalkHelper;
private:
/** per-edge: change heading with this sigma */
static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(4);
static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(8);
/** fast random-number-generator */
std::minstd_rand gen;
@@ -41,10 +42,10 @@ public:
/** ctor */
GridWalkRandomHeadingUpdate() {
;
gen.seed(1234);
}
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) {
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) override {
return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);

7
grid/walk/GridWalkRandomHeadingUpdateAdv.h
View File

@@ -9,6 +9,7 @@
#include "../../math/Distributions.h"
#include "../../nav/dijkstra/Dijkstra.h"
#include "GridWalk.h"
#include "GridWalkState.h"
#include "GridWalkHelper.h"
@@ -23,14 +24,14 @@
* - adds additional randomness which should be more stable
*
*/
template <typename T> class GridWalkRandomHeadingUpdateAdv {
template <typename T> class GridWalkRandomHeadingUpdateAdv : public GridWalk<T> {
friend class GridWalkHelper;
private:
/** per-edge: change heading with this sigma */
static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(5);
static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10);
/** fast random-number-generator */
std::minstd_rand gen;
@@ -45,7 +46,7 @@ public:
;
}
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) {
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) override {
return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);

52
math/Interpolator.h Normal file
View File

@@ -0,0 +1,52 @@
#ifndef INTERPOLATOR_H
#define INTERPOLATOR_H
#include "../Assertions.h"
/**
* interpolate between two adjacent values based on their key
*/
template <typename Key, typename Value> class Interpolator {
public:
/** value at key */
struct Entry {
Key key;
Value value;
Entry(const Key key, const Value& pos) : key(key), value(value) {;}
};
protected:
/** all added entries, SORTED by their key */
std::vector<Entry> entries;
public:
/** add a new value with its key. entries must be added in sorted order! */
void add(const Key key, const Value& value) {
Assert::isTrue (entries.empty() || entries.back().key < key, "entries must be ordered!");
entries.push_back(Entry(key, value));
}
/** get the interpolated value for the given key */
Value get(const Key key) const {
const int idx1 = getIdx(key);
const int idx2 = idx1+1;
const float percent = (key - entries[idx1].key) / (float) (entries[idx2].key - entries[idx1].key);
return entries[idx1].value + (entries[idx2].value - entries[idx1].value) * percent;
}
/** get the nearest index for the given key */
int getIdx(const Key key) const {
// TODO: use O(log(n)) search here!
for (size_t i = 0; i < entries.size(); ++i) {
if (entries[i].key > key) {return i-1;}
}
throw "should not happen";
}
};
#endif // INTERPOLATOR_H

54
math/distribution/Exponential.h.autosave
View File

@@ -1,54 +0,0 @@
#ifndef EXPONENTIAL_H
#define EXPONENTIAL_H
#include <cmath>
#include <random>
namespace Distribution {
template <typename T> class Exponential {
private:
T lambda;
std::minstd_rand gen;
std::exponential_distribution<T> dist;
public:
/** ctor */
Exponential(const T lambda) : lambda(lambda), gen(1234), dist(lambda) {
;
}
/** get probability for the given value */
T getProbability(const T val) const {
return lambda * std::exp(-lambda * val);
}
/** get a normally distributed random number */
T draw() {
return dist(gen);
}
/** set the seed to use */
void setSeed(const long seed) {
gen.seed(seed);
}
/** get the probability for the given value */
static double getProbability(const double lambda, const double val) {
return lambda * std::exp(-lambda * val);
}
};
};
}
#endif // EXPONENTIAL_H