From: Mikko Rasa Date: Sun, 11 Aug 2013 12:28:13 +0000 (+0300) Subject: Rewrite rod simulation code X-Git-Url: http://git.tdb.fi/?a=commitdiff_plain;h=dd05e8ed55a9072daaf18e4c63c3b66e386ba7f4;p=r2c2.git Rewrite rod simulation code Instead of trying to get a perfect solution on the first try, solve rod interconnection constraints iteratively. This simplifies the code and allows for certain rod configurations that would have been prohibitively difficult to solve exactly. It's now also possible to create a mirrored set of rods on the other side of the locomotive with a single statement. --- diff --git a/locos.dat b/locos.dat index cb097cd..2fb9181 100644 --- a/locos.dat +++ b/locos.dat @@ -59,153 +59,160 @@ vehicle \29820-02 axle { position -29.5; wheel_diameter 16; powered true; object "br50-axle.object"; }; axle { position -49; wheel_diameter 16; powered true; object "br50-axle.object"; }; - rod + rod "coupling_r" { - pivot_axle 2; - position 0 -10.5 -3.5; - limit FIXED; + move + { + target_axle 3; + target_position 0 -10.5 -3.5; + }; object "br50-rod-coupling.object"; }; - rod + + rod "main_r" { - pivot_body; - position 27 -14 8; - limit SLIDE_X; - tag "piston"; - object "br50-rod-piston.object"; + move + { + target_axle 3; + target_position 0 -13 -3.5; + }; + rotate + { + target_rod "piston_r"; + target_position 0 1 0; + local_position 37.5 0 0; + }; + object "br50-rod-main.object"; }; - rod + + rod "piston_r" { - pivot_axle 2; - position 0 -13 -3.5; - limit ROTATE; - connect "previous" 37.5 0 0 0; - object "br50-rod-main.object"; + initial_position 28 -14 8; + move + { + target_rod "main_r"; + target_position 37.5 -1 0; + }; + slide + { + target_position 27 -14 8; + axis 1 0 0; + }; + object "br50-rod-piston.object"; }; - rod + + rod "eccentric_r" { - pivot_axle 2; - position 1.5 -15 0; - limit ROTATE; + move + { + target_axle 3; + target_position 1.5 -15 0; + }; + rotate + { + target_rod "expansion_r"; + target_position 0 -0.5 -4.5; + local_position 21 0 0; + }; object "br50-rod-eccentric.object"; }; - rod + + rod "expansion_r" { - pivot_body; - position 10.5 -14.5 16.5; - limit ROTATE; - connect "previous" 0 -4.5 21 0; - tag "link"; + initial_position 10.5 -14.5 16.5; + rotate + { + target_rod "eccentric_r"; + target_position 21 0.5 0; + local_position 0 0 -4.5; + }; object "br50-rod-link.object"; }; - rod - { - pivot_body; - position 32 -16 15.5; - limit SLIDE_X; - object "br50-rod-valve.object"; - }; - rod - { - pivot_rod "link"; - position 0.2 -0.5 -2.5; - limit ROTATE; - connect "previous" 22 -1.5 0 0; + + rod "radius_guide_r" + { + initial_position 33 -16 15.5; + move + { + target_rod "valve_r"; + }; + rotate + { + target_rod "radius_r"; + target_position 22 -1 0; + local_position 0 0 1.5; + }; + }; + + rod "radius_r" + { + initial_position 11 -15 17; + move + { + target_rod "expansion_r"; + target_position 0.2 -0.5 -2.5; + }; + rotate + { + target_rod "radius_guide_r"; + target_position 0 1 1.5; + local_position 22 0 0; + }; object "br50-rod-radius.object"; }; - rod - { - pivot_rod "previous"; - position 22 -0.5 0; - limit ROTATE; - object "br50-rod-combination.object"; - }; - rod + + rod "union_r" { - pivot_rod "piston"; - position 0 -1.5 -2; - limit ROTATE; - connect "previous" 5.5 0 0 -11.5; + move + { + target_rod "piston_r"; + target_position 0 -1.5 -2; + }; + rotate + { + target_rod "combination_r"; + target_position 0 0 -11.5; + local_position 5.5 0 0; + }; object "br50-rod-union.object"; }; - rod - { - pivot_axle 2; - position 3.5 10.5 0; - limit FIXED; - object "br50-rod-coupling.object"; - mirror_object true; - }; - rod - { - pivot_body; - position 27 14 8; - limit SLIDE_X; - tag "piston"; - object "br50-rod-piston.object"; - mirror_object true; - }; - rod - { - pivot_axle 2; - position 3.5 13 0; - limit ROTATE; - connect "previous" 37.5 0 0 0; - object "br50-rod-main.object"; - mirror_object true; - }; - rod - { - pivot_axle 2; - position 0 15 1.5; - limit ROTATE; - object "br50-rod-eccentric.object"; - mirror_object true; - }; - rod - { - pivot_body; - position 10.5 14.5 16.5; - limit ROTATE; - connect "previous" 0 -4.5 21 0; - tag "link"; - object "br50-rod-link.object"; - mirror_object true; + rod "combination_r" + { + initial_position 38 -15.5 17; + move + { + target_rod "radius_r"; + target_position 22 -0.5 0; + }; + rotate + { + target_rod "union_r"; + target_position 5.5 0 0; + local_position 0 0 -11.5; + }; + object "br50-rod-combination.object"; }; - rod + + rod "valve_r" { - pivot_body; - position 32 16 15.5; - limit SLIDE_X; + initial_position 33 -16 15.5; + move + { + target_rod "combination_r"; + target_position 0 -0.5 -1.5; + }; + slide + { + target_position 32 -16 15.5; + axis 1 0 0; + }; object "br50-rod-valve.object"; - mirror_object true; - }; - rod - { - pivot_rod "link"; - position 0.2 0.5 -2.5; - limit ROTATE; - connect "previous" 22 -1.5 0 0; - object "br50-rod-radius.object"; - mirror_object true; }; - rod - { - pivot_rod "previous"; - position 22 0.5 0; - limit ROTATE; - object "br50-rod-combination.object"; - mirror_object true; - }; - rod + + mirror_rods { - pivot_rod "piston"; - position 0 1.5 -2; - limit ROTATE; - connect "previous" 5.5 0 0 -11.5; - object "br50-rod-union.object"; - mirror_object true; + phase_offset -90; }; }; diff --git a/source/libr2c2/vehicle.cpp b/source/libr2c2/vehicle.cpp index 0316887..2ad7166 100644 --- a/source/libr2c2/vehicle.cpp +++ b/source/libr2c2/vehicle.cpp @@ -267,57 +267,69 @@ void Vehicle::turn_axles(float d) void Vehicle::update_rods() { - for(vector::iterator i=rods.begin(); i!=rods.end(); ++i) - { - if(i->type->pivot==VehicleType::Rod::BODY) - i->position = i->type->pivot_point; - else if(i->type->pivot==VehicleType::Rod::AXLE) - { - const Axle &axle = get_fixed_axle(i->type->pivot_index); - const Vector &pp = i->type->pivot_point; - 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); - const Vector &pos = prod.position; - const Vector &off = i->type->pivot_point; - Transform trans = Transform::rotation(prod.angle, Vector(0, 1, 0)); - i->position = pos+trans.transform(off); - } + if(rods.empty()) + return; - if(i->type->connect_index>=0) + for(unsigned n=0; n<10; ++n) + { + float max_d = 0; + for(vector::iterator i=rods.begin(); i!=rods.end(); ++i) { - Rod &crod = rods[i->type->connect_index]; - if(i->type->limit==VehicleType::Rod::ROTATE && crod.type->limit==VehicleType::Rod::SLIDE_X) + const vector &constraints = i->type->constraints; + for(vector::const_iterator j=constraints.begin(); j!=constraints.end(); ++j) { - 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) - { - 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 = (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; + float d = resolve_rod_constraint(*i, *j); + max_d = max(d, max_d); } } + + if(max_d<0.0001) + break; } } +float Vehicle::resolve_rod_constraint(Rod &rod, const VehicleType::RodConstraint &cns) +{ + Vector target; + if(cns.target==VehicleType::RodConstraint::BODY) + target = cns.target_position; + else if(cns.target==VehicleType::RodConstraint::BOGIE) + ; // TODO currently rods must lie in the xz plane of the body + else if(cns.target==VehicleType::RodConstraint::AXLE) + { + const Axle &axle = get_axle(cns.target_index); + target = Vector(axle.type->position, 0, axle.type->wheel_dia/2); + target += Transform::rotation(axle.angle, Vector(0, 1, 0)).transform(cns.target_position); + } + else if(cns.target==VehicleType::RodConstraint::ROD) + { + const Rod &trod = get_rod(cns.target_index); + target = trod.position; + target += Transform::rotation(trod.angle, Vector(0, -1, 0)).transform(cns.target_position); + } + + Vector old_position = rod.position; + if(cns.type==VehicleType::RodConstraint::MOVE) + rod.position = target-Transform::rotation(rod.angle, Vector(0, -1, 0)).transform(cns.local_position); + else if(cns.type==VehicleType::RodConstraint::SLIDE) + { + Vector d = rod.position-target; + rod.position = target+cns.axis*dot(d, cns.axis); + rod.angle = Angle::zero(); + } + else if(cns.type==VehicleType::RodConstraint::ROTATE) + { + Angle old_angle = rod.angle; + Vector d = target-rod.position; + rod.angle = Geometry::atan2(d.z, d.x); + if(cns.local_position.x || cns.local_position.z) + rod.angle -= Geometry::atan2(cns.local_position.z, cns.local_position.x); + return abs(rod.angle-old_angle).radians()*cns.local_position.norm(); + } + + return distance(old_position, rod.position); +} + unsigned Vehicle::get_n_link_slots() const { return 2; @@ -354,7 +366,8 @@ Vehicle::Bogie::Bogie(const VehicleType::Bogie &t): Vehicle::Rod::Rod(const VehicleType::Rod &t): - type(&t) + type(&t), + position(t.initial_position) { } } // namespace R2C2 diff --git a/source/libr2c2/vehicle.h b/source/libr2c2/vehicle.h index b1e9163..f796591 100644 --- a/source/libr2c2/vehicle.h +++ b/source/libr2c2/vehicle.h @@ -110,6 +110,7 @@ private: void check_sensor(const TrackOffsetIter &, unsigned &, bool); void turn_axles(float); void update_rods(); + float resolve_rod_constraint(Rod &, const VehicleType::RodConstraint &); public: virtual unsigned get_n_link_slots() const; diff --git a/source/libr2c2/vehicletype.cpp b/source/libr2c2/vehicletype.cpp index 9d929b3..786d18a 100644 --- a/source/libr2c2/vehicletype.cpp +++ b/source/libr2c2/vehicletype.cpp @@ -95,11 +95,6 @@ VehicleType::Bogie::Bogie(): VehicleType::Rod::Rod(): - pivot(BODY), - pivot_index(0), - pivot_index2(0), - limit(ROTATE), - connect_index(-1), mirror_object(false) { } @@ -113,6 +108,7 @@ VehicleType::Loader::Loader(VehicleType &vt): add("height", &Loader::height); add("length", &Loader::length); add("locomotive", &VehicleType::locomotive); + add("mirror_rods", &Loader::mirror_rods); add("object", &VehicleType::object); add("rod", &Loader::rod); add("rotate_object", &VehicleType::rotate_object); @@ -140,6 +136,10 @@ void VehicleType::Loader::finish() obj.fixed_axles.push_back(&obj.axles[i]); } + for(TagMap::const_iterator i=rod_tags.begin(); i!=rod_tags.end(); ++i) + if(i->second>=0x10000) + throw runtime_error(format("unresolved reference to %s\n", i->first)); + obj.shape = new Geometry::TransformedShape( Geometry::Box(obj.length, obj.width, obj.height), Transform::translation(Vector(0, 0, obj.height/2))); @@ -175,15 +175,51 @@ void VehicleType::Loader::length(float l) obj.length = l/1000; } -void VehicleType::Loader::rod() +void VehicleType::Loader::mirror_rods() +{ + MirrorParametersLoader params; + load_sub_with(params); + + Transform axle_trans = Transform::rotation(params.phase_offset, Vector(0, 1, 0)); + + unsigned index_offset = obj.rods.size(); + for(unsigned i=0; i::iterator j=mr.constraints.begin(); j!=mr.constraints.end(); ++j) + { + j->target_position.y = -j->target_position.y; + j->local_position.y = -j->local_position.y; + j->axis.y = -j->axis.y; + if(j->target==RodConstraint::ROD) + j->target_index += index_offset; + else if(j->target==RodConstraint::AXLE) + j->target_position = axle_trans.transform(j->target_position); + } + + obj.rods.push_back(mr); + } +} + +void VehicleType::Loader::rod(const string &t) { Rod rd; Rod::Loader ldr(rd, rod_tags); load_sub_with(ldr); + + unsigned n = obj.rods.size(); + if(rod_tags.count(t)) + { + unsigned p = rod_tags[t]; + for(vector::iterator i=obj.rods.begin(); i!=obj.rods.end(); ++i) + for(vector::iterator j=i->constraints.begin(); j!=i->constraints.end(); ++j) + if(j->target_index==p) + j->target_index = n; + } + rod_tags[t] = n; obj.rods.push_back(rd); - if(!ldr.get_tag().empty()) - rod_tags[ldr.get_tag()] = obj.rods.size()-1; - rod_tags["previous"] = obj.rods.size()-1; } void VehicleType::Loader::width(float w) @@ -240,75 +276,96 @@ void VehicleType::Bogie::Loader::position(float p) } -VehicleType::Rod::Loader::Loader(Rod &r, const map &t): - DataFile::ObjectLoader(r), +VehicleType::RodConstraint::RodConstraint(): + type(MOVE), + target(BODY), + target_index(0) +{ } + + +VehicleType::RodConstraint::Loader::Loader(RodConstraint &c, TagMap &t): + DataFile::ObjectLoader(c), tags(t) { - add("connect", &Loader::connect); - add("limit", &Rod::limit); - add("mirror_object", &Rod::mirror_object); - add("object", &Rod::object); - add("pivot_body", &Loader::pivot_body); - add("pivot_axle", &Loader::pivot_axle); - add("pivot_axle", &Loader::pivot_bogie_axle); - add("pivot_rod", &Loader::pivot_rod); - add("position", &Loader::position); - add("tag", &Loader::set_tag); + add("axis", &Loader::axis); + add("local_position", &Loader::local_position); + add("target_axle", &Loader::target_axle); + add("target_position", &Loader::target_position); + add("target_rod", &Loader::target_rod); } -void VehicleType::Rod::Loader::connect(const string &t, float px, float pz, float ox, float oz) +void VehicleType::RodConstraint::Loader::axis(float x, float y, float z) { - obj.connect_index = get_item(tags, t); - obj.connect_point = Vector(px/1000, 0, pz/1000); - obj.connect_offset = Vector(ox/1000, 0, oz/1000); + obj.axis = Vector(x, y, z); + obj.axis.normalize(); } -void VehicleType::Rod::Loader::pivot_body() +void VehicleType::RodConstraint::Loader::local_position(float x, float y, float z) { - obj.pivot = BODY; + obj.local_position = Vector(x/1000, y/1000, z/1000); } -void VehicleType::Rod::Loader::pivot_axle(unsigned i) +void VehicleType::RodConstraint::Loader::target_axle(unsigned i) { - obj.pivot = AXLE; - obj.pivot_index = i; + obj.target = AXLE; + obj.target_index = i; + // TODO check range } -void VehicleType::Rod::Loader::pivot_bogie_axle(unsigned i, unsigned j) +void VehicleType::RodConstraint::Loader::target_position(float x, float y, float z) { - obj.pivot = BOGIE_AXLE; - obj.pivot_index = i; - obj.pivot_index2 = j; + obj.target_position = Vector(x/1000, y/1000, z/1000); } -void VehicleType::Rod::Loader::pivot_rod(const string &t) +void VehicleType::RodConstraint::Loader::target_rod(const string &n) { - obj.pivot_index = get_item(tags, t); - obj.pivot = ROD; + obj.target = ROD; + TagMap::iterator i = tags.find(n); + if(i!=tags.end()) + obj.target_index = i->second; + else + { + obj.target_index = 0x10000+tags.size(); + tags[n] = obj.target_index; + } } -void VehicleType::Rod::Loader::position(float x, float y, float z) + +VehicleType::Rod::Loader::Loader(Rod &r, TagMap &t): + DataFile::ObjectLoader(r), + tags(t) { - obj.pivot_point = Vector(x/1000, y/1000, z/1000); + add("initial_position", &Loader::initial_position); + add("mirror_object", &Rod::mirror_object); + add("move", &Loader::constraint); + add("object", &Rod::object); + add("rotate", &Loader::constraint); + add("slide", &Loader::constraint); +} + +template +void VehicleType::Rod::Loader::constraint() +{ + RodConstraint cns; + cns.type = t; + load_sub(cns, tags); + obj.constraints.push_back(cns); } -void VehicleType::Rod::Loader::set_tag(const string &t) +void VehicleType::Rod::Loader::initial_position(float x, float y, float z) { - tag = t; + obj.initial_position = Vector(x/1000, y/1000, z/1000); } -void operator>>(const LexicalConverter &c, VehicleType::Rod::Limit &l) +VehicleType::MirrorParametersLoader::MirrorParametersLoader() { - const string &s = c.get(); - if(s=="FIXED") - l = VehicleType::Rod::FIXED; - else if(s=="ROTATE") - l = VehicleType::Rod::ROTATE; - else if(s=="SLIDE_X") - l = VehicleType::Rod::SLIDE_X; - else - throw lexical_error(format("conversion of '%s' to Rod::Limit", s)); + add("phase_offset", &MirrorParametersLoader::phase_offs); +} + +void VehicleType::MirrorParametersLoader::phase_offs(float o) +{ + phase_offset = Angle::from_degrees(o); } } // namespace R2C2 diff --git a/source/libr2c2/vehicletype.h b/source/libr2c2/vehicletype.h index 31b6992..1305a19 100644 --- a/source/libr2c2/vehicletype.h +++ b/source/libr2c2/vehicletype.h @@ -9,11 +9,14 @@ namespace R2C2 { class VehicleType: public ObjectType { +private: + typedef std::map TagMap; + public: class Loader: public Msp::DataFile::DerivedObjectLoader { private: - std::map rod_tags; + TagMap rod_tags; public: Loader(VehicleType &); @@ -24,7 +27,8 @@ public: void function(unsigned, const std::string &); void height(float); void length(float); - void rod(); + void mirror_rods(); + void rod(const std::string &); void width(float); }; @@ -76,57 +80,81 @@ public: Bogie(); }; - struct Rod + struct RodConstraint { - enum Anchor + enum Type + { + MOVE, + SLIDE, + ROTATE + }; + + enum Target { BODY, + BOGIE, AXLE, - BOGIE_AXLE, ROD }; - enum Limit + class Loader: public Msp::DataFile::ObjectLoader { - FIXED, - ROTATE, - SLIDE_X + private: + TagMap &tags; + + public: + Loader(RodConstraint &, TagMap &); + private: + void axis(float, float, float); + void local_position(float, float, float); + void target_axle(unsigned); + void target_position(float, float, float); + void target_rod(const std::string &); }; + Type type; + Target target; + unsigned target_index; + Vector target_position; + Vector local_position; + Vector axis; + + RodConstraint(); + }; + + struct Rod + { class Loader: public Msp::DataFile::ObjectLoader { private: - const std::map &tags; - std::string tag; + TagMap &tags; public: - Loader(Rod &, const std::map &); - const std::string &get_tag() const { return tag; } + Loader(Rod &, TagMap &); private: - void connect(const std::string &, float, float, float, float); - void limit(Limit); - void pivot_body(); - void pivot_axle(unsigned); - void pivot_bogie_axle(unsigned, unsigned); - void pivot_rod(const std::string &); - void position(float, float, float); - void set_tag(const std::string &); + template + void constraint(); + void initial_position(float, float, float); }; - Anchor pivot; - unsigned pivot_index; - unsigned pivot_index2; - Vector pivot_point; - Limit limit; - int connect_index; - Vector connect_point; - Vector connect_offset; + Vector initial_position; + std::vector constraints; std::string object; bool mirror_object; Rod(); }; + class MirrorParametersLoader: public Msp::DataFile::Loader + { + public: + Angle phase_offset; + + MirrorParametersLoader(); + private: + void phase_offs(float); + }; + typedef std::vector AxleArray; typedef std::vector BogieArray; typedef std::vector RodArray;