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
|
use crate::dynamics::{BallJoint, FixedJoint, PrismaticJoint, RevoluteJoint};
use crate::math::{Isometry, Real, SpacialVector, SPATIAL_DIM};
use crate::na::{Rotation3, UnitQuaternion};
#[derive(Copy, Clone, Debug)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// A joint that prevents all relative movement between two bodies.
///
/// Given two frames of references, this joint aims to ensure these frame always coincide in world-space.
pub struct GenericJoint {
/// The frame of reference for the first body affected by this joint, expressed in the local frame
/// of the first body.
pub local_anchor1: Isometry<Real>,
/// The frame of reference for the second body affected by this joint, expressed in the local frame
/// of the first body.
pub local_anchor2: Isometry<Real>,
/// The impulse applied to the first body affected by this joint.
///
/// The impulse applied to the second body affected by this joint is given by `-impulse`.
/// This combines both linear and angular impulses:
/// - In 2D, `impulse.xy()` gives the linear impulse, and `impulse.z` the angular impulse.
/// - In 3D, `impulse.xyz()` gives the linear impulse, and `(impulse[3], impulse[4], impulse[5])` the angular impulse.
pub impulse: SpacialVector<Real>,
pub min_position: SpacialVector<Real>,
pub max_position: SpacialVector<Real>,
pub min_velocity: SpacialVector<Real>,
pub max_velocity: SpacialVector<Real>,
/// The minimum negative impulse the joint can apply on each DoF. Must be <= 0.0
pub min_impulse: SpacialVector<Real>,
/// The maximum positive impulse the joint can apply on each DoF. Must be >= 0.0
pub max_impulse: SpacialVector<Real>,
/// The minimum negative position impulse the joint can apply on each DoF. Must be <= 0.0
pub min_pos_impulse: SpacialVector<Real>,
/// The maximum positive position impulse the joint can apply on each DoF. Must be >= 0.0
pub max_pos_impulse: SpacialVector<Real>,
}
impl GenericJoint {
/// Creates a new fixed joint from the frames of reference of both bodies.
pub fn new(local_anchor1: Isometry<Real>, local_anchor2: Isometry<Real>) -> Self {
Self {
local_anchor1,
local_anchor2,
impulse: SpacialVector::zeros(),
min_position: SpacialVector::zeros(),
max_position: SpacialVector::zeros(),
min_velocity: SpacialVector::zeros(),
max_velocity: SpacialVector::zeros(),
min_impulse: SpacialVector::repeat(-Real::MAX),
max_impulse: SpacialVector::repeat(Real::MAX),
min_pos_impulse: SpacialVector::repeat(-Real::MAX),
max_pos_impulse: SpacialVector::repeat(Real::MAX),
}
}
pub fn free_dof(&mut self, dof: u8) {
self.min_position[dof as usize] = -Real::MAX;
self.max_position[dof as usize] = Real::MAX;
self.min_velocity[dof as usize] = -Real::MAX;
self.max_velocity[dof as usize] = Real::MAX;
self.min_impulse[dof as usize] = 0.0;
self.max_impulse[dof as usize] = 0.0;
self.min_pos_impulse[dof as usize] = 0.0;
self.max_pos_impulse[dof as usize] = 0.0;
}
pub fn set_dof_limits(&mut self, dof: u8, min: Real, max: Real) {
self.min_position[dof as usize] = min;
self.max_position[dof as usize] = max;
}
}
impl From<RevoluteJoint> for GenericJoint {
fn from(joint: RevoluteJoint) -> Self {
let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]];
let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]];
let quat1 = UnitQuaternion::from_basis_unchecked(&basis1);
let quat2 = UnitQuaternion::from_basis_unchecked(&basis2);
let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1);
let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2);
let mut result = Self::new(local_anchor1, local_anchor2);
result.free_dof(3);
result
}
}
impl From<BallJoint> for GenericJoint {
fn from(joint: BallJoint) -> Self {
let local_anchor1 = Isometry::new(joint.local_anchor1.coords, na::zero());
let local_anchor2 = Isometry::new(joint.local_anchor2.coords, na::zero());
let mut result = Self::new(local_anchor1, local_anchor2);
result.free_dof(3);
result.free_dof(4);
result.free_dof(5);
result
}
}
impl From<PrismaticJoint> for GenericJoint {
fn from(joint: PrismaticJoint) -> Self {
let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]];
let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]];
let quat1 = UnitQuaternion::from_basis_unchecked(&basis1);
let quat2 = UnitQuaternion::from_basis_unchecked(&basis2);
let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1);
let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2);
let mut result = Self::new(local_anchor1, local_anchor2);
result.free_dof(0);
result.set_dof_limits(0, joint.limits[0], joint.limits[1]);
result
}
}
impl From<FixedJoint> for GenericJoint {
fn from(joint: FixedJoint) -> Self {
Self::new(joint.local_anchor1, joint.local_anchor2)
}
}
|
