From da0bd43fe08fe3f89b3f445b19be423cf900e21f Mon Sep 17 00:00:00 2001 From: FrankE Date: Thu, 28 Jan 2016 21:48:04 +0100 Subject: [PATCH] added interface for walkers some new helper methods added interpolater for paths --- floorplan/Floor.h | 3 ++ geo/Point3.h | 4 ++ grid/walk/GridWalk.h | 15 ++++++ grid/walk/GridWalkHelper.h | 2 + grid/walk/GridWalkLightAtTheEndOfTheTunnel.h | 7 +-- grid/walk/GridWalkPushForward.h | 5 +- grid/walk/GridWalkRandomHeadingUpdate.h | 9 ++-- grid/walk/GridWalkRandomHeadingUpdateAdv.h | 7 +-- math/Interpolator.h | 52 +++++++++++++++++++ math/distribution/Exponential.h.autosave | 54 -------------------- 10 files changed, 92 insertions(+), 66 deletions(-) create mode 100644 grid/walk/GridWalk.h create mode 100644 math/Interpolator.h delete mode 100644 math/distribution/Exponential.h.autosave diff --git a/floorplan/Floor.h b/floorplan/Floor.h index 8074979..4940dee 100755 --- a/floorplan/Floor.h +++ b/floorplan/Floor.h @@ -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) {;} diff --git a/geo/Point3.h b/geo/Point3.h index 2c731d3..06c0a19 100644 --- a/geo/Point3.h +++ b/geo/Point3.h @@ -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;} diff --git a/grid/walk/GridWalk.h b/grid/walk/GridWalk.h new file mode 100644 index 0000000..2385d11 --- /dev/null +++ b/grid/walk/GridWalk.h @@ -0,0 +1,15 @@ +#ifndef GRIDWALK_H +#define GRIDWALK_H + +#include "GridWalkState.h" +#include "../Grid.h" + +template class GridWalk { + +public: + + virtual GridWalkState getDestination(Grid& grid, const GridWalkState start, const float distance_m) = 0; + +}; + +#endif // GRIDWALK_H diff --git a/grid/walk/GridWalkHelper.h b/grid/walk/GridWalkHelper.h index 26825f8..092e630 100644 --- a/grid/walk/GridWalkHelper.h +++ b/grid/walk/GridWalkHelper.h @@ -2,6 +2,7 @@ #define GRIDWALKHELPER_H #include "../../geo/Heading.h" +#include "GridWalkState.h" class GridWalkHelper { @@ -41,6 +42,7 @@ public: template static GridWalkState retryOrInvert(Walker& w, const int numRetries, Grid& grid, GridWalkState 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 res; diff --git a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h index ed07181..9b79aa3 100644 --- a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h +++ b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h @@ -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 class GridWalkLightAtTheEndOfTheTunnel { +template class GridWalkLightAtTheEndOfTheTunnel : public GridWalk { friend class GridWalkHelper; @@ -62,7 +63,7 @@ public: } - GridWalkState getDestination(Grid& grid, GridWalkState start, float distance_m) { + GridWalkState getDestination(Grid& grid, GridWalkState start, float distance_m) override { return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m); @@ -72,7 +73,7 @@ private: GridWalkState walk(Grid& grid, GridWalkState cur, float distRest_m) { - drawer.reset();; + drawer.reset(); // calculate the weight for all possible destinations from "cur" for (T& neighbor : grid.neighbors(*cur.node)) { diff --git a/grid/walk/GridWalkPushForward.h b/grid/walk/GridWalkPushForward.h index 42eab20..1a48af3 100644 --- a/grid/walk/GridWalkPushForward.h +++ b/grid/walk/GridWalkPushForward.h @@ -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 class GridWalkPushForward { +template class GridWalkPushForward : public GridWalk { friend class GridWalkHelper; @@ -42,7 +43,7 @@ public: ; } - GridWalkState getDestination(Grid& grid, const GridWalkState start, const float distance_m) { + GridWalkState getDestination(Grid& grid, const GridWalkState start, const float distance_m) override { return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m); diff --git a/grid/walk/GridWalkRandomHeadingUpdate.h b/grid/walk/GridWalkRandomHeadingUpdate.h index 6c5a3f3..285a347 100644 --- a/grid/walk/GridWalkRandomHeadingUpdate.h +++ b/grid/walk/GridWalkRandomHeadingUpdate.h @@ -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 class GridWalkRandomHeadingUpdate { +template class GridWalkRandomHeadingUpdate : public GridWalk { 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 getDestination(Grid& grid, const GridWalkState start, const float distance_m) { + GridWalkState getDestination(Grid& grid, const GridWalkState start, const float distance_m) override { return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m); diff --git a/grid/walk/GridWalkRandomHeadingUpdateAdv.h b/grid/walk/GridWalkRandomHeadingUpdateAdv.h index d96afa4..c761a29 100644 --- a/grid/walk/GridWalkRandomHeadingUpdateAdv.h +++ b/grid/walk/GridWalkRandomHeadingUpdateAdv.h @@ -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 class GridWalkRandomHeadingUpdateAdv { +template class GridWalkRandomHeadingUpdateAdv : public GridWalk { 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 getDestination(Grid& grid, const GridWalkState start, const float distance_m) { + GridWalkState getDestination(Grid& grid, const GridWalkState start, const float distance_m) override { return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m); diff --git a/math/Interpolator.h b/math/Interpolator.h new file mode 100644 index 0000000..efc3a0a --- /dev/null +++ b/math/Interpolator.h @@ -0,0 +1,52 @@ +#ifndef INTERPOLATOR_H +#define INTERPOLATOR_H + +#include "../Assertions.h" + +/** + * interpolate between two adjacent values based on their key + */ +template 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 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 diff --git a/math/distribution/Exponential.h.autosave b/math/distribution/Exponential.h.autosave deleted file mode 100644 index 534f0e9..0000000 --- a/math/distribution/Exponential.h.autosave +++ /dev/null @@ -1,54 +0,0 @@ -#ifndef EXPONENTIAL_H -#define EXPONENTIAL_H - -#include -#include - -namespace Distribution { - - template class Exponential { - - private: - - T lambda; - - std::minstd_rand gen; - std::exponential_distribution 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