1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
|
use crate::dynamics::SpringModel;
use crate::math::{Isometry, Point, Real, Vector};
use crate::utils::WBasis;
use na::{RealField, Unit, Vector5};
#[derive(Copy, Clone, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// A joint that removes all relative motion between two bodies, except for the rotations along one axis.
pub struct RevoluteJoint {
/// Where the revolute joint is attached on the first body, expressed in the local space of the first attached body.
pub local_anchor1: Point<Real>,
/// Where the revolute joint is attached on the second body, expressed in the local space of the second attached body.
pub local_anchor2: Point<Real>,
/// The rotation axis of this revolute joint expressed in the local space of the first attached body.
pub local_axis1: Unit<Vector<Real>>,
/// The rotation axis of this revolute joint expressed in the local space of the second attached body.
pub local_axis2: Unit<Vector<Real>>,
/// The basis orthonormal to `local_axis1`, expressed in the local space of the first attached body.
pub basis1: [Vector<Real>; 2],
/// The basis orthonormal to `local_axis2`, expressed in the local space of the second attached body.
pub basis2: [Vector<Real>; 2],
/// The impulse applied by this joint on the first body.
///
/// The impulse applied to the second body is given by `-impulse`.
pub impulse: Vector5<Real>,
/// Whether or not this joint should enforce translational limits along its axis.
pub limits_enabled: bool,
/// The min an max relative position of the attached bodies along this joint's axis.
pub limits: [Real; 2],
/// The impulse applied by this joint on the first body to enforce the position limit along this joint's axis.
///
/// The impulse applied to the second body is given by `-impulse`.
pub limits_impulse: Real,
/// The target relative angular velocity the motor will attempt to reach.
pub motor_target_vel: Real,
/// The target relative angle along the joint axis the motor will attempt to reach.
pub motor_target_pos: Real,
/// The motor's stiffness.
/// See the documentation of `SpringModel` for more information on this parameter.
pub motor_stiffness: Real,
/// The motor's damping.
/// See the documentation of `SpringModel` for more information on this parameter.
pub motor_damping: Real,
/// The maximal impulse the motor is able to deliver.
pub motor_max_impulse: Real,
/// The angular impulse applied by the motor.
pub motor_impulse: Real,
/// The spring-like model used by the motor to reach the target velocity and .
pub motor_model: SpringModel,
// Used to handle cases where the position target ends up being more than pi radians away.
pub(crate) motor_last_angle: Real,
// The angular impulse expressed in world-space.
pub(crate) world_ang_impulse: Vector<Real>,
// The world-space orientation of the free axis of the first attached body.
pub(crate) prev_axis1: Vector<Real>,
}
impl RevoluteJoint {
/// Creates a new revolute joint with the given point of applications and axis, all expressed
/// in the local-space of the affected bodies.
pub fn new(
local_anchor1: Point<Real>,
local_axis1: Unit<Vector<Real>>,
local_anchor2: Point<Real>,
local_axis2: Unit<Vector<Real>>,
) -> Self {
Self {
local_anchor1,
local_anchor2,
local_axis1,
local_axis2,
basis1: local_axis1.orthonormal_basis(),
basis2: local_axis2.orthonormal_basis(),
impulse: na::zero(),
world_ang_impulse: na::zero(),
limits_enabled: false,
limits: [-Real::MAX, Real::MAX],
limits_impulse: 0.0,
motor_target_vel: 0.0,
motor_target_pos: 0.0,
motor_stiffness: 0.0,
motor_damping: 0.0,
motor_max_impulse: Real::MAX,
motor_impulse: 0.0,
prev_axis1: *local_axis1,
motor_model: SpringModel::default(),
motor_last_angle: 0.0,
}
}
/// Can a SIMD constraint be used for resolving this joint?
pub fn supports_simd_constraints(&self) -> bool {
// SIMD revolute constraints don't support motors and limits right now.
!self.limits_enabled
&& (self.motor_max_impulse == 0.0
|| (self.motor_stiffness == 0.0 && self.motor_damping == 0.0))
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
pub fn configure_motor_model(&mut self, model: SpringModel) {
self.motor_model = model;
}
/// Sets the target velocity this motor needs to reach.
pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) {
self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
}
/// Sets the target angle this motor needs to reach.
pub fn configure_motor_position(&mut self, target_pos: Real, stiffness: Real, damping: Real) {
self.configure_motor(target_pos, 0.0, stiffness, damping)
}
/// Configure both the target angle and target velocity of the motor.
pub fn configure_motor(
&mut self,
target_pos: Real,
target_vel: Real,
stiffness: Real,
damping: Real,
) {
self.motor_target_vel = target_vel;
self.motor_target_pos = target_pos;
self.motor_stiffness = stiffness;
self.motor_damping = damping;
}
/// Estimates the current position of the motor angle.
pub fn estimate_motor_angle(
&self,
body_pos1: &Isometry<Real>,
body_pos2: &Isometry<Real>,
) -> Real {
Self::estimate_motor_angle_from_params(
&(body_pos1 * self.local_axis1),
&(body_pos1 * self.basis1[0]),
&(body_pos2 * self.basis2[0]),
self.motor_last_angle,
)
}
/// Estimates the current position of the motor angle given the joint parameters.
pub fn estimate_motor_angle_from_params(
axis1: &Unit<Vector<Real>>,
tangent1: &Vector<Real>,
tangent2: &Vector<Real>,
motor_last_angle: Real,
) -> Real {
let last_angle_cycles = (motor_last_angle / Real::two_pi()).trunc() * Real::two_pi();
// Measure the position between 0 and 2-pi
let new_angle = if tangent1.cross(&tangent2).dot(&axis1) < 0.0 {
Real::two_pi() - tangent1.angle(&tangent2)
} else {
tangent1.angle(&tangent2)
};
// The last angle between 0 and 2-pi
let last_angle_zero_two_pi = motor_last_angle - last_angle_cycles;
// Figure out the smallest angle differance.
let mut angle_diff = new_angle - last_angle_zero_two_pi;
if angle_diff > Real::pi() {
angle_diff -= Real::two_pi()
} else if angle_diff < -Real::pi() {
angle_diff += Real::two_pi()
}
motor_last_angle + angle_diff
}
}
|
