diff --git a/Assertions.h b/Assertions.h index 7e4e814..e14bf4b 100644 --- a/Assertions.h +++ b/Assertions.h @@ -13,45 +13,47 @@ namespace Assert { - static inline void doThrow(const char* err) { + template static inline void doThrow(const STR err) { #ifdef WITH_ASSERTIONS std::string str = "in: "; str += __PRETTY_FUNCTION__; str += " error: "; str += err; throw Exception(err); + #else + (void) err; #endif } - template static inline void equal(const T v1, const T v2, const char* err) { + template static inline void equal(const T v1, const T v2, const STR err) { if (v1 != v2) {doThrow(err);} } - template static inline void isTrue(const T v, const char* err) { + template static inline void isTrue(const T v, const STR err) { if (!v) {doThrow(err);} } - template static inline void isFalse(const T v, const char* err) { + template static inline void isFalse(const T v, const STR err) { if (v) {doThrow(err);} } - template static inline void isNull(const T v, const char* err) { + template static inline void isNull(const T v, const STR err) { if (v != nullptr) {doThrow(err);} } - template static inline void isNotNull(const T v, const char* err) { + template static inline void isNotNull(const T v, const STR err) { if (v == nullptr) {doThrow(err);} } - template static inline void isNotNaN(const T v, const char* err) { + template static inline void isNotNaN(const T v, const STR err) { if (v != v) {doThrow(err);} } - template static inline void isNot0(const T v, const char* err) { + template static inline void isNot0(const T v, const STR err) { if (v == 0) {doThrow(err);} } - template static inline void isBetween(const T v, const T min, const T max, const char* 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/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h new file mode 100644 index 0000000..2618383 --- /dev/null +++ b/floorplan/v2/Floorplan.h @@ -0,0 +1,188 @@ +#ifndef FLOORPLAN_H +#define FLOORPLAN_H + +#include +#include +#include +#include + +#include "../../geo/Point3.h" +#include "../../geo/Point2.h" + +namespace Floorplan { + + /** 3D polygon */ + struct Polygon2 { + std::vector points; + Polygon2() : points() {;} + Polygon2(const std::initializer_list lst) : points(lst) {;} + bool operator == (const Polygon2& o) const {return std::equal(o.points.begin(), o.points.end(), this->points.begin());} + }; + + /** additional type-info for obstacles */ + enum class ObstacleType { + UNKNOWN, + WALL, + DOOR, + WINDOW, + HANDRAIL, + PILLAR, + _END, + }; + + /** all supported material types */ + enum class Material { + UNKNOWN, + CONCRETE, + WOOD, + DRYWALL, + GLASS, + _END, + }; + + /** types of outlines. either add or remove the selected region */ + enum class OutlineMethod { + ADD, + REMOVE, + _END, + }; + + + + /** an AccessPoint located somewhere within the map */ + struct AccessPoint { + std::string name; + std::string mac; + Point3 pos; + 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);} + }; + + /** a beacon located somewhere within the map */ + struct Beacon { + std::string name; + std::string mac; + Point3 pos; + 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);} + }; + + + /** a polygon denoting a floor's outline. is either added or removed */ + struct FloorOutlinePolygon { + OutlineMethod method; + std::string name; + Polygon2 poly; + FloorOutlinePolygon() : method(OutlineMethod::ADD), name(), poly() {;} + FloorOutlinePolygon(const OutlineMethod method, const std::string& name, const Polygon2& poly) : method(method), name(name), poly(poly) {;} + 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) {;} + virtual ~FloorObstacle() {;} + }; + + /** line obstacle */ + struct FloorObstacleLine : public FloorObstacle { + 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) {;} + }; + + /** 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) {;} + }; + + /** one region (e.g. a room) within the floor, described using a polygon */ + struct FloorRegion { + std::string name; + Polygon2 poly; + FloorRegion() : name(), poly() {;} + FloorRegion(const std::string& name, const Polygon2& poly) : name(name), poly(poly) {;} + }; + + + + /** an image file that can be added to the map */ + struct UnderlayImage { + std::string name; + std::string filename; + Point2 anchor; + float scaleX; + 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 { + + float width; + float depth; + std::vector floors; + + IndoorMap() {;} + + IndoorMap(const IndoorMap& o) = delete; + void operator = (const IndoorMap& o) = delete; + + }; + +} + +#endif // FLOORPLAN_H diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h new file mode 100644 index 0000000..82b5bcb --- /dev/null +++ b/floorplan/v2/FloorplanReader.h @@ -0,0 +1,302 @@ +#ifndef FLOORPLANREADER_H +#define FLOORPLANREADER_H + +#include + +#include "Floorplan.h" + +#include "../../Assertions.h" +#include "../../lib/tinyxml/tinyxml2.h" + +namespace Floorplan { + + using namespace tinyxml2; + + /** + * read an IndoorMaps from XML-data + */ + class Reader { + + public: + + /** 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()); + 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()); + return parse(doc); + } + + private: + + #define FOREACH_NODE(out, in) for( const XMLElement* out = (XMLElement*) in->FirstChild(); out; out = (XMLElement*) out->NextSibling() ) + + static void assertNode(const std::string& node, const XMLElement* 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()); + } + + /** parse the node */ + static IndoorMap* parseMap(const XMLElement* el) { + std::cout << el->Name() << std::endl; + IndoorMap* map = new IndoorMap(); + map->width = el->FloatAttribute("width"); + map->depth = el->FloatAttribute("depth"); + FOREACH_NODE(n, el) { + if (std::string("floors") == n->Name()) {map->floors = parseFloors(n);} + } + return map; + } + + /** parse the node */ + static std::vector parseFloors(const XMLElement* el) { + std::vector floors; + FOREACH_NODE(n, el) { + if (std::string("floor") == n->Name()) {floors.push_back(parseFloor(n));} + } + return floors; + } + + /** parse one node */ + static Floor* parseFloor(const XMLElement* 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("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);} + } + return floor; + } + + /** parse the tag */ + static std::vector parseFloorAccessPoints(const XMLElement* el) { + assertNode("accesspoints", el); + std::vector vec; + FOREACH_NODE(n, el) { + if (std::string("accesspoint") == n->Name()) { vec.push_back(parseAccessPoint(n)); } + } + 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 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 the tag */ +// static FloorKeyValue parseFloorKeyValue(const XMLElement* el) { +// FloorKeyValue res; +// FOREACH_NODE(n, el) { +// if (std::string("keyval") == n->Name()) { res.elements.push_back(parseKeyValueElement(n)); } +// } +// return res; +// } + + /** parse one element */ + static KeyValueElement* parseKeyValueElement(const XMLElement* n) { + KeyValueElement* elem = new KeyValueElement(); + const XMLAttribute* attr = n->FirstAttribute(); + while (attr) { + elem->params[attr->Name()] = attr->Value(); + attr = attr->Next(); + } + return elem; + } + + static AccessPoint* parseAccessPoint(const XMLElement* n) { + assertNode("accesspoint", n); + AccessPoint* ap = new AccessPoint(); + ap->mac = n->Attribute("mac"); + ap->name = n->Attribute("name"); + ap->pos = parsePoint3(n); + return ap; + } + + + /** parse the tag */ + static std::vector parseFloorBeacons(const XMLElement* el) { + std::vector vec; + FOREACH_NODE(n, el) { + if (std::string("beacon") == n->Name()) { vec.push_back(parseBeacon(n)); } + } + return vec; + } + + static Beacon* parseBeacon(const XMLElement* n) { + assertNode("beacon", n); + Beacon* b = new Beacon(); + b->mac = n->Attribute("mac"); + b->name = n->Attribute("name"); + b->pos = parsePoint3(n); + return b; + } + + + static std::vector parseFloorRegions(const XMLElement* el) { + std::vector vec; + FOREACH_NODE(n, el) { + if (std::string("region") == n->Name()) { vec.push_back(parseFloorRegion(n)); } + } + return vec; + } + + static FloorRegion* parseFloorRegion(const XMLElement* n) { + assertNode("region", n); + FloorRegion* reg = new FloorRegion(); + reg->name = n->Attribute("name"); + reg->poly = parsePoly2(n); + return reg; + } + + /** parse the tag */ + static std::vector parseFloorObstacles(const XMLElement* el) { + assertNode("obstacles", el); + std::vector obstacles; + FOREACH_NODE(n, el) { + // if (std::string("wall") == n->Name()) {obstacles.push_back(parseFloorObstacleWall(n));} + // 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));} + } + return obstacles; + } + + /** parse one line */ + static FloorObstacleLine* parseFloorObstacleLine(const XMLElement* 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")) + ); + } + + /** parse one cirlce */ + static FloorObstacleCircle* parseFloorObstacleCircle(const XMLElement* 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") + ); + } + + /** parse a floor's tag */ + static FloorOutline parseFloorOutline(const XMLElement* el) { + FloorOutline outline; + FOREACH_NODE(n, el) { + if (std::string("polygon") == n->Name()) { + outline.push_back(parseFloorPolygon(n)); // TODO + } + } + return outline; + } + + /** parse one polygon */ + static FloorOutlinePolygon* parseFloorPolygon(const XMLElement* el) { + FloorOutlinePolygon* poly = new FloorOutlinePolygon(); + poly->name = el->Attribute("name"); + poly->method = parseOutlineMethod(el->Attribute("method")); + poly->poly = parsePoly2(el); + return poly; + } + + /** parse a 2d-polygon denoted by several points */ + static Polygon2 parsePoly2(const XMLElement* el) { + Polygon2 poly; + FOREACH_NODE(n, el) { + if (std::string("point") == n->Name()) { + const Point2 p2 = parsePoint2(n); + poly.points.push_back(p2); + } + } + return poly; + } + + /** parse a 2D point (x,y) using the given tag's attributes. missing attributes are set to 0 */ + static Point2 parsePoint2(const XMLElement* el) { + Point2 point; + point.x = el->FloatAttribute("x"); + point.y = el->FloatAttribute("y"); + return point; + } + + /** 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) { + Point3 point; + point.x = el->FloatAttribute("x"); + point.y = el->FloatAttribute("y"); + point.z = el->FloatAttribute("z"); + return point; + } + + static ObstacleType parseObstacleType(const std::string type) { + try { + return (ObstacleType) std::stoi(type); + } catch (...) { + return ObstacleType::UNKNOWN; + } + } + + static Material parseMaterial(const std::string material) { + try { + return (Material) std::stoi(material); + } catch (...) { + return Material::UNKNOWN; + } + } + + static OutlineMethod parseOutlineMethod(const std::string method) { + try { + return (OutlineMethod) std::stoi(method); + } catch (...) { + return OutlineMethod::ADD; + } + } + + }; + +} + +#endif // FLOORPLANREADER_H diff --git a/floorplan/v2/FloorplanWriter.h b/floorplan/v2/FloorplanWriter.h new file mode 100644 index 0000000..3479a7b --- /dev/null +++ b/floorplan/v2/FloorplanWriter.h @@ -0,0 +1,243 @@ +#ifndef FLOORPLANWRITER_H +#define FLOORPLANWRITER_H + +#include + +#include "Floorplan.h" + +#include "../../Assertions.h" +#include "../../lib/tinyxml/tinyxml2.h" + +namespace Floorplan { + + using namespace tinyxml2; + + /** + * write floorplans as XML + */ + class Writer { + + public: + + static void writeToFile(const IndoorMap* map, const std::string& file) { + XMLDocument doc; + construct(doc, map); + doc.SaveFile(file.c_str(), false); + } + + static std::string writeToString(const IndoorMap* map) { + XMLDocument doc; + construct(doc, map); + 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 Material m) { + return std::to_string((int)m); + } + + 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"); + doc.InsertEndChild(root); + root->SetAttribute("width", map->width); + root->SetAttribute("depth", map->depth); + + // add all floors to the map + addFloors(doc, root, map); + + } + + /** add all floors to the map */ + static void addFloors(XMLDocument& doc, XMLElement* root, const IndoorMap* map) { + XMLElement* 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) { + + XMLElement* floor = doc.NewElement("floor"); + floor->SetAttribute("atHeight", mf->atHeight); + floor->SetAttribute("height", mf->height); + floor->SetAttribute("name", mf->name.c_str()); + floors->InsertEndChild(floor); + + addFloorOutline(doc, floor, mf); + addFloorObstacles(doc, floor, mf); + addFloorUnderlays(doc, floor, mf); + + // add all sorts of POI + addFloorPOI(doc, floor, mf); + + } + + /** add all sorts of POI to the floor */ + static void addFloorPOI(XMLDocument& doc, XMLElement* floor, const Floor* mf) { + + XMLElement* accesspoints = doc.NewElement("accesspoints"); + for (const AccessPoint* ap : mf->accesspoints) { + XMLElement* accesspoint = doc.NewElement("accesspoint"); + accesspoint->SetAttribute("name", ap->name.c_str()); + accesspoint->SetAttribute("mac", ap->mac.c_str()); + accesspoint->SetAttribute("x", ap->pos.x); + accesspoint->SetAttribute("y", ap->pos.y); + accesspoint->SetAttribute("z", ap->pos.z); + accesspoints->InsertEndChild(accesspoint); + } + floor->InsertEndChild(accesspoints); + + XMLElement* beacons = doc.NewElement("beacons"); + for (const Beacon* b : mf->beacons) { + XMLElement* beacon = doc.NewElement("beacon"); + beacon->SetAttribute("name", b->name.c_str()); + beacon->SetAttribute("mac", b->mac.c_str()); + beacon->SetAttribute("x", b->pos.x); + beacon->SetAttribute("y", b->pos.y); + beacon->SetAttribute("z", b->pos.z); + beacons->InsertEndChild(beacon); + } + floor->InsertEndChild(beacons); + + + } + + static void addFloorOutline(XMLDocument& doc, XMLElement* floor, const Floor* mf) { + + XMLElement* outline = doc.NewElement("outline"); + floor->InsertEndChild(outline); + + // XMLElement* add = doc.NewElement("add"); + // outline->InsertEndChild(add); + for (const FloorOutlinePolygon* poly : mf->outline) { addFloorOutlinePolygon(doc, outline, poly); } + + // XMLElement* 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) { + + std::string method = toString(poly->method); + + XMLElement* 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"); + 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 + polygon->InsertEndChild(point); + } + + } + + /** write one element */ + static void addKeyValueElement(XMLDocument& doc, XMLElement* other, const KeyValueElement* kv) { + XMLElement* elem = doc.NewElement("keyval"); + for (auto it : kv->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 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 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"); + + for (FloorObstacle* fo : mf->obstacles) { + if (dynamic_cast(fo)) { + addFloorObstacleLine(doc, obstacles, (FloorObstacleLine*)fo); + } else if (dynamic_cast(fo)) { + addFloorObstacleCircle(doc, obstacles, (FloorObstacleCircle*)fo); + } + } + + floor->InsertEndChild(obstacles); + + } + + 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()); + 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()); + obstacle->SetAttribute("cx", circle->center.x); + obstacle->SetAttribute("cy", circle->center.y); + obstacle->SetAttribute("radius", circle->radius); + obstacles->InsertEndChild(obstacle); + + } + + }; + +} + +#endif // FLOORPLANWRITER_H diff --git a/geo/BBox2.h b/geo/BBox2.h index 568e74f..19835bf 100644 --- a/geo/BBox2.h +++ b/geo/BBox2.h @@ -70,6 +70,12 @@ public: return true; } + /** grow the bbox by the amount given for each dimension */ + void grow(const Point2& p) { + p1 -= p; // decrease minimum + p2 += p; // increase maximum + } + }; #endif // BBOX2_H diff --git a/geo/BBox3.h b/geo/BBox3.h index 44cd281..a0854b4 100644 --- a/geo/BBox3.h +++ b/geo/BBox3.h @@ -18,8 +18,12 @@ private: public: + /** empty ctor */ BBox3() : p1(MAX,MAX,MAX), p2(MIN,MIN,MIN) {;} + /** ctor with min and max */ + BBox3(const Point3 min, const Point3 max) : p1(min), p2(max) {;} + /** adjust the bounding-box by adding this point */ void add(const Point3& p) { @@ -71,6 +75,12 @@ public: p2 += p; // increase maximum } + /** set both, min/max z to the same value */ + void setZ(const float z) { + p1.z = z; + p2.z = z; + } + /** does the bbox contain the given point? */ bool contains(const Point3& p) const { if (p.x < p1.x) {return false;} diff --git a/geo/Line2.h b/geo/Line2.h index 56c21c1..bb7a12c 100755 --- a/geo/Line2.h +++ b/geo/Line2.h @@ -14,8 +14,13 @@ public: public: + /** empty ctor */ Line2() : p1(), p2() {;} + /** value ctor */ + Line2(const Point2 p1, const Point2 p2) : p1(p1), p2(p2) {;} + + /** value ctor */ Line2(const float x1, const float y1, const float x2, const float y2) : p1(x1,y1), p2(x2,y2) {;} // bool getSegmentIntersection(const Line& other) const { @@ -24,6 +29,30 @@ public: // } + /** get intersection between these two lines (unlimited length!) */ + bool getLineIntersection(const Line2& other, Point2& result) const { + + double bx = p2.x - p1.x; + double by = p2.y - p1.y; + + double dx = other.p2.x - other.p1.x; + double dy = other.p2.y - other.p1.y; + + double b_dot_d_perp = bx*dy - by*dx; + + if(b_dot_d_perp == 0) {return false;} + + double cx = other.p1.x - p1.x; + double cy = other.p1.y - p1.y; + double t = (cx*dy - cy*dx) / b_dot_d_perp; + + result.x = p1.x + t * bx; + result.y = p1.y + t * by; + + return true; + + } + bool getSegmentIntersection(const Line2& other) const { const float bx = p2.x - p1.x; @@ -49,6 +78,34 @@ public: } + 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 759cc64..48455fe 100644 --- a/geo/Point2.h +++ b/geo/Point2.h @@ -38,6 +38,10 @@ 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;} + + + Point2 perpendicular() const {return Point2(-y, x);} /** 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 d775e0c..c796dc2 100644 --- a/geo/Point3.h +++ b/geo/Point3.h @@ -46,6 +46,8 @@ struct Point3 { bool operator == (const Point3& o) const {return x==o.x && y==o.y && z==o.z;} + bool operator != (const Point3& o) const {return x!=o.x || y!=o.y || z!=o.z;} + Point2 xy() const {return Point2(x,y);} /** read-only array access */ diff --git a/tests/floorplan/TestNewFloorplan.cpp b/tests/floorplan/TestNewFloorplan.cpp new file mode 100644 index 0000000..4fac0f9 --- /dev/null +++ b/tests/floorplan/TestNewFloorplan.cpp @@ -0,0 +1,114 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" +#include "../../floorplan/v2/Floorplan.h" +#include + + + +TEST(NewFloorplan, saveAndLoad) { + + std::string xml; + + float f1AtHeight = 1.1f; + float f2AtHeight = 3.3f; + + Floorplan::AccessPoint ap0("ap0", "11:11:11:22:22:22", Point3(1,1,1)); + Floorplan::AccessPoint ap1("ap1", "01:23:45:67:89:ab", Point3(1,2,3)); + Floorplan::AccessPoint ap2("ap2", "22:33:44:55:66:77", Point3(4,5,6)); + + Floorplan::Beacon b0("b0", "22:22:22:11:11:11", Point3(2,2,2)); + Floorplan::Beacon b1("b1", "aa:bb:cc:dd:ee:ff", Point3(3,2,1)); + Floorplan::Beacon b2("b2", "77:66:55:44:33:22", Point3(6,5,4)); + + Floorplan::FloorObstacle fo0(Floorplan::ObstacleType::WALL, Floorplan::Material::CONCRETE, Point2(3,2), Point2(1,2)); + Floorplan::FloorObstacle fo1(Floorplan::ObstacleType::DOOR, Floorplan::Material::WOOD, Point2(4,5), Point2(7,8)); + + Floorplan::FloorOutlinePolygon pa0("pa0", {Point2(1,2), Point2(4,5), Point2(7,8)}); + Floorplan::FloorOutlinePolygon pr0("pr0", {Point2(3,2), Point2(6,5), Point2(9,8)}); + + Floorplan::FloorOutlinePolygon pa1("pa1", {Point2(1,2), Point2(7,8)}); + Floorplan::FloorOutlinePolygon pa2("pa2", {Point2(4,5), Point2(7,8)}); + + // create and export + { + + Floorplan::IndoorMap im; + im.width = 123; + im.depth = 456; + + // floor1 + Floorplan::Floor f1; + f1.atHeight = f1AtHeight; + f1.accesspoints.push_back(ap0); + f1.beacons.push_back(b0); + f1.obstacles.push_back(fo0); + f1.obstacles.push_back(fo1); + f1.outline.add.push_back(pa0); + f1.outline.remove.push_back(pr0); + im.floors.push_back(f1); + + // floor2 + Floorplan::Floor f2; + f2.atHeight = f2AtHeight; + f2.accesspoints.push_back(ap1); + f2.accesspoints.push_back(ap2); + f2.beacons.push_back(b1); + f2.beacons.push_back(b2); + f2.outline.add.push_back(pa1); + f2.outline.add.push_back(pa2); + im.floors.push_back(f2); + + xml = Floorplan::Writer::writeToString(im); + + } + + // load and check + { + + const float delta = 0.0001; + + Floorplan::IndoorMap im = Floorplan::Reader::readFromString(xml); + ASSERT_NEAR(123, im.width, delta); + ASSERT_NEAR(456, im.depth, delta); + ASSERT_EQ(2, im.floors.size()); + + // floor1 + ASSERT_NEAR(f1AtHeight, im.floors[0].atHeight, delta); + ASSERT_EQ(1, im.floors[0].accesspoints.size()); + ASSERT_EQ(1, im.floors[0].beacons.size()); + ASSERT_EQ(2, im.floors[0].obstacles.size()); + ASSERT_EQ(1, im.floors[0].outline.add.size()); + ASSERT_EQ(1, im.floors[0].outline.remove.size()); + + ASSERT_EQ(ap0, im.floors[0].accesspoints[0]); + ASSERT_EQ(b0, im.floors[0].beacons[0]); + ASSERT_EQ(fo0, im.floors[0].obstacles[0]); + ASSERT_EQ(fo1, im.floors[0].obstacles[1]); + ASSERT_EQ(pa0, im.floors[0].outline.add[0]); + ASSERT_EQ(pr0, im.floors[0].outline.remove[0]); + + // floor2 + ASSERT_NEAR(f2AtHeight, im.floors[1].atHeight, delta); + ASSERT_EQ(2, im.floors[1].accesspoints.size()); + ASSERT_EQ(2, im.floors[1].beacons.size()); + ASSERT_EQ(0, im.floors[1].obstacles.size()); + ASSERT_EQ(2, im.floors[1].outline.add.size()); + ASSERT_EQ(0, im.floors[1].outline.remove.size()); + + ASSERT_EQ(ap1, im.floors[1].accesspoints[0]); + ASSERT_EQ(ap2, im.floors[1].accesspoints[1]); + ASSERT_EQ(b1, im.floors[1].beacons[0]); + ASSERT_EQ(b2, im.floors[1].beacons[1]); + ASSERT_EQ(pa1, im.floors[1].outline.add[0]); + ASSERT_EQ(pa2, im.floors[1].outline.add[1]); + + + } + + +} + + + +#endif