Support setting routes for trains

[r2c2.git] / source / libmarklin / train.cpp

diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index e904ebfc5f94b8152890c47c68a9b77cc4ebf189..12c4c26586a89df5ffa1422abf317b314b8e7ae2 100644 (file)
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -11,6 +11,7 @@ Distributed under the GPL
 #include <msp/time/utils.h>
 #include "control.h"
 #include "except.h"
+#include "route.h"
 #include "tracktype.h"
 #include "trafficmanager.h"
 #include "train.h"
@@ -80,6 +81,12 @@ void Train::set_reverse(bool rev)
        loco.set_reverse(rev);
 }
 
+void Train::set_route(const Route *r)
+{
+       route = r;
+       signal_route_changed.emit(route);
+}
+
 void Train::place(Block *block, unsigned entry)
 {
        for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end();)
@@ -321,6 +328,12 @@ unsigned Train::reserve_more()
                        Block *link = last->block->get_link(exit);
                        if(link && link->reserve(this))
                        {
+                               if(route && link->get_turnout_id())
+                               {
+                                       int path = route->get_turnout(link->get_turnout_id());
+                                       if(path>=0)
+                                               trfc_mgr.get_control().get_turnout(link->get_turnout_id()).set_path(path);
+                               }
                                rsv_blocks.push_back(BlockRef(link, link->get_endpoint_by_link(*last->block)));
                                last = &rsv_blocks.back();
                                if(last->block->get_sensor_id())
@@ -422,14 +435,23 @@ unsigned Train::find_speed(float real) const
 {
        if(real<=real_speed[0].speed)
                return 0;
-       if(real>=real_speed[14].speed)
-               return 14;
 
        unsigned low = 0;
        unsigned high = 0;
-       for(high=0; real_speed[high].speed<real; ++high)
-               if(real_speed[high].weight)
-                       low = high;
+       for(unsigned i=0; (!high && i<=14); ++i)
+               if(real_speed[i].weight)
+               {
+                       if(real_speed[i].speed<real)
+                               low = i;
+                       else
+                               high = i;
+               }
+       if(!high)
+       {
+               if(!low)
+                       return 0;
+               return min(static_cast<unsigned>(low*real/real_speed[low].speed), 14U);
+       }
 
        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);