From 0cd038187be1e006e3ed25aeff33d33f3e908292 Mon Sep 17 00:00:00 2001 From: toni Date: Thu, 17 Mar 2016 17:55:43 +0100 Subject: [PATCH 1/9] nothing.. --- nav/dijkstra/Dijkstra.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav/dijkstra/Dijkstra.h b/nav/dijkstra/Dijkstra.h index 0180643..eaff005 100644 --- a/nav/dijkstra/Dijkstra.h +++ b/nav/dijkstra/Dijkstra.h @@ -64,7 +64,7 @@ public: DijkstraNode* dnSrc = toBeProcessedNodes.pop(); // stop when end was reached?? - //if (dnSrc->element == &end) {break;} + //if (dnSrc->element == &end) {break;} // process each neighbor of the current element for (int i = 0; i < acc.getNumNeighbors(*dnSrc->element); ++i) { From e3d245096f380346ecf3bd5c54a1d24712ccae5d Mon Sep 17 00:00:00 2001 From: FrankE Date: Sun, 10 Apr 2016 19:36:51 +0200 Subject: [PATCH 2/9] worked on disjkstra and a* --- grid/Grid.h | 5 ++ nav/a-star/AStar.h | 119 +++++++++++++++++++++++++++++++++ nav/dijkstra/Dijkstra.h | 8 +-- tests/nav/a-star/TestAStar.cpp | 48 +++++++++++++ 4 files changed, 176 insertions(+), 4 deletions(-) create mode 100644 nav/a-star/AStar.h create mode 100644 tests/nav/a-star/TestAStar.cpp diff --git a/grid/Grid.h b/grid/Grid.h index f95d74b..a02cc44 100755 --- a/grid/Grid.h +++ b/grid/Grid.h @@ -152,6 +152,11 @@ public: return (hashes.find(uid) != hashes.end()); } + /** get a list of all nodes within the graph */ + const std::vector& getNodes() const { + return nodes; + } + /** get the center-node the given Point belongs to */ const T& getNodeFor(const GridPoint& p) { const UID uid = getUID(p); diff --git a/nav/a-star/AStar.h b/nav/a-star/AStar.h new file mode 100644 index 0000000..131d18f --- /dev/null +++ b/nav/a-star/AStar.h @@ -0,0 +1,119 @@ +#ifndef ASTAR_H +#define ASTAR_H + +#include +#include +#include +#include +#include +#include +#include + +#include "../../grid/Grid.h" + + +template class AStar { + +public: + + + + //dijkstra with priority queue O(E log V) + template static std::vector get(const T* source, const T* destination, Access acc) { + + // track distances from the source to each other node + std::unordered_map distance; + + // track the previous node for each node along the path + std::unordered_map parent; + + // all nodes + const std::vector& nodes = acc.getAllNodes(); + + // priority queue to check which node is to-be-processed next + std::priority_queue, std::vector>, Comparator2> Q; + + // start with infinite distance + for(const auto& node : nodes){ + distance[&node] = std::numeric_limits::max(); + } + + + // start at the source + distance[source] = 0.0f; + Q.push(std::make_pair(source,distance[source])); + + // proceed until there are now new nodes to follow + while(!Q.empty()) { + + // fetch the next-nearest node from the queue + const T* u = Q.top().first; + + // and check whether we reached the destination + if (u == destination) {break;} + + // remove from the Queue + Q.pop(); + + // process all neighbors for the current element + for( const T& v : acc.getNeighbors(*u)) { + + // weight (distance) between the current node and its neighbor + //const float w = ((Point3)v - (Point3)*u).length(); + const float w = acc.getWeightBetween(v, *u); + + // found a better route? + if (distance[&v] > distance[u] + w) { + distance[&v] = distance[u] + w; + parent[&v] = u; + Q.push(std::make_pair(&v, distance[&v] + acc.getHeuristic(v, *source))); + } + + } + } + + + // construct the path + std::vector path; + const T* p = destination; + path.push_back(destination); + + // until we reached the source-node + while (p!=source) { + if (p) { + p = parent[p]; + path.push_back(p); + } else { + return std::vector(); //if no path could be found, just return an empty vector. + } + } + + // done + return path; + + } + + +// template static std::vector getShortestPathAStar(const T* src, const T* dst, Access acc){ + +// std::vector shortestPath; + +// //here we could do some preprocessing. e.g. area of interest of nodes + + +// // call aStar +// shortestPath = aStar(src, dst, acc); + +// return shortestPath; +// } + + class Comparator2 { + public: + int operator() ( const std::pair& p1, const std::pair& p2){ + return p1.second > p2.second; + } + }; + +}; + +#endif // ASTAR_H diff --git a/nav/dijkstra/Dijkstra.h b/nav/dijkstra/Dijkstra.h index c6ac063..3b1f0f0 100644 --- a/nav/dijkstra/Dijkstra.h +++ b/nav/dijkstra/Dijkstra.h @@ -49,7 +49,7 @@ public: // runs until all nodes were evaluated (void) end; - Log::add("Dijkstra", "calculating dijkstra from " + (std::string)*start + " to ALL OTHER nodes", false); + Log::add("Dijkstra", "calculating dijkstra from " + (std::string)*start + " to " + ((end)?((std::string)*end):"ALL OTHER nodes"), true); Log::tick(); // cleanup previous runs @@ -75,10 +75,10 @@ public: DijkstraNode* dnSrc = toBeProcessedNodes.pop(); // when an end is given, stop when end was reached - if (end != nullptr && dnSrc->element == end) {break;} + if (end != nullptr && dnSrc->element == end) {Log::add("Dijkstra", "reached target node"); break;} // when a maximum weight is given, stop when current cum-dist > maxWeight - if (maxWeight != 0 && dnSrc->cumWeight > maxWeight) {break;} + if (maxWeight != 0 && dnSrc->cumWeight > maxWeight) {Log::add("Dijkstra", "reached distance limit"); break;} // visit (and maybe update) each neighbor of the current element for (int i = 0; i < acc.getNumNeighbors(*dnSrc->element); ++i) { @@ -113,8 +113,8 @@ public: } + Log::add("Dijkstra", "processed " + std::to_string(nodes.size()) + " nodes", false); Log::tock(); - Log::add("Dijkstra", "processed " + std::to_string(nodes.size()) + " nodes"); } diff --git a/tests/nav/a-star/TestAStar.cpp b/tests/nav/a-star/TestAStar.cpp new file mode 100644 index 0000000..4a95d8b --- /dev/null +++ b/tests/nav/a-star/TestAStar.cpp @@ -0,0 +1,48 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" + +#include "../../../grid/Grid.h" +#include "../../../nav/a-star/AStar.h" +#include "../../grid/Plot.h" + +TEST(AStar, build) { + + Grid grid(1); + + int idx1 = grid.add(GP( 0, 0, 0)); + int idx2 = grid.add(GP( 0, 1, 0)); + int idx3 = grid.add(GP( 0,-1, 0)); + int idx4 = grid.add(GP( 1, 0, 0)); + int idx5 = grid.add(GP(-1, 0, 0)); + + grid.connectBiDir(idx1, idx2); + grid.connectBiDir(idx1, idx3); + grid.connectBiDir(idx1, idx4); + grid.connectBiDir(idx1, idx5); + + class TMP { + Grid& grid; + public: + TMP(Grid& 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);} + const std::vector& getAllNodes() const {return grid.getNodes();} + decltype(grid.neighbors(GP())) getNeighbors(const GP& node) const {return grid.neighbors(node);} + float getWeightBetween(const GP& n1, const GP& n2) const {return ((Point3)n1 - (Point3)n2).length();} + float getHeuristic(const GP& n1, const GP& n2) const {return std::abs(n1.x_cm - n2.x_cm) + std::abs(n1.y_cm - n2.y_cm);} + } tmp(grid); + + AStar nav; + std::vector vec = nav.get(&grid[0], &grid[4], tmp); + + for (const GP* g : vec) { + std::cout << g << std::endl; + } + + +} + + + +#endif From e7a5f8eb8cf9d1b9779a4a6ca6e228ff5cdf7ed9 Mon Sep 17 00:00:00 2001 From: FrankE Date: Wed, 13 Apr 2016 11:06:00 +0200 Subject: [PATCH 3/9] added a-star testing version --- nav/a-star/AStar.h | 32 +++++++++++++++++++++----------- 1 file changed, 21 insertions(+), 11 deletions(-) diff --git a/nav/a-star/AStar.h b/nav/a-star/AStar.h index 131d18f..792cad1 100644 --- a/nav/a-star/AStar.h +++ b/nav/a-star/AStar.h @@ -16,16 +16,20 @@ template class AStar { public: - +#define LE_MAX 500000 //dijkstra with priority queue O(E log V) template static std::vector get(const T* source, const T* destination, Access acc) { // track distances from the source to each other node - std::unordered_map distance; + //std::unordered_map distance; + + float distance[LE_MAX]; + // track the previous node for each node along the path - std::unordered_map parent; + //std::unordered_map parent; + const T* parent[LE_MAX]; // all nodes const std::vector& nodes = acc.getAllNodes(); @@ -35,13 +39,14 @@ public: // start with infinite distance for(const auto& node : nodes){ - distance[&node] = std::numeric_limits::max(); + distance[node.getIdx()] = std::numeric_limits::max(); } // start at the source - distance[source] = 0.0f; - Q.push(std::make_pair(source,distance[source])); + distance[source->getIdx()] = 0.0f; + Q.push(std::make_pair(source,distance[source->getIdx()])); + int iter = 0; // proceed until there are now new nodes to follow while(!Q.empty()) { @@ -63,15 +68,20 @@ public: const float w = acc.getWeightBetween(v, *u); // found a better route? - if (distance[&v] > distance[u] + w) { - distance[&v] = distance[u] + w; - parent[&v] = u; - Q.push(std::make_pair(&v, distance[&v] + acc.getHeuristic(v, *source))); + if (distance[v.getIdx()] > distance[u->getIdx()] + w) { + distance[v.getIdx()] = distance[u->getIdx()] + w; + parent[v.getIdx()] = u; + Q.push(std::make_pair(&v, distance[v.getIdx()] + acc.getHeuristic(v, *destination))); // SOURCE OR DEST?! } } + ++iter; } + std::cout << (std::string) *source << std::endl; + std::cout << (std::string) *destination << std::endl; + std::cout << iter << std::endl; + // construct the path std::vector path; @@ -81,7 +91,7 @@ public: // until we reached the source-node while (p!=source) { if (p) { - p = parent[p]; + p = parent[p->getIdx()]; path.push_back(p); } else { return std::vector(); //if no path could be found, just return an empty vector. From c0ab98005a49ce7f2337c7ed7e9c4707e887ea01 Mon Sep 17 00:00:00 2001 From: toni Date: Wed, 13 Apr 2016 11:10:09 +0200 Subject: [PATCH 4/9] added stuff --- geo/Heading.h | 5 +++++ grid/walk/GridWalkPathControl.h | 4 ++-- nav/a-star/AStar.h | 10 +++++++++- 3 files changed, 16 insertions(+), 3 deletions(-) diff --git a/geo/Heading.h b/geo/Heading.h index 73acb43..172e124 100644 --- a/geo/Heading.h +++ b/geo/Heading.h @@ -46,6 +46,11 @@ public: return (Heading(*this) += _rad); } + Heading& operator = (const float _rad) { + rad = _rad; + return *this; + } + /** compare two headings */ bool operator == (const Heading other) const {return rad == other.rad;} bool operator != (const Heading other) const {return rad != other.rad;} diff --git a/grid/walk/GridWalkPathControl.h b/grid/walk/GridWalkPathControl.h index bf5bb21..4980780 100644 --- a/grid/walk/GridWalkPathControl.h +++ b/grid/walk/GridWalkPathControl.h @@ -64,7 +64,7 @@ public: GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad) { // proportional change of the heading - static Distribution::Normal dHead(1, 0.01); + static Distribution::Normal dHead(1, 0.01); // proportional change of the to-be-walked distance static Distribution::Normal dWalk(1, 0.10); @@ -97,7 +97,7 @@ private: const float diff = possibleHead.getDiffHalfRAD(head); // // compare this heading with the requested one - const double angleProb = Distribution::Normal::getProbability(0, Angle::degToRad(25), diff); + const double angleProb = Distribution::Normal::getProbability(0, Angle::degToRad(25), diff); // const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1; // favor best 3 angles equally // nodes own importance diff --git a/nav/a-star/AStar.h b/nav/a-star/AStar.h index 131d18f..486baf3 100644 --- a/nav/a-star/AStar.h +++ b/nav/a-star/AStar.h @@ -43,9 +43,15 @@ public: distance[source] = 0.0f; Q.push(std::make_pair(source,distance[source])); + int iter = 0; + //std::cout << (std::string)*source << std::endl; + //std::cout << (std::string)*destination << std::endl; + // proceed until there are now new nodes to follow while(!Q.empty()) { + ++iter; + // fetch the next-nearest node from the queue const T* u = Q.top().first; @@ -66,12 +72,14 @@ public: if (distance[&v] > distance[u] + w) { distance[&v] = distance[u] + w; parent[&v] = u; - Q.push(std::make_pair(&v, distance[&v] + acc.getHeuristic(v, *source))); + Q.push(std::make_pair(&v, distance[&v] + acc.getHeuristic(v, *destination))); } } } + //std::cout << iter << std::endl; + // construct the path std::vector path; From f77a28735b6a7c4502bea9d69903500b369b7f6f Mon Sep 17 00:00:00 2001 From: FrankE Date: Thu, 21 Apr 2016 08:59:05 +0200 Subject: [PATCH 5/9] added von mises distributionb quick&dirty: added activity to the grid-walkers --- grid/walk/GridWalk.h | 11 +++- grid/walk/GridWalkLightAtTheEndOfTheTunnel.h | 2 +- grid/walk/GridWalkPathControl.h | 16 ++++-- grid/walk/GridWalkRandomHeadingUpdate.h | 2 +- grid/walk/GridWalkShortestPathControl.h | 2 +- grid/walk/GridWalkSimpleControl.h | 2 +- math/Distributions.h | 1 + math/Math.h | 20 +++++++ math/distribution/Bessel.h | 57 +++++++++++++++++++ math/distribution/LUT.h | 58 ++++++++++++++++++++ math/distribution/VonMises.h | 56 +++++++++++++++++++ 11 files changed, 216 insertions(+), 11 deletions(-) create mode 100644 math/Math.h create mode 100644 math/distribution/Bessel.h create mode 100644 math/distribution/LUT.h create mode 100644 math/distribution/VonMises.h diff --git a/grid/walk/GridWalk.h b/grid/walk/GridWalk.h index fc5336f..31c0ea9 100644 --- a/grid/walk/GridWalk.h +++ b/grid/walk/GridWalk.h @@ -4,11 +4,20 @@ #include "GridWalkState.h" #include "../Grid.h" +/** all supported acitivites lukas can detect */ +enum class Activity { + UNKNOWN, + STANDING, + WALKING, + STAIRS, + ELEVATOR, +}; + template class GridWalk { public: - virtual GridWalkState getDestination(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad) = 0; + virtual GridWalkState getDestination(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad, Activity act) = 0; }; diff --git a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h index eccff20..1dc8b58 100644 --- a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h +++ b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h @@ -65,7 +65,7 @@ public: } - GridWalkState getDestination(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad) override { + GridWalkState getDestination(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad, Activity act) override { return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m, headChange_rad); diff --git a/grid/walk/GridWalkPathControl.h b/grid/walk/GridWalkPathControl.h index 4980780..87926ee 100644 --- a/grid/walk/GridWalkPathControl.h +++ b/grid/walk/GridWalkPathControl.h @@ -61,7 +61,7 @@ public: } - GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad) { + GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad, Activity act) { // proportional change of the heading static Distribution::Normal dHead(1, 0.01); @@ -75,13 +75,13 @@ public: static Distribution::Normal sWalk(0, 0.15); if (distance_m == 0) { distance_m = std::abs( sWalk.draw() ); } - return walk(grid, start, distance_m, headChange_rad); + return walk(grid, start, distance_m, headChange_rad, act); } private: - double getProbability(const T& start, const T& prev, const T& possible, const Heading head) const { + double getProbability(const T& start, const T& prev, const T& possible, const Heading head, Activity act) const { // TODO: WHY?! not only when going back to the start? if (start.x_cm == possible.x_cm && start.y_cm == possible.y_cm) { @@ -104,12 +104,16 @@ private: //const double nodeProb = (possible.distToTarget < start.distToTarget) ? 1 : 0.025; // from start const double nodeProb = (possible.distToTarget < prev.distToTarget) ? 1 : pOther; // from previous node + double actProb = 1.0; + if (act == Activity::STAIRS) {actProb = (prev.z_cm != possible.z_cm) ? (0.8) : (0.2);} + if (act == Activity::WALKING) {actProb = (prev.z_cm == possible.z_cm) ? (0.8) : (0.2);} + // bring it together - return angleProb * nodeProb; + return angleProb * nodeProb * actProb; } - GridWalkState walk(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad) { + GridWalkState walk(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad, Activity act) { // try-again distribution //static Distribution::Normal dHead(0, Angle::degToRad(10)); @@ -130,7 +134,7 @@ private: drawer.reset(); for (T& neighbor : grid.neighbors(*cur.node)) { - const double prob = getProbability(*start.node, *cur.node, neighbor, reqHeading); + const double prob = getProbability(*start.node, *cur.node, neighbor, reqHeading, act); drawer.add(neighbor, prob); } diff --git a/grid/walk/GridWalkRandomHeadingUpdate.h b/grid/walk/GridWalkRandomHeadingUpdate.h index 523c418..ce5209e 100644 --- a/grid/walk/GridWalkRandomHeadingUpdate.h +++ b/grid/walk/GridWalkRandomHeadingUpdate.h @@ -47,7 +47,7 @@ public: - GridWalkState getDestination(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad) override { + GridWalkState getDestination(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad, Activity act) override { return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m, -1); diff --git a/grid/walk/GridWalkShortestPathControl.h b/grid/walk/GridWalkShortestPathControl.h index 32512a3..d69c6a1 100644 --- a/grid/walk/GridWalkShortestPathControl.h +++ b/grid/walk/GridWalkShortestPathControl.h @@ -117,7 +117,7 @@ public: int times = 3; float pOther = 0.10; - GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad) { + GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad, Activity act) { // update the center-of-mass diff --git a/grid/walk/GridWalkSimpleControl.h b/grid/walk/GridWalkSimpleControl.h index 7ea6cef..d5911ab 100644 --- a/grid/walk/GridWalkSimpleControl.h +++ b/grid/walk/GridWalkSimpleControl.h @@ -41,7 +41,7 @@ public: - GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad) { + GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad, Activity act) { // proportional change of the heading static Distribution::Normal dHead(1, 0.01); diff --git a/math/Distributions.h b/math/Distributions.h index 0559b24..f4cec9f 100644 --- a/math/Distributions.h +++ b/math/Distributions.h @@ -5,5 +5,6 @@ #include "distribution/Exponential.h" #include "distribution/Logistic.h" #include "distribution/Uniform.h" +#include "distribution/VonMises.h" #endif // DISTRIBUTIONS_H diff --git a/math/Math.h b/math/Math.h new file mode 100644 index 0000000..4d12949 --- /dev/null +++ b/math/Math.h @@ -0,0 +1,20 @@ +#ifndef K_MATH_MATH_H +#define K_MATH_MATH_H + +#include "../Defines.h" + +class Math { + +public: + + /** ensure val stays within [min:max] */ + template static inline Scalar clamp(const Scalar min, const Scalar max, const Scalar val) { + if (unlikely(val < min)) {return min;} + if (unlikely(val > max)) {return max;} + return val; + } + +}; + + +#endif // K_MATH_MATH_H diff --git a/math/distribution/Bessel.h b/math/distribution/Bessel.h new file mode 100644 index 0000000..d3f60b0 --- /dev/null +++ b/math/distribution/Bessel.h @@ -0,0 +1,57 @@ +#ifndef BESSEL_H +#define BESSEL_H + +namespace Distribution { + + /** helper class */ + template class Bessel { + + public: + + /** + * calculation for I_v(z) + * https://en.wikipedia.org/wiki/Bessel_function + * http://www.boost.org/doc/libs/1_35_0/libs/math/doc/sf_and_dist/html/math_toolkit/special/bessel/mbessel.html + */ + T getModified(const int v, const T z) const { + + // running factorials (updated within for-loops) + uint64_t runFac1 = 1; + uint64_t runFac2 = factorial(v); // Delta(k+v+1) = (k+v+1-1)! = (k+v)! [k starts at 0] -> (v)! + + // running potential + T runPot = 1; + const T pot = T(0.25) * z * z; + + // start for-loop at k=1 instead of k=0. allows for running factorial without using if (k==0) + T sum = 0; + for (int k = 0; k < 16; ) { + sum += runPot / runFac1 / runFac2; + ++k; + runFac1 *= k; + runFac2 *= k; + runPot *= pot; + } + + // done + return std::pow( (T(0.5) * z), v) * sum; + + } + + private: + + + + + + static uint64_t factorial(const int n) { + uint64_t res = 1; + for (int i = 2; i <= n; ++i) {res *= i;} + return res; + } + + }; + +} + +#endif // BESSEL_H diff --git a/math/distribution/LUT.h b/math/distribution/LUT.h new file mode 100644 index 0000000..ef46015 --- /dev/null +++ b/math/distribution/LUT.h @@ -0,0 +1,58 @@ +#ifndef LUT_H +#define LUT_H + +#include +#include "../Math.h" + +namespace Distribution { + + template class LUT { + + private: + + Scalar min; + Scalar max; + Scalar diff; + + /** number of samples between min and max */ + int numSamples; + + /** the look-up-table */ + std::vector samples; + + + public: + + /** ctor */ + template LUT(const Distribution dist, const Scalar min, const Scalar max, const int numSamples) : + min(min), max(max), diff(max-min), numSamples(numSamples) { + + buildLUT(dist); + + } + + /** get the probability of val from the LUT */ + Scalar getProbability(const Scalar val) const { + int idx = ((val - min) * numSamples / diff); + idx = Math::clamp(0, numSamples-1, idx); + return samples[idx]; + } + + private: + + /** build the look-up-table */ + template void buildLUT(const Distribution dist) { + + samples.resize(numSamples); + for (int idx = 0; idx < numSamples; ++idx) { + const Scalar val = min + (idx * diff / numSamples); + samples[idx] = dist.getProbability(val); + } + + } + + }; + +} + +#endif // LUT_H diff --git a/math/distribution/VonMises.h b/math/distribution/VonMises.h new file mode 100644 index 0000000..789e0c8 --- /dev/null +++ b/math/distribution/VonMises.h @@ -0,0 +1,56 @@ +#ifndef K_MATH_DISTRIBUTION_VONMISES_H +#define K_MATH_DISTRIBUTION_VONMISES_H + +#include +#include + +#include "../Math.h" +#include "Bessel.h" +#include "LUT.h" + +namespace Distribution { + + /** von-mises distribution */ + template class VonMises { + + private: + + /** center of the distribution */ + const T mu; + + /** like 1.0/variance of the distribution */ + const T kappa; + + /** pre-calcuated look-up-table */ + std::vector lut; + + /** helper for modified bessel I_0(kappa) */ + Bessel bessel; + + public: + + /** ctor */ + VonMises(const T mu, const T kappa) : mu(mu), kappa(kappa) { + + } + + LUT getLUT() const { + return LUT(*this, -M_PI, +M_PI, 10000); + } + + /** get probability for the given value */ + T getProbability(const T _val) const { + + const T val = Math::clamp((T)-M_PI, (T)+M_PI, _val); + + return + std::exp(kappa * std::cos(val - mu)) / + (2 * M_PI * bessel.getModified(0, kappa)); + + } + + }; + +} + +#endif // K_MATH_DISTRIBUTION_VONMISES_H From c95989fb83cdbb78f09e14a577abf0b50e2f18c7 Mon Sep 17 00:00:00 2001 From: FrankE Date: Thu, 21 Apr 2016 10:15:42 +0200 Subject: [PATCH 6/9] added missing stuff --- misc/Time.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/misc/Time.h b/misc/Time.h index d6fa4b3..8a3fa33 100644 --- a/misc/Time.h +++ b/misc/Time.h @@ -12,7 +12,7 @@ public: } - static int diffMS(std::chrono::system_clock::time_point tick1, std::chrono::system_clock::time_point tick2) { + static uint32_t diffMS(std::chrono::system_clock::time_point tick1, std::chrono::system_clock::time_point tick2) { return std::chrono::duration_cast(tick2 - tick1).count(); } From 6d3d2eeb551934af09aece94106d325a36f72e79 Mon Sep 17 00:00:00 2001 From: FrankE Date: Fri, 22 Apr 2016 12:24:22 +0200 Subject: [PATCH 7/9] new activities --- grid/walk/GridWalk.h | 3 ++- grid/walk/GridWalkPathControl.h | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/grid/walk/GridWalk.h b/grid/walk/GridWalk.h index 31c0ea9..cbef10a 100644 --- a/grid/walk/GridWalk.h +++ b/grid/walk/GridWalk.h @@ -9,7 +9,8 @@ enum class Activity { UNKNOWN, STANDING, WALKING, - STAIRS, + STAIRS_UP, + STAIRS_DOWN, ELEVATOR, }; diff --git a/grid/walk/GridWalkPathControl.h b/grid/walk/GridWalkPathControl.h index 87926ee..afae95c 100644 --- a/grid/walk/GridWalkPathControl.h +++ b/grid/walk/GridWalkPathControl.h @@ -105,7 +105,8 @@ private: const double nodeProb = (possible.distToTarget < prev.distToTarget) ? 1 : pOther; // from previous node double actProb = 1.0; - if (act == Activity::STAIRS) {actProb = (prev.z_cm != possible.z_cm) ? (0.8) : (0.2);} + if (act == Activity::STAIRS_UP) {actProb = (prev.z_cm < possible.z_cm) ? (0.8) : (0.2);} + if (act == Activity::STAIRS_DOWN) {actProb = (prev.z_cm > possible.z_cm) ? (0.8) : (0.2);} if (act == Activity::WALKING) {actProb = (prev.z_cm == possible.z_cm) ? (0.8) : (0.2);} // bring it together From 8f6bfa917fe0c27eb94fc2813bf26bb7a8d9b49f Mon Sep 17 00:00:00 2001 From: FrankE Date: Tue, 26 Apr 2016 11:49:12 +0200 Subject: [PATCH 8/9] removed heading::rnd (not seedable) fixed grid-walker issues when conducting multiple runs new helper methods for the interpolatorr --- geo/Heading.h | 14 +++++++------- grid/walk/GridWalkPathControl.h | 14 ++++++++++---- grid/walk/GridWalkSimpleControl.h | 14 +++++++++----- math/Interpolator.h | 6 ++++++ 4 files changed, 32 insertions(+), 16 deletions(-) diff --git a/geo/Heading.h b/geo/Heading.h index 172e124..ca2c293 100644 --- a/geo/Heading.h +++ b/geo/Heading.h @@ -64,13 +64,13 @@ public: float getRAD() const {return rad;} - /** get a random heading */ - static Heading rnd() { - static std::minstd_rand gen(1234); - static std::uniform_real_distribution dist(0, _2PI); - const float rnd = dist(gen); - return Heading(rnd); - } +// /** get a random heading */ +// static Heading rnd() { +// static std::minstd_rand gen(1234); +// static std::uniform_real_distribution dist(0, _2PI); +// const float rnd = dist(gen); +// return Heading(rnd); +// } #undef _2PI diff --git a/grid/walk/GridWalkPathControl.h b/grid/walk/GridWalkPathControl.h index afae95c..7701af6 100644 --- a/grid/walk/GridWalkPathControl.h +++ b/grid/walk/GridWalkPathControl.h @@ -61,18 +61,22 @@ public: } + Distribution::Normal dHead = Distribution::Normal(1, 0.01); + Distribution::Normal dWalk = Distribution::Normal(1, 0.10); + Distribution::Normal sWalk = Distribution::Normal(0, 0.15); + GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad, Activity act) { // proportional change of the heading - static Distribution::Normal dHead(1, 0.01); + //static Distribution::Normal dHead(1, 0.01); // proportional change of the to-be-walked distance - static Distribution::Normal dWalk(1, 0.10); + //static Distribution::Normal dWalk(1, 0.10); distance_m = distance_m*dWalk.draw()*1.4; // TODO: why *2? headChange_rad = headChange_rad*dHead.draw(); - static Distribution::Normal sWalk(0, 0.15); + //static Distribution::Normal sWalk(0, 0.15); if (distance_m == 0) { distance_m = std::abs( sWalk.draw() ); } return walk(grid, start, distance_m, headChange_rad, act); @@ -114,12 +118,14 @@ private: } + Distribution::Uniform dChange = Distribution::Uniform(Angle::degToRad(0), +Angle::degToRad(359)); + GridWalkState walk(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad, Activity act) { // try-again distribution //static Distribution::Normal dHead(0, Angle::degToRad(10)); //static Distribution::Normal dUpdate(0, Angle::degToRad(3)); - static Distribution::Uniform dChange(Angle::degToRad(0), +Angle::degToRad(359)); + // static Distribution::Uniform dChange(Angle::degToRad(0), +Angle::degToRad(359)); int retries = 5; float walked_m = 0; diff --git a/grid/walk/GridWalkSimpleControl.h b/grid/walk/GridWalkSimpleControl.h index d5911ab..f736561 100644 --- a/grid/walk/GridWalkSimpleControl.h +++ b/grid/walk/GridWalkSimpleControl.h @@ -39,20 +39,22 @@ public: gen.seed(1234); } - + Distribution::Normal dHead = Distribution::Normal(1, 0.01); + Distribution::Normal dWalk = Distribution::Normal(1, 0.10); + Distribution::Normal sWalk = Distribution::Normal(0, 0.10); GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad, Activity act) { // proportional change of the heading - static Distribution::Normal dHead(1, 0.01); + // proportional change of the to-be-walked distance - static Distribution::Normal dWalk(1, 0.10); + //static Distribution::Normal dWalk(1, 0.10); distance_m = distance_m*dWalk.draw()*1.4; // TODO: why * X? headChange_rad = headChange_rad*dHead.draw(); - static Distribution::Normal sWalk(0, 0.10); + //static Distribution::Normal sWalk(0, 0.10); if (distance_m == 0) { distance_m = std::abs( sWalk.draw() ); } return walk(grid, start, distance_m, headChange_rad); @@ -90,12 +92,14 @@ private: } + Distribution::Uniform dChange = Distribution::Uniform(Angle::degToRad(0), +Angle::degToRad(359)); + GridWalkState walk(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad) { // try-again distribution //static Distribution::Normal dHead(0, Angle::degToRad(10)); //static Distribution::Normal dUpdate(0, Angle::degToRad(3)); - static Distribution::Uniform dChange(Angle::degToRad(0), +Angle::degToRad(359)); + // static Distribution::Uniform dChange(Angle::degToRad(0), +Angle::degToRad(359)); int retries = 5; float walked_m = 0; diff --git a/math/Interpolator.h b/math/Interpolator.h index 765a196..55ef39f 100644 --- a/math/Interpolator.h +++ b/math/Interpolator.h @@ -34,6 +34,12 @@ public: entries.push_back(InterpolatorEntry(key, value)); } + /** get the smallest added key */ + Key getMinKey() const {return entries.front().key;} + + /** get the largest added key */ + Key getMaxKey() const {return entries.back().key;} + /** get the interpolated value for the given key */ Value get(const Key key) const { From 62d8d6b36b1602262327d682d2fb31f861c6e97c Mon Sep 17 00:00:00 2001 From: FrankE Date: Tue, 26 Apr 2016 15:15:28 +0200 Subject: [PATCH 9/9] refactored random subsystem added compile-time seed support --- grid/walk/GridWalkLightAtTheEndOfTheTunnel.h | 3 +- grid/walk/GridWalkPathControl.h | 2 +- grid/walk/GridWalkPushForward.h | 3 +- grid/walk/GridWalkRandomHeadingUpdate.h | 4 +- grid/walk/GridWalkRandomHeadingUpdateAdv.h | 3 +- grid/walk/GridWalkShortestPathControl.h | 5 +- grid/walk/GridWalkSimpleControl.h | 3 +- grid/walk/GridWalkWeighted.h | 3 +- grid/walk/GridWalkWeighted2.h | 6 +- grid/walk/TestWalkWeighted3.h | 6 +- math/DrawList.h | 4 +- math/Random.h | 28 +++++++ math/distribution/Exponential.h | 6 +- math/distribution/Normal.h | 5 +- math/distribution/Uniform.h | 14 +++- tests/geo/TestHeading.cpp | 12 +-- tests/math/TestDistribution.cpp | 85 ++++++++++++++++++++ 17 files changed, 163 insertions(+), 29 deletions(-) create mode 100644 math/Random.h diff --git a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h index 1dc8b58..ae59caa 100644 --- a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h +++ b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h @@ -6,6 +6,7 @@ #include "../../math/DrawList.h" #include "../../math/Distributions.h" +#include "../../math/DrawList.h" #include "../../nav/dijkstra/Dijkstra.h" @@ -34,7 +35,7 @@ private: DrawList drawer; /** fast random-number-generator */ - std::minstd_rand gen; + RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkPathControl.h b/grid/walk/GridWalkPathControl.h index 7701af6..de7d3a4 100644 --- a/grid/walk/GridWalkPathControl.h +++ b/grid/walk/GridWalkPathControl.h @@ -23,7 +23,7 @@ private: static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10); /** fast random-number-generator */ - std::minstd_rand gen; + RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkPushForward.h b/grid/walk/GridWalkPushForward.h index 3d62bf4..7d01da3 100644 --- a/grid/walk/GridWalkPushForward.h +++ b/grid/walk/GridWalkPushForward.h @@ -8,6 +8,7 @@ #include "../Grid.h" #include "../../math/DrawList.h" +#include "../../math/Random.h" #include "../../nav/dijkstra/Dijkstra.h" #include "GridWalkState.h" @@ -31,7 +32,7 @@ private: static constexpr float HEADING_ALLOWED_SIGMA = Angle::degToRad(20); /** fast random-number-generator */ - std::minstd_rand gen; + RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkRandomHeadingUpdate.h b/grid/walk/GridWalkRandomHeadingUpdate.h index ce5209e..2d5b05e 100644 --- a/grid/walk/GridWalkRandomHeadingUpdate.h +++ b/grid/walk/GridWalkRandomHeadingUpdate.h @@ -4,6 +4,8 @@ #include "../../geo/Heading.h" #include "../Grid.h" + +#include "../../math/Random.h" #include "../../nav/dijkstra/Dijkstra.h" #include "GridWalk.h" @@ -33,7 +35,7 @@ private: static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10); /** fast random-number-generator */ - std::minstd_rand gen; + RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkRandomHeadingUpdateAdv.h b/grid/walk/GridWalkRandomHeadingUpdateAdv.h index 32e07f5..a387755 100644 --- a/grid/walk/GridWalkRandomHeadingUpdateAdv.h +++ b/grid/walk/GridWalkRandomHeadingUpdateAdv.h @@ -8,6 +8,7 @@ #include "../../math/DrawList.h" #include "../../math/Distributions.h" #include "../../nav/dijkstra/Dijkstra.h" +#include "../../math/Random.h" #include "GridWalk.h" #include "GridWalkState.h" @@ -34,7 +35,7 @@ private: static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10); /** fast random-number-generator */ - std::minstd_rand gen; + RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkShortestPathControl.h b/grid/walk/GridWalkShortestPathControl.h index d69c6a1..9254fb9 100644 --- a/grid/walk/GridWalkShortestPathControl.h +++ b/grid/walk/GridWalkShortestPathControl.h @@ -7,6 +7,7 @@ #include "../../math/Distributions.h" #include "../../math/DrawList.h" +#include "../../math/Random.h" #include "../../nav/dijkstra/Dijkstra.h" #include "../../nav/dijkstra/DijkstraPath.h" @@ -17,6 +18,8 @@ #include "GridWalkHelper.h" #include "GridWalk.h" + + #include template class GridWalkShortestPathControl : public GridWalk { @@ -76,7 +79,7 @@ protected: static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10); /** fast random-number-generator */ - std::minstd_rand gen; + RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkSimpleControl.h b/grid/walk/GridWalkSimpleControl.h index f736561..cc3ad88 100644 --- a/grid/walk/GridWalkSimpleControl.h +++ b/grid/walk/GridWalkSimpleControl.h @@ -8,6 +8,7 @@ #include "../../math/Distributions.h" #include "../../math/DrawList.h" +#include "../../math/Random.h" #include "GridWalkState.h" #include "GridWalkHelper.h" @@ -23,7 +24,7 @@ private: static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10); /** fast random-number-generator */ - std::minstd_rand gen; + RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkWeighted.h b/grid/walk/GridWalkWeighted.h index 252130a..cd97f5a 100644 --- a/grid/walk/GridWalkWeighted.h +++ b/grid/walk/GridWalkWeighted.h @@ -6,6 +6,7 @@ #include "../../math/DrawList.h" #include "../../math/Distributions.h" +#include "../../math/DrawList.h" /** * perform walks on the grid based on some sort of weighting @@ -38,7 +39,7 @@ private: DrawList drawer; /** fast random-number-generator */ - std::minstd_rand gen; + RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkWeighted2.h b/grid/walk/GridWalkWeighted2.h index 04cb09f..26a8456 100644 --- a/grid/walk/GridWalkWeighted2.h +++ b/grid/walk/GridWalkWeighted2.h @@ -5,7 +5,9 @@ #include "../Grid.h" #include "../../math/DrawList.h" -#include +#include "../../math/Random.h" + +//#include /** * perform walks on the grid based on some sort of weighting @@ -38,7 +40,7 @@ private: DrawList drawer; /** fast random-number-generator */ - std::minstd_rand gen; + RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/TestWalkWeighted3.h b/grid/walk/TestWalkWeighted3.h index 84f98dc..c8eec62 100644 --- a/grid/walk/TestWalkWeighted3.h +++ b/grid/walk/TestWalkWeighted3.h @@ -5,7 +5,9 @@ #include "../Grid.h" #include "../../math/DrawList.h" -#include +#include "../../math/Random.h" + +//#include /** * perform walks on the grid based on some sort of weighting @@ -38,7 +40,7 @@ private: DrawList drawer; /** fast random-number-generator */ - std::minstd_rand gen; + RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/math/DrawList.h b/math/DrawList.h index a9d3b36..6107b04 100644 --- a/math/DrawList.h +++ b/math/DrawList.h @@ -2,8 +2,8 @@ #define DRAWLIST_H #include -#include +#include "Random.h" #include "../Assertions.h" /** @@ -38,7 +38,7 @@ private: std::vector elements; /** random number generator */ - std::minstd_rand gen; + RandomGenerator gen; public: diff --git a/math/Random.h b/math/Random.h new file mode 100644 index 0000000..6c1393d --- /dev/null +++ b/math/Random.h @@ -0,0 +1,28 @@ +#ifndef RANDOM_H +#define RANDOM_H + +#include +#include +#include "../misc/Time.h" + +#ifdef USE_FIXED_SEED + #define RANDOM_SEED 1234 +#else + #define RANDOM_SEED ( std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1) ) +#endif + +//using RandomGenerator = std::minstd_rand; + +class RandomGenerator : public std::minstd_rand { + +public: + + /** ctor with default seed */ + RandomGenerator() : std::minstd_rand(RANDOM_SEED) {;} + + /** ctor with custom seed */ + RandomGenerator(result_type) : std::minstd_rand(RANDOM_SEED) {;} + +}; + +#endif // RANDOM_H diff --git a/math/distribution/Exponential.h b/math/distribution/Exponential.h index f00f410..db5dabe 100644 --- a/math/distribution/Exponential.h +++ b/math/distribution/Exponential.h @@ -3,7 +3,7 @@ #include #include - +#include "../Random.h" namespace Distribution { @@ -14,13 +14,13 @@ namespace Distribution { const T lambda; - std::minstd_rand gen; + RandomGenerator gen; std::exponential_distribution dist; public: /** ctor */ - Exponential(const T lambda) : lambda(lambda), dist(lambda) { + Exponential(const T lambda) : lambda(lambda), gen(RANDOM_SEED), dist(lambda) { ; } diff --git a/math/distribution/Normal.h b/math/distribution/Normal.h index 4f12a41..893d456 100644 --- a/math/distribution/Normal.h +++ b/math/distribution/Normal.h @@ -3,6 +3,7 @@ #include #include +#include "../Random.h" namespace Distribution { @@ -15,14 +16,14 @@ namespace Distribution { const T sigma; const T _a; - std::minstd_rand gen; + RandomGenerator gen; std::normal_distribution dist; public: /** ctor */ Normal(const T mu, const T sigma) : - mu(mu), sigma(sigma), _a(1.0 / (sigma * std::sqrt(2.0 * M_PI))), gen(1234), dist(mu,sigma) { + mu(mu), sigma(sigma), _a(1.0 / (sigma * std::sqrt(2.0 * M_PI))), gen(RANDOM_SEED), dist(mu,sigma) { } diff --git a/math/distribution/Uniform.h b/math/distribution/Uniform.h index 550aabd..d6011fe 100644 --- a/math/distribution/Uniform.h +++ b/math/distribution/Uniform.h @@ -3,6 +3,9 @@ #include #include +#include "../Random.h" + +#include namespace Distribution { @@ -11,14 +14,17 @@ namespace Distribution { private: - std::minstd_rand gen; - std::uniform_real_distribution dist; + RandomGenerator gen; + + /** depending on T, Dist is either a uniform_real or uniform_int distribution */ + typedef typename std::conditional< std::is_floating_point::value, std::uniform_real_distribution, std::uniform_int_distribution >::type Dist; + Dist dist; + public: /** ctor */ - Uniform(const T min, const T max) : - gen(1234), dist(min, max) { + Uniform(const T min, const T max) : gen(RANDOM_SEED), dist(min, max) { } /** get a uniformaly distributed random number */ diff --git a/tests/geo/TestHeading.cpp b/tests/geo/TestHeading.cpp index 679f7de..44ab787 100644 --- a/tests/geo/TestHeading.cpp +++ b/tests/geo/TestHeading.cpp @@ -52,13 +52,13 @@ TEST(Heading, eq) { } -TEST(Heading, random) { +//TEST(Heading, random) { - // two random values must not be equal - ASSERT_NE(Heading::rnd(), Heading::rnd()); - ASSERT_NE(Heading::rnd(), Heading::rnd()); - ASSERT_NE(Heading::rnd(), Heading::rnd()); +// // two random values must not be equal +// ASSERT_NE(Heading::rnd(), Heading::rnd()); +// ASSERT_NE(Heading::rnd(), Heading::rnd()); +// ASSERT_NE(Heading::rnd(), Heading::rnd()); -} +//} #endif diff --git a/tests/math/TestDistribution.cpp b/tests/math/TestDistribution.cpp index 5498f2f..7c65c53 100644 --- a/tests/math/TestDistribution.cpp +++ b/tests/math/TestDistribution.cpp @@ -2,6 +2,26 @@ #include "../Tests.h" #include "../../math/Distributions.h" +#include "../../misc/Time.h" + +template void benchDist(Dist dist) { + + const int cnt = 1024*1024*32; + float f = 0; + + auto tick = Time::tick(); + for (int i = 0; i < cnt; ++i) { + f = dist.getProbability(f); + } + auto tock = Time::tick(); + + std::cout.imbue(std::locale("en_US.UTF-8")); + std::cout << (cnt / (Time::diffMS(tick, tock)+1) * 1000) << "/sec" << std::endl; + + +} + + TEST(Distribution, Exp) { @@ -15,4 +35,69 @@ TEST(Distribution, Exp) { } +TEST(Distribution, Exp_Performance) { + + Distribution::Exponential dist(0.5); + benchDist(dist); + +} + +TEST(Distribution, BesselHelper) { + + Distribution::Bessel bessel; + + const float delta = 0.001; + ASSERT_NEAR( 1.0, bessel.getModified(0, 0), delta); + ASSERT_NEAR(11.3, bessel.getModified(0, +4), 0.1); + ASSERT_NEAR(11.3, bessel.getModified(0, -4), 0.1); + +} + +TEST(Distribution, VonMises) { + + Distribution::VonMises dist4(0, 4); + Distribution::VonMises dist2(0, 2); + Distribution::VonMises dist1(0, 1); + Distribution::VonMises dist05(0, 0.5); + + const float delta = 0.01; + ASSERT_NEAR(0.768f, dist4.getProbability(0.0f), delta); + ASSERT_NEAR(0.515f, dist2.getProbability(0.0f), delta); + ASSERT_NEAR(0.342f, dist1.getProbability(0.0f), delta); + ASSERT_NEAR(0.24f, dist05.getProbability(0.0f), delta); + +} + + + + + +//#include +//TEST(Distribution, VonMisesDump) { + +// Distribution::VonMises dist1(0, 4.0); +// auto dist2 = dist1.getLUT(); + +// std::ofstream f1("/tmp/1.dat"); +// std::ofstream f2("/tmp/2.dat"); +// for (float i = -4; i <= +4; i += 0.001) { +// f1 << i << " " << dist1.getProbability(i) << std::endl; +// f2 << i << " " << dist2.getProbability(i) << std::endl; +// } +// f1.close(); +// f2.close(); + +//} + +TEST(Distribution, VonMises_Performance) { + Distribution::VonMises dist(0, 2.0); + benchDist(dist); +} + +TEST(Distribution, VonMisesLUT_Performance) { + Distribution::VonMises src(0, 2.0); + auto dist = src.getLUT(); + benchDist(dist); +} + #endif