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

started removing KLib related code:

Browse Source 
- assertions
- distributions
new helper methods
worked on stairs
worked on grid-walkers
worked on navigation
This commit is contained in:
FrankE
2016-01-27 20:03:58 +01:00
parent e6329e1db4
commit 0e05f4bef8
26 changed files with 408 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
Assertions.h Normal file
View File

@@ -0,0 +1,60 @@
#ifndef ASSERTIONS_H
#define ASSERTIONS_H
#include "Exception.h"
/**
* this file provides assertion methods that may be enabled to trace
* coding errors efficiently during debug or disabled to speed up the
* code execution for the release build
*
* compile with -DWITH_ASSERTIONS
*/
namespace Assert {
static inline void doThrow(const char* err) {
#ifdef WITH_ASSERTIONS
std::string str = "in: ";
str += __PRETTY_FUNCTION__;
str += " error: ";
str += err;
throw Exception(err);
#endif
}
template <typename T> static inline void equal(const T v1, const T v2, const char* err) {
if (v1 != v2) {doThrow(err);}
}
template <typename T> static inline void isTrue(const T v, const char* err) {
if (!v) {doThrow(err);}
}
template <typename T> static inline void isFalse(const T v, const char* err) {
if (v) {doThrow(err);}
}
template <typename T> static inline void isNull(const T v, const char* err) {
if (v != nullptr) {doThrow(err);}
}
template <typename T> static inline void isNotNull(const T v, const char* err) {
if (v == nullptr) {doThrow(err);}
}
template <typename T> static inline void isNotNaN(const T v, const char* err) {
if (v != v) {doThrow(err);}
}
template <typename T> static inline void isNot0(const T v, const char* err) {
if (v == 0) {doThrow(err);}
}
template <typename T> static inline void isBetween(const T v, const T min, const T max, const char* err) {
if (v < min || v > max) {doThrow(err);}
}
}
#endif // ASSERTIONS_H

19
floorplan/PlatformStair.h
View File

