added several grid-walks

added new helper methods/classes (e.g. for heading)
new test cases
optimize the dijkstra
cleanups/refactoring
added timed-benchmarks to the log
many more...

geo/Angle.h

@@ -4,7 +4,8 @@
 #include <cmath>
 #include <KLib/Assertions.h>
 
-class Angle {
+struct Angle {
+
 
 public:
 
@@ -28,12 +29,15 @@ public:
 	static float getDiffRAD_2PI_PI(const float r1, const float r2) {
 		_assertBetween(r1, 0, 2*M_PI, "r1 out of bounds");
 		_assertBetween(r2, 0, 2*M_PI, "r2 out of bounds");
-		return fmod(std::abs(r2 - r1), M_PI);
+		float tmp = std::abs(r1-r2);
+		return (tmp <= M_PI) ? (tmp) : (2*M_PI-tmp);
+		//float tmp2 = fmod(tmp, M_PI);
+		//return fmod(std::abs(r2 - r1), M_PI);
 
 	}
 
 	/** convert degrees to radians */
-	static float degToRad(const float deg) {
+	static constexpr float degToRad(const float deg) {
 		return deg / 180 * M_PI;
 	}
 

geo/Heading.h

@@ -0,0 +1,63 @@
+#ifndef HEADING_H
+#define HEADING_H
+
+#include <cmath>
+
+#include "Angle.h"
+
+struct Heading {
+
+#define _2PI (2*M_PI)
+
+private:
+
+	/** heading in radians. 0 = to-the-right */
+	float rad;
+
+public:
+
+	/** ctor with radians */
+	Heading(const float rad) : rad(rad) {
+		_assertBetween(rad, 0, _2PI, "radians out of bounds");
+	}
+
+	/** ctor from(x1,y1) to(x2,y2) */
+	Heading(const float x1, const float y1, const float x2, const float y2) : rad(Angle::getRAD_2PI(x1,y1,x2,y2)) {
+		_assertBetween(rad, 0, _2PI, "radians out of bounds");
+	}
+
+	/** angular difference [0:PI] */
+	float getDiffHalfRAD(const Heading other) const {
+		return Angle::getDiffRAD_2PI_PI(rad, other.rad);
+	}
+
+	/** update the angle but ensure we stay within [0:2PI] */
+	Heading& operator += (const float _rad) {
+		_assertBetween(_rad, -_2PI*0.99, +_2PI*0.99, "radians out of bounds");
+		rad += _rad;
+		if (rad > _2PI)		{rad -= _2PI;}
+		if (rad < 0)		{rad += _2PI;}
+		return *this;
+	}
+
+	/** get an inverted version of this heading (upwards -> downwards, left -> right, ...) */
+	Heading getInverted() const {
+		Heading out(rad);
+		out += M_PI;
+		return out;
+	}
+
+	float getRAD() const {return rad;}
+
+#undef _2PI
+
+};
+
+namespace Headings {
+	static const Heading RIGHT =	Heading(M_PI*0/2);
+	static const Heading UP =		Heading(M_PI*1/2);
+	static const Heading LEFT =		Heading(M_PI*2/2);
+	static const Heading DOWN =		Heading(M_PI*3/2);
+}
+
+#endif // HEADING_H

geo/Point3.h

@@ -1,6 +1,8 @@
 #ifndef POINT3_H
 #define POINT3_H
 
+#include <KLib/Assertions.h>
+
 /**
  * 3D Point
  */
@@ -26,6 +28,14 @@ struct Point3 {
 
 	Point3& operator /= (const float v) {x/=v; y/=v; z/=v; return *this;}
 
+	/** read-only array access */
+	float operator [] (const int idx) const {
+		_assertBetween(idx, 0, 2, "index out of bounds");
+		if (0 == idx)	{return x;}
+		if (1 == idx)	{return y;}
+						return z;
+	}
+
 	float length() const {return std::sqrt(x*x + y*y + z*z);}
 
 	float length(const float norm) const {