FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

worked on raytracing

Browse Source
This commit is contained in:
k-a-z-u
2017-09-13 17:06:55 +02:00
parent 686151b511
commit 7eb3a16e48
3 changed files with 287 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
geo/Line2.h
View File

@@ -3,6 +3,7 @@
//#include <KLib/geo/Line.h>
#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

187
wifi/estimate/ray2d/DataMap2.h
View File

@@ -40,39 +40,14 @@ public:
DataMap(const DataMap&) = delete;
DataMap* operator = (const DataMap& o) = delete;
/*
void blured(DataMap<T>& 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<void(float,float,T)> 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<void(float,float,const T&)> 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<void(int,int,T&)> 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<int> neighbors;
};
struct DataMapSignalEntry : public DataMapNeighbors {
struct Entry {
float rssi;
@@ -213,6 +212,10 @@ struct DataMapSignalEntry {
std::vector<Entry> 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<Entry> 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<Entry> 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<DataMapSignalEntry> {
public:
@@ -242,4 +278,41 @@ public:
};
class DataMap2Factory {
public:
/** combine neighboring nodes into one */
template <typename T> static void combine(const DataMap<T>& map, DataMap<T>& 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 <typename T> static void fillGaps(const DataMap<T>& map, DataMap<T>& 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

98
wifi/estimate/ray2d/WiFiRayTrace2D.h
View File

@@ -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);