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
|
use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
use crate::dynamics::{
IntegrationParameters, RevoluteJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
use crate::math::{Isometry, Real, SIMD_WIDTH};
// TODO: this does not uses SIMD optimizations yet.
#[derive(Debug)]
pub(crate) struct WRevolutePositionConstraint {
constraints: [RevolutePositionConstraint; SIMD_WIDTH],
}
impl WRevolutePositionConstraint {
pub fn from_params(
rbs1: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&RevoluteJoint; SIMD_WIDTH],
) -> Self {
Self {
constraints: gather![|ii| RevolutePositionConstraint::from_params(
(rbs1.0[ii], rbs1.1[ii]),
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii]
)],
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
for constraint in &self.constraints {
constraint.solve(params, positions);
}
}
}
#[derive(Debug)]
pub(crate) struct WRevolutePositionGroundConstraint {
constraints: [RevolutePositionGroundConstraint; SIMD_WIDTH],
}
impl WRevolutePositionGroundConstraint {
pub fn from_params(
rbs1: [&RigidBodyPosition; SIMD_WIDTH],
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&RevoluteJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
Self {
constraints: gather![|ii| RevolutePositionGroundConstraint::from_params(
rbs1[ii],
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii],
flipped[ii]
)],
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
for constraint in &self.constraints {
constraint.solve(params, positions);
}
}
}
|
