503 lines
13 KiB
C++
503 lines
13 KiB
C++
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#ifndef GRIDFACTORY3_H
|
||
#define GRIDFACTORY3_H
|
||
|
||
#include "../../Grid.h"
|
||
#include "../../../floorplan/v2/Floorplan.h"
|
||
#include "HelperPoly3.h"
|
||
#include <unordered_set>
|
||
|
||
#if (GRID_MODE == GM_BOX)
|
||
|
||
#define GF3_ITER_XY for (int y = y1; y <= y2; y += gs_cm) { for (int x = x1; x <= x2; x += gs_cm) {
|
||
|
||
#elif (GRID_MODE == GM_HOBEYCOMB)
|
||
|
||
#define GF3_ITER_XY\
|
||
for (int y = y1; y <= y2; y += gs_cm) {\
|
||
const int xx = (y / gs_cm % 2 == 0) ? (0) : (gs_cm/2);\
|
||
for (int x = x1-xx; x <= x2; x += gs_cm) {
|
||
|
||
#endif
|
||
|
||
template <typename Node> class GridFactory3 {
|
||
|
||
private:
|
||
|
||
|
||
Grid<Node>& grid;
|
||
const int gs_cm;
|
||
|
||
struct NewNode {
|
||
GridPoint pos;
|
||
int type;
|
||
NewNode(const GridPoint pos, const int type) : pos(pos), type(type) {;}
|
||
bool operator == (const NewNode& o) const {return o.pos == pos;}
|
||
};
|
||
|
||
public:
|
||
|
||
GridFactory3(Grid<Node>& grid) : grid(grid), gs_cm(grid.getGridSize_cm()) {
|
||
|
||
}
|
||
|
||
void build(const Floorplan::IndoorMap* map) {
|
||
|
||
std::vector<NewNode> add;
|
||
std::vector<NewNode> rem;
|
||
|
||
for (const Floorplan::Floor* floor : map->floors) {
|
||
|
||
// for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) {
|
||
|
||
// const std::vector<NewNode> pts = getPointsOn(floor, *poly);
|
||
// if (poly->method == Floorplan::OutlineMethod::ADD) {
|
||
// add.insert(add.end(), pts.begin(), pts.end());
|
||
// } else {
|
||
// rem.insert(rem.end(), pts.begin(), pts.end());
|
||
// }
|
||
|
||
// }
|
||
|
||
const std::vector<NewNode> pts = getPointsOn(floor);
|
||
add.insert(add.end(), pts.begin(), pts.end());
|
||
|
||
for (const Floorplan::Stair* stair : floor->stairs) {
|
||
std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(stair->getParts(), floor);
|
||
const std::vector<NewNode> pts = getPointsOn(floor, quads);
|
||
add.insert(add.end(), pts.begin(), pts.end());
|
||
}
|
||
|
||
|
||
}
|
||
|
||
for (const NewNode& nn : add) {
|
||
auto it = std::find(rem.begin(), rem.end(), nn);
|
||
if (it == rem.end()) {
|
||
if (!grid.hasNodeFor(nn.pos)) {
|
||
Node n(nn.pos.x_cm, nn.pos.y_cm, nn.pos.z_cm);
|
||
n.setType(nn.type);
|
||
grid.add(n);
|
||
}
|
||
}
|
||
}
|
||
|
||
connect(map);
|
||
removeIsolatedNodes();
|
||
|
||
}
|
||
|
||
bool isBlocked(const Floorplan::IndoorMap* map, const Node& n1, const Node& n2) {
|
||
|
||
Line2 lNodes(n1.inMeter().xy(), n2.inMeter().xy());
|
||
|
||
for (Floorplan::Floor* floor : map->floors) {
|
||
|
||
if (n1.inMeter().z != floor->atHeight) {continue;}
|
||
if (n2.inMeter().z != floor->atHeight) {continue;}
|
||
|
||
for (Floorplan::FloorObstacle* obs : floor->obstacles) {
|
||
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
|
||
if (line) {
|
||
const std::vector<Line2> lines = getThickLines(line);
|
||
for (const Line2& lObs : lines) {
|
||
if (lObs.getSegmentIntersection(lNodes)) {
|
||
return true;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
return false;
|
||
|
||
}
|
||
|
||
/** as line-obstacles have a thickness, we need 4 lines for the intersection test! */
|
||
static std::vector<Line2> getThickLines(const Floorplan::FloorObstacleLine* line) {
|
||
//const Line2 base(line->from*100, line->to*100);
|
||
const float thickness_m = line->thickness_m;
|
||
const Point2 dir = (line->to - line->from); // obstacle's direction
|
||
const Point2 perp = dir.perpendicular().normalized(); // perpendicular direction (90 degree)
|
||
const Point2 p1 = line->from + perp * thickness_m/2; // start-up
|
||
const Point2 p2 = line->from - perp * thickness_m/2; // start-down
|
||
const Point2 p3 = line->to + perp * thickness_m/2; // end-up
|
||
const Point2 p4 = line->to - perp * thickness_m/2; // end-down
|
||
return {
|
||
Line2(p1, p2),
|
||
Line2(p3, p4),
|
||
Line2(p2, p4),
|
||
Line2(p1, p3),
|
||
};
|
||
}
|
||
|
||
void connect(const Floorplan::IndoorMap* map) {
|
||
|
||
for (Node& n1 : grid) {
|
||
for (Node& n2 : grid) {
|
||
|
||
if (n1 == n2) {continue;}
|
||
|
||
|
||
// stair with floor
|
||
if (
|
||
(n1.getType() == GridNode::TYPE_STAIR && n2.getType() == GridNode::TYPE_FLOOR) ||
|
||
(n2.getType() == GridNode::TYPE_STAIR && n1.getType() == GridNode::TYPE_FLOOR)
|
||
) {
|
||
|
||
const float distxy = n1.inMeter().xy().getDistance(n2.inMeter().xy());
|
||
const float distz_cm = std::abs(n1.z_cm - n2.z_cm);
|
||
if (distxy > 0 && distxy < gs_cm * 1.2 / 100.0f && distz_cm < gs_cm) { // [1.85]
|
||
if (n1.fullyConnected()) {continue;}
|
||
if (n2.fullyConnected()) {continue;}
|
||
grid.connectUniDir(n1, n2);
|
||
}
|
||
|
||
// floor with floor
|
||
} else if (n1.getType() == GridNode::TYPE_FLOOR && n2.getType() == GridNode::TYPE_FLOOR) {
|
||
if (n1.getDistanceInCM(n2) < gs_cm * 1.2 && !isBlocked(map, n1, n2)) { // [1.2 | 1.845]
|
||
if (n1.fullyConnected()) {continue;}
|
||
if (n2.fullyConnected()) {continue;}
|
||
grid.connectUniDir(n1, n2);
|
||
}
|
||
|
||
// stair with stair
|
||
} else if (n1.getType() == GridNode::TYPE_STAIR && n2.getType() == GridNode::TYPE_STAIR) {
|
||
|
||
const float distxy = n1.inMeter().xy().getDistance(n2.inMeter().xy());
|
||
const float distz_cm = std::abs(n1.z_cm - n2.z_cm);
|
||
|
||
// if (n1.getDistanceInCM(n2) < gs_cm * 1.45 && !isBlocked(map, n1, n2)) {
|
||
if (distxy < gs_cm * 1.2 / 100.0f && distz_cm <= gs_cm) { // [1.845]
|
||
if (n1.fullyConnected()) {continue;}
|
||
if (n2.fullyConnected()) {continue;}
|
||
grid.connectUniDir(n1, n2);
|
||
}
|
||
|
||
}
|
||
|
||
|
||
// if (n1.getDistanceInCM(n2) < gs_cm * 1.7 && !isBlocked(map, n1, n2)) {
|
||
// if (n1.fullyConnected()) {continue;}
|
||
// if (n2.fullyConnected()) {continue;}
|
||
// grid.connectUniDir(n1, n2);
|
||
// }
|
||
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
/** recursively get all connected nodes and add them to the set */
|
||
void getConnected(Node& n1, std::unordered_set<int>& visited) {
|
||
|
||
std::unordered_set<int> toVisit;
|
||
toVisit.insert(n1.getIdx());
|
||
|
||
// run while there are new nodes to visit
|
||
while(!toVisit.empty()) {
|
||
|
||
// get the next node
|
||
int nextIdx = *toVisit.begin();
|
||
toVisit.erase(nextIdx);
|
||
visited.insert(nextIdx);
|
||
Node& next = grid[nextIdx];
|
||
|
||
// get all his (unprocessed) neighbors and add them to the region
|
||
for (const Node& n2 : grid.neighbors(next)) {
|
||
if (visited.find(n2.getIdx()) == visited.end()) {
|
||
toVisit.insert(n2.getIdx());
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
}
|
||
|
||
void removeIsolatedNodes() {
|
||
|
||
//std::cout << "todo: remove" << std::endl;
|
||
//return;
|
||
|
||
// try to start at the first stair
|
||
for (Node& n : grid) {
|
||
if (n.getType() == GridNode::TYPE_STAIR) {removeIsolatedNodes(n); return;}
|
||
}
|
||
|
||
// no stair found? try to start at the first node
|
||
removeIsolatedNodes(grid[0]);
|
||
|
||
}
|
||
|
||
/** remove all nodes not connected to n1 */
|
||
void removeIsolatedNodes(Node& n1) {
|
||
|
||
// get the connected region around n1
|
||
//Log::add(name, "getting set of all nodes connected to " + (std::string) n1, false);
|
||
//Log::tick();
|
||
std::unordered_set<int> set;
|
||
getConnected(n1, set);
|
||
//Log::tock();
|
||
|
||
//const int numToRemove = grid.getNumNodes() - set.size();
|
||
//int numRemoved = 0;
|
||
|
||
// remove all other
|
||
//Log::add(name, "removing all nodes NOT connected to " + (std::string) n1, false);
|
||
//Log::tick();
|
||
for (Node& n2 : grid) {
|
||
if (set.find(n2.getIdx()) == set.end()) {
|
||
|
||
// sanity check
|
||
// wouldn't make sense that a stair-node is removed..
|
||
// maybe something went wrong elsewhere???
|
||
Assert::notEqual(n2.getType(), GridNode::TYPE_STAIR, "detected an isolated stair?!");
|
||
Assert::notEqual(n2.getType(), GridNode::TYPE_ELEVATOR, "detected an isolated elevator?!");
|
||
//Assert::notEqual(n2.getType(), GridNode::TYPE_DOOR, "detected an isolated door?!");
|
||
|
||
// proceed ;)
|
||
grid.remove(n2);
|
||
|
||
//++numRemoved;
|
||
//std::cout << numRemoved << ":" << numToRemove << std::endl;
|
||
|
||
}
|
||
}
|
||
//Log::tock();
|
||
|
||
// clean the grid (physically delete the removed nodes)
|
||
grid.cleanup();
|
||
|
||
}
|
||
|
||
|
||
// std::vector<NewNode> getPointsOn(const Floorplan::Floor* floor, const Floorplan::FloorOutlinePolygon& poly) {
|
||
|
||
// std::vector<NewNode> res;
|
||
|
||
// BBox2 bbox;
|
||
// for (Point2 pt : poly.poly.points) {bbox.add(pt);}
|
||
|
||
// int x1 = std::floor(bbox.getMin().x * 100 / gs_cm) * gs_cm;
|
||
// int x2 = std::ceil(bbox.getMax().x * 100 / gs_cm) * gs_cm;
|
||
|
||
// int y1 = std::floor(bbox.getMin().y * 100 / gs_cm) * gs_cm;
|
||
// int y2 = std::ceil(bbox.getMax().y * 100 / gs_cm) * gs_cm;
|
||
|
||
// int z = floor->atHeight * 100;
|
||
|
||
// for (int y = y1; y <= y2; y += gs_cm) {
|
||
// for (int x = x1; x <= x2; x += gs_cm) {
|
||
|
||
// GridPoint gp(x, y, z);
|
||
// int type = poly.outdoor ? GridNode::TYPE_OUTDOOR : GridNode::TYPE_FLOOR;
|
||
// res.push_back(NewNode(gp, type));
|
||
|
||
// }
|
||
// }
|
||
|
||
// return res;
|
||
|
||
// }
|
||
|
||
|
||
|
||
std::vector<NewNode> getPointsOn(const Floorplan::Floor* floor) {
|
||
|
||
std::vector<NewNode> res;
|
||
|
||
BBox2 bbox;
|
||
for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) {
|
||
for (Point2 pt : poly->poly.points) {bbox.add(pt);}
|
||
}
|
||
|
||
int x1 = std::floor(bbox.getMin().x * 100 / gs_cm) * gs_cm;
|
||
int x2 = std::ceil(bbox.getMax().x * 100 / gs_cm) * gs_cm;
|
||
|
||
int y1 = std::floor(bbox.getMin().y * 100 / gs_cm) * gs_cm;
|
||
int y2 = std::ceil(bbox.getMax().y * 100 / gs_cm) * gs_cm;
|
||
|
||
int z = floor->atHeight * 100;
|
||
|
||
struct Combo {
|
||
HelperPoly3 poly;
|
||
const Floorplan::FloorOutlinePolygon* orig;
|
||
Combo(HelperPoly3 poly, const Floorplan::FloorOutlinePolygon* orig) : poly(poly), orig(orig) {;}
|
||
};
|
||
|
||
std::vector<Combo> polygons;
|
||
for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) {
|
||
HelperPoly3 pol(*poly);
|
||
polygons.push_back(Combo(pol, poly));
|
||
}
|
||
|
||
GF3_ITER_XY
|
||
|
||
int type = GridNode::TYPE_FLOOR;
|
||
bool remove = false;
|
||
bool add = false;
|
||
|
||
for (const Combo& c : polygons) {
|
||
if (c.poly.contains(Point2(x,y))) {
|
||
if (c.orig->method == Floorplan::OutlineMethod::ADD) {add = true;}
|
||
if (c.orig->method == Floorplan::OutlineMethod::REMOVE) {remove = true; break;}
|
||
if (c.orig->outdoor) {type = GridNode::TYPE_OUTDOOR;}
|
||
}
|
||
}
|
||
|
||
if (add && !remove) {
|
||
GridPoint gp(x, y, z);
|
||
res.push_back(NewNode(gp, type));
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
return res;
|
||
|
||
}
|
||
|
||
|
||
|
||
//
|
||
|
||
// const std::vector<NewNode> pts = getPointsOn(floor, *poly);
|
||
// if (poly->method == Floorplan::OutlineMethod::ADD) {
|
||
// add.insert(add.end(), pts.begin(), pts.end());
|
||
// } else {
|
||
// rem.insert(rem.end(), pts.begin(), pts.end());
|
||
// }
|
||
|
||
// }
|
||
|
||
static bool bary(Point2 p, Point2 a, Point2 b, Point2 c, float &u, float &v, float &w) {
|
||
const Point2 v0 = b - a, v1 = c - a, v2 = p - a;
|
||
double d00 = dot(v0, v0);
|
||
double d01 = dot(v0, v1);
|
||
double d11 = dot(v1, v1);
|
||
double d20 = dot(v2, v0);
|
||
double d21 = dot(v2, v1);
|
||
double denom = d00 * d11 - d01 * d01;
|
||
v = (d11 * d20 - d01 * d21) / denom;
|
||
w = (d00 * d21 - d01 * d20) / denom;
|
||
u = 1.0f - v - w;
|
||
return (u <= 1 && v <= 1 && w <= 1) && (u >= 0 && v >= 0 && w >= 0);
|
||
}
|
||
|
||
// void isBlocked(const GridPoint& gp) {
|
||
// for (Floorplan::Floor* floor : map->floors) {
|
||
// for (Floorplan::FloorObstacle* obs : floor->obstacles) {
|
||
// Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
|
||
// if (line) {
|
||
// line->
|
||
// }
|
||
// }
|
||
// }
|
||
// }
|
||
|
||
std::vector<NewNode> getPointsOn(const Floorplan::Floor* floor, const std::vector<Floorplan::Quad3>& quads) {
|
||
|
||
std::vector<NewNode> res;
|
||
|
||
// whole stair
|
||
BBox3 bboxStair;
|
||
for (const Floorplan::Quad3& quad : quads) {
|
||
bboxStair.add(quad.p1);
|
||
bboxStair.add(quad.p2);
|
||
bboxStair.add(quad.p3);
|
||
bboxStair.add(quad.p4);
|
||
}
|
||
|
||
// stair's starting and ending z (must be connected to a floor)
|
||
//int z1 = grid.snapZ( (floor->atHeight) * 100 );
|
||
//
|
||
int z2 = grid.snapZ( (floor->atHeight + bboxStair.getMax().z) * 100 );
|
||
|
||
// one quad
|
||
for (const Floorplan::Quad3& quad : quads) {
|
||
|
||
BBox3 bbox;
|
||
bbox.add(quad.p1);
|
||
bbox.add(quad.p2);
|
||
bbox.add(quad.p3);
|
||
bbox.add(quad.p4);
|
||
|
||
|
||
int x1 = std::floor(bbox.getMin().x * 100 / gs_cm) * gs_cm;
|
||
int x2 = std::ceil(bbox.getMax().x * 100 / gs_cm) * gs_cm;
|
||
|
||
int y1 = std::floor(bbox.getMin().y * 100 / gs_cm) * gs_cm;
|
||
int y2 = std::ceil(bbox.getMax().y * 100 / gs_cm) * gs_cm;
|
||
|
||
//int zFloor = floor->atHeight * 100;
|
||
|
||
// for (int y = y1; y <= y2; y += gs_cm) {
|
||
// const int xx = (y / gs_cm % 2 == 0) ? (0) : (gs_cm/2);
|
||
// for (int x = x1-xx; x <= x2; x += gs_cm) {
|
||
|
||
GF3_ITER_XY
|
||
|
||
int z = 0;
|
||
Point2 p(x/100.0f, y/100.0f);
|
||
|
||
float u,v,w;
|
||
if (bary(p, quad.p1.xy(), quad.p2.xy(), quad.p3.xy(), u, v, w)) {
|
||
z = (quad.p1.z*u + quad.p2.z*v + quad.p3.z*w) * 100;
|
||
} else if (bary(p, quad.p1.xy(), quad.p3.xy(), quad.p4.xy(), u, v, w)) {
|
||
z = (quad.p1.z*u + quad.p3.z*v + quad.p4.z*w) * 100;
|
||
} else {
|
||
// outside of the quad -> skip
|
||
//z = (quad.p1.z*u + quad.p3.z*v + quad.p4.z*w) * 100;
|
||
continue;
|
||
|
||
//z = zFloor + (
|
||
// (quad.p1.z*u + quad.p2.z*v + quad.p3.z*w)
|
||
// ) * 100;
|
||
|
||
}
|
||
|
||
//z = grid.snapZ(z);
|
||
|
||
const GridPoint gp(x, y, z);
|
||
const int type = GridNode::TYPE_STAIR;
|
||
res.push_back(NewNode(gp, type));
|
||
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
// scale to ensure starting at floor, and ending at floor
|
||
|
||
return res;
|
||
|
||
}
|
||
|
||
};
|
||
|
||
#endif // GRIDFACTORY3_H
|