From 1c2081d4060649f05fce73add0b943b0bebdb86b Mon Sep 17 00:00:00 2001 From: frank Date: Tue, 3 Apr 2018 14:55:59 +0200 Subject: [PATCH] worked on 3d models within map adjusted grid factory adjusted nav mesh factory minoor changes/fixes new helper classes refactoring --- geo/BBox2.h | 6 + geo/BBox3.h | 13 +- geo/Triangle3.h | 42 ++++++ grid/Grid.h | 5 + grid/factory/v2/GridFactory.h | 68 +++++++++- grid/factory/v2/Helper.h | 13 ++ .../v2/modules/WalkModuleFollowDestination.h | 4 + grid/walk/v3/ReachableSampler.h | 4 +- grid/walk/v3/WalkEvaluator.h | 4 +- grid/walk/v3/Walker.h | 2 +- math/Math.h | 11 ++ math/distribution/ChiSquared.h | 59 ++++++++ math/distribution/Normal.h | 23 ++-- math/distribution/NormalN.h | 39 ++++++ math/distribution/Uniform.h | 7 +- math/random/RandomGenerator.h | 6 +- math/stats/Histogram2.h | 53 ++++++++ navMesh/NavMeshFactory.h | 13 +- synthetic/SyntheticPath.h | 19 ++- wifi/estimate/ray3/Cube.h | 75 ++++++----- wifi/estimate/ray3/MTLReader.h | 127 ++++++++++++++++++ wifi/estimate/ray3/ModelFactory.h | 19 ++- wifi/estimate/ray3/OBJPool.h | 8 +- wifi/estimate/ray3/OBJReader.h | 71 ++++++++-- wifi/estimate/ray3/Obstacle3.h | 22 +++ 25 files changed, 620 insertions(+), 93 deletions(-) create mode 100644 math/distribution/ChiSquared.h create mode 100644 math/stats/Histogram2.h create mode 100644 wifi/estimate/ray3/MTLReader.h diff --git a/geo/BBox2.h b/geo/BBox2.h index d1cf451..05ee2d1 100644 --- a/geo/BBox2.h +++ b/geo/BBox2.h @@ -66,6 +66,12 @@ public: Point2 getCenter() const { return (p1+p2) / 2; } + Point2 getCorner1() const {return Point2(p1.x, p1.y);} + Point2 getCorner2() const {return Point2(p2.x, p1.y);} + Point2 getCorner3() const {return Point2(p1.x, p2.y);} + Point2 getCorner4() const {return Point2(p2.x, p2.y);} + + /** equal? */ bool operator == (const BBox2& o) const { return (p1.x == o.p1.x) && diff --git a/geo/BBox3.h b/geo/BBox3.h index 6218b2f..2d1d863 100644 --- a/geo/BBox3.h +++ b/geo/BBox3.h @@ -54,6 +54,9 @@ public: /** get the bbox's maximum */ const Point3& getMax() const {return p2;} + /** get the bbox's size */ + const Point3 getSize() const {return p2-p1;} + /** equal? */ bool operator == (const BBox3& o) const { return (p1.x == o.p1.x) && @@ -86,11 +89,11 @@ public: p2 += p; // increase maximum } - /** set both, min/max z to the same value */ - void setZ(const float z) { - p1.z = z; - p2.z = z; - } + /** set both, min/max z to the same value */ + void setZ(const float z) { + p1.z = z; + p2.z = z; + } /** does the bbox contain the given point? */ bool contains(const Point3& p) const { diff --git a/geo/Triangle3.h b/geo/Triangle3.h index 9ea7626..af1bda8 100644 --- a/geo/Triangle3.h +++ b/geo/Triangle3.h @@ -86,6 +86,48 @@ public: } + /** add some slight delta to prevent rounding issues */ + bool intersectsDelta(const Ray3& ray, const double delta, Point3& pos) const { + + const Point3 e1 = p2-p1; + const Point3 e2 = p3-p1; + + // make delta an absolute value (independent of the triangle's size) + // larger triangle -> smaller delta, as u,v are [0:1] + //double deltaU = delta/e2.length(); + //double deltaV = delta/e1.length(); + + const double deltaU = delta; + const double deltaV = delta; + + const Point3 h = cross(ray.dir, e2); + const double a = dot(e1, h); + + if (a > -0.00001 && a < 0.00001) {return false;} + + const double f = 1/a; + + const Point3 s = ray.start - p1; + const double u = f * dot(s,h); + + if (u < 0.0-deltaU || u > 1.0+deltaU) {return false;} + + const Point3 q = cross(s, e1); + const double v = f * dot(ray.dir, q); + + if (v < 0.0-deltaV || u + v > 1.0+deltaV) {return false;} + const double t = f * dot(e2,q); + + + if (t > 0.00001) { + pos = ray.start + (ray.dir * t); + return true; + } + + return true; + + } + /* int rayIntersectsTriangle(float *p, float *d, float *v0, float *v1, float *v2) { diff --git a/grid/Grid.h b/grid/Grid.h index fa74144..0b11ea8 100755 --- a/grid/Grid.h +++ b/grid/Grid.h @@ -73,6 +73,11 @@ public: hashes.clear(); } + /** is the grid empty? */ + bool isEmpty() const { + return nodes.empty(); + } + /** no-assign */ void operator = (const Grid& o) = delete; diff --git a/grid/factory/v2/GridFactory.h b/grid/factory/v2/GridFactory.h index 6251d82..24f3349 100755 --- a/grid/factory/v2/GridFactory.h +++ b/grid/factory/v2/GridFactory.h @@ -5,6 +5,8 @@ #include #include +#include "../../../math/Math.h" + #include "../../../floorplan/v2/Floorplan.h" #include "Helper.h" @@ -19,6 +21,8 @@ #include "../../../misc/Debug.h" #include +#include "../../../wifi/estimate/ray3/OBJPool.h" +#include "../../../geo/ConvexHull2.h" #include "GridFactoryListener.h" @@ -47,6 +51,7 @@ private: bool _buildStairs = true; bool _removeIsolated = true; + bool _addTightToObstacle = false; public: @@ -55,6 +60,7 @@ public: } + void setAddTightToObstacle(const bool tight) {this->_addTightToObstacle = tight;} /** whether or not to build stairs */ void setBuildStairs(const bool build) {this->_buildStairs = build;} @@ -204,6 +210,41 @@ public: int cur = 0; int numNodes = 0; + // all 3D objects within the floor + std::vector objObstacles; + for (const Floorplan::FloorObstacle* fo : floor->obstacles) { + + // process all object-obstalces + const Floorplan::FloorObstacleObject* foo = dynamic_cast(fo); + if (foo) { + + // get the obstacle + const Ray3D::Obstacle3D obs = Ray3D::OBJPool::get().getObject(foo->file).rotated_deg(foo->rot).translated(foo->pos); + + // construct its 2D convex hull (in centimter) + HelperPoly poly; + for (const Point2 p : ConvexHull2::get(obs.getPoints2D())) {poly.add(p*100);} + objObstacles.push_back(poly); + + } + } + + // does any of the obj-obstalces contain the given point? + auto isPartOfObject = [&objObstacles, this] (const GridNodeBBox& bb) { + for (HelperPoly poly : objObstacles) { + //if (!_addTightToObstacle) { + if (poly.contains(bb.getCorner1())) {return true;} + if (poly.contains(bb.getCorner2())) {return true;} + if (poly.contains(bb.getCorner3())) {return true;} + if (poly.contains(bb.getCorner4())) {return true;} + //} else { + // //poly.shrink(1); + // if (poly.contains(bb.getCenter())) {return true;} + //} + } + return false; + }; + // build grid-points for floor-outline for(int x_cm = x1; x_cm < x2; x_cm += helper.gridSize()) { for (int y_cm = y1; y_cm < y2; y_cm += helper.gridSize()) { @@ -212,12 +253,17 @@ public: const PartOfOutline part = isPartOfFloorOutline(x_cm, y_cm, floor->outline); if (!part.contained) {continue;} - // check intersection with the floorplan + // bbox to check intersection with the floorplan GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), helper.gridSize()); // slightly grow the bbox to ensure even obstacles that are directly aligned to the bbox are hit bbox.grow(0.1337); - if (intersects(bbox, floor)) {continue;} + if (!_addTightToObstacle) { + if (intersects(bbox, floor)) {continue;} + } + + // intersection with objects? + if (isPartOfObject(bbox)) {continue;} // add to the grid [once] T t(x_cm, y_cm, z_cm); @@ -544,13 +590,21 @@ private: } else if (dynamic_cast(fo)) { const Floorplan::FloorObstacleCircle* circle = (Floorplan::FloorObstacleCircle*) fo; - const Point2 center = bbox.getCenter(); - const float dist = center.getDistance(circle->center*100); - if (dist < circle->radius*100) {return true;} + const float dist = std::min( + bbox.getCorner1().getDistance(circle->center*100), + bbox.getCorner2().getDistance(circle->center*100), + bbox.getCorner3().getDistance(circle->center*100), + bbox.getCorner4().getDistance(circle->center*100) + ); + const float threshold = circle->radius * 100; + if (dist < threshold) {return true;} } else if (dynamic_cast(fo)) { // DOORS ARE NOT AN OBSTACLE + } else if (dynamic_cast(fo)) { + // ADDED EARLIER + } else { throw Exception("TODO: not yet implemented obstacle type"); @@ -592,6 +646,10 @@ private: } else if (dynamic_cast(fo)) { // DOORS ARE NOT AN OBSTACLE + } else if (dynamic_cast(fo)) { + // removed earlier + //std::cout << "GridFactory: TODO: Floorplan::FloorObstacleObject" << std::endl; + } else { throw Exception("TODO: not yet implemented obstacle type"); diff --git a/grid/factory/v2/Helper.h b/grid/factory/v2/Helper.h index e0e51bb..1d28290 100644 --- a/grid/factory/v2/Helper.h +++ b/grid/factory/v2/Helper.h @@ -44,6 +44,19 @@ struct HelperPoly { bbox_cm.add(p.xy()); } + + void shrink(float cm) { + Point2 center; + for (const Point2 pt : points_cm) {center += pt;} + center /= points_cm.size(); + for (Point2& pt : points_cm) { + Point2 dir = pt - center; + float len = dir.length(); + dir = dir.normalized(); + pt = center + dir * (len-cm); + } + } + /** does the polygon contain the given point (in cm)? */ bool contains(const Point2 p_cm) const { diff --git a/grid/walk/v2/modules/WalkModuleFollowDestination.h b/grid/walk/v2/modules/WalkModuleFollowDestination.h index 65532b9..b92be94 100644 --- a/grid/walk/v2/modules/WalkModuleFollowDestination.h +++ b/grid/walk/v2/modules/WalkModuleFollowDestination.h @@ -42,6 +42,10 @@ public: } + const Dijkstra& getDijkstra() const { + return dijkstra; + } + /** ctor WITH known destination*/ WalkModuleFollowDestination(const Grid& grid, const Node& destination) : grid(grid) { setDestination(destination); diff --git a/grid/walk/v3/ReachableSampler.h b/grid/walk/v3/ReachableSampler.h index 7222ece..00ed440 100644 --- a/grid/walk/v3/ReachableSampler.h +++ b/grid/walk/v3/ReachableSampler.h @@ -1,7 +1,7 @@ #ifndef INDOOR_GW3_REACHABLESAMPLER_H #define INDOOR_GW3_REACHABLESAMPLER_H -#include "../../../math/Random.h" +#include "../../../math/random/RandomGenerator.h" #include "Reachable.h" #include "Helper.h" @@ -27,7 +27,7 @@ namespace GW3 { const std::vector& reachableNodes; - mutable RandomGenerator gen; + mutable Random::RandomGenerator gen; mutable std::uniform_real_distribution dOffset; diff --git a/grid/walk/v3/WalkEvaluator.h b/grid/walk/v3/WalkEvaluator.h index 340bbb8..843d237 100644 --- a/grid/walk/v3/WalkEvaluator.h +++ b/grid/walk/v3/WalkEvaluator.h @@ -113,7 +113,9 @@ namespace GW3 { public: - WalkEvalDistance(const Grid& grid, const double sigma = 0.1) : grid(grid), sigma(sigma), dist(0, sigma) {;} + WalkEvalDistance(const Grid& grid, const double sigma = 0.1) : grid(grid), sigma(sigma), dist(0, sigma) { + Assert::isFalse(grid.isEmpty(), "empty grid given"); + } virtual double getProbability(const PotentialWalk& walk) const override { diff --git a/grid/walk/v3/Walker.h b/grid/walk/v3/Walker.h index 9a53f17..0f04439 100644 --- a/grid/walk/v3/Walker.h +++ b/grid/walk/v3/Walker.h @@ -237,7 +237,7 @@ namespace GW3 { Assert::isNot0(walkDist_m, "walking distance must be > 0"); - const GridPoint gpStart = Helper::p3ToGp(params.start); + const GridPoint gpStart = grid.toGridPoint(params.start); const Node* startNode = grid.getNodePtrFor(gpStart); if (!startNode) {throw Exception("start node not found!");} diff --git a/math/Math.h b/math/Math.h index 936e12a..33e7cab 100644 --- a/math/Math.h +++ b/math/Math.h @@ -31,5 +31,16 @@ public: }; +namespace std { + + template const T& min(const T& a, const T& b, const T& c) { + return min(a, min(b,c)); + } + template const T& min(const T& a, const T& b, const T& c, const T& d) { + return min(a, min(b, min(c,d))); + } + +} + #endif // K_MATH_MATH_H diff --git a/math/distribution/ChiSquared.h b/math/distribution/ChiSquared.h new file mode 100644 index 0000000..589b0fc --- /dev/null +++ b/math/distribution/ChiSquared.h @@ -0,0 +1,59 @@ +#ifndef CHISQUARED_H +#define CHISQUARED_H + +#include + +namespace Distribution { + + /** + * https://en.wikipedia.org/wiki/Chi-squared_distribution + * @brief The ChiSquared class + */ + template class ChiSquared { + + private: + + // degrees of freedom + int k; + + public: + + /** ctor */ + ChiSquared(const int k) : k(k) { + ; + } + + Scalar get(const Scalar val) const { + + const Scalar k2 = k/((Scalar)2); + const Scalar k2_1 = k2 - 1; + const Scalar gamma = std::tgamma(k2); + + return 1.0 / (std::pow(2, k2)*gamma) * std::pow(val, k2_1) * std::exp(-val/2); + + } + + + Scalar getInvCDF(const Scalar val) const { + + // brute-force get the inverse CDF... + + const Scalar ss = 0.002; + Scalar sum = 0; + + for (float t = 0; t < 20; t += ss) { + sum += get(t) * ss; + if (sum >= val) { + return t; + } + } + + throw "failed"; + + } + + }; + +} + +#endif // CHISQUARED_H diff --git a/math/distribution/Normal.h b/math/distribution/Normal.h index 632a524..fa3dee1 100644 --- a/math/distribution/Normal.h +++ b/math/distribution/Normal.h @@ -17,7 +17,7 @@ namespace Distribution { const T sigma; const T _a; - Random::RandomGenerator gen; + Random::RandomGenerator gen; std::normal_distribution dist; public: @@ -32,6 +32,11 @@ namespace Distribution { mu(mu), sigma(sigma), _a(1.0 / (sigma * std::sqrt(2.0 * M_PI))), gen(seed), dist(mu,sigma) { } + /** mu = 0, sigma = 1 */ + static Normal unit() { + return Normal(0,1); + } + /** do not allow copy. this will not work as expected for std::normal_distribution when using draw() ?! */ //Normal(const Normal& o) = delete; @@ -51,15 +56,15 @@ namespace Distribution { gen.seed(seed); } - /** get the mean value */ - const T getMu() { - return this->mu; - } + /** get the mean value */ + const T getMu() { + return this->mu; + } - /** get the standard deviation */ - const T getSigma() { - return this->sigma; - } + /** get the standard deviation */ + const T getSigma() { + return this->sigma; + } /** get the probability for the given value */ static T getProbability(const T mu, const T sigma, const T val) { diff --git a/math/distribution/NormalN.h b/math/distribution/NormalN.h index db86011..011f6ae 100644 --- a/math/distribution/NormalN.h +++ b/math/distribution/NormalN.h @@ -8,6 +8,8 @@ #include "../../Assertions.h" #include "../random/RandomGenerator.h" +#include "../../geo/Point2.h" +#include "ChiSquared.h" namespace Distribution { @@ -96,6 +98,43 @@ namespace Distribution { return NormalDistributionN(mean, cov); } + std::vector getContour2(const double percentWithin) { + + const int degreesOfFreedom = 2; // 2D distribution + const ChiSquared chi(degreesOfFreedom); + + // https://people.richland.edu/james/lecture/m170/tbl-chi.html + Assert::isNear(0.103, chi.getInvCDF(0.05), 0.01, "error within chi-squared distribution"); + Assert::isNear(0.211, chi.getInvCDF(0.10), 0.01, "error within chi-squared distribution"); + Assert::isNear(4.605, chi.getInvCDF(0.90), 0.01, "error within chi-squared distribution"); + Assert::isNear(5.991, chi.getInvCDF(0.95), 0.03, "error within chi-squared distribution"); + + // size of the ellipse using confidence intervals + const float mul = chi.getInvCDF(percentWithin); + + std::vector res; + + std::cout << sigma << std::endl; + Eigen::SelfAdjointEigenSolver solver(this->sigma); + + Eigen::Vector2d evec1 = solver.eigenvectors().col(0); + Eigen::Vector2d evec2 = solver.eigenvectors().col(1); + + double eval1 = solver.eigenvalues()(0); + double eval2 = solver.eigenvalues()(1); + + for (int deg = 0; deg <= 360; deg += 5) { + const float rad = deg / 180.0f * M_PI; + Eigen::Vector2d pos = + std::cos(rad) * std::sqrt(mul * eval1) * evec1 + + std::sin(rad) * std::sqrt(mul * eval2) * evec2; + res.push_back(Point2(pos(0), pos(1))); + } + + return res; + + } + }; } diff --git a/math/distribution/Uniform.h b/math/distribution/Uniform.h index af5a873..143fffe 100644 --- a/math/distribution/Uniform.h +++ b/math/distribution/Uniform.h @@ -14,7 +14,7 @@ namespace Distribution { private: - Random::RandomGenerator gen; + Random::RandomGenerator gen; /** depending on T, Dist is either a uniform_real or uniform_int distribution */ typedef typename std::conditional< std::is_floating_point::value, std::uniform_real_distribution, std::uniform_int_distribution >::type Dist; @@ -33,6 +33,11 @@ namespace Distribution { } + /** -1 to +1 */ + static Uniform unit() { + return Uniform(-1, +1); + } + /** get a uniformaly distributed random number */ T draw() { return dist(gen); diff --git a/math/random/RandomGenerator.h b/math/random/RandomGenerator.h index c0dcd82..3aba20a 100644 --- a/math/random/RandomGenerator.h +++ b/math/random/RandomGenerator.h @@ -12,15 +12,15 @@ namespace Random { - class RandomGenerator : public std::minstd_rand { + class RandomGenerator : public std::minstd_rand { public: /** ctor with default seed */ - RandomGenerator() : std::minstd_rand(RANDOM_SEED) {;} + RandomGenerator() : std::minstd_rand(RANDOM_SEED) {;} /** ctor with custom seed */ - RandomGenerator(result_type) : std::minstd_rand(RANDOM_SEED) {;} + RandomGenerator(result_type) : std::minstd_rand(RANDOM_SEED) {;} }; diff --git a/math/stats/Histogram2.h b/math/stats/Histogram2.h new file mode 100644 index 0000000..748f57b --- /dev/null +++ b/math/stats/Histogram2.h @@ -0,0 +1,53 @@ +#ifndef HISTOGRAM2_H +#define HISTOGRAM2_H + +#include +#include "../../geo/BBox2.h" + +namespace Stats { + + /** 2D histogram */ + template class Histogram2 { + + std::vector vec; + BBox2 bbox; + int binsX; + int binsY; + + public: + + /** ctor */ + Histogram2(BBox2 bbox, int binsX, int binsY) : bbox(bbox), binsX(binsX), binsY(binsY) { + vec.resize(binsX*binsY); + } + + Scalar get(Scalar x, Scalar y) const { + const int idx = binIdx(x,y); + return vec[idx]; + } + + void add(Scalar x, Scalar y, Scalar val) { + const int idx = binIdx(x,y); + vec[idx] += val; + } + + int binIdx(const Scalar x, const Scalar y) { + const int ix = binIdxX(x); + const int iy = binIdxY(y); + return ix + iy*binsX; + } + + int binIdxX(const Scalar val) const { + return (val - bbox.getMin().x) / (bbox.getSize().x) * binsX; + } + + int binIdxY(const Scalar val) const { + return (val - bbox.getMin().y) / (bbox.getSize().y) * binsY; + } + + + }; + +} + +#endif // HISTOGRAM2_H diff --git a/navMesh/NavMeshFactory.h b/navMesh/NavMeshFactory.h index d563402..52b05b8 100644 --- a/navMesh/NavMeshFactory.h +++ b/navMesh/NavMeshFactory.h @@ -740,14 +740,11 @@ namespace NM { Floorplan::Polygon2 res; - std::vector src; - Ray3D::Obstacle3D obs = Ray3D::OBJPool::get().getObject(obj->file).rotated_deg(obj->rot).translated(obj->pos); - for (const Triangle3& tria : obs.triangles) { - src.push_back(tria.p1.xy()); - src.push_back(tria.p2.xy()); - src.push_back(tria.p3.xy()); - } - res.points = ConvexHull2::get(src); + // fetch object from pool + const Ray3D::Obstacle3D obs = Ray3D::OBJPool::get().getObject(obj->file).rotated_deg(obj->rot).translated(obj->pos); + + // construct 2D convex hull + res.points = ConvexHull2::get(obs.getPoints2D()); return res; } diff --git a/synthetic/SyntheticPath.h b/synthetic/SyntheticPath.h index 0fbf933..430ca8e 100644 --- a/synthetic/SyntheticPath.h +++ b/synthetic/SyntheticPath.h @@ -9,13 +9,13 @@ /** allows interpolation along a synthetic path */ class SyntheticPath : private Interpolator { - using Base = Interpolator; - using Entry = Base::InterpolatorEntry; const Floorplan::IndoorMap* map; - public: + using Base = Interpolator; + using Entry = Base::InterpolatorEntry; + /** create path using the given ground-truth points from the map */ void create(const Floorplan::IndoorMap* map, std::vector ids) { @@ -38,6 +38,19 @@ public: } + /** get the path's length */ + float getLength() const { + //return Base::getEntries().back().key; + float dist = 0; + for (size_t i = 0; i < getEntries().size()-1; ++i) { + const auto& e1 = getEntries()[i]; + const auto& e2 = getEntries()[i+1]; + dist += e1.value.getDistance(e2.value); + } + return dist; + } + + /** get all individual entries from the underlying data-structure */ const std::vector& getEntries() const { return Base::getEntries(); diff --git a/wifi/estimate/ray3/Cube.h b/wifi/estimate/ray3/Cube.h index bdfbdbd..dcbb426 100644 --- a/wifi/estimate/ray3/Cube.h +++ b/wifi/estimate/ray3/Cube.h @@ -18,6 +18,16 @@ namespace Ray3D { public: + + enum Part { + CUBE_TOP = 1, + CUBE_BOTTOM = 2, + CUBE_LEFT = 4, + CUBE_RIGHT = 8, + CUBE_FRONT = 16, + CUBE_BACK = 32, + }; + // Cube (const Point3 p1, const Point3 p2, const Point3 p3, const Point3 p4, const float h) { //// const Point3 ph(0,0,h); //// addQuad(p1+ph, p2+ph, p3+ph, p4+ph); // top @@ -30,8 +40,8 @@ namespace Ray3D { // } /** ctor with position, size and rotation */ - Cube(const Point3 pos, const Point3 size, const Point3 rot_deg, const bool topAndBottom = true) { - unitCube(topAndBottom); + Cube(const Point3 pos, const Point3 size, const Point3 rot_deg, const Part parts = (Part)63) { + unitCube(parts); transform(pos, size, rot_deg); } @@ -50,8 +60,10 @@ namespace Ray3D { return res; } - static Cube unit() { - return Cube(); + static Cube unit(const Part parts = (Part) 63) { + Cube cube; + cube.unitCube(parts); + return cube; } /** cube from 8 vertices (upper 4, lower 4) */ @@ -77,72 +89,73 @@ namespace Ray3D { private: /** build unit-cube faces */ - void unitCube(const bool topAndBottom) { + void unitCube(const Part parts) { const float s = 1.0f; // left? - addQuad( - Point3(+s, -s, -s), - Point3(+s, -s, +s), - Point3(-s, -s, +s), - Point3(-s, -s, -s) - ); + if (parts & CUBE_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) - ); + if (parts & CUBE_RIGHT) { + addQuad( + Point3(-s, +s, -s), + Point3(-s, +s, +s), + Point3(+s, +s, +s), + Point3(+s, +s, -s) + ); + } - - // small side - if (1 == 1) { - - // front + // front + if (parts & CUBE_FRONT) { addQuad( Point3(-s, -s, -s), Point3(-s, -s, +s), Point3(-s, +s, +s), Point3(-s, +s, -s) ); + } - // read + // back + if (parts & CUBE_BACK) { addQuad( Point3(+s, +s, -s), Point3(+s, +s, +s), Point3(+s, -s, +s), Point3(+s, -s, -s) ); - } - - - if (topAndBottom) { - - // top + // top + if (parts & CUBE_TOP) { addQuad( Point3(+s, +s, +s), Point3(-s, +s, +s), Point3(-s, -s, +s), Point3(+s, -s, +s) ); + } - // bottom + // bottom + if (parts & CUBE_BOTTOM) { addQuad( Point3(+s, -s, -s), Point3(-s, -s, -s), Point3(-s, +s, -s), Point3(+s, +s, -s) ); - } + } diff --git a/wifi/estimate/ray3/MTLReader.h b/wifi/estimate/ray3/MTLReader.h new file mode 100644 index 0000000..3952d14 --- /dev/null +++ b/wifi/estimate/ray3/MTLReader.h @@ -0,0 +1,127 @@ +#ifndef MTLREADER_H +#define MTLREADER_H + +#include +#include +#include +#include +#include "../../../geo/Point2.h" +#include "../../../geo/Point3.h" + +/** + * prase .mtl files + */ +class MTLReader { + +public: + + struct Material { + std::string textureFile = ""; + Point3 diffuse = Point3(1,1,1); + float alpha = 1.0; + }; + + Material* cur = nullptr; + std::unordered_map map; + + /** ctor. use readXYZ() */ + MTLReader() { + ; + } + + /** read .obj from the given file */ + void readFile(const std::string& file) { + std::ifstream is(file); + std::string line; + while(getline(is, line)) {parseLine(line);} + is.close(); + } + + /** read obj from the given data string (.obj file contents) */ + void readData(const std::string& data) { + std::stringstream is(data); + std::string line; + while(getline(is, line)) {parseLine(line);} + } + + /** get the given material */ + const Material& getMaterial(const std::string& mat) const { + const auto& it = map.find(mat); + if (it == map.end()) {throw Exception("material not available");} + return it->second; + } + + +private: + + template + void split(const std::string &s, char delim, Out result) { + std::stringstream ss(s); + std::string item; + while (std::getline(ss, item, delim)) { + *(result++) = item; + } + } + + void replaceAll(std::string& str, const std::string& from, const std::string& to) { + size_t start_pos = 0; + while((start_pos = str.find(from, start_pos)) != std::string::npos) { + size_t end_pos = start_pos + from.length(); + str.replace(start_pos, end_pos, to); + start_pos += to.length(); // In case 'to' contains 'from', like replacing 'x' with 'yx' + } + } + + /** remove empty strings from the vector */ + std::vector nonEmpty(const std::vector& src) { + std::vector res; + for (const std::string& s : src) { + if (!s.empty()) {res.push_back(s);} + } + return res; + } + + std::vector split(const std::string &s, char delim) { + std::vector elems; + split(s, delim, std::back_inserter(elems)); + return elems; + } + + /** parse one line of the .obj file */ + void parseLine(std::string line) { + + if (line.length() < 2) {return;} + + // remove leading "#" + while (line[0] == ' ' || line[0] == '\t') { + line.erase(line.begin()); + } + + // remove other linebreaks + replaceAll(line, "\r", ""); + + const std::vector tokens = nonEmpty(split(line, ' ')); + const std::string token = tokens.front(); + + if ("newmtl" == token) { + const std::string id = tokens[1]; + map[id] = Material(); + cur = &map[id]; + } else if ("map_Ka" == token) { + const std::string texFile = tokens[1]; + cur->textureFile = texFile; + } else if ("Kd" == token) { + cur->diffuse.x = std::stof(tokens[1]); + cur->diffuse.y = std::stof(tokens[2]); + cur->diffuse.z = std::stof(tokens[3]); + } else if ("d" == token) { + cur->alpha = std::stof(tokens[1]); + } + + } + + + +}; + +#endif // MTLREADER_H diff --git a/wifi/estimate/ray3/ModelFactory.h b/wifi/estimate/ray3/ModelFactory.h index acce398..76a740a 100644 --- a/wifi/estimate/ray3/ModelFactory.h +++ b/wifi/estimate/ray3/ModelFactory.h @@ -18,7 +18,7 @@ namespace Ray3D { */ class ModelFactory { - private: + public: bool exportCeilings = true; bool exportObstacles = true; @@ -26,9 +26,12 @@ namespace Ray3D { bool fancyStairs = true; bool exportHandrails = true; bool exportDoors = true; - bool doorsOpen = true; + bool exportAboveDoors = true; + bool doorsOpen = false; bool exportObjects = true; bool exportWallTops = false; + Cube::Part cubeParts = (Cube::Part) 63; // leftright,topbottom,rearfront + std::vector exportFloors; /** the to-be-exported map */ @@ -233,7 +236,9 @@ namespace Ray3D { } //std::vector tmp = getDoorAbove(f, door); //res.insert(res.end(), tmp.begin(), tmp.end()); - res.push_back(getDoorAbove(f, door)); + if (exportAboveDoors) { + res.push_back(getDoorAbove(f, door)); + } } } @@ -274,8 +279,8 @@ namespace Ray3D { const float deg = rad * 180 / M_PI; // cube's destination center - const float cenZ = (!aboveDoor) ? (fpos.z1 + fpos.height/2) : (fpos.z2 - (fpos.height - aboveDoor->height) / 2); - const float height = (!aboveDoor) ? (fpos.height) : (fpos.height - aboveDoor->height); + const double height = (!aboveDoor) ? (fpos.height) : (fpos.height - aboveDoor->height); + const double cenZ = (!aboveDoor) ? (fpos.z1 + height/2) : (fpos.z1 + aboveDoor->height + height/2);// (fpos.z2 - (fpos.height - aboveDoor->height) / 2); const Point3 pos(cen2.x, cen2.y, cenZ); // div by 2.01 to prevent overlapps and z-fighting @@ -286,7 +291,7 @@ namespace Ray3D { const Point3 rot(0,0,deg); // build - Cube cube(pos, size, rot); + Cube cube(pos, size, rot, cubeParts); // done Obstacle3D res(getType(fol), fol->material); @@ -359,7 +364,7 @@ namespace Ray3D { const float sz = door->height / 2.01f; // prevent overlaps const Point3 size(sx, sy, sz); - Cube cube = Cube::unit(); + Cube cube = Cube::unit(cubeParts); cube.transform(mat); cube.transform(pos, size, rot); res.triangles = cube.getTriangles(); diff --git a/wifi/estimate/ray3/OBJPool.h b/wifi/estimate/ray3/OBJPool.h index c8eea61..991925c 100644 --- a/wifi/estimate/ray3/OBJPool.h +++ b/wifi/estimate/ray3/OBJPool.h @@ -91,9 +91,11 @@ namespace Ray3D { // create triangles Obstacle3D obs; - for (const OBJReader::Face& face : reader.getData().faces) { - const Triangle3 tria(face.vnt[0].vertex, face.vnt[1].vertex, face.vnt[2].vertex); - obs.triangles.push_back(tria); + for (const OBJReader::Object& obj : reader.getData().objects) { + for (const OBJReader::Face& face : obj.faces) { + const Triangle3 tria(face.vnt[0].vertex, face.vnt[1].vertex, face.vnt[2].vertex); + obs.triangles.push_back(tria); + } } // store diff --git a/wifi/estimate/ray3/OBJReader.h b/wifi/estimate/ray3/OBJReader.h index c5b7316..d469437 100644 --- a/wifi/estimate/ray3/OBJReader.h +++ b/wifi/estimate/ray3/OBJReader.h @@ -14,7 +14,6 @@ class OBJReader { public: - /** group vertex+normal+texture */ struct VNT { int idxVertex; @@ -31,15 +30,26 @@ public: Face(VNT v1, VNT v2, VNT v3) : vnt({v1,v2,v3}) {;} }; + /** one object within the file */ + struct Object { + std::string material; + std::string name; + std::vector faces; + }; + /** internal data */ struct Data { std::vector vertices; std::vector texCoords; std::vector normals; - std::vector faces; + std::vector materialFiles; + std::vector objects; + Object& curObj() { + if (objects.empty()) {objects.push_back(Object());} + return objects.back(); + } } data; - public: /** ctor. use readXYZ() */ @@ -68,6 +78,24 @@ public: private: + void replaceAll(std::string& str, const std::string& from, const std::string& to) { + size_t start_pos = 0; + while((start_pos = str.find(from, start_pos)) != std::string::npos) { + size_t end_pos = start_pos + from.length(); + str.replace(start_pos, end_pos, to); + start_pos += to.length(); // In case 'to' contains 'from', like replacing 'x' with 'yx' + } + } + + /** remove empty strings from the vector */ + std::vector nonEmpty(const std::vector& src) { + std::vector res; + for (const std::string& s : src) { + if (!s.empty()) {res.push_back(s);} + } + return res; + } + template void split(const std::string &s, char delim, Out result) { std::stringstream ss(s); @@ -84,20 +112,33 @@ private: } /** parse one line of the .obj file */ - void parseLine(const std::string& line) { + void parseLine(std::string line) { if (line.length() < 2) {return;} - const std::vector tokens = split(line, ' '); + // remove other linebreaks + replaceAll(line, "\r", ""); + + const std::vector tokens = nonEmpty(split(line, ' ')); const std::string token = tokens.front(); - if ("v" == token) {parseVertex(tokens);} - if ("vt" == token) {parseTexCoord(tokens);} - if ("vn" == token) {parseNormal(tokens);} - if ("f" == token) {parseFace(tokens);} + if ("mtllib" == token) {data.materialFiles.push_back(tokens[1]);} + if ("usemtl" == token) {data.curObj().material = tokens[1];} + if ("v" == token) {parseVertex(tokens);} + if ("vt" == token) {parseTexCoord(tokens);} + if ("vn" == token) {parseNormal(tokens);} + if ("f" == token) {parseFace(tokens);} + if ("g" == token) {newObject(tokens[1]);} } + /** allocate a new object */ + void newObject(const std::string& name) { + Object o; + o.name = name; + data.objects.push_back(o); + } + /** parse one vertex from the tokenizer */ void parseVertex(const std::vector& t) { const float x = std::stof(t[1]); @@ -135,18 +176,20 @@ private: ++numVertices; const std::string v = vtn[0]; + const std::string vt = (vtn.size() > 1) ? (vtn[1]) : (""); + const std::string vn = (vtn.size() > 2) ? (vtn[2]) : (""); //const std::string vt = t2.getToken('/', false); //const std::string vn = t2.getToken('/', false); // create a new vertex/normal/texture combination VNT vnt; vnt.idxVertex = (std::stoi(v) - 1); - //vnt.idxNormal = (vn.empty()) ? (-1) : (std::stoi(vn) - 1); - //vnt.idxTexture = (vt.empty()) ? (-1) : (std::stoi(vt) - 1); + vnt.idxNormal = (vn.empty()) ? (-1) : (std::stoi(vn) - 1); + vnt.idxTexture = (vt.empty()) ? (-1) : (std::stoi(vt) - 1); if (vnt.idxVertex >= 0) {vnt.vertex = data.vertices[vnt.idxVertex];} - //if (vnt.idxNormal >= 0) {vnt.normal = data.normals[vnt.idxNormal];} - //if (vnt.idxTexture >= 0) {vnt.texture = data.texCoords[vnt.idxTexture];} + if (vnt.idxNormal >= 0) {vnt.normal = data.normals[vnt.idxNormal];} + if (vnt.idxTexture >= 0) {vnt.texture = data.texCoords[vnt.idxTexture];} indices.push_back(vnt); @@ -156,7 +199,7 @@ private: // see: http://www.mathopenref.com/polygontriangles.html for (int i = 1; i < (int) indices.size()-1; ++i) { Face face(indices[0], indices[1], indices[i+1]); - data.faces.push_back(face); + data.curObj().faces.push_back(face); } // sanity check diff --git a/wifi/estimate/ray3/Obstacle3.h b/wifi/estimate/ray3/Obstacle3.h index d9e3bcd..8199713 100644 --- a/wifi/estimate/ray3/Obstacle3.h +++ b/wifi/estimate/ray3/Obstacle3.h @@ -59,6 +59,28 @@ namespace Ray3D { return copy; } + /** get all triangle-edge-points (x,y) within the obstacle */ + std::vector getPoints2D() const { + std::vector res; + for (const Triangle3& tria : triangles) { + res.push_back(tria.p1.xy()); + res.push_back(tria.p2.xy()); + res.push_back(tria.p3.xy()); + } + return res; + } + + /** get all triangle-edge-points (x,y,z) within the obstacle */ + std::vector getPoints3D() const { + std::vector res; + for (const Triangle3& tria : triangles) { + res.push_back(tria.p1); + res.push_back(tria.p2); + res.push_back(tria.p3); + } + return res; + } + }; }