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

initial version

Browse Source
This commit is contained in:
FrankE
2016-01-21 11:10:55 +01:00
parent 8818a9b216
commit a7dc0cabbb
21 changed files with 1397 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

87
CMakeLists.txt Executable file
View File

@@ -0,0 +1,87 @@
# Usage:
# Create build folder, like RC-build next to RobotControl and WifiScan folder
# CD into build folder and execute 'cmake -DCMAKE_BUILD_TYPE=Debug ../RobotControl'
# make
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
# select build type
SET( CMAKE_BUILD_TYPE "${CMAKE_BUILD_TYPE}" )
PROJECT(graphModel)
IF(NOT CMAKE_BUILD_TYPE)
MESSAGE(STATUS "No build type selected. Default to Debug")
SET(CMAKE_BUILD_TYPE "Debug")
ENDIF()
INCLUDE_DIRECTORIES(
../
/mnt/firma/kunden/HandyGames/
)
FILE(GLOB HEADERS
./*.h
./*/*.h
./*/*/*.h
./*/*/*/*.h
./*/*/*/*/*.h
./*/*/*/*/*/*.h
)
FILE(GLOB SOURCES
./*.cpp
./*/*.cpp
./*/*/*.cpp
/mnt/firma/kunden/HandyGames/KLib/inc/tinyxml/*.cpp
)
if(${CMAKE_GENERATOR} MATCHES "Visual Studio")
SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} /D_X86_ /D_USE_MATH_DEFINES")
SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /Zi /Oi /GL /Ot /Ox /D_X86_ /D_USE_MATH_DEFINES")
SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /DEBUG")
SET(CMAKE_EXE_LINKER_FLAGS_RELEASE "${CMAKE_EXE_LINKER_FLAGS_RELEASE} /LTCG /INCREMENTAL:NO")
set(CMAKE_CONFIGURATION_TYPES Release Debug)
else()
# system specific compiler flags
ADD_DEFINITIONS(
-std=gnu++11
-Wall
-Werror=return-type
-Wextra
-g
-O0
-DWITH_TESTS
)
endif()
# build a binary file
ADD_EXECUTABLE(
${PROJECT_NAME}
${HEADERS}
${SOURCES}
)
# needed external libraries
TARGET_LINK_LIBRARIES(
${PROJECT_NAME}
gtest
pthread
)
SET(CMAKE_C_COMPILER ${CMAKE_CXX_COMPILER})

23
Exception.h Executable file
View File

@@ -0,0 +1,23 @@
#ifndef EXCEPTION_H
#define EXCEPTION_H
#include <exception>
#include <string>
class Exception : public std::exception {
private:
/** the exception message */
std::string str;
public:
/** ctor */
Exception(const std::string& str) : str(str) {;}
const char* what() const throw() {return str.c_str();}
};
#endif // EXCEPTION_H

49
floorplan/Floor.h Executable file
View File

@@ -0,0 +1,49 @@
#ifndef FLOOR_H
#define FLOOR_H
#include <vector>
#include "../geo/Line2D.h"
/**
* represents one floor by describing all contained obstacles
*/
class Floor {
private:
/** all obstacles within the floor */
std::vector<Line2D> lines;
/** total width of the floor (in meter) */
double width_cm;
/** total depth of the floor (in meter) */
double depth_cm;
public:
/** ctor */
Floor(const float width_cm, const float depth_cm) : width_cm(width_cm), depth_cm(depth_cm) {;}
/** get all obstacles */
const std::vector<Line2D>& getObstacles() const {
return lines;
}
/** add a new obstacle to this floor */
void addObstacle(const Line2D& l ) {
lines.push_back(l);
}
/** get the floorplan's total width */
float getWidth_cm() const {return width_cm;}
/** get the floorplan's total depth */
float getDepth_cm() const {return depth_cm;}
};
#endif // FLOOR_H

138
floorplan/FloorplanFactorySVG.h Executable file
View File

