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
use na::{DVectorSliceMut, RealField};
use crate::joint::Joint;
use crate::math::{Isometry, JacobianSliceMut, Translation, Vector, Velocity};
use crate::solver::IntegrationParameters;
#[derive(Copy, Clone, Debug)]
pub struct FixedJoint<N: RealField> {
body_to_parent: Isometry<N>,
}
impl<N: RealField> FixedJoint<N> {
pub fn new(pos_wrt_body: Isometry<N>) -> Self {
FixedJoint {
body_to_parent: pos_wrt_body.inverse(),
}
}
}
impl<N: RealField> Joint<N> for FixedJoint<N> {
#[inline]
fn clone(&self) -> Box<Joint<N>> {
Box::new(*self)
}
fn ndofs(&self) -> usize {
0
}
fn body_to_parent(&self, parent_shift: &Vector<N>, body_shift: &Vector<N>) -> Isometry<N> {
let parent_trans = Translation::from(*parent_shift);
let body_trans = Translation::from(*body_shift);
parent_trans * self.body_to_parent * body_trans
}
fn update_jacobians(&mut self, _: &Vector<N>, _: &[N]) {}
fn jacobian(&self, _: &Isometry<N>, _: &mut JacobianSliceMut<N>) {}
fn jacobian_dot(&self, _: &Isometry<N>, _: &mut JacobianSliceMut<N>) {}
fn jacobian_dot_veldiff_mul_coordinates(
&self,
_: &Isometry<N>,
_: &[N],
_: &mut JacobianSliceMut<N>,
) {
}
fn integrate(&mut self, _: &IntegrationParameters<N>, _: &[N]) {}
fn apply_displacement(&mut self, _: &[N]) {}
fn jacobian_mul_coordinates(&self, _: &[N]) -> Velocity<N> {
Velocity::zero()
}
fn jacobian_dot_mul_coordinates(&self, _: &[N]) -> Velocity<N> {
Velocity::zero()
}
fn default_damping(&self, _: &mut DVectorSliceMut<N>) {}
}