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

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);
}