GVONavish/GVONavish/GVOSpeedMeter.h

   1 #pragma once
   2 #include <deque>
   3 #include <algorithm>
   4
   5
   6 class GVOSpeedMeter {
   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         // Google\90æ\90¶\9eH\82\81u\92n\8b\85\82Ì\8aO\8eü\82Í40,075km\81v\81u1\83m\83b\83g\82Í1.85200km\81v
  55         // 1\90¢\8aE\8dÀ\95W\82Í40,075km/16384points
  56         // \8eÀ\8e\9e\8aÔ1\95b\82Å\83Q\81[\83\80\93à0.4\8e\9e\8aÔ
  57         //
  58         // \90Ô\93¹\94¼\8ca\82Í6378.137\82Æ\82·\82é\82Æ\8aO\8eü\82Í2*M_PI_*6378.137=40075.016685578483111
  59         inline double velocityByKnot() const
  60         {
  61                 //
  62                 //static const double k_knotFactor = 40075.0 / 16384.0 / 0.4 / 1.85200;
  63                 static const double k_knotFactor = (2 * M_PI * 6378.137) / 16384.0 / 0.4 / 1.85200;
  64                 return m_velocity * k_knotFactor;
  65         }
  66
  67 private:
  68         inline double calcVelocityPerSecond()
  69         {
  70                 double velocity = 0.0;
  71
  72                 if ( m_velocityArray.size() < 2 ) {
  73                         return 0.0;
  74                 }
  75
  76                 // \88Ú\93®\95½\8bÏ\92l
  77                 for ( VelocityyArray::const_iterator it = m_velocityArray.begin(); it != m_velocityArray.end(); ++it ) {
  78                         velocity += it->velocity;
  79                 }
  80                 velocity /= m_velocityArray.size();
  81                 return velocity;
  82         }
  83
  84         inline void removeOldItem(const uint32_t timeStamp)
  85         {
  86                 VelocityyArray::const_iterator removeMark = m_velocityArray.end();
  87                 for ( VelocityyArray::const_iterator it = m_velocityArray.begin(); it != m_velocityArray.end(); ++it ) {
  88                         const uint32_t dt = timeStamp - it->timeStamp;
  89                         if ( dt <= k_velocityMeasuringDistance ) {
  90                                 break;
  91                         }
  92                         removeMark = it;
  93                 }
  94                 if ( removeMark != m_velocityArray.end() ) {
  95                         m_velocityArray.erase( m_velocityArray.begin(), removeMark );
  96                 }
  97         }
  98
  99         inline void updateVelocityLog()
 100         {
 101                 m_velocityLog.push_back( calcVelocityPerSecond() );
 102                 if ( 3 < m_velocityLog.size() ) {
 103                         m_velocityLog.pop_front();
 104                 }
 105         }
 106
 107         inline double fastestVelocity() const
 108         {
 109                 double fastest = 0.0;
 110
 111                 VelocityLog::const_iterator it = std::max_element( m_velocityLog.begin(), m_velocityLog.end() );
 112                 if ( it != m_velocityLog.end() ) {
 113                         fastest = *it;
 114                 }
 115                 return fastest;
 116         }
 117 };