{
require "mspcore";
require "mspdatafile";
+ require "mspmath";
require "sigc++-2.0";
library "r2c2"
{
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)
{
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;
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))
{
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);
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);
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);
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);
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)
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;
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;
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)
{
for(unsigned i=0; i<vertices.size(); ++i)
{
const float *v = vertices[i]+vertex_offs;
- border.push_back(Vector(v[0], v[1]));
+ border.push_back(Vector(v[0], v[1], 0));
}
}
}
unsigned index = 0;
bld.texcoord(0.25, 0.5);
for(vector<TrackPart>::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<TrackPart>::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<TrackPart>::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);
unsigned index = 0;
for(vector<TrackPart>::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);
}
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;
for(vector<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);
}
}
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; j<n_vertices; ++j)
{
const Profile::Vertex &v = profile.get_vertex(mirror ? n_vertices-1-j : j);
- Vector p = v.pos;
+ Vector p(0, -v.pos.x, v.pos.y);
if(mirror)
- p.x = -p.x;
- p.z = basep.pos.z+p.y+offset.y;
- p.y = basep.pos.y-c*(p.x+offset.x);
- p.x = basep.pos.x+s*(p.x+offset.x);
+ p.y = -p.y;
+ p = basep.pos+trans.transform(offset+p);
- Vector n = v.normal;
+ Vector n(0, -v.normal.x, v.normal.y);
if(mirror)
- n.x = -n.x;
+ n.y = -n.y;
+ n = trans.transform(n);
- bld.normal(s*n.x, -c*n.x, n.y);
+ bld.normal(n.x, n.y, n.z);
bld.vertex(p.x, p.y, p.z);
border.push_back(p);
TrackType3D(Catalogue3D &, const TrackType &);
~TrackType3D();
- void get_bounds(float, Vector &, Vector &) const;
+ void get_bounds(const Angle &, Vector &, Vector &) const;
const Msp::GL::Object &get_object() const { return *object; }
const Msp::GL::Mesh &get_path_mesh(unsigned) const;
void Vehicle3D::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);
- float rot = vehicle.get_rotation();
+ matrix.translate(vehicle.get_position());
+ Angle rot = vehicle.get_rotation();
if(vehicle.get_type().get_rotate_object())
- rot += M_PI;
+ rot += Angle::half_turn();
matrix.rotate(rot, 0, 0, 1);
renderer.matrix_stack() *= matrix;
}
float dist = size/t;
if(!tight)
dist += sin(camera.get_field_of_view()/2)*size;
- GL::Vector3 center((minp.x+maxp.x)/2, (minp.y+maxp.y)/2, 0);
+ GL::Vector3 center = (minp+maxp)/2.0f;
const GL::Vector3 &look = camera.get_look_direction();
- camera.set_position(GL::Vector3(center.x-look.x*dist, center.y-look.y*dist, center.z-look.z*dist));
+ camera.set_position(center-look*dist);
}
void View3D::render()
GL::Vector3 focus = get_focus();
float dist = get_distance();
camera.set_look_direction(look);
- camera.set_position(GL::Vector3(focus.x-look.x*dist, focus.y-look.y*dist, focus.z-look.z*dist));
+ camera.set_position(focus-look*dist);
}
void CameraController::view_all()
{
const GL::Vector3 &pos = camera.get_position();
const GL::Vector3 &look = camera.get_look_direction();
- float xy_len = sqrt(look.x*look.x+look.y*look.y);
- float dx = (look.x*y+look.y*x)/xy_len;
- float dy = (look.y*y-look.x*x)/xy_len;
- camera.set_position(GL::Vector3(pos.x+dx, pos.y+dy, pos.z));
+ GL::Vector3 fwd_dir = normalize(GL::Vector3(look.x, look.y, 0));
+ GL::Vector3 right_dir = cross(fwd_dir, GL::Vector3(0, 0, 1));
+ camera.set_position(pos+fwd_dir*y+right_dir*x);
}
void CameraController::adjust_distance(float delta)
float low = view.get_layout().get_layout().get_catalogue().get_gauge()*5;
if(dist+delta<low)
delta = low-dist;
- camera.set_position(GL::Vector3(pos.x-look.x*delta, pos.y-look.y*delta, pos.z-look.z*delta));
+ camera.set_position(pos-look*delta);
dist += delta;
camera.set_depth_clip(dist*0.02, dist*50);
}
-void CameraController::rotate(float angle)
+void CameraController::rotate(const Angle &angle)
{
GL::Vector3 look = camera.get_look_direction();
- float c = cos(angle);
- float s = sin(angle);
- set_look_direction(GL::Vector3(look.x*c-look.y*s, look.x*s+look.y*c, look.z));
+ set_look_direction(rotated_vector(look, angle));
}
-void CameraController::pitch(float angle)
+void CameraController::pitch(const Angle &angle)
{
GL::Vector3 look = camera.get_look_direction();
float xy_len = sqrt(look.x*look.x+look.y*look.y);
else if(drag_mode==ROTATE)
{
if(axis==0)
- rotate(-change*M_PI);
+ rotate(Angle::from_turns(-change/2));
else if(axis==1)
- pitch(change*M_PI/2);
+ pitch(Angle::from_turns(change/4));
}
else if(drag_mode==DISTANCE && axis==1)
adjust_distance(-change*3*get_distance());
void move(float, float);
void adjust_distance(float);
- void rotate(float);
- void pitch(float);
+ void rotate(const R2C2::Angle &);
+ void pitch(const R2C2::Angle &);
void tick(float);
private:
GL::Vector4 vec = main_view->get_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()
{
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)
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();
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);
}
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));
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;
}
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;
for(vector<MObject>::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);
}
delete *i;
extend_tracks.clear();
- angle = 0;
+ angle = Angle::zero();
signal_done.emit(false);
}
{
Mode m = mode;
mode = NONE;
- angle = 0;
+ angle = Angle::zero();
if(m!=EXTEND)
{
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<MObject>::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);
}
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<MObject>::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<MObject>::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);
}
}
signal_status.emit(format("Elevation: %+.0fmm (%.0fmm)", dz*1000, (center.z+dz)*1000));
for(vector<MObject>::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<MObject>::iterator i=objects.begin(); i!=objects.end(); ++i)
{
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<length)
+ if(delta.x<length)
continue;
pos = sn.position;
dir = sn.rotation;
- length = len;
+ length = delta.x;
}
}
}
}
- center = Vector((minp.x+maxp.x)/2, (minp.y+maxp.y)/2, minp.z);
+ center = (minp+maxp)/2.0f;
+ center.z = minp.z;
for(vector<MObject>::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();
}
}
}
}
-vector<Track *> Manipulator::create_straight(const R2C2::Vector &start, float dir, float length, float limit)
+vector<Track *> 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<float, const TrackType *> types_by_length;
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<float>::iterator i=lengths.begin(); i!=lengths.end(); ++i)
{
Track *track = new Track(designer.get_layout(), *get_item(types_by_length, *i));
track->link_to(*trks.back());
trks.push_back(track);
- pos.x += c**i;
- pos.y += s**i;
+ pos += trans.transform(Vector(*i, 0, 0));
}
}
#define MANIPULATOR_H_
#include <sigc++/sigc++.h>
+#include "libr2c2/geometry.h"
class Designer;
class Selection;
{
R2C2::Object *object;
R2C2::Vector pos;
- float rot;
+ R2C2::Angle rot;
MObject(R2C2::Object *);
};
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<R2C2::Object *> neighbors;
std::vector<R2C2::Track *> extend_tracks;
void update_objects();
void update_neighbors();
void set_slope(TrackOrder &, float, float);
- std::vector<R2C2::Track *> create_straight(const R2C2::Vector &, float, float, float);
+ std::vector<R2C2::Track *> create_straight(const R2C2::Vector &, const R2C2::Angle &, float, float);
};
#endif
if(btn==1)
{
ssnap.position = Vector(gx, gy, 0);
- ssnap.rotation = 0;
+ ssnap.rotation = Angle::zero();
snap_to_tracks(ssnap);
state = ACTIVE;
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();
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);
}
#include <sigc++/sigc++.h>
#include <msp/gl/renderable.h>
+#include "libr2c2/geometry.h"
class Designer;
R2C2::Snap ssnap;
float par_dist;
float perp_dist;
- float adiff;
+ R2C2::Angle adiff;
State state;
Msp::GL::Mesh mesh;
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);
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();
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);
}
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
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());
}
{
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);
}
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; a<M_PI; a+=0.01)
+ for(Angle a; a<Angle::half_turn(); a+=Angle::from_radians(0.01))
{
Vector minp, maxp;
type3d.get_bounds(a, minp, maxp);
float area = (maxp.x-minp.x)*(maxp.y-minp.y);
if(area<min_area || min_area<0)
{
- float c = cos(a);
- float s = sin(a);
- float x = (minp.x+maxp.x)/2;
- float y = (minp.y+maxp.y)/2;
- center = Vector(c*x-s*y, s*x+c*y, minp.z);
+ center = rotated_vector((minp+maxp)/2.0f, a);
+ center.z = minp.z;
angle = a;
width = maxp.x-minp.x+0.01;
height = maxp.y-minp.y+0.01;
Snap sn = picking_track->get_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();
}
float best_score = 0;
GL::Vector3 pos;
GL::Vector3 up;
- for(float angle=0; angle<M_PI; angle+=0.01)
+ for(Angle angle; angle<Angle::half_turn(); angle+=Angle::from_radians(0.01))
{
float min_x = 0;
float max_x = 0;
{
const Vehicle &veh = train.get_vehicle(0);
const Vector &pos = veh.get_position();
- float angle = veh.get_rotation();
+ Angle angle = veh.get_rotation();
if(!forward)
- angle += M_PI;
- float c = cos(angle);
- float s = sin(angle);
+ angle += Angle::half_turn();
+ Vector fwd_vec = rotated_vector(Vector(1, 0, 0), angle);
+ Vector side_vec = rotated_vector(Vector(0, -1, 0), angle);
float l = veh.get_type().get_length();
switch(mode)
{
case ROOF:
- camera.set_position(GL::Vector3(pos.x-l*c, pos.y-l*s, pos.z+0.07));
- camera.set_look_direction(GL::Vector3(c, s, -0.2));
+ camera.set_position(pos-l*fwd_vec+Vector(0, 0, 0.07));
+ camera.set_look_direction(fwd_vec-Vector(0, 0, -0.2));
break;
case SIDE:
- camera.set_position(GL::Vector3(pos.x-l*0.8*c+0.05*s, pos.y-l*0.8*s-0.05*c, pos.z+0.03));
- camera.set_look_direction(GL::Vector3(c-0.2*s, s+0.2*c, 0));
+ camera.set_position(pos-0.8f*fwd_vec+0.05f*side_vec+Vector(0, 0, 0.03));
+ camera.set_look_direction(fwd_vec-side_vec*0.2f);
break;
case HEAD:
- camera.set_position(GL::Vector3(pos.x+l*0.55*c, pos.y+l*0.55*s, pos.z+0.03));
- camera.set_look_direction(GL::Vector3(c, s, 0));
+ camera.set_position(pos+fwd_vec*(l*0.55f)+Vector(0, 0, 0.03));
+ camera.set_look_direction(fwd_vec);
break;
}
{
obj.gauge = g/1000;
obj.path_profile = Profile();
- obj.path_profile.append_vertex(Vector(0.1*obj.gauge, 0), false);
- obj.path_profile.append_vertex(Vector(-0.1*obj.gauge, 0), false);
+ obj.path_profile.append_vertex(Vector(0.1*obj.gauge, 0, 0), false);
+ obj.path_profile.append_vertex(Vector(-0.1*obj.gauge, 0, 0), false);
}
void Catalogue::Loader::layout()
{
TrackPoint start = i->get_point(0);
TrackPoint end = i->get_point(i->get_length());
- if(end.dir>start.dir+0.01 || end.dir<start.dir-0.01)
+ if(abs(end.dir-start.dir).radians()<0.01)
{
(end.dir>start.dir ? left : right) = true;
straight &= ~(1<<i->get_path());
}
- else if(start.dir<-0.01 || start.dir>0.01)
+ else if(abs(start.dir).radians()>0.01)
cross = true;
}
#include <cmath>
#include <vector>
+#include <msp/geometry/affinetransformation.h>
+#include <msp/geometry/angle.h>
+#include <msp/linal/vector.h>
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<float, 3> 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<float> Angle;
+
+typedef Msp::Geometry::AffineTransformation<float, 3> 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
#include <msp/datafile/objectloader.h>
#include <msp/time/timestamp.h>
#include "block.h"
+#include "geometry.h"
namespace R2C2 {
class Signal;
class Track;
class Train;
-class Vector;
class Vehicle;
class Zone;
for(unsigned i=0; i<nsn; ++i)
{
Snap node = get_snap_node(i);
- Vector d(sn.position.x-node.position.x, sn.position.y-node.position.y, sn.position.z-node.position.z);
- if(d.x*d.x+d.y*d.y<limit*limit)
+ Vector span = sn.position-node.position;
+ if(dot(span, span)<limit*limit)
{
sn = node;
return true;
Snap ssn = sn;
if(other.snap(ssn, limit, what))
{
- set_rotation(rotation+ssn.rotation-sn.rotation-M_PI);
+ set_rotation(rotation+ssn.rotation-sn.rotation-Angle::half_turn());
sn = get_snap_node(i);
- position.x += ssn.position.x-sn.position.x;
- position.y += ssn.position.y-sn.position.y;
- position.z += ssn.position.z-sn.position.z;
+ position += ssn.position-sn.position;
return true;
}
}
protected:
Layout &layout;
Vector position;
- float rotation;
+ Angle rotation;
Object(Layout &l): layout(l) { }
public:
virtual const ObjectType &get_type() const = 0;
Layout &get_layout() const { return layout; }
virtual void set_position(const Vector &) = 0;
- virtual void set_rotation(float) = 0;
+ virtual void set_rotation(const Angle &) = 0;
const Vector &get_position() const { return position; }
- float get_rotation() const { return rotation; }
+ const Angle &get_rotation() const { return rotation; }
virtual Object *get_parent() const { return 0; }
virtual unsigned get_n_snap_nodes() const { return 0; }
if(!vertices.empty())
{
- float dx = p.x-vertices.back().pos.x;
- float dy = p.y-vertices.back().pos.y;
- float len = sqrt(dx*dx+dy*dy);
- v.normal.x = dy/len;
- v.normal.y = -dx/len;
+ Vector span = p-vertices.back().pos;
+ span.normalize();
+ v.normal.x = span.y;
+ v.normal.y = -span.x;
if(vertices.back().smooth)
{
Vector &n = vertices.back().normal;
- n.x += v.normal.x;
- n.y += v.normal.y;
- len = sqrt(n.x*n.x+n.y*n.y);
- n.x /= len;
- n.y /= len;
+ n += v.normal;
+ n.normalize();
}
else
vertices.back().normal = v.normal;
void Profile::Loader::point(float x, float y)
{
- obj.append_vertex(Vector(x/1000, y/1000), false);
+ obj.append_vertex(Vector(x/1000, y/1000, 0), false);
}
void Profile::Loader::smooth_point(float x, float y)
{
- obj.append_vertex(Vector(x/1000, y/1000), true);
+ obj.append_vertex(Vector(x/1000, y/1000, 0), true);
}
} // namespace R2C2
unsigned n_endpoints = track->get_type().get_endpoints().size();
for(unsigned j=0; j<n_endpoints; ++j)
{
- float a = track->get_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();
}
}
-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();
}
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 &)
void Signal::save(list<DataFile::Statement> &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));
}
void Signal::Loader::rotation(float d)
{
- obj.set_rotation(d);
+ obj.set_rotation(Angle::from_radians(d));
}
} // namespace R2C2
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:
#ifndef LIBR2C2_SNAP_H_
#define LIBR2C2_SNAP_H_
+#include "geometry.h"
+
namespace R2C2 {
enum SnapType
struct Snap
{
Vector position;
- float rotation;
+ Angle rotation;
};
inline SnapType operator|(SnapType s1, SnapType s2)
(*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)
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)
{
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;
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;
}
for(unsigned j=0; j<other_nsn; ++j)
{
Snap osn = other.get_snap_node(j);
- Vector d(osn.position.x-sn.position.x, osn.position.y-sn.position.y, osn.position.z-sn.position.z);
- float da = osn.rotation-sn.rotation-M_PI;
- while(da<-M_PI)
- da += M_PI*2;
- while(da>M_PI)
- da -= M_PI*2;
-
- if(d.x*d.x+d.y*d.y<limit && d.z*d.z<limit && da>-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)<limit && abs(da).radians()<0.01)
{
break_link(i);
links[i] = otrack;
bool Track::collide_ray(const Vector &start, const Vector &ray) const
{
- Vector local_start(start.x-position.x, start.y-position.y, start.z-position.z);
- float c = cos(rotation);
- float s = sin(rotation);
- 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(-rotation, Vector(0, 0, 1));
+ Vector local_start = trans.transform(start-position);
+ Vector local_ray = trans.transform(ray);
float width = layout.get_catalogue().get_ballast_profile().get_width();
void Track::save(list<DataFile::Statement> &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));
void Track::Loader::rotation(float r)
{
- obj.rotation = r;
+ obj.rotation = Angle::from_radians(r);
}
void Track::Loader::sensor_id(unsigned id)
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; }
namespace R2C2 {
TrackPart::TrackPart():
- dir(0),
length(0),
radius(0),
path(0),
{
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;
}
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(a<length/2-M_PI)
- a += M_PI*2;
- while(a>length/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;
{
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;
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);
}
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
private:
Vector pos;
- float dir;
+ Angle dir;
float length;
float radius;
unsigned path;
for(unsigned j=0; j<n_part_eps; ++j)
{
TrackPoint p = i->get_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;
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
{
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<Endpoint>::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<<i->get_path();
found = true;
}
}
-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)
{ }
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<<p); }
bool has_common_paths(const Endpoint &e) const { return paths&e.paths; }
void Vehicle::turn_axles(float d)
{
for(vector<Axle>::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<Bogie>::iterator i=bogies.begin(); i!=bogies.end(); ++i)
for(vector<Axle>::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();
}
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)
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;
}
}
}
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)
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;
}
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);
Vehicle::Rod::Rod(const VehicleType::Rod &t):
- type(&t),
- angle(0)
+ type(&t)
{ }
struct Axle
{
const VehicleType::Axle *type;
- float angle;
+ Angle angle;
Axle(const VehicleType::Axle &);
};
struct Bogie
{
const VehicleType::Bogie *type;
- float direction;
+ Angle direction;
std::vector<Axle> axles;
Bogie(const VehicleType::Bogie &);
{
const VehicleType::Rod *type;
Vector position;
- float angle;
+ Angle angle;
Rod(const VehicleType::Rod &);
};
// 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);
projects / r2c2.git / commitdiff