Attempt to estimate the exact positions of trains from measured speed data

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

diff --git a/source/libmarklin/trackpart.cpp b/source/libmarklin/trackpart.cpp
index 1ef6e753d509d562692e99c7786a6aaa214fe884..046b9276ac23025a34ac586ef2509d74ca2b1d7f 100644 (file)
--- a/source/libmarklin/trackpart.cpp
+++ b/source/libmarklin/trackpart.cpp
@@ -1,7 +1,7 @@
 /* $Id$
 
 This file is part of the MSP Märklin suite
-Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
+Copyright © 2006-2009 Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
@@ -13,8 +13,6 @@ using namespace std;
 namespace Marklin {
 
 TrackPart::TrackPart():
-       x(0),
-       y(0),
        dir(0),
        length(0),
        radius(0),
@@ -22,23 +20,35 @@ TrackPart::TrackPart():
        dead_end(false)
 { }
 
-void TrackPart::collect_endpoints(vector<Endpoint> &eps)
+void TrackPart::collect_endpoints(vector<Endpoint> &eps) const
 {
-       eps.push_back(Endpoint(x, y, dir+M_PI, 1<<route));
+       eps.push_back(Endpoint(pos.x, pos.y, dir+M_PI, 1<<route));
 
        if(dead_end)
                ;
        else if(radius)
        {
                float a=((radius<0) ? -length : length);
+               Point p=get_point(length*abs(radius));
+               eps.push_back(Endpoint(p.x, p.y, dir+a, 1<<route));
+       }
+       else
+               eps.push_back(Endpoint(pos.x+cos(dir)*length, pos.y+sin(dir)*length, dir, 1<<route));
+}
+
+Point TrackPart::get_point(float d) const
+{
+       if(radius)
+       {
+               float a=d/radius;
                float c=cos(a);
                float s=sin(a);
                float rx=radius*sin(dir);
                float ry=-radius*cos(dir);
-               eps.push_back(Endpoint(x+c*rx-s*ry-rx, y+c*ry+s*rx-ry, dir+a, 1<<route));
+               return Point(pos.x+c*rx-s*ry-rx, pos.y+c*ry+s*rx-ry);
        }
        else
-               eps.push_back(Endpoint(x+cos(dir)*length, y+sin(dir)*length, dir, 1<<route));
+               return Point(pos.x+cos(dir)*d, pos.y+sin(dir)*d);
 }
 
 
@@ -62,15 +72,14 @@ void TrackPart::Loader::finish()
        else
                part.length/=1000;
 
-       part.x/=1000;
-       part.y/=1000;
+       part.pos.x/=1000;
+       part.pos.y/=1000;
        part.dir*=M_PI/180;
 }
 
 void TrackPart::Loader::start(float x, float y, float d)
 {
-       part.x=x;
-       part.y=y;
+       part.pos=Point(x, y);
        part.dir=d;
 }