rapier3d/dynamics/joint/multibody_joint/
multibody_link.rs

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

rapier3d/dynamics/joint/multibody_joint/
multibody_link.rs
