diff --git a/geo/Length.h b/geo/Length.h new file mode 100644 index 0000000..1c3afb9 --- /dev/null +++ b/geo/Length.h @@ -0,0 +1,60 @@ +#ifndef LENGTH_H +#define LENGTH_H + + + + +/** + * unit independent length measurement + */ +template struct Length { + +private: + + const T val; + + /** delete empty ctor */ + Length() = delete; + + /** hidden value ctor */ + Length(const T val) : val(val) {;} + +public: + + /** construct from mm */ + static Length mm(const T mm) {return Length(mm * mul / 1000);} + + /** construct from cm */ + static Length cm(const T cm) {return Length(cm * mul / 100);} + + /** construct from m */ + static Length m(const T m) {return Length(m * mul / 1);} + +public: + + /** get in mm */ + T mm() const {return val * 1000 / mul;} + + /** get in cm */ + T cm() const {return val * 100 / mul;} + + /** get in m */ + T m() const {return val * 1 / mul;} + +public: + + /** add the given length */ + Length operator + (const Length o) {return Length(m+o.m);} + + /** subtract the given length */ + Length operator - (const Length o) {return Length(m+o.m);} + +}; + +/** float. internally stored in meters */ +typedef Length LengthF; + +/** int. internally stored in millimeters */ +typedef Length LengthI; + +#endif // LENGTH_H diff --git a/grid/Grid.h b/grid/Grid.h index 4a789c1..b9cabd7 100755 --- a/grid/Grid.h +++ b/grid/Grid.h @@ -158,6 +158,12 @@ public: return nodes[hashes[uid]]; } + /** get the center-node the given Point belongs to. or nullptr if not present */ + const T* getNodePtrFor(const GridPoint& p) { + auto it = hashes.find(getUID(p)); + return (it == hashes.end()) ? (nullptr) : (&nodes[it->second]); + } + /** get the BBox for the given node */ GridNodeBBox getBBox(const int idx) const { return getBBox(nodes[idx]); diff --git a/grid/factory/GridFactory.h b/grid/factory/GridFactory.h index 44d97a7..043cd5e 100755 --- a/grid/factory/GridFactory.h +++ b/grid/factory/GridFactory.h @@ -2,6 +2,8 @@ #define GRIDFACTORY_H #include +#include + #include "../../floorplan/Floor.h" #include "../../floorplan/Stairs.h" @@ -30,9 +32,10 @@ public: /** add the given floor at the provided height (in cm) */ void addFloor(const Floor& floor, const float z_cm) { - Log::add(name, "adding floor at height " + std::to_string(z_cm)); + Log::add(name, "adding floor at height " + std::to_string(z_cm), false); + Log::tick(); - const float gridSize_cm = grid.getGridSize_cm(); + const int gridSize_cm = grid.getGridSize_cm(); // build grid-points for(int x_cm = 0; x_cm < floor.getWidth_cm(); x_cm += gridSize_cm) { @@ -48,6 +51,8 @@ public: } } + Log::tock(); + connectAdjacent(z_cm); } @@ -55,15 +60,16 @@ public: /** 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)); + Log::add(name, "connecting all adjacent nodes at height " + std::to_string(z_cm), false); + Log::tick(); const int gridSize_cm = grid.getGridSize_cm(); // connect adjacent grid-points - for (int idx = 0; idx < grid.getNumNodes(); ++idx) { + for (T& n1 : grid) { - T& n1 = (T&) grid[idx]; - if (n1.z_cm != z_cm) {continue;} // ugly... different floor -> skip + // not the floor we are looking for? -> skip (ugly.. slow(er)) + if (n1.z_cm != z_cm) {continue;} // square around each point for (int x = -gridSize_cm; x <= gridSize_cm; x += gridSize_cm) { @@ -73,14 +79,14 @@ public: if ((x == y) && (x == 0)) {continue;} // position of the potential neighbor - int ox = n1.x_cm + x; - int oy = n1.y_cm + y; - GridPoint p(ox, oy, n1.z_cm); + const int ox = n1.x_cm + x; + const int oy = n1.y_cm + y; + const GridPoint p(ox, oy, n1.z_cm); // does the grid contain the potential neighbor? - if (grid.hasNodeFor(p)) { - T& n2 = (T&) grid.getNodeFor(p); - grid.connectUniDir(n1, n2); + const T* n2 = grid.getNodePtrFor(p); + if (n2 != nullptr) { + grid.connectUniDir(n1, *n2); // UNI-dir connection as EACH node is processed! } } @@ -88,6 +94,8 @@ public: } + Log::tock(); + } @@ -205,7 +213,7 @@ public: Log::add(name, "searching for isolated nodes"); // get largest connected region - std::set set; + std::unordered_set set; do { const int idxStart = rand() % grid.getNumNodes(); set.clear(); @@ -229,7 +237,7 @@ public: private: /** recursively get all connected nodes and add them to the set */ - void getConnected(const int idx, std::set& set) { + void getConnected(const int idx, std::unordered_set& set) { // get the node behind idx const T& n1 = (T&) grid[idx]; diff --git a/main.cpp b/main.cpp index c04b597..e2e4adc 100755 --- a/main.cpp +++ b/main.cpp @@ -16,7 +16,7 @@ int main(int argc, char** argv) { #ifdef WITH_TESTS ::testing::InitGoogleTest(&argc, argv); //::testing::GTEST_FLAG(filter) = "*Importance*"; - ::testing::GTEST_FLAG(filter) = "*Walk*"; + ::testing::GTEST_FLAG(filter) = "*Length*"; return RUN_ALL_TESTS(); #endif diff --git a/misc/Debug.h b/misc/Debug.h index 7f543f7..72a7cf3 100644 --- a/misc/Debug.h +++ b/misc/Debug.h @@ -6,34 +6,43 @@ #include #include "Time.h" +/** quick and dirty workaround */ +static decltype(Time::tick()) LogLastTick; + class Log { public: - static void add(const char* comp, const std::string what) { + static void add(const char* comp, const std::string what, const bool nl = true) { addComp(comp); std::cout << what; - addTime(); - std::cout << std::endl; + if (nl) {std::cout << std::endl;} } - static void add(const std::string& component, const std::string what) { + static void add(const std::string& component, const std::string what, const bool nl = true) { addComp(component.c_str()); std::cout << what; - addTime(); - std::cout << std::endl; + if (nl) {std::cout << std::endl;} + } + + + + static void tick() { + LogLastTick = Time::tick(); + } + + static void tock() { + const auto cur = Time::tick(); + const int diff_ms = Time::diffMS(LogLastTick, cur); + LogLastTick = cur; + std::cout << " (took: " << diff_ms << "ms)" << std::endl; } private: - static void addTime() { - static auto last = Time::tick(); - const auto cur = Time::tick(); - std::cout << " (+" << Time::diffMS(last, cur) << "ms)"; - last = cur; - } + static void addComp(const char* component) { std::cout << "[" << std::setw(12) << std::setfill(' ') << component << "] "; diff --git a/tests/geo/TestLength.cpp b/tests/geo/TestLength.cpp new file mode 100644 index 0000000..f4ca382 --- /dev/null +++ b/tests/geo/TestLength.cpp @@ -0,0 +1,46 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" +#include "../../geo/Length.h" + +TEST(Length, float) { + + static constexpr float delta = 0.00001; + + LengthF l1 = LengthF::m(1.0); + ASSERT_NEAR(1, l1.m(), delta); + ASSERT_NEAR(100, l1.cm(), delta); + ASSERT_NEAR(1000, l1.mm(), delta); + + LengthF l2 = LengthF::cm(1.0); + ASSERT_NEAR(0.01, l2.m(), delta); + ASSERT_NEAR(1, l2.cm(), delta); + ASSERT_NEAR(10, l2.mm(), delta); + + LengthF l3 = LengthF::mm(1.0); + ASSERT_NEAR(0.001, l3.m(), delta); + ASSERT_NEAR(0.1, l3.cm(), delta); + ASSERT_NEAR(1, l3.mm(), delta); + +} + +TEST(Length, int) { + + LengthI l1 = LengthI::m(1.0); + ASSERT_EQ(1, l1.m()); + ASSERT_EQ(100, l1.cm()); + ASSERT_EQ(1000, l1.mm()); + + LengthI l2 = LengthI::cm(1.0); + ASSERT_EQ(0, l2.m()); + ASSERT_EQ(1, l2.cm()); + ASSERT_EQ(10, l2.mm()); + + LengthI l3 = LengthI::mm(1.0); + ASSERT_EQ(0, l3.m()); + ASSERT_EQ(0, l3.cm()); + ASSERT_EQ(1, l3.mm()); + +} + +#endif