Use single header for MovingWindowFilter

Signed-off-by: Jose Luis Rivero <[email protected]>

include/gz/math/MovingWindowFilter.hh

@@ -37,7 +37,7 @@ namespace gz
     ///
     /// The default window size is 4.
     template< typename T>
-    class GZ_MATH_VISIBLE MovingWindowFilter
+    class MovingWindowFilter
     {
       /// \brief Constructor
       public: MovingWindowFilter(unsigned int _windowSize = 4);
@@ -90,6 +90,127 @@ namespace gz
     using MovingWindowFilterVector3f = MovingWindowFilter<Vector3f>;
     using MovingWindowFilterVector3d = MovingWindowFilter<Vector3d>;
   }
+
+//////////////////////////////////////////////////
+template<typename T>
+MovingWindowFilter<T>::MovingWindowFilter(unsigned int _windowSize)
+{
+  this->SetWindowSize(_windowSize);
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+void MovingWindowFilter<T>::Update(const T _val)
+{
+  // update sum and sample size with incoming _val
+
+  // keep running sum
+  this->sum += _val;
+
+  // shift pointer, wrap around if end has been reached.
+  ++this->valIter;
+  if (this->valIter == this->valHistory.end())
+  {
+    // reset iterator to beginning of queue
+    this->valIter = this->valHistory.begin();
+  }
+
+  // increment sample size
+  ++this->samples;
+
+  if (this->samples > this->valWindowSize)
+  {
+    // subtract old value if buffer already filled
+    this->sum -= (*this->valIter);
+    // put new value into queue
+    (*this->valIter) = _val;
+    // reduce sample size
+    --this->samples;
+  }
+  else
+  {
+    // put new value into queue
+    (*this->valIter) = _val;
+  }
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+void MovingWindowFilter<T>::SetWindowSize(const unsigned int _n)
+{
+  this->valWindowSize = _n;
+  this->valHistory = std::vector<T>(_n, T());
+  this->valIter = this->valHistory.begin();
+  this->sum = T();
+  this->samples = 0;
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+unsigned int MovingWindowFilter<T>::WindowSize() const
+{
+  return this->valWindowSize;
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+bool MovingWindowFilter<T>::WindowFilled() const
+{
+  return this->samples == this->valWindowSize;
+}
+
+//////////////////////////////////////////////////
+template<>
+inline gz::math::Vector3i
+MovingWindowFilter<gz::math::Vector3i>::Value() const
+{
+  auto value = this->sum / this->samples;
+  return value;
+}
+
+//////////////////////////////////////////////////
+template<>
+inline gz::math::Vector3f
+MovingWindowFilter<gz::math::Vector3f>::Value() const
+{
+  gz::math::Vector3f divisor;
+  divisor = static_cast<float>(this->samples);
+  auto value = this->sum / divisor;
+  return value;
+}
+
+//////////////////////////////////////////////////
+template<>
+inline gz::math::Vector3d
+MovingWindowFilter<gz::math::Vector3d>::Value() const
+{
+  auto value = this->sum / this->samples;
+  return value;
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+T MovingWindowFilter<T>::Value() const
+{
+  if (std::is_integral_v<T>)
+  {
+    auto value = this->sum / this->samples;
+    return T(value);
+  }
+  else
+  {
+    auto value = this->sum / static_cast<double>(this->samples);
+    return T(value);
+  }
+}
+
+template class MovingWindowFilter<int>;
+template class MovingWindowFilter<float>;
+template class MovingWindowFilter<double>;
+template class MovingWindowFilter<gz::math::Vector3i>;
+template class MovingWindowFilter<gz::math::Vector3f>;
+template class MovingWindowFilter<gz::math::Vector3d>;
+
   }  // namespace math
 }  // namespace gz
 #endif