worked on 3d models within map

adjusted grid factory
adjusted nav mesh factory
minoor changes/fixes
new helper classes
refactoring

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 = center.getDistance(circle->center*100);
-				if (dist < circle->radius*100) {return true;}
+				const float dist = std::min(
+					bbox.getCorner1().getDistance(circle->center*100),
+					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!");}