GVONavish/GVONavish/GVOSpeedMeter.h

   1 #pragma once
   2 #include <deque>
   3 #include <algorithm>
   4 #include "GVONoncopyable.h"
   5
   6 class GVOSpeedMeter : private GVONoncopyable {
   7 private:
   8         typedef std::deque<double> Array;
   9         struct VelocityLogItem {
  10                 uint32_t timeStamp;
  11                 double velocity;
  12
  13                 VelocityLogItem() :
  14                         timeStamp(),
  15                         velocity()
  16                 {
  17                 }
  18                 VelocityLogItem( const uint32_t timeStamp, const double velocity ) :
  19                         timeStamp( timeStamp ),
  20                         velocity( velocity )
  21                 {
  22                 }
  23         };
  24         typedef std::deque<VelocityLogItem> VelocityyArray;
  25         typedef std::deque<double> VelocityLog;
  26         const uint32_t k_velocityMeasuringDistance = 5000;
  27
  28 private:
  29         VelocityyArray m_velocityArray;
  30         VelocityLog m_velocityLog;
  31         double m_velocity;
  32
  33 public:
  34
  35         GVOSpeedMeter() :
  36                 m_velocity()
  37         {
  38         }
  39
  40         ~GVOSpeedMeter()
  41         {
  42         }
  43
  44         inline void updateVelocity( const double velocity, const uint32_t timeStamp )
  45         {
  46                 m_velocityArray.push_back( VelocityLogItem( timeStamp, velocity ) );
  47
  48                 removeOldItem( timeStamp );
  49                 updateVelocityLog();
  50
  51                 m_velocity = fastestVelocity();;
  52         }
  53
  54         double velocity() const
  55         {
  56                 return m_velocity;
  57         }
  58
  59 private:
  60         inline double calcVelocityPerSecond()
  61         {
  62                 double velocity = 0.0;
  63
  64                 if ( m_velocityArray.size() < 2 ) {
  65                         return 0.0;
  66                 }
  67
  68                 // \88Ú\93®\95½\8bÏ\92l
  69                 for ( VelocityyArray::const_iterator it = m_velocityArray.begin(); it != m_velocityArray.end(); ++it ) {
  70                         velocity += it->velocity;
  71                 }
  72                 velocity /= m_velocityArray.size();
  73                 return velocity;
  74         }
  75
  76         inline void removeOldItem(const uint32_t timeStamp)
  77         {
  78                 VelocityyArray::const_iterator removeMark = m_velocityArray.end();
  79                 for ( VelocityyArray::const_iterator it = m_velocityArray.begin(); it != m_velocityArray.end(); ++it ) {
  80                         const uint32_t dt = timeStamp - it->timeStamp;
  81                         if ( dt <= k_velocityMeasuringDistance ) {
  82                                 break;
  83                         }
  84                         removeMark = it;
  85                 }
  86                 if ( removeMark != m_velocityArray.end() ) {
  87                         m_velocityArray.erase( m_velocityArray.begin(), removeMark );
  88                 }
  89         }
  90
  91         inline void updateVelocityLog()
  92         {
  93                 m_velocityLog.push_back( calcVelocityPerSecond() );
  94                 if ( 3 < m_velocityLog.size() ) {
  95                         m_velocityLog.pop_front();
  96                 }
  97         }
  98
  99         inline double fastestVelocity() const
 100         {
 101                 double fastest = 0.0;
 102
 103                 VelocityLog::const_iterator it = std::max_element( m_velocityLog.begin(), m_velocityLog.end() );
 104                 if ( it != m_velocityLog.end() ) {
 105                         fastest = *it;
 106                 }
 107                 return fastest;
 108         }
 109 };