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

merged

Browse Source
This commit is contained in:
toni
2018-02-12 13:48:11 +01:00
parent 0fef66ac62 2a923dfabc
commit 632b1ca7a7
22 changed files with 1803 additions and 1055 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@@ -99,8 +99,8 @@ ADD_EXECUTABLE(
${SOURCES} ${SOURCES}
) )
SET(EXTRA_LIBS ${EXTRA_LIBS} nl-genl-3 nl-3) #SET(EXTRA_LIBS ${EXTRA_LIBS} nl-genl-3 nl-3)
INCLUDE_DIRECTORIES(/usr/include/libnl3/) #INCLUDE_DIRECTORIES(/usr/include/libnl3/)
#SET(EXTRA_LIBS ${EXTRA_LIBS} iw) #SET(EXTRA_LIBS ${EXTRA_LIBS} iw)
# needed external libraries # needed external libraries

48
floorplan/v2/Floorplan.h
View File

@@ -189,19 +189,41 @@ namespace Floorplan {
struct POI; struct POI;
struct Stair; struct Stair;
struct Elevator; struct Elevator;
struct GroundTruthPoint; struct GroundTruthPoint;
using FloorOutline = std::vector<FloorOutlinePolygon*>; struct FloorOutline : public std::vector<FloorOutlinePolygon*> {
using FloorObstacles = std::vector<FloorObstacle*>; bool enabled = true;
using FloorAccessPoints = std::vector<AccessPoint*>; };
using FloorBeacons = std::vector<Beacon*>; struct FloorObstacles : public std::vector<FloorObstacle*> {
using FloorFingerprintLocations = std::vector<FingerprintLocation*>; bool enabled = true;
using FloorRegions = std::vector<FloorRegion*>; };
using FloorUnderlays = std::vector<UnderlayImage*>; struct FloorAccessPoints : public std::vector<AccessPoint*> {
using FloorPOIs = std::vector<POI*>; bool enabled = true;
using FloorStairs = std::vector<Stair*>; };
using FloorElevators = std::vector<Elevator*>; struct FloorBeacons : public std::vector<Beacon*> {
using FloorGroundTruthPoints = std::vector<GroundTruthPoint*>; bool enabled = true;
};
struct FloorFingerprintLocations : public std::vector<FingerprintLocation*> {
bool enabled = true;
};
struct FloorRegions : public std::vector<FloorRegion*> {
bool enabled = true;
};
struct FloorUnderlays : public std::vector<UnderlayImage*> {
bool enabled = true;
};
struct FloorPOIs : public std::vector<POI*> {
bool enabled = true;
};
struct FloorStairs : public std::vector<Stair*> {
bool enabled = true;
};
struct FloorElevators : public std::vector<Elevator*> {
bool enabled = true;
};
struct FloorGroundTruthPoints : public std::vector<GroundTruthPoint*> {
bool enabled = true;
};
/** describes one floor within the map, starting at a given height */ /** describes one floor within the map, starting at a given height */
struct Floor { struct Floor {
@@ -567,6 +589,8 @@ namespace Floorplan {
/** describe the floorplan's location on earth */ /** describe the floorplan's location on earth */
struct EarthRegistration { struct EarthRegistration {
bool enabled = true;
/** all available correspondences: earth <-> map */ /** all available correspondences: earth <-> map */
std::vector<EarthPosMapPos*> correspondences; std::vector<EarthPosMapPos*> correspondences;

65
floorplan/v2/FloorplanReader.h
View File

@@ -137,15 +137,15 @@ namespace Floorplan {
if (std::string("pois") == n->Name()) {floor->pois = parseFloorPOIs(n);} if (std::string("pois") == n->Name()) {floor->pois = parseFloorPOIs(n);}
if (std::string("stairs") == n->Name()) {floor->stairs = parseFloorStairs(n);} if (std::string("stairs") == n->Name()) {floor->stairs = parseFloorStairs(n);}
if (std::string("elevators") == n->Name()) {floor->elevators = parseFloorElevators(n);} if (std::string("elevators") == n->Name()) {floor->elevators = parseFloorElevators(n);}
if (std::string("gtpoints") == n->Name()) {floor->gtpoints = parseFloorGroundTruthPoints(n);} if (std::string("gtpoints") == n->Name()) {floor->gtpoints = parseFloorGroundTruthPoints(n);}
} }
return floor; return floor;
} }
/** parse the <elevators> tag */ /** parse the <elevators> tag */
static std::vector<Elevator*> parseFloorElevators(const XMLElem* el) { static FloorElevators parseFloorElevators(const XMLElem* el) {
std::vector<Elevator*> vec; FloorElevators vec;
FOREACH_NODE(n, el) { FOREACH_NODE(n, el) {
if (std::string("elevator") == n->Name()) { vec.push_back(parseFloorElevator(n)); } if (std::string("elevator") == n->Name()) { vec.push_back(parseFloorElevator(n)); }
} }
@@ -153,8 +153,8 @@ namespace Floorplan {
} }
/** parse the <stairs> tag */ /** parse the <stairs> tag */
static std::vector<Stair*> parseFloorStairs(const XMLElem* el) { static FloorStairs parseFloorStairs(const XMLElem* el) {
std::vector<Stair*> vec; FloorStairs vec;
FOREACH_NODE(n, el) { FOREACH_NODE(n, el) {
if (std::string("stair") == n->Name()) { vec.push_back(parseFloorStair(n)); } if (std::string("stair") == n->Name()) { vec.push_back(parseFloorStair(n)); }
} }
@@ -207,8 +207,8 @@ namespace Floorplan {
/** parse the <pois> tag */ /** parse the <pois> tag */
static std::vector<POI*> parseFloorPOIs(const XMLElem* el) { static FloorPOIs parseFloorPOIs(const XMLElem* el) {
std::vector<POI*> vec; FloorPOIs vec;
FOREACH_NODE(n, el) { FOREACH_NODE(n, el) {
if (std::string("poi") == n->Name()) { vec.push_back(parseFloorPOI(n)); } if (std::string("poi") == n->Name()) { vec.push_back(parseFloorPOI(n)); }
} }
@@ -225,27 +225,26 @@ namespace Floorplan {
} }
/** parse the <gtpoints> tag */ /** parse the <gtpoints> tag */
static std::vector<GroundTruthPoint*> parseFloorGroundTruthPoints(const XMLElem* el) { static FloorGroundTruthPoints parseFloorGroundTruthPoints(const XMLElem* el) {
std::vector<GroundTruthPoint*> vec; FloorGroundTruthPoints vec;
FOREACH_NODE(n, el) { FOREACH_NODE(n, el) {
if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); } if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); }
} }
return vec; return vec;
} }
/** parse a <gtpoint> tag */
static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) {
GroundTruthPoint* gtp = new GroundTruthPoint();
gtp->id = el->IntAttribute("id");
gtp->pos = parsePoint3(el);
return gtp;
}
/** parse a <gtpoint> tag */
static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) {
GroundTruthPoint* gtp = new GroundTruthPoint();
gtp->id = el->IntAttribute("id");
gtp->pos = parsePoint3(el);
return gtp;
}
/** parse the <accesspoints> tag */ /** parse the <accesspoints> tag */
static std::vector<AccessPoint*> parseFloorAccessPoints(const XMLElem* el) { static FloorAccessPoints parseFloorAccessPoints(const XMLElem* el) {
std::vector<AccessPoint*> vec; FloorAccessPoints vec;
FOREACH_NODE(n, el) { FOREACH_NODE(n, el) {
if (std::string("accesspoint") == n->Name()) { vec.push_back(parseAccessPoint(n)); } if (std::string("accesspoint") == n->Name()) { vec.push_back(parseAccessPoint(n)); }
} }
@@ -317,8 +316,8 @@ namespace Floorplan {
/** parse the <beacons> tag */ /** parse the <beacons> tag */
static std::vector<Beacon*> parseFloorBeacons(const XMLElem* el) { static FloorBeacons parseFloorBeacons(const XMLElem* el) {
std::vector<Beacon*> vec; FloorBeacons vec;
FOREACH_NODE(n, el) { FOREACH_NODE(n, el) {
if (std::string("beacon") == n->Name()) { vec.push_back(parseBeacon(n)); } if (std::string("beacon") == n->Name()) { vec.push_back(parseBeacon(n)); }
} }
@@ -341,9 +340,9 @@ namespace Floorplan {
} }
/** parse <fingerprints> <location>s */ /** parse <fingerprints> <location>s */
static std::vector<FingerprintLocation*> parseFingerprintLocations(const XMLElem* el) { static FloorFingerprintLocations parseFingerprintLocations(const XMLElem* el) {
assertNode("fingerprints", el); assertNode("fingerprints", el);
std::vector<FingerprintLocation*> vec; FloorFingerprintLocations vec;
FOREACH_NODE(n, el) { FOREACH_NODE(n, el) {
if (std::string("location") == n->Name()) { vec.push_back(parseFingerprintLocation(n)); } if (std::string("location") == n->Name()) { vec.push_back(parseFingerprintLocation(n)); }
} }
@@ -363,8 +362,8 @@ namespace Floorplan {
return fpl; return fpl;
} }
static std::vector<FloorRegion*> parseFloorRegions(const XMLElem* el) { static FloorRegions parseFloorRegions(const XMLElem* el) {
std::vector<FloorRegion*> vec; FloorRegions vec;
FOREACH_NODE(n, el) { FOREACH_NODE(n, el) {
if (std::string("region") == n->Name()) { vec.push_back(parseFloorRegion(n)); } if (std::string("region") == n->Name()) { vec.push_back(parseFloorRegion(n)); }
} }
@@ -380,9 +379,9 @@ namespace Floorplan {
} }
/** parse the <obstacles> tag */ /** parse the <obstacles> tag */
static std::vector<FloorObstacle*> parseFloorObstacles(const XMLElem* el) { static FloorObstacles parseFloorObstacles(const XMLElem* el) {
assertNode("obstacles", el); assertNode("obstacles", el);
std::vector<FloorObstacle*> obstacles; FloorObstacles obstacles;
FOREACH_NODE(n, el) { FOREACH_NODE(n, el) {
// if (std::string("wall") == n->Name()) {obstacles.push_back(parseFloorObstacleWall(n));} // if (std::string("wall") == n->Name()) {obstacles.push_back(parseFloorObstacleWall(n));}
// if (std::string("door") == n->Name()) {obstacles.push_back(parseFloorObstacleDoor(n));} // if (std::string("door") == n->Name()) {obstacles.push_back(parseFloorObstacleDoor(n));}

3
grid/factory/v2/Importance.h
View File

@@ -11,7 +11,8 @@
#include "../../../misc/KNNArray.h" #include "../../../misc/KNNArray.h"
#include "../../../math/MiniMat2.h" #include "../../../math/MiniMat2.h"
#include "../../../math/Distributions.h" #include "../../../math/distribution/Normal.h"
#include "../../../math/distribution/Triangle.h"

4
main.cpp
View File

@@ -99,7 +99,7 @@ int main(int argc, char** argv) {
//::testing::GTEST_FLAG(filter) = "*Dijkstra.*"; //::testing::GTEST_FLAG(filter) = "*Dijkstra.*";
//::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*"; //::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*";
::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*";
//::testing::GTEST_FLAG(filter) = "*Offline.readWrite*"; //::testing::GTEST_FLAG(filter) = "*Offline.readWrite*";
@@ -109,7 +109,7 @@ int main(int argc, char** argv) {
//::testing::GTEST_FLAG(filter) = "*Matrix4*"; //::testing::GTEST_FLAG(filter) = "*Matrix4*";
//::testing::GTEST_FLAG(filter) = "*Sphere3*"; //::testing::GTEST_FLAG(filter) = "*Sphere3*";
//::testing::GTEST_FLAG(filter) = "NavMeshD*"; ::testing::GTEST_FLAG(filter) = "Ray.ModelFac*";
//::testing::GTEST_FLAG(filter) = "Timestamp*"; //::testing::GTEST_FLAG(filter) = "Timestamp*";
//::testing::GTEST_FLAG(filter) = "*RayTrace3*"; //::testing::GTEST_FLAG(filter) = "*RayTrace3*";

94
math/stats/Histogram.h Normal file
View File

@@ -0,0 +1,94 @@
#ifndef STATS_HISTOGRAM_H
#define STATS_HISTOGRAM_H
#define WITH_DEBUG_PLOT
#ifdef WITH_DEBUG_PLOT
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
#endif
#include <vector>
namespace Stats {
template <typename Scalar> class Histogram {
#ifdef WITH_DEBUG_PLOT
K::Gnuplot gp;
K::GnuplotPlot plot;
K::GnuplotPlotElementLines lines;
#endif
Scalar min;
Scalar max;
int bins;
int cnt = 0;
std::vector<Scalar> counts;
public:
/** ctor */
Histogram(Scalar min, Scalar max, int bins) : min(min), max(max), bins(bins) {
clear();
#ifdef WITH_DEBUG_PLOT
plot.add(&lines);
#endif
}
int count() const {
return cnt;
}
void add(const Scalar x) {
const int idx = binIdx(x);
counts.at(idx) += 1;
++cnt;
if (cnt % 200 == 0) {showPlot();}
}
void clear() {
counts.clear();
counts.resize(bins);
cnt = 0;
}
#ifdef WITH_DEBUG_PLOT
void showPlot() {
lines.clear();
lines.add(K::GnuplotPoint2(-1,0));
for (size_t idx = 0; idx < counts.size(); ++idx) {
const Scalar val = binValue(idx);
const Scalar sum = counts[idx];
const K::GnuplotPoint2 gp2(val, sum);
lines.add(gp2);
}
gp.draw(plot);
gp.flush();
}
#endif
private:
int binIdx(const Scalar val) const {
return (val - min) / (max-min) * bins;
}
Scalar binValue(const int idx) {
return idx * (max-min) + min;
}
};
}
#endif // STATS_HISTOGRAM_H

32
navMesh/NavMeshRandom.h
View File

@@ -6,6 +6,7 @@
#include "../math/DrawList.h" #include "../math/DrawList.h"
#include "../geo/Point3.h" #include "../geo/Point3.h"
#include "../misc/PerfCheck.h" #include "../misc/PerfCheck.h"
#include "../math/stats/Histogram.h"
#include "NavMeshLocation.h" #include "NavMeshLocation.h"
@@ -20,27 +21,22 @@ namespace NM {
template <typename Tria> class NavMeshRandom { template <typename Tria> class NavMeshRandom {
DrawList<size_t> lst; DrawList<size_t> lst;
std::minstd_rand gen;
std::uniform_real_distribution<float> dOnTriangle = std::uniform_real_distribution<float>(0.0f, 1.0f); std::uniform_real_distribution<float> dOnTriangle = std::uniform_real_distribution<float>(0.0f, 1.0f);
std::uniform_real_distribution<float> dHeading = std::uniform_real_distribution<float>(0, M_PI*2); std::uniform_real_distribution<float> dHeading = std::uniform_real_distribution<float>(0, M_PI*2);
std::vector<const Tria*> triangles; std::vector<const Tria*> triangles;
uint32_t nextSeed() { uint32_t nextSeed() {
static uint32_t seed = 0; static uint32_t seed = 0;
return ++seed; return seed += 13;
} }
public: public:
/** ctor (const/non-const using T) */ /** ctor (const/non-const using T) */
template <typename T> NavMeshRandom(const std::vector<T*>& srcTriangles) : lst(nextSeed()), gen(nextSeed()) { template <typename T> NavMeshRandom(const std::vector<T*>& srcTriangles) : lst(nextSeed()) {
Assert::isFalse(srcTriangles.empty(), "no triangles given. mesh is empty"); Assert::isFalse(srcTriangles.empty(), "no triangles given. mesh is empty");
// 1st = almost always the same number?!
gen(); gen();
// construct a DrawList (probability = size[area] of the triangle // construct a DrawList (probability = size[area] of the triangle
// bigger triangles must be choosen more often // bigger triangles must be choosen more often
for (size_t idx = 0; idx < srcTriangles.size(); ++idx) { for (size_t idx = 0; idx < srcTriangles.size(); ++idx) {
@@ -53,6 +49,9 @@ namespace NM {
/** draw a random point */ /** draw a random point */
NavMeshLocation<Tria> draw() { NavMeshLocation<Tria> draw() {
// re-use to provide stable random numbers!
static std::mt19937 gen;
PERF_REGION(3, "NavMeshRandom::draw()"); PERF_REGION(3, "NavMeshRandom::draw()");
// pick a random triangle to draw from // pick a random triangle to draw from
@@ -63,6 +62,15 @@ namespace NM {
float u = dOnTriangle(gen); float u = dOnTriangle(gen);
float v = dOnTriangle(gen); float v = dOnTriangle(gen);
#ifdef WITH_DEBUG_PLOT_2
static Stats::Histogram<float> histU(0, 1, 200);
static Stats::Histogram<float> histV(0, 1, 200);
if (histU.count() > 200) {histU.showPlot(); histU.clear();}
if (histV.count() > 200) {histV.showPlot(); histV.clear();}
histU.add(u);
histV.add(v);
#endif
// if the (u,v) is outside of the triangle, mirror it so its inside the triangle again // if the (u,v) is outside of the triangle, mirror it so its inside the triangle again
if ((u+v) > 1) { if ((u+v) > 1) {
u = 1.0f - u; u = 1.0f - u;
@@ -78,15 +86,25 @@ namespace NM {
/** draw a random location within the given radius */ /** draw a random location within the given radius */
NavMeshLocation<Tria> drawWithin(const Point3 center, const float radius) { NavMeshLocation<Tria> drawWithin(const Point3 center, const float radius) {
// re-use to provide stable random numbers!
static std::mt19937 gen;
std::uniform_real_distribution<float> dDistance(0.001, radius); std::uniform_real_distribution<float> dDistance(0.001, radius);
while(true) { while(true) {
const float head = dHeading(gen); const float head = dHeading(gen);
const float dist = dDistance(gen); const float dist = dDistance(gen);
const float ox = std::cos(head) * dist; const float ox = std::cos(head) * dist;
const float oy = std::sin(head) * dist; const float oy = std::sin(head) * dist;
#ifdef WITH_DEBUG_PLOT_2
static Stats::Histogram<float> hist(0, 10, 200);
if (hist.count() > 1000) {hist.showPlot(); hist.clear();}
hist.add(dist);
#endif
// 2D destination (ignore z) // 2D destination (ignore z)
const Point2 dst(center.x + ox, center.y + oy); const Point2 dst(center.x + ox, center.y + oy);

1
navMesh/walk/NavMeshWalkParams.h
View File

@@ -20,6 +20,7 @@ namespace NM {
template <typename Tria> float inMeter(const int steps, const NavMeshLocation<Tria>& start) const { template <typename Tria> float inMeter(const int steps, const NavMeshLocation<Tria>& start) const {
Assert::isTrue(isValid(), "invalid step-sizes given"); Assert::isTrue(isValid(), "invalid step-sizes given");
Assert::isNotNull(start.tria, "no starting triangle given");
if (start.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) { if (start.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {
return stepSizeStair_m * steps; return stepSizeStair_m * steps;

5
tests/geo/TestBVH3.cpp
View File

@@ -10,6 +10,8 @@
#include "../../floorplan/v2/FloorplanReader.h" #include "../../floorplan/v2/FloorplanReader.h"
#include "../../wifi/estimate/ray3/ModelFactory.h" #include "../../wifi/estimate/ray3/ModelFactory.h"
using namespace Ray3D;
struct Wrapper { struct Wrapper {
static std::vector<Point3> getVertices(const BBox3& bbox) { static std::vector<Point3> getVertices(const BBox3& bbox) {
@@ -146,7 +148,8 @@ TEST(BVH, treeMap) {
ModelFactory fac(map); ModelFactory fac(map);
fac.setExportCeilings(false); fac.setExportCeilings(false);
fac.setFloors({map->floors[3]}); fac.setFloors({map->floors[3]});
std::vector<Obstacle3D> obs = fac.triangulize(); FloorplanMesh mesh = fac.getMesh();
std::vector<Obstacle3D> obs = mesh.elements;
BVH3Debug<Obstacle3D, BoundingVolumeSphere3, WrapperObs3D> tree; BVH3Debug<Obstacle3D, BoundingVolumeSphere3, WrapperObs3D> tree;

1
tests/ray/TestDataMap3.cpp
View File

@@ -2,6 +2,7 @@
#include "../Tests.h" #include "../Tests.h"
#include "../../wifi/estimate/ray3/DataMap3.h" #include "../../wifi/estimate/ray3/DataMap3.h"
using namespace Ray3D;
TEST(DataMap3, test) { TEST(DataMap3, test) {

13
tests/ray/TestModelFac.cpp
View File

@@ -4,17 +4,22 @@
#include "../../wifi/estimate/ray3/ModelFactory.h" #include "../../wifi/estimate/ray3/ModelFactory.h"
#include "../../floorplan/v2/FloorplanReader.h" #include "../../floorplan/v2/FloorplanReader.h"
#include <fstream> #include <fstream>
using namespace Ray3D;
TEST(Ray, ModelFac) { TEST(Ray, ModelFac) {
std::string file = "/mnt/data/workspaces/IndoorMap/maps/SHL39.xml"; //std::string file = "/apps/paper/diss/data/maps/SHL42_nm.xml";
std::string file = "/mnt/vm/paper/diss/data/maps/SHL42_nm.xml";
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file); Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file);
ModelFactory fac(map); ModelFactory fac(map);
fac.triangulize(); //fac.triangulize();
std::ofstream out("/mnt/vm/fhws.obj"); FloorplanMesh mesh = fac.getMesh();
out << fac.toOBJ() << std::endl;
std::ofstream out("/tmp/fhws.ply");
out << mesh.toPLY() << std::endl;
out.close(); out.close();
} }

3
tests/ray/TestRayTrace3.cpp
View File

@@ -4,6 +4,7 @@
#include "../../wifi/estimate/ray3/WifiRayTrace3D.h" #include "../../wifi/estimate/ray3/WifiRayTrace3D.h"
#include "../../floorplan/v2/FloorplanReader.h" #include "../../floorplan/v2/FloorplanReader.h"
#include <fstream> #include <fstream>
using namespace Ray3D;
TEST(RayTrace3, test) { TEST(RayTrace3, test) {
@@ -19,7 +20,7 @@ TEST(RayTrace3, test) {
ModelFactory fac(map); ModelFactory fac(map);
std::ofstream outOBJ("/tmp/vm/map.obj"); std::ofstream outOBJ("/tmp/vm/map.obj");
outOBJ << fac.toOBJ(); outOBJ << fac.getMesh().toOBJ();
outOBJ.close(); outOBJ.close();
const int gs_cm = 50; const int gs_cm = 50;

201
wifi/estimate/ray3/Cube.h
View File

@@ -1,141 +1,116 @@
#ifndef QUBE_H #ifndef QUBE_H
#define QUBE_H #define QUBE_H
#include <vector>
#include "../../../geo/Triangle3.h"
#include "../../../math/Matrix4.h" #include "../../../math/Matrix4.h"
#include "Mesh.h"
class Cube { namespace Ray3D {
private: class Cube : public Mesh {
std::vector<Triangle3> trias; public:
public:
/** ctor */
Cube() {
unitCube(true);
}
/** ctor with position, size and rotation */
Cube(Point3 pos, Point3 size, Point3 rot_deg, const bool topAndBottom = true) {
unitCube(topAndBottom);
const Matrix4 mRot = Matrix4::getRotationDeg(rot_deg.x, rot_deg.y, rot_deg.z);
const Matrix4 mSize = Matrix4::getScale(size.x, size.y, size.z);
const Matrix4 mPos = Matrix4::getTranslation(pos.x, pos.y, pos.z);
const Matrix4 mat = mPos * mRot * mSize;
transform(mat);
}
/** get the cube's triangles */
const std::vector<Triangle3> getTriangles() const {
return trias;
}
void transform(const Matrix4& mat) {
for (Triangle3& tria : trias) {
Vector4 v1(tria.p1.x, tria.p1.y, tria.p1.z, 1);
Vector4 v2(tria.p2.x, tria.p2.y, tria.p2.z, 1);
Vector4 v3(tria.p3.x, tria.p3.y, tria.p3.z, 1);
v1 = mat*v1;
v2 = mat*v2;
v3 = mat*v3;
tria.p1 = Point3(v1.x, v1.y, v1.z);
tria.p2 = Point3(v2.x, v2.y, v2.z);
tria.p3 = Point3(v3.x, v3.y, v3.z);
/** ctor */
Cube() {
unitCube(true);
} }
} /** ctor with position, size and rotation */
Cube(const Point3 pos, const Point3 size, const Point3 rot_deg, const bool topAndBottom = true) {
/** get a transformed version */ unitCube(topAndBottom);
Cube transformed(const Matrix4& mat) const { transform(pos, size, rot_deg);
Cube res = *this;
res.transform(mat);
return res;
}
private:
/** build unit-cube faces */
void unitCube(const bool topAndBottom) {
const float s = 1.0f;
// left?
addQuad(
Point3(+s, -s, -s),
Point3(+s, -s, +s),
Point3(-s, -s, +s),
Point3(-s, -s, -s)
);
// right?
addQuad(
Point3(-s, +s, -s),
Point3(-s, +s, +s),
Point3(+s, +s, +s),
Point3(+s, +s, -s)
);
// small side
if (1 == 1) {
// front
addQuad(
Point3(-s, -s, -s),
Point3(-s, -s, +s),
Point3(-s, +s, +s),
Point3(-s, +s, -s)
);
// read
addQuad(
Point3(+s, +s, -s),
Point3(+s, +s, +s),
Point3(+s, -s, +s),
Point3(+s, -s, -s)
);
} }
/** get a transformed version */
Cube transformed(const Matrix4& mat) const {
Cube res = *this;
res.transform(mat);
return res;
}
if (topAndBottom) { /** get a transformed version */
Cube transformed(const Point3 pos, const Point3 size, const Point3 rot_deg) const {
Cube res = *this;
res.transform(pos, size, rot_deg);
return res;
}
// top private:
addQuad(
Point3(+s, +s, +s),
Point3(-s, +s, +s),
Point3(-s, -s, +s),
Point3(+s, -s, +s)
);
// bottom /** build unit-cube faces */
void unitCube(const bool topAndBottom) {
const float s = 1.0f;
// left?
addQuad( addQuad(
Point3(+s, -s, -s), Point3(+s, -s, -s),
Point3(-s, -s, -s), Point3(+s, -s, +s),
Point3(-s, -s, +s),
Point3(-s, -s, -s)
);
// right?
addQuad(
Point3(-s, +s, -s), Point3(-s, +s, -s),
Point3(-s, +s, +s),
Point3(+s, +s, +s),
Point3(+s, +s, -s) Point3(+s, +s, -s)
); );
// small side
if (1 == 1) {
// front
addQuad(
Point3(-s, -s, -s),
Point3(-s, -s, +s),
Point3(-s, +s, +s),
Point3(-s, +s, -s)
);
// read
addQuad(
Point3(+s, +s, -s),
Point3(+s, +s, +s),
Point3(+s, -s, +s),
Point3(+s, -s, -s)
);
}
if (topAndBottom) {
// top
addQuad(
Point3(+s, +s, +s),
Point3(-s, +s, +s),
Point3(-s, -s, +s),
Point3(+s, -s, +s)
);
// bottom
addQuad(
Point3(+s, -s, -s),
Point3(-s, -s, -s),
Point3(-s, +s, -s),
Point3(+s, +s, -s)
);
}
} }
}
void addQuad(Point3 p1, Point3 p2, Point3 p3, Point3 p4) { };
trias.push_back( Triangle3(p1,p2,p3) );
trias.push_back( Triangle3(p1,p3,p4) );
}
}; }
#endif // QUBE_H #endif // QUBE_H

344
wifi/estimate/ray3/DataMap3.h
View File

@@ -7,229 +7,233 @@
#include <functional> #include <functional>
#include <mutex> #include <mutex>
template <typename T> class DataMap3 { namespace Ray3D {
private: template <typename T> class DataMap3 {
float sx_m; private:
float sy_m;
float sz_m;
float x_m; float sx_m;
float y_m; float sy_m;
float z_m; float sz_m;
int gridSize_cm; float x_m;
float y_m;
float z_m;
BBox3 bbox; int gridSize_cm;
int nx; BBox3 bbox;
int ny;
int nz;
T* data = nullptr; int nx;
int ny;
int nz;
public: T* data = nullptr;
/** ctor */ public:
DataMap3() {
;
}
~DataMap3() { /** ctor */
DataMap3() {
;
}
// cleanup ~DataMap3() {
cleanup();
} // cleanup
cleanup();
DataMap3(const DataMap3&) = delete; }
DataMap3* operator = (const DataMap3& o) = delete;
DataMap3(const DataMap3&) = delete;
DataMap3* operator = (const DataMap3& o) = delete;
/** does the map contain the given indices? */ /** does the map contain the given indices? */
bool containsGrid(const int x, const int y, const int z) const { bool containsGrid(const int x, const int y, const int z) const {
return (x >= 0) && (y >= 0) && (z >= 0) && return (x >= 0) && (y >= 0) && (z >= 0) &&
(x < nx) && (y < ny) && (z < nz); (x < nx) && (y < ny) && (z < nz);
} }
/** does the map contain the given coordinate? */ /** does the map contain the given coordinate? */
bool contain(const float x_m, const float y_m, const float z_m) const { bool contain(const float x_m, const float y_m, const float z_m) const {
return bbox.contains(Point3(x_m, y_m, z_m)); return bbox.contains(Point3(x_m, y_m, z_m));
} }
void resize(const BBox3 bbox, const int gridSize_cm) { void resize(const BBox3 bbox, const int gridSize_cm) {
// cleanup // cleanup
cleanup(); cleanup();
this->bbox = bbox; this->bbox = bbox;
// slightly increase to pervent out-of-bounds due to rounding // slightly increase to pervent out-of-bounds due to rounding
float buffer_m = 1; float buffer_m = 1;
// start-offset // start-offset
sx_m = bbox.getMin().x - buffer_m; sx_m = bbox.getMin().x - buffer_m;
sy_m = bbox.getMin().y - buffer_m; sy_m = bbox.getMin().y - buffer_m;
sz_m = bbox.getMin().z - buffer_m; sz_m = bbox.getMin().z - buffer_m;
// size in meter // size in meter
x_m = (bbox.getMax().x - bbox.getMin().x) + 2*buffer_m; x_m = (bbox.getMax().x - bbox.getMin().x) + 2*buffer_m;
y_m = (bbox.getMax().y - bbox.getMin().y) + 2*buffer_m; y_m = (bbox.getMax().y - bbox.getMin().y) + 2*buffer_m;
z_m = (bbox.getMax().z - bbox.getMin().z) + 2*buffer_m; z_m = (bbox.getMax().z - bbox.getMin().z) + 2*buffer_m;
// number of elements in the grid // number of elements in the grid
this->gridSize_cm = gridSize_cm; this->gridSize_cm = gridSize_cm;
nx = (x_m*100) / gridSize_cm; nx = (x_m*100) / gridSize_cm;
ny = (y_m*100) / gridSize_cm; ny = (y_m*100) / gridSize_cm;
nz = (z_m*100) / gridSize_cm; nz = (z_m*100) / gridSize_cm;
// allocate and reset all to 0.0 // allocate and reset all to 0.0
data = new T[nx*ny*nz]; data = new T[nx*ny*nz];
} }
/** get the used grid-size (in cm) */ /** get the used grid-size (in cm) */
int getGridSize_cm() const {return gridSize_cm;} int getGridSize_cm() const {return gridSize_cm;}
void set(const float x_m, const float y_m, const float z_m, const T val) { void set(const float x_m, const float y_m, const float z_m, const T val) {
const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm); const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm);
const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm); const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm);
const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm); const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm);
setGrid(ix, iy, iz, val); setGrid(ix, iy, iz, val);
} }
T get(const float x_m, const float y_m, const float z_m) const { T get(const float x_m, const float y_m, const float z_m) const {
const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm ); const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm );
const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm ); const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm );
const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm ); const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm );
return getGrid(ix, iy, iz); return getGrid(ix, iy, iz);
} }
T& getRef(const float x_m, const float y_m, const float z_m) { T& getRef(const float x_m, const float y_m, const float z_m) {
const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm ); const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm );
const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm ); const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm );
const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm ); const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm );
return getGridRef(ix, iy, iz); return getGridRef(ix, iy, iz);
} }
T getGrid(const int ix, const int iy, const int iz) const { T getGrid(const int ix, const int iy, const int iz) const {
Assert::isBetween(ix, 0, nx-1, "x out of range"); Assert::isBetween(ix, 0, nx-1, "x out of range");
Assert::isBetween(iy, 0, ny-1, "y out of range"); Assert::isBetween(iy, 0, ny-1, "y out of range");
Assert::isBetween(iz, 0, nz-1, "z out of range"); Assert::isBetween(iz, 0, nz-1, "z out of range");
const int idx = ix + iy*nx + iz*nx*ny; const int idx = ix + iy*nx + iz*nx*ny;
return data[idx]; return data[idx];
} }
T& getGridRef(const int ix, const int iy, const int iz) { T& getGridRef(const int ix, const int iy, const int iz) {
Assert::isBetween(ix, 0, nx-1, "x out of range"); Assert::isBetween(ix, 0, nx-1, "x out of range");
Assert::isBetween(iy, 0, ny-1, "y out of range"); Assert::isBetween(iy, 0, ny-1, "y out of range");
Assert::isBetween(iz, 0, nz-1, "z out of range"); Assert::isBetween(iz, 0, nz-1, "z out of range");
const int idx = ix + iy*nx + iz*nx*ny; const int idx = ix + iy*nx + iz*nx*ny;
return data[idx]; return data[idx];
} }
void setGrid(const int ix, const int iy, const int iz, const T val) { void setGrid(const int ix, const int iy, const int iz, const T val) {
Assert::isBetween(ix, 0, nx-1, "x out of range"); Assert::isBetween(ix, 0, nx-1, "x out of range");
Assert::isBetween(iy, 0, ny-1, "y out of range"); Assert::isBetween(iy, 0, ny-1, "y out of range");
Assert::isBetween(iz, 0, nz-1, "z out of range"); Assert::isBetween(iz, 0, nz-1, "z out of range");
const int idx = ix + iy*nx + iz*nx*ny; const int idx = ix + iy*nx + iz*nx*ny;
data[idx] = val; data[idx] = val;
} }
void forEach(std::function<void(float,float,float,T)> func) const { void forEach(std::function<void(float,float,float,T)> func) const {
for (int iz = 0; iz < nz; ++iz) { for (int iz = 0; iz < nz; ++iz) {
for (int iy = 0; iy < ny; ++iy) { for (int iy = 0; iy < ny; ++iy) {
for (int ix = 0; ix < nx; ++ix) { for (int ix = 0; ix < nx; ++ix) {
const float x = (ix * gridSize_cm / 100.0f) + sx_m; const float x = (ix * gridSize_cm / 100.0f) + sx_m;
const float y = (iy * gridSize_cm / 100.0f) + sy_m; const float y = (iy * gridSize_cm / 100.0f) + sy_m;
const float z = (iz * gridSize_cm / 100.0f) + sz_m; const float z = (iz * gridSize_cm / 100.0f) + sz_m;
func(x,y,z, getGrid(ix, iy, iz)); func(x,y,z, getGrid(ix, iy, iz));
}
} }
} }
} }
}
/* /*
void dump() { void dump() {
std::ofstream os("/tmp/1.dat"); std::ofstream os("/tmp/1.dat");
const float s = 1;//gridSize_cm / 100.0f; const float s = 1;//gridSize_cm / 100.0f;
// for (int y = 0; y < ny; ++y) { // for (int y = 0; y < ny; ++y) {
// for (int x = 0; x < nx; ++x) { // for (int x = 0; x < nx; ++x) {
// float rssi = data[x+y*nx]; // float rssi = data[x+y*nx];
// rssi = (rssi == 0) ? (-100) : (rssi); // rssi = (rssi == 0) ? (-100) : (rssi);
// os << (x*s) << " " << (y*s) << " " << rssi << "\n"; // os << (x*s) << " " << (y*s) << " " << rssi << "\n";
// } // }
// os << "\n"; // os << "\n";
// } // }
for (int y = 0; y < ny; ++y) { for (int y = 0; y < ny; ++y) {
for (int x = 0; x < nx; ++x) { for (int x = 0; x < nx; ++x) {
float rssi = data[x+y*nx]; float rssi = data[x+y*nx];
rssi = (rssi == 0) ? (-100) : (rssi); rssi = (rssi == 0) ? (-100) : (rssi);
os << rssi << " "; os << rssi << " ";
}
os << "\n";
} }
os << "\n"; os.close();
} }
os.close(); */
} private:
*/
private: void cleanup() {
delete[] data;
void cleanup() { data = nullptr;
delete[] data; }
data = nullptr;
}
};
struct DataMap3SignalEntry {
struct Entry {
float rssi;
float distanceToAP;
Entry(float rssi, float distanceToAP) : rssi(rssi), distanceToAP(distanceToAP) {;}
}; };
std::vector<Entry> entries;
void add(const float rssi, const float distanceToAP) {
static std::mutex mtx;
Entry e(rssi, distanceToAP);
mtx.lock();
entries.push_back(e);
mtx.unlock();
}
float getMaxRSSI() const {
auto comp = [] (const Entry& e1, const Entry& e2) {return e1.rssi < e2.rssi;};
if (entries.empty()) {return -120;}
auto it = std::max_element(entries.begin(), entries.end(), comp);
return it->rssi;
}
}; struct DataMap3SignalEntry {
class DataMap3Signal : public DataMap3<DataMap3SignalEntry> {
public:
/** update average */ struct Entry {
void update(const float x_m, const float y_m, const float z_m, const float rssi, const float distanceToAP) { float rssi;
float distanceToAP;
Entry(float rssi, float distanceToAP) : rssi(rssi), distanceToAP(distanceToAP) {;}
};
DataMap3SignalEntry& entry = getRef(x_m, y_m, z_m); std::vector<Entry> entries;
entry.add(rssi, distanceToAP);
} void add(const float rssi, const float distanceToAP) {
static std::mutex mtx;
Entry e(rssi, distanceToAP);
mtx.lock();
entries.push_back(e);
mtx.unlock();
}
}; float getMaxRSSI() const {
auto comp = [] (const Entry& e1, const Entry& e2) {return e1.rssi < e2.rssi;};
if (entries.empty()) {return -120;}
auto it = std::max_element(entries.begin(), entries.end(), comp);
return it->rssi;
}
};
class DataMap3Signal : public DataMap3<DataMap3SignalEntry> {
public:
/** update average */
void update(const float x_m, const float y_m, const float z_m, const float rssi, const float distanceToAP) {
DataMap3SignalEntry& entry = getRef(x_m, y_m, z_m);
entry.add(rssi, distanceToAP);
}
};
}
#endif // DATAMAP3_H #endif // DATAMAP3_H

156
wifi/estimate/ray3/FloorplanMesh.h Normal file
View File

@@ -0,0 +1,156 @@
#ifndef FLOORPLANMESH_H
#define FLOORPLANMESH_H
#include "Obstacle3.h"
namespace Ray3D {
/**
* meshed version of the floorplan
*/
struct FloorplanMesh {
std::vector<Obstacle3D> elements;
/** DEBUG: convert to .obj file code for exporting */
std::string toOBJ() {
int nVerts = 1;
int nObjs = 0;
std::string res;
// write each obstacle
for (const Obstacle3D& o : elements) {
// write the vertices
for (const Triangle3& t : o.triangles) {
res += "v " + std::to_string(t.p1.x) + " " + std::to_string(t.p1.y) + " " + std::to_string(t.p1.z) + "\n";
res += "v " + std::to_string(t.p2.x) + " " + std::to_string(t.p2.y) + " " + std::to_string(t.p2.z) + "\n";
res += "v " + std::to_string(t.p3.x) + " " + std::to_string(t.p3.y) + " " + std::to_string(t.p3.z) + "\n";
}
// create a new group
res += "g elem_" + std::to_string(++nObjs) + "\n";
// write the group's faces
for (size_t i = 0; i < o.triangles.size(); ++i) {
res += "f " + std::to_string(nVerts+0) + " " + std::to_string(nVerts+1) + " " + std::to_string(nVerts+2) + "\n";
nVerts += 3;
}
}
// done
return res;
}
/** convert to .ply file format */
std::string toPLY() const {
std::stringstream res;
res << "ply\n";
res << "format ascii 1.0\n";
int faces = 0;
int vertices = 0;
for (const Obstacle3D& obs : elements) {
vertices += obs.triangles.size() * 3;
faces += obs.triangles.size();
}
// material
std::vector<Material> mats = {
Material(0,128,0,255), // ground outdoor
Material(64,64,64,255), // ground outdoor
Material(255,96,96,255), // stair
Material(128,128,128,255), // concrete
Material(64,128,255,64), // glass
Material(200,200,200,255), // default
};
res << "element material " << mats.size() << "\n";
res << "property uchar red\n";
res << "property uchar green\n";
res << "property uchar blue\n";
res << "property uchar alpha\n";
res << "element vertex " << vertices << "\n";
res << "property float x\n";
res << "property float y\n";
res << "property float z\n";
res << "property float nx\n";
res << "property float ny\n";
res << "property float nz\n";
res << "property int material_index\n";
res << "property uchar red\n";
res << "property uchar green\n";
res << "property uchar blue\n";
res << "property uchar alpha\n";
res << "element face " << faces << "\n";
res << "property list uchar int vertex_indices\n";
res << "end_header\n";
for (const Material& mat : mats) {
res << mat.r << " " << mat.g << " " << mat.b << " " << mat.a << "\n";
}
for (const Obstacle3D& obs : elements) {
const int matIdx = getMaterial(obs);
const Material& mat = mats[matIdx];
for (const Triangle3& tria : obs.triangles) {
const Point3 n = cross(tria.p2-tria.p1, tria.p3-tria.p1).normalized();
res << tria.p1.x << " " << tria.p1.y << " " << tria.p1.z << " " << n.x << " " << n.y << " " << n.z << " " << matIdx << " " << mat.r << " " << mat.g << " " << mat.b << " " << mat.a << "\n";
res << tria.p2.x << " " << tria.p2.y << " " << tria.p2.z << " " << n.x << " " << n.y << " " << n.z << " " << matIdx << " " << mat.r << " " << mat.g << " " << mat.b << " " << mat.a <<"\n";
res << tria.p3.x << " " << tria.p3.y << " " << tria.p3.z << " " << n.x << " " << n.y << " " << n.z << " " << matIdx << " " << mat.r << " " << mat.g << " " << mat.b << " " << mat.a <<"\n";
}
}
int vidx = 0;
for (const Obstacle3D& obs : elements) {
for (const Triangle3& tria : obs.triangles) {
(void) tria;
res << "3 " << vidx++ << " " << vidx++ << " " << vidx++ << "\n";
}
}
// done
return res.str();
}
struct Material {
int r, g, b, a;
Material(int r, int g, int b, int a) : r(r), g(g), b(b), a(a) {;}
};
int getMaterial(const Obstacle3D& o) const {
if (o.type == Obstacle3D::Type::GROUND_OUTDOOR) {return 0;}
if (o.type == Obstacle3D::Type::GROUND_INDOOR) {return 1;}
if (o.type == Obstacle3D::Type::STAIR) {return 2;}
if (o.mat == Floorplan::Material::CONCRETE) {return 3;}
if (o.mat == Floorplan::Material::GLASS) {return 4;}
return 5;
}
// Color getColor(const Obstacle3D& o) const {
// if (o.type == Obstacle3D::Type::GROUND_OUTDOOR) {return Color(0,128,0,255);}
// if (o.type == Obstacle3D::Type::GROUND_INDOOR) {return Color(64,64,64,255);}
// if (o.mat == Floorplan::Material::CONCRETE) {return Color(128,128,128,255);}
// if (o.mat == Floorplan::Material::GLASS) {return Color(128,128,255,64);}
// return Color(200,200,200,255);
// }
};
}
#endif // FLOORPLANMESH_H

84
wifi/estimate/ray3/MaterialOptions.h
View File

@@ -5,60 +5,64 @@
#include "../../../floorplan/v2/Floorplan.h" #include "../../../floorplan/v2/Floorplan.h"
#include "../../../Assertions.h" #include "../../../Assertions.h"
/** raytracing attributes for one material */ namespace Ray3D {
struct MaterialAttributes {
struct Shadowing { /** raytracing attributes for one material */
float attenuation; struct MaterialAttributes {
Shadowing() : attenuation(NAN) {;}
Shadowing(float attenuation) : attenuation(attenuation) {;}
} shadowing;
struct Reflection { struct Shadowing {
float attenuation; float attenuation;
Reflection() : attenuation(NAN) {;} Shadowing() : attenuation(NAN) {;}
Reflection(float attenuation) : attenuation(attenuation) {;} Shadowing(float attenuation) : attenuation(attenuation) {;}
} reflection; } shadowing;
MaterialAttributes(float shadowA, float reflectA) : shadowing(shadowA), reflection(reflectA) {;} struct Reflection {
float attenuation;
Reflection() : attenuation(NAN) {;}
Reflection(float attenuation) : attenuation(attenuation) {;}
} reflection;
MaterialAttributes() {;} MaterialAttributes(float shadowA, float reflectA) : shadowing(shadowA), reflection(reflectA) {;}
}; MaterialAttributes() {;}
class Materials { };
public: class Materials {
/** singleton access */ public:
static Materials& get() {
static Materials instance;
return instance;
}
/** get the attributes for the given material */ /** singleton access */
inline const MaterialAttributes& getAttributes(const Floorplan::Material mat) const { static Materials& get() {
const int idx = (const int) mat; static Materials instance;
Assert::isBetween(idx, 0, (int)materials.size()-1, "material index out of bounds"); return instance;
return materials[idx]; }
}
private: /** get the attributes for the given material */
inline const MaterialAttributes& getAttributes(const Floorplan::Material mat) const {
const int idx = (const int) mat;
Assert::isBetween(idx, 0, (int)materials.size()-1, "material index out of bounds");
return materials[idx];
}
std::vector<MaterialAttributes> materials; private:
/** hidden ctor */ std::vector<MaterialAttributes> materials;
Materials() {
materials.resize((int)Floorplan::Material::_END); /** hidden ctor */
materials[(int)Floorplan::Material::CONCRETE] = MaterialAttributes(12, 5); Materials() {
materials[(int)Floorplan::Material::DRYWALL] = MaterialAttributes(2, 5);
materials[(int)Floorplan::Material::GLASS] = MaterialAttributes(28, 2);
materials[(int)Floorplan::Material::UNKNOWN] = MaterialAttributes(2, 5);
materials[(int)Floorplan::Material::WOOD] = MaterialAttributes(5, 5);
} materials.resize((int)Floorplan::Material::_END);
materials[(int)Floorplan::Material::CONCRETE] = MaterialAttributes(12, 5);
materials[(int)Floorplan::Material::DRYWALL] = MaterialAttributes(2, 5);
materials[(int)Floorplan::Material::GLASS] = MaterialAttributes(28, 2);
materials[(int)Floorplan::Material::UNKNOWN] = MaterialAttributes(2, 5);
materials[(int)Floorplan::Material::WOOD] = MaterialAttributes(5, 5);
}; }
};
}
#endif // MATERIALOPTIONS_H #endif // MATERIALOPTIONS_H

60
wifi/estimate/ray3/Mesh.h Normal file
View File

@@ -0,0 +1,60 @@
#ifndef RAY3D_MESH_H
#define RAY3D_MESH_H
#include <vector>
#include "../../../geo/Triangle3.h"
#include "../../../math/Matrix4.h"
namespace Ray3D {
class Mesh {
protected:
std::vector<Triangle3> trias;
public:
/** get the mesh's triangles */
const std::vector<Triangle3>& getTriangles() const {
return trias;
}
void transform(const Point3 pos, Point3 size, Point3 rot_deg) {
const Matrix4 mRot = Matrix4::getRotationDeg(rot_deg.x, rot_deg.y, rot_deg.z);
const Matrix4 mSize = Matrix4::getScale(size.x, size.y, size.z);
const Matrix4 mPos = Matrix4::getTranslation(pos.x, pos.y, pos.z);
const Matrix4 mat = mPos * mRot * mSize;
transform(mat);
}
void transform(const Matrix4& mat) {
for (Triangle3& tria : trias) {
Vector4 v1(tria.p1.x, tria.p1.y, tria.p1.z, 1);
Vector4 v2(tria.p2.x, tria.p2.y, tria.p2.z, 1);
Vector4 v3(tria.p3.x, tria.p3.y, tria.p3.z, 1);
v1 = mat*v1;
v2 = mat*v2;
v3 = mat*v3;
tria.p1 = Point3(v1.x, v1.y, v1.z);
tria.p2 = Point3(v2.x, v2.y, v2.z);
tria.p3 = Point3(v3.x, v3.y, v3.z);
}
}
void addQuad(Point3 p1, Point3 p2, Point3 p3, Point3 p4) {
trias.push_back( Triangle3(p1,p2,p3) );
trias.push_back( Triangle3(p1,p3,p4) );
}
};
}
#endif // RAY3D_MESH_H

573
wifi/estimate/ray3/ModelFactory.h
View File

@@ -6,208 +6,463 @@
#include "ModelFactoryHelper.h" #include "ModelFactoryHelper.h"
#include "Obstacle3.h" #include "Obstacle3.h"
#include "Cube.h" #include "Cube.h"
#include "Tube.h"
#include "FloorplanMesh.h"
/** namespace Ray3D {
* convert an indoor map into a 3D model based on triangles
*/
class ModelFactory {
private: /**
* convert an indoor map into a 3D model based on triangles
*/
class ModelFactory {
bool exportCeilings = true; private:
bool exportObstacles = true;
bool exportWallTops = false;
std::vector<Floorplan::Floor*> exportFloors;
const Floorplan::IndoorMap* map; bool exportCeilings = true;
bool exportObstacles = true;
bool exportStairs = true;
bool exportHandrails = true;
bool exportDoors = true;
bool doorsOpen = true;
bool exportWallTops = false;
std::vector<Floorplan::Floor*> exportFloors;
public: /** the to-be-exported map */
const Floorplan::IndoorMap* map;
public:
/** ctor */ /** ctor */
ModelFactory(const Floorplan::IndoorMap* map) : map(map) { ModelFactory(const Floorplan::IndoorMap* map) : map(map) {
} }
void setExportCeilings(bool exp) { /** whether or not to export ceilings */
this->exportCeilings = exp; void setExportCeilings(bool exp) {
} this->exportCeilings = exp;
}
/** limit to-be-exported floors */ /** limit to-be-exported floors */
void setFloors(const std::vector<Floorplan::Floor*> floors) { void setFloors(const std::vector<Floorplan::Floor*> floors) {
this->exportFloors = floors; this->exportFloors = floors;
} }
/** get all triangles grouped by obstacle */ /** convert floorplan to mesh */
std::vector<Obstacle3D> triangulize() { FloorplanMesh getMesh() {
std::vector<Obstacle3D> res; FloorplanMesh mesh;
mesh.elements = triangulize();
return mesh;
// get the to-be-exported floors (either "all" or "user defined") }
const std::vector<Floorplan::Floor*>& floors = (exportFloors.empty()) ? (map->floors) : (exportFloors);
// process each floor private:
for (const Floorplan::Floor* f : floors) {
// triangulize the floor itself (floor/ceiling)
if (exportCeilings) {res.push_back(getTriangles(f));}
// process each obstacle within the floor /** get all triangles grouped by obstacle */
for (const Floorplan::FloorObstacle* fo : f->obstacles) { std::vector<Obstacle3D> triangulize() {
// handle line obstacles std::vector<Obstacle3D> res;
const Floorplan::FloorObstacleLine* fol = dynamic_cast<const Floorplan::FloorObstacleLine*>(fo);
if (fol) { // get the to-be-exported floors (either "all" or "user defined")
if (fol->type == Floorplan::ObstacleType::HANDRAIL) {continue;} const std::vector<Floorplan::Floor*>& floors = (exportFloors.empty()) ? (map->floors) : (exportFloors);
if (exportObstacles) {res.push_back(getTriangles(f, fol));}
// process each floor
for (const Floorplan::Floor* f : floors) {
if (!f->enabled) {continue;}
// triangulize the floor itself (floor/ceiling)
if (exportCeilings) {
std::vector<Obstacle3D> tmp = getFloor(f);
res.insert(res.end(), tmp.begin(), tmp.end());
}
// process each obstacle within the floor
if (f->obstacles.enabled) {
for (const Floorplan::FloorObstacle* fo : f->obstacles) {
std::vector<Obstacle3D> tmp = getObstacle(f, fo);
res.insert(res.end(), tmp.begin(), tmp.end());
}
}
// stairs
if (f->stairs.enabled) {
for (const Floorplan::Stair* stair : f->stairs) {
if (exportStairs) {res.push_back(getStairs(f, stair));}
}
}
// TODO: remove
//break;
}
return res;
}
private:
/** convert a floor (floor/ceiling) into triangles */
std::vector<Obstacle3D> getFloor(const Floorplan::Floor* f) {
std::vector<Obstacle3D> res;
if (!f->enabled) {return res;}
if (!f->outline.enabled) {return res;}
// floor uses an outline based on "add" and "remove" areas
// we need to create the apropriate triangles to model the polygon
// including all holes (remove-areas)
// process all "add" regions by type
// [this allows for overlaps of the same type]
std::unordered_map<std::string, Polygon> types;
for (Floorplan::FloorOutlinePolygon* fop : f->outline) {
if (fop->method == Floorplan::OutlineMethod::ADD) {
if (fop->outdoor) {
types["outdoor"].add(fop->poly);
} else {
types["indoor"].add(fop->poly);
}
} }
} }
// TODO: remove // remove the "remove" regions from EVERY "add" region added within the previous step
//break; for (Floorplan::FloorOutlinePolygon* fop : f->outline) {
if (fop->method == Floorplan::OutlineMethod::REMOVE) {
for (auto& it : types) {
it.second.remove(fop->poly);
}
}
// allow for overlapping outdoor/indoor regions -> outdoor wins [remove outdoor part from indoor parts]
if (fop->outdoor) {
types["indoor"].remove(fop->poly);
}
}
// create an obstacle for each type (indoor, outdoor)
for (auto& it : types) {
// TODO: variable type?
Obstacle3D::Type type = (it.first == "indoor") ? (Obstacle3D::Type::GROUND_INDOOR) : (Obstacle3D::Type::GROUND_OUTDOOR);
Obstacle3D obs(type, Floorplan::Material::CONCRETE);
// convert them into polygons
std::vector<std::vector<Point3>> polys = it.second.get(f->getStartingZ());
// convert polygons (GL_TRIANGLE_STRIP) to triangles
for (const std::vector<Point3>& pts : polys) {
for (int i = 0; i < (int)pts.size() - 2; ++i) {
// floor must be double-sided for reflection to work with the correct normals
Triangle3 tria1 (pts[i+0], pts[i+1], pts[i+2]);
Triangle3 tria2 (pts[i+2], pts[i+1], pts[i+0]);
// ensure the triangle with the normal pointing downwards (towards bulding's cellar)
// is below the triangle that points upwards (towards the sky)
if (tria1.getNormal().z < 0) {tria1 = tria1 - Point3(0,0,0.02);}
if (tria2.getNormal().z < 0) {tria2 = tria2 - Point3(0,0,0.02);}
// add both
obs.triangles.push_back(tria1);
obs.triangles.push_back(tria2);
}
}
res.push_back(obs);
}
return res;
} }
return res; /**
* @brief build the given obstacle
* @param f the floor
* @param fo the obstacle
* @param aboveDoor whether to place this obstacle ABOVE the given door (overwrite)
* @return
*/
std::vector<Obstacle3D> getObstacle(const Floorplan::Floor* f, const Floorplan::FloorObstacle* fo, const Floorplan::FloorObstacleDoor* aboveDoor = nullptr) const {
} std::vector<Obstacle3D> res;
// handle line obstacles
/** DEBUG: convert to .obj file code for exporting */ const Floorplan::FloorObstacleLine* fol = dynamic_cast<const Floorplan::FloorObstacleLine*>(fo);
std::string toOBJ() { if (fol) {
if (exportObstacles) {
const std::vector<Obstacle3D> obs = triangulize(); if (fol->type != Floorplan::ObstacleType::HANDRAIL || exportHandrails) {
res.push_back(getObstacleLine(f, fol, aboveDoor));
int nVerts = 1; }
int nObjs = 0; }
std::string res;
// write each obstacle
for (const Obstacle3D& o : obs) {
// write the vertices
for (const Triangle3& t : o.triangles) {
res += "v " + std::to_string(t.p1.x) + " " + std::to_string(t.p1.y) + " " + std::to_string(t.p1.z) + "\n";
res += "v " + std::to_string(t.p2.x) + " " + std::to_string(t.p2.y) + " " + std::to_string(t.p2.z) + "\n";
res += "v " + std::to_string(t.p3.x) + " " + std::to_string(t.p3.y) + " " + std::to_string(t.p3.z) + "\n";
} }
// create a new group const Floorplan::FloorObstacleDoor* door = dynamic_cast<const Floorplan::FloorObstacleDoor*>(fo);
res += "g elem_" + std::to_string(++nObjs) + "\n"; if (door) {
if (exportObstacles) {
if (exportDoors) {
res.push_back(getDoor(f, door));
}
//std::vector<Obstacle3D> tmp = getDoorAbove(f, door);
//res.insert(res.end(), tmp.begin(), tmp.end());
res.push_back(getDoorAbove(f, door));
}
}
// write the group's faces return res;
for (size_t i = 0; i < o.triangles.size(); ++i) {
res += "f " + std::to_string(nVerts+0) + " " + std::to_string(nVerts+1) + " " + std::to_string(nVerts+2) + "\n"; }
nVerts += 3;
/** convert a line obstacle to 3D triangles */
Obstacle3D getObstacleLine(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor = nullptr) const {
switch (fol->type) {
case Floorplan::ObstacleType::HANDRAIL:
return getHandrail(f, fol);
case Floorplan::ObstacleType::WINDOW:
return getWindow(f, fol, aboveDoor);
case Floorplan::ObstacleType::WALL:
return getWall(f, fol, aboveDoor);
default:
throw Exception("invalid obstacle type");
} }
} }
// done Obstacle3D getWindow(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor) const {
return res; return getWall(f, fol, aboveDoor);
}
} Obstacle3D getWall(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor) const {
private: const float thickness_m = fol->thickness_m;
const Point2 from = (!aboveDoor) ? (fol->from) : (aboveDoor->from);
const Point2 to = (!aboveDoor) ? (fol->to) : (aboveDoor->to);
const Point2 cen2 = (from+to)/2;
/** convert a floor (floor/ceiling) into triangles */ const float rad = std::atan2(to.y - from.y, to.x - from.x);
Obstacle3D getTriangles(const Floorplan::Floor* f) { const float deg = rad * 180 / M_PI;
// floor uses an outline based on "add" and "remove" areas // cube's destination center
// we need to create the apropriate triangles to model the polygon const float cenZ = (!aboveDoor) ? (f->atHeight + f->height/2) : (f->getEndingZ() - (f->height - aboveDoor->height) / 2);
// including all holes (remove-areas) const float height = (!aboveDoor) ? (f->height) : (f->height - aboveDoor->height);
const Point3 pos(cen2.x, cen2.y, cenZ);
// TODO: variable type? // div by 2.01 to prevent overlapps and z-fighting
Obstacle3D res(Floorplan::Material::CONCRETE); const float sx = from.getDistance(to) / 2;
const float sy = thickness_m / 2;
const float sz = height / 2.01f; // prevent overlaps
const Point3 size(sx, sy, sz);
const Point3 rot(0,0,deg);
Polygon poly; // build
Cube cube(pos, size, rot);
// append all "add" and "remove" areas // done
for (Floorplan::FloorOutlinePolygon* fop : f->outline) { Obstacle3D res(getType(fol), fol->material);
switch (fop->method) { res.triangles = cube.getTriangles();
case Floorplan::OutlineMethod::ADD: poly.add(fop->poly); break; return res;
case Floorplan::OutlineMethod::REMOVE: poly.remove(fop->poly); break;
default: throw 1; }
Obstacle3D getDoor(const Floorplan::Floor* f, const Floorplan::FloorObstacleDoor* door) const {
const float thickness_m = 0.10; // TODO??
const Point2 from = door->from;
const Point2 to = door->to;
const float rad = std::atan2(to.y - from.y, to.x - from.x);
float deg = rad * 180 / M_PI;
// div by 2.01 to prevent overlapps and z-fighting
const Point3 rot(0,0,deg);
Point3 pos;
Matrix4 mat = Matrix4::identity();
Obstacle3D res(Obstacle3D::Type::DOOR, door->material);
// normal door? (non-spinner)
if (Floorplan::DoorType::SWING == door->type) {
if (doorsOpen) {deg += (door->swap) ? (-90) : (+90);}
mat = Matrix4::getTranslation(1,0,0); // cube's edge located at 0,0,0
pos = Point3(from.x, from.y, f->atHeight + door->height/2);
const float sx = from.getDistance(to) / 2;
const float sy = thickness_m / 2;
const float sz = door->height / 2.01f; // prevent overlaps
const Point3 size(sx, sy, sz);
Cube cube;
cube.transform(mat);
cube.transform(pos, size, rot);
res.triangles = cube.getTriangles();
} else if (Floorplan::DoorType::REVOLVING == door->type) {
const Point2 cen2 = (from+to)/2;
const Point3 pos(cen2.x, cen2.y, f->atHeight + door->height/2);
// outer and inner radius
const float rOuter = from.getDistance(to) / 2;
const float rInner = rOuter - 0.1;
const float sz = door->height / 2.01f; // prevent overlaps
const Point3 size(1, 1, sz);
// around the doors
Tube tube;
tube.addSegment(0+40-90, 180-40-90, rInner, rOuter, 1, true, true);
tube.addSegment(180+40-90, 360-40-90, rInner, rOuter, 1, true, true);
tube.transform(pos, Point3(1,1,1), rot);
res.triangles = tube.getTriangles();
// the doors
const int numDoors = 3;
Cube cube;
cube.transform(Matrix4::getTranslation(1,0,0));
for (int i = 0; i < numDoors; ++i) {
const int deg = 45 + (360*i / numDoors);
Cube c1 = cube
.transformed(Matrix4::getScale(rInner/2-0.05, thickness_m/2, sz))
.transformed(Matrix4::getTranslation(0.04, 0, 0))
.transformed(Matrix4::getRotationDeg(0,0,deg))
.transformed(Matrix4::getTranslation(pos.x, pos.y, pos.z));
//pos, Point3(rInner/2-0.05, thickness_m/2, sz), Point3(0,0,deg));
std::vector<Triangle3> t1 = c1.getTriangles();
res.triangles.insert(res.triangles.end(), t1.begin(), t1.end());
}
} else {
throw "unsupported door type";
}
return res;
}
/** get the missing part/gap, above the given door */
Obstacle3D getDoorAbove(const Floorplan::Floor* f, const Floorplan::FloorObstacleDoor* door) const {
// find the element above the door (= a connected element)
auto comp = [door] (const Floorplan::FloorObstacle* obs) {
if (obs == door) {return false;}
const Floorplan::FloorObstacleLine* line = dynamic_cast<const Floorplan::FloorObstacleLine*>(obs);
if (!line) {return false;}
return (line->from == door->from || line->to == door->from || line->from == door->to || line->to == door->to);
};
auto it = std::find_if(f->obstacles.begin(), f->obstacles.end(), comp);
const Floorplan::FloorObstacleLine* line = dynamic_cast<const Floorplan::FloorObstacleLine*> (*it);
if (!line) {
throw Exception("did not find a matching element to place above the door");
}
// get the obstacle to place above the door
return getObstacleLine(f, line, door);
}
Obstacle3D getHandrail(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol) const {
// target
Obstacle3D res(getType(fol), fol->material);
if (!exportHandrails) {return res;}
const float thickness_m = 0.05;
const Point2 from = fol->from;
const Point2 to = fol->to;
const Point2 cen2 = (from+to)/2;
// edges
const float z1 = f->atHeight;
const float z2 = f->atHeight + 1.0;
Point3 p1 = Point3(from.x, from.y, z1);
Point3 p2 = Point3(to.x, to.y, z1);
Point3 p3 = Point3(from.x, from.y, z2);
Point3 p4 = Point3(to.x, to.y, z2);
const float rad = std::atan2(to.y - from.y, to.x - from.x);
const float deg = rad * 180 / M_PI;
// cube's destination center
const Point3 pUp(cen2.x, cen2.y, z2);
const float sx = from.getDistance(to) / 2;
const float sy = thickness_m / 2;
const float sz = thickness_m / 2;
const Point3 size(sx, sy, sz);
const Point3 rot(0,0,deg);
// upper bar
const Cube cubeUpper(pUp, size, rot);
const std::vector<Triangle3> tmp = cubeUpper.getTriangles();
res.triangles.insert(res.triangles.end(), tmp.begin(), tmp.end());
const Point3 d1 = p2-p1;
const Point3 d2 = p4-p3;
const int numBars = d2.length() / 0.75f;
for (int i = 1; i < numBars; ++i) {
const Point3 s = p1 + d1 * i / numBars;
const Point3 e = p3 + d2 * i / numBars;
const Point3 c = (s+e)/2;
const Point3 size(thickness_m/2, thickness_m/2, s.getDistance(e)/2 - thickness_m);
const Cube cube(c, size, rot);
const std::vector<Triangle3> tmp = cube.getTriangles();
res.triangles.insert(res.triangles.end(), tmp.begin(), tmp.end());
}
// done
return res;
}
/** convert a line obstacle to 3D triangles */
Obstacle3D getStairs(const Floorplan::Floor* f, const Floorplan::Stair* s) {
Obstacle3D res(Obstacle3D::Type::STAIR, Floorplan::Material::CONCRETE);
std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(s->getParts(), f);
for (const Floorplan::Quad3& quad : quads) {
const Triangle3 t1(quad.p1, quad.p2, quad.p3);
const Triangle3 t2(quad.p3, quad.p4, quad.p1);
res.triangles.push_back(t1);
res.triangles.push_back(t2);
}
return res;
}
static Obstacle3D::Type getType(const Floorplan::FloorObstacleLine* l) {
switch (l->type) {
case Floorplan::ObstacleType::WALL: return Obstacle3D::Type::WALL;
case Floorplan::ObstacleType::WINDOW: return Obstacle3D::Type::WINDOW;
case Floorplan::ObstacleType::HANDRAIL: return Obstacle3D::Type::HANDRAIL;
default: return Obstacle3D::Type::UNKNOWN;
} }
} }
// convert them into polygons };
std::vector<std::vector<Point3>> polys = poly.get(f->getStartingZ());
// convert polygons (GL_TRIANGLE_STRIP) to triangles }
for (const std::vector<Point3>& pts : polys) {
for (int i = 0; i < (int)pts.size() - 2; ++i) {
// floor must be double-sided for reflection to work with the correct normals
Triangle3 tria1 (pts[i+0], pts[i+1], pts[i+2]);
Triangle3 tria2 (pts[i+2], pts[i+1], pts[i+0]);
// ensure the triangle with the normal pointing downwards (towards bulding's cellar)
// is below the triangle that points upwards (towards the sky)
if (tria1.getNormal().z < 0) {tria1 = tria1 - Point3(0,0,0.02);}
if (tria2.getNormal().z < 0) {tria2 = tria2 - Point3(0,0,0.02);}
// add both
res.triangles.push_back(tria1);
res.triangles.push_back(tria2);
}
}
return res;
}
/** convert a line obstacle to 3D triangles */
Obstacle3D getTriangles(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol) {
/*
Obstacle3D res(fol->material);
Point3 p1(fol->from.x, fol->from.y, f->getStartingZ());
Point3 p2(fol->to.x, fol->to.y, f->getStartingZ());
Point3 p3(fol->to.x, fol->to.y, f->getEndingZ());
Point3 p4(fol->from.x, fol->from.y, f->getEndingZ());
Triangle3 t1(p1,p2,p3);
Triangle3 t2(p1,p3,p4);
res.triangles.push_back(t1);
res.triangles.push_back(t2);
*/
const float thickness_m = fol->thickness_m;
const Point2 from = fol->from;
const Point2 to = fol->to;
const Point2 cen2 = (from+to)/2;
const float rad = std::atan2(to.y - from.y, to.x - from.x);
const float deg = rad * 180 / M_PI;
// cube's destination center
const Point3 pos(cen2.x, cen2.y, f->atHeight + f->height/2);
// div by 2.01 to prevent overlapps and z-fi
const float sx = from.getDistance(to) / 2.01f;
const float sy = thickness_m / 2.01f;
const float sz = f->height / 2.01f; // prevent overlaps
const Point3 size(sx, sy, sz);
const Point3 rot(0,0,deg);
// build
Cube cube(pos, size, rot);
// done
Obstacle3D res(fol->material);
res.triangles = cube.getTriangles();
return res;
}
};
#endif // MODELFACTORY_H #endif // MODELFACTORY_H

184
wifi/estimate/ray3/ModelFactoryHelper.h
View File

@@ -4,102 +4,106 @@
#include <Indoor/floorplan/v2/Floorplan.h> #include <Indoor/floorplan/v2/Floorplan.h>
#include "../../../lib/gpc/gpc.h" #include "../../../lib/gpc/gpc.h"
class Polygon { namespace Ray3D {
struct GPCPolygon : gpc_polygon { class Polygon {
GPCPolygon() {
num_contours = 0; struct GPCPolygon : gpc_polygon {
contour = nullptr; GPCPolygon() {
hole = nullptr; num_contours = 0;
} contour = nullptr;
~GPCPolygon() { hole = nullptr;
if (contour) {
gpc_free_polygon(this);
//free(contour->vertex); contour->vertex = nullptr;
} }
free(contour); contour = nullptr; ~GPCPolygon() {
free(hole); hole = nullptr; if (contour) {
gpc_free_polygon(this);
//free(contour->vertex); contour->vertex = nullptr;
}
free(contour); contour = nullptr;
free(hole); hole = nullptr;
}
GPCPolygon& operator = (const GPCPolygon& o) = delete;
GPCPolygon& operator = (GPCPolygon& o) {
this->contour = o.contour;
this->hole = o.hole;
this->num_contours = o.num_contours;
o.contour = nullptr;
o.hole = nullptr;
return *this;
}
};
private:
GPCPolygon state;
public:
void add(const Floorplan::Polygon2& poly) {
GPCPolygon cur = toGPC(poly);
//GPCPolygon out;
gpc_polygon_clip(GPC_UNION, &state, &cur, &state);
//state = out;
}
void remove(const Floorplan::Polygon2& poly) {
GPCPolygon cur = toGPC(poly);
//GPCPolygon out;
gpc_polygon_clip(GPC_DIFF, &state, &cur, &state);
//state = out;
}
std::vector<std::vector<Point3>> get(float z) {
gpc_tristrip res;
res.num_strips = 0;
res.strip = nullptr;
//res.strip = (gpc_vertex_list*) malloc(1024);
gpc_polygon_to_tristrip(&state, &res);
std::vector<std::vector<Point3>> trias;
for (int i = 0; i < res.num_strips; ++i) {
gpc_vertex_list lst = res.strip[i];
std::vector<Point3> tria;
for (int j = 0; j < lst.num_vertices; ++j) {
gpc_vertex& vert = lst.vertex[j];
Point3 p3(vert.x, vert.y, z);
tria.push_back(p3);
}
trias.push_back(tria);
}
gpc_free_tristrip(&res);
return std::move(trias);
} }
GPCPolygon& operator = (const GPCPolygon& o) = delete;
GPCPolygon& operator = (GPCPolygon& o) { private:
this->contour = o.contour;
this->hole = o.hole; GPCPolygon toGPC(Floorplan::Polygon2 poly) {
this->num_contours = o.num_contours;
o.contour = nullptr; std::vector<gpc_vertex> verts;
o.hole = nullptr; for (Point2 p2 : poly.points) {
return *this; gpc_vertex vert; vert.x = p2.x; vert.y = p2.y;
verts.push_back(vert);
}
GPCPolygon gpol;
gpc_vertex_list list;
list.num_vertices = verts.size();
list.vertex = verts.data();
gpc_add_contour(&gpol, &list, 0);
return gpol;
} }
}; };
private: }
GPCPolygon state;
public:
void add(const Floorplan::Polygon2& poly) {
GPCPolygon cur = toGPC(poly);
//GPCPolygon out;
gpc_polygon_clip(GPC_UNION, &state, &cur, &state);
//state = out;
}
void remove(const Floorplan::Polygon2& poly) {
GPCPolygon cur = toGPC(poly);
//GPCPolygon out;
gpc_polygon_clip(GPC_DIFF, &state, &cur, &state);
//state = out;
}
std::vector<std::vector<Point3>> get(float z) {
gpc_tristrip res;
res.num_strips = 0;
res.strip = nullptr;
//res.strip = (gpc_vertex_list*) malloc(1024);
gpc_polygon_to_tristrip(&state, &res);
std::vector<std::vector<Point3>> trias;
for (int i = 0; i < res.num_strips; ++i) {
gpc_vertex_list lst = res.strip[i];
std::vector<Point3> tria;
for (int j = 0; j < lst.num_vertices; ++j) {
gpc_vertex& vert = lst.vertex[j];
Point3 p3(vert.x, vert.y, z);
tria.push_back(p3);
}
trias.push_back(tria);
}
gpc_free_tristrip(&res);
return std::move(trias);
}
private:
GPCPolygon toGPC(Floorplan::Polygon2 poly) {
std::vector<gpc_vertex> verts;
for (Point2 p2 : poly.points) {
gpc_vertex vert; vert.x = p2.x; vert.y = p2.y;
verts.push_back(vert);
}
GPCPolygon gpol;
gpc_vertex_list list;
list.num_vertices = verts.size();
list.vertex = verts.data();
gpc_add_contour(&gpol, &list, 0);
return gpol;
}
};
#endif // MODELFACTORYHELPER_H #endif // MODELFACTORYHELPER_H

36
wifi/estimate/ray3/Obstacle3.h
View File

@@ -7,18 +7,38 @@
#include "../../../floorplan/v2/Floorplan.h" #include "../../../floorplan/v2/Floorplan.h"
struct Obstacle3D { namespace Ray3D {
Floorplan::Material mat; /**
std::vector<Triangle3> triangles; * 3D obstacle
* based on multiple triangles
* has a material and a type
*/
struct Obstacle3D {
/** empty ctor */ enum class Type {
Obstacle3D() : mat() {;} UNKNOWN,
GROUND_INDOOR,
GROUND_OUTDOOR,
STAIR,
HANDRAIL,
DOOR,
WALL,
WINDOW,
};
/** ctor */ Type type;
Obstacle3D(Floorplan::Material mat) : mat(mat) {;} Floorplan::Material mat;
std::vector<Triangle3> triangles;
}; /** empty ctor */
Obstacle3D() : type(Type::UNKNOWN), mat() {;}
/** ctor */
Obstacle3D(Type type, Floorplan::Material mat) : type(type), mat(mat) {;}
};
}
#endif // OBSTACLE3_H #endif // OBSTACLE3_H

122
wifi/estimate/ray3/Tube.h Normal file
View File

@@ -0,0 +1,122 @@
#ifndef TUBE_H
#define TUBE_H
#include "../../../math/Matrix4.h"
#include "Mesh.h"
namespace Ray3D {
/** walled cylinder */
class Tube : public Mesh {
public:
/** ctor */
Tube() {
;
}
/** get a transformed version */
Tube transformed(const Matrix4& mat) const {
Tube res = *this;
res.transform(mat);
return res;
}
/** build */
void addSegment(const float from_deg, const float to_deg, const float rInner, const float rOuter, const float h, bool closeSides, bool topAndBottom) {
const int tiles = 32;
const float deg_per_tile = 360.0f / tiles;
const float rad_per_tile = deg_per_tile / 180.0f * M_PI;
const int startTile = std::round(from_deg / deg_per_tile);
const int endTile = std::round(to_deg / deg_per_tile);
for (int i = startTile; i < endTile; ++i) {
const float startRad = (i+0) * rad_per_tile;
const float endRad = (i+1) * rad_per_tile;
const float xo0 = std::cos(startRad) * rOuter;
const float yo0 = std::sin(startRad) * rOuter;
const float xo1 = std::cos(endRad) * rOuter;
const float yo1 = std::sin(endRad) * rOuter;
const float xi0 = std::cos(startRad) * rInner;
const float yi0 = std::sin(startRad) * rInner;
const float xi1 = std::cos(endRad) * rInner;
const float yi1 = std::sin(endRad) * rInner;
if (closeSides) {
// close start of segment
if (i == startTile) {
addQuad(
Point3(xi0, yi0, -h),
Point3(xo0, yo0, -h),
Point3(xo0, yo0, +h),
Point3(xi0, yi0, +h)
);
}
// close end of segment
if (i == endTile-1) {
addQuad(
Point3(xi1, yi1, +h),
Point3(xo1, yo1, +h),
Point3(xo1, yo1, -h),
Point3(xi1, yi1, -h)
);
}
}
// outer
addQuad(
Point3(xo0, yo0, -h),
Point3(xo1, yo1, -h),
Point3(xo1, yo1, +h),
Point3(xo0, yo0, +h)
);
// innser
addQuad(
Point3(xi0, yi0, +h),
Point3(xi1, yi1, +h),
Point3(xi1, yi1, -h),
Point3(xi0, yi0, -h)
);
if (topAndBottom) {
// top
addQuad(
Point3(xi0, yi0, h),
Point3(xo0, yo0, h),
Point3(xo1, yo1, h),
Point3(xi1, yi1, h)
);
// bottom
addQuad(
Point3(xi1, yi1, -h),
Point3(xo1, yo1, -h),
Point3(xo0, yo0, -h),
Point3(xi0, yi0, -h)
);
}
}
}
};
}
#endif // TUBE_H

825
wifi/estimate/ray3/WifiRayTrace3D.h
View File

@@ -34,478 +34,479 @@
// http://graphics.stanford.edu/courses/cs148-10-summer/docs/2006--degreve--reflection_refraction.pdf // http://graphics.stanford.edu/courses/cs148-10-summer/docs/2006--degreve--reflection_refraction.pdf
struct Intersection { namespace Ray3D {
Point3 pos;
const Obstacle3D* obs;
Intersection(const Point3 pos, const Obstacle3D* obs) : pos(pos), obs(obs) {;}
};
struct StateRay3 : public Ray3 { struct Intersection {
Point3 pos;
//std::vector<Intersection> stack; const Obstacle3D* obs;
Intersection(const Point3 pos, const Obstacle3D* obs) : pos(pos), obs(obs) {;}
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; struct StateRay3 : public Ray3 {
std::vector<Point3> hitLeave;
std::vector<Point3> hitStop;
public: //std::vector<Intersection> stack;
/** ctor */ int depth = 0;
WiFiRaytrace3D(const Floorplan::IndoorMap* map, const int gs, const Point3 apPos) : apPos(apPos) { float totalLen = 0;
float totalAttenuation = 0;
// get the floor's 3D-bbox const Obstacle3D* isWithin = nullptr;
bbox = FloorplanHelper::getBBox(map);
// allocate /** empty ctor */
dm.resize(bbox, gs); StateRay3() {;}
ModelFactory fac(map); /** ctor */
std::vector<Obstacle3D> obstacles = fac.triangulize(); StateRay3(const Point3 start, const Point3 dir) : Ray3(start, dir) {
;
// build bounding volumes
for (Obstacle3D& obs : obstacles) {
tree.add(obs);
} }
//tree.optimize(); StateRay3 enter(const Point3 hitPos, const Obstacle3D* obs) const {
//tree.show(500, false);
int xxx = 0; (void) xxx; StateRay3 next = getNext(hitPos);
Assert::isNull(this->isWithin, "code error: isWithin");
} next.totalAttenuation += Materials::get().getAttributes(obs->mat).shadowing.attenuation;
next.isWithin = obs;
const std::vector<Point3>& getHitEnter() const { return next;
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; StateRay3 leave(const Point3 hitPos, const Obstacle3D* obs) const {
} (void) obs;
StateRay3 next = getNext(hitPos);
next.isWithin = nullptr;
return next;
//#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 StateRay3 reflectAt(const Point3 hitPos, const Obstacle3D* obs) const {
rasterize(ray, nextHit);
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);
ModelFactory 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;
// 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 //#define USE_DEBUG
if (ray.isWithin) {
leave(ray, nextHit);
#ifdef USE_DEBUG private:
hitLeave.push_back(nextHit.pos);
#endif void trace(const StateRay3& ray) {
} else {
enter(ray, nextHit); // get the nearest intersection with the floorplan
reflectAt(ray, nextHit); const Hit3 nextHit = getNearestHit(ray);
#ifdef USE_DEBUG
hitEnter.push_back(nextHit.pos); // stop?
#endif 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) {
static inline float getAttenuation(const Hit3& h) { // continue into the same direction
return Materials::get().getAttributes(h.material).shadowing.attenuation; StateRay3 next = ray.leave(hit.pos, hit.obstacle);
}
static inline float getAttenuationForReflection(const Hit3& h) { // continue
return Materials::get().getAttributes(h.material).reflection.attenuation; trace(next);
}
/** 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"); /** perform reflection and continue tracing */
Assert::isNear(1.0f, hit.normal.length(), 0.01f, "obstacle's normal is not normalized"); void enter(const StateRay3& ray, const Hit3& hit) {
// angle to wide or narrow? -> skip // continue into the same direction
const float d = std::abs(dot(ray.dir, hit.normal)); StateRay3 next = ray.enter(hit.pos, hit.obstacle);
if (d < 0.01) {return;} // parallel
if (d > 0.99) {return;} // perpendicular;
//static std::minstd_rand gen; // continue
//static std::normal_distribution<float> dist(0, 0.02); trace(next);
//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 /** perform reflection and continue tracing */
trace(reflected); 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) { void hitTest(const StateRay3& ray, const Obstacle3D& obs, Hit3& nearest) {
const float minDist = 0.01; // prevent errors hitting the same obstacle twice const float minDist = 0.01; // prevent errors hitting the same obstacle twice
//bool dummy = obs.boundingSphere.intersects(ray); //bool dummy = obs.boundingSphere.intersects(ray);
Point3 hitPoint; Point3 hitPoint;
for (const Triangle3& tria : obs.triangles) { for (const Triangle3& tria : obs.triangles) {
if (tria.intersects(ray, hitPoint)) { if (tria.intersects(ray, hitPoint)) {
//if (!dummy) { //if (!dummy) {
// throw Exception("issue!"); // throw Exception("issue!");
//} //}
const float dist = hitPoint.getDistance(ray.start); const float dist = hitPoint.getDistance(ray.start);
if (dist > minDist && dist < nearest.dist) { if (dist > minDist && dist < nearest.dist) {
nearest.obstacle = &obs; nearest.obstacle = &obs;
nearest.dist = dist; nearest.dist = dist;
nearest.pos = hitPoint; nearest.pos = hitPoint;
nearest.normal = tria.getNormal(); nearest.normal = tria.getNormal();
nearest.material = obs.mat; 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);
} }
} 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
Sphere3 getSphereAround(const std::vector<Triangle3>& trias) { if (ray.isWithin) {
std::vector<Point3> pts; // check intersection only with the current obstacle to get out
for (const Triangle3& tria : trias) { hitTest(ray, *ray.isWithin, nearest);
pts.push_back(tria.p1);
pts.push_back(tria.p2); } else {
pts.push_back(tria.p3);
} // // check intersection with all walls
const Sphere3 sphere = Sphere3::around(pts); // 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;
// sanity assertion
for (const Point3& pt : pts) {
Assert::isTrue(sphere.contains(pt), "bounding sphere error");
} }
return sphere;
} /** 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 #endif // WIFIRAYTRACE3D_H