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));
58 for(unsigned paths = type.get_paths(); !(paths&1); ++active_path, paths>>=1) ;
64 if(layout.has_driver() && turnout_id)
65 layout.get_driver().remove_turnout(turnout_id);
69 Track *Track::clone(Layout *to_layout) const
71 Track *track = new Track((to_layout ? *to_layout : layout), type);
72 track->set_position(position);
73 track->set_rotation(rotation);
77 void Track::set_block(Block *b)
79 if(b && !b->has_track(*this))
80 throw logic_error("track not in block");
81 if(!b && block && block->has_track(*this))
82 throw logic_error("track still in block");
87 Block &Track::get_block() const
90 throw logic_error("!block");
95 void Track::set_position(const Vector &p)
102 void Track::set_rotation(const Angle &r)
104 rotation = wrap_positive(r);
108 void Track::set_tilt(const Angle &t)
114 slope = tan(tilt)*type.get_path_length(0);
119 void Track::set_flex(bool f)
124 void Track::propagate_slope()
126 for(vector<Track *>::const_iterator i=links.begin(); i!=links.end(); ++i)
131 void Track::check_slope()
136 if(links[0] && links[1])
138 Vector epp0 = links[0]->get_snap_node(links[0]->get_link_slot(*this)).position;
139 Vector epp1 = links[1]->get_snap_node(links[1]->get_link_slot(*this)).position;
141 slope = epp1.z-position.z;
142 tilt = Geometry::atan(slope/type.get_path_length(0));
148 Vector epp = links[0]->get_snap_node(links[0]->get_link_slot(*this)).position;
153 Vector epp = links[1]->get_snap_node(links[1]->get_link_slot(*this)).position;
154 position.z = epp.z-slope;
161 void Track::set_turnout_address(unsigned a)
163 if(!type.is_turnout())
164 throw logic_error("not a turnout");
166 throw invalid_argument("Track::set_turnout_id");
168 Driver *driver = (layout.has_driver() ? &layout.get_driver() : 0);
170 if(driver && turnout_id)
171 driver->remove_turnout(turnout_id);
173 layout.create_blocks(*this);
174 layout.update_routes();
175 if(driver && turnout_addr)
176 turnout_id = driver->add_turnout(turnout_addr, type);
181 void Track::set_sensor_address(unsigned a)
183 if(type.is_turnout())
184 throw logic_error("is a turnout");
187 layout.create_blocks(*this);
190 void Track::set_active_path(unsigned p)
192 if(!type.is_turnout())
193 throw logic_error("not a turnout");
194 if(!(type.get_paths()&(1<<p)))
195 throw invalid_argument("Track::set_active_path");
200 signal_path_changing(p);
201 path_changing = true;
202 layout.get_driver().set_turnout(turnout_id, p);
205 float Track::get_path_length(int p) const
209 return type.get_path_length(p);
212 OrientedPoint Track::get_point(unsigned epi, unsigned path, float d) const
214 OrientedPoint p = type.get_point(epi, path, d);
216 p.position = position+rotated_vector(p.position, rotation);
217 p.rotation += rotation;
218 if(type.get_endpoints().size()==2)
220 float dz = tan(tilt)*d;
228 p.position.z += slope-dz;
236 OrientedPoint Track::get_point(unsigned epi, float d) const
238 return get_point(epi, active_path, d);
241 unsigned Track::get_n_snap_nodes() const
243 return type.get_endpoints().size();
246 Snap Track::get_snap_node(unsigned i) const
248 const vector<TrackType::Endpoint> &eps = type.get_endpoints();
250 throw out_of_range("Track::get_snap_node");
253 const TrackType::Endpoint &ep = eps[i];
255 result.position = position+rotated_vector(ep.pos, rotation);
256 if(eps.size()==2 && i==1)
257 result.position.z += slope;
259 result.rotation = rotation+ep.dir;
264 bool Track::snap(Snap &sn, float limit, SnapType what) const
266 if(Object::snap(sn, limit, what))
269 if(what&SNAP_SEGMENT)
271 Vector local = rotated_vector(sn.position-position, -rotation);
273 OrientedPoint np = type.get_nearest_point(local);
274 Vector span = local-np.position;
275 if(dot(span, span)<=limit*limit)
277 sn.position = position+rotated_vector(np.position, rotation);
278 sn.rotation = np.rotation+rotation;
286 SnapType Track::get_default_snap_type_to(const Object &other) const
288 if(dynamic_cast<const Track *>(&other))
294 unsigned Track::get_n_link_slots() const
299 Track *Track::get_link(unsigned i) const
302 throw out_of_range("Track::get_link");
307 int Track::get_link_slot(const Object &other) const
309 for(unsigned i=0; i<links.size(); ++i)
316 bool Track::link_to(Object &other)
318 Track *otrack = dynamic_cast<Track *>(&other);
322 float gauge_ratio = otrack->get_type().get_gauge()/type.get_gauge();
323 if(gauge_ratio<0.99 || gauge_ratio>1.01)
326 float limit = type.get_gauge();
327 if(!flex && !otrack->get_flex())
331 unsigned nsn = get_n_snap_nodes();
332 unsigned other_nsn = other.get_n_snap_nodes();
333 for(unsigned i=0; i<nsn; ++i)
335 Snap sn = get_snap_node(i);
336 for(unsigned j=0; j<other_nsn; ++j)
338 Snap osn = other.get_snap_node(j);
339 Vector span = osn.position-sn.position;
340 Angle da = wrap_balanced(osn.rotation-sn.rotation-Angle::half_turn());
342 if(dot(span, span)<limit && abs(da).radians()<0.01)
345 otrack->break_link(j);
347 otrack->links[j] = this;
349 layout.create_blocks(*this);
351 signal_link_changed.emit(i, otrack);
352 otrack->signal_link_changed.emit(j, this);
361 bool Track::break_link(unsigned i)
364 throw out_of_range("Track::break_link");
366 Track *other = links[i];
371 if(!other->break_link(*this))
373 /* If the call doesn't succeed, it means that the other track already
374 broke the link and is calling us right now. Recreate blocks in the inner
375 call so it occurs before any signals are emitted. */
376 layout.create_blocks(*this);
379 signal_link_changed.emit(i, 0);
384 void Track::add_attachment(TrackAttachment &a)
386 if(find(attachments.begin(), attachments.end(), &a)!=attachments.end())
388 attachments.push_back(&a);
391 void Track::remove_attachment(TrackAttachment &a)
393 AttachmentList::iterator i = find(attachments.begin(), attachments.end(), &a);
394 if(i==attachments.end())
396 attachments.erase(i);
399 Track::AttachmentList Track::get_attachments_ordered(unsigned epi) const
401 AttachmentList result = attachments;
402 result.sort(AttachmentCompare(epi));
406 void Track::save(list<DataFile::Statement> &st) const
408 st.push_back((DataFile::Statement("position"), position.x, position.y, position.z));
409 st.push_back((DataFile::Statement("rotation"), rotation.radians()));
410 st.push_back((DataFile::Statement("tilt"), tilt.radians()));
412 st.push_back((DataFile::Statement("turnout_address"), turnout_addr));
414 st.push_back((DataFile::Statement("sensor_address"), sensor_addr));
416 st.push_back((DataFile::Statement("flex"), true));
419 void Track::save_dynamic(list<DataFile::Statement> &st) const
422 st.push_back((DataFile::Statement("path"), active_path));
425 void Track::turnout_event(unsigned id, unsigned state)
430 path_changing = false;
431 signal_path_changed.emit(active_path);
435 void Track::turnout_failed(unsigned id)
439 path_changing = false;
440 layout.emergency(block, "Turnout failed");
445 Track::Loader::Loader(Track &t):
446 DataFile::ObjectLoader<Track>(t)
448 add("path", &Loader::path);
449 add("position", &Loader::position);
450 add("rotation", &Loader::rotation);
451 add("tilt", &Loader::tilt);
452 add("turnout_id", &Loader::turnout_address);
453 add("turnout_address", &Loader::turnout_address);
454 add("sensor_id", &Loader::sensor_address);
455 add("sensor_address", &Loader::sensor_address);
456 add("flex", &Track::flex);
459 add("slope", &Loader::slope);
462 void Track::Loader::path(unsigned p)
464 obj.set_active_path(p);
465 if(obj.path_changing)
468 obj.signal_path_changed.emit(p);
472 void Track::Loader::position(float x, float y, float z)
474 obj.set_position(Vector(x, y, z));
477 void Track::Loader::rotation(float r)
479 obj.set_rotation(Angle::from_radians(r));
482 void Track::Loader::sensor_address(unsigned addr)
484 obj.set_sensor_address(addr);
487 void Track::Loader::slope(float s)
489 obj.set_tilt(Geometry::atan(s/obj.type.get_path_length(0)));
492 void Track::Loader::tilt(float t)
494 obj.set_tilt(Angle::from_radians(t));
497 void Track::Loader::turnout_address(unsigned addr)
499 obj.set_turnout_address(addr);