Move speed quantization into a separate class

diff --git a/source/libr2c2/speedquantizer.cpp b/source/libr2c2/speedquantizer.cpp
new file mode 100644 (file)
index 0000000..710e104
--- /dev/null
+++ b/source/libr2c2/speedquantizer.cpp
@@ -0,0 +1,133 @@
+/* $Id$
+
+This file is part of R²C²
+Copyright © 2011  Mikkosoft Productions, Mikko Rasa
+Distributed under the GPL
+*/
+
+#include "speedquantizer.h"
+
+using namespace std;
+using namespace Msp;
+
+namespace R2C2 {
+
+SpeedQuantizer::SpeedQuantizer(unsigned n):
+       steps(n+1)
+{
+       if(n<1)
+               throw InvalidParameterValue("Must have at leats one speed step");
+}
+
+void SpeedQuantizer::learn(unsigned i, float s, float w)
+{
+       if(i>=steps.size())
+               throw InvalidParameterValue("Speed step index out of range");
+       steps[i].add(s, w);
+}
+
+float SpeedQuantizer::get_speed(unsigned i) const
+{
+       if(i==0)
+               return 0;
+       if(steps[i].weight)
+               return steps[i].speed;
+
+       unsigned low;
+       unsigned high;
+       for(low=i; low>0; --low)
+               if(steps[low].weight)
+                       break;
+       for(high=i; high+1<steps.size(); ++high)
+               if(steps[high].weight)
+                       break;
+
+       if(steps[high].weight)
+       {
+               if(steps[low].weight)
+                       return (steps[low].speed*(high-i)+steps[high].speed*(i-low))/(high-low);
+               else
+                       return steps[high].speed*i/high;
+       }
+       else if(steps[low].weight)
+               return steps[low].speed*i/low;
+       else
+               return 0;
+}
+
+float SpeedQuantizer::quantize_speed(float speed) const
+{
+       return get_speed(find_speed_step(speed));
+}
+
+unsigned SpeedQuantizer::find_speed_step(float speed) const
+{
+       if(speed<=steps[1].speed*0.5)
+               return 0;
+
+       unsigned low = 0;
+       unsigned high = 0;
+       unsigned last = 0;
+       for(unsigned i=0; (!high && i<steps.size()); ++i)
+               if(steps[i].weight)
+               {
+                       last = i;
+                       if(steps[i].speed>=speed)
+                               high = i;
+                       else if(steps[i].speed>steps[low].speed)
+                               low = i;
+               }
+
+       if(!high)
+       {
+               unsigned limit = steps.size()/5;
+               if(!low)
+               {
+                       if(speed)
+                               return limit;
+                       else
+                               return 0;
+               }
+               return min(min(static_cast<unsigned>(low*speed/steps[low].speed), steps.size()-1), last+limit);
+       }
+
+       float f = (speed-steps[low].speed)/(steps[high].speed-steps[low].speed);
+       return static_cast<unsigned>(low*(1-f)+high*f+0.5);
+}
+
+void SpeedQuantizer::save(list<DataFile::Statement> &st) const
+{
+       for(unsigned i=0; i<steps.size(); ++i)
+               if(steps[i].weight)
+                       st.push_back((DataFile::Statement("step"), i, steps[i].speed, steps[i].weight));
+}
+
+
+SpeedQuantizer::SpeedStep::SpeedStep():
+       speed(0),
+       weight(0)
+{ }
+
+void SpeedQuantizer::SpeedStep::add(float s, float w)
+{
+       speed = (speed*weight+s*w)/(weight+w);
+       weight = min(weight+w, 300.0f);
+}
+
+
+SpeedQuantizer::Loader::Loader(SpeedQuantizer &q):
+       DataFile::ObjectLoader<SpeedQuantizer>(q)
+{
+       add("step", &Loader::step);
+}
+
+void SpeedQuantizer::Loader::step(unsigned i, float s, float w)
+{
+       if(i>=obj.steps.size())
+               return;
+
+       obj.steps[i].speed = s;
+       obj.steps[i].weight = w;
+}
+
+} // namespace R2C2

