From 34e52cd7f004a249ef79960b3b216d59151aeb13 Mon Sep 17 00:00:00 2001 From: FrankE Date: Wed, 13 Jul 2016 19:11:18 +0200 Subject: [PATCH] worked on floorplan (v2) worked on grid-generation (v2) new helper methods for geometry new test cases --- Assertions.h | 8 + CMakeLists.txt | 6 +- api/DummyAPI.h | 4 + api/api.h | 4 + floorplan/v2/Floorplan.h | 341 +++++++++++++++--- floorplan/v2/FloorplanReader.h | 225 ++++++++---- floorplan/v2/FloorplanWriter.h | 212 ++++++----- geo/BBox2.h | 29 +- geo/Line2.h | 66 ++-- geo/Point2.h | 8 +- geo/Point3.h | 33 +- grid/Grid.h | 49 ++- grid/GridNode.h | 32 +- grid/GridPoint.h | 12 +- grid/factory/v2/GridFactory.h | 507 +++++++++++++++++++++++++++ grid/factory/v2/Helper.h | 219 ++++++++++++ grid/factory/v2/Importance.h | 217 ++++++++++++ grid/factory/v2/Stairs.h | 281 +++++++++++++++ math/Math.h | 15 + tests/Tests.h | 2 + tests/api/TestAPI.cpp | 4 + tests/floorplan/TestNewFloorplan.cpp | 10 +- tests/geo/TestBBox.cpp | 61 ++++ tests/grid/TestWalk.cpp | 2 +- wifi/estimate/ray2d/Ray2.h | 4 + wifi/estimate/ray2d/WiFiRayTrace2D.h | 4 + 26 files changed, 2083 insertions(+), 272 deletions(-) create mode 100644 api/DummyAPI.h create mode 100644 api/api.h create mode 100755 grid/factory/v2/GridFactory.h create mode 100644 grid/factory/v2/Helper.h create mode 100644 grid/factory/v2/Importance.h create mode 100644 grid/factory/v2/Stairs.h create mode 100644 tests/api/TestAPI.cpp create mode 100644 tests/geo/TestBBox.cpp create mode 100644 wifi/estimate/ray2d/Ray2.h create mode 100644 wifi/estimate/ray2d/WiFiRayTrace2D.h diff --git a/Assertions.h b/Assertions.h index e14bf4b..116a9e1 100644 --- a/Assertions.h +++ b/Assertions.h @@ -29,6 +29,10 @@ namespace Assert { if (v1 != v2) {doThrow(err);} } + template static inline void notEqual(const T v1, const T v2, const STR err) { + if (v1 == v2) {doThrow(err);} + } + template static inline void isTrue(const T v, const STR err) { if (!v) {doThrow(err);} } @@ -53,6 +57,10 @@ namespace Assert { if (v == 0) {doThrow(err);} } + template static inline void isNear(const T v1, const T v2, const T delta, const STR err) { + if (std::abs(v1-v2) > delta) {doThrow(err);} + } + template static inline void isBetween(const T v, const T min, const T max, const STR err) { if (v < min || v > max) {doThrow(err);} } diff --git a/CMakeLists.txt b/CMakeLists.txt index 1d32b87..395d28e 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,7 +37,6 @@ FILE(GLOB SOURCES ./*/*.cpp ./*/*/*.cpp ./*/*/*/*.cpp - ../KLib/inc/tinyxml/*.cpp ) @@ -65,12 +64,13 @@ ADD_DEFINITIONS( -fstack-protector-all -g3 - -O2 + -O0 + -march=native -DWITH_TESTS -DWITH_ASSERTIONS -DWITH_DEBUG_LOG - -march=native + ) diff --git a/api/DummyAPI.h b/api/DummyAPI.h new file mode 100644 index 0000000..cb9e17c --- /dev/null +++ b/api/DummyAPI.h @@ -0,0 +1,4 @@ +#ifndef DUMMYAPI_H +#define DUMMYAPI_H + +#endif // DUMMYAPI_H diff --git a/api/api.h b/api/api.h new file mode 100644 index 0000000..4ee2cc4 --- /dev/null +++ b/api/api.h @@ -0,0 +1,4 @@ +#ifndef API_H +#define API_H + +#endif // API_H diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index 2618383..ac68973 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -11,7 +11,28 @@ namespace Floorplan { - /** 3D polygon */ + + /** a free key-value meta element */ + struct Meta { + + const std::string EMPTY = ""; + std::unordered_map params; + + const std::string& getVal(const std::string& key) const { + const auto it = params.find(key); + return (it == params.end()) ? (EMPTY) : (it->second); + } + void setVal(const std::string& key, const std::string& val) {params[key] = val;} + + float getFloat(const std::string& key) const { return std::stof(getVal(key)); } + void setFloat(const std::string& key, const float val) { params[key] = std::to_string(val); } + + int getInt(const std::string& key) const { return std::stoi(getVal(key)); } + void setInt(const std::string& key, const int val) { params[key] = std::to_string(val); } + + }; + + /** 2D polygon */ struct Polygon2 { std::vector points; Polygon2() : points() {;} @@ -19,17 +40,37 @@ namespace Floorplan { bool operator == (const Polygon2& o) const {return std::equal(o.points.begin(), o.points.end(), this->points.begin());} }; + /** 3D quad */ + struct Quad3 { + Point3 p1; + Point3 p2; + Point3 p3; + Point3 p4; + Quad3(const Point3 p1, const Point3 p2, const Point3 p3, const Point3 p4) : p1(p1), p2(p2), p3(p3), p4(p4) {;} + Quad3 operator * (const float v) const {return Quad3(p1*v, p2*v, p3*v, p4*v);} + }; + /** additional type-info for obstacles */ enum class ObstacleType { UNKNOWN, WALL, - DOOR, WINDOW, HANDRAIL, PILLAR, _END, }; + /** available door types */ + enum class DoorType { + UNKNOWN, + SWING, // normal + DOUBLE_SWING, + SLIDE, // schiebetuer + DOUBLE_SLIDE, + REVOLVING, // drehtuer + _END, + }; + /** all supported material types */ enum class Material { UNKNOWN, @@ -40,6 +81,10 @@ namespace Floorplan { _END, }; + enum class POIType { + ROOM, + }; + /** types of outlines. either add or remove the selected region */ enum class OutlineMethod { ADD, @@ -47,23 +92,78 @@ namespace Floorplan { _END, }; + struct Floor; + struct FloorOutlinePolygon; + struct FloorObstacle; + struct AccessPoint; + struct Beacon; + struct FloorRegion; + struct UnderlayImage; + struct POI; + struct Stair; + + using FloorOutline = std::vector; + using FloorObstacles = std::vector; + using FloorAccessPoints = std::vector; + using FloorBeacons = std::vector; + using FloorRegions = std::vector; + using FloorUnderlays = std::vector; + using FloorPOIs = std::vector; + using FloorStairs = std::vector; + + /** describes one floor within the map, starting at a given height */ + struct Floor { + + float atHeight; // the floor's starting height + float height; // the floor's total height (from start) + std::string name; // the floor's name + FloorOutline outline; // the floor's outline (ground) + FloorObstacles obstacles; // all obstacles (wall, door, window, ..) within the floor + FloorRegions regions; // all regions within the floor (rooms, ...) + FloorAccessPoints accesspoints; + FloorBeacons beacons; + FloorUnderlays underlays; // underlay images (used for map-building) + FloorPOIs pois; // POIs within the floor + FloorStairs stairs; // all stairs within one floor + //FloorKeyValue other; // other, free elements + + Floor() {;} + + Floor(const Floor& o) = delete; + void operator = (const Floor& o) = delete; + + float getStartingZ() const {return atHeight;} + float getEndingZ() const {return atHeight + height;} + + }; - /** an AccessPoint located somewhere within the map */ + /** a POI located somewhere on a floor */ + struct POI { + POIType type; + std::string name; + Point2 pos; + POI() : type(), name(), pos() {;} + POI(const POIType type, const std::string& name, const Point2& pos) : type(type), name(name), pos(pos) {;} + bool operator == (const POI& o) const {return (o.type == type) && (o.name == name) && (o.pos == pos);} + }; + + /** an AccessPoint located somewhere on a floor */ struct AccessPoint { std::string name; std::string mac; - Point3 pos; + Point3 pos; // z is relative to the floor's height AccessPoint() : name(), mac(), pos() {;} AccessPoint(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(mac), pos(pos) {;} bool operator == (const AccessPoint& o) const {return (o.name == name) && (o.mac == mac) && (o.pos == pos);} + Point3 getPos(const Floor* f) const {return pos + Point3(0,0,f->atHeight);} // relative to the floor's ground }; - /** a beacon located somewhere within the map */ + /** a beacon located somewhere on a floor */ struct Beacon { std::string name; std::string mac; - Point3 pos; + Point3 pos; // z is relative to the floor's height Beacon() : name(), mac(), pos() {;} Beacon(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(mac), pos(pos) {;} bool operator == (const Beacon& o) const {return (o.name == name) && (o.mac == mac) && (o.pos == pos);} @@ -80,31 +180,51 @@ namespace Floorplan { bool operator == (const FloorOutlinePolygon& o) const {return (o.method == method) && (o.name == name) && (o.poly == poly);} }; + + + + /** base-class for one obstacle (wall, door, window, pillar, ..) within a floor */ struct FloorObstacle { - ObstacleType type; Material material; - FloorObstacle() : type(), material() {;} - FloorObstacle(const ObstacleType type, const Material material) : type(type), material(material) {;} + FloorObstacle() : material() {;} + FloorObstacle(const Material material) : material(material) {;} virtual ~FloorObstacle() {;} }; /** line obstacle */ struct FloorObstacleLine : public FloorObstacle { + ObstacleType type; Point2 from; Point2 to; - FloorObstacleLine(const ObstacleType type, const Material material, const Point2 from, const Point2 to) : FloorObstacle(type, material), from(from), to(to) {;} - FloorObstacleLine(const ObstacleType type, const Material material, const float x1, const float y1, const float x2, const float y2) : FloorObstacle(type, material), from(x1,y1), to(x2,y2) {;} + FloorObstacleLine(const ObstacleType type, const Material material, const Point2 from, const Point2 to) : FloorObstacle(material), type(type), from(from), to(to) {;} + FloorObstacleLine(const ObstacleType type, const Material material, const float x1, const float y1, const float x2, const float y2) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2) {;} }; /** circle obstacle */ struct FloorObstacleCircle : public FloorObstacle { Point2 center; float radius; - FloorObstacleCircle(const ObstacleType type, const Material material, const Point2 center, const float radius) : FloorObstacle(type, material), center(center), radius(radius) {;} - FloorObstacleCircle(const ObstacleType type, const Material material, const float cx, const float cy, const float radius) : FloorObstacle(type, material), center(cx,cy), radius(radius) {;} + FloorObstacleCircle(const Material material, const Point2 center, const float radius) : FloorObstacle(material), center(center), radius(radius) {;} + FloorObstacleCircle(const Material material, const float cx, const float cy, const float radius) : FloorObstacle(material), center(cx,cy), radius(radius) {;} }; + /** door obstacle */ + struct FloorObstacleDoor : public FloorObstacle { + DoorType type; + Point2 from; + Point2 to; + float height; + bool swap; + FloorObstacleDoor(const DoorType type, const Material material, const Point2 from, const Point2 to) : FloorObstacle(material), type(type), from(from), to(to), height(2.1), swap(false) {;} + FloorObstacleDoor(const DoorType type, const Material material, const float x1, const float y1, const float x2, const float y2, const float height, const bool swap) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2), height(height), swap(swap) {;} + float getSize() const {return (to-from).length();} + }; + + + + + /** one region (e.g. a room) within the floor, described using a polygon */ struct FloorRegion { std::string name; @@ -115,6 +235,158 @@ namespace Floorplan { + static Point3 xy0(const Point2 p) { + return Point3(p.x, p.y, 0); + } + + /** describes a plane as starting point, ending point and fixed width */ + struct StairPart { + + /** z is relative to the floor's height */ + Point3 start; + + /** z is relative to the floor's height */ + Point3 end; + + /** the width for this element */ + float width; + + /** whether to connect this element with the previous one */ + bool connectWithPrev; + + StairPart() {;} + + StairPart(const Point3 start, const Point3 end, const float width) : start(start), end(end), width(width), connectWithPrev(false) {;} + + StairPart(Point3 center, float deg, float length, float width, float height) { + this->start = Point3(-length/2, 0, -height/2); + this->end = Point3(+length/2, 0, +height/2); + this->width = width; + rotate(deg/180.0*M_PI); + move(center); + } + + /** convenience method for array access [0:1] to start and end */ + Point3& operator [] (const int idx) { + switch (idx) { + case 0: return start; + case 1: return end; + default: throw "error"; + } + } + + void rotate(const float rad) { + const float ca = std::cos(rad); + const float sa = std::sin(rad); + start = Point3(ca*start.x - sa*start.y, sa*start.x + ca * start.y, start.z); + end = Point3(ca*end.x - sa*end.y, sa*end.x + ca * end.y, end.z);; + } + + void move(const Point3 p) { + start += p; + end += p; + } + + }; + + static std::vector getQuads(const std::vector& parts, const Floor* floor) { + + std::vector vec; + + for (const StairPart& part : parts) { + + const float width = part.width; + const Point3 start = part.start; + const Point3 end = part.end; + const Point3 dir = end - start; // direction vector + const Point2 dir2(dir.x, dir.y); // direction without height (just 2D) + const Point2 perp = dir2.perpendicular(); // perendicular vector + const Point2 perpN = perp / perp.length(); // normalized perpendicular vector + const Point3 p1 = start + xy0(perpN * width / 2) + Point3(0,0,floor->atHeight); + const Point3 p2 = start - xy0(perpN * width / 2) + Point3(0,0,floor->atHeight); + const Point3 p3 = end - xy0(perpN * width / 2) + Point3(0,0,floor->atHeight); + const Point3 p4 = end + xy0(perpN * width / 2) + Point3(0,0,floor->atHeight); + const Quad3 q(p1,p2,p3,p4); + vec.push_back(q); + + // connect + if (part.connectWithPrev) { + if (vec.size() >= 2) { + Quad3& a = vec[vec.size()-2]; + Quad3& b = vec[vec.size()-1]; + Point3 pa = (a.p3 + b.p2)/2; a.p3 = pa; b.p2 = pa; + Point3 pb = (a.p4 + b.p1)/2; a.p4 = pb; b.p1 = pb; + } + } + + } + + return vec; + + } + +// /** +// * get the ABSOLUTE quad for this stair-part +// * (relative-height + floor-height = absolute-height +// */ +// Quad3 getQuad(const Floor* floor) const { + +// } + + /** base-class for stairs */ + struct Stair { + Meta* meta = nullptr; + virtual std::vector getParts() const = 0; + }; + +// /** just a normal, straigt stair */ +// struct StairNormal : public Stair { +// Point2 center; +// float angleDeg; +// float atHeight; +// float height; +// float width; +// float length; +// StairNormal(const Point2 center, const float atHeight, const float angleDeg, const float height, const float width, const float length) : +// center(center), angleDeg(angleDeg), atHeight(atHeight), height(height), width(width), length(length) { + + +// } +// std::vector getParts() const { +// std::vector parts; +// Point3 cen(center.x, center.y, atHeight); +// parts.push_back(StairPart(cen, angleDeg, length, width, height)); +// return parts; +// } +// }; + +// /** OLD 3-part stair, up[left]->platform->up[right] */ +// struct StairFreeformOLD : public Stair { +// float width; +// std::vector nodes; +// StairFreeformOLD() {;} +// std::vector getParts() const { +// std::vector parts; +// for (int i = 1; i < (int)nodes.size(); ++i) { +// const Point3 p1 = nodes[i-1]; +// const Point3 p2 = nodes[i ]; +// parts.push_back(StairPart(p1, p2, width)); +// } +// return parts; +// } +// }; + + /** 3-part stair, up[left]->platform->up[right] */ + struct StairFreeform : public Stair { + std::vector parts; + StairFreeform() {;} + std::vector getParts() const {return parts;} + }; + + + + + /** an image file that can be added to the map */ struct UnderlayImage { std::string name; @@ -124,50 +396,7 @@ namespace Floorplan { float scaleY; }; - /** a free key-value element */ - struct KeyValueElement { - std::string empty; - std::unordered_map params; - const std::string& getVal(const std::string& key) const { - auto it = params.find(key); - return (it == params.end()) ? (empty) : (it->second); - } - float getFloat(const std::string& key) const { return std::stof(getVal(key)); } - void setVal(const std::string& key, const std::string& val) { - params[key] = val; - } - void setFloat(const std::string& key, const float val) { - params[key] = std::to_string(val); - } - }; - using FloorOutline = std::vector; - using FloorObstacles = std::vector; - using FloorAccessPoints = std::vector; - using FloorBeacons = std::vector; - using FloorRegions = std::vector; - using FloorUnderlays = std::vector; - - /** describes one floor within the map, starting at a given height */ - struct Floor { - - float atHeight; // the floor's starting height - float height; // the floor's total height (from start) - std::string name; // the floor's name - FloorOutline outline; // the floor's outline (ground) - FloorObstacles obstacles; // all obstacles (wall, door, window, ..) within the floor - FloorRegions regions; // all regions within the floor (rooms, ...) - FloorAccessPoints accesspoints; - FloorBeacons beacons; - FloorUnderlays underlays; // underlay images (used for map-building) - //FloorKeyValue other; // other, free elements - - Floor() {;} - - Floor(const Floor& o) = delete; - void operator = (const Floor& o) = delete; - - }; /** describes the whole indoor map */ struct IndoorMap { diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 82b5bcb..790ba97 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -10,7 +10,8 @@ namespace Floorplan { - using namespace tinyxml2; + using XMLAttr = tinyxml2::XMLAttribute; + using XMLElem = tinyxml2::XMLElement; /** * read an IndoorMaps from XML-data @@ -22,35 +23,37 @@ namespace Floorplan { /** read an IndoorMap from the given XML-file */ static IndoorMap* readFromFile(const std::string& file) { setlocale(LC_NUMERIC, "C"); - XMLDocument doc; - doc.LoadFile(file.c_str()); + tinyxml2::XMLDocument doc; + const tinyxml2::XMLError res = doc.LoadFile(file.c_str()); + if (res != tinyxml2::XMLError::XML_SUCCESS) {throw Exception("error while loading XML " + file);} return parse(doc); } /** read an IndoorMap from the given XMl-string */ static IndoorMap* readFromString(const std::string& str) { setlocale(LC_NUMERIC, "C"); - XMLDocument doc; - doc.Parse(str.c_str(), str.length()); + tinyxml2::XMLDocument doc; + const tinyxml2::XMLError res = doc.Parse(str.c_str(), str.length()); + if (res != tinyxml2::XMLError::XML_SUCCESS) {throw Exception("error while parsing XML");} return parse(doc); } private: - #define FOREACH_NODE(out, in) for( const XMLElement* out = (XMLElement*) in->FirstChild(); out; out = (XMLElement*) out->NextSibling() ) + #define FOREACH_NODE(out, in) for( const XMLElem* out = (XMLElem*) in->FirstChild(); out; out = (XMLElem*) out->NextSibling() ) - static void assertNode(const std::string& node, const XMLElement* el) { + static void assertNode(const std::string& node, const XMLElem* el) { std::string err = std::string("unexpected node '") + el->Name() + "' expected '" + node + "'"; Assert::equal(node, std::string(el->Name()), err); } /** parse the complete document */ - static IndoorMap* parse(XMLDocument& doc) { - return parseMap((XMLElement*)doc.FirstChild()); + static IndoorMap* parse(tinyxml2::XMLDocument& doc) { + return parseMap((XMLElem*)doc.FirstChild()); } /** parse the node */ - static IndoorMap* parseMap(const XMLElement* el) { + static IndoorMap* parseMap(const XMLElem* el) { std::cout << el->Name() << std::endl; IndoorMap* map = new IndoorMap(); map->width = el->FloatAttribute("width"); @@ -62,7 +65,7 @@ namespace Floorplan { } /** parse the node */ - static std::vector parseFloors(const XMLElement* el) { + static std::vector parseFloors(const XMLElem* el) { std::vector floors; FOREACH_NODE(n, el) { if (std::string("floor") == n->Name()) {floors.push_back(parseFloor(n));} @@ -71,25 +74,91 @@ namespace Floorplan { } /** parse one node */ - static Floor* parseFloor(const XMLElement* el) { + static Floor* parseFloor(const XMLElem* el) { Floor* floor = new Floor(); floor->atHeight = el->FloatAttribute("atHeight"); floor->height = el->FloatAttribute("height"); floor->name = el->Attribute("name"); FOREACH_NODE(n, el) { - if (std::string("outline") == n->Name()) {floor->outline = parseFloorOutline(n);} - if (std::string("obstacles") == n->Name()) {floor->obstacles = parseFloorObstacles(n);} + if (std::string("outline") == n->Name()) {floor->outline = parseFloorOutline(n);} + if (std::string("obstacles") == n->Name()) {floor->obstacles = parseFloorObstacles(n);} if (std::string("accesspoints") == n->Name()) {floor->accesspoints = parseFloorAccessPoints(n);} - if (std::string("beacons") == n->Name()) {floor->beacons = parseFloorBeacons(n);} - if (std::string("regions") == n->Name()) {floor->regions = parseFloorRegions(n);} - if (std::string("underlays") == n->Name()) {floor->underlays = parseFloorUnderlays(n);} + if (std::string("beacons") == n->Name()) {floor->beacons = parseFloorBeacons(n);} + if (std::string("regions") == n->Name()) {floor->regions = parseFloorRegions(n);} + if (std::string("underlays") == n->Name()) {floor->underlays = parseFloorUnderlays(n);} + if (std::string("pois") == n->Name()) {floor->pois = parseFloorPOIs(n);} + if (std::string("stairs") == n->Name()) {floor->stairs = parseFloorStairs(n);} } return floor; } + + /** parse the tag */ + static std::vector parseFloorStairs(const XMLElem* el) { + std::vector vec; + FOREACH_NODE(n, el) { + if (std::string("stair") == n->Name()) { vec.push_back(parseFloorStair(n)); } + } + return vec; + } + + /** parse a tag */ + static Stair* parseFloorStair(const XMLElem* el) { + Stair* stair = nullptr; + if (el->IntAttribute("type") == 0) { + stair = new StairFreeform(); + FOREACH_NODE(n, el) { + if (std::string("part") == n->Name()) { + StairPart part; + part.connectWithPrev = n->BoolAttribute("connect"); + part.start.x = n->FloatAttribute("x1"); + part.start.y = n->FloatAttribute("y1"); + part.start.z = n->FloatAttribute("z1"); + part.end.x = n->FloatAttribute("x2"); + part.end.y = n->FloatAttribute("y2"); + part.end.z = n->FloatAttribute("z2"); + part.width = n->FloatAttribute("w"); + ((StairFreeform*)stair)->parts.push_back(part); + } else { + throw "not yet implemented"; + } + } + } else { + throw "not yet implemented"; + } + + // stair meta information? + const XMLElem* meta = el->FirstChildElement("meta"); + if (meta) { + stair->meta = parseMetaElement(meta); + } + + return stair; + + } + + + /** parse the tag */ + static std::vector parseFloorPOIs(const XMLElem* el) { + std::vector vec; + FOREACH_NODE(n, el) { + if (std::string("poi") == n->Name()) { vec.push_back(parseFloorPOI(n)); } + } + return vec; + } + + /** parse a tag */ + static POI* parseFloorPOI(const XMLElem* el) { + POI* poi = new POI(); + poi->name = el->Attribute("name"); + poi->type = (POIType) el->IntAttribute("type"); + poi->pos = parsePoint2(el); + return poi; + } + + /** parse the tag */ - static std::vector parseFloorAccessPoints(const XMLElement* el) { - assertNode("accesspoints", el); + static std::vector parseFloorAccessPoints(const XMLElem* el) { std::vector vec; FOREACH_NODE(n, el) { if (std::string("accesspoint") == n->Name()) { vec.push_back(parseAccessPoint(n)); } @@ -97,29 +166,29 @@ namespace Floorplan { return vec; } - /** parse the tag */ - static FloorUnderlays parseFloorUnderlays(const XMLElement* el) { - FloorUnderlays res; - FOREACH_NODE(n, el) { - if (std::string("underlay") == n->Name()) { res.push_back(parseFloorUnderlay(n)); } - } - return res; - } + /** parse the tag */ + static FloorUnderlays parseFloorUnderlays(const XMLElem* el) { + FloorUnderlays res; + FOREACH_NODE(n, el) { + if (std::string("underlay") == n->Name()) { res.push_back(parseFloorUnderlay(n)); } + } + return res; + } - /** parse an underlay image */ - static UnderlayImage* parseFloorUnderlay(const XMLElement* el) { - UnderlayImage* img = new UnderlayImage(); - img->anchor.x = el->FloatAttribute("x"); - img->anchor.y = el->FloatAttribute("y"); - img->scaleX = el->FloatAttribute("sx"); - img->scaleY = el->FloatAttribute("sy"); - img->name = el->Attribute("name"); - img->filename = el->Attribute("file"); - return img; - } + /** parse an underlay image */ + static UnderlayImage* parseFloorUnderlay(const XMLElem* el) { + UnderlayImage* img = new UnderlayImage(); + img->anchor.x = el->FloatAttribute("x"); + img->anchor.y = el->FloatAttribute("y"); + img->scaleX = el->FloatAttribute("sx"); + img->scaleY = el->FloatAttribute("sy"); + img->name = el->Attribute("name"); + img->filename = el->Attribute("file"); + return img; + } // /** parse the tag */ -// static FloorKeyValue parseFloorKeyValue(const XMLElement* el) { +// static FloorKeyValue parseFloorKeyValue(const XMLElem* el) { // FloorKeyValue res; // FOREACH_NODE(n, el) { // if (std::string("keyval") == n->Name()) { res.elements.push_back(parseKeyValueElement(n)); } @@ -127,10 +196,10 @@ namespace Floorplan { // return res; // } - /** parse one element */ - static KeyValueElement* parseKeyValueElement(const XMLElement* n) { - KeyValueElement* elem = new KeyValueElement(); - const XMLAttribute* attr = n->FirstAttribute(); + /** parse one element */ + static Meta* parseMetaElement(const XMLElem* n) { + Meta* elem = new Meta(); + const XMLAttr* attr = n->FirstAttribute(); while (attr) { elem->params[attr->Name()] = attr->Value(); attr = attr->Next(); @@ -138,7 +207,7 @@ namespace Floorplan { return elem; } - static AccessPoint* parseAccessPoint(const XMLElement* n) { + static AccessPoint* parseAccessPoint(const XMLElem* n) { assertNode("accesspoint", n); AccessPoint* ap = new AccessPoint(); ap->mac = n->Attribute("mac"); @@ -149,7 +218,7 @@ namespace Floorplan { /** parse the tag */ - static std::vector parseFloorBeacons(const XMLElement* el) { + static std::vector parseFloorBeacons(const XMLElem* el) { std::vector vec; FOREACH_NODE(n, el) { if (std::string("beacon") == n->Name()) { vec.push_back(parseBeacon(n)); } @@ -157,7 +226,7 @@ namespace Floorplan { return vec; } - static Beacon* parseBeacon(const XMLElement* n) { + static Beacon* parseBeacon(const XMLElem* n) { assertNode("beacon", n); Beacon* b = new Beacon(); b->mac = n->Attribute("mac"); @@ -167,7 +236,7 @@ namespace Floorplan { } - static std::vector parseFloorRegions(const XMLElement* el) { + static std::vector parseFloorRegions(const XMLElem* el) { std::vector vec; FOREACH_NODE(n, el) { if (std::string("region") == n->Name()) { vec.push_back(parseFloorRegion(n)); } @@ -175,7 +244,7 @@ namespace Floorplan { return vec; } - static FloorRegion* parseFloorRegion(const XMLElement* n) { + static FloorRegion* parseFloorRegion(const XMLElem* n) { assertNode("region", n); FloorRegion* reg = new FloorRegion(); reg->name = n->Attribute("name"); @@ -184,7 +253,7 @@ namespace Floorplan { } /** parse the tag */ - static std::vector parseFloorObstacles(const XMLElement* el) { + static std::vector parseFloorObstacles(const XMLElem* el) { assertNode("obstacles", el); std::vector obstacles; FOREACH_NODE(n, el) { @@ -192,38 +261,46 @@ namespace Floorplan { // if (std::string("door") == n->Name()) {obstacles.push_back(parseFloorObstacleDoor(n));} // if (std::string("window") == n->Name()) {obstacles.push_back(parseFloorObstacleWindow(n));} // if (std::string("pillar") == n->Name()) {obstacles.push_back(parseFloorObstaclePillar(n));} - if (std::string("obstacle") == n->Name()) {obstacles.push_back(parseFloorObstacleLine(n));} // OLD - if (std::string("line") == n->Name()) {obstacles.push_back(parseFloorObstacleLine(n));} - if (std::string("circle") == n->Name()) {obstacles.push_back(parseFloorObstacleCircle(n));} + //if (std::string("obstacle") == n->Name()) {obstacles.push_back(parseFloorObstacleLine(n));} // OLD + if (std::string("line") == n->Name()) {obstacles.push_back(parseFloorObstacleLine(n));} + if (std::string("circle") == n->Name()) {obstacles.push_back(parseFloorObstacleCircle(n));} + if (std::string("door") == n->Name()) {obstacles.push_back(parseFloorObstacleDoor(n));} } return obstacles; } /** parse one line */ - static FloorObstacleLine* parseFloorObstacleLine(const XMLElement* el) { + static FloorObstacleLine* parseFloorObstacleLine(const XMLElem* el) { return new FloorObstacleLine( - parseObstacleType(el->Attribute("type")), - parseMaterial(el->Attribute("material")), - el->FloatAttribute("x1"), el->FloatAttribute("y1"), - el->FloatAttribute("x2"), el->FloatAttribute("y2") - // parsePoint2((XMLElement*) el->FirstChildElement("from")), - // parsePoint2((XMLElement*) el->FirstChildElement("to")) - ); + parseObstacleType(el->Attribute("type")), + parseMaterial(el->Attribute("material")), + el->FloatAttribute("x1"), el->FloatAttribute("y1"), + el->FloatAttribute("x2"), el->FloatAttribute("y2") + ); } /** parse one cirlce */ - static FloorObstacleCircle* parseFloorObstacleCircle(const XMLElement* el) { + static FloorObstacleCircle* parseFloorObstacleCircle(const XMLElem* el) { return new FloorObstacleCircle( - parseObstacleType(el->Attribute("type")), - parseMaterial(el->Attribute("material")), - el->FloatAttribute("cx"), el->FloatAttribute("cy"), - //parsePoint2((XMLElement*) el->FirstChildElement("from")), - el->FloatAttribute("radius") - ); + parseMaterial(el->Attribute("material")), + el->FloatAttribute("cx"), el->FloatAttribute("cy"), + el->FloatAttribute("radius") + ); + } + + /** parse one door */ + static FloorObstacleDoor* parseFloorObstacleDoor(const XMLElem* el) { + return new FloorObstacleDoor( + parseDoorType(el->Attribute("type")), + parseMaterial(el->Attribute("material")), + el->FloatAttribute("x1"), el->FloatAttribute("y1"), + el->FloatAttribute("x2"), el->FloatAttribute("y2"), + el->FloatAttribute("height"), el->BoolAttribute("swap") + ); } /** parse a floor's tag */ - static FloorOutline parseFloorOutline(const XMLElement* el) { + static FloorOutline parseFloorOutline(const XMLElem* el) { FloorOutline outline; FOREACH_NODE(n, el) { if (std::string("polygon") == n->Name()) { @@ -234,7 +311,7 @@ namespace Floorplan { } /** parse one polygon */ - static FloorOutlinePolygon* parseFloorPolygon(const XMLElement* el) { + static FloorOutlinePolygon* parseFloorPolygon(const XMLElem* el) { FloorOutlinePolygon* poly = new FloorOutlinePolygon(); poly->name = el->Attribute("name"); poly->method = parseOutlineMethod(el->Attribute("method")); @@ -243,7 +320,7 @@ namespace Floorplan { } /** parse a 2d-polygon denoted by several points */ - static Polygon2 parsePoly2(const XMLElement* el) { + static Polygon2 parsePoly2(const XMLElem* el) { Polygon2 poly; FOREACH_NODE(n, el) { if (std::string("point") == n->Name()) { @@ -255,7 +332,7 @@ namespace Floorplan { } /** parse a 2D point (x,y) using the given tag's attributes. missing attributes are set to 0 */ - static Point2 parsePoint2(const XMLElement* el) { + static Point2 parsePoint2(const XMLElem* el) { Point2 point; point.x = el->FloatAttribute("x"); point.y = el->FloatAttribute("y"); @@ -263,7 +340,7 @@ namespace Floorplan { } /** parse a 3D point (x,y,z) using the given tag's attributes. missing attributes are set to 0 */ - static Point3 parsePoint3(const XMLElement* el) { + static Point3 parsePoint3(const XMLElem* el) { Point3 point; point.x = el->FloatAttribute("x"); point.y = el->FloatAttribute("y"); @@ -279,6 +356,10 @@ namespace Floorplan { } } + static DoorType parseDoorType(const std::string type) { + try { return (DoorType) std::stoi(type); } catch (...) { return DoorType::UNKNOWN; } + } + static Material parseMaterial(const std::string material) { try { return (Material) std::stoi(material); diff --git a/floorplan/v2/FloorplanWriter.h b/floorplan/v2/FloorplanWriter.h index 3479a7b..96763d7 100644 --- a/floorplan/v2/FloorplanWriter.h +++ b/floorplan/v2/FloorplanWriter.h @@ -10,7 +10,9 @@ namespace Floorplan { - using namespace tinyxml2; + using XMLAttr = tinyxml2::XMLAttribute; + using XMLElem = tinyxml2::XMLElement; + using XMLDoc = tinyxml2::XMLDocument; /** * write floorplans as XML @@ -20,36 +22,29 @@ namespace Floorplan { public: static void writeToFile(const IndoorMap* map, const std::string& file) { - XMLDocument doc; + XMLDoc doc; construct(doc, map); doc.SaveFile(file.c_str(), false); } static std::string writeToString(const IndoorMap* map) { - XMLDocument doc; + XMLDoc doc; construct(doc, map); - XMLPrinter printer; + tinyxml2::XMLPrinter printer; doc.Accept( &printer ); return printer.CStr(); } private: - static std::string toString(const ObstacleType t) { - return std::to_string((int)t); - } + static std::string toString(const ObstacleType t) { return std::to_string((int)t); } + static std::string toString(const DoorType t) { return std::to_string((int)t); } + static std::string toString(const Material m) { return std::to_string((int)m); } + static std::string toString(const OutlineMethod m) { return std::to_string((int)m); } - static std::string toString(const Material m) { - return std::to_string((int)m); - } + static void construct(XMLDoc& doc, const IndoorMap* map) { - static std::string toString(const OutlineMethod m) { - return std::to_string((int)m); - } - - static void construct(XMLDocument& doc, const IndoorMap* map) { - - XMLElement* root = doc.NewElement("map"); + XMLElem* root = doc.NewElement("map"); doc.InsertEndChild(root); root->SetAttribute("width", map->width); root->SetAttribute("depth", map->depth); @@ -60,16 +55,16 @@ namespace Floorplan { } /** add all floors to the map */ - static void addFloors(XMLDocument& doc, XMLElement* root, const IndoorMap* map) { - XMLElement* floors = doc.NewElement("floors"); + static void addFloors(XMLDoc& doc, XMLElem* root, const IndoorMap* map) { + XMLElem* floors = doc.NewElement("floors"); for(const Floor* mf : map->floors) {addFloor(doc, floors, mf);} root->InsertEndChild(floors); } /** add one floor to the floors-node */ - static void addFloor(XMLDocument& doc, XMLElement* floors, const Floor* mf) { + static void addFloor(XMLDoc& doc, XMLElem* floors, const Floor* mf) { - XMLElement* floor = doc.NewElement("floor"); + XMLElem* floor = doc.NewElement("floor"); floor->SetAttribute("atHeight", mf->atHeight); floor->SetAttribute("height", mf->height); floor->SetAttribute("name", mf->name.c_str()); @@ -77,19 +72,64 @@ namespace Floorplan { addFloorOutline(doc, floor, mf); addFloorObstacles(doc, floor, mf); - addFloorUnderlays(doc, floor, mf); + addFloorUnderlays(doc, floor, mf); // add all sorts of POI addFloorPOI(doc, floor, mf); + addStairs(doc, floor, mf); + + } + + /** add all stairs to the floor */ + static void addStairs(XMLDoc& doc, XMLElem* floor, const Floor* mf) { + + XMLElem* stairs = doc.NewElement("stairs"); + for (const Stair* _stair : mf->stairs) { + XMLElem* elem = doc.NewElement("stair"); + addMetaElement(doc, elem, _stair->meta); + if (dynamic_cast(_stair)) { + const StairFreeform* stair = (StairFreeform*) _stair; + elem->SetAttribute("type", 0); // TODO: other types? + for (const StairPart& part : stair->parts) { + XMLElem* elemPart = doc.NewElement("part"); + elemPart->SetAttribute("connect", part.connectWithPrev); + elemPart->SetAttribute("x1", part.start.x); + elemPart->SetAttribute("y1", part.start.y); + elemPart->SetAttribute("z1", part.start.z); + elemPart->SetAttribute("x2", part.end.x); + elemPart->SetAttribute("y2", part.end.y); + elemPart->SetAttribute("z2", part.end.z); + elemPart->SetAttribute("w", part.width); + elem->InsertEndChild(elemPart); + } + + } else { + throw "not yet implemented"; + } + stairs->InsertEndChild(elem); + } + floor->InsertEndChild(stairs); + } /** add all sorts of POI to the floor */ - static void addFloorPOI(XMLDocument& doc, XMLElement* floor, const Floor* mf) { + static void addFloorPOI(XMLDoc& doc, XMLElem* floor, const Floor* mf) { - XMLElement* accesspoints = doc.NewElement("accesspoints"); + XMLElem* pois = doc.NewElement("pois"); + for (const POI* poi : mf->pois) { + XMLElem* elem = doc.NewElement("poi"); + elem->SetAttribute("name", poi->name.c_str()); + elem->SetAttribute("type", (int) poi->type); + elem->SetAttribute("x", poi->pos.x); + elem->SetAttribute("y", poi->pos.y); + pois->InsertEndChild(elem); + } + floor->InsertEndChild(pois); + + XMLElem* accesspoints = doc.NewElement("accesspoints"); for (const AccessPoint* ap : mf->accesspoints) { - XMLElement* accesspoint = doc.NewElement("accesspoint"); + XMLElem* accesspoint = doc.NewElement("accesspoint"); accesspoint->SetAttribute("name", ap->name.c_str()); accesspoint->SetAttribute("mac", ap->mac.c_str()); accesspoint->SetAttribute("x", ap->pos.x); @@ -99,9 +139,9 @@ namespace Floorplan { } floor->InsertEndChild(accesspoints); - XMLElement* beacons = doc.NewElement("beacons"); + XMLElem* beacons = doc.NewElement("beacons"); for (const Beacon* b : mf->beacons) { - XMLElement* beacon = doc.NewElement("beacon"); + XMLElem* beacon = doc.NewElement("beacon"); beacon->SetAttribute("name", b->name.c_str()); beacon->SetAttribute("mac", b->mac.c_str()); beacon->SetAttribute("x", b->pos.x); @@ -114,32 +154,32 @@ namespace Floorplan { } - static void addFloorOutline(XMLDocument& doc, XMLElement* floor, const Floor* mf) { + static void addFloorOutline(XMLDoc& doc, XMLElem* floor, const Floor* mf) { - XMLElement* outline = doc.NewElement("outline"); + XMLElem* outline = doc.NewElement("outline"); floor->InsertEndChild(outline); - // XMLElement* add = doc.NewElement("add"); + // XMLElem* add = doc.NewElement("add"); // outline->InsertEndChild(add); for (const FloorOutlinePolygon* poly : mf->outline) { addFloorOutlinePolygon(doc, outline, poly); } - // XMLElement* remove = doc.NewElement("remove"); + // XMLElem* remove = doc.NewElement("remove"); // outline->InsertEndChild(remove); // for (const FloorOutlinePolygon& poly : mf.outline.remove) { addFloorOutlinePolygon(doc, remove, poly); } } - static void addFloorOutlinePolygon(XMLDocument& doc, XMLElement* dst, const FloorOutlinePolygon* poly) { + static void addFloorOutlinePolygon(XMLDoc& doc, XMLElem* dst, const FloorOutlinePolygon* poly) { std::string method = toString(poly->method); - XMLElement* polygon = doc.NewElement("polygon"); + XMLElem* polygon = doc.NewElement("polygon"); polygon->SetAttribute("name", poly->name.c_str()); polygon->SetAttribute("method", method.c_str()); dst->InsertEndChild(polygon); for (Point2 p : poly->poly.points) { - XMLElement* point = doc.NewElement("point"); + XMLElem* point = doc.NewElement("point"); point->SetAttribute("x", p.x); point->SetAttribute("y", p.y); // skip the z-attribute as the value is also given by the polygons floor: floor.atHeight @@ -148,58 +188,49 @@ namespace Floorplan { } - /** write one element */ - static void addKeyValueElement(XMLDocument& doc, XMLElement* other, const KeyValueElement* kv) { - XMLElement* elem = doc.NewElement("keyval"); - for (auto it : kv->params) { + /** write one element */ + static void addMetaElement(XMLDoc& doc, XMLElem* other, const Meta* meta) { + if (meta == nullptr) {return;} // nothing to add + XMLElem* elem = doc.NewElement("meta"); + for (auto it : meta->params) { elem->Attribute(it.first.c_str(), it.second.c_str()); } other->InsertEndChild(elem); } - /** write the tag */ - static void addFloorUnderlays(XMLDocument& doc, XMLElement* floor, const Floor* mf) { - XMLElement* other = doc.NewElement("underlays"); - for (const UnderlayImage* kv : mf->underlays) { - addFloorUnderlay(doc, other, kv); - } - floor->InsertEndChild(other); - } + /** write the tag */ + static void addFloorUnderlays(XMLDoc& doc, XMLElem* floor, const Floor* mf) { + XMLElem* other = doc.NewElement("underlays"); + for (const UnderlayImage* kv : mf->underlays) { + addFloorUnderlay(doc, other, kv); + } + floor->InsertEndChild(other); + } - /** write one underlay */ - static void addFloorUnderlay(XMLDocument& doc, XMLElement* underlays, const UnderlayImage* img) { - XMLElement* underlay = doc.NewElement("underlay"); - underlay->SetAttribute("x", img->anchor.x); - underlay->SetAttribute("y", img->anchor.y); - underlay->SetAttribute("sx", img->scaleX); - underlay->SetAttribute("sy", img->scaleY); - underlay->SetAttribute("name", img->name.c_str()); - underlay->SetAttribute("file", img->filename.c_str()); - underlays->InsertEndChild(underlay); - } + /** write one underlay */ + static void addFloorUnderlay(XMLDoc& doc, XMLElem* underlays, const UnderlayImage* img) { + XMLElem* underlay = doc.NewElement("underlay"); + underlay->SetAttribute("x", img->anchor.x); + underlay->SetAttribute("y", img->anchor.y); + underlay->SetAttribute("sx", img->scaleX); + underlay->SetAttribute("sy", img->scaleY); + underlay->SetAttribute("name", img->name.c_str()); + underlay->SetAttribute("file", img->filename.c_str()); + underlays->InsertEndChild(underlay); + } + static void addFloorObstacles(XMLDoc& doc, XMLElem* floor, const Floor* mf) { -// /** write the tag */ -// static void addFloorKeyValue(XMLDocument& doc, XMLElement* floor, const Floor* mf) { -// XMLElement* other = doc.NewElement("other"); -// for (const KeyValueElement* kv : mf->other.elements) { -// addKeyValueElement(doc, other, kv); -// } -// floor->InsertEndChild(other); -// } - - - - static void addFloorObstacles(XMLDocument& doc, XMLElement* floor, const Floor* mf) { - - XMLElement* obstacles = doc.NewElement("obstacles"); + XMLElem* obstacles = doc.NewElement("obstacles"); for (FloorObstacle* fo : mf->obstacles) { if (dynamic_cast(fo)) { addFloorObstacleLine(doc, obstacles, (FloorObstacleLine*)fo); } else if (dynamic_cast(fo)) { addFloorObstacleCircle(doc, obstacles, (FloorObstacleCircle*)fo); + } else if (dynamic_cast(fo)) { + addFloorObstacleDoor(doc, obstacles, (FloorObstacleDoor*)fo); } } @@ -207,37 +238,46 @@ namespace Floorplan { } - static void addFloorObstacleLine(XMLDocument& doc, XMLElement* obstacles, FloorObstacleLine* line) { - - XMLElement* obstacle = doc.NewElement("line"); - const std::string type = toString(line->type); - const std::string material = toString(line->material); - obstacle->SetAttribute("material", material.c_str()); - obstacle->SetAttribute("type", type.c_str()); + /** write a line obstacle (wall, handrail, ..) */ + static void addFloorObstacleLine(XMLDoc& doc, XMLElem* obstacles, FloorObstacleLine* line) { + XMLElem* obstacle = doc.NewElement("line"); + obstacle->SetAttribute("material", toString(line->material).c_str()); + obstacle->SetAttribute("type", toString(line->type).c_str()); obstacle->SetAttribute("x1", line->from.x); obstacle->SetAttribute("y1", line->from.y); obstacle->SetAttribute("x2", line->to.x); obstacle->SetAttribute("y2", line->to.y); obstacles->InsertEndChild(obstacle); - } - static void addFloorObstacleCircle(XMLDocument& doc, XMLElement* obstacles, FloorObstacleCircle* circle) { - - XMLElement* obstacle = doc.NewElement("line"); - const std::string type = toString(circle->type); - const std::string material = toString(circle->material); - obstacle->SetAttribute("material", material.c_str()); - obstacle->SetAttribute("type", type.c_str()); + /** write a circle obstacle (pillar, ..) */ + static void addFloorObstacleCircle(XMLDoc& doc, XMLElem* obstacles, FloorObstacleCircle* circle) { + XMLElem* obstacle = doc.NewElement("circle"); + obstacle->SetAttribute("material", toString(circle->material).c_str()); obstacle->SetAttribute("cx", circle->center.x); obstacle->SetAttribute("cy", circle->center.y); obstacle->SetAttribute("radius", circle->radius); obstacles->InsertEndChild(obstacle); + } + /** write a door obstacle */ + static void addFloorObstacleDoor(XMLDoc& doc, XMLElem* obstacles, FloorObstacleDoor* door) { + XMLElem* obstacle = doc.NewElement("door"); + obstacle->SetAttribute("material", toString(door->material).c_str()); + obstacle->SetAttribute("type", toString(door->type).c_str()); + obstacle->SetAttribute("x1", door->from.x); + obstacle->SetAttribute("y1", door->from.y); + obstacle->SetAttribute("x2", door->to.x); + obstacle->SetAttribute("y2", door->to.y); + obstacle->SetAttribute("height", door->height); + obstacle->SetAttribute("swap", door->swap); + obstacles->InsertEndChild(obstacle); } }; + + } #endif // FLOORPLANWRITER_H diff --git a/geo/BBox2.h b/geo/BBox2.h index 19835bf..e3d2a39 100644 --- a/geo/BBox2.h +++ b/geo/BBox2.h @@ -42,6 +42,10 @@ public: /** get the bbox's maximum */ const Point2& getMax() const {return p2;} + /** get the bbox's center point */ + Point2 getCenter() const { return (p1+p2) / 2; } + + /** equal? */ bool operator == (const BBox2& o) const { return (p1.x == o.p1.x) && @@ -52,16 +56,27 @@ public: /** does the BBox intersect with the given line? */ bool intersects (const Line2& l) const { - Line2 l1(p1.x, p1.y, p2.x, p1.y); // upper - Line2 l2(p1.x, p2.y, p2.x, p2.y); // lower - Line2 l3(p1.x, p1.y, p1.x, p2.y); // left - Line2 l4(p2.x, p1.y, p2.x, p2.y); // right + const Line2 l1(p1.x, p1.y, p2.x, p1.y); // upper + const Line2 l2(p1.x, p2.y, p2.x, p2.y); // lower + const Line2 l3(p1.x, p1.y, p1.x, p2.y); // left + const Line2 l4(p2.x, p1.y, p2.x, p2.y); // right return l.getSegmentIntersection(l1) || l.getSegmentIntersection(l2) || l.getSegmentIntersection(l3) || l.getSegmentIntersection(l4); } + std::vector lines() const { + return std::vector( + { + Line2(p1.x, p1.y, p2.x, p1.y), + Line2(p1.x, p2.y, p2.x, p2.y), + Line2(p1.x, p1.y, p1.x, p2.y), + Line2(p2.x, p1.y, p2.x, p2.y) + } + ); + } + bool contains(const Point2& p) const { if (p.x < p1.x) {return false;} if (p.x > p2.x) {return false;} @@ -76,6 +91,12 @@ public: p2 += p; // increase maximum } + /** grow the bbox by the amount given for each dimension */ + void grow(const float val) { + p1 -= Point2(val, val); // decrease minimum + p2 += Point2(val, val); // increase maximum + } + }; #endif // BBOX2_H diff --git a/geo/Line2.h b/geo/Line2.h index bb7a12c..9350697 100755 --- a/geo/Line2.h +++ b/geo/Line2.h @@ -55,6 +55,35 @@ public: bool getSegmentIntersection(const Line2& other) const { + const double delta = 0.0000001; + + const double bx = p2.x - p1.x; + const double by = p2.y - p1.y; + + const double dx = other.p2.x - other.p1.x; + const double dy = other.p2.y - other.p1.y; + + const double b_dot_d_perp = bx*dy - by*dx; + + if (std::abs(b_dot_d_perp) == 0) {return false;} + + const double cx = other.p1.x - p1.x; + const double cy = other.p1.y - p1.y; + + const double t = (cx * dy - cy * dx) / b_dot_d_perp; + if(t < 0+delta || t > 1-delta) {return false;} + + const double u = (cx * by - cy * bx) / b_dot_d_perp; + if(u < 0+delta || u > 1-delta) {return false;} + + return true; + + } + + bool getSegmentIntersection(const Line2& other, Point2& result) const { + + const float delta = 0.000001; + const float bx = p2.x - p1.x; const float by = p2.y - p1.y; @@ -63,49 +92,24 @@ public: const float b_dot_d_perp = bx*dy - by*dx; - if (b_dot_d_perp == 0) {return false;} + if(b_dot_d_perp == 0) {return false;} const float cx = other.p1.x - p1.x; const float cy = other.p1.y - p1.y; const float t = (cx * dy - cy * dx) / b_dot_d_perp; - if(t < 0 || t > 1) {return false;} + if(t < 0+delta || t > 1-delta) {return false;} const float u = (cx * by - cy * bx) / b_dot_d_perp; - if(u < 0 || u > 1) {return false;} + if(u < 0+delta || u > 1-delta) {return false;} + + result.x = p1.x + t * bx; + result.y = p1.y + t * by; return true; } - bool getSegmentIntersection(const Line2& other, Point2& result) const { - - const double bx = p2.x - p1.x; - const double by = p2.y - p1.y; - - const double dx = other.p2.x - other.p1.x; - const double dy = other.p2.y - other.p1.y; - - const double b_dot_d_perp = bx*dy - by*dx; - - if(b_dot_d_perp == 0) {return false;} - - const double cx = other.p1.x - p1.x; - const double cy = other.p1.y - p1.y; - - const double t = (cx * dy - cy * dx) / b_dot_d_perp; - if(t < 0 || t > 1) {return false;} - - const double u = (cx * by - cy * bx) / b_dot_d_perp; - if(u < 0 || u > 1) {return false;} - - result.x = p1.x + t * bx; - result.y = p1.y + t * by; - - return true; - - } - }; #endif // LINE2D_H diff --git a/geo/Point2.h b/geo/Point2.h index 48455fe..f18cdaa 100644 --- a/geo/Point2.h +++ b/geo/Point2.h @@ -38,10 +38,14 @@ struct Point2 { bool operator == (const Point2& o) const {return x==o.x && y==o.y;} - bool operator != (const Point2& o) const {return x!=o.x || y!=o.y;} + bool operator != (const Point2& o) const {return x!=o.x || y!=o.y;} - Point2 perpendicular() const {return Point2(-y, x);} + Point2 perpendicular() const {return Point2(-y, x);} + + float length() const {return std::sqrt(x*x + y*y);} + + Point2 normalized() const {return (*this) / length();} /** get the distance between this point and the other one */ float getDistance(const Point2& o) const { diff --git a/geo/Point3.h b/geo/Point3.h index c796dc2..2d867fd 100644 --- a/geo/Point3.h +++ b/geo/Point3.h @@ -21,6 +21,8 @@ struct Point3 { Point3(const float x, const float y, const float z) : x(x), y(y), z(z) {;} + Point3 operator - () const {return Point3(-x, -y, -z);} + Point3 operator + (const Point3& o) const {return Point3(x+o.x, y+o.y, z+o.z);} @@ -37,19 +39,43 @@ struct Point3 { Point3& operator /= (const float v) {x/=v; y/=v; z/=v; return *this;} + Point3& operator += (const Point3& o) {x+=o.x; y+=o.y; z+=o.z; return *this;} Point3& operator -= (const Point3& o) {x-=o.x; y-=o.y; z-=o.z; return *this;} + Point3& operator *= (const Point3& o) {x*=o.x; y*=o.y; z*=o.z; return *this;} + + Point3& operator /= (const Point3& o) {x/=o.x; y/=o.y; z/=o.z; return *this;} + bool operator < (const Point3& o) const {return x delta;} + }; #endif // POINT3_H diff --git a/grid/Grid.h b/grid/Grid.h index a02cc44..9403520 100755 --- a/grid/Grid.h +++ b/grid/Grid.h @@ -4,6 +4,7 @@ #include #include #include +#include #include "../Exception.h" #include "GridPoint.h" @@ -53,6 +54,12 @@ public: /** no-copy */ Grid(const Grid& o) = delete; + /** reset the grid (empty) */ + void reset() { + nodes.clear(); + hashes.clear(); + } + /** no-assign */ void operator = (const Grid& o) = delete; @@ -77,14 +84,16 @@ public: } /** add the given (not necessarly aligned) element to the grid */ - int addUnaligned(const T& elem) { - const UID uid = getUID(elem); // get the UID for this new element - Assert::isTrue(hashes.find(uid) == hashes.end(), "node's UID is already taken!"); // avoid potential errors - const int idx = nodes.size(); // next free index - nodes.push_back(elem); // add it to the grid - nodes.back()._idx = idx; // let the node know his own index - hashes[uid] = idx; // add an UID->index lookup - return idx; // done + int addUnaligned(const T& elem, const bool check=true) { + const UID uid = getUID(elem); // get the UID for this new element + if (check) { + Assert::isTrue(hashes.find(uid) == hashes.end(), "node's UID is already taken!"); // avoid potential errors + } + const int idx = nodes.size(); // next free index + nodes.push_back(elem); // add it to the grid + nodes.back()._idx = idx; // let the node know his own index + hashes[uid] = idx; // add an UID->index lookup + return idx; // done } /** connect (uni-dir) i1 -> i2 */ @@ -94,9 +103,11 @@ public: /** connect (uni-dir) i1 -> i2 */ void connectUniDir(T& n1, const T& n2) { + Assert::isFalse(n1.hasNeighbor(n2._idx), "this neighbor is already connected"); // already connected? + Assert::notEqual(n1.getIdx(), n2.getIdx(), "can not connect node with itself"); n1._neighbors[n1._numNeighbors] = n2._idx; ++n1._numNeighbors; - Assert::isBetween(n1._numNeighbors, 0, 10, "number of neighbors out of bounds!"); + Assert::isBetween(n1._numNeighbors, (uint8_t) 0, (uint8_t) 10, "number of neighbors out of bounds!"); } /** @@ -165,11 +176,25 @@ public: } /** get the center-node the given Point belongs to. or nullptr if not present */ - const T* getNodePtrFor(const GridPoint& p) { + const T* getNodePtrFor(const GridPoint& p) const { auto it = hashes.find(getUID(p)); return (it == hashes.end()) ? (nullptr) : (&nodes[it->second]); } + /** get the node nearest to the given positon */ + const T& getNearestNode(const GridPoint& p) const { + auto comp = [p] (const T& a, const T& b) { return a.getDistanceInMeter(p) < b.getDistanceInMeter(p); }; + auto it = std::min_element(nodes.begin(), nodes.end(), comp); + return nodes[it->getIdx()]; + } + + /** get the node nearest to the given positon, examining only the nodes given by the provided index-array */ + const T& getNearestNode(const GridPoint& p, const std::vector& indices) const { + auto comp = [&] (const int a, const int b) { return nodes[a].getDistanceInMeter(p) < nodes[b].getDistanceInMeter(p); }; + auto it = std::min_element(indices.begin(), indices.end(), comp); + return nodes[it->getIdx()]; + } + /** get the BBox for the given node */ GridNodeBBox getBBox(const int idx) const { return getBBox(nodes[idx]); @@ -188,7 +213,7 @@ public: UID getUID(const GridPoint& p) const { const uint64_t x = std::round(p.x_cm / (float)gridSize_cm); const uint64_t y = std::round(p.y_cm / (float)gridSize_cm); - const uint64_t z = std::round(p.z_cm / (float)gridSize_cm); + const uint64_t z = std::round(p.z_cm / (float)gridSize_cm * 5); // z is usually much lower and not always aligned -> allow more room for hashes return (z << 40) | (y << 20) | (x << 0); } @@ -446,7 +471,7 @@ private: void assertAligned(const T& elem) { if (((int)elem.x_cm % gridSize_cm) != 0) {throw Exception("element's x is not aligned!");} if (((int)elem.y_cm % gridSize_cm) != 0) {throw Exception("element's y is not aligned!");} - if (((int)elem.z_cm % gridSize_cm) != 0) {throw Exception("element's z is not aligned!");} + //if (((int)elem.z_cm % gridSize_cm) != 0) {throw Exception("element's z is not aligned!");} } }; diff --git a/grid/GridNode.h b/grid/GridNode.h index 76c172f..68727e1 100755 --- a/grid/GridNode.h +++ b/grid/GridNode.h @@ -21,15 +21,26 @@ private: /** grant full access to the grid */ template friend class Grid; - /** INTERNAL: array-index */ + /** INTERNAL: node's index array-index */ int _idx; + /** semantic annotation for this node */ + uint8_t _type; + /** INTERNAL: number of neighbors */ - int _numNeighbors; + uint8_t _numNeighbors; /** INTERNAL: store neighbors (via index) */ int _neighbors[10]; + +public: + + static const uint8_t TYPE_FLOOR = 0; + static const uint8_t TYPE_STAIR = 1; + static const uint8_t TYPE_ELEVATOR = 2; + static const uint8_t TYPE_DOOR = 3; + public: GridNode() : _idx(-1), _numNeighbors(0), _neighbors() {;} @@ -40,6 +51,23 @@ public: /** get the number of neighbors for this node */ int getNumNeighbors() const {return _numNeighbors;} + /** reached neighbor limit? */ + bool fullyConnected() const {return _numNeighbors >= 10;} + + /** is this node connected to the given index? */ + bool hasNeighbor(const int idx) const { + for (int i = 0; i < _numNeighbors; ++i) { + if (_neighbors[i] == idx) {return true;} + } + return false; + } + + /** get the node's semantic type */ + uint8_t getType() const {return _type;} + + /** set the node's semantic type */ + void setType(const uint8_t type) {this->_type = type;} + // /** get the n-th neighbor for this node */ // template inline T& getNeighbor(const int nth, const Grid& grid) const { // return grid.getNeighbor(_idx, nth); diff --git a/grid/GridPoint.h b/grid/GridPoint.h index fe862c2..29c2ac3 100755 --- a/grid/GridPoint.h +++ b/grid/GridPoint.h @@ -30,12 +30,22 @@ struct GridPoint { return x_cm == o.x_cm && y_cm == o.y_cm && z_cm == o.z_cm; } + /** not equal? */ + bool operator != (const GridPoint& o) const { + return x_cm != o.x_cm || y_cm != o.y_cm || z_cm != o.z_cm; + } + /** get the distance (in meter) betwen this and the given point */ float getDistanceInMeter(const GridPoint& other) const { + return getDistanceInCM(other) / 100.0f; + } + + /** get the distance (in centimeter) betwen this and the given point */ + float getDistanceInCM(const GridPoint& other) const { const int dx = x_cm - other.x_cm; const int dy = y_cm - other.y_cm; const int dz = z_cm - other.z_cm; - return std::sqrt(dx*dx + dy*dy + dz*dz) / 100.0f; + return std::sqrt(dx*dx + dy*dy + dz*dz); } /** cast to Point3 */ diff --git a/grid/factory/v2/GridFactory.h b/grid/factory/v2/GridFactory.h new file mode 100755 index 0000000..4e8b118 --- /dev/null +++ b/grid/factory/v2/GridFactory.h @@ -0,0 +1,507 @@ +#ifndef GRIDFACTORY_V2_H +#define GRIDFACTORY_V2_H + +#include +#include +#include + +#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 + + +/** 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 class GridFactory { + + /** logging name */ + static constexpr const char* name = "GridFac2"; + +private: + + /** the grid to build into */ + Grid& grid; + + /** calculation helper */ + Helper helper; + + /** stair builder */ + Stairs stairs; + + + bool _buildStairs = true; + bool _removeIsolated = true; + +public: + + /** ctor with the grid to fill */ + GridFactory(Grid& 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& 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& 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 minOnly(const std::vector& 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 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& 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& 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 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 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& visited) { + + std::unordered_set 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(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(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(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(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 diff --git a/grid/factory/v2/Helper.h b/grid/factory/v2/Helper.h new file mode 100644 index 0000000..7305b02 --- /dev/null +++ b/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 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 class Helper { + +private: + + Grid& grid; + +public: + + /** ctor */ + Helper(Grid& 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 diff --git a/grid/factory/v2/Importance.h b/grid/factory/v2/Importance.h new file mode 100644 index 0000000..6f04efc --- /dev/null +++ b/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 static void addOutlineNodes(Grid& dst, Grid& src) { + + for (const T& n : src) { + if (n.getNumNeighbors() < 8) { + if (!dst.hasNodeFor(n)) { + dst.add(n); + } + } + } + + } + + /** attach importance-factors to the grid */ + template static void addImportance(Grid& 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 inv(g.getGridSize_cm()); + addOutlineNodes(inv, g); + //GridFactory fac(inv); + //fac.addInverted(g, z_cm); + + // sanity check + Assert::isFalse(inv.getNumNodes() == 0, "inverted grid is empty!"); + + // construct KNN search + KNN, 3> knn(inv); + + // the number of neighbors to use + static constexpr int numNeighbors = 12; + + // create list of all doors + std::vector 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> knnArrDoors(doors); + KNN>, 3> knnDoors(knnArrDoors); + + Distribution::Normal 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 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 static bool isStaircase(Grid& 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 static bool isDoor( T& nSrc, std::vector 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 avoidWalls(0.0, 0.5); + + // favour walking near walls (likely) + static Distribution::Normal stickToWalls(0.9, 0.5); + + // favour walking far away (likely) + static Distribution::Normal 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 diff --git a/grid/factory/v2/Stairs.h b/grid/factory/v2/Stairs.h new file mode 100644 index 0000000..8e16084 --- /dev/null +++ b/grid/factory/v2/Stairs.h @@ -0,0 +1,281 @@ +#ifndef STAIRS_H +#define STAIRS_H + +#include +#include + +#include "../../Grid.h" +#include "Helper.h" +#include "../../../floorplan/v2/Floorplan.h" + + + + + +template class Stairs { + +private: + + /** the grid to build into */ + Grid& grid; + + /** calculation helper */ + Helper helper; + + +private: + +// struct Entry { +// GridPoint pos; +// std::vector 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 con; + Intermediate(const XY xy, const T node) : xy(xy),node(node), idx(-1) {;} + }; + + +public: + + /** ctor */ + Stairs(Grid& grid) : grid(grid), helper(grid) { + ; + } + + + + + + void build(const Floorplan::Floor* floor, const Floorplan::Stair* stair) { + + const int gs_cm = grid.getGridSize_cm(); + + std::vector stairNodes; + + // process each part + int partIdx = 0; + + const std::vector parts = stair->getParts(); + const std::vector 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 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 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& 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& 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 diff --git a/math/Math.h b/math/Math.h index 4d12949..936e12a 100644 --- a/math/Math.h +++ b/math/Math.h @@ -2,6 +2,7 @@ #define K_MATH_MATH_H #include "../Defines.h" +#include "../geo/Point3.h" class Math { @@ -14,6 +15,20 @@ public: return val; } + /** get the cross-product of u and v */ + static inline Point3 cross(const Point3 u, const Point3 v) { + float x = u.y*v.z - u.z*v.y; + float y = u.z*v.x - u.x*v.z; + float z = u.x*v.y - u.y*v.x; + return Point3(x,y,z); + } + + /** get the normal-vector for u and v */ + static inline Point3 normal(const Point3 u, const Point3 v) { + const Point3 p = cross(u,v); + return p/p.length(); + } + }; diff --git a/tests/Tests.h b/tests/Tests.h index de784ff..0bca2de 100755 --- a/tests/Tests.h +++ b/tests/Tests.h @@ -9,6 +9,8 @@ static inline std::string getDataFile(const std::string& name) { return "/mnt/data/workspaces/Indoor/tests/data/" + name; } + + #endif #endif // TESTS_H diff --git a/tests/api/TestAPI.cpp b/tests/api/TestAPI.cpp new file mode 100644 index 0000000..47359c3 --- /dev/null +++ b/tests/api/TestAPI.cpp @@ -0,0 +1,4 @@ +#ifndef TESTAPI_CPP +#define TESTAPI_CPP + +#endif // TESTAPI_CPP diff --git a/tests/floorplan/TestNewFloorplan.cpp b/tests/floorplan/TestNewFloorplan.cpp index 4fac0f9..132691d 100644 --- a/tests/floorplan/TestNewFloorplan.cpp +++ b/tests/floorplan/TestNewFloorplan.cpp @@ -2,10 +2,17 @@ #include "../Tests.h" #include "../../floorplan/v2/Floorplan.h" +#include "../../floorplan/v2/FloorplanReader.h" +#include "../../floorplan/v2/FloorplanWriter.h" + #include +TEST(NewFloorplan, saveAndLoad) { + Floorplan::Reader r; + Floorplan::Writer w; +} - +/* TEST(NewFloorplan, saveAndLoad) { std::string xml; @@ -108,6 +115,7 @@ TEST(NewFloorplan, saveAndLoad) { } +*/ diff --git a/tests/geo/TestBBox.cpp b/tests/geo/TestBBox.cpp new file mode 100644 index 0000000..f8d4d46 --- /dev/null +++ b/tests/geo/TestBBox.cpp @@ -0,0 +1,61 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" + +#include "../../geo/BBox2.h" + +TEST(BBox, noIntersect) { + + BBox2 bb(Point2(1,1), Point2(3,3)); + + Line2 l1(Point2(0,0), Point2(9,0)); // below + Line2 l2(Point2(0,4), Point2(9,4)); // above + + Line2 l3(Point2(0,0), Point2(0,9)); // left + Line2 l4(Point2(4,0), Point2(4,9)); // right + + ASSERT_FALSE(bb.intersects(l1)); + ASSERT_FALSE(bb.intersects(l2)); + ASSERT_FALSE(bb.intersects(l3)); + ASSERT_FALSE(bb.intersects(l4)); + +} + +TEST(BBox, noDirectIntersect) { + + BBox2 bb(Point2(1,1), Point2(3,3)); + + Line2 l1(Point2(0,1), Point2(9,1)); // lower + Line2 l2(Point2(0,3), Point2(9,3)); // upper + + Line2 l3(Point2(1,0), Point2(1,9)); // left + Line2 l4(Point2(3,0), Point2(3,9)); // right + + ASSERT_FALSE(bb.intersects(l1)); + ASSERT_FALSE(bb.intersects(l2)); + ASSERT_FALSE(bb.intersects(l3)); + ASSERT_FALSE(bb.intersects(l4)); + +} + + +TEST(BBox, positiveIntersect) { + + BBox2 bb(Point2(1,1), Point2(3,3)); + + Line2 l1(Point2(0,2), Point2(2,2)); // left hit + Line2 l2(Point2(2,2), Point2(4,2)); // right hit + + Line2 l3(Point2(2,0), Point2(2,2)); // lower hit + Line2 l4(Point2(2,2), Point2(2,4)); // upper hit + + ASSERT_TRUE(bb.intersects(l1)); + ASSERT_TRUE(bb.intersects(l2)); + ASSERT_TRUE(bb.intersects(l3)); + ASSERT_TRUE(bb.intersects(l4)); + +} + + + +#endif diff --git a/tests/grid/TestWalk.cpp b/tests/grid/TestWalk.cpp index 18ed0a7..4f28eb8 100644 --- a/tests/grid/TestWalk.cpp +++ b/tests/grid/TestWalk.cpp @@ -130,7 +130,7 @@ TEST(Walk, DISABLED_plot) { for (int i = 0; i < 5000; ++i) { pStates.clear(); for (GridWalkState& state : states) { - state = walk.getDestination(g, state, std::abs(dWalk(gen)), dTurn(gen)); + state = walk.getDestination(g, state, std::abs(dWalk(gen)), dTurn(gen), Activity::UNKNOWN); pStates.add(K::GnuplotPoint3(state.node->x_cm, state.node->y_cm, state.node->z_cm+10)); } p.gp.draw(p.splot); diff --git a/wifi/estimate/ray2d/Ray2.h b/wifi/estimate/ray2d/Ray2.h new file mode 100644 index 0000000..8da2e71 --- /dev/null +++ b/wifi/estimate/ray2d/Ray2.h @@ -0,0 +1,4 @@ +#ifndef RAY2_H +#define RAY2_H + +#endif // RAY2_H diff --git a/wifi/estimate/ray2d/WiFiRayTrace2D.h b/wifi/estimate/ray2d/WiFiRayTrace2D.h new file mode 100644 index 0000000..88ba9ac --- /dev/null +++ b/wifi/estimate/ray2d/WiFiRayTrace2D.h @@ -0,0 +1,4 @@ +#ifndef WIFIRAYTRACE2D_H +#define WIFIRAYTRACE2D_H + +#endif // WIFIRAYTRACE2D_H