Merge branch 'master' of https://git.frank-ebner.de/FHWS/Indoor

geo/BBox2.h

@@ -66,6 +66,12 @@ public:
 	Point2 getCenter() const { return (p1+p2) / 2; }
 
 
+	Point2 getCorner1() const {return Point2(p1.x, p1.y);}
+	Point2 getCorner2() const {return Point2(p2.x, p1.y);}
+	Point2 getCorner3() const {return Point2(p1.x, p2.y);}
+	Point2 getCorner4() const {return Point2(p2.x, p2.y);}
+
+
 	/** equal? */
 	bool operator == (const BBox2& o) const {
 		return	(p1.x == o.p1.x) &&

geo/BBox3.h

@@ -54,6 +54,9 @@ public:
 	/** get the bbox's maximum */
 	const Point3& getMax() const {return p2;}
 
+	/** get the bbox's size */
+	const Point3 getSize() const {return p2-p1;}
+
 	/** equal? */
 	bool operator == (const BBox3& o) const {
 		return	(p1.x == o.p1.x) &&
@@ -86,11 +89,11 @@ public:
 		p2 += p;	// increase maximum
 	}
 
-        /** set both, min/max z to the same value */
+		/** set both, min/max z to the same value */
-        void setZ(const float z) {
+		void setZ(const float z) {
-            p1.z = z;
+			p1.z = z;
-            p2.z = z;
+			p2.z = z;
-        }
+		}
 
 	/** does the bbox contain the given point? */
 	bool contains(const Point3& p) const {

geo/Triangle3.h

@@ -86,6 +86,48 @@ public:
 
 	}
 
+	/** add some slight delta to prevent rounding issues */
+	bool intersectsDelta(const Ray3& ray, const double delta, Point3& pos) const {
+
+		const Point3 e1 = p2-p1;
+		const Point3 e2 = p3-p1;
+
+		// make delta an absolute value (independent of the triangle's size)
+		// larger triangle -> smaller delta, as u,v are [0:1]
+		//double deltaU = delta/e2.length();
+		//double deltaV = delta/e1.length();
+
+		const double deltaU = delta;
+		const double deltaV = delta;
+
+		const Point3 h = cross(ray.dir, e2);
+		const double a = dot(e1, h);
+
+		if (a > -0.00001 && a < 0.00001) {return false;}
+
+		const double f = 1/a;
+
+		const Point3 s = ray.start - p1;
+		const double u = f * dot(s,h);
+
+		if (u < 0.0-deltaU || u > 1.0+deltaU) {return false;}
+
+		const Point3 q = cross(s, e1);
+		const double v = f * dot(ray.dir, q);
+
+		if (v < 0.0-deltaV || u + v > 1.0+deltaV) {return false;}
+		const double t = f * dot(e2,q);
+
+
+		if (t > 0.00001) {
+			pos = ray.start + (ray.dir * t);
+			return true;
+		}
+
+		return true;
+
+	}
+
 /*
 	int rayIntersectsTriangle(float *p, float *d,
 				float *v0, float *v1, float *v2) {

grid/Grid.h

@@ -73,6 +73,11 @@ public:
 		hashes.clear();
 	}
 
+	/** is the grid empty? */
+	bool isEmpty() const {
+		return nodes.empty();
+	}
+
 	/** no-assign */
 	void operator = (const Grid& o) = delete;
 

grid/factory/v2/GridFactory.h

@@ -5,6 +5,8 @@
 #include <unordered_set>
 #include <set>
 
+#include "../../../math/Math.h"
+
 #include "../../../floorplan/v2/Floorplan.h"
 
 #include "Helper.h"
@@ -19,6 +21,8 @@
 #include "../../../misc/Debug.h"
 #include <functional>
 
+#include "../../../wifi/estimate/ray3/OBJPool.h"
+#include "../../../geo/ConvexHull2.h"
 
 #include "GridFactoryListener.h"
 
@@ -47,6 +51,7 @@ private:
 
 	bool _buildStairs = true;
 	bool _removeIsolated = true;
+	bool _addTightToObstacle = false;
 
 public:
 
@@ -55,6 +60,7 @@ public:
 
 	}
 
+	void setAddTightToObstacle(const bool tight) {this->_addTightToObstacle = tight;}
 
 	/** whether or not to build stairs */
 	void setBuildStairs(const bool build) {this->_buildStairs = build;}
@@ -204,6 +210,41 @@ public:
 		int cur = 0;
 		int numNodes = 0;
 
