worked on floorplan (v2)

worked on grid-generation (v2)
new helper methods for geometry
new test cases

grid/factory/v2/GridFactory.h

@@ -0,0 +1,507 @@
+#ifndef GRIDFACTORY_V2_H
+#define GRIDFACTORY_V2_H
+
+#include <string>
+#include <unordered_set>
+#include <set>
+
+#include "../../../floorplan/v2/Floorplan.h"
+
+#include "Helper.h"
+#include "Stairs.h"
+
+#include "../../../geo/Units.h"
+#include "../../GridNodeBBox.h"
+#include "../../Grid.h"
+
+#include "../../../misc/Debug.h"
+#include <functional>
+
+
+/** listen for events during the build process */
+class GridFactoryListener {
+
+public:
+
+	virtual void onGridBuildUpdateMajor(const std::string& what) = 0;
+	virtual void onGridBuildUpdateMajor(const int cnt, const int cur) = 0;
+
+	virtual void onGridBuildUpdateMinor(const std::string& what) = 0;
+	virtual void onGridBuildUpdateMinor(const int cnt, const int cur) = 0;
+
+};
+
+
+template <typename T> class GridFactory {
+
+	/** logging name */
+	static constexpr const char* name = "GridFac2";
+
+private:
+
+	/** the grid to build into */
+	Grid<T>& grid;
+
+	/** calculation helper */
+	Helper<T> helper;
+
+	/** stair builder */
+	Stairs<T> stairs;
+
+
+	bool _buildStairs = true;
+	bool _removeIsolated = true;
+
+public:
+
+	/** ctor with the grid to fill */
+	GridFactory(Grid<T>& grid) : grid(grid), helper(grid), stairs(grid) {;}
+
+
+	/** whether or not to build stairs */
+	void setBuildStairs(const bool build) {this->_buildStairs = build;}
+
+	/** whether or not to remove isolated nodes */
+	void setRemoveIsolated(const bool remove) {this->_removeIsolated = remove;}
+
+
+	/** build using the given map */
+	void build(const Floorplan::IndoorMap* map, GridFactoryListener* listener = nullptr) {
+
+		Log::add(name, "building grid from IndoorMap", true);
+
+		const int total = map->floors.size()*2 + 1;
+		int cur = 0;
+
+		// build all the floors
+		if (listener) {listener->onGridBuildUpdateMajor("adding floors");}
+		for (Floorplan::Floor* f : map->floors) {
+			addFloor(f, listener);
+			if (listener) {listener->onGridBuildUpdateMajor(total, ++cur);}
+		}
+
+		// build all stairs
+		if (listener) {listener->onGridBuildUpdateMajor("adding stairs");}
+		if (_buildStairs) {
+			for (Floorplan::Floor* f : map->floors) {
+				buildStairs(f, listener);
+				if (listener) {listener->onGridBuildUpdateMajor(total, ++cur);}
+			}
+		}
+
+		// remove isolated nodes
+		if (_removeIsolated) {
+			if (listener) {listener->onGridBuildUpdateMajor("removing isolated nodes");}
+			removeIsolatedNodes();
+			if (listener) {listener->onGridBuildUpdateMajor(total, ++cur);}
+		}
+
+
+	}
+
+	/** get the 2D-bbox for the given floor's outline */
+	BBox2 getFloorOutlineBBox(const Floorplan::FloorOutline& outline) {
+		BBox2 bb;
+		for (Floorplan::FloorOutlinePolygon* poly : outline) {
+			for (Point2 p : poly->poly.points) {
+				bb.add(p * 100);		// convert m to cm
+			}
+		}
+		return bb;
+	}
+
+	bool isPartOfFloorOutline(const int x_cm, const int y_cm, const Floorplan::FloorOutline& outline) {
+
+		bool add = false;
+		for (Floorplan::FloorOutlinePolygon* poly : outline) {
+			if (poly->method != Floorplan::OutlineMethod::ADD) {continue;}
+			HelperPoly pol(*poly);
+			if (pol.contains(Point2(x_cm, y_cm))) {add = true;}
+		}
+		if (!add) {return false;}
+		for (Floorplan::FloorOutlinePolygon* poly : outline) {
+			if (poly->method != Floorplan::OutlineMethod::REMOVE) {continue;}
+			HelperPoly pol(*poly);
+			if (pol.contains(Point2(x_cm, y_cm))) {add = false;}			// TODO
+		}
+		return add;
+	}
+
+
+
+	/** add the given floor to the grid */
+	void addFloor(const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) {
+
+		Log::add(name, "adding floor " + floor->name, true);
+		if (listener) {listener->onGridBuildUpdateMinor("adding floor " + floor->name);}
+
+		const BBox2 bbox = getFloorOutlineBBox(floor->outline);
+		const int x1 = helper.align(bbox.getMin().x);
+		const int x2 = helper.align(bbox.getMax().x);
+		const int y1 = helper.align(bbox.getMin().y);
+		const int y2 = helper.align(bbox.getMax().y);
+		const int z_cm = (floor->atHeight*100);
+
+		const int total = (x2-x1) / helper.gridSize();
+		int cur = 0;
+
+		// 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()) {
+
+				// does the outline-polygon contain this position?
+				if (!isPartOfFloorOutline(x_cm, y_cm, floor->outline)) {continue;}
+
+				// 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.012345);
+				if (intersects(bbox, floor)) {continue;}
+
+				// add to the grid
+				T t(x_cm, y_cm, z_cm);
+				t.setType(getType(bbox, floor));
+				if (grid.hasNodeFor(t)) {continue;}
+				grid.add(t);
+
+			}
+			if (listener) {listener->onGridBuildUpdateMinor(total, ++cur);}
+		}
+
+		// connect the g
+		connectAdjacent(z_cm);
+
+	}
+
+	void buildStairs(const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) {
+
+		const int total = floor->stairs.size();
+		int cur = 0;
+
+		// process each stair within the floor
+		for (const Floorplan::Stair* stair : floor->stairs) {
+			if (listener) {listener->onGridBuildUpdateMinor("adding " + floor->name + " stair " + std::to_string(cur+1));}
+			stairs.build(floor, stair);
+			if (listener) {listener->onGridBuildUpdateMinor(total, ++cur);}
+		}
+
+	}
+
+
+
+
+
+
+
+
+
+
+
+//	void addWithin(const Floorplan::Floor* floor, std::vector<Entry>& nodeList) {
+
+//		const int fz1_cm = floor->atHeight*100;
+//		const int fz2_cm = (floor->atHeight+floor->height)*100;
+
+//		for (Entry& e : nodeList) {
+
+//			if ( (e.pos.z_cm <= fz1_cm) || (e.pos.z_cm >= fz2_cm) ) {
+//				e.idx = grid.getNearestNode(e.pos).getIdx();
+//				e.part = {9999};
+//			} else {
+//				const T t(e.pos.x_cm, e.pos.y_cm, e.pos.z_cm);
+//				e.idx = grid.addUnaligned(t, false);
+//			}
+//		}
+
+
+//	}
+
+//	void filter(std::vector<Entry>& nodeList) {
+
+//		const int gs_cm = grid.getGridSize_cm();
+//		const int limit_cm = gs_cm * 0.50;
+
+//		// remove duplicate nodes or nodes, that are too close to each other
+//		for(auto it = nodeList.begin(); it != nodeList.end(); ) {
+
+//			// currently examined node
+//			const Entry& e1 = *it;
+
+//			// matches for nodes that are NOT the same instance AND nearby (< grid-size)
+//			auto matches = [&] (const Entry& e2) { return (e1.pos != e2.pos) && (e1.pos.getDistanceInCM(e2.pos) <= limit_cm); };
+//			auto match = std::find_if(nodeList.begin(), nodeList.end(), matches);
+
+//			// remove if this node has a nearby neighbor
+//			if (match != nodeList.end()) {
+
+//				// combine both nodes within one:
+//				// the node belongs to more than one stair-part
+//				Entry& e2 = *match;
+//				e2.part.insert(e2.part.end(), e1.part.begin(), e1.part.end());
+//				it = nodeList.erase(it);
+
+//			} else {
+//				++it;
+//			}
+
+//		}
+
+//	}
+
+//	std::vector<Entry> minOnly(const std::vector<Entry>& lst) {
+//		auto comp = [&] (const Entry& a, const Entry& b) {return grid[a.idx].z_cm < grid[b.idx].z_cm;};
+//		auto it = std::min_element(lst.begin(), lst.end(), comp);
+//		T& min = grid[it->idx];
+//		std::vector<Entry> res;
+//		for (const Entry& e : lst) {
+//			if (grid[e.idx].z_cm == min.z_cm) {res.push_back(e);}
+//		}
+//		return res;
+//	}
+
+	/** connect all neighboring nodes part of the given index-vector */
+	void connectAdjacent(const std::vector<int>& indices) {
+
+		for (const int idx : indices) {
+
+			// connect the node with its neighbors
+			connectAdjacent(grid[idx]);
+
+		}
+
+	}
+
+	/** connect all neighboring nodes located on the given height-plane */
+	void connectAdjacent(const float z_cm) {
+
+		Log::add(name, "connecting all adjacent nodes at height " + std::to_string(z_cm), false);
+		Log::tick();
+
+		// connect adjacent grid-points
+		for (T& n1 : grid) {
+
+			// not the floor we are looking for? -> skip (ugly.. slow(er))
+			if (n1.z_cm != z_cm) {continue;}
+
+			// connect the node with its neighbors
+			connectAdjacent(n1);
+
+		}
+
+		Log::tock();
+
+	}
+
+	/** connect the given node with its neighbors */
+	void connectAdjacent(T& n1) {
+
+		const int gridSize_cm = grid.getGridSize_cm();
+
+		// square around the node
+		for (int x =  -1; x <= +1; ++x) {
+			for (int y =  -1; y <= +1; ++y) {
+
+				// skip the center (node itself)
+				if ((x == y) && (x == 0)) {continue;}
+
+				// position of the potential neighbor
+				const int ox = n1.x_cm + x * gridSize_cm;
+				const int oy = n1.y_cm + y * gridSize_cm;
+				const GridPoint p(ox, oy, n1.z_cm);
+
+				// does the grid contain the potential neighbor?
+				const T* n2 = grid.getNodePtrFor(p);
+				if (n2 != nullptr) {
+					grid.connectUniDir(n1, *n2);			// UNI-dir connection as EACH node is processed!
+				}
+
+			}
+		}
+
+	}
+
+	/** add the inverted version of the given z-layer */
+	void addInverted(const Grid<T>& gIn, const float z_cm) {
+
+		// get the original grid's bbox
+		BBox3 bb = gIn.getBBox();
+
+		// ensure we have an outer boundary
+		bb.grow(gIn.getGridSize_cm() * 2);
+
+		const int gridSize_cm = grid.getGridSize_cm();
+
+		// build new grid-points
+		for(int x_cm = bb.getMin().x; x_cm <= bb.getMax().x; x_cm += gridSize_cm) {
+			for (int y_cm = bb.getMin().y; y_cm < bb.getMax().y; y_cm += gridSize_cm) {
+
+				// does the input-grid contain such a point?
+				GridPoint gp(x_cm, y_cm, z_cm);
+				if (gIn.hasNodeFor(gp)) {continue;}
+
+				// add to the grid
+				grid.add(T(x_cm, y_cm, z_cm));
+
+			}
+		}
+
+	}
+
+//	// TODO: how to determine the starting index?!
+
+//	// IDEAS: find all segments:
+//	// start at a random point, add all connected points to the set
+//	// start at a NEW random point ( not part of the already processed points), add connected points to a new set
+//	// repeat until all points processed
+//	// how to handle multiple floor layers?!?!
+//	// run after all floors AND staircases were added??
+//	// OR: random start, check segment size, < 50% of all nodes? start again
+
+//	void removeIsolatedNodes() {
+
+//		Log::add(name, "searching for isolated nodes");
+
+//		// get largest connected region
+//		std::unordered_set<int> set;
+//		do {
+//			const int idxStart = rand() % grid.getNumNodes();
+//			set.clear();
+//			Log::add(name, "getting connected region starting at " + (std::string) grid[idxStart]);
+//			getConnected(grid[idxStart], set);
+//			Log::add(name, "region size is " + std::to_string(set.size()) + " nodes");
+
+//		} while (set.size() < 0.5 * grid.getNumNodes());
+
+//		// remove all other
+//		Log::add(name, "removing the isolated nodes");
+//		for (int i = 0; i < grid.getNumNodes(); ++i) {
+//			if (set.find(i) == set.end()) {grid.remove(i);}
+//		}
+
+//		// clean the grid
+//		grid.cleanup();
+
+//	}
+
+	void removeIsolatedNodes() {
+
+		// try to start at the first stair
+		for (T& n : grid) {
+			if (n.getType() == GridNode::TYPE_STAIR) {removeIsolatedNodes(n); return;}
+		}
+
+		// no stair found? try to start at the first node
+		removeIsolatedNodes(grid[0]);
+
+	}
+
+	/** remove all nodes not connected to n1 */
+	void removeIsolatedNodes(T& n1) {
+
+		// get the connected region around n1
+		Log::add(name, "getting set of all nodes connected to " + (std::string) n1, false);
+		Log::tick();
+		std::unordered_set<int> set;
+		getConnected(n1, set);
+		Log::tock();
+
+		// remove all other
+		Log::add(name, "removing all nodes NOT connected to " + (std::string) n1, false);
+		Log::tick();
+		for (T& n2 : grid) {
+			if (set.find(n2.getIdx()) == set.end()) {grid.remove(n2);}
+		}
+		Log::tock();
+
+		// clean the grid (physically delete the removed nodes)
+		grid.cleanup();
+
+	}
+
+private:
+
+	/** recursively get all connected nodes and add them to the set */
+	void getConnected(T& n1, std::unordered_set<int>& visited) {
+
+		std::unordered_set<int> toVisit;
+		toVisit.insert(n1.getIdx());
+
+		// run while there are new nodes to visit
+		while(!toVisit.empty()) {
+
+			// get the next node
+			int nextIdx = *toVisit.begin();
+			toVisit.erase(nextIdx);
+			visited.insert(nextIdx);
+			T& next = grid[nextIdx];
+
+			// get all his (unprocessed) neighbors and add them to the region
+			for (const T& n2 : grid.neighbors(next)) {
+				if (visited.find(n2.getIdx()) == visited.end()) {
+					toVisit.insert(n2.getIdx());
+				}
+			}
+
+		}
+
+	}
+
+private:
+
+
+
+	/** does the bbox intersect with any of the floor's walls? */
+	static inline bool intersects(const GridNodeBBox& bbox, const Floorplan::Floor* floor) {
+
+		// process each obstacle
+		// (obstacles use meter, while the bbox is in centimeter!
+		for (Floorplan::FloorObstacle* fo : floor->obstacles) {
+
+			// depends on the type of obstacle
+			if (dynamic_cast<Floorplan::FloorObstacleLine*>(fo)) {
+				const Floorplan::FloorObstacleLine* line = (Floorplan::FloorObstacleLine*) fo;
+				const Line2 l2(line->from*100, line->to*100);
+				if (bbox.intersects(l2)) {return true;}
+
+			} 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;}
+
+			} else if (dynamic_cast<Floorplan::FloorObstacleDoor*>(fo)) {
+				// DOORS ARE NOT AN OBSTACLE
+
+			} else {
+				throw Exception("TODO: not yet implemented obstacle type");
+
+			}
+
+		}
+		return false;
+	}
+
+	/** does the bbox intersect with any of the floor's walls? */
+	static inline int getType(const GridNodeBBox& bbox, const Floorplan::Floor* floor) {
+
+		// process each obstacle
+		for (Floorplan::FloorObstacle* fo : floor->obstacles) {
+
+			if (dynamic_cast<Floorplan::FloorObstacleDoor*>(fo)) {
+				const Floorplan::FloorObstacleDoor* door = (Floorplan::FloorObstacleDoor*) fo;
+				const Line2 l2(door->from*100, door->to*100);
+				if (bbox.intersects(l2)) {return GridNode::TYPE_DOOR;}
+
+			}
+
+		}
+
+		return GridNode::TYPE_FLOOR;
+
+	}
+
+
+};
+
+#endif // GRIDFACTORY_V2_H

