From a7dc0cabbbe2bc94cc5df93fa07f88f10b9016ff Mon Sep 17 00:00:00 2001 From: kazu Date: Thu, 21 Jan 2016 11:10:55 +0100 Subject: [PATCH] initial version --- CMakeLists.txt | 87 +++++ Exception.h | 23 ++ floorplan/Floor.h | 49 +++ floorplan/FloorplanFactorySVG.h | 138 +++++++ geo/Angle.h | 33 ++ geo/Line2D.h | 21 + geo/Units.h | 16 + grid/Grid.h | 141 +++++++ grid/GridNode.h | 34 ++ grid/GridNodeBBox.h | 46 +++ grid/GridPoint.h | 41 ++ grid/factory/GridFactory.h | 55 +++ main.cpp | 25 ++ tests/Tests.h | 14 + tests/data/test.svg | 402 ++++++++++++++++++++ tests/floorplan/TestFloorplanFactorySVG.cpp | 27 ++ tests/geo/TestAngle.cpp | 21 + tests/grid/TestBBox.cpp | 47 +++ tests/grid/TestGrid.cpp | 112 ++++++ tests/grid/TestGridFactory.cpp | 45 +++ tests/grid/TestGridPoint.cpp | 20 + 21 files changed, 1397 insertions(+) create mode 100755 CMakeLists.txt create mode 100755 Exception.h create mode 100755 floorplan/Floor.h create mode 100755 floorplan/FloorplanFactorySVG.h create mode 100755 geo/Angle.h create mode 100755 geo/Line2D.h create mode 100755 geo/Units.h create mode 100755 grid/Grid.h create mode 100755 grid/GridNode.h create mode 100755 grid/GridNodeBBox.h create mode 100755 grid/GridPoint.h create mode 100755 grid/factory/GridFactory.h create mode 100755 main.cpp create mode 100755 tests/Tests.h create mode 100755 tests/data/test.svg create mode 100755 tests/floorplan/TestFloorplanFactorySVG.cpp create mode 100755 tests/geo/TestAngle.cpp create mode 100755 tests/grid/TestBBox.cpp create mode 100755 tests/grid/TestGrid.cpp create mode 100755 tests/grid/TestGridFactory.cpp create mode 100755 tests/grid/TestGridPoint.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100755 index 0000000..a9d53d1 --- /dev/null +++ b/CMakeLists.txt @@ -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}) + diff --git a/Exception.h b/Exception.h new file mode 100755 index 0000000..43bc3c1 --- /dev/null +++ b/Exception.h @@ -0,0 +1,23 @@ +#ifndef EXCEPTION_H +#define EXCEPTION_H + +#include +#include + +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 diff --git a/floorplan/Floor.h b/floorplan/Floor.h new file mode 100755 index 0000000..226cf84 --- /dev/null +++ b/floorplan/Floor.h @@ -0,0 +1,49 @@ +#ifndef FLOOR_H +#define FLOOR_H + +#include +#include "../geo/Line2D.h" + +/** + * represents one floor by describing all contained obstacles + */ +class Floor { + +private: + + + /** all obstacles within the floor */ + std::vector 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& 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 diff --git a/floorplan/FloorplanFactorySVG.h b/floorplan/FloorplanFactorySVG.h new file mode 100755 index 0000000..9154431 --- /dev/null +++ b/floorplan/FloorplanFactorySVG.h @@ -0,0 +1,138 @@ +#ifndef FLOORPLANFACTORYSVG_H +#define FLOORPLANFACTORYSVG_H + +#include "Floor.h" +#include +#include "../Exception.h" + +#include + +/** + * 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 diff --git a/geo/Angle.h b/geo/Angle.h new file mode 100755 index 0000000..d3cc1ab --- /dev/null +++ b/geo/Angle.h @@ -0,0 +1,33 @@ +#ifndef ANGLE_H +#define ANGLE_H + +#include + +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 diff --git a/geo/Line2D.h b/geo/Line2D.h new file mode 100755 index 0000000..a4389b8 --- /dev/null +++ b/geo/Line2D.h @@ -0,0 +1,21 @@ +#ifndef LINE2D_H +#define LINE2D_H + +#include + +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 diff --git a/geo/Units.h b/geo/Units.h new file mode 100755 index 0000000..05a38e8 --- /dev/null +++ b/geo/Units.h @@ -0,0 +1,16 @@ +#ifndef UNITS_H +#define UNITS_H + +class Units { + +public: + + /** convert from m to cm */ + template static inline T mToCm(const T m) {return m * 100;} + + /** convert from cm to m */ + template static inline T cmToM(const T cm) {return cm / 100;} + +}; + +#endif // UNITS_H diff --git a/grid/Grid.h b/grid/Grid.h new file mode 100755 index 0000000..7f22973 --- /dev/null +++ b/grid/Grid.h @@ -0,0 +1,141 @@ +#ifndef GRID_H +#define GRID_H + +#include +#include + +#include "../Exception.h" +#include "GridPoint.h" +#include "GridNode.h" +#include + +/** + * grid of the given grid-size, storing some value which + * extends GridPoint + */ +template class Grid { + +typedef uint64_t UID; + +private: + + /** all elements (nodes) within the grid */ + std::vector nodes; + + /** UID -> index mapping */ + std::unordered_map 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 diff --git a/grid/GridNode.h b/grid/GridNode.h new file mode 100755 index 0000000..d07bf32 --- /dev/null +++ b/grid/GridNode.h @@ -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 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 diff --git a/grid/GridNodeBBox.h b/grid/GridNodeBBox.h new file mode 100755 index 0000000..229f90c --- /dev/null +++ b/grid/GridNodeBBox.h @@ -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 diff --git a/grid/GridPoint.h b/grid/GridPoint.h new file mode 100755 index 0000000..801d807 --- /dev/null +++ b/grid/GridPoint.h @@ -0,0 +1,41 @@ +#ifndef GRIDPOINT_H +#define GRIDPOINT_H + +#include + +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 diff --git a/grid/factory/GridFactory.h b/grid/factory/GridFactory.h new file mode 100755 index 0000000..753c00a --- /dev/null +++ b/grid/factory/GridFactory.h @@ -0,0 +1,55 @@ +#ifndef GRIDFACTORY_H +#define GRIDFACTORY_H + +#include +#include "../../floorplan/Floor.h" +#include "../../geo/Units.h" +#include "../GridNodeBBox.h" +#include "../Grid.h" + +template class GridFactory { + +private: + + Grid& grid; + + +public: + + /** ctor with the grid to fill */ + GridFactory(Grid& 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 diff --git a/main.cpp b/main.cpp new file mode 100755 index 0000000..73cfa5f --- /dev/null +++ b/main.cpp @@ -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; + +} diff --git a/tests/Tests.h b/tests/Tests.h new file mode 100755 index 0000000..909c8be --- /dev/null +++ b/tests/Tests.h @@ -0,0 +1,14 @@ +#ifndef TESTS_H +#define TESTS_H + +#ifdef WITH_TESTS + +#include + +static inline std::string getDataFile(const std::string& name) { + return "/mnt/firma/kunden/indoor/tests/data/" + name; +} + +#endif + +#endif // TESTS_H diff --git a/tests/data/test.svg b/tests/data/test.svg new file mode 100755 index 0000000..a36f4e7 --- /dev/null +++ b/tests/data/test.svg @@ -0,0 +1,402 @@ + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tests/floorplan/TestFloorplanFactorySVG.cpp b/tests/floorplan/TestFloorplanFactorySVG.cpp new file mode 100755 index 0000000..2c3077c --- /dev/null +++ b/tests/floorplan/TestFloorplanFactorySVG.cpp @@ -0,0 +1,27 @@ +#ifdef WITH_TESTS + +//#include "../Tests.h" +//#include "../../floorplan/FloorplanFactorySVG.h" +//#include + + + +//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 diff --git a/tests/geo/TestAngle.cpp b/tests/geo/TestAngle.cpp new file mode 100755 index 0000000..244afe6 --- /dev/null +++ b/tests/geo/TestAngle.cpp @@ -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 diff --git a/tests/grid/TestBBox.cpp b/tests/grid/TestBBox.cpp new file mode 100755 index 0000000..cd19a2d --- /dev/null +++ b/tests/grid/TestBBox.cpp @@ -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 diff --git a/tests/grid/TestGrid.cpp b/tests/grid/TestGrid.cpp new file mode 100755 index 0000000..9acc3a3 --- /dev/null +++ b/tests/grid/TestGrid.cpp @@ -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 diff --git a/tests/grid/TestGridFactory.cpp b/tests/grid/TestGridFactory.cpp new file mode 100755 index 0000000..a68a01c --- /dev/null +++ b/tests/grid/TestGridFactory.cpp @@ -0,0 +1,45 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" +#include "../../grid/factory/GridFactory.h" +#include "../../floorplan/FloorplanFactorySVG.h" + +#include +#include +#include + +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 diff --git a/tests/grid/TestGridPoint.cpp b/tests/grid/TestGridPoint.cpp new file mode 100755 index 0000000..25981c2 --- /dev/null +++ b/tests/grid/TestGridPoint.cpp @@ -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