avian3d/dynamics/solver/xpbd/
angular_constraint.rs

1use super::XpbdConstraint;
2use crate::prelude::*;
3
4/// An angular constraint applies an angular correction around a given axis.
5pub trait AngularConstraint: XpbdConstraint<2> {
6    /// Applies an angular correction to two bodies.
7    ///
8    /// Returns the angular impulse that is applied proportional
9    /// to the inverse moments of inertia of the bodies.
10    #[cfg(feature = "2d")]
11    fn apply_angular_lagrange_update(
12        &self,
13        body1: &mut RigidBodyQueryItem,
14        body2: &mut RigidBodyQueryItem,
15        delta_lagrange: Scalar,
16    ) -> Scalar {
17        if delta_lagrange.abs() <= Scalar::EPSILON {
18            return 0.0;
19        }
20
21        self.apply_angular_impulse(body1, body2, -delta_lagrange)
22    }
23
24    /// Applies an angular impulse to two bodies.
25    ///
26    /// Returns the impulse that is applied proportional
27    /// to the inverse moments of inertia of the bodies.
28    #[cfg(feature = "2d")]
29    fn apply_angular_impulse(
30        &self,
31        body1: &mut RigidBodyQueryItem,
32        body2: &mut RigidBodyQueryItem,
33        impulse: Scalar,
34    ) -> Scalar {
35        let inv_inertia1 = body1.effective_global_angular_inertia().inverse();
36        let inv_inertia2 = body2.effective_global_angular_inertia().inverse();
37
38        // Apply rotational updates
39        if body1.rb.is_dynamic() && body1.dominance() <= body2.dominance() {
40            let delta_angle = Self::get_delta_rot(inv_inertia1, impulse);
41            *body1.rotation = body1.rotation.add_angle(delta_angle);
42        }
43        if body2.rb.is_dynamic() && body2.dominance() <= body1.dominance() {
44            let delta_angle = Self::get_delta_rot(inv_inertia2, -impulse);
45            *body2.rotation = body2.rotation.add_angle(delta_angle);
46        }
47
48        impulse
49    }
50
51    /// Applies an angular correction to two bodies.
52    ///
53    /// Returns the angular impulse that is applied proportional
54    /// to the inverse moments of inertia of the bodies.
55    #[cfg(feature = "3d")]
56    fn apply_angular_lagrange_update(
57        &self,
58        body1: &mut RigidBodyQueryItem,
59        body2: &mut RigidBodyQueryItem,
60        delta_lagrange: Scalar,
61        axis: Vector,
62    ) -> Vector {
63        if delta_lagrange.abs() <= Scalar::EPSILON {
64            return Vector::ZERO;
65        }
66
67        let impulse = -delta_lagrange * axis;
68
69        self.apply_angular_impulse(body1, body2, impulse)
70    }
71
72    /// Applies an angular impulse to two bodies.
73    ///
74    /// Returns the impulse that is applied proportional
75    /// to the inverse moments of inertia of the bodies.
76    #[cfg(feature = "3d")]
77    fn apply_angular_impulse(
78        &self,
79        body1: &mut RigidBodyQueryItem,
80        body2: &mut RigidBodyQueryItem,
81        impulse: Vector,
82    ) -> Vector {
83        let inv_inertia1 = body1.effective_global_angular_inertia().inverse();
84        let inv_inertia2 = body2.effective_global_angular_inertia().inverse();
85
86        // Apply rotational updates
87        if body1.rb.is_dynamic() {
88            // In 3D, adding quaternions can result in unnormalized rotations,
89            // which causes stability issues (see #235) and panics when trying to rotate unit vectors.
90            // TODO: It would be nice to avoid normalization if possible.
91            //       Maybe the math above can be done in a way that keeps rotations normalized?
92            let delta_quat = Self::get_delta_rot(inv_inertia1, impulse);
93            body1.rotation.0 = delta_quat * body1.rotation.0;
94            *body1.rotation = body1.rotation.fast_renormalize();
95        }
96        if body2.rb.is_dynamic() {
97            // See comments for `body1` above.
98            let delta_quat = Self::get_delta_rot(inv_inertia2, -impulse);
99            body2.rotation.0 = delta_quat * body2.rotation.0;
100            *body2.rotation = body2.rotation.fast_renormalize();
101        }
102
103        impulse
104    }
105
106    /// Applies an angular correction that aligns the orientation of the bodies.
107    ///
108    /// Returns the torque exerted by the alignment.
109    #[cfg(feature = "2d")]
110    fn align_orientation(
111        &self,
112        body1: &mut RigidBodyQueryItem,
113        body2: &mut RigidBodyQueryItem,
114        angle: Scalar,
115        lagrange: &mut Scalar,
116        compliance: Scalar,
117        dt: Scalar,
118    ) -> Torque {
119        if angle.abs() <= Scalar::EPSILON {
120            return Torque::ZERO;
121        }
122
123        let w1 = body1.effective_global_angular_inertia().inverse();
124        let w2 = body2.effective_global_angular_inertia().inverse();
125        let w = [w1, w2];
126
127        // Compute Lagrange multiplier update
128        let delta_lagrange = self.compute_lagrange_update(*lagrange, angle, &w, compliance, dt);
129        *lagrange += delta_lagrange;
130
131        // Apply angular correction to aling the bodies
132        self.apply_angular_lagrange_update(body1, body2, delta_lagrange);
133
134        // Return constraint torque
135        self.compute_torque(delta_lagrange, dt)
136    }
137
138    /// Applies an angular correction that aligns the orientation of the bodies.
139    ///
140    /// Returns the torque exerted by the alignment.
141    #[cfg(feature = "3d")]
142    fn align_orientation(
143        &self,
144        body1: &mut RigidBodyQueryItem,
145        body2: &mut RigidBodyQueryItem,
146        rotation_difference: Vector,
147        lagrange: &mut Scalar,
148        compliance: Scalar,
149        dt: Scalar,
150    ) -> Torque {
151        let angle = rotation_difference.length();
152
153        if angle <= Scalar::EPSILON {
154            return Torque::ZERO;
155        }
156
157        let axis = rotation_difference / angle;
158
159        // Compute generalized inverse masses
160        let w1 = AngularConstraint::compute_generalized_inverse_mass(self, body1, axis);
161        let w2 = AngularConstraint::compute_generalized_inverse_mass(self, body2, axis);
162        let w = [w1, w2];
163
164        // Compute Lagrange multiplier update
165        let delta_lagrange = self.compute_lagrange_update(*lagrange, angle, &w, compliance, dt);
166        *lagrange += delta_lagrange;
167
168        // Apply angular correction to aling the bodies
169        self.apply_angular_lagrange_update(body1, body2, delta_lagrange, axis);
170
171        // Return constraint torque
172        self.compute_torque(delta_lagrange, axis, dt)
173    }
174
175    /// Applies angular constraints for interactions between two bodies.
176    ///
177    /// Here in 2D, `axis` is a unit vector with the Z coordinate set to 1 or -1. It controls if the body should rotate counterclockwise or clockwise.
178    ///
179    /// Returns the angular impulse that is applied proportional to the inverse masses of the bodies.
180    #[cfg(feature = "2d")]
181    fn apply_angular_correction(
182        &self,
183        body1: &mut RigidBodyQueryItem,
184        body2: &mut RigidBodyQueryItem,
185        delta_lagrange: Scalar,
186        axis: Vector3,
187    ) -> Scalar {
188        if delta_lagrange.abs() <= Scalar::EPSILON {
189            return 0.0;
190        }
191
192        // Compute angular impulse
193        // `axis.z` is 1 or -1 and it controls if the body should rotate counterclockwise or clockwise
194        let p = -delta_lagrange * axis.z;
195
196        let inv_inertia1 = body1.effective_global_angular_inertia().inverse();
197        let inv_inertia2 = body2.effective_global_angular_inertia().inverse();
198
199        // Apply rotational updates
200        if body1.rb.is_dynamic() && body1.dominance() <= body2.dominance() {
201            let delta_angle = Self::get_delta_rot(inv_inertia1, p);
202            *body1.rotation = body1.rotation.add_angle(delta_angle);
203        }
204        if body2.rb.is_dynamic() && body2.dominance() <= body1.dominance() {
205            let delta_angle = Self::get_delta_rot(inv_inertia2, -p);
206            *body2.rotation = body2.rotation.add_angle(delta_angle);
207        }
208
209        p
210    }
211
212    /// Applies angular constraints for interactions between two bodies.
213    ///
214    /// Returns the angular impulse that is applied proportional to the inverse masses of the bodies.
215    #[cfg(feature = "3d")]
216    fn apply_angular_correction(
217        &self,
218        body1: &mut RigidBodyQueryItem,
219        body2: &mut RigidBodyQueryItem,
220        delta_lagrange: Scalar,
221        axis: Vector,
222    ) -> Vector {
223        if delta_lagrange.abs() <= Scalar::EPSILON {
224            return Vector::ZERO;
225        }
226
227        // Compute angular impulse
228        let p = -delta_lagrange * axis;
229
230        let inv_inertia1 = body1.effective_global_angular_inertia().inverse();
231        let inv_inertia2 = body2.effective_global_angular_inertia().inverse();
232
233        // Apply rotational updates
234        if body1.rb.is_dynamic() {
235            // In 3D, adding quaternions can result in unnormalized rotations,
236            // which causes stability issues (see #235) and panics when trying to rotate unit vectors.
237            // TODO: It would be nice to avoid normalization if possible.
238            //       Maybe the math above can be done in a way that keeps rotations normalized?
239            let delta_quat = Self::get_delta_rot(inv_inertia1, p);
240            body1.rotation.0 = delta_quat * body1.rotation.0;
241            *body1.rotation = body1.rotation.fast_renormalize();
242        }
243        if body2.rb.is_dynamic() {
244            // See comments for `body1` above.
245            let delta_quat = Self::get_delta_rot(inv_inertia2, -p);
246            body2.rotation.0 = delta_quat * body2.rotation.0;
247            *body2.rotation = body2.rotation.fast_renormalize();
248        }
249
250        p
251    }
252
253    /// Computes the generalized inverse mass of a body when applying an angular correction
254    /// around `axis`.
255    ///
256    /// In 2D, `axis` should only have the z axis set to either -1 or 1 to indicate counterclockwise or
257    /// clockwise rotation.
258    #[cfg(feature = "2d")]
259    fn compute_generalized_inverse_mass(&self, body: &RigidBodyQueryItem, axis: Vector3) -> Scalar {
260        if body.rb.is_dynamic() {
261            axis.dot(body.angular_inertia.inverse() * axis)
262        } else {
263            // Static and kinematic bodies are a special case, where 0.0 can be thought of as infinite mass.
264            0.0
265        }
266    }
267
268    /// Computes the generalized inverse mass of a body when applying an angular correction
269    /// around `axis`.
270    #[cfg(feature = "3d")]
271    fn compute_generalized_inverse_mass(&self, body: &RigidBodyQueryItem, axis: Vector) -> Scalar {
272        if body.rb.is_dynamic() {
273            axis.dot(body.effective_global_angular_inertia().inverse() * axis)
274        } else {
275            // Static and kinematic bodies are a special case, where 0.0 can be thought of as infinite mass.
276            0.0
277        }
278    }
279
280    /// Computes the update in rotation when applying an angular correction `p`.
281    #[cfg(feature = "2d")]
282    fn get_delta_rot(inverse_inertia: Scalar, p: Scalar) -> Scalar {
283        // Equation 8/9 but in 2D
284        inverse_inertia * p
285    }
286
287    /// Computes the update in rotation when applying an angular correction `p`.
288    #[cfg(feature = "3d")]
289    fn get_delta_rot(inverse_inertia: Matrix3, p: Vector) -> Quaternion {
290        // Equation 8/9
291        Quaternion::from_scaled_axis(inverse_inertia * p)
292    }
293
294    /// Computes the torque acting along the constraint using the equation `tau = lambda * n / h^2`,
295    /// where `n` is the Z axis in 2D.
296    #[cfg(feature = "2d")]
297    fn compute_torque(&self, lagrange: Scalar, dt: Scalar) -> Torque {
298        // Eq (17)
299        lagrange / dt.powi(2)
300    }
301
302    /// Computes the torque acting along the constraint using the equation `tau = lambda * n / h^2`
303    #[cfg(feature = "3d")]
304    fn compute_torque(&self, lagrange: Scalar, axis: Vector, dt: Scalar) -> Torque {
305        // Eq (17)
306        lagrange * axis / dt.powi(2)
307    }
308}
avian3d/dynamics/solver/xpbd/angular_constraint.rs

avian3d/dynamics/solver/xpbd/
angular_constraint.rs
