nphysics2d/solver/
nonlinear_constraint.rs

1use na::{RealField, Unit};
2use ncollide::query::ContactKinematic;
3
4use crate::math::Vector;
5use crate::object::{BodyHandle, BodyPartHandle, BodySet, ColliderHandle};
6use crate::solver::IntegrationParameters;
7
8/// A generic non-linear position constraint.
9pub struct GenericNonlinearConstraint<N: RealField + Copy, Handle: BodyHandle> {
10    /// The first body affected by the constraint.
11    pub body1: BodyPartHandle<Handle>,
12    /// The second body affected by the constraint.
13    pub body2: Option<BodyPartHandle<Handle>>,
14    /// Whether this constraint affects the bodies translation or orientation.
15    pub is_angular: bool,
16    //  FIXME: rename ndofs1?
17    /// Number of degree of freedom of the first body.
18    pub dim1: usize,
19    /// Number of degree of freedom of the second body.
20    pub dim2: usize,
21    /// Index of the first entry of the constraint jacobian multiplied by the inverse mass of the first body.
22    pub wj_id1: usize,
23    /// Index of the first entry of the constraint jacobian multiplied by the inverse mass of the second body.
24    pub wj_id2: usize,
25    /// The target position change this constraint must apply.
26    pub rhs: N,
27    /// The scaling parameter of the SOR-prox method.
28    pub r: N,
29}
30
31impl<N: RealField + Copy, Handle: BodyHandle> GenericNonlinearConstraint<N, Handle> {
32    /// Initialize a new nonlinear constraint.
33    pub fn new(
34        body1: BodyPartHandle<Handle>,
35        body2: Option<BodyPartHandle<Handle>>,
36        is_angular: bool,
37        dim1: usize,
38        dim2: usize,
39        wj_id1: usize,
40        wj_id2: usize,
41        rhs: N,
42        r: N,
43    ) -> Self {
44        GenericNonlinearConstraint {
45            body1,
46            body2,
47            is_angular,
48            dim1,
49            dim2,
50            wj_id1,
51            wj_id2,
52            rhs,
53            r,
54        }
55    }
56}
57
58/// Implemented by structures that generate non-linear constraints.
59pub trait NonlinearConstraintGenerator<N: RealField + Copy, Handle: BodyHandle> {
60    /// Maximum of non-linear position constraint this generator needs to output.
61    fn num_position_constraints(&self, bodies: &dyn BodySet<N, Handle = Handle>) -> usize;
62    /// Generate the `i`-th position constraint of this generator.
63    fn position_constraint(
64        &self,
65        parameters: &IntegrationParameters<N>,
66        i: usize,
67        bodies: &mut dyn BodySet<N, Handle = Handle>,
68        jacobians: &mut [N],
69    ) -> Option<GenericNonlinearConstraint<N, Handle>>;
70}
71
72/// A non-linear position-based non-penetration constraint.
73#[derive(Debug)]
74pub struct NonlinearUnilateralConstraint<
75    N: RealField + Copy,
76    Handle: BodyHandle,
77    CollHandle: ColliderHandle,
78> {
79    /// The scaling parameter of the SOR-prox method.
80    pub r: N,
81    /// The target position change this constraint must apply.
82    pub rhs: N,
83
84    /// Number of degree of freedom of the first body.
85    pub ndofs1: usize,
86    /// The first body affected by the constraint.
87    pub body1: BodyPartHandle<Handle>,
88    /// The first collider affected by the constraint.
89    pub collider1: CollHandle,
90
91    /// Number of degree of freedom of the second body.
92    pub ndofs2: usize,
93    /// The second body affected by the constraint.
94    pub body2: BodyPartHandle<Handle>,
95    /// The second collider affected by the constraint.
96    pub collider2: CollHandle,
97
98    /// The kinematic information used to update the contact location.
99    pub kinematic: ContactKinematic<N>,
100
101    /// The contact normal on the local space of `self.body1`.
102    pub normal1: Unit<Vector<N>>,
103    /// The contact normal on the local space of `self.body2`.
104    pub normal2: Unit<Vector<N>>,
105}
106
107impl<N: RealField + Copy, Handle: BodyHandle, CollHandle: ColliderHandle>
108    NonlinearUnilateralConstraint<N, Handle, CollHandle>
109{
110    /// Create a new nonlinear position-based non-penetration constraint.
111    pub fn new(
112        body1: BodyPartHandle<Handle>,
113        collider1: CollHandle,
114        ndofs1: usize,
115        body2: BodyPartHandle<Handle>,
116        collider2: CollHandle,
117        ndofs2: usize,
118        normal1: Unit<Vector<N>>,
119        normal2: Unit<Vector<N>>,
120        kinematic: ContactKinematic<N>,
121    ) -> Self {
122        let r = N::zero();
123        let rhs = N::zero();
124
125        NonlinearUnilateralConstraint {
126            r,
127            rhs,
128            ndofs1,
129            body1,
130            collider1,
131            ndofs2,
132            body2,
133            collider2,
134            kinematic,
135            normal1,
136            normal2,
137        }
138    }
139}
140
141/// A non-linear position constraint generator to enforce multibody joint limits.
142pub struct MultibodyJointLimitsNonlinearConstraintGenerator;
143
144impl MultibodyJointLimitsNonlinearConstraintGenerator {
145    /// Creates the constraint generator from the given multibody link.
146    pub fn new() -> Self {
147        MultibodyJointLimitsNonlinearConstraintGenerator
148    }
149}
150
151impl<N: RealField + Copy, Handle: BodyHandle> NonlinearConstraintGenerator<N, Handle>
152    for MultibodyJointLimitsNonlinearConstraintGenerator
153{
154    fn num_position_constraints(&self, _: &dyn BodySet<N, Handle = Handle>) -> usize {
155        0
156    }
157
158    fn position_constraint(
159        &self,
160        _: &IntegrationParameters<N>,
161        _: usize,
162        _: &mut dyn BodySet<N, Handle = Handle>,
163        _: &mut [N],
164    ) -> Option<GenericNonlinearConstraint<N, Handle>> {
165        None
166    }
167}
nphysics2d/solver/nonlinear_constraint.rs

nphysics2d/solver/
nonlinear_constraint.rs
