huge commit
- worked on about everything - grid walker using plugable modules - wifi models - new distributions - worked on geometric data-structures - added typesafe timestamps - worked on grid-building - added sensor-classes - added sensor analysis (step-detection, turn-detection) - offline data reader - many test-cases
This commit is contained in:
62
grid/walk/v2/modules/WalkModuleSpread.h
Normal file
62
grid/walk/v2/modules/WalkModuleSpread.h
Normal file
@@ -0,0 +1,62 @@
|
||||
#ifndef WALKMODULESPREAD_H
|
||||
#define WALKMODULESPREAD_H
|
||||
|
||||
#include "WalkModule.h"
|
||||
#include "WalkStateHeading.h"
|
||||
|
||||
|
||||
/**
|
||||
* simply try to move away from the starting node as much as possible
|
||||
*/
|
||||
template <typename Node, typename WalkState> class WalkModuleSpread : public WalkModule<Node, WalkState> {
|
||||
|
||||
private:
|
||||
|
||||
Point3 avg;
|
||||
|
||||
public:
|
||||
|
||||
/** ctor */
|
||||
WalkModuleSpread() {
|
||||
;
|
||||
}
|
||||
|
||||
|
||||
virtual void updateBefore(WalkState& state) override {
|
||||
(void) state;
|
||||
avg = avg * 0.999 + state.startPos.inMeter() * 0.001;
|
||||
}
|
||||
|
||||
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) state;
|
||||
(void) curNode;
|
||||
(void) nextNode;
|
||||
}
|
||||
|
||||
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());
|
||||
|
||||
if (dNew > dOld) {return 0.8;}
|
||||
if (curNode.z_cm != potentialNode.z_cm) {return 0.8;}
|
||||
if (dNew == dOld) {return 0.2;}
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif // WALKMODULESPREAD_H
|
||||
Reference in New Issue
Block a user