@@ -0,0 +1,138 @@
#ifndef FLOORPLANFACTORYSVG_H
#define FLOORPLANFACTORYSVG_H
#include "Floor.h"
#include <string>
#include "../Exception.h"
#include <KLib/gfx/svg/SVGLoader.h>
/**
* load floorplans from SVG files
*/
class FloorplanFactorySVG {
/**
* helper class for SVG floorplans.
*
* converts between the SVG's scale and real-world scale
*/
class SVGScaler {
private:
/** the scaling factor to apply to the svg data */
const double scalingFactor;
public:
/** ctor */
SVGScaler(const double scalingFactor) : scalingFactor(scalingFactor) {
;
}
/** scale (x, y) into (_x, _y) */
void scale(const double x, const double y, double& _x, double& _y) const {
_x = x * scalingFactor;
_y = y * scalingFactor;
}
/** scale the given point into a new output point */
K::Point scale(const K::Point p) const {
K::Point ret;
scale (p.x, p.y, ret.x, ret.y);
return ret;
}
/** scale the given line into a new output line */
Line2D scale(const K::Line l) const {
Line2D ret;
scale (l.p1.x, l.p1.y, ret.p1.x, ret.p1.y);
scale (l.p2.x, l.p2.y, ret.p2.x, ret.p2.y);
return ret;
}
};
private:
K::SVGFile svg;
SVGScaler scaler;
double width;
double depth;
public:
/** ctor with svg filename and scaling factor */
FloorplanFactorySVG(const std::string& file, const float scaling) : scaler(scaling) {
// load the SVG
K::SVGLoader::load(K::File(file), &svg);
scaler.scale(svg.getWidth(), svg.getHeight(), width, depth);
}
/** get the width of the building */
float getWidth() const {return width;}
/** get the depth of the building */
float getDepth() const {return depth;}
/** get the floor identified by the given layer name */
Floor getFloor(const std::string layerName) {
// get the requested SVG layer
K::SVGComposite* sc = svg.getLayers();
K::SVGLayer* layer = sc->getContainedLayerNamed(layerName);
if (!layer) {throw Exception("SVG has no layer named '" + layerName + "'");}
// create and parse the new floor
Floor floor(width, depth);
load(layer, floor);
return floor;
}
private:
/** recursive loading/parsing of nested SVG elements */
void load(K::SVGElement* el, Floor& floor) {
switch (el->getType()) {
case SVGElementType::COMPOSITE: {
for (K::SVGElement* sub : ((K::SVGComposite*)el)->getChilds()) {
load(sub, floor);
}
break;
}
case SVGElementType::LAYER: {
K::SVGLayer* layer = (K::SVGLayer*) el;
for (K::SVGElement* sub : layer->getChilds()) {
load(sub, floor);
}
break;
}
case SVGElementType::PATH: {
for (const K::Line& line : ((K::SVGPath*)el)->getLines()) {
floor.addObstacle( scaler.scale(line) );
}
break;
}
case SVGElementType::TEXT: {
break;
}
default:
throw "should not happen!";
}
}
};
#endif // FLOORPLANFACTORYSVG_H

33
geo/Angle.h Executable file
View File

@@ -0,0 +1,33 @@
#ifndef ANGLE_H
#define ANGLE_H
#include <cmath>
class Angle {
public:
/** get the radians from (x1,y1) to (x2,y2) */
static float getRAD(const float x1, const float y1, const float x2, const float y2) {
const float tmp = std::atan2(y2-y1, x2-x1);
return (tmp < 0) ? (tmp + 2*M_PI) : (tmp);
}
/** get the degrees from (x1,y1) to (x2,y2) */
static float getDEG(const float x1, const float y1, const float x2, const float y2) {
return radToDeg(getRAD(x1,y1,x2,y2));
}
/** convert degrees to radians */
static float degToRad(const float deg) {
return deg / 180 * M_PI;
}
/** convert radians to degrees */
static float radToDeg(const float rad) {
return rad * 180 / M_PI;
}
};
#endif // ANGLE_H

21
geo/Line2D.h Executable file
View File

@@ -0,0 +1,21 @@
#ifndef LINE2D_H
#define LINE2D_H
#include <KLib/geo/Line.h>
class Line2D : public K::Line {
public:
Line2D() : K::Line() {;}
Line2D(const float x1, const float y1, const float x2, const float y2) : K::Line(x1,y1,x2,y2) {;}
bool getSegmentIntersection(const Line& other) const {
static K::Point p;
return K::Line::getSegmentIntersection(other, p);
}
};
#endif // LINE2D_H

16
geo/Units.h Executable file
View File

@@ -0,0 +1,16 @@
#ifndef UNITS_H
#define UNITS_H
class Units {
public:
/** convert from m to cm */
template <typename T> static inline T mToCm(const T m) {return m * 100;}
/** convert from cm to m */
template <typename T> static inline T cmToM(const T cm) {return cm / 100;}
};
#endif // UNITS_H

141
grid/Grid.h Executable file
View File

