diff --git a/geo/Line2.h b/geo/Line2.h index f7e1b93..3224b6e 100755 --- a/geo/Line2.h +++ b/geo/Line2.h @@ -3,6 +3,7 @@ //#include #include "Point2.h" +#include "Ray2.h" class Line2 { @@ -141,6 +142,69 @@ public: } + + bool getSegmentIntersectionInt(const Line2& other, Point2& result) const { + + int mul = 100; + + const float p0_x = std::round(p1.x*mul), p1_x = std::round(p2.x*mul), p2_x = std::round(other.p1.x*mul), p3_x = std::round(other.p2.x*mul); + const float p0_y = std::round(p1.y*mul), p1_y = std::round(p2.y*mul), p2_y = std::round(other.p1.y*mul), p3_y = std::round(other.p2.y*mul); + + const float s1_x = p1_x - p0_x; + const float s1_y = p1_y - p0_y; + const float s2_x = p3_x - p2_x; + const float s2_y = p3_y - p2_y; + + const float s = (-s1_y * (p0_x - p2_x) + s1_x * (p0_y - p2_y)) / (-s2_x * s1_y + s1_x * s2_y); + const float t = ( s2_x * (p0_y - p2_y) - s2_y * (p0_x - p2_x)) / (-s2_x * s1_y + s1_x * s2_y); + + if (s >= 0 && s <= 1 && t >= 0 && t <= 1) { + result.x = (p0_x + (t * s1_x)) / mul; + result.y = (p0_y + (t * s1_y)) / mul; + return true; + } + + return false; + + } + + /** does the line intersect with the given ray? */ + bool intersects(const Ray2& ray, Point2& result) const { + + //https://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect/565282#565282 + + const float p0_x = p1.x, p1_x = p2.x; + const float p0_y = p1.y, p1_y = p2.y; + + const float p2_x = ray.start.x;//, p3_x = other.p2.x; + const float p2_y = ray.start.y;//, p3_y = other.p2.y; + + const float s1_x = p1_x - p0_x; + const float s1_y = p1_y - p0_y; + + const float s2_x = ray.dir.x; // p3_x - p2_x; + const float s2_y = ray.dir.y; // p3_y - p2_y; + + // ray_start + s * ray_dir + const float s = (-s1_y * (p0_x - p2_x) + s1_x * (p0_y - p2_y)) / (-s2_x * s1_y + s1_x * s2_y); + + // before the ray's start? + if (s < 0) {return false;} + + // line.p1 + t * (line.p2-line.p1) + const float t = ( s2_x * (p0_y - p2_y) - s2_y * (p0_x - p2_x)) / (-s2_x * s1_y + s1_x * s2_y); + + // t must be between 0 and 1, otherwise we are before the line's start / after the line's end + if (t < 0 || t > 1) {return false;} + + // intersection + result.x = (p0_x + (t * s1_x)); + result.y = (p0_y + (t * s1_y)); + return true; + + } + + }; #endif // LINE2D_H diff --git a/wifi/estimate/ray2d/DataMap2.h b/wifi/estimate/ray2d/DataMap2.h index 79b11d8..2331728 100644 --- a/wifi/estimate/ray2d/DataMap2.h +++ b/wifi/estimate/ray2d/DataMap2.h @@ -40,39 +40,14 @@ public: DataMap(const DataMap&) = delete; DataMap* operator = (const DataMap& o) = delete; - /* - void blured(DataMap& dst) const { - - const int s = 2; - dst.resize(this->bbox, this->gridSize_cm); - - for (int iy = 0; iy < ny; ++iy) { - for (int ix = 0; ix < nx; ++ix) { - - float valSum = 0; - int cntSum = 0; - - for (int oy = -s; oy <= +s; ++oy) { - for (int ox = -s; ox <= +s; ++ox) { - - const int x = ix+ox; - const int y = iy+oy; - - if (containsGrid(x,y)) { - valSum += getGrid(x,y); - ++cntSum; - } - - } - } - - dst.setGrid(ix, iy, valSum/cntSum); - - } - } - + T& operator [] (const int idx) { + return data[idx]; } - */ + + const T& operator [] (const int idx) const { + return data[idx]; + } + /** does the map contain the given indices? */ bool containsGrid(const int x, const int y) const { @@ -84,6 +59,10 @@ public: return bbox.contains(Point2(x_m, y_m)); } + void resize(const DataMap& other) { + resize(other.bbox, other.gridSize_cm); + } + void resize(const BBox2 bbox, const int gridSize_cm) { // cleanup @@ -122,6 +101,10 @@ public: setGrid(ix, iy, val); } + T& getRef(const int idx) const { + return data[idx]; + } + T get(const float x_m, const float y_m) const { const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm ); const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm ); @@ -148,6 +131,13 @@ public: return data[idx]; } + const T& getGridRef(const int ix, const int iy) const { + Assert::isBetween(ix, 0, nx-1, "x out of range"); + Assert::isBetween(iy, 0, ny-1, "y out of range"); + const int idx = ix + iy*nx; + return data[idx]; + } + void setGrid(const int ix, const int iy, const T val) { Assert::isBetween(ix, 0, nx-1, "x out of range"); Assert::isBetween(iy, 0, ny-1, "y out of range"); @@ -155,41 +145,43 @@ public: data[idx] = val; } - void forEach(std::function func) const { + /** convert grid indices to point coordinates */ + Point2 gridToPos(const int ix, const int iy) const { + return Point2( + (ix * gridSize_cm / 100.0f) + sx_m, + (iy * gridSize_cm / 100.0f) + sy_m + ); + } + + /** convert 1D array index to point coordinates */ + Point2 idxToPos(const int idx) const { + const int ix = idx % nx; + const int iy = idx / nx; + return gridToPos(ix, iy); + } + + /** convert 2D to 1D index */ + int getIndex(const int ix, const int iy) const { + return ix + iy*nx; + } + + void forEach(std::function func) const { for (int iy = 0; iy < ny; ++iy) { for (int ix = 0; ix < nx; ++ix) { const float x = (ix * gridSize_cm / 100.0f) + sx_m; const float y = (iy * gridSize_cm / 100.0f) + sy_m; - func(x,y,getGrid(ix, iy)); + func(x,y,getGridRef(ix, iy)); } } } - /* - void dump() { - - std::ofstream os("/tmp/1.dat"); - const float s = 1;//gridSize_cm / 100.0f; -// for (int y = 0; y < ny; ++y) { -// for (int x = 0; x < nx; ++x) { -// float rssi = data[x+y*nx]; -// rssi = (rssi == 0) ? (-100) : (rssi); -// os << (x*s) << " " << (y*s) << " " << rssi << "\n"; -// } -// os << "\n"; -// } - for (int y = 0; y < ny; ++y) { - for (int x = 0; x < nx; ++x) { - float rssi = data[x+y*nx]; - rssi = (rssi == 0) ? (-100) : (rssi); - os << rssi << " "; + void forEachGrid(std::function func) { + for (int iy = 0; iy < ny; ++iy) { + for (int ix = 0; ix < nx; ++ix) { + func(ix,iy,getGridRef(ix, iy)); } - os << "\n"; } - os.close(); - } - */ private: @@ -202,8 +194,15 @@ private: }; +struct DataMapNeighbors { -struct DataMapSignalEntry { + /** reference to all neighbors */ + std::vector neighbors; + +}; + + +struct DataMapSignalEntry : public DataMapNeighbors { struct Entry { float rssi; @@ -213,6 +212,10 @@ struct DataMapSignalEntry { std::vector entries; + void add(const DataMapSignalEntry& o) { + for (const Entry& e : o.entries) {entries.push_back(e);} + } + void add(const float rssi, const float distanceToAP) { Entry e(rssi, distanceToAP); entries.push_back(e); @@ -225,9 +228,42 @@ struct DataMapSignalEntry { return it->rssi; } + float getFirstRSSI() const { + auto comp = [] (const Entry& e1, const Entry& e2) {return e1.distanceToAP < e2.distanceToAP;}; + if (entries.empty()) {return -120;} + auto it = std::min_element(entries.begin(), entries.end(), comp); + return it->rssi; + } + + float getAvgFirst() const { + if (entries.empty()) {return -120;} + if (entries.size()==1) {return entries.front().rssi;} + std::vector copy = entries; + auto comp = [] (const Entry& e1, const Entry& e2) {return e1.rssi > e2.rssi;}; + std::sort(copy.begin(), copy.end(), comp); + + float sum = 0; + int cnt = std::min((int)copy.size(), 15); + for (int i = 0; i < cnt; ++i) { + sum += copy[i].rssi; + } + return sum/cnt; + + } + +// float get2ndMaxRSSI() const { +// if (entries.empty()) {return -120;} +// if (entries.size()==1) {return entries.front().rssi;} +// std::vector copy = entries; +// auto comp = [] (const Entry& e1, const Entry& e2) {return e1.rssi < e2.rssi;}; +// std::sort(copy.begin(), copy.end(), comp); +// return copy[copy.size()-2].rssi; +// } + }; + class DataMapSignal : public DataMap { public: @@ -242,4 +278,41 @@ public: }; + +class DataMap2Factory { + +public: + + + /** combine neighboring nodes into one */ + template static void combine(const DataMap& map, DataMap& dst) { + + auto forEach = [&] (const float, const float, const T& n) { + for (int idx : n.neighbors) { + dst[idx].add(n); + } + }; + + map.forEach(forEach); + + } + + /** fill empty fields with the values of their immediate neighbors */ + template static void fillGaps(const DataMap& map, DataMap& dst) { + + auto forEach = [&] (const float, const float, const T& n) { + if (n.entries.empty()) { + for (int idx : n.neighbors) { + dst[idx].add(n); + } + } + }; + + map.forEach(forEach); + + } + + +}; + #endif // DATAMAP2_H diff --git a/wifi/estimate/ray2d/WiFiRayTrace2D.h b/wifi/estimate/ray2d/WiFiRayTrace2D.h index 928ec4b..bc25b41 100644 --- a/wifi/estimate/ray2d/WiFiRayTrace2D.h +++ b/wifi/estimate/ray2d/WiFiRayTrace2D.h @@ -141,14 +141,97 @@ public: // usleep(1000*100); // } + + // tree.show(); tree.optimize(250); // int depth = tree.getDepth(); tree.show(1500,false); + constructNeighbors(dm); + showNeighbors(dm); + + int i = 0; + } +public: + + void showNeighbors(DataMapSignal& map) { + + static K::Gnuplot gp; + K::GnuplotPlot plot; + K::GnuplotPlotElementLines lines; plot.add(&lines); + + auto func = [&] (const int ix, const int iy, const DataMapSignalEntry& e) { + + const Point2 p1 = map.gridToPos(ix, iy); + + for (const int idx : e.neighbors) { + const Point2 p2 = map.idxToPos(idx); + + K::GnuplotPoint2 gp1(p1.x, p1.y); + K::GnuplotPoint2 gp2(p2.x, p2.y); + lines.addSegment(gp1, gp2); + + } + + }; + + map.forEachGrid(func); + + gp.draw(plot); + gp.flush(); + + int i = 0; + + + } + + /** construct neighborship relations between nodes not intersected by walls */ + void constructNeighbors(DataMapSignal& map) { + + auto func = [&] (const int ix, const int iy, DataMapSignalEntry& e) { + for (int dy = -1; dy <= +1; ++dy) { + for (int dx = -1; dx <= +1; ++dx) { + + // x/y index for the potential neighbor + const int ix2 = ix+dx; + const int iy2 = iy+dy; + + // out of bounds? + if (!map.containsGrid(ix2,iy2)) {continue;} + + // intersection test + const Point2 p1 = map.gridToPos(ix, iy); + const Point2 p2 = map.gridToPos(ix2, iy2); + const Line2 line(p1,p2); + const Point2 dir = (p2-p1).normalized(); + const Ray2 ray(p1, dir); + + bool isConnectable = true; + auto onHit = [&] (const Obstacle2D& obs) { + + if (obs.line.getSegmentIntersection(line)) {isConnectable = false;} + + }; + + tree.getHits(ray, onHit); + + if (isConnectable) { + e.neighbors.push_back(map.getIndex(ix2, iy2)); + } + + } + } + }; + + map.forEachGrid(func); + + } + + const DataMapSignal& estimate() { for (int i = 0; i < Limit::RAYS; ++i) { @@ -257,7 +340,11 @@ private: } - static inline void hitTest(const Line2& longRay, const Obstacle2D& obs, Hit& nearest) { + static inline double crossVal(const Point2 v, const Point2 w) { + return ((double)v.x*(double)w.y) - ((double)v.y*(double)w.x); + } + + static inline void hitTest(const Ray2& ray, const Obstacle2D& obs, Hit& nearest) { const float minDist = 0.01; // prevent errors hitting the same obstacle twice @@ -266,8 +353,8 @@ private: // get the line Point2 hit; - if (obs.line.getSegmentIntersection(longRay, hit)) { - const float dist = hit.getDistance(longRay.p1); + if ( obs.line.intersects(ray, hit) ) { // TODO rounding issues?! + const float dist = hit.getDistance(ray.start); if (dist > minDist && dist < nearest.dist) { nearest.obstacle = &obs; nearest.dist = dist; @@ -291,9 +378,10 @@ private: //int hits = 0; - const auto onHit = [longRay, &nearest] (const Obstacle2D& obs) { + const auto onHit = [ray, &nearest] (const Obstacle2D& obs) { //++hits; - hitTest(longRay, obs, nearest); + //hitTest(longRay, obs, nearest); + hitTest(ray, obs, nearest); }; tree.getHits(ray, onHit);