@@ -1,17 +1,32 @@
#ifndef PLATFORMSTAIR_H
#define PLATFORMSTAIR_H
#include "Stair.h";
#include "Stair.h"
class PlatformStair {
struct PlatformStair {
/** stair to the platform */
Stair s1;
/** platform in the center */
BBox2 platform;
/** stair from the platform */
Stair s2;
PlatformStair() : s1(), platform(), s2() {;}
// PlatformStair(const Line2& startEdge, const Line2& endEdge, const int w2, const int w2) {
// s1.start = startEdge;
// s2.start = endEdge;
// Point2 edgeDir = startEdge.p2 - startEdge.p1;
// Point2 perp(edgeDir.y, -edgeDir.x);
// platform.add(startEdge.p1 + perp);
// plat
// }
};
#endif // PLATFORMSTAIR_H

7
floorplan/Stair.h
View File

@@ -13,6 +13,13 @@ struct Stair {
/** the direction to move all the starting points to */
Point2 dir;
/** empty ctor */
Stair() : start(), dir() {;}
/** ctor with starting edge and stair-direction */
Stair(const Line2& start, const Point2& dir) : start(start), dir(dir) {;}
};
#endif // STAIR_H

8
geo/Angle.h
View File

@@ -2,7 +2,7 @@
#define ANGLE_H
#include <cmath>
#include <KLib/Assertions.h>
#include "../Assertions.h"
struct Angle {
@@ -11,7 +11,7 @@ public:
/** get the radians from (x1,y1) to (x2,y2) between 0 (to-the-right) and <2_PI */
static float getRAD_2PI(const float x1, const float y1, const float x2, const float y2) {
_assertFalse( (x1==x2)&&(y1==y2), "(x1,y1) must not equal (x2,y2)!!");
Assert::isFalse( (x1==x2)&&(y1==y2), "(x1,y1) must not equal (x2,y2)!!");
const float tmp = std::atan2(y2-y1, x2-x1);
return (tmp < 0) ? (tmp + 2*M_PI) : (tmp);
}
@@ -27,8 +27,8 @@ public:
* - as a change-in-direction between [0:PI]
*/
static float getDiffRAD_2PI_PI(const float r1, const float r2) {
_assertBetween(r1, 0, (float)(2*M_PI), "r1 out of bounds");
_assertBetween(r2, 0, (float)(2*M_PI), "r2 out of bounds");
Assert::isBetween(r1, 0.0f, (float)(2*M_PI), "r1 out of bounds");
Assert::isBetween(r2, 0.0f, (float)(2*M_PI), "r2 out of bounds");
float tmp = std::abs(r1-r2);
return (tmp <= M_PI) ? (tmp) : (2*M_PI-tmp);
}

4
geo/BBox2.h
View File

@@ -19,8 +19,12 @@ protected:
public:
/** empty ctor */
BBox2() : p1(MAX,MAX), p2(MIN,MIN) {;}
/** ctor */
BBox2(const Point2& p1, const Point2& p2) : p1(p1), p2(p2) {;}
/** adjust the bounding-box by adding this point */
void add(const Point2& p) {

6
geo/Heading.h
View File

@@ -19,12 +19,12 @@ public:
/** ctor with radians */
Heading(const float rad) : rad(rad) {
_assertBetween(rad, 0, _2PI, "radians out of bounds");
Assert::isBetween(rad, 0.0f, (float)_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");
Assert::isBetween(rad, 0.0f, (float)_2PI, "radians out of bounds");
}
/** angular difference [0:PI] */
@@ -34,7 +34,7 @@ public:
/** 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");
Assert::isBetween(_rad, float(-_2PI*0.99), float(+_2PI*0.99), "radians out of bounds");
rad += _rad;
if (rad >= _2PI) {rad -= _2PI;}
else if (rad < 0) {rad += _2PI;}

4
geo/Point3.h
View File

@@ -1,7 +1,7 @@
#ifndef POINT3_H
#define POINT3_H
#include <KLib/Assertions.h>
#include "../Assertions.h"
#include <cmath>
/**
@@ -37,7 +37,7 @@ struct Point3 {
/** read-only array access */
float operator [] (const int idx) const {
_assertBetween(idx, 0, 2, "index out of bounds");
Assert::isBetween(idx, 0, 2, "index out of bounds");
if (0 == idx) {return x;}
if (1 == idx) {return y;}
return z;

8
grid/Grid.h
View File

@@ -9,7 +9,7 @@
#include "GridPoint.h"
#include "GridNode.h"
#include <KLib/Assertions.h>
#include "../Assertions.h"
#include "../geo/BBox3.h"
#include "../misc/Debug.h"
@@ -95,7 +95,7 @@ public:
void connectUniDir(T& n1, const T& n2) {
n1._neighbors[n1._numNeighbors] = n2._idx;
++n1._numNeighbors;
_assertBetween(n1._numNeighbors, 0, 10, "number of neighbors out of bounds!");
Assert::isBetween(n1._numNeighbors, 0, 10, "number of neighbors out of bounds!");
}
/**
@@ -188,13 +188,13 @@ public:
/** array access */
T& operator [] (const int idx) {
_assertBetween(idx, 0, getNumNodes()-1, "index out of bounds");
Assert::isBetween(idx, 0, getNumNodes()-1, "index out of bounds");
return nodes[idx];
}
/** const array access */
const T& operator [] (const int idx) const {
_assertBetween(idx, 0, getNumNodes()-1, "index out of bounds");
Assert::isBetween(idx, 0, getNumNodes()-1, "index out of bounds");
return nodes[idx];
}

4
grid/GridPoint.h
View File

@@ -4,7 +4,7 @@
#include <cmath>
#include "../geo/Point3.h"
#include <KLib/Assertions.h>
#include "../Assertions.h"
struct GridPoint {
@@ -46,7 +46,7 @@ struct GridPoint {
/** read-only array access */
float operator [] (const int idx) const {
_assertBetween(idx, 0, 2, "index out of bounds");
Assert::isBetween(idx, 0, 2, "index out of bounds");
if (0 == idx) {return x_cm;}
if (1 == idx) {return y_cm;}
{return z_cm;}

176
grid/factory/GridFactory.h
View File

@@ -6,6 +6,7 @@
#include "../../floorplan/Floor.h"
#include "../../floorplan/Stairs.h"
#include "../../floorplan/PlatformStair.h"
#include "../../geo/Units.h"
#include "../GridNodeBBox.h"
@@ -57,40 +58,32 @@ public:
}
/** connect all neighboring nodes part of the given index-vector */
void connectAdjacent(const std::vector<int>& indices) {
for (const int idx : indices) {
// connect the node with its neighbors
connectAdjacent(grid[idx]);
}
}
/** connect all neighboring nodes located on the given height-plane */
void connectAdjacent(const float z_cm) {
Log::add(name, "connecting all adjacent nodes at height " + std::to_string(z_cm), false);
Log::tick();
const int gridSize_cm = grid.getGridSize_cm();
// connect adjacent grid-points
for (T& n1 : grid) {
// not the floor we are looking for? -> skip (ugly.. slow(er))
if (n1.z_cm != z_cm) {continue;}
// square around each point
for (int x = -gridSize_cm; x <= gridSize_cm; x += gridSize_cm) {
for (int y = -gridSize_cm; y <= gridSize_cm; y += gridSize_cm) {
// skip the center (node itself)
if ((x == y) && (x == 0)) {continue;}
// position of the potential neighbor
const int ox = n1.x_cm + x;
const int oy = n1.y_cm + y;
const GridPoint p(ox, oy, n1.z_cm);
// does the grid contain the potential neighbor?
const T* n2 = grid.getNodePtrFor(p);
if (n2 != nullptr) {
grid.connectUniDir(n1, *n2); // UNI-dir connection as EACH node is processed!
}
}
}
// connect the node with its neighbors
connectAdjacent(n1);
}
@@ -98,54 +91,124 @@ public:
}
/** connect the given node with its neighbors */
void connectAdjacent(T& n1) {
const int gridSize_cm = grid.getGridSize_cm();
// square around the node
for (int x = -gridSize_cm; x <= gridSize_cm; x += gridSize_cm) {
for (int y = -gridSize_cm; y <= gridSize_cm; y += gridSize_cm) {
// skip the center (node itself)
if ((x == y) && (x == 0)) {continue;}
// position of the potential neighbor
const int ox = n1.x_cm + x;
const int oy = n1.y_cm + y;
const GridPoint p(ox, oy, n1.z_cm);
// does the grid contain the potential neighbor?
const T* n2 = grid.getNodePtrFor(p);
if (n2 != nullptr) {
grid.connectUniDir(n1, *n2); // UNI-dir connection as EACH node is processed!
}
}
}
}
/** shrink the given bbox to be grid-aligned */
BBox2 shrinkAlign(const BBox2& bb) {
const float gridSize_cm = grid.getGridSize_cm();
Point2 p1 = bb.getMin();
Point2 p2 = bb.getMax();
p1.x = std::ceil(p1.x/gridSize_cm)*gridSize_cm;
p1.y = std::ceil(p1.y/gridSize_cm)*gridSize_cm;
p2.x = std::floor(p2.x/gridSize_cm)*gridSize_cm;
p2.y = std::floor(p2.y/gridSize_cm)*gridSize_cm;
BBox2 res; res.add(p1); res.add(p2); return res;
}
/** add a new platform-stair between the two given floors */
void buildPlatformStair(const PlatformStair& s, const float z1_cm, const float z2_cm) {
const float zCenter_cm = (z2_cm + z1_cm) / 2;
std::vector<int> indices;
// add the platform in the middle
BBox2 bb = shrinkAlign(s.platform);
const int gridSize_cm = grid.getGridSize_cm();
for (int x_cm = bb.getMin().x; x_cm <= bb.getMax().x; x_cm += gridSize_cm) {
for (int y_cm = bb.getMin().y; y_cm <= bb.getMax().y; y_cm += gridSize_cm) {
int idx = grid.add(T(x_cm, y_cm, zCenter_cm));
indices.push_back(idx);
}
}
// connect the plattform in the middle
connectAdjacent(indices);
// TODO: interconnect (x-y) the stair lines???
buildStair(s.s1, z1_cm, zCenter_cm);
buildStair(s.s2, z2_cm, zCenter_cm);
}
void addStairs(const Stairs& stairs, const float z1_cm, const float z2_cm) {
Log::add(name, "adding stairs between " + std::to_string(z1_cm) + " and " + std::to_string(z2_cm), false);
Log::tick();
for (const Stair& s : stairs) {
// potential starting-point for the stair
for (T& n : grid) {
// node lies on the stair's starting edge?
if (n.z_cm == z1_cm && grid.getBBox(n).intersects(s.start)) {
// construct end-point by using the stair's direction
const Point3 end = Point3(n.x_cm, n.y_cm, z2_cm) + Point3(s.dir.x, s.dir.y, 0);
GridPoint gp(end.x, end.y, end.z);
// does such and end-point exist within the grap? -> construct stair
if (grid.hasNodeFor(gp)) {
T& n2 = (T&) grid.getNodeFor(gp);
buildStair(n, n2);
}
}
}
}
for (const Stair& s : stairs) { buildStair(s, z1_cm, z2_cm); }
Log::tock();
}
/** build a stair (z-transition) from n1 to n2 */
void buildStair(T& n1, T& n2) {
void buildStair(const Stair& s, const float z1_cm, const float z2_cm) {
// potential starting-point for the stair
for (T& n : grid) {
// node lies on the stair's starting edge?
if (n.z_cm == z1_cm && grid.getBBox(n).intersects(s.start)) {
// construct end-point by using the stair's direction
const Point3 end = Point3(n.x_cm, n.y_cm, z2_cm) + Point3(s.dir.x, s.dir.y, 0);
GridPoint gp(end.x, end.y, end.z);
// does such and end-point exist within the grid? -> construct stair-line
if (grid.hasNodeFor(gp)) {
T& n2 = (T&) grid.getNodeFor(gp);
buildStairLine(n, n2);
}
}
}
}
/** build a stair (z-transition) from n1 to n2 */
void buildStairLine(T& _n1, T& _n2) {
const int gridSize_cm = grid.getGridSize_cm();
// local copies, needed for std::swap to work
T n1 = _n1; T n2 = _n2;
// ensure we work from lower to upper levels
if (n2.z_cm < n1.z_cm) { std::swap(n1, n2); }
//TODO: ensure n1 is below n2
const float zDiff = n2.z_cm - n1.z_cm;
const float xDiff = n2.x_cm - n1.x_cm;
const float yDiff = n2.y_cm - n1.y_cm;
int idx1 = n1.getIdx();
int idx2 = -1;
const int idx3 = n2.getIdx();
const int gridSize_cm = grid.getGridSize_cm();
int idx1 = n1.getIdx(); // starting node
int idx2 = -1; // next node
const int idx3 = n2.getIdx(); // final node
// move upards in gridSize steps
for (int _z = gridSize_cm; _z < zDiff; _z+= gridSize_cm) {
@@ -170,9 +233,8 @@ public:
}
// add the last segment
if (idx2 != -1) {
grid.connectBiDir(idx2, idx3);
}
Assert::isTrue(idx2 != -1, "strange stair issue?!");
grid.connectBiDir(idx2, idx3);
}

16
grid/factory/GridImportance.h
View File

@@ -11,7 +11,7 @@
#include "../../nav/dijkstra/Dijkstra.h"
#include "../../nav/dijkstra/DijkstraPath.h"
#include <KLib/math/distribution/Normal.h>
#include "../../math/Distributions.h"
@@ -69,7 +69,7 @@ public:
n1.imp = 1.0f;
n1.imp += getWallImportance(n1, Units::cmToM(std::sqrt(squaredDist[0])) );
n1.imp += getWallImportance( Units::cmToM(std::sqrt(squaredDist[0])) );
//addDoor(n1, neighbors);
// is the current node a door?
@@ -86,7 +86,7 @@ public:
// process each node again
for (T& n1 : g) {
static K::NormalDistribution favorDoors(0.0, 1.0);
static Distribution::Normal<float> favorDoors(0.0f, 1.0f);
// get the distance to the nearest door
const float dist_m = Units::cmToM(knnDoors.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
@@ -141,7 +141,7 @@ public:
//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;
n.impPath = 1.0 + Distribution::Normal<float>::getProbability(0, 1.0, dist_m) * 0.8;
}
@@ -191,16 +191,16 @@ public:
}
/** get the importance of the given node depending on its nearest wall */
template <typename T> float getWallImportance(T& nSrc, float dist_m) {
float getWallImportance(float dist_m) {
// avoid sticking too close to walls (unlikely)
static K::NormalDistribution avoidWalls(0.0, 0.4);
static Distribution::Normal<float> avoidWalls(0.0, 0.4);
// favour walking near walls (likely)
static K::NormalDistribution sticToWalls(0.9, 0.5);
static Distribution::Normal<float> sticToWalls(0.9, 0.5);
// favour walking far away (likely)
static K::NormalDistribution farAway(2.2, 0.5);
static Distribution::Normal<float> farAway(2.2, 0.5);
if (dist_m > 2.0) {dist_m = 2.0;}
// overall importance

2
grid/walk/GridWalkHelper.h
View File

@@ -40,7 +40,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) {
_assertTrue(distance_m >= 0, "distance must not be negative!");
Assert::isTrue(distance_m >= 0, "distance must not be negative!");
GridWalkState<T> res;

4
grid/walk/GridWalkLightAtTheEndOfTheTunnel.h
View File

@@ -5,7 +5,7 @@
#include "../Grid.h"
#include "../../math/DrawList.h"
#include <KLib/math/distribution/Normal.h>
#include "../../math/Distributions.h"
#include "../../nav/dijkstra/Dijkstra.h"
@@ -84,7 +84,7 @@ private:
const float diff = cur.heading.getDiffHalfRAD(potentialHeading);
// probability for this direction change?
double prob = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff);
double prob = Distribution::Normal<float>::getProbability(0, HEADING_DIFF_SIGMA, diff);
// perfer locations reaching the target
const double shortening = cur.node->distToTarget - neighbor.distToTarget;

5
grid/walk/GridWalkPushForward.h
View File

@@ -8,9 +8,6 @@
#include "../Grid.h"
#include "../../math/DrawList.h"
#include <KLib/math/distribution/Normal.h>
#include <KLib/math/distribution/Exponential.h>
#include "../../nav/dijkstra/Dijkstra.h"
#include "GridWalkState.h"
@@ -70,7 +67,7 @@ private:
const float diffRad = potentialHeading.getDiffHalfRAD(cur.heading);
// weight this change
const float prob1 = K::NormalDistribution::getProbability(0, HEADING_ALLOWED_SIGMA, diffRad);
const float prob1 = Distribution::Normal<float>::getProbability(0, HEADING_ALLOWED_SIGMA, diffRad);
// distance from average? and previous distance from average

2
grid/walk/GridWalkRandomHeadingUpdate.h
View File

@@ -4,8 +4,6 @@
#include "../../geo/Heading.h"
#include "../Grid.h"
#include <KLib/math/distribution/Normal.h>
#include "../../nav/dijkstra/Dijkstra.h"
#include "GridWalkState.h"

14
grid/walk/GridWalkRandomHeadingUpdateAdv.h
View File

@@ -6,9 +6,7 @@
#include "../Grid.h"
#include "../../math/DrawList.h"
#include <KLib/math/distribution/Normal.h>
#include <KLib/math/distribution/Exponential.h>
#include "../../math/Distributions.h"
#include "../../nav/dijkstra/Dijkstra.h"
#include "GridWalkState.h"
@@ -55,11 +53,7 @@ public:
private:
// https://de.wikipedia.org/wiki/Logistische_Verteilung
/** alpha = move the center, beta = slope */
const float logisticDist(const float x, const float alpha, const float beta) {
return 1 / (1 + std::exp( -((x-alpha)/beta) ) );
}
// NOTE: allocate >>ONCE<<! otherwise random numbers will NOT work!
@@ -83,10 +77,10 @@ private:
const float diffRad = potentialHeading.getDiffHalfRAD(cur.heading);
// weight this change
const float prob1 = K::NormalDistribution::getProbability(0, Angle::degToRad(40), diffRad);
const float prob1 = Distribution::Normal<float>::getProbability(0, Angle::degToRad(40), diffRad);
// add the node's importance factor into the calculation
const float prob2 = logisticDist(neighbor.imp, 1.0, 0.05);
const float prob2 = Distribution::Logistic<float>::getCDF(neighbor.imp, 1.0, 0.05);
//const float prob2 = std::pow(neighbor.imp, 10);
// final importance

4
grid/walk/GridWalkWeighted.h
View File

@@ -5,7 +5,7 @@
#include "../Grid.h"
#include "../../math/DrawList.h"
#include <KLib/math/distribution/Normal.h>
#include "../../math/Distributions.h"
/**
* perform walks on the grid based on some sort of weighting
@@ -87,7 +87,7 @@ private:
const float diff = cur.heading.getDiffHalfRAD(potentialHeading);
// probability for this direction change?
double prob = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff);
double prob = Distribution::Normal<float>::getProbability(0, HEADING_DIFF_SIGMA, diff);
// perfer locations reaching the target
const double shortening = cur.node->distToTarget - neighbor.distToTarget;

8
math/Distributions.h Normal file
View File

@@ -0,0 +1,8 @@
#ifndef DISTRIBUTIONS_H
#define DISTRIBUTIONS_H
#include "distribution/Normal.h"
#include "distribution/Exponential.h"
#include "distribution/Logistic.h"
#endif // DISTRIBUTIONS_H

4
math/DrawList.h
View File

@@ -4,7 +4,7 @@
#include <vector>
#include <random>
#include <KLib/Assertions.h>
#include "../Assertions.h"
/**
* add elements of a certain probability
@@ -72,7 +72,7 @@ public:
const auto tmp = std::lower_bound(elements.begin(), elements.end(), rndVal);
// sanity check
_assertFalse(tmp == elements.end(), "draw() did not find a valid element");
Assert::isFalse(tmp == elements.end(), "draw() did not find a valid element");
// done
return (*tmp).element;

4
math/distribution/Exponential.h Normal file
View File

@@ -0,0 +1,4 @@
#ifndef EXPONENTIAL_H
#define EXPONENTIAL_H
#endif // EXPONENTIAL_H

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

@@ -0,0 +1,54 @@
#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

22
math/distribution/Logistic.h Normal file
View File

@@ -0,0 +1,22 @@
#ifndef LOGISTIC_H
#define LOGISTIC_H
namespace Distribution {
// https://de.wikipedia.org/wiki/Logistische_Verteilung
template <typename T> class Logistic {
public:
/** alpha = move the center, beta = slope */
static T getCDF(const T x, const T alpha, const T beta) {
return 1 / (1 + std::exp( -((x-alpha)/beta) ) );
}
};
}
#endif // LOGISTIC_H

57
math/distribution/Normal.h Normal file
View File

@@ -0,0 +1,57 @@
#ifndef NORMAL_H
#define NORMAL_H
#include <cmath>
#include <random>
namespace Distribution {
/** normal distribution */
template <typename T> class Normal {
private:
const T mu;
const T sigma;
const T _a;
std::minstd_rand gen;
std::normal_distribution<T> 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) {
}
/** get probability for the given value */
T getProbability(const T val) const {
const T b = -0.5 * ((val-mu)/sigma) * ((val-mu)/sigma);
return _a * std::exp(b);
}
/** 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 T getProbability(const T mu, const T sigma, const T val) {
const T a = 1.0 / (sigma * std::sqrt(2.0 * M_PI));
const T b = -0.5 * ((val-mu)/sigma) * ((val-mu)/sigma);
return a * std::exp(b);
}
};
}
#endif // NORMAL_H

5
nav/dijkstra/Dijkstra.h
View File

@@ -13,7 +13,7 @@
#include "../../misc/Time.h"
#include "../../Defines.h"
#include <KLib/Assertions.h>
#include "../../Assertions.h"
template <typename T> class Dijkstra {
@@ -32,6 +32,7 @@ public:
// NOTE: end is currently ignored!
// runs until all nodes were evaluated
(void) end;
Log::add("Dijkstra", "calculating dijkstra from " + (std::string)start + " to ALL OTHER nodes", false);
Log::tick();
@@ -89,7 +90,7 @@ public:
// get the distance-weight to the neighbor
const float weight = acc.getWeightBetween(*dnSrc->element, *dst);
_assertTrue(weight >= 0, "edge-weight must not be negative!");
Assert::isTrue(weight >= 0, "edge-weight must not be negative!");
// update the weight to the destination?
const float potentialWeight = dnSrc->cumWeight + weight;

2
tests/grid/TestGrid.cpp
View File

@@ -176,7 +176,7 @@ TEST(Grid, neighborIter) {
grid.connectBiDir(idx1, idx5);
int i = 0;
for (GP& node : grid.neighbors(idx1)) {++i;}
for (GP& node : grid.neighbors(idx1)) { (void) node; ++i; }
ASSERT_EQ(4, i);
}

18
tests/grid/TestWalk.cpp
View File

@@ -12,7 +12,8 @@
#include "../../grid/walk/GridWalkWeighted.h"
#include "../../grid/walk/GridWalkLightAtTheEndOfTheTunnel.h"
TEST(Walk, DISABLED_plot) {
TEST(Walk, plot) {
//TEST(Walk, DISABLED_plot) {
Grid<GP> g(20);
GridFactory<GP> gf(g);
@@ -25,12 +26,24 @@ TEST(Walk, DISABLED_plot) {
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);
}
//TODO: write as something simpler? e.g. as box with rotation??
PlatformStair pStair;
pStair.platform = BBox2(Point2(200, 200), Point2(400,500));
pStair.s1 = Stair(Line2( 600,200, 600,350 ), Point2(-200,0));
pStair.s2 = Stair(Line2( 600,400, 600,500 ), Point2(-250,0));
//gf.buildPlatformStair(pStair, 20, 340);
gf.removeIsolated();
// calculate node importance based on the floorplan (walls, ...)
@@ -95,7 +108,10 @@ TEST(Walk, DISABLED_plot) {
}
Plot p;
// show the grid
//p.build(g);
p.addFloor(f1, 20);
if (use3D) {
p.addFloor(f2, 340);