nphysics3d/joint/
prismatic_constraint.rs

1use na::{DVector, RealField, Unit};
2use std::ops::Range;
3
4use crate::joint::{unit_constraint, JointConstraint};
5use crate::math::{AngularVector, Point, Vector, DIM, SPATIAL_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 remove all be one translational degrees of freedom.
14pub struct PrismaticConstraint<N: RealField + Copy, Handle: BodyHandle> {
15    b1: BodyPartHandle<Handle>,
16    b2: BodyPartHandle<Handle>,
17    anchor1: Point<N>,
18    anchor2: Point<N>,
19    axis1: Unit<Vector<N>>,
20    lin_impulses: Vector<N>,
21    ang_impulses: AngularVector<N>,
22    break_torque_squared: N,
23    break_force_squared: N,
24    broken: bool,
25    limit_impulse: N,
26    bilateral_ground_rng: Range<usize>,
27    bilateral_rng: Range<usize>,
28
29    min_offset: Option<N>,
30    max_offset: Option<N>,
31}
32
33impl<N: RealField + Copy, Handle: BodyHandle> PrismaticConstraint<N, Handle> {
34    /// Create a new prismatic constraint that ensures the relative motion between the two
35    /// body parts are restricted to a single translation along the `axis1` axis (expressed in
36    /// the local coordinates frame of `b1`).
37    pub fn new(
38        b1: BodyPartHandle<Handle>,
39        b2: BodyPartHandle<Handle>,
40        anchor1: Point<N>,
41        axis1: Unit<Vector<N>>,
42        anchor2: Point<N>,
43    ) -> Self {
44        let min_offset = None;
45        let max_offset = None;
46
47        PrismaticConstraint {
48            b1,
49            b2,
50            anchor1,
51            anchor2,
52            axis1,
53            lin_impulses: Vector::zeros(),
54            ang_impulses: AngularVector::zeros(),
55            break_force_squared: N::max_value(),
56            break_torque_squared: N::max_value(),
57            broken: false,
58            limit_impulse: N::zero(),
59            bilateral_ground_rng: 0..0,
60            bilateral_rng: 0..0,
61            min_offset,
62            max_offset,
63        }
64    }
65
66    /// The maximum force this joint can absorb before breaking.
67    pub fn set_break_force(&mut self, break_force: N) {
68        self.break_force_squared = break_force * break_force;
69    }
70
71    /// The maximum torque this joint can absorb before breaking.
72    pub fn set_break_torque(&mut self, break_torque: N) {
73        self.break_torque_squared = break_torque * break_torque;
74    }
75
76    /// The lower limit, if any, of the relative translation (along the joint axis) of the body parts attached to this joint.
77    pub fn min_offset(&self) -> Option<N> {
78        self.min_offset
79    }
80
81    /// The upper limit, if any, of the relative translation (along the joint axis) of the body parts attached to this joint.
82    pub fn max_offset(&self) -> Option<N> {
83        self.max_offset
84    }
85
86    /// Disable the lower limit of the relative translational motion along the joint axis.
87    pub fn disable_min_offset(&mut self) {
88        self.min_offset = None;
89    }
90
91    /// Disable the upper limit of the relative translational motion along the joint axis.
92    pub fn disable_max_offset(&mut self) {
93        self.max_offset = None;
94    }
95
96    /// Enables the lower limit of the relative translational motion along the joint axis.
97    pub fn enable_min_offset(&mut self, limit: N) {
98        self.min_offset = Some(limit);
99        self.assert_limits();
100    }
101
102    /// Disable the lower limit of the relative translational motion along the joint axis.
103    pub fn enable_max_offset(&mut self, limit: N) {
104        self.max_offset = Some(limit);
105        self.assert_limits();
106    }
107
108    fn assert_limits(&self) {
109        if let (Some(min_offset), Some(max_offset)) = (self.min_offset, self.max_offset) {
110            assert!(
111                min_offset <= max_offset,
112                "RevoluteJoint constraint limits: the min angle must be larger than (or equal to) the max angle.");
113        }
114    }
115}
116
117impl<N: RealField + Copy, Handle: BodyHandle> JointConstraint<N, Handle>
118    for PrismaticConstraint<N, Handle>
119{
120    fn is_broken(&self) -> bool {
121        self.broken
122    }
123
124    fn num_velocity_constraints(&self) -> usize {
125        (SPATIAL_DIM - 1) + 2
126    }
127
128    fn anchors(&self) -> (BodyPartHandle<Handle>, BodyPartHandle<Handle>) {
129        (self.b1, self.b2)
130    }
131
132    fn velocity_constraints(
133        &mut self,
134        _: &IntegrationParameters<N>,
135        bodies: &dyn BodySet<N, Handle = Handle>,
136        ext_vels: &DVector<N>,
137        ground_j_id: &mut usize,
138        j_id: &mut usize,
139        jacobians: &mut [N],
140        constraints: &mut LinearConstraints<N, usize>,
141    ) {
142        let body1 = try_ret!(bodies.get(self.b1.0));
143        let body2 = try_ret!(bodies.get(self.b2.0));
144        let part1 = try_ret!(body1.part(self.b1.1));
145        let part2 = try_ret!(body2.part(self.b2.1));
146
147        /*
148         *
149         * Joint constraints.
150         *
151         */
152        let pos1 = body1.position_at_material_point(part1, &self.anchor1);
153        let pos2 = body2.position_at_material_point(part2, &self.anchor2);
154
155        let anchor1 = Point::from(pos1.translation.vector);
156        let anchor2 = Point::from(pos2.translation.vector);
157
158        let assembly_id1 = body1.companion_id();
159        let assembly_id2 = body2.companion_id();
160
161        let first_bilateral_ground = constraints.bilateral_ground.len();
162        let first_bilateral = constraints.bilateral.len();
163
164        let axis = pos1 * self.axis1;
165
166        helper::restrict_relative_linear_velocity_to_axis(
167            body1,
168            part1,
169            self.b1,
170            body2,
171            part2,
172            self.b2,
173            assembly_id1,
174            assembly_id2,
175            &anchor1,
176            &anchor2,
177            &axis,
178            ext_vels,
179            self.lin_impulses.as_slice(),
180            0,
181            ground_j_id,
182            j_id,
183            jacobians,
184            constraints,
185        );
186
187        helper::cancel_relative_angular_velocity(
188            body1,
189            part1,
190            self.b1,
191            body2,
192            part2,
193            self.b2,
194            assembly_id1,
195            assembly_id2,
196            &anchor1,
197            &anchor2,
198            ext_vels,
199            &self.ang_impulses,
200            DIM - 1,
201            ground_j_id,
202            j_id,
203            jacobians,
204            constraints,
205        );
206
207        /*
208         *
209         * Limit constraints.
210         *
211         */
212        unit_constraint::build_linear_limits_velocity_constraint(
213            body1,
214            part1,
215            self.b1,
216            body2,
217            part2,
218            self.b2,
219            assembly_id1,
220            assembly_id2,
221            &anchor1,
222            &anchor2,
223            &axis,
224            self.min_offset,
225            self.max_offset,
226            ext_vels,
227            self.limit_impulse,
228            SPATIAL_DIM - 1,
229            ground_j_id,
230            j_id,
231            jacobians,
232            constraints,
233        );
234
235        self.bilateral_ground_rng = first_bilateral_ground..constraints.bilateral_ground.len();
236        self.bilateral_rng = first_bilateral..constraints.bilateral.len();
237    }
238
239    fn cache_impulses(&mut self, constraints: &LinearConstraints<N, usize>, inv_dt: N) {
240        for c in &constraints.bilateral_ground[self.bilateral_ground_rng.clone()] {
241            if c.impulse_id < DIM - 1 {
242                self.lin_impulses[c.impulse_id] = c.impulse;
243            } else if c.impulse_id < SPATIAL_DIM - 1 {
244                self.ang_impulses[c.impulse_id + 1 - DIM] = c.impulse;
245            } else {
246                self.limit_impulse = c.impulse
247            }
248        }
249
250        for c in &constraints.bilateral[self.bilateral_rng.clone()] {
251            if c.impulse_id < DIM - 1 {
252                self.lin_impulses[c.impulse_id] = c.impulse;
253            } else if c.impulse_id < SPATIAL_DIM - 1 {
254                self.ang_impulses[c.impulse_id + 1 - DIM] = c.impulse;
255            } else {
256                self.limit_impulse = c.impulse
257            }
258        }
259
260        let inv_dt2 = inv_dt * inv_dt;
261
262        if self.lin_impulses.norm_squared() * inv_dt2 > self.break_force_squared
263            || self.ang_impulses.norm_squared() * inv_dt2 > self.break_torque_squared
264        {
265            self.broken = true;
266        }
267    }
268}
269
270impl<N: RealField + Copy, Handle: BodyHandle> NonlinearConstraintGenerator<N, Handle>
271    for PrismaticConstraint<N, Handle>
272{
273    fn num_position_constraints(&self, bodies: &dyn BodySet<N, Handle = Handle>) -> usize {
274        // FIXME: calling this at each iteration of the non-linear resolution is costly.
275        if self.is_active(bodies) {
276            if self.min_offset.is_some() || self.max_offset.is_some() {
277                3
278            } else {
279                2
280            }
281        } else {
282            0
283        }
284    }
285
286    fn position_constraint(
287        &self,
288        parameters: &IntegrationParameters<N>,
289        i: usize,
290        bodies: &mut dyn BodySet<N, Handle = Handle>,
291        jacobians: &mut [N],
292    ) -> Option<GenericNonlinearConstraint<N, Handle>> {
293        let body1 = bodies.get(self.b1.0)?;
294        let body2 = bodies.get(self.b2.0)?;
295        let part1 = body1.part(self.b1.1)?;
296        let part2 = body2.part(self.b2.1)?;
297
298        let pos1 = body1.position_at_material_point(part1, &self.anchor1);
299        let pos2 = body2.position_at_material_point(part2, &self.anchor2);
300
301        let anchor1 = Point::from(pos1.translation.vector);
302        let anchor2 = Point::from(pos2.translation.vector);
303
304        if i == 0 {
305            return helper::cancel_relative_rotation(
306                parameters,
307                body1,
308                part1,
309                self.b1,
310                body2,
311                part2,
312                self.b2,
313                &anchor1,
314                &anchor2,
315                &pos1.rotation,
316                &pos2.rotation,
317                jacobians,
318            );
319        } else if i == 1 {
320            let axis = pos1 * self.axis1;
321
322            return helper::project_anchor_to_axis(
323                parameters, body1, part1, self.b1, body2, part2, self.b2, &anchor1, &anchor2,
324                &axis, jacobians,
325            );
326        } else if i == 2 {
327            let axis = pos1 * self.axis1;
328
329            return unit_constraint::build_linear_limits_position_constraint(
330                parameters,
331                body1,
332                part1,
333                self.b1,
334                body2,
335                part2,
336                self.b2,
337                &anchor1,
338                &anchor2,
339                &axis,
340                self.min_offset,
341                self.max_offset,
342                jacobians,
343            );
344        }
345
346        None
347    }
348}
nphysics3d/joint/prismatic_constraint.rs

nphysics3d/joint/
prismatic_constraint.rs
