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

added new sanity checks and compile-time assertions to prevent errors

Browse Source 
fixed stair-building issue
new test-cases
added elevator support
fixed/improved some walker modules
This commit is contained in:
FrankE
2016-09-10 15:12:39 +02:00
parent 7baeecb3f9
commit 82f8828a04
26 changed files with 996 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
grid/Grid.h
View File

@@ -108,9 +108,9 @@ public:
void connectUniDir(T& n1, const T& n2) {
Assert::isFalse(n1.hasNeighbor(n2._idx), "this neighbor is already connected"); // already connected?
Assert::notEqual(n1.getIdx(), n2.getIdx(), "can not connect node with itself");
Assert::isFalse(n1.fullyConnected(), "this node has already reached its neighbor-limit");
n1._neighbors[n1._numNeighbors] = n2._idx;
++n1._numNeighbors;
Assert::isBetween(n1._numNeighbors, (uint8_t) 0, (uint8_t) 10, "number of neighbors out of bounds!");
}
/**

8
grid/GridNode.h
View File

@@ -18,6 +18,10 @@ template<typename> class Grid;
*/
struct GridNode {
private:
static int constexpr MAX_NEIGHBORS = 10;
private:
/** grant full access to the grid */
@@ -27,7 +31,7 @@ private:
int _idx;
/** INTERNAL: store neighbors (via index) */
int _neighbors[10];
int _neighbors[MAX_NEIGHBORS];
/** INTERNAL: number of neighbors */
uint8_t _numNeighbors;
@@ -61,7 +65,7 @@ public:
int getNumNeighbors() const {return _numNeighbors;}
/** reached neighbor limit? */
bool fullyConnected() const {return _numNeighbors >= 10;}
bool fullyConnected() const {return _numNeighbors >= MAX_NEIGHBORS;}
/** is this node connected to the given index? */
bool hasNeighbor(const int idx) const {

2
grid/GridPoint.h
View File

@@ -37,6 +37,8 @@ struct GridPoint {
GridPoint operator * (const float f) const {return GridPoint(x_cm*f, y_cm*f, z_cm*f);}
GridPoint operator + (const GridPoint& o) const {return GridPoint(x_cm+o.x_cm, y_cm+o.y_cm, z_cm+o.z_cm);}
GridPoint& operator += (const GridPoint& o) {x_cm += o.x_cm; y_cm += o.y_cm; z_cm += o.z_cm; return *this;}
GridPoint& operator /= (const float f) {x_cm /= f; y_cm /= f; z_cm /= f; return *this;}

99
grid/factory/v2/Elevators.h Normal file
View File

@@ -0,0 +1,99 @@
#ifndef GRID_ELEVATORS_H
#define GRID_ELEVATORS_H
#include <vector>
#include <set>
#include "../../Grid.h"
#include "Helper.h"
#include "../../../floorplan/v2/Floorplan.h"
#include <fstream>
template <typename T> class Elevators {
private:
/** the grid to build into */
Grid<T>& grid;
/** calculation helper */
Helper<T> helper;
public:
/** ctor */
Elevators(Grid<T>& grid) : grid(grid), helper(grid) {
;
}
~Elevators() {
;
}
/** build the given elevator */
void build(const Floorplan::Floor* floor, const Floorplan::Elevator* elevator) {
const int gs_cm = grid.getGridSize_cm();
struct IntPos {
int x_cm;
int y_cm;
IntPos(int x_cm, int y_cm) : x_cm(x_cm), y_cm(y_cm) {;}
};
// identify all grid-aligned nodes that belong to the elevator
std::vector<IntPos> nodesWithin;
const HelperPoly poly(elevator->getPoints());
auto callback = [&] (const int x_cm, const int y_cm) {
const GridPoint gp1(x_cm, y_cm, floor->getStartingZ()*100); // starting floor
const GridPoint gp2(x_cm, y_cm, floor->getEndingZ()*100); // the floor above
// ensure such a node is present in both floors (and thus a connection is possible)
if (grid.hasNodeFor(gp1) && grid.hasNodeFor(gp2)) {
nodesWithin.push_back(IntPos(x_cm, y_cm));
}
};
poly.forEachGridPoint(gs_cm, callback);
// now create the interconnection in z-direction
const int z1_cm = std::ceil((floor->getStartingZ()*100+1) / gs_cm) * gs_cm; // the next node above the current flor
const int z2_cm = std::floor((floor->getEndingZ()*100-1) / gs_cm) * gs_cm; // the last node below the next floor
for (const IntPos nodePos : nodesWithin) {
// create nodes BETWEEN the two floors (skip the floors themselves! -> floor1+gridSize <-> floor2-gridSize
for (int z_cm = z1_cm; z_cm <= z2_cm; z_cm += gs_cm) {
const GridPoint gp1(nodePos.x_cm, nodePos.y_cm, z_cm); // the to-be-added node
Assert::isFalse(grid.hasNodeFor(gp1), "elevator collission"); // such a node must not yet exist! otherwise we e.g. collide with a stari
const int idx = grid.add(T(gp1.x_cm, gp1.y_cm, gp1.z_cm)); // create the node
grid[idx].setType(GridNode::TYPE_ELEVATOR); // set the node-type
}
// connect each of the new nodes with the node below it. NOW ALSO EXAMINE THE floor above (z2_cm + gs_cm)
for (int z_cm = z1_cm; z_cm <= z2_cm + gs_cm; z_cm += gs_cm) {
const GridPoint gpBelow(nodePos.x_cm, nodePos.y_cm, z_cm-gs_cm);
const GridPoint gp(nodePos.x_cm, nodePos.y_cm, z_cm);
Assert::isTrue(grid.hasNodeFor(gpBelow), "missing node");
Assert::isTrue(grid.hasNodeFor(gp), "missing node");
T& n1 = (T&) grid.getNodeFor(gpBelow);
T& n2 = (T&) grid.getNodeFor(gp);
grid.connectBiDir(n1, n2);
}
}
}
};
#endif // GRID_ELEVATORS_H

102
grid/factory/v2/GridFactory.h
View File

@@ -8,7 +8,8 @@
#include "../../../floorplan/v2/Floorplan.h"
#include "Helper.h"
#include "Stairs.h"
#include "Stairs2.h"
#include "Elevators.h"
#include "../../../geo/Units.h"
#include "../../GridNodeBBox.h"
@@ -37,6 +38,9 @@ private:
/** stair builder */
Stairs<T> stairs;
/** elevator builder */
Elevators<T> elevators;
bool _buildStairs = true;
bool _removeIsolated = true;
@@ -44,7 +48,9 @@ private:
public:
/** ctor with the grid to fill */
GridFactory(Grid<T>& grid) : grid(grid), helper(grid), stairs(grid) {;}
explicit GridFactory(Grid<T>& grid) : grid(grid), helper(grid), stairs(grid), elevators(grid) {
}
/** whether or not to build stairs */
@@ -59,7 +65,7 @@ public:
Log::add(name, "building grid from IndoorMap", true);
const int total = map->floors.size()*2 + 1;
const int total = map->floors.size()*3 + 1;
int cur = 0;
// build all the floors
@@ -78,6 +84,15 @@ public:
}
}
// build all elevators
if (listener) {listener->onGridBuildUpdateMajor("adding elevators");}
if (_buildStairs) {
for (Floorplan::Floor* f : map->floors) {
buildElevators(f, listener);
if (listener) {listener->onGridBuildUpdateMajor(total, ++cur);}
}
}
// remove isolated nodes
if (_removeIsolated) {
if (listener) {listener->onGridBuildUpdateMajor("removing isolated nodes");}
@@ -180,78 +195,23 @@ public:
}
void buildElevators(const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) {
const int total = floor->elevators.size();
int cur = 0;
// process each elevator within the floor
for (const Floorplan::Elevator* elevator : floor->elevators) {
if (listener) {listener->onGridBuildUpdateMinor("adding " + floor->name + " elevator " + std::to_string(cur+1));}
elevators.build(floor, elevator);
if (listener) {listener->onGridBuildUpdateMinor(total, ++cur);}
}
}
// void addWithin(const Floorplan::Floor* floor, std::vector<Entry>& nodeList) {
// const int fz1_cm = floor->atHeight*100;
// const int fz2_cm = (floor->atHeight+floor->height)*100;
// for (Entry& e : nodeList) {
// if ( (e.pos.z_cm <= fz1_cm) || (e.pos.z_cm >= fz2_cm) ) {
// e.idx = grid.getNearestNode(e.pos).getIdx();
// e.part = {9999};
// } else {
// const T t(e.pos.x_cm, e.pos.y_cm, e.pos.z_cm);
// e.idx = grid.addUnaligned(t, false);
// }
// }
// }
// void filter(std::vector<Entry>& nodeList) {
// const int gs_cm = grid.getGridSize_cm();
// const int limit_cm = gs_cm * 0.50;
// // remove duplicate nodes or nodes, that are too close to each other
// for(auto it = nodeList.begin(); it != nodeList.end(); ) {
// // currently examined node
// const Entry& e1 = *it;
// // matches for nodes that are NOT the same instance AND nearby (< grid-size)
// auto matches = [&] (const Entry& e2) { return (e1.pos != e2.pos) && (e1.pos.getDistanceInCM(e2.pos) <= limit_cm); };
// auto match = std::find_if(nodeList.begin(), nodeList.end(), matches);
// // remove if this node has a nearby neighbor
// if (match != nodeList.end()) {
// // combine both nodes within one:
// // the node belongs to more than one stair-part
// Entry& e2 = *match;
// e2.part.insert(e2.part.end(), e1.part.begin(), e1.part.end());
// it = nodeList.erase(it);
// } else {
// ++it;
// }
// }
// }
// std::vector<Entry> minOnly(const std::vector<Entry>& lst) {
// auto comp = [&] (const Entry& a, const Entry& b) {return grid[a.idx].z_cm < grid[b.idx].z_cm;};
// auto it = std::min_element(lst.begin(), lst.end(), comp);
// T& min = grid[it->idx];
// std::vector<Entry> res;
// for (const Entry& e : lst) {
// if (grid[e.idx].z_cm == min.z_cm) {res.push_back(e);}
// }
// return res;
// }
/** connect all neighboring nodes part of the given index-vector */
void connectAdjacent(const std::vector<int>& indices) {

59
grid/factory/v2/Helper.h
View File

@@ -26,7 +26,12 @@ struct HelperPoly {
/** ctor from floorplan-quad */
HelperPoly(const Floorplan::Quad3& quad) {
add(quad.p1); add(quad.p2); add(quad.p3); add(quad.p4);
add(quad.p1*100); add(quad.p2*100); add(quad.p3*100); add(quad.p4*100);
}
/** ctor from floorplan-polygon */
HelperPoly(const Floorplan::Polygon2& poly) {
for (Point2 p : poly.points) { add(p * 100); }
}
void add(const Point2 p) {
@@ -67,6 +72,30 @@ struct HelperPoly {
}
/** call a user-function for each GRID-ALIGNED point within the polygon */
void forEachGridPoint(const int gridSize_cm, std::function<void(int x_cm, int y_cm)> callback) const {
int x1 = std::floor(bbox_cm.getMin().x / gridSize_cm) * gridSize_cm;
int x2 = std::ceil(bbox_cm.getMax().x / gridSize_cm) * gridSize_cm;
int y1 = std::floor(bbox_cm.getMin().y / gridSize_cm) * gridSize_cm;
int y2 = std::ceil(bbox_cm.getMax().y / gridSize_cm) * gridSize_cm;
// process each point within the (aligned) bbox
for (int y = y1; y <= y2; y += gridSize_cm) {
for (int x = x1; x <= x2; x += gridSize_cm) {
// does this point belong to the polygon?
if (!contains(Point2(x,y))) {continue;}
// call the callback
callback(x,y);
}
}
}
};
@@ -94,7 +123,7 @@ public:
}
/** connect the given node to all its neighbors */
/** connect the given node to all its neighbors (x,y) */
void connectToNeighbors(T& n1) {
const int gs_cm = grid.getGridSize_cm();
@@ -120,6 +149,32 @@ public:
}
/** connect the given node to all its neighbors )x,y,z) */
void connectToNeighborsXYZ(T& n1) {
const int gs_cm = grid.getGridSize_cm();
for (int z = -gs_cm; z <= +gs_cm; z += gs_cm) {
for (int y = -gs_cm; y <= +gs_cm; y += gs_cm) {
for (int x = -gs_cm; x <= +gs_cm; x += gs_cm) {
// skip the node itself
if (x == 0 && y == 0 && z == 0) {continue;}
// try to find a matching neighbor
const GridPoint gp(n1.x_cm + x, n1.y_cm + y, n1.z_cm + z);
const T* n2 = grid.getNodePtrFor(gp);
if (!n2) {continue;}
// connect
if (n1.hasNeighbor(n2->getIdx())) {continue;}
grid.connectUniDir(n1, *n2);
}
}
}
}
int gridSize() const {
return grid.getGridSize_cm();;

5
grid/factory/v2/Importance.h
View File

@@ -72,6 +72,7 @@ public:
switch(n1.getType()) {
case GridNode::TYPE_DOOR: doors.push_back(n1); break;
case GridNode::TYPE_STAIR: stairs.push_back(n1); break;
case GridNode::TYPE_ELEVATOR: stairs.push_back(n1); break;
}
}
@@ -87,7 +88,7 @@ public:
// probability adjustments
Distribution::Normal<float> avoidWalls(0.0, 0.35);
Distribution::Normal<float> favorDoors(0.0f, 0.5f);
Distribution::Normal<float> favorStairs(0.0f, 3.5f);
Distribution::Normal<float> favorStairs(0.0f, 1.5f);
if (l) {
l->onGridBuildUpdateMajor(2, 1);
@@ -124,7 +125,7 @@ public:
// final probability
n1.navImportance = 1.0f;
n1.navImportance += favorDoors.getProbability(distToDoor_m) * 1.25f;
n1.navImportance += favorStairs.getProbability(distToStair_m) * 2.5f;
n1.navImportance += favorStairs.getProbability(distToStair_m) * 30.5f;
// use wall avoidance
if (useNormal) {

207
grid/factory/v2/Stairs2.h Normal file
View File

@@ -0,0 +1,207 @@
#ifndef STAIRS_H
#define STAIRS_H
#include <vector>
#include <set>
#include "../../Grid.h"
#include "Helper.h"
#include "../../../floorplan/v2/Floorplan.h"
#include <fstream>
#include <unordered_set>
/**
* this one should be both, simpler and more robus than v1.
* - prevents duplicate (x,y) nodes
* - slightly grows the stair-quads to ensure they overlap (sometimes the do not by a few mm)
* - only connects the corener nodes between adjacent quads (seems better for most stair-situations)
*
*/
template <typename T> class Stairs {
private:
/** the grid to build into */
Grid<T>& grid;
/** calculation helper */
Helper<T> helper;
// keep a list of all vertices below stairwells and remove them hereafter
std::vector<T*> toDelete;
private:
/** helper struct */
struct StairNode {
const int x_cm;
const int y_cm;
const int belongsToQuadIdx;
int gridIdx = -1;
StairNode(const int x_cm, const int y_cm, const int quadIdx) : x_cm(x_cm), y_cm(y_cm), belongsToQuadIdx(quadIdx) {;}
};
public:
/** ctor */
Stairs(Grid<T>& grid) : grid(grid), helper(grid) {
}
~Stairs() {
finalize();
}
void build(const Floorplan::Floor* floor, const Floorplan::Stair* stair) {
const int gs_cm = grid.getGridSize_cm();
// get all of the parts for the stair
const std::vector<Floorplan::StairPart> parts = stair->getParts();
// convert each part to a quad. hereby, slightly grow each quad, to ensure stair-parts are connected without gaps!
const std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(parts, floor, 1.05);
std::vector<StairNode> stairNodes;
// process each quad to get a list of all stair-nodes to add
for (int i = 0; i < (int) quads.size(); ++i) {
// create a 2D polygon (ignore z) for the quad
const Floorplan::Quad3& quad = quads[i];
HelperPoly poly(quad);
// get a callback for each node that belongs to the quad
poly.forEachGridPoint(gs_cm, [&] (int x_cm, int y_cm) {
const StairNode sn(x_cm, y_cm, i);
// IMPORTANT
// skip nodes that belong to more than one quad -> skip duplicates -> much more stable!
// NOTE: currently this will kill circular stairs that are above themselves
// FIX: skip dupliactes only between adjacent quads? this should help
auto comp = [&] (const StairNode& sn1) { return sn1.x_cm == sn.x_cm && sn1.y_cm == sn.y_cm; };
auto it = std::find_if(stairNodes.begin(), stairNodes.end(), comp);
if (it == stairNodes.end()) {stairNodes.push_back(sn);}
});
}
// add the new nodes to the grid
// nodes that are near to the two floors that stair is within, are replaced by already existing floor-nodes!
for (StairNode& sn : stairNodes) {
// use the nodes (x,y) to reconstruct the z-value for this position using barycentric interpolation
const Point2 p(sn.x_cm, sn.y_cm);
// the stair-quads centimeter position
const Floorplan::Quad3& quad = quads[sn.belongsToQuadIdx];
const Point3 p1 = quad.p1 * 100;
const Point3 p2 = quad.p2 * 100;
const Point3 p3 = quad.p3 * 100;
const Point3 p4 = quad.p4 * 100;
// get the z-value from one of the both triangles
int z_cm;
float u,v,w;
if (helper.bary(p, p1.xy(), p2.xy(), p3.xy(), u, v, w)) {
z_cm = p1.z*u + p2.z*v + p3.z*w;
} else {
helper.bary(p, p1.xy(), p3.xy(), p4.xy(), u, v, w);
z_cm = p1.z*u + p3.z*v + p4.z*w;
}
// the to-be-added position
GridPoint gp(sn.x_cm, sn.y_cm, z_cm);
// if a node is near an existing one (the floor above/below) use the existing one!
// this ensures the stair is connected to the floor above and below
if (grid.hasNodeFor(gp)) {
sn.gridIdx = grid.getNodeFor(gp).getIdx();
} else {
sn.gridIdx = grid.add(T(gp.x_cm, gp.y_cm, gp.z_cm));
// check if there is a nearby floor-node to delete
const int deleteDist_cm = 100;
const float distToBelow = gp.z_cm - floor->getStartingZ()*100;
const float distToAbove = floor->getEndingZ()*100 - gp.z_cm;
if (distToBelow > gs_cm && distToBelow < deleteDist_cm) {
T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getStartingZ()*100));
if (n) {toDelete.push_back(n);}
} else if (distToAbove > gs_cm && distToAbove < deleteDist_cm) {
T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getEndingZ()*100));
if (n) {toDelete.push_back(n);}
}
}
}
// now connect all new nodes with their neighbors
// do not perform normal grid-connection but examine the nodes within the generated vector
// this allows for some additional checks/criteria to be used
for (const StairNode& sn1 : stairNodes) {
T& n1 = (T&) grid[sn1.gridIdx];
for (const StairNode& sn2 : stairNodes) {
// node full?
if (n1.fullyConnected()) {continue;}
T& n2 = (T&) grid[sn2.gridIdx];
// do not connect node with itself
if (n2.getIdx() == n1.getIdx()) {continue;}
if (n1.hasNeighbor(n2.getIdx())) {continue;}
// connect adjacent stair-nodes
// but only if their stair-parts are also adjacent
// this addresses several error-situations with round stairs (end connected to the start) , ...
const int dx = sn1.x_cm - sn2.x_cm;
const int dy = sn1.y_cm - sn2.y_cm;
const int dz = sn1.belongsToQuadIdx - sn2.belongsToQuadIdx;
// connect
if (std::abs(dx) <= gs_cm && std::abs(dy) <= gs_cm && std::abs(dz) <= 1) {
grid.connectUniDir(n1, n2);
}
}
}
}
void finalize() {
// delete all pending nodes and perform a cleanup
if (!toDelete.empty()) {
for (T* n : toDelete) {grid.remove(*n);}
toDelete.clear();
grid.cleanup();
}
}
};
#endif // STAIRS_H

