diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index 90edb51..6c82947 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -349,8 +349,9 @@ namespace Floorplan { Point2 from; Point2 to; float thickness_m; - FloorObstacleLine(const ObstacleType type, const Material material, const Point2 from, const Point2 to, const float thickness_m = 0.2f) : FloorObstacle(material), type(type), from(from), to(to), thickness_m(thickness_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) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2), thickness_m(thickness_m) {;} + float height_m = 0; // 0 = floor's height + 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) {;} }; /** circle obstacle */ @@ -380,7 +381,8 @@ namespace Floorplan { std::string file; Point3 pos; Point3 rot; - FloorObstacleObject(const std::string& file, const Point3 pos, const Point3 rot) : file(file), pos(pos), rot(rot) {;} + Point3 scale = Point3(1,1,1); + FloorObstacleObject(const std::string& file, const Point3 pos, const Point3 rot, const Point3 scale) : file(file), pos(pos), rot(rot), scale(scale) {;} }; diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 5c18ccf..f7dd458 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -403,7 +403,8 @@ namespace Floorplan { parseMaterial(el->Attribute("material")), el->FloatAttribute("x1"), el->FloatAttribute("y1"), el->FloatAttribute("x2"), el->FloatAttribute("y2"), - (el->FloatAttribute("thickness") > 0) ? (el->FloatAttribute("thickness")) : (0.15) // default wall thickness in m + (el->FloatAttribute("thickness") > 0) ? (el->FloatAttribute("thickness")) : (0.15), // default wall thickness in m + el->FloatAttribute("height") ); } @@ -433,7 +434,8 @@ namespace Floorplan { return new FloorObstacleObject( el->Attribute("file"), Point3(el->FloatAttribute("x"), el->FloatAttribute("y"), el->FloatAttribute("z")), - Point3(el->FloatAttribute("rx"), el->FloatAttribute("ry"), el->FloatAttribute("rz")) + Point3(el->FloatAttribute("rx", 0), el->FloatAttribute("ry", 0), el->FloatAttribute("rz", 0)), + Point3(el->FloatAttribute("sx", 1), el->FloatAttribute("sy", 1), el->FloatAttribute("sz", 1)) ); } diff --git a/floorplan/v2/FloorplanWriter.h b/floorplan/v2/FloorplanWriter.h index 1619c69..42e6478 100644 --- a/floorplan/v2/FloorplanWriter.h +++ b/floorplan/v2/FloorplanWriter.h @@ -334,6 +334,7 @@ namespace Floorplan { obstacle->SetAttribute("x2", line->to.x); obstacle->SetAttribute("y2", line->to.y); obstacle->SetAttribute("thickness", line->thickness_m); + if (line->height_m != 0) {obstacle->SetAttribute("height", line->height_m);} obstacles->InsertEndChild(obstacle); } @@ -344,7 +345,7 @@ namespace Floorplan { obstacle->SetAttribute("cx", circle->center.x); obstacle->SetAttribute("cy", circle->center.y); obstacle->SetAttribute("radius", circle->radius); - obstacle->SetAttribute("height", circle->height); + if (circle->height != 0) {obstacle->SetAttribute("height", circle->height);} obstacles->InsertEndChild(obstacle); } @@ -369,9 +370,12 @@ namespace Floorplan { obstacle->SetAttribute("x", obj->pos.x); obstacle->SetAttribute("y", obj->pos.y); obstacle->SetAttribute("z", obj->pos.z); - obstacle->SetAttribute("rx", obj->rot.x); - obstacle->SetAttribute("ry", obj->rot.y); - obstacle->SetAttribute("rz", obj->rot.z); + if (obj->rot.x != 0) {obstacle->SetAttribute("rx", obj->rot.x);} + if (obj->rot.y != 0) {obstacle->SetAttribute("ry", obj->rot.y);} + if (obj->rot.z != 0) {obstacle->SetAttribute("rz", obj->rot.z);} + if (obj->scale.x != 1) {obstacle->SetAttribute("sx", obj->scale.x);} + if (obj->scale.y != 1) {obstacle->SetAttribute("sy", obj->scale.y);} + if (obj->scale.z != 1) {obstacle->SetAttribute("sz", obj->scale.z);} obstacles->InsertEndChild(obstacle); } diff --git a/geo/Triangle3.h b/geo/Triangle3.h index af1bda8..f86c4b4 100644 --- a/geo/Triangle3.h +++ b/geo/Triangle3.h @@ -47,6 +47,14 @@ public: return *this; } + /** scale using 3 individual components */ + Triangle3& operator *= (const Point3 o) { + p1 *= o; + p2 *= o; + p3 *= o; + return *this; + } + Point3 getNormal() const { return cross( p2-p1, p3-p1 ).normalized(); } diff --git a/grid/factory/v2/GridFactory.h b/grid/factory/v2/GridFactory.h index 24f3349..61c0601 100755 --- a/grid/factory/v2/GridFactory.h +++ b/grid/factory/v2/GridFactory.h @@ -52,6 +52,7 @@ private: bool _buildStairs = true; bool _removeIsolated = true; bool _addTightToObstacle = false; + bool _abortOnError = true; public: @@ -60,6 +61,8 @@ public: } + void setAbortOnError(const bool abort) {this->_abortOnError = abort;} + void setAddTightToObstacle(const bool tight) {this->_addTightToObstacle = tight;} /** whether or not to build stairs */ @@ -181,7 +184,9 @@ public: // belongs to a "add" polygon? -> remember until all polygons were checked // [might still belong to a "remove" polygon] - res.outdoor = poly->outdoor; + if (poly->outdoor) { + res.outdoor = true; // belonging to an outdoor region overwrites all other belongings + } } } @@ -219,7 +224,7 @@ public: if (foo) { // get the obstacle - const Ray3D::Obstacle3D obs = Ray3D::OBJPool::get().getObject(foo->file).rotated_deg(foo->rot).translated(foo->pos); + const Ray3D::Obstacle3D obs = Ray3D::OBJPool::get().getObject(foo->file).scaled(foo->scale).rotated_deg(foo->rot).translated(foo->pos); // construct its 2D convex hull (in centimter) HelperPoly poly; @@ -295,6 +300,8 @@ public: void buildStairs(const Floorplan::IndoorMap* map, const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) { + stairs.setAbortOnError(_abortOnError); + const int total = floor->stairs.size(); int cur = 0; diff --git a/grid/factory/v2/Stairs2.h b/grid/factory/v2/Stairs2.h index f16b772..4ce9d4b 100644 --- a/grid/factory/v2/Stairs2.h +++ b/grid/factory/v2/Stairs2.h @@ -32,6 +32,7 @@ private: std::vector toDelete; bool tryImproveStairConnections = true; + bool abortOnError = true; private: @@ -47,7 +48,6 @@ private: StairNode(const int x_cm, const int y_cm, const int quadIdx) : x_cm(x_cm), y_cm(y_cm), belongsToQuadIdx(quadIdx) {;} - }; @@ -62,7 +62,10 @@ public: finalize(); } - + /** whether to abort when errors are detected */ + void setAbortOnError(const bool abort) { + this->abortOnError = abort; + } @@ -184,7 +187,11 @@ public: // be sure both are connected to a floor if (!end1OK || !end2OK) { - throw Exception("stair's start or end is not directly connectable to a floor"); + if (abortOnError) { + throw Exception("stair's start or end is not directly connectable to a floor"); + } else{ + std::cout << "stair's start or end is not directly connectable to a floor" << std::endl; + } } } diff --git a/navMesh/NavMeshFactory.h b/navMesh/NavMeshFactory.h index 107ac5d..bf8a78c 100644 --- a/navMesh/NavMeshFactory.h +++ b/navMesh/NavMeshFactory.h @@ -632,7 +632,7 @@ namespace NM { Floorplan::Polygon2 res; // fetch object from pool - const 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).scaled(obj->scale).rotated_deg(obj->rot).translated(obj->pos); // construct 2D convex hull res.points = ConvexHull2::get(obs.getPoints2D()); diff --git a/tests/ray/TestRayTrace3.cpp b/tests/ray/TestRayTrace3.cpp index 663cf8f..f38f12b 100644 --- a/tests/ray/TestRayTrace3.cpp +++ b/tests/ray/TestRayTrace3.cpp @@ -20,7 +20,7 @@ TEST(RayTrace3, test) { ModelFactory fac(map); std::ofstream outOBJ("/tmp/vm/map.obj"); - outOBJ << fac.getMesh().toOBJ(); + outOBJ << fac.getMesh().toOBJ("obj").obj; outOBJ.close(); const int gs_cm = 50; diff --git a/wifi/estimate/ray3/ModelFactory.h b/wifi/estimate/ray3/ModelFactory.h index fbd039c..234c433 100644 --- a/wifi/estimate/ray3/ModelFactory.h +++ b/wifi/estimate/ray3/ModelFactory.h @@ -282,7 +282,7 @@ namespace Ray3D { // attributes const float r = foc->radius; - const float h = (foc->height > 0) ? (foc->height) : (fpos.height); + const float h = (foc->height > 0) ? (foc->height) : (fpos.height); // use either floor's height or user height const Point3 pos(foc->center.x, foc->center.y, fpos.z1 + h/2); // build @@ -310,7 +310,8 @@ namespace Ray3D { const float deg = rad * 180 / M_PI; // cube's destination center - const double height = (!aboveDoor) ? (fpos.height) : (fpos.height - aboveDoor->height); + const float _height = (fol->height_m > 0) ? (fol->height_m) : (fpos.height); // use either floor's height or user height + const double height = (!aboveDoor) ? (_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); @@ -338,6 +339,7 @@ namespace Ray3D { const std::string& name = foo->file; Obstacle3D obs = OBJPool::get().getObject(name); + obs = obs.scaled(foo->scale); obs = obs.rotated_deg( Point3(foo->rot.x, foo->rot.y, foo->rot.z) ); obs = obs.translated(foo->pos + Point3(0,0,fpos.z1)); obs.type = Obstacle3D::Type::OBJECT; diff --git a/wifi/estimate/ray3/Obstacle3.h b/wifi/estimate/ray3/Obstacle3.h index 8199713..ff84594 100644 --- a/wifi/estimate/ray3/Obstacle3.h +++ b/wifi/estimate/ray3/Obstacle3.h @@ -38,6 +38,14 @@ namespace Ray3D { /** ctor */ Obstacle3D(Type type, Floorplan::Material mat) : type(type), mat(mat) {;} + /** scaled copy */ + Obstacle3D scaled(const Point3 scale) const { + Obstacle3D copy = *this; + for (Triangle3& tria : copy.triangles) { + tria *= scale; + } + return copy; + } /** translated copy */ Obstacle3D translated(const Point3 pos) const {