rox2d/rox2d/
joint.rs

1use std::rc::Rc;
2
3use super::{
4    body::Body,
5    math::{Mat2x2, Vec2},
6    world::World,
7};
8
9#[derive(Debug, Clone, Copy)]
10pub struct Joint {
11    /// Mass matrix (M)
12    mass_matrix: Mat2x2,
13
14    local_anchor1: Vec2,
15    local_anchor2: Vec2,
16
17    r1: Vec2,
18    r2: Vec2,
19    bias: Vec2,
20
21    /// Accumulated impulse (P)
22    accumulated_impulse: Vec2,
23    bias_factor: f32,
24    softness: f32,
25}
26
27impl Joint {
28    pub fn new(body1: &mut Body, body2: &mut Body, anchor: Vec2) -> Self {
29        let rot1 = Mat2x2::from_angle(body1.rotation);
30        let rot2 = Mat2x2::from_angle(body2.rotation);
31        let rot1_t = rot1.transpose();
32        let rot2_t = rot2.transpose();
33
34        let local_anchor1 = rot1_t * (anchor - body1.position);
35        let local_anchor2 = rot2_t * (anchor - body2.position);
36
37        Self {
38            mass_matrix: Mat2x2::ZERO,
39            local_anchor1,
40            local_anchor2,
41            r1: Vec2::ZERO,
42            r2: Vec2::ZERO,
43            bias: Vec2::ZERO,
44            accumulated_impulse: Vec2::ZERO,
45            bias_factor: 0.2,
46            softness: 0.0,
47        }
48    }
49
50    pub fn pre_step(
51        &mut self,
52        body1: &mut Body,
53        body2: &mut Body,
54        inv_dt: f32,
55    ) {
56        // Pre-compute anchors, mass matrix, and bias.
57        let rot1 = Mat2x2::from_angle(body1.rotation);
58        let rot2 = Mat2x2::from_angle(body2.rotation);
59
60        self.r1 = rot1 * self.local_anchor1;
61        self.r2 = rot2 * self.local_anchor2;
62
63        // deltaV = deltaV0 + K * impulse
64        // invM = [(1/m1 + 1/m2) * eye(2)
65        //         - skew(r1) * invI1 * skew(r1)
66        //         - skew(r2) * invI2 * skew(r2)]
67        //
68        //      = [1/m1+1/m2     0    ]
69        //         + invI1 * [r1.y*r1.y -r1.x*r1.y]
70        //         + invI2 * [r1.y*r1.y -r1.x*r1.y]
71        //
72        //        [    0     1/m1+1/m2]
73        //        [-r1.x*r1.y r1.x*r1.x]
74        //        [-r1.x*r1.y r1.x*r1.x]
75        let body1_inv_i = body1.inv_moment_of_inertia;
76        let body2_inv_i = body2.inv_moment_of_inertia;
77        let k1 = Mat2x2::new(
78            body1.inv_mass + body2.inv_mass,
79            0.0,
80            0.0,
81            body1.inv_mass + body2.inv_mass,
82        );
83        let k2 = Mat2x2::new(
84            body1_inv_i * self.r1.y * self.r1.y,
85            -body1_inv_i * self.r1.x * self.r1.y,
86            -body1_inv_i * self.r1.x * self.r1.y,
87            body1_inv_i * self.r1.x * self.r1.x,
88        );
89        let k3 = Mat2x2::new(
90            body2_inv_i * self.r2.y * self.r2.y,
91            -body2_inv_i * self.r2.x * self.r2.y,
92            -body2_inv_i * self.r2.x * self.r2.y,
93            body2_inv_i * self.r2.x * self.r2.x,
94        );
95        let k = k1 + k2 + k3 + Mat2x2::from_diag(Vec2::splat(self.softness));
96
97        self.mass_matrix = k.invert();
98
99        let p1 = body1.position + self.r1;
100        let p2 = body2.position + self.r2;
101        let dp = p2 - p1;
102
103        if World::POSITION_CORRECTION {
104            self.bias = -self.bias_factor * inv_dt * dp;
105        } else {
106            self.bias = Vec2::ZERO;
107        }
108
109        if World::WARM_STARTING {
110            // Apply accumulated impulse.
111            body1.velocity -= body1.inv_mass * self.accumulated_impulse;
112            body1.angular_velocity -= body1.inv_moment_of_inertia
113                * self.r1.cross(self.accumulated_impulse);
114
115            body2.velocity += body2.inv_mass * self.accumulated_impulse;
116            body2.angular_velocity += body2.inv_moment_of_inertia
117                * self.r2.cross(self.accumulated_impulse);
118        } else {
119            self.accumulated_impulse = Vec2::ZERO;
120        }
121    }
122
123    pub fn apply_impulse(&mut self, body1: &mut Body, body2: &mut Body) {
124        let dv = body2.velocity
125            + Vec2::scalar_cross(body2.angular_velocity, self.r2)
126            - body1.velocity
127            - Vec2::scalar_cross(body1.angular_velocity, self.r1);
128
129        let impulse = self.mass_matrix
130            * (-dv - self.bias - self.softness * self.accumulated_impulse);
131
132        body1.velocity -= body1.inv_mass * impulse;
133        body1.angular_velocity -=
134            body1.inv_moment_of_inertia * self.r1.cross(impulse);
135
136        body2.velocity += body2.inv_mass * impulse;
137        body2.angular_velocity +=
138            body2.inv_moment_of_inertia * self.r2.cross(impulse);
139
140        self.accumulated_impulse += impulse;
141    }
142}
rox2d/rox2d/joint.rs

rox2d/rox2d/
joint.rs
