Add Track::get_endpoint_position to avoid duplicating calculations

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

diff --git a/source/libmarklin/track.cpp b/source/libmarklin/track.cpp
index aaf1b75526b1564eb3b2aa6257ea930c41f9cc48..6f83e5b05238080c63a95fdbcd9d5182661f86f7 100644 (file)
--- a/source/libmarklin/track.cpp
+++ b/source/libmarklin/track.cpp
@@ -59,6 +59,13 @@ const Track::Endpoint *Track::get_endpoint_by_link(Track *other) const
        return 0;
 }
 
+Point Track::get_endpoint_position(const Endpoint &ep) const
+{
+       float c=cos(rot);
+       float s=sin(rot);
+       return Point(pos.x+c*ep.pos.x-s*ep.pos.y, pos.y+s*ep.pos.x+c*ep.pos.y, pos.z+ep.pos.z);
+}
+
 float Track::get_length() const
 {
        float len=parts.front().length;
@@ -94,21 +101,19 @@ bool Track::snap_to(Track &other, bool link)
        float limit=(link && !flex) ? 1e-6 : 1e-4;
        for(EndpointSeq::iterator i=endpoints.begin(); i!=endpoints.end(); ++i)
        {
-               float x=pos.x+i->pos.x*cos(rot)-i->pos.y*sin(rot);
-               float y=pos.y+i->pos.y*cos(rot)+i->pos.x*sin(rot);
+               Point epp=get_endpoint_position(*i);
                for(EndpointSeq::iterator j=other.endpoints.begin(); j!=other.endpoints.end(); ++j)
                {
                        if(j->link)
                                continue;
 
-                       float x2=other.pos.x+j->pos.x*cos(other.rot)-j->pos.y*sin(other.rot);
-                       float y2=other.pos.y+j->pos.y*cos(other.rot)+j->pos.x*sin(other.rot);
-                       float dx=x2-x;
-                       float dy=y2-y;
+                       Point epp2=other.get_endpoint_position(*j);
+                       float dx=epp2.x-epp.x;
+                       float dy=epp2.y-epp.y;
                        if(dx*dx+dy*dy<limit)
                        {
                                set_rotation(other.rot+j->rot-i->rot+M_PI);
-                               set_position(Point(x2-(i->pos.x*cos(rot)-i->pos.y*sin(rot)), y2-(i->pos.y*cos(rot)+i->pos.x*sin(rot)), other.pos.z+j->pos.z-i->pos.z));
+                               set_position(Point(epp2.x-(i->pos.x*cos(rot)-i->pos.y*sin(rot)), epp2.y-(i->pos.y*cos(rot)+i->pos.x*sin(rot)), other.pos.z+j->pos.z-i->pos.z));
                                if(link)
                                {
                                        if(i->link)
@@ -128,14 +133,12 @@ bool Track::snap(Point &pt, float &d) const
 {
        for(EndpointSeq::const_iterator i=endpoints.begin(); i!=endpoints.end(); ++i)
        {
-               float x=pos.x+i->pos.x*cos(rot)-i->pos.y*sin(rot);
-               float y=pos.y+i->pos.y*cos(rot)+i->pos.x*sin(rot);
-               float dx=pt.x-x;
-               float dy=pt.y-y;
+               Point epp=get_endpoint_position(*i);
+               float dx=pt.x-epp.x;
+               float dy=pt.y-epp.y;
                if(dx*dx+dy*dy<1e-4)
                {
-                       pt.x=x;
-                       pt.y=y;
+                       pt=epp;
                        d=rot+i->rot;
                        return true;
                }