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

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

diff --git a/source/libmarklin/track.cpp b/source/libmarklin/track.cpp
index 61f0d350e60f31e6af3ef0e432c23970dcaeece5..e544081d04f6754f6c9074245df581e358c3b8f2 100644 (file)
--- a/source/libmarklin/track.cpp
+++ b/source/libmarklin/track.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
 */
 
@@ -87,7 +87,7 @@ Point Track::get_endpoint_position(unsigned epi) const
        float c=cos(rot);
        float s=sin(rot);
 
-       Point p(pos.x+c*ep.x-s*ep.y, pos.y+s*ep.x+c*ep.y, pos.z);
+       Point p(pos.x+c*ep.pos.x-s*ep.pos.y, pos.y+s*ep.pos.x+c*ep.pos.y, pos.z);
        if(eps.size()==2 && epi==1)
                p.z+=slope;
        return p;
@@ -125,7 +125,7 @@ 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].x*cos(rot)-eps[i].y*sin(rot)), epp2.y-(eps[i].y*cos(rot)+eps[i].x*sin(rot)), epp2.z));
+                               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));
 
                                if(link)
                                {
@@ -246,6 +246,68 @@ int Track::traverse(unsigned i, unsigned route) const
        return -1;
 }
 
+Point Track::get_point(unsigned epi, unsigned route, float d) const
+{
+       const vector<Endpoint> &eps=type.get_endpoints();
+       if(epi>=eps.size())
+               throw InvalidParameterValue("Endpoint index out of range");
+
+       float x=eps[epi].pos.x;
+       float y=eps[epi].pos.y;
+
+       const vector<TrackPart> &parts=type.get_parts();
+       const TrackPart *last_part=0;
+       while(1)
+       {
+               for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
+               {
+                       if((eps[epi].routes&(1<<route)) && i->route!=route)
+                               continue;
+                       if(&*i==last_part)
+                               continue;
+
+                       vector<Endpoint> part_eps;
+                       i->collect_endpoints(part_eps);
+                       for(unsigned j=0; j<part_eps.size(); ++j)
+                       {
+                               float dx=part_eps[j].pos.x-x;
+                               float dy=part_eps[j].pos.y-y;
+                               if(dx*dx+dy*dy<1e-6)
+                               {
+                                       float plen=i->length;
+                                       if(i->radius)
+                                               plen*=abs(i->radius);
+                                       if(d<plen)
+                                       {
+                                               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);
+                                       }
+                                       else if(part_eps.size()>1)
+                                       {
+                                               d-=plen;
+                                               x=part_eps[1-j].pos.x;
+                                               y=part_eps[1-j].pos.y;
+                                               last_part=&*i;
+                                               i=parts.begin();
+                                               break;
+                                       }
+                                       else
+                                               return pos;
+                               }
+                       }
+               }
+
+               if(!last_part)
+                       throw Exception("Internal error (Endpoint does not match any part)");
+               else
+                       return pos;
+       }
+}
+
 Track *Track::copy() const
 {
        Track *trk=new Track(type);