From a35e043196da06b99813b9caf32c1bf10e41da0d Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Tue, 6 Feb 2018 17:34:29 +0100 Subject: [PATCH] worked on 3D model creation --- floorplan/v2/Floorplan.h | 48 +- floorplan/v2/FloorplanReader.h | 65 +- main.cpp | 2 +- tests/geo/TestBVH3.cpp | 2 + tests/ray/TestDataMap3.cpp | 1 + tests/ray/TestModelFac.cpp | 11 +- tests/ray/TestRayTrace3.cpp | 3 +- wifi/estimate/ray3/Cube.h | 210 +++--- wifi/estimate/ray3/DataMap3.h | 344 +++++----- wifi/estimate/ray3/FloorplanMesh.h | 166 ++++- wifi/estimate/ray3/MaterialOptions.h | 84 +-- wifi/estimate/ray3/ModelFactory.h | 533 ++++++++++----- wifi/estimate/ray3/ModelFactoryHelper.h | 184 +++--- wifi/estimate/ray3/Obstacle3.h | 55 +- wifi/estimate/ray3/WifiRayTrace3D.h | 825 ++++++++++++------------ 15 files changed, 1442 insertions(+), 1091 deletions(-) diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index 8844d85..1b6494d 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -189,19 +189,41 @@ namespace Floorplan { struct POI; struct Stair; struct Elevator; - struct GroundTruthPoint; + struct GroundTruthPoint; - using FloorOutline = std::vector; - using FloorObstacles = std::vector; - using FloorAccessPoints = std::vector; - using FloorBeacons = std::vector; - using FloorFingerprintLocations = std::vector; - using FloorRegions = std::vector; - using FloorUnderlays = std::vector; - using FloorPOIs = std::vector; - using FloorStairs = std::vector; - using FloorElevators = std::vector; - using FloorGroundTruthPoints = std::vector; + struct FloorOutline : public std::vector { + bool enabled = true; + }; + struct FloorObstacles : public std::vector { + bool enabled = true; + }; + struct FloorAccessPoints : public std::vector { + bool enabled = true; + }; + struct FloorBeacons : public std::vector { + bool enabled = true; + }; + struct FloorFingerprintLocations : public std::vector { + bool enabled = true; + }; + struct FloorRegions : public std::vector { + bool enabled = true; + }; + struct FloorUnderlays : public std::vector { + bool enabled = true; + }; + struct FloorPOIs : public std::vector { + bool enabled = true; + }; + struct FloorStairs : public std::vector { + bool enabled = true; + }; + struct FloorElevators : public std::vector { + bool enabled = true; + }; + struct FloorGroundTruthPoints : public std::vector { + bool enabled = true; + }; /** describes one floor within the map, starting at a given height */ struct Floor { @@ -567,6 +589,8 @@ namespace Floorplan { /** describe the floorplan's location on earth */ struct EarthRegistration { + bool enabled = true; + /** all available correspondences: earth <-> map */ std::vector correspondences; diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 5d14315..00b7b01 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -137,15 +137,15 @@ namespace Floorplan { if (std::string("pois") == n->Name()) {floor->pois = parseFloorPOIs(n);} if (std::string("stairs") == n->Name()) {floor->stairs = parseFloorStairs(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; } /** parse the tag */ - static std::vector parseFloorElevators(const XMLElem* el) { - std::vector vec; + static FloorElevators parseFloorElevators(const XMLElem* el) { + FloorElevators vec; FOREACH_NODE(n, el) { if (std::string("elevator") == n->Name()) { vec.push_back(parseFloorElevator(n)); } } @@ -153,8 +153,8 @@ namespace Floorplan { } /** parse the tag */ - static std::vector parseFloorStairs(const XMLElem* el) { - std::vector vec; + static FloorStairs parseFloorStairs(const XMLElem* el) { + FloorStairs vec; FOREACH_NODE(n, el) { if (std::string("stair") == n->Name()) { vec.push_back(parseFloorStair(n)); } } @@ -207,8 +207,8 @@ namespace Floorplan { /** parse the tag */ - static std::vector parseFloorPOIs(const XMLElem* el) { - std::vector vec; + static FloorPOIs parseFloorPOIs(const XMLElem* el) { + FloorPOIs vec; FOREACH_NODE(n, el) { if (std::string("poi") == n->Name()) { vec.push_back(parseFloorPOI(n)); } } @@ -225,27 +225,26 @@ namespace Floorplan { } - /** parse the tag */ - static std::vector parseFloorGroundTruthPoints(const XMLElem* el) { - std::vector vec; - FOREACH_NODE(n, el) { - if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); } - } - return vec; - } - - /** parse a tag */ - static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) { - GroundTruthPoint* gtp = new GroundTruthPoint(); - gtp->id = el->IntAttribute("id"); - gtp->pos = parsePoint3(el); - return gtp; - } + /** parse the tag */ + static FloorGroundTruthPoints parseFloorGroundTruthPoints(const XMLElem* el) { + FloorGroundTruthPoints vec; + FOREACH_NODE(n, el) { + if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); } + } + return vec; + } + /** parse a tag */ + static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) { + GroundTruthPoint* gtp = new GroundTruthPoint(); + gtp->id = el->IntAttribute("id"); + gtp->pos = parsePoint3(el); + return gtp; + } /** parse the tag */ - static std::vector parseFloorAccessPoints(const XMLElem* el) { - std::vector vec; + static FloorAccessPoints parseFloorAccessPoints(const XMLElem* el) { + FloorAccessPoints vec; FOREACH_NODE(n, el) { if (std::string("accesspoint") == n->Name()) { vec.push_back(parseAccessPoint(n)); } } @@ -317,8 +316,8 @@ namespace Floorplan { /** parse the tag */ - static std::vector parseFloorBeacons(const XMLElem* el) { - std::vector vec; + static FloorBeacons parseFloorBeacons(const XMLElem* el) { + FloorBeacons vec; FOREACH_NODE(n, el) { if (std::string("beacon") == n->Name()) { vec.push_back(parseBeacon(n)); } } @@ -341,9 +340,9 @@ namespace Floorplan { } /** parse s */ - static std::vector parseFingerprintLocations(const XMLElem* el) { + static FloorFingerprintLocations parseFingerprintLocations(const XMLElem* el) { assertNode("fingerprints", el); - std::vector vec; + FloorFingerprintLocations vec; FOREACH_NODE(n, el) { if (std::string("location") == n->Name()) { vec.push_back(parseFingerprintLocation(n)); } } @@ -363,8 +362,8 @@ namespace Floorplan { return fpl; } - static std::vector parseFloorRegions(const XMLElem* el) { - std::vector vec; + static FloorRegions parseFloorRegions(const XMLElem* el) { + FloorRegions vec; FOREACH_NODE(n, el) { if (std::string("region") == n->Name()) { vec.push_back(parseFloorRegion(n)); } } @@ -380,9 +379,9 @@ namespace Floorplan { } /** parse the tag */ - static std::vector parseFloorObstacles(const XMLElem* el) { + static FloorObstacles parseFloorObstacles(const XMLElem* el) { assertNode("obstacles", el); - std::vector obstacles; + FloorObstacles obstacles; FOREACH_NODE(n, el) { // if (std::string("wall") == n->Name()) {obstacles.push_back(parseFloorObstacleWall(n));} // if (std::string("door") == n->Name()) {obstacles.push_back(parseFloorObstacleDoor(n));} diff --git a/main.cpp b/main.cpp index b86cc50..61ab6ed 100755 --- a/main.cpp +++ b/main.cpp @@ -111,7 +111,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*Matrix4*"; //::testing::GTEST_FLAG(filter) = "*Sphere3*"; - ::testing::GTEST_FLAG(filter) = "FileReader*"; + ::testing::GTEST_FLAG(filter) = "Ray.ModelFac*"; //::testing::GTEST_FLAG(filter) = "Timestamp*"; //::testing::GTEST_FLAG(filter) = "*RayTrace3*"; diff --git a/tests/geo/TestBVH3.cpp b/tests/geo/TestBVH3.cpp index 480667d..5108b9f 100644 --- a/tests/geo/TestBVH3.cpp +++ b/tests/geo/TestBVH3.cpp @@ -10,6 +10,8 @@ #include "../../floorplan/v2/FloorplanReader.h" #include "../../wifi/estimate/ray3/ModelFactory.h" +using namespace Ray3D; + struct Wrapper { static std::vector getVertices(const BBox3& bbox) { diff --git a/tests/ray/TestDataMap3.cpp b/tests/ray/TestDataMap3.cpp index a233d12..d8a147d 100644 --- a/tests/ray/TestDataMap3.cpp +++ b/tests/ray/TestDataMap3.cpp @@ -2,6 +2,7 @@ #include "../Tests.h" #include "../../wifi/estimate/ray3/DataMap3.h" +using namespace Ray3D; TEST(DataMap3, test) { diff --git a/tests/ray/TestModelFac.cpp b/tests/ray/TestModelFac.cpp index 6238819..5056bef 100644 --- a/tests/ray/TestModelFac.cpp +++ b/tests/ray/TestModelFac.cpp @@ -4,17 +4,20 @@ #include "../../wifi/estimate/ray3/ModelFactory.h" #include "../../floorplan/v2/FloorplanReader.h" #include +using namespace Ray3D; TEST(Ray, ModelFac) { - std::string file = "/mnt/data/workspaces/IndoorMap/maps/SHL39.xml"; + std::string file = "/apps/paper/diss/data/maps/SHL41_nm.xml"; Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file); ModelFactory fac(map); - fac.triangulize(); + //fac.triangulize(); - std::ofstream out("/mnt/vm/fhws.obj"); - out << fac.toOBJ() << std::endl; + FloorplanMesh mesh = fac.getMesh(); + + std::ofstream out("/tmp/fhws.ply"); + out << mesh.toPLY() << std::endl; out.close(); } diff --git a/tests/ray/TestRayTrace3.cpp b/tests/ray/TestRayTrace3.cpp index c88a7f3..663cf8f 100644 --- a/tests/ray/TestRayTrace3.cpp +++ b/tests/ray/TestRayTrace3.cpp @@ -4,6 +4,7 @@ #include "../../wifi/estimate/ray3/WifiRayTrace3D.h" #include "../../floorplan/v2/FloorplanReader.h" #include +using namespace Ray3D; TEST(RayTrace3, test) { @@ -19,7 +20,7 @@ TEST(RayTrace3, test) { ModelFactory fac(map); std::ofstream outOBJ("/tmp/vm/map.obj"); - outOBJ << fac.toOBJ(); + outOBJ << fac.getMesh().toOBJ(); outOBJ.close(); const int gs_cm = 50; diff --git a/wifi/estimate/ray3/Cube.h b/wifi/estimate/ray3/Cube.h index 2beb8ae..dc36332 100644 --- a/wifi/estimate/ray3/Cube.h +++ b/wifi/estimate/ray3/Cube.h @@ -5,137 +5,141 @@ #include "../../../geo/Triangle3.h" #include "../../../math/Matrix4.h" -class Cube { +namespace Ray3D { -private: + class Cube { - std::vector trias; + private: -public: + std::vector trias; - /** ctor */ - Cube() { - unitCube(true); - } + public: - /** 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); - } + /** ctor */ + Cube() { + unitCube(true); + } - /** get the cube's triangles */ - const std::vector getTriangles() const { - return trias; - } + /** 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); + } - void transform(const Matrix4& mat) { + /** get the cube's triangles */ + const std::vector getTriangles() const { + return trias; + } - for (Triangle3& tria : trias) { + void transform(const Matrix4& mat) { - 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); + for (Triangle3& tria : trias) { - v1 = mat*v1; - v2 = mat*v2; - v3 = mat*v3; + 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); - 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); + 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); + + } } - } - - /** get a transformed version */ - Cube transformed(const Matrix4& mat) const { - 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; } + private: + + /** build unit-cube faces */ + void unitCube(const bool topAndBottom) { + + const float s = 1.0f; - if (topAndBottom) { - // top - addQuad( - Point3(+s, +s, +s), - Point3(-s, +s, +s), - Point3(-s, -s, +s), - Point3(+s, -s, +s) - ); - - // bottom + // left? addQuad( 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) ); + + // 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) ); + } - 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 diff --git a/wifi/estimate/ray3/DataMap3.h b/wifi/estimate/ray3/DataMap3.h index cc0d8ea..04145e7 100644 --- a/wifi/estimate/ray3/DataMap3.h +++ b/wifi/estimate/ray3/DataMap3.h @@ -7,229 +7,233 @@ #include #include -template class DataMap3 { +namespace Ray3D { -private: + template class DataMap3 { - float sx_m; - float sy_m; - float sz_m; + private: - float x_m; - float y_m; - float z_m; + float sx_m; + float sy_m; + float sz_m; - int gridSize_cm; + float x_m; + float y_m; + float z_m; - BBox3 bbox; + int gridSize_cm; - int nx; - int ny; - int nz; + BBox3 bbox; - T* data = nullptr; + int nx; + int ny; + int nz; -public: + T* data = nullptr; - /** ctor */ - DataMap3() { - ; - } + public: - ~DataMap3() { + /** ctor */ + DataMap3() { + ; + } - // cleanup - cleanup(); + ~DataMap3() { - } + // 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? */ - bool containsGrid(const int x, const int y, const int z) const { - return (x >= 0) && (y >= 0) && (z >= 0) && - (x < nx) && (y < ny) && (z < nz); - } + /** does the map contain the given indices? */ + bool containsGrid(const int x, const int y, const int z) const { + return (x >= 0) && (y >= 0) && (z >= 0) && + (x < nx) && (y < ny) && (z < nz); + } - /** does the map contain the given coordinate? */ - 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)); - } + /** does the map contain the given coordinate? */ + 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)); + } - 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 - float buffer_m = 1; + // slightly increase to pervent out-of-bounds due to rounding + float buffer_m = 1; - // start-offset - sx_m = bbox.getMin().x - buffer_m; - sy_m = bbox.getMin().y - buffer_m; - sz_m = bbox.getMin().z - buffer_m; + // start-offset + sx_m = bbox.getMin().x - buffer_m; + sy_m = bbox.getMin().y - buffer_m; + sz_m = bbox.getMin().z - buffer_m; - // size in meter - x_m = (bbox.getMax().x - bbox.getMin().x) + 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; + // size in meter + x_m = (bbox.getMax().x - bbox.getMin().x) + 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; - // number of elements in the grid - this->gridSize_cm = gridSize_cm; - nx = (x_m*100) / gridSize_cm; - ny = (y_m*100) / gridSize_cm; - nz = (z_m*100) / gridSize_cm; + // number of elements in the grid + this->gridSize_cm = gridSize_cm; + nx = (x_m*100) / gridSize_cm; + ny = (y_m*100) / gridSize_cm; + nz = (z_m*100) / gridSize_cm; - // allocate and reset all to 0.0 - data = new T[nx*ny*nz]; + // allocate and reset all to 0.0 + data = new T[nx*ny*nz]; - } + } - /** get the used grid-size (in cm) */ - int getGridSize_cm() const {return gridSize_cm;} + /** get the used grid-size (in 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) { - 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 iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm); - setGrid(ix, iy, iz, 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 iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm); + const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm); + setGrid(ix, iy, iz, val); + } - 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 iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm ); - const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm ); - return getGrid(ix, iy, iz); - } + 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 iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm ); + const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm ); + return getGrid(ix, iy, iz); + } - 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 iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm ); - const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm ); - return getGridRef(ix, iy, iz); - } + 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 iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm ); + const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm ); + return getGridRef(ix, iy, iz); + } - T getGrid(const int ix, const int iy, const int iz) const { - Assert::isBetween(ix, 0, nx-1, "x out of range"); - Assert::isBetween(iy, 0, ny-1, "y out of range"); - Assert::isBetween(iz, 0, nz-1, "z out of range"); - const int idx = ix + iy*nx + iz*nx*ny; - return data[idx]; - } + T getGrid(const int ix, const int iy, const int iz) const { + Assert::isBetween(ix, 0, nx-1, "x out of range"); + Assert::isBetween(iy, 0, ny-1, "y out of range"); + Assert::isBetween(iz, 0, nz-1, "z out of range"); + const int idx = ix + iy*nx + iz*nx*ny; + return data[idx]; + } - T& getGridRef(const int ix, const int iy, const int iz) { - Assert::isBetween(ix, 0, nx-1, "x out of range"); - Assert::isBetween(iy, 0, ny-1, "y out of range"); - Assert::isBetween(iz, 0, nz-1, "z out of range"); - const int idx = ix + iy*nx + iz*nx*ny; - return data[idx]; - } + T& getGridRef(const int ix, const int iy, const int iz) { + Assert::isBetween(ix, 0, nx-1, "x out of range"); + Assert::isBetween(iy, 0, ny-1, "y out of range"); + Assert::isBetween(iz, 0, nz-1, "z out of range"); + const int idx = ix + iy*nx + iz*nx*ny; + return data[idx]; + } - 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(iy, 0, ny-1, "y out of range"); - Assert::isBetween(iz, 0, nz-1, "z out of range"); - const int idx = ix + iy*nx + iz*nx*ny; - data[idx] = 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(iy, 0, ny-1, "y out of range"); + Assert::isBetween(iz, 0, nz-1, "z out of range"); + const int idx = ix + iy*nx + iz*nx*ny; + data[idx] = val; + } - void forEach(std::function func) const { - for (int iz = 0; iz < nz; ++iz) { - for (int iy = 0; iy < ny; ++iy) { - for (int ix = 0; ix < nx; ++ix) { - const float x = (ix * gridSize_cm / 100.0f) + sx_m; - const float y = (iy * gridSize_cm / 100.0f) + sy_m; - const float z = (iz * gridSize_cm / 100.0f) + sz_m; - func(x,y,z, getGrid(ix, iy, iz)); + void forEach(std::function func) const { + for (int iz = 0; iz < nz; ++iz) { + for (int iy = 0; iy < ny; ++iy) { + for (int ix = 0; ix < nx; ++ix) { + const float x = (ix * gridSize_cm / 100.0f) + sx_m; + const float y = (iy * gridSize_cm / 100.0f) + sy_m; + const float z = (iz * gridSize_cm / 100.0f) + sz_m; + func(x,y,z, getGrid(ix, iy, iz)); + } } } } - } - /* - void dump() { + /* + void dump() { - std::ofstream os("/tmp/1.dat"); - const float s = 1;//gridSize_cm / 100.0f; -// for (int y = 0; y < ny; ++y) { -// for (int x = 0; x < nx; ++x) { -// float rssi = data[x+y*nx]; -// rssi = (rssi == 0) ? (-100) : (rssi); -// os << (x*s) << " " << (y*s) << " " << rssi << "\n"; -// } -// os << "\n"; -// } - for (int y = 0; y < ny; ++y) { - for (int x = 0; x < nx; ++x) { - float rssi = data[x+y*nx]; - rssi = (rssi == 0) ? (-100) : (rssi); - os << rssi << " "; + std::ofstream os("/tmp/1.dat"); + const float s = 1;//gridSize_cm / 100.0f; + // for (int y = 0; y < ny; ++y) { + // for (int x = 0; x < nx; ++x) { + // float rssi = data[x+y*nx]; + // rssi = (rssi == 0) ? (-100) : (rssi); + // os << (x*s) << " " << (y*s) << " " << rssi << "\n"; + // } + // os << "\n"; + // } + for (int y = 0; y < ny; ++y) { + for (int x = 0; x < nx; ++x) { + float rssi = data[x+y*nx]; + rssi = (rssi == 0) ? (-100) : (rssi); + os << rssi << " "; + } + os << "\n"; } - os << "\n"; + os.close(); + } - os.close(); + */ - } - */ + private: -private: - - void cleanup() { - delete[] data; - data = nullptr; - } + void cleanup() { + delete[] data; + data = nullptr; + } -}; - - - -struct DataMap3SignalEntry { - - - - struct Entry { - float rssi; - float distanceToAP; - Entry(float rssi, float distanceToAP) : rssi(rssi), distanceToAP(distanceToAP) {;} }; - std::vector 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 { -public: - /** update average */ - void update(const float x_m, const float y_m, const float z_m, const float rssi, const float distanceToAP) { + struct Entry { + float rssi; + float distanceToAP; + Entry(float rssi, float distanceToAP) : rssi(rssi), distanceToAP(distanceToAP) {;} + }; - DataMap3SignalEntry& entry = getRef(x_m, y_m, z_m); - entry.add(rssi, distanceToAP); + std::vector 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; + } + + + }; + + class DataMap3Signal : public DataMap3 { + + 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 diff --git a/wifi/estimate/ray3/FloorplanMesh.h b/wifi/estimate/ray3/FloorplanMesh.h index d29f236..38af565 100644 --- a/wifi/estimate/ray3/FloorplanMesh.h +++ b/wifi/estimate/ray3/FloorplanMesh.h @@ -3,46 +3,154 @@ #include "Obstacle3.h" -/** - * meshed version of the floorplan - */ -struct FloorplanMesh { +namespace Ray3D { - std::vector elements; + /** + * meshed version of the floorplan + */ + struct FloorplanMesh { - /** DEBUG: convert to .obj file code for exporting */ - std::string toOBJ() { + std::vector elements; - int nVerts = 1; - int nObjs = 0; - std::string res; + /** DEBUG: convert to .obj file code for exporting */ + std::string toOBJ() { - // write each obstacle - for (const Obstacle3D& o : elements) { + 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; + } - // 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; } - // 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 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 diff --git a/wifi/estimate/ray3/MaterialOptions.h b/wifi/estimate/ray3/MaterialOptions.h index 37565c0..7bd3f94 100644 --- a/wifi/estimate/ray3/MaterialOptions.h +++ b/wifi/estimate/ray3/MaterialOptions.h @@ -5,60 +5,64 @@ #include "../../../floorplan/v2/Floorplan.h" #include "../../../Assertions.h" -/** raytracing attributes for one material */ -struct MaterialAttributes { +namespace Ray3D { - struct Shadowing { - float attenuation; - Shadowing() : attenuation(NAN) {;} - Shadowing(float attenuation) : attenuation(attenuation) {;} - } shadowing; + /** raytracing attributes for one material */ + struct MaterialAttributes { - struct Reflection { - float attenuation; - Reflection() : attenuation(NAN) {;} - Reflection(float attenuation) : attenuation(attenuation) {;} - } reflection; + struct Shadowing { + float attenuation; + Shadowing() : attenuation(NAN) {;} + Shadowing(float attenuation) : attenuation(attenuation) {;} + } 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 */ - static Materials& get() { - static Materials instance; - return instance; - } + public: - /** 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]; - } + /** singleton access */ + static Materials& get() { + static Materials instance; + return instance; + } -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 materials; + private: - /** hidden ctor */ - Materials() { + std::vector materials; - 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); + /** hidden ctor */ + Materials() { - } + 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 diff --git a/wifi/estimate/ray3/ModelFactory.h b/wifi/estimate/ray3/ModelFactory.h index c31a7a0..66dd6ae 100644 --- a/wifi/estimate/ray3/ModelFactory.h +++ b/wifi/estimate/ray3/ModelFactory.h @@ -8,210 +8,401 @@ #include "Cube.h" #include "FloorplanMesh.h" -/** - * convert an indoor map into a 3D model based on triangles - */ -class ModelFactory { +namespace Ray3D { -private: + /** + * convert an indoor map into a 3D model based on triangles + */ + class ModelFactory { - bool exportCeilings = true; - bool exportObstacles = true; - bool exportWallTops = false; - std::vector exportFloors; + private: - const Floorplan::IndoorMap* map; + bool exportCeilings = true; + bool exportObstacles = true; + bool exportStairs = true; + bool exportHandrails = true; + bool exportDoors = false; + bool exportWallTops = false; + std::vector exportFloors; -public: + /** the to-be-exported map */ + const Floorplan::IndoorMap* map; + + public: - /** ctor */ - ModelFactory(const Floorplan::IndoorMap* map) : map(map) { - - } - - void setExportCeilings(bool exp) { - this->exportCeilings = exp; - } - - /** limit to-be-exported floors */ - void setFloors(const std::vector floors) { - this->exportFloors = floors; - } - - /** convert floorplan to mesh */ - FloorplanMesh getMesh() { - - FloorplanMesh mesh; - mesh.elements = triangulize(); - return mesh; - - } - -private: - - - /** get all triangles grouped by obstacle */ - std::vector triangulize() { - - std::vector res; - - // get the to-be-exported floors (either "all" or "user defined") - const std::vector& floors = (exportFloors.empty()) ? (map->floors) : (exportFloors); - - // process each floor - for (const Floorplan::Floor* f : floors) { - - // triangulize the floor itself (floor/ceiling) - if (exportCeilings) { - std::vector tmp = getFloor(f); - res.insert(res.end(), tmp.begin(), tmp.end()); - } - - // process each obstacle within the floor - for (const Floorplan::FloorObstacle* fo : f->obstacles) { - - // handle line obstacles - const Floorplan::FloorObstacleLine* fol = dynamic_cast(fo); - if (fol) { - if (fol->type == Floorplan::ObstacleType::HANDRAIL) {continue;} - if (exportObstacles) {res.push_back(getTriangles(f, fol));} - } - - } - - // TODO: remove - //break; + /** ctor */ + ModelFactory(const Floorplan::IndoorMap* map) : map(map) { } - return res; - - } - - - - -private: - - /** convert a floor (floor/ceiling) into triangles */ - std::vector getFloor(const Floorplan::Floor* f) { - - // 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 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); - } - } + /** whether or not to export ceilings */ + void setExportCeilings(bool exp) { + this->exportCeilings = exp; } - // remove the "remove" regions from EVERY "add" region added within the previous step - for (Floorplan::FloorOutlinePolygon* fop : f->outline) { - if (fop->method == Floorplan::OutlineMethod::REMOVE) { - for (auto& it : types) { - it.second.remove(fop->poly); - } - } + /** limit to-be-exported floors */ + void setFloors(const std::vector floors) { + this->exportFloors = floors; } - std::vector res; + /** convert floorplan to mesh */ + FloorplanMesh getMesh() { - // 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> polys = it.second.get(f->getStartingZ()); - - // convert polygons (GL_TRIANGLE_STRIP) to triangles - for (const std::vector& 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); + FloorplanMesh mesh; + mesh.elements = triangulize(); + return mesh; } - return res; + private: - } - /** convert a line obstacle to 3D triangles */ - Obstacle3D getTriangles(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol) { + /** get all triangles grouped by obstacle */ + std::vector triangulize() { - /* - Obstacle3D res(fol->material); + std::vector res; - 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()); + // get the to-be-exported floors (either "all" or "user defined") + const std::vector& floors = (exportFloors.empty()) ? (map->floors) : (exportFloors); - Triangle3 t1(p1,p2,p3); - Triangle3 t2(p1,p3,p4); + // process each floor + for (const Floorplan::Floor* f : floors) { - res.triangles.push_back(t1); - res.triangles.push_back(t2); + if (!f->enabled) {continue;} - */ + // triangulize the floor itself (floor/ceiling) + if (exportCeilings) { + std::vector tmp = getFloor(f); + res.insert(res.end(), tmp.begin(), tmp.end()); + } - const float thickness_m = fol->thickness_m; - const Point2 from = fol->from; - const Point2 to = fol->to; - const Point2 cen2 = (from+to)/2; + // process each obstacle within the floor + if (f->obstacles.enabled) { + for (const Floorplan::FloorObstacle* fo : f->obstacles) { + std::vector tmp = getObstacle(f, fo); + res.insert(res.end(), tmp.begin(), tmp.end()); + } + } - const float rad = std::atan2(to.y - from.y, to.x - from.x); - const float deg = rad * 180 / M_PI; + // stairs + if (f->stairs.enabled) { + for (const Floorplan::Stair* stair : f->stairs) { - // cube's destination center - const Point3 pos(cen2.x, cen2.y, f->atHeight + f->height/2); + if (exportStairs) {res.push_back(getStairs(f, stair));} - // 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); + // TODO: remove + //break; - // done - Obstacle3D res(Obstacle3D::Type::WALL, fol->material); - res.triangles = cube.getTriangles(); - return res; + } - } + return res; + + } -}; + + private: + + /** convert a floor (floor/ceiling) into triangles */ + std::vector getFloor(const Floorplan::Floor* f) { + + std::vector 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 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); + } + } + } + + // remove the "remove" regions from EVERY "add" region added within the previous step + for (Floorplan::FloorOutlinePolygon* fop : f->outline) { + if (fop->method == Floorplan::OutlineMethod::REMOVE) { + for (auto& it : types) { + it.second.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> polys = it.second.get(f->getStartingZ()); + + // convert polygons (GL_TRIANGLE_STRIP) to triangles + for (const std::vector& 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; + + } + + /** + * @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 getObstacle(const Floorplan::Floor* f, const Floorplan::FloorObstacle* fo, const Floorplan::FloorObstacleDoor* aboveDoor = nullptr) const { + + std::vector res; + + // handle line obstacles + const Floorplan::FloorObstacleLine* fol = dynamic_cast(fo); + if (fol) { + if (exportObstacles) { + if (fol->type != Floorplan::ObstacleType::HANDRAIL || exportHandrails) { + res.push_back(getObstacleLine(f, fol, aboveDoor)); + } + } + } + + const Floorplan::FloorObstacleDoor* door = dynamic_cast(fo); + if (door) { + if (exportObstacles) { + if (exportDoors) { + res.push_back(getDoor(f, door)); + } + //std::vector tmp = getDoorAbove(f, door); + //res.insert(res.end(), tmp.begin(), tmp.end()); + res.push_back(getDoorAbove(f, door)); + } + } + + return res; + + } + + /** 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"); + } + + } + + Obstacle3D getWindow(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor) const { + return getWall(f, fol, aboveDoor); + } + + Obstacle3D getWall(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor) const { + + 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; + + 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 float cenZ = (!aboveDoor) ? (f->atHeight + f->height/2) : (f->getEndingZ() - (f->height - aboveDoor->height) / 2); + const float height = (!aboveDoor) ? (f->height) : (f->height - aboveDoor->height); + const Point3 pos(cen2.x, cen2.y, cenZ); + + // div by 2.01 to prevent overlapps and z-fighting + 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); + + // build + Cube cube(pos, size, rot); + + // done + Obstacle3D res(getType(fol), fol->material); + res.triangles = cube.getTriangles(); + return res; + + } + + 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 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 + door->height/2); + + // div by 2.01 to prevent overlapps and z-fighting + 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); + const Point3 rot(0,0,deg); + + // build + Cube cube(pos, size, rot); + + // done + Obstacle3D res(Obstacle3D::Type::DOOR, door->material); + res.triangles = cube.getTriangles(); + 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(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 (*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 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 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 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; + } + } + + }; + +} #endif // MODELFACTORY_H diff --git a/wifi/estimate/ray3/ModelFactoryHelper.h b/wifi/estimate/ray3/ModelFactoryHelper.h index a48688f..3fa8847 100644 --- a/wifi/estimate/ray3/ModelFactoryHelper.h +++ b/wifi/estimate/ray3/ModelFactoryHelper.h @@ -4,102 +4,106 @@ #include #include "../../../lib/gpc/gpc.h" -class Polygon { +namespace Ray3D { - struct GPCPolygon : gpc_polygon { - GPCPolygon() { - num_contours = 0; - contour = nullptr; - hole = nullptr; - } - ~GPCPolygon() { - if (contour) { - gpc_free_polygon(this); - //free(contour->vertex); contour->vertex = nullptr; + class Polygon { + + struct GPCPolygon : gpc_polygon { + GPCPolygon() { + num_contours = 0; + contour = nullptr; + hole = nullptr; } - free(contour); contour = nullptr; - free(hole); hole = nullptr; + ~GPCPolygon() { + 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> 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> trias; + + for (int i = 0; i < res.num_strips; ++i) { + gpc_vertex_list lst = res.strip[i]; + std::vector 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) { - this->contour = o.contour; - this->hole = o.hole; - this->num_contours = o.num_contours; - o.contour = nullptr; - o.hole = nullptr; - return *this; + + private: + + GPCPolygon toGPC(Floorplan::Polygon2 poly) { + + std::vector 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; + } + }; -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> 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> trias; - - for (int i = 0; i < res.num_strips; ++i) { - gpc_vertex_list lst = res.strip[i]; - std::vector 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 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 diff --git a/wifi/estimate/ray3/Obstacle3.h b/wifi/estimate/ray3/Obstacle3.h index ef58e1f..14cea1a 100644 --- a/wifi/estimate/ray3/Obstacle3.h +++ b/wifi/estimate/ray3/Obstacle3.h @@ -7,33 +7,38 @@ #include "../../../floorplan/v2/Floorplan.h" -/** - * 3D obstacle - * based on multiple triangles - * has a material and a type - */ -struct Obstacle3D { +namespace Ray3D { + + /** + * 3D obstacle + * based on multiple triangles + * has a material and a type + */ + struct Obstacle3D { + + enum class Type { + UNKNOWN, + GROUND_INDOOR, + GROUND_OUTDOOR, + STAIR, + HANDRAIL, + DOOR, + WALL, + WINDOW, + }; + + Type type; + Floorplan::Material mat; + std::vector triangles; + + /** empty ctor */ + Obstacle3D() : type(Type::UNKNOWN), mat() {;} + + /** ctor */ + Obstacle3D(Type type, Floorplan::Material mat) : type(type), mat(mat) {;} - enum class Type { - UNKNOWN, - GROUND_INDOOR, - GROUND_OUTDOOR, - STAIR, - DOOR, - WALL, }; - Type type; - Floorplan::Material mat; - std::vector triangles; - - /** empty ctor */ - Obstacle3D() : type(Type::UNKNOWN), mat() {;} - - /** ctor */ - Obstacle3D(Type type, Floorplan::Material mat) : type(type), mat(mat) {;} - -}; - +} #endif // OBSTACLE3_H diff --git a/wifi/estimate/ray3/WifiRayTrace3D.h b/wifi/estimate/ray3/WifiRayTrace3D.h index fabb3e8..0934382 100644 --- a/wifi/estimate/ray3/WifiRayTrace3D.h +++ b/wifi/estimate/ray3/WifiRayTrace3D.h @@ -34,478 +34,479 @@ // http://graphics.stanford.edu/courses/cs148-10-summer/docs/2006--degreve--reflection_refraction.pdf -struct Intersection { - Point3 pos; - const Obstacle3D* obs; - Intersection(const Point3 pos, const Obstacle3D* obs) : pos(pos), obs(obs) {;} -}; +namespace Ray3D { -struct StateRay3 : public Ray3 { - - //std::vector stack; - - int depth = 0; - float totalLen = 0; - float totalAttenuation = 0; - - const Obstacle3D* isWithin = nullptr; - - /** empty ctor */ - StateRay3() {;} - - /** ctor */ - StateRay3(const Point3 start, const Point3 dir) : Ray3(start, dir) { - ; - } - - StateRay3 enter(const Point3 hitPos, const Obstacle3D* obs) const { - - StateRay3 next = getNext(hitPos); - Assert::isNull(this->isWithin, "code error: isWithin"); - next.totalAttenuation += Materials::get().getAttributes(obs->mat).shadowing.attenuation; - next.isWithin = obs; - return next; - - } - - StateRay3 leave(const Point3 hitPos, const Obstacle3D* obs) const { - - (void) obs; - StateRay3 next = getNext(hitPos); - next.isWithin = nullptr; - return next; - - } - - StateRay3 reflectAt(const Point3 hitPos, const Obstacle3D* obs) const { - - StateRay3 next = getNext(hitPos); - - next.totalAttenuation += Materials::get().getAttributes(obs->mat).reflection.attenuation; - next.isWithin = nullptr; // AIR - return next; - - } - -private: - - StateRay3 getNext(const Point3 hitPos) const { - StateRay3 next = *this; - ++next.depth; - next.totalLen += (start.getDistance(hitPos)); - next.start = hitPos; - return next; - } - -public: - - - - inline float getRSSI(const float addDist = 0) const { - const float txp = -40; - const float gamma = 1.2f; - return (txp - 10*gamma*std::log10(totalLen + addDist)) - totalAttenuation; - } - - int getDepth() const { - //return stack.size(); - return depth; - } - - float getLength() const { - return totalLen; - } - -}; - -struct Hit3 { - - const Obstacle3D* obstacle; - Triangle3 obstacleTria; - - float dist; - Point3 pos; - Point3 normal; - Floorplan::Material material; - bool stopHere = false; - bool invalid = false; - - Hit3() {;} - Hit3(const float dist, const Point3 pos, const Point3 normal) : dist(dist), pos(pos), normal(normal) {;} - -}; - - - - - -struct Obstacle3DWrapper { - - static std::vector getVertices(const Obstacle3D& obs) { - std::vector 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 getDebugLines(const Obstacle3D& obs) { - std::vector 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 tree; - - struct Limit { - static constexpr int RAYS = 15000; - static constexpr int HITS = 25; - static constexpr float RSSI = -110; + struct Intersection { + Point3 pos; + const Obstacle3D* obs; + Intersection(const Point3 pos, const Obstacle3D* obs) : pos(pos), obs(obs) {;} }; - std::vector hitEnter; - std::vector hitLeave; - std::vector hitStop; + struct StateRay3 : public Ray3 { -public: + //std::vector stack; - /** ctor */ - WiFiRaytrace3D(const Floorplan::IndoorMap* map, const int gs, const Point3 apPos) : apPos(apPos) { + int depth = 0; + float totalLen = 0; + float totalAttenuation = 0; - // get the floor's 3D-bbox - bbox = FloorplanHelper::getBBox(map); + const Obstacle3D* isWithin = nullptr; - // allocate - dm.resize(bbox, gs); + /** empty ctor */ + StateRay3() {;} - ModelFactory fac(map); - std::vector obstacles = fac.triangulize(); - - // build bounding volumes - for (Obstacle3D& obs : obstacles) { - tree.add(obs); + /** ctor */ + StateRay3(const Point3 start, const Point3 dir) : Ray3(start, dir) { + ; } - //tree.optimize(); - //tree.show(500, false); + StateRay3 enter(const Point3 hitPos, const Obstacle3D* obs) const { - int xxx = 0; (void) xxx; - - } - - const std::vector& getHitEnter() const { - return hitEnter; - } - const std::vector& getHitLeave() const { - return hitLeave; - } - const std::vector& getHitStop() const { - return hitStop; - } - - const DataMap3Signal& estimate() { - - std::minstd_rand gen; - std::uniform_real_distribution dx(-1.0, +1.0); - std::uniform_real_distribution dy(-1.0, +1.0); - std::uniform_real_distribution 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); + 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; } - 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 - rasterize(ray, nextHit); + 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 getVertices(const Obstacle3D& obs) { + std::vector 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 getDebugLines(const Obstacle3D& obs) { + std::vector 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 tree; + + struct Limit { + static constexpr int RAYS = 15000; + static constexpr int HITS = 25; + static constexpr float RSSI = -110; + }; + + std::vector hitEnter; + std::vector hitLeave; + std::vector 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 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& getHitEnter() const { + return hitEnter; + } + const std::vector& getHitLeave() const { + return hitLeave; + } + const std::vector& getHitStop() const { + return hitStop; + } + + const DataMap3Signal& estimate() { + + std::minstd_rand gen; + std::uniform_real_distribution dx(-1.0, +1.0); + std::uniform_real_distribution dy(-1.0, +1.0); + std::uniform_real_distribution 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 - 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 + //#define USE_DEBUG + + + private: + + void trace(const StateRay3& ray) { + + // get the nearest intersection with the floorplan + const Hit3 nextHit = getNearestHit(ray); + + // stop? + if (nextHit.invalid) { + #ifdef USE_DEBUG + hitStop.push_back(nextHit.pos); + #endif + return; + } + + // rasterize the ray's way onto the map + rasterize(ray, nextHit); + + // continue? + if ((nextHit.stopHere) || (ray.getRSSI(nextHit.dist) < Limit::RSSI) || (ray.getDepth() > Limit::HITS)) { + #ifdef USE_DEBUG + hitStop.push_back(nextHit.pos); + #endif + return; + } + + + // apply effects + if (ray.isWithin) { + leave(ray, nextHit); + #ifdef USE_DEBUG + hitLeave.push_back(nextHit.pos); + #endif + } else { + enter(ray, nextHit); + reflectAt(ray, nextHit); + #ifdef USE_DEBUG + hitEnter.push_back(nextHit.pos); + #endif + } + + + } + + static inline float getAttenuation(const Hit3& h) { + return Materials::get().getAttributes(h.material).shadowing.attenuation; + } + + static inline float getAttenuationForReflection(const Hit3& h) { + return Materials::get().getAttributes(h.material).reflection.attenuation; } - } + /** perform reflection and continue tracing */ + void leave(const StateRay3& ray, const Hit3& hit) { - static inline float getAttenuation(const Hit3& h) { - return Materials::get().getAttributes(h.material).shadowing.attenuation; - } + // continue into the same direction + StateRay3 next = ray.leave(hit.pos, hit.obstacle); - static inline float getAttenuationForReflection(const Hit3& h) { - return Materials::get().getAttributes(h.material).reflection.attenuation; - } + // continue + 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"); - Assert::isNear(1.0f, hit.normal.length(), 0.01f, "obstacle's normal is not normalized"); + /** perform reflection and continue tracing */ + void enter(const StateRay3& ray, const Hit3& hit) { - // 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; + // continue into the same direction + StateRay3 next = ray.enter(hit.pos, hit.obstacle); - //static std::minstd_rand gen; - //static std::normal_distribution dist(0, 0.02); - //const float mod = dist(gen); + // continue + trace(next); - // 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); + /** perform reflection and continue tracing */ + void reflectAt(const StateRay3& ray, const Hit3& hit) { - } + if (hit.normal.length() < 0.9) { + int i = 0; (void) i; + } + + Assert::isNear(1.0f, ray.dir.length(), 0.01f, "ray's direction is not normalized"); + Assert::isNear(1.0f, hit.normal.length(), 0.01f, "obstacle's normal is not normalized"); + + // angle to wide or narrow? -> skip + const float d = std::abs(dot(ray.dir, hit.normal)); + if (d < 0.01) {return;} // parallel + if (d > 0.99) {return;} // perpendicular; + + //static std::minstd_rand gen; + //static std::normal_distribution 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; - for (const Triangle3& tria : obs.triangles) { - if (tria.intersects(ray, hitPoint)) { + Point3 hitPoint; + for (const Triangle3& tria : obs.triangles) { + if (tria.intersects(ray, hitPoint)) { - //if (!dummy) { - // throw Exception("issue!"); - //} + //if (!dummy) { + // throw Exception("issue!"); + //} - const float dist = hitPoint.getDistance(ray.start); - if (dist > minDist && dist < nearest.dist) { - nearest.obstacle = &obs; - nearest.dist = dist; - nearest.pos = hitPoint; - nearest.normal = tria.getNormal(); - nearest.material = obs.mat; + const float dist = hitPoint.getDistance(ray.start); + if (dist > minDist && dist < nearest.dist) { + nearest.obstacle = &obs; + nearest.dist = dist; + nearest.pos = hitPoint; + nearest.normal = tria.getNormal(); + nearest.material = obs.mat; + } } } - } - - } - - - Hit3 getNearestHit(const StateRay3& ray) { - - Assert::isNear(1.0f, ray.dir.length(), 0.01f, "not normalized!"); - - int hits = 0; - const float MAX = 999999; - Hit3 nearest; nearest.dist = MAX; - - - // if the ray is currently within something, its only option is to get out of it - if (ray.isWithin) { - - // check intersection only with the current obstacle to get out - hitTest(ray, *ray.isWithin, nearest); - - } else { - -// // check intersection with all walls -// for (const Obstacle3D& obs : obstacles) { - -// // fast opt-out -// //if (!obs.boundingSphere.intersects(ray)) {continue;} - -// hitTest(ray, obs, nearest); - -// } - - - auto onHit = [&] (const Obstacle3D& obs) { - ++hits; - hitTest(ray, obs, nearest); - }; - - tree.getHits(ray, onHit); - - } - - // no hit with floorplan: limit to bounding-box! - if (nearest.dist == MAX) { - nearest.invalid = true; - } - - //std::cout << hits << std::endl; - - return nearest; - - } - - - /** rasterize the ray (current pos -> hit) onto the map */ - void rasterize(const StateRay3& ray, const Hit3& hit) { - - const Point3 dir = hit.pos - ray.start; - const Point3 dirN = dir.normalized(); - const Point3 step = dirN * (dm.getGridSize_cm()/100.0f) * 1.0f; // TODO * 1.0 ?? - const int steps = dir.length() / step.length(); - - // sanity check - // ensure the direction towards the nearest intersection is the same as the ray's direction - // otherwise the intersection-test is invalid -#ifdef WITH_ASSERTIONS - if (dir.normalized().getDistance(ray.dir) > 0.1) { - return; - std::cout << "direction to the nearest hit is not the same direction as the ray has. incorrect intersection test?!" << std::endl; - } -#endif - - for (int i = 0; i <= steps; ++i) { - - const Point3 dst = ray.start + (step*i); - const float partLen = ray.start.getDistance(dst); - //const float len = ray.totalLength + partLen; - -// const float curRSSI = dm.get(dst.x, dst.y); - const float newRSSI = ray.getRSSI(partLen); - const float totalLen = ray.getLength() + partLen; - -// // ray stronger than current rssi? -// if (curRSSI == 0 || curRSSI < newRSSI) { -// dm.set(dst.x, dst.y, newRSSI); -// } - - dm.update(dst.x, dst.y, dst.z, newRSSI, totalLen); } - } + 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; - /* - Sphere3 getSphereAround(const std::vector& trias) { + // if the ray is currently within something, its only option is to get out of it + if (ray.isWithin) { - std::vector 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); + // 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; - // 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& trias) { + + std::vector 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