rapier3d/dynamics/solver/joint_constraint/
joint_constraint_builder.rs

1use crate::dynamics::solver::ConstraintsCounts;
2use crate::dynamics::solver::MotorParameters;
3use crate::dynamics::solver::joint_constraint::JointSolverBody;
4use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
5    JointConstraint, WritebackId,
6};
7use crate::dynamics::solver::solver_body::SolverBodies;
8use crate::dynamics::{GenericJoint, ImpulseJoint, IntegrationParameters, JointIndex};
9use crate::math::{ANG_DIM, AngVector, DIM, Isometry, Matrix, Point, Real, Rotation, Vector};
10use crate::prelude::RigidBodySet;
11use crate::utils;
12use crate::utils::{IndexMut2, SimdCrossMatrix, SimdDot, SimdRealCopy};
13#[cfg(feature = "dim3")]
14use crate::utils::{SimdBasis, SimdQuat};
15use na::SMatrix;
16
17#[cfg(feature = "simd-is-enabled")]
18use crate::math::{SIMD_WIDTH, SimdReal};
19
20pub struct JointConstraintBuilder {
21    body1: u32,
22    body2: u32,
23    joint_id: JointIndex,
24    joint: GenericJoint,
25    constraint_id: usize,
26}
27
28impl JointConstraintBuilder {
29    pub fn generate(
30        joint: &ImpulseJoint,
31        bodies: &RigidBodySet,
32        joint_id: JointIndex,
33        out_builder: &mut Self,
34        out_constraint_id: &mut usize,
35    ) {
36        let rb1 = &bodies[joint.body1];
37        let rb2 = &bodies[joint.body2];
38        let solver_body1 = rb1.effective_active_set_offset();
39        let solver_body2 = rb2.effective_active_set_offset();
40
41        *out_builder = Self {
42            body1: solver_body1,
43            body2: solver_body2,
44            joint_id,
45            joint: joint.data,
46            constraint_id: *out_constraint_id,
47        };
48        // Since solver body poses are given in center-of-mass space,
49        // we need to transform the anchors to that space.
50        out_builder.joint.transform_to_solver_body_space(rb1, rb2);
51
52        let count = ConstraintsCounts::from_joint(joint);
53        *out_constraint_id += count.num_constraints;
54    }
55
56    pub fn update(
57        &self,
58        params: &IntegrationParameters,
59        bodies: &SolverBodies,
60        out: &mut [JointConstraint<Real, 1>],
61    ) {
62        // NOTE: right now, the "update", is basically reconstructing all the
63        //       constraints. Could we make this more incremental?
64
65        let rb1 = bodies.get_pose(self.body1);
66        let rb2 = bodies.get_pose(self.body2);
67        let frame1 = rb1.pose * self.joint.local_frame1;
68        let frame2 = rb2.pose * self.joint.local_frame2;
69        let world_com1 = Point::from(rb1.pose.translation.vector);
70        let world_com2 = Point::from(rb2.pose.translation.vector);
71
72        let joint_body1 = JointSolverBody {
73            im: rb1.im,
74            ii: rb1.ii,
75            world_com: world_com1,
76            solver_vel: [self.body1],
77        };
78        let joint_body2 = JointSolverBody {
79            im: rb2.im,
80            ii: rb2.ii,
81            world_com: world_com2,
82            solver_vel: [self.body2],
83        };
84
85        JointConstraint::<Real, 1>::update(
86            params,
87            self.joint_id,
88            &joint_body1,
89            &joint_body2,
90            &frame1,
91            &frame2,
92            &self.joint,
93            &mut out[self.constraint_id..],
94        );
95    }
96}
97
98#[cfg(feature = "simd-is-enabled")]
99pub struct JointConstraintBuilderSimd {
100    body1: [u32; SIMD_WIDTH],
101    body2: [u32; SIMD_WIDTH],
102    joint_id: [JointIndex; SIMD_WIDTH],
103    local_frame1: Isometry<SimdReal>,
104    local_frame2: Isometry<SimdReal>,
105    locked_axes: u8,
106    constraint_id: usize,
107}
108
109#[cfg(feature = "simd-is-enabled")]
110impl JointConstraintBuilderSimd {
111    pub fn generate(
112        joint: [&ImpulseJoint; SIMD_WIDTH],
113        bodies: &RigidBodySet,
114        joint_id: [JointIndex; SIMD_WIDTH],
115        out_builder: &mut Self,
116        out_constraint_id: &mut usize,
117    ) {
118        let rb1 = array![|ii| &bodies[joint[ii].body1]];
119        let rb2 = array![|ii| &bodies[joint[ii].body2]];
120
121        let body1 = array![|ii| if rb1[ii].is_dynamic_or_kinematic() {
122            rb1[ii].ids.active_set_offset
123        } else {
124            u32::MAX
125        }];
126        let body2 = array![|ii| if rb2[ii].is_dynamic_or_kinematic() {
127            rb2[ii].ids.active_set_offset
128        } else {
129            u32::MAX
130        }];
131
132        let local_frame1 = array![|ii| if body1[ii] != u32::MAX {
133            joint[ii].data.local_frame1
134        } else {
135            rb1[ii].pos.position * joint[ii].data.local_frame1
136        }]
137        .into();
138        let local_frame2 = array![|ii| if body2[ii] != u32::MAX {
139            joint[ii].data.local_frame2
140        } else {
141            rb2[ii].pos.position * joint[ii].data.local_frame2
142        }]
143        .into();
144
145        *out_builder = Self {
146            body1,
147            body2,
148            joint_id,
149            local_frame1,
150            local_frame2,
151            locked_axes: joint[0].data.locked_axes.bits(),
152            constraint_id: *out_constraint_id,
153        };
154
155        let count = ConstraintsCounts::from_joint(joint[0]);
156        *out_constraint_id += count.num_constraints;
157    }
158
159    pub fn update(
160        &mut self,
161        params: &IntegrationParameters,
162        bodies: &SolverBodies,
163        out: &mut [JointConstraint<SimdReal, SIMD_WIDTH>],
164    ) {
165        // NOTE: right now, the "update", is basically reconstructing all the
166        //       constraints. Could we make this more incremental?
167
168        let rb1 = bodies.gather_poses(self.body1);
169        let rb2 = bodies.gather_poses(self.body2);
170        let frame1 = rb1.pose * self.local_frame1;
171        let frame2 = rb2.pose * self.local_frame2;
172
173        let joint_body1 = JointSolverBody {
174            im: rb1.im,
175            ii: rb1.ii,
176            world_com: rb1.pose.translation.vector.into(),
177            solver_vel: self.body1,
178        };
179        let joint_body2 = JointSolverBody {
180            im: rb2.im,
181            ii: rb2.ii,
182            world_com: rb2.pose.translation.vector.into(),
183            solver_vel: self.body2,
184        };
185
186        JointConstraint::<SimdReal, SIMD_WIDTH>::update(
187            params,
188            self.joint_id,
189            &joint_body1,
190            &joint_body2,
191            &frame1,
192            &frame2,
193            self.locked_axes,
194            &mut out[self.constraint_id..],
195        );
196    }
197}
198
199#[derive(Debug, Copy, Clone)]
200pub struct JointConstraintHelper<N: SimdRealCopy> {
201    pub basis: Matrix<N>,
202    #[cfg(feature = "dim3")]
203    pub basis2: Matrix<N>, // TODO: used for angular coupling. Can we avoid storing this?
204    pub cmat1_basis: SMatrix<N, ANG_DIM, DIM>,
205    pub cmat2_basis: SMatrix<N, ANG_DIM, DIM>,
206    #[cfg(feature = "dim3")]
207    pub ang_basis: SMatrix<N, ANG_DIM, ANG_DIM>,
208    pub lin_err: Vector<N>,
209    pub ang_err: Rotation<N>,
210}
211
212impl<N: SimdRealCopy> JointConstraintHelper<N> {
213    pub fn new(
214        frame1: &Isometry<N>,
215        frame2: &Isometry<N>,
216        world_com1: &Point<N>,
217        world_com2: &Point<N>,
218        locked_lin_axes: u8,
219    ) -> Self {
220        let mut frame1 = *frame1;
221        let basis = frame1.rotation.to_rotation_matrix().into_inner();
222        let lin_err = frame2.translation.vector - frame1.translation.vector;
223
224        // Adjust the point of application of the force for the first body,
225        // by snapping free axes to the second frame’s center (to account for
226        // the allowed relative movement).
227        {
228            let mut new_center1 = frame2.translation.vector; // First, assume all dofs are free.
229
230            // Then snap the locked ones.
231            for i in 0..DIM {
232                if locked_lin_axes & (1 << i) != 0 {
233                    let axis = basis.column(i);
234                    new_center1 -= axis * lin_err.dot(&axis);
235                }
236            }
237            frame1.translation.vector = new_center1;
238        }
239
240        let r1 = frame1.translation.vector - world_com1.coords;
241        let r2 = frame2.translation.vector - world_com2.coords;
242
243        let cmat1 = r1.gcross_matrix();
244        let cmat2 = r2.gcross_matrix();
245
246        #[cfg(feature = "dim3")]
247        let mut ang_basis = frame1.rotation.diff_conj1_2(&frame2.rotation).transpose();
248        #[allow(unused_mut)] // The mut is needed for 3D
249        let mut ang_err = frame1.rotation.inverse() * frame2.rotation;
250
251        #[cfg(feature = "dim3")]
252        {
253            let sgn = N::one().simd_copysign(frame1.rotation.dot(&frame2.rotation));
254            ang_basis *= sgn;
255            *ang_err.as_mut_unchecked() *= sgn;
256        }
257
258        Self {
259            basis,
260            #[cfg(feature = "dim3")]
261            basis2: frame2.rotation.to_rotation_matrix().into_inner(),
262            cmat1_basis: cmat1 * basis,
263            cmat2_basis: cmat2 * basis,
264            #[cfg(feature = "dim3")]
265            ang_basis,
266            lin_err,
267            ang_err,
268        }
269    }
270
271    pub fn limit_linear<const LANES: usize>(
272        &self,
273        params: &IntegrationParameters,
274        joint_id: [JointIndex; LANES],
275        body1: &JointSolverBody<N, LANES>,
276        body2: &JointSolverBody<N, LANES>,
277        limited_axis: usize,
278        limits: [N; 2],
279        writeback_id: WritebackId,
280    ) -> JointConstraint<N, LANES> {
281        let zero = N::zero();
282        let mut constraint =
283            self.lock_linear(params, joint_id, body1, body2, limited_axis, writeback_id);
284
285        let dist = self.lin_err.dot(&constraint.lin_jac);
286        let min_enabled = dist.simd_le(limits[0]);
287        let max_enabled = limits[1].simd_le(dist);
288
289        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
290        let cfm_coeff = N::splat(params.joint_cfm_coeff());
291        let rhs_bias =
292            ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) * erp_inv_dt;
293        constraint.rhs = constraint.rhs_wo_bias + rhs_bias;
294        constraint.cfm_coeff = cfm_coeff;
295        constraint.impulse_bounds = [
296            N::splat(-Real::INFINITY).select(min_enabled, zero),
297            N::splat(Real::INFINITY).select(max_enabled, zero),
298        ];
299
300        constraint
301    }
302
303    pub fn limit_linear_coupled<const LANES: usize>(
304        &self,
305        params: &IntegrationParameters,
306        joint_id: [JointIndex; LANES],
307        body1: &JointSolverBody<N, LANES>,
308        body2: &JointSolverBody<N, LANES>,
309        coupled_axes: u8,
310        limits: [N; 2],
311        writeback_id: WritebackId,
312    ) -> JointConstraint<N, LANES> {
313        let zero = N::zero();
314        let mut lin_jac = Vector::zeros();
315        let mut ang_jac1: AngVector<N> = na::zero();
316        let mut ang_jac2: AngVector<N> = na::zero();
317
318        for i in 0..DIM {
319            if coupled_axes & (1 << i) != 0 {
320                let coeff = self.basis.column(i).dot(&self.lin_err);
321                lin_jac += self.basis.column(i) * coeff;
322                #[cfg(feature = "dim2")]
323                {
324                    ang_jac1 += self.cmat1_basis[i] * coeff;
325                    ang_jac2 += self.cmat2_basis[i] * coeff;
326                }
327                #[cfg(feature = "dim3")]
328                {
329                    ang_jac1 += self.cmat1_basis.column(i) * coeff;
330                    ang_jac2 += self.cmat2_basis.column(i) * coeff;
331                }
332            }
333        }
334
335        // FIXME: handle min limit too.
336
337        let dist = lin_jac.norm();
338        let inv_dist = crate::utils::simd_inv(dist);
339        lin_jac *= inv_dist;
340        ang_jac1 *= inv_dist;
341        ang_jac2 *= inv_dist;
342
343        let rhs_wo_bias = (dist - limits[1]).simd_min(zero) * N::splat(params.inv_dt());
344
345        let ii_ang_jac1 = body1.ii * ang_jac1;
346        let ii_ang_jac2 = body2.ii * ang_jac2;
347
348        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
349        let cfm_coeff = N::splat(params.joint_cfm_coeff());
350        let rhs_bias = (dist - limits[1]).simd_max(zero) * erp_inv_dt;
351        let rhs = rhs_wo_bias + rhs_bias;
352        let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)];
353
354        JointConstraint {
355            joint_id,
356            solver_vel1: body1.solver_vel,
357            solver_vel2: body2.solver_vel,
358            im1: body1.im,
359            im2: body2.im,
360            impulse: N::zero(),
361            impulse_bounds,
362            lin_jac,
363            ang_jac1,
364            ang_jac2,
365            ii_ang_jac1,
366            ii_ang_jac2,
367            inv_lhs: N::zero(), // Will be set during orthogonalization.
368            cfm_coeff,
369            cfm_gain: N::zero(),
370            rhs,
371            rhs_wo_bias,
372            writeback_id,
373        }
374    }
375
376    pub fn motor_linear<const LANES: usize>(
377        &self,
378        params: &IntegrationParameters,
379        joint_id: [JointIndex; LANES],
380        body1: &JointSolverBody<N, LANES>,
381        body2: &JointSolverBody<N, LANES>,
382        motor_axis: usize,
383        motor_params: &MotorParameters<N>,
384        limits: Option<[N; 2]>,
385        writeback_id: WritebackId,
386    ) -> JointConstraint<N, LANES> {
387        let inv_dt = N::splat(params.inv_dt());
388        let mut constraint =
389            self.lock_linear(params, joint_id, body1, body2, motor_axis, writeback_id);
390
391        let mut rhs_wo_bias = N::zero();
392        if motor_params.erp_inv_dt != N::zero() {
393            let dist = self.lin_err.dot(&constraint.lin_jac);
394            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
395        }
396
397        let mut target_vel = motor_params.target_vel;
398        if let Some(limits) = limits {
399            let dist = self.lin_err.dot(&constraint.lin_jac);
400            target_vel =
401                target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt);
402        };
403
404        rhs_wo_bias += -target_vel;
405
406        constraint.cfm_coeff = motor_params.cfm_coeff;
407        constraint.cfm_gain = motor_params.cfm_gain;
408        constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
409        constraint.rhs = rhs_wo_bias;
410        constraint.rhs_wo_bias = rhs_wo_bias;
411        constraint
412    }
413
414    pub fn motor_linear_coupled<const LANES: usize>(
415        &self,
416        params: &IntegrationParameters,
417        joint_id: [JointIndex; LANES],
418        body1: &JointSolverBody<N, LANES>,
419        body2: &JointSolverBody<N, LANES>,
420        coupled_axes: u8,
421        motor_params: &MotorParameters<N>,
422        limits: Option<[N; 2]>,
423        writeback_id: WritebackId,
424    ) -> JointConstraint<N, LANES> {
425        let inv_dt = N::splat(params.inv_dt());
426
427        let mut lin_jac = Vector::zeros();
428        let mut ang_jac1: AngVector<N> = na::zero();
429        let mut ang_jac2: AngVector<N> = na::zero();
430
431        for i in 0..DIM {
432            if coupled_axes & (1 << i) != 0 {
433                let coeff = self.basis.column(i).dot(&self.lin_err);
434                lin_jac += self.basis.column(i) * coeff;
435                #[cfg(feature = "dim2")]
436                {
437                    ang_jac1 += self.cmat1_basis[i] * coeff;
438                    ang_jac2 += self.cmat2_basis[i] * coeff;
439                }
440                #[cfg(feature = "dim3")]
441                {
442                    ang_jac1 += self.cmat1_basis.column(i) * coeff;
443                    ang_jac2 += self.cmat2_basis.column(i) * coeff;
444                }
445            }
446        }
447
448        let dist = lin_jac.norm();
449        let inv_dist = crate::utils::simd_inv(dist);
450        lin_jac *= inv_dist;
451        ang_jac1 *= inv_dist;
452        ang_jac2 *= inv_dist;
453
454        let mut rhs_wo_bias = N::zero();
455        if motor_params.erp_inv_dt != N::zero() {
456            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
457        }
458
459        let mut target_vel = motor_params.target_vel;
460        if let Some(limits) = limits {
461            target_vel =
462                target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt);
463        };
464
465        rhs_wo_bias += -target_vel;
466
467        let ii_ang_jac1 = body1.ii * ang_jac1;
468        let ii_ang_jac2 = body2.ii * ang_jac2;
469
470        JointConstraint {
471            joint_id,
472            solver_vel1: body1.solver_vel,
473            solver_vel2: body2.solver_vel,
474            im1: body1.im,
475            im2: body2.im,
476            impulse: N::zero(),
477            impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse],
478            lin_jac,
479            ang_jac1,
480            ang_jac2,
481            ii_ang_jac1,
482            ii_ang_jac2,
483            inv_lhs: N::zero(), // Will be set during orthogonalization.
484            cfm_coeff: motor_params.cfm_coeff,
485            cfm_gain: motor_params.cfm_gain,
486            rhs: rhs_wo_bias,
487            rhs_wo_bias,
488            writeback_id,
489        }
490    }
491
492    pub fn lock_linear<const LANES: usize>(
493        &self,
494        params: &IntegrationParameters,
495        joint_id: [JointIndex; LANES],
496        body1: &JointSolverBody<N, LANES>,
497        body2: &JointSolverBody<N, LANES>,
498        locked_axis: usize,
499        writeback_id: WritebackId,
500    ) -> JointConstraint<N, LANES> {
501        let lin_jac = self.basis.column(locked_axis).into_owned();
502        #[cfg(feature = "dim2")]
503        let ang_jac1 = self.cmat1_basis[locked_axis];
504        #[cfg(feature = "dim2")]
505        let ang_jac2 = self.cmat2_basis[locked_axis];
506        #[cfg(feature = "dim3")]
507        let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned();
508        #[cfg(feature = "dim3")]
509        let ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned();
510
511        let rhs_wo_bias = N::zero();
512        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
513        let cfm_coeff = N::splat(params.joint_cfm_coeff());
514        let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
515
516        let ii_ang_jac1 = body1.ii * ang_jac1;
517        let ii_ang_jac2 = body2.ii * ang_jac2;
518
519        JointConstraint {
520            joint_id,
521            solver_vel1: body1.solver_vel,
522            solver_vel2: body2.solver_vel,
523            im1: body1.im,
524            im2: body2.im,
525            impulse: N::zero(),
526            impulse_bounds: [-N::splat(Real::MAX), N::splat(Real::MAX)],
527            lin_jac,
528            ang_jac1,
529            ang_jac2,
530            ii_ang_jac1,
531            ii_ang_jac2,
532            inv_lhs: N::zero(), // Will be set during orthogonalization.
533            cfm_coeff,
534            cfm_gain: N::zero(),
535            rhs: rhs_wo_bias + rhs_bias,
536            rhs_wo_bias,
537            writeback_id,
538        }
539    }
540
541    pub fn limit_angular<const LANES: usize>(
542        &self,
543        params: &IntegrationParameters,
544        joint_id: [JointIndex; LANES],
545        body1: &JointSolverBody<N, LANES>,
546        body2: &JointSolverBody<N, LANES>,
547        _limited_axis: usize,
548        limits: [N; 2],
549        writeback_id: WritebackId,
550    ) -> JointConstraint<N, LANES> {
551        let zero = N::zero();
552        let half = N::splat(0.5);
553        let s_limits = [(limits[0] * half).simd_sin(), (limits[1] * half).simd_sin()];
554        #[cfg(feature = "dim2")]
555        let s_ang = (self.ang_err.angle() * half).simd_sin();
556        #[cfg(feature = "dim3")]
557        let s_ang = self.ang_err.imag()[_limited_axis];
558        let min_enabled = s_ang.simd_le(s_limits[0]);
559        let max_enabled = s_limits[1].simd_le(s_ang);
560
561        let impulse_bounds = [
562            N::splat(-Real::INFINITY).select(min_enabled, zero),
563            N::splat(Real::INFINITY).select(max_enabled, zero),
564        ];
565
566        #[cfg(feature = "dim2")]
567        let ang_jac = N::one();
568        #[cfg(feature = "dim3")]
569        let ang_jac = self.ang_basis.column(_limited_axis).into_owned();
570        let rhs_wo_bias = N::zero();
571        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
572        let cfm_coeff = N::splat(params.joint_cfm_coeff());
573        let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero)
574            - (s_limits[0] - s_ang).simd_max(zero))
575            * erp_inv_dt;
576
577        let ii_ang_jac1 = body1.ii * ang_jac;
578        let ii_ang_jac2 = body2.ii * ang_jac;
579
580        JointConstraint {
581            joint_id,
582            solver_vel1: body1.solver_vel,
583            solver_vel2: body2.solver_vel,
584            im1: body1.im,
585            im2: body2.im,
586            impulse: N::zero(),
587            impulse_bounds,
588            lin_jac: na::zero(),
589            ang_jac1: ang_jac,
590            ang_jac2: ang_jac,
591            ii_ang_jac1,
592            ii_ang_jac2,
593            inv_lhs: N::zero(), // Will be set during orthogonalization.
594            cfm_coeff,
595            cfm_gain: N::zero(),
596            rhs: rhs_wo_bias + rhs_bias,
597            rhs_wo_bias,
598            writeback_id,
599        }
600    }
601
602    pub fn motor_angular<const LANES: usize>(
603        &self,
604        joint_id: [JointIndex; LANES],
605        body1: &JointSolverBody<N, LANES>,
606        body2: &JointSolverBody<N, LANES>,
607        _motor_axis: usize,
608        motor_params: &MotorParameters<N>,
609        writeback_id: WritebackId,
610    ) -> JointConstraint<N, LANES> {
611        #[cfg(feature = "dim2")]
612        let ang_jac = N::one();
613        #[cfg(feature = "dim3")]
614        let ang_jac = self.basis.column(_motor_axis).into_owned();
615
616        let mut rhs_wo_bias = N::zero();
617        if motor_params.erp_inv_dt != N::zero() {
618            let ang_dist;
619
620            #[cfg(feature = "dim2")]
621            {
622                ang_dist = self.ang_err.angle();
623            }
624
625            #[cfg(feature = "dim3")]
626            {
627                // Clamp the component from -1.0 to 1.0 to account for slight imprecision
628                let clamped_err = self.ang_err.imag()[_motor_axis].simd_clamp(-N::one(), N::one());
629                ang_dist = clamped_err.simd_asin() * N::splat(2.0);
630            }
631
632            let target_ang = motor_params.target_pos;
633            rhs_wo_bias += utils::smallest_abs_diff_between_angles(ang_dist, target_ang)
634                * motor_params.erp_inv_dt;
635        }
636
637        rhs_wo_bias += -motor_params.target_vel;
638
639        let ii_ang_jac1 = body1.ii * ang_jac;
640        let ii_ang_jac2 = body2.ii * ang_jac;
641
642        JointConstraint {
643            joint_id,
644            solver_vel1: body1.solver_vel,
645            solver_vel2: body2.solver_vel,
646            im1: body1.im,
647            im2: body2.im,
648            impulse: N::zero(),
649            impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse],
650            lin_jac: na::zero(),
651            ang_jac1: ang_jac,
652            ang_jac2: ang_jac,
653            ii_ang_jac1,
654            ii_ang_jac2,
655            inv_lhs: N::zero(), // Will be set during orthogonalization.
656            cfm_coeff: motor_params.cfm_coeff,
657            cfm_gain: motor_params.cfm_gain,
658            rhs: rhs_wo_bias,
659            rhs_wo_bias,
660            writeback_id,
661        }
662    }
663
664    pub fn lock_angular<const LANES: usize>(
665        &self,
666        params: &IntegrationParameters,
667        joint_id: [JointIndex; LANES],
668        body1: &JointSolverBody<N, LANES>,
669        body2: &JointSolverBody<N, LANES>,
670        _locked_axis: usize,
671        writeback_id: WritebackId,
672    ) -> JointConstraint<N, LANES> {
673        #[cfg(feature = "dim2")]
674        let ang_jac = N::one();
675        #[cfg(feature = "dim3")]
676        let ang_jac = self.ang_basis.column(_locked_axis).into_owned();
677
678        let rhs_wo_bias = N::zero();
679        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
680        let cfm_coeff = N::splat(params.joint_cfm_coeff());
681        #[cfg(feature = "dim2")]
682        let rhs_bias = self.ang_err.im * erp_inv_dt;
683        #[cfg(feature = "dim3")]
684        let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt;
685
686        let ii_ang_jac1 = body1.ii * ang_jac;
687        let ii_ang_jac2 = body2.ii * ang_jac;
688
689        JointConstraint {
690            joint_id,
691            solver_vel1: body1.solver_vel,
692            solver_vel2: body2.solver_vel,
693            im1: body1.im,
694            im2: body2.im,
695            impulse: N::zero(),
696            impulse_bounds: [-N::splat(Real::MAX), N::splat(Real::MAX)],
697            lin_jac: na::zero(),
698            ang_jac1: ang_jac,
699            ang_jac2: ang_jac,
700            ii_ang_jac1,
701            ii_ang_jac2,
702            inv_lhs: N::zero(), // Will be set during orthogonalization.
703            cfm_coeff,
704            cfm_gain: N::zero(),
705            rhs: rhs_wo_bias + rhs_bias,
706            rhs_wo_bias,
707            writeback_id,
708        }
709    }
710
711    /// Orthogonalize the constraints and set their inv_lhs field.
712    pub fn finalize_constraints<const LANES: usize>(constraints: &mut [JointConstraint<N, LANES>]) {
713        let len = constraints.len();
714
715        if len == 0 {
716            return;
717        }
718
719        let imsum = constraints[0].im1 + constraints[0].im2;
720
721        // Use the modified Gram-Schmidt orthogonalization.
722        for j in 0..len {
723            let c_j = &mut constraints[j];
724            let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
725                + c_j.ii_ang_jac1.gdot(c_j.ang_jac1)
726                + c_j.ii_ang_jac2.gdot(c_j.ang_jac2);
727            let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain;
728            let inv_dot_jj = crate::utils::simd_inv(dot_jj);
729            c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs.
730            c_j.cfm_gain = cfm_gain;
731
732            if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] {
733                // Don't remove constraints with limited forces from the others
734                // because they may not deliver the necessary forces to fulfill
735                // the removed parts of other constraints.
736                continue;
737            }
738
739            for i in (j + 1)..len {
740                let (c_i, c_j) = constraints.index_mut_const(i, j);
741
742                let dot_ij = c_i.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
743                    + c_i.ii_ang_jac1.gdot(c_j.ang_jac1)
744                    + c_i.ii_ang_jac2.gdot(c_j.ang_jac2);
745                let coeff = dot_ij * inv_dot_jj;
746
747                c_i.lin_jac -= c_j.lin_jac * coeff;
748                c_i.ang_jac1 -= c_j.ang_jac1 * coeff;
749                c_i.ang_jac2 -= c_j.ang_jac2 * coeff;
750                c_i.ii_ang_jac1 -= c_j.ii_ang_jac1 * coeff;
751                c_i.ii_ang_jac2 -= c_j.ii_ang_jac2 * coeff;
752                c_i.rhs_wo_bias -= c_j.rhs_wo_bias * coeff;
753                c_i.rhs -= c_j.rhs * coeff;
754            }
755        }
756    }
757}
758
759impl JointConstraintHelper<Real> {
760    #[cfg(feature = "dim3")]
761    pub fn limit_angular_coupled(
762        &self,
763        params: &IntegrationParameters,
764        joint_id: [JointIndex; 1],
765        body1: &JointSolverBody<Real, 1>,
766        body2: &JointSolverBody<Real, 1>,
767        coupled_axes: u8,
768        limits: [Real; 2],
769        writeback_id: WritebackId,
770    ) -> JointConstraint<Real, 1> {
771        // NOTE: right now, this only supports exactly 2 coupled axes.
772        let ang_coupled_axes = coupled_axes >> DIM;
773        assert_eq!(ang_coupled_axes.count_ones(), 2);
774        let not_coupled_index = ang_coupled_axes.trailing_ones() as usize;
775        let axis1 = self.basis.column(not_coupled_index).into_owned();
776        let axis2 = self.basis2.column(not_coupled_index).into_owned();
777
778        let rot = Rotation::rotation_between(&axis1, &axis2).unwrap_or_else(Rotation::identity);
779        let (ang_jac, angle) = rot
780            .axis_angle()
781            .map(|(axis, angle)| (axis.into_inner(), angle))
782            .unwrap_or_else(|| (axis1.orthonormal_basis()[0], 0.0));
783
784        let min_enabled = angle <= limits[0];
785        let max_enabled = limits[1] <= angle;
786
787        let impulse_bounds = [
788            if min_enabled { -Real::INFINITY } else { 0.0 },
789            if max_enabled { Real::INFINITY } else { 0.0 },
790        ];
791
792        let rhs_wo_bias = 0.0;
793
794        let erp_inv_dt = params.joint_erp_inv_dt();
795        let cfm_coeff = params.joint_cfm_coeff();
796        let rhs_bias = ((angle - limits[1]).max(0.0) - (limits[0] - angle).max(0.0)) * erp_inv_dt;
797
798        let ii_ang_jac1 = body1.ii * ang_jac;
799        let ii_ang_jac2 = body2.ii * ang_jac;
800
801        JointConstraint {
802            joint_id,
803            solver_vel1: body1.solver_vel,
804            solver_vel2: body2.solver_vel,
805            im1: body1.im,
806            im2: body2.im,
807            impulse: 0.0,
808            impulse_bounds,
809            lin_jac: na::zero(),
810            ang_jac1: ang_jac,
811            ang_jac2: ang_jac,
812            ii_ang_jac1,
813            ii_ang_jac2,
814            inv_lhs: 0.0, // Will be set during orthogonalization.
815            cfm_coeff,
816            cfm_gain: 0.0,
817            rhs: rhs_wo_bias + rhs_bias,
818            rhs_wo_bias,
819            writeback_id,
820        }
821    }
822}
rapier3d/dynamics/solver/joint_constraint/joint_constraint_builder.rs

rapier3d/dynamics/solver/joint_constraint/
joint_constraint_builder.rs