grid/factory/v2/Helper.h

@@ -0,0 +1,219 @@
+#ifndef GRID_FACTORY_HELPER_H
+#define GRID_FACTORY_HELPER_H
+
+#include "../../../geo/Point2.h"
+#include "../../../geo/Point3.h"
+#include "../../../geo/BBox2.h"
+#include "../../../geo/BBox3.h"
+#include "../../../floorplan/v2/Floorplan.h"
+#include "../../../grid/Grid.h"
+
+/** helper class for polygon methods */
+struct HelperPoly {
+
+	BBox2 bbox_cm;
+	std::vector<Point2> points_cm;
+
+	/** empty ctor */
+	HelperPoly() {
+		;
+	}
+
+	/** ctor from floorplan-polygon */
+	HelperPoly(const Floorplan::FloorOutlinePolygon& poly) {
+		for (Point2 p : poly.poly.points) { add(p * 100); }
+	}
+
+	/** ctor from floorplan-quad */
+	HelperPoly(const Floorplan::Quad3& quad) {
+		add(quad.p1); add(quad.p2); add(quad.p3); add(quad.p4);
+	}
+
+	void add(const Point2 p) {
+		points_cm.push_back(p);
+		bbox_cm.add(p);
+	}
+
+	void add(const Point3& p) {
+		points_cm.push_back(p.xy());
+		bbox_cm.add(p.xy());
+	}
+
+	/** does the polygon contain the given point (in cm)? */
+	bool contains(const Point2 p_cm) const {
+
+		// not within bbox? -> not within polygon
+		if (!bbox_cm.contains(p_cm)) {return false;}
+
+		// ensure the point is at least a bit outside of the polygon
+		const float x1_cm = bbox_cm.getMin().x - 17.71920;
+		const float y1_cm = bbox_cm.getMin().y - 23.10923891;
+
+		// construct line between point outside of the polygon and the point in question
+		const Line2 l(x1_cm, y1_cm, p_cm.x, p_cm.y);
+
+		// determine the number of intersections
+		int hits = 0;
+		const int cnt = points_cm.size();
+		for (int i = 0; i < cnt; ++i) {
+			const Point2 p1 = points_cm[(i+0)%cnt];
+			const Point2 p2 = points_cm[(i+1)%cnt];
+			const Line2 l12(p1, p2);
+			if (l12.getSegmentIntersection(l)) {++hits;}
+		}
+
+		// inside or outside?
+		return ((hits % 2) == 1);
+
+	}
+
+};
+
+
+
+
+
+
+
+
+
+
+
+
+
+template <typename T> class Helper {
+
+private:
+
+	Grid<T>& grid;
+
+public:
+
+	/** ctor */
+	Helper(Grid<T>& grid) : grid(grid) {
+
+	}
+
+	/** connect the given node to all its neighbors */
+	void connectToNeighbors(T& n1) {
+
+		const int gs_cm = grid.getGridSize_cm();
+
+		for (int y = -gs_cm; y <= +gs_cm; y += gs_cm) {
+			for (int x = -gs_cm; x <= +gs_cm; x += gs_cm) {
+
+				// skip the node itself
+				if (x == 0 && y == 0) {continue;}
+
+				// try to find a matching neighbor
+				const GridPoint gp(n1.x_cm + x, n1.y_cm + y, n1.z_cm);
+				const T* n2 = grid.getNodePtrFor(gp);
+				if (!n2) {continue;}
+
+				// connect
+				if (n1.hasNeighbor(n2->getIdx())) {continue;}
+				grid.connectUniDir(n1, *n2);
+
+			}
+		}
+
+	}
+
+
+
+	int gridSize() const {
+		return grid.getGridSize_cm();;
+	}
+
+	float align(const float val) {
+		const float gridSize_cm = gridSize();
+		return std::round(val/gridSize_cm) * gridSize_cm;
+	}
+
+	float alignF(const float val) {
+		const float gridSize_cm = gridSize();
+		return std::floor(val/gridSize_cm) * gridSize_cm;
+	}
+
+	float alignC(const float val) {
+		const float gridSize_cm = gridSize();
+		return std::ceil(val/gridSize_cm) * gridSize_cm;
+	}
+
+	Point3 align(const Point3 p) {
+		return Point3( align(p.x), align(p.y), (p.z) );		// TODO: align z or not?
+	}
+
+	float min(float a, float b, float c, float d) {
+		if (a < b && a < c && a < d) {return a;}
+		if (b < a && b < c && b < d) {return b;}
+		if (c < a && c < b && c < d) {return c;}
+		if (d < a && d < b && d < c) {return d;}
+	}
+	float max(float a, float b, float c, float d) {
+		if (a > b && a > c && a > d) {return a;}
+		if (b > a && b > c && b > d) {return b;}
+		if (c > a && c > b && c > d) {return c;}
+		if (d > a && d > b && d > c) {return d;}
+	}
+
+	void clamp(float& val, const int min, const int max) {
+		while (val < min) {val += gridSize();}
+		while (val > max) {val -= gridSize();}
+	}
+
+
+	void limit(Floorplan::Quad3& q2, const Floorplan::Quad3& q1) {
+		const float x1 = min(q1.p1.x, q1.p2.x, q1.p3.x, q1.p4.x);
+		const float x2 = max(q1.p1.x, q1.p2.x, q1.p3.x, q1.p4.x);
+		const float y1 = min(q1.p1.y, q1.p2.y, q1.p3.y, q1.p4.y);
+		const float y2 = max(q1.p1.y, q1.p2.y, q1.p3.y, q1.p4.y);
+		const float z1 = min(q1.p1.z, q1.p2.z, q1.p3.z, q1.p4.z);
+		const float z2 = max(q1.p1.z, q1.p2.z, q1.p3.z, q1.p4.z);
+//		clamp(q2.p1.x, x1, x2);	clamp(q2.p2.x, x1, x2); clamp(q2.p3.x, x1, x2); clamp(q2.p4.x, x1, x2);
+//		clamp(q2.p1.y, y1, y2);	clamp(q2.p2.y, y1, y2); clamp(q2.p3.y, y1, y2); clamp(q2.p4.y, y1, y2);
+		clamp(q2.p1.z, z1, z2);	clamp(q2.p2.z, z1, z2); clamp(q2.p3.z, z1, z2); clamp(q2.p4.z, z1, z2);
+	}
+
+	Floorplan::Quad3 align(const Floorplan::Quad3& q) {
+		Floorplan::Quad3 q2 = Floorplan::Quad3(align(q.p1), align(q.p2), align(q.p3), align(q.p4));
+		//limit(q2, q);
+		return q2;
+	}
+
+
+
+
+
+	static float dot(const Point2 a, const Point2 b) {
+		return a.x * b.x + a.y * b.y;
+	}
+
+	static float area(const Point2 a, const Point2 b, const Point2 c) {
+		//Point2 ab = b-a;
+		//Point2 ac = c-a;
+		//return 0.5f * std::sqrt(dot(ab,ab)*dot(ac,ac) - dot(ab,ac)*dot(ab,ac));
+		Point2 p1 = b-a;
+		Point2 p2 = c-a;
+		return std::abs(p1.x*p2.y - p2.x*p1.y) * 0.5;
+	}
+
+	static bool bary(Point2 p, Point2 a, Point2 b, Point2 c, float &u, float &v, float &w) {
+		Point2 v0 = b - a, v1 = c - a, v2 = p - a;
+		float d00 = dot(v0, v0);
+		float d01 = dot(v0, v1);
+		float d11 = dot(v1, v1);
+		float d20 = dot(v2, v0);
+		float d21 = dot(v2, v1);
+		float denom = d00 * d11 - d01 * d01;
+		v = (d11 * d20 - d01 * d21) / denom;
+		w = (d00 * d21 - d01 * d20) / denom;
+		u = 1.0f - v - w;
+		return (u <= 1 && v <= 1 && w <= 1) && (u >= 0 && v >= 0 && w >= 0);
+	}
+
+
+
+};
+
+#endif // GRID_FACTORY_HELPER_H