@@ -0,0 +1,141 @@
#ifndef GRID_H
#define GRID_H
#include <vector>
#include <unordered_map>
#include "../Exception.h"
#include "GridPoint.h"
#include "GridNode.h"
#include <iostream>
/**
* grid of the given grid-size, storing some value which
* extends GridPoint
*/
template <int gridSize_cm, typename T> class Grid {
typedef uint64_t UID;
private:
/** all elements (nodes) within the grid */
std::vector<T> nodes;
/** UID -> index mapping */
std::unordered_map<UID, int> hashes;
public:
/** ctor */
Grid() {
static_assert((sizeof(T::_idx) > 0), "T must inherit from GridNode!");
static_assert((sizeof(T::x_cm) > 0), "T must inherit from GridPoint!");
}
/** no-copy */
Grid(const Grid& o) = delete;
/** no-assign */
void operator = (const Grid& o) = delete;
/**
* add the given element to the grid.
* returns the index of the element within the internal data-structure
* @param elem the element to add
*/
int add(const T& elem) {
assertAligned(elem); // assert that the to-be-added element is aligned to the grid
const int idx = nodes.size(); // next free index
const UID uid = getUID(elem); // get the UID for this new element
nodes.push_back(elem); // add it to the grid
nodes.back()._idx = idx;
hashes[uid] = idx; // add an UID->index lookup
return idx; // done
}
/**
* connect (bi-directional) the two provided nodes
* @param idx1 index of the first element
* @param idx2 index of the second element
*/
void connect(const int idx1, const int idx2) {
T& n1 = nodes[idx1]; // get the first element
T& n2 = nodes[idx2]; // get the second element
connect(n1, n2);
}
/**
* connect (bi-directional) the two provided nodes
* @param n1 the first node
* @param n2 the second node
*/
void connect(T& n1, T& n2) {
n1._neighbors[n1._numNeighbors] = n2._idx; // add them both as neighbors
n2._neighbors[n2._numNeighbors] = n1._idx;
++n1._numNeighbors; // increment the neighbor-counter
++n2._numNeighbors;
}
/** get the number of contained nodes */
int getNumNodes() const {
return nodes.size();
}
/** get the number of neighbors for the given element */
int getNumNeighbors(const int idx) const {
return getNumNeighbors(nodes[idx]);
}
/** get the number of neighbors for the given element */
int getNumNeighbors(const T& e) const {
return e._numNeighbors;
}
/** get the center-node the given Point belongs to */
const T& getNodeFor(const GridPoint& p) {
const UID uid = getUID(p);
if (hashes.find(uid) == hashes.end()) {throw Exception("element not found!");}
return nodes[hashes[uid]];
}
/** get the BBox for the given node */
GridNodeBBox getBBox(const int idx) const {
return getBBox(nodes[idx]);
}
/** get the BBox for the given node */
GridNodeBBox getBBox(const T& node) const {
return GridNodeBBox(node, gridSize_cm);
}
/**
* get an UID for the given point.
* this works only for aligned points.
*
*/
UID getUID(const GridPoint& p) const {
const uint64_t x = std::round(p.x_cm / gridSize_cm);
const uint64_t y = std::round(p.y_cm / gridSize_cm);
const uint64_t z = std::round(p.z_cm / gridSize_cm);
return (z << 40) | (y << 20) | (x << 0);
}
/** array access */
const T& operator [] (const int idx) const {
return nodes[idx];
}
private:
/** asssert that the given element is aligned to the grid */
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!");}
}
};
#endif // GRID_H

34
grid/GridNode.h Executable file
View File

@@ -0,0 +1,34 @@
#ifndef GRIDNODE_H
#define GRIDNODE_H
#include "GridNodeBBox.h"
#include "GridPoint.h"
/**
* INTERNAL DATASTRUCTURE
* this data-structure is internally used by the Grid
* to store additional information for each node besides
* the user's requested data-structure
*/
class GridNode {
template<int, typename> friend class Grid;
/** INTERNAL: array-index */
int _idx = -1;
/** INTERNAL: store neighbors (via index) */
int _numNeighbors = 0;
/** INTERNAL: number of neighbors */
int _neighbors[10] = {};
public:
GridNode() {;}
};
#endif // GRIDNODE_H

46
grid/GridNodeBBox.h Executable file
View File

