gizmo_physics_rigid/components/
rigid_body.rs

1use gizmo_math::{Mat3, Quat, Vec3};
2use serde::{Deserialize, Serialize};
3use bevy_reflect::Reflect;
4
5use super::Velocity;
6use gizmo_physics_core::{Collider, ColliderShape};
7
8#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, Reflect)]
9pub enum BodyType {
10    Dynamic,   // Fully simulated
11    Kinematic, // Moved by user, affects others
12    Static,    // Never moves
13}
14
15#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, Reflect)]
16pub struct RigidBody {
17    pub body_type: BodyType,
18    pub mass: f32,
19    pub restitution: f32,
20    pub friction: f32,
21    pub linear_damping: f32,
22    pub angular_damping: f32,
23    pub use_gravity: bool,
24    pub is_sleeping: bool,
25    pub ccd_enabled: bool,
26    pub local_inertia: Vec3,
27    pub lock_rotation_x: bool,
28    pub lock_rotation_y: bool,
29    pub lock_rotation_z: bool,
30    pub lock_translation_x: bool,
31    pub lock_translation_y: bool,
32    pub lock_translation_z: bool,
33    pub sleep_counter: u32, // Frames below sleep threshold
34    pub center_of_mass: Vec3,
35    pub fracture_threshold: Option<f32>, // Impulse threshold for fracturing
36    pub force_accumulator: Vec3,
37    pub torque_accumulator: Vec3,
38}
39
40impl Default for RigidBody {
41    fn default() -> Self {
42        Self {
43            body_type: BodyType::Dynamic,
44            mass: 1.0,
45            restitution: 0.5,
46            friction: 0.5,
47            linear_damping: 0.01,
48            angular_damping: 0.05,
49            use_gravity: true,
50            is_sleeping: false,
51            ccd_enabled: false,
52            local_inertia: Vec3::splat(1.0),
53            lock_rotation_x: false,
54            lock_rotation_y: false,
55            lock_rotation_z: false,
56            lock_translation_x: false,
57            lock_translation_y: false,
58            lock_translation_z: false,
59            sleep_counter: 0,
60            center_of_mass: Vec3::ZERO,
61            fracture_threshold: None,
62            force_accumulator: Vec3::ZERO,
63            torque_accumulator: Vec3::ZERO,
64        }
65    }
66}
67
68impl RigidBody {
69    pub fn new(mass: f32, restitution: f32, friction: f32, use_gravity: bool) -> Self {
70        Self {
71            mass,
72            restitution,
73            friction,
74            use_gravity,
75            ..Default::default()
76        }
77    }
78
79    pub fn new_static() -> Self {
80        Self {
81            body_type: BodyType::Static,
82            mass: 0.0,
83            restitution: 0.0,
84            friction: 1.0,
85            linear_damping: 0.0,
86            angular_damping: 0.0,
87            use_gravity: false,
88            is_sleeping: true,
89            local_inertia: Vec3::ZERO,
90            lock_rotation_x: true,
91            lock_rotation_y: true,
92            lock_rotation_z: true,
93            lock_translation_x: true,
94            lock_translation_y: true,
95            lock_translation_z: true,
96            ..Default::default()
97        }
98    }
99
100    pub fn new_kinematic() -> Self {
101        Self {
102            body_type: BodyType::Kinematic,
103            mass: 0.0,
104            restitution: 0.0,
105            friction: 0.5,
106            linear_damping: 0.0,
107            angular_damping: 0.0,
108            use_gravity: false,
109            ccd_enabled: true,
110            local_inertia: Vec3::ZERO,
111            ..Default::default()
112        }
113    }
114
115    pub fn with_fracture_threshold(mut self, threshold: f32) -> Self {
116        self.fracture_threshold = Some(threshold);
117        self
118    }
119
120    pub fn wake_up(&mut self) {
121        self.is_sleeping = false;
122        self.sleep_counter = 0;
123    }
124
125    pub fn can_sleep(&self, velocity: &Velocity) -> bool {
126        if self.is_kinematic() {
127            return false; // Kinematic bodies never sleep — user controls their motion
128        }
129        if !self.is_dynamic() {
130            return true; // Static bodies are always "asleep"
131        }
132
133        const SLEEP_LINEAR_THRESHOLD: f32 = 0.05;
134        const SLEEP_ANGULAR_THRESHOLD: f32 = 0.05;
135
136        velocity.linear.length_squared() < SLEEP_LINEAR_THRESHOLD * SLEEP_LINEAR_THRESHOLD
137            && velocity.angular.length_squared() < SLEEP_ANGULAR_THRESHOLD * SLEEP_ANGULAR_THRESHOLD
138    }
139
140    pub fn update_sleep_state(&mut self, velocity: &Velocity) {
141        const SLEEP_FRAMES_REQUIRED: u32 = 60; // ~1 second at 60fps
142
143        if self.can_sleep(velocity) {
144            self.sleep_counter += 1;
145            if self.sleep_counter >= SLEEP_FRAMES_REQUIRED {
146                self.is_sleeping = true;
147            }
148        } else {
149            self.sleep_counter = 0;
150            self.is_sleeping = false;
151        }
152    }
153
154    #[inline]
155    pub fn is_dynamic(&self) -> bool {
156        matches!(self.body_type, BodyType::Dynamic)
157    }
158
159    #[inline]
160    pub fn is_kinematic(&self) -> bool {
161        matches!(self.body_type, BodyType::Kinematic)
162    }
163
164    #[inline]
165    pub fn is_static(&self) -> bool {
166        matches!(self.body_type, BodyType::Static)
167    }
168
169    #[inline]
170    pub fn enforce_locks(&self, vel: &mut Velocity) {
171        if self.lock_translation_x {
172            vel.linear.x = 0.0;
173        }
174        if self.lock_translation_y {
175            vel.linear.y = 0.0;
176        }
177        if self.lock_translation_z {
178            vel.linear.z = 0.0;
179        }
180        if self.lock_rotation_x {
181            vel.angular.x = 0.0;
182        }
183        if self.lock_rotation_y {
184            vel.angular.y = 0.0;
185        }
186        if self.lock_rotation_z {
187            vel.angular.z = 0.0;
188        }
189    }
190
191    #[inline]
192    pub fn inv_mass(&self) -> f32 {
193        if self.mass == 0.0 || !self.is_dynamic() {
194            0.0
195        } else {
196            1.0 / self.mass
197        }
198    }
199
200    #[inline]
201    pub fn inv_local_inertia(&self) -> Vec3 {
202        if self.mass == 0.0 || !self.is_dynamic() {
203            Vec3::ZERO
204        } else {
205            Vec3::new(
206                if self.local_inertia.x == 0.0 {
207                    0.0
208                } else {
209                    1.0 / self.local_inertia.x
210                },
211                if self.local_inertia.y == 0.0 {
212                    0.0
213                } else {
214                    1.0 / self.local_inertia.y
215                },
216                if self.local_inertia.z == 0.0 {
217                    0.0
218                } else {
219                    1.0 / self.local_inertia.z
220                },
221            )
222        }
223    }
224
225    /// Get inverse world-space inertia tensor
226    pub fn inv_world_inertia_tensor_identity(&self) -> Mat3 {
227        Mat3::from_diagonal(self.inv_local_inertia())
228    }
229
230    /// Get world-space inertia tensor from local inertia and rotation
231    pub fn world_inertia_tensor(&self, rotation: Quat) -> Mat3 {
232        let rot_mat = Mat3::from_quat(rotation);
233        let local_inertia_mat = Mat3::from_diagonal(self.local_inertia);
234        rot_mat * local_inertia_mat * rot_mat.transpose()
235    }
236
237    /// Get inverse world-space inertia tensor
238    pub fn inv_world_inertia_tensor(&self, rotation: Quat) -> Mat3 {
239        if self.mass == 0.0 || !self.is_dynamic() {
240            return Mat3::ZERO;
241        }
242        let rot_mat = Mat3::from_quat(rotation);
243        let inv_local = Mat3::from_diagonal(self.inv_local_inertia());
244        let mut inv_world = rot_mat * inv_local * rot_mat.transpose();
245
246        // Zero out locked world axes
247        if self.lock_rotation_x {
248            inv_world.x_axis = Vec3::ZERO;
249            inv_world.y_axis.x = 0.0;
250            inv_world.z_axis.x = 0.0;
251        }
252        if self.lock_rotation_y {
253            inv_world.y_axis = Vec3::ZERO;
254            inv_world.x_axis.y = 0.0;
255            inv_world.z_axis.y = 0.0;
256        }
257        if self.lock_rotation_z {
258            inv_world.z_axis = Vec3::ZERO;
259            inv_world.x_axis.z = 0.0;
260            inv_world.y_axis.z = 0.0;
261        }
262
263        inv_world
264    }
265
266    pub fn clear_forces(&mut self) {
267        self.force_accumulator = Vec3::ZERO;
268        self.torque_accumulator = Vec3::ZERO;
269    }
270
271    pub fn calculate_box_inertia(&mut self, w: f32, h: f32, d: f32) {
272        let m = self.mass;
273        self.local_inertia = Vec3::new(
274            (m / 12.0) * (h * h + d * d),
275            (m / 12.0) * (w * w + d * d),
276            (m / 12.0) * (w * w + h * h),
277        );
278    }
279
280    pub fn calculate_sphere_inertia(&mut self, r: f32) {
281        let i = 0.4 * self.mass * r * r;
282        self.local_inertia = Vec3::splat(i);
283    }
284
285    pub fn calculate_capsule_inertia(&mut self, r: f32, half_h: f32) {
286        let m = self.mass;
287        let h = half_h * 2.0;
288        let vol_cyl = std::f32::consts::PI * r * r * h;
289        let vol_sph = 4.0 / 3.0 * std::f32::consts::PI * r * r * r;
290        let total_vol = vol_cyl + vol_sph;
291
292        let m_cyl = if total_vol > 0.0 {
293            m * vol_cyl / total_vol
294        } else {
295            0.0
296        };
297        let m_sph = if total_vol > 0.0 {
298            m * vol_sph / total_vol
299        } else {
300            0.0
301        };
302
303        let i_y = m_cyl * (r * r) / 2.0 + m_sph * 2.0 * (r * r) / 5.0;
304        let i_cyl_xz = m_cyl * (3.0 * r * r + h * h) / 12.0;
305        let i_sph_xz =
306            m_sph * (0.4 * r * r + half_h * half_h + 0.75 * r * half_h + 0.140625 * r * r);
307        let i_xz = i_cyl_xz + i_sph_xz;
308
309        self.local_inertia = Vec3::new(i_xz, i_y, i_xz);
310    }
311
312    pub fn update_inertia_from_collider(&mut self, collider: &Collider) {
313        match &collider.shape {
314            ColliderShape::Box(b) => {
315                let w = b.half_extents.x * 2.0;
316                let h = b.half_extents.y * 2.0;
317                let d = b.half_extents.z * 2.0;
318                self.calculate_box_inertia(w, h, d);
319            }
320            ColliderShape::Sphere(s) => {
321                self.calculate_sphere_inertia(s.radius);
322            }
323            ColliderShape::Capsule(c) => {
324                self.calculate_capsule_inertia(c.radius, c.half_height);
325            }
326            ColliderShape::Plane(_) => {
327                self.local_inertia = Vec3::splat(f32::INFINITY);
328            }
329            ColliderShape::TriMesh(_) | ColliderShape::ConvexHull(_) => {
330                self.calculate_box_inertia(1.0, 1.0, 1.0);
331            }
332            ColliderShape::Compound(shapes) => {
333                let mut total_vol = 0.0;
334                let mut vols = Vec::with_capacity(shapes.len());
335                for (_, sub_shape) in shapes {
336                    let temp_col = Collider {
337                        shape: (**sub_shape).clone(),
338                        ..Default::default()
339                    };
340                    let v = temp_col.volume();
341                    vols.push(v);
342                    total_vol += v;
343                }
344
345                if total_vol > 0.0 {
346                    let mut com = Vec3::ZERO;
347                    for (i, (local_t, _)) in shapes.iter().enumerate() {
348                        let mass_i = (vols[i] / total_vol) * self.mass;
349                        com += local_t.position * mass_i;
350                    }
351                    if self.mass > 0.0 {
352                        self.center_of_mass = com / self.mass;
353                    }
354
355                    let mut inertia = Vec3::ZERO;
356                    for (i, (local_t, sub_shape)) in shapes.iter().enumerate() {
357                        let mass_i = (vols[i] / total_vol) * self.mass;
358
359                        let mut temp_rb = RigidBody {
360                            mass: mass_i,
361                            ..Default::default()
362                        };
363                        let temp_col = Collider {
364                            shape: (**sub_shape).clone(),
365                            ..Default::default()
366                        };
367                        temp_rb.update_inertia_from_collider(&temp_col);
368
369                        let d = local_t.position - self.center_of_mass;
370                        let d_sq = d.length_squared();
371
372                        inertia.x += temp_rb.local_inertia.x + mass_i * (d_sq - d.x * d.x);
373                        inertia.y += temp_rb.local_inertia.y + mass_i * (d_sq - d.y * d.y);
374                        inertia.z += temp_rb.local_inertia.z + mass_i * (d_sq - d.z * d.z);
375                    }
376                    self.local_inertia = inertia;
377                } else {
378                    self.calculate_box_inertia(1.0, 1.0, 1.0);
379                }
380            }
381        }
382    }
383}
384
385gizmo_core::impl_component!(RigidBody);
gizmo_physics_rigid/components/rigid_body.rs

gizmo_physics_rigid/components/
rigid_body.rs
