SpeedCalculator.hh

[詳解]

 
//
// Copyright (C) 2011-2020 Yuki YOKOKURA
// This program is free software;
// you can redistribute it and/or modify it under the terms of the FreeBSD License.
// For details, see the License.txt file.
 
#ifndef SPEEDCALCULATOR
#define SPEEDCALCULATOR
 
#include <cmath>
#include "RingBuffer.hh"
 
namespace ARCS {    // ARCS名前空間
    template <size_t N>
    class SpeedCalculator {
        public:
            SpeedCalculator()
                : PositionBuff(), TimeBuff(), FirstTime(true)
            {
                
            }
 
            SpeedCalculator(SpeedCalculator&& r)
                : PositionBuff(r.PositionBuff), TimeBuff(r.TimeBuff), FirstTime(r.FirstTime)
            {
                
            }
 
            ~SpeedCalculator(){
                
            }
            
            double GetSpeed(const double Position, const double Time){
                if(FirstTime){
                    // 初めて呼ばれた場合は，
                    PositionBuff.FillBuffer(Position);  // 初期位置でバッファを埋めておく
                    TimeBuff.FillBuffer(Time);          // 初期時刻でバッファを埋めておく
                    FirstTime = false;                  // フラグリセット
                    return 0;                           // 最初の1回目は速度を計算できないのでゼロ
                }else{
                    // 2回目以降は，
                    const double dx = Position - PositionBuff.GetFinalValue();  // 現在位置と位置リングバッファの最後尾との偏差の計算
                    const double dt = Time - TimeBuff.GetFinalValue();          // 現在時刻と時刻リングバッファの最後尾との偏差の計算
                    const double v = dx/dt;                 // 速度の計算
                    PositionBuff.SetFirstValue(Position);   // 位置リングバッファの先頭に現在位置を詰める
                    TimeBuff.SetFirstValue(Time);           // 時刻リングバッファの先頭に現在時刻を詰める
                    if(std::isfinite(v)){
                        return v;   // 速度が有限値なら計算結果を返す
                    }else{
                        return 0;   // ゼロ割のときはゼロにしておく
                    }
                }
            }
            
            void Reset(void){
                FirstTime = true;
            }
            
        private:
            SpeedCalculator(const SpeedCalculator&) = delete;                   
            const SpeedCalculator& operator=(const SpeedCalculator&) = delete;  
            RingBuffer<double, N, false> PositionBuff;  
            RingBuffer<double, N, false> TimeBuff;      
            bool FirstTime;                             
    };
}
 
#endif