From: Mikko Rasa Date: Sun, 19 May 2013 12:31:15 +0000 (+0300) Subject: Make use of the mspmath library X-Git-Url: http://git.tdb.fi/?a=commitdiff_plain;h=7a36d396eded897c421424905b2c938d770df341;p=r2c2.git Make use of the mspmath library --- diff --git a/Build b/Build index 11a61c7..eed4769 100644 --- a/Build +++ b/Build @@ -2,6 +2,7 @@ package "r2c2" { require "mspcore"; require "mspdatafile"; + require "mspmath"; require "sigc++-2.0"; library "r2c2" diff --git a/source/3d/axle.cpp b/source/3d/axle.cpp index c9f7a70..e52303a 100644 --- a/source/3d/axle.cpp +++ b/source/3d/axle.cpp @@ -34,8 +34,7 @@ void Axle3D::setup_render(GL::Renderer &renderer, const GL::Tag &) const { GL::Matrix matrix; - const Vector &pos = vehicle.get_position(); - matrix.translate(pos.x, pos.y, pos.z); + matrix.translate(vehicle.get_position()); matrix.rotate(vehicle.get_rotation(), 0, 0, 1); if(bogie) diff --git a/source/3d/bogie.cpp b/source/3d/bogie.cpp index 8e99ceb..e00e373 100644 --- a/source/3d/bogie.cpp +++ b/source/3d/bogie.cpp @@ -26,14 +26,13 @@ void Bogie3D::setup_render(Msp::GL::Renderer &renderer, const GL::Tag &) const { GL::Matrix matrix; - const Vector &pos = vehicle.get_position(); - matrix.translate(pos.x, pos.y, pos.z); + matrix.translate(vehicle.get_position()); matrix.rotate(vehicle.get_rotation(), 0, 0, 1); matrix.translate(bogie.type->position, 0, 0); - float dir = bogie.direction; + Angle dir = bogie.direction; if(bogie.type->rotate_object) - dir += M_PI; + dir += Angle::half_turn(); matrix.rotate(dir, 0, 0, 1); renderer.matrix_stack() *= matrix; diff --git a/source/3d/endpoint.cpp b/source/3d/endpoint.cpp index 5d7ebb5..97e05a4 100644 --- a/source/3d/endpoint.cpp +++ b/source/3d/endpoint.cpp @@ -31,8 +31,8 @@ void Endpoint3D::render(GL::Renderer &renderer, const GL::Tag &tag) const GL::MatrixStack::Push push_mtx(renderer.matrix_stack()); GL::Matrix matrix; - matrix.translate(sn.position.x, sn.position.y, sn.position.z); - matrix.rotate(sn.rotation+M_PI, 0, 0, 1); + matrix.translate(sn.position); + matrix.rotate(sn.rotation+Angle::half_turn(), 0, 0, 1); renderer.matrix_stack() *= matrix; if(track.get_track().get_link(index)) diff --git a/source/3d/layout.cpp b/source/3d/layout.cpp index 6042515..d97dfe5 100644 --- a/source/3d/layout.cpp +++ b/source/3d/layout.cpp @@ -44,7 +44,7 @@ void Layout3D::get_bounds(Vector &minp, Vector &maxp) const { Vector tmin; Vector tmax; - i->second->get_bounds(0, tmin, tmax); + i->second->get_bounds(Angle::zero(), tmin, tmax); minp.x = min(minp.x, tmin.x); minp.y = min(minp.y, tmin.y); maxp.x = max(maxp.x, tmax.x); diff --git a/source/3d/overlay.cpp b/source/3d/overlay.cpp index bab469c..a9083e5 100644 --- a/source/3d/overlay.cpp +++ b/source/3d/overlay.cpp @@ -95,8 +95,7 @@ void Overlay3D::render(GL::Renderer &renderer, const GL::Tag &) const const Icon &icon = *i->second; - Vector node = i->first->get_node(); - GL::Vector3 p = renderer.get_camera()->project(GL::Vector3(node.x, node.y, node.z)); + GL::Vector3 p = renderer.get_camera()->project(i->first->get_node()); p.x = int(p.x*0.5*fb_width-icon.width*size/2); p.y = int(p.y*0.5*fb_height); diff --git a/source/3d/rod.cpp b/source/3d/rod.cpp index 10b5f46..5ae4584 100644 --- a/source/3d/rod.cpp +++ b/source/3d/rod.cpp @@ -25,11 +25,10 @@ void Rod3D::render(GL::Renderer &renderer, const GL::Tag &tag) const void Rod3D::setup_render(GL::Renderer &renderer, const GL::Tag &) const { GL::Matrix matrix; - const Vector &pos = vehicle.get_position(); - matrix.translate(pos.x, pos.y, pos.z); + matrix.translate(vehicle.get_position()); matrix.rotate(vehicle.get_rotation(), 0, 0, 1); - matrix.translate(rod.position.x, rod.position.y, rod.position.z); + matrix.translate(rod.position); if(rod.type->mirror_object) matrix.scale(1, -1, 1); matrix.rotate(rod.angle, 0, -1, 0); diff --git a/source/3d/signal.cpp b/source/3d/signal.cpp index 0942bce..4f12467 100644 --- a/source/3d/signal.cpp +++ b/source/3d/signal.cpp @@ -25,8 +25,7 @@ Signal3D::~Signal3D() void Signal3D::setup_render(GL::Renderer &renderer, const GL::Tag &) const { - const Vector &pos = signal.get_position(); - renderer.matrix_stack() *= GL::Matrix::translation(pos.x, pos.y, pos.z); + renderer.matrix_stack() *= GL::Matrix::translation(signal.get_position()); renderer.matrix_stack() *= GL::Matrix::rotation(signal.get_rotation(), 0, 0, 1); // XXX Use track gauge, configure signal side renderer.matrix_stack() *= GL::Matrix::translation(0, -0.035, 0); diff --git a/source/3d/track.cpp b/source/3d/track.cpp index 72973f3..bd148a7 100644 --- a/source/3d/track.cpp +++ b/source/3d/track.cpp @@ -47,20 +47,13 @@ Track3D::~Track3D() delete *i; } -void Track3D::get_bounds(float angle, Vector &minp, Vector &maxp) const +void Track3D::get_bounds(const Angle &angle, Vector &minp, Vector &maxp) const { type.get_bounds(angle-track.get_rotation(), minp, maxp); - float c = cos(-angle); - float s = sin(-angle); - - const Vector &pos = track.get_position(); - minp.x += c*pos.x-s*pos.y; - maxp.x += c*pos.x-s*pos.y; - minp.y += s*pos.x+c*pos.y; - maxp.y += s*pos.x+c*pos.y; - minp.z += pos.z; - maxp.z += pos.z; + Vector pos = rotated_vector(track.get_position(), -angle); + minp += pos; + maxp += pos; float slope = track.get_slope(); if(slope>0) @@ -71,26 +64,18 @@ void Track3D::get_bounds(float angle, Vector &minp, Vector &maxp) const Vector Track3D::get_node() const { - const Vector &pos = track.get_position(); Vector minp; Vector maxp; - type.get_bounds(0, minp, maxp); - float rot = track.get_rotation(); - float c = cos(rot); - float s = sin(rot); + type.get_bounds(Angle::zero(), minp, maxp); - Vector center((minp.x+maxp.x)/2, (minp.y+maxp.y)/2, 0); - return Vector(pos.x+c*center.x-s*center.y, pos.y+s*center.x+c*center.y, pos.z+0.02); + return track.get_position()+rotated_vector((minp+maxp)/2.0f, track.get_rotation())+Vector(0, 0, 0.02); } GL::Matrix Track3D::create_matrix() const { - const Vector &pos = track.get_position(); - float rot = track.get_rotation(); - GL::Matrix matrix; - matrix.translate(pos.x, pos.y, pos.z); - matrix.rotate(rot, 0, 0, 1); + matrix.translate(track.get_position()); + matrix.rotate(track.get_rotation(), 0, 0, 1); matrix.rotate(track.get_slope()/track.get_type().get_total_length(), 0, -1, 0); return matrix; diff --git a/source/3d/track.h b/source/3d/track.h index f3bfb45..641f234 100644 --- a/source/3d/track.h +++ b/source/3d/track.h @@ -32,7 +32,7 @@ public: Layout3D &get_layout() const { return layout; } Track &get_track() const { return track; } const TrackType3D &get_type() const { return type; } - void get_bounds(float, Vector &, Vector &) const; + void get_bounds(const Angle &, Vector &, Vector &) const; Path3D &get_path() { return *path; } virtual Vector get_node() const; diff --git a/source/3d/tracktype.cpp b/source/3d/tracktype.cpp index ab417cf..a5e97cc 100644 --- a/source/3d/tracktype.cpp +++ b/source/3d/tracktype.cpp @@ -33,7 +33,7 @@ Iter graham_scan(Iter begin, Iter end) for(Iter k=begin, i=k++, j=k++;; ) { // Compute winding by cross product - float turn = (j->x-i->x)*(k->y-j->y) - (k->x-j->x)*(j->y-i->y); + float turn = cross(*j-*i, *k-*j).z; if(turn<1e-5) { @@ -96,7 +96,7 @@ TrackType3D::TrackType3D(Catalogue3D &cat3d, const TrackType &tt): for(unsigned i=0; i::const_iterator i=parts.begin(); i!=parts.end(); ++i) - build_part(*i, ballast_profile, Vector(0, -ballast_min.y), false, bld, index); + build_part(*i, ballast_profile, Vector(0, -ballast_min.y, 0), false, bld, index); bld.texcoord(0.75, 0.5); float y = ballast_h-rail_min.y; for(vector::const_iterator i=parts.begin(); i!=parts.end(); ++i) - build_part(*i, rail_profile, Vector(-gauge/2, y), true, bld, index); + build_part(*i, rail_profile, Vector(0, gauge/2, y), true, bld, index); for(vector::const_iterator i=parts.begin(); i!=parts.end(); ++i) - build_part(*i, rail_profile, Vector(gauge/2, y), false, bld, index); + build_part(*i, rail_profile, Vector(0, -gauge/2, y), false, bld, index); object = new GL::Object; object->set_mesh(mesh); @@ -134,7 +134,7 @@ TrackType3D::TrackType3D(Catalogue3D &cat3d, const TrackType &tt): unsigned index = 0; for(vector::const_iterator j=parts.begin(); j!=parts.end(); ++j) if(j->get_path()==i) - build_part(*j, cat.get_path_profile(), Vector(0, ballast_h+1.5*rail_h), false, bld, index); + build_part(*j, cat.get_path_profile(), Vector(0, 0, ballast_h+1.5*rail_h), false, bld, index); } path_meshes.push_back(m); } @@ -154,10 +154,9 @@ TrackType3D::~TrackType3D() delete *i; } -void TrackType3D::get_bounds(float angle, Vector &minp, Vector &maxp) const +void TrackType3D::get_bounds(const Angle &angle, Vector &minp, Vector &maxp) const { - float c = cos(-angle); - float s = sin(-angle); + Transform trans = Transform::rotation(-angle, Vector(0, 0, 1)); minp = maxp = Vector(); minp.z = min_z; @@ -165,13 +164,12 @@ void TrackType3D::get_bounds(float angle, Vector &minp, Vector &maxp) const for(vector::const_iterator i=border.begin(); i!=border.end(); ++i) { - float x = c*i->x-s*i->y; - float y = s*i->x+c*i->y; + Vector v = trans.transform(*i); - minp.x = min(minp.x, x); - minp.y = min(minp.y, y); - maxp.x = max(maxp.x, x); - maxp.y = max(maxp.y, y); + minp.x = min(minp.x, v.x); + minp.y = min(minp.y, v.y); + maxp.x = max(maxp.x, v.x); + maxp.y = max(maxp.y, v.y); } } @@ -193,24 +191,22 @@ void TrackType3D::build_part(const TrackPart &part, const Profile &profile, cons for(unsigned i=0; i<=nsegs; ++i) { TrackPoint basep = part.get_point(i*plen/nsegs); - float c = cos(basep.dir); - float s = sin(basep.dir); + Transform trans = Transform::rotation(basep.dir, Vector(0, 0, 1)); for(unsigned j=0; jget_camera().unproject(GL::Vector4(p.x, p.y, 0, 0)); const GL::Vector3 &pos = main_view->get_camera().get_position(); - return Vector(pos.x-vec.x*pos.z/vec.z, pos.y-vec.y*pos.z/vec.z); + return pos-GL::Vector3(vec.x, vec.y, vec.z)*(pos.z/vec.z); } void Designer::tick() @@ -450,7 +450,7 @@ void Designer::button_press(unsigned btn) { bool shift = keyboard.get_button_state(Input::KEY_SHIFT_L) || keyboard.get_button_state(Input::KEY_SHIFT_R); - Vector pointer(mouse.get_axis_value(0), mouse.get_axis_value(1)); + Vector pointer(mouse.get_axis_value(0), mouse.get_axis_value(1), 0); Vector ground = map_pointer_to_ground(pointer); if(mode==CATALOGUE) @@ -491,7 +491,7 @@ void Designer::button_press(unsigned btn) void Designer::axis_motion(unsigned, float, float) { - Vector pointer(mouse.get_axis_value(0), mouse.get_axis_value(1)); + Vector pointer(mouse.get_axis_value(0), mouse.get_axis_value(1), 0); float wx = pointer.x*window.get_width(); float wy = pointer.y*window.get_height(); @@ -587,8 +587,8 @@ void Designer::measure_changed() float pard = measure.get_parallel_distance()*1000; float perpd = measure.get_perpendicular_distance()*1000; float d = sqrt(pard*pard+perpd*perpd); - float adiff = measure.get_angle_difference()*180/M_PI; - string info = format("Par %.1fmm - Perp %.1fmm - Total %.1fmm - Angle %.1f°", pard, perpd, d, adiff); + const Angle &adiff = measure.get_angle_difference(); + string info = format("Par %.1fmm - Perp %.1fmm - Total %.1fmm - Angle %.1f°", pard, perpd, d, adiff.degrees()); lbl_status->set_text(info); } diff --git a/source/designer/manipulator.cpp b/source/designer/manipulator.cpp index 802adb1..8620fe7 100644 --- a/source/designer/manipulator.cpp +++ b/source/designer/manipulator.cpp @@ -14,8 +14,7 @@ Manipulator::Manipulator(Designer &d, Input::Mouse &m, Selection &s): designer(d), mouse(m), selection(s), - mode(NONE), - angle(0) + mode(NONE) { mouse.signal_button_press.connect(sigc::bind_return(sigc::mem_fun(this, &Manipulator::button_press), false)); mouse.signal_axis_motion.connect(sigc::bind_return(sigc::mem_fun(this, &Manipulator::axis_motion), false)); @@ -37,7 +36,7 @@ void Manipulator::start_rotate() if(mode) cancel(); - rot_origin = atan2(gpointer.y-center.y, gpointer.x-center.x); + rot_origin = Geometry::atan2(gpointer.y-center.y, gpointer.x-center.x); mode = ROTATE; } @@ -243,22 +242,15 @@ void Manipulator::connect() if(abs(dz)>0.02) continue; - float adiff = sn1.rotation+M_PI-sn2.rotation; - while(adiff<-M_PI) - adiff += M_PI*2; - while(adiff>M_PI) - adiff -= M_PI*2; - if(abs(adiff)>0.01) + Angle adiff = wrap_balanced(sn1.rotation+Angle::half_turn()-sn2.rotation); + if(abs(adiff).radians()>0.01) continue; - float c = cos(sn1.rotation); - float s = sin(sn1.rotation); - float dx = sn2.position.x-sn1.position.x; - float dy = sn2.position.y-sn1.position.y; - if(abs(dx*s-dy*c)>limit) + Vector delta = rotated_vector(sn2.position-sn1.position, -sn1.rotation); + if(abs(delta.y)>limit) continue; - gap = dx*c+dy*s; + gap = delta.x; if(gap<0) continue; @@ -297,7 +289,7 @@ void Manipulator::cancel() for(vector::iterator i=objects.begin(); i!=objects.end(); ++i) { - i->object->set_position(Vector(center.x+i->pos.x, center.y+i->pos.y, center.z+i->pos.z)); + i->object->set_position(center+i->pos); i->object->set_rotation(i->rot); } @@ -305,7 +297,7 @@ void Manipulator::cancel() delete *i; extend_tracks.clear(); - angle = 0; + angle = Angle::zero(); signal_done.emit(false); } @@ -321,7 +313,7 @@ void Manipulator::button_press(unsigned btn) { Mode m = mode; mode = NONE; - angle = 0; + angle = Angle::zero(); if(m!=EXTEND) { @@ -367,11 +359,10 @@ void Manipulator::axis_motion(unsigned axis, float value, float) if(mode==MOVE) { - Vector delta(gpointer.x-move_origin.x, gpointer.y-move_origin.y, 0); - Vector offset(center.x+delta.x, center.y+delta.y, center.z); + Vector offset = center+gpointer-move_origin; for(vector::iterator i=objects.begin(); i!=objects.end(); ++i) { - i->object->set_position(Vector(offset.x+i->pos.x, offset.y+i->pos.y, offset.z+i->pos.z)); + i->object->set_position(offset+i->pos); i->object->set_rotation(i->rot); } @@ -394,32 +385,29 @@ void Manipulator::axis_motion(unsigned axis, float value, float) if(snapped) { - float da = snapped->object->get_rotation()-snapped->rot; - float c = cos(da); - float s = sin(da); + Angle da = snapped->object->get_rotation()-snapped->rot; + Transform trans = Transform::rotation(da, Vector(0, 0, 1)); const Vector &sp = snapped->object->get_position(); for(vector::iterator i=objects.begin(); i!=objects.end(); ++i) { if(&*i==snapped) continue; - Vector dp(i->pos.x-snapped->pos.x, i->pos.y-snapped->pos.y, 0); - i->object->set_position(Vector(sp.x+c*dp.x-s*dp.y, sp.y+s*dp.x+c*dp.y, sp.z+i->pos.z-snapped->pos.z)); + i->object->set_position(sp+trans.transform(i->pos-snapped->pos)); i->object->set_rotation(i->rot+da); } } } else if(mode==ROTATE) { - float a = atan2(gpointer.y-center.y, gpointer.x-center.x); + Angle a = Geometry::atan2(gpointer.y-center.y, gpointer.x-center.x); angle += a-rot_origin; rot_origin = a; + Transform trans = Transform::rotation(angle, Vector(0, 0, 1)); for(vector::iterator i=objects.begin(); i!=objects.end(); ++i) { - float c = cos(angle); - float s = sin(angle); - i->object->set_position(Vector(center.x+c*i->pos.x-s*i->pos.y, center.y+s*i->pos.x+c*i->pos.y, center.z+i->pos.z)); + i->object->set_position(center+trans.transform(i->pos)); i->object->set_rotation(angle+i->rot); } } @@ -430,12 +418,12 @@ void Manipulator::axis_motion(unsigned axis, float value, float) signal_status.emit(format("Elevation: %+.0fmm (%.0fmm)", dz*1000, (center.z+dz)*1000)); for(vector::iterator i=objects.begin(); i!=objects.end(); ++i) - i->object->set_position(Vector(center.x+i->pos.x, center.y+i->pos.y, center.z+i->pos.z+dz)); + i->object->set_position(center+i->pos+Vector(0, 0, dz)); } else if(mode==EXTEND) { Vector pos; - float dir = 0; + Angle dir; float length = 0; for(vector::iterator i=objects.begin(); i!=objects.end(); ++i) { @@ -446,18 +434,14 @@ void Manipulator::axis_motion(unsigned axis, float value, float) continue; Snap sn = i->object->get_snap_node(j); - float c = cos(sn.rotation); - float s = sin(sn.rotation); - float dx = gpointer.x-sn.position.x; - float dy = gpointer.y-sn.position.y; + Vector delta = rotated_vector(gpointer-sn.position, -sn.rotation); - float len = dx*c+dy*s; - if(len::iterator i=objects.begin(); i!=objects.end(); ++i) { - const Vector &p = i->object->get_position(); - i->pos = Vector(p.x-center.x, p.y-center.y, p.z-center.z); + i->pos = i->object->get_position()-center; i->rot = i->object->get_rotation(); } } @@ -601,7 +585,7 @@ void Manipulator::set_slope(TrackOrder &track, float z, float dz) } } -vector Manipulator::create_straight(const R2C2::Vector &start, float dir, float length, float limit) +vector Manipulator::create_straight(const R2C2::Vector &start, const Angle &dir, float length, float limit) { const Catalogue::TrackMap &track_types = designer.get_catalogue().get_tracks(); std::map types_by_length; @@ -662,8 +646,7 @@ vector Manipulator::create_straight(const R2C2::Vector &start, float di if(!lengths.empty()) { Vector pos = start; - float c = cos(dir); - float s = sin(dir); + Transform trans = Transform::rotation(dir, Vector(0, 0, 1)); for(vector::iterator i=lengths.begin(); i!=lengths.end(); ++i) { Track *track = new Track(designer.get_layout(), *get_item(types_by_length, *i)); @@ -674,8 +657,7 @@ vector Manipulator::create_straight(const R2C2::Vector &start, float di track->link_to(*trks.back()); trks.push_back(track); - pos.x += c**i; - pos.y += s**i; + pos += trans.transform(Vector(*i, 0, 0)); } } diff --git a/source/designer/manipulator.h b/source/designer/manipulator.h index 12e551b..1c46af6 100644 --- a/source/designer/manipulator.h +++ b/source/designer/manipulator.h @@ -2,6 +2,7 @@ #define MANIPULATOR_H_ #include +#include "libr2c2/geometry.h" class Designer; class Selection; @@ -22,7 +23,7 @@ private: { R2C2::Object *object; R2C2::Vector pos; - float rot; + R2C2::Angle rot; MObject(R2C2::Object *); }; @@ -50,8 +51,8 @@ private: R2C2::Vector gpointer; Mode mode; R2C2::Vector move_origin; - float angle; - float rot_origin; + R2C2::Angle angle; + R2C2::Angle rot_origin; float elev_origin; std::set neighbors; std::vector extend_tracks; @@ -75,7 +76,7 @@ private: void update_objects(); void update_neighbors(); void set_slope(TrackOrder &, float, float); - std::vector create_straight(const R2C2::Vector &, float, float, float); + std::vector create_straight(const R2C2::Vector &, const R2C2::Angle &, float, float); }; #endif diff --git a/source/designer/measure.cpp b/source/designer/measure.cpp index 69ada81..c7fead1 100644 --- a/source/designer/measure.cpp +++ b/source/designer/measure.cpp @@ -29,7 +29,7 @@ void Measure::button_press(float gx, float gy, unsigned btn) if(btn==1) { ssnap.position = Vector(gx, gy, 0); - ssnap.rotation = 0; + ssnap.rotation = Angle::zero(); snap_to_tracks(ssnap); state = ACTIVE; @@ -61,18 +61,12 @@ void Measure::pointer_motion(float gx, float gy) if(state!=STARTING) { - Vector delta(pointer.x-ssnap.position.x, pointer.y-ssnap.position.y, 0); - float c = cos(ssnap.rotation); - float s = sin(ssnap.rotation); + Vector delta = rotated_vector(pointer-ssnap.position, -ssnap.rotation); - par_dist = delta.x*c+delta.y*s; - perp_dist = delta.x*s-delta.y*c; + par_dist = delta.x; + perp_dist = delta.y; - adiff = sn.rotation-ssnap.rotation+M_PI; - while(adiff<-M_PI) - adiff += M_PI*2; - while(adiff>M_PI) - adiff -= M_PI*2; + adiff = wrap_balanced(sn.rotation-ssnap.rotation+Angle::half_turn()); update_mesh(); @@ -87,7 +81,7 @@ void Measure::render(GL::Renderer &renderer, const GL::Tag &) const GL::Renderer::Push push(renderer); const Vector &pos = (state==ACTIVE ? ssnap.position : pointer); - renderer.matrix_stack() *= GL::Matrix::translation(pos.x, pos.y, pos.z); + renderer.matrix_stack() *= GL::Matrix::translation(pos); mesh.draw(renderer); } diff --git a/source/designer/measure.h b/source/designer/measure.h index 022c314..c569698 100644 --- a/source/designer/measure.h +++ b/source/designer/measure.h @@ -3,6 +3,7 @@ #include #include +#include "libr2c2/geometry.h" class Designer; @@ -26,7 +27,7 @@ private: R2C2::Snap ssnap; float par_dist; float perp_dist; - float adiff; + R2C2::Angle adiff; State state; Msp::GL::Mesh mesh; @@ -34,7 +35,7 @@ public: Measure(Designer &); float get_parallel_distance() const { return par_dist; } float get_perpendicular_distance() const { return perp_dist; } - float get_angle_difference() const { return adiff; } + const R2C2::Angle &get_angle_difference() const { return adiff; } void start(); void button_press(float, float, unsigned); void pointer_motion(float, float); diff --git a/source/designer/svgexporter.cpp b/source/designer/svgexporter.cpp index 62e869c..ca95f90 100644 --- a/source/designer/svgexporter.cpp +++ b/source/designer/svgexporter.cpp @@ -67,8 +67,8 @@ void SvgExporter::save(const string &fn) void SvgExporter::save_track(const Track &track, xmlpp::Element &group) { const Vector &pos = track.get_position(); - float rot = track.get_rotation(); - string transform = format("translate(%.3f %.3f) rotate(%.3f)", pos.x*1000, -pos.y*1000, -rot*180/M_PI); + const Angle &rot = track.get_rotation(); + string transform = format("translate(%.3f %.3f) rotate(%.3f)", pos.x*1000, -pos.y*1000, -rot.degrees()); group.set_attribute("transform", transform); const TrackType &type = track.get_type(); @@ -79,12 +79,11 @@ void SvgExporter::save_track(const Track &track, xmlpp::Element &group) xmlpp::Element *elem = group.add_child("path"); elem->set_attribute("class", "endpoint"); - float dx = -sin(i->dir)*gauge; - float dy = -cos(i->dir)*gauge; + Vector delta = rotated_vector(Vector(0, gauge, 0), i->dir); string data = format("M %.3f %.3f L %.3f %.3f", - i->pos.x*1000+dx, -i->pos.y*1000+dy, - i->pos.x*1000-dx, -i->pos.y*1000-dy); + i->pos.x*1000+delta.x, -i->pos.y*1000-delta.y, + i->pos.x*1000-delta.x, -i->pos.y*1000+delta.y); elem->set_attribute("d", data); } @@ -98,27 +97,23 @@ void SvgExporter::save_track(const Track &track, xmlpp::Element &group) xmlpp::Element *elem = group.add_child("path"); elem->set_attribute("class", "rail"); - float cs = cos(start.dir); - float ss = sin(start.dir); - float ce = cos(end.dir); - float se = sin(end.dir); - float dx1 = -ss*(gauge+rail_width)*0.5; - float dy1 = -cs*(gauge+rail_width)*0.5; - float dx2 = -se*(gauge+rail_width)*0.5; - float dy2 = -ce*(gauge+rail_width)*0.5; + Vector delta1 = rotated_vector(Vector(0, (gauge+rail_width)*0.5, 0), start.dir); + Vector delta2 = rotated_vector(Vector(0, (gauge+rail_width)*0.5, 0), end.dir); // Largely an educated guess, but seems to be accurate enough float clen = i->get_length()*1000/2.9; + Vector ctrl1 = rotated_vector(Vector(clen, 0, 0), start.dir); + Vector ctrl2 = rotated_vector(Vector(clen, 0, 0), end.dir); string data = format("M %.3f %.3f C %.3f %.3f %.3f %.3f %.3f %.3f") - (start.pos.x*1000+dx1)(-start.pos.y*1000+dy1) - (start.pos.x*1000+dx1+cs*clen)(-start.pos.y*1000+dy1-ss*clen) - (end.pos.x*1000+dx2-ce*clen)(-end.pos.y*1000+dy2+se*clen) - (end.pos.x*1000+dx2)(-end.pos.y*1000+dy2).str(); + (start.pos.x*1000+delta1.x)(-start.pos.y*1000-delta1.y) + (start.pos.x*1000+delta1.x+ctrl1.x)(-start.pos.y*1000-delta1.y-ctrl1.y) + (end.pos.x*1000+delta2.x-ctrl2.x)(-end.pos.y*1000-delta2.y+ctrl2.y) + (end.pos.x*1000+delta2.x)(-end.pos.y*1000-delta2.y).str(); data += format(" M %.3f %.3f C %.3f %.3f %.3f %.3f %.3f %.3f") - (start.pos.x*1000-dx1)(-start.pos.y*1000-dy1) - (start.pos.x*1000-dx1+cs*clen)(-start.pos.y*1000-dy1-ss*clen) - (end.pos.x*1000-dx2-ce*clen)(-end.pos.y*1000-dy2+se*clen) - (end.pos.x*1000-dx2)(-end.pos.y*1000-dy2).str(); + (start.pos.x*1000-delta1.x)(-start.pos.y*1000+delta1.y) + (start.pos.x*1000-delta1.x+ctrl1.x)(-start.pos.y*1000+delta1.y-ctrl1.y) + (end.pos.x*1000-delta2.x-ctrl2.x)(-end.pos.y*1000+delta2.y+ctrl2.y) + (end.pos.x*1000-delta2.x)(-end.pos.y*1000+delta2.y).str(); elem->set_attribute("d", data); } else @@ -126,32 +121,30 @@ void SvgExporter::save_track(const Track &track, xmlpp::Element &group) xmlpp::Element *elem = group.add_child("path"); elem->set_attribute("class", "rail"); - float dx = -sin(start.dir)*(gauge+rail_width)*0.5; - float dy = -cos(start.dir)*(gauge+rail_width)*0.5; + Vector delta = rotated_vector(Vector(0, (gauge+rail_width)*0.5, 0), start.dir); string data = format("M %.3f %.3f L %.3f %.3f", - start.pos.x*1000+dx, -start.pos.y*1000+dy, - end.pos.x*1000+dx, -end.pos.y*1000+dy); + start.pos.x*1000+delta.x, -start.pos.y*1000-delta.y, + end.pos.x*1000+delta.x, -end.pos.y*1000-delta.y); data += format(" M %.3f %.3f L %.3f %.3f", - start.pos.x*1000-dx, -start.pos.y*1000-dy, - end.pos.x*1000-dx, -end.pos.y*1000-dy); + start.pos.x*1000-delta.x, -start.pos.y*1000+delta.y, + end.pos.x*1000-delta.x, -end.pos.y*1000+delta.y); elem->set_attribute("d", data); } } TrackPoint label_pt = parts.front().get_point(parts.front().get_length()/2); - while(rot+label_pt.dir>M_PI/2) - label_pt.dir -= M_PI; - while(rot+label_pt.dir<-M_PI/2) - label_pt.dir += M_PI; + label_pt.dir.wrap_with_base(-Angle::quarter_turn()); + if(label_pt.dir>Angle::quarter_turn()) + label_pt.dir -= Angle::half_turn(); - label_pt.pos.x = label_pt.pos.x*1000+sin(label_pt.dir)*gauge*0.25; - label_pt.pos.y = label_pt.pos.y*1000-cos(label_pt.dir)*gauge*0.25; + label_pt.pos *= 1000; + label_pt.pos += rotated_vector(Vector(0, -gauge*0.25, 0), label_pt.dir); xmlpp::Element *elem = group.add_child("text"); elem->set_attribute("class", "artnr"); elem->set_attribute("transform", format("translate(%.3f %.3f) rotate(%.3f)", - label_pt.pos.x, -label_pt.pos.y, -label_pt.dir*180/M_PI)); + label_pt.pos.x, -label_pt.pos.y, -label_pt.dir.degrees())); elem->set_child_text(track.get_type().get_article_number().str()); } diff --git a/source/designer/trackwrap.cpp b/source/designer/trackwrap.cpp index 863434f..398285f 100644 --- a/source/designer/trackwrap.cpp +++ b/source/designer/trackwrap.cpp @@ -28,7 +28,7 @@ void TrackWrap::render(GL::Renderer &renderer, const GL::Tag &) const { GL::MatrixStack::Push push(renderer.matrix_stack()); const Vector &pos = i->track->get_position(); - renderer.matrix_stack() *= GL::Matrix::translation(pos.x, pos.y, pos.z); + renderer.matrix_stack() *= GL::Matrix::translation(pos); renderer.matrix_stack() *= GL::Matrix::rotation(i->track->get_rotation(), 0, 0, 1); i->mesh->draw(renderer); } @@ -56,22 +56,19 @@ GL::Mesh &TrackWrap::get_mesh(const TrackType &type) const TrackType3D &type3d = layout.get_catalogue().get_track(type); float min_area = -1; - float angle = 0; + Angle angle; Vector center; float width = 0; float height = 0; - for(float a=0; aget_snap_node(picking_entry); - GL::MatrixStack::modelview() *= GL::Matrix::translation(sn.position.x, sn.position.y, sn.position.z+0.03); - GL::MatrixStack::modelview() *= GL::Matrix::rotation(sn.rotation+M_PI, 0, 0, 1); + GL::MatrixStack::modelview() *= GL::Matrix::translation(sn.position+GL::Vector3(0, 0, 0.03)); + GL::MatrixStack::modelview() *= GL::Matrix::rotation(sn.rotation+Angle::half_turn(), 0, 0, 1); arrow_mesh.draw(); } @@ -313,7 +313,7 @@ void Engineer::view_all() float best_score = 0; GL::Vector3 pos; GL::Vector3 up; - for(float angle=0; angleget_point(0); TrackPoint end = i->get_point(i->get_length()); - if(end.dir>start.dir+0.01 || end.dirstart.dir ? left : right) = true; straight &= ~(1<get_path()); } - else if(start.dir<-0.01 || start.dir>0.01) + else if(abs(start.dir).radians()>0.01) cross = true; } diff --git a/source/libr2c2/geometry.h b/source/libr2c2/geometry.h index 67c5412..09044e3 100644 --- a/source/libr2c2/geometry.h +++ b/source/libr2c2/geometry.h @@ -3,28 +3,34 @@ #include #include +#include +#include +#include namespace R2C2 { -struct Vector -{ - float x, y, z; - - Vector(): x(0), y(0), z(0) { } - Vector(float x_, float y_): x(x_), y(y_), z(0) { } - Vector(float x_, float y_, float z_): x(x_), y(y_), z(z_) { } -}; +typedef Msp::LinAl::Vector Vector; inline float distance(const Vector &p, const Vector &q) -{ return sqrt((p.x-q.x)*(p.x-q.x) + (p.y-q.y)*(p.y-q.y) + (p.z-q.z)*(p.z-q.z)); } +{ return (p-q).norm(); } + +typedef Msp::Geometry::Angle Angle; + +typedef Msp::Geometry::AffineTransformation Transform; + +inline Vector rotated_vector(const Vector &v, const Angle &a) +{ return Transform::rotation(a, Vector(0, 0, 1)).transform(v); } + +inline Vector vector_at_angle(const Angle &a) +{ return rotated_vector(Vector(1, 0, 0), a); } struct TrackPoint { Vector pos; - float dir; + Angle dir; float grade; - TrackPoint(): dir(0), grade(0) { } + TrackPoint(): grade(0) { } }; } // namespace R2C2 diff --git a/source/libr2c2/layout.h b/source/libr2c2/layout.h index 5573673..48a2796 100644 --- a/source/libr2c2/layout.h +++ b/source/libr2c2/layout.h @@ -6,6 +6,7 @@ #include #include #include "block.h" +#include "geometry.h" namespace R2C2 { @@ -16,7 +17,6 @@ class Route; class Signal; class Track; class Train; -class Vector; class Vehicle; class Zone; diff --git a/source/libr2c2/object.cpp b/source/libr2c2/object.cpp index ba1810f..cc56ede 100644 --- a/source/libr2c2/object.cpp +++ b/source/libr2c2/object.cpp @@ -18,8 +18,8 @@ bool Object::snap(Snap &sn, float limit, SnapType what) const for(unsigned i=0; iget_type().get_endpoints().size(); for(unsigned j=0; jget_snap_node(j).rotation-rotation; - while(a<-M_PI/2) - a += M_PI*2; - while(a>M_PI*3/2) - a -= M_PI*2; - if(a>=M_PI/2) + Angle a = wrap_with_base(track->get_snap_node(j).rotation-rotation, -Angle::quarter_turn()); + if(a>=Angle::quarter_turn()) { BlockIter biter = TrackIter(track, j).block_iter(); entry = biter.entry(); @@ -93,19 +89,11 @@ void Signal::normalize_location() } } -void Signal::set_rotation(float r) +void Signal::set_rotation(const Angle &r) { - float a = rotation-r; - while(a>M_PI*3/2) - a -= M_PI*2; - while(a<-M_PI/2) - a += M_PI*2; - if(a>=M_PI/2) - { - rotation += M_PI; - if(rotation>M_PI*2) - rotation -= M_PI*2; - } + Angle a = wrap_with_base(rotation-r, -Angle::quarter_turn()); + if(a>=Angle::quarter_turn()) + rotation = wrap_positive(rotation+Angle::half_turn()); normalize_location(); } @@ -137,15 +125,14 @@ SnapType Signal::get_default_snap_type_to(const Object &other) const bool Signal::collide_ray(const Vector &start, const Vector &ray) const { // XXX Totally hardcoded stuff, should be replaced with a geometry system - Vector center = position; - center.x += sin(rotation)*0.035; - center.y -= cos(rotation)*0.035; - Vector d(center.x-start.x, center.y-start.y); - float x = (d.x*ray.x+d.y*ray.y)/(ray.x*ray.x+ray.y*ray.y); - Vector nearest(start.x+ray.x*x-center.x, start.y+ray.y*x-center.y, start.z+ray.z*x-center.z); - if(nearest.z<0|| nearest.z>0.12) + Vector center = position+rotated_vector(Vector(0, -0.035, 0), rotation); + Vector span = center-start; + float x = (span.x*ray.x+span.y*ray.y)/(ray.x*ray.x+ray.y*ray.y); + Vector nearest = start+ray*x-center; + if(nearest.z<0 || nearest.z>0.12) return false; - return nearest.x*nearest.x+nearest.y*nearest.y<0.0001; + nearest.z = 0; + return dot(nearest, nearest)<0.0001; } void Signal::tick(const Time::TimeDelta &) @@ -225,7 +212,7 @@ void Signal::reset() void Signal::save(list &st) const { st.push_back((DataFile::Statement("position"), position.x, position.y, position.z)); - st.push_back((DataFile::Statement("rotation"), rotation)); + st.push_back((DataFile::Statement("rotation"), rotation.radians())); if(address) st.push_back((DataFile::Statement("address"), address)); } @@ -251,7 +238,7 @@ void Signal::Loader::position(float x, float y, float z) void Signal::Loader::rotation(float d) { - obj.set_rotation(d); + obj.set_rotation(Angle::from_radians(d)); } } // namespace R2C2 diff --git a/source/libr2c2/signal.h b/source/libr2c2/signal.h index b604951..8adbd1b 100644 --- a/source/libr2c2/signal.h +++ b/source/libr2c2/signal.h @@ -48,7 +48,7 @@ public: void set_address(unsigned); virtual void set_position(const Vector &); - virtual void set_rotation(float); + virtual void set_rotation(const Angle &); private: void normalize_location(); public: diff --git a/source/libr2c2/snap.h b/source/libr2c2/snap.h index 844af3d..25777b3 100644 --- a/source/libr2c2/snap.h +++ b/source/libr2c2/snap.h @@ -1,6 +1,8 @@ #ifndef LIBR2C2_SNAP_H_ #define LIBR2C2_SNAP_H_ +#include "geometry.h" + namespace R2C2 { enum SnapType @@ -14,7 +16,7 @@ enum SnapType struct Snap { Vector position; - float rotation; + Angle rotation; }; inline SnapType operator|(SnapType s1, SnapType s2) diff --git a/source/libr2c2/track.cpp b/source/libr2c2/track.cpp index 6a6cbd9..7b5e1a2 100644 --- a/source/libr2c2/track.cpp +++ b/source/libr2c2/track.cpp @@ -74,13 +74,9 @@ void Track::set_position(const Vector &p) (*i)->check_slope(); } -void Track::set_rotation(float r) +void Track::set_rotation(const Angle &r) { - rotation = r; - while(rotation<0) - rotation += M_PI*2; - while(rotation>M_PI*2) - rotation -= M_PI*2; + rotation = wrap_positive(r); } void Track::set_slope(float s) @@ -161,10 +157,8 @@ void Track::set_active_path(unsigned p) TrackPoint Track::get_point(unsigned epi, unsigned path, float d) const { TrackPoint p = type.get_point(epi, path, d); - float c = cos(rotation); - float s = sin(rotation); - p.pos = Vector(position.x+c*p.pos.x-s*p.pos.y, position.y+s*p.pos.x+c*p.pos.y, position.z); + p.pos = position+rotated_vector(p.pos, rotation); p.dir += rotation; if(type.get_endpoints().size()==2) { @@ -204,10 +198,7 @@ Snap Track::get_snap_node(unsigned i) const Snap result; const TrackType::Endpoint &ep = eps[i]; - float c = cos(rotation); - float s = sin(rotation); - - result.position = Vector(position.x+c*ep.pos.x-s*ep.pos.y, position.y+s*ep.pos.x+c*ep.pos.y, position.z); + result.position = position+rotated_vector(ep.pos, rotation); if(eps.size()==2 && i==1) result.position.z += slope; @@ -223,16 +214,13 @@ bool Track::snap(Snap &sn, float limit, SnapType what) const if(what&SNAP_SEGMENT) { - Vector local(sn.position.x-position.x, sn.position.y-position.y, sn.position.z-position.z); - float c = cos(rotation); - float s = sin(rotation); - local = Vector(c*local.x+s*local.y, c*local.y-s*local.x, local.z); + Vector local = rotated_vector(sn.position-position, -rotation); TrackPoint tp = type.get_nearest_point(local); - Vector d(local.x-tp.pos.x, local.y-tp.pos.y, local.z-tp.pos.z); - if(d.x*d.x+d.y*d.y+d.z*d.z<=limit*limit) + Vector span = local-tp.pos; + if(dot(span, span)<=limit*limit) { - sn.position = Vector(position.x+tp.pos.x*c-tp.pos.y*s, position.y+tp.pos.y*c+tp.pos.x*s, position.z+tp.pos.z); + sn.position = position+rotated_vector(tp.pos, rotation); sn.rotation = tp.dir+rotation; return true; } @@ -290,14 +278,10 @@ bool Track::link_to(Object &other) for(unsigned j=0; jM_PI) - da -= M_PI*2; - - if(d.x*d.x+d.y*d.y-0.01 && da<0.01) + Vector span = osn.position-sn.position; + Angle da = wrap_balanced(osn.rotation-sn.rotation-Angle::half_turn()); + + if(dot(span, span) &st) const { st.push_back((DataFile::Statement("position"), position.x, position.y, position.z)); - st.push_back((DataFile::Statement("rotation"), rotation)); + st.push_back((DataFile::Statement("rotation"), rotation.radians())); st.push_back((DataFile::Statement("slope"), slope)); if(turnout_id) st.push_back((DataFile::Statement("turnout_id"), turnout_id)); @@ -391,7 +373,7 @@ void Track::Loader::position(float x, float y, float z) void Track::Loader::rotation(float r) { - obj.rotation = r; + obj.rotation = Angle::from_radians(r); } void Track::Loader::sensor_id(unsigned id) diff --git a/source/libr2c2/track.h b/source/libr2c2/track.h index 13409d5..7dfa752 100644 --- a/source/libr2c2/track.h +++ b/source/libr2c2/track.h @@ -54,7 +54,7 @@ public: void set_block(Block *); Block &get_block() const; virtual void set_position(const Vector &); - virtual void set_rotation(float); + virtual void set_rotation(const Angle &); void set_slope(float); void set_flex(bool); float get_slope() const { return slope; } diff --git a/source/libr2c2/trackpart.cpp b/source/libr2c2/trackpart.cpp index 71845bb..d8dac07 100644 --- a/source/libr2c2/trackpart.cpp +++ b/source/libr2c2/trackpart.cpp @@ -7,7 +7,6 @@ using namespace Msp; namespace R2C2 { TrackPart::TrackPart(): - dir(0), length(0), radius(0), path(0), @@ -29,19 +28,17 @@ TrackPoint TrackPart::get_point(float d) const { TrackPoint result; + Transform dir_trans = Transform::rotation(dir, Vector(0, 0, 1)); if(radius) { - float a = d/radius; - float c = cos(a); - float s = sin(a); - float rx = radius*sin(dir); - float ry = -radius*cos(dir); - result.pos = Vector(pos.x+c*rx-s*ry-rx, pos.y+c*ry+s*rx-ry); + Angle a = Angle::from_radians(d/radius); + Vector r = dir_trans.transform(Vector(0, -radius, 0)); + result.pos = pos-r+Transform::rotation(a, Vector(0, 0, 1)).transform(r); result.dir = dir+a; } else { - result.pos = Vector(pos.x+cos(dir)*d, pos.y+sin(dir)*d); + result.pos = pos+dir_trans.transform(Vector(d, 0, 0)); result.dir = dir; } @@ -51,29 +48,27 @@ TrackPoint TrackPart::get_point(float d) const TrackPoint TrackPart::get_nearest_point(const Vector &p) const { TrackPoint tp; - float c = cos(dir); - float s = sin(dir); + Transform dir_trans = Transform::rotation(dir, Vector(0, 0, 1)); if(radius) { - Vector v(p.x-pos.x+radius*s, p.y-pos.y-radius*c); - float a = atan2(v.y, v.x)+M_PI/2-dir; + Vector r = dir_trans.transform(Vector(0, -radius, 0)); + Vector v = p-pos+r; + Angle a = Geometry::atan2(v.y, v.x)+Angle::quarter_turn()-dir; if(radius<0) - a = M_PI-a; - while(alength/2+M_PI) - a -= M_PI*2; - a = min(max(a, 0.0f), length); + a = Angle::half_turn()-a; + a.wrap_with_base(Angle::from_radians(length/2)-Angle::half_turn()); + a = min(max(a, Angle::zero()), Angle::from_radians(length)); if(radius<0) a = -a; - tp.pos = Vector(pos.x+radius*(sin(dir+a)-s), pos.y+radius*(c-cos(dir+a))); + tp.pos = pos-r+Transform::rotation(a, Vector(0, 0, 1)).transform(r); tp.dir = dir+a; } else { - Vector v(p.x-pos.x, p.y-pos.y); - float d = min(max(c*v.x+s*v.y, 0.0f), length); - tp.pos = Vector(pos.x+c*d, pos.y+s*d); + Vector v = p-pos; + Vector dir_vec = dir_trans.transform(Vector(1, 0, 0)); + float d = min(max(dot(dir_vec, v), 0.0f), length); + tp.pos = pos+dir_vec*d; tp.dir = dir; } return tp; @@ -90,16 +85,11 @@ void TrackPart::check_link(TrackPart &other) { TrackPoint p2 = other.get_point(j ? other.get_length() : 0); - float dx = p2.pos.x-p1.pos.x; - float dy = p2.pos.y-p1.pos.y; + Vector span = p2.pos-p1.pos; - float da = p2.dir-p1.dir+M_PI*((i+j+1)%2); - while(da>M_PI) - da -= M_PI*2; - while(da<-M_PI) - da += M_PI*2; + Angle da = wrap_balanced(p2.dir-p1.dir+Angle::half_turn()*float((i+j+1)%2)); - if(dx*dx+dy*dy<1e-6 && da>=-0.01 && da<=0.01) + if(dot(span, span)<1e-6 && abs(da).radians()<=0.01) { links[i] = &other; other.links[j] = this; @@ -118,24 +108,22 @@ TrackPart *TrackPart::get_link(unsigned i) const bool TrackPart::collide_ray(const Vector &start, const Vector &ray, float width) const { - Vector local_start(start.x-pos.x, start.y-pos.y, start.z); - float c = cos(dir); - float s = sin(dir); - local_start = Vector(c*local_start.x+s*local_start.y, c*local_start.y-s*local_start.x, local_start.z); - Vector local_ray(c*ray.x+s*ray.y, c*ray.y-s*ray.x, ray.z); + Transform trans = Transform::rotation(dir, Vector(0, 0, -1)); + Vector local_start = trans.transform(start); + Vector local_ray = trans.transform_linear(ray); float d = -local_start.z/local_ray.z; if(d<0) return false; - Vector base(local_start.x+d*local_ray.x, local_start.y+d*local_ray.y); + Vector base = local_start+d*local_ray; if(radius) { base.y -= radius; if(radius<0) base.y = -base.y; - float r = sqrt(base.x*base.x+base.y*base.y)-abs(radius); + float r = base.norm()-abs(radius); float a = atan2(base.x, -base.y); return (a>=0 && a<=length && r>=-width/2 && r<=width/2); } @@ -164,15 +152,13 @@ void TrackPart::Loader::finish() else obj.length /= 1000; - obj.pos.x /= 1000; - obj.pos.y /= 1000; - obj.dir *= M_PI/180; + obj.pos /= 1000; } void TrackPart::Loader::start(float x, float y, float d) { - obj.pos = Vector(x, y); - obj.dir = d; + obj.pos = Vector(x, y, 0); + obj.dir = Angle::from_degrees(d); } } // namespace R2C2 diff --git a/source/libr2c2/trackpart.h b/source/libr2c2/trackpart.h index 787a6ca..9143ff8 100644 --- a/source/libr2c2/trackpart.h +++ b/source/libr2c2/trackpart.h @@ -20,7 +20,7 @@ public: private: Vector pos; - float dir; + Angle dir; float length; float radius; unsigned path; diff --git a/source/libr2c2/tracktype.cpp b/source/libr2c2/tracktype.cpp index 3be1ecb..fbed4c1 100644 --- a/source/libr2c2/tracktype.cpp +++ b/source/libr2c2/tracktype.cpp @@ -76,9 +76,8 @@ TrackPoint TrackType::get_point(unsigned epi, unsigned path, float d) const for(unsigned j=0; jget_point(j ? i->get_length() : 0); - float dx = p.pos.x-endpoints[epi].pos.x; - float dy = p.pos.y-endpoints[epi].pos.y; - if(dx*dx+dy*dy<1e-6) + Vector span = p.pos-endpoints[epi].pos; + if(dot(span, span)<1e-6) { part = &*i; part_ep = j; @@ -98,7 +97,7 @@ TrackPoint TrackType::get_point(unsigned epi, unsigned path, float d) const d = plen-d; TrackPoint p = part->get_point(d); if(part_ep==1) - p.dir += M_PI; + p.dir += Angle::half_turn(); return p; } else @@ -156,21 +155,16 @@ void TrackType::collect_endpoints() { TrackPoint p = i->get_point(j ? i->get_length() : 0); if(j==0) - p.dir += M_PI; + p.dir += Angle::half_turn(); bool found = false; for(vector::iterator k=endpoints.begin(); k!=endpoints.end(); ++k) { - float dx = k->pos.x-p.pos.x; - float dy = k->pos.y-p.pos.y; + Vector d = k->pos-p.pos; - float da = k->dir-p.dir; - while(da>M_PI) - da -= M_PI*2; - while(da<-M_PI) - da += M_PI*2; + Angle da = wrap_balanced(k->dir-p.dir); - if(dx*dx+dy*dy<1e-6 && da>-0.01 && da<0.01) + if(dot(d, d)<1e-6 && abs(da).radians()<0.01) { k->paths |= 1<get_path(); found = true; @@ -184,8 +178,8 @@ void TrackType::collect_endpoints() } } -TrackType::Endpoint::Endpoint(float x, float y, float d, unsigned p): - pos(x, y), +TrackType::Endpoint::Endpoint(float x, float y, const Angle &d, unsigned p): + pos(x, y, 0), dir(d), paths(p) { } diff --git a/source/libr2c2/tracktype.h b/source/libr2c2/tracktype.h index afc8521..f446064 100644 --- a/source/libr2c2/tracktype.h +++ b/source/libr2c2/tracktype.h @@ -15,10 +15,10 @@ public: struct Endpoint { Vector pos; - float dir; // Direction outwards from the endpoint + Angle dir; // Direction outwards from the endpoint unsigned paths; - Endpoint(float, float, float, unsigned); + Endpoint(float, float, const Angle &, unsigned); bool has_path(unsigned p) const { return paths&(1<::iterator i=axles.begin(); i!=axles.end(); ++i) - i->angle += d*2/i->type->wheel_dia; + i->angle += Angle::from_radians(d*2/i->type->wheel_dia); for(vector::iterator i=bogies.begin(); i!=bogies.end(); ++i) for(vector::iterator j=i->axles.begin(); j!=i->axles.end(); ++j) - j->angle += d*2/j->type->wheel_dia; + j->angle += Angle::from_radians(d*2/j->type->wheel_dia); update_rods(); } @@ -291,19 +291,17 @@ void Vehicle::update_rods() else if(i->type->pivot==VehicleType::Rod::AXLE) { const Axle &axle = get_fixed_axle(i->type->pivot_index); - float c = cos(axle.angle); - float s = sin(axle.angle); const Vector &pp = i->type->pivot_point; - i->position = Vector(axle.type->position+pp.x*c+pp.z*s, pp.y, axle.type->wheel_dia/2+pp.z*c-pp.x*s); + Transform trans = Transform::rotation(axle.angle, Vector(0, -1, 0)); + i->position = Vector(axle.type->position, 0, axle.type->wheel_dia/2)+trans.transform(pp); } else if(i->type->pivot==VehicleType::Rod::ROD) { const Rod &prod = get_rod(i->type->pivot_index); - float c = cos(prod.angle); - float s = sin(prod.angle); const Vector &pos = prod.position; const Vector &off = i->type->pivot_point; - i->position = Vector(pos.x+off.x*c-off.z*s, pos.y+off.y, pos.z+off.z*c+off.x*s); + Transform trans = Transform::rotation(prod.angle, Vector(0, 1, 0)); + i->position = pos+trans.transform(off); } if(i->type->connect_index>=0) @@ -311,30 +309,27 @@ void Vehicle::update_rods() Rod &crod = rods[i->type->connect_index]; if(i->type->limit==VehicleType::Rod::ROTATE && crod.type->limit==VehicleType::Rod::SLIDE_X) { - float dx = (crod.position.x+i->type->connect_offset.x)-i->position.x; - float dz = (crod.position.z+i->type->connect_offset.z)-i->position.z; - float cd = sqrt(i->type->connect_point.x*i->type->connect_point.x+i->type->connect_point.z*i->type->connect_point.z); - float ca = atan2(i->type->connect_point.z, i->type->connect_point.x); - dx = sqrt(cd*cd-dz*dz)*(dx>0 ? 1 : -1); - i->angle = atan2(dz, dx)-ca; - crod.position.x = i->position.x+dx-i->type->connect_offset.x; + Vector span = crod.position+i->type->connect_offset-i->position; + float cd = i->type->connect_point.norm(); + Angle ca = Geometry::atan2(i->type->connect_point.z, i->type->connect_point.x); + span.x = sqrt(cd*cd-span.z*span.z)*(span.x>0 ? 1 : -1); + i->angle = Geometry::atan2(span.z, span.x)-ca; + crod.position.x = i->position.x+span.x-i->type->connect_offset.x; } else if(i->type->limit==VehicleType::Rod::ROTATE && crod.type->limit==VehicleType::Rod::ROTATE) { - float dx = crod.position.x-i->position.x; - float dz = crod.position.z-i->position.z; - float d = sqrt(dx*dx+dz*dz); - float cd1 = sqrt(i->type->connect_point.x*i->type->connect_point.x+i->type->connect_point.z*i->type->connect_point.z); - float cd2 = sqrt(i->type->connect_offset.x*i->type->connect_offset.x+i->type->connect_offset.z*i->type->connect_offset.z); + Vector span = crod.position-i->position; + float d = span.norm(); + float cd1 = i->type->connect_point.norm(); + float cd2 = i->type->connect_offset.norm(); float a = (d*d+cd1*cd1-cd2*cd2)/(2*d); float b = sqrt(cd1*cd1-a*a); - float sign = (dx*i->type->connect_point.z-dz*i->type->connect_point.x>0 ? 1 : -1); - float cx = (dx*a-dz*b*sign)/d; - float cz = (dz*a+dx*b*sign)/d; - float ca1 = atan2(i->type->connect_point.z, i->type->connect_point.x); - float ca2 = atan2(i->type->connect_offset.z, i->type->connect_offset.x); - i->angle = atan2(cz, cx)-ca1; - crod.angle = atan2(cz-dz, cx-dx)-ca2; + float sign = (cross(i->type->connect_point, span).y>0 ? 1 : -1); + Vector conn = Vector(span.x*a-span.z*b, 0, span.z*a+span.x*b)/(d*sign); + Angle ca1 = Geometry::atan2(i->type->connect_point.z, i->type->connect_point.x); + Angle ca2 = Geometry::atan2(i->type->connect_offset.z, i->type->connect_offset.x); + i->angle = Geometry::atan2(conn.z, conn.x)-ca1; + crod.angle = Geometry::atan2(conn.z-span.z, conn.x-span.x)-ca2; } } } @@ -349,10 +344,7 @@ void Vehicle::adjust_for_distance(TrackPosition &front, TrackPosition &back, flo Vector front_point = front.get_point().pos; Vector back_point = back.get_point().pos; - float dx = front_point.x-back_point.x; - float dy = front_point.y-back_point.y; - float dz = front_point.z-back_point.z; - float dist = sqrt(dx*dx+dy*dy+dz*dz); + float dist = distance(front_point, back_point); float diff = tdist-dist; if(diff<-margin && adjust_dir<=0) @@ -375,13 +367,11 @@ void Vehicle::adjust_for_distance(TrackPosition &front, TrackPosition &back, flo TrackPoint Vehicle::get_point(const Vector &front, const Vector &back, float ratio) const { - float dx = front.x-back.x; - float dy = front.y-back.y; - float dz = front.z-back.z; + Vector span = front-back; TrackPoint tp; - tp.pos = Vector(back.x+dx*ratio, back.y+dy*ratio, back.z+dz*ratio); - tp.dir = atan2(dy, dx); + tp.pos = back+span*ratio; + tp.dir = Geometry::atan2(span.y, span.x); return tp; } @@ -423,14 +413,12 @@ int Vehicle::get_link_slot(const Object &other) const Vehicle::Axle::Axle(const VehicleType::Axle &t): - type(&t), - angle(0) + type(&t) { } Vehicle::Bogie::Bogie(const VehicleType::Bogie &t): - type(&t), - direction(0) + type(&t) { for(VehicleType::AxleArray::const_iterator i=type->axles.begin(); i!=type->axles.end(); ++i) axles.push_back(*i); @@ -438,8 +426,7 @@ Vehicle::Bogie::Bogie(const VehicleType::Bogie &t): Vehicle::Rod::Rod(const VehicleType::Rod &t): - type(&t), - angle(0) + type(&t) { } diff --git a/source/libr2c2/vehicle.h b/source/libr2c2/vehicle.h index bbd156c..bd815ab 100644 --- a/source/libr2c2/vehicle.h +++ b/source/libr2c2/vehicle.h @@ -33,7 +33,7 @@ public: struct Axle { const VehicleType::Axle *type; - float angle; + Angle angle; Axle(const VehicleType::Axle &); }; @@ -41,7 +41,7 @@ public: struct Bogie { const VehicleType::Bogie *type; - float direction; + Angle direction; std::vector axles; Bogie(const VehicleType::Bogie &); @@ -51,7 +51,7 @@ public: { const VehicleType::Rod *type; Vector position; - float angle; + Angle angle; Rod(const VehicleType::Rod &); }; @@ -95,7 +95,7 @@ public: // TODO implement these - should call place() with suitable parameters virtual void set_position(const Vector &) { } - virtual void set_rotation(float) { } + virtual void set_rotation(const Angle &) { } void place(Track &, unsigned, float, PlaceMode = CENTER); void unplace(); void advance(float);