Merge branch 'master' of https://git.frank-ebner.de/FHWS/Indoor
This commit is contained in:
toni
@@ -452,7 +452,7 @@ namespace Floorplan {
|
||||
|
||||
|
||||
/** snap quad-edges together if their distance is below the given maximum. is used to close minor gaps */
|
||||
static void snapQuads(std::vector<Floorplan::Quad3>& quads, const float maxDist) {
|
||||
static inline void snapQuads(std::vector<Floorplan::Quad3>& quads, const float maxDist) {
|
||||
|
||||
for (Floorplan::Quad3& quad1 : quads) {
|
||||
for (int i1 = 0; i1 < 4; ++i1) {
|
||||
@@ -479,7 +479,7 @@ namespace Floorplan {
|
||||
}
|
||||
|
||||
/** convert stair-parts to quads. the scaling factor may be used to slightly grow each quad. e.g. needed to ensure that the quads overlap */
|
||||
static std::vector<Quad3> getQuads(const std::vector<StairPart>& parts, const Floor* floor, const float s = 1.0f) {
|
||||
static inline std::vector<Quad3> getQuads(const std::vector<StairPart>& parts, const Floor* floor, const float s = 1.0f) {
|
||||
|
||||
std::vector<Quad3> vec;
|
||||
|
||||
|
||||
100
geo/Line2.h
100
geo/Line2.h
@@ -33,29 +33,53 @@ public:
|
||||
// }
|
||||
|
||||
|
||||
/** get intersection between these two lines (unlimited length!) */
|
||||
bool getLineIntersection(const Line2& other, Point2& result) const {
|
||||
float getAngle() const {
|
||||
return std::atan2(p2.y-p1.y, p2.x-p1.x);
|
||||
}
|
||||
|
||||
double bx = p2.x - p1.x;
|
||||
double by = p2.y - p1.y;
|
||||
void reverse() {
|
||||
std::swap(p1, p2);
|
||||
}
|
||||
|
||||
double dx = other.p2.x - other.p1.x;
|
||||
double dy = other.p2.y - other.p1.y;
|
||||
float getLength() const {
|
||||
return p1.getDistance(p2);
|
||||
}
|
||||
|
||||
double b_dot_d_perp = bx*dy - by*dx;
|
||||
/** keep the starting point, but make the line longer by the given length */
|
||||
Line2 longerAtEnd(const float l) {
|
||||
Point2 dir = p2-p1;
|
||||
return Line2(p1, p1+dir+dir.normalized()*l);
|
||||
}
|
||||
|
||||
if(b_dot_d_perp == 0) {return false;}
|
||||
/** keep the ending point, but make the line longer by the given length */
|
||||
Line2 longerAtStart(const float l) {
|
||||
Point2 dir = p1-p2;
|
||||
return Line2(p2, p2+dir+dir.normalized()*l);
|
||||
}
|
||||
|
||||
double cx = other.p1.x - p1.x;
|
||||
double cy = other.p1.y - p1.y;
|
||||
double t = (cx*dy - cy*dx) / b_dot_d_perp;
|
||||
/** get intersection between these two lines (unlimited length!) */
|
||||
bool getLineIntersection(const Line2& other, Point2& result) const {
|
||||
|
||||
result.x = p1.x + t * bx;
|
||||
result.y = p1.y + t * by;
|
||||
double bx = p2.x - p1.x;
|
||||
double by = p2.y - p1.y;
|
||||
|
||||
return true;
|
||||
double dx = other.p2.x - other.p1.x;
|
||||
double dy = other.p2.y - other.p1.y;
|
||||
|
||||
}
|
||||
double b_dot_d_perp = bx*dy - by*dx;
|
||||
|
||||
if(b_dot_d_perp == 0) {return false;}
|
||||
|
||||
double cx = other.p1.x - p1.x;
|
||||
double cy = other.p1.y - p1.y;
|
||||
double t = (cx*dy - cy*dx) / b_dot_d_perp;
|
||||
|
||||
result.x = p1.x + t * bx;
|
||||
result.y = p1.y + t * by;
|
||||
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
bool getSegmentIntersection(const Line2& other) const {
|
||||
|
||||
@@ -207,4 +231,50 @@ public:
|
||||
|
||||
};
|
||||
|
||||
|
||||
static inline bool intersects(const Line2& l1, const Line2& l2, bool limit, Point2& pos) {
|
||||
|
||||
// (sx1,sy1) + (dx1, dy1)*u = (sx2, sy2) + (dx2, dy2)*v
|
||||
// solve((c+d*v-a)/b = (g+h*v-e)/f, v)
|
||||
// solve((a+b*u-c)/d = (e+f*u-g)/h, u)
|
||||
// bf != 0
|
||||
// dh != 0
|
||||
// df != bh // kollinear (b/d != f/h)
|
||||
|
||||
const float sx1 = l1.p1.x; // a
|
||||
const float sy1 = l1.p1.y; // e
|
||||
|
||||
const float dx1 = l1.p2.x - l1.p1.x; // b
|
||||
const float dy1 = l1.p2.y - l1.p1.y; // f
|
||||
|
||||
const float sx2 = l2.p1.x; // c
|
||||
const float sy2 = l2.p1.y; // g
|
||||
|
||||
const float dx2 = l2.p2.x - l2.p1.x; // d
|
||||
const float dy2 = l2.p2.y - l2.p1.y; // h
|
||||
|
||||
// collinear?
|
||||
const float a1 = dx2*dy1;
|
||||
const float a2 = dx1*dy2;
|
||||
if (std::abs(a1-a2) < 0.0001) {return false;}
|
||||
|
||||
const float v = (dy1*(sx1-sx2) + dx1*(sy2-sy1)) / (dx2*dy1 - dx1*dy2);
|
||||
const float u = (dy2*(sx1-sx2) + dx2*(sy2-sy1)) / (dx2*dy1 - dx1*dy2);
|
||||
|
||||
//const float x = sx2 + v*dx2;
|
||||
//const float y = sy2 + v*dy2;
|
||||
|
||||
const float x = sx1 + u*dx1;
|
||||
const float y = sy1 + u*dy1;
|
||||
|
||||
if (!limit || (u >= 0 && v >= 0 && u <= 1 && v <= 1)) {
|
||||
pos = Point2(x,y);
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif // LINE2D_H
|
||||
|
||||
16
geo/Point2.h
16
geo/Point2.h
@@ -44,6 +44,7 @@ struct Point2 {
|
||||
|
||||
bool operator != (const Point2& o) const {return x!=o.x || y!=o.y;}
|
||||
|
||||
bool eq (const Point2& o, const float delta) const { return eq(x,o.x,delta) && eq(y,o.y,delta); }
|
||||
|
||||
Point2 perpendicular() const {return Point2(-y, x);}
|
||||
|
||||
@@ -66,6 +67,11 @@ struct Point2 {
|
||||
return "(" + std::to_string(x) + "," + std::to_string(y) + ")";
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
static inline bool eq(const float a, const float b, const float delta) {return std::abs(a-b) <= delta;}
|
||||
static inline bool ne(const float a, const float b, const float delta) {return std::abs(a-b) > delta;}
|
||||
|
||||
};
|
||||
|
||||
inline float dot(const Point2 p1, const Point2 p2) {
|
||||
@@ -78,4 +84,14 @@ inline void swap(Point2& p1, Point2& p2) {
|
||||
std::swap(p1.y, p2.y);
|
||||
}
|
||||
|
||||
namespace std {
|
||||
template <> struct hash<Point2> {
|
||||
std::size_t operator()(const Point2& p) const {
|
||||
uint32_t x = *((uint32_t*)&(p.x));
|
||||
uint32_t y = *((uint32_t*)&(p.y));
|
||||
return std::hash<uint32_t>()(x^y);
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
#endif // POINT2_H
|
||||
|
||||
125
geo/Polygon2.h
Normal file
125
geo/Polygon2.h
Normal file
@@ -0,0 +1,125 @@
|
||||
#ifndef POLYGON2_H
|
||||
#define POLYGON2_H
|
||||
|
||||
#include "Point2.h"
|
||||
#include <vector>
|
||||
#include "Line2.h"
|
||||
#include "BBox2.h"
|
||||
|
||||
static bool polygonContainsPoint(const std::vector<Point2>& poly, const Point2 p);
|
||||
|
||||
class Polygon2 {
|
||||
|
||||
std::vector<Point2> pts;
|
||||
|
||||
public:
|
||||
|
||||
void add(const Point2 p) {
|
||||
pts.push_back(p);
|
||||
}
|
||||
|
||||
std::vector<Point2>::iterator begin() {return pts.begin();}
|
||||
std::vector<Point2>::iterator end() {return pts.end();}
|
||||
|
||||
std::vector<Point2>::const_iterator begin() const {return pts.begin();}
|
||||
std::vector<Point2>::const_iterator end() const {return pts.end();}
|
||||
|
||||
std::vector<Point2>& getVector() {return pts;}
|
||||
|
||||
Point2& operator [] (const size_t idx) {
|
||||
return pts[idx];
|
||||
}
|
||||
|
||||
|
||||
|
||||
/** get polygon as list of lines */
|
||||
std::vector<Line2> getLines() const {
|
||||
std::vector<Line2> res;
|
||||
for (size_t i = 0; i < pts.size(); ++i) {
|
||||
const Point2 p1 = pts[(i+0)];
|
||||
const Point2 p2 = pts[(i+1)%pts.size()];
|
||||
res.push_back(Line2(p1, p2));
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
struct CutRes {
|
||||
Point2 p;
|
||||
size_t i1,i2; // line, given by two points, within this poly
|
||||
size_t j1,j2; // line, given by two points, within other, given poly
|
||||
CutRes(Point2 p, size_t i1, size_t i2, size_t j1, size_t j2) : p(p), i1(i1), i2(i2), j1(j1), j2(j2) {;}
|
||||
};
|
||||
|
||||
|
||||
BBox2 getBBox() const {
|
||||
BBox2 bb;
|
||||
for (const Point2 p : pts) {bb.add(p);}
|
||||
return bb;
|
||||
}
|
||||
|
||||
|
||||
bool contains(const Point2 p) const {
|
||||
return polygonContainsPoint(pts, p);
|
||||
}
|
||||
|
||||
std::vector<CutRes> getIntersections(const Polygon2& o, bool limit = true) const {
|
||||
|
||||
std::vector<CutRes> res;
|
||||
for (size_t i = 0; i < pts.size(); ++i) {
|
||||
const size_t i1 = (i+0);
|
||||
const size_t i2 = (i+1) % pts.size();
|
||||
const Line2 l1(pts[i1], pts[i2]);
|
||||
|
||||
for (size_t j = 0; j < o.pts.size(); ++j) {
|
||||
const size_t j1 = (j+0);
|
||||
const size_t j2 = (j+1) % o.pts.size();
|
||||
const Line2 l2(o.pts[j1], o.pts[j2]);
|
||||
|
||||
Point2 p;
|
||||
//if (l1.getSegmentIntersection(l2, p)) {
|
||||
if (intersects(l1, l2, limit, p)) {
|
||||
res.push_back(CutRes(p, i1,i2, j1,j2));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/** does the given polygon (list of points) contain the given point? */
|
||||
static bool polygonContainsPoint(const std::vector<Point2>& poly, const Point2 p) {
|
||||
|
||||
// https://en.wikipedia.org/wiki/Winding_number
|
||||
// http://geomalgorithms.com/a03-_inclusion.html
|
||||
|
||||
// BBox2 bb
|
||||
// bb.grow(Point2(0.001, 0.001));
|
||||
// if (!bb.contains(p)) {return false;}
|
||||
|
||||
double sum = 0;
|
||||
for (size_t i = 0; i < poly.size(); ++i) {
|
||||
|
||||
const Point2 p1 = poly[i];
|
||||
const Point2 p2 = poly[(i+1)%poly.size()];
|
||||
|
||||
const Point2 d1 = (p1-p).normalized();
|
||||
const Point2 d2 = (p2-p).normalized();
|
||||
|
||||
sum += std::acos(dot(d1, d2));
|
||||
|
||||
}
|
||||
|
||||
// normalize (x = number of windings)
|
||||
const double x = sum / M_PI / 2;
|
||||
|
||||
// only look at the fractional part
|
||||
//const double y = x - std::floor(x);
|
||||
|
||||
return std::abs(x) >= 0.975;// && (std::abs(y) >= 0.9 || std::abs(y) < 0.1);
|
||||
|
||||
}
|
||||
|
||||
#endif // POLYGON2_H
|
||||
@@ -55,6 +55,24 @@ public:
|
||||
return *this;
|
||||
}
|
||||
|
||||
/** switch between CW<->CCW */
|
||||
void reverse() {
|
||||
std::swap(p2, p3);
|
||||
}
|
||||
|
||||
void rotate_deg(const Point3 rot) {
|
||||
p1 = p1.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI);
|
||||
p2 = p2.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI);
|
||||
p3 = p3.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI);
|
||||
}
|
||||
|
||||
// void ensureCCW() {
|
||||
// Point3 norm = cross(p3-p1, p2-p1);
|
||||
// if (norm.z < 0) {
|
||||
// reverse();
|
||||
// }
|
||||
// }
|
||||
|
||||
Point3 getNormal() const {
|
||||
return cross( p2-p1, p3-p1 ).normalized();
|
||||
}
|
||||
|
||||
@@ -363,7 +363,7 @@ namespace NM {
|
||||
return false;
|
||||
}
|
||||
|
||||
bool m_keepInterResults = false;
|
||||
// bool m_keepInterResults = false;
|
||||
bool m_filterLowHangingObstacles = false;
|
||||
bool m_filterLedgeSpans = false;
|
||||
bool m_filterWalkableLowHeightSpans = false;
|
||||
|
||||
33
wifi/estimate/ray3/FloorPos.h
Normal file
33
wifi/estimate/ray3/FloorPos.h
Normal file
@@ -0,0 +1,33 @@
|
||||
#ifndef FLOORPOS_H
|
||||
#define FLOORPOS_H
|
||||
|
||||
#include "Obstacle3.h"
|
||||
|
||||
namespace Ray3D {
|
||||
|
||||
/** used to model ceiling thickness */
|
||||
struct FloorPos {
|
||||
float fh;
|
||||
float z1;
|
||||
float z2;
|
||||
float height;
|
||||
FloorPos(const Floorplan::Floor* f) : fh(0.01), z1(f->getStartingZ()), z2(f->getEndingZ()-fh), height(z2-z1) {;}
|
||||
};
|
||||
|
||||
static Obstacle3D::Type getType(const Floorplan::FloorObstacleLine* l) {
|
||||
switch (l->type) {
|
||||
case Floorplan::ObstacleType::WALL: return Obstacle3D::Type::WALL;
|
||||
case Floorplan::ObstacleType::WINDOW: return Obstacle3D::Type::WINDOW;
|
||||
case Floorplan::ObstacleType::HANDRAIL: return Obstacle3D::Type::HANDRAIL;
|
||||
default: return Obstacle3D::Type::UNKNOWN;
|
||||
}
|
||||
}
|
||||
|
||||
static Obstacle3D::Type getType(const Floorplan::FloorObstacleCircle* c) {
|
||||
(void) c;
|
||||
return Obstacle3D::Type::WALL;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif // FLOORPOS_H
|
||||
@@ -205,7 +205,8 @@ namespace Ray3D {
|
||||
for (const Obstacle3D& obs : elements) {
|
||||
for (const Triangle3& tria : obs.triangles) {
|
||||
(void) tria;
|
||||
res << "3 " << (vidx++) << " " << (vidx++) << " " << (vidx++) << "\n";
|
||||
res << "3 " << (vidx+0) << " " << (vidx+1) << " " << (vidx+2) << "\n";
|
||||
vidx += 3;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -10,6 +10,11 @@
|
||||
#include "Tube.h"
|
||||
#include "Cylinder.h"
|
||||
#include "FloorplanMesh.h"
|
||||
#include "FloorPos.h"
|
||||
|
||||
#include "Walls.h"
|
||||
#include "WallsViaCubes.h"
|
||||
#include "WallsViaCuttedQuads.h"
|
||||
|
||||
#include "OBJPool.h"
|
||||
|
||||
@@ -23,7 +28,7 @@ namespace Ray3D {
|
||||
public:
|
||||
|
||||
bool exportCeilings = true;
|
||||
bool exportObstacles = true;
|
||||
bool exportObstacles = true;
|
||||
bool exportStairs = true;
|
||||
bool fancyStairs = true;
|
||||
bool exportHandrails = true;
|
||||
@@ -39,6 +44,9 @@ namespace Ray3D {
|
||||
/** the to-be-exported map */
|
||||
const Floorplan::IndoorMap* map;
|
||||
|
||||
//Walls* walls = new WallsViaCubes();
|
||||
Walls* walls = new WallsViaCuttedQuads();
|
||||
|
||||
public:
|
||||
|
||||
|
||||
@@ -82,6 +90,9 @@ namespace Ray3D {
|
||||
|
||||
if (!f->enabled) {continue;}
|
||||
|
||||
// reset wall-factory
|
||||
walls->clear();
|
||||
|
||||
// triangulize the floor itself (floor/ceiling)
|
||||
if (exportCeilings) {
|
||||
std::vector<Obstacle3D> tmp = getFloor(f);
|
||||
@@ -96,12 +107,14 @@ namespace Ray3D {
|
||||
}
|
||||
}
|
||||
|
||||
// append all created walls
|
||||
const std::vector<Obstacle3D>& oWalls = walls->get();
|
||||
res.insert(res.end(),oWalls.begin(), oWalls.end());
|
||||
|
||||
// stairs
|
||||
if (f->stairs.enabled) {
|
||||
for (const Floorplan::Stair* stair : f->stairs) {
|
||||
|
||||
if (exportStairs) {res.push_back(getStairs(f, stair));}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -265,7 +278,8 @@ namespace Ray3D {
|
||||
case Floorplan::ObstacleType::WINDOW:
|
||||
return getWindow(f, fol, aboveDoor);
|
||||
case Floorplan::ObstacleType::WALL:
|
||||
return getWall(f, fol, aboveDoor);
|
||||
addWall(f, fol, aboveDoor);
|
||||
return Obstacle3D();
|
||||
default:
|
||||
throw Exception("invalid obstacle type");
|
||||
}
|
||||
@@ -273,7 +287,8 @@ namespace Ray3D {
|
||||
}
|
||||
|
||||
Obstacle3D getWindow(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor) const {
|
||||
return getWall(f, fol, aboveDoor);
|
||||
//return getWall(f, fol, aboveDoor);
|
||||
return Obstacle3D();
|
||||
}
|
||||
|
||||
Obstacle3D getPillar(const Floorplan::Floor* f, const Floorplan::FloorObstacleCircle* foc) const {
|
||||
@@ -297,39 +312,8 @@ namespace Ray3D {
|
||||
|
||||
}
|
||||
|
||||
Obstacle3D getWall(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor) const {
|
||||
|
||||
FloorPos fpos(f);
|
||||
|
||||
const float thickness_m = fol->thickness_m;
|
||||
const Point2 from = (!aboveDoor) ? (fol->from) : (aboveDoor->from);
|
||||
const Point2 to = (!aboveDoor) ? (fol->to) : (aboveDoor->to);
|
||||
const Point2 cen2 = (from+to)/2;
|
||||
|
||||
const float rad = std::atan2(to.y - from.y, to.x - from.x);
|
||||
const float deg = rad * 180 / M_PI;
|
||||
|
||||
// cube's destination center
|
||||
const float _height = (fol->height_m > 0) ? (fol->height_m) : (fpos.height); // use either floor's height or user height
|
||||
const double height = (!aboveDoor) ? (_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
|
||||
const float sx = from.getDistance(to) / 2;
|
||||
const float sy = thickness_m / 2;
|
||||
const float sz = height / 2.01f; // prevent overlaps
|
||||
const Point3 size(sx, sy, sz);
|
||||
const Point3 rot(0,0,deg);
|
||||
|
||||
// build
|
||||
Cube cube(pos, size, rot, cubeParts);
|
||||
|
||||
// done
|
||||
Obstacle3D res(getType(fol), fol->material);
|
||||
res.triangles = cube.getTriangles();
|
||||
return res;
|
||||
|
||||
void addWall(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor) const {
|
||||
walls->add(f, fol, aboveDoor);
|
||||
}
|
||||
|
||||
/** 3D Obstacle from .obj 3D mesh */
|
||||
@@ -351,26 +335,6 @@ namespace Ray3D {
|
||||
obs = obs.translated(foo->pos + Point3(0,0,fpos.z1));
|
||||
obs.type = Obstacle3D::Type::OBJECT;
|
||||
|
||||
// std::vector<Triangle3> trias;
|
||||
// for (const OBJReader::Face& face : reader.getData().faces) {
|
||||
// Point3 p1 = face.vnt[0].vertex;
|
||||
// Point3 p2 = face.vnt[1].vertex;
|
||||
// Point3 p3 = face.vnt[2].vertex;
|
||||
// p1 = p1.rot(foo->rot.x/180.0f*M_PI, foo->rot.y/180.0f*M_PI, foo->rot.z/180.0f*M_PI);
|
||||
// p2 = p2.rot(foo->rot.x/180.0f*M_PI, foo->rot.y/180.0f*M_PI, foo->rot.z/180.0f*M_PI);
|
||||
// p3 = p3.rot(foo->rot.x/180.0f*M_PI, foo->rot.y/180.0f*M_PI, foo->rot.z/180.0f*M_PI);
|
||||
// p1 += foo->pos; p1.z += fpos.z1;
|
||||
// p2 += foo->pos; p2.z += fpos.z1;
|
||||
// p3 += foo->pos; p3.z += fpos.z1;
|
||||
// const Triangle3 tria(p1, p2, p3);
|
||||
// trias.push_back(tria);
|
||||
// }
|
||||
|
||||
// // done
|
||||
// Obstacle3D res(Obstacle3D::Type::OBJECT, Floorplan::Material::WOOD);
|
||||
// res.triangles = trias;
|
||||
// return res;
|
||||
|
||||
return obs;
|
||||
|
||||
}
|
||||
@@ -672,30 +636,6 @@ namespace Ray3D {
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
static Obstacle3D::Type getType(const Floorplan::FloorObstacleLine* l) {
|
||||
switch (l->type) {
|
||||
case Floorplan::ObstacleType::WALL: return Obstacle3D::Type::WALL;
|
||||
case Floorplan::ObstacleType::WINDOW: return Obstacle3D::Type::WINDOW;
|
||||
case Floorplan::ObstacleType::HANDRAIL: return Obstacle3D::Type::HANDRAIL;
|
||||
default: return Obstacle3D::Type::UNKNOWN;
|
||||
}
|
||||
}
|
||||
|
||||
static Obstacle3D::Type getType(const Floorplan::FloorObstacleCircle* c) {
|
||||
(void) c;
|
||||
return Obstacle3D::Type::WALL;
|
||||
}
|
||||
|
||||
/** used to model ceiling thickness */
|
||||
struct FloorPos {
|
||||
float fh;
|
||||
float z1;
|
||||
float z2;
|
||||
float height;
|
||||
FloorPos(const Floorplan::Floor* f) : fh(0.01), z1(f->getStartingZ()), z2(f->getEndingZ()-fh), height(z2-z1) {;}
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@@ -26,6 +26,7 @@ namespace Ray3D {
|
||||
WALL,
|
||||
WINDOW,
|
||||
OBJECT,
|
||||
ERROR,
|
||||
};
|
||||
|
||||
Type type;
|
||||
@@ -38,6 +39,27 @@ namespace Ray3D {
|
||||
/** ctor */
|
||||
Obstacle3D(Type type, Floorplan::Material mat) : type(type), mat(mat) {;}
|
||||
|
||||
|
||||
/** append a new triangle. REFERENCE ONLY VALID UNTIL NEXT ADD */
|
||||
Triangle3& addTriangle(const Point3 p1, const Point3 p2, const Point3 p3) {
|
||||
triangles.push_back(Triangle3(p1, p2, p3));
|
||||
return triangles.back();
|
||||
}
|
||||
|
||||
/** append a new quad by splitting into two triangles */
|
||||
void addQuad(const Point3 p1, const Point3 p2, const Point3 p3, const Point3 p4) {
|
||||
addTriangle(p1, p2, p3);
|
||||
addTriangle(p1, p3, p4);
|
||||
}
|
||||
|
||||
/** reverse all faces (CW<->CCW) */
|
||||
void reverseFaces() {
|
||||
for (Triangle3& t : triangles) {
|
||||
t.reverse();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/** scaled copy */
|
||||
Obstacle3D scaled(const Point3 scale) const {
|
||||
Obstacle3D copy = *this;
|
||||
@@ -60,9 +82,10 @@ namespace Ray3D {
|
||||
Obstacle3D rotated_deg(const Point3 rot) const {
|
||||
Obstacle3D copy = *this;
|
||||
for (Triangle3& tria : copy.triangles) {
|
||||
tria.p1 = tria.p1.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI);
|
||||
tria.p2 = tria.p2.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI);
|
||||
tria.p3 = tria.p3.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI);
|
||||
// tria.p1 = tria.p1.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI);
|
||||
// tria.p2 = tria.p2.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI);
|
||||
// tria.p3 = tria.p3.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI);
|
||||
tria.rotate_deg(rot);
|
||||
}
|
||||
return copy;
|
||||
}
|
||||
|
||||
22
wifi/estimate/ray3/Walls.h
Normal file
22
wifi/estimate/ray3/Walls.h
Normal file
@@ -0,0 +1,22 @@
|
||||
#ifndef WALLS_H
|
||||
#define WALLS_H
|
||||
|
||||
#include "Obstacle3.h"
|
||||
|
||||
namespace Ray3D {
|
||||
|
||||
class Walls {
|
||||
|
||||
public:
|
||||
|
||||
virtual void clear() = 0;
|
||||
|
||||
virtual void add(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor) = 0;
|
||||
|
||||
virtual const std::vector<Obstacle3D>& get() = 0;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // WALLS_H
|
||||
69
wifi/estimate/ray3/WallsViaCubes.h
Normal file
69
wifi/estimate/ray3/WallsViaCubes.h
Normal file
@@ -0,0 +1,69 @@
|
||||
#ifndef WALLSVIACUBE_H
|
||||
#define WALLSVIACUBE_H
|
||||
|
||||
#include "Walls.h"
|
||||
#include "FloorPos.h"
|
||||
#include "Cube.h"
|
||||
|
||||
namespace Ray3D {
|
||||
|
||||
/**
|
||||
* simply use one 3D cube per wall
|
||||
* if walls intersect in the 2D view, cubes will also intersect
|
||||
*/
|
||||
class WallsViaCubes : public Walls {
|
||||
|
||||
Cube::Part cubeParts = (Cube::Part) 63; // leftright,topbottom,rearfront
|
||||
|
||||
std::vector<Obstacle3D> vec;
|
||||
|
||||
public:
|
||||
|
||||
void clear() override {
|
||||
vec.clear();
|
||||
}
|
||||
|
||||
void add(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor) override {
|
||||
|
||||
FloorPos fpos(f);
|
||||
|
||||
const float thickness_m = fol->thickness_m;
|
||||
const Point2 from = (!aboveDoor) ? (fol->from) : (aboveDoor->from);
|
||||
const Point2 to = (!aboveDoor) ? (fol->to) : (aboveDoor->to);
|
||||
const Point2 cen2 = (from+to)/2;
|
||||
|
||||
const float rad = std::atan2(to.y - from.y, to.x - from.x);
|
||||
const float deg = rad * 180 / M_PI;
|
||||
|
||||
// cube's destination center
|
||||
const float _height = (fol->height_m > 0) ? (fol->height_m) : (fpos.height); // use either floor's height or user height
|
||||
const double height = (!aboveDoor) ? (_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
|
||||
const float sx = from.getDistance(to) / 2;
|
||||
const float sy = thickness_m / 2;
|
||||
const float sz = height / 2.01f; // prevent overlaps
|
||||
const Point3 size(sx, sy, sz);
|
||||
const Point3 rot(0,0,deg);
|
||||
|
||||
// build
|
||||
Cube cube(pos, size, rot, cubeParts);
|
||||
|
||||
// done
|
||||
Obstacle3D res(getType(fol), fol->material);
|
||||
res.triangles = cube.getTriangles();
|
||||
vec.push_back(res);
|
||||
|
||||
}
|
||||
|
||||
const std::vector<Obstacle3D>& get() override {
|
||||
return vec;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // WALLSVIACUBE_H
|
||||
293
wifi/estimate/ray3/WallsViaCuttedQuads.h
Normal file
293
wifi/estimate/ray3/WallsViaCuttedQuads.h
Normal file
@@ -0,0 +1,293 @@
|
||||
#ifndef WALLSVIACUTTEDQUADS_H
|
||||
#define WALLSVIACUTTEDQUADS_H
|
||||
|
||||
#include "../../../geo/Line2.h"
|
||||
#include "../../../geo/Polygon2.h"
|
||||
#include "Walls.h"
|
||||
#include "FloorPos.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
namespace Ray3D {
|
||||
|
||||
/**
|
||||
* interpret walls als quads (polygons)
|
||||
* intersect them with each other to prevent overlaps
|
||||
*/
|
||||
class WallsViaCuttedQuads : public Walls {
|
||||
|
||||
private:
|
||||
|
||||
struct Wall {
|
||||
|
||||
/** algorithms set error flag here */
|
||||
bool error = false;
|
||||
|
||||
/** original line from floorplan */
|
||||
const Floorplan::FloorObstacleLine* line;
|
||||
|
||||
/** outlines after applying thickness */
|
||||
Line2 l1;
|
||||
Line2 l2;
|
||||
|
||||
Wall(const Floorplan::FloorObstacleLine* line) : line(line) {
|
||||
|
||||
const Point2 from = line->from;
|
||||
const Point2 to = line->to;
|
||||
const Point2 dir = (to-from).normalized();
|
||||
const Point2 dirP = dir.perpendicular();
|
||||
const float w = line->thickness_m;
|
||||
const float w2 = w/2;
|
||||
|
||||
const Point2 p1 = from + dirP * w2;
|
||||
const Point2 p2 = from - dirP * w2;
|
||||
const Point2 p3 = to - dirP * w2;
|
||||
const Point2 p4 = to + dirP * w2;
|
||||
|
||||
l1 = Line2(p1, p4);
|
||||
l2 = Line2(p2, p3);
|
||||
|
||||
}
|
||||
|
||||
/** get points for CCW wall outline (p1->p2->p3->p4) */
|
||||
Point2 getP1() const {return l1.p1;}
|
||||
Point2 getP2() const {return l1.p2;}
|
||||
Point2 getP3() const {return l2.p2;}
|
||||
Point2 getP4() const {return l2.p1;}
|
||||
|
||||
Point2& getP1() {return l1.p1;}
|
||||
Point2& getP2() {return l1.p2;}
|
||||
Point2& getP3() {return l2.p2;}
|
||||
Point2& getP4() {return l2.p1;}
|
||||
|
||||
struct CutRes {
|
||||
Point2 p;
|
||||
Line2* l1; // affected line within this wall
|
||||
Line2* l2; // affected line within the other wall
|
||||
CutRes(Point2 p, Line2* l1, Line2* l2) : p(p), l1(l1), l2(l2) {;}
|
||||
};
|
||||
|
||||
/** get all intersecting points between the two walls */
|
||||
std::vector<CutRes> getIntersections(Wall& o, bool limit = true) {
|
||||
std::vector<CutRes> res;
|
||||
Point2 p;
|
||||
if (intersects(l1, o.l1, limit, p)) {res.push_back(CutRes(p,&l1, &o.l1));}
|
||||
if (intersects(l1, o.l2, limit, p)) {res.push_back(CutRes(p,&l1, &o.l2));}
|
||||
if (intersects(l2, o.l1, limit, p)) {res.push_back(CutRes(p,&l2, &o.l1));}
|
||||
if (intersects(l2, o.l2, limit, p)) {res.push_back(CutRes(p,&l2, &o.l2));}
|
||||
return res;
|
||||
}
|
||||
|
||||
/** is this wall directly attached to the given wall? */
|
||||
bool directlyConnectedTo(const Wall& o) const {
|
||||
const float d = 0.001;
|
||||
return
|
||||
(line->from.eq( o.line->from, d)) ||
|
||||
(line->to.eq( o.line->from, d)) ||
|
||||
(line->from.eq( o.line->to, d)) ||
|
||||
(line->to.eq( o.line->to, d));
|
||||
}
|
||||
|
||||
/** does this wall end within the given wall? */
|
||||
bool endsWithin(const Wall& o) const {
|
||||
return o.containsPoint(line->from) || o.containsPoint(line->to);
|
||||
}
|
||||
|
||||
/** does this wall contain the given point */
|
||||
bool containsPoint(const Point2 p) const {
|
||||
return polygonContainsPoint({l1.p1, l1.p2, l2.p2, l2.p1}, p);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
void cropLine(Line2* l, Point2 p) {
|
||||
|
||||
// determine which line-end to crop
|
||||
if (p.getDistance(l->p1) < p.getDistance(l->p2)) {
|
||||
l->p1 = p;
|
||||
} else {
|
||||
l->p2 = p;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void cutInMiddle(Point2& pa1, Point2& pa2, Point2& pb1, Point2& pb2) {
|
||||
|
||||
// line from pa1->pa2, and pb1->pb2
|
||||
// intersection expected near pa2|pb1
|
||||
|
||||
Line2 l1(pa1, pa2); l1 = l1.longerAtEnd(10);
|
||||
Line2 l2(pb1, pb2); l2 = l2.longerAtStart(10);
|
||||
|
||||
Point2 pi;
|
||||
if (intersects(l1, l2, true, pi)) {
|
||||
pa2 = pi; // replace end of line1
|
||||
pb1 = pi; // replace start of line2
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
std::vector<Wall> walls;
|
||||
const Floorplan::Floor* floor;
|
||||
std::vector<Obstacle3D> obs;
|
||||
|
||||
public:
|
||||
|
||||
|
||||
void clear() override {
|
||||
walls.clear();
|
||||
}
|
||||
|
||||
void add(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor) override {
|
||||
if (fol->type != Floorplan::ObstacleType::WALL) {return;}
|
||||
if (aboveDoor) {return;}
|
||||
this->floor = f;
|
||||
walls.push_back(Wall(fol));
|
||||
}
|
||||
|
||||
virtual const std::vector<Obstacle3D>& get() override {
|
||||
std::vector<Wall> tmp = walls;
|
||||
tmp = cutConnected(tmp);
|
||||
tmp = cutProtruding(tmp);
|
||||
obs = toObstacle(tmp, floor);
|
||||
return obs;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/** convert all walls to obstacles */
|
||||
std::vector<Obstacle3D> toObstacle(const std::vector<Wall>& walls, const Floorplan::Floor* f) {
|
||||
std::vector<Obstacle3D> res;
|
||||
for (const Wall& w : walls) {
|
||||
res.push_back(toObstacle(w, f));
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
/** convert one wall into an obstacle */
|
||||
Obstacle3D toObstacle(const Wall& wall, const Floorplan::Floor* f) {
|
||||
|
||||
Obstacle3D::Type type = (wall.error) ? (Obstacle3D::Type::ERROR) : (getType(wall.line));
|
||||
Obstacle3D obs(type, wall.line->material);
|
||||
|
||||
const float z1 = f->getStartingZ();
|
||||
const float z2 = f->getEndingZ();
|
||||
|
||||
const Point3 p1 = Point3(wall.getP1().x, wall.getP1().y, z1);
|
||||
const Point3 p2 = Point3(wall.getP2().x, wall.getP2().y, z1);
|
||||
const Point3 p3 = Point3(wall.getP3().x, wall.getP3().y, z1);
|
||||
const Point3 p4 = Point3(wall.getP4().x, wall.getP4().y, z1);
|
||||
|
||||
const Point3 p1u = Point3(wall.getP1().x, wall.getP1().y, z2);
|
||||
const Point3 p2u = Point3(wall.getP2().x, wall.getP2().y, z2);
|
||||
const Point3 p3u = Point3(wall.getP3().x, wall.getP3().y, z2);
|
||||
const Point3 p4u = Point3(wall.getP4().x, wall.getP4().y, z2);
|
||||
|
||||
obs.addQuad(p1, p2, p2u, p1u);
|
||||
obs.addQuad(p2, p3, p3u, p2u);
|
||||
obs.addQuad(p3, p4, p4u, p3u);
|
||||
obs.addQuad(p4, p1, p1u, p4u);
|
||||
|
||||
obs.addQuad(p1u, p2u, p3u, p4u);
|
||||
obs.addQuad(p4, p3, p2, p1);
|
||||
|
||||
obs.reverseFaces();
|
||||
|
||||
return obs;
|
||||
|
||||
}
|
||||
|
||||
/** cut off walls ending within another wall */
|
||||
std::vector<Wall> cutProtruding(std::vector<Wall> walls) {
|
||||
|
||||
// if one wall ends within another one cut it off
|
||||
|
||||
for (size_t i = 0; i < walls.size(); ++i) {
|
||||
Wall& w1 = walls[i];
|
||||
|
||||
for (size_t j = i+1; j < walls.size(); ++j) {
|
||||
Wall& w2 = walls[j];
|
||||
|
||||
if (i == j) {continue;}
|
||||
|
||||
// if the two walls are directly connected (share one node) -> ignore this case here!
|
||||
if (w1.directlyConnectedTo(w2)) {continue;}
|
||||
|
||||
// not the case we are looking for?
|
||||
if (!w1.endsWithin(w2) && !w2.endsWithin(w1)) {continue;}
|
||||
|
||||
// get all intersection points between the two walls
|
||||
std::vector<Wall::CutRes> isects = w1.getIntersections(w2);
|
||||
|
||||
// this should be 0 (no intersections) or 2 (one for each outline)
|
||||
if (!isects.empty() && isects.size() != 2) {
|
||||
w1.error = true;
|
||||
w2.error = true;
|
||||
std::cout << "detected strange wall intersection" << std::endl;
|
||||
}
|
||||
|
||||
// check all detected intersections
|
||||
for (const auto isect : isects) {
|
||||
|
||||
// if one of the line-ends p1/p2 from wall1 ends within wall2, crop it by setting it to the intersection
|
||||
if (w2.containsPoint(isect.l1->p1)) {isect.l1->p1 = isect.p;}
|
||||
if (w2.containsPoint(isect.l1->p2)) {isect.l1->p2 = isect.p;}
|
||||
|
||||
// if one of the line-ends p1/p2 from wall2 ends within wall1, crop it by setting it to the intersection
|
||||
if (w1.containsPoint(isect.l2->p1)) {isect.l2->p1 = isect.p;}
|
||||
if (w1.containsPoint(isect.l2->p2)) {isect.l2->p2 = isect.p;}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return walls;
|
||||
|
||||
}
|
||||
|
||||
|
||||
/** if two walls share one node, cut both ends in the middle (like 45 degree) */
|
||||
std::vector<Wall> cutConnected(std::vector<Wall> walls) {
|
||||
|
||||
for (size_t i = 0; i < walls.size(); ++i) {
|
||||
Wall& w1 = walls[i];
|
||||
|
||||
for (size_t j = i+1; j < walls.size(); ++j) {
|
||||
Wall& w2 = walls[j];
|
||||
|
||||
if (i == j) {continue;}
|
||||
|
||||
// if the two walls are note directly connected -> ignore
|
||||
if (!w1.directlyConnectedTo(w2)) {continue;}
|
||||
|
||||
const float d = 0.001;
|
||||
if (w1.line->to.eq(w2.line->from, d)) {
|
||||
cutInMiddle(w1.getP1(), w1.getP2(), w2.getP1(), w2.getP2());
|
||||
cutInMiddle(w1.getP4(), w1.getP3(), w2.getP4(), w2.getP3());
|
||||
} else if (w1.line->to.eq(w2.line->to, d)) {
|
||||
cutInMiddle(w1.getP1(), w1.getP2(), w2.getP3(), w2.getP4());
|
||||
cutInMiddle(w1.getP4(), w1.getP3(), w2.getP2(), w2.getP1());
|
||||
} else if (w1.line->from.eq(w2.line->to, d)) {
|
||||
cutInMiddle(w1.getP3(), w1.getP4(), w2.getP3(), w2.getP4());
|
||||
cutInMiddle(w1.getP2(), w1.getP1(), w2.getP2(), w2.getP1());
|
||||
} else if (w1.line->from.eq(w2.line->from, d)) {
|
||||
cutInMiddle(w1.getP3(), w1.getP4(), w2.getP1(), w2.getP2());
|
||||
cutInMiddle(w1.getP2(), w1.getP1(), w2.getP4(), w2.getP3());
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return walls;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // WALLSVIACUTTEDQUADS_H
|
||||
Reference in New Issue
Block a user