diff --git a/source/libr2c2/speedquantizer.h b/source/libr2c2/speedquantizer.h
new file mode 100644 (file)
index 0000000..d6dc1ac
--- /dev/null
+++ b/source/libr2c2/speedquantizer.h
@@ -0,0 +1,53 @@
+/* $Id$
+
+This file is part of R²C²
+Copyright © 2011  Mikkosoft Productions, Mikko Rasa
+Distributed under the GPL
+*/
+
+#ifndef LIBR2C2_SPEEDQUANTIZER_H_
+#define LIBR2C2_SPEEDQUANTIZER_H_
+
+#include <list>
+#include <vector>
+#include <msp/datafile/objectloader.h>
+
+namespace R2C2 {
+
+class SpeedQuantizer
+{
+public:
+       class Loader: public Msp::DataFile::ObjectLoader<SpeedQuantizer>
+       {
+       public:
+               Loader(SpeedQuantizer &);
+       private:
+               void step(unsigned, float, float);
+       };
+
+private:
+       struct SpeedStep
+       {
+               float speed;
+               float weight;
+
+               SpeedStep();
+               void add(float, float);
+       };
+
+       std::vector<SpeedStep> steps;
+
+public:
+       SpeedQuantizer(unsigned);
+
+       void learn(unsigned, float, float);
+       float get_speed(unsigned) const;
+       float quantize_speed(float) const;
+       unsigned find_speed_step(float) const;
+
+       void save(std::list<Msp::DataFile::Statement> &) const;
+};
+
+} // namespace R2C2
+
+#endif

