104 lines
2.5 KiB
C++
104 lines
2.5 KiB
C++
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#ifndef WALKMODULEHEADING_H
|
||
#define WALKMODULEHEADING_H
|
||
|
||
#include "WalkModule.h"
|
||
#include "WalkStateHeading.h"
|
||
|
||
#include "../../../../Assertions.h"
|
||
|
||
#include "../../../../geo/Heading.h"
|
||
#include "../../../../math/distribution/Normal.h"
|
||
#include "../../../../math/distribution/LUT.h"
|
||
#include "../../../../math/distribution/VonMises.h"
|
||
|
||
|
||
#include "../../../../geo/Heading.h"
|
||
|
||
|
||
|
||
|
||
/** keep the state's heading */
|
||
template <typename Node, typename WalkState> class WalkModuleHeading : public WalkModule<Node, WalkState> {
|
||
|
||
private:
|
||
|
||
/** van-Mises distribution */
|
||
Distribution::LUT<double> dist;
|
||
|
||
/** van-Mises draw list */
|
||
DrawList<double> draw;
|
||
|
||
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, const Node& startNode) override {
|
||
|
||
(void) startNode;
|
||
|
||
// add noise
|
||
state.heading.direction += draw.get();
|
||
|
||
}
|
||
|
||
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
|
||
|
||
(void) state;
|
||
(void) startNode;
|
||
(void) endNode;
|
||
// if (startNode.x_cm != endNode.x_cm || startNode.y_cm != endNode.y_cm) {
|
||
// Heading head(startNode.x_cm, startNode.y_cm, endNode.x_cm, endNode.y_cm);
|
||
// state.startHeading = head;
|
||
// }
|
||
|
||
}
|
||
|
||
/** one step (edge) is taken */
|
||
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
|
||
|
||
// TODO
|
||
(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
|
||
const Heading stateHead = state.heading.direction;
|
||
|
||
// get the difference
|
||
const float angularDiff = head.getDiffHalfRAD(stateHead);//head.getRAD() - stateHead.getRAD();
|
||
|
||
// determine probability
|
||
return dist.getProbability(angularDiff);
|
||
|
||
}
|
||
|
||
|
||
};
|
||
|
||
#endif // WALKMODULEHEADING_H
|