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

worked on 3D model creation

Browse Source
This commit is contained in:
k-a-z-u
2018-02-06 17:34:29 +01:00
parent 0bb1b707de
commit a35e043196
15 changed files with 1442 additions and 1091 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
floorplan/v2/Floorplan.h
View File

@@ -189,19 +189,41 @@ namespace Floorplan {
struct POI;
struct Stair;
struct Elevator;
struct GroundTruthPoint;
struct GroundTruthPoint;
using FloorOutline = std::vector<FloorOutlinePolygon*>;
using FloorObstacles = std::vector<FloorObstacle*>;
using FloorAccessPoints = std::vector<AccessPoint*>;
using FloorBeacons = std::vector<Beacon*>;
using FloorFingerprintLocations = std::vector<FingerprintLocation*>;
using FloorRegions = std::vector<FloorRegion*>;
using FloorUnderlays = std::vector<UnderlayImage*>;
using FloorPOIs = std::vector<POI*>;
using FloorStairs = std::vector<Stair*>;
using FloorElevators = std::vector<Elevator*>;
using FloorGroundTruthPoints = std::vector<GroundTruthPoint*>;
struct FloorOutline : public std::vector<FloorOutlinePolygon*> {
bool enabled = true;
};
struct FloorObstacles : public std::vector<FloorObstacle*> {
bool enabled = true;
};
struct FloorAccessPoints : public std::vector<AccessPoint*> {
bool enabled = true;
};
struct FloorBeacons : public std::vector<Beacon*> {
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 */
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<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("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 <elevators> tag */
static std::vector<Elevator*> parseFloorElevators(const XMLElem* el) {
std::vector<Elevator*> 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 <stairs> tag */
static std::vector<Stair*> parseFloorStairs(const XMLElem* el) {
std::vector<Stair*> 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 <pois> tag */
static std::vector<POI*> parseFloorPOIs(const XMLElem* el) {
std::vector<POI*> 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 <gtpoints> tag */
static std::vector<GroundTruthPoint*> parseFloorGroundTruthPoints(const XMLElem* el) {
std::vector<GroundTruthPoint*> vec;
FOREACH_NODE(n, el) {
if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); }
}
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 the <gtpoints> 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 <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 */
static std::vector<AccessPoint*> parseFloorAccessPoints(const XMLElem* el) {
std::vector<AccessPoint*> 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 <beacons> tag */
static std::vector<Beacon*> parseFloorBeacons(const XMLElem* el) {
std::vector<Beacon*> 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 <fingerprints> <location>s */
static std::vector<FingerprintLocation*> parseFingerprintLocations(const XMLElem* el) {
static FloorFingerprintLocations parseFingerprintLocations(const XMLElem* el) {
assertNode("fingerprints", el);
std::vector<FingerprintLocation*> 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<FloorRegion*> parseFloorRegions(const XMLElem* el) {
std::vector<FloorRegion*> 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 <obstacles> tag */
static std::vector<FloorObstacle*> parseFloorObstacles(const XMLElem* el) {
static FloorObstacles parseFloorObstacles(const XMLElem* el) {
assertNode("obstacles", el);
std::vector<FloorObstacle*> 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));}

2
main.cpp
View File

@@ -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*";

2
tests/geo/TestBVH3.cpp
View File

@@ -10,6 +10,8 @@
#include "../../floorplan/v2/FloorplanReader.h"
#include "../../wifi/estimate/ray3/ModelFactory.h"
using namespace Ray3D;
struct Wrapper {
static std::vector<Point3> getVertices(const BBox3& bbox) {

1
tests/ray/TestDataMap3.cpp
View File

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

11
tests/ray/TestModelFac.cpp
View File

@@ -4,17 +4,20 @@
#include "../../wifi/estimate/ray3/ModelFactory.h"
#include "../../floorplan/v2/FloorplanReader.h"
#include <fstream>
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();
}

3
tests/ray/TestRayTrace3.cpp
View File

@@ -4,6 +4,7 @@
#include "../../wifi/estimate/ray3/WifiRayTrace3D.h"
#include "../../floorplan/v2/FloorplanReader.h"
#include <fstream>
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;

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

@@ -5,137 +5,141 @@
#include "../../../geo/Triangle3.h"
#include "../../../math/Matrix4.h"
class Cube {
namespace Ray3D {
private:
class Cube {
std::vector<Triangle3> trias;
private:
public:
std::vector<Triangle3> 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<Triangle3> 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<Triangle3> 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

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

@@ -7,229 +7,233 @@
#include <functional>
#include <mutex>
template <typename T> class DataMap3 {
namespace Ray3D {
private:
template <typename T> 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<void(float,float,float,T)> 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<void(float,float,float,T)> 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<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 */
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<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;
}
};
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

166
wifi/estimate/ray3/FloorplanMesh.h
View File

@@ -3,46 +3,154 @@
#include "Obstacle3.h"
/**
* meshed version of the floorplan
*/
struct FloorplanMesh {
namespace Ray3D {
std::vector<Obstacle3D> elements;
/**
* meshed version of the floorplan
*/
struct FloorplanMesh {
/** DEBUG: convert to .obj file code for exporting */
std::string toOBJ() {
std::vector<Obstacle3D> 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<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 "../../../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<MaterialAttributes> materials;
private:
/** hidden ctor */
Materials() {
std::vector<MaterialAttributes> 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

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

@@ -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<Floorplan::Floor*> 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<Floorplan::Floor*> 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<Floorplan::Floor*> 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<Obstacle3D> triangulize() {
std::vector<Obstacle3D> res;
// 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
for (const Floorplan::Floor* f : floors) {
// 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
for (const Floorplan::FloorObstacle* fo : f->obstacles) {
// handle line obstacles
const Floorplan::FloorObstacleLine* fol = dynamic_cast<const Floorplan::FloorObstacleLine*>(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<Obstacle3D> 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<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);
}
}
/** 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<Floorplan::Floor*> floors) {
this->exportFloors = floors;
}
std::vector<Obstacle3D> 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<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);
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<Obstacle3D> triangulize() {
/*
Obstacle3D res(fol->material);
std::vector<Obstacle3D> 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<Floorplan::Floor*>& 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<Obstacle3D> 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<Obstacle3D> 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<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);
}
}
}
// 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<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;
}
/**
* @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
const Floorplan::FloorObstacleLine* fol = dynamic_cast<const Floorplan::FloorObstacleLine*>(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<const Floorplan::FloorObstacleDoor*>(fo);
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));
}
}
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<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;
}
}
};
}
#endif // MODELFACTORY_H

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

@@ -4,102 +4,106 @@
#include <Indoor/floorplan/v2/Floorplan.h>
#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<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) {
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<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;
}
};
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

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

@@ -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<Triangle3> 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<Triangle3> triangles;
/** empty ctor */
Obstacle3D() : type(Type::UNKNOWN), mat() {;}
/** ctor */
Obstacle3D(Type type, Floorplan::Material mat) : type(type), mat(mat) {;}
};
}
#endif // OBSTACLE3_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
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<Intersection> stack;
int depth = 0;
float totalLen = 0;
float totalAttenuation = 0;
const Obstacle3D* isWithin = nullptr;
/** empty ctor */
StateRay3() {;}
/** ctor */
StateRay3(const Point3 start, const Point3 dir) : Ray3(start, dir) {
;
}
StateRay3 enter(const Point3 hitPos, const Obstacle3D* obs) const {
StateRay3 next = getNext(hitPos);
Assert::isNull(this->isWithin, "code error: isWithin");
next.totalAttenuation += Materials::get().getAttributes(obs->mat).shadowing.attenuation;
next.isWithin = obs;
return next;
}
StateRay3 leave(const Point3 hitPos, const Obstacle3D* obs) const {
(void) obs;
StateRay3 next = getNext(hitPos);
next.isWithin = nullptr;
return next;
}
StateRay3 reflectAt(const Point3 hitPos, const Obstacle3D* obs) const {
StateRay3 next = getNext(hitPos);
next.totalAttenuation += Materials::get().getAttributes(obs->mat).reflection.attenuation;
next.isWithin = nullptr; // AIR
return next;
}
private:
StateRay3 getNext(const Point3 hitPos) const {
StateRay3 next = *this;
++next.depth;
next.totalLen += (start.getDistance(hitPos));
next.start = hitPos;
return next;
}
public:
inline float getRSSI(const float addDist = 0) const {
const float txp = -40;
const float gamma = 1.2f;
return (txp - 10*gamma*std::log10(totalLen + addDist)) - totalAttenuation;
}
int getDepth() const {
//return stack.size();
return depth;
}
float getLength() const {
return totalLen;
}
};
struct Hit3 {
const Obstacle3D* obstacle;
Triangle3 obstacleTria;
float dist;
Point3 pos;
Point3 normal;
Floorplan::Material material;
bool stopHere = false;
bool invalid = false;
Hit3() {;}
Hit3(const float dist, const Point3 pos, const Point3 normal) : dist(dist), pos(pos), normal(normal) {;}
};
struct Obstacle3DWrapper {
static std::vector<Point3> getVertices(const Obstacle3D& obs) {
std::vector<Point3> pts;
for (const Triangle3& tria : obs.triangles) {
pts.push_back(tria.p1);
pts.push_back(tria.p2);
pts.push_back(tria.p3);
}
return pts;
}
static std::vector<Point3> getDebugLines(const Obstacle3D& obs) {
std::vector<Point3> pts;
for (const Triangle3& tria : obs.triangles) {
pts.push_back(tria.p1); pts.push_back(tria.p2);
pts.push_back(tria.p2); pts.push_back(tria.p3);
pts.push_back(tria.p3); pts.push_back(tria.p1);
}
return pts;
}
};
class WiFiRaytrace3D {
private:
BBox3 bbox;
Point3 apPos;
DataMap3Signal dm;
BVH3Debug<Obstacle3D, BoundingVolumeSphere3, Obstacle3DWrapper> tree;
struct Limit {
static constexpr int RAYS = 15000;
static constexpr int HITS = 25;
static constexpr float RSSI = -110;
struct Intersection {
Point3 pos;
const Obstacle3D* obs;
Intersection(const Point3 pos, const Obstacle3D* obs) : pos(pos), obs(obs) {;}
};
std::vector<Point3> hitEnter;
std::vector<Point3> hitLeave;
std::vector<Point3> hitStop;
struct StateRay3 : public Ray3 {
public:
//std::vector<Intersection> 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<Obstacle3D> 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<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);
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<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
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<float> 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<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;
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<Triangle3>& trias) {
// if the ray is currently within something, its only option is to get out of it
if (ray.isWithin) {
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);
// 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<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