Take z coordinate into account when computing points on tracks

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

diff --git a/source/libmarklin/track.cpp b/source/libmarklin/track.cpp
index d2822f61efacbf92d1af2bccb9a531b533dadc37..6c72c469a7540972f540e029d68bece473963c09 100644 (file)
--- a/source/libmarklin/track.cpp
+++ b/source/libmarklin/track.cpp
@@ -257,6 +257,17 @@ Point Track::get_point(unsigned epi, unsigned path, 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;
@@ -282,18 +293,20 @@ Point Track::get_point(unsigned epi, unsigned path, 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;