grid/factory/v2/Importance.h

@@ -0,0 +1,217 @@
+#ifndef IMPORTANCE_H
+#define IMPORTANCE_H
+
+#include "../../Grid.h"
+
+#include "../../../misc/KNN.h"
+#include "../../../misc/KNNArray.h"
+
+#include "../../../math/MiniMat2.h"
+#include "../../../math/Distributions.h"
+
+
+class Importance {
+
+private:
+
+	static constexpr const char* name = "GridImp";
+
+public:
+
+
+	template <typename T> static void addOutlineNodes(Grid<T>& dst, Grid<T>& src) {
+
+		for (const T& n : src) {
+			if (n.getNumNeighbors() < 8) {
+				if (!dst.hasNodeFor(n)) {
+					dst.add(n);
+				}
+			}
+		}
+
+	}
+
+	/** attach importance-factors to the grid */
+	template <typename T> static void addImportance(Grid<T>& g, const float z_cm) {
+
+		Log::add(name, "adding importance information to all nodes at height " + std::to_string(z_cm));
+
+		// get an inverted version of the grid
+		Grid<T> inv(g.getGridSize_cm());
+		addOutlineNodes(inv, g);
+		//GridFactory<T> fac(inv);
+		//fac.addInverted(g, z_cm);
+
+		// sanity check
+		Assert::isFalse(inv.getNumNodes() == 0, "inverted grid is empty!");
+
+		// construct KNN search
+		KNN<Grid<T>, 3> knn(inv);
+
+		// the number of neighbors to use
+		static constexpr int numNeighbors = 12;
+
+		// create list of all doors
+		std::vector<T> doors;
+
+		// process each node
+		for (T& n1 : g) {
+
+			// is the current node a door?
+			//if (isDoor(n1, neighbors)) {doors.push_back(n1);}					// OLD
+			if (n1.getType() == GridNode::TYPE_DOOR) {doors.push_back(n1);}		// NEW!
+
+			// favor stairs just like doors
+			//if (isStaircase(g, n1)) {doors.push_back(n1);}					// OLD
+			if (n1.getType() == GridNode::TYPE_STAIR) {doors.push_back(n1);}	// NEW
+
+		}
+
+		KNNArray<std::vector<T>> knnArrDoors(doors);
+		KNN<KNNArray<std::vector<T>>, 3> knnDoors(knnArrDoors);
+
+		Distribution::Normal<float> favorDoors(0.0f, 0.6f);
+
+		// process each node again
+		for (T& n1 : g) {
+
+			// skip nodes on other than the requested floor-level
+			//if (n1.z_cm != z_cm) {continue;}
+
+			// get the 10 nearest neighbors and their distance
+			size_t indices[numNeighbors];
+			float squaredDist[numNeighbors];
+			float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
+			knn.get(point, numNeighbors, indices, squaredDist);
+
+			// get the neighbors
+			std::vector<T*> neighbors;
+			for (int i = 0; i < numNeighbors; ++i) {
+				neighbors.push_back(&inv[indices[i]]);
+			}
+
+			n1.imp = 1.0f;
+
+			//if (n1.getType() == GridNode::TYPE_FLOOR) {
+
+			// get the distance to the nearest door
+			const float distToWall_m = Units::cmToM(std::sqrt(squaredDist[0]) + g.getGridSize_cm());
+
+			// get the distance to the nearest door
+			const float distToDoor_m = Units::cmToM(knnDoors.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
+
+			n1.imp =
+					1 +
+					getWallImportance( distToWall_m ) +
+					favorDoors.getProbability(distToDoor_m);
+
+
+			//}
+			//addDoor(n1, neighbors);
+
+			// importance for this node (based on the distance from the next door)
+			//n1.imp += favorDoors.getProbability(dist_m) * 0.30;
+
+
+			//n1.imp = (dist_m < 0.2) ? (1) : (0.5);
+		}
+
+	}
+
+	/** is the given node connected to a staircase? */
+	template <typename T> static bool isStaircase(Grid<T>& g, T& node) {
+
+		return node.getType() == GridNode::TYPE_STAIR;
+
+//		// if this node has a neighbor with a different z, this is a stair
+//		for (T& neighbor : g.neighbors(node)) {
+//			if (neighbor.z_cm != node.z_cm) {return true;}
+//		}
+//		return false;
+
+	}
+
+	/** is the given node (and its inverted neighbors) a door? */
+	template <typename T> static bool isDoor( T& nSrc, std::vector<T*> neighbors ) {
+
+		if (nSrc.getType() != GridNode::TYPE_FLOOR) {return false;}
+
+		MiniMat2 m1;
+//		MiniMat2 m2;
+		Point3 center = nSrc;
+
+		// calculate the centroid of the nSrc's nearest-neighbors
+		Point3 centroid(0,0,0);
+		for (const T* n : neighbors) {
+			centroid = centroid + (Point3)*n;
+		}
+		centroid /= neighbors.size();
+
+		// if nSrc is too far from the centroid, this does not make sense
+		if ((centroid-center).length() > 40) {return false;}
+
+		// build covariance of the nearest-neighbors
+		int used = 0;
+		for (const T* n : neighbors) {
+
+			const Point3 d1 = (Point3)*n - centroid;
+			if (d1.length() > 100) {continue;}	// radius search
+			m1.addSquared(d1.x, d1.y);
+
+//			const Point3 d2 = (Point3)*n - center;
+//			if (d2.length() > 100) {continue;}	// radius search
+//			m2.addSquared(d2.x, d2.y);
+
+			++used;
+
+		}
+
+		// we need at least two points for the covariance
+		if (used < 6) {return false;}
+
+		// check eigenvalues
+		MiniMat2::EV ev1 = m1.getEigenvalues();
+//		MiniMat2::EV ev2 = m2.getEigenvalues();
+
+		// ensure e1 > e2
+		if (ev1.e1 < ev1.e2) {std::swap(ev1.e1, ev1.e2);}
+//		if (ev2.e1 < ev2.e2) {std::swap(ev2.e1, ev2.e2);}
+
+		// door?
+		const float ratio1 = (ev1.e2/ev1.e1);
+//		const float ratio2 = (ev2.e2/ev2.e1);
+//		const float ratio3 = std::max(ratio1, ratio2) / std::min(ratio1, ratio2);
+
+		return (ratio1 < 0.30 && ratio1 > 0.05) ;
+
+	}
+
+	/** get the importance of the given node depending on its nearest wall */
+	static float getWallImportance(float dist_m) {
+
+		// avoid sticking too close to walls (unlikely)
+		static Distribution::Normal<float> avoidWalls(0.0, 0.5);
+
+		// favour walking near walls (likely)
+		static Distribution::Normal<float> stickToWalls(0.9, 0.5);
+
+		// favour walking far away (likely)
+		static Distribution::Normal<float> farAway(2.2, 0.5);
+		if (dist_m > 2.0) {dist_m = 2.0;}
+
+		// overall importance
+//		return	- avoidWalls.getProbability(dist_m) * 0.30		// avoid walls
+//				+ stickToWalls.getProbability(dist_m) * 0.15	// walk near walls
+//				+ farAway.getProbability(dist_m) * 0.15			// walk in the middle
+		return	- avoidWalls.getProbability(dist_m)		// avoid walls
+				//+ stickToWalls.getProbability(dist_m)	// walk near walls
+				//+ farAway.getProbability(dist_m)			// walk in the middle
+		;
+
+
+	}
+
+
+};
+
+#endif // IMPORTANCE_H

grid/factory/v2/Stairs.h

@@ -0,0 +1,281 @@
+#ifndef STAIRS_H
+#define STAIRS_H
+
+#include <vector>
+#include <set>
+
+#include "../../Grid.h"
+#include "Helper.h"
+#include "../../../floorplan/v2/Floorplan.h"
+
+
+
+
+
+template <typename T> class Stairs {
+
+private:
+
+	/** the grid to build into */
+	Grid<T>& grid;
+
+	/** calculation helper */
+	Helper<T> helper;
+
+
+private:
+
+//	struct Entry {
+//		GridPoint pos;
+//		std::vector<int> part;
+//		int idx;
+//		Entry(const GridPoint pos, int part) : pos(pos) {
+//			this->part.push_back(part);
+//		}
+//	};
+
+	/** 2D integer point */
+	struct XY {
+		int x;
+		int y;
+		XY() : x(-1), y(-1) {;}
+		XY(const int x, const int y) : x(x), y(y) {;}
+		bool operator == (const XY& o) const {return (x == o.x) && (y == o.y);}
+	};
+
+
+	/** 3D integer point */
+	struct XYZ {
+		int x;
+		int y;
+		int z;
+		XYZ() : x(-1), y(-1), z(-1) {;}
+		XYZ(const int x, const int y, const int z) : x(x), y(y), z(z) {;}
+		bool operator == (const XYZ& o) const {return (x == o.x) && (y == o.y) && (z == o.z);}
+	};
+
+	/** and intermediate grid node */
+	struct Intermediate {
+		XY xy;
+		T node;
+		int idx;
+		std::vector<XY> con;
+		Intermediate(const XY xy, const T node) : xy(xy),node(node), idx(-1) {;}
+	};
+
+
+public:
+
+	/** ctor */
+	Stairs(Grid<T>& grid) : grid(grid), helper(grid) {
+		;
+	}
+
+
+
+
+
+	void build(const Floorplan::Floor* floor, const Floorplan::Stair* stair) {
+
+		const int gs_cm = grid.getGridSize_cm();
+
+		std::vector<Intermediate> stairNodes;
+
+		// process each part
+		int partIdx = 0;
+
+		const std::vector<Floorplan::StairPart> parts = stair->getParts();
+		const std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(parts, floor);
+
+		for (int i = 0; i < (int)parts.size(); ++i) {
+
+			//const Floorplan::StairPart& part = parts[i];
+			const Floorplan::Quad3& quad = helper.align(quads[i]*100);
+
+			const Point3 p1 = quad.p1;
+			const Point3 p2 = quad.p2;
+			const Point3 p3 = quad.p3;
+			const Point3 p4 = quad.p4;
+
+			HelperPoly poly; poly.add(p1.xy()); poly.add(p2.xy()); poly.add(p3.xy()); poly.add(p4.xy());
+
+			const int x1 = helper.align(poly.bbox_cm.getMin().x);		// ALIGNF?
+			const int y1 = helper.align(poly.bbox_cm.getMin().y);
+			const int x2 = helper.align(poly.bbox_cm.getMax().x);		// ALIGNC?
+			const int y2 = helper.align(poly.bbox_cm.getMax().y);
+
+			std::vector<XYZ> points;
+
+			// get all points that are part of the stair-part-polygon
+			for (int x = x1; x <= x2; x+=gs_cm) {
+				for (int y = y1; y <= y2; y+=gs_cm) {
+					if (!poly.contains( Point2(x,y) )) {continue;}
+					points.push_back(XYZ(x,y,-1));
+				}
+			}
+
+			// now determine each corresponding z-coordinate
+			for (XYZ& xy : points) {
+
+				const Point2 p(xy.x, xy.y);
+				float u,v,w;
+
+				if (helper.bary(p, p1.xy(), p2.xy(), p3.xy(), u, v, w)) {
+					xy.z = p1.z*u + p2.z*v + p3.z*w;
+				} else {
+					helper.bary(p, p1.xy(), p3.xy(), p4.xy(), u, v, w);
+					xy.z = p1.z*u + p3.z*v + p4.z*w;
+				}
+
+			}
+
+			// stair start, ensure min snaps to the floor
+			if (partIdx == 0) {
+				const float minz_cm = minZ(points);
+				const float maxz_cm = maxZ(points);
+				const int mz = floor->atHeight * 100;
+				if (minz_cm != mz) {
+					for (XYZ& xyz : points) {
+						const float percent = ((xyz.z - minz_cm) / (maxz_cm - minz_cm));
+						xyz.z = mz + percent * (maxz_cm - mz);
+					}
+				}
+			}
+
+			// stair end, ensure max snaps to the floor
+			if (partIdx == (int) parts.size() - 1) {
+				const float minz_cm = minZ(points);
+				const float maxz_cm = maxZ(points);
+				const int mz = (floor->atHeight+floor->height) * 100;
+				if (maxz_cm != mz) {
+					for (XYZ& xyz : points) {
+						const float percent = ((xyz.z - minz_cm) / (maxz_cm - minz_cm));
+						xyz.z = minz_cm + percent * (mz - minz_cm);
+					}
+				}
+			}
+
+
+
+			for (XYZ& xy : points) {
+
+				// construct a grid-node
+				T node(xy.x, xy.y, xy.z);
+
+				// construct an intermediate-node containing all desired connections
+				Intermediate iNode(XY(xy.x, xy.y), node);
+				for (int ox = -1; ox <= +1; ++ox) {
+					for (int oy = -1; oy <= +1; ++oy) {
+						if (ox == 0 && oy == 0) {continue;}
+						const int nx = xy.x + ox * gs_cm;
+						const int ny = xy.y + oy * gs_cm;
+						iNode.con.push_back( XY(nx, ny) );
+					}
+				}
+				stairNodes.push_back(iNode);
+
+			}
+
+
+			++partIdx;
+
+		}
+
+
+		// keep a list of all vertices below stairwells and remove them hereafter
+		std::vector<T*> toDelete;
+
+		// add all stair nodes or replace them with already existing ones
+		for (Intermediate& iNode : stairNodes) {
+
+			// nearer than minDiff? -> use existing node
+			const float minDiff = gs_cm * 0.49;
+
+			// nearer than maxDiff? -> delete as this one is unreachable below the stair
+			const float maxDiff = 150;
+
+			// distance between the stair node and the floor above / below
+			const float zDiff1 = std::abs(iNode.node.z_cm - 100*floor->getStartingZ());
+			const float zDiff2 = std::abs(iNode.node.z_cm - 100*floor->getEndingZ());
+
+			iNode.idx = -1;
+
+			// try to connect the stair-node to the floor below
+			if (zDiff1 < minDiff) {
+				const T* n2 = grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, 100*floor->getStartingZ()));
+				if (n2) {
+					iNode.idx = n2->getIdx();
+				} else {
+					iNode.idx = grid.add(iNode.node);				// add a new one
+					helper.connectToNeighbors(grid[iNode.idx]);		// and connect to the floor's grid nodes
+				}
+
+			// try to connect the stair-node to the floor above
+			} else if (zDiff2 < minDiff) {
+				const T* n2 = grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, 100*floor->getEndingZ()));
+				if (n2) {
+					iNode.idx = n2->getIdx();
+				} else {
+					iNode.idx = grid.add(iNode.node);				// add a new one
+					helper.connectToNeighbors(grid[iNode.idx]);		// and connect to the floor's grid nodes
+				}
+
+			// remove nodes directly below the stair
+			} else if (zDiff1 < maxDiff) {
+				T* n2 = (T*) grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, 100*floor->getStartingZ()));
+				if (n2) {toDelete.push_back(n2);}
+
+			// remove nodes directly above the stair
+			} else if (zDiff2 < maxDiff) {
+				T* n2 = (T*) grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, 100*floor->getEndingZ()));
+				if (n2) {toDelete.push_back(n2);}
+			}
+
+			// no matching node found -> add a new one to the grid
+			if (iNode.idx == -1) {
+				iNode.idx = grid.add(iNode.node);
+			}
+
+			// add semantic information
+			grid[iNode.idx].setType(GridNode::TYPE_STAIR);
+
+		}
+
+		// connect to-be-connected stair-nodes (each node with its [previously configured] neighbors)
+		for (const Intermediate& t1 : stairNodes) {
+			for (const XY& connectTo : t1.con) {
+				for (const Intermediate& t2 : stairNodes) {
+					if (connectTo == t2.xy) {
+						T& n1 = grid[t1.idx];				// use the nodes already added to the grid
+						T& n2 = grid[t2.idx];				// use the nodes already added to the grid
+						if (n1.hasNeighbor(n2.getIdx()))	{continue;}
+						if (n1.getIdx() == n2.getIdx())		{continue;}
+						if (n1.getNumNeighbors() >= 10)		{continue;}
+						grid.connectUniDir(n1, n2);
+					}
+				}
+			}
+		}
+
+		// delete all pending nodes and perform a cleanup
+		for (T* n : toDelete) {grid.remove(*n);}
+		grid.cleanup();
+
+
+	}
+
+	static float minZ(const std::vector<XYZ>& points) {
+		auto comp = [] (const XYZ& a, const XYZ& b) {return a.z < b.z;};
+		auto it = std::min_element(points.begin(), points.end(), comp);
+		return it->z;
+	}
+
+	static float maxZ(const std::vector<XYZ>& points) {
+		auto comp = [] (const XYZ& a, const XYZ& b) {return a.z < b.z;};
+		auto it = std::max_element(points.begin(), points.end(), comp);
+		return it->z;
+	}
+
+};
+
+#endif // STAIRS_H