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 calcVelocity()
  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
  82                 // 1\95b\93\96\82½\82è\82Ì\88Ú\93®\97Ê\82ð\8eZ\8fo\82·\82é
  83                 const uint32_t dt = m_velocityArray.back().timeStamp - m_velocityArray.front().timeStamp;
  84                 const double normalizedDeltaTime = (double)k_velocityMeasuringDistance / (double)dt;
  85                 return velocity / normalizedDeltaTime;
  86         }
  87
  88         inline void removeOldItem(const uint32_t timeStamp)
  89         {
  90                 VelocityyArray::const_iterator removeMark = m_velocityArray.end();
  91                 for ( VelocityyArray::const_iterator it = m_velocityArray.begin(); it != m_velocityArray.end(); ++it ) {
  92                         const uint32_t dt = timeStamp - it->timeStamp;
  93                         if ( dt <= k_velocityMeasuringDistance ) {
  94                                 break;
  95                         }
  96                         removeMark = it;
  97                 }
  98                 if ( removeMark != m_velocityArray.end() ) {
  99                         m_velocityArray.erase( m_velocityArray.begin(), removeMark );
 100                 }
 101         }
 102
 103         inline void updateVelocityLog()
 104         {
 105                 m_velocityLog.push_back( calcVelocity() );
 106                 if ( 3 < m_velocityLog.size() ) {
 107                         m_velocityLog.pop_front();
 108                 }
 109         }
 110
 111         inline double fastestVelocity() const
 112         {
 113                 double fastest = 0.0;
 114
 115                 VelocityLog::const_iterator it = std::min_element( m_velocityLog.begin(), m_velocityLog.end() );
 116                 if ( it != m_velocityLog.end() ) {
 117                         fastest = *it;
 118                 }
 119                 return fastest;
 120         }
 121 };