dijkstra is now bleching fast

deleting from the grid is now bleaching fast
added new helper methods
many new test-cases
many new methods for geo classes and others
added a bunch of new grid-walkers

geo/Angle.h

@@ -27,13 +27,10 @@ public:
 	 * - as a change-in-direction between [0:PI]
 	 */
 	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");
+		_assertBetween(r1, 0, (float)(2*M_PI), "r1 out of bounds");
+		_assertBetween(r2, 0, (float)(2*M_PI), "r2 out of bounds");
 		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 */

geo/Heading.h

@@ -2,6 +2,7 @@
 #define HEADING_H
 
 #include <cmath>
+#include <random>
 
 #include "Angle.h"
 
@@ -35,11 +36,16 @@ public:
 	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;}
+		if		(rad >= _2PI)	{rad -= _2PI;}
+		else if	(rad < 0)		{rad += _2PI;}
 		return *this;
 	}
 
+	/** update the angle but ensure we stay within [0:2PI] */
+	Heading operator + (const float _rad) const {
+		return (Heading(*this) += _rad);
+	}
+
 	/** get an inverted version of this heading (upwards -> downwards, left -> right, ...) */
 	Heading getInverted() const {
 		Heading out(rad);
@@ -49,6 +55,13 @@ public:
 
 	float getRAD() const {return rad;}
 
+	/** get a random heading */
+	static Heading rnd() {
+		static std::minstd_rand gen; gen.seed(1234);
+		static std::uniform_real_distribution<float> dist(0, _2PI);
+		return Heading(dist(gen));
+	}
+
 #undef _2PI
 
 };

geo/Line2.h

@@ -26,23 +26,23 @@ public:
 
 	bool getSegmentIntersection(const Line2& other) const {
 
-		const double bx = p2.x - p1.x;
-		const double by = p2.y - p1.y;
+		const float bx = p2.x - p1.x;
+		const float by = p2.y - p1.y;
 
-		const double dx = other.p2.x - other.p1.x;
-		const double dy = other.p2.y - other.p1.y;
+		const float dx = other.p2.x - other.p1.x;
+		const float dy = other.p2.y - other.p1.y;
 
-		const double b_dot_d_perp = bx*dy - by*dx;
+		const float b_dot_d_perp = bx*dy - by*dx;
 
-		if(b_dot_d_perp == 0) {return false;}
+		if (b_dot_d_perp == 0) {return false;}
 
-		const double cx = other.p1.x - p1.x;
-		const double cy = other.p1.y - p1.y;
+		const float cx = other.p1.x - p1.x;
+		const float cy = other.p1.y - p1.y;
 
-		const double t = (cx * dy - cy * dx) / b_dot_d_perp;
+		const float t = (cx * dy - cy * dx) / b_dot_d_perp;
 		if(t < 0 || t > 1) {return false;}
 
-		const double u = (cx * by - cy * bx) / b_dot_d_perp;
+		const float u = (cx * by - cy * bx) / b_dot_d_perp;
 		if(u < 0 || u > 1) {return false;}
 
 		return true;

geo/Point3.h

@@ -2,6 +2,7 @@
 #define POINT3_H
 
 #include <KLib/Assertions.h>
+#include <cmath>
 
 /**
  * 3D Point
@@ -28,6 +29,12 @@ struct Point3 {
 
 	Point3& operator /= (const float v) {x/=v; y/=v; z/=v; return *this;}
 
+	Point3& operator += (const Point3& o) {x+=o.x; y+=o.y; z+=o.z; return *this;}
+
+	Point3& operator -= (const Point3& o) {x-=o.x; y-=o.y; z-=o.z; return *this;}
+
+	bool operator == (const Point3& o) const {return x==o.x && y==o.y && z==o.z;}
+
 	/** read-only array access */
 	float operator [] (const int idx) const {
 		_assertBetween(idx, 0, 2, "index out of bounds");
@@ -36,6 +43,14 @@ struct Point3 {
 						return z;
 	}
 
+	/** get the distance between this point and the other one */
+	float getDistance(const Point3& o) const {
+		const float dx = x - o.x;
+		const float dy = y - o.y;
+		const float dz = z - o.z;
+		return std::sqrt(dx*dx + dy*dy + dz*dz);
+	}
+
 	float length() const {return std::sqrt(x*x + y*y + z*z);}
 
 	float length(const float norm) const {