455 lines
11 KiB
C++
455 lines
11 KiB
C++
#ifndef WIFIRAYTRACE2D_H
|
|
#define WIFIRAYTRACE2D_H
|
|
|
|
#include "../../../geo/Point2.h"
|
|
#include "../../../geo/Line2.h"
|
|
#include "../../../geo/BBox2.h"
|
|
#include "../../../geo/Ray2.h"
|
|
|
|
#include "../../../floorplan/v2/Floorplan.h"
|
|
#include "../../../floorplan/v2/FloorplanHelper.h"
|
|
|
|
#include "../../../geo/volume/BVHDebug.h"
|
|
|
|
#include "DataMap2.h"
|
|
#include "MaterialOptions.h"
|
|
|
|
#include <random>
|
|
|
|
// http://www.realtimerendering.com/resources/RTNews/html/rtnv10n1.html#art3
|
|
// http://math.stackexchange.com/questions/13261/how-to-get-a-reflection-vector
|
|
|
|
// RADIO WAVES
|
|
// http://people.seas.harvard.edu/%7Ejones/es151/prop_models/propagation.html Indoor Wireless RF Channel
|
|
// http://www.electronics-radio.com/articles/antennas-propagation/propagation-overview/radio-em-wave-reflection.php
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
struct Obstacle2D {
|
|
Floorplan::Material mat;
|
|
Line2 line;
|
|
Obstacle2D(Floorplan::Material mat, Line2 line) : mat(mat), line(line) {;}
|
|
};
|
|
|
|
|
|
struct Obstacle2DWrapper {
|
|
static std::vector<Point2> getVertices(const Obstacle2D& obs) {
|
|
return {obs.line.p1, obs.line.p2};
|
|
}
|
|
|
|
static std::vector<Point2> getDebugLines(const Obstacle2D& obs) {
|
|
return {obs.line.p1, obs.line.p2};
|
|
}
|
|
};
|
|
|
|
struct Hit {
|
|
const Obstacle2D* obstacle;
|
|
float dist;
|
|
Point2 pos;
|
|
Point2 normal;
|
|
Floorplan::Material material;
|
|
bool stopHere = false;
|
|
Hit() {;}
|
|
Hit(const float dist, const Point2 pos, const Point2 normal) : dist(dist), pos(pos), normal(normal) {;}
|
|
};
|
|
|
|
struct StateRay2 : public Ray2 {
|
|
|
|
/** already travelled distance from the AP (by all previous rays */
|
|
float totalLength;
|
|
|
|
/** attenuation taken since the start */
|
|
float totalAttenuation;
|
|
|
|
const Obstacle2D* lastObstacle;
|
|
int depth = 0;
|
|
|
|
/** empty ctor */
|
|
StateRay2() : totalLength(NAN), totalAttenuation(NAN) {;}
|
|
|
|
/** ctor */
|
|
StateRay2(const Point2 start, const Point2 dir) : Ray2(start, dir), totalLength(0), totalAttenuation(0) {
|
|
;
|
|
}
|
|
|
|
inline float getRSSI(const float addDist = 0) const {
|
|
const float txp = -40;
|
|
const float gamma = 1.2f;
|
|
return (txp - 10*gamma*std::log10(totalLength + addDist)) - totalAttenuation;
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
class WiFiRaytrace2D {
|
|
|
|
private:
|
|
|
|
const Floorplan::Floor* floor;
|
|
BBox2 bbox;
|
|
Point2 apPos;
|
|
|
|
BVH2Debug<Obstacle2D, BoundingVolumeCircle2, Obstacle2DWrapper> tree;
|
|
|
|
DataMapSignal dm;
|
|
|
|
struct Limit {
|
|
static constexpr int RAYS = 2000;
|
|
static constexpr int HITS = 25;
|
|
static constexpr float RSSI = -110;
|
|
};
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
WiFiRaytrace2D(const Floorplan::Floor* floor, const int gs, const Point2 apPos) : floor(floor), apPos(apPos) {
|
|
|
|
// get the floor's 3D-bbox
|
|
BBox3 bb3 = FloorplanHelper::getBBox(floor);
|
|
|
|
// 2D only party
|
|
bbox = BBox2(bb3.getMin().xy(), bb3.getMax().xy());
|
|
|
|
// allocate
|
|
dm.resize(bbox, gs);
|
|
|
|
// build tree
|
|
for (Floorplan::FloorObstacle* fo : floor->obstacles) {
|
|
|
|
const Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(fo);
|
|
const Floorplan::FloorObstacleDoor* door = dynamic_cast<Floorplan::FloorObstacleDoor*>(fo);
|
|
|
|
if (line) {
|
|
Obstacle2D obs(line->material, Line2(line->from, line->to));
|
|
tree.add(obs, true);
|
|
} else if (door) {
|
|
Obstacle2D obs(door->material, Line2(door->from, door->to));
|
|
tree.add(obs, true);
|
|
}
|
|
|
|
}
|
|
|
|
// for (int i = 0; i < 200; ++i) {
|
|
// tree.optimize(1);
|
|
// tree.show(1500, false);
|
|
// 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) {
|
|
|
|
std::cout << "ray: " << i << std::endl;
|
|
|
|
// angle between starting-rays
|
|
const float angle = (float)M_PI*2.0f * i / Limit::RAYS;
|
|
|
|
// direction
|
|
const Point2 dir(std::cos(angle), std::sin(angle));
|
|
|
|
// ray
|
|
const StateRay2 ray(apPos, dir);
|
|
|
|
// run!
|
|
trace(ray);
|
|
|
|
}
|
|
|
|
return dm;
|
|
|
|
}
|
|
|
|
|
|
|
|
private:
|
|
|
|
static float dot(const Point2 p1, const Point2 p2) {
|
|
return (p1.x * p2.x) + (p1.y * p2.y);
|
|
}
|
|
|
|
void trace(const StateRay2& ray) {
|
|
|
|
|
|
// get the nearest intersection with the floorplan
|
|
const Hit hit = getNearestHit(ray);
|
|
|
|
// rasterize the ray's way onto the map
|
|
rasterize(ray, hit);
|
|
|
|
// continue?
|
|
if (hit.stopHere) {return;}
|
|
if (ray.getRSSI(hit.dist) < Limit::RSSI) {return;}
|
|
if (ray.depth > Limit::HITS) {return;}
|
|
|
|
// apply effects
|
|
reflected(ray, hit);
|
|
shadowed(ray, hit);
|
|
|
|
}
|
|
|
|
static inline float getAttenuation(const Hit& h) {
|
|
return Materials::get().getAttributes(h.material).shadowing.attenuation;
|
|
}
|
|
|
|
static inline float getAttenuationForReflection(const Hit& h) {
|
|
return Materials::get().getAttributes(h.material).reflection.attenuation;
|
|
}
|
|
|
|
/** perform reflection and continue tracing */
|
|
void shadowed(const StateRay2& ray, const Hit& hit) {
|
|
|
|
StateRay2 next = ray; // continue into the same direction
|
|
next.depth = ray.depth + 1;
|
|
next.lastObstacle = hit.obstacle;
|
|
next.start = hit.pos;
|
|
next.totalLength += hit.dist;
|
|
next.totalAttenuation += getAttenuation(hit); // contribute attenuation
|
|
|
|
trace(next);
|
|
|
|
}
|
|
|
|
/** perform reflection and continue tracing */
|
|
void reflected(const StateRay2& ray, const Hit& hit) {
|
|
|
|
Assert::isNear(1.0f, ray.dir.length(), 0.01f, "not normalized");
|
|
Assert::isNear(1.0f, hit.normal.length(), 0.01f, "not normalized");
|
|
|
|
// angle to wide or narrow? -> skip
|
|
const float d = std::abs(dot(ray.dir, hit.normal));
|
|
if (d < 0.05) {return;} // parallel
|
|
if (d > 0.95) {return;} // perpendicular;
|
|
|
|
static std::minstd_rand gen;
|
|
static std::normal_distribution<float> dist(0, 0.02);
|
|
const float mod = dist(gen);
|
|
Point2 normalMod(
|
|
hit.normal.x * std::cos(mod) - hit.normal.y * std::sin(mod),
|
|
hit.normal.y * std::cos(mod) - hit.normal.x * std::sin(mod)
|
|
);
|
|
|
|
|
|
// reflected ray;
|
|
StateRay2 reflected;
|
|
reflected.depth = ray.depth + 1;
|
|
reflected.lastObstacle = hit.obstacle;
|
|
reflected.start = hit.pos;
|
|
reflected.totalLength = ray.totalLength + hit.dist;
|
|
reflected.totalAttenuation = ray.totalAttenuation + getAttenuationForReflection(hit); // TODO
|
|
reflected.dir = (ray.dir - normalMod * (dot(ray.dir, hit.normal)) * 2).normalized(); // slight variation
|
|
|
|
trace(reflected);
|
|
|
|
}
|
|
|
|
|
|
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
|
|
|
|
// do not hit the last obstacle again
|
|
//if (ray.lastObstacle == fo) {continue;}
|
|
|
|
// get the line
|
|
Point2 hit;
|
|
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;
|
|
nearest.pos = hit;
|
|
nearest.normal = (obs.line.p2 - obs.line.p1).perpendicular().normalized();
|
|
nearest.material = obs.mat;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
Hit getNearestHit(const StateRay2& ray) const {
|
|
|
|
Assert::isNear(1.0f, ray.dir.length(), 0.01f, "not normalized!");
|
|
|
|
const Line2 longRay(ray.start, ray.start + ray.dir*1000);
|
|
|
|
//const float minDist = 0;//0.01; // prevent errors hitting the same obstacle twice
|
|
const float MAX = 999999;
|
|
Hit nearest; nearest.dist = MAX;
|
|
|
|
//int hits = 0;
|
|
|
|
const auto onHit = [ray, &nearest] (const Obstacle2D& obs) {
|
|
//++hits;
|
|
//hitTest(longRay, obs, nearest);
|
|
hitTest(ray, obs, nearest);
|
|
};
|
|
|
|
tree.getHits(ray, onHit);
|
|
|
|
// no hit with floorplan: limit to bounding-box!
|
|
if (nearest.dist == MAX) {
|
|
//const BBox2 bb( Point2(0,0), Point2(w, h) );
|
|
Point2 hit;
|
|
for (Line2 l : bbox.lines()) {
|
|
if (l.getSegmentIntersection(longRay, hit)) {
|
|
nearest.obstacle = nullptr;
|
|
nearest.dist = hit.getDistance(ray.start);
|
|
nearest.pos = hit;
|
|
nearest.normal = (l.p2 - l.p1).perpendicular().normalized();
|
|
nearest.material = Floorplan::Material::UNKNOWN;
|
|
nearest.stopHere = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
return nearest;
|
|
|
|
}
|
|
|
|
/** rasterize the ray (current pos -> hit) onto the map */
|
|
void rasterize(const StateRay2& ray, const Hit& hit) {
|
|
|
|
const Point2 dir = hit.pos - ray.start;
|
|
const Point2 dirN = dir.normalized();
|
|
const Point2 step = dirN * (dm.getGridSize_cm()/100.0f) * 0.7f; // TODO * 1.0 ??
|
|
const int steps = dir.length() / step.length();
|
|
|
|
// sanity check
|
|
// ensure the direction towards the nearest intersection is the same as the ray's direction
|
|
// otherwise the intersection-test is invalid
|
|
#ifdef WITH_ASSERTIONS
|
|
if (dir.normalized().getDistance(ray.dir) > 0.1) {
|
|
return;
|
|
std::cout << "direction to the nearest hit is not the same direction as the ray has. incorrect intersection test?!" << std::endl;
|
|
}
|
|
#endif
|
|
|
|
for (int i = 0; i <= steps; ++i) {
|
|
|
|
const Point2 dst = ray.start + (step*i);
|
|
const float partLen = ray.start.getDistance(dst);
|
|
//const float len = ray.totalLength + partLen;
|
|
|
|
// const float curRSSI = dm.get(dst.x, dst.y);
|
|
const float newRSSI = ray.getRSSI(partLen);
|
|
const float totalLen = ray.totalLength + partLen;
|
|
|
|
// // ray stronger than current rssi?
|
|
// if (curRSSI == 0 || curRSSI < newRSSI) {
|
|
// dm.set(dst.x, dst.y, newRSSI);
|
|
// }
|
|
|
|
dm.update(dst.x, dst.y, newRSSI, totalLen);
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
};
|
|
|
|
#endif // WIFIRAYTRACE2D_H
|