rapier2d/dynamics/solver/joint_constraint/
joint_velocity_constraint.rs

1use crate::dynamics::solver::SolverVel;
2use crate::dynamics::solver::joint_constraint::JointConstraintHelper;
3use crate::dynamics::{
4    GenericJoint, IntegrationParameters, JointAxesMask, JointGraphEdge, JointIndex,
5};
6use crate::math::{AngVector, AngularInertia, DIM, Isometry, Point, Real, SPATIAL_DIM, Vector};
7use crate::utils::{SimdDot, SimdRealCopy};
8
9use crate::dynamics::solver::solver_body::SolverBodies;
10#[cfg(feature = "simd-is-enabled")]
11use crate::math::{SIMD_WIDTH, SimdReal};
12#[cfg(feature = "dim2")]
13use crate::num::Zero;
14#[cfg(feature = "simd-is-enabled")]
15use na::SimdValue;
16
17#[derive(Copy, Clone, PartialEq, Debug)]
18pub struct MotorParameters<N: SimdRealCopy> {
19    pub erp_inv_dt: N,
20    pub cfm_coeff: N,
21    pub cfm_gain: N,
22    pub target_pos: N,
23    pub target_vel: N,
24    pub max_impulse: N,
25}
26
27impl<N: SimdRealCopy> Default for MotorParameters<N> {
28    fn default() -> Self {
29        Self {
30            erp_inv_dt: N::zero(),
31            cfm_coeff: N::zero(),
32            cfm_gain: N::zero(),
33            target_pos: N::zero(),
34            target_vel: N::zero(),
35            max_impulse: N::zero(),
36        }
37    }
38}
39
40#[derive(Copy, Clone, PartialEq, Eq, Debug)]
41pub enum WritebackId {
42    Dof(usize),
43    Limit(usize),
44    Motor(usize),
45}
46
47// TODO: right now we only use this for impulse_joints.
48// However, it may actually be a good idea to use this everywhere in
49// the solver, to avoid fetching data from the rigid-body set
50// every time.
51#[derive(Copy, Clone)]
52pub struct JointSolverBody<N: SimdRealCopy, const LANES: usize> {
53    pub im: Vector<N>,
54    pub ii: AngularInertia<N>,
55    pub world_com: Point<N>, // TODO: is this still needed now that the solver body poses are expressed at the center of mass?
56    pub solver_vel: [u32; LANES],
57}
58
59impl<N: SimdRealCopy, const LANES: usize> JointSolverBody<N, LANES> {
60    pub fn invalid() -> Self {
61        Self {
62            im: Vector::zeros(),
63            ii: AngularInertia::zero(),
64            world_com: Point::origin(),
65            solver_vel: [u32::MAX; LANES],
66        }
67    }
68}
69
70#[derive(Debug, Copy, Clone)]
71pub struct JointConstraint<N: SimdRealCopy, const LANES: usize> {
72    pub solver_vel1: [u32; LANES],
73    pub solver_vel2: [u32; LANES],
74
75    pub joint_id: [JointIndex; LANES],
76
77    pub impulse: N,
78    pub impulse_bounds: [N; 2],
79    pub lin_jac: Vector<N>,
80    pub ang_jac1: AngVector<N>,
81    pub ang_jac2: AngVector<N>,
82
83    pub ii_ang_jac1: AngVector<N>,
84    pub ii_ang_jac2: AngVector<N>,
85
86    pub inv_lhs: N,
87    pub rhs: N,
88    pub rhs_wo_bias: N,
89    pub cfm_gain: N,
90    pub cfm_coeff: N,
91
92    pub im1: Vector<N>,
93    pub im2: Vector<N>,
94
95    pub writeback_id: WritebackId,
96}
97
98impl<N: SimdRealCopy, const LANES: usize> JointConstraint<N, LANES> {
99    #[profiling::function]
100    pub fn solve_generic(
101        &mut self,
102        solver_vel1: &mut SolverVel<N>,
103        solver_vel2: &mut SolverVel<N>,
104    ) {
105        let dlinvel = self.lin_jac.dot(&(solver_vel2.linear - solver_vel1.linear));
106        let dangvel =
107            self.ang_jac2.gdot(solver_vel2.angular) - self.ang_jac1.gdot(solver_vel1.angular);
108
109        let rhs = dlinvel + dangvel + self.rhs;
110        let total_impulse = (self.impulse + self.inv_lhs * (rhs - self.cfm_gain * self.impulse))
111            .simd_clamp(self.impulse_bounds[0], self.impulse_bounds[1]);
112        let delta_impulse = total_impulse - self.impulse;
113        self.impulse = total_impulse;
114
115        let lin_impulse = self.lin_jac * delta_impulse;
116        let ii_ang_impulse1 = self.ii_ang_jac1 * delta_impulse;
117        let ii_ang_impulse2 = self.ii_ang_jac2 * delta_impulse;
118
119        solver_vel1.linear += lin_impulse.component_mul(&self.im1);
120        solver_vel1.angular += ii_ang_impulse1;
121        solver_vel2.linear -= lin_impulse.component_mul(&self.im2);
122        solver_vel2.angular -= ii_ang_impulse2;
123    }
124
125    pub fn remove_bias_from_rhs(&mut self) {
126        self.rhs = self.rhs_wo_bias;
127    }
128}
129
130impl JointConstraint<Real, 1> {
131    pub fn update(
132        params: &IntegrationParameters,
133        joint_id: JointIndex,
134        body1: &JointSolverBody<Real, 1>,
135        body2: &JointSolverBody<Real, 1>,
136        frame1: &Isometry<Real>,
137        frame2: &Isometry<Real>,
138        joint: &GenericJoint,
139        out: &mut [Self],
140    ) -> usize {
141        let mut len = 0;
142        let locked_axes = joint.locked_axes.bits();
143        let motor_axes = joint.motor_axes.bits() & !locked_axes;
144        let limit_axes = joint.limit_axes.bits() & !locked_axes;
145        let coupled_axes = joint.coupled_axes.bits();
146
147        // Compute per-joint ERP and CFM coefficients
148        let erp_inv_dt = joint.softness.erp_inv_dt(params.dt);
149        let cfm_coeff = joint.softness.cfm_coeff(params.dt);
150
151        // The has_lin/ang_coupling test is needed to avoid shl overflow later.
152        let has_lin_coupling = (coupled_axes & JointAxesMask::LIN_AXES.bits()) != 0;
153        let first_coupled_lin_axis_id =
154            (coupled_axes & JointAxesMask::LIN_AXES.bits()).trailing_zeros() as usize;
155
156        #[cfg(feature = "dim3")]
157        let has_ang_coupling = (coupled_axes & JointAxesMask::ANG_AXES.bits()) != 0;
158        #[cfg(feature = "dim3")]
159        let first_coupled_ang_axis_id =
160            (coupled_axes & JointAxesMask::ANG_AXES.bits()).trailing_zeros() as usize;
161
162        let builder = JointConstraintHelper::new(
163            frame1,
164            frame2,
165            &body1.world_com,
166            &body2.world_com,
167            locked_axes,
168        );
169
170        let start = len;
171        for i in DIM..SPATIAL_DIM {
172            if (motor_axes & !coupled_axes) & (1 << i) != 0 {
173                out[len] = builder.motor_angular(
174                    [joint_id],
175                    body1,
176                    body2,
177                    i - DIM,
178                    &joint.motors[i].motor_params(params.dt),
179                    WritebackId::Motor(i),
180                );
181                len += 1;
182            }
183        }
184        for i in 0..DIM {
185            if (motor_axes & !coupled_axes) & (1 << i) != 0 {
186                let limits = if limit_axes & (1 << i) != 0 {
187                    Some([joint.limits[i].min, joint.limits[i].max])
188                } else {
189                    None
190                };
191
192                out[len] = builder.motor_linear(
193                    params,
194                    [joint_id],
195                    body1,
196                    body2,
197                    i,
198                    &joint.motors[i].motor_params(params.dt),
199                    limits,
200                    WritebackId::Motor(i),
201                );
202                len += 1;
203            }
204        }
205
206        if (motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
207            // TODO: coupled angular motor constraint.
208        }
209
210        if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
211            let limits = if (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 {
212                Some([
213                    joint.limits[first_coupled_lin_axis_id].min,
214                    joint.limits[first_coupled_lin_axis_id].max,
215                ])
216            } else {
217                None
218            };
219
220            out[len] = builder.motor_linear_coupled(
221                params,
222                [joint_id],
223                body1,
224                body2,
225                coupled_axes,
226                &joint.motors[first_coupled_lin_axis_id].motor_params(params.dt),
227                limits,
228                WritebackId::Motor(first_coupled_lin_axis_id),
229            );
230            len += 1;
231        }
232
233        JointConstraintHelper::finalize_constraints(&mut out[start..len]);
234
235        let start = len;
236        for i in DIM..SPATIAL_DIM {
237            if locked_axes & (1 << i) != 0 {
238                out[len] = builder.lock_angular(
239                    params,
240                    [joint_id],
241                    body1,
242                    body2,
243                    i - DIM,
244                    WritebackId::Dof(i),
245                    erp_inv_dt,
246                    cfm_coeff,
247                );
248                len += 1;
249            }
250        }
251        for i in 0..DIM {
252            if locked_axes & (1 << i) != 0 {
253                out[len] = builder.lock_linear(
254                    params,
255                    [joint_id],
256                    body1,
257                    body2,
258                    i,
259                    WritebackId::Dof(i),
260                    erp_inv_dt,
261                    cfm_coeff,
262                );
263                len += 1;
264            }
265        }
266
267        for i in DIM..SPATIAL_DIM {
268            if (limit_axes & !coupled_axes) & (1 << i) != 0 {
269                out[len] = builder.limit_angular(
270                    params,
271                    [joint_id],
272                    body1,
273                    body2,
274                    i - DIM,
275                    [joint.limits[i].min, joint.limits[i].max],
276                    WritebackId::Limit(i),
277                    erp_inv_dt,
278                    cfm_coeff,
279                );
280                len += 1;
281            }
282        }
283        for i in 0..DIM {
284            if (limit_axes & !coupled_axes) & (1 << i) != 0 {
285                out[len] = builder.limit_linear(
286                    params,
287                    [joint_id],
288                    body1,
289                    body2,
290                    i,
291                    [joint.limits[i].min, joint.limits[i].max],
292                    WritebackId::Limit(i),
293                    erp_inv_dt,
294                    cfm_coeff,
295                );
296                len += 1;
297            }
298        }
299
300        #[cfg(feature = "dim3")]
301        if has_ang_coupling && (limit_axes & (1 << first_coupled_ang_axis_id)) != 0 {
302            out[len] = builder.limit_angular_coupled(
303                params,
304                [joint_id],
305                body1,
306                body2,
307                coupled_axes,
308                [
309                    joint.limits[first_coupled_ang_axis_id].min,
310                    joint.limits[first_coupled_ang_axis_id].max,
311                ],
312                WritebackId::Limit(first_coupled_ang_axis_id),
313                erp_inv_dt,
314                cfm_coeff,
315            );
316            len += 1;
317        }
318
319        if has_lin_coupling && (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 {
320            out[len] = builder.limit_linear_coupled(
321                params,
322                [joint_id],
323                body1,
324                body2,
325                coupled_axes,
326                [
327                    joint.limits[first_coupled_lin_axis_id].min,
328                    joint.limits[first_coupled_lin_axis_id].max,
329                ],
330                WritebackId::Limit(first_coupled_lin_axis_id),
331                erp_inv_dt,
332                cfm_coeff,
333            );
334            len += 1;
335        }
336        JointConstraintHelper::finalize_constraints(&mut out[start..len]);
337
338        len
339    }
340
341    pub fn solve(&mut self, solver_vels: &mut SolverBodies) {
342        let mut solver_vel1 = solver_vels.get_vel(self.solver_vel1[0]);
343        let mut solver_vel2 = solver_vels.get_vel(self.solver_vel2[0]);
344
345        self.solve_generic(&mut solver_vel1, &mut solver_vel2);
346
347        solver_vels.set_vel(self.solver_vel1[0], solver_vel1);
348        solver_vels.set_vel(self.solver_vel2[0], solver_vel2);
349    }
350
351    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
352        let joint = &mut joints_all[self.joint_id[0]].weight;
353        match self.writeback_id {
354            WritebackId::Dof(i) => joint.impulses[i] = self.impulse,
355            WritebackId::Limit(i) => joint.data.limits[i].impulse = self.impulse,
356            WritebackId::Motor(i) => joint.data.motors[i].impulse = self.impulse,
357        }
358    }
359}
360
361#[cfg(feature = "simd-is-enabled")]
362impl JointConstraint<SimdReal, SIMD_WIDTH> {
363    pub fn update(
364        params: &IntegrationParameters,
365        joint_id: [JointIndex; SIMD_WIDTH],
366        body1: &JointSolverBody<SimdReal, SIMD_WIDTH>,
367        body2: &JointSolverBody<SimdReal, SIMD_WIDTH>,
368        frame1: &Isometry<SimdReal>,
369        frame2: &Isometry<SimdReal>,
370        locked_axes: u8,
371        softness: crate::dynamics::SpringCoefficients<SimdReal>,
372        out: &mut [Self],
373    ) -> usize {
374        let dt = SimdReal::splat(params.dt);
375        let erp_inv_dt = softness.erp_inv_dt(dt);
376        let cfm_coeff = softness.cfm_coeff(dt);
377
378        let builder = JointConstraintHelper::new(
379            frame1,
380            frame2,
381            &body1.world_com,
382            &body2.world_com,
383            locked_axes,
384        );
385
386        let mut len = 0;
387        for i in 0..DIM {
388            if locked_axes & (1 << i) != 0 {
389                out[len] = builder.lock_linear(
390                    params,
391                    joint_id,
392                    body1,
393                    body2,
394                    i,
395                    WritebackId::Dof(i),
396                    erp_inv_dt,
397                    cfm_coeff,
398                );
399                len += 1;
400            }
401        }
402
403        for i in DIM..SPATIAL_DIM {
404            if locked_axes & (1 << i) != 0 {
405                out[len] = builder.lock_angular(
406                    params,
407                    joint_id,
408                    body1,
409                    body2,
410                    i - DIM,
411                    WritebackId::Dof(i),
412                    erp_inv_dt,
413                    cfm_coeff,
414                );
415                len += 1;
416            }
417        }
418
419        JointConstraintHelper::finalize_constraints(&mut out[..len]);
420        len
421    }
422
423    pub fn solve(&mut self, solver_vels: &mut SolverBodies) {
424        let mut solver_vel1 = solver_vels.gather_vels(self.solver_vel1);
425        let mut solver_vel2 = solver_vels.gather_vels(self.solver_vel2);
426
427        self.solve_generic(&mut solver_vel1, &mut solver_vel2);
428
429        solver_vels.scatter_vels(self.solver_vel1, solver_vel1);
430        solver_vels.scatter_vels(self.solver_vel2, solver_vel2);
431    }
432
433    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
434        let impulses: [_; SIMD_WIDTH] = self.impulse.into();
435
436        // TODO: should we move the iteration on ii deeper in the nested match?
437        for ii in 0..SIMD_WIDTH {
438            let joint = &mut joints_all[self.joint_id[ii]].weight;
439            match self.writeback_id {
440                WritebackId::Dof(i) => joint.impulses[i] = impulses[ii],
441                WritebackId::Limit(i) => joint.data.limits[i].impulse = impulses[ii],
442                WritebackId::Motor(i) => joint.data.motors[i].impulse = impulses[ii],
443            }
444        }
445    }
446}
rapier2d/dynamics/solver/joint_constraint/joint_velocity_constraint.rs

rapier2d/dynamics/solver/joint_constraint/
joint_velocity_constraint.rs
