294 lines
8.5 KiB
C++
294 lines
8.5 KiB
C++
#ifndef WALLSVIACUTTEDQUADS_H
|
|
#define WALLSVIACUTTEDQUADS_H
|
|
|
|
#include "../../../geo/Line2.h"
|
|
#include "../../../geo/Polygon2.h"
|
|
#include "Walls.h"
|
|
|
|
|
|
namespace Ray3D {
|
|
|
|
/**
|
|
* interpret walls als quads (polygons)
|
|
* intersect them with each other to prevent overlaps
|
|
*/
|
|
class WallsViaCuttedQuads : public Walls {
|
|
|
|
private:
|
|
|
|
struct Wall {
|
|
|
|
const Floorplan::FloorObstacleLine* line;
|
|
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);
|
|
|
|
}
|
|
|
|
Point2 getP1() const {return l1.p1;}
|
|
Point2 getP2() const {return l1.p2;}
|
|
Point2 getP3() const {return l2.p2;}
|
|
Point2 getP4() const {return l2.p1;}
|
|
|
|
struct CutRes {
|
|
Point2 p;
|
|
Line2* l1; // within this wall
|
|
Line2* l2; // within the other wall
|
|
CutRes(Point2 p, Line2* l1, Line2* l2) : p(p), l1(l1), l2(l2) {;}
|
|
};
|
|
|
|
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;
|
|
}
|
|
|
|
bool containsPoint(const Point2 p) {
|
|
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;
|
|
}
|
|
|
|
}
|
|
|
|
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 = cut(walls);
|
|
obs = toObs(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> res;
|
|
for (const Wall& w : walls) {
|
|
res.push_back(toObs(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 obs(getType(wall.line), 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);
|
|
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();
|
|
|
|
// // 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) {
|
|
|
|
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;}
|
|
|
|
std::vector<Wall::CutRes> isect = w1.getIntersections(w2);
|
|
|
|
// for (const auto c : isect) {
|
|
// out << "<circle cx='" << ox+c.p.x*s << "' cy='" << oy+(h-c.p.y)*s << "' r='2.0'/>\n";
|
|
// }
|
|
|
|
// check all detected intersections
|
|
for (const auto c : isect) {
|
|
|
|
// 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 both polygons end within the other one (this is the case for edges where two lines are conencted) -> ignore
|
|
if (p1EndsWithinP2 && p2EndsWithinP1) {
|
|
|
|
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;
|
|
|
|
// 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
|
|
|
|
bool use45 = false;
|
|
|
|
if (use45) {
|
|
|
|
// farthest from center
|
|
cropLine(isect[3].l1, isect[3].p);
|
|
cropLine(isect[3].l2, isect[3].p);
|
|
|
|
// 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);
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return walls;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
}
|
|
|
|
#endif // WALLSVIACUTTEDQUADS_H
|