75
grid/walk/v2/modules/WalkModuleFavorZ.h
View File

@@ -7,15 +7,46 @@
#include "../../../../geo/Heading.h"
#include "../../../../math/Distributions.h"
#include "../../../../Assertions.h"
/** state-parameter needed for WalkModuleFavorZ */
struct WalkStateFavorZ {
/** nested struct to prevent name clashes */
struct {
/**
* 0 = up / down / stay is legal
* > 0 force states to walk upwards
* < 0 force states to walk downwards
*
* shifted towards 0 after every taken edge
* so: we force states to walk into the same z-direction for some time
*/
int zTendence = 0;
} favorZ;
};
/** favor z-transitions */
template <typename Node, typename WalkState> class WalkModuleFavorZ : public WalkModule<Node, WalkState> {
private:
// force states to walk into the same z-direction for 30 edges
const int keepForXEdges = 12;
public:
/** ctor */
WalkModuleFavorZ() {
;
// ensure the template WalkState inherits from 'WalkStateFavorZ'
StaticAssert::AinheritsB<WalkState, WalkStateFavorZ>();
}
virtual void updateBefore(WalkState& state) override {
@@ -30,9 +61,27 @@ public:
}
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
(void) state;
(void) curNode;
(void) nextNode;
// currently no walk-tendence configured
if (state.favorZ.zTendence == 0) {
// does the taken edge indicate a z-change?
const int diff = nextNode.z_cm - curNode.z_cm;
// if so, keep this z-direction for the next few edges to come!
if (diff != 0) {
state.favorZ.zTendence = (diff > 0) ? (+keepForXEdges) : (-keepForXEdges);
}
// currently there IS a walk-tendence configured
} else {
// update the tendence (shift towards 0)
if (state.favorZ.zTendence < 0) {++state.favorZ.zTendence;}
else if (state.favorZ.zTendence > 0) {--state.favorZ.zTendence;}
}
}
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
@@ -40,11 +89,19 @@ public:
(void) state;
(void) startNode;
if (curNode.z_cm != potentialNode.z_cm) {
return 40;
} else {
return 1;
}
const int tendence = state.favorZ.zTendence;
const int diff = potentialNode.z_cm - curNode.z_cm;
// tendence available + tendence match? -> high score!
if (tendence > 0 && diff > 0) {return 0.95;}
if (tendence < 0 && diff < 0) {return 0.95;}
// tendence available + tendence mismatch? -> very low score!
if (tendence > 0 && diff < 0) {return 0.05;}
if (tendence < 0 && diff > 0) {return 0.05;}
// no tendence available -> just favor z-transitions over non-z-transitions
return (diff != 0) ? (0.7) : (0.3);
}

