294 lines
8.2 KiB
C++
294 lines
8.2 KiB
C++
#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
|