nphysics3d/joint/
ball_joint.rs

1use na::{
2    self, DVectorSliceMut, Isometry3, Matrix3, RealField, Translation3, UnitQuaternion, Vector3,
3    VectorSlice3,
4};
5
6use crate::joint::Joint;
7use crate::math::{JacobianSliceMut, Velocity};
8use crate::solver::IntegrationParameters;
9use crate::utils::GeneralizedCross;
10
11/// A joint that allows only all rotational degrees of freedom between two multibody links.
12#[derive(Copy, Clone, Debug)]
13pub struct BallJoint<N: RealField + Copy> {
14    rot: UnitQuaternion<N>,
15
16    jacobian_v: Matrix3<N>,
17    jacobian_dot_v: Matrix3<N>,
18}
19
20impl<N: RealField + Copy> BallJoint<N> {
21    /// Create a ball joint with the an initial position given by a rotation in axis-angle form.
22    pub fn new(axisangle: Vector3<N>) -> Self {
23        BallJoint {
24            rot: UnitQuaternion::new(axisangle),
25            jacobian_v: na::zero(),
26            jacobian_dot_v: na::zero(),
27        }
28    }
29}
30
31impl<N: RealField + Copy> Joint<N> for BallJoint<N> {
32    #[inline]
33    fn ndofs(&self) -> usize {
34        3
35    }
36
37    fn body_to_parent(&self, parent_shift: &Vector3<N>, body_shift: &Vector3<N>) -> Isometry3<N> {
38        let trans = Translation3::from(parent_shift - self.rot * body_shift);
39        Isometry3::from_parts(trans, self.rot)
40    }
41
42    fn update_jacobians(&mut self, body_shift: &Vector3<N>, vels: &[N]) {
43        let shift = self.rot * -body_shift;
44        let angvel = VectorSlice3::from_slice(vels);
45
46        self.jacobian_v = shift.gcross_matrix_tr();
47        self.jacobian_dot_v = angvel.cross(&shift).gcross_matrix_tr();
48    }
49
50    fn jacobian(&self, transform: &Isometry3<N>, out: &mut JacobianSliceMut<N>) {
51        // FIXME: could we avoid the computation of rotation matrix on each `jacobian_*`  ?
52        let rotmat = transform.rotation.to_rotation_matrix();
53        out.fixed_rows_mut::<3>(0)
54            .copy_from(&(rotmat * self.jacobian_v));
55        out.fixed_rows_mut::<3>(3).copy_from(rotmat.matrix());
56    }
57
58    fn jacobian_dot(&self, transform: &Isometry3<N>, out: &mut JacobianSliceMut<N>) {
59        let rotmat = transform.rotation.to_rotation_matrix();
60        out.fixed_rows_mut::<3>(0)
61            .copy_from(&(rotmat * self.jacobian_dot_v));
62    }
63
64    fn jacobian_dot_veldiff_mul_coordinates(
65        &self,
66        transform: &Isometry3<N>,
67        acc: &[N],
68        out: &mut JacobianSliceMut<N>,
69    ) {
70        let angvel = Vector3::from_row_slice(&acc[..3]);
71        let rotmat = transform.rotation.to_rotation_matrix();
72        let res = rotmat * angvel.gcross_matrix() * self.jacobian_v;
73        out.fixed_rows_mut::<3>(0).copy_from(&res);
74    }
75
76    fn jacobian_mul_coordinates(&self, acc: &[N]) -> Velocity<N> {
77        let angvel = Vector3::from_row_slice(&acc[..3]);
78        let linvel = self.jacobian_v * angvel;
79        Velocity::new(linvel, angvel)
80    }
81
82    fn jacobian_dot_mul_coordinates(&self, acc: &[N]) -> Velocity<N> {
83        let angvel = Vector3::from_row_slice(&acc[..3]);
84        let linvel = self.jacobian_dot_v * angvel;
85        Velocity::new(linvel, na::zero())
86    }
87
88    fn default_damping(&self, out: &mut DVectorSliceMut<N>) {
89        out.fill(na::convert(0.1f64))
90    }
91
92    fn integrate(&mut self, parameters: &IntegrationParameters<N>, vels: &[N]) {
93        let angvel = Vector3::from_row_slice(&vels[..3]);
94        let disp = UnitQuaternion::new_eps(angvel * parameters.dt(), N::zero());
95        self.rot = disp * self.rot;
96    }
97
98    fn apply_displacement(&mut self, disp: &[N]) {
99        let angle = Vector3::from_row_slice(&disp[..3]);
100        let disp = UnitQuaternion::new(angle);
101        self.rot = disp * self.rot;
102    }
103
104    #[inline]
105    fn clone(&self) -> Box<dyn Joint<N>> {
106        Box::new(*self)
107    }
108}
nphysics3d/joint/ball_joint.rs

nphysics3d/joint/
ball_joint.rs
