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

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(Copy, 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.
     */
    /// The multibody joint of this link.
    pub joint: MultibodyJoint,
    // TODO: should this be removed in favor of the rigid-body position?
    pub(crate) local_to_world: Isometry<Real>,
    pub(crate) local_to_parent: Isometry<Real>,
    pub(crate) shift02: Vector<Real>,
    pub(crate) 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,
        }
    }

    /// The multibody joint of this link.
    pub fn joint(&self) -> &MultibodyJoint {
        &self.joint
    }

    /// The handle of the rigid-body of this link.
    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
        }
    }

    /// The world-space transform of the rigid-body attached to this link.
    #[inline]
    pub fn local_to_world(&self) -> &Isometry<Real> {
        &self.local_to_world
    }

    /// The position of the rigid-body attached to this link relative to its parent.
    #[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
    }
}