worked on 3d models within map

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

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 {