514 lines
11 KiB
C++
514 lines
11 KiB
C++
#ifndef WIFIRAYTRACE3D_H
|
|
#define WIFIRAYTRACE3D_H
|
|
|
|
#include "../../../geo/Point2.h"
|
|
#include "../../../geo/Line2.h"
|
|
#include "../../../geo/BBox2.h"
|
|
|
|
#include "../../../geo/volume/BVH.h"
|
|
#include "../../../geo/volume/BVHDebug.h"
|
|
|
|
#include "../../../floorplan/v2/Floorplan.h"
|
|
#include "../../../floorplan/v2/FloorplanHelper.h"
|
|
|
|
#include "DataMap3.h"
|
|
#include "../../../geo/Ray3.h"
|
|
#include "MaterialOptions.h"
|
|
#include "../../../floorplan/3D/Obstacle3.h"
|
|
#include "../../../floorplan/3D/Builder.h"
|
|
//#include "ObstacleTree.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
|
|
|
|
|
|
// 3D
|
|
// http://graphics.stanford.edu/courses/cs148-10-summer/docs/2006--degreve--reflection_refraction.pdf
|
|
|
|
using namespace Floorplan3D;
|
|
|
|
namespace Ray3D {
|
|
|
|
struct Intersection {
|
|
Point3 pos;
|
|
const Obstacle3D* obs;
|
|
Intersection(const Point3 pos, const Obstacle3D* obs) : pos(pos), obs(obs) {;}
|
|
};
|
|
|
|
struct StateRay3 : public Ray3 {
|
|
|
|
//std::vector<Intersection> stack;
|
|
|
|
int depth = 0;
|
|
float totalLen = 0;
|
|
float totalAttenuation = 0;
|
|
|
|
const Obstacle3D* isWithin = nullptr;
|
|
|
|
/** empty ctor */
|
|
StateRay3() {;}
|
|
|
|
/** ctor */
|
|
StateRay3(const Point3 start, const Point3 dir) : Ray3(start, dir) {
|
|
;
|
|
}
|
|
|
|
StateRay3 enter(const Point3 hitPos, const Obstacle3D* obs) const {
|
|
|
|
StateRay3 next = getNext(hitPos);
|
|
Assert::isNull(this->isWithin, "code error: isWithin");
|
|
next.totalAttenuation += Materials::get().getAttributes(obs->mat).shadowing.attenuation;
|
|
next.isWithin = obs;
|
|
return next;
|
|
|
|
}
|
|
|
|
StateRay3 leave(const Point3 hitPos, const Obstacle3D* obs) const {
|
|
|
|
(void) obs;
|
|
StateRay3 next = getNext(hitPos);
|
|
next.isWithin = nullptr;
|
|
return next;
|
|
|
|
}
|
|
|
|
StateRay3 reflectAt(const Point3 hitPos, const Obstacle3D* obs) const {
|
|
|
|
StateRay3 next = getNext(hitPos);
|
|
|
|
next.totalAttenuation += Materials::get().getAttributes(obs->mat).reflection.attenuation;
|
|
next.isWithin = nullptr; // AIR
|
|
return next;
|
|
|
|
}
|
|
|
|
private:
|
|
|
|
StateRay3 getNext(const Point3 hitPos) const {
|
|
StateRay3 next = *this;
|
|
++next.depth;
|
|
next.totalLen += (start.getDistance(hitPos));
|
|
next.start = hitPos;
|
|
return next;
|
|
}
|
|
|
|
public:
|
|
|
|
|
|
|
|
inline float getRSSI(const float addDist = 0) const {
|
|
const float txp = -40;
|
|
const float gamma = 1.2f;
|
|
return (txp - 10*gamma*std::log10(totalLen + addDist)) - totalAttenuation;
|
|
}
|
|
|
|
int getDepth() const {
|
|
//return stack.size();
|
|
return depth;
|
|
}
|
|
|
|
float getLength() const {
|
|
return totalLen;
|
|
}
|
|
|
|
};
|
|
|
|
struct Hit3 {
|
|
|
|
const Obstacle3D* obstacle;
|
|
Triangle3 obstacleTria;
|
|
|
|
float dist;
|
|
Point3 pos;
|
|
Point3 normal;
|
|
Floorplan::Material material;
|
|
bool stopHere = false;
|
|
bool invalid = false;
|
|
|
|
Hit3() {;}
|
|
Hit3(const float dist, const Point3 pos, const Point3 normal) : dist(dist), pos(pos), normal(normal) {;}
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
struct Obstacle3DWrapper {
|
|
|
|
static std::vector<Point3> getVertices(const Obstacle3D& obs) {
|
|
std::vector<Point3> pts;
|
|
for (const Triangle3& tria : obs.triangles) {
|
|
pts.push_back(tria.p1);
|
|
pts.push_back(tria.p2);
|
|
pts.push_back(tria.p3);
|
|
}
|
|
return pts;
|
|
}
|
|
|
|
static std::vector<Point3> getDebugLines(const Obstacle3D& obs) {
|
|
std::vector<Point3> pts;
|
|
for (const Triangle3& tria : obs.triangles) {
|
|
pts.push_back(tria.p1); pts.push_back(tria.p2);
|
|
pts.push_back(tria.p2); pts.push_back(tria.p3);
|
|
pts.push_back(tria.p3); pts.push_back(tria.p1);
|
|
}
|
|
return pts;
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
class WiFiRaytrace3D {
|
|
|
|
private:
|
|
|
|
BBox3 bbox;
|
|
Point3 apPos;
|
|
|
|
DataMap3Signal dm;
|
|
|
|
BVH3Debug<Obstacle3D, BoundingVolumeSphere3, Obstacle3DWrapper> tree;
|
|
|
|
struct Limit {
|
|
static constexpr int RAYS = 15000;
|
|
static constexpr int HITS = 25;
|
|
static constexpr float RSSI = -110;
|
|
};
|
|
|
|
std::vector<Point3> hitEnter;
|
|
std::vector<Point3> hitLeave;
|
|
std::vector<Point3> hitStop;
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
WiFiRaytrace3D(const Floorplan::IndoorMap* map, const int gs, const Point3 apPos) : apPos(apPos) {
|
|
|
|
// get the floor's 3D-bbox
|
|
bbox = FloorplanHelper::getBBox(map);
|
|
|
|
// allocate
|
|
dm.resize(bbox, gs);
|
|
|
|
Builder fac(map);
|
|
std::vector<Obstacle3D> obstacles = fac.getMesh().elements;
|
|
|
|
// build bounding volumes
|
|
for (Obstacle3D& obs : obstacles) {
|
|
tree.add(obs);
|
|
}
|
|
|
|
//tree.optimize();
|
|
//tree.show(500, false);
|
|
|
|
int xxx = 0; (void) xxx;
|
|
|
|
}
|
|
|
|
const std::vector<Point3>& getHitEnter() const {
|
|
return hitEnter;
|
|
}
|
|
const std::vector<Point3>& getHitLeave() const {
|
|
return hitLeave;
|
|
}
|
|
const std::vector<Point3>& getHitStop() const {
|
|
return hitStop;
|
|
}
|
|
|
|
const DataMap3Signal& estimate() {
|
|
|
|
std::minstd_rand gen;
|
|
std::uniform_real_distribution<float> dx(-1.0, +1.0);
|
|
std::uniform_real_distribution<float> dy(-1.0, +1.0);
|
|
std::uniform_real_distribution<float> dz(-1.0, +1.0);
|
|
|
|
#pragma omp parallel for
|
|
for (int i = 0; i < Limit::RAYS; ++i) {
|
|
|
|
std::cout << "ray: " << i << std::endl;
|
|
|
|
// direction
|
|
const Point3 dir = Point3(dx(gen), dy(gen), dz(gen)).normalized();
|
|
|
|
// ray
|
|
const StateRay3 ray(apPos, dir);
|
|
|
|
// run!
|
|
trace(ray);
|
|
|
|
}
|
|
|
|
return dm;
|
|
|
|
}
|
|
|
|
|
|
//#define USE_DEBUG
|
|
|
|
|
|
private:
|
|
|
|
void trace(const StateRay3& ray) {
|
|
|
|
// get the nearest intersection with the floorplan
|
|
const Hit3 nextHit = getNearestHit(ray);
|
|
|
|
// stop?
|
|
if (nextHit.invalid) {
|
|
#ifdef USE_DEBUG
|
|
hitStop.push_back(nextHit.pos);
|
|
#endif
|
|
return;
|
|
}
|
|
|
|
// rasterize the ray's way onto the map
|
|
rasterize(ray, nextHit);
|
|
|
|
// continue?
|
|
if ((nextHit.stopHere) || (ray.getRSSI(nextHit.dist) < Limit::RSSI) || (ray.getDepth() > Limit::HITS)) {
|
|
#ifdef USE_DEBUG
|
|
hitStop.push_back(nextHit.pos);
|
|
#endif
|
|
return;
|
|
}
|
|
|
|
|
|
// apply effects
|
|
if (ray.isWithin) {
|
|
leave(ray, nextHit);
|
|
#ifdef USE_DEBUG
|
|
hitLeave.push_back(nextHit.pos);
|
|
#endif
|
|
} else {
|
|
enter(ray, nextHit);
|
|
reflectAt(ray, nextHit);
|
|
#ifdef USE_DEBUG
|
|
hitEnter.push_back(nextHit.pos);
|
|
#endif
|
|
}
|
|
|
|
|
|
}
|
|
|
|
static inline float getAttenuation(const Hit3& h) {
|
|
return Materials::get().getAttributes(h.material).shadowing.attenuation;
|
|
}
|
|
|
|
static inline float getAttenuationForReflection(const Hit3& h) {
|
|
return Materials::get().getAttributes(h.material).reflection.attenuation;
|
|
}
|
|
|
|
|
|
/** perform reflection and continue tracing */
|
|
void leave(const StateRay3& ray, const Hit3& hit) {
|
|
|
|
// continue into the same direction
|
|
StateRay3 next = ray.leave(hit.pos, hit.obstacle);
|
|
|
|
// continue
|
|
trace(next);
|
|
|
|
}
|
|
|
|
/** perform reflection and continue tracing */
|
|
void enter(const StateRay3& ray, const Hit3& hit) {
|
|
|
|
// continue into the same direction
|
|
StateRay3 next = ray.enter(hit.pos, hit.obstacle);
|
|
|
|
// continue
|
|
trace(next);
|
|
|
|
}
|
|
|
|
|
|
/** perform reflection and continue tracing */
|
|
void reflectAt(const StateRay3& ray, const Hit3& hit) {
|
|
|
|
if (hit.normal.length() < 0.9) {
|
|
int i = 0; (void) i;
|
|
}
|
|
|
|
Assert::isNear(1.0f, ray.dir.length(), 0.01f, "ray's direction is not normalized");
|
|
Assert::isNear(1.0f, hit.normal.length(), 0.01f, "obstacle's normal is not normalized");
|
|
|
|
// angle to wide or narrow? -> skip
|
|
const float d = std::abs(dot(ray.dir, hit.normal));
|
|
if (d < 0.01) {return;} // parallel
|
|
if (d > 0.99) {return;} // perpendicular;
|
|
|
|
//static std::minstd_rand gen;
|
|
//static std::normal_distribution<float> dist(0, 0.02);
|
|
//const float mod = dist(gen);
|
|
|
|
// reflected ray direction using surface normal
|
|
const Point3 dir = ray.dir;
|
|
const Point3 normal = hit.normal;
|
|
const Point3 refDir = dir - (normal * 2 * dot(dir, normal));
|
|
|
|
// reflected ray;
|
|
StateRay3 reflected = ray.reflectAt(hit.pos, hit.obstacle);
|
|
reflected.dir = refDir.normalized();
|
|
|
|
// continue
|
|
trace(reflected);
|
|
|
|
}
|
|
|
|
|
|
|
|
void hitTest(const StateRay3& ray, const Obstacle3D& obs, Hit3& nearest) {
|
|
|
|
const float minDist = 0.01; // prevent errors hitting the same obstacle twice
|
|
|
|
//bool dummy = obs.boundingSphere.intersects(ray);
|
|
|
|
Point3 hitPoint;
|
|
for (const Triangle3& tria : obs.triangles) {
|
|
if (tria.intersects(ray, hitPoint)) {
|
|
|
|
//if (!dummy) {
|
|
// throw Exception("issue!");
|
|
//}
|
|
|
|
const float dist = hitPoint.getDistance(ray.start);
|
|
if (dist > minDist && dist < nearest.dist) {
|
|
nearest.obstacle = &obs;
|
|
nearest.dist = dist;
|
|
nearest.pos = hitPoint;
|
|
nearest.normal = tria.getNormal();
|
|
nearest.material = obs.mat;
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
Hit3 getNearestHit(const StateRay3& ray) {
|
|
|
|
Assert::isNear(1.0f, ray.dir.length(), 0.01f, "not normalized!");
|
|
|
|
int hits = 0;
|
|
const float MAX = 999999;
|
|
Hit3 nearest; nearest.dist = MAX;
|
|
|
|
|
|
// if the ray is currently within something, its only option is to get out of it
|
|
if (ray.isWithin) {
|
|
|
|
// check intersection only with the current obstacle to get out
|
|
hitTest(ray, *ray.isWithin, nearest);
|
|
|
|
} else {
|
|
|
|
// // check intersection with all walls
|
|
// for (const Obstacle3D& obs : obstacles) {
|
|
|
|
// // fast opt-out
|
|
// //if (!obs.boundingSphere.intersects(ray)) {continue;}
|
|
|
|
// hitTest(ray, obs, nearest);
|
|
|
|
// }
|
|
|
|
|
|
auto onHit = [&] (const Obstacle3D& obs) {
|
|
++hits;
|
|
hitTest(ray, obs, nearest);
|
|
};
|
|
|
|
tree.getHits(ray, onHit);
|
|
|
|
}
|
|
|
|
// no hit with floorplan: limit to bounding-box!
|
|
if (nearest.dist == MAX) {
|
|
nearest.invalid = true;
|
|
}
|
|
|
|
//std::cout << hits << std::endl;
|
|
|
|
return nearest;
|
|
|
|
}
|
|
|
|
|
|
/** rasterize the ray (current pos -> hit) onto the map */
|
|
void rasterize(const StateRay3& ray, const Hit3& hit) {
|
|
|
|
const Point3 dir = hit.pos - ray.start;
|
|
const Point3 dirN = dir.normalized();
|
|
const Point3 step = dirN * (dm.getGridSize_cm()/100.0f) * 1.0f; // 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 Point3 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.getLength() + partLen;
|
|
|
|
// // ray stronger than current rssi?
|
|
// if (curRSSI == 0 || curRSSI < newRSSI) {
|
|
// dm.set(dst.x, dst.y, newRSSI);
|
|
// }
|
|
|
|
dm.update(dst.x, dst.y, dst.z, newRSSI, totalLen);
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
/*
|
|
Sphere3 getSphereAround(const std::vector<Triangle3>& trias) {
|
|
|
|
std::vector<Point3> pts;
|
|
for (const Triangle3& tria : trias) {
|
|
pts.push_back(tria.p1);
|
|
pts.push_back(tria.p2);
|
|
pts.push_back(tria.p3);
|
|
}
|
|
const Sphere3 sphere = Sphere3::around(pts);
|
|
|
|
// sanity assertion
|
|
for (const Point3& pt : pts) {
|
|
Assert::isTrue(sphere.contains(pt), "bounding sphere error");
|
|
}
|
|
|
|
return sphere;
|
|
|
|
}
|
|
*/
|
|
|
|
};
|
|
|
|
}
|
|
|
|
#endif // WIFIRAYTRACE3D_H
|