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

worked on 3d models within map

Browse Source 
adjusted grid factory
adjusted nav mesh factory
minoor changes/fixes
new helper classes
refactoring
This commit is contained in:
FrankE
2018-04-03 14:55:59 +02:00
parent f3b6155157
commit 1c2081d406
25 changed files with 620 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
grid/factory/v2/GridFactory.h
View File

@@ -5,6 +5,8 @@
#include <unordered_set>
#include <set>
#include "../../../math/Math.h"
#include "../../../floorplan/v2/Floorplan.h"
#include "Helper.h"
@@ -19,6 +21,8 @@
#include "../../../misc/Debug.h"
#include <functional>
#include "../../../wifi/estimate/ray3/OBJPool.h"
#include "../../../geo/ConvexHull2.h"
#include "GridFactoryListener.h"
@@ -47,6 +51,7 @@ private:
bool _buildStairs = true;
bool _removeIsolated = true;
bool _addTightToObstacle = false;
public:
@@ -55,6 +60,7 @@ public:
}
void setAddTightToObstacle(const bool tight) {this->_addTightToObstacle = tight;}
/** whether or not to build stairs */
void setBuildStairs(const bool build) {this->_buildStairs = build;}
@@ -204,6 +210,41 @@ public:
int cur = 0;
int numNodes = 0;
// all 3D objects within the floor
std::vector<HelperPoly> objObstacles;
for (const Floorplan::FloorObstacle* fo : floor->obstacles) {
// process all object-obstalces
const Floorplan::FloorObstacleObject* foo = dynamic_cast<const Floorplan::FloorObstacleObject*>(fo);
if (foo) {
// get the obstacle
const Ray3D::Obstacle3D obs = Ray3D::OBJPool::get().getObject(foo->file).rotated_deg(foo->rot).translated(foo->pos);
// construct its 2D convex hull (in centimter)
HelperPoly poly;
for (const Point2 p : ConvexHull2::get(obs.getPoints2D())) {poly.add(p*100);}
objObstacles.push_back(poly);
}
}
// does any of the obj-obstalces contain the given point?
auto isPartOfObject = [&objObstacles, this] (const GridNodeBBox& bb) {
for (HelperPoly poly : objObstacles) {
//if (!_addTightToObstacle) {
if (poly.contains(bb.getCorner1())) {return true;}
if (poly.contains(bb.getCorner2())) {return true;}
if (poly.contains(bb.getCorner3())) {return true;}
if (poly.contains(bb.getCorner4())) {return true;}
//} else {
// //poly.shrink(1);
// if (poly.contains(bb.getCenter())) {return true;}
//}
}
return false;
};
// build grid-points for floor-outline
for(int x_cm = x1; x_cm < x2; x_cm += helper.gridSize()) {
for (int y_cm = y1; y_cm < y2; y_cm += helper.gridSize()) {
@@ -212,12 +253,17 @@ public:
const PartOfOutline part = isPartOfFloorOutline(x_cm, y_cm, floor->outline);
if (!part.contained) {continue;}
// check intersection with the floorplan
// bbox to check intersection with the floorplan
GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), helper.gridSize());
// slightly grow the bbox to ensure even obstacles that are directly aligned to the bbox are hit
bbox.grow(0.1337);
if (intersects(bbox, floor)) {continue;}
if (!_addTightToObstacle) {
if (intersects(bbox, floor)) {continue;}
}
// intersection with objects?
if (isPartOfObject(bbox)) {continue;}
// add to the grid [once]
T t(x_cm, y_cm, z_cm);
@@ -544,13 +590,21 @@ private:
} else if (dynamic_cast<Floorplan::FloorObstacleCircle*>(fo)) {
const Floorplan::FloorObstacleCircle* circle = (Floorplan::FloorObstacleCircle*) fo;
const Point2 center = bbox.getCenter();
const float dist = center.getDistance(circle->center*100);
if (dist < circle->radius*100) {return true;}
const float dist = std::min(
bbox.getCorner1().getDistance(circle->center*100),
bbox.getCorner2().getDistance(circle->center*100),
bbox.getCorner3().getDistance(circle->center*100),
bbox.getCorner4().getDistance(circle->center*100)
);
const float threshold = circle->radius * 100;
if (dist < threshold) {return true;}
} else if (dynamic_cast<Floorplan::FloorObstacleDoor*>(fo)) {
// DOORS ARE NOT AN OBSTACLE
} else if (dynamic_cast<Floorplan::FloorObstacleObject*>(fo)) {
// ADDED EARLIER
} else {
throw Exception("TODO: not yet implemented obstacle type");
@@ -592,6 +646,10 @@ private:
} else if (dynamic_cast<Floorplan::FloorObstacleDoor*>(fo)) {
// DOORS ARE NOT AN OBSTACLE
} else if (dynamic_cast<Floorplan::FloorObstacleObject*>(fo)) {
// removed earlier
//std::cout << "GridFactory: TODO: Floorplan::FloorObstacleObject" << std::endl;
} else {
throw Exception("TODO: not yet implemented obstacle type");