ramp_test.cpp

#include <iostream>
#include <cmath>
using namespace std;

class Ramp {
public:
    Ramp(float v_start, float v_max, float v_end, float acceleration, float L)
    {
        /* 加速時間と減速時間を計算
         http://keisan.casio.jp/exec/system/1164005431 */
        float t_start = fabs(v_max - v_start) / acceleration;
        float t_end = fabs(v_end - v_max) / acceleration;
        /* 絶対値を求めておく */
        float w_start = fabs(v_start);
        float w_max = fabs(v_max);
        float w_end = fabs(v_end);
        /* 加速が終わる時の距離、減速が始まる時の距離を求める */
        _n1 = (w_start + w_max) * t_start / 2;
        _n2 = L - (w_end + w_max) * t_end / 2;
        /* 台形にならない場合 */
        if(_n1 > _n2) {
            /*
             速度と距離に関する公式
             http://manapedia.jp/text/1466?page=3

             ve^2 - vb^2 = 2aL より
             ve^2 = 2aL + vb1^2 ... (1)
             ve^2 = 2a(N-L) + vb2^2 ... (2)
             (1)=(2)のLを計算する
             2aL + vb1^2 = 2a(N-L) + vb2^2
             2aL = 2aN - 2aL + vb2^2 - vb1^2
             4aL = 2aN + vb2^2 - vb1^2
             L = 2aN + vb2^2 - vb1^2 / 4a
             */
            _n1 = _n2 = (2 * acceleration * L + v_end * v_end - v_start * v_start) / (4 * acceleration);
        }
        _n3 = L;
        _v_start = v_start;
        _v_max = v_max;
        _v_end = v_end;
        _a_start = (w_max > w_start ? acceleration : -acceleration);
        _a_end = (w_max > w_end ? acceleration : -acceleration);
        cout << _n1 << '\t' << _n2 << '\t' << _n3 << '\t' << _v_start << '\t' << v_max << '\t' << _v_end << '\t' << _a_start << '\t' << _a_end << endl;
    }

    float next(float n)
    {
        float v;
        if(n < _n1) {
            /* 加速中の移動距離から現在の速度を求める */
            /* ve^2 = 2aL + vb^2 */
            v = 2 * _a_start * n + _v_start * _v_start;
            v = sqrtf(v);
            v = copysignf(v, _v_max);
        } else if(n < _n2) {
            v = _v_max;
        } else if(n < _n3) {
            /* 減速中の移動距離から現在の速度を求める */
            /* ve^2 = 2aL + vb^2 */
            v = 2 * _a_end * (_n3 - n) + _v_end * _v_end;
            v = sqrtf(v);
            v = copysignf(v, _v_max);
        } else {
            v = _v_end;
        }
        return v;
    }

private:
    float _n1;
    float _n2;
    float _n3;
    float _v_start;
    float _v_max;
    float _v_end;
    float _a_start;
    float _a_end;
};

int main(void){
    // Here your code !
    #if 1
    Ramp r0(  2,  10,   2, 0.5, 100); // ok
    Ramp r1( 10,   2,  10, 0.5, 100); // ok
    Ramp r2( -2, -10,  -2, 0.5, 100); // ok
    Ramp r3(-10,  -2, -10, 0.5, 100); // ok
    Ramp r4(  0,   5,  10, 0.5, 100); // ok
    Ramp r5( -2,  -5, -10, 0.5, 100); // ok
    #else
    Ramp r0(  2,  10,   2, 2, 100); // ok
    Ramp r1( 10,   2,  10, 2, 100); // ok
    Ramp r2( -2, -10,  -2, 2, 100); // ok
    Ramp r3(-10,  -2, -10, 2, 100); // ok
    Ramp r4(  0,   5,  10, 2, 100); // ok
    Ramp r5( -2,  -5, -10, 2, 100); // ok
    #endif

    for(int i=0; i<105; i++){
        cout << i << "\t" << r0.next(i) << "\t" << r1.next(i) << "\t" << r2.next(i) << "\t" << r3.next(i) << "\t" << r4.next(i) << "\t" << r5.next(i) <<endl;
    }
}