[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 | |
---|
[13] | 11 | class Gauss1D : public ESCFunction |
---|
[11] | 12 | { |
---|
| 13 | private: |
---|
[13] | 14 | float a_, b_, c_, d_; |
---|
[11] | 15 | |
---|
| 16 | public: |
---|
[13] | 17 | Gauss1D(float a=0, float b=1, float c=0, float d=1) : a_(a), b_(b), c_(c), d_(d) { } |
---|
[11] | 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 | |
---|
[13] | 31 | return a_ + b_*std::exp(-(state[0]-c_)*(state[0]-c_)/(2*d_*d_)); |
---|
[11] | 32 | } |
---|
| 33 | }; |
---|
| 34 | |
---|
[13] | 35 | class Gauss2D : public ESCFunction |
---|
| 36 | { |
---|
| 37 | private: |
---|
| 38 | float a_, b_; |
---|
| 39 | std::vector<float> c_, d_; |
---|
| 40 | |
---|
| 41 | public: |
---|
| 42 | Gauss2D(float a, float b, std::vector<float> c, std::vector<float> d) |
---|
| 43 | { |
---|
| 44 | if (c.empty()) |
---|
| 45 | { |
---|
| 46 | c_.push_back(0); |
---|
| 47 | c_.push_back(0); |
---|
| 48 | } |
---|
| 49 | else |
---|
| 50 | c_ = c; |
---|
| 51 | |
---|
| 52 | if (d.empty()) |
---|
| 53 | { |
---|
| 54 | d_.push_back(1); |
---|
| 55 | d_.push_back(1); |
---|
| 56 | } |
---|
| 57 | else |
---|
| 58 | d_ = d; |
---|
| 59 | } |
---|
| 60 | |
---|
| 61 | std::vector<float> init() |
---|
| 62 | { |
---|
| 63 | std::vector<float> state; |
---|
| 64 | state.push_back(0); |
---|
| 65 | state.push_back(0); |
---|
| 66 | return state; |
---|
| 67 | } |
---|
| 68 | |
---|
| 69 | float value(const std::vector<float> &state) const |
---|
| 70 | { |
---|
| 71 | if (state.size() != 1) |
---|
| 72 | throw std::runtime_error("invalid state size"); |
---|
| 73 | |
---|
| 74 | double exponent = (state[0]-c_[0])*(state[0]-c_[0])/(2*d_[0]*d_[0]) + |
---|
| 75 | (state[1]-c_[1])*(state[1]-c_[1])/(2*d_[1]*d_[1]); |
---|
| 76 | |
---|
| 77 | return a_ + b_*std::exp(-exponent); |
---|
| 78 | } |
---|
| 79 | }; |
---|
| 80 | |
---|
| 81 | |
---|
[11] | 82 | class ESCSystem |
---|
| 83 | { |
---|
| 84 | protected: |
---|
| 85 | ESCFunction *function_; |
---|
| 86 | std::vector<float> state_; |
---|
| 87 | |
---|
| 88 | public: |
---|
| 89 | ESCSystem(ESCFunction *function) : function_(function) |
---|
| 90 | { |
---|
| 91 | if (!function) |
---|
| 92 | throw std::runtime_error("no function specified"); |
---|
| 93 | |
---|
| 94 | reset(); |
---|
| 95 | } |
---|
| 96 | |
---|
| 97 | void reset() |
---|
| 98 | { |
---|
| 99 | state_ = function_->init(); |
---|
| 100 | } |
---|
| 101 | |
---|
| 102 | float step(const std::vector<float> &vel) |
---|
| 103 | { |
---|
| 104 | if (state_.size() != vel.size()) |
---|
| 105 | throw std::runtime_error("invalid state size"); |
---|
| 106 | |
---|
| 107 | for (size_t ii=0; ii < state_.size() && ii < vel.size(); ++ii) |
---|
| 108 | state_[ii] += vel[ii]; |
---|
| 109 | |
---|
| 110 | return function_->value(state_); |
---|
| 111 | } |
---|
| 112 | |
---|
| 113 | float set(const std::vector<float> &pos) |
---|
| 114 | { |
---|
| 115 | if (state_.size() != pos.size()) |
---|
| 116 | throw std::runtime_error("invalid state size"); |
---|
| 117 | |
---|
| 118 | state_ = pos; |
---|
| 119 | return function_->value(state_); |
---|
| 120 | } |
---|
| 121 | |
---|
| 122 | float value() const |
---|
| 123 | { |
---|
| 124 | return function_->value(state_); |
---|
| 125 | } |
---|
| 126 | |
---|
| 127 | const std::vector<float> &state() |
---|
| 128 | { |
---|
| 129 | return state_; |
---|
| 130 | } |
---|
| 131 | }; |
---|