Add average vector calculation.

4 files changed, 121 insertions, 0 deletions

diff --git a/geometry/avg_vector.hpp b/geometry/avg_vector.hpp
new file mode 100644
index 0000000000..44770a9114
--- /dev/null
+++ b/geometry/avg_vector.hpp
@@ -0,0 +1,78 @@
+#pragma once
+
+#include "../base/math.hpp"
+#include "../base/assert.hpp"
+
+#include "../std/deque.hpp"
+#include "../std/array.hpp"
+
+
+namespace math
+{
+  template <class T, size_t Dim> class AvgVector
+  {
+    typedef deque<array<T, Dim> > ContT;
+    typedef typename ContT::value_type ValueT;
+    ContT m_vectors;
+    size_t m_count;
+
+    static T Distance(ValueT const & a1, ValueT const & a2)
+    {
+      T res = 0;
+      for (size_t i = 0; i < Dim; ++i)
+        res += my::sq(a1[i] - a2[i]);
+
+      return sqrt(res);
+    }
+
+    void Average(ValueT const & a1, ValueT const & a2, T * res) const
+    {
+      for (size_t i = 0; i < Dim; ++i)
+        res[i] = (a1[i] + a2[i]) / 2.0;
+    }
+
+    void CalcAverage(T * res) const
+    {
+      T minD = numeric_limits<T>::max();
+      size_t I = 0, J = 1;
+
+      size_t const count = m_vectors.size();
+      ASSERT_GREATER ( count, 1, () );
+      for (size_t i = 0; i < count - 1; ++i)
+        for (size_t j = i+1; j < count; ++j)
+        {
+          T const d = Distance(m_vectors[i], m_vectors[j]);
+          if (d < minD)
+          {
+            I = i;
+            J = j;
+            minD = d;
+          }
+        }
+
+      Average(m_vectors[I], m_vectors[J], res);
+    }
+
+  public:
+    AvgVector(size_t count = 1) : m_count(count)
+    {
+      STATIC_ASSERT(is_floating_point<T>::value);
+    }
+
+    void SetCount(size_t count) { m_count = count; }
+
+    /// @param[in]  Next measurement.
+    /// @param[out] Average value.
+    void Next(T * arr)
+    {
+      if (m_vectors.size() == m_count)
+        m_vectors.pop_front();
+
+      m_vectors.push_back(ValueT());
+      memcpy(m_vectors.back().data(), arr, Dim*sizeof(T));
+
+      if (m_vectors.size() > 1)
+        CalcAverage(arr);
+    }
+  };
+}
diff --git a/geometry/geometry.pro b/geometry/geometry.pro
index 08689d8225..886d24f55e 100644
--- a/geometry/geometry.pro
+++ b/geometry/geometry.pro
@@ -38,3 +38,4 @@ HEADERS += \
   any_rect2d.hpp \
   region2d/binary_operators.hpp \
   region2d/boost_concept.hpp \
+  avg_vector.hpp \
diff --git a/geometry/geometry_tests/geometry_tests.pro b/geometry/geometry_tests/geometry_tests.pro
index d1f5428e79..f71ee12a64 100644
--- a/geometry/geometry_tests/geometry_tests.pro
+++ b/geometry/geometry_tests/geometry_tests.pro
@@ -38,3 +38,4 @@ SOURCES += \
   robust_test.cpp \
   anyrect_test.cpp \
   region2d_binary_op_test.cpp \
+  vector_test.cpp \
diff --git a/geometry/geometry_tests/vector_test.cpp b/geometry/geometry_tests/vector_test.cpp
new file mode 100644
index 0000000000..c7cc0e162f
--- /dev/null
+++ b/geometry/geometry_tests/vector_test.cpp
@@ -0,0 +1,41 @@
+#include "../../testing/testing.hpp"
+
+#include "../avg_vector.hpp"
+
+
+namespace
+{
+template <class T, size_t N> bool EqualArrays(T (&a1)[N], T (&a2)[N])
+  {
+    for (size_t i = 0; i < N; ++i)
+      if (!my::AlmostEqual(a1[i], a2[i]))
+        return false;
+    return true;
+  }
+}
+
+UNIT_TEST(AvgVector_Smoke)
+{
+  math::AvgVector<double, 3> holder(3);
+
+  double ethalon1[] = { 5, 5, 5 };
+  double ethalon2[] = { 5.5, 5.5, 5.5 };
+  double ethalon3[] = { 6, 6, 6 };
+
+  double arr1[] = { 5, 5, 5 };
+  double arr2[] = { 6, 6, 6 };
+  double arr3[] = { 5, 5, 5 };
+  double arr4[] = { 6, 6, 6 };
+
+  holder.Next(arr1);
+  TEST(EqualArrays(arr1, ethalon1), ());
+
+  holder.Next(arr2);
+  TEST(EqualArrays(arr2, ethalon2), ());
+
+  holder.Next(arr3);
+  TEST(EqualArrays(arr3, ethalon1), ());
+
+  holder.Next(arr4);
+  TEST(EqualArrays(arr4, ethalon3), ());
+}