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

fixed some grid-factory issues

Browse Source 
added some new attributes
minor changes
This commit is contained in:
FrankE
2017-06-01 15:58:58 +02:00
parent 34271b5cb7
commit 9ea7da557b
6 changed files with 47 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -487,7 +487,23 @@ private:
private:
/** 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),
};
}
/** does the bbox intersect with any of the floor's walls? */
static inline bool intersects(const GridNodeBBox& bbox, const Floorplan::Floor* floor) {
@@ -499,8 +515,15 @@ private:
// depends on the type of obstacle
if (dynamic_cast<Floorplan::FloorObstacleLine*>(fo)) {
const Floorplan::FloorObstacleLine* line = (Floorplan::FloorObstacleLine*) fo;
const Line2 l2(line->from*100, line->to*100);
if (bbox.intersects(l2)) {return true;}
// old method (does not support thickness)
//const Line2 l2(line->from*100, line->to*100);
//if (bbox.intersects(l2)) {return true;}
const std::vector<Line2> lines = getThickLines(line);
for (const Line2& l : lines) {
if (bbox.intersects(l*100)) {return true;}
}
} else if (dynamic_cast<Floorplan::FloorObstacleCircle*>(fo)) {
const Floorplan::FloorObstacleCircle* circle = (Floorplan::FloorObstacleCircle*) fo;