2 #include <msp/core/maputils.h>
3 #include <msp/strings/format.h>
9 #include "trackattachment.h"
10 #include "tracktype.h"
17 struct AttachmentCompare
21 AttachmentCompare(unsigned e): entry(e) { }
23 bool operator()(const R2C2::TrackAttachment *a1, const R2C2::TrackAttachment *a2) const
24 { return a1->get_offset_from_endpoint(entry)<a2->get_offset_from_endpoint(entry); }
31 Track::Track(Layout &l, const TrackType &t):
39 links(type.get_endpoints().size()),
45 turnout_addr = layout.allocate_turnout_address();
47 if(layout.has_driver())
49 Driver &driver = layout.get_driver();
50 turnout_id = driver.add_turnout(turnout_addr, type);
51 driver.signal_turnout.connect(sigc::mem_fun(this, &Track::turnout_event));
52 driver.signal_turnout_failed.connect(sigc::mem_fun(this, &Track::turnout_failed));
59 for(unsigned paths = type.get_paths(); !(paths&1); ++active_path, paths>>=1) ;
65 if(layout.has_driver() && turnout_id)
66 layout.get_driver().remove_turnout(turnout_id);
70 Track *Track::clone(Layout *to_layout) const
72 Track *track = new Track((to_layout ? *to_layout : layout), type);
73 track->set_position(position);
74 track->set_rotation(rotation);
78 void Track::set_block(Block *b)
80 if(b && !b->has_track(*this))
81 throw logic_error("track not in block");
82 if(!b && block && block->has_track(*this))
83 throw logic_error("track still in block");
88 Block &Track::get_block() const
91 throw logic_error("!block");
96 void Track::set_position(const Vector &p)
103 void Track::set_rotation(const Angle &r)
105 rotation = wrap_positive(r);
109 void Track::set_tilt(const Angle &t)
115 slope = tan(tilt)*type.get_path_length(0);
120 void Track::set_flex(bool f)
125 void Track::propagate_slope()
127 for(vector<Track *>::const_iterator i=links.begin(); i!=links.end(); ++i)
132 void Track::check_slope()
137 if(links[0] && links[1])
139 Vector epp0 = links[0]->get_snap_node(links[0]->get_link_slot(*this)).position;
140 Vector epp1 = links[1]->get_snap_node(links[1]->get_link_slot(*this)).position;
142 slope = epp1.z-position.z;
143 tilt = Geometry::atan(slope/type.get_path_length(0));
149 Vector epp = links[0]->get_snap_node(links[0]->get_link_slot(*this)).position;
154 Vector epp = links[1]->get_snap_node(links[1]->get_link_slot(*this)).position;
155 position.z = epp.z-slope;
162 void Track::set_turnout_address(unsigned a)
164 if(!type.is_turnout())
165 throw logic_error("not a turnout");
167 throw invalid_argument("Track::set_turnout_id");
169 Driver *driver = (layout.has_driver() ? &layout.get_driver() : 0);
171 if(driver && turnout_id)
172 driver->remove_turnout(turnout_id);
174 layout.create_blocks(*this);
175 layout.update_routes();
176 if(driver && turnout_addr)
177 turnout_id = driver->add_turnout(turnout_addr, type);
182 void Track::set_sensor_address(unsigned a)
184 if(type.is_turnout())
185 throw logic_error("is a turnout");
188 layout.create_blocks(*this);
191 void Track::set_active_path(unsigned p)
193 if(!type.is_turnout())
194 throw logic_error("not a turnout");
195 if(!(type.get_paths()&(1<<p)))
196 throw invalid_argument("Track::set_active_path");
201 signal_path_changing(p);
202 path_changing = true;
203 layout.get_driver().set_turnout(turnout_id, p);
206 float Track::get_path_length(int p) const
210 return type.get_path_length(p);
213 OrientedPoint Track::get_point(unsigned epi, unsigned path, float d) const
215 OrientedPoint p = type.get_point(epi, path, d);
217 p.position = position+rotated_vector(p.position, rotation);
218 p.rotation += rotation;
219 if(type.get_endpoints().size()==2)
221 float dz = tan(tilt)*d;
229 p.position.z += slope-dz;
237 OrientedPoint Track::get_point(unsigned epi, float d) const
239 return get_point(epi, active_path, d);
242 unsigned Track::get_n_snap_nodes() const
244 return type.get_endpoints().size();
247 Snap Track::get_snap_node(unsigned i) const
249 const vector<TrackType::Endpoint> &eps = type.get_endpoints();
251 throw out_of_range("Track::get_snap_node");
254 const TrackType::Endpoint &ep = eps[i];
256 result.position = position+rotated_vector(ep.pos, rotation);
257 if(eps.size()==2 && i==1)
258 result.position.z += slope;
260 result.rotation = rotation+ep.dir;
265 bool Track::snap(Snap &sn, float limit, SnapType what) const
267 if(Object::snap(sn, limit, what))
270 if(what&SNAP_SEGMENT)
272 Vector local = rotated_vector(sn.position-position, -rotation);
274 OrientedPoint np = type.get_nearest_point(local);
275 Vector span = local-np.position;
276 if(dot(span, span)<=limit*limit)
278 sn.position = position+rotated_vector(np.position, rotation);
279 sn.rotation = np.rotation+rotation;
287 SnapType Track::get_default_snap_type_to(const Object &other) const
289 if(dynamic_cast<const Track *>(&other))
295 unsigned Track::get_n_link_slots() const
300 Track *Track::get_link(unsigned i) const
303 throw out_of_range("Track::get_link");
308 int Track::get_link_slot(const Object &other) const
310 for(unsigned i=0; i<links.size(); ++i)
317 bool Track::link_to(Object &other)
319 Track *otrack = dynamic_cast<Track *>(&other);
323 float limit = layout.get_catalogue().get_gauge();
324 if(!flex && !otrack->get_flex())
328 unsigned nsn = get_n_snap_nodes();
329 unsigned other_nsn = other.get_n_snap_nodes();
330 for(unsigned i=0; i<nsn; ++i)
332 Snap sn = get_snap_node(i);
333 for(unsigned j=0; j<other_nsn; ++j)
335 Snap osn = other.get_snap_node(j);
336 Vector span = osn.position-sn.position;
337 Angle da = wrap_balanced(osn.rotation-sn.rotation-Angle::half_turn());
339 if(dot(span, span)<limit && abs(da).radians()<0.01)
342 otrack->break_link(j);
344 otrack->links[j] = this;
346 layout.create_blocks(*this);
348 signal_link_changed.emit(i, otrack);
349 otrack->signal_link_changed.emit(j, this);
358 bool Track::break_link(unsigned i)
361 throw out_of_range("Track::break_link");
363 Track *other = links[i];
368 if(!other->break_link(*this))
370 /* If the call doesn't succeed, it means that the other track already
371 broke the link and is calling us right now. Recreate blocks in the inner
372 call so it occurs before any signals are emitted. */
373 layout.create_blocks(*this);
376 signal_link_changed.emit(i, 0);
381 void Track::add_attachment(TrackAttachment &a)
383 if(find(attachments.begin(), attachments.end(), &a)!=attachments.end())
385 attachments.push_back(&a);
388 void Track::remove_attachment(TrackAttachment &a)
390 AttachmentList::iterator i = find(attachments.begin(), attachments.end(), &a);
391 if(i==attachments.end())
393 attachments.erase(i);
396 Track::AttachmentList Track::get_attachments_ordered(unsigned epi) const
398 AttachmentList result = attachments;
399 result.sort(AttachmentCompare(epi));
403 void Track::save(list<DataFile::Statement> &st) const
405 st.push_back((DataFile::Statement("position"), position.x, position.y, position.z));
406 st.push_back((DataFile::Statement("rotation"), rotation.radians()));
407 st.push_back((DataFile::Statement("tilt"), tilt.radians()));
409 st.push_back((DataFile::Statement("turnout_address"), turnout_addr));
411 st.push_back((DataFile::Statement("sensor_address"), sensor_addr));
413 st.push_back((DataFile::Statement("flex"), true));
416 void Track::turnout_event(unsigned id, unsigned state)
421 path_changing = false;
422 signal_path_changed.emit(active_path);
426 void Track::turnout_failed(unsigned id)
430 path_changing = false;
431 layout.emergency(block, "Turnout failed");
436 Track::Loader::Loader(Track &t):
437 DataFile::ObjectLoader<Track>(t)
439 add("position", &Loader::position);
440 add("rotation", &Loader::rotation);
441 add("tilt", &Loader::tilt);
442 add("turnout_id", &Loader::turnout_address);
443 add("turnout_address", &Loader::turnout_address);
444 add("sensor_id", &Loader::sensor_address);
445 add("sensor_address", &Loader::sensor_address);
446 add("flex", &Track::flex);
449 add("slope", &Loader::slope);
452 void Track::Loader::position(float x, float y, float z)
454 obj.set_position(Vector(x, y, z));
457 void Track::Loader::rotation(float r)
459 obj.set_rotation(Angle::from_radians(r));
462 void Track::Loader::sensor_address(unsigned addr)
464 obj.set_sensor_address(addr);
467 void Track::Loader::slope(float s)
469 obj.set_tilt(Geometry::atan(s/obj.type.get_path_length(0)));
472 void Track::Loader::tilt(float t)
474 obj.set_tilt(Angle::from_radians(t));
477 void Track::Loader::turnout_address(unsigned addr)
479 obj.set_turnout_address(addr);