diff --git a/source/libr2c2/train.cpp b/source/libr2c2/train.cpp
index ea4599fa9545eb2465d9ba2d0a1a5c85efc583d5..16077ef8e4878823fe406e6098911e2ba6ee0e4b 100644 (file)
--- a/source/libr2c2/train.cpp
+++ b/source/libr2c2/train.cpp
@@ -1,7 +1,7 @@
 /* $Id$
 
 This file is part of R²C²
-Copyright © 2006-2010  Mikkosoft Productions, Mikko Rasa
+Copyright © 2006-2011  Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
@@ -15,6 +15,7 @@ Distributed under the GPL
 #include "layout.h"
 #include "route.h"
 #include "simplecontroller.h"
+#include "speedquantizer.h"
 #include "timetable.h"
 #include "trackiter.h"
 #include "tracktype.h"
@@ -65,13 +66,17 @@ Train::Train(Layout &l, const VehicleType &t, unsigned a, const string &p):
        status("Unplaced"),
        travel_dist(0),
        pure_speed(false),
-       real_speed(layout.get_driver().get_protocol_speed_steps(protocol)+1),
+       speed_quantizer(0),
        accurate_position(false),
        overshoot_dist(false)
 {
        if(!loco_type.is_locomotive())
                throw InvalidParameterValue("Initial vehicle must be a locomotive");
 
+       unsigned speed_steps = layout.get_driver().get_protocol_speed_steps(protocol);
+       if(speed_steps)
+               speed_quantizer = new SpeedQuantizer(speed_steps);
+
        vehicles.push_back(new Vehicle(layout, loco_type));
 
        layout.add_train(*this);
@@ -558,7 +563,6 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
                timetable->tick(t);
        controller->tick(dt);
        float speed = controller->get_speed();
-       unsigned speed_step = find_speed_step(speed);
 
        if(controller->get_reverse()!=reverse)
        {
@@ -570,17 +574,24 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 
                reserve_more();
        }
-       if(speed_step!=current_speed_step && !speed_changing && !driver.is_halted() && driver.get_power())
+
+       if(speed_quantizer)
        {
-               speed_changing = true;
-               driver.set_loco_speed(address, speed_step);
+               unsigned speed_step = speed_quantizer->find_speed_step(speed);
+               if(speed_step!=current_speed_step && !speed_changing && !driver.is_halted() && driver.get_power())
+               {
+                       speed_changing = true;
+                       driver.set_loco_speed(address, speed_step);
 
-               pure_speed = false;
+                       pure_speed = false;
 
-               if(speed_step)
-                       set_status(format("Traveling %d kmh", get_travel_speed()));
-               else
-                       set_status("Waiting");
+                       if(speed_step)
+                               set_status(format("Traveling %d kmh", get_travel_speed()));
+                       else
+                               set_status("Waiting");
+               }
+
+               speed = speed_quantizer->get_speed(current_speed_step);
        }
 
        if(speed)
@@ -595,11 +606,7 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
                for(BlockList::const_iterator i=blocks.begin(); (!ok && i!=cur_blocks_end); ++i)
                        ok = (*i)->has_track(*track);
 
-               float d;
-               if(real_speed.size()>1)
-                       d = get_real_speed(current_speed_step)*(dt/Time::sec);
-               else
-                       d = speed*(dt/Time::sec);
+               float d = speed*(dt/Time::sec);
                if(ok)
                {
                        SetFlag setf(advancing);
@@ -644,9 +651,12 @@ void Train::save(list<DataFile::Statement> &st) const
                if(i!=vehicles.begin())
                        st.push_back((DataFile::Statement("vehicle"), (*i)->get_type().get_article_number()));
 
-       for(unsigned i=0; i<real_speed.size(); ++i)
-               if(real_speed[i].weight)
-                       st.push_back((DataFile::Statement("real_speed"), i, real_speed[i].speed, real_speed[i].weight));
+       if(speed_quantizer)
+       {
+               DataFile::Statement ss("quantized_speed");
+               speed_quantizer->save(ss.sub);
+               st.push_back(ss);
+       }
 
        if(!blocks.empty() && cur_blocks_end!=blocks.begin())
        {
@@ -737,11 +747,8 @@ void Train::sensor_event(unsigned addr, bool state)
 
                        if(pure_speed)
                        {
-                               if(current_speed_step>0)
-                               {
-                                       RealSpeed &rs = real_speed[current_speed_step];
-                                       rs.add(travel_dist/travel_time_secs, travel_time_secs);
-                               }
+                               if(speed_quantizer && current_speed_step>0)
+                                       speed_quantizer->learn(current_speed_step, travel_dist/travel_time_secs, travel_time_secs);
                                set_status(format("Traveling %d kmh", get_travel_speed()));
                        }
 
@@ -1143,77 +1150,13 @@ float Train::get_reserved_distance_until(const Block *until_block, bool back) co
        return result;
 }
 
-float Train::get_real_speed(unsigned i) const
-{
-       if(i==0)
-               return 0;
-       if(real_speed[i].weight)
-               return real_speed[i].speed;
-
-       unsigned low;
-       unsigned high;
-       for(low=i; low>0; --low)
-               if(real_speed[low].weight)
-                       break;
-       for(high=i; high+1<real_speed.size(); ++high)
-               if(real_speed[high].weight)
-                       break;
-
-       if(real_speed[high].weight)
-       {
-               if(real_speed[low].weight)
-               {
-                       float f = float(i-low)/(high-low);
-                       return real_speed[low].speed*(1-f)+real_speed[high].speed*f;
-               }
-               else
-                       return real_speed[high].speed*float(i)/high;
-       }
-       else if(real_speed[low].weight)
-               return real_speed[low].speed*float(i)/low;
-       else
-               return 0;
-}
-
-unsigned Train::find_speed_step(float real) const
-{
-       if(real_speed.size()<=1)
-               return 0;
-       if(real<=real_speed[1].speed*0.5)
-               return 0;
-
-       unsigned low = 0;
-       unsigned high = 0;
-       unsigned last = 0;
-       for(unsigned i=0; (!high && i<real_speed.size()); ++i)
-               if(real_speed[i].weight)
-               {
-                       last = i;
-                       if(real_speed[i].speed>=real)
-                               high = i;
-                       else if(real_speed[i].speed>real_speed[low].speed)
-                               low = i;
-               }
-       if(!high)
-       {
-               unsigned limit = real_speed.size()/5;
-               if(!low)
-               {
-                       if(real)
-                               return limit;
-                       else
-                               return 0;
-               }
-               return min(min(static_cast<unsigned>(low*real/real_speed[low].speed), real_speed.size()-1), last+limit);
-       }
-
-       float f = (real-real_speed[low].speed)/(real_speed[high].speed-real_speed[low].speed);
-       return static_cast<unsigned>(low*(1-f)+high*f+0.5);
-}
-
 float Train::get_travel_speed() const
 {
-       float speed = get_real_speed(current_speed_step);
+       float speed = 0;
+       if(speed_quantizer)
+               speed = speed_quantizer->get_speed(current_speed_step);
+       else
+               speed = controller->get_speed();
        float scale = layout.get_catalogue().get_scale();
        return static_cast<int>(round(speed/scale*3.6/5))*5;
 }
@@ -1348,18 +1291,6 @@ Train::RouteRef::RouteRef(const Route *r, unsigned d):
 { }
 
 
-Train::RealSpeed::RealSpeed():
-       speed(0),
-       weight(0)
-{ }
-
-void Train::RealSpeed::add(float s, float w)
-{
-       speed = (speed*weight+s*w)/(weight+w);
-       weight = min(weight+w, 300.0f);
-}
-
-
 Train::Loader::Loader(Train &t):
        DataFile::BasicLoader<Train>(t),
        prev_block(0),
@@ -1369,7 +1300,7 @@ Train::Loader::Loader(Train &t):
        add("block_hint",  &Loader::block_hint);
        add("name",        &Loader::name);
        add("priority",    &Train::priority);
-       add("real_speed",  &Loader::real_speed);
+       add("quantized_speed",  &Loader::quantized_speed);
        add("route",       &Loader::route);
        add("timetable",   &Loader::timetable);
        add("vehicle",     &Loader::vehicle);
@@ -1435,12 +1366,10 @@ void Train::Loader::name(const string &n)
        obj.set_name(n);
 }
 
-void Train::Loader::real_speed(unsigned i, float speed, float weight)
+void Train::Loader::quantized_speed()
 {
-       if(i>=obj.real_speed.size())
-               return;
-       obj.real_speed[i].speed = speed;
-       obj.real_speed[i].weight = weight;
+       if(obj.speed_quantizer)
+               load_sub(*obj.speed_quantizer);
 }
 
 void Train::Loader::route(const string &n)

diff --git a/source/libr2c2/train.h b/source/libr2c2/train.h
index 48ee867a597c7c22e56045dc88df27a7b29bf88c..b72fbf635c1d613c8e7f4aab07333dc4fb29eed3 100644 (file)
--- a/source/libr2c2/train.h
+++ b/source/libr2c2/train.h
@@ -1,7 +1,7 @@
 /* $Id$
 
 This file is part of R²C²
-Copyright © 2006-2010  Mikkosoft Productions, Mikko Rasa
+Copyright © 2006-2011  Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
@@ -19,6 +19,7 @@ namespace R2C2 {
 
 class ArticleNumber;
 class Route;
+class SpeedQuantizer;
 class Timetable;
 class Vehicle;
 class VehicleType;
@@ -40,7 +41,7 @@ public:
                void block(unsigned);
                void block_hint(unsigned);
                void name(const std::string &);
-               void real_speed(unsigned, float, float);
+               void quantized_speed();
                void route(const std::string &);
                void timetable();
                void vehicle(ArticleNumber);
@@ -63,15 +64,6 @@ private:
                RouteRef(const Route *, unsigned = 0);
        };
 
-       struct RealSpeed
-       {
-               float speed;
-               float weight;
-
-               RealSpeed();
-               void add(float, float);
-       };
-
        typedef std::list<BlockIter> BlockList;
 
        Layout &layout;
@@ -104,7 +96,7 @@ private:
        Msp::Time::TimeStamp last_entry_time;
        float travel_dist;
        bool pure_speed;
-       std::vector<RealSpeed> real_speed;
+       SpeedQuantizer *speed_quantizer;
        bool accurate_position;
        float overshoot_dist;