Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef DWB_PLUGINS_ONE_D_VELOCITY_ITERATOR_H
00036 #define DWB_PLUGINS_ONE_D_VELOCITY_ITERATOR_H
00037
00038 #include <algorithm>
00039 #include <cmath>
00040
00041 namespace dwb_plugins
00042 {
00043
00044 const double EPSILON = 1E-5;
00045
00056 inline double projectVelocity(double v0, double accel, double decel, double dt, double target)
00057 {
00058 double v1;
00059 if (v0 < target)
00060 {
00061 v1 = v0 + accel * dt;
00062 return std::min(target, v1);
00063 }
00064 else
00065 {
00066 v1 = v0 + decel * dt;
00067 return std::max(target, v1);
00068 }
00069 }
00070
00085 class OneDVelocityIterator
00086 {
00087 public:
00098 OneDVelocityIterator(double current, double min, double max, double acc_limit, double decel_limit, double acc_time,
00099 int num_samples)
00100 {
00101 if (current < min)
00102 {
00103 current = min;
00104 }
00105 else if (current > max)
00106 {
00107 current = max;
00108 }
00109 max_vel_ = projectVelocity(current, acc_limit, decel_limit, acc_time, max);
00110 min_vel_ = projectVelocity(current, acc_limit, decel_limit, acc_time, min);
00111 reset();
00112
00113 if (fabs(min_vel_ - max_vel_) < EPSILON)
00114 {
00115 increment_ = 1.0;
00116 return;
00117 }
00118 num_samples = std::max(2, num_samples);
00119
00120
00121 increment_ = (max_vel_ - min_vel_) / std::max(1, (num_samples - 1));
00122 }
00123
00127 double getVelocity() const
00128 {
00129 if (return_zero_now_) return 0.0;
00130 return current_;
00131 }
00132
00136 OneDVelocityIterator& operator++()
00137 {
00138 if (return_zero_ && current_ < 0.0 && current_ + increment_ > 0.0 && current_ + increment_ <= max_vel_ + EPSILON)
00139 {
00140 return_zero_now_ = true;
00141 return_zero_ = false;
00142 }
00143 else
00144 {
00145 current_ += increment_;
00146 return_zero_now_ = false;
00147 }
00148 return *this;
00149 }
00150
00154 void reset()
00155 {
00156 current_ = min_vel_;
00157 return_zero_ = true;
00158 return_zero_now_ = false;
00159 }
00160
00164 bool isFinished() const
00165 {
00166 return current_ > max_vel_ + EPSILON;
00167 }
00168
00169 private:
00170 bool return_zero_, return_zero_now_;
00171 double min_vel_, max_vel_;
00172 double current_;
00173 double increment_;
00174 };
00175 }
00176
00177 #endif // DWB_PLUGINS_ONE_D_VELOCITY_ITERATOR_H