@@ -0,0 +1,46 @@
#ifndef GRIDNODEBBOX_H
#define GRIDNODEBBOX_H
#include "GridPoint.h"
#include "../geo/Line2D.h"
/**
* describes the 2D (one floor)
* bounding-box for one node on the grid
*/
struct GridNodeBBox {
/** smaller half */
float x1_cm, y1_cm;
/** larger half */
float x2_cm, y2_cm;
/** ctor */
GridNodeBBox(const GridPoint& center, const int gridSize_cm) {
x1_cm = center.x_cm - gridSize_cm/2; // smaller half
y1_cm = center.y_cm - gridSize_cm/2;
x2_cm = center.x_cm + gridSize_cm/2; // larger half
y2_cm = center.y_cm + gridSize_cm/2;
}
/** equal? */
bool operator == (const GridNodeBBox& o) const {
return (x1_cm == o.x1_cm) && (y1_cm == o.y1_cm) && (x2_cm == o.x2_cm) && (y2_cm == o.y2_cm);
}
/** does the BBox intersect with the given line? */
bool intersects (const Line2D& l) const {
Line2D l1(x1_cm, y1_cm, x2_cm, y1_cm); // upper
Line2D l2(x1_cm, y2_cm, x2_cm, y2_cm); // lower
Line2D l3(x1_cm, y1_cm, x1_cm, y2_cm); // left
Line2D l4(x2_cm, y1_cm, x2_cm, y2_cm); // right
return l.getSegmentIntersection(l1) ||
l.getSegmentIntersection(l2) ||
l.getSegmentIntersection(l3) ||
l.getSegmentIntersection(l4);
}
};
#endif // GRIDNODEBBOX_H

41
grid/GridPoint.h Executable file
View File

@@ -0,0 +1,41 @@
#ifndef GRIDPOINT_H
#define GRIDPOINT_H
#include <cmath>
struct GridPoint {
/** x-position (in centimeter) */
const float x_cm;
/** y-position (in centimeter) */
const float y_cm;
/** z-position (in centimeter) */
const float z_cm;
/** empty ctor */
GridPoint() : x_cm(0), y_cm(0), z_cm(0) {;}
/** ctor */
GridPoint(const float x_cm, const float y_cm, const float z_cm) : x_cm(x_cm), y_cm(y_cm), z_cm(z_cm) {;}
/** 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 {
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;
}
};
#endif // GRIDPOINT_H

55
grid/factory/GridFactory.h Executable file
View File

@@ -0,0 +1,55 @@
#ifndef GRIDFACTORY_H
#define GRIDFACTORY_H
#include <string>
#include "../../floorplan/Floor.h"
#include "../../geo/Units.h"
#include "../GridNodeBBox.h"
#include "../Grid.h"
template <int gridSize_cm, typename T> class GridFactory {
private:
Grid<gridSize_cm, T>& grid;
public:
/** ctor with the grid to fill */
GridFactory(Grid<gridSize_cm, T>& grid) : grid(grid) {;}
/** add the given floor at the provided height (in cm) */
void addFloor(const Floor& floor, const float z_cm) {
for(int x_cm = 0; x_cm < floor.getWidth_cm(); x_cm += gridSize_cm) {
for (int y_cm = 0; y_cm < floor.getDepth_cm(); y_cm += gridSize_cm) {
// check intersection with the floorplan
GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), gridSize_cm);
if (intersects(bbox, floor)) {continue;}
// add to the grid
grid.add(T(x_cm, y_cm, z_cm));
}
}
int i = 0;
}
private:
/** does the bbox intersect with any of the floor's walls? */
bool intersects(const GridNodeBBox& bbox, const Floor& floor) {
for (const Line2D l : floor.getObstacles()) {
if (bbox.intersects(l)) {return true;}
}
return false;
}
};
#endif // GRIDFACTORY_H

25
main.cpp Executable file
View File

@@ -0,0 +1,25 @@
#include "grid/Grid.h"
#include "grid/GridPoint.h"
class Test : public GridPoint {
};
#include "tests/Tests.h"
int main(int argc, char** argv) {
#ifdef WITH_TESTS
::testing::InitGoogleTest(&argc, argv);
//::testing::GTEST_FLAG(filter) = "*first*";
return RUN_ALL_TESTS();
#endif
return 0;
}

14
tests/Tests.h Executable file
View File

@@ -0,0 +1,14 @@
#ifndef TESTS_H
#define TESTS_H
#ifdef WITH_TESTS
#include <gtest/gtest.h>
static inline std::string getDataFile(const std::string& name) {
return "/mnt/firma/kunden/indoor/tests/data/" + name;
}
#endif
#endif // TESTS_H

402
tests/data/test.svg Executable file
View File

