source/libr2c2/signal.cpp

   1 #include "blockiter.h"
   2 #include "driver.h"
   3 #include "layout.h"
   4 #include "signal.h"
   5 #include "signaltype.h"
   6 #include "trackiter.h"
   7 #include "tracktype.h"
   8 #include "train.h"
   9
  10 using namespace std;
  11 using namespace Msp;
  12
  13 namespace R2C2 {
  14
  15 Signal::Signal(Layout &l, const SignalType &t):
  16         Object(l),
  17         type(t),
  18         address(0),
  19         track(0),
  20         block(0),
  21         entry(0),
  22         train(0),
  23         check_allocated_blocks(false),
  24         passing(false)
  25 {
  26         layout.add_signal(*this);
  27
  28         layout.signal_block_reserved.connect(sigc::mem_fun(this, &Signal::block_reserved));
  29 }
  30
  31 Signal::~Signal()
  32 {
  33         layout.remove_signal(*this);
  34 }
  35
  36 Signal *Signal::clone(Layout *to_layout) const
  37 {
  38         Signal *sig = new Signal((to_layout ? *to_layout : layout), type);
  39         sig->set_position(position);
  40         sig->set_rotation(rotation);
  41         return sig;
  42 }
  43
  44 void Signal::set_address(unsigned a)
  45 {
  46         address = a;
  47
  48         if(layout.has_driver() && address)
  49                 layout.get_driver().add_signal(address, type);
  50 }
  51
  52 void Signal::set_position(const Vector &p)
  53 {
  54         const set<Track *> &tracks = layout.get_tracks();
  55         float dist = -1;
  56         for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
  57                 if(!(*i)->get_type().is_turnout())
  58                 {
  59                         TrackPoint n = (*i)->get_nearest_point(p);
  60                         float d = distance(p, n.pos);
  61                         if(d<dist || dist<0)
  62                         {
  63                                 position = n.pos;
  64                                 rotation = n.dir;
  65                                 track = *i;
  66                                 dist = d;
  67                         }
  68                 }
  69
  70         normalize_location();
  71 }
  72
  73 void Signal::normalize_location()
  74 {
  75         block = &track->get_block();
  76
  77         unsigned n_endpoints = track->get_type().get_endpoints().size();
  78         for(unsigned j=0; j<n_endpoints; ++j)
  79         {
  80                 float a = track->get_endpoint_direction(j)-rotation;
  81                 while(a<-M_PI/2)
  82                         a += M_PI*2;
  83                 while(a>M_PI*3/2)
  84                         a -= M_PI*2;
  85                 if(a>=M_PI/2)
  86                 {
  87                         BlockIter biter = TrackIter(track, j).block_iter();
  88                         entry = biter.entry();
  89                 }
  90         }
  91 }
  92
  93 void Signal::set_rotation(float r)
  94 {
  95         float a = rotation-r;
  96         while(a>M_PI*3/2)
  97                 a -= M_PI*2;
  98         while(a<-M_PI/2)
  99                 a += M_PI*2;
 100         if(a>=M_PI/2)
 101         {
 102                 rotation += M_PI;
 103                 if(rotation>M_PI*2)
 104                         rotation -= M_PI*2;
 105         }
 106
 107         normalize_location();
 108 }
 109
 110 bool Signal::collide_ray(const Vector &start, const Vector &ray) const
 111 {
 112         // XXX Totally hardcoded stuff, should be replaced with a geometry system
 113         Vector center = position;
 114         center.x += sin(rotation)*0.035;
 115         center.y -= cos(rotation)*0.035;
 116         Vector d(center.x-start.x, center.y-start.y);
 117         float x = (d.x*ray.x+d.y*ray.y)/(ray.x*ray.x+ray.y*ray.y);
 118         Vector nearest(start.x+ray.x*x-center.x, start.y+ray.y*x-center.y, start.z+ray.z*x-center.z);
 119         if(nearest.z<0|| nearest.z>0.12)
 120                 return false;
 121         return nearest.x*nearest.x+nearest.y*nearest.y<0.0001;
 122 }
 123
 124 void Signal::tick(const Time::TimeDelta &)
 125 {
 126         if(check_allocated_blocks)
 127         {
 128                 unsigned n_blocks = 0;
 129                 BlockIter iter(block, entry);
 130                 iter = iter.next();
 131                 while(iter && iter->get_train()==train)
 132                 {
 133                         if(iter->get_sensor_id())
 134                                 ++n_blocks;
 135                         iter=iter.next();
 136                 }
 137                 check_allocated_blocks = false;
 138
 139                 const list<SignalType::Indication> &indications = type.get_indications();
 140                 unsigned aspect = indications.back().aspect;
 141                 for(list<SignalType::Indication>::const_iterator i=indications.begin(); i!=indications.end(); ++i)
 142                         if(n_blocks>=i->free_blocks)
 143                         {
 144                                 aspect = i->aspect;
 145                                 break;
 146                         }
 147
 148                 layout.get_driver().set_signal(address, aspect);
 149         }
 150 }
 151
 152 void Signal::block_reserved(const Block &b, Train *t)
 153 {
 154         if(&b==block)
 155         {
 156                 if(t)
 157                 {
 158                         int train_entry = t->get_entry_to_block(*block);
 159                         if(train_entry>=0 && static_cast<unsigned>(train_entry)==entry)
 160                         {
 161                                 if(train_conn)
 162                                         train_conn.disconnect();
 163                                 train = t;
 164                                 passing = false;
 165                                 train_conn = train->signal_advanced.connect(sigc::mem_fun(this, &Signal::train_advanced));
 166                                 check_allocated_blocks = true;
 167                         }
 168                 }
 169                 else
 170                 {
 171                         layout.get_driver().set_signal(address, type.get_indications().back().aspect);
 172                         reset();
 173                 }
 174         }
 175         else if(train && t==train)
 176                 check_allocated_blocks = true;
 177 }
 178
 179 void Signal::train_advanced(Block &b)
 180 {
 181         if(&b==block)
 182                 passing = true;
 183         else if(passing && b.get_sensor_id())
 184         {
 185                 layout.get_driver().set_signal(address, type.get_indications().back().aspect);
 186                 reset();
 187         }
 188 }
 189
 190 void Signal::reset()
 191 {
 192         train = 0;
 193         if(train_conn)
 194                 train_conn.disconnect();
 195         check_allocated_blocks = false;
 196 }
 197
 198 void Signal::save(list<DataFile::Statement> &st) const
 199 {
 200         st.push_back((DataFile::Statement("position"), position.x, position.y, position.z));
 201         st.push_back((DataFile::Statement("rotation"), rotation));
 202         if(address)
 203                 st.push_back((DataFile::Statement("address"), address));
 204 }
 205
 206
 207 Signal::Loader::Loader(Signal &s):
 208         DataFile::ObjectLoader<Signal>(s)
 209 {
 210         add("address",  &Loader::address);
 211         add("position", &Loader::position);
 212         add("rotation", &Loader::rotation);
 213 }
 214
 215 void Signal::Loader::address(unsigned a)
 216 {
 217         obj.set_address(a);
 218 }
 219
 220 void Signal::Loader::position(float x, float y, float z)
 221 {
 222         obj.set_position(Vector(x, y, z));
 223 }
 224
 225 void Signal::Loader::rotation(float d)
 226 {
 227         obj.set_rotation(d);
 228 }
 229
 230 } // namespace R2C2