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

new helper methods

Browse Source 
adjusted wall intersection
This commit is contained in:
FrankE
2018-07-22 17:32:44 +02:00
parent 8ea7b7f3b6
commit 083a1c2cf2
5 changed files with 132 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wifi/estimate/ray3/FloorPos.h
View File

@@ -1,6 +1,8 @@
#ifndef FLOORPOS_H
#define FLOORPOS_H
#include "Obstacle3.h"
namespace Ray3D {
/** used to model ceiling thickness */

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

@@ -26,6 +26,7 @@ namespace Ray3D {
WALL,
WINDOW,
OBJECT,
ERROR,
};
Type type;

222
wifi/estimate/ray3/WallsViaCuttedQuads.h
View File

@@ -4,7 +4,9 @@
#include "../../../geo/Line2.h"
#include "../../../geo/Polygon2.h"
#include "Walls.h"
#include "FloorPos.h"
#include <iostream>
namespace Ray3D {
@@ -18,7 +20,13 @@ namespace Ray3D {
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;
@@ -41,18 +49,25 @@ namespace Ray3D {
}
/** 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; // within this wall
Line2* l2; // within the other wall
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;
@@ -63,7 +78,23 @@ namespace Ray3D {
return res;
}
bool containsPoint(const Point2 p) {
/** 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);
}
@@ -80,6 +111,22 @@ namespace Ray3D {
}
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;
@@ -99,43 +146,34 @@ namespace Ray3D {
}
virtual const std::vector<Obstacle3D>& get() override {
std::vector<Wall> tmp = cut(walls);
obs = toObs(tmp, floor);
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> toObs(const std::vector<Wall>& walls, const Floorplan::Floor* f) {
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(toObs(w, f));
res.push_back(toObstacle(w, f));
}
return res;
}
// bool isCW(Line2 l) {
// float a = l.getAngle();
// return a < 0 || a <= M_PI/2 || a >= M_PI*1.5;
// //return a >= 0 && a <= M_PI;
// }
/** convert one wall into an obstacle */
Obstacle3D toObs(const Wall& wall, const Floorplan::Floor* f) {
Obstacle3D toObstacle(const Wall& wall, const Floorplan::Floor* f) {
Obstacle3D obs(getType(wall.line), wall.line->material);
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();
// Line2 l1(wall.getP1(), wall.getP2());
// Line2 l2(wall.getP3(), wall.getP4());
// float len1 = l1.getLength();
// float a1 = l1.getAngle() * 180 / M_PI;
// float a2 = l2.getAngle() * 180 / M_PI;
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);
@@ -156,30 +194,14 @@ namespace Ray3D {
obs.reverseFaces();
// // front
// Triangle3 t1( Point3(0, 0, z1), Point3(len1, 0, z1), Point3(len1, 0, z2) );
// t1.reverse(); t1.rotate_deg(Point3(0,0,a1)); t1 += Point3(l1.p1.x, l1.p1.y, 0);
// Triangle3 t2( Point3(0, 0, z1), Point3(len1, 0, z2), Point3(0, 0, z2) );
// t2.reverse(); t2.rotate_deg(Point3(0,0,a1)); t2 += Point3(l1.p1.x, l1.p1.y, 0);
// // rear
// Triangle3 t3( Point3(0, 0, z1), Point3(len1, 0, z1), Point3(len1, 0, z2) );
// t3.reverse(); t3.rotate_deg(Point3(0,0,a2)); t3 += Point3(l2.p1.x, l2.p1.y, 0);
// Triangle3 t4( Point3(0, 0, z1), Point3(len1, 0, z2), Point3(0, 0, z2) );
// t4.reverse(); t4.rotate_deg(Point3(0,0,a2)); t4 += Point3(l2.p1.x, l2.p1.y, 0);
// obs.triangles.push_back(t1);
// obs.triangles.push_back(t2);
// obs.triangles.push_back(t3);
// obs.triangles.push_back(t4);
//// obs.triangles.push_back(t4);
return obs;
}
std::vector<Wall> cut(std::vector<Wall> walls) {
/** 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];
@@ -189,93 +211,71 @@ namespace Ray3D {
if (i == j) {continue;}
std::vector<Wall::CutRes> isect = w1.getIntersections(w2);
// if the two walls are directly connected (share one node) -> ignore this case here!
if (w1.directlyConnectedTo(w2)) {continue;}
// for (const auto c : isect) {
// out << "<circle cx='" << ox+c.p.x*s << "' cy='" << oy+(h-c.p.y)*s << "' r='2.0'/>\n";
// }
// 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 c : isect) {
for (const auto isect : isects) {
// we have two polygons and need to decide which line to crop:
// we want to cut-off the polygon, that has ending points within the other polygon!
// -> check which line (that contains the intersection point) ends within the other polygon
const bool p1EndsWithinP2 = w2.containsPoint(c.l1->p1) || w2.containsPoint(c.l1->p2);
const bool p2EndsWithinP1 = w1.containsPoint(c.l2->p1) || w1.containsPoint(c.l2->p2);
// 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 both polygons end within the other one (this is the case for edges where two lines are conencted) -> ignore
if (p1EndsWithinP2 && p2EndsWithinP1) {
// 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;}
isect = w1.getIntersections(w2, false);
if (isect.size() != 4) {continue;}
}
// center of the 4 intersections
Point2 isectCen;
for (const auto c2 : isect) {isectCen += c2.p;}
isectCen /= isect.size();
}
// 2 vectors along the two walls, both pointing AWAY from the center of intersection
const Point2 dir1 = isectCen.getDistance(w1.line->from) < isectCen.getDistance(w1.line->to) ? (w1.line->to - w1.line->from) : (w1.line->from - w1.line->to);
const Point2 dir2 = isectCen.getDistance(w2.line->from) < isectCen.getDistance(w2.line->to) ? (w2.line->to - w2.line->from) : (w2.line->from - w2.line->to);
}
// average between both, after normalization
const Point2 dirFromCen = (dir1.normalized()+dir2.normalized()) / 2;
return walls;
// pointing INWARDS (between both walls) away from the intersection center
const Point2 innerCenter = isectCen + dirFromCen * 10;
}
// sort the 4 intersections by distance from innerCenter
// we want to know the farthest and the nearest of the 4
auto comp = [innerCenter] (const Wall::CutRes& a, const Wall::CutRes& b) {
return a.p.getDistance(innerCenter) < b.p.getDistance(innerCenter);
};
std::sort(isect.begin(), isect.end(), comp);
// w1 will be made longer
// w2 will be made shorter
/** if two walls share one node, cut both ends in the middle (like 45 degree) */
std::vector<Wall> cutConnected(std::vector<Wall> walls) {
bool use45 = false;
for (size_t i = 0; i < walls.size(); ++i) {
Wall& w1 = walls[i];
if (use45) {
for (size_t j = i+1; j < walls.size(); ++j) {
Wall& w2 = walls[j];
// farthest from center
cropLine(isect[3].l1, isect[3].p);
cropLine(isect[3].l2, isect[3].p);
if (i == j) {continue;}
// nearest to center
cropLine(isect[0].l2, isect[0].p);
cropLine(isect[0].l1, isect[0].p);
} else {
// farthest from center
cropLine(isect[3].l1, isect[3].p);
// nearest to center
cropLine(isect[0].l2, isect[0].p);
//cropLine(isect[2].l2, isect[2].p);
// shared point
if (isect[3].l1 != isect[2].l1) {
cropLine(isect[2].l2, isect[2].p);
cropLine(isect[2].l1, isect[2].p);
} else {
cropLine(isect[1].l2, isect[1].p);
cropLine(isect[1].l1, isect[1].p);
}
}
} else if (p1EndsWithinP2) {
// crop the intersecting line within polygon 1
cropLine(c.l1, c.p);
} else if (p2EndsWithinP1) {
// crop the intersecting line within polygon 2
cropLine(c.l2, c.p);
}
// 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());
}
}