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

worked on floorplan (v2)

Browse Source 
worked on grid-generation (v2)
new helper methods for geometry
new test cases
This commit is contained in:
FrankE
2016-07-13 19:11:18 +02:00
parent cc21cbb0ea
commit 34e52cd7f0
26 changed files with 2083 additions and 272 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
Assertions.h
View File

@@ -29,6 +29,10 @@ namespace Assert {
if (v1 != v2) {doThrow(err);}
}
template <typename T, typename STR> static inline void notEqual(const T v1, const T v2, const STR err) {
if (v1 == v2) {doThrow(err);}
}
template <typename T, typename STR> static inline void isTrue(const T v, const STR err) {
if (!v) {doThrow(err);}
}
@@ -53,6 +57,10 @@ namespace Assert {
if (v == 0) {doThrow(err);}
}
template <typename T, typename STR> static inline void isNear(const T v1, const T v2, const T delta, const STR err) {
if (std::abs(v1-v2) > delta) {doThrow(err);}
}
template <typename T, typename STR> static inline void isBetween(const T v, const T min, const T max, const STR err) {
if (v < min || v > max) {doThrow(err);}
}

6
CMakeLists.txt
View File

@@ -37,7 +37,6 @@ FILE(GLOB SOURCES
./*/*.cpp
./*/*/*.cpp
./*/*/*/*.cpp
../KLib/inc/tinyxml/*.cpp
)
@@ -65,12 +64,13 @@ ADD_DEFINITIONS(
-fstack-protector-all
-g3
-O2
-O0
-march=native
-DWITH_TESTS
-DWITH_ASSERTIONS
-DWITH_DEBUG_LOG
-march=native
)

4
api/DummyAPI.h Normal file
View File

@@ -0,0 +1,4 @@
#ifndef DUMMYAPI_H
#define DUMMYAPI_H
#endif // DUMMYAPI_H

4
api/api.h Normal file
View File

@@ -0,0 +1,4 @@
#ifndef API_H
#define API_H
#endif // API_H

341
floorplan/v2/Floorplan.h
View File

@@ -11,7 +11,28 @@
namespace Floorplan {
/** 3D polygon */
/** a free key-value meta element */
struct Meta {
const std::string EMPTY = "";
std::unordered_map<std::string, std::string> params;
const std::string& getVal(const std::string& key) const {
const auto it = params.find(key);
return (it == params.end()) ? (EMPTY) : (it->second);
}
void setVal(const std::string& key, const std::string& val) {params[key] = val;}
float getFloat(const std::string& key) const { return std::stof(getVal(key)); }
void setFloat(const std::string& key, const float val) { params[key] = std::to_string(val); }
int getInt(const std::string& key) const { return std::stoi(getVal(key)); }
void setInt(const std::string& key, const int val) { params[key] = std::to_string(val); }
};
/** 2D polygon */
struct Polygon2 {
std::vector<Point2> points;
Polygon2() : points() {;}
@@ -19,17 +40,37 @@ namespace Floorplan {
bool operator == (const Polygon2& o) const {return std::equal(o.points.begin(), o.points.end(), this->points.begin());}
};
/** 3D quad */
struct Quad3 {
Point3 p1;
Point3 p2;
Point3 p3;
Point3 p4;
Quad3(const Point3 p1, const Point3 p2, const Point3 p3, const Point3 p4) : p1(p1), p2(p2), p3(p3), p4(p4) {;}
Quad3 operator * (const float v) const {return Quad3(p1*v, p2*v, p3*v, p4*v);}
};
/** additional type-info for obstacles */
enum class ObstacleType {
UNKNOWN,
WALL,
DOOR,
WINDOW,
HANDRAIL,
PILLAR,
_END,
};
/** available door types */
enum class DoorType {
UNKNOWN,
SWING, // normal
DOUBLE_SWING,
SLIDE, // schiebetuer
DOUBLE_SLIDE,
REVOLVING, // drehtuer
_END,
};
/** all supported material types */
enum class Material {
UNKNOWN,
@@ -40,6 +81,10 @@ namespace Floorplan {
_END,
};
enum class POIType {
ROOM,
};
/** types of outlines. either add or remove the selected region */
enum class OutlineMethod {
ADD,
@@ -47,23 +92,78 @@ namespace Floorplan {
_END,
};
struct Floor;
struct FloorOutlinePolygon;
struct FloorObstacle;
struct AccessPoint;
struct Beacon;
struct FloorRegion;
struct UnderlayImage;
struct POI;
struct Stair;
using FloorOutline = std::vector<FloorOutlinePolygon*>;
using FloorObstacles = std::vector<FloorObstacle*>;
using FloorAccessPoints = std::vector<AccessPoint*>;
using FloorBeacons = std::vector<Beacon*>;
using FloorRegions = std::vector<FloorRegion*>;
using FloorUnderlays = std::vector<UnderlayImage*>;
using FloorPOIs = std::vector<POI*>;
using FloorStairs = std::vector<Stair*>;
/** describes one floor within the map, starting at a given height */
struct Floor {
float atHeight; // the floor's starting height
float height; // the floor's total height (from start)
std::string name; // the floor's name
FloorOutline outline; // the floor's outline (ground)
FloorObstacles obstacles; // all obstacles (wall, door, window, ..) within the floor
FloorRegions regions; // all regions within the floor (rooms, ...)
FloorAccessPoints accesspoints;
FloorBeacons beacons;
FloorUnderlays underlays; // underlay images (used for map-building)
FloorPOIs pois; // POIs within the floor
FloorStairs stairs; // all stairs within one floor
//FloorKeyValue other; // other, free elements
Floor() {;}
Floor(const Floor& o) = delete;
void operator = (const Floor& o) = delete;
float getStartingZ() const {return atHeight;}
float getEndingZ() const {return atHeight + height;}
};
/** an AccessPoint located somewhere within the map */
/** a POI located somewhere on a floor */
struct POI {
POIType type;
std::string name;
Point2 pos;
POI() : type(), name(), pos() {;}
POI(const POIType type, const std::string& name, const Point2& pos) : type(type), name(name), pos(pos) {;}
bool operator == (const POI& o) const {return (o.type == type) && (o.name == name) && (o.pos == pos);}
};
/** an AccessPoint located somewhere on a floor */
struct AccessPoint {
std::string name;
std::string mac;
Point3 pos;
Point3 pos; // z is relative to the floor's height
AccessPoint() : name(), mac(), pos() {;}
AccessPoint(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(mac), pos(pos) {;}
bool operator == (const AccessPoint& o) const {return (o.name == name) && (o.mac == mac) && (o.pos == pos);}
Point3 getPos(const Floor* f) const {return pos + Point3(0,0,f->atHeight);} // relative to the floor's ground
};
/** a beacon located somewhere within the map */
/** a beacon located somewhere on a floor */
struct Beacon {
std::string name;
std::string mac;
Point3 pos;
Point3 pos; // z is relative to the floor's height
Beacon() : name(), mac(), pos() {;}
Beacon(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(mac), pos(pos) {;}
bool operator == (const Beacon& o) const {return (o.name == name) && (o.mac == mac) && (o.pos == pos);}
@@ -80,31 +180,51 @@ namespace Floorplan {
bool operator == (const FloorOutlinePolygon& o) const {return (o.method == method) && (o.name == name) && (o.poly == poly);}
};
/** base-class for one obstacle (wall, door, window, pillar, ..) within a floor */
struct FloorObstacle {
ObstacleType type;
Material material;
FloorObstacle() : type(), material() {;}
FloorObstacle(const ObstacleType type, const Material material) : type(type), material(material) {;}
FloorObstacle() : material() {;}
FloorObstacle(const Material material) : material(material) {;}
virtual ~FloorObstacle() {;}
};
/** line obstacle */
struct FloorObstacleLine : public FloorObstacle {
ObstacleType type;
Point2 from;
Point2 to;
FloorObstacleLine(const ObstacleType type, const Material material, const Point2 from, const Point2 to) : FloorObstacle(type, material), from(from), to(to) {;}
FloorObstacleLine(const ObstacleType type, const Material material, const float x1, const float y1, const float x2, const float y2) : FloorObstacle(type, material), from(x1,y1), to(x2,y2) {;}
FloorObstacleLine(const ObstacleType type, const Material material, const Point2 from, const Point2 to) : FloorObstacle(material), type(type), from(from), to(to) {;}
FloorObstacleLine(const ObstacleType type, const Material material, const float x1, const float y1, const float x2, const float y2) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2) {;}
};
/** circle obstacle */
struct FloorObstacleCircle : public FloorObstacle {
Point2 center;
float radius;
FloorObstacleCircle(const ObstacleType type, const Material material, const Point2 center, const float radius) : FloorObstacle(type, material), center(center), radius(radius) {;}
FloorObstacleCircle(const ObstacleType type, const Material material, const float cx, const float cy, const float radius) : FloorObstacle(type, material), center(cx,cy), radius(radius) {;}
FloorObstacleCircle(const Material material, const Point2 center, const float radius) : FloorObstacle(material), center(center), radius(radius) {;}
FloorObstacleCircle(const Material material, const float cx, const float cy, const float radius) : FloorObstacle(material), center(cx,cy), radius(radius) {;}
};
/** door obstacle */
struct FloorObstacleDoor : public FloorObstacle {
DoorType type;
Point2 from;
Point2 to;
float height;
bool swap;
FloorObstacleDoor(const DoorType type, const Material material, const Point2 from, const Point2 to) : FloorObstacle(material), type(type), from(from), to(to), height(2.1), swap(false) {;}
FloorObstacleDoor(const DoorType type, const Material material, const float x1, const float y1, const float x2, const float y2, const float height, const bool swap) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2), height(height), swap(swap) {;}
float getSize() const {return (to-from).length();}
};
/** one region (e.g. a room) within the floor, described using a polygon */
struct FloorRegion {
std::string name;
@@ -115,6 +235,158 @@ namespace Floorplan {
static Point3 xy0(const Point2 p) {
return Point3(p.x, p.y, 0);
}
/** describes a plane as starting point, ending point and fixed width */
struct StairPart {
/** z is relative to the floor's height */
Point3 start;
/** z is relative to the floor's height */
Point3 end;
/** the width for this element */
float width;
/** whether to connect this element with the previous one */
bool connectWithPrev;
StairPart() {;}
StairPart(const Point3 start, const Point3 end, const float width) : start(start), end(end), width(width), connectWithPrev(false) {;}
StairPart(Point3 center, float deg, float length, float width, float height) {
this->start = Point3(-length/2, 0, -height/2);
this->end = Point3(+length/2, 0, +height/2);
this->width = width;
rotate(deg/180.0*M_PI);
move(center);
}
/** convenience method for array access [0:1] to start and end */
Point3& operator [] (const int idx) {
switch (idx) {
case 0: return start;
case 1: return end;
default: throw "error";
}
}
void rotate(const float rad) {
const float ca = std::cos(rad);
const float sa = std::sin(rad);
start = Point3(ca*start.x - sa*start.y, sa*start.x + ca * start.y, start.z);
end = Point3(ca*end.x - sa*end.y, sa*end.x + ca * end.y, end.z);;
}
void move(const Point3 p) {
start += p;
end += p;
}
};
static std::vector<Quad3> getQuads(const std::vector<StairPart>& parts, const Floor* floor) {
std::vector<Quad3> vec;
for (const StairPart& part : parts) {
const float width = part.width;
const Point3 start = part.start;
const Point3 end = part.end;
const Point3 dir = end - start; // direction vector
const Point2 dir2(dir.x, dir.y); // direction without height (just 2D)
const Point2 perp = dir2.perpendicular(); // perendicular vector
const Point2 perpN = perp / perp.length(); // normalized perpendicular vector
const Point3 p1 = start + xy0(perpN * width / 2) + Point3(0,0,floor->atHeight);
const Point3 p2 = start - xy0(perpN * width / 2) + Point3(0,0,floor->atHeight);
const Point3 p3 = end - xy0(perpN * width / 2) + Point3(0,0,floor->atHeight);
const Point3 p4 = end + xy0(perpN * width / 2) + Point3(0,0,floor->atHeight);
const Quad3 q(p1,p2,p3,p4);
vec.push_back(q);
// connect
if (part.connectWithPrev) {
if (vec.size() >= 2) {
Quad3& a = vec[vec.size()-2];
Quad3& b = vec[vec.size()-1];
Point3 pa = (a.p3 + b.p2)/2; a.p3 = pa; b.p2 = pa;
Point3 pb = (a.p4 + b.p1)/2; a.p4 = pb; b.p1 = pb;
}
}
}
return vec;
}
// /**
// * get the ABSOLUTE quad for this stair-part
// * (relative-height + floor-height = absolute-height
// */
// Quad3 getQuad(const Floor* floor) const {
// }
/** base-class for stairs */
struct Stair {
Meta* meta = nullptr;
virtual std::vector<StairPart> getParts() const = 0;
};
// /** just a normal, straigt stair */
// struct StairNormal : public Stair {
// Point2 center;
// float angleDeg;
// float atHeight;
// float height;
// float width;
// float length;
// StairNormal(const Point2 center, const float atHeight, const float angleDeg, const float height, const float width, const float length) :
// center(center), angleDeg(angleDeg), atHeight(atHeight), height(height), width(width), length(length) {
// }
// std::vector<StairPart> getParts() const {
// std::vector<StairPart> parts;
// Point3 cen(center.x, center.y, atHeight);
// parts.push_back(StairPart(cen, angleDeg, length, width, height));
// return parts;
// }
// };
// /** OLD 3-part stair, up[left]->platform->up[right] */
// struct StairFreeformOLD : public Stair {
// float width;
// std::vector<Point3> nodes;
// StairFreeformOLD() {;}
// std::vector<StairPart> getParts() const {
// std::vector<StairPart> parts;
// for (int i = 1; i < (int)nodes.size(); ++i) {
// const Point3 p1 = nodes[i-1];
// const Point3 p2 = nodes[i ];
// parts.push_back(StairPart(p1, p2, width));
// }
// return parts;
// }
// };
/** 3-part stair, up[left]->platform->up[right] */
struct StairFreeform : public Stair {
std::vector<StairPart> parts;
StairFreeform() {;}
std::vector<StairPart> getParts() const {return parts;}
};
/** an image file that can be added to the map */
struct UnderlayImage {
std::string name;
@@ -124,50 +396,7 @@ namespace Floorplan {
float scaleY;
};
/** a free key-value element */
struct KeyValueElement {
std::string empty;
std::unordered_map<std::string, std::string> params;
const std::string& getVal(const std::string& key) const {
auto it = params.find(key);
return (it == params.end()) ? (empty) : (it->second);
}
float getFloat(const std::string& key) const { return std::stof(getVal(key)); }
void setVal(const std::string& key, const std::string& val) {
params[key] = val;
}
void setFloat(const std::string& key, const float val) {
params[key] = std::to_string(val);
}
};
using FloorOutline = std::vector<FloorOutlinePolygon*>;
using FloorObstacles = std::vector<FloorObstacle*>;
using FloorAccessPoints = std::vector<AccessPoint*>;
using FloorBeacons = std::vector<Beacon*>;
using FloorRegions = std::vector<FloorRegion*>;
using FloorUnderlays = std::vector<UnderlayImage*>;
/** describes one floor within the map, starting at a given height */
struct Floor {
float atHeight; // the floor's starting height
float height; // the floor's total height (from start)
std::string name; // the floor's name
FloorOutline outline; // the floor's outline (ground)
FloorObstacles obstacles; // all obstacles (wall, door, window, ..) within the floor
FloorRegions regions; // all regions within the floor (rooms, ...)
FloorAccessPoints accesspoints;
FloorBeacons beacons;
FloorUnderlays underlays; // underlay images (used for map-building)
//FloorKeyValue other; // other, free elements
Floor() {;}
Floor(const Floor& o) = delete;
void operator = (const Floor& o) = delete;
};
/** describes the whole indoor map */
struct IndoorMap {

225
floorplan/v2/FloorplanReader.h
View File

@@ -10,7 +10,8 @@
namespace Floorplan {
using namespace tinyxml2;
using XMLAttr = tinyxml2::XMLAttribute;
using XMLElem = tinyxml2::XMLElement;
/**
* read an IndoorMaps from XML-data
@@ -22,35 +23,37 @@ namespace Floorplan {
/** read an IndoorMap from the given XML-file */
static IndoorMap* readFromFile(const std::string& file) {
setlocale(LC_NUMERIC, "C");
XMLDocument doc;
doc.LoadFile(file.c_str());
tinyxml2::XMLDocument doc;
const tinyxml2::XMLError res = doc.LoadFile(file.c_str());
if (res != tinyxml2::XMLError::XML_SUCCESS) {throw Exception("error while loading XML " + file);}
return parse(doc);
}
/** read an IndoorMap from the given XMl-string */
static IndoorMap* readFromString(const std::string& str) {
setlocale(LC_NUMERIC, "C");
XMLDocument doc;
doc.Parse(str.c_str(), str.length());
tinyxml2::XMLDocument doc;
const tinyxml2::XMLError res = doc.Parse(str.c_str(), str.length());
if (res != tinyxml2::XMLError::XML_SUCCESS) {throw Exception("error while parsing XML");}
return parse(doc);
}
private:
#define FOREACH_NODE(out, in) for( const XMLElement* out = (XMLElement*) in->FirstChild(); out; out = (XMLElement*) out->NextSibling() )
#define FOREACH_NODE(out, in) for( const XMLElem* out = (XMLElem*) in->FirstChild(); out; out = (XMLElem*) out->NextSibling() )
static void assertNode(const std::string& node, const XMLElement* el) {
static void assertNode(const std::string& node, const XMLElem* el) {
std::string err = std::string("unexpected node '") + el->Name() + "' expected '" + node + "'";
Assert::equal(node, std::string(el->Name()), err);
}
/** parse the complete document */
static IndoorMap* parse(XMLDocument& doc) {
return parseMap((XMLElement*)doc.FirstChild());
static IndoorMap* parse(tinyxml2::XMLDocument& doc) {
return parseMap((XMLElem*)doc.FirstChild());
}
/** parse the <map> node */
static IndoorMap* parseMap(const XMLElement* el) {
static IndoorMap* parseMap(const XMLElem* el) {
std::cout << el->Name() << std::endl;
IndoorMap* map = new IndoorMap();
map->width = el->FloatAttribute("width");
@@ -62,7 +65,7 @@ namespace Floorplan {
}
/** parse the <floors> node */
static std::vector<Floor*> parseFloors(const XMLElement* el) {
static std::vector<Floor*> parseFloors(const XMLElem* el) {
std::vector<Floor*> floors;
FOREACH_NODE(n, el) {
if (std::string("floor") == n->Name()) {floors.push_back(parseFloor(n));}
@@ -71,25 +74,91 @@ namespace Floorplan {
}
/** parse one <floor> node */
static Floor* parseFloor(const XMLElement* el) {
static Floor* parseFloor(const XMLElem* el) {
Floor* floor = new Floor();
floor->atHeight = el->FloatAttribute("atHeight");
floor->height = el->FloatAttribute("height");
floor->name = el->Attribute("name");
FOREACH_NODE(n, el) {
if (std::string("outline") == n->Name()) {floor->outline = parseFloorOutline(n);}
if (std::string("obstacles") == n->Name()) {floor->obstacles = parseFloorObstacles(n);}
if (std::string("outline") == n->Name()) {floor->outline = parseFloorOutline(n);}
if (std::string("obstacles") == n->Name()) {floor->obstacles = parseFloorObstacles(n);}
if (std::string("accesspoints") == n->Name()) {floor->accesspoints = parseFloorAccessPoints(n);}
if (std::string("beacons") == n->Name()) {floor->beacons = parseFloorBeacons(n);}
if (std::string("regions") == n->Name()) {floor->regions = parseFloorRegions(n);}
if (std::string("underlays") == n->Name()) {floor->underlays = parseFloorUnderlays(n);}
if (std::string("beacons") == n->Name()) {floor->beacons = parseFloorBeacons(n);}
if (std::string("regions") == n->Name()) {floor->regions = parseFloorRegions(n);}
if (std::string("underlays") == n->Name()) {floor->underlays = parseFloorUnderlays(n);}
if (std::string("pois") == n->Name()) {floor->pois = parseFloorPOIs(n);}
if (std::string("stairs") == n->Name()) {floor->stairs = parseFloorStairs(n);}
}
return floor;
}
/** parse the <stairs> tag */
static std::vector<Stair*> parseFloorStairs(const XMLElem* el) {
std::vector<Stair*> vec;
FOREACH_NODE(n, el) {
if (std::string("stair") == n->Name()) { vec.push_back(parseFloorStair(n)); }
}
return vec;
}
/** parse a <stair> tag */
static Stair* parseFloorStair(const XMLElem* el) {
Stair* stair = nullptr;
if (el->IntAttribute("type") == 0) {
stair = new StairFreeform();
FOREACH_NODE(n, el) {
if (std::string("part") == n->Name()) {
StairPart part;
part.connectWithPrev = n->BoolAttribute("connect");
part.start.x = n->FloatAttribute("x1");
part.start.y = n->FloatAttribute("y1");
part.start.z = n->FloatAttribute("z1");
part.end.x = n->FloatAttribute("x2");
part.end.y = n->FloatAttribute("y2");
part.end.z = n->FloatAttribute("z2");
part.width = n->FloatAttribute("w");
((StairFreeform*)stair)->parts.push_back(part);
} else {
throw "not yet implemented";
}
}
} else {
throw "not yet implemented";
}
// stair meta information?
const XMLElem* meta = el->FirstChildElement("meta");
if (meta) {
stair->meta = parseMetaElement(meta);
}
return stair;
}
/** parse the <pois> tag */
static std::vector<POI*> parseFloorPOIs(const XMLElem* el) {
std::vector<POI*> vec;
FOREACH_NODE(n, el) {
if (std::string("poi") == n->Name()) { vec.push_back(parseFloorPOI(n)); }
}
return vec;
}
/** parse a <poi> tag */
static POI* parseFloorPOI(const XMLElem* el) {
POI* poi = new POI();
poi->name = el->Attribute("name");
poi->type = (POIType) el->IntAttribute("type");
poi->pos = parsePoint2(el);
return poi;
}
/** parse the <accesspoints> tag */
static std::vector<AccessPoint*> parseFloorAccessPoints(const XMLElement* el) {
assertNode("accesspoints", el);
static std::vector<AccessPoint*> parseFloorAccessPoints(const XMLElem* el) {
std::vector<AccessPoint*> vec;
FOREACH_NODE(n, el) {
if (std::string("accesspoint") == n->Name()) { vec.push_back(parseAccessPoint(n)); }
@@ -97,29 +166,29 @@ namespace Floorplan {
return vec;
}
/** parse the <underlays> tag */
static FloorUnderlays parseFloorUnderlays(const XMLElement* el) {
FloorUnderlays res;
FOREACH_NODE(n, el) {
if (std::string("underlay") == n->Name()) { res.push_back(parseFloorUnderlay(n)); }
}
return res;
}
/** parse the <underlays> tag */
static FloorUnderlays parseFloorUnderlays(const XMLElem* el) {
FloorUnderlays res;
FOREACH_NODE(n, el) {
if (std::string("underlay") == n->Name()) { res.push_back(parseFloorUnderlay(n)); }
}
return res;
}
/** parse an underlay image */
static UnderlayImage* parseFloorUnderlay(const XMLElement* el) {
UnderlayImage* img = new UnderlayImage();
img->anchor.x = el->FloatAttribute("x");
img->anchor.y = el->FloatAttribute("y");
img->scaleX = el->FloatAttribute("sx");
img->scaleY = el->FloatAttribute("sy");
img->name = el->Attribute("name");
img->filename = el->Attribute("file");
return img;
}
/** parse an underlay image */
static UnderlayImage* parseFloorUnderlay(const XMLElem* el) {
UnderlayImage* img = new UnderlayImage();
img->anchor.x = el->FloatAttribute("x");
img->anchor.y = el->FloatAttribute("y");
img->scaleX = el->FloatAttribute("sx");
img->scaleY = el->FloatAttribute("sy");
img->name = el->Attribute("name");
img->filename = el->Attribute("file");
return img;
}
// /** parse the <other> tag */
// static FloorKeyValue parseFloorKeyValue(const XMLElement* el) {
// static FloorKeyValue parseFloorKeyValue(const XMLElem* el) {
// FloorKeyValue res;
// FOREACH_NODE(n, el) {
// if (std::string("keyval") == n->Name()) { res.elements.push_back(parseKeyValueElement(n)); }
@@ -127,10 +196,10 @@ namespace Floorplan {
// return res;
// }
/** parse one <keyval> element */
static KeyValueElement* parseKeyValueElement(const XMLElement* n) {
KeyValueElement* elem = new KeyValueElement();
const XMLAttribute* attr = n->FirstAttribute();
/** parse one <meta> element */
static Meta* parseMetaElement(const XMLElem* n) {
Meta* elem = new Meta();
const XMLAttr* attr = n->FirstAttribute();
while (attr) {
elem->params[attr->Name()] = attr->Value();
attr = attr->Next();
@@ -138,7 +207,7 @@ namespace Floorplan {
return elem;
}
static AccessPoint* parseAccessPoint(const XMLElement* n) {
static AccessPoint* parseAccessPoint(const XMLElem* n) {
assertNode("accesspoint", n);
AccessPoint* ap = new AccessPoint();
ap->mac = n->Attribute("mac");
@@ -149,7 +218,7 @@ namespace Floorplan {
/** parse the <beacons> tag */
static std::vector<Beacon*> parseFloorBeacons(const XMLElement* el) {
static std::vector<Beacon*> parseFloorBeacons(const XMLElem* el) {
std::vector<Beacon*> vec;
FOREACH_NODE(n, el) {
if (std::string("beacon") == n->Name()) { vec.push_back(parseBeacon(n)); }
@@ -157,7 +226,7 @@ namespace Floorplan {
return vec;
}
static Beacon* parseBeacon(const XMLElement* n) {
static Beacon* parseBeacon(const XMLElem* n) {
assertNode("beacon", n);
Beacon* b = new Beacon();
b->mac = n->Attribute("mac");
@@ -167,7 +236,7 @@ namespace Floorplan {
}
static std::vector<FloorRegion*> parseFloorRegions(const XMLElement* el) {
static std::vector<FloorRegion*> parseFloorRegions(const XMLElem* el) {
std::vector<FloorRegion*> vec;
FOREACH_NODE(n, el) {
if (std::string("region") == n->Name()) { vec.push_back(parseFloorRegion(n)); }
@@ -175,7 +244,7 @@ namespace Floorplan {
return vec;
}
static FloorRegion* parseFloorRegion(const XMLElement* n) {
static FloorRegion* parseFloorRegion(const XMLElem* n) {
assertNode("region", n);
FloorRegion* reg = new FloorRegion();
reg->name = n->Attribute("name");
@@ -184,7 +253,7 @@ namespace Floorplan {
}
/** parse the <obstacles> tag */
static std::vector<FloorObstacle*> parseFloorObstacles(const XMLElement* el) {
static std::vector<FloorObstacle*> parseFloorObstacles(const XMLElem* el) {
assertNode("obstacles", el);
std::vector<FloorObstacle*> obstacles;
FOREACH_NODE(n, el) {
@@ -192,38 +261,46 @@ namespace Floorplan {
// if (std::string("door") == n->Name()) {obstacles.push_back(parseFloorObstacleDoor(n));}
// if (std::string("window") == n->Name()) {obstacles.push_back(parseFloorObstacleWindow(n));}
// if (std::string("pillar") == n->Name()) {obstacles.push_back(parseFloorObstaclePillar(n));}
if (std::string("obstacle") == n->Name()) {obstacles.push_back(parseFloorObstacleLine(n));} // OLD
if (std::string("line") == n->Name()) {obstacles.push_back(parseFloorObstacleLine(n));}
if (std::string("circle") == n->Name()) {obstacles.push_back(parseFloorObstacleCircle(n));}
//if (std::string("obstacle") == n->Name()) {obstacles.push_back(parseFloorObstacleLine(n));} // OLD
if (std::string("line") == n->Name()) {obstacles.push_back(parseFloorObstacleLine(n));}
if (std::string("circle") == n->Name()) {obstacles.push_back(parseFloorObstacleCircle(n));}
if (std::string("door") == n->Name()) {obstacles.push_back(parseFloorObstacleDoor(n));}
}
return obstacles;
}
/** parse one line */
static FloorObstacleLine* parseFloorObstacleLine(const XMLElement* el) {
static FloorObstacleLine* parseFloorObstacleLine(const XMLElem* el) {
return new FloorObstacleLine(
parseObstacleType(el->Attribute("type")),
parseMaterial(el->Attribute("material")),
el->FloatAttribute("x1"), el->FloatAttribute("y1"),
el->FloatAttribute("x2"), el->FloatAttribute("y2")
// parsePoint2((XMLElement*) el->FirstChildElement("from")),
// parsePoint2((XMLElement*) el->FirstChildElement("to"))
);
parseObstacleType(el->Attribute("type")),
parseMaterial(el->Attribute("material")),
el->FloatAttribute("x1"), el->FloatAttribute("y1"),
el->FloatAttribute("x2"), el->FloatAttribute("y2")
);
}
/** parse one cirlce */
static FloorObstacleCircle* parseFloorObstacleCircle(const XMLElement* el) {
static FloorObstacleCircle* parseFloorObstacleCircle(const XMLElem* el) {
return new FloorObstacleCircle(
parseObstacleType(el->Attribute("type")),
parseMaterial(el->Attribute("material")),
el->FloatAttribute("cx"), el->FloatAttribute("cy"),
//parsePoint2((XMLElement*) el->FirstChildElement("from")),
el->FloatAttribute("radius")
);
parseMaterial(el->Attribute("material")),
el->FloatAttribute("cx"), el->FloatAttribute("cy"),
el->FloatAttribute("radius")
);
}
/** parse one door */
static FloorObstacleDoor* parseFloorObstacleDoor(const XMLElem* el) {
return new FloorObstacleDoor(
parseDoorType(el->Attribute("type")),
parseMaterial(el->Attribute("material")),
el->FloatAttribute("x1"), el->FloatAttribute("y1"),
el->FloatAttribute("x2"), el->FloatAttribute("y2"),
el->FloatAttribute("height"), el->BoolAttribute("swap")
);
}
/** parse a floor's <outline> tag */
static FloorOutline parseFloorOutline(const XMLElement* el) {
static FloorOutline parseFloorOutline(const XMLElem* el) {
FloorOutline outline;
FOREACH_NODE(n, el) {
if (std::string("polygon") == n->Name()) {
@@ -234,7 +311,7 @@ namespace Floorplan {
}
/** parse one polygon */
static FloorOutlinePolygon* parseFloorPolygon(const XMLElement* el) {
static FloorOutlinePolygon* parseFloorPolygon(const XMLElem* el) {
FloorOutlinePolygon* poly = new FloorOutlinePolygon();
poly->name = el->Attribute("name");
poly->method = parseOutlineMethod(el->Attribute("method"));
@@ -243,7 +320,7 @@ namespace Floorplan {
}
/** parse a 2d-polygon denoted by several points */
static Polygon2 parsePoly2(const XMLElement* el) {
static Polygon2 parsePoly2(const XMLElem* el) {
Polygon2 poly;
FOREACH_NODE(n, el) {
if (std::string("point") == n->Name()) {
@@ -255,7 +332,7 @@ namespace Floorplan {
}
/** parse a 2D point (x,y) using the given tag's attributes. missing attributes are set to 0 */
static Point2 parsePoint2(const XMLElement* el) {
static Point2 parsePoint2(const XMLElem* el) {
Point2 point;
point.x = el->FloatAttribute("x");
point.y = el->FloatAttribute("y");
@@ -263,7 +340,7 @@ namespace Floorplan {
}
/** parse a 3D point (x,y,z) using the given tag's attributes. missing attributes are set to 0 */
static Point3 parsePoint3(const XMLElement* el) {
static Point3 parsePoint3(const XMLElem* el) {
Point3 point;
point.x = el->FloatAttribute("x");
point.y = el->FloatAttribute("y");
@@ -279,6 +356,10 @@ namespace Floorplan {
}
}
static DoorType parseDoorType(const std::string type) {
try { return (DoorType) std::stoi(type); } catch (...) { return DoorType::UNKNOWN; }
}
static Material parseMaterial(const std::string material) {
try {
return (Material) std::stoi(material);

212
floorplan/v2/FloorplanWriter.h
View File

@@ -10,7 +10,9 @@
namespace Floorplan {
using namespace tinyxml2;
using XMLAttr = tinyxml2::XMLAttribute;
using XMLElem = tinyxml2::XMLElement;
using XMLDoc = tinyxml2::XMLDocument;
/**
* write floorplans as XML
@@ -20,36 +22,29 @@ namespace Floorplan {
public:
static void writeToFile(const IndoorMap* map, const std::string& file) {
XMLDocument doc;
XMLDoc doc;
construct(doc, map);
doc.SaveFile(file.c_str(), false);
}
static std::string writeToString(const IndoorMap* map) {
XMLDocument doc;
XMLDoc doc;
construct(doc, map);
XMLPrinter printer;
tinyxml2::XMLPrinter printer;
doc.Accept( &printer );
return printer.CStr();
}
private:
static std::string toString(const ObstacleType t) {
return std::to_string((int)t);
}
static std::string toString(const ObstacleType t) { return std::to_string((int)t); }
static std::string toString(const DoorType t) { return std::to_string((int)t); }
static std::string toString(const Material m) { return std::to_string((int)m); }
static std::string toString(const OutlineMethod m) { return std::to_string((int)m); }
static std::string toString(const Material m) {
return std::to_string((int)m);
}
static void construct(XMLDoc& doc, const IndoorMap* map) {
static std::string toString(const OutlineMethod m) {
return std::to_string((int)m);
}
static void construct(XMLDocument& doc, const IndoorMap* map) {
XMLElement* root = doc.NewElement("map");
XMLElem* root = doc.NewElement("map");
doc.InsertEndChild(root);
root->SetAttribute("width", map->width);
root->SetAttribute("depth", map->depth);
@@ -60,16 +55,16 @@ namespace Floorplan {
}
/** add all floors to the map */
static void addFloors(XMLDocument& doc, XMLElement* root, const IndoorMap* map) {
XMLElement* floors = doc.NewElement("floors");
static void addFloors(XMLDoc& doc, XMLElem* root, const IndoorMap* map) {
XMLElem* floors = doc.NewElement("floors");
for(const Floor* mf : map->floors) {addFloor(doc, floors, mf);}
root->InsertEndChild(floors);
}
/** add one floor to the floors-node */
static void addFloor(XMLDocument& doc, XMLElement* floors, const Floor* mf) {
static void addFloor(XMLDoc& doc, XMLElem* floors, const Floor* mf) {
XMLElement* floor = doc.NewElement("floor");
XMLElem* floor = doc.NewElement("floor");
floor->SetAttribute("atHeight", mf->atHeight);
floor->SetAttribute("height", mf->height);
floor->SetAttribute("name", mf->name.c_str());
@@ -77,19 +72,64 @@ namespace Floorplan {
addFloorOutline(doc, floor, mf);
addFloorObstacles(doc, floor, mf);
addFloorUnderlays(doc, floor, mf);
addFloorUnderlays(doc, floor, mf);
// add all sorts of POI
addFloorPOI(doc, floor, mf);
addStairs(doc, floor, mf);
}
/** add all stairs to the floor */
static void addStairs(XMLDoc& doc, XMLElem* floor, const Floor* mf) {
XMLElem* stairs = doc.NewElement("stairs");
for (const Stair* _stair : mf->stairs) {
XMLElem* elem = doc.NewElement("stair");
addMetaElement(doc, elem, _stair->meta);
if (dynamic_cast<const StairFreeform*>(_stair)) {
const StairFreeform* stair = (StairFreeform*) _stair;
elem->SetAttribute("type", 0); // TODO: other types?
for (const StairPart& part : stair->parts) {
XMLElem* elemPart = doc.NewElement("part");
elemPart->SetAttribute("connect", part.connectWithPrev);
elemPart->SetAttribute("x1", part.start.x);
elemPart->SetAttribute("y1", part.start.y);
elemPart->SetAttribute("z1", part.start.z);
elemPart->SetAttribute("x2", part.end.x);
elemPart->SetAttribute("y2", part.end.y);
elemPart->SetAttribute("z2", part.end.z);
elemPart->SetAttribute("w", part.width);
elem->InsertEndChild(elemPart);
}
} else {
throw "not yet implemented";
}
stairs->InsertEndChild(elem);
}
floor->InsertEndChild(stairs);
}
/** add all sorts of POI to the floor */
static void addFloorPOI(XMLDocument& doc, XMLElement* floor, const Floor* mf) {
static void addFloorPOI(XMLDoc& doc, XMLElem* floor, const Floor* mf) {
XMLElement* accesspoints = doc.NewElement("accesspoints");
XMLElem* pois = doc.NewElement("pois");
for (const POI* poi : mf->pois) {
XMLElem* elem = doc.NewElement("poi");
elem->SetAttribute("name", poi->name.c_str());
elem->SetAttribute("type", (int) poi->type);
elem->SetAttribute("x", poi->pos.x);
elem->SetAttribute("y", poi->pos.y);
pois->InsertEndChild(elem);
}
floor->InsertEndChild(pois);
XMLElem* accesspoints = doc.NewElement("accesspoints");
for (const AccessPoint* ap : mf->accesspoints) {
XMLElement* accesspoint = doc.NewElement("accesspoint");
XMLElem* accesspoint = doc.NewElement("accesspoint");
accesspoint->SetAttribute("name", ap->name.c_str());
accesspoint->SetAttribute("mac", ap->mac.c_str());
accesspoint->SetAttribute("x", ap->pos.x);
@@ -99,9 +139,9 @@ namespace Floorplan {
}
floor->InsertEndChild(accesspoints);
XMLElement* beacons = doc.NewElement("beacons");
XMLElem* beacons = doc.NewElement("beacons");
for (const Beacon* b : mf->beacons) {
XMLElement* beacon = doc.NewElement("beacon");
XMLElem* beacon = doc.NewElement("beacon");
beacon->SetAttribute("name", b->name.c_str());
beacon->SetAttribute("mac", b->mac.c_str());
beacon->SetAttribute("x", b->pos.x);
@@ -114,32 +154,32 @@ namespace Floorplan {
}
static void addFloorOutline(XMLDocument& doc, XMLElement* floor, const Floor* mf) {
static void addFloorOutline(XMLDoc& doc, XMLElem* floor, const Floor* mf) {
XMLElement* outline = doc.NewElement("outline");
XMLElem* outline = doc.NewElement("outline");
floor->InsertEndChild(outline);
// XMLElement* add = doc.NewElement("add");
// XMLElem* add = doc.NewElement("add");
// outline->InsertEndChild(add);
for (const FloorOutlinePolygon* poly : mf->outline) { addFloorOutlinePolygon(doc, outline, poly); }
// XMLElement* remove = doc.NewElement("remove");
// XMLElem* remove = doc.NewElement("remove");
// outline->InsertEndChild(remove);
// for (const FloorOutlinePolygon& poly : mf.outline.remove) { addFloorOutlinePolygon(doc, remove, poly); }
}
static void addFloorOutlinePolygon(XMLDocument& doc, XMLElement* dst, const FloorOutlinePolygon* poly) {
static void addFloorOutlinePolygon(XMLDoc& doc, XMLElem* dst, const FloorOutlinePolygon* poly) {
std::string method = toString(poly->method);
XMLElement* polygon = doc.NewElement("polygon");
XMLElem* polygon = doc.NewElement("polygon");
polygon->SetAttribute("name", poly->name.c_str());
polygon->SetAttribute("method", method.c_str());
dst->InsertEndChild(polygon);
for (Point2 p : poly->poly.points) {
XMLElement* point = doc.NewElement("point");
XMLElem* point = doc.NewElement("point");
point->SetAttribute("x", p.x);
point->SetAttribute("y", p.y);
// skip the z-attribute as the value is also given by the polygons floor: floor.atHeight
@@ -148,58 +188,49 @@ namespace Floorplan {
}
/** write one <keyval> element */
static void addKeyValueElement(XMLDocument& doc, XMLElement* other, const KeyValueElement* kv) {
XMLElement* elem = doc.NewElement("keyval");
for (auto it : kv->params) {
/** write one <meta> element */
static void addMetaElement(XMLDoc& doc, XMLElem* other, const Meta* meta) {
if (meta == nullptr) {return;} // nothing to add
XMLElem* elem = doc.NewElement("meta");
for (auto it : meta->params) {
elem->Attribute(it.first.c_str(), it.second.c_str());
}
other->InsertEndChild(elem);
}
/** write the <underlays> tag */
static void addFloorUnderlays(XMLDocument& doc, XMLElement* floor, const Floor* mf) {
XMLElement* other = doc.NewElement("underlays");
for (const UnderlayImage* kv : mf->underlays) {
addFloorUnderlay(doc, other, kv);
}
floor->InsertEndChild(other);
}
/** write the <underlays> tag */
static void addFloorUnderlays(XMLDoc& doc, XMLElem* floor, const Floor* mf) {
XMLElem* other = doc.NewElement("underlays");
for (const UnderlayImage* kv : mf->underlays) {
addFloorUnderlay(doc, other, kv);
}
floor->InsertEndChild(other);
}
/** write one underlay */
static void addFloorUnderlay(XMLDocument& doc, XMLElement* underlays, const UnderlayImage* img) {
XMLElement* underlay = doc.NewElement("underlay");
underlay->SetAttribute("x", img->anchor.x);
underlay->SetAttribute("y", img->anchor.y);
underlay->SetAttribute("sx", img->scaleX);
underlay->SetAttribute("sy", img->scaleY);
underlay->SetAttribute("name", img->name.c_str());
underlay->SetAttribute("file", img->filename.c_str());
underlays->InsertEndChild(underlay);
}
/** write one underlay */
static void addFloorUnderlay(XMLDoc& doc, XMLElem* underlays, const UnderlayImage* img) {
XMLElem* underlay = doc.NewElement("underlay");
underlay->SetAttribute("x", img->anchor.x);
underlay->SetAttribute("y", img->anchor.y);
underlay->SetAttribute("sx", img->scaleX);
underlay->SetAttribute("sy", img->scaleY);
underlay->SetAttribute("name", img->name.c_str());
underlay->SetAttribute("file", img->filename.c_str());
underlays->InsertEndChild(underlay);
}
static void addFloorObstacles(XMLDoc& doc, XMLElem* floor, const Floor* mf) {
// /** write the <other> tag */
// static void addFloorKeyValue(XMLDocument& doc, XMLElement* floor, const Floor* mf) {
// XMLElement* other = doc.NewElement("other");
// for (const KeyValueElement* kv : mf->other.elements) {
// addKeyValueElement(doc, other, kv);
// }
// floor->InsertEndChild(other);
// }
static void addFloorObstacles(XMLDocument& doc, XMLElement* floor, const Floor* mf) {
XMLElement* obstacles = doc.NewElement("obstacles");
XMLElem* obstacles = doc.NewElement("obstacles");
for (FloorObstacle* fo : mf->obstacles) {
if (dynamic_cast<FloorObstacleLine*>(fo)) {
addFloorObstacleLine(doc, obstacles, (FloorObstacleLine*)fo);
} else if (dynamic_cast<FloorObstacleCircle*>(fo)) {
addFloorObstacleCircle(doc, obstacles, (FloorObstacleCircle*)fo);
} else if (dynamic_cast<FloorObstacleDoor*>(fo)) {
addFloorObstacleDoor(doc, obstacles, (FloorObstacleDoor*)fo);
}
}
@@ -207,37 +238,46 @@ namespace Floorplan {
}
static void addFloorObstacleLine(XMLDocument& doc, XMLElement* obstacles, FloorObstacleLine* line) {
XMLElement* obstacle = doc.NewElement("line");
const std::string type = toString(line->type);
const std::string material = toString(line->material);
obstacle->SetAttribute("material", material.c_str());
obstacle->SetAttribute("type", type.c_str());
/** write a line obstacle (wall, handrail, ..) */
static void addFloorObstacleLine(XMLDoc& doc, XMLElem* obstacles, FloorObstacleLine* line) {
XMLElem* obstacle = doc.NewElement("line");
obstacle->SetAttribute("material", toString(line->material).c_str());
obstacle->SetAttribute("type", toString(line->type).c_str());
obstacle->SetAttribute("x1", line->from.x);
obstacle->SetAttribute("y1", line->from.y);
obstacle->SetAttribute("x2", line->to.x);
obstacle->SetAttribute("y2", line->to.y);
obstacles->InsertEndChild(obstacle);
}
static void addFloorObstacleCircle(XMLDocument& doc, XMLElement* obstacles, FloorObstacleCircle* circle) {
XMLElement* obstacle = doc.NewElement("line");
const std::string type = toString(circle->type);
const std::string material = toString(circle->material);
obstacle->SetAttribute("material", material.c_str());
obstacle->SetAttribute("type", type.c_str());
/** write a circle obstacle (pillar, ..) */
static void addFloorObstacleCircle(XMLDoc& doc, XMLElem* obstacles, FloorObstacleCircle* circle) {
XMLElem* obstacle = doc.NewElement("circle");
obstacle->SetAttribute("material", toString(circle->material).c_str());
obstacle->SetAttribute("cx", circle->center.x);
obstacle->SetAttribute("cy", circle->center.y);
obstacle->SetAttribute("radius", circle->radius);
obstacles->InsertEndChild(obstacle);
}
/** write a door obstacle */
static void addFloorObstacleDoor(XMLDoc& doc, XMLElem* obstacles, FloorObstacleDoor* door) {
XMLElem* obstacle = doc.NewElement("door");
obstacle->SetAttribute("material", toString(door->material).c_str());
obstacle->SetAttribute("type", toString(door->type).c_str());
obstacle->SetAttribute("x1", door->from.x);
obstacle->SetAttribute("y1", door->from.y);
obstacle->SetAttribute("x2", door->to.x);
obstacle->SetAttribute("y2", door->to.y);
obstacle->SetAttribute("height", door->height);
obstacle->SetAttribute("swap", door->swap);
obstacles->InsertEndChild(obstacle);
}
};
}
#endif // FLOORPLANWRITER_H

29
geo/BBox2.h
View File

@@ -42,6 +42,10 @@ public:
/** get the bbox's maximum */
const Point2& getMax() const {return p2;}
/** get the bbox's center point */
Point2 getCenter() const { return (p1+p2) / 2; }
/** equal? */
bool operator == (const BBox2& o) const {
return (p1.x == o.p1.x) &&
@@ -52,16 +56,27 @@ public:
/** does the BBox intersect with the given line? */
bool intersects (const Line2& l) const {
Line2 l1(p1.x, p1.y, p2.x, p1.y); // upper
Line2 l2(p1.x, p2.y, p2.x, p2.y); // lower
Line2 l3(p1.x, p1.y, p1.x, p2.y); // left
Line2 l4(p2.x, p1.y, p2.x, p2.y); // right
const Line2 l1(p1.x, p1.y, p2.x, p1.y); // upper
const Line2 l2(p1.x, p2.y, p2.x, p2.y); // lower
const Line2 l3(p1.x, p1.y, p1.x, p2.y); // left
const Line2 l4(p2.x, p1.y, p2.x, p2.y); // right
return l.getSegmentIntersection(l1) ||
l.getSegmentIntersection(l2) ||
l.getSegmentIntersection(l3) ||
l.getSegmentIntersection(l4);
}
std::vector<Line2> lines() const {
return std::vector<Line2>(
{
Line2(p1.x, p1.y, p2.x, p1.y),
Line2(p1.x, p2.y, p2.x, p2.y),
Line2(p1.x, p1.y, p1.x, p2.y),
Line2(p2.x, p1.y, p2.x, p2.y)
}
);
}
bool contains(const Point2& p) const {
if (p.x < p1.x) {return false;}
if (p.x > p2.x) {return false;}
@@ -76,6 +91,12 @@ public:
p2 += p; // increase maximum
}
/** grow the bbox by the amount given for each dimension */
void grow(const float val) {
p1 -= Point2(val, val); // decrease minimum
p2 += Point2(val, val); // increase maximum
}
};
#endif // BBOX2_H

66
geo/Line2.h
View File

@@ -55,6 +55,35 @@ public:
bool getSegmentIntersection(const Line2& other) const {
const double delta = 0.0000001;
const double bx = p2.x - p1.x;
const double by = p2.y - p1.y;
const double dx = other.p2.x - other.p1.x;
const double dy = other.p2.y - other.p1.y;
const double b_dot_d_perp = bx*dy - by*dx;
if (std::abs(b_dot_d_perp) == 0) {return false;}
const double cx = other.p1.x - p1.x;
const double cy = other.p1.y - p1.y;
const double t = (cx * dy - cy * dx) / b_dot_d_perp;
if(t < 0+delta || t > 1-delta) {return false;}
const double u = (cx * by - cy * bx) / b_dot_d_perp;
if(u < 0+delta || u > 1-delta) {return false;}
return true;
}
bool getSegmentIntersection(const Line2& other, Point2& result) const {
const float delta = 0.000001;
const float bx = p2.x - p1.x;
const float by = p2.y - p1.y;
@@ -63,49 +92,24 @@ public:
const float b_dot_d_perp = bx*dy - by*dx;
if (b_dot_d_perp == 0) {return false;}
if(b_dot_d_perp == 0) {return false;}
const float cx = other.p1.x - p1.x;
const float cy = other.p1.y - p1.y;
const float t = (cx * dy - cy * dx) / b_dot_d_perp;
if(t < 0 || t > 1) {return false;}
if(t < 0+delta || t > 1-delta) {return false;}
const float u = (cx * by - cy * bx) / b_dot_d_perp;
if(u < 0 || u > 1) {return false;}
if(u < 0+delta || u > 1-delta) {return false;}
result.x = p1.x + t * bx;
result.y = p1.y + t * by;
return true;
}
bool getSegmentIntersection(const Line2& other, Point2& result) const {
const double bx = p2.x - p1.x;
const double by = p2.y - p1.y;
const double dx = other.p2.x - other.p1.x;
const double dy = other.p2.y - other.p1.y;
const double b_dot_d_perp = bx*dy - by*dx;
if(b_dot_d_perp == 0) {return false;}
const double cx = other.p1.x - p1.x;
const double cy = other.p1.y - p1.y;
const double t = (cx * dy - cy * dx) / b_dot_d_perp;
if(t < 0 || t > 1) {return false;}
const double u = (cx * by - cy * bx) / b_dot_d_perp;
if(u < 0 || u > 1) {return false;}
result.x = p1.x + t * bx;
result.y = p1.y + t * by;
return true;
}
};
#endif // LINE2D_H

8
geo/Point2.h
View File

@@ -38,10 +38,14 @@ struct Point2 {
bool operator == (const Point2& o) const {return x==o.x && y==o.y;}
bool operator != (const Point2& o) const {return x!=o.x || y!=o.y;}
bool operator != (const Point2& o) const {return x!=o.x || y!=o.y;}
Point2 perpendicular() const {return Point2(-y, x);}
Point2 perpendicular() const {return Point2(-y, x);}
float length() const {return std::sqrt(x*x + y*y);}
Point2 normalized() const {return (*this) / length();}
/** get the distance between this point and the other one */
float getDistance(const Point2& o) const {

33
geo/Point3.h
View File

@@ -21,6 +21,8 @@ struct Point3 {
Point3(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
Point3 operator - () const {return Point3(-x, -y, -z);}
Point3 operator + (const Point3& o) const {return Point3(x+o.x, y+o.y, z+o.z);}
@@ -37,19 +39,43 @@ struct Point3 {
Point3& operator /= (const float v) {x/=v; y/=v; z/=v; return *this;}
Point3& operator += (const Point3& o) {x+=o.x; y+=o.y; z+=o.z; return *this;}
Point3& operator -= (const Point3& o) {x-=o.x; y-=o.y; z-=o.z; return *this;}
Point3& operator *= (const Point3& o) {x*=o.x; y*=o.y; z*=o.z; return *this;}
Point3& operator /= (const Point3& o) {x/=o.x; y/=o.y; z/=o.z; return *this;}
bool operator < (const Point3& o) const {return x<o.x && y<o.y && z<o.z;}
bool operator == (const Point3& o) const {return x==o.x && y==o.y && z==o.z;}
bool operator != (const Point3& o) const {return x!=o.x || y!=o.y || z!=o.z;}
bool operator != (const Point3& o) const {return x!=o.x || y!=o.y || z!=o.z;}
bool eq (const Point3& o, const float delta) const { return eq(x,o.x,delta) && eq(y,o.y,delta) && eq(z,o.z,delta); }
Point2 xy() const {return Point2(x,y);}
Point3 rotX(const float r) const {
return Point3(x, y*cos(r) - z*sin(r), y*sin(r) + z*cos(r));
}
Point3 rotY(const float r) const {
return Point3(z*sin(r) + x*cos(r), y, z*cos(r) - x*sin(r));
}
Point3 rotZ(const float r) const {
return Point3(x*cos(r) - y*sin(r), x*sin(r) + y*cos(r), z);
}
Point3 rot(const float rx, const float ry, const float rz) const {
return rotX(rx).rotY(ry).rotZ(rz);
//return rotZ(rz).rotY(ry).rotX(rx);
}
/** read-only array access */
float operator [] (const int idx) const {
Assert::isBetween(idx, 0, 2, "index out of bounds");
@@ -76,6 +102,11 @@ struct Point3 {
), 1.0f/norm);
}
private:
static inline bool eq(const float a, const float b, const float delta) {return std::abs(a-b) <= delta;}
static inline bool ne(const float a, const float b, const float delta) {return std::abs(a-b) > delta;}
};
#endif // POINT3_H

49
grid/Grid.h
View File

@@ -4,6 +4,7 @@
#include <vector>
#include <iostream>
#include <unordered_map>
#include <algorithm>
#include "../Exception.h"
#include "GridPoint.h"
@@ -53,6 +54,12 @@ public:
/** no-copy */
Grid(const Grid& o) = delete;
/** reset the grid (empty) */
void reset() {
nodes.clear();
hashes.clear();
}
/** no-assign */
void operator = (const Grid& o) = delete;
@@ -77,14 +84,16 @@ public:
}
/** add the given (not necessarly aligned) element to the grid */
int addUnaligned(const T& elem) {
const UID uid = getUID(elem); // get the UID for this new element
Assert::isTrue(hashes.find(uid) == hashes.end(), "node's UID is already taken!"); // avoid potential errors
const int idx = nodes.size(); // next free index
nodes.push_back(elem); // add it to the grid
nodes.back()._idx = idx; // let the node know his own index
hashes[uid] = idx; // add an UID->index lookup
return idx; // done
int addUnaligned(const T& elem, const bool check=true) {
const UID uid = getUID(elem); // get the UID for this new element
if (check) {
Assert::isTrue(hashes.find(uid) == hashes.end(), "node's UID is already taken!"); // avoid potential errors
}
const int idx = nodes.size(); // next free index
nodes.push_back(elem); // add it to the grid
nodes.back()._idx = idx; // let the node know his own index
hashes[uid] = idx; // add an UID->index lookup
return idx; // done
}
/** connect (uni-dir) i1 -> i2 */
@@ -94,9 +103,11 @@ public:
/** connect (uni-dir) i1 -> i2 */
void connectUniDir(T& n1, const T& n2) {
Assert::isFalse(n1.hasNeighbor(n2._idx), "this neighbor is already connected"); // already connected?
Assert::notEqual(n1.getIdx(), n2.getIdx(), "can not connect node with itself");
n1._neighbors[n1._numNeighbors] = n2._idx;
++n1._numNeighbors;
Assert::isBetween(n1._numNeighbors, 0, 10, "number of neighbors out of bounds!");
Assert::isBetween(n1._numNeighbors, (uint8_t) 0, (uint8_t) 10, "number of neighbors out of bounds!");
}
/**
@@ -165,11 +176,25 @@ public:
}
/** get the center-node the given Point belongs to. or nullptr if not present */
const T* getNodePtrFor(const GridPoint& p) {
const T* getNodePtrFor(const GridPoint& p) const {
auto it = hashes.find(getUID(p));
return (it == hashes.end()) ? (nullptr) : (&nodes[it->second]);
}
/** get the node nearest to the given positon */
const T& getNearestNode(const GridPoint& p) const {
auto comp = [p] (const T& a, const T& b) { return a.getDistanceInMeter(p) < b.getDistanceInMeter(p); };
auto it = std::min_element(nodes.begin(), nodes.end(), comp);
return nodes[it->getIdx()];
}
/** get the node nearest to the given positon, examining only the nodes given by the provided index-array */
const T& getNearestNode(const GridPoint& p, const std::vector<int>& indices) const {
auto comp = [&] (const int a, const int b) { return nodes[a].getDistanceInMeter(p) < nodes[b].getDistanceInMeter(p); };
auto it = std::min_element(indices.begin(), indices.end(), comp);
return nodes[it->getIdx()];
}
/** get the BBox for the given node */
GridNodeBBox getBBox(const int idx) const {
return getBBox(nodes[idx]);
@@ -188,7 +213,7 @@ public:
UID getUID(const GridPoint& p) const {
const uint64_t x = std::round(p.x_cm / (float)gridSize_cm);
const uint64_t y = std::round(p.y_cm / (float)gridSize_cm);
const uint64_t z = std::round(p.z_cm / (float)gridSize_cm);
const uint64_t z = std::round(p.z_cm / (float)gridSize_cm * 5); // z is usually much lower and not always aligned -> allow more room for hashes
return (z << 40) | (y << 20) | (x << 0);
}
@@ -446,7 +471,7 @@ private:
void assertAligned(const T& elem) {
if (((int)elem.x_cm % gridSize_cm) != 0) {throw Exception("element's x is not aligned!");}
if (((int)elem.y_cm % gridSize_cm) != 0) {throw Exception("element's y is not aligned!");}
if (((int)elem.z_cm % gridSize_cm) != 0) {throw Exception("element's z is not aligned!");}
//if (((int)elem.z_cm % gridSize_cm) != 0) {throw Exception("element's z is not aligned!");}
}
};

32
grid/GridNode.h
View File

@@ -21,15 +21,26 @@ private:
/** grant full access to the grid */
template<typename> friend class Grid;
/** INTERNAL: array-index */
/** INTERNAL: node's index array-index */
int _idx;
/** semantic annotation for this node */
uint8_t _type;
/** INTERNAL: number of neighbors */
int _numNeighbors;
uint8_t _numNeighbors;
/** INTERNAL: store neighbors (via index) */
int _neighbors[10];
public:
static const uint8_t TYPE_FLOOR = 0;
static const uint8_t TYPE_STAIR = 1;
static const uint8_t TYPE_ELEVATOR = 2;
static const uint8_t TYPE_DOOR = 3;
public:
GridNode() : _idx(-1), _numNeighbors(0), _neighbors() {;}
@@ -40,6 +51,23 @@ public:
/** get the number of neighbors for this node */
int getNumNeighbors() const {return _numNeighbors;}
/** reached neighbor limit? */
bool fullyConnected() const {return _numNeighbors >= 10;}
/** is this node connected to the given index? */
bool hasNeighbor(const int idx) const {
for (int i = 0; i < _numNeighbors; ++i) {
if (_neighbors[i] == idx) {return true;}
}
return false;
}
/** get the node's semantic type */
uint8_t getType() const {return _type;}
/** set the node's semantic type */
void setType(const uint8_t type) {this->_type = type;}
// /** get the n-th neighbor for this node */
// template <int gridSize_cm, typename T> inline T& getNeighbor(const int nth, const Grid<gridSize_cm, T>& grid) const {
// return grid.getNeighbor(_idx, nth);

12
grid/GridPoint.h
View File

@@ -30,12 +30,22 @@ struct GridPoint {
return x_cm == o.x_cm && y_cm == o.y_cm && z_cm == o.z_cm;
}
/** not equal? */
bool operator != (const GridPoint& o) const {
return x_cm != o.x_cm || y_cm != o.y_cm || z_cm != o.z_cm;
}
/** get the distance (in meter) betwen this and the given point */
float getDistanceInMeter(const GridPoint& other) const {
return getDistanceInCM(other) / 100.0f;
}
/** get the distance (in centimeter) betwen this and the given point */
float getDistanceInCM(const GridPoint& other) const {
const int dx = x_cm - other.x_cm;
const int dy = y_cm - other.y_cm;
const int dz = z_cm - other.z_cm;
return std::sqrt(dx*dx + dy*dy + dz*dz) / 100.0f;
return std::sqrt(dx*dx + dy*dy + dz*dz);
}
/** cast to Point3 */

507
grid/factory/v2/GridFactory.h Executable file
View File

@@ -0,0 +1,507 @@
#ifndef GRIDFACTORY_V2_H
#define GRIDFACTORY_V2_H
#include <string>
#include <unordered_set>
#include <set>
#include "../../../floorplan/v2/Floorplan.h"
#include "Helper.h"
#include "Stairs.h"
#include "../../../geo/Units.h"
#include "../../GridNodeBBox.h"
#include "../../Grid.h"
#include "../../../misc/Debug.h"
#include <functional>
/** listen for events during the build process */
class GridFactoryListener {
public:
virtual void onGridBuildUpdateMajor(const std::string& what) = 0;
virtual void onGridBuildUpdateMajor(const int cnt, const int cur) = 0;
virtual void onGridBuildUpdateMinor(const std::string& what) = 0;
virtual void onGridBuildUpdateMinor(const int cnt, const int cur) = 0;
};
template <typename T> class GridFactory {
/** logging name */
static constexpr const char* name = "GridFac2";
private:
/** the grid to build into */
Grid<T>& grid;
/** calculation helper */
Helper<T> helper;
/** stair builder */
Stairs<T> stairs;
bool _buildStairs = true;
bool _removeIsolated = true;
public:
/** ctor with the grid to fill */
GridFactory(Grid<T>& grid) : grid(grid), helper(grid), stairs(grid) {;}
/** whether or not to build stairs */
void setBuildStairs(const bool build) {this->_buildStairs = build;}
/** whether or not to remove isolated nodes */
void setRemoveIsolated(const bool remove) {this->_removeIsolated = remove;}
/** build using the given map */
void build(const Floorplan::IndoorMap* map, GridFactoryListener* listener = nullptr) {
Log::add(name, "building grid from IndoorMap", true);
const int total = map->floors.size()*2 + 1;
int cur = 0;
// build all the floors
if (listener) {listener->onGridBuildUpdateMajor("adding floors");}
for (Floorplan::Floor* f : map->floors) {
addFloor(f, listener);
if (listener) {listener->onGridBuildUpdateMajor(total, ++cur);}
}
// build all stairs
if (listener) {listener->onGridBuildUpdateMajor("adding stairs");}
if (_buildStairs) {
for (Floorplan::Floor* f : map->floors) {
buildStairs(f, listener);
if (listener) {listener->onGridBuildUpdateMajor(total, ++cur);}
}
}
// remove isolated nodes
if (_removeIsolated) {
if (listener) {listener->onGridBuildUpdateMajor("removing isolated nodes");}
removeIsolatedNodes();
if (listener) {listener->onGridBuildUpdateMajor(total, ++cur);}
}
}
/** get the 2D-bbox for the given floor's outline */
BBox2 getFloorOutlineBBox(const Floorplan::FloorOutline& outline) {
BBox2 bb;
for (Floorplan::FloorOutlinePolygon* poly : outline) {
for (Point2 p : poly->poly.points) {
bb.add(p * 100); // convert m to cm
}
}
return bb;
}
bool isPartOfFloorOutline(const int x_cm, const int y_cm, const Floorplan::FloorOutline& outline) {
bool add = false;
for (Floorplan::FloorOutlinePolygon* poly : outline) {
if (poly->method != Floorplan::OutlineMethod::ADD) {continue;}
HelperPoly pol(*poly);
if (pol.contains(Point2(x_cm, y_cm))) {add = true;}
}
if (!add) {return false;}
for (Floorplan::FloorOutlinePolygon* poly : outline) {
if (poly->method != Floorplan::OutlineMethod::REMOVE) {continue;}
HelperPoly pol(*poly);
if (pol.contains(Point2(x_cm, y_cm))) {add = false;} // TODO
}
return add;
}
/** add the given floor to the grid */
void addFloor(const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) {
Log::add(name, "adding floor " + floor->name, true);
if (listener) {listener->onGridBuildUpdateMinor("adding floor " + floor->name);}
const BBox2 bbox = getFloorOutlineBBox(floor->outline);
const int x1 = helper.align(bbox.getMin().x);
const int x2 = helper.align(bbox.getMax().x);
const int y1 = helper.align(bbox.getMin().y);
const int y2 = helper.align(bbox.getMax().y);
const int z_cm = (floor->atHeight*100);
const int total = (x2-x1) / helper.gridSize();
int cur = 0;
// 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()) {
// does the outline-polygon contain this position?
if (!isPartOfFloorOutline(x_cm, y_cm, floor->outline)) {continue;}
// 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.012345);
if (intersects(bbox, floor)) {continue;}
// add to the grid
T t(x_cm, y_cm, z_cm);
t.setType(getType(bbox, floor));
if (grid.hasNodeFor(t)) {continue;}
grid.add(t);
}
if (listener) {listener->onGridBuildUpdateMinor(total, ++cur);}
}
// connect the g
connectAdjacent(z_cm);
}
void buildStairs(const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) {
const int total = floor->stairs.size();
int cur = 0;
// process each stair within the floor
for (const Floorplan::Stair* stair : floor->stairs) {
if (listener) {listener->onGridBuildUpdateMinor("adding " + floor->name + " stair " + std::to_string(cur+1));}
stairs.build(floor, stair);
if (listener) {listener->onGridBuildUpdateMinor(total, ++cur);}
}
}
// void addWithin(const Floorplan::Floor* floor, std::vector<Entry>& nodeList) {
// const int fz1_cm = floor->atHeight*100;
// const int fz2_cm = (floor->atHeight+floor->height)*100;
// for (Entry& e : nodeList) {
// if ( (e.pos.z_cm <= fz1_cm) || (e.pos.z_cm >= fz2_cm) ) {
// e.idx = grid.getNearestNode(e.pos).getIdx();
// e.part = {9999};
// } else {
// const T t(e.pos.x_cm, e.pos.y_cm, e.pos.z_cm);
// e.idx = grid.addUnaligned(t, false);
// }
// }
// }
// void filter(std::vector<Entry>& nodeList) {
// const int gs_cm = grid.getGridSize_cm();
// const int limit_cm = gs_cm * 0.50;
// // remove duplicate nodes or nodes, that are too close to each other
// for(auto it = nodeList.begin(); it != nodeList.end(); ) {
// // currently examined node
// const Entry& e1 = *it;
// // matches for nodes that are NOT the same instance AND nearby (< grid-size)
// auto matches = [&] (const Entry& e2) { return (e1.pos != e2.pos) && (e1.pos.getDistanceInCM(e2.pos) <= limit_cm); };
// auto match = std::find_if(nodeList.begin(), nodeList.end(), matches);
// // remove if this node has a nearby neighbor
// if (match != nodeList.end()) {
// // combine both nodes within one:
// // the node belongs to more than one stair-part
// Entry& e2 = *match;
// e2.part.insert(e2.part.end(), e1.part.begin(), e1.part.end());
// it = nodeList.erase(it);
// } else {
// ++it;
// }
// }
// }
// std::vector<Entry> minOnly(const std::vector<Entry>& lst) {
// auto comp = [&] (const Entry& a, const Entry& b) {return grid[a.idx].z_cm < grid[b.idx].z_cm;};
// auto it = std::min_element(lst.begin(), lst.end(), comp);
// T& min = grid[it->idx];
// std::vector<Entry> res;
// for (const Entry& e : lst) {
// if (grid[e.idx].z_cm == min.z_cm) {res.push_back(e);}
// }
// return res;
// }
/** connect all neighboring nodes part of the given index-vector */
void connectAdjacent(const std::vector<int>& indices) {
for (const int idx : indices) {
// connect the node with its neighbors
connectAdjacent(grid[idx]);
}
}
/** connect all neighboring nodes located on the given height-plane */
void connectAdjacent(const float z_cm) {
Log::add(name, "connecting all adjacent nodes at height " + std::to_string(z_cm), false);
Log::tick();
// connect adjacent grid-points
for (T& n1 : grid) {
// not the floor we are looking for? -> skip (ugly.. slow(er))
if (n1.z_cm != z_cm) {continue;}
// connect the node with its neighbors
connectAdjacent(n1);
}
Log::tock();
}
/** connect the given node with its neighbors */
void connectAdjacent(T& n1) {
const int gridSize_cm = grid.getGridSize_cm();
// square around the node
for (int x = -1; x <= +1; ++x) {
for (int y = -1; y <= +1; ++y) {
// skip the center (node itself)
if ((x == y) && (x == 0)) {continue;}
// position of the potential neighbor
const int ox = n1.x_cm + x * gridSize_cm;
const int oy = n1.y_cm + y * gridSize_cm;
const GridPoint p(ox, oy, n1.z_cm);
// does the grid contain the potential neighbor?
const T* n2 = grid.getNodePtrFor(p);
if (n2 != nullptr) {
grid.connectUniDir(n1, *n2); // UNI-dir connection as EACH node is processed!
}
}
}
}
/** add the inverted version of the given z-layer */
void addInverted(const Grid<T>& gIn, const float z_cm) {
// get the original grid's bbox
BBox3 bb = gIn.getBBox();
// ensure we have an outer boundary
bb.grow(gIn.getGridSize_cm() * 2);
const int gridSize_cm = grid.getGridSize_cm();
// build new grid-points
for(int x_cm = bb.getMin().x; x_cm <= bb.getMax().x; x_cm += gridSize_cm) {
for (int y_cm = bb.getMin().y; y_cm < bb.getMax().y; y_cm += gridSize_cm) {
// does the input-grid contain such a point?
GridPoint gp(x_cm, y_cm, z_cm);
if (gIn.hasNodeFor(gp)) {continue;}
// add to the grid
grid.add(T(x_cm, y_cm, z_cm));
}
}
}
// // TODO: how to determine the starting index?!
// // IDEAS: find all segments:
// // start at a random point, add all connected points to the set
// // start at a NEW random point ( not part of the already processed points), add connected points to a new set
// // repeat until all points processed
// // how to handle multiple floor layers?!?!
// // run after all floors AND staircases were added??
// // OR: random start, check segment size, < 50% of all nodes? start again
// void removeIsolatedNodes() {
// Log::add(name, "searching for isolated nodes");
// // get largest connected region
// std::unordered_set<int> set;
// do {
// const int idxStart = rand() % grid.getNumNodes();
// set.clear();
// Log::add(name, "getting connected region starting at " + (std::string) grid[idxStart]);
// getConnected(grid[idxStart], set);
// Log::add(name, "region size is " + std::to_string(set.size()) + " nodes");
// } while (set.size() < 0.5 * grid.getNumNodes());
// // remove all other
// Log::add(name, "removing the isolated nodes");
// for (int i = 0; i < grid.getNumNodes(); ++i) {
// if (set.find(i) == set.end()) {grid.remove(i);}
// }
// // clean the grid
// grid.cleanup();
// }
void removeIsolatedNodes() {
// try to start at the first stair
for (T& 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(T& 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();
// remove all other
Log::add(name, "removing all nodes NOT connected to " + (std::string) n1, false);
Log::tick();
for (T& n2 : grid) {
if (set.find(n2.getIdx()) == set.end()) {grid.remove(n2);}
}
Log::tock();
// clean the grid (physically delete the removed nodes)
grid.cleanup();
}
private:
/** recursively get all connected nodes and add them to the set */
void getConnected(T& 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);
T& next = grid[nextIdx];
// get all his (unprocessed) neighbors and add them to the region
for (const T& n2 : grid.neighbors(next)) {
if (visited.find(n2.getIdx()) == visited.end()) {
toVisit.insert(n2.getIdx());
}
}
}
}
private:
/** does the bbox intersect with any of the floor's walls? */
static inline bool intersects(const GridNodeBBox& bbox, const Floorplan::Floor* floor) {
// process each obstacle
// (obstacles use meter, while the bbox is in centimeter!
for (Floorplan::FloorObstacle* fo : floor->obstacles) {
// 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;}
} 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;}
} else if (dynamic_cast<Floorplan::FloorObstacleDoor*>(fo)) {
// DOORS ARE NOT AN OBSTACLE
} else {
throw Exception("TODO: not yet implemented obstacle type");
}
}
return false;
}
/** does the bbox intersect with any of the floor's walls? */
static inline int getType(const GridNodeBBox& bbox, const Floorplan::Floor* floor) {
// process each obstacle
for (Floorplan::FloorObstacle* fo : floor->obstacles) {
if (dynamic_cast<Floorplan::FloorObstacleDoor*>(fo)) {
const Floorplan::FloorObstacleDoor* door = (Floorplan::FloorObstacleDoor*) fo;
const Line2 l2(door->from*100, door->to*100);
if (bbox.intersects(l2)) {return GridNode::TYPE_DOOR;}
}
}
return GridNode::TYPE_FLOOR;
}
};
#endif // GRIDFACTORY_V2_H

219
grid/factory/v2/Helper.h Normal file
View File

@@ -0,0 +1,219 @@
#ifndef GRID_FACTORY_HELPER_H
#define GRID_FACTORY_HELPER_H
#include "../../../geo/Point2.h"
#include "../../../geo/Point3.h"
#include "../../../geo/BBox2.h"
#include "../../../geo/BBox3.h"
#include "../../../floorplan/v2/Floorplan.h"
#include "../../../grid/Grid.h"
/** helper class for polygon methods */
struct HelperPoly {
BBox2 bbox_cm;
std::vector<Point2> points_cm;
/** empty ctor */
HelperPoly() {
;
}
/** ctor from floorplan-polygon */
HelperPoly(const Floorplan::FloorOutlinePolygon& poly) {
for (Point2 p : poly.poly.points) { add(p * 100); }
}
/** ctor from floorplan-quad */
HelperPoly(const Floorplan::Quad3& quad) {
add(quad.p1); add(quad.p2); add(quad.p3); add(quad.p4);
}
void add(const Point2 p) {
points_cm.push_back(p);
bbox_cm.add(p);
}
void add(const Point3& p) {
points_cm.push_back(p.xy());
bbox_cm.add(p.xy());
}
/** does the polygon contain the given point (in cm)? */
bool contains(const Point2 p_cm) const {
// not within bbox? -> not within polygon
if (!bbox_cm.contains(p_cm)) {return false;}
// ensure the point is at least a bit outside of the polygon
const float x1_cm = bbox_cm.getMin().x - 17.71920;
const float y1_cm = bbox_cm.getMin().y - 23.10923891;
// construct line between point outside of the polygon and the point in question
const Line2 l(x1_cm, y1_cm, p_cm.x, p_cm.y);
// determine the number of intersections
int hits = 0;
const int cnt = points_cm.size();
for (int i = 0; i < cnt; ++i) {
const Point2 p1 = points_cm[(i+0)%cnt];
const Point2 p2 = points_cm[(i+1)%cnt];
const Line2 l12(p1, p2);
if (l12.getSegmentIntersection(l)) {++hits;}
}
// inside or outside?
return ((hits % 2) == 1);
}
};
template <typename T> class Helper {
private:
Grid<T>& grid;
public:
/** ctor */
Helper(Grid<T>& grid) : grid(grid) {
}
/** connect the given node to all its neighbors */
void connectToNeighbors(T& n1) {
const int gs_cm = grid.getGridSize_cm();
for (int y = -gs_cm; y <= +gs_cm; y += gs_cm) {
for (int x = -gs_cm; x <= +gs_cm; x += gs_cm) {
// skip the node itself
if (x == 0 && y == 0) {continue;}
// try to find a matching neighbor
const GridPoint gp(n1.x_cm + x, n1.y_cm + y, n1.z_cm);
const T* n2 = grid.getNodePtrFor(gp);
if (!n2) {continue;}
// connect
if (n1.hasNeighbor(n2->getIdx())) {continue;}
grid.connectUniDir(n1, *n2);
}
}
}
int gridSize() const {
return grid.getGridSize_cm();;
}
float align(const float val) {
const float gridSize_cm = gridSize();
return std::round(val/gridSize_cm) * gridSize_cm;
}
float alignF(const float val) {
const float gridSize_cm = gridSize();
return std::floor(val/gridSize_cm) * gridSize_cm;
}
float alignC(const float val) {
const float gridSize_cm = gridSize();
return std::ceil(val/gridSize_cm) * gridSize_cm;
}
Point3 align(const Point3 p) {
return Point3( align(p.x), align(p.y), (p.z) ); // TODO: align z or not?
}
float min(float a, float b, float c, float d) {
if (a < b && a < c && a < d) {return a;}
if (b < a && b < c && b < d) {return b;}
if (c < a && c < b && c < d) {return c;}
if (d < a && d < b && d < c) {return d;}
}
float max(float a, float b, float c, float d) {
if (a > b && a > c && a > d) {return a;}
if (b > a && b > c && b > d) {return b;}
if (c > a && c > b && c > d) {return c;}
if (d > a && d > b && d > c) {return d;}
}
void clamp(float& val, const int min, const int max) {
while (val < min) {val += gridSize();}
while (val > max) {val -= gridSize();}
}
void limit(Floorplan::Quad3& q2, const Floorplan::Quad3& q1) {
const float x1 = min(q1.p1.x, q1.p2.x, q1.p3.x, q1.p4.x);
const float x2 = max(q1.p1.x, q1.p2.x, q1.p3.x, q1.p4.x);
const float y1 = min(q1.p1.y, q1.p2.y, q1.p3.y, q1.p4.y);
const float y2 = max(q1.p1.y, q1.p2.y, q1.p3.y, q1.p4.y);
const float z1 = min(q1.p1.z, q1.p2.z, q1.p3.z, q1.p4.z);
const float z2 = max(q1.p1.z, q1.p2.z, q1.p3.z, q1.p4.z);
// clamp(q2.p1.x, x1, x2); clamp(q2.p2.x, x1, x2); clamp(q2.p3.x, x1, x2); clamp(q2.p4.x, x1, x2);
// clamp(q2.p1.y, y1, y2); clamp(q2.p2.y, y1, y2); clamp(q2.p3.y, y1, y2); clamp(q2.p4.y, y1, y2);
clamp(q2.p1.z, z1, z2); clamp(q2.p2.z, z1, z2); clamp(q2.p3.z, z1, z2); clamp(q2.p4.z, z1, z2);
}
Floorplan::Quad3 align(const Floorplan::Quad3& q) {
Floorplan::Quad3 q2 = Floorplan::Quad3(align(q.p1), align(q.p2), align(q.p3), align(q.p4));
//limit(q2, q);
return q2;
}
static float dot(const Point2 a, const Point2 b) {
return a.x * b.x + a.y * b.y;
}
static float area(const Point2 a, const Point2 b, const Point2 c) {
//Point2 ab = b-a;
//Point2 ac = c-a;
//return 0.5f * std::sqrt(dot(ab,ab)*dot(ac,ac) - dot(ab,ac)*dot(ab,ac));
Point2 p1 = b-a;
Point2 p2 = c-a;
return std::abs(p1.x*p2.y - p2.x*p1.y) * 0.5;
}
static bool bary(Point2 p, Point2 a, Point2 b, Point2 c, float &u, float &v, float &w) {
Point2 v0 = b - a, v1 = c - a, v2 = p - a;
float d00 = dot(v0, v0);
float d01 = dot(v0, v1);
float d11 = dot(v1, v1);
float d20 = dot(v2, v0);
float d21 = dot(v2, v1);
float 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);
}
};
#endif // GRID_FACTORY_HELPER_H

217
grid/factory/v2/Importance.h Normal file
View File

@@ -0,0 +1,217 @@
#ifndef IMPORTANCE_H
#define IMPORTANCE_H
#include "../../Grid.h"
#include "../../../misc/KNN.h"
#include "../../../misc/KNNArray.h"
#include "../../../math/MiniMat2.h"
#include "../../../math/Distributions.h"
class Importance {
private:
static constexpr const char* name = "GridImp";
public:
template <typename T> static void addOutlineNodes(Grid<T>& dst, Grid<T>& src) {
for (const T& n : src) {
if (n.getNumNeighbors() < 8) {
if (!dst.hasNodeFor(n)) {
dst.add(n);
}
}
}
}
/** attach importance-factors to the grid */
template <typename T> static void addImportance(Grid<T>& g, const float z_cm) {
Log::add(name, "adding importance information to all nodes at height " + std::to_string(z_cm));
// get an inverted version of the grid
Grid<T> inv(g.getGridSize_cm());
addOutlineNodes(inv, g);
//GridFactory<T> fac(inv);
//fac.addInverted(g, z_cm);
// sanity check
Assert::isFalse(inv.getNumNodes() == 0, "inverted grid is empty!");
// construct KNN search
KNN<Grid<T>, 3> knn(inv);
// the number of neighbors to use
static constexpr int numNeighbors = 12;
// create list of all doors
std::vector<T> doors;
// process each node
for (T& n1 : g) {
// is the current node a door?
//if (isDoor(n1, neighbors)) {doors.push_back(n1);} // OLD
if (n1.getType() == GridNode::TYPE_DOOR) {doors.push_back(n1);} // NEW!
// favor stairs just like doors
//if (isStaircase(g, n1)) {doors.push_back(n1);} // OLD
if (n1.getType() == GridNode::TYPE_STAIR) {doors.push_back(n1);} // NEW
}
KNNArray<std::vector<T>> knnArrDoors(doors);
KNN<KNNArray<std::vector<T>>, 3> knnDoors(knnArrDoors);
Distribution::Normal<float> favorDoors(0.0f, 0.6f);
// process each node again
for (T& n1 : g) {
// skip nodes on other than the requested floor-level
//if (n1.z_cm != z_cm) {continue;}
// get the 10 nearest neighbors and their distance
size_t indices[numNeighbors];
float squaredDist[numNeighbors];
float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
knn.get(point, numNeighbors, indices, squaredDist);
// get the neighbors
std::vector<T*> neighbors;
for (int i = 0; i < numNeighbors; ++i) {
neighbors.push_back(&inv[indices[i]]);
}
n1.imp = 1.0f;
//if (n1.getType() == GridNode::TYPE_FLOOR) {
// get the distance to the nearest door
const float distToWall_m = Units::cmToM(std::sqrt(squaredDist[0]) + g.getGridSize_cm());
// get the distance to the nearest door
const float distToDoor_m = Units::cmToM(knnDoors.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
n1.imp =
1 +
getWallImportance( distToWall_m ) +
favorDoors.getProbability(distToDoor_m);
//}
//addDoor(n1, neighbors);
// importance for this node (based on the distance from the next door)
//n1.imp += favorDoors.getProbability(dist_m) * 0.30;
//n1.imp = (dist_m < 0.2) ? (1) : (0.5);
}
}
/** is the given node connected to a staircase? */
template <typename T> static bool isStaircase(Grid<T>& g, T& node) {
return node.getType() == GridNode::TYPE_STAIR;
// // if this node has a neighbor with a different z, this is a stair
// for (T& neighbor : g.neighbors(node)) {
// if (neighbor.z_cm != node.z_cm) {return true;}
// }
// return false;
}
/** is the given node (and its inverted neighbors) a door? */
template <typename T> static bool isDoor( T& nSrc, std::vector<T*> neighbors ) {
if (nSrc.getType() != GridNode::TYPE_FLOOR) {return false;}
MiniMat2 m1;
// MiniMat2 m2;
Point3 center = nSrc;
// calculate the centroid of the nSrc's nearest-neighbors
Point3 centroid(0,0,0);
for (const T* n : neighbors) {
centroid = centroid + (Point3)*n;
}
centroid /= neighbors.size();
// if nSrc is too far from the centroid, this does not make sense
if ((centroid-center).length() > 40) {return false;}
// build covariance of the nearest-neighbors
int used = 0;
for (const T* n : neighbors) {
const Point3 d1 = (Point3)*n - centroid;
if (d1.length() > 100) {continue;} // radius search
m1.addSquared(d1.x, d1.y);
// const Point3 d2 = (Point3)*n - center;
// if (d2.length() > 100) {continue;} // radius search
// m2.addSquared(d2.x, d2.y);
++used;
}
// we need at least two points for the covariance
if (used < 6) {return false;}
// check eigenvalues
MiniMat2::EV ev1 = m1.getEigenvalues();
// MiniMat2::EV ev2 = m2.getEigenvalues();
// ensure e1 > e2
if (ev1.e1 < ev1.e2) {std::swap(ev1.e1, ev1.e2);}
// if (ev2.e1 < ev2.e2) {std::swap(ev2.e1, ev2.e2);}
// door?
const float ratio1 = (ev1.e2/ev1.e1);
// const float ratio2 = (ev2.e2/ev2.e1);
// const float ratio3 = std::max(ratio1, ratio2) / std::min(ratio1, ratio2);
return (ratio1 < 0.30 && ratio1 > 0.05) ;
}
/** get the importance of the given node depending on its nearest wall */
static float getWallImportance(float dist_m) {
// avoid sticking too close to walls (unlikely)
static Distribution::Normal<float> avoidWalls(0.0, 0.5);
// favour walking near walls (likely)
static Distribution::Normal<float> stickToWalls(0.9, 0.5);
// favour walking far away (likely)
static Distribution::Normal<float> farAway(2.2, 0.5);
if (dist_m > 2.0) {dist_m = 2.0;}
// overall importance
// return - avoidWalls.getProbability(dist_m) * 0.30 // avoid walls
// + stickToWalls.getProbability(dist_m) * 0.15 // walk near walls
// + farAway.getProbability(dist_m) * 0.15 // walk in the middle
return - avoidWalls.getProbability(dist_m) // avoid walls
//+ stickToWalls.getProbability(dist_m) // walk near walls
//+ farAway.getProbability(dist_m) // walk in the middle
;
}
};
#endif // IMPORTANCE_H

281
grid/factory/v2/Stairs.h Normal file
View File

@@ -0,0 +1,281 @@
#ifndef STAIRS_H
#define STAIRS_H
#include <vector>
#include <set>
#include "../../Grid.h"
#include "Helper.h"
#include "../../../floorplan/v2/Floorplan.h"
template <typename T> class Stairs {
private:
/** the grid to build into */
Grid<T>& grid;
/** calculation helper */
Helper<T> helper;
private:
// struct Entry {
// GridPoint pos;
// std::vector<int> part;
// int idx;
// Entry(const GridPoint pos, int part) : pos(pos) {
// this->part.push_back(part);
// }
// };
/** 2D integer point */
struct XY {
int x;
int y;
XY() : x(-1), y(-1) {;}
XY(const int x, const int y) : x(x), y(y) {;}
bool operator == (const XY& o) const {return (x == o.x) && (y == o.y);}
};
/** 3D integer point */
struct XYZ {
int x;
int y;
int z;
XYZ() : x(-1), y(-1), z(-1) {;}
XYZ(const int x, const int y, const int z) : x(x), y(y), z(z) {;}
bool operator == (const XYZ& o) const {return (x == o.x) && (y == o.y) && (z == o.z);}
};
/** and intermediate grid node */
struct Intermediate {
XY xy;
T node;
int idx;
std::vector<XY> con;
Intermediate(const XY xy, const T node) : xy(xy),node(node), idx(-1) {;}
};
public:
/** ctor */
Stairs(Grid<T>& grid) : grid(grid), helper(grid) {
;
}
void build(const Floorplan::Floor* floor, const Floorplan::Stair* stair) {
const int gs_cm = grid.getGridSize_cm();
std::vector<Intermediate> stairNodes;
// process each part
int partIdx = 0;
const std::vector<Floorplan::StairPart> parts = stair->getParts();
const std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(parts, floor);
for (int i = 0; i < (int)parts.size(); ++i) {
//const Floorplan::StairPart& part = parts[i];
const Floorplan::Quad3& quad = helper.align(quads[i]*100);
const Point3 p1 = quad.p1;
const Point3 p2 = quad.p2;
const Point3 p3 = quad.p3;
const Point3 p4 = quad.p4;
HelperPoly poly; poly.add(p1.xy()); poly.add(p2.xy()); poly.add(p3.xy()); poly.add(p4.xy());
const int x1 = helper.align(poly.bbox_cm.getMin().x); // ALIGNF?
const int y1 = helper.align(poly.bbox_cm.getMin().y);
const int x2 = helper.align(poly.bbox_cm.getMax().x); // ALIGNC?
const int y2 = helper.align(poly.bbox_cm.getMax().y);
std::vector<XYZ> points;
// get all points that are part of the stair-part-polygon
for (int x = x1; x <= x2; x+=gs_cm) {
for (int y = y1; y <= y2; y+=gs_cm) {
if (!poly.contains( Point2(x,y) )) {continue;}
points.push_back(XYZ(x,y,-1));
}
}
// now determine each corresponding z-coordinate
for (XYZ& xy : points) {
const Point2 p(xy.x, xy.y);
float u,v,w;
if (helper.bary(p, p1.xy(), p2.xy(), p3.xy(), u, v, w)) {
xy.z = p1.z*u + p2.z*v + p3.z*w;
} else {
helper.bary(p, p1.xy(), p3.xy(), p4.xy(), u, v, w);
xy.z = p1.z*u + p3.z*v + p4.z*w;
}
}
// stair start, ensure min snaps to the floor
if (partIdx == 0) {
const float minz_cm = minZ(points);
const float maxz_cm = maxZ(points);
const int mz = floor->atHeight * 100;
if (minz_cm != mz) {
for (XYZ& xyz : points) {
const float percent = ((xyz.z - minz_cm) / (maxz_cm - minz_cm));
xyz.z = mz + percent * (maxz_cm - mz);
}
}
}
// stair end, ensure max snaps to the floor
if (partIdx == (int) parts.size() - 1) {
const float minz_cm = minZ(points);
const float maxz_cm = maxZ(points);
const int mz = (floor->atHeight+floor->height) * 100;
if (maxz_cm != mz) {
for (XYZ& xyz : points) {
const float percent = ((xyz.z - minz_cm) / (maxz_cm - minz_cm));
xyz.z = minz_cm + percent * (mz - minz_cm);
}
}
}
for (XYZ& xy : points) {
// construct a grid-node
T node(xy.x, xy.y, xy.z);
// construct an intermediate-node containing all desired connections
Intermediate iNode(XY(xy.x, xy.y), node);
for (int ox = -1; ox <= +1; ++ox) {
for (int oy = -1; oy <= +1; ++oy) {
if (ox == 0 && oy == 0) {continue;}
const int nx = xy.x + ox * gs_cm;
const int ny = xy.y + oy * gs_cm;
iNode.con.push_back( XY(nx, ny) );
}
}
stairNodes.push_back(iNode);
}
++partIdx;
}
// keep a list of all vertices below stairwells and remove them hereafter
std::vector<T*> toDelete;
// add all stair nodes or replace them with already existing ones
for (Intermediate& iNode : stairNodes) {
// nearer than minDiff? -> use existing node
const float minDiff = gs_cm * 0.49;
// nearer than maxDiff? -> delete as this one is unreachable below the stair
const float maxDiff = 150;
// distance between the stair node and the floor above / below
const float zDiff1 = std::abs(iNode.node.z_cm - 100*floor->getStartingZ());
const float zDiff2 = std::abs(iNode.node.z_cm - 100*floor->getEndingZ());
iNode.idx = -1;
// try to connect the stair-node to the floor below
if (zDiff1 < minDiff) {
const T* n2 = grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, 100*floor->getStartingZ()));
if (n2) {
iNode.idx = n2->getIdx();
} else {
iNode.idx = grid.add(iNode.node); // add a new one
helper.connectToNeighbors(grid[iNode.idx]); // and connect to the floor's grid nodes
}
// try to connect the stair-node to the floor above
} else if (zDiff2 < minDiff) {
const T* n2 = grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, 100*floor->getEndingZ()));
if (n2) {
iNode.idx = n2->getIdx();
} else {
iNode.idx = grid.add(iNode.node); // add a new one
helper.connectToNeighbors(grid[iNode.idx]); // and connect to the floor's grid nodes
}
// remove nodes directly below the stair
} else if (zDiff1 < maxDiff) {
T* n2 = (T*) grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, 100*floor->getStartingZ()));
if (n2) {toDelete.push_back(n2);}
// remove nodes directly above the stair
} else if (zDiff2 < maxDiff) {
T* n2 = (T*) grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, 100*floor->getEndingZ()));
if (n2) {toDelete.push_back(n2);}
}
// no matching node found -> add a new one to the grid
if (iNode.idx == -1) {
iNode.idx = grid.add(iNode.node);
}
// add semantic information
grid[iNode.idx].setType(GridNode::TYPE_STAIR);
}
// connect to-be-connected stair-nodes (each node with its [previously configured] neighbors)
for (const Intermediate& t1 : stairNodes) {
for (const XY& connectTo : t1.con) {
for (const Intermediate& t2 : stairNodes) {
if (connectTo == t2.xy) {
T& n1 = grid[t1.idx]; // use the nodes already added to the grid
T& n2 = grid[t2.idx]; // use the nodes already added to the grid
if (n1.hasNeighbor(n2.getIdx())) {continue;}
if (n1.getIdx() == n2.getIdx()) {continue;}
if (n1.getNumNeighbors() >= 10) {continue;}
grid.connectUniDir(n1, n2);
}
}
}
}
// delete all pending nodes and perform a cleanup
for (T* n : toDelete) {grid.remove(*n);}
grid.cleanup();
}
static float minZ(const std::vector<XYZ>& points) {
auto comp = [] (const XYZ& a, const XYZ& b) {return a.z < b.z;};
auto it = std::min_element(points.begin(), points.end(), comp);
return it->z;
}
static float maxZ(const std::vector<XYZ>& points) {
auto comp = [] (const XYZ& a, const XYZ& b) {return a.z < b.z;};
auto it = std::max_element(points.begin(), points.end(), comp);
return it->z;
}
};
#endif // STAIRS_H

15
math/Math.h
View File

@@ -2,6 +2,7 @@
#define K_MATH_MATH_H
#include "../Defines.h"
#include "../geo/Point3.h"
class Math {
@@ -14,6 +15,20 @@ public:
return val;
}
/** get the cross-product of u and v */
static inline Point3 cross(const Point3 u, const Point3 v) {
float x = u.y*v.z - u.z*v.y;
float y = u.z*v.x - u.x*v.z;
float z = u.x*v.y - u.y*v.x;
return Point3(x,y,z);
}
/** get the normal-vector for u and v */
static inline Point3 normal(const Point3 u, const Point3 v) {
const Point3 p = cross(u,v);
return p/p.length();
}
};

2
tests/Tests.h
View File

@@ -9,6 +9,8 @@ static inline std::string getDataFile(const std::string& name) {
return "/mnt/data/workspaces/Indoor/tests/data/" + name;
}
#endif
#endif // TESTS_H

4
tests/api/TestAPI.cpp Normal file
View File

@@ -0,0 +1,4 @@
#ifndef TESTAPI_CPP
#define TESTAPI_CPP
#endif // TESTAPI_CPP

10
tests/floorplan/TestNewFloorplan.cpp
View File

@@ -2,10 +2,17 @@
#include "../Tests.h"
#include "../../floorplan/v2/Floorplan.h"
#include "../../floorplan/v2/FloorplanReader.h"
#include "../../floorplan/v2/FloorplanWriter.h"
#include <cstdlib>
TEST(NewFloorplan, saveAndLoad) {
Floorplan::Reader r;
Floorplan::Writer w;
}
/*
TEST(NewFloorplan, saveAndLoad) {
std::string xml;
@@ -108,6 +115,7 @@ TEST(NewFloorplan, saveAndLoad) {
}
*/

61
tests/geo/TestBBox.cpp Normal file
View File

@@ -0,0 +1,61 @@
#ifdef WITH_TESTS
#include "../Tests.h"
#include "../../geo/BBox2.h"
TEST(BBox, noIntersect) {
BBox2 bb(Point2(1,1), Point2(3,3));
Line2 l1(Point2(0,0), Point2(9,0)); // below
Line2 l2(Point2(0,4), Point2(9,4)); // above
Line2 l3(Point2(0,0), Point2(0,9)); // left
Line2 l4(Point2(4,0), Point2(4,9)); // right
ASSERT_FALSE(bb.intersects(l1));
ASSERT_FALSE(bb.intersects(l2));
ASSERT_FALSE(bb.intersects(l3));
ASSERT_FALSE(bb.intersects(l4));
}
TEST(BBox, noDirectIntersect) {
BBox2 bb(Point2(1,1), Point2(3,3));
Line2 l1(Point2(0,1), Point2(9,1)); // lower
Line2 l2(Point2(0,3), Point2(9,3)); // upper
Line2 l3(Point2(1,0), Point2(1,9)); // left
Line2 l4(Point2(3,0), Point2(3,9)); // right
ASSERT_FALSE(bb.intersects(l1));
ASSERT_FALSE(bb.intersects(l2));
ASSERT_FALSE(bb.intersects(l3));
ASSERT_FALSE(bb.intersects(l4));
}
TEST(BBox, positiveIntersect) {
BBox2 bb(Point2(1,1), Point2(3,3));
Line2 l1(Point2(0,2), Point2(2,2)); // left hit
Line2 l2(Point2(2,2), Point2(4,2)); // right hit
Line2 l3(Point2(2,0), Point2(2,2)); // lower hit
Line2 l4(Point2(2,2), Point2(2,4)); // upper hit
ASSERT_TRUE(bb.intersects(l1));
ASSERT_TRUE(bb.intersects(l2));
ASSERT_TRUE(bb.intersects(l3));
ASSERT_TRUE(bb.intersects(l4));
}
#endif

2
tests/grid/TestWalk.cpp
View File

@@ -130,7 +130,7 @@ TEST(Walk, DISABLED_plot) {
for (int i = 0; i < 5000; ++i) {
pStates.clear();
for (GridWalkState<GP>& state : states) {
state = walk.getDestination(g, state, std::abs(dWalk(gen)), dTurn(gen));
state = walk.getDestination(g, state, std::abs(dWalk(gen)), dTurn(gen), Activity::UNKNOWN);
pStates.add(K::GnuplotPoint3(state.node->x_cm, state.node->y_cm, state.node->z_cm+10));
}
p.gp.draw(p.splot);

4
wifi/estimate/ray2d/Ray2.h Normal file
View File

@@ -0,0 +1,4 @@
#ifndef RAY2_H
#define RAY2_H
#endif // RAY2_H

4
wifi/estimate/ray2d/WiFiRayTrace2D.h Normal file
View File

@@ -0,0 +1,4 @@
#ifndef WIFIRAYTRACE2D_H
#define WIFIRAYTRACE2D_H
#endif // WIFIRAYTRACE2D_H