WaterRocket_AutoParachute

test.cpp

#include <iostream>
#include <vector>
#include <algorithm>
#include <map>
#include <string>
#include <cstdio>
#include <cstdlib>

#include "TopDetector.h"

#define INF 99999999
#define FOR(i,a,b) for (int i=(a);i<(b);i++)
#define RFOR(i,a,b) for (int i=(b)-1;i>=(a);i--)
#define REP(i,n) for (int i=0;i<(n);i++)
#define RREP(i,n) for (int i=(n)-1;i>=0;i--)


#define MAX_ERROR 0.5   // 誤差の最大値[m]

using namespace std;

// [min_x,max_x)の一様乱数を返す
float random(float min_x,float max_x);
// f(x) = a ( t - top_time ) ^ 2 + max_height
float parabola(float t,float a,float top_time,float max_height);
// シミュレーションを行う
void test(float a,float top_time,float max_height,float from_time,float wait_time,int N);

int main(void){
    float a = -4.9;
    int n_datas[5] = {10,30,50,100,500};
    
    srand(1);
    
    printf("%8s","a");
    printf("%8s","top_t");
    printf("%8s","from_t");
    printf("%8s","wait_t");
    printf("%8s","N");
    printf("%8s","a ans");
    printf("%10s\n","top_t ans");
    
    REP(i,60)
        cout << "-";
    cout << endl;
    
    // t^2の係数, 最高到達時間，最高高度，開始時間，サンプリング周波数，サンプリング数
    REP(i,5)
        test(a, 25.0,  30,  20.0,  0.03, n_datas[i]);
    
    REP(i,5)
        test(a, 25.0,  30,  15.0,  0.03, n_datas[i]);
    
    REP(i,5)
        test(a, 55.0,  100,  40.0,  0.03, n_datas[i]);
    
    return 0;
}


// [min_x,max_x)の一様乱数を返す
float random(float min_x,float max_x){
    return (max_x-min_x)*float(rand())/(RAND_MAX + 1)+min_x;
}

// f(x) = a ( t - top_time ) ^ 2 + max_height
float parabola(float t,float a,float top_time,float max_height){
    float delta_t = t - top_time;
    
    return a * (delta_t * delta_t) + max_height;
}

// シミュレーションを行う
void test(float a,float top_time,float max_height,float from_time,float wait_time,int N){
    TopDetector td;
    
    float t,y;
    
    REP(i,N){
        t = from_time + wait_time * i;
        y = parabola(t,a,top_time,max_height) + random(-MAX_ERROR,MAX_ERROR);
        
        td.push( t , y );
    }
    
    td.calc();
    
    printf("%8.2lf",a);
    printf("%8.2lf",top_time);
    printf("%8.2lf",from_time);
    printf("%8.2lf",wait_time);
    printf("%8d",N);
    printf("%8.2lf",td.get_a());
    printf("%8.2lf\n",td.get_top_time());
}

TopDetector.h

#ifndef TOP_DETECTOR_HEADER_FILE
#define TOP_DETECTOR_HEADER_FILE

class TopDetector {
  public:
    // コンストラクタ
    TopDetector() {
      reset();
    }

    // 変数の初期化を行うメソッド
    void reset() {
      for (int i = 0 ; i < 3 ; i++ ) {
        _x[i] = 0.0;
        _b[i] = 0.0;
        for (int j = 0 ; j < 3 ; j++ ) {
          _A[i][j] = 0.0;
        }
      }
      _n_data = 0;
    }

    // データを追加するメソッド
    void push(float t, float y) {
      const float t0 = 1;
      const float t1 = t0 * t;
      const float t2 = t1 * t;
      const float t3 = t2 * t;
      const float t4 = t3 * t;

      _A[0][0] += t4;
      _A[0][1] += t3;
      _A[0][2] += t2;

      _A[1][0] += t3;
      _A[1][1] += t2;
      _A[1][2] += t1;

      _A[2][0] += t2;
      _A[2][1] += t1;
      _A[2][2] += t0;

      _b[0] += t2 * y;
      _b[1] += t1 * y;
      _b[2] += t0 * y;

      _n_data++;
    }

    // データが有れば放物線の係数を計算するメソッド
    void calc() {
      if ( _n_data == 0 )
        return;
      solve<3>(_A, _x, _b);

    }

    // 追加されたデータ数を返すメソッド
    int get_n_data() {
      return _n_data;
    }

    // 計算済みならばt^2のパラメータを返すメソッド
    float get_a() {
      return _x[0];
    }

    // 計算済みならばt^1のパラメータを返すメソッド
    float get_b() {
      return _x[1];
    }

    // 計算済みならばt^0のパラメータを返すメソッド
    float get_c() {
      return _x[2];
    }

    // 頂点時間
    float get_top_time() {
      if ( _x[1] == 0.0 )
        return 1e+10;
      else
        return -_x[1] / (2.0f * _x[0]);
    }

  private:

    template<int N>
    void solve(const float M[N][N], float x[N], const float b[N]) {
        float A[N][N];
        float B[N];
        float temp;
        
        // コピー
        for(int i = 0 ; i < N ; i++ ){
            B[i] = b[i];
            for(int j = 0 ; j < N ; j++ ){
                A[i][j] = M[i][j];
            }
        }
        
        for (int i = 0 ; i < N ; i++ ) {
            // ピボット選択
            for (int j = i + 1 ; j < N ; j++ ) {
                if ( abs(A[i][j]) >= 1e-8 )
                    break;

                for (int k = 0 ; k < N ; k++ )
                    A[i][k] += A[j][k];
                B[i] += B[j];
            }

            // 対角成分を1に
            temp = A[i][i];
            for (int j = i ; j < N ; j++ )
                A[i][j] /= temp;
            B[i] /= temp;

            // 前進消去
            for (int j = i + 1 ; j < N ; j++ ) {
                temp = A[j][i];

                for (int k = i ; k < N ; k++ )
                    A[j][k] -= temp * A[i][k];
                B[j] -= temp * B[i];
            }
        }
        
        // 後進消去
        for (int i = N - 1 ; i >= 0 ; i-- ){
            for (int j = i - 1 ; j >= 0 ; j-- ){
                B[j] -= A[j][i] * B[i];
            }
        }
        
        for(int i = 0 ; i < N ; i++ )
            x[i] = B[i];
    }

    float _A[3][3];     // 係数行列
    float _b[3];        // 出力ベクトル
    float _x[3];        // 解ベクトル

    int _n_data;
};

#endif