@@ -0,0 +1,402 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<!-- Created with Inkscape (http://www.inkscape.org/) -->
<svg
xmlns:dc="http://purl.org/dc/elements/1.1/"
xmlns:cc="http://creativecommons.org/ns#"
xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
xmlns:svg="http://www.w3.org/2000/svg"
xmlns="http://www.w3.org/2000/svg"
xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
width="512"
height="512"
id="svg3269"
version="1.1"
inkscape:version="0.48.4 r9939"
sodipodi:docname="test.svg">
<defs
id="defs3271" />
<sodipodi:namedview
id="base"
pagecolor="#ffffff"
bordercolor="#666666"
borderopacity="1.0"
inkscape:pageopacity="0.0"
inkscape:pageshadow="2"
inkscape:zoom="0.98994949"
inkscape:cx="145.84558"
inkscape:cy="284.04468"
inkscape:document-units="px"
inkscape:current-layer="layer16"
showgrid="false"
inkscape:window-width="1366"
inkscape:window-height="672"
inkscape:window-x="0"
inkscape:window-y="33"
inkscape:window-maximized="1" />
<metadata
id="metadata3274">
<rdf:RDF>
<cc:Work
rdf:about="">
<dc:format>image/svg+xml</dc:format>
<dc:type
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
<dc:title />
</cc:Work>
</rdf:RDF>
</metadata>
<g
inkscape:groupmode="layer"
id="layer2"
inkscape:label="1"
style="display:inline">
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 204.28571,352.71429 141.42858,1.42857"
id="path4093"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 339.28571,372.71429 -0.71428,-71.42858"
id="path4095"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 329.28571,302.71429 84.28572,75.71428"
id="path4097"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 423.57143,363.42857 -67.85714,59.28572"
id="path4099"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 362.85714,441.28571 -5.71428,-45.71428"
id="path4101"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 365.71429,397 222.85714,407"
id="path4103"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 235.71429,429.85714 222.14286,333.42857"
id="path4105"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 195,109.14286 320,237.71429"
id="path4107"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 297.14286,237.71429 424.66569,149.83612"
id="path4109"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 417.24799,166.28571 370.71429,54.142857"
id="path4111"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 390.71429,49.857142 345,151.28571"
id="path4113"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 366.0522,143.55112 183.57143,119.85714"
id="path4115"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 40.714286,63.428571 74.285714,190.57143"
id="path4117"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 58.571429,184.14286 130.71429,92.714285"
id="path4119"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 146.42857,109.85714 35.714286,69.142857"
id="path4121"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 26.428571,295.57143 133.57143,202.71429"
id="path4123"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 118.57143,194.85714 50,87.85715"
id="path4125"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 150.71429,289.85714 75.71428,-50"
id="path4127"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 212.85714,238.42857 35,42.14286"
id="path4129"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 255,269.85714 155,326.28571"
id="path4131"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 166.42857,309.85714 143.57143,452"
id="path4133"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 165.71429,447 59.683107,422.43945"
id="path4135"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 77.591012,449.2654 32.857143,277"
id="path4137"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 6.0609153,9.954185 10.101525,502.90863"
id="path4139"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -7.0710678,495.83756 526.2894778,2.0203"
id="path4141"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 507.09658,520.08122 503.05597,-4.1879507"
id="path4143"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 527.29963,10.964338 -17.172593,16.0151"
id="path4145"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 361.63461,259.46186 102.02541,59.599"
id="path3780"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 353.55339,277.64461 509.11688,207.94408"
id="path3782"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 445.47727,326.13193 498.00521,202.89332"
id="path3784"
inkscape:connector-curvature="0" />
</g>
<g
inkscape:groupmode="layer"
id="layer15"
inkscape:label="2"
style="display:inline">
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 29.294424,91.77654 139.40105,29.147082"
id="path13690"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 117.1777,22.076014 43.43656,83.842666"
id="path13692"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 27.274119,80.664862 180.81731,98.847608"
id="path13694"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 33.335034,129.15218 164.65487,159.45676"
id="path13696"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 151.52288,143.29432 36.36549,108.08632"
id="path13698"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 168.69548,249.36034 119.198,-19.1929"
id="path13700"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 271.73104,224.10652 19.19289,66.67007"
id="path13702"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 318.19805,274.61415 127.27922,335.2233"
id="path13704"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 144.45181,347.34513 38.385797,124.10142"
id="path13706"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 248.49753,73.593794 279.81226,185.72073"
id="path13708"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 257.5889,183.70042 406.08132,149.35524"
id="path13710"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 240.41631,79.654709 375.77675,59.451658"
id="path13712"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 363.65492,48.33998 41.41625,115.15739"
id="path13714"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 366.68538,465.53298 446.48743,354.4162"
id="path13716"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 425.27422,352.3959 48.48733,91.92388"
id="path13718"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 359.61431,444.31978 484.87322,434.21825"
id="path13720"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 38.385797,387.75124 161.62441,483.71573"
id="path13722"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 43.43656,395.83246 136.37059,1.01015"
id="path13724"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 159.6041,383.71063 61.61931,62.62945"
id="path13726"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 148.49242,487.75634 84.85282,-57.5787"
id="path13728"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 269.71073,361.48727 20.20305,51.51778"
id="path13730"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 261.62951,371.5888 74.75129,-3.03046"
id="path13732"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 273.75134,420.07612 60.60915,-69.70053"
id="path13734"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 383.85797,231.17759 473.76155,154.406"
id="path13736"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 458.60926,156.4263 33.33503,83.84266"
id="path13738"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 445.47727,292.7969 493.9646,223.09637"
id="path13740"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 384.86812,217.03546 79.80205,80.8122"
id="path13742"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 433.35544,23.086167 59.599,2.020305"
id="path13744"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 484.87322,21.065862 -3.03045,45.456864"
id="path13746"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 438.40621,17.025251 58.58884,49.497475"
id="path13748"
inkscape:connector-curvature="0" />
</g>
<g
inkscape:groupmode="layer"
id="layer16"
inkscape:label="1_2">
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 406.08132,60.461811 443.45697,135.2131 476.792,119.05066 439.41636,51.370438 406.08132,60.461811"
id="path13751"
inkscape:connector-curvature="0"
sodipodi:nodetypes="ccccc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 243.44676,440.27917 8.08122,42.42641 80.81221,-2.02031 1.01015,-40.4061 -89.90358,0"
id="path13753"
inkscape:connector-curvature="0"
sodipodi:nodetypes="ccccc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 185.86807,167.53798 12.12183,51.51778 41.41625,-10.10152 -14.14213,-51.51778 -39.39595,10.10152"
id="path13755"
inkscape:connector-curvature="0"
sodipodi:nodetypes="ccccc" />
</g>
<g
inkscape:groupmode="layer"
id="layer1"
inkscape:label="path"
style="display:none">
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 23.233509,328.15223 5.498667,29.84991 5.04738,38.94466 5.726216,27.35837 6.778541,32.38611 10.28423,17.93307 19.192898,6.06092 21.213204,-2.0203 21.213205,1.01015 26.26396,0 24.24367,-9.09137 15.15228,-16.16245 6.06092,-22.22335 -2.02031,-26.26397 0,-25.25381 3.03046,-27.27412 6.06092,-18.18275 20.20305,-19.19289 28.28427,-11.11168 25.25381,-6.06092 19.1929,-14.14213 -4.04061,-28.28427 -13.13198,-27.27412 -22.22336,-11.11168 -24.24366,-6.06092 -24.24366,-12.12183 -20.20305,-13.13198 -11.11168,-22.22336 -3.03046,-25.25381 8.08122,-32.32488 13.13199,-23.23351 25.25381,-16.162441 26.26397,-14.142136 27.27412,-7.071068 27.27411,-4.04061 35.35534,-5.050763 25.25382,-7.071068 27.27412,-2.020305 20.20305,13.131983 23.23351,16.162441 27.27411,12.121831 22.22336,15.152288 5.05076,30.304578 -6.43065,37.00542 -18.34108,26.88819 -31.1166,16.74083 -31.95699,18.46513 -36.0808,17.41488 -23.9052,28.67296 10.79173,29.03585 29.28066,13.55625 26.653,8.13847 27.19771,14.80725 13.80025,29.23264 -4.56452,29.55883 -18.1487,24.13104 -18.94412,26.82052 -22.51132,18.63377 -32.37271,9.74766 -30.9338,4.32323 -31.41691,-1.63983"
id="path3778"
inkscape:connector-curvature="0"
sodipodi:nodetypes="ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc" />
</g>
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 18 KiB

27
tests/floorplan/TestFloorplanFactorySVG.cpp Executable file
View File

@@ -0,0 +1,27 @@
#ifdef WITH_TESTS
//#include "../Tests.h"
//#include "../../floorplan/FloorplanFactorySVG.h"
//#include <cstdlib>
//TEST(FloorplanFactorySVG, parse) {
// const std::string filename = getDataFile("test.svg");
// FloorplanFactorySVG factory(filename, 1.0);
// Floor f1 = factory.getFloor("1");
// ASSERT_EQ(30, f1.getObstacles().size());
// Floor f2 = factory.getFloor("2");
// ASSERT_EQ(30, f2.getObstacles().size());
// Floor f3 = factory.getFloor("1_2");
// ASSERT_EQ(12, f3.getObstacles().size());
//}
#endif

21
tests/geo/TestAngle.cpp Executable file
View File

@@ -0,0 +1,21 @@
#ifdef WITH_TESTS
#include "../Tests.h"
#include "../../geo/Angle.h"
TEST(Angle, calc) {
ASSERT_EQ(0, Angle::getDEG(0,0, +1,0)); // to the right
ASSERT_EQ(90, Angle::getDEG(0,0, 0,+1)); // upwards
ASSERT_EQ(180, Angle::getDEG(0,0, -1,0)); // to the left
ASSERT_EQ(270, Angle::getDEG(0,0, 0,-1)); // downwards
ASSERT_EQ(45, Angle::getDEG(0,0, +1,+1)); // to the upper right
ASSERT_EQ(135, Angle::getDEG(0,0, -1,+1)); // to the upper left
ASSERT_EQ(225, Angle::getDEG(0,0, -1,-1)); // to the lower left
ASSERT_EQ(315, Angle::getDEG(0,0, +1,-1)); // to the lower right
}
#endif

47
tests/grid/TestBBox.cpp Executable file
View File

@@ -0,0 +1,47 @@
#ifdef WITH_TESTS
#include "../Tests.h"
#include "../../grid/GridNodeBBox.h"
TEST(BBox, equals) {
GridNodeBBox bb1(GridPoint(2,2,2), 2);
GridNodeBBox bb2(GridPoint(2,2,2), 2);
GridNodeBBox bb3(GridPoint(3,2,2), 2);
GridNodeBBox bb4(GridPoint(2,3,2), 2);
GridNodeBBox bb5(GridPoint(2,2,3), 2);
GridNodeBBox bb6(GridPoint(2,2,2), 4);
ASSERT_TRUE(bb1 == bb2);
ASSERT_TRUE(bb1 == bb5); // z doesnt matter for the bbox
ASSERT_FALSE(bb1 == bb3);
ASSERT_FALSE(bb1 == bb4);
ASSERT_FALSE(bb1 == bb6);
}
TEST(BBox, intersect) {
GridNodeBBox bb1(GridPoint(20,20,20), 20);
// left
ASSERT_FALSE(bb1.intersects( Line2D(9, -999, 9, +999) ));
ASSERT_TRUE (bb1.intersects( Line2D(11, -999, 11, +999) ));
// right
ASSERT_TRUE (bb1.intersects( Line2D(29, -999, 29, +999) ));
ASSERT_FALSE(bb1.intersects( Line2D(31, -999, 31, +999) ));
// top
ASSERT_FALSE(bb1.intersects( Line2D(-999, 9, +999, 9) ));
ASSERT_TRUE (bb1.intersects( Line2D(-999, 11, +999, 11) ));
// bottom
ASSERT_TRUE (bb1.intersects( Line2D(-999, 29, +999, 29) ));
ASSERT_FALSE(bb1.intersects( Line2D(-999, 31, +999, 31) ));
}
#endif

112
tests/grid/TestGrid.cpp Executable file
View File

@@ -0,0 +1,112 @@
#ifdef WITH_TESTS
#include "../Tests.h"
#include "../../grid/Grid.h"
#include "../../grid/GridPoint.h"
#include "../../grid/GridNode.h"
class GP : public GridNode, public GridPoint {
public:
GP() : GridNode(), GridPoint() {;}
GP(int x, int y, int z) : GridNode(), GridPoint(x,y,z) {;}
};
TEST(Grid, add) {
Grid<20, GP> grid;
ASSERT_EQ(0, grid.add(GP()));
ASSERT_EQ(1, grid.add(GP()));
ASSERT_EQ(2, grid.add(GP()));
ASSERT_EQ(3, grid.add(GP()));
// not aligned? -> error
ASSERT_THROW(grid.add(GP(10,10,10)), std::exception);
ASSERT_THROW(grid.add(GP(10,20,20)), std::exception);
ASSERT_THROW(grid.add(GP(20,10,20)), std::exception);
ASSERT_THROW(grid.add(GP(20,20,10)), std::exception);
ASSERT_EQ(4, grid.add(GP(20,20,20)));
// access
ASSERT_EQ(GP(20,20,20), grid[4]);
}
TEST(Grid, BBox) {
Grid<20, GP> grid;
int idx = grid.add(GP(40,40,40));
ASSERT_EQ(30, grid.getBBox(idx).x1_cm);
ASSERT_EQ(50, grid.getBBox(idx).x2_cm);
ASSERT_EQ(30, grid.getBBox(idx).y1_cm);
ASSERT_EQ(50, grid.getBBox(idx).y2_cm);
}
TEST(Grid, neighbors) {
Grid<1, GP> grid;
int idx1 = grid.add(GP( 0, 0, 0));
int idx2 = grid.add(GP( 0, 1, 0));
int idx3 = grid.add(GP( 0,-1, 0));
int idx4 = grid.add(GP( 1, 0, 0));
int idx5 = grid.add(GP(-1, 0, 0));
grid.connect(idx1, idx2);
grid.connect(idx1, idx3);
grid.connect(idx1, idx4);
grid.connect(idx1, idx5);
ASSERT_EQ(4, grid.getNumNeighbors(idx1));
ASSERT_EQ(1, grid.getNumNeighbors(idx2));
ASSERT_EQ(1, grid.getNumNeighbors(idx3));
ASSERT_EQ(1, grid.getNumNeighbors(idx4));
ASSERT_EQ(1, grid.getNumNeighbors(idx5));
}
TEST(Grid, uid) {
Grid<20, GP> grid;
GP gp(20,40,60);
uint64_t uid = grid.getUID(gp);
const int mask = (1 << 20) - 1; // 20-bit mask
ASSERT_EQ(3, uid >> 40 & mask); // x
ASSERT_EQ(2, uid >> 20 & mask); // y
ASSERT_EQ(1, uid >> 0 & mask); // z
}
TEST(Grid, nearest) {
Grid<20, GP> grid;
GP c1(20,20,20);
GP c2(40,40,40);
grid.add(c1);
grid.add(c2);
ASSERT_EQ(c1, grid.getNodeFor(GP(15,20,20)));
ASSERT_EQ(c1, grid.getNodeFor(GP(20,15,20)));
ASSERT_EQ(c1, grid.getNodeFor(GP(20,20,15)));
ASSERT_EQ(c1, grid.getNodeFor(GP(25,20,20)));
ASSERT_EQ(c1, grid.getNodeFor(GP(20,25,20)));
ASSERT_EQ(c1, grid.getNodeFor(GP(20,20,25)));
ASSERT_EQ(c2, grid.getNodeFor(GP(35,40,40)));
ASSERT_EQ(c2, grid.getNodeFor(GP(40,35,40)));
ASSERT_EQ(c2, grid.getNodeFor(GP(40,40,35)));
ASSERT_EQ(c2, grid.getNodeFor(GP(45,40,40)));
ASSERT_EQ(c2, grid.getNodeFor(GP(40,45,40)));
ASSERT_EQ(c2, grid.getNodeFor(GP(40,40,45)));
}
#endif

45
tests/grid/TestGridFactory.cpp Executable file
View File

@@ -0,0 +1,45 @@
#ifdef WITH_TESTS
#include "../Tests.h"
#include "../../grid/factory/GridFactory.h"
#include "../../floorplan/FloorplanFactorySVG.h"
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
class GP : public GridNode, public GridPoint {
public:
GP() : GridNode(), GridPoint() {;}
GP(int x, int y, int z) : GridNode(), GridPoint(x,y,z) {;}
};
TEST(GridFactory, create) {
Grid<20, GP> g;
GridFactory<20, GP> gf(g);
FloorplanFactorySVG fpf(getDataFile("test.svg"), 2);
Floor f1 = fpf.getFloor("1");
gf.addFloor(f1, 0);
K::Gnuplot gp;
K::GnuplotSplot splot;
K::GnuplotSplotElementPoints points;
for (int i = 0; i < g.getNumNodes(); ++i) {
const GP& node = g[i];
points.add(K::GnuplotPoint3(node.x_cm, node.y_cm, node.z_cm));
}
splot.add(&points);
gp.draw(splot);
gp.flush();
sleep(10);
}
#endif

20
tests/grid/TestGridPoint.cpp Executable file
View File

@@ -0,0 +1,20 @@
#ifdef WITH_TESTS
#include "../Tests.h"
#include "../../grid/GridPoint.h"
TEST(GridPoint, distance) {
GridPoint p1(0,0,0);
GridPoint p2(100,0,0);
GridPoint p3(0,100,0);
GridPoint p4(0,0,100);
ASSERT_EQ(1, p1.getDistanceInMeter(p2));
ASSERT_EQ(1, p1.getDistanceInMeter(p3));
ASSERT_EQ(1, p1.getDistanceInMeter(p4));
}
#endif