[11] | 1 | #include <limits> |
---|
| 2 | #include <exception> |
---|
| 3 | |
---|
| 4 | class ESCFunction |
---|
| 5 | { |
---|
| 6 | public: |
---|
| 7 | virtual std::vector<float> init() = 0; |
---|
| 8 | virtual float value(const std::vector<float> &state) const = 0; |
---|
| 9 | }; |
---|
| 10 | |
---|
| 11 | class Gauss : public ESCFunction |
---|
| 12 | { |
---|
| 13 | private: |
---|
| 14 | float a_, b_, c_; |
---|
| 15 | |
---|
| 16 | public: |
---|
| 17 | Gauss(float a=1, float b=0, float c=1) : a_(a), b_(b), c_(c) { } |
---|
| 18 | |
---|
| 19 | std::vector<float> init() |
---|
| 20 | { |
---|
| 21 | std::vector<float> state; |
---|
| 22 | state.push_back(0); |
---|
| 23 | return state; |
---|
| 24 | } |
---|
| 25 | |
---|
| 26 | float value(const std::vector<float> &state) const |
---|
| 27 | { |
---|
| 28 | if (state.size() != 1) |
---|
| 29 | throw std::runtime_error("invalid state size"); |
---|
| 30 | |
---|
| 31 | return a_*std::exp(-(state[0]-b_)*(state[0]-b_)/(2*c_*c_)); |
---|
| 32 | } |
---|
| 33 | }; |
---|
| 34 | |
---|
| 35 | class ESCSystem |
---|
| 36 | { |
---|
| 37 | protected: |
---|
| 38 | ESCFunction *function_; |
---|
| 39 | std::vector<float> state_; |
---|
| 40 | |
---|
| 41 | public: |
---|
| 42 | ESCSystem(ESCFunction *function) : function_(function) |
---|
| 43 | { |
---|
| 44 | if (!function) |
---|
| 45 | throw std::runtime_error("no function specified"); |
---|
| 46 | |
---|
| 47 | reset(); |
---|
| 48 | } |
---|
| 49 | |
---|
| 50 | void reset() |
---|
| 51 | { |
---|
| 52 | state_ = function_->init(); |
---|
| 53 | } |
---|
| 54 | |
---|
| 55 | float step(const std::vector<float> &vel) |
---|
| 56 | { |
---|
| 57 | if (state_.size() != vel.size()) |
---|
| 58 | throw std::runtime_error("invalid state size"); |
---|
| 59 | |
---|
| 60 | for (size_t ii=0; ii < state_.size() && ii < vel.size(); ++ii) |
---|
| 61 | state_[ii] += vel[ii]; |
---|
| 62 | |
---|
| 63 | return function_->value(state_); |
---|
| 64 | } |
---|
| 65 | |
---|
| 66 | float set(const std::vector<float> &pos) |
---|
| 67 | { |
---|
| 68 | if (state_.size() != pos.size()) |
---|
| 69 | throw std::runtime_error("invalid state size"); |
---|
| 70 | |
---|
| 71 | state_ = pos; |
---|
| 72 | return function_->value(state_); |
---|
| 73 | } |
---|
| 74 | |
---|
| 75 | float value() const |
---|
| 76 | { |
---|
| 77 | return function_->value(state_); |
---|
| 78 | } |
---|
| 79 | |
---|
| 80 | const std::vector<float> &state() |
---|
| 81 | { |
---|
| 82 | return state_; |
---|
| 83 | } |
---|
| 84 | }; |
---|