From: Mikko Rasa Date: Thu, 21 May 2009 16:57:02 +0000 (+0000) Subject: Attempt to estimate the exact positions of trains from measured speed data X-Git-Url: http://git.tdb.fi/?a=commitdiff_plain;h=319f90fd59587efc1242c34f307abc29f323642b;p=r2c2.git Attempt to estimate the exact positions of trains from measured speed data Improve track vertex array creation Use Point instead of separate coordinate members for Endpoint and TrackPart --- diff --git a/source/3d/track.cpp b/source/3d/track.cpp index d4b7079..5857e52 100644 --- a/source/3d/track.cpp +++ b/source/3d/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 */ @@ -14,8 +14,6 @@ Distributed under the GPL using namespace std; using namespace Msp; -#include - namespace Marklin { Track3D::Track3D(Track &t, unsigned q): @@ -96,10 +94,10 @@ void Track3D::render_endpoints() const float s=sin(ep.dir); glBegin(GL_QUADS); - glVertex3f(ep.x-s*0.025, ep.y+c*0.025, 0); - glVertex3f(ep.x+s*0.025, ep.y-c*0.025, 0); - glVertex3f(ep.x+s*0.025, ep.y-c*0.025, 0.02); - glVertex3f(ep.x-s*0.025, ep.y+c*0.025, 0.02); + glVertex3f(ep.pos.x-s*0.025, ep.pos.y+c*0.025, 0); + glVertex3f(ep.pos.x+s*0.025, ep.pos.y-c*0.025, 0); + glVertex3f(ep.pos.x+s*0.025, ep.pos.y-c*0.025, 0.02); + glVertex3f(ep.pos.x-s*0.025, ep.pos.y+c*0.025, 0.02); glEnd(); } @@ -173,63 +171,30 @@ void Track3D::build_part(const TrackPart &part, GL::VertexArrayBuilder &va_build } static unsigned psize=profile.size(); - const float &radius=part.radius; - const float &x=part.x; - const float &y=part.y; - const float &length=part.length; - const float &dir=part.dir; + unsigned nsegs=(part.radius ? static_cast(part.length*(1<(part.length*(1<=6) ? length/2 : length; - Point p(x+c*len*i-s*profile[j].y, y+s*len*i+c*profile[j].y, profile[j].z+i*track.get_slope()); - va_builder.vertex(p.x, p.y, p.z); - if(profile[j].z==0) - border.push_back(p); - } + unsigned k=j&~1; + float dy=profile[k+1].y-profile[k].y; + float dz=profile[k+1].z-profile[k].z; + float d=sqrt(dy*dy+dz*dz); + va_builder.normal(s*dz/d, -c*dz/d, dy/d); + + Point v(p.x+c*profile[j].x-s*profile[j].y, p.y+c*profile[j].y+s*profile[j].x, profile[j].z+i*track.get_slope()/nsegs); + va_builder.vertex(v.x, v.y, v.z); + if(profile[j].z==0) + border.push_back(v); } } diff --git a/source/engineer/engineer.cpp b/source/engineer/engineer.cpp index fc6d75d..5466302 100644 --- a/source/engineer/engineer.cpp +++ b/source/engineer/engineer.cpp @@ -263,6 +263,28 @@ void Engineer::tick() GL::pop_matrix(); } + const list &trains=trfc_mgr->get_trains(); + for(list::const_iterator i=trains.begin(); i!=trains.end(); ++i) + { + GL::PushMatrix _push; + + const Point &tp=(*i)->get_position(); + GL::translate(tp.x, tp.y, 0.02); + GL::Immediate imm((GL::COLOR4_UBYTE, GL::VERTEX2)); + imm.color(0.8f, 0.8f, 1.0f); + imm.begin(GL::TRIANGLE_FAN); + imm.vertex(0, 0); + for(unsigned j=0; j<=12; ++j) + imm.vertex(0.02*cos(j*M_PI/6), 0.02*sin(j*M_PI/6)); + imm.end(); + + GL::rotate(cam_rot*180/M_PI, 0, 0, 1); + GL::translate(0.03, -0.02, 0); + GL::scale_uniform(0.04); + ui_res.get_default_font().draw_string((*i)->get_name()); + GL::Texture::unbind(); + } + GL::matrix_mode(GL::PROJECTION); GL::load_identity(); GL::ortho_bottomleft(screen_w, screen_h); diff --git a/source/libmarklin/block.cpp b/source/libmarklin/block.cpp index 03cb2dc..94be126 100644 --- a/source/libmarklin/block.cpp +++ b/source/libmarklin/block.cpp @@ -5,16 +5,15 @@ Copyright © 2006-2009 Mikkosoft Productions, Mikko Rasa Distributed under the GPL */ +#include #include "control.h" #include "block.h" #include "tracktype.h" #include "trafficmanager.h" #include "turnout.h" -using namespace Msp; - -#include using namespace std; +using namespace Msp; namespace Marklin { @@ -135,10 +134,6 @@ bool Block::reserve(const Train *t) if(!t || !train) { train=t; - if(train) - cout<<"Block "<get_name()<<'\n'; - else - cout<<"Block "< &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 &parts=type.get_parts(); + const TrackPart *last_part=0; + while(1) + { + for(vector::const_iterator i=parts.begin(); i!=parts.end(); ++i) + { + if((eps[epi].routes&(1<route!=route) + continue; + if(&*i==last_part) + continue; + + vector part_eps; + i->collect_endpoints(part_eps); + for(unsigned j=0; jlength; + if(i->radius) + plen*=abs(i->radius); + if(dget_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); diff --git a/source/libmarklin/track.h b/source/libmarklin/track.h index c434224..11c9fb3 100644 --- a/source/libmarklin/track.h +++ b/source/libmarklin/track.h @@ -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 */ @@ -70,6 +70,7 @@ public: Track *get_link(unsigned) const; void check_slope(); int traverse(unsigned, unsigned) const; + Point get_point(unsigned, unsigned, float) const; /** Creates a copy of the track. The new track will be almost identical, but diff --git a/source/libmarklin/trackpart.cpp b/source/libmarklin/trackpart.cpp index 1ef6e75..046b927 100644 --- 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 &eps) +void TrackPart::collect_endpoints(vector &eps) const { - eps.push_back(Endpoint(x, y, dir+M_PI, 1< #include "endpoint.h" +#include "geometry.h" namespace Marklin { @@ -29,7 +30,7 @@ struct TrackPart void start(float, float, float); }; - float x, y; + Point pos; float dir; float length; float radius; @@ -38,7 +39,8 @@ struct TrackPart TrackPart(); - void collect_endpoints(std::vector &); + void collect_endpoints(std::vector &) const; + Point get_point(float) const; }; } // namespace Marklin diff --git a/source/libmarklin/tracktype.cpp b/source/libmarklin/tracktype.cpp index 0e84a31..2376e90 100644 --- a/source/libmarklin/tracktype.cpp +++ b/source/libmarklin/tracktype.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 */ @@ -56,8 +56,8 @@ void TrackType::collect_endpoints() bool rm=false; for(vector::iterator j=i+1; j!=endpoints.end();) { - float dx=i->x-j->x; - float dy=i->y-j->y; + float dx=i->pos.x-j->pos.x; + float dy=i->pos.y-j->pos.y; if(dx*dx+dy*dy<0.0001) { float da=i->dir-j->dir; diff --git a/source/libmarklin/trafficmanager.cpp b/source/libmarklin/trafficmanager.cpp index 4cc186b..6de46a2 100644 --- a/source/libmarklin/trafficmanager.cpp +++ b/source/libmarklin/trafficmanager.cpp @@ -75,9 +75,13 @@ void TrafficManager::add_train(Train *t) void TrafficManager::tick() { Time::TimeStamp t=Time::now(); + Time::TimeDelta dt; + if(last_tick) + dt=t-last_tick; + last_tick=t; for(list::iterator i=trains.begin(); i!=trains.end(); ++i) - (*i)->tick(t); + (*i)->tick(t, dt); } void TrafficManager::turnout_route_changed(unsigned, Turnout *) diff --git a/source/libmarklin/trafficmanager.h b/source/libmarklin/trafficmanager.h index 293783f..ac3f395 100644 --- a/source/libmarklin/trafficmanager.h +++ b/source/libmarklin/trafficmanager.h @@ -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 */ @@ -24,6 +24,7 @@ private: Layout &layout; std::list blocks; std::list trains; + Msp::Time::TimeStamp last_tick; public: sigc::signal signal_block_reserved; diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp index 4974318..f55d7ad 100644 --- a/source/libmarklin/train.cpp +++ b/source/libmarklin/train.cpp @@ -10,13 +10,14 @@ Distributed under the GPL #include #include #include "control.h" +#include "tracktype.h" #include "trafficmanager.h" #include "train.h" +using namespace std; using namespace Msp; #include -using namespace std; namespace Marklin { @@ -26,7 +27,11 @@ Train::Train(TrafficManager &tm, Locomotive &l): target_speed(0), status("Unplaced"), travel_dist(0), - real_speed(0) + travel_speed(0), + pure_speed(false), + speed_scale(0.02), + speed_scale_weight(0), + cur_track(0) { trfc_mgr.add_train(this); @@ -44,7 +49,8 @@ void Train::set_name(const string &n) void Train::set_speed(unsigned speed) { - unsigned old_speed=target_speed; + if(!target_speed && speed) + travel_speed=static_cast(round(speed*speed_scale*87*3.6/5))*5; target_speed=speed; if(!target_speed) @@ -53,32 +59,10 @@ void Train::set_speed(unsigned speed) for(list::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i) i->block->reserve(0); rsv_blocks.clear(); - try_reserve=Time::TimeStamp(); - loco.set_speed(0); - if(old_speed) - set_status("Stopped"); - } - else - { - unsigned n=reserve_more(); - if(n==0) - { - try_reserve=Time::now()+2*Time::sec; - set_status("Blocked"); - } - else if(n==1) - { - loco.set_speed(3); - try_reserve=Time::now()+2*Time::sec; - set_status("Slow"); - } - else - { - loco.set_speed(speed); - if(!old_speed) - set_status("Traveling --- kmh"); - } } + + update_speed(); + pure_speed=false; } void Train::place(Block *block, unsigned entry) @@ -102,6 +86,7 @@ void Train::place(Block *block, unsigned entry) } cur_blocks.push_back(BlockRef(block, entry)); + set_position(block->get_endpoints()[entry]); set_status("Stopped"); } @@ -122,25 +107,35 @@ bool Train::free_block(Block *block) return false; } -void Train::tick(const Time::TimeStamp &t) +void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt) { if(try_reserve && t>try_reserve) + update_speed(); + + if(cur_track) { - unsigned n=reserve_more(); - if(n>=2) - { - loco.set_speed(target_speed); - set_status("Traveling --- kmh"); - try_reserve=Time::TimeStamp(); - } - else if(n==1) + unsigned route=0; + if(cur_track->get_turnout_id()) + route=trfc_mgr.get_control().get_turnout(cur_track->get_turnout_id()).get_route(); + + offset+=speed_scale*loco.get_speed()*(dt/Time::sec); + if(offset>cur_track->get_type().get_route_length(route)) { - loco.set_speed(3); - set_status("Slow"); - try_reserve=t+2*Time::sec; + int out=cur_track->traverse(cur_track_ep, route); + if(out>=0) + { + Track *next=cur_track->get_link(out); + if(next) + cur_track_ep=next->get_endpoint_by_link(*cur_track); + cur_track=next; + offset=0; + } + else + cur_track=0; } - else - try_reserve=t+2*Time::sec; + + if(cur_track) + pos=cur_track->get_point(cur_track_ep, route, offset); } } @@ -157,52 +152,53 @@ void Train::sensor_event(bool state, Sensor *sensor) if(i!=rsv_blocks.begin()) { - real_speed=static_cast(round(travel_dist/((Time::now()-last_entry_time)/Time::sec)*87*3.6/5))*5; - set_status(format("Traveling %3d kmh", real_speed)); + float travel_time_secs=(Time::now()-last_entry_time)/Time::sec; + travel_speed=static_cast(round(travel_dist/travel_time_secs*87*3.6/5))*5; + + if(pure_speed) + { + float weight=loco.get_speed()*travel_dist; + if(weight) + { + weight*=weight; + float scale=travel_dist/travel_time_secs/loco.get_speed(); + cout<<"Updating speed_scale: "<::iterator j=rsv_blocks.begin(); j!=i; ++j) { j->block->traverse(j->entry, &block_len); - cout<<"Advancing: block "<block<<" (sensor "<block->get_sensor_id()<<") length "<block->get_sensor_id()==addr) + set_position(i->block->get_endpoints()[i->entry]); + if(target_speed) - { - unsigned n=reserve_more(); - if(n==0) - { - loco.set_speed(0); - try_reserve=Time::now()+2*Time::sec; - set_status("Blocked"); - } - else if(n==1) - { - loco.set_speed(3); - try_reserve=Time::now()+2*Time::sec; - set_status("Slow"); - } - } + update_speed(); } else { - cout<<"Train "<::iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i) if(i->block->get_sensor_id()==addr) { ++i; for(list::iterator j=cur_blocks.begin(); j!=i; ++j) j->block->reserve(0); - cout<<" "<::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i) if(i->block->get_sensor_id()) @@ -261,15 +255,55 @@ unsigned Train::reserve_more() last=0; } - cout<<" "<get_endpoint_position(cur_track_ep); +} + } // namespace Marklin diff --git a/source/libmarklin/train.h b/source/libmarklin/train.h index 43ee84d..6a7d2de 100644 --- a/source/libmarklin/train.h +++ b/source/libmarklin/train.h @@ -38,9 +38,18 @@ private: unsigned target_speed; Msp::Time::TimeStamp try_reserve; std::string status; + Msp::Time::TimeStamp last_entry_time; float travel_dist; - unsigned real_speed; + unsigned travel_speed; + bool pure_speed; + float speed_scale; + float speed_scale_weight; + + Track *cur_track; + unsigned cur_track_ep; + float offset; + Point pos; public: sigc::signal signal_name_changed; @@ -53,13 +62,16 @@ public: const std::string &get_name() const { return name; } Locomotive &get_locomotive() const { return loco; } const std::string &get_status() const { return status; } + const Point &get_position() const { return pos; } void place(Block *, unsigned); bool free_block(Block *); - void tick(const Msp::Time::TimeStamp &); + void tick(const Msp::Time::TimeStamp &, const Msp::Time::TimeDelta &); private: void sensor_event(bool, Sensor *); unsigned reserve_more(); + void update_speed(); void set_status(const std::string &); + void set_position(const Block::Endpoint &); }; } // namespace Marklin