315 lines
7.9 KiB
C++
315 lines
7.9 KiB
C++
#ifndef WIFIRAYTRACE2D_H
|
|
#define WIFIRAYTRACE2D_H
|
|
|
|
#include "../../../geo/Point2.h"
|
|
#include "../../../geo/Line2.h"
|
|
#include "../../../geo/BBox2.h"
|
|
|
|
#include "../../../floorplan/v2/Floorplan.h"
|
|
#include "../../../floorplan/v2/FloorplanHelper.h"
|
|
|
|
#include "DataMap2.h"
|
|
#include "Ray2.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 StateRay2 : public Ray2 {
|
|
|
|
/** already travelled distance from the AP (by all previous rays */
|
|
float totalLength;
|
|
|
|
/** attenuation taken since the start */
|
|
float totalAttenuation;
|
|
|
|
void* 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;
|
|
}
|
|
|
|
};
|
|
|
|
struct Hit {
|
|
void* 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) {;}
|
|
};
|
|
|
|
|
|
|
|
class WiFiRaytrace2D {
|
|
|
|
private:
|
|
|
|
const Floorplan::Floor* floor;
|
|
BBox2 bbox;
|
|
Point2 apPos;
|
|
|
|
DataMapSignal dm;
|
|
|
|
struct Limit {
|
|
static constexpr int RAYS = 2000;
|
|
static constexpr int HITS = 11;
|
|
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);
|
|
|
|
}
|
|
|
|
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;}
|
|
//const float curLength = ray.totalLength + hit.dist;
|
|
//if (curLength > 55) {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);
|
|
|
|
}
|
|
|
|
|
|
|
|
Hit getNearestHit(const StateRay2& ray) {
|
|
|
|
Assert::isNear(1.0f, ray.dir.length(), 0.01f, "not normalized!");
|
|
|
|
const Line2 longRay(ray.start, ray.start + ray.dir*100);
|
|
|
|
const float minDist = 0;//0.01; // prevent errors hitting the same obstacle twice
|
|
const float MAX = 999999;
|
|
Hit nearest; nearest.dist = MAX;
|
|
|
|
// check intersection with walls
|
|
for (Floorplan::FloorObstacle* fo : floor->obstacles) {
|
|
|
|
// do not hit the last obstacle again
|
|
if (ray.lastObstacle == fo) {continue;}
|
|
|
|
// get the line
|
|
const Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(fo);
|
|
const Floorplan::FloorObstacleDoor* door = dynamic_cast<Floorplan::FloorObstacleDoor*>(fo);
|
|
if (!line && !door) {continue;}
|
|
Line2 obstacle;
|
|
if (line) {obstacle = Line2(line->from, line->to);}
|
|
if (door) {obstacle = Line2(door->from, door->to);}
|
|
|
|
Point2 hit;
|
|
if (obstacle.getSegmentIntersection(longRay, hit)) {
|
|
const float dist = hit.getDistance(ray.start);
|
|
if (dist > minDist && dist < nearest.dist) {
|
|
nearest.obstacle = fo;
|
|
nearest.dist = dist;
|
|
nearest.pos = hit;
|
|
nearest.normal = (obstacle.p2 - obstacle.p1).perpendicular().normalized();
|
|
nearest.material = fo->material;
|
|
}
|
|
}
|
|
}
|
|
|
|
// 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
|