+
+ 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();