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;
};
};
void Vehicle::update_rods()
{
- for(vector<Rod>::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<Rod>::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<VehicleType::RodConstraint> &constraints = i->type->constraints;
+ for(vector<VehicleType::RodConstraint>::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<float>(d.z, d.x);
+ if(cns.local_position.x || cns.local_position.z)
+ rod.angle -= Geometry::atan2<float>(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;
Vehicle::Rod::Rod(const VehicleType::Rod &t):
- type(&t)
+ type(&t),
+ position(t.initial_position)
{ }
} // namespace R2C2
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;
VehicleType::Rod::Rod():
- pivot(BODY),
- pivot_index(0),
- pivot_index2(0),
- limit(ROTATE),
- connect_index(-1),
mirror_object(false)
{ }
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);
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<float, 3>(
Geometry::Box<float>(obj.length, obj.width, obj.height),
Transform::translation(Vector(0, 0, obj.height/2)));
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<index_offset; ++i)
+ {
+ Rod mr = obj.rods[i];
+ mr.initial_position.y = -mr.initial_position.y;
+ mr.mirror_object = !mr.mirror_object;
+ for(vector<RodConstraint>::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<Rod>::iterator i=obj.rods.begin(); i!=obj.rods.end(); ++i)
+ for(vector<RodConstraint>::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)
}
-VehicleType::Rod::Loader::Loader(Rod &r, const map<string, unsigned> &t):
- DataFile::ObjectLoader<Rod>(r),
+VehicleType::RodConstraint::RodConstraint():
+ type(MOVE),
+ target(BODY),
+ target_index(0)
+{ }
+
+
+VehicleType::RodConstraint::Loader::Loader(RodConstraint &c, TagMap &t):
+ DataFile::ObjectLoader<RodConstraint>(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<Rod>(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<RodConstraint::MOVE>);
+ add("object", &Rod::object);
+ add("rotate", &Loader::constraint<RodConstraint::ROTATE>);
+ add("slide", &Loader::constraint<RodConstraint::SLIDE>);
+}
+
+template<VehicleType::RodConstraint::Type t>
+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
class VehicleType: public ObjectType
{
+private:
+ typedef std::map<std::string, unsigned> TagMap;
+
public:
class Loader: public Msp::DataFile::DerivedObjectLoader<VehicleType, ObjectType::Loader>
{
private:
- std::map<std::string, unsigned> rod_tags;
+ TagMap rod_tags;
public:
Loader(VehicleType &);
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);
};
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<RodConstraint>
{
- 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<Rod>
{
private:
- const std::map<std::string, unsigned> &tags;
- std::string tag;
+ TagMap &tags;
public:
- Loader(Rod &, const std::map<std::string, unsigned> &);
- 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<RodConstraint::Type>
+ 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<RodConstraint> 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<Axle> AxleArray;
typedef std::vector<Bogie> BogieArray;
typedef std::vector<Rod> RodArray;
projects / r2c2.git / commitdiff