nphysics2d/solver/
helper.rs

1//! Utilities for computing velocity and position constraints.
2
3#[cfg(feature = "dim3")]
4use na;
5use na::{DVector, DVectorSlice, RealField, Unit};
6use std::ops::Neg;
7
8use crate::math::{AngularVector, Force, Point, Rotation, Vector};
9use crate::object::{Body, BodyHandle, BodyPart, BodyPartHandle};
10use crate::solver::{
11    BilateralConstraint, BilateralGroundConstraint, ConstraintGeometry, GenericNonlinearConstraint,
12    ImpulseLimits, IntegrationParameters, LinearConstraints,
13};
14
15/// The direction of a force in world-space.
16#[derive(Copy, Clone, Debug)]
17pub enum ForceDirection<N: RealField + Copy> {
18    /// A linear force toward a direction.
19    Linear(Unit<Vector<N>>),
20    /// A torque wrt. an axis.
21    Angular(Unit<AngularVector<N>>),
22}
23
24impl<N: RealField + Copy> ForceDirection<N> {
25    /// The force (at the specified point) resulting from this unit force applied at the specified point.
26    #[inline]
27    pub fn at_point(&self, pt: &Point<N>) -> Force<N> {
28        match self {
29            ForceDirection::Linear(normal) => Force::linear_at_point(**normal, pt),
30            ForceDirection::Angular(axis) => Force::torque_from_vector(**axis),
31        }
32    }
33}
34
35impl<N: RealField + Copy> Neg for ForceDirection<N> {
36    type Output = Self;
37
38    #[inline]
39    fn neg(self) -> Self {
40        match self {
41            ForceDirection::Linear(n) => ForceDirection::Linear(-n),
42            ForceDirection::Angular(a) => ForceDirection::Angular(-a),
43        }
44    }
45}
46
47/// Fills all the jacobians (and the jacobians multiplied by the invers augmented mass matricxs) for a
48/// constraint applying a force at the points `center1, center2` and the direction `dir`.
49///
50/// If the force is a torque, it is applied at the centers of mass of the body parts.
51/// Every input are expressed in world-space.
52#[inline]
53pub fn constraint_pair_geometry<N: RealField + Copy, B: ?Sized + Body<N>, H: BodyHandle>(
54    body1: &B,
55    part1: &dyn BodyPart<N>,
56    handle1: BodyPartHandle<H>,
57    body2: &B,
58    part2: &dyn BodyPart<N>,
59    handle2: BodyPartHandle<H>,
60    center1: &Point<N>,
61    center2: &Point<N>,
62    dir: &ForceDirection<N>,
63    ground_j_id: &mut usize,
64    j_id: &mut usize,
65    jacobians: &mut [N],
66    ext_vels1: Option<&DVectorSlice<N>>,
67    ext_vels2: Option<&DVectorSlice<N>>,
68    mut out_vel: Option<&mut N>,
69) -> ConstraintGeometry<N> {
70    let mut res = ConstraintGeometry::new();
71
72    res.ndofs1 = body1.status_dependent_ndofs();
73    res.ndofs2 = body2.status_dependent_ndofs();
74
75    let out_j_id;
76    if res.ndofs1 == 0 || res.ndofs2 == 0 {
77        res.j_id1 = *ground_j_id;
78        out_j_id = ground_j_id;
79    } else {
80        res.j_id1 = *j_id;
81        out_j_id = j_id;
82    }
83
84    res.j_id2 = res.j_id1 + res.ndofs1;
85    res.wj_id1 = res.j_id2 + res.ndofs2;
86    res.wj_id2 = res.wj_id1 + res.ndofs1;
87
88    let mut inv_r = N::zero();
89
90    body1.fill_constraint_geometry(
91        part1,
92        res.ndofs1,
93        center1,
94        dir,
95        res.j_id1,
96        res.wj_id1,
97        jacobians,
98        &mut inv_r,
99        ext_vels1,
100        // Unfortunate pattern we have to do to avoid borrowing issues.
101        // See https://internals.rust-lang.org/t/should-option-mut-t-implement-copy/3715/6
102        out_vel.as_mut().map(|v| &mut **v),
103    );
104
105    body2.fill_constraint_geometry(
106        part2,
107        res.ndofs2,
108        center2,
109        &dir.neg(),
110        res.j_id2,
111        res.wj_id2,
112        jacobians,
113        &mut inv_r,
114        ext_vels2,
115        out_vel,
116    );
117
118    if handle1.0 == handle2.0 {
119        let j1 = DVectorSlice::from_slice(&jacobians[res.j_id1..], res.ndofs1);
120        let j2 = DVectorSlice::from_slice(&jacobians[res.j_id2..], res.ndofs2);
121        let invm_j1 = DVectorSlice::from_slice(&jacobians[res.wj_id1..], res.ndofs1);
122        let invm_j2 = DVectorSlice::from_slice(&jacobians[res.wj_id2..], res.ndofs2);
123
124        inv_r += j2.dot(&invm_j1) + j1.dot(&invm_j2);
125    }
126
127    if !inv_r.is_zero() {
128        res.r = N::one() / inv_r;
129    } else {
130        res.r = N::one()
131    }
132
133    *out_j_id += (res.ndofs1 + res.ndofs2) * 2;
134    res
135}
136
137/// Test if a constraint between the two given bodies should be a ground
138/// constraint (a constraint between a dynamic body and one without any degree of freedom).
139#[inline]
140pub fn constraints_are_ground_constraints<N: RealField + Copy, B: ?Sized + Body<N>>(
141    body1: &B,
142    body2: &B,
143) -> bool {
144    body1.status_dependent_ndofs() == 0 || body2.status_dependent_ndofs() == 0
145}
146
147/// Retrieve the external velocity subvectors for the given bodies.
148#[inline(always)]
149pub fn split_ext_vels<'a, N: RealField + Copy, B: ?Sized + Body<N>>(
150    body1: &B,
151    body2: &B,
152    assembly_id1: usize,
153    assembly_id2: usize,
154    ext_vels: &'a DVector<N>,
155) -> (DVectorSlice<'a, N>, DVectorSlice<'a, N>) {
156    let ndofs1 = body1.status_dependent_ndofs();
157    let ndofs2 = body2.status_dependent_ndofs();
158    (
159        ext_vels.rows(assembly_id1, ndofs1),
160        ext_vels.rows(assembly_id2, ndofs2),
161    )
162}
163
164/// Generates velocity constraints to cancel the relative linear velocity of two body parts wrt the given axis.
165///
166/// All inputs mut be given in world-space.
167pub fn cancel_relative_linear_velocity_wrt_axis<
168    N: RealField + Copy,
169    B: ?Sized + Body<N>,
170    H: BodyHandle,
171    Id,
172>(
173    body1: &B,
174    part1: &dyn BodyPart<N>,
175    handle1: BodyPartHandle<H>,
176    body2: &B,
177    part2: &dyn BodyPart<N>,
178    handle2: BodyPartHandle<H>,
179    assembly_id1: usize,
180    assembly_id2: usize,
181    anchor1: &Point<N>,
182    anchor2: &Point<N>,
183    axis: &Unit<Vector<N>>,
184    ext_vels: &DVector<N>,
185    impulse: N,
186    impulse_id: Id,
187    ground_j_id: &mut usize,
188    j_id: &mut usize,
189    jacobians: &mut [N],
190    constraints: &mut LinearConstraints<N, Id>,
191) {
192    let limits = ImpulseLimits::Independent {
193        min: -N::max_value(),
194        max: N::max_value(),
195    };
196
197    let force = ForceDirection::Linear(*axis);
198    let mut rhs = N::zero();
199    let (ext_vels1, ext_vels2) = split_ext_vels(body1, body2, assembly_id1, assembly_id2, ext_vels);
200
201    let geom = constraint_pair_geometry(
202        body1,
203        part1,
204        handle1,
205        body2,
206        part2,
207        handle2,
208        anchor1,
209        anchor2,
210        &force,
211        ground_j_id,
212        j_id,
213        jacobians,
214        Some(&ext_vels1),
215        Some(&ext_vels2),
216        Some(&mut rhs),
217    );
218
219    if geom.ndofs1 == 0 || geom.ndofs2 == 0 {
220        constraints
221            .bilateral_ground
222            .push(BilateralGroundConstraint::new(
223                geom,
224                assembly_id1,
225                assembly_id2,
226                limits,
227                rhs,
228                impulse,
229                impulse_id,
230            ));
231    } else {
232        constraints.bilateral.push(BilateralConstraint::new(
233            geom,
234            assembly_id1,
235            assembly_id2,
236            limits,
237            rhs,
238            impulse,
239            impulse_id,
240        ));
241    }
242}
243
244/// Generates velocity constraints to cancel the relative linear velocity of two body parts.
245///
246/// All inputs mut be given in world-space.
247pub fn cancel_relative_linear_velocity<N: RealField + Copy, B: ?Sized + Body<N>, H: BodyHandle>(
248    body1: &B,
249    part1: &dyn BodyPart<N>,
250    handle1: BodyPartHandle<H>,
251    body2: &B,
252    part2: &dyn BodyPart<N>,
253    handle2: BodyPartHandle<H>,
254    assembly_id1: usize,
255    assembly_id2: usize,
256    anchor1: &Point<N>,
257    anchor2: &Point<N>,
258    ext_vels: &DVector<N>,
259    impulses: &Vector<N>,
260    impulse_id: usize,
261    ground_j_id: &mut usize,
262    j_id: &mut usize,
263    jacobians: &mut [N],
264    constraints: &mut LinearConstraints<N, usize>,
265) {
266    #[cfg(feature = "dim2")]
267    let canonical_basis = [Vector::x(), Vector::y()];
268    #[cfg(feature = "dim3")]
269    let canonical_basis = [Vector::x(), Vector::y(), Vector::z()];
270
271    for (i, dir) in canonical_basis.iter().enumerate() {
272        cancel_relative_linear_velocity_wrt_axis(
273            body1,
274            part1,
275            handle1,
276            body2,
277            part2,
278            handle2,
279            assembly_id1,
280            assembly_id2,
281            anchor1,
282            anchor2,
283            &Unit::new_unchecked(*dir),
284            ext_vels,
285            impulses[i],
286            impulse_id + i,
287            ground_j_id,
288            j_id,
289            jacobians,
290            constraints,
291        );
292    }
293}
294
295/// Generate position constraints to cancel the relative translation of two bodies wrt the given axis.
296///
297/// All inputs mut be given in world-space.
298pub fn cancel_relative_translation_wrt_axis<N: RealField + Copy, B: ?Sized + Body<N>, H: BodyHandle>(
299    parameters: &IntegrationParameters<N>,
300    body1: &B,
301    part1: &dyn BodyPart<N>,
302    handle1: BodyPartHandle<H>,
303    body2: &B,
304    part2: &dyn BodyPart<N>,
305    handle2: BodyPartHandle<H>,
306    anchor1: &Point<N>,
307    anchor2: &Point<N>,
308    axis: &Unit<Vector<N>>,
309    jacobians: &mut [N],
310) -> Option<GenericNonlinearConstraint<N, H>> {
311    let mut depth = axis.dot(&(anchor2 - anchor1));
312
313    let force = if depth < N::zero() {
314        depth = -depth;
315        ForceDirection::Linear(-*axis)
316    } else {
317        ForceDirection::Linear(*axis)
318    };
319
320    if depth > parameters.allowed_linear_error {
321        let mut j_id = 0;
322        let mut ground_j_id = 0;
323
324        let geom = constraint_pair_geometry(
325            body1,
326            part1,
327            handle1,
328            body2,
329            part2,
330            handle2,
331            anchor1,
332            anchor2,
333            &force,
334            &mut ground_j_id,
335            &mut j_id,
336            jacobians,
337            None,
338            None,
339            None,
340        );
341
342        let rhs = -depth;
343        let constraint = GenericNonlinearConstraint::new(
344            handle1,
345            Some(handle2),
346            false,
347            geom.ndofs1,
348            geom.ndofs2,
349            geom.wj_id1,
350            geom.wj_id2,
351            rhs,
352            geom.r,
353        );
354
355        Some(constraint)
356    } else {
357        None
358    }
359}
360
361/// Generate position constraints to cancel the relative translation of two bodies.
362///
363/// All inputs mut be given in world-space.
364pub fn cancel_relative_translation<N: RealField + Copy, B: ?Sized + Body<N>, H: BodyHandle>(
365    parameters: &IntegrationParameters<N>,
366    body1: &B,
367    part1: &dyn BodyPart<N>,
368    handle1: BodyPartHandle<H>,
369    body2: &B,
370    part2: &dyn BodyPart<N>,
371    handle2: BodyPartHandle<H>,
372    anchor1: &Point<N>,
373    anchor2: &Point<N>,
374    jacobians: &mut [N],
375) -> Option<GenericNonlinearConstraint<N, H>> {
376    let error = anchor2 - anchor1;
377
378    if let Some((dir, depth)) = Unit::try_new_and_get(error, parameters.allowed_linear_error) {
379        let mut j_id = 0;
380        let mut ground_j_id = 0;
381
382        let geom = constraint_pair_geometry(
383            body1,
384            part1,
385            handle1,
386            body2,
387            part2,
388            handle2,
389            anchor1,
390            anchor2,
391            &ForceDirection::Linear(dir),
392            &mut ground_j_id,
393            &mut j_id,
394            jacobians,
395            None,
396            None,
397            None,
398        );
399
400        let rhs = -depth;
401        let constraint = GenericNonlinearConstraint::new(
402            handle1,
403            Some(handle2),
404            false,
405            geom.ndofs1,
406            geom.ndofs2,
407            geom.wj_id1,
408            geom.wj_id2,
409            rhs,
410            geom.r,
411        );
412
413        Some(constraint)
414    } else {
415        None
416    }
417}
418
419/// Generate velocity constraints to cancel the relative angular velocity of two bodies wrt. the given axis.
420///
421/// All inputs mut be given in world-space.
422pub fn cancel_relative_angular_velocity_wrt_axis<
423    N: RealField + Copy,
424    B: ?Sized + Body<N>,
425    H: BodyHandle,
426    Id,
427>(
428    body1: &B,
429    part1: &dyn BodyPart<N>,
430    handle1: BodyPartHandle<H>,
431    body2: &B,
432    part2: &dyn BodyPart<N>,
433    handle2: BodyPartHandle<H>,
434    assembly_id1: usize,
435    assembly_id2: usize,
436    anchor1: &Point<N>,
437    anchor2: &Point<N>,
438    axis: &Unit<AngularVector<N>>,
439    ext_vels: &DVector<N>,
440    impulse: N,
441    impulse_id: Id,
442    ground_j_id: &mut usize,
443    j_id: &mut usize,
444    jacobians: &mut [N],
445    constraints: &mut LinearConstraints<N, Id>,
446) {
447    let limits = ImpulseLimits::Independent {
448        min: -N::max_value(),
449        max: N::max_value(),
450    };
451
452    let (ext_vels1, ext_vels2) = split_ext_vels(body1, body2, assembly_id1, assembly_id2, ext_vels);
453    let force = ForceDirection::Angular(*axis);
454    let mut rhs = N::zero();
455
456    let geom = constraint_pair_geometry(
457        body1,
458        part1,
459        handle1,
460        body2,
461        part2,
462        handle2,
463        anchor1,
464        anchor2,
465        &force,
466        ground_j_id,
467        j_id,
468        jacobians,
469        Some(&ext_vels1),
470        Some(&ext_vels2),
471        Some(&mut rhs),
472    );
473
474    if geom.ndofs1 == 0 || geom.ndofs2 == 0 {
475        constraints
476            .bilateral_ground
477            .push(BilateralGroundConstraint::new(
478                geom,
479                assembly_id1,
480                assembly_id2,
481                limits,
482                rhs,
483                impulse,
484                impulse_id,
485            ));
486    } else {
487        constraints.bilateral.push(BilateralConstraint::new(
488            geom,
489            assembly_id1,
490            assembly_id2,
491            limits,
492            rhs,
493            impulse,
494            impulse_id,
495        ));
496    }
497}
498
499/// Generate velocity constraints to cancel the relative angular velocity of two bodies.
500///
501/// All inputs mut be given in world-space.
502pub fn cancel_relative_angular_velocity<N: RealField + Copy, B: ?Sized + Body<N>, H: BodyHandle>(
503    body1: &B,
504    part1: &dyn BodyPart<N>,
505    handle1: BodyPartHandle<H>,
506    body2: &B,
507    part2: &dyn BodyPart<N>,
508    handle2: BodyPartHandle<H>,
509    assembly_id1: usize,
510    assembly_id2: usize,
511    anchor1: &Point<N>,
512    anchor2: &Point<N>,
513    ext_vels: &DVector<N>,
514    impulses: &AngularVector<N>,
515    impulse_id: usize,
516    ground_j_id: &mut usize,
517    j_id: &mut usize,
518    jacobians: &mut [N],
519    constraints: &mut LinearConstraints<N, usize>,
520) {
521    #[cfg(feature = "dim2")]
522    let canonical_basis = [AngularVector::x()];
523    #[cfg(feature = "dim3")]
524    let canonical_basis = [Vector::x(), Vector::y(), Vector::z()];
525
526    for (i, dir) in canonical_basis.iter().enumerate() {
527        cancel_relative_angular_velocity_wrt_axis(
528            body1,
529            part1,
530            handle1,
531            body2,
532            part2,
533            handle2,
534            assembly_id1,
535            assembly_id2,
536            anchor1,
537            anchor2,
538            &Unit::new_unchecked(*dir),
539            ext_vels,
540            impulses[i],
541            impulse_id + i,
542            ground_j_id,
543            j_id,
544            jacobians,
545            constraints,
546        );
547    }
548}
549
550/// Generate position constraints to cancel the relative rotation of two bodies.
551///
552/// All inputs mut be given in world-space.
553pub fn cancel_relative_rotation<N: RealField + Copy, B: ?Sized + Body<N>, H: BodyHandle>(
554    parameters: &IntegrationParameters<N>,
555    body1: &B,
556    part1: &dyn BodyPart<N>,
557    handle1: BodyPartHandle<H>,
558    body2: &B,
559    part2: &dyn BodyPart<N>,
560    handle2: BodyPartHandle<H>,
561    anchor1: &Point<N>,
562    anchor2: &Point<N>,
563    rotation1: &Rotation<N>,
564    rotation2: &Rotation<N>,
565    jacobians: &mut [N],
566) -> Option<GenericNonlinearConstraint<N, H>> {
567    let error = (rotation2 / rotation1).scaled_axis();
568
569    if let Some((dir, depth)) = Unit::try_new_and_get(error, parameters.allowed_angular_error) {
570        let mut j_id = 0;
571        let mut ground_j_id = 0;
572
573        let geom = constraint_pair_geometry(
574            body1,
575            part1,
576            handle1,
577            body2,
578            part2,
579            handle2,
580            anchor1,
581            anchor2,
582            &ForceDirection::Angular(dir),
583            &mut ground_j_id,
584            &mut j_id,
585            jacobians,
586            None,
587            None,
588            None,
589        );
590
591        let rhs = -depth;
592        let constraint = GenericNonlinearConstraint::new(
593            handle1,
594            Some(handle2),
595            true,
596            geom.ndofs1,
597            geom.ndofs2,
598            geom.wj_id1,
599            geom.wj_id2,
600            rhs,
601            geom.r,
602        );
603
604        Some(constraint)
605    } else {
606        None
607    }
608}
609
610/// Generate velocity constraints to cancel the relative angular velocity of two bodies along all axis except the one provided.
611///
612/// All inputs mut be given in world-space.
613#[cfg(feature = "dim3")]
614pub fn restrict_relative_angular_velocity_to_axis<
615    N: RealField + Copy,
616    B: ?Sized + Body<N>,
617    H: BodyHandle,
618>(
619    body1: &B,
620    part1: &dyn BodyPart<N>,
621    handle1: BodyPartHandle<H>,
622    body2: &B,
623    part2: &dyn BodyPart<N>,
624    handle2: BodyPartHandle<H>,
625    assembly_id1: usize,
626    assembly_id2: usize,
627    axis: &Unit<AngularVector<N>>,
628    anchor1: &Point<N>,
629    anchor2: &Point<N>,
630    ext_vels: &DVector<N>,
631    impulses: &[N],
632    impulse_id: usize,
633    ground_j_id: &mut usize,
634    j_id: &mut usize,
635    jacobians: &mut [N],
636    constraints: &mut LinearConstraints<N, usize>,
637) {
638    let limits = ImpulseLimits::Independent {
639        min: -N::max_value(),
640        max: N::max_value(),
641    };
642
643    let (ext_vels1, ext_vels2) = split_ext_vels(body1, body2, assembly_id1, assembly_id2, ext_vels);
644
645    let mut i = 0;
646    AngularVector::orthonormal_subspace_basis(&[axis.into_inner()], |dir| {
647        let dir = ForceDirection::Angular(Unit::new_unchecked(*dir));
648        let mut rhs = N::zero();
649        let geom = constraint_pair_geometry(
650            body1,
651            part1,
652            handle1,
653            body2,
654            part2,
655            handle2,
656            anchor1,
657            anchor2,
658            &dir,
659            ground_j_id,
660            j_id,
661            jacobians,
662            Some(&ext_vels1),
663            Some(&ext_vels2),
664            Some(&mut rhs),
665        );
666
667        if geom.ndofs1 == 0 || geom.ndofs2 == 0 {
668            constraints
669                .bilateral_ground
670                .push(BilateralGroundConstraint::new(
671                    geom,
672                    assembly_id1,
673                    assembly_id2,
674                    limits,
675                    rhs,
676                    impulses[i],
677                    impulse_id + i,
678                ));
679        } else {
680            constraints.bilateral.push(BilateralConstraint::new(
681                geom,
682                assembly_id1,
683                assembly_id2,
684                limits,
685                rhs,
686                impulses[i],
687                impulse_id + i,
688            ));
689        }
690
691        i += 1;
692
693        true
694    });
695}
696
697/// Generate position constraints to moved the body parts such that the given axis will become aligned.
698///
699/// All inputs mut be given in world-space.
700#[cfg(feature = "dim3")]
701pub fn align_axis<N: RealField + Copy, B: ?Sized + Body<N>, H: BodyHandle>(
702    parameters: &IntegrationParameters<N>,
703    body1: &B,
704    part1: &dyn BodyPart<N>,
705    handle1: BodyPartHandle<H>,
706    body2: &B,
707    part2: &dyn BodyPart<N>,
708    handle2: BodyPartHandle<H>,
709    anchor1: &Point<N>,
710    anchor2: &Point<N>,
711    axis1: &Unit<Vector<N>>,
712    axis2: &Unit<Vector<N>>,
713    jacobians: &mut [N],
714) -> Option<GenericNonlinearConstraint<N, H>> {
715    // Angular regularization for two coincident axis.
716    let mut error;
717    if let Some(error_rot) = Rotation::rotation_between_axis(&axis1, &axis2) {
718        error = error_rot.scaled_axis();
719    } else {
720        // Error equal to Pi, select one orthogonal direction.
721        let imin = axis1.iamin();
722        error = Vector::zeros();
723        error[imin] = N::one();
724        error = error.cross(&axis1).normalize() * N::pi();
725    }
726
727    if let Some((dir, depth)) = Unit::try_new_and_get(error, parameters.allowed_angular_error) {
728        let mut j_id = 0;
729        let mut ground_j_id = 0;
730
731        let geom = constraint_pair_geometry(
732            body1,
733            part1,
734            handle1,
735            body2,
736            part2,
737            handle2,
738            anchor1,
739            anchor2,
740            &ForceDirection::Angular(dir),
741            &mut ground_j_id,
742            &mut j_id,
743            jacobians,
744            None,
745            None,
746            None,
747        );
748
749        let rhs = -depth;
750        let constraint = GenericNonlinearConstraint::new(
751            handle1,
752            Some(handle2),
753            true,
754            geom.ndofs1,
755            geom.ndofs2,
756            geom.wj_id1,
757            geom.wj_id2,
758            rhs,
759            geom.r,
760        );
761
762        Some(constraint)
763    } else {
764        None
765    }
766}
767
768/// Generate velocity constraints to cancel the relative linear velocity of two bodies along all axis except the one provided.
769///
770/// All inputs mut be given in world-space.
771pub fn restrict_relative_linear_velocity_to_axis<
772    N: RealField + Copy,
773    B: ?Sized + Body<N>,
774    H: BodyHandle,
775>(
776    body1: &B,
777    part1: &dyn BodyPart<N>,
778    handle1: BodyPartHandle<H>,
779    body2: &B,
780    part2: &dyn BodyPart<N>,
781    handle2: BodyPartHandle<H>,
782    assembly_id1: usize,
783    assembly_id2: usize,
784    anchor1: &Point<N>,
785    anchor2: &Point<N>,
786    axis1: &Unit<Vector<N>>,
787    ext_vels: &DVector<N>,
788    impulses: &[N],
789    impulse_id: usize,
790    ground_j_id: &mut usize,
791    j_id: &mut usize,
792    jacobians: &mut [N],
793    constraints: &mut LinearConstraints<N, usize>,
794) {
795    let limits = ImpulseLimits::Independent {
796        min: -N::max_value(),
797        max: N::max_value(),
798    };
799
800    let (ext_vels1, ext_vels2) = split_ext_vels(body1, body2, assembly_id1, assembly_id2, ext_vels);
801
802    let mut i = 0;
803    Vector::orthonormal_subspace_basis(&[axis1.into_inner()], |dir| {
804        let dir = ForceDirection::Linear(Unit::new_unchecked(*dir));
805        let mut rhs = N::zero();
806
807        let geom = constraint_pair_geometry(
808            body1,
809            part1,
810            handle1,
811            body2,
812            part2,
813            handle2,
814            anchor1,
815            anchor2,
816            &dir,
817            ground_j_id,
818            j_id,
819            jacobians,
820            Some(&ext_vels1),
821            Some(&ext_vels2),
822            Some(&mut rhs),
823        );
824
825        if geom.ndofs1 == 0 || geom.ndofs2 == 0 {
826            constraints
827                .bilateral_ground
828                .push(BilateralGroundConstraint::new(
829                    geom,
830                    assembly_id1,
831                    assembly_id2,
832                    limits,
833                    rhs,
834                    impulses[i],
835                    impulse_id + i,
836                ));
837        } else {
838            constraints.bilateral.push(BilateralConstraint::new(
839                geom,
840                assembly_id1,
841                assembly_id2,
842                limits,
843                rhs,
844                impulses[i],
845                impulse_id + i,
846            ));
847        }
848
849        i += 1;
850
851        true
852    });
853}
854
855/// Generate position constraints to project `anchor2` into the axis with direction `axis1` and passing through the `anchor1`.
856///
857/// All inputs mut be given in world-space.
858pub fn project_anchor_to_axis<N: RealField + Copy, B: ?Sized + Body<N>, H: BodyHandle>(
859    parameters: &IntegrationParameters<N>,
860    body1: &B,
861    part1: &dyn BodyPart<N>,
862    handle1: BodyPartHandle<H>,
863    body2: &B,
864    part2: &dyn BodyPart<N>,
865    handle2: BodyPartHandle<H>,
866    anchor1: &Point<N>,
867    anchor2: &Point<N>,
868    axis1: &Unit<Vector<N>>,
869    jacobians: &mut [N],
870) -> Option<GenericNonlinearConstraint<N, H>> {
871    // Linear regularization of a point on an axis.
872    let dpt = anchor2 - anchor1;
873    let proj = anchor1 + axis1.into_inner() * axis1.dot(&dpt);
874    let error = anchor2 - proj;
875
876    if let Some((dir, depth)) = Unit::try_new_and_get(error, parameters.allowed_linear_error) {
877        let mut j_id = 0;
878        let mut ground_j_id = 0;
879
880        let geom = constraint_pair_geometry(
881            body1,
882            part1,
883            handle1,
884            body2,
885            part2,
886            handle2,
887            anchor1,
888            anchor2,
889            &ForceDirection::Linear(dir),
890            &mut ground_j_id,
891            &mut j_id,
892            jacobians,
893            None,
894            None,
895            None,
896        );
897
898        let rhs = -depth;
899        let constraint = GenericNonlinearConstraint::new(
900            handle1,
901            Some(handle2),
902            false,
903            geom.ndofs1,
904            geom.ndofs2,
905            geom.wj_id1,
906            geom.wj_id2,
907            rhs,
908            geom.r,
909        );
910
911        Some(constraint)
912    } else {
913        None
914    }
915}
916
917/// Generate position constraints to ensure the two given axis are seperated by the given angle.
918///
919/// All inputs mut be given in world-space.
920#[cfg(feature = "dim3")]
921pub fn restore_angle_between_axis<N: RealField + Copy, B: ?Sized + Body<N>, H: BodyHandle>(
922    parameters: &IntegrationParameters<N>,
923    body1: &B,
924    part1: &dyn BodyPart<N>,
925    handle1: BodyPartHandle<H>,
926    body2: &B,
927    part2: &dyn BodyPart<N>,
928    handle2: BodyPartHandle<H>,
929    anchor1: &Point<N>,
930    anchor2: &Point<N>,
931    axis1: &Unit<Vector<N>>,
932    axis2: &Unit<Vector<N>>,
933    angle: N,
934    jacobians: &mut [N],
935) -> Option<GenericNonlinearConstraint<N, H>> {
936    // Angular regularization for two coincident axis.
937    let mut separation;
938    if let Some(separation_rot) = Rotation::rotation_between_axis(&axis1, &axis2) {
939        separation = separation_rot.scaled_axis();
940    } else {
941        // Separation equal to Pi, select one orthogonal direction.
942        let imin = axis1.iamin();
943        separation = Vector::zeros();
944        separation[imin] = N::one();
945        separation = separation.cross(&axis1).normalize() * N::pi();
946    }
947
948    if let Some((mut dir, curr_ang)) = Unit::try_new_and_get(separation, N::default_epsilon()) {
949        let mut error = curr_ang - angle;
950
951        if error < N::zero() {
952            error = -error;
953            dir = -dir;
954        }
955
956        if error < parameters.allowed_angular_error {
957            return None;
958        }
959
960        let mut j_id = 0;
961        let mut ground_j_id = 0;
962
963        let geom = constraint_pair_geometry(
964            body1,
965            part1,
966            handle1,
967            body2,
968            part2,
969            handle2,
970            anchor1,
971            anchor2,
972            &ForceDirection::Angular(dir),
973            &mut ground_j_id,
974            &mut j_id,
975            jacobians,
976            None,
977            None,
978            None,
979        );
980
981        let rhs = -error;
982        let constraint = GenericNonlinearConstraint::new(
983            handle1,
984            Some(handle2),
985            true,
986            geom.ndofs1,
987            geom.ndofs2,
988            geom.wj_id1,
989            geom.wj_id2,
990            rhs,
991            geom.r,
992        );
993
994        Some(constraint)
995    } else {
996        None
997    }
998}
nphysics2d/solver/helper.rs

nphysics2d/solver/
helper.rs
