avian3d/dynamics/solver/xpbd/
positional_constraint.rs

1use super::XpbdConstraint;
2use crate::prelude::*;
3
4/// A positional constraint applies a positional correction
5/// with a given direction and magnitude at the local contact points `r1` and  `r2`.
6pub trait PositionConstraint: XpbdConstraint<2> {
7    /// Applies a positional correction to two bodies.
8    ///
9    /// Returns the positional impulse that is applied proportional to the inverse masses of the bodies.
10    #[allow(clippy::too_many_arguments)]
11    fn apply_positional_lagrange_update(
12        &self,
13        body1: &mut RigidBodyQueryItem,
14        body2: &mut RigidBodyQueryItem,
15        delta_lagrange: Scalar,
16        direction: Vector,
17        r1: Vector,
18        r2: Vector,
19    ) -> Vector {
20        if delta_lagrange.abs() <= Scalar::EPSILON {
21            return Vector::ZERO;
22        }
23
24        let impulse = delta_lagrange * direction;
25
26        self.apply_positional_impulse(body1, body2, impulse, r1, r2)
27    }
28
29    /// Applies a positional impulse to two bodies.
30    ///
31    /// Returns the impulse that is applied proportional to the inverse masses of the bodies.
32    fn apply_positional_impulse(
33        &self,
34        body1: &mut RigidBodyQueryItem,
35        body2: &mut RigidBodyQueryItem,
36        impulse: Vector,
37        r1: Vector,
38        r2: Vector,
39    ) -> Vector {
40        let inv_mass1 = body1.effective_inverse_mass();
41        let inv_mass2 = body2.effective_inverse_mass();
42        let inv_inertia1 = body1.effective_global_angular_inertia().inverse();
43        let inv_inertia2 = body2.effective_global_angular_inertia().inverse();
44
45        // Apply positional and rotational updates
46        if body1.rb.is_dynamic() && body1.dominance() <= body2.dominance() {
47            body1.accumulated_translation.0 += impulse * inv_mass1;
48
49            #[cfg(feature = "2d")]
50            {
51                let delta_angle = Self::get_delta_rot(inv_inertia1, r1, impulse);
52                *body1.rotation = body1.rotation.add_angle(delta_angle);
53            }
54            #[cfg(feature = "3d")]
55            {
56                // In 3D, adding quaternions can result in unnormalized rotations,
57                // which causes stability issues (see #235) and panics when trying to rotate unit vectors.
58                // TODO: It would be nice to avoid normalization if possible.
59                //       Maybe the math above can be done in a way that keeps rotations normalized?
60                let delta_quat = Self::get_delta_rot(inv_inertia1, r1, impulse);
61                body1.rotation.0 = delta_quat * body1.rotation.0;
62                *body1.rotation = body1.rotation.fast_renormalize();
63            }
64        }
65        if body2.rb.is_dynamic() && body2.dominance() <= body1.dominance() {
66            body2.accumulated_translation.0 -= impulse * inv_mass2;
67
68            #[cfg(feature = "2d")]
69            {
70                let delta_angle = Self::get_delta_rot(inv_inertia2, r2, -impulse);
71                *body2.rotation = body2.rotation.add_angle(delta_angle);
72            }
73            #[cfg(feature = "3d")]
74            {
75                // See comments for `body1` above.
76                let delta_quat = Self::get_delta_rot(inv_inertia2, r2, -impulse);
77                body2.rotation.0 = delta_quat * body2.rotation.0;
78                *body2.rotation = body2.rotation.fast_renormalize();
79            }
80        }
81
82        impulse
83    }
84
85    /// Computes the generalized inverse mass of a body when applying a positional correction
86    /// at point `r` along the vector `n`.
87    #[cfg(feature = "2d")]
88    fn compute_generalized_inverse_mass(
89        &self,
90        body: &RigidBodyQueryItem,
91        r: Vector,
92        n: Vector,
93    ) -> Scalar {
94        if body.rb.is_dynamic() {
95            body.mass.inverse() + body.angular_inertia.inverse() * r.perp_dot(n).powi(2)
96        } else {
97            // Static and kinematic bodies are a special case, where 0.0 can be thought of as infinite mass.
98            0.0
99        }
100    }
101
102    /// Computes the generalized inverse mass of a body when applying a positional correction
103    /// at point `r` along the vector `n`.
104    #[cfg(feature = "3d")]
105    fn compute_generalized_inverse_mass(
106        &self,
107        body: &RigidBodyQueryItem,
108        r: Vector,
109        n: Vector,
110    ) -> Scalar {
111        if body.rb.is_dynamic() {
112            let inverse_inertia = body.effective_global_angular_inertia().inverse();
113
114            let r_cross_n = r.cross(n); // Compute the cross product only once
115
116            // The line below is equivalent to Eq (2) because the component-wise multiplication of a transposed vector and another vector is equal to the dot product of the two vectors.
117            // a^T * b = a • b
118            body.mass.inverse() + r_cross_n.dot(inverse_inertia * r_cross_n)
119        } else {
120            // Static and kinematic bodies are a special case, where 0.0 can be thought of as infinite mass.
121            0.0
122        }
123    }
124
125    /// Computes the update in rotation when applying a positional correction `p` at point `r`.
126    #[cfg(feature = "2d")]
127    fn get_delta_rot(inverse_inertia: Scalar, r: Vector, p: Vector) -> Scalar {
128        // Equation 8/9 but in 2D
129        inverse_inertia * r.perp_dot(p)
130    }
131
132    /// Computes the update in rotation when applying a positional correction `p` at point `r`.
133    #[cfg(feature = "3d")]
134    fn get_delta_rot(inverse_inertia: Matrix3, r: Vector, p: Vector) -> Quaternion {
135        // Equation 8/9
136        Quaternion::from_scaled_axis(inverse_inertia * r.cross(p))
137    }
138
139    /// Computes the force acting along the constraint using the equation f = lambda * n / h^2
140    fn compute_force(&self, lagrange: Scalar, direction: Vector, dt: Scalar) -> Vector {
141        lagrange * direction / dt.powi(2)
142    }
143}
avian3d/dynamics/solver/xpbd/positional_constraint.rs

avian3d/dynamics/solver/xpbd/
positional_constraint.rs
