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
use na::{
self, DVectorSliceMut, Isometry3, Matrix3, RealField, Translation3, UnitQuaternion, Vector3,
VectorSlice3, U3,
};
use crate::joint::Joint;
use crate::math::{JacobianSliceMut, Velocity};
use crate::solver::IntegrationParameters;
use crate::utils::GeneralizedCross;
#[derive(Copy, Clone, Debug)]
pub struct BallJoint<N: RealField> {
rot: UnitQuaternion<N>,
jacobian_v: Matrix3<N>,
jacobian_dot_v: Matrix3<N>,
}
impl<N: RealField> BallJoint<N> {
pub fn new(axisangle: Vector3<N>) -> Self {
BallJoint {
rot: UnitQuaternion::new(axisangle),
jacobian_v: na::zero(),
jacobian_dot_v: na::zero(),
}
}
}
impl<N: RealField> Joint<N> for BallJoint<N> {
#[inline]
fn ndofs(&self) -> usize {
3
}
fn body_to_parent(&self, parent_shift: &Vector3<N>, body_shift: &Vector3<N>) -> Isometry3<N> {
let trans = Translation3::from(parent_shift - self.rot * body_shift);
Isometry3::from_parts(trans, self.rot)
}
fn update_jacobians(&mut self, body_shift: &Vector3<N>, vels: &[N]) {
let shift = self.rot * -body_shift;
let angvel = VectorSlice3::from_slice(vels);
self.jacobian_v = shift.gcross_matrix_tr();
self.jacobian_dot_v = angvel.cross(&shift).gcross_matrix_tr();
}
fn jacobian(&self, transform: &Isometry3<N>, out: &mut JacobianSliceMut<N>) {
let rotmat = transform.rotation.to_rotation_matrix();
out.fixed_rows_mut::<U3>(0)
.copy_from(&(rotmat * self.jacobian_v));
out.fixed_rows_mut::<U3>(3).copy_from(rotmat.matrix());
}
fn jacobian_dot(&self, transform: &Isometry3<N>, out: &mut JacobianSliceMut<N>) {
let rotmat = transform.rotation.to_rotation_matrix();
out.fixed_rows_mut::<U3>(0)
.copy_from(&(rotmat * self.jacobian_dot_v));
}
fn jacobian_dot_veldiff_mul_coordinates(
&self,
transform: &Isometry3<N>,
acc: &[N],
out: &mut JacobianSliceMut<N>,
) {
let angvel = Vector3::from_row_slice(&acc[..3]);
let rotmat = transform.rotation.to_rotation_matrix();
let res = rotmat * angvel.gcross_matrix() * self.jacobian_v;
out.fixed_rows_mut::<U3>(0).copy_from(&res);
}
fn jacobian_mul_coordinates(&self, acc: &[N]) -> Velocity<N> {
let angvel = Vector3::from_row_slice(&acc[..3]);
let linvel = self.jacobian_v * angvel;
Velocity::new(linvel, angvel)
}
fn jacobian_dot_mul_coordinates(&self, acc: &[N]) -> Velocity<N> {
let angvel = Vector3::from_row_slice(&acc[..3]);
let linvel = self.jacobian_dot_v * angvel;
Velocity::new(linvel, na::zero())
}
fn default_damping(&self, out: &mut DVectorSliceMut<N>) {
out.fill(na::convert(0.1f64))
}
fn integrate(&mut self, parameters: &IntegrationParameters<N>, vels: &[N]) {
let angvel = Vector3::from_row_slice(&vels[..3]);
let disp = UnitQuaternion::new_eps(angvel * parameters.dt(), N::zero());
self.rot = disp * self.rot;
}
fn apply_displacement(&mut self, disp: &[N]) {
let angle = Vector3::from_row_slice(&disp[..3]);
let disp = UnitQuaternion::new(angle);
self.rot = disp * self.rot;
}
#[inline]
fn clone(&self) -> Box<dyn Joint<N>> {
Box::new(*self)
}
}