worked on 2D/3D raytracing

adjusted BVH
improved 2D/3D BVH
new bounding volumes
new test cases
renamed some test-cases for grouping reasons
made GPC header-only using slight adjustments

CMakeLists.txt

@@ -75,8 +75,8 @@ ADD_DEFINITIONS(
 	-fstack-protector-all
 
 	-g3
-#	-O0
-	-O2
+	-O0
+	#-O2
 	-march=native
 
 	-DWITH_TESTS

geo/BBox2.h

@@ -1,8 +1,9 @@
-#ifndef BBOX2_H
-#define BBOX2_H
+#ifndef GEO_BBOX2_H
+#define GEO_BBOX2_H
 
 #include "Point2.h"
 #include "Line2.h"
+#include <vector>
 
 class BBox2 {
 
@@ -105,6 +106,21 @@ public:
 		p2 += Point2(val, val);	// increase maximum
 	}
 
+	/** grow the bbox by the amount given for each dimension */
+	void growRel(const float val) {
+		const Point2 center = (p1+p2)/2;
+		p1 += (p1-center)*val;	// decrease minimum
+		p2 += (p2-center)*val;	// increase maximum
+	}
+
+
+	/** combine two bboxes */
+	static BBox2 join(const BBox2& bb1, const BBox2& bb2) {
+		const Point2 min( std::min(bb1.p1.x, bb2.p1.x), std::min(bb1.p1.y, bb2.p1.y) );
+		const Point2 max( std::max(bb1.p2.x, bb2.p2.x), std::max(bb1.p2.y, bb2.p2.y) );
+		return BBox2(min, max);
+	}
+
 };
 
-#endif // BBOX2_H
+#endif // GEO_BBOX2_H

geo/BBox3.h

@@ -103,6 +103,13 @@ public:
 		return true;
 	}
 
+	/** combine two bboxes */
+	static BBox3 join(const BBox3& bb1, const BBox3& bb2) {
+		const Point3 min( std::min(bb1.p1.x, bb2.p1.x), std::min(bb1.p1.y, bb2.p1.y), std::min(bb1.p1.z, bb2.p1.z) );
+		const Point3 max( std::max(bb1.p2.x, bb2.p2.x), std::max(bb1.p2.y, bb2.p2.y), std::max(bb1.p2.z, bb2.p2.z) );
+		return BBox3(min,max);
+	}
+
 };
 
 #endif // BBOX3_H

geo/Circle2.h

@@ -0,0 +1,304 @@
+#ifndef CIRCLE2_H
+#define CIRCLE2_H
+
+#include <vector>
+
+#include "Point2.h"
+#include "Ray2.h"
+
+#include "../Assertions.h"
+
+//#include <KLib/misc/gnuplot/Gnuplot.h>
+//#include <KLib/misc/gnuplot/GnuplotPlot.h>
+//#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+//#include <KLib/misc/gnuplot/GnuplotPlotElementPoints.h>
+
+struct Circle2 {
+
+	Point2 center;
+
+	float radius;
+
+public:
+
+	/** empty ctor */
+	Circle2() : center(), radius(0) {;}
+
+	/** ctor */
+	Circle2(const Point2 center, const float radius) : center(center), radius(radius) {;}
+
+
+	/** does this circle contain the given point? */
+	bool contains(const Point2 p) const {
+		return center.getDistance(p) <= radius;
+	}
+
+	/** does this circle contain the given point? */
+	bool containsAll(const std::vector<Point2>& pts) const {
+		for (const Point2& p : pts) {
+			if (!contains(p)) {return false;}
+		}
+		return true;
+	}
+
+	/** get a point on the circle for the given radians */
+	Point2 getPointAt(const float rad) const {
+		return center + Point2(std::cos(rad), std::sin(rad)) * radius;
+	}
+
+
+	/** does this circle contain the given circle? */
+	bool contains(const Circle2 c) const {
+		return (center.getDistance(c.center)+c.radius) <= radius;
+	}
+
+	/** does this circle intersect with the given ray? */
+	bool intersects(const Ray2 ray) const {
+
+		// https://math.stackexchange.com/questions/311921/get-location-of-vector-circle-intersection
+		const float a = ray.dir.x*ray.dir.x + ray.dir.y*ray.dir.y;
+		const float b = 2 * ray.dir.x * (ray.start.x-center.x) + 2 * ray.dir.y * (ray.start.y - center.y);
+		const float c = (ray.start.x-center.x) * (ray.start.x-center.x) + (ray.start.y - center.y)*(ray.start.y - center.y) - radius*radius;
+		const float discr = b*b - 4*a*c;
+
+		return discr >= 0;
+
+	}
+
+
+	/** configure this sphere to contain the given point-set */
+	void adjustToPointSet(const std::vector<Point2>& lst) {
+
+		//adjustToPointSetAPX(lst);
+		adjustToPointSetExact(lst);
+
+		// validate
+		for (const Point2& p : lst) {
+			Assert::isTrue(this->contains(p), "calculated circle seems incorrect");
+		}
+
+	}
+
+	/** combine two circles into a new one containing both */
+	static Circle2 join(const Circle2& a, const Circle2& b) {
+
+		// if one already contains the other, just return it as-is
+		if (a.contains(b)) {return a;}
+		if (b.contains(a)) {return b;}
+
+		// create both maximum ends
+		const Point2 out1 = a.center + (a.center-b.center).normalized() * a.radius;
+		const Point2 out2 = b.center + (b.center-a.center).normalized() * b.radius;
+
+		// center is within both ends, so is the radius
+		Point2 newCen = (out1 + out2) / 2;
+		float newRad = out1.getDistance(out2) / 2 * 1.0001;	// slightly larger to prevent rounding issues
+		Circle2 res(newCen, newRad);
+
+		// check
+		Assert::isTrue(res.contains(a), "sphere joining error. base-spheres are not contained.");
+		Assert::isTrue(res.contains(b), "sphere joining error. base-spheres are not contained.");
+
+		return res;
+
+	}
+
+	float getArea() const  {
+		return M_PI * (radius*radius);
+	}
+
+private:
+
+	/*
+	void show(std::vector<Point2> pts, const Point2 P, const Point2 Q, const Point2 R = Point2(NAN, NAN)) {
+
+		static K::Gnuplot gp;
+
+		K::GnuplotPlot plot;
+		K::GnuplotPlotElementPoints gPoints; plot.add(&gPoints); gPoints.setColor(K::GnuplotColor::fromHexStr("#0000ff")); gPoints.setPointSize(1);
+		K::GnuplotPlotElementLines gLines; plot.add(&gLines);
+
+		for (const Point2 p : pts) {
+			K::GnuplotPoint2 p2(p.x, p.y);
+			gPoints.add(p2);
+		}
+
+		K::GnuplotPoint2 gP(P.x, P.y);
+		K::GnuplotPoint2 gQ(Q.x, Q.y);
+		gLines.addSegment(gP, gQ);
+
+		K::GnuplotPoint2 gR(R.x, R.y);
+		//gLines.addSegment(gP, gR);
+		gLines.addSegment(gQ, gR);
+
+		for (float f = 0; f < M_PI*2; f += 0.1) {
+			Point2 p = center + Point2(std::cos(f), std::sin(f)) * radius;
+			K::GnuplotPoint2 gp (p.x, p.y);
+			gLines.add(gp);
+		}
+
+		gp << "set size ratio -1\n";
+		gp.draw(plot);
+		gp.flush();
+
+		int i = 0; (void) i;
+
+	}
+	*/
+
+
+	// Graphic Gems 2 - Jon Rokne
+	void adjustToPointSetExact(const std::vector<Point2>& _lst) {
+
+		if (_lst.size() == 2) {
+			this->center = (_lst[0] + _lst[1]) / 2;
+			this->radius = _lst[0].getDistance(_lst[1]) / 2 * 1.0001f;
+			return;
+		}
+
+		std::vector<Point2> lst = _lst;
+
+		// find point P having min(p.y)
+		// NOTE: seems like the original work uses another coordinate system. so we search for max(p.y) instead!
+		auto compMinY = [] (const Point2 p1, const Point2 p2) {return p1.y < p2.y;};
+		auto it1 = std::max_element(lst.begin(), lst.end(), compMinY);
+		Point2 P = *it1;
+		lst.erase(it1);
+
+		// find a point Q such that the angle of the line segment
+		// PQ with the x axis is minimal
+		auto compMinAngleX = [&] (const Point2 p1, const Point2 p2) {
+
+			const Point2 PQ1 = p1 - P;
+			const Point2 PQ2 = p2 - P;
+
+			const float angle1 = dot(PQ1.normalized(), Point2(0,1));
+			const float angle2 = dot(PQ2.normalized(), Point2(0,1));
+
+			return std::acos(angle1) < std::acos(angle2);
+
+		};
+		auto it2 = std::min_element(lst.begin(), lst.end(), compMinAngleX);
+		Point2 Q = *it2;
+		lst.erase(it2);
+
+		// get the angle abc which is the angle at "b"
+		auto angle = [] (const Point2 a, const Point2 b, const Point2 c) {
+			const Point2 ba = a - b;
+			const Point2 bc = c - b;
+			return std::acos(dot(ba.normalized(), bc.normalized()));
+		};
+
+		// TODO: how many loops?
+		for (size_t xx = 0; xx < lst.size()*10; ++xx) {
+
+			auto compMinAnglePRQ = [P,Q,angle]  (const Point2 p1, const Point2 p2) {
+				return std::abs(angle(P,p1,Q)) < std::abs(angle(P,p2,Q));
+			};
+
+			auto it3 = std::min_element(lst.begin(), lst.end(), compMinAnglePRQ);
+
+			Point2 R = *it3;
+			lst.erase(it3);
+
+			const float anglePRQ = angle(P,R,Q);
+			const float angleRPQ = angle(R,P,Q);
+			const float anglePQR = angle(P,Q,R);
+
+			//check for case 1 (angle PRQ is obtuse), the circle is determined
+			//by two points, P and Q. radius = |(P-Q)/2|, center = (P+Q)/2
+			if (anglePRQ > M_PI/2) {
+
+				this->radius = P.getDistance(Q) / 2 * 1.001f;
+				this->center = (P+Q)/2;
+				//if (!containsAll(_lst)) {show(_lst, P, Q, R);}
+				return;
+
+			}
+
+			if (angleRPQ > M_PI/2) {
+				lst.push_back(P);
+				P = R;
+				continue;
+			}
+
+			if (anglePQR > M_PI/2) {
+				lst.push_back(Q);
+				Q = R;
+				continue;
+			}
+
+			const Point2 mPQ = (P+Q)/2;
+			const Point2 mQR = (Q+R)/2;
+
+			const float numer = -(-mPQ.y * R.y + mPQ.y * Q.y + mQR.y * R.y - mQR.y * Q.y - mPQ.x * R.x + mPQ.x * Q.x + mQR.x * R.x - mQR.x * Q.x);
+			const float denom =  (-Q.x * R.y + P.x * R.y - P.x * Q.y + Q.y * R.x - P.y * R.x + P.y * Q.x);
+			const float t = numer / denom;
+
+			const float cx  = -t * (Q.y - P.y) + mPQ.x;
+			const float cy  =  t * (Q.x - P.x) + mPQ.y;
+			this->center = Point2(cx, cy);
+			this->radius = this->center.getDistance(P) * 1.001f;
+
+			//if (!containsAll(_lst)) {show(_lst, P, Q, R);}
+
+			return;
+
+		}
+
+		throw Exception("should not happen");
+
+	}
+
+	/*
+	void adjustToPointSetRefine(const std::vector<Point2>& lst) {
+
+		float bestArea = 99999999;
+
+		for (size_t i = 0; i < lst.size(); ++i) {
+			for (size_t j = 0; j < lst.size(); ++j) {
+				if (i == j) {continue;}
+
+				const Point2 center = (lst[i] + lst[j]) / 3;
+				const float radius = lst[i].getDistance(lst[j]);
+				const Circle2 test(center, radius);
+
+				if (test.containsAll(lst)) {
+					if (test.getArea() < bestArea) {
+						bestArea = test.getArea();
+						this->radius = test.radius;
+						this->center = test.center;
+					}
+				}
+			}
+		}
+
+	}
+*/
+	void adjustToPointSetAPX(const std::vector<Point2>& lst) {
+
+		// NOT OPTIMAL but fast
+
+		// calculate the point set's center
+		Point2 sum(0,0);
+		for (const Point2 p : lst) {sum += p;}
+		const Point2 center = sum / lst.size();
+
+		// calculate the sphere's radius (maximum distance from center
+		float radius = 0;
+		for (const Point2 p : lst) {
+			const float dist = center.getDistance(p);
+			if (dist > radius) {radius = dist;}
+		}
+
+		this->radius = radius;
+		this->center = center;
+
+
+
+
+	}
+
+};
+
+#endif // CIRCLE2_H

