|
|
|
|
@@ -4,12 +4,14 @@
|
|
|
|
|
#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 "Ray2.h"
|
|
|
|
|
#include "MaterialOptions.h"
|
|
|
|
|
|
|
|
|
|
#include <random>
|
|
|
|
|
@@ -24,6 +26,38 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
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 */
|
|
|
|
|
@@ -32,7 +66,7 @@ struct StateRay2 : public Ray2 {
|
|
|
|
|
/** attenuation taken since the start */
|
|
|
|
|
float totalAttenuation;
|
|
|
|
|
|
|
|
|
|
void* lastObstacle;
|
|
|
|
|
const Obstacle2D* lastObstacle;
|
|
|
|
|
int depth = 0;
|
|
|
|
|
|
|
|
|
|
/** empty ctor */
|
|
|
|
|
@@ -51,17 +85,6 @@ struct StateRay2 : public Ray2 {
|
|
|
|
|
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
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 {
|
|
|
|
|
@@ -72,11 +95,13 @@ private:
|
|
|
|
|
BBox2 bbox;
|
|
|
|
|
Point2 apPos;
|
|
|
|
|
|
|
|
|
|
BVH2Debug<Obstacle2D, BoundingVolumeCircle2, Obstacle2DWrapper> tree;
|
|
|
|
|
|
|
|
|
|
DataMapSignal dm;
|
|
|
|
|
|
|
|
|
|
struct Limit {
|
|
|
|
|
static constexpr int RAYS = 2000;
|
|
|
|
|
static constexpr int HITS = 11;
|
|
|
|
|
static constexpr int HITS = 25;
|
|
|
|
|
static constexpr float RSSI = -110;
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
@@ -94,6 +119,34 @@ public:
|
|
|
|
|
// 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);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
const DataMapSignal& estimate() {
|
|
|
|
|
@@ -139,17 +192,13 @@ private:
|
|
|
|
|
|
|
|
|
|
// 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);
|
|
|
|
|
reflected(ray, hit);
|
|
|
|
|
shadowed(ray, hit);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static inline float getAttenuation(const Hit& h) {
|
|
|
|
|
@@ -208,43 +257,46 @@ private:
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
static inline void hitTest(const Line2& longRay, const Obstacle2D& obs, Hit& nearest) {
|
|
|
|
|
|
|
|
|
|
Hit getNearestHit(const StateRay2& ray) {
|
|
|
|
|
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.getSegmentIntersection(longRay, hit)) {
|
|
|
|
|
const float dist = hit.getDistance(longRay.p1);
|
|
|
|
|
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*100);
|
|
|
|
|
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 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) {
|
|
|
|
|
//int hits = 0;
|
|
|
|
|
|
|
|
|
|
// do not hit the last obstacle again
|
|
|
|
|
if (ray.lastObstacle == fo) {continue;}
|
|
|
|
|
const auto onHit = [longRay, &nearest] (const Obstacle2D& obs) {
|
|
|
|
|
//++hits;
|
|
|
|
|
hitTest(longRay, obs, nearest);
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
// 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;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
tree.getHits(ray, onHit);
|
|
|
|
|
|
|
|
|
|
// no hit with floorplan: limit to bounding-box!
|
|
|
|
|
if (nearest.dist == MAX) {
|
|
|
|
|
|