47
grid/walk/v2/modules/WalkModuleHeading.h
View File

@@ -4,10 +4,52 @@
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../Assertions.h"
#include "../../../../geo/Heading.h"
#include "../../../../math/Distributions.h"
#include "../../../../geo/Heading.h"
/**
* base-class e.g. needed for GridWalkHeading and GridWalkHeadingControl to work
*/
struct WalkStateHeading {
/** used for better naming: heading.error instead of headingError */
struct _Heading {
/**
* the direction [0:2pi] the walk should move to
* e.g. indiciated by:
* compass
* integration over gyroscope values
*/
Heading direction;
/**
* (cumulative) error between walked edges and requested direction (above).
* is used to ensure that (even though the grid contains only 45° edges) we
* approximately walk into the requested direction.
*/
float error = 0;
/** ctor */
_Heading(const Heading direction, const float error) : direction(direction), error(error) {;}
} heading;
/** ctor */
explicit WalkStateHeading(const Heading& direction, const float error) : heading(direction, error) {;}
};
/** keep the state's heading */
template <typename Node, typename WalkState> class WalkModuleHeading : public WalkModule<Node, WalkState> {
@@ -23,7 +65,10 @@ public:
/** ctor */
WalkModuleHeading() : dist(Distribution::VonMises<double>(0.0f, 1.0f).getLUT()), draw(dist.getDrawList()) {
;
// ensure the template WalkState inherits from 'WalkStateHeading'!
StaticAssert::AinheritsB<WalkState, WalkStateHeading>();
}
virtual void updateBefore(WalkState& state) override {

7
grid/walk/v2/modules/WalkModuleHeadingControl.h
View File

@@ -58,6 +58,9 @@ public:
(void) state;
// for elevator edges [same (x,y) but different z] do not adjust anything
if (curNode.x_cm == nextNode.x_cm && curNode.y_cm == nextNode.y_cm && curNode.z_cm != nextNode.z_cm) {return;}
// get the heading denoted by the way from curNode to nextNode
const Heading head(curNode.x_cm, curNode.y_cm, nextNode.x_cm, nextNode.y_cm);
@@ -80,6 +83,10 @@ public:
(void) startNode;
// for elevator edges [same (x,y) but different z] just return 1
if (curNode.x_cm == potentialNode.x_cm && curNode.y_cm == potentialNode.y_cm && curNode.z_cm != potentialNode.z_cm) {return 1.0;}
// get the heading between curNode and potentialNode
const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);

2
grid/walk/v2/modules/WalkModuleNodeImportance.h
View File

@@ -4,6 +4,7 @@
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../Assertions.h"
/**
* favor edges based on the importance-factor of the next node.
@@ -44,7 +45,6 @@ public:
(void) curNode;
const double prob = potentialNode.getNavImportance();
//return std::pow(prob, 10);
return prob;
}

77
grid/walk/v2/modules/WalkModulePreventVisited.h Normal file
View File

@@ -0,0 +1,77 @@
#ifndef WALKMODULEPREVENTVISITED_H
#define WALKMODULEPREVENTVISITED_H
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../data/RingBuffer.h"
#include "../../../../Assertions.h"
struct WalkStatePreventVisited {
struct PV {
RingBuffer<int> history;
PV(const int size) : history(size) {;}
} preventVisited;
/** ctor */
explicit WalkStatePreventVisited(const int historySize) : preventVisited(historySize) {;}
};
/**
* prevent a state from visiting nodes he has already visited
* within a certain timeframe (ringbuffer)
*
* this should avoid deadlocks in some situations where the transition
* just switched back and forth between two nodes
*
*/
template <typename Node, typename WalkState> class WalkModulePreventVisited : public WalkModule<Node, WalkState> {
private:
public:
/** ctor */
WalkModulePreventVisited() {
// ensure the templated WalkState inherits from 'WalkStatePreventVisited'
StaticAssert::AinheritsB<WalkState, WalkStatePreventVisited>();
}
virtual void updateBefore(WalkState& state) override {
(void) state;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
(void) state;
(void) startNode;
(void) endNode;
}
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
(void) curNode;
state.preventVisited.history.add(nextNode.getIdx());
}
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
(void) startNode;
(void) curNode;
return (state.preventVisited.history.contains(potentialNode.getIdx())) ? 0.001 : 0.999;
}
};
#endif // WALKMODULEPREVENTVISITED_H

