worked on grid-walker v3
This commit is contained in:
k-a-z-u
@@ -27,6 +27,11 @@ public:
|
|||||||
Assert::isBetween(rad, 0.0f, (float)_2PI, "radians out of bounds");
|
Assert::isBetween(rad, 0.0f, (float)_2PI, "radians out of bounds");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/** ctor from(x,y) -> to(x,y) */
|
||||||
|
Heading(const Point2 from, const Point2 to) : Heading(from.x, from.y, to.x, to.y) {
|
||||||
|
;
|
||||||
|
}
|
||||||
|
|
||||||
/** POSITIVE angular difference [0:PI] */
|
/** POSITIVE angular difference [0:PI] */
|
||||||
float getDiffHalfRAD(const Heading other) const {
|
float getDiffHalfRAD(const Heading other) const {
|
||||||
return Angle::getDiffRAD_2PI_PI(rad, other.rad);
|
return Angle::getDiffRAD_2PI_PI(rad, other.rad);
|
||||||
|
|||||||
@@ -1,113 +0,0 @@
|
|||||||
#ifndef GRIDWALK3HELPER_H
|
|
||||||
#define GRIDWALK3HELPER_H
|
|
||||||
|
|
||||||
#include "../../../nav/dijkstra/Dijkstra.h"
|
|
||||||
#include <set>
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
template <typename Node> class GridWalk3Helper {
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
/** one walk along several nodes */
|
|
||||||
struct Walk3 : public std::vector<const Node*> {
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
struct Walks3 : std::vector<Walk3> {
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
struct Nodes3 : std::vector<const Node*> {
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
/** get all possible walks from start within a given region */
|
|
||||||
static Walks3 getAllPossibleWalks(Grid<Node>& grid, const Node* start, const float dist_m) {
|
|
||||||
|
|
||||||
struct Access {
|
|
||||||
Grid<Node>& grid;
|
|
||||||
Access(Grid<Node>& grid) : grid(grid) {;}
|
|
||||||
int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();}
|
|
||||||
Node* getNeighbor(const Node& n, const int idx) const {return &grid.getNeighbor(n, idx);}
|
|
||||||
float getWeightBetween(const Node& n1, const Node& n2) const {return n1.inMeter().getDistance(n2.inMeter());}
|
|
||||||
} acc(grid);
|
|
||||||
|
|
||||||
const float addDist_m = grid.getGridSize_cm() / 100.0f;
|
|
||||||
const float maxDist_m = dist_m * 1.1 + addDist_m;
|
|
||||||
Dijkstra<Node> dijkstra;
|
|
||||||
dijkstra.build(start, nullptr, acc, maxDist_m);
|
|
||||||
|
|
||||||
const std::unordered_map<const Node*, DijkstraNode<Node>*>& nodes = dijkstra.getNodes();
|
|
||||||
|
|
||||||
Walks3 walks;
|
|
||||||
|
|
||||||
for (const auto& it : nodes) {
|
|
||||||
Walk3 walk;
|
|
||||||
DijkstraNode<Node>* node = it.second;
|
|
||||||
do {
|
|
||||||
const Node* gridNode = node->element;
|
|
||||||
walk.insert(walk.begin(), gridNode); // push_front() as dijkstra is inverted
|
|
||||||
node = node->previous;
|
|
||||||
} while (node);
|
|
||||||
walks.push_back(walk);
|
|
||||||
}
|
|
||||||
|
|
||||||
return walks;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
/** get all reachable nodes that are within a given range */
|
|
||||||
static Nodes3 getAllReachableNodes(Grid<Node>& grid, const Node* start, const float dist_m) {
|
|
||||||
|
|
||||||
Nodes3 res;
|
|
||||||
std::unordered_map<uint32_t, float> distances;
|
|
||||||
std::vector<uint32_t> toVisit;
|
|
||||||
|
|
||||||
toVisit.push_back(start->getIdx());
|
|
||||||
distances[start->getIdx()] = 0.0f;
|
|
||||||
|
|
||||||
while (!toVisit.empty()) {
|
|
||||||
|
|
||||||
int curIdx = toVisit.front();
|
|
||||||
toVisit.erase(toVisit.begin());
|
|
||||||
|
|
||||||
const Node& curNode = grid[curIdx];
|
|
||||||
const float curDistance = distances[curIdx];
|
|
||||||
res.push_back(&curNode); // remember for output
|
|
||||||
|
|
||||||
if (curDistance <= dist_m) {
|
|
||||||
for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
|
|
||||||
|
|
||||||
const int neighborIdx = curNode.getNeighborIdx(i);
|
|
||||||
const Node& neighbor = grid[neighborIdx];
|
|
||||||
const float addDist = neighbor.inMeter().getDistance(curNode.inMeter());
|
|
||||||
const float totalDist = curDistance + addDist;
|
|
||||||
|
|
||||||
// this is like in dijkstra. keep the smallest distance to reach a node:
|
|
||||||
|
|
||||||
// not yet reached -> store distance
|
|
||||||
if (distances.find(neighborIdx) == distances.end()) {
|
|
||||||
toVisit.push_back(neighborIdx);
|
|
||||||
distances[neighborIdx] = totalDist;
|
|
||||||
|
|
||||||
// reached earlier but found shorter way
|
|
||||||
} else {
|
|
||||||
if (distances[neighborIdx] > totalDist) {
|
|
||||||
distances[neighborIdx] = totalDist;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
return res;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
#endif // GRIDWALK3HELPER_H
|
|
||||||
@@ -1,257 +0,0 @@
|
|||||||
#ifndef GRIDWALKER3_H
|
|
||||||
#define GRIDWALKER3_H
|
|
||||||
|
|
||||||
#include "../../../data/Timestamp.h"
|
|
||||||
#include "../../Grid.h"
|
|
||||||
#include "../../../math/DrawList.h"
|
|
||||||
#include "../../../math/Distributions.h"
|
|
||||||
#include "../../../math/Stats.h"
|
|
||||||
#include "../../../geo/Heading.h"
|
|
||||||
#include "../../../math/stats/Variance.h"
|
|
||||||
#include "GridWalk3Helper.h"
|
|
||||||
#include "../../../geo/BBox2.h"
|
|
||||||
|
|
||||||
/**
|
|
||||||
* modular grid-walker that takes various sub-components to determine
|
|
||||||
* p(e) and thus randomly pick edges
|
|
||||||
*/
|
|
||||||
template <typename Node> class GridWalker3 {
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
/** all modules to evaluate */
|
|
||||||
// std::vector<WalkModule<Node, WalkState>*> modules;
|
|
||||||
|
|
||||||
RandomGenerator rnd;
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
/** paremters for the walk */
|
|
||||||
struct WalkParams {
|
|
||||||
Point3 start;
|
|
||||||
float distance_m;
|
|
||||||
Heading heading = Heading(0);
|
|
||||||
};
|
|
||||||
|
|
||||||
/** result of the random walk */
|
|
||||||
struct WalkResult {
|
|
||||||
Point3 position;
|
|
||||||
Heading heading = Heading(0);
|
|
||||||
double probability = 1.0;
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
using Helper = GridWalk3Helper<Node>;
|
|
||||||
using Walk = typename GridWalk3Helper<Node>::Walk3;
|
|
||||||
using Walks = typename GridWalk3Helper<Node>::Walks3;
|
|
||||||
using Nodes = typename GridWalk3Helper<Node>::Nodes3;
|
|
||||||
|
|
||||||
GridPoint p3ToGp(const Point3 p) const {
|
|
||||||
const Point3 p100 = p*100;
|
|
||||||
return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) );
|
|
||||||
}
|
|
||||||
|
|
||||||
Point3 gpToP3(const GridPoint gp) const {
|
|
||||||
return Point3(gp.x_cm / 100.0f, gp.y_cm / 100.0f, gp.z_cm / 100.0f);
|
|
||||||
}
|
|
||||||
|
|
||||||
/** perform the walk based on the configured setup */
|
|
||||||
const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) {
|
|
||||||
//return getDestination(grid, GridPoint(start.x*100, start.y*100, start.z*100), ctrl, dist_m);
|
|
||||||
return _drawThenCheck(grid, params);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// /** perform the walk based on the configured setup */
|
|
||||||
// const Point3 getDestination(Grid<Node>& grid, const GridPoint start, const params) {
|
|
||||||
|
|
||||||
// //return _getFromPossibleWalks(grid, start, ctrl, dist_m);
|
|
||||||
// //return _drawThenCheck(grid, start, ctrl, dist_m);
|
|
||||||
// throw "error";
|
|
||||||
// }
|
|
||||||
|
|
||||||
/** does the given grid-node contain the provided point-in-question? */
|
|
||||||
const bool contains(const Grid<Node>& grid, const Node* n, Point2 pt) {
|
|
||||||
const float gridSize_m = grid.getGridSize_cm() / 100.0f;
|
|
||||||
const float d = gridSize_m / 2.0f;
|
|
||||||
const Point2 pMin = n->inMeter().xy() - Point2(d, d);
|
|
||||||
const Point2 pMax = n->inMeter().xy() + Point2(d, d);
|
|
||||||
const BBox2 bbox(pMin, pMax);
|
|
||||||
return bbox.contains(pt);
|
|
||||||
}
|
|
||||||
|
|
||||||
/** does one of the given grid-node scontains the provided point-in-question? */
|
|
||||||
const Node* contains(const Grid<Node>& grid, const Nodes& nodes, Point2 pt) {
|
|
||||||
for (const Node* n : nodes) {
|
|
||||||
if (contains(grid, n, pt)) {return n;}
|
|
||||||
}
|
|
||||||
return nullptr;
|
|
||||||
}
|
|
||||||
|
|
||||||
const WalkResult _drawThenCheck(Grid<Node>& grid, const WalkParams& params) {
|
|
||||||
|
|
||||||
const GridPoint gpStart = p3ToGp(params.start);
|
|
||||||
const Node* startNode = grid.getNodePtrFor(gpStart);
|
|
||||||
|
|
||||||
static Distribution::Normal<float> dDist(1, 0.02);
|
|
||||||
static Distribution::Normal<float> dHead(0, 0.01);
|
|
||||||
|
|
||||||
// include one additional grid-cell (increased distance)
|
|
||||||
const float secBuffer_m = grid.getGridSize_cm() / 100.0f;
|
|
||||||
const float range_m = params.distance_m + secBuffer_m;
|
|
||||||
const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m);
|
|
||||||
|
|
||||||
WalkResult res;
|
|
||||||
res.heading = params.heading;
|
|
||||||
res.position = params.start;
|
|
||||||
float realDist_m = params.distance_m;
|
|
||||||
|
|
||||||
int cnt = 0;
|
|
||||||
while(true) {
|
|
||||||
|
|
||||||
const Point2 dir = res.heading.asVector();
|
|
||||||
const Point2 dst = params.start.xy() + (dir * realDist_m);
|
|
||||||
|
|
||||||
// is dst reachable?
|
|
||||||
const Node* n = contains(grid, reachableNodes, dst);
|
|
||||||
if (n) {
|
|
||||||
|
|
||||||
const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
|
|
||||||
const GridPoint gp = p3ToGp(p3);
|
|
||||||
|
|
||||||
if (grid.hasNodeFor(gp)) {
|
|
||||||
res.position = p3; // update position
|
|
||||||
res.heading; // keep as-is
|
|
||||||
res.probability; // keep as-is
|
|
||||||
return res; // done
|
|
||||||
} else {
|
|
||||||
std::cout << "WARN dst not found" << std::endl;
|
|
||||||
//throw "should not happen";
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// reduce probability with every new run
|
|
||||||
++cnt;
|
|
||||||
res.probability /= 2;
|
|
||||||
|
|
||||||
// before trying again, modify distance and angle
|
|
||||||
//if (1 == 1) {
|
|
||||||
res.heading = params.heading + dHead.draw() * cnt;
|
|
||||||
realDist_m = params.distance_m * dDist.draw();// * cnt;
|
|
||||||
//}
|
|
||||||
|
|
||||||
// reached max retries?
|
|
||||||
if (cnt > 8) {
|
|
||||||
res.position = params.start; // reset
|
|
||||||
res.heading = params.heading; // reset
|
|
||||||
res.probability = 1e-50; // unlikely
|
|
||||||
return res;
|
|
||||||
} // did not work out....
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
throw "error";
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// const Node* _getFromPossibleWalks(Grid<Node>& grid, const GridPoint start, Control& ctrl, const float dist_m) {
|
|
||||||
|
|
||||||
// const Node* startNode = grid.getNodePtrFor(start);
|
|
||||||
|
|
||||||
// Heading desiredHeading = ctrl.heading;
|
|
||||||
|
|
||||||
// DrawList<Walk> weightedWalks;
|
|
||||||
|
|
||||||
// const Walks walks = Helper::getAllPossibleWalks(grid, startNode, dist_m);
|
|
||||||
// for (const Walk& walk : walks) {
|
|
||||||
// const double prob = eval(walk, desiredHeading, dist_m);
|
|
||||||
// weightedWalks.add(walk, prob);
|
|
||||||
// }
|
|
||||||
|
|
||||||
|
|
||||||
// Walk res = weightedWalks.get();
|
|
||||||
// const Node* dst = res.back();
|
|
||||||
// return dst;
|
|
||||||
|
|
||||||
// }
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
double evalDistance(const Walk& w, const float desiredDist) const {
|
|
||||||
const Node* nStart = w.front();
|
|
||||||
const Node* nEnd = w.back();
|
|
||||||
const float walkDist = nStart->inMeter().getDistance(nEnd->inMeter());
|
|
||||||
return Distribution::Normal<double>::getProbability(desiredDist, 0.1, walkDist);
|
|
||||||
}
|
|
||||||
|
|
||||||
double evalHeadingStartEnd(const Walk& w, const Heading desiredHeading) const {
|
|
||||||
const Node* nStart = w.front();
|
|
||||||
const Node* nEnd = w.back();
|
|
||||||
if (nStart == nEnd) {
|
|
||||||
std::cout << "warn! start == end" << std::endl;
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
Heading head(nStart->x_cm, nStart->y_cm, nEnd->x_cm, nEnd->y_cm);
|
|
||||||
const float diff = head.getDiffHalfRAD(desiredHeading);
|
|
||||||
return Distribution::Normal<double>::getProbability(0, 0.3, diff);
|
|
||||||
}
|
|
||||||
|
|
||||||
double evalHeadingChanges(const Walk& w) const {
|
|
||||||
Stats::Variance<float> var;
|
|
||||||
for (int i = 0; i < w.size()-2; ++i) {
|
|
||||||
const Node* n0 = w[i+0];
|
|
||||||
const Node* n1 = w[i+1];
|
|
||||||
const Node* n2 = w[i+2];
|
|
||||||
Heading h1(n0->x_cm, n0->y_cm, n1->x_cm, n1->y_cm);
|
|
||||||
Heading h2(n1->x_cm, n1->y_cm, n2->x_cm, n2->y_cm);
|
|
||||||
const float diff = h1.getDiffHalfRAD(h2);
|
|
||||||
var.add(diff);
|
|
||||||
}
|
|
||||||
const float totalVar = var.get();
|
|
||||||
return Distribution::Normal<double>::getProbability(0, 0.3, totalVar);
|
|
||||||
}
|
|
||||||
|
|
||||||
double eval(const Walk& w, const Heading desiredHeading, const float desiredDistance) const {
|
|
||||||
|
|
||||||
return 1.0
|
|
||||||
* evalHeadingStartEnd(w, desiredHeading)
|
|
||||||
* evalDistance(w, desiredDistance)
|
|
||||||
// * evalHeadingChanges(w);
|
|
||||||
;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// Walk getRandomWalk2(Grid<Node>& grid, const Node* start, const float dist_m) const {
|
|
||||||
|
|
||||||
// Walk walk;
|
|
||||||
|
|
||||||
// float dist = 0;
|
|
||||||
|
|
||||||
// const Node* cur = start;
|
|
||||||
// while(true) {
|
|
||||||
|
|
||||||
// walk.push_back(cur);
|
|
||||||
// if (dist > dist_m) {break;}
|
|
||||||
|
|
||||||
// const int numNeighbors = cur->getNumNeighbors();
|
|
||||||
// //std::cout << "neighbors: " << numNeighbors << std::endl;
|
|
||||||
// int idx = rand() % numNeighbors;
|
|
||||||
// const Node* next = &grid.getNeighbor(*cur, idx);
|
|
||||||
// dist += next->inMeter().getDistance(cur->inMeter());
|
|
||||||
// cur = next;
|
|
||||||
|
|
||||||
// }
|
|
||||||
|
|
||||||
// return walk;
|
|
||||||
|
|
||||||
// }
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
#endif // GRIDWALKER3_H
|
|
||||||
133
grid/walk/v3/Helper.h
Normal file
133
grid/walk/v3/Helper.h
Normal file
@@ -0,0 +1,133 @@
|
|||||||
|
#ifndef INDOOR_GW3_HELPER_H
|
||||||
|
#define INDOOR_GW3_HELPER_H
|
||||||
|
|
||||||
|
#include "../../../nav/dijkstra/Dijkstra.h"
|
||||||
|
#include "../../Grid.h"
|
||||||
|
|
||||||
|
#include "Structs.h"
|
||||||
|
|
||||||
|
#include <set>
|
||||||
|
|
||||||
|
namespace GW3 {
|
||||||
|
|
||||||
|
template <typename Node> class Helper {
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
static GridPoint p3ToGp(const Point3 p) {
|
||||||
|
const Point3 p100 = p*100;
|
||||||
|
return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) );
|
||||||
|
}
|
||||||
|
|
||||||
|
static Point3 gpToP3(const GridPoint gp) {
|
||||||
|
return Point3(gp.x_cm / 100.0f, gp.y_cm / 100.0f, gp.z_cm / 100.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
/** does the given grid-node contain the provided point-in-question? */
|
||||||
|
static bool contains(const Grid<Node>& grid, const Node* n, Point2 pt) {
|
||||||
|
const float gridSize_m = grid.getGridSize_cm() / 100.0f;
|
||||||
|
const float d = gridSize_m / 2.0f;
|
||||||
|
const Point2 pMin = n->inMeter().xy() - Point2(d, d);
|
||||||
|
const Point2 pMax = n->inMeter().xy() + Point2(d, d);
|
||||||
|
const BBox2 bbox(pMin, pMax);
|
||||||
|
return bbox.contains(pt);
|
||||||
|
}
|
||||||
|
|
||||||
|
/** does one of the given grid-nodes contains the provided point-in-question? */
|
||||||
|
static const Node* contains(const Grid<Node>& grid, const Nodes<Node>& nodes, Point2 pt) {
|
||||||
|
for (const Node* n : nodes) {
|
||||||
|
if (contains(grid, n, pt)) {return n;}
|
||||||
|
}
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
/** get all possible walks from start within a given region */
|
||||||
|
static Walks<Node> getAllPossibleWalks(Grid<Node>& grid, const Node* start, const float dist_m) {
|
||||||
|
|
||||||
|
struct Access {
|
||||||
|
Grid<Node>& grid;
|
||||||
|
Access(Grid<Node>& grid) : grid(grid) {;}
|
||||||
|
int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();}
|
||||||
|
Node* getNeighbor(const Node& n, const int idx) const {return &grid.getNeighbor(n, idx);}
|
||||||
|
float getWeightBetween(const Node& n1, const Node& n2) const {return n1.inMeter().getDistance(n2.inMeter());}
|
||||||
|
} acc(grid);
|
||||||
|
|
||||||
|
const float addDist_m = grid.getGridSize_cm() / 100.0f;
|
||||||
|
const float maxDist_m = dist_m * 1.1 + addDist_m;
|
||||||
|
Dijkstra<Node> dijkstra;
|
||||||
|
dijkstra.build(start, nullptr, acc, maxDist_m);
|
||||||
|
|
||||||
|
const std::unordered_map<const Node*, DijkstraNode<Node>*>& nodes = dijkstra.getNodes();
|
||||||
|
|
||||||
|
Walks<Node> walks;
|
||||||
|
|
||||||
|
for (const auto& it : nodes) {
|
||||||
|
Walk<Node> walk;
|
||||||
|
DijkstraNode<Node>* node = it.second;
|
||||||
|
do {
|
||||||
|
const Node* gridNode = node->element;
|
||||||
|
walk.insert(walk.begin(), gridNode); // push_front() as dijkstra is inverted
|
||||||
|
node = node->previous;
|
||||||
|
} while (node);
|
||||||
|
walks.push_back(walk);
|
||||||
|
}
|
||||||
|
|
||||||
|
return walks;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/** get all reachable nodes that are within a given range */
|
||||||
|
static Nodes<Node> getAllReachableNodes(Grid<Node>& grid, const Node* start, const float dist_m) {
|
||||||
|
|
||||||
|
Nodes<Node> res;
|
||||||
|
std::unordered_map<uint32_t, float> distances;
|
||||||
|
std::vector<uint32_t> toVisit;
|
||||||
|
|
||||||
|
toVisit.push_back(start->getIdx());
|
||||||
|
distances[start->getIdx()] = 0.0f;
|
||||||
|
|
||||||
|
while (!toVisit.empty()) {
|
||||||
|
|
||||||
|
int curIdx = toVisit.front();
|
||||||
|
toVisit.erase(toVisit.begin());
|
||||||
|
|
||||||
|
const Node& curNode = grid[curIdx];
|
||||||
|
const float curDistance = distances[curIdx];
|
||||||
|
res.push_back(&curNode); // remember for output
|
||||||
|
|
||||||
|
if (curDistance <= dist_m) {
|
||||||
|
for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
|
||||||
|
|
||||||
|
const int neighborIdx = curNode.getNeighborIdx(i);
|
||||||
|
const Node& neighbor = grid[neighborIdx];
|
||||||
|
const float addDist = neighbor.inMeter().getDistance(curNode.inMeter());
|
||||||
|
const float totalDist = curDistance + addDist;
|
||||||
|
|
||||||
|
// this is like in dijkstra. keep the smallest distance to reach a node:
|
||||||
|
|
||||||
|
// not yet reached -> store distance
|
||||||
|
if (distances.find(neighborIdx) == distances.end()) {
|
||||||
|
toVisit.push_back(neighborIdx);
|
||||||
|
distances[neighborIdx] = totalDist;
|
||||||
|
|
||||||
|
// reached earlier but found shorter way
|
||||||
|
} else {
|
||||||
|
if (distances[neighborIdx] > totalDist) {
|
||||||
|
distances[neighborIdx] = totalDist;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
return res;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif // INDOOR_GW3_HELPER_H
|
||||||
43
grid/walk/v3/Structs.h
Normal file
43
grid/walk/v3/Structs.h
Normal file
@@ -0,0 +1,43 @@
|
|||||||
|
#ifndef INDOOR_GW3_STRUCTS_H
|
||||||
|
#define INDOOR_GW3_STRUCTS_H
|
||||||
|
|
||||||
|
#include "../../../geo/Heading.h"
|
||||||
|
#include "../../../geo/Point3.h"
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
namespace GW3 {
|
||||||
|
|
||||||
|
/** paremters for the walk */
|
||||||
|
struct WalkParams {
|
||||||
|
Point3 start;
|
||||||
|
float distance_m;
|
||||||
|
Heading heading = Heading(0);
|
||||||
|
};
|
||||||
|
|
||||||
|
/** result of the random walk */
|
||||||
|
struct WalkResult {
|
||||||
|
Point3 position;
|
||||||
|
Heading heading = Heading(0);
|
||||||
|
double probability = 1.0;
|
||||||
|
};
|
||||||
|
|
||||||
|
/** several nodes */
|
||||||
|
template <typename Node> struct Nodes : public std::vector<const Node*> {
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
/** one walk along several nodes */
|
||||||
|
template <typename Node> struct Walk : public std::vector<const Node*> {
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
/** several walks */
|
||||||
|
template <typename Node> struct Walks : public std::vector<Walk<Node>> {
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif // INDOOR_GW3_STRUCTS_H
|
||||||
91
grid/walk/v3/WalkEvaluator.h
Normal file
91
grid/walk/v3/WalkEvaluator.h
Normal file
@@ -0,0 +1,91 @@
|
|||||||
|
#ifndef INDOOR_GW3_WALKEVALUATOR_H
|
||||||
|
#define INDOOR_GW3_WALKEVALUATOR_H
|
||||||
|
|
||||||
|
#include "Structs.h"
|
||||||
|
#include "Helper.h"
|
||||||
|
|
||||||
|
#include "../../../math/Distributions.h"
|
||||||
|
#include "../../../grid/Grid.h"
|
||||||
|
|
||||||
|
namespace GW3 {
|
||||||
|
|
||||||
|
/** interface for all evaluators that return a probability for a given walk */
|
||||||
|
template <typename Node> class WalkEvaluator {
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
/** get the probability for the given walk */
|
||||||
|
//virtual double getProbability(const Walk<Node>& walk) const = 0;
|
||||||
|
|
||||||
|
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const = 0;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
/** evaluate the grid-node-importance importance(end) */
|
||||||
|
template <typename Node> class WalkEvalEndNodeProbability : public WalkEvaluator<Node> {
|
||||||
|
|
||||||
|
Grid<Node>* grid;
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
WalkEvalEndNodeProbability(Grid<Node>* grid) : grid(grid) {;}
|
||||||
|
|
||||||
|
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
|
||||||
|
|
||||||
|
(void) params;
|
||||||
|
(void) pStart;
|
||||||
|
|
||||||
|
const GridPoint gp = Helper<Node>::p3ToGp(pEnd);
|
||||||
|
const Node& node = grid->getNodeFor(gp);
|
||||||
|
const double p = node.getWalkImportance();
|
||||||
|
return std::pow(p,10);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/** evaluate the difference between head(start,end) and the requested heading */
|
||||||
|
template <typename Node> class WalkEvalHeadingStartEnd : public WalkEvaluator<Node> {
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
|
||||||
|
|
||||||
|
(void) params;
|
||||||
|
|
||||||
|
if (pStart == pEnd) {
|
||||||
|
std::cout << "warn! start-position == end-positon" << std::endl;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
const Heading head(pStart.xy(), pEnd.xy());
|
||||||
|
const float diff = head.getDiffHalfRAD(params.heading);
|
||||||
|
//const float diff = Heading::getSignedDiff(params.heading, head);
|
||||||
|
return Distribution::Normal<double>::getProbability(0, 0.04, diff);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
/** evaluate the difference between distance(start, end) and the requested distance */
|
||||||
|
template <typename Node> class WalkEvalDistance : public WalkEvaluator<Node> {
|
||||||
|
|
||||||
|
const double sigma = 0.1f;
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
|
||||||
|
|
||||||
|
const float walkedDistance_m = pStart.getDistance(pEnd);
|
||||||
|
return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif // INDOOR_GW3_WALKEVALUATOR_H
|
||||||
20
grid/walk/v3/WalkGenerator.h
Normal file
20
grid/walk/v3/WalkGenerator.h
Normal file
@@ -0,0 +1,20 @@
|
|||||||
|
#ifndef INDOOR_GRIDWALKER3GENERATOR_H
|
||||||
|
#define INDOOR_GRIDWALKER3GENERATOR_H
|
||||||
|
|
||||||
|
#include "Structs.h"
|
||||||
|
|
||||||
|
|
||||||
|
namespace GW3 {
|
||||||
|
|
||||||
|
class GridWalker3Generator {
|
||||||
|
|
||||||
|
virtual Walk getPossibleWalks() = 0;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif // INDOOR_GRIDWALKER3GENERATOR_H
|
||||||
376
grid/walk/v3/Walker.h
Normal file
376
grid/walk/v3/Walker.h
Normal file
@@ -0,0 +1,376 @@
|
|||||||
|
#ifndef INDOOR_GW3_WALKER_H
|
||||||
|
#define INDOOR_GW3_WALKER_H
|
||||||
|
|
||||||
|
#include "../../../data/Timestamp.h"
|
||||||
|
#include "../../../math/DrawList.h"
|
||||||
|
#include "../../../math/Distributions.h"
|
||||||
|
#include "../../../math/Stats.h"
|
||||||
|
#include "../../../geo/Heading.h"
|
||||||
|
#include "../../../math/stats/Variance.h"
|
||||||
|
#include "../../../geo/BBox2.h"
|
||||||
|
|
||||||
|
#include "../../Grid.h"
|
||||||
|
|
||||||
|
#include "Helper.h"
|
||||||
|
#include "Structs.h"
|
||||||
|
#include "WalkEvaluator.h"
|
||||||
|
|
||||||
|
namespace GW3 {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* modular grid-walker that takes various sub-components to determine
|
||||||
|
* p(e) and thus randomly pick edges
|
||||||
|
*/
|
||||||
|
template <typename Node> class WalkerBase {
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
/** get a new destination for the given start */
|
||||||
|
virtual const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const = 0;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
template <typename Node> class WalkerDirectDestination : public WalkerBase<Node> {
|
||||||
|
|
||||||
|
//RandomGenerator rnd;
|
||||||
|
|
||||||
|
std::vector<WalkEvaluator<Node>*> evals;
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
/** make the code a little more readable */
|
||||||
|
using Helper = GW3::Helper<Node>;
|
||||||
|
using Walk = typename GW3::Walk<Node>;
|
||||||
|
using Walks = typename GW3::Walks<Node>;
|
||||||
|
using Nodes = typename GW3::Nodes<Node>;
|
||||||
|
|
||||||
|
/** add a new evaluator to the walker */
|
||||||
|
void addEvaluator(WalkEvaluator<Node>* eval) {
|
||||||
|
this->evals.push_back(eval);
|
||||||
|
}
|
||||||
|
|
||||||
|
/** perform the walk based on the configured setup */
|
||||||
|
const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const override {
|
||||||
|
|
||||||
|
static std::mt19937 rndGen;
|
||||||
|
|
||||||
|
const GridPoint gpStart = Helper::p3ToGp(params.start);
|
||||||
|
const Node* startNode = grid.getNodePtrFor(gpStart);
|
||||||
|
|
||||||
|
// include one additional grid-cell (increased distance)
|
||||||
|
const float secBuffer_m = grid.getGridSize_cm() / 100.0f;
|
||||||
|
const float range_m = params.distance_m + secBuffer_m;
|
||||||
|
const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m);
|
||||||
|
|
||||||
|
WalkResult res;
|
||||||
|
res.heading = params.heading;
|
||||||
|
res.position = params.start;
|
||||||
|
float realDist_m = params.distance_m;
|
||||||
|
|
||||||
|
|
||||||
|
const Point2 dir = res.heading.asVector();
|
||||||
|
const Point2 dst = params.start.xy() + (dir * realDist_m);
|
||||||
|
|
||||||
|
// is dst reachable?
|
||||||
|
const Node* n = Helper::contains(grid, reachableNodes, dst);
|
||||||
|
if (n) {
|
||||||
|
|
||||||
|
const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
|
||||||
|
const GridPoint gp = Helper::p3ToGp(p3);
|
||||||
|
|
||||||
|
if (grid.hasNodeFor(gp)) {
|
||||||
|
res.position = p3; // update position
|
||||||
|
res.heading; // keep as-is
|
||||||
|
res.probability; // keep as-is
|
||||||
|
return res; // done
|
||||||
|
} else {
|
||||||
|
std::cout << "WARN dst not found" << std::endl;
|
||||||
|
//throw "should not happen";
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// not found -> try random pick among all reachable nodes
|
||||||
|
const float gridSize_m = grid.getGridSize_cm() / 100.0f;
|
||||||
|
std::uniform_int_distribution<int> dNode(0, (int)reachableNodes.size() - 1);
|
||||||
|
|
||||||
|
const Node* dstNode = reachableNodes[dNode(rndGen)];
|
||||||
|
|
||||||
|
// random position within destination-node
|
||||||
|
std::uniform_real_distribution<float> dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f);
|
||||||
|
const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0);
|
||||||
|
|
||||||
|
const Point3 start = params.start;
|
||||||
|
const Point3 end = Helper::gpToP3(*dstNode) + dstOffset;
|
||||||
|
|
||||||
|
double p = 1;
|
||||||
|
for (const WalkEvaluator<Node>* eval : evals) {
|
||||||
|
const double p1 = eval->getProbability(start, end, params);
|
||||||
|
p *= p1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (start == end) {
|
||||||
|
res.probability = 0;
|
||||||
|
return res;
|
||||||
|
}
|
||||||
|
|
||||||
|
res.heading = Heading(start.xy(), end.xy());
|
||||||
|
res.probability = p;
|
||||||
|
res.position = end;
|
||||||
|
return res;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* 1) get all reachable nodes from "start" (within a certain distance)
|
||||||
|
* 2) randomly pick one of em
|
||||||
|
* 3) scatter positon within the node's square -> end position
|
||||||
|
* 4) evaluate the probability of walking from start -> end
|
||||||
|
*/
|
||||||
|
template <typename Node> class WalkerWeightedRandom : public WalkerBase<Node> {
|
||||||
|
|
||||||
|
std::vector<WalkEvaluator<Node>*> evals;
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
/** make the code a little more readable */
|
||||||
|
using Helper = GW3::Helper<Node>;
|
||||||
|
using Walk = typename GW3::Walk<Node>;
|
||||||
|
using Walks = typename GW3::Walks<Node>;
|
||||||
|
using Nodes = typename GW3::Nodes<Node>;
|
||||||
|
|
||||||
|
/** add a new evaluator to the walker */
|
||||||
|
void addEvaluator(WalkEvaluator<Node>* eval) {
|
||||||
|
this->evals.push_back(eval);
|
||||||
|
}
|
||||||
|
|
||||||
|
/** perform the walk based on the configured setup */
|
||||||
|
const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const override {
|
||||||
|
|
||||||
|
static std::minstd_rand rndGen;
|
||||||
|
|
||||||
|
const GridPoint gpStart = Helper::p3ToGp(params.start);
|
||||||
|
const Node* startNode = grid.getNodePtrFor(gpStart);
|
||||||
|
|
||||||
|
// include one additional grid-cell (increased distance)
|
||||||
|
const float secBuffer_m = grid.getGridSize_cm() / 100.0f;
|
||||||
|
const float range_m = params.distance_m + secBuffer_m;
|
||||||
|
const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m);
|
||||||
|
|
||||||
|
// not found -> try random pick among all reachable nodes
|
||||||
|
const float gridSize_m = grid.getGridSize_cm() / 100.0f;
|
||||||
|
std::uniform_int_distribution<int> dNode(0, (int)reachableNodes.size() - 1);
|
||||||
|
|
||||||
|
|
||||||
|
Point3 best;
|
||||||
|
double bestP = 0;
|
||||||
|
|
||||||
|
const Point3 start = params.start;
|
||||||
|
|
||||||
|
// try X random destinations, evaluate them, remember the best one (reduces the number of "stupid particles")
|
||||||
|
for (int i = 0; i < 25; ++i) {
|
||||||
|
|
||||||
|
const Node* dstNode = reachableNodes[dNode(rndGen)];
|
||||||
|
|
||||||
|
// random position within destination-node
|
||||||
|
std::uniform_real_distribution<float> dFinal(-gridSize_m*0.485f, +gridSize_m*0.4585f);
|
||||||
|
const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0);
|
||||||
|
|
||||||
|
// destination = node-center + offset (within the node's bbox)
|
||||||
|
const Point3 end = Helper::gpToP3(*dstNode) + dstOffset;
|
||||||
|
|
||||||
|
// sanity check
|
||||||
|
Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid");
|
||||||
|
|
||||||
|
if (start == end) {continue;}
|
||||||
|
|
||||||
|
double p = 1;
|
||||||
|
for (const WalkEvaluator<Node>* eval : evals) {
|
||||||
|
const double p1 = eval->getProbability(start, end, params);
|
||||||
|
p *= p1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (p > bestP) {bestP = p; best = end;}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
//const Point3 end = lst.get();
|
||||||
|
const Point3 end = best;
|
||||||
|
WalkResult res;
|
||||||
|
if (start == end) {
|
||||||
|
res.probability = 0;
|
||||||
|
} else {
|
||||||
|
res.heading = Heading(start.xy(), end.xy());
|
||||||
|
res.probability = bestP; // 1
|
||||||
|
}
|
||||||
|
res.position = end;
|
||||||
|
return res;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
//const WalkResult _drawThenCheck(Grid<Node>& grid, const WalkParams& params) {
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// Distribution::Normal<float> dDist(1, 0.02);
|
||||||
|
// Distribution::Normal<float> dHead(0, 0.01);
|
||||||
|
|
||||||
|
// int cnt = 0;
|
||||||
|
// while(true) {
|
||||||
|
|
||||||
|
// const Point2 dir = res.heading.asVector();
|
||||||
|
// const Point2 dst = params.start.xy() + (dir * realDist_m);
|
||||||
|
|
||||||
|
// // is dst reachable?
|
||||||
|
// const Node* n = Helper::contains(grid, reachableNodes, dst);
|
||||||
|
// if (n) {
|
||||||
|
|
||||||
|
// const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
|
||||||
|
// const GridPoint gp = Helper::p3ToGp(p3);
|
||||||
|
|
||||||
|
// if (grid.hasNodeFor(gp)) {
|
||||||
|
// res.position = p3; // update position
|
||||||
|
// res.heading; // keep as-is
|
||||||
|
// res.probability; // keep as-is
|
||||||
|
// return res; // done
|
||||||
|
// } else {
|
||||||
|
// std::cout << "WARN dst not found" << std::endl;
|
||||||
|
// //throw "should not happen";
|
||||||
|
// }
|
||||||
|
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // reduce probability with every new run
|
||||||
|
// ++cnt;
|
||||||
|
// res.probability /= 2;
|
||||||
|
|
||||||
|
// // before trying again, modify distance and angle
|
||||||
|
// //if (1 == 1) {
|
||||||
|
// res.heading = params.heading + dHead.draw() * cnt;
|
||||||
|
// realDist_m = params.distance_m * dDist.draw();// * cnt;
|
||||||
|
// //}
|
||||||
|
|
||||||
|
// // reached max retries?
|
||||||
|
// if (cnt > 8) {
|
||||||
|
// res.position = params.start; // reset
|
||||||
|
// res.heading = params.heading; // reset
|
||||||
|
// res.probability = 1e-50; // unlikely
|
||||||
|
// return res;
|
||||||
|
// } // did not work out....
|
||||||
|
|
||||||
|
// }
|
||||||
|
|
||||||
|
// throw "error";
|
||||||
|
|
||||||
|
//}
|
||||||
|
|
||||||
|
|
||||||
|
// const Node* _getFromPossibleWalks(Grid<Node>& grid, const GridPoint start, Control& ctrl, const float dist_m) {
|
||||||
|
|
||||||
|
// const Node* startNode = grid.getNodePtrFor(start);
|
||||||
|
|
||||||
|
// Heading desiredHeading = ctrl.heading;
|
||||||
|
|
||||||
|
// DrawList<Walk> weightedWalks;
|
||||||
|
|
||||||
|
// const Walks walks = Helper::getAllPossibleWalks(grid, startNode, dist_m);
|
||||||
|
// for (const Walk& walk : walks) {
|
||||||
|
// const double prob = eval(walk, desiredHeading, dist_m);
|
||||||
|
// weightedWalks.add(walk, prob);
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
// Walk res = weightedWalks.get();
|
||||||
|
// const Node* dst = res.back();
|
||||||
|
// return dst;
|
||||||
|
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// double evalDistance(const Walk& w, const float desiredDist) const {
|
||||||
|
// const Node* nStart = w.front();
|
||||||
|
// const Node* nEnd = w.back();
|
||||||
|
// const float walkDist = nStart->inMeter().getDistance(nEnd->inMeter());
|
||||||
|
// return Distribution::Normal<double>::getProbability(desiredDist, 0.1, walkDist);
|
||||||
|
// }
|
||||||
|
|
||||||
|
// double evalHeadingStartEnd(const Walk& w, const Heading desiredHeading) const {
|
||||||
|
// const Node* nStart = w.front();
|
||||||
|
// const Node* nEnd = w.back();
|
||||||
|
// if (nStart == nEnd) {
|
||||||
|
// std::cout << "warn! start == end" << std::endl;
|
||||||
|
// return 0;
|
||||||
|
// }
|
||||||
|
// Heading head(nStart->x_cm, nStart->y_cm, nEnd->x_cm, nEnd->y_cm);
|
||||||
|
// const float diff = head.getDiffHalfRAD(desiredHeading);
|
||||||
|
// return Distribution::Normal<double>::getProbability(0, 0.3, diff);
|
||||||
|
// }
|
||||||
|
|
||||||
|
// double evalHeadingChanges(const Walk& w) const {
|
||||||
|
// Stats::Variance<float> var;
|
||||||
|
// for (int i = 0; i < w.size()-2; ++i) {
|
||||||
|
// const Node* n0 = w[i+0];
|
||||||
|
// const Node* n1 = w[i+1];
|
||||||
|
// const Node* n2 = w[i+2];
|
||||||
|
// Heading h1(n0->x_cm, n0->y_cm, n1->x_cm, n1->y_cm);
|
||||||
|
// Heading h2(n1->x_cm, n1->y_cm, n2->x_cm, n2->y_cm);
|
||||||
|
// const float diff = h1.getDiffHalfRAD(h2);
|
||||||
|
// var.add(diff);
|
||||||
|
// }
|
||||||
|
// const float totalVar = var.get();
|
||||||
|
// return Distribution::Normal<double>::getProbability(0, 0.3, totalVar);
|
||||||
|
// }
|
||||||
|
|
||||||
|
// double eval(const Walk& w, const Heading desiredHeading, const float desiredDistance) const {
|
||||||
|
|
||||||
|
// return 1.0
|
||||||
|
// * evalHeadingStartEnd(w, desiredHeading)
|
||||||
|
// * evalDistance(w, desiredDistance)
|
||||||
|
// // * evalHeadingChanges(w);
|
||||||
|
// ;
|
||||||
|
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// Walk getRandomWalk2(Grid<Node>& grid, const Node* start, const float dist_m) const {
|
||||||
|
|
||||||
|
// Walk walk;
|
||||||
|
|
||||||
|
// float dist = 0;
|
||||||
|
|
||||||
|
// const Node* cur = start;
|
||||||
|
// while(true) {
|
||||||
|
|
||||||
|
// walk.push_back(cur);
|
||||||
|
// if (dist > dist_m) {break;}
|
||||||
|
|
||||||
|
// const int numNeighbors = cur->getNumNeighbors();
|
||||||
|
// //std::cout << "neighbors: " << numNeighbors << std::endl;
|
||||||
|
// int idx = rand() % numNeighbors;
|
||||||
|
// const Node* next = &grid.getNeighbor(*cur, idx);
|
||||||
|
// dist += next->inMeter().getDistance(cur->inMeter());
|
||||||
|
// cur = next;
|
||||||
|
|
||||||
|
// }
|
||||||
|
|
||||||
|
// return walk;
|
||||||
|
|
||||||
|
// }
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif // INDOOR_GW3_WALKER_H
|
||||||
Reference in New Issue
Block a user