diff --git a/geo/Plane3.h b/geo/Plane3.h new file mode 100644 index 0000000..eb479bb --- /dev/null +++ b/geo/Plane3.h @@ -0,0 +1,26 @@ +#ifndef PLANE3_H +#define PLANE3_H + +#include "Point3.h" + +class Plane3 { + +private: + + Point3 p1; + Point3 p2; + Point3 p3; + Point3 p4; + +public: + + /** ctor */ + Plane3(Point3 p1, Point3 p2, Point3 p3, Point3 p4) : p1(p1), p2(p2), p3(p3), p4(p4) { + + } + + + +}; + +#endif // PLANE3_H diff --git a/geo/Point3.h b/geo/Point3.h index ccce6e5..6038b0b 100644 --- a/geo/Point3.h +++ b/geo/Point3.h @@ -116,8 +116,8 @@ private: }; -inline bool dot(const Point3& p1, const Point3& p2) { - return p1.x*p2.x + p1.y*p2.y + p1.z*p2.z; +inline float dot(const Point3& p1, const Point3& p2) { + return (p1.x*p2.x) + (p1.y*p2.y) + (p1.z*p2.z); } inline Point3 cross(const Point3& a, const Point3& b) { diff --git a/geo/Ray3.h b/geo/Ray3.h new file mode 100644 index 0000000..eb23012 --- /dev/null +++ b/geo/Ray3.h @@ -0,0 +1,28 @@ +#ifndef RAY3_H +#define RAY3_H + +#include "Point3.h" + +struct Ray3 { + + /** starting point */ + Point3 start; + + /** ray's direction */ + Point3 dir; + +public: + + /** empty */ + Ray3() : start(), dir() { + ; + } + + /** ctor */ + Ray3(Point3 start, Point3 dir) : start(start), dir(dir) { + ; + } + +}; + +#endif // RAY3_H diff --git a/geo/Sphere3.h b/geo/Sphere3.h new file mode 100644 index 0000000..e272e9b --- /dev/null +++ b/geo/Sphere3.h @@ -0,0 +1,128 @@ +#ifndef SPHERE3_H +#define SPHERE3_H + +#include "Point3.h" +#include "Ray3.h" + +#include + + +struct Sphere3 { + + Point3 center; + + float radius; + + /** empty ctor */ + Sphere3() : center(), radius(0) {;} + + /** ctor */ + Sphere3(const Point3 center, const float radius) : center(center), radius(radius) {;} + + /** create a sphere that fully contains the given point-set */ + static Sphere3 around(const std::vector& lst) { + + // NOT OPTIMAL but fast + Point3 sum(0,0,0); + for (const Point3 p : lst) {sum += p;} + const Point3 center = sum / lst.size(); + float radius = 0; + for (const Point3 p : lst) { + const float dist = center.getDistance(p); + if (dist > radius) {radius = dist;} + } + return Sphere3(center, radius); + + } + + + + + static Sphere3 join(const Sphere3& a, const Sphere3& b) { + + if (a.contains(b)) {return a;} + if (b.contains(a)) {return b;} + + Point3 newCen = (a.center + b.center) / 2; + float newRad = (a.center.getDistance(b.center) + std::max(a.radius, b.radius) * 2) / 2; + return Sphere3(newCen, newRad); + } + + /** does this sphere contain the given sphere? */ + bool contains(const Sphere3& o) const { + return (o.center.getDistance(this->center) + o.radius) < this->radius; + } + + /** does the sphere contain the given point? */ + bool contains(const Point3& p) const { + return center.getDistance(p) <= radius; + } + + bool intersects(const Ray3& ray) const { + + if (contains(ray.start)) {return true;} + + /* + // https://stackoverflow.com/questions/6533856/ray-sphere-intersection + + const float xA = ray.start.x; + const float yA = ray.start.y; + const float zA = ray.start.z; + + const float xB = (ray.start + ray.dir).x; + const float yB = (ray.start + ray.dir).y; + const float zB = (ray.start + ray.dir).z; + + const float xC = center.x; + const float yC = center.y; + const float zC = center.z; + + const float a = (xB-xA)*(xB-xA)+(yB-yA)*(yB-yA)+(zB-zA)*(zB-zA); + const float b = 2*((xB-xA)*(xA-xC)+(yB-yA)*(yA-yC)+(zB-zA)*(zA-zC)); + const float c = (xA-xC)*(xA-xC)+(yA-yC)*(yA-yC)+(zA-zC)*(zA-zC)-(radius*radius); + + const float delta = (b*b) - (4*a*c); + + // intersection? + return delta >= 0; + */ + + + // http://www.ccs.neu.edu/home/fell/CSU540/programs/RayTracingFormulas.htm + + const double x0 = ray.start.x; + const double y0 = ray.start.y; + const double z0 = ray.start.z; + + const double cx = center.x; + const double cy = center.y; + const double cz = center.z; + + const double dx = ray.dir.x; + const double dy = ray.dir.y; + const double dz = ray.dir.z; + + const double a = dx*dx + dy*dy + dz*dz; + const double b = 2*dx*(x0-cx) + 2*dy*(y0-cy) + 2*dz*(z0-cz); + const double c = cx*cx + cy*cy + cz*cz + x0*x0 + y0*y0 + z0*z0 + -2*(cx*x0 + cy*y0 + cz*z0) - radius*radius; + + const double discriminant = (b*b) - (4*a*c); + return discriminant >= 0; + + + /* + // http://www.pixelnerve.com/v/2009/02/08/ray-sphere-intersection/ + const float a = ray.dir.length(); + //if (a == 0.0) return 0; + const float b = 2.0f * ( dot(ray.start, ray.dir) - dot(ray.dir, center)) ; + const Point3 tempDiff = center - ray.start; + const float c = tempDiff.length() - (radius*radius); + const float disc = b * b - 4 * a * c; + return disc >= 0.0f; + */ + + } + +}; + +#endif // SPHERE3_H diff --git a/geo/TestSphere3.cpp b/geo/TestSphere3.cpp new file mode 100644 index 0000000..e69de29 diff --git a/geo/Triangle3.h b/geo/Triangle3.h new file mode 100644 index 0000000..b70a239 --- /dev/null +++ b/geo/Triangle3.h @@ -0,0 +1,118 @@ +#ifndef TRIANGLE3_H +#define TRIANGLE3_H + +#include "Point3.h" +#include "Ray3.h" + +struct Triangle3 { + + Point3 p1; + Point3 p2; + Point3 p3; + +public: + + /** empty ctor */ + Triangle3() : p1(), p2(), p3() {;} + + /** ctor */ + Triangle3(Point3 p1, Point3 p2, Point3 p3) : p1(p1), p2(p2), p3(p3) {;} + + +// Point3 cross(Point3 b, Point3 c) const { +// return Point3( +// b.y*c.z - c.y*b.z, +// b.z*c.x - c.z*b.x, +// b.x*c.y - c.x*b.y +// ); +// } + +// float dot(Point3 b, Point3 c) const { +// return b.x*c.x + b.y*c.y + b.z*c.z; +// } + + Triangle3 operator - (const Point3 o) const { + return Triangle3(p1-o, p2-o, p3-o); + } + + Point3 getNormal() const { + return cross( p2-p1, p3-p1 ).normalized(); + } + + + // http://www.lighthouse3d.com/tutorials/maths/ray-triangle-intersection/ + bool intersects(Ray3 ray, Point3& pos) const { + + const Point3 e1 = p2-p1; + const Point3 e2 = p3-p1; + + const Point3 h = cross(ray.dir, e2); + const float a = dot(e1, h); + + if (a > -0.00001 && a < 0.00001) {return false;} + + const float f = 1/a; + + const Point3 s = ray.start - p1; + const float u = f * dot(s,h); + + if (u < 0.0 || u > 1.0) {return false;} + + const Point3 q = cross(s, e1); + const float v = f * dot(ray.dir, q); + + if (v < 0.0 || u + v > 1.0) {return false;} + const float t = f * dot(e2,q); + + + if (t > 0.00001) { + pos = ray.start + (ray.dir * t); + return true; + } + + return false; + + } + +/* + int rayIntersectsTriangle(float *p, float *d, + float *v0, float *v1, float *v2) { + + float e1[3],e2[3],h[3],s[3],q[3]; + float a,f,u,v; + + //crossProduct(h,d,e2); + a = innerProduct(e1,h); + + if (a > -0.00001 && a < 0.00001) + return(false); + + f = 1/a; + vector(s,p,v0); + u = f * (innerProduct(s,h)); + + if (u < 0.0 || u > 1.0) + return(false); + + crossProduct(q,s,e1); + v = f * innerProduct(d,q); + + if (v < 0.0 || u + v > 1.0) + return(false); + + // at this stage we can compute t to find out where + // the intersection point is on the line + t = f * innerProduct(e2,q); + + if (t > 0.00001) // ray intersection + return(true); + + else // this means that there is a line intersection + // but not a ray intersection + return (false); + + } +*/ +}; + +#endif // TRIANGLE3_H diff --git a/geo/volume/BVH.h b/geo/volume/BVH.h new file mode 100644 index 0000000..0333f40 --- /dev/null +++ b/geo/volume/BVH.h @@ -0,0 +1,21 @@ +#ifndef BOUNDINGVOLUMEHIERARCHY_H +#define BOUNDINGVOLUMEHIERARCHY_H + +#include "BoundingVolume.h" +#include "BoundingVolumeAABB.h" +#include "BoundingVolumeSphere.h" + +class BVH { + +public: + + /** add a new volume to the tree */ + void add(BoundingVolume* bv) { + + } + + + +}; + +#endif // BOUNDINGVOLUMEHIERARCHY_H diff --git a/geo/volume/BoundingVolume.h b/geo/volume/BoundingVolume.h new file mode 100644 index 0000000..d259895 --- /dev/null +++ b/geo/volume/BoundingVolume.h @@ -0,0 +1,18 @@ +#ifndef BOUNDINGVOLUME_H +#define BOUNDINGVOLUME_H + +#include "../Point3.h" + +class BoundingVolume { + +public: + + /** get the volume's size (something like m^3) */ + virtual float getVolumeSize() const = 0; + + /** does the volume contain the given point? */ + virtual bool contains(const Point3 p) const = 0; + +}; + +#endif // BOUNDINGVOLUME_H diff --git a/geo/volume/BoundingVolumeAABB.h b/geo/volume/BoundingVolumeAABB.h new file mode 100644 index 0000000..b99c666 --- /dev/null +++ b/geo/volume/BoundingVolumeAABB.h @@ -0,0 +1,30 @@ +#ifndef BOUNDINGVOLUMEBOX_H +#define BOUNDINGVOLUMEBOX_H + +#include "BoundingVolume.h" +#include "../Point3.h" + +class BoundingVolumeAABB : public BoundingVolume { + + static constexpr float MAX = +99999; + static constexpr float MIN = -99999; + + /** minimum */ + Point3 p1; + + /** maximum */ + Point3 p2; + +public: + + /** empty ctor */ + BoundingVolumeAABB() : p1(MAX,MAX,MAX), p2(MIN,MIN,MIN) {;} + + float getVolumeSize() const override { + return (p2.x-p1.x) * (p2.y-p1.y) * (p2.z-p1.z); + } + + +}; + +#endif // BOUNDINGVOLUMEBOX_H diff --git a/geo/volume/BoundingVolumeBox.h b/geo/volume/BoundingVolumeBox.h new file mode 100644 index 0000000..60ec5cd --- /dev/null +++ b/geo/volume/BoundingVolumeBox.h @@ -0,0 +1,4 @@ +#ifndef BOUNDINGVOLUMEBOX_H +#define BOUNDINGVOLUMEBOX_H + +#endif // BOUNDINGVOLUMEBOX_H diff --git a/geo/volume/BoundingVolumeHierarchy.h b/geo/volume/BoundingVolumeHierarchy.h new file mode 100644 index 0000000..3682e2e --- /dev/null +++ b/geo/volume/BoundingVolumeHierarchy.h @@ -0,0 +1,4 @@ +#ifndef BOUNDINGVOLUMEHIERARCHY_H +#define BOUNDINGVOLUMEHIERARCHY_H + +#endif // BOUNDINGVOLUMEHIERARCHY_H diff --git a/geo/volume/BoundingVolumeSphere.h b/geo/volume/BoundingVolumeSphere.h new file mode 100644 index 0000000..22345a6 --- /dev/null +++ b/geo/volume/BoundingVolumeSphere.h @@ -0,0 +1,27 @@ +#ifndef BOUNDINGVOLUMESPHERE_H +#define BOUNDINGVOLUMESPHERE_H + +#include "../Point3.h" + +class BoundingVolumeSphere : public BoundingVolume { + +private: + + Point3 center; + + float radius; + +public: + + float getVolumeSize() const override { + return (4.0f / 3.0f) * M_PI * (radius*radius*radius); + } + + /** does the volume contain the given point? */ + virtual bool contains(const Point3 p) const { + return (center-p).length() <= radius; + } + +}; + +#endif // BOUNDINGVOLUMESPHERE_H diff --git a/main.cpp b/main.cpp index 46f1192..4fe22c9 100755 --- a/main.cpp +++ b/main.cpp @@ -30,7 +30,12 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*Offline.readWrite*"; - ::testing::GTEST_FLAG(filter) = "*Jensen*"; + //::testing::GTEST_FLAG(filter) = "*Triangle*"; + //::testing::GTEST_FLAG(filter) = "*Ray.ModelFac*"; + //::testing::GTEST_FLAG(filter) = "*DataMap3*"; + ::testing::GTEST_FLAG(filter) = "*RayTrace3*"; + //::testing::GTEST_FLAG(filter) = "*Matrix4*"; + //::testing::GTEST_FLAG(filter) = "*Sphere3*"; //::testing::GTEST_FLAG(filter) = "*Barometer*"; diff --git a/math/Matrix4.h b/math/Matrix4.h new file mode 100644 index 0000000..07e8001 --- /dev/null +++ b/math/Matrix4.h @@ -0,0 +1,114 @@ +#ifndef MATRIX4_H +#define MATRIX4_H + +#include +#include + +class Matrix4 { + +private: + + float data[16]; + +public: + + Matrix4(std::initializer_list lst) { + int idx = 0; + for (float f : lst) { + data[idx] = f; ++idx; + } + } + + static Matrix4 identity() { + return Matrix4( {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1} ); + } + + static Matrix4 getRotationDeg(const float degX, const float degY, const float degZ) { + return getRotationRad(degX/180.0f*M_PI, degY/180.0f*M_PI, degZ/180.0f*M_PI); + } + + static Matrix4 getRotationRad(const float radX, const float radY, const float radZ) { + + const float g = radX; const float b = radY; const float a = radZ; + const float a11 = std::cos(a)*std::cos(b); + const float a12 = std::cos(a)*std::sin(b)*std::sin(g)-std::sin(a)*std::cos(g); + const float a13 = std::cos(a)*std::sin(b)*std::cos(g)+std::sin(a)*std::sin(g); + const float a21 = std::sin(a)*std::cos(b); + const float a22 = std::sin(a)*std::sin(b)*std::sin(g)+std::cos(a)*std::cos(g); + const float a23 = std::sin(a)*std::sin(b)*std::cos(g)-std::cos(a)*std::sin(g); + const float a31 = -std::sin(b); + const float a32 = std::cos(b)*std::sin(g); + const float a33 = std::cos(b)*std::cos(g); + return Matrix4({ + a11, a12, a13, 0, + a21, a22, a23, 0, + a31, a32, a33, 0, + 0, 0, 0, 1 + }); + + } + + static Matrix4 getTranslation(const float x, const float y, const float z) { + return Matrix4({ + 1, 0, 0, x, + 0, 1, 0, y, + 0, 0, 1, z, + 0, 0, 0, 1 + }); + } + + static Matrix4 getScale(const float x, const float y, const float z) { + return Matrix4({ + x, 0, 0, 0, + 0, y, 0, 0, + 0, 0, z, 0, + 0, 0, 0, 1 + }); + } + + float operator [] (const int idx) const {return data[idx];} + + bool operator == (const Matrix4& o) const { + for (int i = 0; i < 16; ++i) { + if (data[i] != o.data[i]) {return false;} + } + return true; + } + +}; + +struct Vector4 { + + float x,y,z,w; + + Vector4() : x(0), y(0), z(0), w(0) {;} + + Vector4(float x, float y, float z, float w) : x(x), y(y), z(z), w(w) {;} + + bool operator == (const Vector4 o) const { + return (x==o.x) && (y==o.y) && (z==o.z) && (w==o.w); + } + +}; + +inline Vector4 operator * (const Matrix4& mat, const Vector4& vec) { + + return Vector4( + (mat[ 0]*vec.x + mat[ 1]*vec.y + mat[ 2]*vec.z + mat[ 3]*vec.w), + (mat[ 4]*vec.x + mat[ 5]*vec.y + mat[ 6]*vec.z + mat[ 7]*vec.w), + (mat[ 8]*vec.x + mat[ 9]*vec.y + mat[10]*vec.z + mat[11]*vec.w), + (mat[12]*vec.x + mat[13]*vec.y + mat[14]*vec.z + mat[15]*vec.w) + ); + +} + +inline Matrix4 operator * (const Matrix4& m1, const Matrix4& m2) { + return Matrix4({ + m1[ 0]*m2[ 0] + m1[ 1]*m2[ 4] + m1[ 2]*m2[ 8] + m1[ 3]*m2[12], m1[ 0]*m2[ 1] + m1[ 1]*m2[ 5] + m1[ 2]*m2[ 9] + m1[ 3]*m2[13], m1[ 0]*m2[ 2] + m1[ 1]*m2[ 6] + m1[ 2]*m2[10] + m1[ 3]*m2[14], m1[ 0]*m2[ 3] + m1[ 1]*m2[ 7] + m1[ 2]*m2[11] + m1[ 3]*m2[15], + m1[ 4]*m2[ 0] + m1[ 5]*m2[ 4] + m1[ 6]*m2[ 8] + m1[ 7]*m2[12], m1[ 4]*m2[ 1] + m1[ 5]*m2[ 5] + m1[ 6]*m2[ 9] + m1[ 7]*m2[13], m1[ 4]*m2[ 2] + m1[ 5]*m2[ 6] + m1[ 6]*m2[10] + m1[ 7]*m2[14], m1[ 4]*m2[ 3] + m1[ 5]*m2[ 7] + m1[ 6]*m2[11] + m1[ 7]*m2[15], + m1[ 8]*m2[ 0] + m1[ 9]*m2[ 4] + m1[10]*m2[ 8] + m1[11]*m2[12], m1[ 8]*m2[ 1] + m1[ 9]*m2[ 5] + m1[10]*m2[ 9] + m1[11]*m2[13], m1[ 8]*m2[ 2] + m1[ 9]*m2[ 6] + m1[10]*m2[10] + m1[11]*m2[14], m1[ 8]*m2[ 3] + m1[ 9]*m2[ 7] + m1[10]*m2[11] + m1[11]*m2[15], + m1[12]*m2[ 0] + m1[13]*m2[ 4] + m1[14]*m2[ 8] + m1[15]*m2[12], m1[12]*m2[ 1] + m1[13]*m2[ 5] + m1[14]*m2[ 9] + m1[15]*m2[13], m1[12]*m2[ 2] + m1[13]*m2[ 6] + m1[14]*m2[10] + m1[15]*m2[14], m1[12]*m2[ 3] + m1[13]*m2[ 7] + m1[14]*m2[11] + m1[15]*m2[15] + }); +} + +#endif // MATRIX4_H diff --git a/tests/geo/TestSphere3.cpp b/tests/geo/TestSphere3.cpp new file mode 100644 index 0000000..937ff65 --- /dev/null +++ b/tests/geo/TestSphere3.cpp @@ -0,0 +1,63 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" + +#include "../../geo/Sphere3.h" + +TEST(Sphere3, contains) { + + Sphere3 a(Point3(0,0,0), 10); + + Sphere3 b1(Point3(8,0,0), 1); + Sphere3 b2(Point3(0,8,0), 1); + Sphere3 b3(Point3(0,0,8), 1); + ASSERT_TRUE(a.contains(b1)); + ASSERT_TRUE(a.contains(b2)); + ASSERT_TRUE(a.contains(b3)); + ASSERT_FALSE(b1.contains(a)); + ASSERT_FALSE(b2.contains(a)); + ASSERT_FALSE(b3.contains(a)); + + Sphere3 c1(Point3(8,0,0), 2.01); + Sphere3 c2(Point3(0,8,0), 2.01); + Sphere3 c3(Point3(0,0,8), 2.01); + ASSERT_FALSE(a.contains(c1)); + ASSERT_FALSE(a.contains(c2)); + ASSERT_FALSE(a.contains(c3)); + ASSERT_FALSE(c1.contains(a)); + ASSERT_FALSE(c2.contains(a)); + ASSERT_FALSE(c3.contains(a)); + +} + +TEST(Sphere3, join) { + + // no overlap + Sphere3 a1(Point3(-1,0,0), 1); + Sphere3 a2(Point3(+1,0,0), 1); + Sphere3 a3 = Sphere3::join(a1, a2); + ASSERT_EQ(Point3(0,0,0), a3.center); + ASSERT_EQ(2, a3.radius); + + // overlap + Sphere3 b1(Point3(-1,0,0), 1.5); + Sphere3 b2(Point3(+1,0,0), 1.5); + Sphere3 b3 = Sphere3::join(b1, b2); + ASSERT_EQ(Point3(0,0,0), b3.center); + ASSERT_EQ(2.5, b3.radius); + + // fully within + Sphere3 c1(Point3(-1,0,0), 3.6); + Sphere3 c2(Point3(+1,0,0), 1.5); + Sphere3 c3 = Sphere3::join(c1, c2); + ASSERT_EQ(c1.center, c3.center); + ASSERT_EQ(c1.radius, c3.radius); + Sphere3 c4 = Sphere3::join(c2, c1); + ASSERT_EQ(c1.center, c4.center); + ASSERT_EQ(c1.radius, c4.radius); + +} + + + +#endif diff --git a/tests/geo/TestTriangle3.cpp b/tests/geo/TestTriangle3.cpp new file mode 100644 index 0000000..fe3b892 --- /dev/null +++ b/tests/geo/TestTriangle3.cpp @@ -0,0 +1,36 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" +#include "../../geo/Triangle3.h" + +// https://stackoverflow.com/questions/17458562/efficient-aabb-triangle-intersection-in-c-sharp +// http://fileadmin.cs.lth.se/cs/Personal/Tomas_Akenine-Moller/code/tribox3.txt +TEST(Triangle3, intersect) { + + Point3 dst; + + { + Triangle3 tria(Point3(-1,0,0), Point3(+1,0,0), Point3(0,1,0)); + Ray3 ray(Point3(0,0.5,-1), Point3(0,0,1)); + ASSERT_TRUE(tria.intersects(ray, dst)); + ASSERT_EQ(Point3(0, 0.5, 0), dst); + } + + + { + Triangle3 tria(Point3(-1,0,0), Point3(+1,0,0), Point3(0,1,1)); + Ray3 ray(Point3(0,0.5,-1), Point3(0,0,1)); + ASSERT_TRUE(tria.intersects(ray, dst)); + ASSERT_EQ(Point3(0, 0.5, 0.5), dst); + } + + { + Triangle3 tria(Point3(10.4253730774, 49.0029830933, 0.00995028018951), Point3(10.4253730774, 49.0029830933, 3.99004983902), Point3(10.4253730774, 50.197013855, 3.99004983902)); + Ray3 ray(Point3(67.2000045776, 36.0, 3.5), Point3(0.154123231769, -0.631025910378, -0.76029753685)); + ASSERT_FALSE(tria.intersects(ray, dst)); + } + + +} + +#endif diff --git a/tests/math/TestMatrix4.cpp b/tests/math/TestMatrix4.cpp new file mode 100644 index 0000000..d664259 --- /dev/null +++ b/tests/math/TestMatrix4.cpp @@ -0,0 +1,37 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" +#include "../../math/Matrix4.h" + + +TEST(Matrix4, mul) { + + Matrix4 mat1 = Matrix4::identity(); + Matrix4 mat2 = Matrix4::identity(); + + ASSERT_EQ(mat1, mat2); + +} + +TEST(Matrix4, vecMul) { + + Matrix4 mat = Matrix4::identity(); + + Vector4 vec(1,1,1,1); + + Vector4 v2 = mat*vec; + + int i = 0; (void) i; + +} + +TEST(Matrix4, rot) { + + Matrix4 mat = Matrix4::getRotationDeg(0,0,0); + Vector4 vec(1,0,0,1); + Vector4 v2 = mat*vec; + ASSERT_EQ(v2, vec); + +} + +#endif diff --git a/tests/ray/TestDataMap3.cpp b/tests/ray/TestDataMap3.cpp new file mode 100644 index 0000000..a233d12 --- /dev/null +++ b/tests/ray/TestDataMap3.cpp @@ -0,0 +1,27 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" +#include "../../wifi/estimate/ray3/DataMap3.h" + +TEST(DataMap3, test) { + + BBox3 bbox(Point3(0,0,0), Point3(10,10,10)); + + DataMap3 dm; + dm.resize(bbox, 10); + + dm.set(0,0,0, 10); + ASSERT_EQ(10, dm.get(0,0,0)); + + dm.set(0,5,0, 11); + ASSERT_EQ(11, dm.get(0,5,0)); + + dm.set(5,0,0, 12); + ASSERT_EQ(12, dm.get(5,0,0)); + + dm.set(0,0,5, 13); + ASSERT_EQ(13, dm.get(0,0,5)); + +} + +#endif diff --git a/tests/ray/TestModelFac.cpp b/tests/ray/TestModelFac.cpp new file mode 100644 index 0000000..6238819 --- /dev/null +++ b/tests/ray/TestModelFac.cpp @@ -0,0 +1,22 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" +#include "../../wifi/estimate/ray3/ModelFactory.h" +#include "../../floorplan/v2/FloorplanReader.h" +#include + +TEST(Ray, ModelFac) { + + std::string file = "/mnt/data/workspaces/IndoorMap/maps/SHL39.xml"; + Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file); + + ModelFactory fac(map); + fac.triangulize(); + + std::ofstream out("/mnt/vm/fhws.obj"); + out << fac.toOBJ() << std::endl; + out.close(); + +} + +#endif diff --git a/tests/ray/TestRayTrace3.cpp b/tests/ray/TestRayTrace3.cpp new file mode 100644 index 0000000..edccf3c --- /dev/null +++ b/tests/ray/TestRayTrace3.cpp @@ -0,0 +1,66 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" +#include "../../wifi/estimate/ray3/WifiRayTrace3D.h" +#include "../../floorplan/v2/FloorplanReader.h" +#include + +TEST(RayTrace3, test) { + + + //std::string file = "/mnt/data/workspaces/raytest2.xml"; + //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file); + //Floorplan::AccessPoint* ap = map->floors[0]->accesspoints[0]; + + std::string file = "/mnt/data/workspaces/IndoorMap/maps/SHL39.xml"; + Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file); + Floorplan::AccessPoint* ap = map->floors[0]->accesspoints[4]; + + ModelFactory fac(map); + std::ofstream outOBJ("/mnt/vm/map.obj"); + outOBJ << fac.toOBJ(); + outOBJ.close(); + + const int gs_cm = 100; + + WiFiRaytrace3D rt(map, gs_cm, ap->pos); + const DataMap3Signal& dms = rt.estimate(); + + const char sep = ';'; + + std::ofstream out("/mnt/vm/rays.xyz.txt"); + auto lambda = [&] (const float x, const float y, const float z, const DataMap3SignalEntry& e) { + + const float min = -120; + const float max = -40; + float rssi = e.getMaxRSSI(); + if (rssi > max) {rssi = max;} + + if (rssi > -100) { + const float v = ((rssi - min) / (max-min)) * 255; + out + << x << sep << y << sep << z << sep + << v << sep << v << sep << v + << "\n"; + } + }; + + + dms.forEach(lambda); + out.close(); + + std::ofstream outHits("/mnt/vm/hits.xyz.txt"); + for (const Point3 hit : rt.getHitEnter()) { + outHits << hit.x << sep << hit.y << sep << hit.z << sep << 0 << sep << 255 << sep << 0 << "\n"; + } + for (const Point3 hit : rt.getHitLeave()) { + outHits << hit.x << sep << hit.y << sep << hit.z << sep << 0 << sep << 0 << sep << 255 << "\n"; + } + for (const Point3 hit : rt.getHitStop()) { + outHits << hit.x << sep << hit.y << sep << hit.z << sep << 0 << sep << 0 << sep << 0 << "\n"; + } + outHits.close(); + +} + +#endif diff --git a/wifi/estimate/ray2d/DataMap2.h b/wifi/estimate/ray2d/DataMap2.h new file mode 100644 index 0000000..2a38a1e --- /dev/null +++ b/wifi/estimate/ray2d/DataMap2.h @@ -0,0 +1,244 @@ +#ifndef DATAMAP2_H +#define DATAMAP2_H + +#include "../../../geo/BBox2.h" +#include + +template class DataMap { + +private: + + float sx_m; + float sy_m; + + float x_m; + float y_m; + int gridSize_cm; + + BBox2 bbox; + + int nx; + int ny; + + T* data = nullptr; + +public: + + /** ctor */ + DataMap() { + ; + } + + ~DataMap() { + + // cleanup + cleanup(); + + } + + DataMap(const DataMap&) = delete; + DataMap* operator = (const DataMap& o) = delete; + + /* + void blured(DataMap& dst) const { + + const int s = 2; + dst.resize(this->bbox, this->gridSize_cm); + + for (int iy = 0; iy < ny; ++iy) { + for (int ix = 0; ix < nx; ++ix) { + + float valSum = 0; + int cntSum = 0; + + for (int oy = -s; oy <= +s; ++oy) { + for (int ox = -s; ox <= +s; ++ox) { + + const int x = ix+ox; + const int y = iy+oy; + + if (containsGrid(x,y)) { + valSum += getGrid(x,y); + ++cntSum; + } + + } + } + + dst.setGrid(ix, iy, valSum/cntSum); + + } + } + + } + */ + + /** does the map contain the given indices? */ + bool containsGrid(const int x, const int y) const { + return (x >= 0) && (y >= 0) && (x < nx) && (y < ny); + } + + /** does the map contain the given coordinate? */ + bool contain(const float x_m, const float y_m) const { + return bbox.contains(Point2(x_m, y_m)); + } + + void resize(const BBox2 bbox, const int gridSize_cm) { + + // cleanup + cleanup(); + + this->bbox = bbox; + + // 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; + + // 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; + + // number of elements in the grid + this->gridSize_cm = gridSize_cm; + nx = (x_m*100) / gridSize_cm; + ny = (y_m*100) / gridSize_cm; + + // allocate and reset all to 0.0 + data = new T[nx*ny]; + //std::fill(&data[0], &data[nx*ny], 0); + + } + + /** 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 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); + setGrid(ix, iy, val); + } + + T get(const float x_m, const float y_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 ); + return getGrid(ix, iy); + } + + T& getRef(const float x_m, const float y_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 ); + return getGridRef(ix, iy); + } + + T getGrid(const int ix, const int iy) const { + Assert::isBetween(ix, 0, nx-1, "x out of range"); + Assert::isBetween(iy, 0, ny-1, "y out of range"); + const int idx = ix + iy*nx; + return data[idx]; + } + + T& getGridRef(const int ix, const int iy) { + Assert::isBetween(ix, 0, nx-1, "x out of range"); + Assert::isBetween(iy, 0, ny-1, "y out of range"); + const int idx = ix + iy*nx; + return data[idx]; + } + + void setGrid(const int ix, const int iy, const T val) { + Assert::isBetween(ix, 0, nx-1, "x out of range"); + Assert::isBetween(iy, 0, ny-1, "y out of range"); + const int idx = ix + iy*nx; + data[idx] = val; + } + + void forEach(std::function func) const { + 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; + func(x,y,getGrid(ix, iy)); + } + } + } + + /* + 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 << " "; + } + os << "\n"; + } + os.close(); + + } + */ + +private: + + void cleanup() { + delete[] data; + data = nullptr; + } + + +}; + + + +struct DataMapSignalEntry { + + 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) { + Entry e(rssi, distanceToAP); + entries.push_back(e); + } + + 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 DataMapSignal : public DataMap { + +public: + + /** update average */ + void update(const float x_m, const float y_m, const float rssi, const float distanceToAP) { + + DataMapSignalEntry& entry = getRef(x_m, y_m); + entry.add(rssi, distanceToAP); + + } + +}; + +#endif // DATAMAP2_H diff --git a/wifi/estimate/ray2d/MaterialOptions.h b/wifi/estimate/ray2d/MaterialOptions.h new file mode 100644 index 0000000..504bfa9 --- /dev/null +++ b/wifi/estimate/ray2d/MaterialOptions.h @@ -0,0 +1,63 @@ +#ifndef MATERIALOPTIONS_H +#define MATERIALOPTIONS_H + +#include "../../../floorplan/v2/Floorplan.h" +#include "../../../Assertions.h" + +/** raytracing attributes for one material */ +struct MaterialAttributes { + + struct Shadowing { + float attenuation; + Shadowing() : attenuation(NAN) {;} + Shadowing(float attenuation) : attenuation(attenuation) {;} + } shadowing; + + struct Reflection { + float attenuation; + Reflection() : attenuation(NAN) {;} + Reflection(float attenuation) : attenuation(attenuation) {;} + } reflection; + + MaterialAttributes(float shadowA, float reflectA) : shadowing(shadowA), reflection(reflectA) {;} + + MaterialAttributes() {;} + +}; + +class Materials { + +public: + + /** singleton access */ + static Materials& get() { + static Materials instance; + return instance; + } + + /** 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]; + } + +private: + + std::vector materials; + + /** 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/ray2d/Ray2.h b/wifi/estimate/ray2d/Ray2.h index 8da2e71..258f066 100644 --- a/wifi/estimate/ray2d/Ray2.h +++ b/wifi/estimate/ray2d/Ray2.h @@ -1,4 +1,24 @@ #ifndef RAY2_H #define RAY2_H +#include "../../../geo/Point2.h" + +struct Ray2 { + + /** starting position */ + Point2 start; + + /** ray direction */ + Point2 dir; + + /** empty ctor */ + Ray2() {;} + + /** ctor */ + Ray2(const Point2 start, const Point2 dir) : start(start), dir(dir.normalized()) { + ; + } + +}; + #endif // RAY2_H diff --git a/wifi/estimate/ray2d/WiFiRayTrace2D.h b/wifi/estimate/ray2d/WiFiRayTrace2D.h index 88ba9ac..f85eace 100644 --- a/wifi/estimate/ray2d/WiFiRayTrace2D.h +++ b/wifi/estimate/ray2d/WiFiRayTrace2D.h @@ -1,4 +1,314 @@ #ifndef WIFIRAYTRACE2D_H #define WIFIRAYTRACE2D_H +#include "../../../geo/Point2.h" +#include "../../../geo/Line2.h" +#include "../../../geo/BBox2.h" + +#include "../../../floorplan/v2/Floorplan.h" +#include "../../../floorplan/v2/FloorplanHelper.h" + +#include "DataMap2.h" +#include "Ray2.h" +#include "MaterialOptions.h" + +#include + +// http://www.realtimerendering.com/resources/RTNews/html/rtnv10n1.html#art3 +// http://math.stackexchange.com/questions/13261/how-to-get-a-reflection-vector + +// RADIO WAVES +// http://people.seas.harvard.edu/%7Ejones/es151/prop_models/propagation.html Indoor Wireless RF Channel +// http://www.electronics-radio.com/articles/antennas-propagation/propagation-overview/radio-em-wave-reflection.php + + + + +struct StateRay2 : public Ray2 { + + /** already travelled distance from the AP (by all previous rays */ + float totalLength; + + /** attenuation taken since the start */ + float totalAttenuation; + + void* lastObstacle; + int depth = 0; + + /** empty ctor */ + StateRay2() : totalLength(NAN), totalAttenuation(NAN) {;} + + /** ctor */ + StateRay2(const Point2 start, const Point2 dir) : Ray2(start, dir), totalLength(0), totalAttenuation(0) { + ; + } + + inline float getRSSI(const float addDist = 0) const { + const float txp = -40; + const float gamma = 1.2f; + return (txp - 10*gamma*std::log10(totalLength + addDist)) - totalAttenuation; + } + +}; + +struct Hit { + void* obstacle; + float dist; + Point2 pos; + Point2 normal; + Floorplan::Material material; + bool stopHere = false; + Hit() {;} + Hit(const float dist, const Point2 pos, const Point2 normal) : dist(dist), pos(pos), normal(normal) {;} +}; + + + +class WiFiRaytrace2D { + +private: + + const Floorplan::Floor* floor; + BBox2 bbox; + Point2 apPos; + + DataMapSignal dm; + + struct Limit { + static constexpr int RAYS = 2000; + static constexpr int HITS = 11; + static constexpr float RSSI = -110; + }; + +public: + + /** ctor */ + WiFiRaytrace2D(const Floorplan::Floor* floor, const int gs, const Point2 apPos) : floor(floor), apPos(apPos) { + + // get the floor's 3D-bbox + BBox3 bb3 = FloorplanHelper::getBBox(floor); + + // 2D only party + bbox = BBox2(bb3.getMin().xy(), bb3.getMax().xy()); + + // allocate + dm.resize(bbox, gs); + + } + + const DataMapSignal& estimate() { + + for (int i = 0; i < Limit::RAYS; ++i) { + + std::cout << "ray: " << i << std::endl; + + // angle between starting-rays + const float angle = (float)M_PI*2.0f * i / Limit::RAYS; + + // direction + const Point2 dir(std::cos(angle), std::sin(angle)); + + // ray + const StateRay2 ray(apPos, dir); + + // run! + trace(ray); + + } + + return dm; + + } + + + +private: + + static float dot(const Point2 p1, const Point2 p2) { + return (p1.x * p2.x) + (p1.y * p2.y); + } + + void trace(const StateRay2& ray) { + + + // get the nearest intersection with the floorplan + const Hit hit = getNearestHit(ray); + + // rasterize the ray's way onto the map + rasterize(ray, hit); + + // continue? + if (hit.stopHere) {return;} + //const float curLength = ray.totalLength + hit.dist; + //if (curLength > 55) {return;} + if (ray.getRSSI(hit.dist) < Limit::RSSI) {return;} + if (ray.depth > Limit::HITS) {return;} + + + // apply effects + //reflected(ray, hit); + shadowed(ray, hit); + + + } + + static inline float getAttenuation(const Hit& h) { + return Materials::get().getAttributes(h.material).shadowing.attenuation; + } + + static inline float getAttenuationForReflection(const Hit& h) { + return Materials::get().getAttributes(h.material).reflection.attenuation; + } + + /** perform reflection and continue tracing */ + void shadowed(const StateRay2& ray, const Hit& hit) { + + StateRay2 next = ray; // continue into the same direction + next.depth = ray.depth + 1; + next.lastObstacle = hit.obstacle; + next.start = hit.pos; + next.totalLength += hit.dist; + next.totalAttenuation += getAttenuation(hit); // contribute attenuation + + trace(next); + + } + + /** perform reflection and continue tracing */ + void reflected(const StateRay2& ray, const Hit& hit) { + + Assert::isNear(1.0f, ray.dir.length(), 0.01f, "not normalized"); + Assert::isNear(1.0f, hit.normal.length(), 0.01f, "not normalized"); + + // angle to wide or narrow? -> skip + const float d = std::abs(dot(ray.dir, hit.normal)); + if (d < 0.05) {return;} // parallel + if (d > 0.95) {return;} // perpendicular; + + static std::minstd_rand gen; + static std::normal_distribution dist(0, 0.02); + const float mod = dist(gen); + Point2 normalMod( + hit.normal.x * std::cos(mod) - hit.normal.y * std::sin(mod), + hit.normal.y * std::cos(mod) - hit.normal.x * std::sin(mod) + ); + + + // reflected ray; + StateRay2 reflected; + reflected.depth = ray.depth + 1; + reflected.lastObstacle = hit.obstacle; + reflected.start = hit.pos; + reflected.totalLength = ray.totalLength + hit.dist; + reflected.totalAttenuation = ray.totalAttenuation + getAttenuationForReflection(hit); // TODO + reflected.dir = (ray.dir - normalMod * (dot(ray.dir, hit.normal)) * 2).normalized(); // slight variation + + trace(reflected); + + } + + + + Hit getNearestHit(const StateRay2& ray) { + + Assert::isNear(1.0f, ray.dir.length(), 0.01f, "not normalized!"); + + const Line2 longRay(ray.start, ray.start + ray.dir*100); + + const float minDist = 0;//0.01; // prevent errors hitting the same obstacle twice + const float MAX = 999999; + Hit nearest; nearest.dist = MAX; + + // check intersection with walls + for (Floorplan::FloorObstacle* fo : floor->obstacles) { + + // do not hit the last obstacle again + if (ray.lastObstacle == fo) {continue;} + + // get the line + const Floorplan::FloorObstacleLine* line = dynamic_cast(fo); + const Floorplan::FloorObstacleDoor* door = dynamic_cast(fo); + if (!line && !door) {continue;} + Line2 obstacle; + if (line) {obstacle = Line2(line->from, line->to);} + if (door) {obstacle = Line2(door->from, door->to);} + + Point2 hit; + if (obstacle.getSegmentIntersection(longRay, hit)) { + const float dist = hit.getDistance(ray.start); + if (dist > minDist && dist < nearest.dist) { + nearest.obstacle = fo; + nearest.dist = dist; + nearest.pos = hit; + nearest.normal = (obstacle.p2 - obstacle.p1).perpendicular().normalized(); + nearest.material = fo->material; + } + } + } + + // no hit with floorplan: limit to bounding-box! + if (nearest.dist == MAX) { + //const BBox2 bb( Point2(0,0), Point2(w, h) ); + Point2 hit; + for (Line2 l : bbox.lines()) { + if (l.getSegmentIntersection(longRay, hit)) { + nearest.obstacle = nullptr; + nearest.dist = hit.getDistance(ray.start); + nearest.pos = hit; + nearest.normal = (l.p2 - l.p1).perpendicular().normalized(); + nearest.material = Floorplan::Material::UNKNOWN; + nearest.stopHere = true; + } + } + } + + return nearest; + + } + + /** rasterize the ray (current pos -> hit) onto the map */ + void rasterize(const StateRay2& ray, const Hit& hit) { + + const Point2 dir = hit.pos - ray.start; + const Point2 dirN = dir.normalized(); + const Point2 step = dirN * (dm.getGridSize_cm()/100.0f) * 0.7f; // 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 Point2 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.totalLength + partLen; + +// // ray stronger than current rssi? +// if (curRSSI == 0 || curRSSI < newRSSI) { +// dm.set(dst.x, dst.y, newRSSI); +// } + + dm.update(dst.x, dst.y, newRSSI, totalLen); + + } + + + } + + + + +}; + #endif // WIFIRAYTRACE2D_H diff --git a/wifi/estimate/ray3/Cube.h b/wifi/estimate/ray3/Cube.h new file mode 100644 index 0000000..2beb8ae --- /dev/null +++ b/wifi/estimate/ray3/Cube.h @@ -0,0 +1,141 @@ +#ifndef QUBE_H +#define QUBE_H + +#include +#include "../../../geo/Triangle3.h" +#include "../../../math/Matrix4.h" + +class Cube { + +private: + + std::vector trias; + +public: + + /** ctor */ + Cube() { + unitCube(true); + } + + /** ctor with position, size and rotation */ + Cube(Point3 pos, Point3 size, Point3 rot_deg, const bool topAndBottom = true) { + unitCube(topAndBottom); + const Matrix4 mRot = Matrix4::getRotationDeg(rot_deg.x, rot_deg.y, rot_deg.z); + const Matrix4 mSize = Matrix4::getScale(size.x, size.y, size.z); + const Matrix4 mPos = Matrix4::getTranslation(pos.x, pos.y, pos.z); + const Matrix4 mat = mPos * mRot * mSize; + transform(mat); + } + + /** get the cube's triangles */ + const std::vector getTriangles() const { + return trias; + } + + void transform(const Matrix4& mat) { + + for (Triangle3& tria : trias) { + + Vector4 v1(tria.p1.x, tria.p1.y, tria.p1.z, 1); + Vector4 v2(tria.p2.x, tria.p2.y, tria.p2.z, 1); + Vector4 v3(tria.p3.x, tria.p3.y, tria.p3.z, 1); + + v1 = mat*v1; + v2 = mat*v2; + v3 = mat*v3; + + tria.p1 = Point3(v1.x, v1.y, v1.z); + tria.p2 = Point3(v2.x, v2.y, v2.z); + tria.p3 = Point3(v3.x, v3.y, v3.z); + + } + + } + + /** 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) + ); + + } + + + + if (topAndBottom) { + + // top + addQuad( + Point3(+s, +s, +s), + Point3(-s, +s, +s), + Point3(-s, -s, +s), + Point3(+s, -s, +s) + ); + + // bottom + addQuad( + Point3(+s, -s, -s), + Point3(-s, -s, -s), + Point3(-s, +s, -s), + Point3(+s, +s, -s) + ); + + } + + } + + void addQuad(Point3 p1, Point3 p2, Point3 p3, Point3 p4) { + trias.push_back( Triangle3(p1,p2,p3) ); + trias.push_back( Triangle3(p1,p3,p4) ); + } + +}; + +#endif // QUBE_H diff --git a/wifi/estimate/ray3/DataMap3.h b/wifi/estimate/ray3/DataMap3.h new file mode 100644 index 0000000..f72106a --- /dev/null +++ b/wifi/estimate/ray3/DataMap3.h @@ -0,0 +1,229 @@ +#ifndef DATAMAP3_H +#define DATAMAP3_H + + +#include "../../../geo/BBox3.h" +#include +#include + +template class DataMap3 { + +private: + + float sx_m; + float sy_m; + float sz_m; + + float x_m; + float y_m; + float z_m; + + int gridSize_cm; + + BBox3 bbox; + + int nx; + int ny; + int nz; + + T* data = nullptr; + +public: + + /** ctor */ + DataMap3() { + ; + } + + ~DataMap3() { + + // cleanup + cleanup(); + + } + + 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 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) { + + // cleanup + cleanup(); + + this->bbox = bbox; + + // 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; + + // 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; + + // 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;} + + 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& 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& 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 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() { + + 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.close(); + + } + */ + +private: + + 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) { + Entry e(rssi, distanceToAP); + entries.push_back(e); + } + + 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/MaterialOptions.h b/wifi/estimate/ray3/MaterialOptions.h new file mode 100644 index 0000000..37565c0 --- /dev/null +++ b/wifi/estimate/ray3/MaterialOptions.h @@ -0,0 +1,64 @@ +#ifndef MATERIALOPTIONS_H +#define MATERIALOPTIONS_H + + +#include "../../../floorplan/v2/Floorplan.h" +#include "../../../Assertions.h" + +/** raytracing attributes for one material */ +struct MaterialAttributes { + + struct Shadowing { + float attenuation; + Shadowing() : attenuation(NAN) {;} + Shadowing(float attenuation) : attenuation(attenuation) {;} + } shadowing; + + struct Reflection { + float attenuation; + Reflection() : attenuation(NAN) {;} + Reflection(float attenuation) : attenuation(attenuation) {;} + } reflection; + + MaterialAttributes(float shadowA, float reflectA) : shadowing(shadowA), reflection(reflectA) {;} + + MaterialAttributes() {;} + +}; + +class Materials { + +public: + + /** singleton access */ + static Materials& get() { + static Materials instance; + return instance; + } + + /** 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]; + } + +private: + + std::vector materials; + + /** 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 new file mode 100644 index 0000000..91f0b83 --- /dev/null +++ b/wifi/estimate/ray3/ModelFactory.h @@ -0,0 +1,201 @@ +#ifndef MODELFACTORY_H +#define MODELFACTORY_H + +#include "../../../floorplan/v2/Floorplan.h" +#include "../../../geo/Triangle3.h" +#include "ModelFactoryHelper.h" +#include "Obstacle3.h" +#include "Cube.h" + +/** + * convert an indoor map into a 3D model based on triangles + */ +class ModelFactory { + +private: + + bool exportCeilings = true; + bool exportObstacles = true; + bool exportWallTops = false; + + const Floorplan::IndoorMap* map; + +public: + + + /** ctor */ + ModelFactory(const Floorplan::IndoorMap* map) : map(map) { + + } + + + /** get all triangles grouped by obstacle */ + std::vector triangulize() { + + std::vector res; + + // process each floor + for (const Floorplan::Floor* f : map->floors) { + + // triangulize the floor itself (floor/ceiling) + if (exportCeilings) {res.push_back(getTriangles(f));} + + // 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; + + } + + return res; + + } + + + /** DEBUG: convert to .obj file code for exporting */ + std::string toOBJ() { + + const std::vector obs = triangulize(); + + int nVerts = 1; + int nObjs = 0; + std::string res; + + // write each obstacle + for (const Obstacle3D& o : obs) { + + // write the vertices + for (const Triangle3& t : o.triangles) { + res += "v " + std::to_string(t.p1.x) + " " + std::to_string(t.p1.y) + " " + std::to_string(t.p1.z) + "\n"; + res += "v " + std::to_string(t.p2.x) + " " + std::to_string(t.p2.y) + " " + std::to_string(t.p2.z) + "\n"; + res += "v " + std::to_string(t.p3.x) + " " + std::to_string(t.p3.y) + " " + std::to_string(t.p3.z) + "\n"; + } + + // create a new group + 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; + + } + +private: + + /** convert a floor (floor/ceiling) into triangles */ + Obstacle3D getTriangles(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) + + // TODO: variable type? + Obstacle3D res(Floorplan::Material::CONCRETE); + + Polygon poly; + + // append all "add" and "remove" areas + for (Floorplan::FloorOutlinePolygon* fop : f->outline) { + switch (fop->method) { + case Floorplan::OutlineMethod::ADD: poly.add(fop->poly); break; + case Floorplan::OutlineMethod::REMOVE: poly.remove(fop->poly); break; + default: throw 1; + } + } + + // convert them into polygons + std::vector> polys = poly.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 + res.triangles.push_back(tria1); + res.triangles.push_back(tria2); + + } + } + + return res; + + } + + /** convert a line obstacle to 3D triangles */ + Obstacle3D getTriangles(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol) { + + /* + Obstacle3D res(fol->material); + + Point3 p1(fol->from.x, fol->from.y, f->getStartingZ()); + Point3 p2(fol->to.x, fol->to.y, f->getStartingZ()); + Point3 p3(fol->to.x, fol->to.y, f->getEndingZ()); + Point3 p4(fol->from.x, fol->from.y, f->getEndingZ()); + + Triangle3 t1(p1,p2,p3); + Triangle3 t2(p1,p3,p4); + + res.triangles.push_back(t1); + res.triangles.push_back(t2); + + */ + + const float thickness_m = fol->thickness_m; + const Point2 from = fol->from; + const Point2 to = fol->to; + const Point2 cen2 = (from+to)/2; + + const float rad = std::atan2(to.y - from.y, to.x - from.x); + const float deg = rad * 180 / M_PI; + + // cube's destination center + const Point3 pos(cen2.x, cen2.y, f->atHeight + f->height/2); + + // div by 2.01 to prevent overlapps and z-fi + const float sx = from.getDistance(to) / 2.01f; + const float sy = thickness_m / 2.01f; + const float sz = f->height / 2.01f; // prevent overlaps + const Point3 size(sx, sy, sz); + const Point3 rot(0,0,deg); + + // build + Cube cube(pos, size, rot); + + // done + Obstacle3D res(fol->material); + res.triangles = cube.getTriangles(); + return res; + + } + + + +}; + +#endif // MODELFACTORY_H diff --git a/wifi/estimate/ray3/ModelFactoryHelper.h b/wifi/estimate/ray3/ModelFactoryHelper.h new file mode 100644 index 0000000..a48688f --- /dev/null +++ b/wifi/estimate/ray3/ModelFactoryHelper.h @@ -0,0 +1,105 @@ +#ifndef MODELFACTORYHELPER_H +#define MODELFACTORYHELPER_H + +#include +#include "../../../lib/gpc/gpc.h" + +class Polygon { + + 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; + } + 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); + + } + +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 new file mode 100644 index 0000000..115a88b --- /dev/null +++ b/wifi/estimate/ray3/Obstacle3.h @@ -0,0 +1,24 @@ +#ifndef OBSTACLE3_H +#define OBSTACLE3_H + +#include +#include "../../../geo/Triangle3.h" +#include "../../../geo/Sphere3.h" + +#include "../../../floorplan/v2/Floorplan.h" + +struct Obstacle3D { + + Floorplan::Material mat; + std::vector triangles; + + /** empty ctor */ + Obstacle3D() : mat() {;} + + /** ctor */ + Obstacle3D(Floorplan::Material mat) : mat(mat) {;} + +}; + + +#endif // OBSTACLE3_H diff --git a/wifi/estimate/ray3/ObstacleTree.h b/wifi/estimate/ray3/ObstacleTree.h new file mode 100644 index 0000000..8f08179 --- /dev/null +++ b/wifi/estimate/ray3/ObstacleTree.h @@ -0,0 +1,139 @@ +#ifndef OBSTACLETREE_H +#define OBSTACLETREE_H + +#include "../../../geo/Sphere3.h" +#include "Obstacle3.h" +#include + +struct ObstacleNode { + bool isLeaf = true; + Sphere3 boundSphere; + std::vector sub; + ObstacleNode(bool isLeaf = false) : isLeaf(isLeaf) {;} +}; + +struct ObstacleLeaf : public ObstacleNode { + Obstacle3D obs; + ObstacleLeaf() : ObstacleNode(true) {;} +}; + + + +class ObstacleTree { + + ObstacleNode root; + +public: + + /** append a new leaf */ + void add(ObstacleLeaf* leaf) { + root.sub.push_back(leaf); + } + + void optimize() { + while(true) { + bool did = concat(); + if (!did) {break;} + } + } + + std::vector getHits(const Ray3 ray) const { + std::vector obs; + getHits(ray, &root, obs); + return obs; + } + + void getHits(const Ray3 ray, const ObstacleNode* node, std::vector& hits) const { + for (const ObstacleNode* sub : node->sub) { + if (sub->boundSphere.intersects(ray)) { + if (sub->isLeaf) { + ObstacleLeaf* leaf = (ObstacleLeaf*)(sub); + hits.push_back(&leaf->obs); + } else { + if (sub->boundSphere.intersects(ray)) {getHits(ray, sub, hits);} + } + } + } + } + + + bool concat() { + + bool concated = false; + + // first, sort all elements by radius (smallest first) + auto compRadius = [] (const ObstacleNode* l1, const ObstacleNode* l2) { + return l1->boundSphere.radius < l2->boundSphere.radius; + }; + std::sort(root.sub.begin(), root.sub.end(), compRadius); + + ObstacleNode newRoot; + + // combine nearby elements + //for (size_t i = 0; i < root.sub.size(); ++i) { + while(true) { + + // get [and remove] the next element + ObstacleLeaf* l0 = (ObstacleLeaf*) root.sub[0]; + root.sub.erase(root.sub.begin()+0); + + // find another element that yields minimal increase in volume + auto compNear = [l0] (const ObstacleNode* l1, const ObstacleNode* l2) { + const float d1 = Sphere3::join(l0->boundSphere, l1->boundSphere).radius; + const float d2 = Sphere3::join(l0->boundSphere, l2->boundSphere).radius; + return d1 < d2; + }; + auto it = std::min_element(root.sub.begin(), root.sub.end(), compNear); + ObstacleNode* l1 = *it; + + float increment = Sphere3::join(l0->boundSphere, l1->boundSphere).radius / l0->boundSphere.radius; + + const bool combine = (root.sub.size() > 1) && (it != root.sub.end()) && (increment < 1.75); + + if (combine) { + + // combine both into a new node + ObstacleNode* node = new ObstacleNode(); + node->sub.push_back(l0); + node->sub.push_back(*it); + node->boundSphere = Sphere3::join(l0->boundSphere, (*it)->boundSphere); + root.sub.erase(it); + newRoot.sub.push_back(node); + + concated = true; + + } else { + + ObstacleNode* node = new ObstacleNode(); + node->sub.push_back(l0); + node->boundSphere = l0->boundSphere; + newRoot.sub.push_back(node); + + } + + // done? + if (root.sub.size() == 1) { + + ObstacleNode* node = new ObstacleNode(); + node->sub.push_back(root.sub.front()); + node->boundSphere = root.sub.front()->boundSphere; + newRoot.sub.push_back(node); + break; + + } else if (root.sub.size() == 0) { + break; + } + + //--i; + + } + + root = newRoot; + + return concated; + + } + +}; + +#endif // OBSTACLETREE_H diff --git a/wifi/estimate/ray3/WifiRayTrace3D.h b/wifi/estimate/ray3/WifiRayTrace3D.h new file mode 100644 index 0000000..06321d3 --- /dev/null +++ b/wifi/estimate/ray3/WifiRayTrace3D.h @@ -0,0 +1,473 @@ +#ifndef WIFIRAYTRACE3D_H +#define WIFIRAYTRACE3D_H + +#include "../../../geo/Point2.h" +#include "../../../geo/Line2.h" +#include "../../../geo/BBox2.h" + +#include "../../../floorplan/v2/Floorplan.h" +#include "../../../floorplan/v2/FloorplanHelper.h" + +#include "DataMap3.h" +#include "../../../geo/Ray3.h" +#include "MaterialOptions.h" +#include "Obstacle3.h" +#include "ModelFactory.h" +#include "ObstacleTree.h" + +#include + +// http://www.realtimerendering.com/resources/RTNews/html/rtnv10n1.html#art3 +// http://math.stackexchange.com/questions/13261/how-to-get-a-reflection-vector + +// RADIO WAVES +// http://people.seas.harvard.edu/%7Ejones/es151/prop_models/propagation.html Indoor Wireless RF Channel +// http://www.electronics-radio.com/articles/antennas-propagation/propagation-overview/radio-em-wave-reflection.php + + +// 3D +// http://graphics.stanford.edu/courses/cs148-10-summer/docs/2006--degreve--reflection_refraction.pdf + + +struct Intersection { + Point3 pos; + const Obstacle3D* obs; + Intersection(const Point3 pos, const Obstacle3D* obs) : pos(pos), obs(obs) {;} +}; + +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 { + + 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) {;} + +}; + + + + + + + + + + +class WiFiRaytrace3D { + +private: + + BBox3 bbox; + Point3 apPos; + + DataMap3Signal dm; + + //std::vector obstacles; + ObstacleTree tree; + + struct Limit { + static constexpr int RAYS = 1000; + static constexpr int HITS = 16; + 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.triangulize(); + + + + // build bounding volumes + for (Obstacle3D& obs : obstacles) { + ObstacleLeaf* leaf = new ObstacleLeaf(); + leaf->obs = obs; + leaf->boundSphere = getSphereAround(obs.triangles); + if (leaf->boundSphere.radius == 0) { + throw Exception("invalid item detected"); + } + tree.add(leaf); + } + tree.optimize(); + + 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); + + 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; + + } + + + +private: + + void trace(const StateRay3& ray) { + + // get the nearest intersection with the floorplan + const Hit3 nextHit = getNearestHit(ray); + + // stop? + if (nextHit.invalid) { + hitStop.push_back(nextHit.pos); + 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)) { + hitStop.push_back(nextHit.pos); + return; + } + + + // apply effects + if (ray.isWithin) { + leave(ray, nextHit); + hitLeave.push_back(nextHit.pos); + } else { + enter(ray, nextHit); + reflectAt(ray, nextHit); + hitEnter.push_back(nextHit.pos); + } + + + } + + static inline float getAttenuation(const Hit3& h) { + return Materials::get().getAttributes(h.material).shadowing.attenuation; + } + + static inline float getAttenuationForReflection(const Hit3& h) { + return Materials::get().getAttributes(h.material).reflection.attenuation; + } + + + /** perform reflection and continue tracing */ + void leave(const StateRay3& ray, const Hit3& hit) { + + // continue into the same direction + StateRay3 next = ray.leave(hit.pos, hit.obstacle); + + // continue + trace(next); + + } + + /** perform reflection and continue tracing */ + void enter(const StateRay3& ray, const Hit3& hit) { + + // continue into the same direction + StateRay3 next = ray.enter(hit.pos, hit.obstacle); + + // continue + trace(next); + + } + + + /** perform reflection and continue tracing */ + void reflectAt(const StateRay3& ray, const Hit3& hit) { + + if (hit.normal.length() < 0.9) { + int i = 0; (void) i; + } + + Assert::isNear(1.0f, ray.dir.length(), 0.01f, "ray's direction is not normalized"); + Assert::isNear(1.0f, hit.normal.length(), 0.01f, "obstacle's normal is not normalized"); + + // angle to wide or narrow? -> skip + const float d = std::abs(dot(ray.dir, hit.normal)); + if (d < 0.01) {return;} // parallel + if (d > 0.99) {return;} // perpendicular; + + //static std::minstd_rand gen; + //static std::normal_distribution dist(0, 0.02); + //const float mod = dist(gen); + + // reflected ray direction using surface normal + const Point3 dir = ray.dir; + const Point3 normal = hit.normal; + const Point3 refDir = dir - (normal * 2 * dot(dir, normal)); + + // reflected ray; + StateRay3 reflected = ray.reflectAt(hit.pos, hit.obstacle); + reflected.dir = refDir.normalized(); + + // continue + trace(reflected); + + } + + + + void hitTest(const StateRay3& ray, const Obstacle3D& obs, Hit3& nearest) { + + const float minDist = 0.01; // prevent errors hitting the same obstacle twice + + //bool dummy = obs.boundingSphere.intersects(ray); + + Point3 hitPoint; + for (const Triangle3& tria : obs.triangles) { + if (tria.intersects(ray, hitPoint)) { + + //if (!dummy) { + // throw Exception("issue!"); + //} + + const float dist = hitPoint.getDistance(ray.start); + if (dist > minDist && dist < nearest.dist) { + nearest.obstacle = &obs; + nearest.dist = dist; + nearest.pos = hitPoint; + nearest.normal = tria.getNormal(); + nearest.material = obs.mat; + } + } + } + + } + + + Hit3 getNearestHit(const StateRay3& ray) { + + Assert::isNear(1.0f, ray.dir.length(), 0.01f, "not normalized!"); + + 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); + +// } + + std::vector obs = tree.getHits(ray); + for (const Obstacle3D* o : obs) { + hitTest(ray, *o, nearest); + } + + } + + // no hit with floorplan: limit to bounding-box! + if (nearest.dist == MAX) { + nearest.invalid = true; + } + + return nearest; + + } + + + /** rasterize the ray (current pos -> hit) onto the map */ + void rasterize(const StateRay3& ray, const Hit3& hit) { + + const Point3 dir = hit.pos - ray.start; + const Point3 dirN = dir.normalized(); + const Point3 step = dirN * (dm.getGridSize_cm()/100.0f) * 1.0f; // TODO * 1.0 ?? + const int steps = dir.length() / step.length(); + + // sanity check + // ensure the direction towards the nearest intersection is the same as the ray's direction + // otherwise the intersection-test is invalid +#ifdef WITH_ASSERTIONS + if (dir.normalized().getDistance(ray.dir) > 0.1) { + return; + std::cout << "direction to the nearest hit is not the same direction as the ray has. incorrect intersection test?!" << std::endl; + } +#endif + + for (int i = 0; i <= steps; ++i) { + + const Point3 dst = ray.start + (step*i); + const float partLen = ray.start.getDistance(dst); + //const float len = ray.totalLength + partLen; + +// const float curRSSI = dm.get(dst.x, dst.y); + const float newRSSI = ray.getRSSI(partLen); + const float totalLen = ray.getLength() + partLen; + +// // ray stronger than current rssi? +// if (curRSSI == 0 || curRSSI < newRSSI) { +// dm.set(dst.x, dst.y, newRSSI); +// } + + dm.update(dst.x, dst.y, dst.z, newRSSI, totalLen); + + } + + + } + + + Sphere3 getSphereAround(const std::vector& 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