diff --git a/geo/Point2.h b/geo/Point2.h index 907f145..759cc64 100644 --- a/geo/Point2.h +++ b/geo/Point2.h @@ -1,6 +1,8 @@ #ifndef POINT2_H #define POINT2_H +#include + /** * 2D Point */ @@ -15,10 +17,35 @@ struct Point2 { /** ctor */ Point2(const float x, const float y) : x(x), y(y) {;} + Point2 operator + (const Point2& o) const {return Point2(x+o.x, y+o.y);} Point2 operator - (const Point2& o) const {return Point2(x-o.x, y-o.y);} + Point2 operator * (const float v) const {return Point2(v*x, v*y);} + + Point2 operator / (const float v) const {return Point2(x/v, y/v);} + + + Point2& operator *= (const float v) {x*=v; y*=v; return *this;} + + Point2& operator /= (const float v) {x/=v; y/=v; return *this;} + + Point2& operator += (const Point2& o) {x+=o.x; y+=o.y; return *this;} + + Point2& operator -= (const Point2& o) {x-=o.x; y-=o.y; return *this;} + + + bool operator == (const Point2& o) const {return x==o.x && y==o.y;} + + + /** get the distance between this point and the other one */ + float getDistance(const Point2& o) const { + const float dx = x - o.x; + const float dy = y - o.y; + return std::sqrt(dx*dx + dy*dy); + } + }; #endif // POINT2_H diff --git a/geo/Point3.h b/geo/Point3.h index 06c0a19..5518190 100644 --- a/geo/Point3.h +++ b/geo/Point3.h @@ -3,6 +3,7 @@ #include "../Assertions.h" #include +#include "Point2.h" /** * 3D Point @@ -29,6 +30,7 @@ struct Point3 { Point3 operator / (const float v) const {return Point3(x/v, y/v, z/v);} + Point3& operator *= (const float v) {x*=v; y*=v; z*=v; return *this;} Point3& operator /= (const float v) {x/=v; y/=v; z/=v; return *this;} @@ -37,8 +39,11 @@ struct Point3 { 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;} + Point2 xy() const {return Point2(x,y);} + /** read-only array access */ float operator [] (const int idx) const { Assert::isBetween(idx, 0, 2, "index out of bounds"); diff --git a/grid/walk/GridWalkRandomHeadingUpdate.h b/grid/walk/GridWalkRandomHeadingUpdate.h index 285a347..9ab272a 100644 --- a/grid/walk/GridWalkRandomHeadingUpdate.h +++ b/grid/walk/GridWalkRandomHeadingUpdate.h @@ -30,7 +30,7 @@ template class GridWalkRandomHeadingUpdate : public GridWalk { private: /** per-edge: change heading with this sigma */ - static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(8); + static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10); /** fast random-number-generator */ std::minstd_rand gen; diff --git a/math/Interpolator.h b/math/Interpolator.h index efc3a0a..765a196 100644 --- a/math/Interpolator.h +++ b/math/Interpolator.h @@ -2,6 +2,10 @@ #define INTERPOLATOR_H #include "../Assertions.h" +#include "../Exception.h" + +#include +#include /** * interpolate between two adjacent values based on their key @@ -11,40 +15,55 @@ template class Interpolator { public: /** value at key */ - struct Entry { + struct InterpolatorEntry { Key key; Value value; - Entry(const Key key, const Value& pos) : key(key), value(value) {;} + InterpolatorEntry(const Key key, const Value& value) : key(key), value(value) {;} }; protected: /** all added entries, SORTED by their key */ - std::vector entries; + std::vector entries; public: /** add a new value with its key. entries must be added in sorted order! */ void add(const Key key, const Value& value) { Assert::isTrue (entries.empty() || entries.back().key < key, "entries must be ordered!"); - entries.push_back(Entry(key, value)); + entries.push_back(InterpolatorEntry(key, value)); } /** get the interpolated value for the given key */ Value get(const Key key) const { - const int idx1 = getIdx(key); - const int idx2 = idx1+1; + + const int idx2 = getIdxAfter(key); + if (idx2 == 0) {return entries.front().value;} // key < everything within the list + if (idx2 == -1) {return entries.back().value;} // key > everything within the list + + // interpolate + const int idx1 = (idx2 > 0) ? (idx2 - 1) : (idx2); const float percent = (key - entries[idx1].key) / (float) (entries[idx2].key - entries[idx1].key); return entries[idx1].value + (entries[idx2].value - entries[idx1].value) * percent; + } +protected: + /** get the nearest index for the given key */ - int getIdx(const Key key) const { - // TODO: use O(log(n)) search here! - for (size_t i = 0; i < entries.size(); ++i) { - if (entries[i].key > key) {return i-1;} - } - throw "should not happen"; + int getIdxAfter(const Key key) const { + + // compare key against one entry + static auto comp = [] (const Key& key, const InterpolatorEntry& ie1) {return key < ie1.key;}; + + // find the next entry > than key + const auto it = std::upper_bound(entries.begin(), entries.end(), key, comp); + + // none found? (key > everything within the list) + return (it == entries.end()) ? (-1) : (it-entries.begin()); + //if (it == entries.end()) {throw Exception("key not found: " + std::to_string(key));} + //return it - entries.begin(); + } }; diff --git a/math/MovingAVG.h b/math/MovingAVG.h new file mode 100644 index 0000000..d3c0ee3 --- /dev/null +++ b/math/MovingAVG.h @@ -0,0 +1,58 @@ +#ifndef MOVINGAVG_H +#define MOVINGAVG_H + +#include + +template class MovingAVG { + +private: + + /** up to "size" elements */ + std::vector values; + + /** track the current sum of the vector's values */ + T curSum = 0; + + /** the number of elements to average */ + int size; + +public: + + /** ctor */ + MovingAVG(const int size) : size(size) {;} + + /** add a new value */ + void add(const T val) { + + // add new value + values.push_back(val); + curSum += val; + + // too many values? + if ((int) values.size() > size) { + curSum -= values.front(); + values.erase(values.begin()); + } + + } + + /** get the current average */ + T get() const { + return curSum / values.size(); + } + + + /** get the number of used entries */ + int getNumUsed() const { + return (int) values.size(); + } + + /** get number of entries to average */ + int getSize() const { + return size; + } + + +}; + +#endif // MOVINGAVG_H diff --git a/tests/grid/TestWalk.cpp b/tests/grid/TestWalk.cpp index 8670d63..059f0ca 100644 --- a/tests/grid/TestWalk.cpp +++ b/tests/grid/TestWalk.cpp @@ -12,8 +12,8 @@ #include "../../grid/walk/GridWalkWeighted.h" #include "../../grid/walk/GridWalkLightAtTheEndOfTheTunnel.h" -TEST(Walk, plot) { -//TEST(Walk, DISABLED_plot) { +//TEST(Walk, plot) { +TEST(Walk, DISABLED_plot) { Grid g(20); GridFactory gf(g); diff --git a/tests/math/TestInterpolator.cpp b/tests/math/TestInterpolator.cpp new file mode 100644 index 0000000..1fcb39c --- /dev/null +++ b/tests/math/TestInterpolator.cpp @@ -0,0 +1,40 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" +#include "../../math/Interpolator.h" + +TEST(Interpolator, addAndGet) { + + Interpolator interp; + interp.add(10, 20); + interp.add(20, 30); + interp.add(30, 50); + + ASSERT_EQ(20, std::round(interp.get(0))); // before + ASSERT_EQ(20, std::round(interp.get(10))); // matches + + ASSERT_EQ(21, std::round(interp.get(11))); + ASSERT_EQ(25, std::round(interp.get(15))); + ASSERT_EQ(29, std::round(interp.get(19))); + + ASSERT_EQ(30, std::round(interp.get(20))); //matches + + ASSERT_EQ(40, std::round(interp.get(25))); + ASSERT_EQ(42, std::round(interp.get(26))); + + ASSERT_EQ(50, std::round(interp.get(30))); // matches + ASSERT_EQ(50, std::round(interp.get(99))); // after + +} + +TEST(Interpolator, sanity) { + + Interpolator interp; + interp.add(10, 20); + ASSERT_THROW( interp.add(10,30), std::exception ); + interp.add(12, 22); + ASSERT_THROW( interp.add(11,21), std::exception ); + +} + +#endif diff --git a/tests/math/TestMovingAVG.cpp b/tests/math/TestMovingAVG.cpp new file mode 100644 index 0000000..723456f --- /dev/null +++ b/tests/math/TestMovingAVG.cpp @@ -0,0 +1,21 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" +#include "../../math/MovingAVG.h" + +TEST(MovingAVG, add) { + + MovingAVG avg(3); + + avg.add(1); ASSERT_EQ(1, avg.get()); + avg.add(1); ASSERT_EQ(1, avg.get()); + avg.add(1); ASSERT_EQ(1, avg.get()); + + avg.add(4); ASSERT_EQ(2, avg.get()); + avg.add(4); ASSERT_EQ(3, avg.get()); + + avg.add(4); ASSERT_EQ(4, avg.get()); + +} + +#endif