added new sanity-check assertions

fixed issue with angles [bad interface]
- adjusted other parts accordingly
- added corresponding test-cases
started working on absolute heading

grid/factory/v2/Importance.h

@@ -153,6 +153,8 @@ public:
 
 
 			// sanity check
+			Assert::isNotNaN(n1.walkImportance, "detected NaN walk importance for " + n1.asString());
+			Assert::isNotNaN(n1.navImportance, "detected NaN walk importance for " + n1.asString());
 			Assert::isTrue(n1.walkImportance >= 0, "detected negative walk importance. does not make sense!");
 			Assert::isTrue(n1.navImportance >= 0, "detected negative nav importance. does not make sense!");
 

grid/walk/v2/GridWalker.h

@@ -104,9 +104,19 @@ public:
 		//if (cnt != 0) {probability /= cnt;} else {probability = 1.0;}
 		probability = 1.0;
 		//probability  = (maxEdgeProb.isValid()) ? (maxEdgeProb.get()) : (1.0);		// dist_m might be zero -> no edges -> no maximum
-		probability *= curNode->getWalkImportance();// < 0.4f ? (0.1) : (1.0);			// "kill" particles that walk near walls (most probably trapped ones)
+
+		// add the walk importance to the probabiliy [each node has a to-be-walked-probability depending on its distance to walls, etc...]
+		const float walkImportance = curNode->getWalkImportance();
+		Assert::isNotNaN(walkImportance, "grid-node's walk-importance is NaN. Did you forget to calculate the importance values after building the grid?");
+		Assert::isBetween(walkImportance, 0.0f, 2.5f, "grid-node's walk-importance is out of range. Did you forget to calculate the importance values after building the grid?");
+		probability *= walkImportance;// < 0.4f ? (0.1) : (1.0);			// "kill" particles that walk near walls (most probably trapped ones)
+
+
 		//probability = std::pow(probability, 5);
 
+		// sanity check
+		Assert::isNotNaN(probability, "detected NaN grid-walk probability");
+
 		// update after
 		updateAfter(currentState, *startNode, *curNode);
 

grid/walk/v2/modules/WalkModuleAbsoluteHeadingControl.h

@@ -0,0 +1,85 @@
+#ifndef WALKMODULEABSOLUTEHEADINGCONTROL_H
+#define WALKMODULEABSOLUTEHEADINGCONTROL_H
+
+/**
+ * compare the state's absolute heading against a given compass [control]
+ */
+
+#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 WalkModuleAbsoluteHeadingControl : public WalkModule<Node, WalkState> {
+
+	const float sigma_rad;
+	const Control* ctrl;
+
+public:
+
+	/** ctor. 180 should be OK! */
+	WalkModuleAbsoluteHeadingControl(const Control* ctrl, const float sensorNoiseDegreesSigma) :
+		sigma_rad(Angle::degToRad(sensorNoiseDegreesSigma)),
+		ctrl(ctrl) {
+
+		// ensure the template WalkState inherits from 'WalkStateHeading'!
+		StaticAssert::AinheritsB<WalkState, WalkStateHeading>();
+
+	}
+
+
+	virtual void updateBefore(WalkState& state, const Node& startNode) override {
+		(void) state;
+		(void) startNode;
+	}
+
+	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;
+
+		// NOTE: ctrl->turnAngle is cumulative SINCE the last transition!
+		// reset this one after every transition!
+		Assert::isBetween(ctrl->compassAzimuth_rad, 0.0f, (float)(2*M_PI), "the given absolute heading is out of bounds");
+
+		// ignore for stairs?
+		//if (potentialNode.getType() == GridNode::TYPE_STAIR)	{return 1.0;}
+
+		// for elevator edges [same (x,y) but different z] just return 1
+		if (potentialNode.getType() == GridNode::TYPE_ELEVATOR)	{return 1.0;}
+		if (curNode.getType() == GridNode::TYPE_ELEVATOR)		{return 1.0;}
+		//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);
+
+		// compare the heading against the state's heading - the last error
+		const Heading stateHead = state.heading.direction;
+
+		// get the difference
+		const float angularDiff = head.getDiffHalfRAD(stateHead);
+
+		if (angularDiff > Angle::degToRad(180))	{return 0.05;}
+		if (angularDiff > Angle::degToRad(90))	{return 0.25;}
+												{return 0.70;}
+
+	}
+
+};
+
+#endif // WALKMODULEABSOLUTEHEADINGCONTROL_H

grid/walk/v2/modules/WalkModuleHeadingControl.h

@@ -77,7 +77,8 @@ public:
 		const Heading stateHead = state.heading.direction;
 
 		// get the error (signed difference) between both
-		const float angularDiff = stateHead.getSignedDiff(head);
+//		const float angularDiff = stateHead.getSignedDiff(head);
+		const float angularDiff = Heading::getSignedDiff(head, stateHead);
 
 		// adjust the error.
 		// note:	the error may get > +/- 2PI but this is not an issue!

grid/walk/v2/modules/WalkModuleHeadingVonMises.h

@@ -81,7 +81,8 @@ public:
         const Heading stateHead = state.heading.direction;
 
         // get the error (signed difference) between both
-        const float angularDiff = stateHead.getSignedDiff(head);
+		//const float angularDiff = stateHead.getSignedDiff(head);
+		const float angularDiff = Heading::getSignedDiff(head, stateHead);
 
         // adjust the error.
         // note:	the error may get > +/- 2PI but this is not an issue!