FrankE
a203305628
- changed the wifi-estimation api - adjusted test-cases - worked on grid-bulding and grid-importance - new walking modules - fixed some minor issues
79 lines
1.9 KiB
C++
79 lines
1.9 KiB
C++
#ifndef WALKMODULEHEADINGCONTROL_H
|
|
#define WALKMODULEHEADINGCONTROL_H
|
|
|
|
#include "WalkModule.h"
|
|
#include "WalkStateHeading.h"
|
|
|
|
#include "../../../../geo/Heading.h"
|
|
#include "../../../../math/Distributions.h"
|
|
|
|
|
|
/** keep the state's heading */
|
|
template <typename Node, typename WalkState, typename Control> class WalkModuleHeadingControl : public WalkModule<Node, WalkState> {
|
|
|
|
private:
|
|
|
|
/** van-Mises distribution */
|
|
Distribution::LUT<double> dist;
|
|
|
|
/** van-Mises draw list */
|
|
DrawList<double> draw;
|
|
|
|
Control* ctrl;
|
|
|
|
//std::unordered_map<WalkState*, float> errorTracker;
|
|
|
|
public:
|
|
|
|
/** ctor 3.0 should be OK! */
|
|
WalkModuleHeadingControl(Control* ctrl) : dist(Distribution::VonMises<double>(0.0f, 2.0).getLUT()), draw(dist.getDrawList()), ctrl(ctrl) {
|
|
;
|
|
}
|
|
|
|
|
|
virtual void updateBefore(WalkState& state) override {
|
|
|
|
const float var = draw.get() * 0.20;//0.05;
|
|
//const float var = 0;
|
|
state.startHeading += ctrl->turnAngle + var;
|
|
|
|
}
|
|
|
|
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) startNode;
|
|
|
|
// get the heading between curNode and potentialNode
|
|
const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);
|
|
|
|
// compare the heading against the state's heading - the last error
|
|
const Heading stateHead = state.startHeading;
|
|
|
|
// get the difference
|
|
const float angularDiff = head.getDiffHalfRAD(stateHead);
|
|
|
|
// determine probability
|
|
const float prob = dist.getProbability(angularDiff);
|
|
return prob;
|
|
|
|
}
|
|
|
|
|
|
|
|
};
|
|
|
|
#endif // WALKMODULEHEADINGCONTROL_H
|