Consider failed track or block traversal a fatal error

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

diff --git a/source/libmarklin/track.cpp b/source/libmarklin/track.cpp
index 01b789373c6fb655b712a6558533ec588adc08f9..3c2a410fa9d55bf2ab131985a76f7ea4266d4f1c 100644 (file)
--- a/source/libmarklin/track.cpp
+++ b/source/libmarklin/track.cpp
@@ -151,7 +151,12 @@ bool Track::snap_to(Track &other, bool link)
                        if(dx*dx+dy*dy<limit)
                        {
                                set_rotation(other.rot+other_eps[j].dir-eps[i].dir+M_PI);
-                               set_position(Point(epp2.x-(eps[i].pos.x*cos(rot)-eps[i].pos.y*sin(rot)), epp2.y-(eps[i].pos.y*cos(rot)+eps[i].pos.x*sin(rot)), epp2.z));
+                               Point p(epp2.x-(eps[i].pos.x*cos(rot)-eps[i].pos.y*sin(rot)),
+                                       epp2.y-(eps[i].pos.y*cos(rot)+eps[i].pos.x*sin(rot)),
+                                       epp2.z);
+                               if(eps.size()==2 && i==1)
+                                       p.z -= slope;
+                               set_position(p);
 
                                if(link)
                                {
@@ -218,7 +223,7 @@ Track *Track::get_link(unsigned i) const
        return links[i];
 }
 
-int Track::traverse(unsigned i, unsigned route) const
+unsigned Track::traverse(unsigned i, unsigned path) const
 {
        const vector<Endpoint> &eps = type.get_endpoints();
        if(i>=eps.size())
@@ -226,25 +231,25 @@ int Track::traverse(unsigned i, unsigned route) const
 
        const Endpoint &ep = eps[i];
        
-       if(ep.routes&(1<<route))
+       if(ep.paths&(1<<path))
        {
-               // Find the other endpoint for this route
+               // Find the other endpoint for this path
                for(unsigned j=0; j<eps.size(); ++j)
-                       if((eps[j].routes&(1<<route)) && j!=i)
+                       if((eps[j].paths&(1<<path)) && j!=i)
                                return j;
        }
        else
        {
-               // Find an endpoint that's connected to this one and has the requested route
+               // Find an endpoint that's connected to this one and has the requested path
                for(unsigned j=0; j<eps.size(); ++j)
-                       if((eps[j].routes&(1<<route)) && (eps[j].routes&ep.routes))
+                       if((eps[j].paths&(1<<path)) && (eps[j].paths&ep.paths))
                                return j;
        }
 
-       return -1;
+       throw Exception("Track endpoint did not have a counterpart");
 }
 
-Point Track::get_point(unsigned epi, unsigned route, float d) const
+Point Track::get_point(unsigned epi, unsigned path, float d) const
 {
        const vector<Endpoint> &eps = type.get_endpoints();
        if(epi>=eps.size())
@@ -252,6 +257,17 @@ Point Track::get_point(unsigned epi, unsigned route, float d) const
 
        float x = eps[epi].pos.x;
        float y = eps[epi].pos.y;
+       float z = 0;
+       float slope_norm = 0;
+       if(eps.size()==2)
+       {
+               slope_norm = slope/type.get_total_length();
+               if(epi==1)
+               {
+                       z = slope;
+                       slope_norm = -slope_norm;
+               }
+       }
 
        const vector<TrackPart> &parts = type.get_parts();
        const TrackPart *last_part = 0;
@@ -259,7 +275,7 @@ Point Track::get_point(unsigned epi, unsigned route, float d) const
        {
                for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
                {
-                       if((eps[epi].routes&(1<<route)) && i->route!=route)
+                       if((eps[epi].paths&(1<<path)) && i->path!=path)
                                continue;
                        if(&*i==last_part)
                                continue;
@@ -277,18 +293,20 @@ Point Track::get_point(unsigned epi, unsigned route, float d) const
                                                plen *= abs(i->radius);
                                        if(d<plen)
                                        {
+                                               z += d*slope_norm;
                                                if(j==1)
                                                        d = plen-d;
                                                Point p = i->get_point(d);
                                                float c = cos(rot);
                                                float s = sin(rot);
-                                               return Point(pos.x+c*p.x-s*p.y, pos.y+c*p.y+s*p.x);
+                                               return Point(pos.x+c*p.x-s*p.y, pos.y+c*p.y+s*p.x, pos.z+z);
                                        }
                                        else if(part_eps.size()>1)
                                        {
                                                d -= plen;
                                                x = part_eps[1-j].pos.x;
                                                y = part_eps[1-j].pos.y;
+                                               z += plen*slope_norm;
                                                last_part = &*i;
                                                i = parts.begin();
                                                break;