+		// all 3D objects within the floor
+		std::vector<HelperPoly> objObstacles;
+		for (const Floorplan::FloorObstacle* fo : floor->obstacles) {
+
+			// process all object-obstalces
+			const Floorplan::FloorObstacleObject* foo = dynamic_cast<const Floorplan::FloorObstacleObject*>(fo);
+			if (foo) {
+
+				// get the obstacle
+				const Ray3D::Obstacle3D obs = Ray3D::OBJPool::get().getObject(foo->file).rotated_deg(foo->rot).translated(foo->pos);
+
+				// construct its 2D convex hull (in centimter)
+				HelperPoly poly;
+				for (const Point2 p : ConvexHull2::get(obs.getPoints2D())) {poly.add(p*100);}
+				objObstacles.push_back(poly);
+
+			}
+		}
+
+		// does any of the obj-obstalces contain the given point?
+		auto isPartOfObject = [&objObstacles, this] (const GridNodeBBox& bb) {
+			for (HelperPoly poly : objObstacles) {
+				//if (!_addTightToObstacle) {
+					if (poly.contains(bb.getCorner1())) {return true;}
+					if (poly.contains(bb.getCorner2())) {return true;}
+					if (poly.contains(bb.getCorner3())) {return true;}
+					if (poly.contains(bb.getCorner4())) {return true;}
+				//} else {
+				//	//poly.shrink(1);
+				//	if (poly.contains(bb.getCenter())) {return true;}
+				//}
+			}
+			return false;
+		};
+
 		// build grid-points for floor-outline
 		for(int x_cm = x1; x_cm < x2; x_cm += helper.gridSize()) {
 			for (int y_cm = y1; y_cm < y2; y_cm += helper.gridSize()) {
@@ -212,12 +253,17 @@ public:
 				const PartOfOutline part = isPartOfFloorOutline(x_cm, y_cm, floor->outline);
 				if (!part.contained) {continue;}
 
-				// check intersection with the floorplan
+				// bbox to check intersection with the floorplan
 				GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), helper.gridSize());
 
 				// slightly grow the bbox to ensure even obstacles that are directly aligned to the bbox are hit
 				bbox.grow(0.1337);
-				if (intersects(bbox, floor)) {continue;}
+				if (!_addTightToObstacle) {
+					if (intersects(bbox, floor)) {continue;}
+				}
+
+				// intersection with objects?
+				if (isPartOfObject(bbox)) {continue;}
 
 				// add to the grid [once]
 				T t(x_cm, y_cm, z_cm);
