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
|
use crate::dynamics::SpringModel;
use crate::math::{Point, Real, Rotation, UnitVector, Vector};
#[derive(Copy, Clone, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// A joint that removes all relative linear motion between a pair of points on two bodies.
pub struct BallJoint {
/// Where the ball joint is attached on the first body, expressed in the first body local frame.
pub local_anchor1: Point<Real>,
/// Where the ball joint is attached on the second body, expressed in the second body local frame.
pub local_anchor2: Point<Real>,
/// The impulse applied by this joint on the first body.
///
/// The impulse applied to the second body is given by `-impulse`.
pub impulse: Vector<Real>,
/// The target relative angular velocity the motor will attempt to reach.
#[cfg(feature = "dim2")]
pub motor_target_vel: Real,
/// The target relative angular velocity the motor will attempt to reach.
#[cfg(feature = "dim3")]
pub motor_target_vel: Vector<Real>,
/// The target angular position of this joint, expressed as an axis-angle.
pub motor_target_pos: Rotation<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.
#[cfg(feature = "dim2")]
pub motor_impulse: Real,
/// The angular impulse applied by the motor.
#[cfg(feature = "dim3")]
pub motor_impulse: Vector<Real>,
/// The spring-like model used by the motor to reach the target velocity and .
pub motor_model: SpringModel,
/// Are the limits enabled for this joint?
pub limits_enabled: bool,
/// The axis of the limit cone for this joint, if the local-space of the first body.
pub limits_local_axis1: UnitVector<Real>,
/// The axis of the limit cone for this joint, if the local-space of the first body.
pub limits_local_axis2: UnitVector<Real>,
/// The maximum angle allowed between the two limit axes in world-space.
pub limits_angle: Real,
/// The impulse applied to enforce joint limits.
pub limits_impulse: Real,
}
impl BallJoint {
/// Creates a new Ball joint from two anchors given on the local spaces of the respective bodies.
pub fn new(local_anchor1: Point<Real>, local_anchor2: Point<Real>) -> Self {
Self::with_impulse(local_anchor1, local_anchor2, Vector::zeros())
}
pub(crate) fn with_impulse(
local_anchor1: Point<Real>,
local_anchor2: Point<Real>,
impulse: Vector<Real>,
) -> Self {
Self {
local_anchor1,
local_anchor2,
impulse,
motor_target_vel: na::zero(),
motor_target_pos: Rotation::identity(),
motor_stiffness: 0.0,
motor_damping: 0.0,
motor_impulse: na::zero(),
motor_max_impulse: Real::MAX,
motor_model: SpringModel::default(),
limits_enabled: false,
limits_local_axis1: Vector::x_axis(),
limits_local_axis2: Vector::x_axis(),
limits_angle: Real::MAX,
limits_impulse: 0.0,
}
}
/// Can a SIMD constraint be used for resolving this joint?
pub fn supports_simd_constraints(&self) -> bool {
// SIMD ball 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 and velocity correction factor this motor.
#[cfg(feature = "dim2")]
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 velocity and velocity correction factor this motor.
#[cfg(feature = "dim3")]
pub fn configure_motor_velocity(&mut self, target_vel: Vector<Real>, factor: Real) {
self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
}
/// Sets the target orientation this motor needs to reach.
pub fn configure_motor_position(
&mut self,
target_pos: Rotation<Real>,
stiffness: Real,
damping: Real,
) {
self.configure_motor(target_pos, na::zero(), stiffness, damping)
}
/// Sets the target orientation this motor needs to reach.
#[cfg(feature = "dim2")]
pub fn configure_motor(
&mut self,
target_pos: Rotation<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;
}
/// Configure both the target orientation and target velocity of the motor.
#[cfg(feature = "dim3")]
pub fn configure_motor(
&mut self,
target_pos: Rotation<Real>,
target_vel: Vector<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;
}
}
|
