nphysics3d/joint/
cartesian_constraint.rs

1use na::{DVector, RealField};
2use std::ops::Range;
3
4use crate::joint::JointConstraint;
5use crate::math::{AngularVector, Point, Rotation, ANGULAR_DIM};
6use crate::object::{BodyHandle, BodyPartHandle, BodySet};
7use crate::solver::helper;
8use crate::solver::{
9    GenericNonlinearConstraint, IntegrationParameters, LinearConstraints,
10    NonlinearConstraintGenerator,
11};
12
13/// A constraint that removes all relative angular motion between two body parts.
14pub struct CartesianConstraint<N: RealField + Copy, Handle: BodyHandle> {
15    b1: BodyPartHandle<Handle>,
16    b2: BodyPartHandle<Handle>,
17    anchor1: Point<N>,
18    ref_frame1: Rotation<N>,
19    anchor2: Point<N>,
20    ref_frame2: Rotation<N>,
21    ang_impulses: AngularVector<N>,
22    break_torque_squared: N,
23    broken: bool,
24    bilateral_ground_rng: Range<usize>,
25    bilateral_rng: Range<usize>,
26}
27
28impl<N: RealField + Copy, Handle: BodyHandle> CartesianConstraint<N, Handle> {
29    /// Creates a cartesian constraint between two body parts.
30    ///
31    /// This will ensure the rotational parts of the frames given identified by `ref_frame1` and
32    /// `ref_frame2` and attached to the corresponding bodies will coincide.
33    pub fn new(
34        b1: BodyPartHandle<Handle>,
35        b2: BodyPartHandle<Handle>,
36        anchor1: Point<N>,
37        ref_frame1: Rotation<N>,
38        anchor2: Point<N>,
39        ref_frame2: Rotation<N>,
40    ) -> Self {
41        CartesianConstraint {
42            b1,
43            b2,
44            anchor1,
45            ref_frame1,
46            anchor2,
47            ref_frame2,
48            break_torque_squared: N::max_value(),
49            broken: false,
50            ang_impulses: AngularVector::zeros(),
51            bilateral_ground_rng: 0..0,
52            bilateral_rng: 0..0,
53        }
54    }
55
56    /// Changes the reference frame for the first body part.
57    pub fn set_reference_frame_1(&mut self, ref_frame1: Rotation<N>) {
58        self.ref_frame1 = ref_frame1
59    }
60
61    /// Changes the reference frame for the second body part.
62    pub fn set_reference_frame_2(&mut self, frame2: Rotation<N>) {
63        self.ref_frame2 = frame2
64    }
65
66    /// Changes the attach point for the first body part.
67    pub fn set_anchor_1(&mut self, anchor1: Point<N>) {
68        self.anchor1 = anchor1
69    }
70
71    /// Changes the attach point for the second body part.
72    pub fn set_anchor_2(&mut self, anchor2: Point<N>) {
73        self.anchor2 = anchor2
74    }
75
76    /// The maximum torque this joint can absorb before breaking.
77    pub fn set_break_torque(&mut self, break_torque: N) {
78        self.break_torque_squared = break_torque * break_torque;
79    }
80}
81
82impl<N: RealField + Copy, Handle: BodyHandle> JointConstraint<N, Handle>
83    for CartesianConstraint<N, Handle>
84{
85    fn is_broken(&self) -> bool {
86        self.broken
87    }
88
89    fn num_velocity_constraints(&self) -> usize {
90        ANGULAR_DIM
91    }
92
93    fn anchors(&self) -> (BodyPartHandle<Handle>, BodyPartHandle<Handle>) {
94        (self.b1, self.b2)
95    }
96
97    fn velocity_constraints(
98        &mut self,
99        _: &IntegrationParameters<N>,
100        bodies: &dyn BodySet<N, Handle = Handle>,
101        ext_vels: &DVector<N>,
102        ground_j_id: &mut usize,
103        j_id: &mut usize,
104        jacobians: &mut [N],
105        constraints: &mut LinearConstraints<N, usize>,
106    ) {
107        let body1 = try_ret!(bodies.get(self.b1.0));
108        let body2 = try_ret!(bodies.get(self.b2.0));
109        let part1 = try_ret!(body1.part(self.b1.1));
110        let part2 = try_ret!(body2.part(self.b2.1));
111
112        let pos1 = body1.position_at_material_point(part1, &self.anchor1) * self.ref_frame1;
113        let pos2 = body2.position_at_material_point(part2, &self.anchor2) * self.ref_frame2;
114
115        let anchor1 = Point::from(pos1.translation.vector);
116        let anchor2 = Point::from(pos2.translation.vector);
117
118        let assembly_id1 = body1.companion_id();
119        let assembly_id2 = body2.companion_id();
120
121        let first_bilateral_ground = constraints.bilateral_ground.len();
122        let first_bilateral = constraints.bilateral.len();
123
124        helper::cancel_relative_angular_velocity(
125            body1,
126            part1,
127            self.b1,
128            body2,
129            part2,
130            self.b2,
131            assembly_id1,
132            assembly_id2,
133            &anchor1,
134            &anchor2,
135            ext_vels,
136            &self.ang_impulses,
137            0,
138            ground_j_id,
139            j_id,
140            jacobians,
141            constraints,
142        );
143
144        self.bilateral_ground_rng = first_bilateral_ground..constraints.bilateral_ground.len();
145        self.bilateral_rng = first_bilateral..constraints.bilateral.len();
146    }
147
148    fn cache_impulses(&mut self, constraints: &LinearConstraints<N, usize>, inv_dt: N) {
149        for c in &constraints.bilateral_ground[self.bilateral_ground_rng.clone()] {
150            self.ang_impulses[c.impulse_id] = c.impulse;
151        }
152
153        for c in &constraints.bilateral[self.bilateral_rng.clone()] {
154            self.ang_impulses[c.impulse_id] = c.impulse;
155        }
156
157        if self.ang_impulses.norm_squared() * inv_dt * inv_dt > self.break_torque_squared {
158            self.broken = true;
159        }
160    }
161}
162
163impl<N: RealField + Copy, Handle: BodyHandle> NonlinearConstraintGenerator<N, Handle>
164    for CartesianConstraint<N, Handle>
165{
166    fn num_position_constraints(&self, bodies: &dyn BodySet<N, Handle = Handle>) -> usize {
167        // FIXME: calling this at each iteration of the non-linear resolution is costly.
168        if self.is_active(bodies) {
169            1
170        } else {
171            0
172        }
173    }
174
175    fn position_constraint(
176        &self,
177        parameters: &IntegrationParameters<N>,
178        _: usize,
179        bodies: &mut dyn BodySet<N, Handle = Handle>,
180        jacobians: &mut [N],
181    ) -> Option<GenericNonlinearConstraint<N, Handle>> {
182        let body1 = bodies.get(self.b1.0)?;
183        let body2 = bodies.get(self.b2.0)?;
184        let part1 = body1.part(self.b1.1)?;
185        let part2 = body2.part(self.b2.1)?;
186
187        let pos1 = body1.position_at_material_point(part1, &self.anchor1) * self.ref_frame1;
188        let pos2 = body2.position_at_material_point(part2, &self.anchor2) * self.ref_frame2;
189
190        let anchor1 = Point::from(pos1.translation.vector);
191        let anchor2 = Point::from(pos2.translation.vector);
192
193        let rotation1 = pos1.rotation;
194        let rotation2 = pos2.rotation;
195
196        helper::cancel_relative_rotation(
197            parameters, body1, part1, self.b1, body2, part2, self.b2, &anchor1, &anchor2,
198            &rotation1, &rotation2, jacobians,
199        )
200    }
201}
nphysics3d/joint/cartesian_constraint.rs

nphysics3d/joint/
cartesian_constraint.rs
