rapier2d/dynamics/joint/multibody_joint/
multibody_link.rs

1use crate::alloc_prelude::*;
2use core::ops::{Deref, DerefMut};
3
4#[cfg(doc)]
5use crate::dynamics::Multibody;
6use crate::dynamics::{MultibodyJoint, RigidBodyHandle};
7use crate::math::{Pose, Real, Vector};
8use crate::prelude::RigidBodyVelocity;
9
10/// One link of a multibody.
11#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
12#[derive(Copy, Clone, Debug)]
13pub struct MultibodyLink {
14    // FIXME: make all those private.
15    pub(crate) internal_id: usize,
16    pub(crate) assembly_id: usize,
17
18    pub(crate) parent_internal_id: usize,
19    pub(crate) rigid_body: RigidBodyHandle,
20
21    /*
22     * Change at each time step.
23     */
24    /// The multibody joint of this link.
25    pub joint: MultibodyJoint,
26    // TODO: should this be removed in favor of the rigid-body position?
27    pub(crate) local_to_world: Pose,
28    pub(crate) local_to_parent: Pose,
29    pub(crate) shift02: Vector,
30    pub(crate) shift23: Vector,
31
32    /// The velocity added by the joint, in world-space.
33    pub(crate) joint_velocity: RigidBodyVelocity<Real>,
34}
35
36impl MultibodyLink {
37    /// Creates a new multibody link.
38    pub fn new(
39        rigid_body: RigidBodyHandle,
40        internal_id: usize,
41        assembly_id: usize,
42        parent_internal_id: usize,
43        joint: MultibodyJoint,
44        local_to_world: Pose,
45        local_to_parent: Pose,
46    ) -> Self {
47        let joint_velocity = RigidBodyVelocity::zero();
48
49        MultibodyLink {
50            internal_id,
51            assembly_id,
52            parent_internal_id,
53            joint,
54            local_to_world,
55            local_to_parent,
56            shift02: Vector::ZERO,
57            shift23: Vector::ZERO,
58            joint_velocity,
59            rigid_body,
60        }
61    }
62
63    /// The multibody joint of this link.
64    pub fn joint(&self) -> &MultibodyJoint {
65        &self.joint
66    }
67
68    /// The handle of the rigid-body of this link.
69    pub fn rigid_body_handle(&self) -> RigidBodyHandle {
70        self.rigid_body
71    }
72
73    /// Checks if this link is the root of the multibody.
74    #[inline]
75    pub fn is_root(&self) -> bool {
76        self.internal_id == 0
77    }
78
79    /// The handle of this multibody link.
80    #[inline]
81    pub fn link_id(&self) -> usize {
82        self.internal_id
83    }
84
85    /// The offset of this link's degrees of freedom in the multibody's
86    /// generalized coordinate / velocity vectors.
87    ///
88    /// The link's `ndofs` generalized velocities occupy
89    /// `assembly_id..assembly_id + ndofs` in [`Multibody::generalized_velocity`]
90    /// (and the matching slice of any displacement passed to
91    /// [`Multibody::apply_displacements`]).
92    #[inline]
93    pub fn assembly_id(&self) -> usize {
94        self.assembly_id
95    }
96
97    /// The handle of the parent link.
98    #[inline]
99    pub fn parent_id(&self) -> Option<usize> {
100        if self.internal_id != 0 {
101            Some(self.parent_internal_id)
102        } else {
103            None
104        }
105    }
106
107    /// The world-space transform of the rigid-body attached to this link.
108    #[inline]
109    pub fn local_to_world(&self) -> &Pose {
110        &self.local_to_world
111    }
112
113    /// The position of the rigid-body attached to this link relative to its parent.
114    #[inline]
115    pub fn local_to_parent(&self) -> &Pose {
116        &self.local_to_parent
117    }
118}
119
120// FIXME: keep this even if we already have the Index2 traits?
121#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
122#[derive(Clone, Debug)]
123pub(crate) struct MultibodyLinkVec(pub Vec<MultibodyLink>);
124
125impl MultibodyLinkVec {
126    #[inline]
127    pub fn get_mut_with_parent(&mut self, i: usize) -> (&mut MultibodyLink, &MultibodyLink) {
128        let parent_id = self[i].parent_internal_id;
129
130        assert!(
131            parent_id != i,
132            "Internal error: circular rigid body dependency."
133        );
134        assert!(parent_id < self.len(), "Invalid parent index.");
135
136        let [rb, parent_rb] = self
137            .0
138            .get_disjoint_mut([i, parent_id])
139            .expect("indices are in bounds and distinct");
140        (rb, &*parent_rb)
141    }
142}
143
144impl Deref for MultibodyLinkVec {
145    type Target = Vec<MultibodyLink>;
146
147    #[inline]
148    fn deref(&self) -> &Vec<MultibodyLink> {
149        let MultibodyLinkVec(ref me) = *self;
150        me
151    }
152}
153
154impl DerefMut for MultibodyLinkVec {
155    #[inline]
156    fn deref_mut(&mut self) -> &mut Vec<MultibodyLink> {
157        let MultibodyLinkVec(ref mut me) = *self;
158        me
159    }
160}
rapier2d/dynamics/joint/multibody_joint/multibody_link.rs

rapier2d/dynamics/joint/multibody_joint/
multibody_link.rs