geo/Point2.h

@@ -68,6 +68,11 @@ struct Point2 {
 
 };
 
+inline float dot(const Point2 p1, const Point2 p2) {
+	return (p1.x*p2.x) + (p1.y*p2.y);
+}
+
+
 inline void swap(Point2& p1, Point2& p2) {
 	std::swap(p1.x, p2.x);
 	std::swap(p1.y, p2.y);

geo/Point3.h

@@ -1,5 +1,5 @@
-#ifndef POINT3_H
-#define POINT3_H
+#ifndef GEO_POINT3_H
+#define GEO_POINT3_H
 
 #include "../Assertions.h"
 #include <cmath>
@@ -128,4 +128,4 @@ inline Point3 cross(const Point3 a, const Point3 b) {
 	);
 }
 
-#endif // POINT3_H
+#endif // GEO_POINT3_H

geo/Ray2.h

@@ -0,0 +1,29 @@
+#ifndef GEO_RAY2_H
+#define GEO_RAY2_H
+
+
+#include "Point2.h"
+
+struct Ray2 {
+
+	/** starting point */
+	Point2 start;
+
+	/** ray's direction */
+	Point2 dir;
+
+public:
+
+	/** empty */
+	Ray2() : start(), dir() {
+		;
+	}
+
+	/** ctor */
+	Ray2(Point2 start, Point2 dir) : start(start), dir(dir) {
+		;
+	}
+
+};
+
+#endif // GEO_RAY2_H

geo/Ray3.h

@@ -1,5 +1,5 @@
-#ifndef RAY3_H
-#define RAY3_H
+#ifndef GEO_RAY3_H
+#define GEO_RAY3_H
 
 #include "Point3.h"
 
@@ -25,4 +25,4 @@ public:
 
 };
 
-#endif // RAY3_H
+#endif // GEO_RAY3_H

geo/Sphere3.h

@@ -1,5 +1,5 @@
-#ifndef SPHERE3_H
-#define SPHERE3_H
+#ifndef GEO_SPHERE3_H
+#define GEO_SPHERE3_H
 
 #include "Point3.h"
 #include "Ray3.h"
@@ -172,7 +172,7 @@ public:
 
 		// center is within both ends, so is the radius
 		Point3 newCen = (out1 + out2) / 2;
-		float newRad = out1.getDistance(out2) / 2 * 1.001;	// slightly larger to prevent rounding issues
+		float newRad = out1.getDistance(out2) / 2 * 1.0001;	// slightly larger to prevent rounding issues
 		Sphere3 res(newCen, newRad);
 
 		// check
@@ -185,4 +185,4 @@ public:
 
 };
 
-#endif // SPHERE3_H
+#endif // GEO_SPHERE3_H

geo/volume/BVH.h

@@ -4,29 +4,36 @@
 #include <vector>
 #include <functional>
 
+#include "../Ray2.h"
+#include "../Ray3.h"
+
 #include "BoundingVolume.h"
-#include "BoundingVolumeAABB.h"
-#include "BoundingVolumeSphere.h"
+
+#include "BoundingVolumeAABB2.h"
+#include "BoundingVolumeCircle2.h"
+
+#include "BoundingVolumeAABB3.h"
+#include "BoundingVolumeSphere3.h"
 
 
 
