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
|
use std::ops::{Deref, DerefMut};
use crate::dynamics::{MultibodyJoint, RigidBodyHandle};
use crate::math::{Isometry, Real, Vector};
use crate::prelude::RigidBodyVelocity;
/// One link of a multibody.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct MultibodyLink {
// FIXME: make all those private.
pub(crate) internal_id: usize,
pub(crate) assembly_id: usize,
pub(crate) parent_internal_id: usize,
pub(crate) rigid_body: RigidBodyHandle,
/*
* Change at each time step.
*/
pub joint: MultibodyJoint,
// TODO: should this be removed in favor of the rigid-body position?
pub local_to_world: Isometry<Real>,
pub local_to_parent: Isometry<Real>,
pub shift02: Vector<Real>,
pub shift23: Vector<Real>,
/// The velocity added by the joint, in world-space.
pub(crate) joint_velocity: RigidBodyVelocity,
}
impl MultibodyLink {
/// Creates a new multibody link.
pub fn new(
rigid_body: RigidBodyHandle,
internal_id: usize,
assembly_id: usize,
parent_internal_id: usize,
joint: MultibodyJoint,
local_to_world: Isometry<Real>,
local_to_parent: Isometry<Real>,
) -> Self {
let joint_velocity = RigidBodyVelocity::zero();
MultibodyLink {
internal_id,
assembly_id,
parent_internal_id,
joint,
local_to_world,
local_to_parent,
shift02: na::zero(),
shift23: na::zero(),
joint_velocity,
rigid_body,
}
}
pub fn joint(&self) -> &MultibodyJoint {
&self.joint
}
pub fn rigid_body_handle(&self) -> RigidBodyHandle {
self.rigid_body
}
/// Checks if this link is the root of the multibody.
#[inline]
pub fn is_root(&self) -> bool {
self.internal_id == 0
}
/// The handle of this multibody link.
#[inline]
pub fn link_id(&self) -> usize {
self.internal_id
}
/// The handle of the parent link.
#[inline]
pub fn parent_id(&self) -> Option<usize> {
if self.internal_id != 0 {
Some(self.parent_internal_id)
} else {
None
}
}
#[inline]
pub fn local_to_world(&self) -> &Isometry<Real> {
&self.local_to_world
}
#[inline]
pub fn local_to_parent(&self) -> &Isometry<Real> {
&self.local_to_parent
}
}
// FIXME: keep this even if we already have the Index2 traits?
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub(crate) struct MultibodyLinkVec(pub Vec<MultibodyLink>);
impl MultibodyLinkVec {
#[inline]
pub fn get_mut_with_parent(&mut self, i: usize) -> (&mut MultibodyLink, &MultibodyLink) {
let parent_id = self[i].parent_internal_id;
assert!(
parent_id != i,
"Internal error: circular rigid body dependency."
);
assert!(parent_id < self.len(), "Invalid parent index.");
unsafe {
let rb = &mut *(self.get_unchecked_mut(i) as *mut _);
let parent_rb = &*(self.get_unchecked(parent_id) as *const _);
(rb, parent_rb)
}
}
}
impl Deref for MultibodyLinkVec {
type Target = Vec<MultibodyLink>;
#[inline]
fn deref(&self) -> &Vec<MultibodyLink> {
let MultibodyLinkVec(ref me) = *self;
me
}
}
impl DerefMut for MultibodyLinkVec {
#[inline]
fn deref_mut(&mut self) -> &mut Vec<MultibodyLink> {
let MultibodyLinkVec(ref mut me) = *self;
me
}
}
|
