1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
use crate::prelude::*;
/// An angular constraint applies an angular correction around a given axis.
pub trait AngularConstraint: XpbdConstraint<2> {
/// Applies angular constraints for interactions between two bodies.
///
/// 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.
///
/// Returns the angular impulse that is applied proportional to the inverse masses of the bodies.
#[cfg(feature = "2d")]
fn apply_angular_correction(
&self,
body1: &mut RigidBodyQueryItem,
body2: &mut RigidBodyQueryItem,
delta_lagrange: Scalar,
axis: Vector3,
) -> Scalar {
if delta_lagrange.abs() <= Scalar::EPSILON {
return 0.0;
}
// Compute angular impulse
// `axis.z` is 1 or -1 and it controls if the body should rotate counterclockwise or clockwise
let p = -delta_lagrange * axis.z;
let rot1 = *body1.rotation;
let rot2 = *body2.rotation;
let inv_inertia1 = body1.effective_world_inv_inertia();
let inv_inertia2 = body2.effective_world_inv_inertia();
// Apply rotational updates
if body1.rb.is_dynamic() && body1.dominance() <= body2.dominance() {
*body1.rotation += Self::get_delta_rot(rot1, inv_inertia1, p);
}
if body2.rb.is_dynamic() && body2.dominance() <= body1.dominance() {
*body2.rotation -= Self::get_delta_rot(rot2, inv_inertia2, p);
}
p
}
/// Applies angular constraints for interactions between two bodies.
///
/// Returns the angular impulse that is applied proportional to the inverse masses of the bodies.
#[cfg(feature = "3d")]
fn apply_angular_correction(
&self,
body1: &mut RigidBodyQueryItem,
body2: &mut RigidBodyQueryItem,
delta_lagrange: Scalar,
axis: Vector,
) -> Vector {
if delta_lagrange.abs() <= Scalar::EPSILON {
return Vector::ZERO;
}
// Compute angular impulse
let p = -delta_lagrange * axis;
let rot1 = *body1.rotation;
let rot2 = *body2.rotation;
let inv_inertia1 = body1.effective_world_inv_inertia();
let inv_inertia2 = body2.effective_world_inv_inertia();
// Apply rotational updates
if body1.rb.is_dynamic() {
*body1.rotation += Self::get_delta_rot(rot1, inv_inertia1, p);
}
if body2.rb.is_dynamic() {
*body2.rotation -= Self::get_delta_rot(rot2, inv_inertia2, p);
}
p
}
/// Computes the generalized inverse mass of a body when applying an angular correction
/// around `axis`.
///
/// In 2D, `axis` should only have the z axis set to either -1 or 1 to indicate counterclockwise or
/// clockwise rotation.
#[cfg(feature = "2d")]
fn compute_generalized_inverse_mass(&self, body: &RigidBodyQueryItem, axis: Vector3) -> Scalar {
if body.rb.is_dynamic() {
axis.dot(body.inverse_inertia.0 * axis)
} else {
// Static and kinematic bodies are a special case, where 0.0 can be thought of as infinite mass.
0.0
}
}
/// Computes the generalized inverse mass of a body when applying an angular correction
/// around `axis`.
#[cfg(feature = "3d")]
fn compute_generalized_inverse_mass(&self, body: &RigidBodyQueryItem, axis: Vector) -> Scalar {
if body.rb.is_dynamic() {
axis.dot(body.effective_world_inv_inertia() * axis)
} else {
// Static and kinematic bodies are a special case, where 0.0 can be thought of as infinite mass.
0.0
}
}
/// Computes the update in rotation when applying an angular correction `p`.
#[cfg(feature = "2d")]
fn get_delta_rot(_rot: Rotation, inverse_inertia: Scalar, p: Scalar) -> Rotation {
// Equation 8/9 but in 2D
Rotation::from_radians(inverse_inertia * p)
}
/// Computes the update in rotation when applying an angular correction `p`.
#[cfg(feature = "3d")]
fn get_delta_rot(rot: Rotation, inverse_inertia: Matrix3, p: Vector) -> Rotation {
// Equation 8/9
Rotation(Quaternion::from_vec4(0.5 * (inverse_inertia * p).extend(0.0)) * rot.0)
}
/// Computes the torque acting along the constraint using the equation tau = lambda * n / h^2
fn compute_torque(&self, lagrange: Scalar, axis: Vector3, dt: Scalar) -> Torque {
// Eq (17)
#[cfg(feature = "2d")]
{
lagrange * axis.z / dt.powi(2)
}
#[cfg(feature = "3d")]
{
lagrange * axis / dt.powi(2)
}
}
}