worked on 3D walls

floorplan/3D/WallsViaCuttedQuads.h

@@ -3,9 +3,12 @@
 
 #include "../../geo/Line2.h"
 #include "../../geo/Polygon2.h"
+#include "../../geo/GPCPolygon2.h"
+
 #include "Walls.h"
 #include "misc.h"
 
+#include <functional>
 #include <iostream>
 
 namespace Floorplan3D {
@@ -174,32 +177,133 @@ namespace Floorplan3D {
 			Obstacle3D obs(type, wall.line->material);
 
 			const float z1 = fp.z1;
-			const float z2 = fp.z2;
+			const float z2 = (wall.line->height_m > 0) ? (fp.z1 + wall.line->height_m) : (fp.z2);
 
-			const Point3 p1 = Point3(wall.getP1().x, wall.getP1().y, z1);
+//			const Point3 p1 = Point3(wall.getP1().x, wall.getP1().y, z1);
-			const Point3 p2 = Point3(wall.getP2().x, wall.getP2().y, z1);
+//			const Point3 p2 = Point3(wall.getP2().x, wall.getP2().y, z1);
-			const Point3 p3 = Point3(wall.getP3().x, wall.getP3().y, z1);
+//			const Point3 p3 = Point3(wall.getP3().x, wall.getP3().y, z1);
-			const Point3 p4 = Point3(wall.getP4().x, wall.getP4().y, z1);
+//			const Point3 p4 = Point3(wall.getP4().x, wall.getP4().y, z1);
 
-			const Point3 p1u = Point3(wall.getP1().x, wall.getP1().y, z2);
+//			const Point3 p1u = Point3(wall.getP1().x, wall.getP1().y, z2);
-			const Point3 p2u = Point3(wall.getP2().x, wall.getP2().y, z2);
+//			const Point3 p2u = Point3(wall.getP2().x, wall.getP2().y, z2);
-			const Point3 p3u = Point3(wall.getP3().x, wall.getP3().y, z2);
+//			const Point3 p3u = Point3(wall.getP3().x, wall.getP3().y, z2);
-			const Point3 p4u = Point3(wall.getP4().x, wall.getP4().y, z2);
+//			const Point3 p4u = Point3(wall.getP4().x, wall.getP4().y, z2);
 
-			obs.addQuad(p1, p2, p2u, p1u);
+//			obs.addQuad(p1, p2, p2u, p1u);
-			obs.addQuad(p2, p3, p3u, p2u);
+//			obs.addQuad(p2, p3, p3u, p2u);
-			obs.addQuad(p3, p4, p4u, p3u);
+//			obs.addQuad(p3, p4, p4u, p3u);
-			obs.addQuad(p4, p1, p1u, p4u);
+//			obs.addQuad(p4, p1, p1u, p4u);
 
-			obs.addQuad(p1u, p2u, p3u, p4u);
+//			obs.addQuad(p1u, p2u, p3u, p4u);
-			obs.addQuad(p4, p3, p2, p1);
+//			obs.addQuad(p4, p3, p2, p1);
 
-			obs.reverseFaces();
+//			obs.reverseFaces();
+
+//			return obs;
+
+			Point3 p0 = Point3(wall.line->from.x, wall.line->from.y, z1);
+
+			Point3 p1 = Point3(wall.getP1().x, wall.getP1().y, z1);
+			Point3 p2 = Point3(wall.getP2().x, wall.getP2().y, z1);
+			Point3 p3 = Point3(wall.getP3().x, wall.getP3().y, z1);
+			Point3 p4 = Point3(wall.getP4().x, wall.getP4().y, z1);
+
+			Point3 p1u = Point3(wall.getP1().x, wall.getP1().y, z2);
+			Point3 p2u = Point3(wall.getP2().x, wall.getP2().y, z2);
+			Point3 p3u = Point3(wall.getP3().x, wall.getP3().y, z2);
+			Point3 p4u = Point3(wall.getP4().x, wall.getP4().y, z2);
+
+			Point3 o = p0;
+			float t = wall.line->thickness_m / 2;
+
+			const Point3 o1;// = p1;
+			const Point3 o2;// = p4;
+			const float a = std::atan2(wall.getP2().y - wall.getP1().y, wall.getP2().x - wall.getP1().x);
+
+			auto flatten = [a,o] (const Point3 p) {return (p - o).rotZ(-a).xz();};
+			//auto flatten2 = [a,o] (const Point3 p) {return (p - o).rotZ(-a).xz();};
+
+			auto unFlattenFront =	[o,t,a] (const Point3 p) {return Point3(p.x, +t, p.y).rotZ(a)+o;};
+			auto unFlattenBack =	[o,t,a] (const Point3 p) {return Point3(p.x, -t, p.y).rotZ(a)+o;};
+
+			const Point2 fp1 = flatten(p1);
+			const Point2 fp2 = flatten(p2);
+			const Point2 fp3 = flatten(p3);
+			const Point2 fp4 = flatten(p4);
+			const Point2 fp1u = flatten(p1u);
+			const Point2 fp2u = flatten(p2u);
+			const Point2 fp3u = flatten(p3u);
+			const Point2 fp4u = flatten(p4u);
+
+
+			Polygon2 front;
+			front.add({fp1, fp2, fp2u, fp1u});
+
+			GPCPolygon2 gpFront;
+			gpFront.add(front);
+
+			Polygon2 back;
+			back.add({fp3, fp4, fp4u, fp3u});
+
+			GPCPolygon2 gpBack;
+			gpBack.add(back);
+
+			for (const Floorplan::FloorObstacleWallDoor* door : wall.line->doors) {
+				Polygon2 pDoor;
+
+				const Point2 pds = door->getStart(wall.line);
+				const Point2 pde = door->getEnd(wall.line);
+
+				const Point3  dp1(pds.x, pds.y, z1);
+				const Point3  dp2(pde.x, pde.y, z1);
+				const Point3 dp2u(pde.x, pde.y, z1+door->height);
+				const Point3 dp1u(pds.x, pds.y, z1+door->height);
+
+				//auto flattenD1 = [dp1,a] (const Point3 p) {return (p - dp1).rotZ(-a).xz();};
+				//auto unFlatten1 = [dp1,a] (const Point3 p) {return Point3(p.x, 0, p.y).rotZ(a)+dp1;};
+
+				pDoor.add(flatten(dp1));
+				pDoor.add(flatten(dp2));
+				pDoor.add(flatten(dp2u));
+				pDoor.add(flatten(dp1u));
+
+				gpFront.remove(pDoor);
+				gpBack.remove(pDoor);
+
+			}
+
+			std::vector<Triangle3> triasFront = gpFront.getTriangles();
+
+			for (Triangle3 tria : triasFront) {
+				fixTria(tria, unFlattenFront);
+				tria.reverse();
+				obs.triangles.push_back(tria);
+			}
+
+			std::vector<Triangle3> triasBack = gpBack.getTriangles();
+
+			for (Triangle3 tria : triasBack) {
+				fixTria(tria, unFlattenBack);
+				obs.triangles.push_back(tria);
+			}
+
+
+//			for (const Floorplan::FloorObstacleWallDoor* door : wall.line->doors) {
+//				const Point2 p1 = door->getStart(wall.line);
+//				const Point2 p2 = door->getEnd(wall.line);
+
+//			}
 
 			return obs;
 
 		}
 
+		void fixTria(Triangle3& t, std::function<Point3(Point3)> func) {
+			t.p1 = func(t.p1);
+			t.p2 = func(t.p2);
+			t.p3 = func(t.p3);
+		}
+
 		/** cut off walls ending within another wall */
 		std::vector<Wall> cutProtruding(std::vector<Wall> walls) {
 

floorplan/v2/Floorplan.h

@@ -192,6 +192,7 @@ namespace Floorplan {
 	struct Stair;
 	struct Elevator;
 	struct GroundTruthPoint;
+	struct FloorObstacleWallDoor;
 
 	struct FloorOutline : public std::vector<FloorOutlinePolygon*> {
 		bool enabled = true;
@@ -350,6 +351,7 @@ namespace Floorplan {
 		Point2 to;
 		float thickness_m;
 		float height_m = 0;	// 0 = floor's height
+		std::vector<FloorObstacleWallDoor*> doors;
 		FloorObstacleLine(const ObstacleType type, const Material material, const Point2 from, const Point2 to, const float thickness_m = 0.2f, const float height_m = 0) : FloorObstacle(material), type(type), from(from), to(to), thickness_m(thickness_m), height_m(height_m) {;}
 		FloorObstacleLine(const ObstacleType type, const Material material, const float x1, const float y1, const float x2, const float y2, const float thickness_m = 0.2f, const float height_m = 0) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2), thickness_m(thickness_m), height_m(height_m) {;}
 	};
@@ -376,6 +378,25 @@ namespace Floorplan {
 		float getSize() const {return (to-from).length();}
 	};
 
+	/** door obstacle */
+	struct FloorObstacleWallDoor : public FloorObstacle {
+		DoorType type;
+		float atLinePos;
+		float width;
+		float height;
+		bool leftRight = false;
+		bool inOut = false;
+		FloorObstacleWallDoor(const DoorType type, const Material material, const float atLinePos, const float width, const float height) : FloorObstacle(material), type(type), atLinePos(atLinePos), width(width), height(height) {;}
+		Point2 getStart(const FloorObstacleLine* wall) const {
+			const Point2 dir = wall->to - wall->from;
+			return wall->from + dir * atLinePos;
+		}
+		Point2 getEnd(const FloorObstacleLine* wall) const {
+			const Point2 dir = wall->to - wall->from;
+			return getStart(wall) + dir.normalized() * (leftRight ? -width : +width);
+		}
+	};
+
 	/** 3D obstacle */
 	struct FloorObstacleObject : public FloorObstacle {
 		std::string file;

geo/GPCPolygon2.h

@@ -2,6 +2,8 @@
 #define GPCPOLYGON2_H
 
 #include "../lib/gpc/gpc.cpp.h"
+#include "Polygon2.h"
+#include "Triangle3.h"
 
 class GPCPolygon2 {
 
@@ -44,6 +46,11 @@ public:
 		;
 	}
 
+	void add(const Polygon2& poly) {
+		GPCPolygon cur;
+		toGPC(poly, cur);
+		gpc_polygon_clip(GPC_UNION, &state, &cur, &state);
+	}
 	void add(const Floorplan::Polygon2& poly) {
 		GPCPolygon cur;
 		toGPC(poly, cur);
@@ -55,6 +62,11 @@ public:
 		toGPC(poly, cur);
 		gpc_polygon_clip(GPC_DIFF, &state, &cur, &state);
 	}
+	void remove(const Polygon2& poly) {
+		GPCPolygon cur;
+		toGPC(poly, cur);
+		gpc_polygon_clip(GPC_DIFF, &state, &cur, &state);
+	}
 
 	std::vector<std::vector<Point3>> get() {
 
@@ -88,6 +100,57 @@ public:
 
 	}
 
+	std::vector<Triangle3> getTriangles() {
+
+		gpc_tristrip res;
+		res.num_strips = 0;
+		res.strip = nullptr;
+
+		//res.strip = (gpc_vertex_list*) malloc(1024);
+		gpc_polygon_to_tristrip(&state, &res);
+
+		std::vector<Triangle3> trias;
+
+		for (int i = 0; i < res.num_strips; ++i) {
+			gpc_vertex_list lst = res.strip[i];
+			for (int j = 2; j < lst.num_vertices; ++j) {
+
+				gpc_vertex& v1 = lst.vertex[j - 2];
+				gpc_vertex& v2 = lst.vertex[j - 1];
+				gpc_vertex& v3 = lst.vertex[j];
+
+				// https://en.wikipedia.org/wiki/Triangle_strip
+				// GL_TRIANGLE_STRIP
+				// Draws a series of triangles (three-sided polygons) using vertices v0, v1, v2, then v2, v1, v3 (note the order), then v2, v3, v4, and so on. The ordering is to ensure that the triangles are all drawn with the same orientation so that the strip can correctly form part of a surface.
+				// For odd n, vertices n, n+1, and n+2 define triangle n. For even n, vertices n+1, n, and n+2 define triangle n. N-2 triangles are drawn.
+
+				if (j % 2 == 0) {
+					Triangle3 tria(
+						Point3(v1.x, v1.y, z),
+						Point3(v2.x, v2.y, z),
+						Point3(v3.x, v3.y, z)
+					);
+					trias.push_back(tria);
+				} else {
+					Triangle3 tria(
+						Point3(v2.x, v2.y, z),
+						Point3(v1.x, v1.y, z),
+						Point3(v3.x, v3.y, z)
+					);
+					trias.push_back(tria);
+				}
+
+
+			}
+
+		}
+
+		gpc_free_tristrip(&res);
+
+		return std::move(trias);
+
+	}
+
 	std::vector<std::vector<Point3>> get(float z) {
 
 		gpc_tristrip res;
@@ -118,7 +181,7 @@ public:
 
 private:
 
-	void toGPC(Floorplan::Polygon2 poly, GPCPolygon& result) {
+	void toGPC(const Floorplan::Polygon2& poly, GPCPolygon& result) {
 
 		std::vector<gpc_vertex> verts;
 		for (Point2 p2 : poly.points) {
@@ -133,5 +196,20 @@ private:
 
 	}
 
+	void toGPC(const Polygon2& poly, GPCPolygon& result) {
+
+		std::vector<gpc_vertex> verts;
+		for (Point2 p2 : poly) {
+			gpc_vertex vert; vert.x = p2.x; vert.y = p2.y;
+			verts.push_back(vert);
+		}
+
+		gpc_vertex_list list;
+		list.num_vertices = verts.size();
+		list.vertex = verts.data();
+		gpc_add_contour(&result, &list, 0);
+
+	}
+
 };
 #endif

geo/Line2.h

@@ -232,7 +232,7 @@ public:
 };
 
 
-static inline bool intersects(const Line2& l1, const Line2& l2, bool limit, Point2& pos) {
+static inline bool intersects(const Line2& l1, const Line2& l2, bool limit, Point2& pos, float* _u = nullptr) {
 
 	// (sx1,sy1) + (dx1, dy1)*u  =  (sx2, sy2) + (dx2, dy2)*v
 	// solve((c+d*v-a)/b = (g+h*v-e)/f, v)
@@ -269,6 +269,7 @@ static inline bool intersects(const Line2& l1, const Line2& l2, bool limit, Poin
 
 	if (!limit || (u >= 0 && v >= 0 && u <= 1 && v <= 1)) {
 		pos = Point2(x,y);
+		if (_u) {*_u = u;}
 		return true;
 	}
 
@@ -277,4 +278,6 @@ static inline bool intersects(const Line2& l1, const Line2& l2, bool limit, Poin
 }
 
 
+
+
 #endif // LINE2D_H

geo/Point2.h

@@ -85,6 +85,7 @@ inline void swap(Point2& p1, Point2& p2) {
 }
 
 namespace std {
+
     template <> struct hash<Point2> {
 	    std::size_t operator()(const Point2& p) const {
 			uint32_t x = *((uint32_t*)&(p.x));
@@ -92,6 +93,7 @@ namespace std {
 			return std::hash<uint32_t>()(x^y);
 		}
     };
+
 }
 
 #endif // POINT2_H

geo/Point3.h

@@ -59,6 +59,7 @@ template <typename Scalar> struct _Point3 {
 
 
 	Point2 xy() const {return Point2(x,y);}
+	Point2 xz() const {return Point2(x,z);}
 
 
 	_Point3 rotX(const Scalar r) const {

geo/Polygon2.h

@@ -18,6 +18,10 @@ public:
 		pts.push_back(p);
 	}
 
+	void add(std::initializer_list<Point2> lst) {
+		for (const Point2 p : lst) {add(p);}
+	}
+
 	std::vector<Point2>::iterator begin() {return pts.begin();}
 	std::vector<Point2>::iterator end() {return pts.end();}
 

tests/geo/TestBVH2.cpp

@@ -10,7 +10,7 @@
 
 #include "../../floorplan/v2/Floorplan.h"
 #include "../../floorplan/v2/FloorplanReader.h"
-#include "../../wifi/estimate/ray3/ModelFactory.h"
+//#include "../../wifi/estimate/ray3/ModelFactory.h"
 
 struct WrapperBBox2 {
 

tests/geo/TestBVH3.cpp

@@ -8,9 +8,9 @@
 
 #include "../../floorplan/v2/Floorplan.h"
 #include "../../floorplan/v2/FloorplanReader.h"
-#include "../../wifi/estimate/ray3/ModelFactory.h"
+#include "../../floorplan/3D/Builder.h"
 
-using namespace Ray3D;
+using namespace Floorplan3D;
 
 struct Wrapper {
 
@@ -140,37 +140,37 @@ TEST(BVH, tree3) {
 
 }
 
-TEST(BVH, treeMap) {
+//TEST(BVH, treeMap) {
 
-	std::string file = "/apps/SHL39.xml";
+//	std::string file = "/apps/SHL39.xml";
-	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file);
+//	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file);
 
-	ModelFactory fac(map);
+//	ModelFactory fac(map);
-	fac.setExportCeilings(false);
+//	fac.setExportCeilings(false);
-	fac.setFloors({map->floors[3]});
+//	fac.setFloors({map->floors[3]});
-	FloorplanMesh mesh = fac.getMesh();
+//	FloorplanMesh mesh = fac.getMesh();
-	std::vector<Obstacle3D> obs = mesh.elements;
+//	std::vector<Obstacle3D> obs = mesh.elements;
 
-	BVH3Debug<Obstacle3D, BoundingVolumeSphere3, WrapperObs3D> tree;
+//	BVH3Debug<Obstacle3D, BoundingVolumeSphere3, WrapperObs3D> tree;
 
-	for (const Obstacle3D& o : obs) {
+//	for (const Obstacle3D& o : obs) {
-		tree.add(o);
+//		tree.add(o);
-	}
+//	}
 
-	//tree.show(150);
+//	//tree.show(150);
 
-	//int rounds = tree.optimize();
+//	//int rounds = tree.optimize();
 
-	for (int i = 0; i < 200; ++i) {
+//	for (int i = 0; i < 200; ++i) {
-		tree.optimize(1);
+//		tree.optimize(1);
-		//if (i%3==0) {
+//		//if (i%3==0) {
-		tree.show(250, false);
+//		tree.show(250, false);
-		//}
+//		//}
-	}
+//	}
 
-	int i = 0; (void) i;
+//	int i = 0; (void) i;
 
-}
+//}
 
 TEST(BVH, treeRandom) {
 

tests/ray/TestModelFac.cpp

@@ -1,22 +1,21 @@
 #ifdef WITH_TESTS
 
 #include "../Tests.h"
-#include "../../wifi/estimate/ray3/ModelFactory.h"
+#include "../../floorplan/3D/Builder.h"
 #include "../../floorplan/v2/FloorplanReader.h"
 #include <fstream>
-using namespace Ray3D;
 
-TEST(Ray, ModelFac) {
+TEST(Floorplan3D, ModelFac) {
 
 	//std::string file = "/apps/paper/diss/data/maps/SHL42_nm.xml";
 	std::string file = "/mnt/vm/paper/diss/data/maps/SHL42_nm.xml";
 
 	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file);
 
-	ModelFactory fac(map);
+	Floorplan3D::Builder fac(map);
 	//fac.triangulize();
 
-	FloorplanMesh mesh = fac.getMesh();
+	Floorplan3D::FloorplanMesh mesh = fac.getMesh();
 
 	std::ofstream out("/tmp/fhws.ply");
 	out << mesh.toPLY() << std::endl;

tests/ray/TestRayTrace3.cpp

@@ -18,7 +18,7 @@ TEST(RayTrace3, test) {
 	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file);
 	Floorplan::AccessPoint* ap = map->floors[0]->accesspoints[4];
 
-	ModelFactory fac(map);
+	Builder fac(map);
 	std::ofstream outOBJ("/tmp/vm/map.obj");
 	outOBJ << fac.getMesh().toOBJ("obj").obj;
 	outOBJ.close();