@@ -544,13 +590,21 @@ private:
 
 			} else if (dynamic_cast<Floorplan::FloorObstacleCircle*>(fo)) {
 				const Floorplan::FloorObstacleCircle* circle = (Floorplan::FloorObstacleCircle*) fo;
-				const Point2 center = bbox.getCenter();
+				const float dist = std::min(
-				const float dist = center.getDistance(circle->center*100);
+					bbox.getCorner1().getDistance(circle->center*100),
-				if (dist < circle->radius*100) {return true;}
+					bbox.getCorner2().getDistance(circle->center*100),
+					bbox.getCorner3().getDistance(circle->center*100),
+					bbox.getCorner4().getDistance(circle->center*100)
+				);
+				const float threshold = circle->radius * 100;
+				if (dist < threshold) {return true;}
 
 			} else if (dynamic_cast<Floorplan::FloorObstacleDoor*>(fo)) {
 				// DOORS ARE NOT AN OBSTACLE
 
+			} else if (dynamic_cast<Floorplan::FloorObstacleObject*>(fo)) {
+				// ADDED EARLIER
+
 			} else {
 				throw Exception("TODO: not yet implemented obstacle type");
 
@@ -592,6 +646,10 @@ private:
 			} else if (dynamic_cast<Floorplan::FloorObstacleDoor*>(fo)) {
 				// DOORS ARE NOT AN OBSTACLE
 
+			} else if (dynamic_cast<Floorplan::FloorObstacleObject*>(fo)) {
+				// removed earlier
+				//std::cout << "GridFactory: TODO: Floorplan::FloorObstacleObject" << std::endl;
+
 			} else {
 				throw Exception("TODO: not yet implemented obstacle type");
 

grid/factory/v2/Helper.h

@@ -44,6 +44,19 @@ struct HelperPoly {
 		bbox_cm.add(p.xy());
 	}
 
+
+	void shrink(float cm) {
+		Point2 center;
+		for (const Point2 pt : points_cm) {center += pt;}
+		center /= points_cm.size();
+		for (Point2& pt : points_cm) {
+			Point2 dir = pt - center;
+			float len = dir.length();
+			dir = dir.normalized();
+			pt = center + dir * (len-cm);
+		}
+	}
+
 	/** does the polygon contain the given point (in cm)? */
 	bool contains(const Point2 p_cm) const {
 

grid/walk/v2/modules/WalkModuleFollowDestination.h

@@ -42,6 +42,10 @@ public:
 
 	}
 
+	const Dijkstra<Node>& getDijkstra() const {
+		return dijkstra;
+	}
+
 	/** ctor WITH known destination*/
 	WalkModuleFollowDestination(const Grid<Node>& grid, const Node& destination) : grid(grid) {
 		setDestination(destination);

grid/walk/v3/ReachableSampler.h

@@ -1,7 +1,7 @@
 #ifndef INDOOR_GW3_REACHABLESAMPLER_H
 #define INDOOR_GW3_REACHABLESAMPLER_H
 
-#include "../../../math/Random.h"
+#include "../../../math/random/RandomGenerator.h"
 
 #include "Reachable.h"
 #include "Helper.h"
@@ -27,7 +27,7 @@ namespace GW3 {
 
 		const std::vector<Entry>& reachableNodes;
 
-		mutable RandomGenerator gen;
+		mutable Random::RandomGenerator gen;
 
 		mutable std::uniform_real_distribution<float> dOffset;
 

grid/walk/v3/WalkEvaluator.h

@@ -113,7 +113,9 @@ namespace GW3 {
 
 	public:
 
-		WalkEvalDistance(const Grid<Node>& grid, const double sigma = 0.1) : grid(grid), sigma(sigma), dist(0, sigma) {;}
+		WalkEvalDistance(const Grid<Node>& grid, const double sigma = 0.1) : grid(grid), sigma(sigma), dist(0, sigma) {
+			Assert::isFalse(grid.isEmpty(), "empty grid given");
+		}
 
 		virtual double getProbability(const PotentialWalk& walk) const override {
 

grid/walk/v3/Walker.h

@@ -237,7 +237,7 @@ namespace GW3 {
 
 			Assert::isNot0(walkDist_m, "walking distance must be > 0");
 
-			const GridPoint gpStart = Helper::p3ToGp(params.start);
+			const GridPoint gpStart = grid.toGridPoint(params.start);
 			const Node* startNode = grid.getNodePtrFor(gpStart);
 			if (!startNode) {throw Exception("start node not found!");}
 

math/Math.h

@@ -31,5 +31,16 @@ public:
 
 };
 
+namespace std {
+
+	template<class T> const T& min(const T& a, const T& b, const T& c) {
+		return min(a, min(b,c));
+	}
+	template<class T> const T& min(const T& a, const T& b, const T& c, const T& d) {
+		return min(a, min(b, min(c,d)));
+	}
+
+}
+
 
 #endif // K_MATH_MATH_H

math/distribution/ChiSquared.h

@@ -0,0 +1,59 @@
+#ifndef CHISQUARED_H
+#define CHISQUARED_H
+
+#include <cmath>
+
+namespace Distribution {
+
+	/**
+	 * https://en.wikipedia.org/wiki/Chi-squared_distribution
+	 * @brief The ChiSquared class
+	 */
+	template <typename Scalar> class ChiSquared {
+
+	private:
+
+		// degrees of freedom
+		int k;
+
+	public:
+
+		/** ctor */
+		ChiSquared(const int k) : k(k) {
+			;
+		}
+
+		Scalar get(const Scalar val) const {
+
+			const Scalar k2 = k/((Scalar)2);
+			const Scalar k2_1 = k2 - 1;
+			const Scalar gamma = std::tgamma(k2);
+
+			return 1.0 / (std::pow(2, k2)*gamma) * std::pow(val, k2_1) * std::exp(-val/2);
+
+		}
+
+
+		Scalar getInvCDF(const Scalar val) const {
+
+			// brute-force get the inverse CDF...
+
+			const Scalar ss = 0.002;
+			Scalar sum = 0;
+
+			for (float t = 0; t < 20; t += ss) {
+				sum += get(t) * ss;
+				if (sum >= val) {
+					return t;
+				}
+			}
+
+			throw "failed";
+
+		}
+
+	};
+
+}
+
+#endif // CHISQUARED_H

math/distribution/Normal.h

@@ -17,7 +17,7 @@ namespace Distribution {
 		const T sigma;
 		const T _a;
 
-        Random::RandomGenerator gen;
+		Random::RandomGenerator gen;
 		std::normal_distribution<T> dist;
 
 	public:
@@ -32,6 +32,11 @@ namespace Distribution {
 			mu(mu), sigma(sigma), _a(1.0 / (sigma * std::sqrt(2.0 * M_PI))), gen(seed), dist(mu,sigma) {
 		}
 
+		/** mu = 0, sigma = 1 */
+		static Normal unit() {
+			return Normal(0,1);
+		}
+
 		/** do not allow copy. this will not work as expected for std::normal_distribution when using draw() ?! */
 		//Normal(const Normal& o) = delete;
 
@@ -51,15 +56,15 @@ namespace Distribution {
 			gen.seed(seed);
 		}
 
-        /** get the mean value */
+		/** get the mean value */
-        const T getMu() {
+		const T getMu() {
-            return this->mu;
+			return this->mu;
-        }
+		}
 
-        /** get the standard deviation */
+		/** get the standard deviation */
-        const T getSigma() {
+		const T getSigma() {
-            return this->sigma;
+			return this->sigma;
-        }
+		}
 
 		/** get the probability for the given value */
 		static T getProbability(const T mu, const T sigma, const T val) {

math/distribution/NormalN.h

@@ -8,6 +8,8 @@
 
 #include "../../Assertions.h"
 #include "../random/RandomGenerator.h"
+#include "../../geo/Point2.h"
+#include "ChiSquared.h"
 
 namespace Distribution {
 
@@ -96,6 +98,43 @@ namespace Distribution {
                 return NormalDistributionN(mean, cov);
         }
 
+		std::vector<Point2> getContour2(const double percentWithin) {
+
+			const int degreesOfFreedom = 2;	// 2D distribution
+			const ChiSquared<double> chi(degreesOfFreedom);
+
+			// https://people.richland.edu/james/lecture/m170/tbl-chi.html
+			Assert::isNear(0.103, chi.getInvCDF(0.05), 0.01, "error within chi-squared distribution");
+			Assert::isNear(0.211, chi.getInvCDF(0.10), 0.01, "error within chi-squared distribution");
+			Assert::isNear(4.605, chi.getInvCDF(0.90), 0.01, "error within chi-squared distribution");
+			Assert::isNear(5.991, chi.getInvCDF(0.95), 0.03, "error within chi-squared distribution");
+
+			// size of the ellipse using confidence intervals
+			const float mul = chi.getInvCDF(percentWithin);
+
+			std::vector<Point2> res;
+
+			std::cout << sigma << std::endl;
+			Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> solver(this->sigma);
+
+			Eigen::Vector2d evec1 = solver.eigenvectors().col(0);
+			Eigen::Vector2d evec2 = solver.eigenvectors().col(1);
+
+			double eval1 = solver.eigenvalues()(0);
+			double eval2 = solver.eigenvalues()(1);
+
+			for (int deg = 0; deg <= 360; deg += 5) {
+				const float rad = deg / 180.0f * M_PI;
+				Eigen::Vector2d pos =
+						std::cos(rad) * std::sqrt(mul * eval1) * evec1 +
+						std::sin(rad) * std::sqrt(mul * eval2) * evec2;
+				res.push_back(Point2(pos(0), pos(1)));
+			}
+
+			return res;
+
+		}
+
     };
 
 }

math/distribution/Uniform.h

@@ -14,7 +14,7 @@ namespace Distribution {
 
 	private:
 
-        Random::RandomGenerator gen;
+		Random::RandomGenerator gen;
 
 		/** depending on T, Dist is either a uniform_real or uniform_int distribution */
 		typedef typename std::conditional< std::is_floating_point<T>::value, std::uniform_real_distribution<T>, std::uniform_int_distribution<T> >::type Dist;
@@ -33,6 +33,11 @@ namespace Distribution {
 
 		}
 
+		/** -1 to +1 */
+		static Uniform unit() {
+			return Uniform(-1, +1);
+		}
+
 		/** get a uniformaly distributed random number */
 		T draw() {
 			return dist(gen);

math/random/RandomGenerator.h

@@ -12,15 +12,15 @@
 
 namespace Random {
 
-    class RandomGenerator : public std::minstd_rand {
+	class RandomGenerator : public std::minstd_rand {
 
     public:
 
         /** ctor with default seed */
-        RandomGenerator() : std::minstd_rand(RANDOM_SEED) {;}
+		RandomGenerator() : std::minstd_rand(RANDOM_SEED) {;}
 
         /** ctor with custom seed */
-        RandomGenerator(result_type) : std::minstd_rand(RANDOM_SEED) {;}
+		RandomGenerator(result_type) : std::minstd_rand(RANDOM_SEED) {;}
 
     };
 

math/stats/Histogram2.h

@@ -0,0 +1,53 @@
+#ifndef HISTOGRAM2_H
+#define HISTOGRAM2_H
+
+#include <vector>
+#include "../../geo/BBox2.h"
+
+namespace Stats {
+
+	/** 2D histogram */
+	template <typename Scalar> class Histogram2 {
+
+		std::vector<Scalar> vec;
+		BBox2 bbox;
+		int binsX;
+		int binsY;
+
+	public:
+
+		/** ctor */
+		Histogram2(BBox2 bbox, int binsX, int binsY) : bbox(bbox), binsX(binsX), binsY(binsY) {
+			vec.resize(binsX*binsY);
+		}
+
+		Scalar get(Scalar x, Scalar y) const {
+			const int idx = binIdx(x,y);
+			return vec[idx];
+		}
+
+		void add(Scalar x, Scalar y, Scalar val) {
+			const int idx = binIdx(x,y);
+			vec[idx] += val;
+		}
+
+		int binIdx(const Scalar x, const Scalar y) {
+			const int ix = binIdxX(x);
+			const int iy = binIdxY(y);
+			return ix + iy*binsX;
+		}
+
+		int binIdxX(const Scalar val) const {
+			return (val - bbox.getMin().x) / (bbox.getSize().x) * binsX;
+		}
+
+		int binIdxY(const Scalar val) const {
+			return (val - bbox.getMin().y) / (bbox.getSize().y) * binsY;
+		}
+
+
+	};
+
+}
+
+#endif // HISTOGRAM2_H

navMesh/NavMeshFactory.h

@@ -631,14 +631,11 @@ namespace NM {
 
 			Floorplan::Polygon2 res;
 
-			std::vector<Point2> src;
+			// fetch object from pool
-			Ray3D::Obstacle3D obs = Ray3D::OBJPool::get().getObject(obj->file).rotated_deg(obj->rot).translated(obj->pos);
+			const Ray3D::Obstacle3D obs = Ray3D::OBJPool::get().getObject(obj->file).rotated_deg(obj->rot).translated(obj->pos);
-			for (const Triangle3& tria : obs.triangles) {
+
-				src.push_back(tria.p1.xy());
+			// construct 2D convex hull
-				src.push_back(tria.p2.xy());
+			res.points = ConvexHull2::get(obs.getPoints2D());
-				src.push_back(tria.p3.xy());
-			}
-			res.points = ConvexHull2::get(src);
 			return res;
 
 		}

synthetic/SyntheticPath.h

@@ -9,13 +9,13 @@
 /** allows interpolation along a synthetic path */
 class SyntheticPath : private Interpolator<float, Point3> {
 
-	using Base = Interpolator<float, Point3>;
-	using Entry = Base::InterpolatorEntry;
 	const Floorplan::IndoorMap* map;
 
-
 public:
 
+	using Base = Interpolator<float, Point3>;
+	using Entry = Base::InterpolatorEntry;
+
 	/** create path using the given ground-truth points from the map */
 	void create(const Floorplan::IndoorMap* map, std::vector<int> ids) {
 
@@ -38,6 +38,19 @@ public:
 
 	}
 
+	/** get the path's length */
+	float getLength() const {
+		//return Base::getEntries().back().key;
+		float dist = 0;
+		for (size_t i = 0; i < getEntries().size()-1; ++i) {
+			const auto& e1 = getEntries()[i];
+			const auto& e2 = getEntries()[i+1];
+			dist += e1.value.getDistance(e2.value);
+		}
+		return dist;
+	}
+
+
 	/** get all individual entries from the underlying data-structure */
 	const std::vector<Entry>& getEntries() const {
 		return Base::getEntries();

wifi/estimate/ray3/Cube.h

@@ -18,6 +18,16 @@ namespace Ray3D {
 
 	public:
 
+
+		enum Part {
+			CUBE_TOP = 1,
+			CUBE_BOTTOM = 2,
+			CUBE_LEFT = 4,
+			CUBE_RIGHT = 8,
+			CUBE_FRONT = 16,
+			CUBE_BACK = 32,
+		};
+
 //		Cube (const Point3 p1, const Point3 p2, const Point3 p3, const Point3 p4, const float h) {
 ////			const Point3 ph(0,0,h);
 ////			addQuad(p1+ph,	p2+ph,	p3+ph,	p4+ph);		// top
@@ -30,8 +40,8 @@ namespace Ray3D {
 //		}
 
 		/** ctor with position, size and rotation */
-		Cube(const Point3 pos, const Point3 size, const Point3 rot_deg, const bool topAndBottom = true) {
+		Cube(const Point3 pos, const Point3 size, const Point3 rot_deg, const Part parts = (Part)63) {
-			unitCube(topAndBottom);
+			unitCube(parts);
 			transform(pos, size, rot_deg);
 		}
 
@@ -50,8 +60,10 @@ namespace Ray3D {
 			return res;
 		}
 
-		static Cube unit() {
+		static Cube unit(const Part parts = (Part) 63) {
-			return Cube();
+			Cube cube;
+			cube.unitCube(parts);
+			return cube;
 		}
 
 		/** cube from 8 vertices (upper 4, lower 4) */
@@ -77,72 +89,73 @@ namespace Ray3D {
 	private:
 
 		/** build unit-cube faces */
-		void unitCube(const bool topAndBottom) {
+		void unitCube(const Part parts) {
 
 			const float s = 1.0f;
 
 
 
 			// left?
-			addQuad(
+			if (parts & CUBE_LEFT) {
-				Point3(+s, -s, -s),
+				addQuad(
-				Point3(+s, -s, +s),
+					Point3(+s, -s, -s),
-				Point3(-s, -s, +s),
+					Point3(+s, -s, +s),
-				Point3(-s, -s, -s)
+					Point3(-s, -s, +s),
-			);
+					Point3(-s, -s, -s)
+				);
+			}
 
 			// right?
-			addQuad(
+			if (parts & CUBE_RIGHT) {
-				Point3(-s, +s, -s),
+				addQuad(
-				Point3(-s, +s, +s),
+					Point3(-s, +s, -s),
-				Point3(+s, +s, +s),
+					Point3(-s, +s, +s),
-				Point3(+s, +s, -s)
+					Point3(+s, +s, +s),
-			);
+					Point3(+s, +s, -s)
+				);
+			}
 
-
+			// front
-			// small side
+			if (parts & CUBE_FRONT) {
-			if (1 == 1) {
-
-				// front
 				addQuad(
 					Point3(-s, -s, -s),
 					Point3(-s, -s, +s),
 					Point3(-s, +s, +s),
 					Point3(-s, +s, -s)
 				);
+			}
 
-				// read
+			// back
+			if (parts & CUBE_BACK) {
 				addQuad(
 					Point3(+s, +s, -s),
 					Point3(+s, +s, +s),
 					Point3(+s, -s, +s),
 					Point3(+s, -s, -s)
 				);
-
 			}
 
-
+			// top
-
+			if (parts & CUBE_TOP) {
-			if (topAndBottom) {
-
-				// top
 				addQuad(
 					Point3(+s, +s, +s),
 					Point3(-s, +s, +s),
 					Point3(-s, -s, +s),
 					Point3(+s, -s, +s)
 				);
+			}
 
-				// bottom
+			// bottom
+			if (parts & CUBE_BOTTOM) {
 				addQuad(
 					Point3(+s, -s, -s),
 					Point3(-s, -s, -s),
 					Point3(-s, +s, -s),
 					Point3(+s, +s, -s)
 				);
-
 			}
 
+
 		}
 
 

wifi/estimate/ray3/MTLReader.h

@@ -0,0 +1,127 @@
+#ifndef MTLREADER_H
+#define MTLREADER_H
+
+#include <vector>
+#include <unordered_map>
+#include <string>
+#include <fstream>
+#include "../../../geo/Point2.h"
+#include "../../../geo/Point3.h"
+
+/**
+ * prase .mtl files
+ */
+class MTLReader {
+
+public:
+
+	struct Material {
+		std::string textureFile = "";
+		Point3 diffuse = Point3(1,1,1);
+		float alpha = 1.0;
+	};
+
+	Material* cur = nullptr;
+	std::unordered_map<std::string, Material> map;
+
+	/** ctor. use readXYZ() */
+	MTLReader() {
+		;
+	}
+
+	/** read .obj from the given file */
+	void readFile(const std::string& file) {
+		std::ifstream is(file);
+		std::string line;
+		while(getline(is, line)) {parseLine(line);}
+		is.close();
+	}
+
+	/** read obj from the given data string (.obj file contents) */
+	void readData(const std::string& data) {
+		std::stringstream is(data);
+		std::string line;
+		while(getline(is, line)) {parseLine(line);}
+	}
+
+	/** get the given material */
+	const Material& getMaterial(const std::string& mat) const {
+		const auto& it = map.find(mat);
+		if (it == map.end()) {throw Exception("material not available");}
+		return it->second;
+	}
+
+
+private:
+
+	template<typename Out>
+	void split(const std::string &s, char delim, Out result) {
+		std::stringstream ss(s);
+		std::string item;
+		while (std::getline(ss, item, delim)) {
+			*(result++) = item;
+		}
+	}
+
+	void replaceAll(std::string& str, const std::string& from, const std::string& to) {
+		size_t start_pos = 0;
+		while((start_pos = str.find(from, start_pos)) != std::string::npos) {
+			size_t end_pos = start_pos + from.length();
+			str.replace(start_pos, end_pos, to);
+			start_pos += to.length(); // In case 'to' contains 'from', like replacing 'x' with 'yx'
+		}
+	}
+
+	/** remove empty strings from the vector */
+	std::vector<std::string> nonEmpty(const std::vector<std::string>& src) {
+		std::vector<std::string> res;
+		for (const std::string& s : src) {
+			if (!s.empty()) {res.push_back(s);}
+		}
+		return res;
+	}
+
+	std::vector<std::string> split(const std::string &s, char delim) {
+		std::vector<std::string> elems;
+		split(s, delim, std::back_inserter(elems));
+		return elems;
+	}
+
+	/** parse one line of the .obj file */
+	void parseLine(std::string line) {
+
+		if (line.length() < 2) {return;}
+
+		// remove leading "#"
+		while (line[0] == ' ' || line[0] == '\t') {
+			line.erase(line.begin());
+		}
+
+		// remove other linebreaks
+		replaceAll(line, "\r", "");
+
+		const std::vector<std::string> tokens = nonEmpty(split(line, ' '));
+		const std::string token = tokens.front();
+
+		if ("newmtl" == token) {
+			const std::string id = tokens[1];
+			map[id] = Material();
+			cur = &map[id];
+		} else if ("map_Ka" == token) {
+			const std::string texFile = tokens[1];
+			cur->textureFile = texFile;
+		} else if ("Kd" == token) {
+			cur->diffuse.x = std::stof(tokens[1]);
+			cur->diffuse.y = std::stof(tokens[2]);
+			cur->diffuse.z = std::stof(tokens[3]);
+		} else if ("d" == token) {
+			cur->alpha = std::stof(tokens[1]);
+		}
+
+	}
+
+
+
+};
+
+#endif // MTLREADER_H

wifi/estimate/ray3/ModelFactory.h

@@ -19,7 +19,7 @@ namespace Ray3D {
 	 */
 	class ModelFactory {
 
-	private:
+	public:
 
 		bool exportCeilings = true;
 		bool exportObstacles = true;
@@ -27,9 +27,12 @@ namespace Ray3D {
 		bool fancyStairs = true;
 		bool exportHandrails = true;
 		bool exportDoors = true;
-		bool doorsOpen = true;
+		bool exportAboveDoors = true;
+		bool doorsOpen = false;
 		bool exportObjects = true;
 		bool exportWallTops = false;
+		Cube::Part cubeParts = (Cube::Part) 63;	// leftright,topbottom,rearfront
+
 		std::vector<Floorplan::Floor*> exportFloors;
 
 		/** the to-be-exported map */
@@ -234,7 +237,9 @@ namespace Ray3D {
 					}
 					//std::vector<Obstacle3D> tmp = getDoorAbove(f, door);
 					//res.insert(res.end(), tmp.begin(), tmp.end());
-					res.push_back(getDoorAbove(f, door));
+					if (exportAboveDoors) {
+						res.push_back(getDoorAbove(f, door));
+					}
 				}
 			}
 
@@ -275,8 +280,8 @@ namespace Ray3D {
 			const float deg = rad * 180 / M_PI;
 
 			// cube's destination center
-			const float cenZ =		(!aboveDoor) ? (fpos.z1 + fpos.height/2) :	(fpos.z2 - (fpos.height - aboveDoor->height) / 2);
+			const double height =	(!aboveDoor) ? (fpos.height) :				(fpos.height - aboveDoor->height);
-			const float height =	(!aboveDoor) ? (fpos.height) :					(fpos.height - aboveDoor->height);
+			const double cenZ =		(!aboveDoor) ? (fpos.z1 + height/2) :		(fpos.z1 + aboveDoor->height + height/2);// (fpos.z2 - (fpos.height - aboveDoor->height) / 2);
 			const Point3 pos(cen2.x, cen2.y, cenZ);
 
 			// div by 2.01 to prevent overlapps and z-fighting
@@ -287,7 +292,7 @@ namespace Ray3D {
 			const Point3 rot(0,0,deg);
 
 			// build
-			Cube cube(pos, size, rot);
+			Cube cube(pos, size, rot, cubeParts);
 
 			// done
 			Obstacle3D res(getType(fol), fol->material);
@@ -360,7 +365,7 @@ namespace Ray3D {
 				const float sz = door->height / 2.01f;	// prevent overlaps
 				const Point3 size(sx, sy, sz);
 
-				Cube cube = Cube::unit();
+				Cube cube = Cube::unit(cubeParts);
 				cube.transform(mat);
 				cube.transform(pos, size, rot);
 				res.triangles = cube.getTriangles();

wifi/estimate/ray3/OBJPool.h

@@ -99,9 +99,11 @@ namespace Ray3D {
 
 			// create triangles
 			Obstacle3D obs;
-			for (const OBJReader::Face& face : reader.getData().faces) {
+			for (const OBJReader::Object& obj : reader.getData().objects) {
-				const Triangle3 tria(face.vnt[0].vertex, face.vnt[1].vertex, face.vnt[2].vertex);
+				for (const OBJReader::Face& face : obj.faces) {
-				obs.triangles.push_back(tria);
+					const Triangle3 tria(face.vnt[0].vertex, face.vnt[1].vertex, face.vnt[2].vertex);
+					obs.triangles.push_back(tria);
+				}
 			}
 
 			// store

wifi/estimate/ray3/OBJReader.h

@@ -14,7 +14,6 @@ class OBJReader {
 
 public:
 
-
 	/** group vertex+normal+texture */
 	struct VNT {
 		int idxVertex;
@@ -31,15 +30,26 @@ public:
 		Face(VNT v1, VNT v2, VNT v3) : vnt{v1,v2,v3} {;}
 	};
 
+	/** one object within the file */
+	struct Object {
+		std::string material;
+		std::string name;
+		std::vector<Face> faces;
+	};
+
 	/** internal data */
 	struct Data {
 		std::vector<Point3> vertices;
 		std::vector<Point2> texCoords;
 		std::vector<Point3> normals;
-		std::vector<Face> faces;
+		std::vector<std::string> materialFiles;
+		std::vector<Object> objects;
+		Object& curObj() {
+			if (objects.empty()) {objects.push_back(Object());}
+			return objects.back();
+		}
 	} data;
 
-
 public:
 
 	/** ctor. use readXYZ() */
@@ -68,6 +78,24 @@ public:
 
 private:
 
+	void replaceAll(std::string& str, const std::string& from, const std::string& to) {
+		size_t start_pos = 0;
+		while((start_pos = str.find(from, start_pos)) != std::string::npos) {
+			size_t end_pos = start_pos + from.length();
+			str.replace(start_pos, end_pos, to);
+			start_pos += to.length(); // In case 'to' contains 'from', like replacing 'x' with 'yx'
+		}
+	}
+
+	/** remove empty strings from the vector */
+	std::vector<std::string> nonEmpty(const std::vector<std::string>& src) {
+		std::vector<std::string> res;
+		for (const std::string& s : src) {
+			if (!s.empty()) {res.push_back(s);}
+		}
+		return res;
+	}
+
 	template<typename Out>
 	void split(const std::string &s, char delim, Out result) {
 		std::stringstream ss(s);
@@ -84,20 +112,33 @@ private:
 	}
 
 	/** parse one line of the .obj file */
-	void parseLine(const std::string& line) {
+	void parseLine(std::string line) {
 
 		if (line.length() < 2) {return;}
 
-		const std::vector<std::string> tokens = split(line, ' ');
+		// remove other linebreaks
+		replaceAll(line, "\r", "");
+
+		const std::vector<std::string> tokens = nonEmpty(split(line, ' '));
 		const std::string token = tokens.front();
 
-		if ("v"  == token) {parseVertex(tokens);}
+		if ("mtllib" == token)	{data.materialFiles.push_back(tokens[1]);}
-		if ("vt" == token) {parseTexCoord(tokens);}
+		if ("usemtl" == token)	{data.curObj().material = tokens[1];}
-		if ("vn" == token) {parseNormal(tokens);}
+		if ("v"  == token)		{parseVertex(tokens);}
-		if ("f"  == token) {parseFace(tokens);}
+		if ("vt" == token)		{parseTexCoord(tokens);}
+		if ("vn" == token)		{parseNormal(tokens);}
+		if ("f"  == token)		{parseFace(tokens);}
+		if ("g"  == token)		{newObject(tokens[1]);}
 
 	}
 
+	/** allocate a new object */
+	void newObject(const std::string& name) {
+		Object o;
+		o.name = name;
+		data.objects.push_back(o);
+	}
+
 	/** parse one vertex from the tokenizer */
 	void parseVertex(const std::vector<std::string>& t) {
 		const float x = std::stof(t[1]);
@@ -135,18 +176,20 @@ private:
 
 			++numVertices;
 			const std::string v = vtn[0];
+			const std::string vt = (vtn.size() > 1) ? (vtn[1]) : ("");
+			const std::string vn = (vtn.size() > 2) ? (vtn[2]) : ("");
 			//const std::string vt = t2.getToken('/', false);
 			//const std::string vn = t2.getToken('/', false);
 
 			// create a new vertex/normal/texture combination
 			VNT vnt;
 			vnt.idxVertex =								(std::stoi(v)  - 1);
-			//vnt.idxNormal =		(vn.empty()) ? (-1) :	(std::stoi(vn) - 1);
+			vnt.idxNormal =		(vn.empty()) ? (-1) :	(std::stoi(vn) - 1);
-			//vnt.idxTexture =	(vt.empty()) ? (-1) :	(std::stoi(vt) - 1);
+			vnt.idxTexture =	(vt.empty()) ? (-1) :	(std::stoi(vt) - 1);
 
 			if (vnt.idxVertex >= 0)		{vnt.vertex =	data.vertices[vnt.idxVertex];}
-			//if (vnt.idxNormal >= 0)		{vnt.normal =	data.normals[vnt.idxNormal];}
+			if (vnt.idxNormal >= 0)		{vnt.normal =	data.normals[vnt.idxNormal];}
-			//if (vnt.idxTexture >= 0)	{vnt.texture =	data.texCoords[vnt.idxTexture];}
+			if (vnt.idxTexture >= 0)	{vnt.texture =	data.texCoords[vnt.idxTexture];}
 
 			indices.push_back(vnt);
 
@@ -156,7 +199,7 @@ private:
 		// see: http://www.mathopenref.com/polygontriangles.html
 		for (int i = 1; i < (int) indices.size()-1; ++i) {
 			Face face(indices[0], indices[1], indices[i+1]);
-			data.faces.push_back(face);
+			data.curObj().faces.push_back(face);
 		}
 
 		// sanity check

wifi/estimate/ray3/Obstacle3.h

@@ -59,6 +59,28 @@ namespace Ray3D {
 			return copy;
 		}
 
+		/** get all triangle-edge-points (x,y) within the obstacle */
+		std::vector<Point2> getPoints2D() const {
+			std::vector<Point2> res;
+			for (const Triangle3& tria : triangles) {
+				res.push_back(tria.p1.xy());
+				res.push_back(tria.p2.xy());
+				res.push_back(tria.p3.xy());
+			}
+			return res;
+		}
+
+		/** get all triangle-edge-points (x,y,z) within the obstacle */
+		std::vector<Point3> getPoints3D() const {
+			std::vector<Point3> res;
+			for (const Triangle3& tria : triangles) {
+				res.push_back(tria.p1);
+				res.push_back(tria.p2);
+				res.push_back(tria.p3);
+			}
+			return res;
+		}
+
 	};
 
 }