source/libr2c2/speedquantizer.cpp

   1 #include "speedquantizer.h"
   2
   3 using namespace std;
   4 using namespace Msp;
   5
   6 namespace R2C2 {
   7
   8 SpeedQuantizer::SpeedQuantizer(unsigned n):
   9         steps(n+1),
  10         weight_limit(10)
  11 {
  12         if(n<1)
  13                 throw invalid_argument("SpeedQuantizer::SpeedQuantizer");
  14 }
  15
  16 void SpeedQuantizer::learn(unsigned i, float s, float w)
  17 {
  18         if(i>=steps.size())
  19                 throw out_of_range("SpeedQuantizer::learn");
  20         steps[i].add(s, w);
  21 }
  22
  23 float SpeedQuantizer::get_speed(unsigned i) const
  24 {
  25         if(i>=steps.size())
  26                 throw out_of_range("SpeedQuantizer::get_speed");
  27         if(i==0)
  28                 return 0;
  29         if(steps[i].weight>=weight_limit)
  30                 return steps[i].speed;
  31
  32         unsigned low;
  33         unsigned high;
  34         for(low=i; low>0; --low)
  35                 if(steps[low].weight>=weight_limit)
  36                         break;
  37         for(high=i; high+1<steps.size(); ++high)
  38                 if(steps[high].weight>=weight_limit)
  39                         break;
  40
  41         if(steps[high].weight)
  42         {
  43                 if(steps[low].weight)
  44                         return (steps[low].speed*(high-i)+steps[high].speed*(i-low))/(high-low);
  45                 else
  46                         return steps[high].speed*i/high;
  47         }
  48         else if(steps[low].weight)
  49                 return steps[low].speed*i/low;
  50         else
  51                 return 0;
  52 }
  53
  54 float SpeedQuantizer::quantize_speed(float speed) const
  55 {
  56         return get_speed(find_speed_step(speed));
  57 }
  58
  59 unsigned SpeedQuantizer::find_speed_step(float speed) const
  60 {
  61         if(speed<=steps[1].speed*0.5)
  62                 return 0;
  63
  64         unsigned low = 0;
  65         unsigned high = 0;
  66         unsigned last = 0;
  67         for(unsigned i=0; (!high && i<steps.size()); ++i)
  68                 if(steps[i].weight>=weight_limit)
  69                 {
  70                         last = i;
  71                         if(steps[i].speed>=speed)
  72                                 high = i;
  73                         else if(steps[i].speed>steps[low].speed)
  74                                 low = i;
  75                 }
  76
  77         if(!high)
  78         {
  79                 unsigned limit = steps.size()/5;
  80                 if(!low)
  81                 {
  82                         if(speed)
  83                                 return limit;
  84                         else
  85                                 return 0;
  86                 }
  87                 return min(min<unsigned>(low*speed/steps[low].speed, steps.size()-1), last+limit);
  88         }
  89
  90         float f = (speed-steps[low].speed)/(steps[high].speed-steps[low].speed);
  91         return static_cast<unsigned>(low*(1-f)+high*f+0.5);
  92 }
  93
  94 void SpeedQuantizer::save(list<DataFile::Statement> &st) const
  95 {
  96         for(unsigned i=0; i<steps.size(); ++i)
  97                 if(steps[i].weight)
  98                         st.push_back((DataFile::Statement("step"), i, steps[i].speed, steps[i].weight));
  99 }
 100
 101
 102 SpeedQuantizer::SpeedStep::SpeedStep():
 103         speed(0),
 104         weight(0)
 105 { }
 106
 107 void SpeedQuantizer::SpeedStep::add(float s, float w)
 108 {
 109         speed = (speed*weight+s*w)/(weight+w);
 110         weight = min(weight+w, 300.0f);
 111 }
 112
 113
 114 SpeedQuantizer::Loader::Loader(SpeedQuantizer &q):
 115         DataFile::ObjectLoader<SpeedQuantizer>(q)
 116 {
 117         add("step", &Loader::step);
 118 }
 119
 120 void SpeedQuantizer::Loader::step(unsigned i, float s, float w)
 121 {
 122         if(i>=obj.steps.size())
 123                 return;
 124
 125         obj.steps[i].speed = s;
 126         obj.steps[i].weight = w;
 127 }
 128
 129 } // namespace R2C2