-
-template <typename Element, typename Volume, typename Wrapper> class BVH {
+template <typename Element, typename Ray, typename Point, typename Volume, typename Wrapper> class BVH {
 
 protected:
 
 	/** one node within the tree */
 	struct BVHNode {
-		bool isLeaf = true;
+		bool isLeaf;
+		bool check;
 		Volume boundingVolume;
 		std::vector<BVHNode*> childNodes;
-		BVHNode(bool isLeaf = false) : isLeaf(isLeaf) {;}
+		BVHNode(bool isLeaf = false, bool check = true) : isLeaf(isLeaf), check(check) {;}
 	};
 
 	/** one leaf within the tree */
 	struct BVHLeaf : public BVHNode {
 		Element element;
-		BVHLeaf(const Element& e) : BVHNode(true), element(e) {;}
+		BVHLeaf(const Element& e, const bool check) : BVHNode(true, check), element(e) {;}
 	};
 
 	/** the tree's root */
@@ -40,10 +47,10 @@ public:
 	}
 
 	/** add a new volume to the tree */
-	void add(const Element& element) {
+	void add(const Element& element, const bool leafCheck = true) {
 
 		// create a new leaf for this element
-		BVHLeaf* leaf = new BVHLeaf(element);
+		BVHLeaf* leaf = new BVHLeaf(element, leafCheck);
 
 		// get the element's boundin volume
 		leaf->boundingVolume = getBoundingVolume(element);
@@ -63,17 +70,17 @@ public:
 		return max;
 	}
 
-	void getHits(const Ray3 ray, std::function<void(const Element&)> func) const {
-		//int tests = 0; int leafs = 0;
+
+	void getHits(const Ray& ray, const std::function<void(const Element&)>& func) const {
 		getHits(ray, &root, func);
-		//std::cout << tests << " " << leafs << std::endl;
 	}
 
-	void getHits(const Ray3 ray, const BVHNode* node, std::function<void(const Element&)> func) const {
+	// this one has to be as fast as possible!
+	static void getHits(const Ray& ray, const BVHNode* node, const std::function<void(const Element&)>& func) {
 		for (const BVHNode* sub : node->childNodes) {
-			if (sub->boundingVolume.intersects(ray)) {
+			if (!sub->check || sub->boundingVolume.intersects(ray)) {
 				if (sub->isLeaf) {
-					BVHLeaf* leaf = (BVHLeaf*)(sub);	// TODO: cast
+					const BVHLeaf* leaf = static_cast<const BVHLeaf*>(sub);
 					func(leaf->element);
 				} else {
 					getHits(ray, sub, func);
@@ -82,8 +89,50 @@ public:
 		}
 	}
 
+	/** get the tree's depth */
+	int getDepth() const {
+		return getDepth(&root, 1);
+	}
+
+
 private:
 
+	/** call the given function for each leaf within the given subtree */
+	void forEachLeaf(const BVHNode* n, std::function<void(const BVHNode*)> func) const {
+		if (n->isLeaf) {
+			func(n);
+		} else {
+			for (BVHNode* child : n->childNodes) {
+				forEachLeaf(child, func);
+			}
+		}
+	}
+
+	/** determine/approximate a new bounding volume around n1+n2 */
+	Volume getVolAround(const BVHNode* n1, const BVHNode* n2) const {
+		//return getVolAroundExact(n1, n2);
+		return getVolAroundAPX(n1, n2);
+	}
+
+	/** determine the bounding-volume around n1 and n2 by (slowly) calculating a new, exact volume based on all leaf-elements */
+	Volume getVolAroundExact(const BVHNode* n1, const BVHNode* n2) const {
+		std::vector<Point> verts;
+		auto onLeaf = [&] (const BVHNode* n) {
+			BVHLeaf* leaf = (BVHLeaf*) n;
+			std::vector<Point> subVerts = Wrapper::getVertices(leaf->element);
+			verts.insert(verts.end(),  subVerts.begin(), subVerts.end());
+		};
+		forEachLeaf(n1, onLeaf);
+		forEachLeaf(n2, onLeaf);
+		return Volume::fromVertices(verts);
+	}
+
+	/** approximate the bounding-volume around n1 and n2 by (quickly) joining their current volumes. the result might be unnecessarily large */
+	Volume getVolAroundAPX(const BVHNode* n1, const BVHNode* n2) const {
+		return Volume::join(n1->boundingVolume, n2->boundingVolume);
+	}
+
+
 	bool combineBest() {
 
 		// nothing to do?
@@ -104,7 +153,7 @@ private:
 				BVHNode* n1 = root.childNodes[i];
 				BVHNode* n2 = root.childNodes[j];
 
-				const Volume newVol = Volume::join(n1->boundingVolume, n2->boundingVolume);
+				const Volume newVol = getVolAround(n1,n2);
 				const float newVolSize = newVol.getVolumeSize();
 				if (newVolSize < best.volSize) {
 					best.vol = newVol;
@@ -226,13 +275,32 @@ private:
 
 	}
 
+	int getDepth(const BVHNode* node, const int cur) const {
+		if (node->isLeaf) {
+			return cur;
+		} else {
+			int res = cur;
+			for (const BVHNode* sub : node->childNodes) {
+				const int subDepth = getDepth(sub, cur+1);
+				if (subDepth > res) {res = subDepth;}
+			}
+			return res;
+		}
+	}
+
 	/** get a bounding-volume for the given element */
 	Volume getBoundingVolume(const Element& element) {
-		const std::vector<Point3> verts = Wrapper::getVertices(element);
+		const std::vector<Point> verts = Wrapper::getVertices(element);
 		return Volume::fromVertices(verts);
 	}
 
+};
 
+template <typename Element, typename Volume, typename Wrapper> class BVH3 : public BVH<Element, Ray3, Point3, Volume, Wrapper> {
+
+};
+
+template <typename Element, typename Volume, typename Wrapper> class BVH2 : public BVH<Element, Ray2, Point2, Volume, Wrapper> {
 
 };
 

geo/volume/BVHDebug.h

@@ -2,39 +2,52 @@
 #define BVHDEBUG_H
 
 #include "BVH.h"
+
+
+#include <KLib/misc/gnuplot/Gnuplot.h>
+
 #include <KLib/misc/gnuplot/GnuplotSplot.h>
 #include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
 #include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
 #include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
-#include <KLib/misc/gnuplot/Gnuplot.h>
+
+#include <KLib/misc/gnuplot/GnuplotPlot.h>
+#include <KLib/misc/gnuplot/GnuplotPlotElementPoints.h>
+#include <KLib/misc/gnuplot/GnuplotPlotElementColorLines.h>
+#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+
 #include "../BBox3.h"
 #include <random>
 
-/** adds some debug helpers to the BVH */
-template <typename Element, typename Volume, typename Wrapper> class BVHDebug : public BVH<Element, Volume, Wrapper> {
+///** adds some debug helpers to the BVH */
+//template <typename Element, typename Ray, typename Point, typename Volume, typename Wrapper> class BVHDebug : public BVH<Element, Ray, Point, Volume, Wrapper> {
+
+
+//};
+
+template <typename Element, typename Volume, typename Wrapper> class BVH3Debug : public BVH<Element, Ray3, Point3, Volume, Wrapper> {
+
+	using BVHNode = typename BVH<Element, Ray3, Point3, Volume, Wrapper>::BVHNode;
+	using BVHLeaf = typename BVH<Element, Ray3, Point3, Volume, Wrapper>::BVHLeaf;
 
-	using BVHNode = typename BVH<Element, Volume, Wrapper>::BVHNode;
-	using BVHLeaf = typename BVH<Element, Volume, Wrapper>::BVHLeaf;
 
 public:
 
-//	std::vecto<std::string> colors {
-//		"#888888", "#888800", "#008888", "#880088", "#ee0000", "#00ee00", "#0000ee"
-//	};
-
 	void show(int maxPts = 1500, bool showLeafs = true) {
 
 		std::stringstream out;
 
 		static K::Gnuplot gp;
 		K::GnuplotSplot plot;
-		K::GnuplotSplotElementColorPoints pVol;		plot.add(&pVol);		//pVol.setColor(K::GnuplotColor::fromRGB(128,128,128));
-		K::GnuplotSplotElementPoints pElemPoints;	plot.add(&pElemPoints);	pElemPoints.setColor(K::GnuplotColor::fromRGB(0,0,255));
-		K::GnuplotSplotElementLines pElemLines;		plot.add(&pElemLines);	pElemLines.getStroke().setColor(K::GnuplotColor::fromRGB(0,0,255));
 
-		const int depth = recurse(maxPts, showLeafs, 0, &this->root, pVol, pElemPoints, pElemLines);
+		K::GnuplotSplotElementColorPoints pVol;			plot.add(&pVol);		//pVol.setColor(K::GnuplotColor::fromRGB(128,128,128));
+		K::GnuplotSplotElementPoints pElemPoints;		plot.add(&pElemPoints);	pElemPoints.setColor(K::GnuplotColor::fromRGB(0,0,255));
+		K::GnuplotSplotElementLines pElemLines;			plot.add(&pElemLines);	pElemLines.getStroke().setColor(K::GnuplotColor::fromRGB(0,0,255));
 
-		plot.getAxisCB().setRange(0, depth);
+		const int maxDepth = this->getDepth();
+		recurse(maxPts, showLeafs, 0, &this->root, pVol, pElemPoints, pElemLines);
+
+		plot.getAxisCB().setRange(0, maxDepth);
 
 		gp << "set view equal xyz\n";
 		gp.draw(plot);
@@ -44,6 +57,31 @@ public:
 
 private:
 
+	Point3 getRandomPoint(BoundingVolumeSphere3 sphere) {
+		static std::minstd_rand gen;
+		std::uniform_real_distribution<float> dist(-1, +1);
+		Point3 dir = Point3(dist(gen), dist(gen), dist(gen)).normalized() * sphere.radius;
+		return sphere.center + dir;
+	}
+
+	void addLines(const Element& elem, K::GnuplotSplotElementLines& elemLines) {
+
+		std::vector<Point3> pts = Wrapper::getDebugLines(elem);
+
+		for (size_t i = 0; i< pts.size(); i+=2) {
+
+			const Point3 p1 = pts[i+0];
+			const Point3 p2 = pts[i+1];
+
+			K::GnuplotPoint3 gp1(p1.x, p1.y, p1.z);
+			K::GnuplotPoint3 gp2(p2.x, p2.y, p2.z);
+
+			elemLines.addSegment(gp1, gp2);
+
+		}
+
+	}
+
 	int recurse(int maxPts, bool showLeafs, int curDepth, const BVHNode* node, K::GnuplotSplotElementColorPoints& vol, K::GnuplotSplotElementPoints& pElemPoints, K::GnuplotSplotElementLines& elemLines) {
 
 		int resDepth = curDepth;
@@ -76,24 +114,53 @@ private:
 
 	}
 
-	Point3 getRandomPoint(BoundingVolumeSphere sphere) {
-		static std::minstd_rand gen;
-		std::uniform_real_distribution<float> dist(-1, +1);
-		Point3 dir = Point3(dist(gen), dist(gen), dist(gen)).normalized() * sphere.radius;
-		return sphere.center + dir;
+};
+
+template <typename Element, typename Volume, typename Wrapper> class BVH2Debug : public BVH<Element, Ray2, Point2, Volume, Wrapper> {
+
+	using BVHNode = typename BVH<Element, Ray2, Point2, Volume, Wrapper>::BVHNode;
+	using BVHLeaf = typename BVH<Element, Ray2, Point2, Volume, Wrapper>::BVHLeaf;
+
+public:
+
+	void show(int maxPts = 1500, bool showLeafs = true) {
+
+		std::stringstream out;
+
+		static K::Gnuplot gp;
+		K::GnuplotPlot plot;
+
+
+		K::GnuplotPlotElementColorLines pVolLines;	plot.add(&pVolLines);
+		K::GnuplotPlotElementPoints pElemPoints;	plot.add(&pElemPoints);	pElemPoints.setColor(K::GnuplotColor::fromRGB(0,0,255));
+		K::GnuplotPlotElementLines pElemLines;		plot.add(&pElemLines);	pElemLines.getStroke().setColor(K::GnuplotColor::fromRGB(0,0,255));
+
+		const int maxDepth = this->getDepth();
+		recurse(maxDepth, showLeafs, 0, &this->root, plot, pVolLines, pElemPoints, pElemLines);
+
+		plot.getObjects().reOrderByZIndex();
+		plot.getAxisCB().setRange(0, maxDepth);
+
+		gp << "set size ratio -1\n";
+		gp.draw(plot);
+		gp.flush();
+
 	}
 
-	void addLines(const Element& elem, K::GnuplotSplotElementLines& elemLines) {
+private:
 
-		std::vector<Point3> pts = Wrapper::getDebugLines(elem);
+
+	void addLines(const Element& elem, K::GnuplotPlotElementLines& elemLines) {
+
+		std::vector<Point2> pts = Wrapper::getDebugLines(elem);
 
 		for (size_t i = 0; i< pts.size(); i+=2) {
 
-			const Point3 p1 = pts[i+0];
-			const Point3 p2 = pts[i+1];
+			const Point2 p1 = pts[i+0];
+			const Point2 p2 = pts[i+1];
 
-			K::GnuplotPoint3 gp1(p1.x, p1.y, p1.z);
-			K::GnuplotPoint3 gp2(p2.x, p2.y, p2.z);
+			K::GnuplotPoint2 gp1(p1.x, p1.y);
+			K::GnuplotPoint2 gp2(p2.x, p2.y);
 
 			elemLines.addSegment(gp1, gp2);
 
@@ -101,6 +168,75 @@ private:
 
 	}
 
+	std::vector<std::string> colors = {
+		"#888800", "#444400", "#008888", "#004444", "#880088", "#440044", "#ee0000", "#880000", "#00ee00", "#008800", "#0000ee", "#000088",
+		"#888800", "#444400", "#008888", "#004444", "#880088", "#440044", "#ee0000", "#880000", "#00ee00", "#008800", "#0000ee", "#000088",
+		"#888800", "#444400", "#008888", "#004444", "#880088", "#440044", "#ee0000", "#880000", "#00ee00", "#008800", "#0000ee", "#000088"
+	};
+
+	void showVolume(const BoundingVolumeCircle2& circle, int maxDepth, int curDepth, K::GnuplotPlot& plot, K::GnuplotPlotElementColorLines& pVolLines) {
+		K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon();
+		for (int i = 0; i < 20; ++i) {
+			const float f = M_PI*2 * i / 19;
+			const Point2 p = circle.getPointAt(f);
+			poly->add(K::GnuplotCoordinate2(p.x, p.y, K::GnuplotCoordinateSystem::FIRST));
+			poly->getFill().setColor(K::GnuplotColor::fromHexStr(colors[maxDepth-curDepth]));
+			poly->getFill().setStyle(K::GnuplotFillStyle::SOLID);
+			poly->setZIndex(curDepth);
+		}
+		plot.getObjects().add(poly);
+	}
+
+	void showVolume(const BoundingVolumeAABB2& _aabb, int maxDepth, int curDepth, K::GnuplotPlot& plot, K::GnuplotPlotElementColorLines& pVolLines) {
+		BBox2 bbox2 = _aabb;
+		bbox2.grow( (10-curDepth) / 100.0f );
+//		pVolLines.add(K::GnuplotPoint2(bbox2.getMin().x, bbox2.getMin().y), curDepth);
+//		pVolLines.add(K::GnuplotPoint2(bbox2.getMax().x, bbox2.getMin().y), curDepth);
+//		pVolLines.add(K::GnuplotPoint2(bbox2.getMax().x, bbox2.getMax().y), curDepth);
+//		pVolLines.add(K::GnuplotPoint2(bbox2.getMin().x, bbox2.getMax().y), curDepth);
+//		pVolLines.add(K::GnuplotPoint2(bbox2.getMin().x, bbox2.getMin().y), curDepth);
+//		pVolLines.splitFace();
+		K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon();
+		poly->getStroke().setColor(K::GnuplotColor::fromHexStr(colors[maxDepth-curDepth]));
+		//poly->getFill().setColor(K::GnuplotColor::fromHexStr(colors[maxDepth-curDepth]));
+		//poly->getFill().setStyle(K::GnuplotFillStyle::SOLID);
+		poly->add(K::GnuplotCoordinate2(bbox2.getMin().x, bbox2.getMin().y, K::GnuplotCoordinateSystem::FIRST));
+		poly->add(K::GnuplotCoordinate2(bbox2.getMax().x, bbox2.getMin().y, K::GnuplotCoordinateSystem::FIRST));
+		poly->add(K::GnuplotCoordinate2(bbox2.getMax().x, bbox2.getMax().y, K::GnuplotCoordinateSystem::FIRST));
+		poly->add(K::GnuplotCoordinate2(bbox2.getMin().x, bbox2.getMax().y, K::GnuplotCoordinateSystem::FIRST));
+		poly->close();
+		poly->setZIndex(curDepth);
+		plot.getObjects().add(poly);
+	}
+
+	int recurse(int maxDepth, bool showLeafs, int curDepth, const BVHNode* node, K::GnuplotPlot& plot, K::GnuplotPlotElementColorLines& pVolLines, K::GnuplotPlotElementPoints& pElemPoints, K::GnuplotPlotElementLines& elemLines) {
+
+		int resDepth = curDepth;
+
+		for (BVHNode* sub : node->childNodes) {
+			resDepth = recurse(maxDepth, showLeafs, curDepth+1, sub, plot, pVolLines, pElemPoints, elemLines);
+		}
+
+		if (!node->isLeaf || showLeafs) {
+			if (node != &this->root) {
+				//const int numPts = maxPts / (curDepth+1);
+				showVolume(node->boundingVolume, maxDepth, curDepth, plot, pVolLines);
+			}
+		}
+
+		if (node->isLeaf) {
+			BVHLeaf* leaf = (BVHLeaf*) node;
+			std::vector<Point2> verts = Wrapper::getVertices(leaf->element);
+			for (const Point2 p : verts) {
+				pElemPoints.add(K::GnuplotPoint2(p.x, p.y));
+			}
+			addLines(leaf->element, elemLines);
+		}
+
+		return resDepth;
+
+	}
+
 };
 
 #endif // BVHDEBUG_H

geo/volume/BoundingVolumeAABB2.h

@@ -0,0 +1,36 @@
+#ifndef BOUNDINGVOLUMEAABB2_H
+#define BOUNDINGVOLUMEAABB2_H
+
+#include <vector>
+
+#include "BoundingVolume.h"
+#include "../BBox2.h"
+
+class BoundingVolumeAABB2 : public BBox2 {
+
+public:
+
+	BoundingVolumeAABB2() {;}
+
+	BoundingVolumeAABB2(const BBox2& bb) : BBox2(bb) {;}
+
+	float getVolumeSize() const  {
+		const float dx = getMax().x - getMin().x;
+		const float dy = getMax().y - getMin().y;
+		return (dx*dy);
+	}
+
+	/** construct a volume around the given point-set */
+	static BoundingVolumeAABB2 fromVertices(const std::vector<Point2>& verts) {
+		BoundingVolumeAABB2 bvs;
+		for (const Point2 p : verts) {bvs.add(p);}
+		return bvs;
+	}
+
+	static BoundingVolumeAABB2 join(const BoundingVolumeAABB2 a, const BoundingVolumeAABB2 b) {
+		return BoundingVolumeAABB2(BBox2::join(a, b));
+	}
+
+};
+
+#endif // BOUNDINGVOLUMEAABB2_H

geo/volume/BoundingVolumeBox.h

@@ -1,4 +0,0 @@
-#ifndef BOUNDINGVOLUMEBOX_H
-#define BOUNDINGVOLUMEBOX_H
-
-#endif // BOUNDINGVOLUMEBOX_H

geo/volume/BoundingVolumeCircle2.h

@@ -0,0 +1,38 @@
+#ifndef BOUDINGVOLUMECIRCLE2_H
+#define BOUDINGVOLUMECIRCLE2_H
+
+#include <vector>
+
+#include "BoundingVolume.h"
+#include "../Circle2.h"
+
+class BoundingVolumeCircle2 : public Circle2 {
+
+public:
+
+	BoundingVolumeCircle2() {;}
+
+	BoundingVolumeCircle2(const Circle2& c) : Circle2(c) {;}
+
+	float getVolumeSize() const  {
+		return M_PI * (radius*radius);
+	}
+
+	bool intersects(const Ray2 ray) const {
+		return Circle2::intersects(ray);
+	}
+
+	/** construct a volume around the given point-set */
+	static BoundingVolumeCircle2 fromVertices(const std::vector<Point2>& verts) {
+		BoundingVolumeCircle2 bvs;
+		bvs.adjustToPointSet(verts);
+		return bvs;
+	}
+
+	static BoundingVolumeCircle2 join(const BoundingVolumeCircle2 a, const BoundingVolumeCircle2 b) {
+		return BoundingVolumeCircle2(Circle2::join(a, b));
+	}
+
+};
+
+#endif // BOUDINGVOLUMECIRCLE2_H

geo/volume/BoundingVolumeHierarchy.h

@@ -1,4 +0,0 @@
-#ifndef BOUNDINGVOLUMEHIERARCHY_H
-#define BOUNDINGVOLUMEHIERARCHY_H
-
-#endif // BOUNDINGVOLUMEHIERARCHY_H

geo/volume/BoundingVolumeSphere3.h

@@ -5,13 +5,13 @@
 #include "../Sphere3.h"
 #include "../Point3.h"
 
-class BoundingVolumeSphere : public BoundingVolume, public Sphere3 {
+class BoundingVolumeSphere3 : public BoundingVolume, public Sphere3 {
 
 public:
 
-	BoundingVolumeSphere() {;}
+	BoundingVolumeSphere3() {;}
 
-	BoundingVolumeSphere(const Sphere3& s) : Sphere3(s) {;}
+	BoundingVolumeSphere3(const Sphere3& s) : Sphere3(s) {;}
 
 	float getVolumeSize() const  {
 		return (4.0f / 3.0f) * M_PI * (radius*radius*radius);
@@ -27,25 +27,25 @@ public:
 
 	/** does the volume intersect with the given volume? */
 	bool intersects(const BoundingVolume& other) const  {
-		const BoundingVolumeSphere& sphere = (const BoundingVolumeSphere&) other;
+		const BoundingVolumeSphere3& sphere = (const BoundingVolumeSphere3&) other;
 		return Sphere3::intersects(sphere);
 	}
 
 	/** does the volume contain the given volume? */
 	bool contains(const BoundingVolume& other) const {
-		const BoundingVolumeSphere& sphere = (const BoundingVolumeSphere&) other;
+		const BoundingVolumeSphere3& sphere = (const BoundingVolumeSphere3&) other;
 		return Sphere3::contains(sphere);
 	}
 
 	/** construct a volume around the given point-set */
-	static BoundingVolumeSphere fromVertices(const std::vector<Point3>& verts) {
-		BoundingVolumeSphere bvs;
+	static BoundingVolumeSphere3 fromVertices(const std::vector<Point3>& verts) {
+		BoundingVolumeSphere3 bvs;
 		bvs.adjustToPointSet(verts);
 		return bvs;
 	}
 
-	static BoundingVolumeSphere join(const BoundingVolumeSphere a, const BoundingVolumeSphere b) {
-		return BoundingVolumeSphere(Sphere3::join(a, b));
+	static BoundingVolumeSphere3 join(const BoundingVolumeSphere3 a, const BoundingVolumeSphere3 b) {
+		return BoundingVolumeSphere3(Sphere3::join(a, b));
 	}
 
 };

lib/gpc/gpc.cpp.h

@@ -54,8 +54,8 @@ Copyright: (C) Advanced Interfaces Group,
 #define TRUE               1
 #endif
 
-#define LEFT               0
-#define RIGHT              1
+#define GPC_LEFT               0
+#define GPC_RIGHT              1
 
 #define ABOVE              0
 #define BELOW              1
@@ -504,8 +504,8 @@ static edge_node *build_lmt(lmt_node **lmt, sb_tree **sbtree,
                        &(e[i + 1]) : NULL;
             e[i].pred= ((num_edges > 1) && (i > 0)) ? &(e[i - 1]) : NULL;
             e[i].next_bound= NULL;
-            e[i].bside[CLIP]= (op == GPC_DIFF) ? RIGHT : LEFT;
-            e[i].bside[SUBJ]= LEFT;
+			e[i].bside[CLIP]= (op == GPC_DIFF) ? GPC_RIGHT : GPC_LEFT;
+			e[i].bside[SUBJ]= GPC_LEFT;
           }
           insert_bound(bound_list(lmt, edge_table[min].vertex.y), e);
         }
@@ -554,8 +554,8 @@ static edge_node *build_lmt(lmt_node **lmt, sb_tree **sbtree,
                        &(e[i + 1]) : NULL;
             e[i].pred= ((num_edges > 1) && (i > 0)) ? &(e[i - 1]) : NULL;
             e[i].next_bound= NULL;
-            e[i].bside[CLIP]= (op == GPC_DIFF) ? RIGHT : LEFT;
-            e[i].bside[SUBJ]= LEFT;
+			e[i].bside[CLIP]= (op == GPC_DIFF) ? GPC_RIGHT : GPC_LEFT;
+			e[i].bside[SUBJ]= GPC_LEFT;
           }
           insert_bound(bound_list(lmt, edge_table[min].vertex.y), e);
         }
@@ -736,7 +736,7 @@ static int count_contours(polygon_node *polygon)
     {
       /* Count the vertices in the current contour */
       nv= 0;
-      for (v= polygon->proxy->v[LEFT]; v; v= v->next)
+	  for (v= polygon->proxy->v[GPC_LEFT]; v; v= v->next)
         nv++;
 
       /* Record valid vertex counts in the active field */
@@ -748,7 +748,7 @@ static int count_contours(polygon_node *polygon)
       else
       {
         /* Invalid contour: just free the heap */
-        for (v= polygon->proxy->v[LEFT]; v; v= nextv)
+		for (v= polygon->proxy->v[GPC_LEFT]; v; v= nextv)
         {
           nextv= v->next;
           FREE(v);
@@ -770,10 +770,10 @@ static void add_left(polygon_node *p, double x, double y)
   nv->y= y;
 
   /* Add vertex nv to the left end of the polygon's vertex list */
-  nv->next= p->proxy->v[LEFT];
+  nv->next= p->proxy->v[GPC_LEFT];
 
-  /* Update proxy->[LEFT] to point to nv */
-  p->proxy->v[LEFT]= nv;
+  /* Update proxy->[GPC_LEFT] to point to nv */
+  p->proxy->v[GPC_LEFT]= nv;
 }
 
 
@@ -787,8 +787,8 @@ static void merge_left(polygon_node *p, polygon_node *q, polygon_node *list)
   if (p->proxy != q->proxy)
   {
     /* Assign p's vertex list to the left end of q's list */
-    p->proxy->v[RIGHT]->next= q->proxy->v[LEFT];
-    q->proxy->v[LEFT]= p->proxy->v[LEFT];
+	p->proxy->v[GPC_RIGHT]->next= q->proxy->v[GPC_LEFT];
+	q->proxy->v[GPC_LEFT]= p->proxy->v[GPC_LEFT];
 
     /* Redirect any p->proxy references to q->proxy */
     
@@ -815,10 +815,10 @@ static void add_right(polygon_node *p, double x, double y)
   nv->next= NULL;
 
   /* Add vertex nv to the right end of the polygon's vertex list */
-  p->proxy->v[RIGHT]->next= nv;
+  p->proxy->v[GPC_RIGHT]->next= nv;
 
-  /* Update proxy->v[RIGHT] to point to nv */
-  p->proxy->v[RIGHT]= nv;
+  /* Update proxy->v[GPC_RIGHT] to point to nv */
+  p->proxy->v[GPC_RIGHT]= nv;
 }
 
 
@@ -832,8 +832,8 @@ static void merge_right(polygon_node *p, polygon_node *q, polygon_node *list)
   if (p->proxy != q->proxy)
   {
     /* Assign p's vertex list to the right end of q's list */
-    q->proxy->v[RIGHT]->next= p->proxy->v[LEFT];
-    q->proxy->v[RIGHT]= p->proxy->v[RIGHT];
+	q->proxy->v[GPC_RIGHT]->next= p->proxy->v[GPC_LEFT];
+	q->proxy->v[GPC_RIGHT]= p->proxy->v[GPC_RIGHT];
 
     /* Redirect any p->proxy references to q->proxy */
     for (target= p->proxy; list; list= list->next)
@@ -869,9 +869,9 @@ static void add_local_min(polygon_node **p, edge_node *edge,
   (*p)->active= TRUE;
   (*p)->next= existing_min;
 
-  /* Make v[LEFT] and v[RIGHT] point to new vertex nv */
-  (*p)->v[LEFT]= nv;
-  (*p)->v[RIGHT]= nv;
+  /* Make v[GPC_LEFT] and v[GPC_RIGHT] point to new vertex nv */
+  (*p)->v[GPC_LEFT]= nv;
+  (*p)->v[GPC_RIGHT]= nv;
 
   /* Assign polygon p to the edge */
   edge->outp[ABOVE]= *p;
@@ -911,10 +911,10 @@ static void new_tristrip(polygon_node **tn, edge_node *edge,
   {
     MALLOC(*tn, sizeof(polygon_node), "tristrip node creation", polygon_node);
     (*tn)->next= NULL;
-    (*tn)->v[LEFT]= NULL;
-    (*tn)->v[RIGHT]= NULL;
+	(*tn)->v[GPC_LEFT]= NULL;
+	(*tn)->v[GPC_RIGHT]= NULL;
     (*tn)->active= 1;
-    add_vertex(&((*tn)->v[LEFT]), x, y); 
+	add_vertex(&((*tn)->v[GPC_LEFT]), x, y);
     edge->outp[ABOVE]= *tn;
   }
   else
@@ -1125,7 +1125,7 @@ void gpc_polygon_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip,
   polygon_node  *out_poly= NULL, *p, *q, *poly, *npoly, *cf= NULL;
   vertex_node   *vtx, *nv;
   h_state        horiz[2];
-  int            in[2], exists[2], parity[2]= {LEFT, LEFT};
+  int            in[2], exists[2], parity[2]= {GPC_LEFT, GPC_LEFT};
   int            c, v, contributing, search, scanbeam= 0, sbt_entries= 0;
   int            vclass, bl, br, tl, tr;
   double        *sbt= NULL, xb, px, yb, yt, dy, ix, iy;
@@ -1178,7 +1178,7 @@ void gpc_polygon_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip,
 
   /* Invert clip polygon for difference operation */
   if (op == GPC_DIFF)
-    parity[CLIP]= RIGHT;
+	parity[CLIP]= GPC_RIGHT;
 
   local_min= lmt;
 
@@ -1721,7 +1721,7 @@ void gpc_polygon_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip,
           "vertex creation", gpc_vertex);
       
         v= result->contour[c].num_vertices - 1;
-        for (vtx= poly->proxy->v[LEFT]; vtx; vtx= nv)
+		for (vtx= poly->proxy->v[GPC_LEFT]; vtx; vtx= nv)
         {
           nv= vtx->next;
           result->contour[c].vertex[v].x= vtx->x;
@@ -1786,7 +1786,7 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip,
   vertex_node   *lt, *ltn, *rt, *rtn;
   h_state        horiz[2];
   vertex_type    cft;
-  int            in[2], exists[2], parity[2]= {LEFT, LEFT};
+  int            in[2], exists[2], parity[2]= {GPC_LEFT, GPC_LEFT};
   int            s, v, contributing, search, scanbeam= 0, sbt_entries= 0;
   int            vclass, bl, br, tl, tr;
   double        *sbt= NULL, xb, px, nx, yb, yt, dy, ix, iy;
@@ -1831,7 +1831,7 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip,
 
   /* Invert clip polygon for difference operation */
   if (op == GPC_DIFF)
-    parity[CLIP]= RIGHT;
+	parity[CLIP]= GPC_RIGHT;
 
   local_min= lmt;
 
@@ -1990,17 +1990,17 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip,
           case ERI:
             edge->outp[ABOVE]= cf->outp[ABOVE];
             if (xb != cf->xb)
-              VERTEX(edge, ABOVE, RIGHT, xb, yb);
+			  VERTEX(edge, ABOVE, GPC_RIGHT, xb, yb);
             cf= NULL;
             break;
           case ELI:
-            VERTEX(edge, BELOW, LEFT, xb, yb);
+			VERTEX(edge, BELOW, GPC_LEFT, xb, yb);
             edge->outp[ABOVE]= NULL;
             cf= edge;
             break;
           case EMX:
             if (xb != cf->xb)
-              VERTEX(edge, BELOW, RIGHT, xb, yb);
+			  VERTEX(edge, BELOW, GPC_RIGHT, xb, yb);
             edge->outp[ABOVE]= NULL;
             cf= NULL;
             break;
@@ -2008,11 +2008,11 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip,
             if (cft == LED)
 	    {
               if (cf->bot.y != yb)
-                VERTEX(cf, BELOW, LEFT, cf->xb, yb);
+				VERTEX(cf, BELOW, GPC_LEFT, cf->xb, yb);
               new_tristrip(&tlist, cf, cf->xb, yb);
 	    }
             edge->outp[ABOVE]= cf->outp[ABOVE];
-            VERTEX(edge, ABOVE, RIGHT, xb, yb);
+			VERTEX(edge, ABOVE, GPC_RIGHT, xb, yb);
             break;
           case ILI:
             new_tristrip(&tlist, edge, xb, yb);
@@ -2023,33 +2023,33 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip,
             if (cft == LED)
 	    {
               if (cf->bot.y != yb)
-                VERTEX(cf, BELOW, LEFT, cf->xb, yb);
+				VERTEX(cf, BELOW, GPC_LEFT, cf->xb, yb);
               new_tristrip(&tlist, cf, cf->xb, yb);
 	    }
-            VERTEX(edge, BELOW, RIGHT, xb, yb);
+			VERTEX(edge, BELOW, GPC_RIGHT, xb, yb);
             edge->outp[ABOVE]= NULL;
             break;
           case IMX:
-            VERTEX(edge, BELOW, LEFT, xb, yb);
+			VERTEX(edge, BELOW, GPC_LEFT, xb, yb);
             edge->outp[ABOVE]= NULL;
             cft= IMX;
             break;
 	  case IMM:
-            VERTEX(edge, BELOW, LEFT, xb, yb);
+			VERTEX(edge, BELOW, GPC_LEFT, xb, yb);
             edge->outp[ABOVE]= cf->outp[ABOVE];
             if (xb != cf->xb)
-              VERTEX(cf, ABOVE, RIGHT, xb, yb);
+			  VERTEX(cf, ABOVE, GPC_RIGHT, xb, yb);
             cf= edge;
             break;
           case EMM:
-            VERTEX(edge, BELOW, RIGHT, xb, yb);
+			VERTEX(edge, BELOW, GPC_RIGHT, xb, yb);
             edge->outp[ABOVE]= NULL;
             new_tristrip(&tlist, edge, xb, yb);
             cf= edge;
             break;
           case LED:
             if (edge->bot.y == yb)
-              VERTEX(edge, BELOW, LEFT, xb, yb);
+			  VERTEX(edge, BELOW, GPC_LEFT, xb, yb);
             edge->outp[ABOVE]= edge->outp[BELOW];
             cf= edge;
             cft= LED;
@@ -2060,21 +2060,21 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip,
 	    {
               if (cf->bot.y == yb)
 	      {
-                VERTEX(edge, BELOW, RIGHT, xb, yb);
+				VERTEX(edge, BELOW, GPC_RIGHT, xb, yb);
 	      }
               else
 	      {
                 if (edge->bot.y == yb)
 		{
-                  VERTEX(cf, BELOW, LEFT, cf->xb, yb);
-                  VERTEX(edge, BELOW, RIGHT, xb, yb);
+				  VERTEX(cf, BELOW, GPC_LEFT, cf->xb, yb);
+				  VERTEX(edge, BELOW, GPC_RIGHT, xb, yb);
 		}
 	      }
 	    }
             else
 	    {
-              VERTEX(edge, BELOW, RIGHT, xb, yb);
-              VERTEX(edge, ABOVE, RIGHT, xb, yb);
+			  VERTEX(edge, BELOW, GPC_RIGHT, xb, yb);
+			  VERTEX(edge, ABOVE, GPC_RIGHT, xb, yb);
 	    }
             cf= NULL;
             break;
@@ -2199,8 +2199,8 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip,
             if (p)
             {
               P_EDGE(prev_edge, e0, ABOVE, px, iy);
-              VERTEX(prev_edge, ABOVE, LEFT, px, iy);
-              VERTEX(e0, ABOVE, RIGHT, ix, iy);
+			  VERTEX(prev_edge, ABOVE, GPC_LEFT, px, iy);
+			  VERTEX(e0, ABOVE, GPC_RIGHT, ix, iy);
               e1->outp[ABOVE]= e0->outp[ABOVE];
               e0->outp[ABOVE]= NULL;
             }
@@ -2209,8 +2209,8 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip,
             if (q)
             {
               N_EDGE(next_edge, e1, ABOVE, nx, iy);
-              VERTEX(e1, ABOVE, LEFT, ix, iy);
-              VERTEX(next_edge, ABOVE, RIGHT, nx, iy);
+			  VERTEX(e1, ABOVE, GPC_LEFT, ix, iy);
+			  VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy);
               e0->outp[ABOVE]= e1->outp[ABOVE];
               e1->outp[ABOVE]= NULL;
             }
@@ -2218,29 +2218,29 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip,
           case EMX:
             if (p && q)
             {
-              VERTEX(e0, ABOVE, LEFT, ix, iy);
+			  VERTEX(e0, ABOVE, GPC_LEFT, ix, iy);
               e0->outp[ABOVE]= NULL;
               e1->outp[ABOVE]= NULL;
             }
             break;
           case IMN:
             P_EDGE(prev_edge, e0, ABOVE, px, iy);
-            VERTEX(prev_edge, ABOVE, LEFT, px, iy);
+			VERTEX(prev_edge, ABOVE, GPC_LEFT, px, iy);
             N_EDGE(next_edge, e1, ABOVE, nx, iy);
-            VERTEX(next_edge, ABOVE, RIGHT, nx, iy);
+			VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy);
             new_tristrip(&tlist, prev_edge, px, iy); 
             e1->outp[ABOVE]= prev_edge->outp[ABOVE];
-            VERTEX(e1, ABOVE, RIGHT, ix, iy);
+			VERTEX(e1, ABOVE, GPC_RIGHT, ix, iy);
             new_tristrip(&tlist, e0, ix, iy);
             next_edge->outp[ABOVE]= e0->outp[ABOVE];
-            VERTEX(next_edge, ABOVE, RIGHT, nx, iy);
+			VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy);
             break;
           case ILI:
             if (p)
             {
-              VERTEX(e0, ABOVE, LEFT, ix, iy);
+			  VERTEX(e0, ABOVE, GPC_LEFT, ix, iy);
               N_EDGE(next_edge, e1, ABOVE, nx, iy);
-              VERTEX(next_edge, ABOVE, RIGHT, nx, iy);
+			  VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy);
               e1->outp[ABOVE]= e0->outp[ABOVE];
               e0->outp[ABOVE]= NULL;
             }
@@ -2248,9 +2248,9 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip,
           case IRI:
             if (q)
             {
-              VERTEX(e1, ABOVE, RIGHT, ix, iy);
+			  VERTEX(e1, ABOVE, GPC_RIGHT, ix, iy);
               P_EDGE(prev_edge, e0, ABOVE, px, iy);
-              VERTEX(prev_edge, ABOVE, LEFT, px, iy);
+			  VERTEX(prev_edge, ABOVE, GPC_LEFT, px, iy);
               e0->outp[ABOVE]= e1->outp[ABOVE];
               e1->outp[ABOVE]= NULL;
             }
@@ -2258,40 +2258,40 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip,
           case IMX:
             if (p && q)
             {
-              VERTEX(e0, ABOVE, RIGHT, ix, iy);
-              VERTEX(e1, ABOVE, LEFT, ix, iy);
+			  VERTEX(e0, ABOVE, GPC_RIGHT, ix, iy);
+			  VERTEX(e1, ABOVE, GPC_LEFT, ix, iy);
               e0->outp[ABOVE]= NULL;
               e1->outp[ABOVE]= NULL;
               P_EDGE(prev_edge, e0, ABOVE, px, iy);
-              VERTEX(prev_edge, ABOVE, LEFT, px, iy);
+			  VERTEX(prev_edge, ABOVE, GPC_LEFT, px, iy);
               new_tristrip(&tlist, prev_edge, px, iy);
               N_EDGE(next_edge, e1, ABOVE, nx, iy);
-              VERTEX(next_edge, ABOVE, RIGHT, nx, iy);
+			  VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy);
               next_edge->outp[ABOVE]= prev_edge->outp[ABOVE];
-              VERTEX(next_edge, ABOVE, RIGHT, nx, iy);
+			  VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy);
             }
             break;
           case IMM:
             if (p && q)
             {
-              VERTEX(e0, ABOVE, RIGHT, ix, iy);
-              VERTEX(e1, ABOVE, LEFT, ix, iy);
+			  VERTEX(e0, ABOVE, GPC_RIGHT, ix, iy);
+			  VERTEX(e1, ABOVE, GPC_LEFT, ix, iy);
               P_EDGE(prev_edge, e0, ABOVE, px, iy);
-              VERTEX(prev_edge, ABOVE, LEFT, px, iy);
+			  VERTEX(prev_edge, ABOVE, GPC_LEFT, px, iy);
               new_tristrip(&tlist, prev_edge, px, iy);
               N_EDGE(next_edge, e1, ABOVE, nx, iy);
-              VERTEX(next_edge, ABOVE, RIGHT, nx, iy);
+			  VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy);
               e1->outp[ABOVE]= prev_edge->outp[ABOVE];
-              VERTEX(e1, ABOVE, RIGHT, ix, iy);
+			  VERTEX(e1, ABOVE, GPC_RIGHT, ix, iy);
               new_tristrip(&tlist, e0, ix, iy);
               next_edge->outp[ABOVE]= e0->outp[ABOVE];
-              VERTEX(next_edge, ABOVE, RIGHT, nx, iy);
+			  VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy);
             }
             break;
           case EMM:
             if (p && q)
             {
-              VERTEX(e0, ABOVE, LEFT, ix, iy);
+			  VERTEX(e0, ABOVE, GPC_LEFT, ix, iy);
               new_tristrip(&tlist, e1, ix, iy);
               e0->outp[ABOVE]= e1->outp[ABOVE];
             }
@@ -2408,13 +2408,13 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip,
         v= 0;
         if (INVERT_TRISTRIPS)
         {
-          lt= tn->v[RIGHT];
-          rt= tn->v[LEFT];
+		  lt= tn->v[GPC_RIGHT];
+		  rt= tn->v[GPC_LEFT];
         }
         else
         {
-          lt= tn->v[LEFT];
-          rt= tn->v[RIGHT];
+		  lt= tn->v[GPC_LEFT];
+		  rt= tn->v[GPC_RIGHT];
         }
         while (lt || rt)
         {
@@ -2442,12 +2442,12 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip,
       else
       {
         /* Invalid tristrip: just free the heap */
-        for (lt= tn->v[LEFT]; lt; lt= ltn)
+		for (lt= tn->v[GPC_LEFT]; lt; lt= ltn)
         {
           ltn= lt->next;
           FREE(lt);
         }
-        for (rt= tn->v[RIGHT]; rt; rt=rtn)
+		for (rt= tn->v[GPC_RIGHT]; rt; rt=rtn)
         {
           rtn= rt->next;
           FREE(rt);

main.cpp

@@ -36,7 +36,8 @@ int main(int argc, char** argv) {
 	//::testing::GTEST_FLAG(filter) = "*Matrix4*";
 	//::testing::GTEST_FLAG(filter) = "*Sphere3*";
 
-	::testing::GTEST_FLAG(filter) = "*RayTrace3*";
+	::testing::GTEST_FLAG(filter) = "Geo_*";
+	//::testing::GTEST_FLAG(filter) = "*RayTrace3*";
 	//::testing::GTEST_FLAG(filter) = "*BVH*";
 
 

math/DrawList.h

@@ -58,7 +58,7 @@ public:
 	}
 
 	/** ctor with custom seed */
-	DrawList(const uint32_t seed) : cumProbability(0), gen(defRndGen(seed)) {
+	DrawList(const uint32_t seed) : cumProbability(0), defRndGen(seed), gen(defRndGen) {
 		;
 	}
 

tests/geo/TestAngle.cpp

@@ -4,7 +4,7 @@
 
 #include "../../geo/Angle.h"
 
-TEST(Angle, dir) {
+TEST(Geo_Angle, dir) {
 
 	// angle -> pointer -> angle
 	ASSERT_NEAR(0, Angle::getRAD_2PI(Angle::getPointer(0)), 0.0001);
@@ -14,7 +14,7 @@ TEST(Angle, dir) {
 
 }
 
-TEST(Angle, safe) {
+TEST(Geo_Angle, safe) {
 
 	ASSERT_EQ(0,	(int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(0)))));
 	ASSERT_EQ(0,	(int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(360)))));
@@ -35,7 +35,7 @@ TEST(Angle, safe) {
 
 }
 
-TEST(Angle, calc) {
+TEST(Geo_Angle, calc) {
 
 	ASSERT_EQ(0,	(int)Angle::getDEG_360(0,0, +1,0));		// to the right
 	ASSERT_EQ(90,	(int)Angle::getDEG_360(0,0, 0,+1));		// upwards
@@ -49,7 +49,7 @@ TEST(Angle, calc) {
 
 }
 
-TEST(Angle, signedDiff) {
+TEST(Geo_Angle, signedDiff) {
 
 	const float d = 0.00001f;
 	ASSERT_NEAR(+M_PI/2,	Angle::getSignedDiffRAD_2PI(0, M_PI/2), d);			// CCW
@@ -62,7 +62,7 @@ TEST(Angle, signedDiff) {
 
 }
 
-TEST(Angle, diff) {
+TEST(Geo_Angle, diff) {
 
 	const float r = Angle::getRAD_2PI(0,0,	+1,0);		// to the right
 	const float u = Angle::getRAD_2PI(0,0, 0,+1);		// upwards

tests/geo/TestBBox2.cpp

@@ -4,7 +4,7 @@
 
 #include "../../geo/BBox2.h"
 
-TEST(BBox, noIntersect) {
+TEST(Geo_BBox2, noIntersect) {
 
 	BBox2 bb(Point2(1,1), Point2(3,3));
 
@@ -21,7 +21,8 @@ TEST(BBox, noIntersect) {
 
 }
 
-TEST(BBox, noDirectIntersect) {
+/*
+TEST(Geo_BBox2, noDirectIntersect) {
 
 	BBox2 bb(Point2(1,1), Point2(3,3));
 
@@ -37,9 +38,10 @@ TEST(BBox, noDirectIntersect) {
 	ASSERT_FALSE(bb.intersects(l4));
 
 }
+*/
 
 
-TEST(BBox, positiveIntersect) {
+TEST(Geo_BBox2, positiveIntersect) {
 
 	BBox2 bb(Point2(1,1), Point2(3,3));
 

tests/geo/TestBBox3.cpp

@@ -0,0 +1,18 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+
+#include "../../geo/BBox3.h"
+
+TEST(Geo_BBox3, noIntersect) {
+
+	BBox3 bb1(Point3(0,0,0), Point3(1,1,1));
+	BBox3 bb2(Point3(1,1,1), Point3(2,3,4));
+
+	BBox3 joined = BBox3::join(bb1, bb2);
+	ASSERT_EQ(Point3(2,3,4), joined.getMax());
+	ASSERT_EQ(Point3(0,0,0), joined.getMin());
+
+}
+
+#endif

tests/geo/TestBVH2.cpp

@@ -0,0 +1,176 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+
+#include "../../geo/volume/BVH.h"
+#include "../../geo/volume/BVHDebug.h"
+
+#include "../../geo/BBox2.h"
+#include "../../geo/Line2.h"
+
+#include "../../floorplan/v2/Floorplan.h"
+#include "../../floorplan/v2/FloorplanReader.h"
+#include "../../wifi/estimate/ray3/ModelFactory.h"
+
+struct WrapperBBox2 {
+
+	static std::vector<Point2> getVertices(const BBox2& bbox) {
+		return {bbox.getMin(), bbox.getMax()};
+	}
+
+	static std::vector<Point2> getDebugLines(const BBox2& bbox) {
+
+		Point2 p1(bbox.getMin().x, bbox.getMin().y);
+		Point2 p2(bbox.getMax().x, bbox.getMin().y);
+		Point2 p3(bbox.getMax().x, bbox.getMax().y);
+		Point2 p4(bbox.getMin().x, bbox.getMax().y);
+
+		std::vector<Point2> res;
+		res.push_back(p1); res.push_back(p2);
+		res.push_back(p2); res.push_back(p3);
+		res.push_back(p3); res.push_back(p4);
+		res.push_back(p4); res.push_back(p1);
+		return res;
+
+	}
+
+};
+
+struct WrapperLine2 {
+
+	static std::vector<Point2> getVertices(const Line2& l) {
+		return {l.p1, l.p2};
+	}
+
+	static std::vector<Point2> getDebugLines(const Line2& l) {
+		return {l.p1, l.p2};
+	}
+
+};
+
+
+/*
+TEST(Geo_BVH2, tree) {
+
+	BVH3Debug<BBox3, BoundingVolumeSphere3, Wrapper> tree;
+
+	BBox3 bb1(Point3(0,0,0), Point3(1,1,1));
+	tree.add(bb1);
+
+	BBox3 bb2(Point3(-1,-1,-1), Point3(0,0,0));
+	tree.add(bb2);
+
+	tree.optimize();
+	tree.show();
+
+	int i = 0; (void) i;
+
+}
+
+TEST(Geo_BVH2, tree2) {
+
+	BVH3Debug<BBox3, BoundingVolumeSphere3, Wrapper> tree;
+
+	BBox3 bb1(Point3(0,0,0), Point3(1,1,1));
+	tree.add(bb1);
+
+	BBox3 bb2(Point3(-1,0,0), Point3(0,1,1));
+	tree.add(bb2);
+
+	tree.optimize();
+	tree.show();
+
+	int i = 0; (void) i;
+
+}
+
+TEST(Geo_BVH2, tree3) {
+
+	BVH3Debug<BBox3, BoundingVolumeSphere3, Wrapper> tree;
+
+	BBox3 bb1 = BBox3::around(Point3(+0.5, +0.5, 0.0), Point3(0.25, 0.25, 0.25));
+	tree.add(bb1);
+
+	BBox3 bb2 = BBox3::around(Point3(-0.5, +0.5, 0.0), Point3(0.25, 0.25, 0.25));
+	tree.add(bb2);
+
+	BBox3 bb3 = BBox3::around(Point3(-0.0, +0.5, 0.0), Point3(0.36, 0.36, 0.36));
+	tree.add(bb3);
+
+	BBox3 bb4 = BBox3::around(Point3(-0.0, +0.0, 0.0), Point3(0.5, 0.5, 0.5));
+	tree.add(bb4);
+
+	tree.optimize(1);
+	tree.show();
+
+	tree.optimize(1);
+	tree.show();
+
+	tree.optimize(1);
+	tree.show();
+
+	int i = 0; (void) i;
+
+}
+
+TEST(Geo_BVH2, treeMap) {
+
+	std::string file = "/apps/SHL39.xml";
+	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file);
+
+	ModelFactory fac(map);
+	fac.setExportCeilings(false);
+	fac.setFloors({map->floors[3]});
+	std::vector<Obstacle3D> obs = fac.triangulize();
+
+	BVH3Debug<Obstacle3D, BoundingVolumeSphere3, WrapperObs3D> tree;
+
+	for (const Obstacle3D& o : obs) {
+		tree.add(o);
+	}
+
+	//tree.show(150);
+
+	//int rounds = tree.optimize();
+
+	for (int i = 0; i < 200; ++i) {
+		tree.optimize(1);
+		//if (i%3==0) {
+		tree.show(250, false);
+		//}
+	}
+
+	int i = 0; (void) i;
+
+}
+*/
+TEST(Geo_BVH2, treeRandom) {
+
+	//BVH2Debug<Line2, BoundingVolumeCircle2, WrapperLine2> tree;
+	BVH2Debug<Line2, BoundingVolumeAABB2, WrapperLine2> tree;
+
+	std::minstd_rand gen;
+	std::uniform_real_distribution<float> dPos(-4.0, +4.0);
+	std::uniform_real_distribution<float> dDir(+0.1, +0.5);
+
+	for (int i = 0; i < 50; ++i) {
+		const Point2 pos(dPos(gen), dPos(gen));
+		const Point2 dir(dDir(gen), dDir(gen));
+		const Line2 line(pos, pos+dir);
+		tree.add(line);
+	}
+
+	tree.show();
+
+	if (1 == 0) {
+		for (int i = 0; i < 100; ++i) {
+			tree.optimize(1);
+			tree.show();
+			usleep(1000*100);
+		}
+	}
+	int i = 0; (void) i;
+
+}
+
+#endif

tests/geo/TestBVH3.cpp

@@ -77,7 +77,7 @@ struct WrapperObs3D {
 
 TEST(BVH, tree) {
 
-	BVHDebug<BBox3, BoundingVolumeSphere, Wrapper> tree;
+	BVH3Debug<BBox3, BoundingVolumeSphere3, Wrapper> tree;
 
 	BBox3 bb1(Point3(0,0,0), Point3(1,1,1));
 	tree.add(bb1);
@@ -94,7 +94,7 @@ TEST(BVH, tree) {
 
 TEST(BVH, tree2) {
 
-	BVHDebug<BBox3, BoundingVolumeSphere, Wrapper> tree;
+	BVH3Debug<BBox3, BoundingVolumeSphere3, Wrapper> tree;
 
 	BBox3 bb1(Point3(0,0,0), Point3(1,1,1));
 	tree.add(bb1);
@@ -111,7 +111,7 @@ TEST(BVH, tree2) {
 
 TEST(BVH, tree3) {
 
-	BVHDebug<BBox3, BoundingVolumeSphere, Wrapper> tree;
+	BVH3Debug<BBox3, BoundingVolumeSphere3, Wrapper> tree;
 
 	BBox3 bb1 = BBox3::around(Point3(+0.5, +0.5, 0.0), Point3(0.25, 0.25, 0.25));
 	tree.add(bb1);
@@ -148,7 +148,7 @@ TEST(BVH, treeMap) {
 	fac.setFloors({map->floors[3]});
 	std::vector<Obstacle3D> obs = fac.triangulize();
 
-	BVHDebug<Obstacle3D, BoundingVolumeSphere, WrapperObs3D> tree;
+	BVH3Debug<Obstacle3D, BoundingVolumeSphere3, WrapperObs3D> tree;
 
 	for (const Obstacle3D& o : obs) {
 		tree.add(o);
@@ -171,7 +171,7 @@ TEST(BVH, treeMap) {
 
 TEST(BVH, treeRandom) {
 
-	BVHDebug<BBox3, BoundingVolumeSphere, Wrapper> tree;
+	BVH3Debug<BBox3, BoundingVolumeSphere3, Wrapper> tree;
 
 	std::minstd_rand gen;
 	std::uniform_real_distribution<float> dPos(-4.0, +4.0);

tests/geo/TestCircle2.cpp

@@ -0,0 +1,85 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+
+#include "../../geo/Circle2.h"
+
+TEST(Geo_Circle2, intersect) {
+
+	Circle2 c1(Point2(3,2), 1.0);
+
+	// left of circle
+	Ray2 r1(Point2(1,0), Point2(0,1));
+	ASSERT_FALSE(c1.intersects(r1));
+
+	// right of circle
+	Ray2 r2(Point2(5,0), Point2(0,1));
+	ASSERT_FALSE(c1.intersects(r2));
+
+
+	// through circle (from bottom)
+	Ray2 r3(Point2(3,0), Point2(0,1));
+	ASSERT_TRUE(c1.intersects(r3));
+	// through circle (from bottom)
+	Ray2 r4(Point2(3.5,0), Point2(0,1));
+	ASSERT_TRUE(c1.intersects(r4));
+
+	// within circle
+	Ray2 r5(Point2(3,2), Point2(0,1));
+	ASSERT_TRUE(c1.intersects(r5));
+
+
+	// through circle (from left)
+	Ray2 r6(Point2(0,2), Point2(1,0));
+	ASSERT_TRUE(c1.intersects(r6));
+	// through circle (from right)
+	Ray2 r7(Point2(10,2), Point2(-1,0));
+	ASSERT_TRUE(c1.intersects(r7));
+	// through circle (from top)
+	Ray2 r8(Point2(3,10), Point2(0,-1));
+	ASSERT_TRUE(c1.intersects(r8));
+
+
+}
+
+TEST(Geo_Circle2, contains) {
+
+	Circle2 c(Point2(0,0), 1.01);
+
+	ASSERT_TRUE(c.contains(Point2(+1,0)));
+	ASSERT_TRUE(c.contains(Point2(-1,0)));
+
+	ASSERT_TRUE(c.contains(Point2(0,+1)));
+	ASSERT_TRUE(c.contains(Point2(0,-1)));
+
+	ASSERT_FALSE(c.contains(Point2(+1,+1)));
+	ASSERT_FALSE(c.contains(Point2(-1,-1)));
+
+}
+
+TEST(Geo_Circle2, join) {
+
+	// no overlap
+	Circle2 a1(Point2(-1,0), 0.5);
+	Circle2 a2(Point2(+1,0), 0.5);
+	Circle2 a3 = Circle2::join(a1,a2);
+	ASSERT_EQ(Point2(0,0), a3.center);
+	ASSERT_NEAR(1.5, a3.radius, 0.01);
+
+	// overlap
+	Circle2 b1(Point2(0,+1), 1.5);
+	Circle2 b2(Point2(0,-1), 1.5);
+	Circle2 b3 = Circle2::join(b1,b2);
+	ASSERT_EQ(Point2(0,0), b3.center);
+	ASSERT_NEAR(2.5, b3.radius, 0.01);
+
+	// within
+	Circle2 c1(Point2(0,+1), 3.0);
+	Circle2 c2(Point2(0,-1), 1.0);
+	Circle2 c3 = Circle2::join(c1,c2);
+	ASSERT_EQ(c1.center, c3.center);
+	ASSERT_NEAR(c1.radius, c3.radius, 0.01);
+
+}
+
+#endif

tests/geo/TestHeading.cpp

@@ -3,7 +3,7 @@
 #include "../Tests.h"
 #include "../../geo/Heading.h"
 
-TEST(Heading, diff) {
+TEST(Geo_Heading, diff) {
 
 	// 180 degree turn
 	{
@@ -26,7 +26,7 @@ TEST(Heading, diff) {
 
 }
 
-TEST(Heading, mod) {
+TEST(Geo_Heading, mod) {
 
 	const float d = 0.0001;
 
@@ -36,7 +36,7 @@ TEST(Heading, mod) {
 
 }
 
-TEST(Heading, ctor) {
+TEST(Geo_Heading, ctor) {
 
 	// OK
 	Heading(0);
@@ -54,7 +54,7 @@ TEST(Heading, ctor) {
 
 }
 
-TEST(Heading, eq) {
+TEST(Geo_Heading, eq) {
 
 	ASSERT_EQ(Heading(0), Heading(0));
 	ASSERT_EQ(Heading(1), Heading(1));

tests/geo/TestLength.cpp

@@ -3,7 +3,7 @@
 #include "../Tests.h"
 #include "../../geo/Length.h"
 
-TEST(Length, float) {
+TEST(Geo_Length, float) {
 
 	static constexpr float delta = 0.00001;
 
@@ -24,7 +24,7 @@ TEST(Length, float) {
 
 }
 
-TEST(Length, int) {
+TEST(Geo_Length, int) {
 
 	LengthI l1 = LengthI::m(1.0);
 	ASSERT_EQ(1, l1.m());

tests/geo/TestPoint2.cpp

@@ -0,0 +1,23 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+#include "../../geo/Point2.h"
+
+TEST(Geo_Point2, math) {
+
+	Point2 p1(1,2);
+	p1 += Point2(2,3);
+	ASSERT_EQ(p1, Point2(3,5));
+
+	Point2 p2 = Point2(-2,-1) + p1;
+	ASSERT_EQ(p2, Point2(1, 4));
+
+	p2 -= Point2(1, 2);
+	ASSERT_EQ(p2, Point2(0,2));
+
+	Point2 p3 = Point2(1,2)*2;
+	ASSERT_EQ(p3, Point2(2,4));
+
+}
+
+#endif

tests/geo/TestPoint3.cpp

@@ -3,7 +3,7 @@
 #include "../Tests.h"
 #include "../../geo/Point3.h"
 
-TEST(Point3, math) {
+TEST(Geo_Point3, math) {
 
 	Point3 p1(1,2,3);
 	p1 += Point3(2,3,4);

tests/geo/TestSphere3.cpp

@@ -4,7 +4,7 @@
 
 #include "../../geo/Sphere3.h"
 
-TEST(Sphere3, contains) {
+TEST(Geo_Sphere3, contains) {
 
 	Sphere3 a(Point3(0,0,0), 10);
 
@@ -30,31 +30,31 @@ TEST(Sphere3, contains) {
 
 }
 
-TEST(Sphere3, join) {
+TEST(Geo_Sphere3, join) {
 
 	// no overlap
 	Sphere3 a1(Point3(-1,0,0), 1);
 	Sphere3 a2(Point3(+1,0,0), 1);
 	Sphere3 a3 = Sphere3::join(a1, a2);
 	ASSERT_EQ(Point3(0,0,0), a3.center);
-	ASSERT_EQ(2, a3.radius);
+	ASSERT_NEAR(2, a3.radius, 0.001);
 
 	// overlap
 	Sphere3 b1(Point3(-1,0,0), 1.5);
 	Sphere3 b2(Point3(+1,0,0), 1.5);
 	Sphere3 b3 = Sphere3::join(b1, b2);
 	ASSERT_EQ(Point3(0,0,0), b3.center);
-	ASSERT_EQ(2.5, b3.radius);
+	ASSERT_NEAR(2.5, b3.radius, 0.001);
 
 	// fully within
 	Sphere3 c1(Point3(-1,0,0), 3.6);
 	Sphere3 c2(Point3(+1,0,0), 1.5);
 	Sphere3 c3 = Sphere3::join(c1, c2);
 	ASSERT_EQ(c1.center, c3.center);
-	ASSERT_EQ(c1.radius, c3.radius);
+	ASSERT_NEAR(c1.radius, c3.radius, 0.001);
 	Sphere3 c4 = Sphere3::join(c2, c1);
 	ASSERT_EQ(c1.center, c4.center);
-	ASSERT_EQ(c1.radius, c4.radius);
+	ASSERT_NEAR(c1.radius, c4.radius, 0.001);
 
 }
 

tests/geo/TestTriangle3.cpp

@@ -5,7 +5,7 @@
 
 // https://stackoverflow.com/questions/17458562/efficient-aabb-triangle-intersection-in-c-sharp
 // http://fileadmin.cs.lth.se/cs/Personal/Tomas_Akenine-Moller/code/tribox3.txt
-TEST(Triangle3, intersect) {
+TEST(Geo_Triangle3, intersect) {
 
 	Point3 dst;
 

tests/ray/TestRayTrace2.cpp

@@ -0,0 +1,37 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+#include "../../wifi/estimate/ray2d/WiFiRayTrace2D.h"
+#include "../../floorplan/v2/FloorplanReader.h"
+#include <fstream>
+
+TEST(RayTrace2, test) {
+
+	//std::string file = "/mnt/data/workspaces/raytest2.xml";
+	//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file);
+	//Floorplan::AccessPoint* ap = map->floors[0]->accesspoints[0];
+
+	//std::string file = "/apps/SHL39.xml";
+	std::string file = "/mnt/data/workspaces/IndoorMap/maps/SHL39.xml";
+	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file);
+	Floorplan::Floor* floor = map->floors[0];
+	Floorplan::AccessPoint* ap = floor->accesspoints[4];
+
+//	ModelFactory fac(map);
+//	std::ofstream outOBJ("/tmp/vm/map.obj");
+//	outOBJ << fac.toOBJ();
+//	outOBJ.close();
+
+	const int gs_cm = 50;
+
+	WiFiRaytrace2D rt(floor, gs_cm, ap->pos.xy());
+
+	std::chrono::time_point<std::chrono::high_resolution_clock> start = std::chrono::high_resolution_clock::now();
+	const DataMapSignal& dms = rt.estimate();
+	std::chrono::time_point<std::chrono::high_resolution_clock> end = std::chrono::high_resolution_clock::now();
+	auto result = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
+	std::cout << "it took: " << result << " msec" << std::endl;
+
+}
+
+#endif

tests/ray/TestRayTrace3.cpp

@@ -12,7 +12,8 @@ TEST(RayTrace3, test) {
 	//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file);
 	//Floorplan::AccessPoint* ap = map->floors[0]->accesspoints[0];
 
-	std::string file = "/apps/SHL39.xml";
+	//std::string file = "/apps/SHL39.xml";
+	std::string file = "/mnt/data/workspaces/IndoorMap/maps/SHL39.xml";
 	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file);
 	Floorplan::AccessPoint* ap = map->floors[0]->accesspoints[4];
 

wifi/estimate/ray2d/DataMap2.h

@@ -3,6 +3,7 @@
 
 #include "../../../geo/BBox2.h"
 #include <vector>
+#include <functional>
 
 template <typename T> class DataMap {
 

wifi/estimate/ray2d/Ray2.h

@@ -1,24 +0,0 @@
-#ifndef RAY2_H
-#define RAY2_H
-
-#include "../../../geo/Point2.h"
-
-struct Ray2 {
-
-	/** starting position */
-	Point2 start;
-
-	/** ray direction */
-	Point2 dir;
-
-	/** empty ctor */
-	Ray2() {;}
-
-	/** ctor */
-	Ray2(const Point2 start, const Point2 dir) : start(start), dir(dir.normalized()) {
-		;
-	}
-
-};
-
-#endif // RAY2_H

wifi/estimate/ray2d/WiFiRayTrace2D.h

@@ -4,12 +4,14 @@
 #include "../../../geo/Point2.h"
 #include "../../../geo/Line2.h"
 #include "../../../geo/BBox2.h"
+#include "../../../geo/Ray2.h"
 
 #include "../../../floorplan/v2/Floorplan.h"
 #include "../../../floorplan/v2/FloorplanHelper.h"
 
+#include "../../../geo/volume/BVHDebug.h"
+
 #include "DataMap2.h"
-#include "Ray2.h"
 #include "MaterialOptions.h"
 
 #include <random>
@@ -24,6 +26,38 @@
 
 
 
+
+
+
+
+struct Obstacle2D {
+	Floorplan::Material mat;
+	Line2 line;
+	Obstacle2D(Floorplan::Material mat, Line2 line) : mat(mat), line(line) {;}
+};
+
+
+struct Obstacle2DWrapper {
+	static std::vector<Point2> getVertices(const Obstacle2D& obs) {
+		return {obs.line.p1, obs.line.p2};
+	}
+
+	static std::vector<Point2> getDebugLines(const Obstacle2D& obs) {
+		return {obs.line.p1, obs.line.p2};
+	}
+};
+
+struct Hit {
+	const Obstacle2D* obstacle;
+	float dist;
+	Point2 pos;
+	Point2 normal;
+	Floorplan::Material material;
+	bool stopHere = false;
+	Hit() {;}
+	Hit(const float dist, const Point2 pos, const Point2 normal) : dist(dist), pos(pos), normal(normal) {;}
+};
+
 struct StateRay2 : public Ray2 {
 
 	/** already travelled distance from the AP (by all previous rays */
@@ -32,7 +66,7 @@ struct StateRay2 : public Ray2 {
 	/** attenuation taken since the start */
 	float totalAttenuation;
 
-	void* lastObstacle;
+	const Obstacle2D* lastObstacle;
 	int depth = 0;
 
 	/** empty ctor */
@@ -51,17 +85,6 @@ struct StateRay2 : public Ray2 {
 
 };
 
-struct Hit {
-	void* obstacle;
-	float dist;
-	Point2 pos;
-	Point2 normal;
-	Floorplan::Material material;
-	bool stopHere = false;
-	Hit() {;}
-	Hit(const float dist, const Point2 pos, const Point2 normal) : dist(dist), pos(pos), normal(normal) {;}
-};
-
 
 
 class WiFiRaytrace2D {
@@ -72,11 +95,13 @@ private:
 	BBox2 bbox;
 	Point2 apPos;
 
+	BVH2Debug<Obstacle2D, BoundingVolumeCircle2, Obstacle2DWrapper> tree;
+
 	DataMapSignal dm;
 
 	struct Limit {
 		static constexpr int RAYS = 2000;
-		static constexpr int HITS = 11;
+		static constexpr int HITS = 25;
 		static constexpr float RSSI = -110;
 	};
 
@@ -94,6 +119,34 @@ public:
 		// allocate
 		dm.resize(bbox, gs);
 
+		// build tree
+		for (Floorplan::FloorObstacle* fo : floor->obstacles) {
+
+			const Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(fo);
+			const Floorplan::FloorObstacleDoor* door = dynamic_cast<Floorplan::FloorObstacleDoor*>(fo);
+
+			if (line) {
+				Obstacle2D obs(line->material, Line2(line->from, line->to));
+				tree.add(obs, true);
+			} else if (door) {
+				Obstacle2D obs(door->material, Line2(door->from, door->to));
+				tree.add(obs, true);
+			}
+
+		}
+
+//		for (int i = 0; i < 200; ++i) {
+//			tree.optimize(1);
+//			tree.show(1500, false);
+//			usleep(1000*100);
+//		}
+
+//		tree.show();
+		tree.optimize(250);
+//		int depth = tree.getDepth();
+		tree.show(1500,false);
+
+
 	}
 
 	const DataMapSignal& estimate() {
@@ -139,17 +192,13 @@ private:
 
 		// continue?
 		if (hit.stopHere) {return;}
-		//const float curLength = ray.totalLength + hit.dist;
-		//if (curLength > 55) {return;}
 		if (ray.getRSSI(hit.dist) < Limit::RSSI) {return;}
 		if (ray.depth > Limit::HITS) {return;}
 
-
 		// apply effects
-		//reflected(ray, hit);
+		reflected(ray, hit);
 		shadowed(ray, hit);
 
-
 	}
 
 	static inline float getAttenuation(const Hit& h) {
@@ -208,43 +257,46 @@ private:
 	}
 
 
+	static inline void hitTest(const Line2& longRay, const Obstacle2D& obs, Hit& nearest) {
 
-	Hit getNearestHit(const StateRay2& ray) {
+		const float minDist = 0.01;		// prevent errors hitting the same obstacle twice
+
+		// do not hit the last obstacle again
+		//if (ray.lastObstacle == fo) {continue;}
+
+		// get the line
+		Point2 hit;
+		if (obs.line.getSegmentIntersection(longRay, hit)) {
+			const float dist = hit.getDistance(longRay.p1);
+			if (dist > minDist && dist < nearest.dist) {
+				nearest.obstacle = &obs;
+				nearest.dist = dist;
+				nearest.pos = hit;
+				nearest.normal = (obs.line.p2 - obs.line.p1).perpendicular().normalized();
+				nearest.material = obs.mat;
+			}
+		}
+
+	}
+
+	Hit getNearestHit(const StateRay2& ray) const {
 
 		Assert::isNear(1.0f, ray.dir.length(), 0.01f, "not normalized!");
 
-		const Line2 longRay(ray.start, ray.start + ray.dir*100);
+		const Line2 longRay(ray.start, ray.start + ray.dir*1000);
 
-		const float minDist = 0;//0.01;		// prevent errors hitting the same obstacle twice
+		//const float minDist = 0;//0.01;		// prevent errors hitting the same obstacle twice
 		const float MAX = 999999;
 		Hit nearest; nearest.dist = MAX;
 
-		// check intersection with walls
-		for (Floorplan::FloorObstacle* fo : floor->obstacles) {
+		//int hits = 0;
 
-			// do not hit the last obstacle again
-			if (ray.lastObstacle == fo) {continue;}
+		const auto onHit = [longRay, &nearest] (const Obstacle2D& obs) {
+			//++hits;
+			hitTest(longRay, obs, nearest);
+		};
 
-			// get the line
-			const Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(fo);
-			const Floorplan::FloorObstacleDoor* door = dynamic_cast<Floorplan::FloorObstacleDoor*>(fo);
-			if (!line && !door) {continue;}
-			Line2 obstacle;
-			if (line) {obstacle = Line2(line->from, line->to);}
-			if (door) {obstacle = Line2(door->from, door->to);}
-
-			Point2 hit;
-			if (obstacle.getSegmentIntersection(longRay, hit)) {
-				const float dist = hit.getDistance(ray.start);
-				if (dist > minDist && dist < nearest.dist) {
-					nearest.obstacle = fo;
-					nearest.dist = dist;
-					nearest.pos = hit;
-					nearest.normal = (obstacle.p2 - obstacle.p1).perpendicular().normalized();
-					nearest.material = fo->material;
-				}
-			}
-		}
+		tree.getHits(ray, onHit);
 
 		// no hit with floorplan: limit to bounding-box!
 		if (nearest.dist == MAX) {

wifi/estimate/ray3/WifiRayTrace3D.h

@@ -70,6 +70,7 @@ struct StateRay3 : public Ray3 {
 
 	StateRay3 leave(const Point3 hitPos, const Obstacle3D* obs) const {
 
+		(void) obs;
 		StateRay3 next = getNext(hitPos);
 		next.isWithin = nullptr;
 		return next;
@@ -173,7 +174,7 @@ private:
 
 	DataMap3Signal dm;
 
-	BVHDebug<Obstacle3D, BoundingVolumeSphere, Obstacle3DWrapper> tree;
+	BVH3Debug<Obstacle3D, BoundingVolumeSphere3, Obstacle3DWrapper> tree;
 
 	struct Limit {
 		static constexpr int RAYS = 15000;