nphysics2d/solver/constraint.rs
1use na::RealField;
2
3/// Logical information of the geometry of a constraint.
4#[derive(Copy, Clone, Debug, Default)]
5pub struct ConstraintGeometry<N: RealField + Copy> {
6 /// Index of the first entry of the jacobian of the constraint affecting the first body.
7 pub j_id1: usize,
8 /// Index of the first entry of the jacobian of the constraint affecting the second body.
9 pub j_id2: usize,
10 /// Index of the first entry of the constraint jacobian multiplied by the inverse mass of the first body.
11 pub wj_id1: usize,
12 /// Index of the first entry of the constraint jacobian multiplied by the inverse mass of the second body.
13 pub wj_id2: usize,
14 /// Number of degree of freedom of the first body.
15 pub ndofs1: usize,
16 /// Number of degree of freedom of the second body.
17 pub ndofs2: usize,
18 /// Scaling parameter of the SOR-prox method.
19 pub r: N,
20}
21
22impl<N: RealField + Copy> ConstraintGeometry<N> {
23 /// Create a costraint geometry initialized to zero.
24 #[inline]
25 pub fn new() -> Self {
26 ConstraintGeometry {
27 j_id1: 0,
28 j_id2: 0,
29 wj_id1: 0,
30 wj_id2: 0,
31 ndofs1: 0,
32 ndofs2: 0,
33 r: N::zero(),
34 }
35 }
36
37 /// Return `true` if this constraint involve a body with zero degrees of freedom.
38 #[inline]
39 pub fn is_ground_constraint(&self) -> bool {
40 self.ndofs1 == 0 || self.ndofs2 == 0
41 }
42}
43
44/// A unilateral (inequality) consraint.
45pub struct UnilateralConstraint<N: RealField + Copy, Id> {
46 /// The impulse applied by this constraint.
47 pub impulse: N,
48
49 /// The scaling parameter of the SOR-prox method.
50 pub r: N,
51 /// The target velocity change this constraint must apply.
52 pub rhs: N,
53
54 /// The index of the impulse used for its storage in an impuse cache.
55 pub impulse_id: Id,
56
57 /// The assembly index of the first body.
58 pub assembly_id1: usize,
59 /// The assembly index of the second body.
60 pub assembly_id2: usize,
61
62 /// Index of the first entry of the jacobian of the constraint affecting the first body.
63 pub j_id1: usize,
64 /// Index of the first entry of the constraint jacobian multiplied by the inverse mass of the second body.
65 pub j_id2: usize,
66
67 /// Index of the first entry of the constraint jacobian multiplied by the inverse mass of the first body.
68 pub wj_id1: usize,
69 /// Index of the first entry of the constraint jacobian multiplied by the inverse mass of the second body.
70 pub wj_id2: usize,
71
72 /// Number of degree of freedom of the first body.
73 pub ndofs1: usize,
74 /// Number of degree of freedom of the second body.
75 pub ndofs2: usize,
76}
77
78impl<N: RealField + Copy, Id> UnilateralConstraint<N, Id> {
79 /// Create a new unilateral constraint.
80 #[inline]
81 pub fn new(
82 geom: ConstraintGeometry<N>,
83 assembly_id1: usize,
84 assembly_id2: usize,
85 rhs: N,
86 impulse: N,
87 impulse_id: Id,
88 ) -> Self {
89 assert!(geom.ndofs1 != 0 && geom.ndofs2 != 0);
90 UnilateralConstraint {
91 impulse,
92 r: geom.r,
93 rhs,
94 impulse_id,
95 assembly_id1,
96 assembly_id2,
97 j_id1: geom.j_id1,
98 j_id2: geom.j_id2,
99 wj_id1: geom.wj_id1,
100 wj_id2: geom.wj_id2,
101 ndofs1: geom.ndofs1,
102 ndofs2: geom.ndofs2,
103 }
104 }
105}
106
107/// A unilateral (inequality) constraint between a dynamic body and one without any degrees of freedom.
108pub struct UnilateralGroundConstraint<N: RealField + Copy, Id> {
109 /// The impulse applied by the constraint.
110 pub impulse: N,
111
112 /// The scaling parameter used by the SOR-prox method.
113 pub r: N,
114 /// The target velocity change this constraint must apply.
115 pub rhs: N,
116
117 /// The index of the impulse used for its storage in an impuse cache.
118 pub impulse_id: Id,
119 /// The assembly index of the dynamic body.
120 pub assembly_id: usize,
121 /// Index of the first entry of the jacobian of the constraint affecting the dynamic body.
122 pub j_id: usize,
123 /// Index of the first entry of the constraint jacobian multiplied by the inverse mass of the dynamic body.
124 pub wj_id: usize,
125 /// Number of degree of freedom of the dynamic body.
126 pub ndofs: usize,
127}
128
129impl<N: RealField + Copy, Id> UnilateralGroundConstraint<N, Id> {
130 /// Create a new unilateral ground constraint.
131 #[inline]
132 pub fn new(
133 geom: ConstraintGeometry<N>,
134 assembly_id1: usize,
135 assembly_id2: usize,
136 rhs: N,
137 impulse: N,
138 impulse_id: Id,
139 ) -> Self {
140 if geom.ndofs1 == 0 {
141 UnilateralGroundConstraint {
142 impulse,
143 r: geom.r,
144 rhs,
145 impulse_id,
146 assembly_id: assembly_id2,
147 j_id: geom.j_id2,
148 wj_id: geom.wj_id2,
149 ndofs: geom.ndofs2,
150 }
151 } else {
152 UnilateralGroundConstraint {
153 impulse,
154 r: geom.r,
155 rhs,
156 impulse_id,
157 assembly_id: assembly_id1,
158 j_id: geom.j_id1,
159 wj_id: geom.wj_id1,
160 ndofs: geom.ndofs1,
161 }
162 }
163 }
164}
165
166/// Limits of impulse applicable by a bilateral constraint.
167#[derive(Copy, Clone, Debug)]
168pub enum ImpulseLimits<N: RealField + Copy> {
169 /// Limits that are absolute threshold.
170 Independent {
171 /// The lower bound of the impulse.
172 min: N,
173 /// The upper bound of the impulse.
174 max: N,
175 },
176 /// Limit proportional to the impulse of another unilateral constraint.
177 Dependent {
178 /// Index of the unilateral constraint which this limit depends on.
179 dependency: usize,
180 /// The coefficient by which the dependent impulse is multiplied to obtain the impulse limit.
181 coeff: N,
182 },
183}
184
185/// A bilateral (equality) constraint between two bodies.
186pub struct BilateralConstraint<N: RealField + Copy, Id> {
187 /// The impulse applied by this constraint.
188 pub impulse: N,
189
190 /// The scaling parameter of the SOR-prox method.
191 pub r: N,
192 /// The target velocity change this constraint must apply.
193 pub rhs: N,
194
195 /// Limits of impulse applicable by this constraint.
196 pub limits: ImpulseLimits<N>,
197
198 /// The index of the impulse used for its storage in an impuse cache.
199 pub impulse_id: Id,
200
201 /// The assembly index of the first body.
202 pub assembly_id1: usize,
203 /// The assembly index of the second body.
204 pub assembly_id2: usize,
205
206 /// Index of the first entry of the jacobian of the constraint affecting the first body.
207 pub j_id1: usize,
208 /// Index of the first entry of the constraint jacobian multiplied by the inverse mass of the second body.
209 pub j_id2: usize,
210
211 /// Index of the first entry of the constraint jacobian multiplied by the inverse mass of the first body.
212 pub wj_id1: usize,
213 /// Index of the first entry of the constraint jacobian multiplied by the inverse mass of the second body.
214 pub wj_id2: usize,
215
216 /// Number of degree of freedom of the first body.
217 pub ndofs1: usize,
218 /// Number of degree of freedom of the second body.
219 pub ndofs2: usize,
220}
221
222impl<N: RealField + Copy, Id> BilateralConstraint<N, Id> {
223 /// Create a new bilateral constraint.
224 #[inline]
225 pub fn new(
226 geom: ConstraintGeometry<N>,
227 assembly_id1: usize,
228 assembly_id2: usize,
229 limits: ImpulseLimits<N>,
230 rhs: N,
231 impulse: N,
232 impulse_id: Id,
233 ) -> Self {
234 assert!(geom.ndofs1 != 0 && geom.ndofs2 != 0);
235 BilateralConstraint {
236 impulse,
237 r: geom.r,
238 rhs,
239 limits,
240 impulse_id,
241 assembly_id1,
242 assembly_id2,
243 j_id1: geom.j_id1,
244 j_id2: geom.j_id2,
245 wj_id1: geom.wj_id1,
246 wj_id2: geom.wj_id2,
247 ndofs1: geom.ndofs1,
248 ndofs2: geom.ndofs2,
249 }
250 }
251}
252
253/// A bilateral (equality) constraint between a dynamic body and one without any degrees of freedom.
254pub struct BilateralGroundConstraint<N: RealField + Copy, Id> {
255 /// The impulse applied by the constraint.
256 pub impulse: N,
257
258 /// The scaling parameter used by the SOR-prox method.
259 pub r: N,
260 /// The target velocity change this constraint must apply.
261 pub rhs: N,
262
263 /// Limits of impulse applicable by this constraint.
264 pub limits: ImpulseLimits<N>,
265
266 /// The index of the impulse used for its storage in an impuse cache.
267 pub impulse_id: Id,
268 /// The assembly index of the dynamic body.
269 pub assembly_id: usize,
270 /// Index of the first entry of the jacobian of the constraint affecting the dynamic body.
271 pub j_id: usize,
272 /// Index of the first entry of the constraint jacobian multiplied by the inverse mass of the dynamic body.
273 pub wj_id: usize,
274 /// Number of degree of freedom of the dynamic body.
275 pub ndofs: usize,
276}
277
278impl<N: RealField + Copy, Id> BilateralGroundConstraint<N, Id> {
279 /// Create a new unilateral ground constraint.
280 #[inline]
281 pub fn new(
282 geom: ConstraintGeometry<N>,
283 assembly_id1: usize,
284 assembly_id2: usize,
285 limits: ImpulseLimits<N>,
286 rhs: N,
287 impulse: N,
288 impulse_id: Id,
289 ) -> Self {
290 if geom.ndofs1 == 0 {
291 BilateralGroundConstraint {
292 impulse,
293 r: geom.r,
294 rhs,
295 limits,
296 impulse_id,
297 assembly_id: assembly_id2,
298 j_id: geom.j_id2,
299 wj_id: geom.wj_id2,
300 ndofs: geom.ndofs2,
301 }
302 } else {
303 BilateralGroundConstraint {
304 impulse,
305 r: geom.r,
306 rhs,
307 limits,
308 impulse_id,
309 assembly_id: assembly_id1,
310 j_id: geom.j_id1,
311 wj_id: geom.wj_id1,
312 ndofs: geom.ndofs1,
313 }
314 }
315 }
316}