frank
ca6fed5371
worked on grid-generation added helper library for nav-meshes started working on nav meshes
493 lines
13 KiB
C++
493 lines
13 KiB
C++
#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
|