- TrackPoint tp;
- tp.pos = Point(back.x+dx*ratio, back.y+dy*ratio, back.z+dz*ratio);
- tp.dir = atan2(dy, dx);
+ 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();
+ }