/* $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
*/
using namespace std;
using namespace Msp;
-#include <iostream>
-
namespace Marklin {
Track3D::Track3D(Track &t, unsigned q):
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();
}
}
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<unsigned>(part.length*(1<<quality))+1 : 1);
+ float plen=part.length;
+ if(part.radius)
+ plen*=abs(part.radius);
- unsigned nsegs;
- if(radius)
- {
- nsegs=static_cast<unsigned>(part.length*(1<<quality))+1;
- Point center(x-sin(dir)*radius, y+cos(dir)*radius, 0);
- float r=fabs(radius);
- float start=((radius<0)?M_PI:0)+dir;
- float angle=(radius<0)?-length:length;
- int inv=(radius<0)?-1:1;
- for(unsigned i=0; i<=nsegs; ++i)
- {
- float a=start+i*angle/nsegs;
- float c=cos(a);
- float s=sin(a);
-
- for(unsigned j=0; j<profile.size(); ++j)
- {
- 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 p(center.x+s*(r-profile[j].y*inv), center.y-c*(r-profile[j].y*inv), profile[j].z+i*track.get_slope()/nsegs);
- va_builder.vertex(p.x, p.y, p.z);
- if(profile[j].z==0)
- border.push_back(p);
- }
- }
- }
- else
+ for(unsigned i=0; i<=nsegs; ++i)
{
- nsegs=1;
- float c=cos(dir);
- float s=sin(dir);
- for(unsigned i=0; i<2; ++i)
+ float a=part.dir+(part.radius ? i*plen/nsegs/part.radius : 0);
+ float c=cos(a);
+ float s=sin(a);
+ Point p=part.get_point(i*plen/nsegs);
+
+ for(unsigned j=0; j<profile.size(); ++j)
{
- for(unsigned j=0; j<profile.size(); ++j)
- {
- 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);
-
- float len=(part.dead_end && i==1 && j>=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);
}
}
GL::pop_matrix();
}
+ const list<Train *> &trains=trfc_mgr->get_trains();
+ for(list<Train *>::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);
Distributed under the GPL
*/
+#include <iostream>
#include "control.h"
#include "block.h"
#include "tracktype.h"
#include "trafficmanager.h"
#include "turnout.h"
-using namespace Msp;
-
-#include <iostream>
using namespace std;
+using namespace Msp;
namespace Marklin {
if(!t || !train)
{
train=t;
- if(train)
- cout<<"Block "<<this<<" reserved for train "<<train->get_name()<<'\n';
- else
- cout<<"Block "<<this<<" freed\n";
trfc_mgr.signal_block_reserved.emit(*this, train);
return true;
}
#ifndef MARKLIN_ENDPOINT_H_
#define MARKLIN_ENDPOINT_H_
+#include "geometry.h"
+
namespace Marklin {
struct Endpoint
{
- float x, y;
- float dir; // Direction outwards from the endpoint
+ Point pos;
+ float dir; // Direction outwards from the endpoint
unsigned routes;
- Endpoint(): x(0), y(0), dir(0), routes(0) { }
- Endpoint(float x_, float y_, float d, unsigned r): x(x_), y(y_), dir(d), routes(r) { }
+ Endpoint(): dir(0), routes(0) { }
+ Endpoint(float x, float y, float d, unsigned r): pos(x, y), dir(d), routes(r) { }
};
} // namespace Marklin
/* $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
*/
float x, y, z;
Point(): x(0), y(0), z(0) { }
+ Point(float x_, float y_): x(x_), y(y_), z(0) { }
Point(float x_, float y_, float z_): x(x_), y(y_), z(z_) { }
};
/* $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
*/
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;
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)
{
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);
/* $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
*/
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
/* $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
*/
namespace Marklin {
TrackPart::TrackPart():
- x(0),
- y(0),
dir(0),
length(0),
radius(0),
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);
}
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;
}
/* $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
*/
#include <msp/datafile/loader.h>
#include "endpoint.h"
+#include "geometry.h"
namespace Marklin {
void start(float, float, float);
};
- float x, y;
+ Point pos;
float dir;
float length;
float radius;
TrackPart();
- void collect_endpoints(std::vector<Endpoint> &);
+ void collect_endpoints(std::vector<Endpoint> &) const;
+ Point get_point(float) const;
};
} // namespace Marklin
/* $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
*/
bool rm=false;
for(vector<Endpoint>::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;
void TrafficManager::tick()
{
Time::TimeStamp t=Time::now();
+ Time::TimeDelta dt;
+ if(last_tick)
+ dt=t-last_tick;
+ last_tick=t;
for(list<Train *>::iterator i=trains.begin(); i!=trains.end(); ++i)
- (*i)->tick(t);
+ (*i)->tick(t, dt);
}
void TrafficManager::turnout_route_changed(unsigned, Turnout *)
/* $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
*/
Layout &layout;
std::list<Block *> blocks;
std::list<Train *> trains;
+ Msp::Time::TimeStamp last_tick;
public:
sigc::signal<void, const Block &, const Train *> signal_block_reserved;
#include <msp/time/units.h>
#include <msp/time/utils.h>
#include "control.h"
+#include "tracktype.h"
#include "trafficmanager.h"
#include "train.h"
+using namespace std;
using namespace Msp;
#include <iostream>
-using namespace std;
namespace Marklin {
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);
void Train::set_speed(unsigned speed)
{
- unsigned old_speed=target_speed;
+ if(!target_speed && speed)
+ travel_speed=static_cast<int>(round(speed*speed_scale*87*3.6/5))*5;
target_speed=speed;
if(!target_speed)
for(list<BlockRef>::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)
}
cur_blocks.push_back(BlockRef(block, entry));
+ set_position(block->get_endpoints()[entry]);
set_status("Stopped");
}
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);
}
}
if(i!=rsv_blocks.begin())
{
- real_speed=static_cast<int>(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<int>(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: "<<speed_scale<<'x'<<speed_scale_weight<<" + "<<scale<<'x'<<weight<<'\n';
+ speed_scale=(speed_scale*speed_scale_weight+scale*weight)/(speed_scale_weight+weight);
+ speed_scale_weight+=weight;
+ cout<<" Result: "<<speed_scale<<'x'<<speed_scale_weight<<'\n';
+ }
+ }
travel_dist=0;
float block_len;
for(list<BlockRef>::iterator j=rsv_blocks.begin(); j!=i; ++j)
{
j->block->traverse(j->entry, &block_len);
- cout<<"Advancing: block "<<j->block<<" (sensor "<<j->block->get_sensor_id()<<") length "<<block_len<<'\n';
travel_dist+=block_len;
}
last_entry_time=Time::now();
+ pure_speed=true;
cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
- cout<<"Train "<<name<<" advanced, "<<cur_blocks.size()<<" cur_blocks, "<<rsv_blocks.size()<<" rsv_blocks\n";
}
+ for(i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
+ if(i->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 "<<name<<" finding blocks to free\n";
for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
if(i->block->get_sensor_id()==addr)
{
++i;
for(list<BlockRef>::iterator j=cur_blocks.begin(); j!=i; ++j)
j->block->reserve(0);
- cout<<" "<<distance(cur_blocks.begin(), i)<<" blocks freed, ";
cur_blocks.erase(cur_blocks.begin(), i);
- cout<<cur_blocks.size()<<" cur_blocks\n";
+ break;
}
if(target_speed)
if(!last)
return 0;
- cout<<"Train "<<name<<" reserving more blocks\n";
-
unsigned nsens=0;
for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
if(i->block->get_sensor_id())
last=0;
}
- cout<<" "<<rsv_blocks.size()<<" rsv_blocks\n";
-
return nsens;
}
+void Train::update_speed()
+{
+ if(!target_speed)
+ {
+ loco.set_speed(0);
+ try_reserve=Time::TimeStamp();
+ set_status("Stopped");
+ }
+ else
+ {
+ unsigned n=reserve_more();
+ if(n==0)
+ {
+ loco.set_speed(0);
+ pure_speed=false;
+ try_reserve=Time::now()+2*Time::sec;
+ set_status("Blocked");
+ }
+ else if(n==1)
+ {
+ loco.set_speed(3);
+ pure_speed=false;
+ try_reserve=Time::now()+2*Time::sec;
+ set_status("Slow");
+ }
+ else
+ {
+ loco.set_speed(target_speed);
+ try_reserve=Time::TimeStamp();
+ set_status(format("Traveling %d kmh", travel_speed));
+ }
+ }
+}
+
void Train::set_status(const string &s)
{
status=s;
signal_status_changed.emit(s);
}
+void Train::set_position(const Block::Endpoint &bep)
+{
+ cur_track=bep.track;
+ cur_track_ep=bep.track_ep;
+ offset=0;
+ pos=cur_track->get_endpoint_position(cur_track_ep);
+}
+
} // namespace Marklin
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<void, const std::string &> signal_name_changed;
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
projects / r2c2.git / commitdiff