51
grid/walk/v2/modules/WalkModuleSpread.h
View File

@@ -4,6 +4,21 @@
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../Assertions.h"
/** state-parameter needed for WalkModuleSpread */
struct WalkStateSpread {
/** nested struct to prevent name-clashes */
struct {
/** keep something like a moving-average-position we want to strictly depart from */
GridPoint departFrom;
} spread;
};
/**
* simply try to move away from the starting node as much as possible
@@ -12,19 +27,26 @@ template <typename Node, typename WalkState> class WalkModuleSpread : public Wal
private:
Point3 avg;
/**
* how fast to adjust the average-position to depart from
* values between 3% and 10% seem fine
*/
const float kappa = 0.10;
public:
/** ctor */
WalkModuleSpread() {
;
/** ensure the templated WalkState inherits from WalkStateSpread */
StaticAssert::AinheritsB<WalkState, WalkStateSpread>();
}
virtual void updateBefore(WalkState& state) override {
(void) state;
avg = avg * 0.999 + state.position.inMeter() * 0.001;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
@@ -34,24 +56,27 @@ public:
}
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
(void) state;
(void) curNode;
(void) nextNode;
state.spread.departFrom = state.spread.departFrom * (1.0f-kappa) + nextNode * (kappa);
}
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
(void) state;
(void) startNode;
(void) curNode;
const float dOld = avg.getDistance(curNode.inMeter());
const float dNew = avg.getDistance(potentialNode.inMeter());
// current distance from the depart-from position
const float dOld = state.spread.departFrom.getDistanceInCM(curNode);
if (dNew > dOld) {return 0.8;}
if (curNode.z_cm != potentialNode.z_cm) {return 0.8;}
if (dNew == dOld) {return 0.2;}
return 0;
// potential distance from the depart-from position
const float dNew = state.spread.departFrom.getDistanceInCM(potentialNode);
// now, favor edges that depart even further from the depart-from position!
if (dNew > dOld) {return 0.90;} // departing
if (dNew == dOld) {return 0.09;} // distance does not change
{return 0.01;} // NOT departing.. unlikely
}

36
grid/walk/v2/modules/WalkStateHeading.h
View File

@@ -1,43 +1,7 @@
#ifndef WALKSTATEHEADING_H
#define WALKSTATEHEADING_H
#include "../../../../geo/Heading.h"
/**
* base-class e.g. needed for GridWalkHeading and GridWalkHeadingControl to work
*/
struct WalkStateHeading {
/** used for better naming: heading.error instead of headingError */
struct _Heading {
/**
* the direction [0:2pi] the walk should move to
* e.g. indiciated by:
* compass
* integration over gyroscope values
*/
Heading direction;
/**
* (cumulative) error between walked edges and requested direction (above).
* is used to ensure that (even though the grid contains only 45° edges) we
* approximately walk into the requested direction.
*/
float error = 0;
/** ctor */
_Heading(const Heading direction, const float error) : direction(direction), error(error) {;}
} heading;
/** ctor */
explicit WalkStateHeading(const Heading& direction, const float error) : heading(direction, error) {;}
};
#endif // WALKSTATEHEADING_H