diff --git a/geo/Angle.h b/geo/Angle.h index 25e5fc1..219064a 100755 --- a/geo/Angle.h +++ b/geo/Angle.h @@ -3,6 +3,7 @@ #include #include "../Assertions.h" +#include "Point2.h" struct Angle { @@ -16,11 +17,17 @@ public: return (tmp < 0) ? (tmp + 2*M_PI) : (tmp); } + /** get the radians from (0,0) to (p.x,p.y) between 0 (to-the-right) and <2_PI */ + static float getRAD_2PI(const Point2& p) { + return getRAD_2PI(0, 0, p.x, p.y); + } + /** get the degrees from (x1,y1) to (x2,y2) between 0 (to-the-right) and <360 */ static float getDEG_360(const float x1, const float y1, const float x2, const float y2) { return radToDeg(getRAD_2PI(x1,y1,x2,y2)); } + /** * gets the angular difference between * - the given radians [0:2PI] @@ -35,12 +42,19 @@ public: /** convert degrees to radians */ static constexpr float degToRad(const float deg) { - return deg / 180 * M_PI; + return deg / 180.0f * M_PI; } /** convert radians to degrees */ static float radToDeg(const float rad) { - return rad * 180 / M_PI; + return rad * 180.0f / M_PI; + } + + /** get a pointer vector (length 1) pointing to the given angle (in radians) */ + static Point2 getPointer(const float rad) { + const float x = cos(rad); + const float y = sin(rad); + return Point2(x,y); } }; diff --git a/geo/Heading.h b/geo/Heading.h index 965d6b2..73acb43 100644 --- a/geo/Heading.h +++ b/geo/Heading.h @@ -46,6 +46,10 @@ public: return (Heading(*this) += _rad); } + /** compare two headings */ + bool operator == (const Heading other) const {return rad == other.rad;} + bool operator != (const Heading other) const {return rad != other.rad;} + /** get an inverted version of this heading (upwards -> downwards, left -> right, ...) */ Heading getInverted() const { Heading out(rad); @@ -57,9 +61,10 @@ public: /** get a random heading */ static Heading rnd() { - static std::minstd_rand gen; gen.seed(1234); + static std::minstd_rand gen(1234); static std::uniform_real_distribution dist(0, _2PI); - return Heading(dist(gen)); + const float rnd = dist(gen); + return Heading(rnd); } #undef _2PI diff --git a/grid/Grid.h b/grid/Grid.h index 40a7cfd..dc4e9fb 100755 --- a/grid/Grid.h +++ b/grid/Grid.h @@ -235,6 +235,11 @@ public: remove(nodes[idx]); } + + /** + * mark the given node for deletion + * see: cleanup() + */ void remove(T& node) { // disconnect from all neighbors @@ -250,8 +255,11 @@ public: } + /** remove all nodes marked for deletion */ void cleanup() { + debugMemoryUsage(); + Log::add(name, "running grid cleanup", false); Log::tick(); @@ -293,6 +301,8 @@ public: rebuildHashes(); + debugMemoryUsage(); + } /** rebuild the UID-hash-list */ @@ -310,6 +320,18 @@ public: } + /** debug-print the grid's current memory usage */ + void debugMemoryUsage() { + const uint32_t bytes = nodes.size() * sizeof(T); + const uint32_t numNodes = nodes.size(); + uint32_t numNeighbors = 0; + //uint32_t numNeighborsUnused = 0; + for (T& n : nodes) { + numNeighbors += n._numNeighbors; + } + Log::add(name, "memory: " + std::to_string(bytes/1024.0f/1024.0f) + " MB in " + std::to_string(numNodes) + " nodes"); + } + // /** // * remove all nodes, marked for deletion. // * BEWARE: this will invalidate all indices used externally! diff --git a/grid/factory/GridFactory.h b/grid/factory/GridFactory.h index 959a81d..74e7257 100755 --- a/grid/factory/GridFactory.h +++ b/grid/factory/GridFactory.h @@ -198,7 +198,8 @@ public: /** build a stair (z-transition) from n1 to n2 */ void buildStairLine(T& _n1, T& _n2) { - const int gridSize_cm = grid.getGridSize_cm(); + // half the grid size = small steps + const int gridSize_cm = grid.getGridSize_cm() / 2; // local copies, needed for std::swap to work T n1 = _n1; T n2 = _n2; diff --git a/grid/factory/GridImportance.h b/grid/factory/GridImportance.h index 80551e9..34d39a5 100644 --- a/grid/factory/GridImportance.h +++ b/grid/factory/GridImportance.h @@ -197,7 +197,7 @@ public: static Distribution::Normal avoidWalls(0.0, 0.4); // favour walking near walls (likely) - static Distribution::Normal sticToWalls(0.9, 0.5); + static Distribution::Normal stickToWalls(0.9, 0.5); // favour walking far away (likely) static Distribution::Normal farAway(2.2, 0.5); @@ -205,7 +205,7 @@ public: // overall importance return - avoidWalls.getProbability(dist_m) * 0.30 // avoid walls - + sticToWalls.getProbability(dist_m) * 0.15 // walk near walls + + stickToWalls.getProbability(dist_m) * 0.15 // walk near walls + farAway.getProbability(dist_m) * 0.15 // walk in the middle ; diff --git a/grid/walk/GridWalk.h b/grid/walk/GridWalk.h index 2385d11..fc5336f 100644 --- a/grid/walk/GridWalk.h +++ b/grid/walk/GridWalk.h @@ -8,7 +8,7 @@ template class GridWalk { public: - virtual GridWalkState getDestination(Grid& grid, const GridWalkState start, const float distance_m) = 0; + virtual GridWalkState getDestination(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad) = 0; }; diff --git a/grid/walk/GridWalkHelper.h b/grid/walk/GridWalkHelper.h index 092e630..310d47a 100644 --- a/grid/walk/GridWalkHelper.h +++ b/grid/walk/GridWalkHelper.h @@ -22,7 +22,13 @@ public: const float d1 = h.getDiffHalfRAD(h1); const float d2 = h.getDiffHalfRAD(h2); //return (h.getDiffHalfRAD(h1) < h.getDiffHalfRAD(h2)); - return (d1 == d2) ? (rand() < RAND_MAX/2) : (d1 < d2); // same heading? > random decision + //return (d1 == d2) ? (rand() < RAND_MAX/2) : (d1 < d2); // same heading? > random decision + //return ((from.z_cm != n1.z_cm) && (from.z_cm == n2.z_cm)) || + // (d1 < d2) || + //(rand() < RAND_MAX/2); + if (d1 < d2) {return true;} + else if (d1 == d2) {return n1.z_cm != n2.z_cm;} + return false; }; auto neighbors = grid.neighbors(from); @@ -39,7 +45,7 @@ public: * if this also fails * - add some randomness and try again */ - template static GridWalkState retryOrInvert(Walker& w, const int numRetries, Grid& grid, GridWalkState start, float distance_m) { + template static GridWalkState retryOrInvert(Walker& w, const int numRetries, Grid& grid, GridWalkState start, float distance_m, const float headChange_rad) { Assert::isTrue(distance_m >= 0, "distance must not be negative!"); Assert::isNotNull(start.node, "starting node must not be null"); @@ -53,13 +59,14 @@ public: // 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 = w.walk(grid, start, distance_m); + res = w.walk(grid, start, distance_m, headChange_rad); } while (res.node == nullptr && --retries); // still reaching a dead end? // -> try a walk in the opposite direction instead if (res.node == nullptr) { - res = w.walk(grid, GridWalkState(start.node, start.heading.getInverted()), distance_m); + GridWalkState newState(start.node, start.heading.getInverted()); + res = w.walk(grid, newState, distance_m, headChange_rad); } // still nothing found? -> modify and try again diff --git a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h index 9b79aa3..45ea259 100644 --- a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h +++ b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h @@ -63,15 +63,15 @@ public: } - GridWalkState getDestination(Grid& grid, GridWalkState start, float distance_m) override { + GridWalkState getDestination(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad) override { - return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m); + return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m, headChange_rad); } private: - GridWalkState walk(Grid& grid, GridWalkState cur, float distRest_m) { + GridWalkState walk(Grid& grid, GridWalkState& cur, float distRest_m, const float headChange_rad) { drawer.reset(); @@ -105,7 +105,10 @@ private: } - GridWalkState next(nullptr, cur.heading); + //GridWalkState next(nullptr, cur.heading); + + // create a copy (to keep heading and walked distance) + GridWalkState next = cur; // pick a random destination T& nDir = drawer.get(); @@ -116,6 +119,7 @@ private: next.heading += headingChangeDist(gen); next.node = &nDir; + //// // compare two neighbors according to their implied heading change //// auto compp = [&] (const T& n1, const T& n2) { //// Heading h1 = GridWalkHelper::getHeading(*cur.node, n1); @@ -145,13 +149,22 @@ private: //// } // get the distance up to this neighbor - distRest_m -= next.node->getDistanceInMeter(*cur.node); + const float walked_m = next.node->getDistanceInMeter(*cur.node); + distRest_m -= walked_m; + + // update the heading-change and walked-distance + next.headingChange_rad += next.heading.getRAD() - cur.heading.getRAD(); + next.distanceWalked_m += walked_m; + + if (next.node->z_cm != cur.node->z_cm) { + int i = 0; + } // done? if (distRest_m <= 0) {return next;} // another round.. - return walk(grid, next, distRest_m); + return walk(grid, next, distRest_m, headChange_rad); } diff --git a/grid/walk/GridWalkPushForward.h b/grid/walk/GridWalkPushForward.h index 1a48af3..3d62bf4 100644 --- a/grid/walk/GridWalkPushForward.h +++ b/grid/walk/GridWalkPushForward.h @@ -43,7 +43,7 @@ public: ; } - GridWalkState getDestination(Grid& grid, const GridWalkState start, const float distance_m) override { + GridWalkState getDestination(Grid& grid, const GridWalkState& start, const float distance_m) override { return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m); @@ -54,7 +54,7 @@ private: // NOTE: allocate >>ONCE< drawer; - GridWalkState walk(Grid& grid, const GridWalkState cur, float distRest_m) { + GridWalkState walk(Grid& grid, const GridWalkState& cur, float distRest_m) { drawer.reset(); @@ -93,7 +93,8 @@ private: // all neighbors are unlikely? -> start over if (drawer.getCumProbability() < 0.01) {return GridWalkState();} - GridWalkState next; + // create a copy (to keep heading and walked distance) + GridWalkState next = cur; // pick the neighbor best matching this new heading next.node = drawer.get(); @@ -103,7 +104,12 @@ private: next.avg = (cur.avg * 0.9) + (Point3(next.node->x_cm, next.node->y_cm, next.node->z_cm) * 0.1); // get the distance up to this neighbor - distRest_m -= next.node->getDistanceInMeter(*cur.node); + const float walked_m = next.node->getDistanceInMeter(*cur.node); + distRest_m -= walked_m; + + // update the heading-change and walked-distance + next.headingChange_rad += next.heading.getRAD() - cur.heading.getRAD(); + next.distanceWalked_m += walked_m; // done? if (distRest_m <= 0) {return next;} diff --git a/grid/walk/GridWalkRandomHeadingUpdate.h b/grid/walk/GridWalkRandomHeadingUpdate.h index 9ab272a..523c418 100644 --- a/grid/walk/GridWalkRandomHeadingUpdate.h +++ b/grid/walk/GridWalkRandomHeadingUpdate.h @@ -45,20 +45,27 @@ public: gen.seed(1234); } - GridWalkState getDestination(Grid& grid, const GridWalkState start, const float distance_m) override { - return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m); + + GridWalkState getDestination(Grid& grid, const GridWalkState& start, const float distance_m, const float headChange_rad) override { + + + return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m, -1); } private: - GridWalkState walk(Grid& grid, const GridWalkState cur, float distRest_m) { + GridWalkState walk(Grid& grid, const GridWalkState& cur, float distRest_m, float headChange = -1) { - GridWalkState next; + // create a copy (to keep heading and walked distance) + GridWalkState next = cur; + + // change the heading at the beginning of each walk + if (headChange == -1) {headChange = headingChangeDist(gen);} // get a new random heading - next.heading = cur.heading + headingChangeDist(gen); + next.heading = cur.heading + headChange; // pick the neighbor best matching this new heading next.node = &GridWalkHelper::getBestNeighbor(grid, *cur.node, next.heading); @@ -71,13 +78,18 @@ private: } // get the distance up to this neighbor - distRest_m -= next.node->getDistanceInMeter(*cur.node); + const float walked_m = next.node->getDistanceInMeter(*cur.node); + distRest_m -= walked_m; + + // update the heading-change and walked-distance + next.headingChange_rad += next.heading.getRAD() - cur.heading.getRAD(); + next.distanceWalked_m += walked_m; // done? if (distRest_m <= 0) {return next;} // another round.. - return walk(grid, next, distRest_m); + return walk(grid, next, distRest_m, headChange); } diff --git a/grid/walk/GridWalkRandomHeadingUpdateAdv.h b/grid/walk/GridWalkRandomHeadingUpdateAdv.h index c761a29..32e07f5 100644 --- a/grid/walk/GridWalkRandomHeadingUpdateAdv.h +++ b/grid/walk/GridWalkRandomHeadingUpdateAdv.h @@ -46,7 +46,7 @@ public: ; } - GridWalkState getDestination(Grid& grid, const GridWalkState start, const float distance_m) override { + GridWalkState getDestination(Grid& grid, const GridWalkState& start, const float distance_m) override { return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m); @@ -60,13 +60,18 @@ private: // NOTE: allocate >>ONCE< drawer; - GridWalkState walk(Grid& grid, const GridWalkState cur, float distRest_m) { + GridWalkState walk(Grid& grid, const GridWalkState& cur, float distRest_m, float headChange = -1) { drawer.reset(); - GridWalkState next; + + // create a copy (to keep heading and walked distance) + GridWalkState next = cur; + + // change the heading at the beginning of each walk + if (headChange == -1) {headChange = headingChangeDist(gen);} // get a new random heading - next.heading = cur.heading + headingChangeDist(gen); + next.heading = cur.heading + headChange; // weight all neighbors based on this heading for (T& neighbor : grid.neighbors(*cur.node)) { @@ -81,8 +86,8 @@ private: const float prob1 = Distribution::Normal::getProbability(0, Angle::degToRad(40), diffRad); // add the node's importance factor into the calculation - const float prob2 = Distribution::Logistic::getCDF(neighbor.imp, 1.0, 0.05); - //const float prob2 = std::pow(neighbor.imp, 10); + //const float prob2 = Distribution::Logistic::getCDF(neighbor.imp, 1.0, 0.05); + const float prob2 = std::pow(neighbor.imp, 10); // final importance const float prob = prob1 * prob2; @@ -106,13 +111,18 @@ private: // } // get the distance up to this neighbor - distRest_m -= next.node->getDistanceInMeter(*cur.node); + const float walked_m = next.node->getDistanceInMeter(*cur.node); + distRest_m -= walked_m; + + // update the heading-change and walked-distance + next.headingChange_rad += next.heading.getRAD() - cur.heading.getRAD(); + next.distanceWalked_m += walked_m; // done? if (distRest_m <= 0) {return next;} // another round.. - return walk(grid, next, distRest_m); + return walk(grid, next, distRest_m, headChange); } diff --git a/grid/walk/GridWalkSimpleControl.h b/grid/walk/GridWalkSimpleControl.h new file mode 100644 index 0000000..82f0c6e --- /dev/null +++ b/grid/walk/GridWalkSimpleControl.h @@ -0,0 +1,141 @@ +#ifndef GRIDWALKSIMPLECONTROL_H +#define GRIDWALKSIMPLECONTROL_H + + + +#include "../../geo/Heading.h" +#include "../Grid.h" + +#include "../../math/Distributions.h" +#include "../../math/DrawList.h" + +#include "GridWalkState.h" +#include "GridWalkHelper.h" +#include "GridWalk.h" + +template class GridWalkSimpleControl : public GridWalk { + + friend class GridWalkHelper; + +private: + + /** per-edge: change heading with this sigma */ + static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10); + + /** fast random-number-generator */ + std::minstd_rand gen; + + /** 0-mean normal distribution */ + std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); + + + + DrawList drawer; + +public: + + /** ctor */ + GridWalkSimpleControl() { + gen.seed(1234); + } + + + + GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad) { + + // proportional change of the heading + static Distribution::Normal dHead(1, 0.10); + + // proportional change of the to-be-walked distance + static Distribution::Normal dWalk(1, 0.10); + + distance_m = distance_m*dWalk.draw(); // TODO: why *2? + headChange_rad = headChange_rad*dHead.draw(); + + + return walk(grid, start, distance_m, headChange_rad); + + } + +private: + + double getProbability(const T& start, const T& possible, const Heading head) const { + + // TODO: WHY?! not only when going back to the start? + if (start.x_cm == possible.x_cm && start.y_cm == possible.y_cm) {return 0.1;} + + // get the angle between START and the possible next node + const Heading possibleHead = GridWalkHelper::getHeading(start, possible); + + // calculate the difference + const float diff = possibleHead.getDiffHalfRAD(head); + +// // compare this heading with the requested one + const double angleProb = Distribution::Normal::getProbability(0, Angle::degToRad(30), diff); +// const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1; // favor best 3 angles equally + + // nodes own importance + //const double nodeProb = Distribution::Logistic::getCDF(possible.imp, 1, 0.5); + const double nodeProb = std::pow(possible.imp, 5); + //const double nodeProb = 1.0; + + // bring it together + return angleProb * nodeProb; + + } + + 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)); + + int retries = 5; + float walked_m = 0; + GridWalkState cur = start; + + // the desired heading + Heading reqHeading = start.heading + (headChange_rad); + + // walk until done + while(walked_m < distance_m) { + + // evaluate all neighbors + drawer.reset(); + for (T& neighbor : grid.neighbors(*cur.node)) { + + const double prob = getProbability(*start.node, neighbor, reqHeading); + drawer.add(neighbor, prob); + + } + + // too bad? start over! + if (drawer.getCumProbability() < 0.1 && (--retries) >= 0) { + walked_m = 0; + cur = start; + WHAT THE HELL + if (retries == 0) { reqHeading = dChange.draw(); } + continue; + } + + // get the neighbor + const T& neighbor = drawer.get(); + + // update + walked_m += neighbor.getDistanceInMeter(*cur.node); + cur.node = &neighbor; + + } + + cur.distanceWalked_m = NAN; + cur.headingChange_rad = NAN; + cur.heading = reqHeading; + + return cur; + + } + +}; + +#endif // GRIDWALKSIMPLECONTROL_H diff --git a/grid/walk/GridWalkState.h b/grid/walk/GridWalkState.h index 48fc6f3..a92b02c 100644 --- a/grid/walk/GridWalkState.h +++ b/grid/walk/GridWalkState.h @@ -12,13 +12,19 @@ template struct GridWalkState { /** the current heading */ Heading heading; + /** cumulative heading change */ + float headingChange_rad; + + /** cumulative distance change */ + float distanceWalked_m; + /** empty ctor */ - GridWalkState() : node(nullptr), heading(0) {;} + GridWalkState() : node(nullptr), heading(0), headingChange_rad(0), distanceWalked_m(0) {;} Point3 avg = Point3(0,0,0); /** ctor with user-node and heading */ - GridWalkState(const T* node, const Heading heading) : node(node), heading(heading) {;} + GridWalkState(const T* node, const Heading heading) : node(node), heading(heading), headingChange_rad(0), distanceWalked_m(0) {;} }; #endif // GRIDWALKSTATE_H diff --git a/math/Distributions.h b/math/Distributions.h index cb9f0ab..0559b24 100644 --- a/math/Distributions.h +++ b/math/Distributions.h @@ -4,5 +4,6 @@ #include "distribution/Normal.h" #include "distribution/Exponential.h" #include "distribution/Logistic.h" +#include "distribution/Uniform.h" #endif // DISTRIBUTIONS_H diff --git a/math/distribution/Uniform.h b/math/distribution/Uniform.h new file mode 100644 index 0000000..550aabd --- /dev/null +++ b/math/distribution/Uniform.h @@ -0,0 +1,38 @@ +#ifndef UNIFORM_H +#define UNIFORM_H + +#include +#include + +namespace Distribution { + + /** uniform distribution */ + template class Uniform { + + private: + + std::minstd_rand gen; + std::uniform_real_distribution dist; + + public: + + /** ctor */ + Uniform(const T min, const T max) : + gen(1234), dist(min, max) { + + } + /** get a uniformaly distributed random number */ + T draw() { + return dist(gen); + } + + /** set the seed to use */ + void setSeed(const long seed) { + gen.seed(seed); + } + + }; + +} + +#endif // UNIFORM_H diff --git a/tests/geo/TestAngle.cpp b/tests/geo/TestAngle.cpp index ff20ce6..428ecd3 100755 --- a/tests/geo/TestAngle.cpp +++ b/tests/geo/TestAngle.cpp @@ -4,6 +4,16 @@ #include "../../geo/Angle.h" +TEST(Angle, dir) { + + // angle -> pointer -> angle + ASSERT_NEAR(0, Angle::getRAD_2PI(Angle::getPointer(0)), 0.0001); + ASSERT_NEAR(1, Angle::getRAD_2PI(Angle::getPointer(1)), 0.0001); + ASSERT_NEAR(2, Angle::getRAD_2PI(Angle::getPointer(2)), 0.0001); + ASSERT_NEAR(3, Angle::getRAD_2PI(Angle::getPointer(3)), 0.0001); + +} + TEST(Angle, calc) { ASSERT_EQ(0, Angle::getDEG_360(0,0, +1,0)); // to the right diff --git a/tests/geo/TestHeading.cpp b/tests/geo/TestHeading.cpp index 0fc226b..679f7de 100644 --- a/tests/geo/TestHeading.cpp +++ b/tests/geo/TestHeading.cpp @@ -26,4 +26,39 @@ TEST(Heading, diff) { } +TEST(Heading, ctor) { + + // OK + Heading(0); + Heading(1); + Heading(2); + Heading(3); + Heading(4); + Heading(5); + Heading(6); + Heading(2*M_PI-0.0001); + + // out of range + ASSERT_THROW(Heading(-0.0001), std::exception); + ASSERT_THROW(Heading(2*M_PI+0.0001), std::exception); + +} + +TEST(Heading, eq) { + + ASSERT_EQ(Heading(0), Heading(0)); + ASSERT_EQ(Heading(1), Heading(1)); + ASSERT_EQ(Heading(2), Heading(2)); + +} + +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()); + +} + #endif diff --git a/tests/grid/TestWalk.cpp b/tests/grid/TestWalk.cpp index 059f0ca..18ed0a7 100644 --- a/tests/grid/TestWalk.cpp +++ b/tests/grid/TestWalk.cpp @@ -97,7 +97,8 @@ TEST(Walk, DISABLED_plot) { GridWalkLightAtTheEndOfTheTunnel walk(g, tmp, *end); std::minstd_rand gen; - std::normal_distribution dist(0.3, 0.3); + std::normal_distribution dWalk(0.3, 0.3); + std::normal_distribution dTurn(0.3, 0.3); K::GnuplotSplotElementPoints pStates; pStates.setColorHex("#880000"); @@ -129,7 +130,7 @@ TEST(Walk, DISABLED_plot) { for (int i = 0; i < 5000; ++i) { pStates.clear(); for (GridWalkState& state : states) { - state = walk.getDestination(g, state, std::abs(dist(gen))); + state = walk.getDestination(g, state, std::abs(dWalk(gen)), dTurn(gen)); pStates.add(K::GnuplotPoint3(state.node->x_cm, state.node->y_cm, state.node->z_cm+10)); } p.gp.draw(p.splot);