kazu
06e0e0a5aa
added corresponding test-cases switched to newer version of tinyxml due to some issues adjusted affected code-parts accordingly for better re-use, moved ceiling-calculation to a new class some minor fixes new helper methods worked on wifi-opt
136 lines
3.6 KiB
C++
136 lines
3.6 KiB
C++
#ifndef FLOORPLANHELPER2_H
|
|
#define FLOORPLANHELPER2_H
|
|
|
|
#include "../../geo/BBox3.h"
|
|
#include "../../geo/Line2.h"
|
|
#include "../../Assertions.h"
|
|
|
|
#include "Floorplan.h"
|
|
|
|
#include "../../sensors/MACAddress.h"
|
|
|
|
/**
|
|
* helper methods for the floorplan
|
|
*/
|
|
class FloorplanHelper {
|
|
|
|
public:
|
|
|
|
|
|
/** get the AP for the given MAC [if available] */
|
|
static std::pair<Floorplan::AccessPoint*, Floorplan::Floor*> getAP(const Floorplan::IndoorMap* map, const MACAddress& mac) {
|
|
for (Floorplan::Floor* f : map->floors) {
|
|
for (Floorplan::AccessPoint* ap : f->accesspoints) {
|
|
if (MACAddress(ap->mac) == mac) {
|
|
return std::make_pair(ap, f);
|
|
}
|
|
}
|
|
}
|
|
return std::make_pair(nullptr, nullptr);
|
|
}
|
|
|
|
public:
|
|
|
|
/** align all floorplan values to the given grid size. needed for the grid factory */
|
|
static void align(Floorplan::IndoorMap* map, const int gridSize_cm) {
|
|
for (Floorplan::Floor* floor : map->floors) {
|
|
floor->atHeight = align_m(floor->atHeight, gridSize_cm);
|
|
floor->height = align_m(floor->height, gridSize_cm);
|
|
for (Floorplan::Stair* stair : floor->stairs) {
|
|
for (Floorplan::StairPart& part : ((Floorplan::StairFreeform*)stair)->parts) {
|
|
part.start.z = align_m(part.start.z, gridSize_cm);
|
|
part.end.z = align_m(part.end.z, gridSize_cm);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
static inline float align_m(const float val_m, const int gridSize_cm) {
|
|
const float val_cm = val_m * 100;
|
|
const int snapped = std::round(val_cm / gridSize_cm);
|
|
const float res_cm = snapped * gridSize_cm;
|
|
return res_cm / 100.0f;
|
|
}
|
|
|
|
/** get a BBox for the whole map */
|
|
static BBox3 getBBox(const Floorplan::IndoorMap* map) {
|
|
|
|
BBox3 bbox;
|
|
|
|
for (const Floorplan::Floor* floor : map->floors) {
|
|
const BBox3 bbFloor = getBBox(floor);
|
|
bbox.add(bbFloor);
|
|
}
|
|
|
|
return bbox;
|
|
|
|
}
|
|
|
|
/** get a BBox for the whole floor */
|
|
static BBox3 getBBox(const Floorplan::Floor* floor) {
|
|
|
|
BBox3 bbox;
|
|
|
|
Assert::isNot0(floor->outline.size(), "floor " + floor->name + " has no outline!");
|
|
|
|
for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) {
|
|
for (const Point2 p2 : poly->poly.points) {
|
|
bbox.add(Point3(p2.x, p2.y, floor->atHeight));
|
|
bbox.add(Point3(p2.x, p2.y, floor->atHeight + floor->height));
|
|
}
|
|
}
|
|
|
|
return bbox;
|
|
|
|
}
|
|
|
|
/** does the map contain an obstacle that intersects the given line? (in meter!) */
|
|
static bool intersectsObstacle(const Floorplan::IndoorMap* map, const Point3& p1, const Point3& p2) {
|
|
|
|
// sanity check
|
|
if (std::abs(p2.z-p1.z) > 0.5) {
|
|
return false;}
|
|
|
|
for (const Floorplan::Floor* floor : map->floors) {
|
|
if (intersectsObstacle(floor, p1, p2)) {return true;}
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
/** does the floor contain an obstacle that intersects the given line? (in meter!) */
|
|
static bool intersectsObstacle(const Floorplan::Floor* floor, const Point3& p1, const Point3& p2) {
|
|
|
|
// sanity check
|
|
if (std::abs(p2.z-p1.z) > 0.5) {
|
|
return false;}
|
|
|
|
// only inspect elements that lie near the ground
|
|
if (p1.z < floor->getStartingZ() - 0.15) {return false;}
|
|
if (p1.z > floor->getStartingZ() + 0.15) {return false;}
|
|
|
|
const Line2 line1(p1.x,p1.y, p2.x,p2.y);
|
|
|
|
for (const Floorplan::FloorObstacle* obs : floor->obstacles) {
|
|
|
|
if (dynamic_cast<const Floorplan::FloorObstacleLine*>(obs)) {
|
|
const Floorplan::FloorObstacleLine* obsLine = dynamic_cast<const Floorplan::FloorObstacleLine*>(obs);
|
|
const Line2 line2(obsLine->from.x, obsLine->from.y, obsLine->to.x, obsLine->to.y);
|
|
if (line1.getSegmentIntersection(line2)) {return true;}
|
|
} else if (dynamic_cast<const Floorplan::FloorObstacleDoor*>(obs)) {
|
|
;
|
|
} else {
|
|
throw "not yet supported";
|
|
}
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // FLOORPLANHELPER2_H
|