diff --git a/Assertions.h b/Assertions.h index 567ef4d..1608526 100644 --- a/Assertions.h +++ b/Assertions.h @@ -67,7 +67,6 @@ namespace Assert { 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) { std::stringstream ss; ss << "\n[" << min << ":" << max << "] but is " << v << "\n"; @@ -77,4 +76,24 @@ namespace Assert { } +namespace StaticAssert { + + // yes and no have a different size so sizeof(yes) != sizeof(no) + using yes = uint32_t; + using no = uint64_t; + + template struct isXbaseOfY { + static yes test(const BaseClass&); // will be chosen if X is base of Y + static no test(...); // will be chosen otherwise + static const SubClass& helper(); + static const bool value = sizeof(test(helper())) == sizeof(yes); // true if test(const B&) was chosen + }; + + template static inline void AinheritsB() { + static_assert(isXbaseOfY::value, "A does not inherit from B!"); + } + + +} + #endif // ASSERTIONS_H diff --git a/data/RingBuffer.h b/data/RingBuffer.h index 85f28d2..3259aa2 100644 --- a/data/RingBuffer.h +++ b/data/RingBuffer.h @@ -21,14 +21,18 @@ public: ; } - /** add a new element */ - void add(const Element& element) { + /** add a new element WITH overflow check! */ + void addSafe(const Element& element) { Assert::isTrue(available != (int)data.size(), "buffer overflow"); + add(element); + } + + /** add a new element WIUTHOUT checking for overflows */ + void add(const Element& element) { data[iWrite] = element; ++iWrite; iWrite %= data.size(); ++available; - //if (available > (int)data.size()) {available = data.size();} } /** get the next element */ @@ -42,6 +46,25 @@ public: return tmp; } + /** peek into the given element without removing it */ + const Element& peek(const int idx) const { + const Element& tmp = data[(iRead + idx) % data.size()]; + return tmp; + } + + /** peek into the given element without removing it */ + const Element& operator [] (const int idx) const { + return peek(idx); + } + + /** does the buffer contain the given element? */ + bool contains(const Element& e) const { + for (int i = 0; i < available; ++i) { + if (peek(i) == e) {return true;} + } + return false; + } + /** reset the ringbuffer */ void reset() { iWrite = 0; diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index 13c0f86..95648d0 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -116,6 +116,7 @@ namespace Floorplan { struct UnderlayImage; struct POI; struct Stair; + struct Elevator; using FloorOutline = std::vector; using FloorObstacles = std::vector; @@ -125,6 +126,7 @@ namespace Floorplan { using FloorUnderlays = std::vector; using FloorPOIs = std::vector; using FloorStairs = std::vector; + using FloorElevators = std::vector; /** describes one floor within the map, starting at a given height */ struct Floor { @@ -140,6 +142,7 @@ namespace Floorplan { FloorUnderlays underlays; // underlay images (used for map-building) FloorPOIs pois; // POIs within the floor FloorStairs stairs; // all stairs within one floor + FloorElevators elevators; // all elevators within one floor //FloorKeyValue other; // other, free elements Floor() {;} @@ -249,8 +252,6 @@ namespace Floorplan { FloorRegion(const std::string& name, const Polygon2& poly) : name(name), poly(poly) {;} }; - - static Point3 xy0(const Point2 p) { return Point3(p.x, p.y, 0); } @@ -305,7 +306,36 @@ namespace Floorplan { }; - static std::vector getQuads(const std::vector& parts, const Floor* floor) { + + /** snap quad-edges together if their distance is below the given maximum. is used to close minor gaps */ + static void snapQuads(std::vector& quads, const float maxDist) { + + for (Floorplan::Quad3& quad1 : quads) { + for (int i1 = 0; i1 < 4; ++i1) { + Point3& p1 = (Point3&) quad1[i1]; + for (const Floorplan::Quad3& quad2 : quads) { + + if (&quad1 == &quad2) {continue;} + for (int i1 = 0; i1 < 4; ++i1) { + Point3& p2 = (Point3&) quad2[i1]; + + if (p1.getDistance(p2) < maxDist) { + const Point3 p3 = (p1+p2) / 2; + p1 = p3; + p2 = p3; + break; + } + + } + } + + } + } + + } + + /** convert stair-parts to quads. the scaling factor may be used to slightly grow each quad. e.g. needed to ensure that the quads overlap */ + static std::vector getQuads(const std::vector& parts, const Floor* floor, const float s = 1.0f) { std::vector vec; @@ -318,10 +348,10 @@ namespace Floorplan { 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 Point3 p1 = start + xy0(perpN * width*s / 2) + Point3(0,0,floor->atHeight); + const Point3 p2 = start - xy0(perpN * width*s / 2) + Point3(0,0,floor->atHeight); + const Point3 p3 = end - xy0(perpN * width*s / 2) + Point3(0,0,floor->atHeight); + const Point3 p4 = end + xy0(perpN * width*s / 2) + Point3(0,0,floor->atHeight); const Quad3 q(p1,p2,p3,p4); vec.push_back(q); @@ -337,17 +367,12 @@ namespace Floorplan { } + snapQuads(vec, 0.05); 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 { @@ -355,43 +380,6 @@ namespace Floorplan { 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; @@ -399,7 +387,42 @@ namespace Floorplan { std::vector getParts() const {return parts;} }; + /** + * describes an elevator between two floors + * so: the height depends on the height of the floor! + */ + struct Elevator { + /** the elevator's center */ + Point2 center; + + /** the elevator's width (in meter) */ + float width; + + /** the elevator's depth (in meter) */ + float depth; + + /** the elevator's rotation (in radians) */ + float rotation; + + /** get the 4 corner points for the elevator */ + Polygon2 getPoints() const { + const Point2 p1 = Point2(+width/2, +depth/2).rotated(rotation) + center; + const Point2 p2 = Point2(-width/2, +depth/2).rotated(rotation) + center; + const Point2 p3 = Point2(-width/2, -depth/2).rotated(rotation) + center; + const Point2 p4 = Point2(+width/2, -depth/2).rotated(rotation) + center; + Polygon2 poly; + poly.points = {p1, p2, p3, p4}; + return poly; + } + + /** empty ctor */ + Elevator() : center(), width(2), depth(2), rotation(0) {;} + + /** ctor */ + Elevator(const Point2 center) : center(center), width(2), depth(2), rotation(0) {;} + + }; diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 790ba97..d2b626e 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -88,11 +88,21 @@ namespace Floorplan { 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);} + if (std::string("elevators") == n->Name()) {floor->elevators = parseFloorElevators(n);} } return floor; } + /** parse the tag */ + static std::vector parseFloorElevators(const XMLElem* el) { + std::vector vec; + FOREACH_NODE(n, el) { + if (std::string("elevator") == n->Name()) { vec.push_back(parseFloorElevator(n)); } + } + return vec; + } + /** parse the tag */ static std::vector parseFloorStairs(const XMLElem* el) { std::vector vec; @@ -102,6 +112,16 @@ namespace Floorplan { return vec; } + /** parse an tag */ + static Elevator* parseFloorElevator(const XMLElem* el) { + Elevator* elev = new Elevator(); + elev->center = Point2(el->FloatAttribute("cx"), el->FloatAttribute("cy")); + elev->depth = el->FloatAttribute("depth"); + elev->width = el->FloatAttribute("width"); + elev->rotation = el->FloatAttribute("rotation"); + return elev; + } + /** parse a tag */ static Stair* parseFloorStair(const XMLElem* el) { Stair* stair = nullptr; diff --git a/floorplan/v2/FloorplanWriter.h b/floorplan/v2/FloorplanWriter.h index 96763d7..bd0bc4e 100644 --- a/floorplan/v2/FloorplanWriter.h +++ b/floorplan/v2/FloorplanWriter.h @@ -78,6 +78,24 @@ namespace Floorplan { addFloorPOI(doc, floor, mf); addStairs(doc, floor, mf); + addElevators(doc, floor, mf); + + } + + /** add all elevators to the floor */ + static void addElevators(XMLDoc& doc, XMLElem* floor, const Floor* mf) { + + XMLElem* elevators = doc.NewElement("elevators"); + for (const Elevator* elevator : mf->elevators) { + XMLElem* elem = doc.NewElement("elevator"); + elem->SetAttribute("cx", elevator->center.x); + elem->SetAttribute("cy", elevator->center.y); + elem->SetAttribute("width", elevator->width); + elem->SetAttribute("depth", elevator->depth); + elem->SetAttribute("rotation", elevator->rotation); + elevators->InsertEndChild(elem); + } + floor->InsertEndChild(elevators); } diff --git a/grid/Grid.h b/grid/Grid.h index d41b29e..873635d 100755 --- a/grid/Grid.h +++ b/grid/Grid.h @@ -108,9 +108,9 @@ public: 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"); + Assert::isFalse(n1.fullyConnected(), "this node has already reached its neighbor-limit"); n1._neighbors[n1._numNeighbors] = n2._idx; ++n1._numNeighbors; - Assert::isBetween(n1._numNeighbors, (uint8_t) 0, (uint8_t) 10, "number of neighbors out of bounds!"); } /** diff --git a/grid/GridNode.h b/grid/GridNode.h index 495aac9..d648b82 100755 --- a/grid/GridNode.h +++ b/grid/GridNode.h @@ -18,6 +18,10 @@ template class Grid; */ struct GridNode { +private: + + static int constexpr MAX_NEIGHBORS = 10; + private: /** grant full access to the grid */ @@ -27,7 +31,7 @@ private: int _idx; /** INTERNAL: store neighbors (via index) */ - int _neighbors[10]; + int _neighbors[MAX_NEIGHBORS]; /** INTERNAL: number of neighbors */ uint8_t _numNeighbors; @@ -61,7 +65,7 @@ public: int getNumNeighbors() const {return _numNeighbors;} /** reached neighbor limit? */ - bool fullyConnected() const {return _numNeighbors >= 10;} + bool fullyConnected() const {return _numNeighbors >= MAX_NEIGHBORS;} /** is this node connected to the given index? */ bool hasNeighbor(const int idx) const { diff --git a/grid/GridPoint.h b/grid/GridPoint.h index 3e17dc4..5fb804a 100755 --- a/grid/GridPoint.h +++ b/grid/GridPoint.h @@ -37,6 +37,8 @@ struct GridPoint { GridPoint operator * (const float f) const {return GridPoint(x_cm*f, y_cm*f, z_cm*f);} + GridPoint operator + (const GridPoint& o) const {return GridPoint(x_cm+o.x_cm, y_cm+o.y_cm, z_cm+o.z_cm);} + GridPoint& operator += (const GridPoint& o) {x_cm += o.x_cm; y_cm += o.y_cm; z_cm += o.z_cm; return *this;} GridPoint& operator /= (const float f) {x_cm /= f; y_cm /= f; z_cm /= f; return *this;} diff --git a/grid/factory/v2/Elevators.h b/grid/factory/v2/Elevators.h new file mode 100644 index 0000000..4c035c8 --- /dev/null +++ b/grid/factory/v2/Elevators.h @@ -0,0 +1,99 @@ +#ifndef GRID_ELEVATORS_H +#define GRID_ELEVATORS_H + +#include +#include + +#include "../../Grid.h" +#include "Helper.h" +#include "../../../floorplan/v2/Floorplan.h" + +#include + + + +template class Elevators { + +private: + + /** the grid to build into */ + Grid& grid; + + /** calculation helper */ + Helper helper; + + +public: + + /** ctor */ + Elevators(Grid& grid) : grid(grid), helper(grid) { + ; + } + + ~Elevators() { + ; + } + + + /** build the given elevator */ + void build(const Floorplan::Floor* floor, const Floorplan::Elevator* elevator) { + + const int gs_cm = grid.getGridSize_cm(); + + + struct IntPos { + int x_cm; + int y_cm; + IntPos(int x_cm, int y_cm) : x_cm(x_cm), y_cm(y_cm) {;} + }; + + // identify all grid-aligned nodes that belong to the elevator + std::vector nodesWithin; + const HelperPoly poly(elevator->getPoints()); + + auto callback = [&] (const int x_cm, const int y_cm) { + + const GridPoint gp1(x_cm, y_cm, floor->getStartingZ()*100); // starting floor + const GridPoint gp2(x_cm, y_cm, floor->getEndingZ()*100); // the floor above + + // ensure such a node is present in both floors (and thus a connection is possible) + if (grid.hasNodeFor(gp1) && grid.hasNodeFor(gp2)) { + nodesWithin.push_back(IntPos(x_cm, y_cm)); + } + + }; + poly.forEachGridPoint(gs_cm, callback); + + // now create the interconnection in z-direction + const int z1_cm = std::ceil((floor->getStartingZ()*100+1) / gs_cm) * gs_cm; // the next node above the current flor + const int z2_cm = std::floor((floor->getEndingZ()*100-1) / gs_cm) * gs_cm; // the last node below the next floor + + for (const IntPos nodePos : nodesWithin) { + + // create nodes BETWEEN the two floors (skip the floors themselves! -> floor1+gridSize <-> floor2-gridSize + for (int z_cm = z1_cm; z_cm <= z2_cm; z_cm += gs_cm) { + const GridPoint gp1(nodePos.x_cm, nodePos.y_cm, z_cm); // the to-be-added node + Assert::isFalse(grid.hasNodeFor(gp1), "elevator collission"); // such a node must not yet exist! otherwise we e.g. collide with a stari + const int idx = grid.add(T(gp1.x_cm, gp1.y_cm, gp1.z_cm)); // create the node + grid[idx].setType(GridNode::TYPE_ELEVATOR); // set the node-type + } + + // connect each of the new nodes with the node below it. NOW ALSO EXAMINE THE floor above (z2_cm + gs_cm) + for (int z_cm = z1_cm; z_cm <= z2_cm + gs_cm; z_cm += gs_cm) { + const GridPoint gpBelow(nodePos.x_cm, nodePos.y_cm, z_cm-gs_cm); + const GridPoint gp(nodePos.x_cm, nodePos.y_cm, z_cm); + Assert::isTrue(grid.hasNodeFor(gpBelow), "missing node"); + Assert::isTrue(grid.hasNodeFor(gp), "missing node"); + T& n1 = (T&) grid.getNodeFor(gpBelow); + T& n2 = (T&) grid.getNodeFor(gp); + grid.connectBiDir(n1, n2); + } + + } + + + } + +}; + +#endif // GRID_ELEVATORS_H diff --git a/grid/factory/v2/GridFactory.h b/grid/factory/v2/GridFactory.h index 8568656..c8c79d5 100755 --- a/grid/factory/v2/GridFactory.h +++ b/grid/factory/v2/GridFactory.h @@ -8,7 +8,8 @@ #include "../../../floorplan/v2/Floorplan.h" #include "Helper.h" -#include "Stairs.h" +#include "Stairs2.h" +#include "Elevators.h" #include "../../../geo/Units.h" #include "../../GridNodeBBox.h" @@ -37,6 +38,9 @@ private: /** stair builder */ Stairs stairs; + /** elevator builder */ + Elevators elevators; + bool _buildStairs = true; bool _removeIsolated = true; @@ -44,7 +48,9 @@ private: public: /** ctor with the grid to fill */ - GridFactory(Grid& grid) : grid(grid), helper(grid), stairs(grid) {;} + explicit GridFactory(Grid& grid) : grid(grid), helper(grid), stairs(grid), elevators(grid) { + + } /** whether or not to build stairs */ @@ -59,7 +65,7 @@ public: Log::add(name, "building grid from IndoorMap", true); - const int total = map->floors.size()*2 + 1; + const int total = map->floors.size()*3 + 1; int cur = 0; // build all the floors @@ -78,6 +84,15 @@ public: } } + // build all elevators + if (listener) {listener->onGridBuildUpdateMajor("adding elevators");} + if (_buildStairs) { + for (Floorplan::Floor* f : map->floors) { + buildElevators(f, listener); + if (listener) {listener->onGridBuildUpdateMajor(total, ++cur);} + } + } + // remove isolated nodes if (_removeIsolated) { if (listener) {listener->onGridBuildUpdateMajor("removing isolated nodes");} @@ -180,78 +195,23 @@ public: } + void buildElevators(const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) { + + const int total = floor->elevators.size(); + int cur = 0; + + // process each elevator within the floor + for (const Floorplan::Elevator* elevator : floor->elevators) { + if (listener) {listener->onGridBuildUpdateMinor("adding " + floor->name + " elevator " + std::to_string(cur+1));} + elevators.build(floor, elevator); + 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) { diff --git a/grid/factory/v2/Helper.h b/grid/factory/v2/Helper.h index 7305b02..ebf3c50 100644 --- a/grid/factory/v2/Helper.h +++ b/grid/factory/v2/Helper.h @@ -26,7 +26,12 @@ struct HelperPoly { /** ctor from floorplan-quad */ HelperPoly(const Floorplan::Quad3& quad) { - add(quad.p1); add(quad.p2); add(quad.p3); add(quad.p4); + add(quad.p1*100); add(quad.p2*100); add(quad.p3*100); add(quad.p4*100); + } + + /** ctor from floorplan-polygon */ + HelperPoly(const Floorplan::Polygon2& poly) { + for (Point2 p : poly.points) { add(p * 100); } } void add(const Point2 p) { @@ -67,6 +72,30 @@ struct HelperPoly { } + /** call a user-function for each GRID-ALIGNED point within the polygon */ + void forEachGridPoint(const int gridSize_cm, std::function callback) const { + + int x1 = std::floor(bbox_cm.getMin().x / gridSize_cm) * gridSize_cm; + int x2 = std::ceil(bbox_cm.getMax().x / gridSize_cm) * gridSize_cm; + + int y1 = std::floor(bbox_cm.getMin().y / gridSize_cm) * gridSize_cm; + int y2 = std::ceil(bbox_cm.getMax().y / gridSize_cm) * gridSize_cm; + + // process each point within the (aligned) bbox + for (int y = y1; y <= y2; y += gridSize_cm) { + for (int x = x1; x <= x2; x += gridSize_cm) { + + // does this point belong to the polygon? + if (!contains(Point2(x,y))) {continue;} + + // call the callback + callback(x,y); + + } + } + + } + }; @@ -94,7 +123,7 @@ public: } - /** connect the given node to all its neighbors */ + /** connect the given node to all its neighbors (x,y) */ void connectToNeighbors(T& n1) { const int gs_cm = grid.getGridSize_cm(); @@ -120,6 +149,32 @@ public: } + /** connect the given node to all its neighbors )x,y,z) */ + void connectToNeighborsXYZ(T& n1) { + + const int gs_cm = grid.getGridSize_cm(); + + for (int z = -gs_cm; z <= +gs_cm; z += gs_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 && z == 0) {continue;} + + // try to find a matching neighbor + const GridPoint gp(n1.x_cm + x, n1.y_cm + y, n1.z_cm + z); + 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();; diff --git a/grid/factory/v2/Importance.h b/grid/factory/v2/Importance.h index 3e706ef..8b5f92b 100644 --- a/grid/factory/v2/Importance.h +++ b/grid/factory/v2/Importance.h @@ -72,6 +72,7 @@ public: switch(n1.getType()) { case GridNode::TYPE_DOOR: doors.push_back(n1); break; case GridNode::TYPE_STAIR: stairs.push_back(n1); break; + case GridNode::TYPE_ELEVATOR: stairs.push_back(n1); break; } } @@ -87,7 +88,7 @@ public: // probability adjustments Distribution::Normal avoidWalls(0.0, 0.35); Distribution::Normal favorDoors(0.0f, 0.5f); - Distribution::Normal favorStairs(0.0f, 3.5f); + Distribution::Normal favorStairs(0.0f, 1.5f); if (l) { l->onGridBuildUpdateMajor(2, 1); @@ -124,7 +125,7 @@ public: // final probability n1.navImportance = 1.0f; n1.navImportance += favorDoors.getProbability(distToDoor_m) * 1.25f; - n1.navImportance += favorStairs.getProbability(distToStair_m) * 2.5f; + n1.navImportance += favorStairs.getProbability(distToStair_m) * 30.5f; // use wall avoidance if (useNormal) { diff --git a/grid/factory/v2/Stairs2.h b/grid/factory/v2/Stairs2.h new file mode 100644 index 0000000..a73cdd9 --- /dev/null +++ b/grid/factory/v2/Stairs2.h @@ -0,0 +1,207 @@ +#ifndef STAIRS_H +#define STAIRS_H + +#include +#include + +#include "../../Grid.h" +#include "Helper.h" +#include "../../../floorplan/v2/Floorplan.h" + +#include +#include + +/** + * this one should be both, simpler and more robus than v1. + * - prevents duplicate (x,y) nodes + * - slightly grows the stair-quads to ensure they overlap (sometimes the do not by a few mm) + * - only connects the corener nodes between adjacent quads (seems better for most stair-situations) + * + */ +template class Stairs { + +private: + + /** the grid to build into */ + Grid& grid; + + /** calculation helper */ + Helper helper; + + // keep a list of all vertices below stairwells and remove them hereafter + std::vector toDelete; + +private: + + /** helper struct */ + struct StairNode { + const int x_cm; + const int y_cm; + const int belongsToQuadIdx; + int gridIdx = -1; + StairNode(const int x_cm, const int y_cm, const int quadIdx) : x_cm(x_cm), y_cm(y_cm), belongsToQuadIdx(quadIdx) {;} + }; + + +public: + + /** ctor */ + Stairs(Grid& grid) : grid(grid), helper(grid) { + + } + + ~Stairs() { + finalize(); + } + + + + + + void build(const Floorplan::Floor* floor, const Floorplan::Stair* stair) { + + const int gs_cm = grid.getGridSize_cm(); + + // get all of the parts for the stair + const std::vector parts = stair->getParts(); + + // convert each part to a quad. hereby, slightly grow each quad, to ensure stair-parts are connected without gaps! + const std::vector quads = Floorplan::getQuads(parts, floor, 1.05); + + std::vector stairNodes; + + // process each quad to get a list of all stair-nodes to add + for (int i = 0; i < (int) quads.size(); ++i) { + + // create a 2D polygon (ignore z) for the quad + const Floorplan::Quad3& quad = quads[i]; + HelperPoly poly(quad); + + // get a callback for each node that belongs to the quad + poly.forEachGridPoint(gs_cm, [&] (int x_cm, int y_cm) { + + const StairNode sn(x_cm, y_cm, i); + + // IMPORTANT + // skip nodes that belong to more than one quad -> skip duplicates -> much more stable! + // NOTE: currently this will kill circular stairs that are above themselves + // FIX: skip dupliactes only between adjacent quads? this should help + auto comp = [&] (const StairNode& sn1) { return sn1.x_cm == sn.x_cm && sn1.y_cm == sn.y_cm; }; + auto it = std::find_if(stairNodes.begin(), stairNodes.end(), comp); + if (it == stairNodes.end()) {stairNodes.push_back(sn);} + + }); + + } + + // add the new nodes to the grid + // nodes that are near to the two floors that stair is within, are replaced by already existing floor-nodes! + for (StairNode& sn : stairNodes) { + + // use the nodes (x,y) to reconstruct the z-value for this position using barycentric interpolation + const Point2 p(sn.x_cm, sn.y_cm); + + // the stair-quads centimeter position + const Floorplan::Quad3& quad = quads[sn.belongsToQuadIdx]; + const Point3 p1 = quad.p1 * 100; + const Point3 p2 = quad.p2 * 100; + const Point3 p3 = quad.p3 * 100; + const Point3 p4 = quad.p4 * 100; + + // get the z-value from one of the both triangles + int z_cm; + float u,v,w; + if (helper.bary(p, p1.xy(), p2.xy(), p3.xy(), u, v, w)) { + z_cm = p1.z*u + p2.z*v + p3.z*w; + } else { + helper.bary(p, p1.xy(), p3.xy(), p4.xy(), u, v, w); + z_cm = p1.z*u + p3.z*v + p4.z*w; + } + + // the to-be-added position + GridPoint gp(sn.x_cm, sn.y_cm, z_cm); + + // if a node is near an existing one (the floor above/below) use the existing one! + // this ensures the stair is connected to the floor above and below + if (grid.hasNodeFor(gp)) { + sn.gridIdx = grid.getNodeFor(gp).getIdx(); + } else { + + sn.gridIdx = grid.add(T(gp.x_cm, gp.y_cm, gp.z_cm)); + + // check if there is a nearby floor-node to delete + const int deleteDist_cm = 100; + const float distToBelow = gp.z_cm - floor->getStartingZ()*100; + const float distToAbove = floor->getEndingZ()*100 - gp.z_cm; + if (distToBelow > gs_cm && distToBelow < deleteDist_cm) { + T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getStartingZ()*100)); + if (n) {toDelete.push_back(n);} + } else if (distToAbove > gs_cm && distToAbove < deleteDist_cm) { + T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getEndingZ()*100)); + if (n) {toDelete.push_back(n);} + } + + + } + + + + + } + + // now connect all new nodes with their neighbors + // do not perform normal grid-connection but examine the nodes within the generated vector + // this allows for some additional checks/criteria to be used + for (const StairNode& sn1 : stairNodes) { + T& n1 = (T&) grid[sn1.gridIdx]; + + for (const StairNode& sn2 : stairNodes) { + + // node full? + if (n1.fullyConnected()) {continue;} + + T& n2 = (T&) grid[sn2.gridIdx]; + + // do not connect node with itself + if (n2.getIdx() == n1.getIdx()) {continue;} + + if (n1.hasNeighbor(n2.getIdx())) {continue;} + + // connect adjacent stair-nodes + // but only if their stair-parts are also adjacent + // this addresses several error-situations with round stairs (end connected to the start) , ... + const int dx = sn1.x_cm - sn2.x_cm; + const int dy = sn1.y_cm - sn2.y_cm; + const int dz = sn1.belongsToQuadIdx - sn2.belongsToQuadIdx; + + // connect + if (std::abs(dx) <= gs_cm && std::abs(dy) <= gs_cm && std::abs(dz) <= 1) { + grid.connectUniDir(n1, n2); + } + + } + + + } + + + + } + + + void finalize() { + + // delete all pending nodes and perform a cleanup + if (!toDelete.empty()) { + for (T* n : toDelete) {grid.remove(*n);} + toDelete.clear(); + grid.cleanup(); + } + + } + + + +}; + +#endif // STAIRS_H diff --git a/grid/walk/v2/modules/WalkModuleFavorZ.h b/grid/walk/v2/modules/WalkModuleFavorZ.h index e6207a3..59e01a6 100644 --- a/grid/walk/v2/modules/WalkModuleFavorZ.h +++ b/grid/walk/v2/modules/WalkModuleFavorZ.h @@ -7,15 +7,46 @@ #include "../../../../geo/Heading.h" #include "../../../../math/Distributions.h" +#include "../../../../Assertions.h" + +/** state-parameter needed for WalkModuleFavorZ */ +struct WalkStateFavorZ { + + /** nested struct to prevent name clashes */ + struct { + + /** + * 0 = up / down / stay is legal + * > 0 force states to walk upwards + * < 0 force states to walk downwards + * + * shifted towards 0 after every taken edge + * so: we force states to walk into the same z-direction for some time + */ + int zTendence = 0; + + } favorZ; + +}; + /** favor z-transitions */ template class WalkModuleFavorZ : public WalkModule { +private: + + // force states to walk into the same z-direction for 30 edges + const int keepForXEdges = 12; + + public: /** ctor */ WalkModuleFavorZ() { - ; + + // ensure the template WalkState inherits from 'WalkStateFavorZ' + StaticAssert::AinheritsB(); + } virtual void updateBefore(WalkState& state) override { @@ -30,9 +61,27 @@ public: } virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override { - (void) state; - (void) curNode; - (void) nextNode; + + // currently no walk-tendence configured + if (state.favorZ.zTendence == 0) { + + // does the taken edge indicate a z-change? + const int diff = nextNode.z_cm - curNode.z_cm; + + // if so, keep this z-direction for the next few edges to come! + if (diff != 0) { + state.favorZ.zTendence = (diff > 0) ? (+keepForXEdges) : (-keepForXEdges); + } + + // currently there IS a walk-tendence configured + } else { + + // update the tendence (shift towards 0) + if (state.favorZ.zTendence < 0) {++state.favorZ.zTendence;} + else if (state.favorZ.zTendence > 0) {--state.favorZ.zTendence;} + + } + } double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override { @@ -40,11 +89,19 @@ public: (void) state; (void) startNode; - if (curNode.z_cm != potentialNode.z_cm) { - return 40; - } else { - return 1; - } + const int tendence = state.favorZ.zTendence; + const int diff = potentialNode.z_cm - curNode.z_cm; + + // tendence available + tendence match? -> high score! + if (tendence > 0 && diff > 0) {return 0.95;} + if (tendence < 0 && diff < 0) {return 0.95;} + + // tendence available + tendence mismatch? -> very low score! + if (tendence > 0 && diff < 0) {return 0.05;} + if (tendence < 0 && diff > 0) {return 0.05;} + + // no tendence available -> just favor z-transitions over non-z-transitions + return (diff != 0) ? (0.7) : (0.3); } diff --git a/grid/walk/v2/modules/WalkModuleHeading.h b/grid/walk/v2/modules/WalkModuleHeading.h index b96bbc3..827200c 100644 --- a/grid/walk/v2/modules/WalkModuleHeading.h +++ b/grid/walk/v2/modules/WalkModuleHeading.h @@ -4,10 +4,52 @@ #include "WalkModule.h" #include "WalkStateHeading.h" +#include "../../../../Assertions.h" + #include "../../../../geo/Heading.h" #include "../../../../math/Distributions.h" +#include "../../../../geo/Heading.h" + +/** + * base-class e.g. needed for GridWalkHeading and GridWalkHeadingControl to work + */ +struct WalkStateHeading { + + /** used for better naming: heading.error instead of headingError */ + struct _Heading { + + /** + * the direction [0:2pi] the walk should move to + * e.g. indiciated by: + * compass + * integration over gyroscope values + */ + Heading direction; + + /** + * (cumulative) error between walked edges and requested direction (above). + * is used to ensure that (even though the grid contains only 45° edges) we + * approximately walk into the requested direction. + */ + float error = 0; + + /** ctor */ + _Heading(const Heading direction, const float error) : direction(direction), error(error) {;} + + } heading; + + + + + /** ctor */ + explicit WalkStateHeading(const Heading& direction, const float error) : heading(direction, error) {;} + +}; + + + /** keep the state's heading */ template class WalkModuleHeading : public WalkModule { @@ -23,7 +65,10 @@ public: /** ctor */ WalkModuleHeading() : dist(Distribution::VonMises(0.0f, 1.0f).getLUT()), draw(dist.getDrawList()) { - ; + + // ensure the template WalkState inherits from 'WalkStateHeading'! + StaticAssert::AinheritsB(); + } virtual void updateBefore(WalkState& state) override { diff --git a/grid/walk/v2/modules/WalkModuleHeadingControl.h b/grid/walk/v2/modules/WalkModuleHeadingControl.h index 772f267..319d1b1 100644 --- a/grid/walk/v2/modules/WalkModuleHeadingControl.h +++ b/grid/walk/v2/modules/WalkModuleHeadingControl.h @@ -58,6 +58,9 @@ public: (void) state; + // for elevator edges [same (x,y) but different z] do not adjust anything + if (curNode.x_cm == nextNode.x_cm && curNode.y_cm == nextNode.y_cm && curNode.z_cm != nextNode.z_cm) {return;} + // get the heading denoted by the way from curNode to nextNode const Heading head(curNode.x_cm, curNode.y_cm, nextNode.x_cm, nextNode.y_cm); @@ -80,6 +83,10 @@ public: (void) startNode; + + // for elevator edges [same (x,y) but different z] just return 1 + if (curNode.x_cm == potentialNode.x_cm && curNode.y_cm == potentialNode.y_cm && curNode.z_cm != potentialNode.z_cm) {return 1.0;} + // get the heading between curNode and potentialNode const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm); diff --git a/grid/walk/v2/modules/WalkModuleNodeImportance.h b/grid/walk/v2/modules/WalkModuleNodeImportance.h index d0abb2b..6801d98 100644 --- a/grid/walk/v2/modules/WalkModuleNodeImportance.h +++ b/grid/walk/v2/modules/WalkModuleNodeImportance.h @@ -4,6 +4,7 @@ #include "WalkModule.h" #include "WalkStateHeading.h" +#include "../../../../Assertions.h" /** * favor edges based on the importance-factor of the next node. @@ -44,7 +45,6 @@ public: (void) curNode; const double prob = potentialNode.getNavImportance(); - //return std::pow(prob, 10); return prob; } diff --git a/grid/walk/v2/modules/WalkModulePreventVisited.h b/grid/walk/v2/modules/WalkModulePreventVisited.h new file mode 100644 index 0000000..afbaaa0 --- /dev/null +++ b/grid/walk/v2/modules/WalkModulePreventVisited.h @@ -0,0 +1,77 @@ +#ifndef WALKMODULEPREVENTVISITED_H +#define WALKMODULEPREVENTVISITED_H + +#include "WalkModule.h" +#include "WalkStateHeading.h" + +#include "../../../../data/RingBuffer.h" +#include "../../../../Assertions.h" + +struct WalkStatePreventVisited { + + struct PV { + + RingBuffer history; + + PV(const int size) : history(size) {;} + + } preventVisited; + + /** ctor */ + explicit WalkStatePreventVisited(const int historySize) : preventVisited(historySize) {;} + +}; + +/** + * prevent a state from visiting nodes he has already visited + * within a certain timeframe (ringbuffer) + * + * this should avoid deadlocks in some situations where the transition + * just switched back and forth between two nodes + * + */ +template class WalkModulePreventVisited : public WalkModule { + +private: + +public: + + /** ctor */ + WalkModulePreventVisited() { + + // ensure the templated WalkState inherits from 'WalkStatePreventVisited' + StaticAssert::AinheritsB(); + + } + + + virtual void updateBefore(WalkState& state) override { + (void) state; + } + + virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override { + (void) state; + (void) startNode; + (void) endNode; + } + + virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override { + + (void) curNode; + + state.preventVisited.history.add(nextNode.getIdx()); + + } + + double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override { + + (void) startNode; + (void) curNode; + + return (state.preventVisited.history.contains(potentialNode.getIdx())) ? 0.001 : 0.999; + + } + + +}; +#endif // WALKMODULEPREVENTVISITED_H diff --git a/grid/walk/v2/modules/WalkModuleSpread.h b/grid/walk/v2/modules/WalkModuleSpread.h index 17450b4..0e933bb 100644 --- a/grid/walk/v2/modules/WalkModuleSpread.h +++ b/grid/walk/v2/modules/WalkModuleSpread.h @@ -4,6 +4,21 @@ #include "WalkModule.h" #include "WalkStateHeading.h" +#include "../../../../Assertions.h" + +/** state-parameter needed for WalkModuleSpread */ +struct WalkStateSpread { + + /** nested struct to prevent name-clashes */ + struct { + + /** keep something like a moving-average-position we want to strictly depart from */ + GridPoint departFrom; + + } spread; + +}; + /** * simply try to move away from the starting node as much as possible @@ -12,19 +27,26 @@ template class WalkModuleSpread : public Wal private: - Point3 avg; + /** + * how fast to adjust the average-position to depart from + * values between 3% and 10% seem fine + */ + const float kappa = 0.10; public: + /** ctor */ WalkModuleSpread() { - ; + + /** ensure the templated WalkState inherits from WalkStateSpread */ + StaticAssert::AinheritsB(); + } virtual void updateBefore(WalkState& state) override { (void) state; - avg = avg * 0.999 + state.position.inMeter() * 0.001; } virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override { @@ -34,24 +56,27 @@ public: } virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override { - (void) state; + (void) curNode; - (void) nextNode; + state.spread.departFrom = state.spread.departFrom * (1.0f-kappa) + nextNode * (kappa); + } double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override { - (void) state; (void) startNode; - (void) curNode; - const float dOld = avg.getDistance(curNode.inMeter()); - const float dNew = avg.getDistance(potentialNode.inMeter()); + // current distance from the depart-from position + const float dOld = state.spread.departFrom.getDistanceInCM(curNode); - if (dNew > dOld) {return 0.8;} - if (curNode.z_cm != potentialNode.z_cm) {return 0.8;} - if (dNew == dOld) {return 0.2;} - return 0; + // potential distance from the depart-from position + const float dNew = state.spread.departFrom.getDistanceInCM(potentialNode); + + // now, favor edges that depart even further from the depart-from position! + + if (dNew > dOld) {return 0.90;} // departing + if (dNew == dOld) {return 0.09;} // distance does not change + {return 0.01;} // NOT departing.. unlikely } diff --git a/grid/walk/v2/modules/WalkStateHeading.h b/grid/walk/v2/modules/WalkStateHeading.h index bade121..12519bb 100644 --- a/grid/walk/v2/modules/WalkStateHeading.h +++ b/grid/walk/v2/modules/WalkStateHeading.h @@ -1,43 +1,7 @@ #ifndef WALKSTATEHEADING_H #define WALKSTATEHEADING_H -#include "../../../../geo/Heading.h" -/** - * base-class e.g. needed for GridWalkHeading and GridWalkHeadingControl to work - */ -struct WalkStateHeading { - - /** used for better naming: heading.error instead of headingError */ - struct _Heading { - - /** - * the direction [0:2pi] the walk should move to - * e.g. indiciated by: - * compass - * integration over gyroscope values - */ - Heading direction; - - /** - * (cumulative) error between walked edges and requested direction (above). - * is used to ensure that (even though the grid contains only 45° edges) we - * approximately walk into the requested direction. - */ - float error = 0; - - /** ctor */ - _Heading(const Heading direction, const float error) : direction(direction), error(error) {;} - - } heading; - - - - - /** ctor */ - explicit WalkStateHeading(const Heading& direction, const float error) : heading(direction, error) {;} - -}; #endif // WALKSTATEHEADING_H diff --git a/main.cpp b/main.cpp index 304cd38..c3a7960 100755 --- a/main.cpp +++ b/main.cpp @@ -17,9 +17,10 @@ int main(int argc, char** argv) { ::testing::InitGoogleTest(&argc, argv); // skip all tests starting with LIVE_ - ::testing::GTEST_FLAG(filter) = "*Barometer*"; + //::testing::GTEST_FLAG(filter) = "*Barometer*"; - ::testing::GTEST_FLAG(filter) = "*Activity*"; + ::testing::GTEST_FLAG(filter) = "*Stairs*"; + //::testing::GTEST_FLAG(filter) = "*RingBuffer*"; //::testing::GTEST_FLAG(filter) = "*Grid.*"; //::testing::GTEST_FLAG(filter) = "*Dijkstra.*"; diff --git a/sensors/imu/StepDetection.h b/sensors/imu/StepDetection.h index 426d446..e33ce27 100644 --- a/sensors/imu/StepDetection.h +++ b/sensors/imu/StepDetection.h @@ -35,8 +35,8 @@ private: bool waitForUp = false; const Timestamp blockTime = Timestamp::fromMS(150); // 150-250 looks good - const float upperThreshold = +0.4f; // + is usually smaller than down (look at graphs) - const float lowerThreshold = -0.8f; + const float upperThreshold = +0.4*0.6f; // + is usually smaller than down (look at graphs) + const float lowerThreshold = -0.8*0.6f; // the 0.8 is for testing! public: diff --git a/tests/Tests.h b/tests/Tests.h index eff92ac..fbe9932 100755 --- a/tests/Tests.h +++ b/tests/Tests.h @@ -6,7 +6,8 @@ #include static inline std::string getDataFile(const std::string& name) { - return "/home/toni/Documents/programme/localization/Indoor/tests/data/" + name; + return "/mnt/data/workspaces/Indoor/tests/data/" + name; + //return "/home/toni/Documents/programme/localization/Indoor/tests/data/" + name; } diff --git a/tests/data/TestRingBuffer.cpp b/tests/data/TestRingBuffer.cpp index 2ad4816..29cc88e 100644 --- a/tests/data/TestRingBuffer.cpp +++ b/tests/data/TestRingBuffer.cpp @@ -60,6 +60,97 @@ TEST(RingBuffer, add) { } +TEST(RingBuffer, peek) { + + RingBuffer buf(4); + + buf.add(11); + ASSERT_EQ(11, buf.peek(0)); + + buf.add(13); + ASSERT_EQ(11, buf.peek(0)); + ASSERT_EQ(13, buf.peek(1)); + + buf.add(10); + ASSERT_EQ(11, buf.peek(0)); + ASSERT_EQ(13, buf.peek(1)); + ASSERT_EQ(10, buf.peek(2)); + + buf.add(17); + ASSERT_EQ(11, buf.peek(0)); + ASSERT_EQ(13, buf.peek(1)); + ASSERT_EQ(10, buf.peek(2)); + ASSERT_EQ(17, buf.peek(3)); + + buf.add(18); + ASSERT_EQ(18, buf.peek(0)); + ASSERT_EQ(13, buf.peek(1)); + ASSERT_EQ(10, buf.peek(2)); + ASSERT_EQ(17, buf.peek(3)); + + buf.add(9); + ASSERT_EQ(18, buf.peek(0)); + ASSERT_EQ( 9, buf.peek(1)); + ASSERT_EQ(10, buf.peek(2)); + ASSERT_EQ(17, buf.peek(3)); + + buf.add(88); + ASSERT_EQ(18, buf.peek(0)); + ASSERT_EQ( 9, buf.peek(1)); + ASSERT_EQ(88, buf.peek(2)); + ASSERT_EQ(17, buf.peek(3)); + + buf.add(54); + ASSERT_EQ(18, buf.peek(0)); + ASSERT_EQ( 9, buf.peek(1)); + ASSERT_EQ(88, buf.peek(2)); + ASSERT_EQ(54, buf.peek(3)); + + buf.add(1); + ASSERT_EQ( 1, buf.peek(0)); + ASSERT_EQ( 9, buf.peek(1)); + ASSERT_EQ(88, buf.peek(2)); + ASSERT_EQ(54, buf.peek(3)); + +} + +TEST(RingBuffer, contains) { + + RingBuffer buf(4); + + ASSERT_FALSE(buf.contains(0)); + buf.add(0); + ASSERT_TRUE(buf.contains(0)); + + ASSERT_FALSE(buf.contains(3)); + buf.add(3); + ASSERT_TRUE(buf.contains(0)); + ASSERT_TRUE(buf.contains(3)); + + ASSERT_FALSE(buf.contains(7)); + buf.add(7); + ASSERT_TRUE(buf.contains(0)); + ASSERT_TRUE(buf.contains(3)); + ASSERT_TRUE(buf.contains(7)); + + ASSERT_FALSE(buf.contains(8)); + buf.add(8); + ASSERT_TRUE(buf.contains(0)); + ASSERT_TRUE(buf.contains(3)); + ASSERT_TRUE(buf.contains(7)); + ASSERT_TRUE(buf.contains(8)); + + + ASSERT_FALSE(buf.contains(11)); + buf.add(11); + ASSERT_FALSE(buf.contains(0)); + ASSERT_TRUE(buf.contains(3)); + ASSERT_TRUE(buf.contains(7)); + ASSERT_TRUE(buf.contains(8)); + ASSERT_TRUE(buf.contains(11)); + +} + TEST(RingBuffer, iterator) { RingBuffer buf(4); diff --git a/tests/grid/TestStairs.cpp b/tests/grid/TestStairs.cpp new file mode 100644 index 0000000..a727007 --- /dev/null +++ b/tests/grid/TestStairs.cpp @@ -0,0 +1,99 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" + +#include "Plot.h" +#include "../../grid/factory/v2/GridFactory.h" +#include "../../grid/factory/v2/Importance.h" + +#include "../../grid/walk/v2/GridWalker.h" +#include "../../grid/walk/v2/modules/WalkModuleFavorZ.h" +#include "../../grid/walk/v2/modules/WalkModuleSpread.h" +#include "../../grid/walk/v2/modules/WalkModuleHeading.h" +#include "../../grid/walk/v2/modules/WalkModuleNodeImportance.h" +#include "../../grid/walk/v2/modules/WalkModulePreventVisited.h" + +#include "../../floorplan/v2/FloorplanReader.h" + +#include "../../data/RingBuffer.h" + + +#include +#include +#include + + +// ENSURE UNIQUE CLASS NAME +struct MyNode345092134 : public GridPoint, public GridNode { + float navImportance = 0; + float getNavImportance() const {return navImportance;} + MyNode345092134() {;} + MyNode345092134(const int x, const int y, const int z) : GridPoint(x,y,z) {;} +}; + +struct MyState102395234 : public WalkState, public WalkStateSpread, public WalkStateFavorZ { + RingBuffer history; + MyState102395234(const int x, const int y, const int z) : WalkState(GridPoint(x,y,z)), history(5) {;} +}; + +//TEST(Walk, plot) { +TEST(Stairs, live_testWalk) { + + Grid g(20); + GridFactory gf(g); + + Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(getDataFile("MapStairs.xml")); + gf.build(map, nullptr); + Importance::addImportance(g); + + K::Gnuplot gp; + K::GnuplotSplot splot; + K::GnuplotSplotElementPoints pnodes; splot.add(&pnodes); pnodes.setColorHex("#888888"); pnodes.setPointType(7); + K::GnuplotSplotElementPoints pstates; splot.add(&pstates); pstates.setColorHex("#0000ff"); pstates.setPointType(7); pstates.setPointSize(1); + + for (int i = 0; i < g.getNumNodes(); i+=2) { + const MyNode345092134& gp = g[i]; + pnodes.add(K::GnuplotPoint3(gp.x_cm, gp.y_cm, gp.z_cm)); + } + + std::vector states; + for (int i = 0; i < 5000; ++i) { + states.push_back(MyState102395234(20,20,0)); + } + + GridWalker gw; + WalkModuleFavorZ modFavorZ; + WalkModuleSpread modSpread; + WalkModuleNodeImportance modImp; + WalkModulePreventVisited modSkipDup; + //WalkModuleHeading modHead; + + gw.addModule(&modImp); + gw.addModule(&modFavorZ); + gw.addModule(&modSpread); + gw.addModule(&modSkipDup); + //gw.addModule(&modHead); + + for (int run = 0; run < 5000; ++run) { + + pstates.clear(); + for (int i = 0; i < 100; ++i) { + MyState102395234& state = states[i]; + state = gw.getDestination(g, state, 0.75); + pstates.add(K::GnuplotPoint3(state.position.x_cm, state.position.y_cm, state.position.z_cm)); + } + + gp.draw(splot); + gp.flush(); + usleep(1000*50); + + } + + + + + + +} + +#endif diff --git a/tests/sensors/pressure/TestBarometer.cpp b/tests/sensors/pressure/TestBarometer.cpp index bb5a204..5dcdfe1 100644 --- a/tests/sensors/pressure/TestBarometer.cpp +++ b/tests/sensors/pressure/TestBarometer.cpp @@ -127,7 +127,7 @@ TEST(Barometer, Activity) { //read file std::string line; - std::string filename = getDataFile("baro/logfile_UAH_R2_S3_baro.dat"); + std::string filename = getDataFile("barometer/baro1.dat"); std::ifstream infile(filename); while (std::getline(infile, line))