symtropy_physics/
body.rs

1// Copyright (C) 2024-2026 Tristan Stoltz / Luminous Dynamics
2// SPDX-License-Identifier: Apache-2.0 OR MIT
3// Commercial licensing: see COMMERCIAL_LICENSE.md at repository root
4
5use nalgebra::SVector;
6use serde::{Deserialize, Serialize};
7use symtropy_math::{Bivector, Point, Shape, Transform};
8
9/// Unique identifier for a body handle.
10#[derive(Copy, Clone, Debug, Hash, PartialEq, Eq, PartialOrd, Ord, Serialize, Deserialize)]
11pub struct BodyHandle(pub usize);
12
13/// Unique identifier for networked bodies.
14#[derive(Copy, Clone, Debug, Hash, PartialEq, Eq, PartialOrd, Ord, Serialize, Deserialize)]
15pub struct NetId(pub u64);
16
17/// Type of rigid body.
18#[derive(Copy, Clone, Debug, PartialEq, Eq, Serialize, Deserialize)]
19pub enum BodyType {
20    Static,
21    Kinematic,
22    Dynamic,
23}
24
25/// N-dimensional rigid body.
26pub struct RigidBody<const D: usize> {
27    pub handle: BodyHandle,
28    pub net_id: Option<NetId>,
29    pub body_type: BodyType,
30    pub transform: Transform<D>,
31    pub linear_velocity: SVector<f64, D>,
32    pub angular_velocity: Bivector<D>,
33    pub mass: f64,
34    pub inv_mass: f64,
35    pub inertia: SVector<f64, D>,
36    pub inv_inertia: SVector<f64, D>,
37    pub collider: Box<dyn Shape<D>>,
38    pub friction: f64,
39    pub restitution: f64,
40    pub linear_damping: f64,
41    pub angular_damping: f64,
42    pub force_accumulator: SVector<f64, D>,
43    pub torque_accumulator: Bivector<D>,
44    pub sleeping: bool,
45    pub sleep_timer: f32,
46    pub sleep_counter: u32,
47    pub is_sensor: bool,
48    pub collision_group: u32,
49    pub collision_mask: u32,
50}
51
52impl<const D: usize> RigidBody<D> {
53    pub fn new(
54        handle: BodyHandle,
55        body_type: BodyType,
56        transform: Transform<D>,
57        collider: Box<dyn Shape<D>>,
58        mass: f64,
59        inertia: SVector<f64, D>,
60    ) -> Self {
61        let inv_mass = if mass > 0.0 { 1.0 / mass } else { 0.0 };
62        let inv_inertia = inertia.map(|i| if i > 0.0 { 1.0 / i } else { 0.0 });
63
64        Self {
65            handle,
66            net_id: None,
67            body_type,
68            transform,
69            linear_velocity: SVector::zeros(),
70            angular_velocity: Bivector::zero(),
71            mass,
72            inv_mass,
73            inertia,
74            inv_inertia,
75            collider,
76            friction: 0.5,
77            restitution: 0.2,
78            linear_damping: 0.0,
79            angular_damping: 0.0,
80            force_accumulator: SVector::zeros(),
81            torque_accumulator: Bivector::zero(),
82            sleeping: false,
83            sleep_timer: 0.0,
84            sleep_counter: 0,
85            is_sensor: false,
86            collision_group: 0x0001,
87            collision_mask: 0xFFFF,
88        }
89    }
90
91    pub fn static_body(
92        handle: BodyHandle,
93        position: Point<D>,
94        collider: Box<dyn Shape<D>>,
95    ) -> Self {
96        Self::new(
97            handle,
98            BodyType::Static,
99            Transform::from_translation(position),
100            collider,
101            0.0,
102            SVector::zeros(),
103        )
104    }
105
106    pub fn dynamic_sphere(handle: BodyHandle, position: Point<D>, radius: f64, mass: f64) -> Self {
107        use symtropy_math::Sphere;
108        let inertia = 0.4 * mass * radius * radius;
109        Self::new(
110            handle,
111            BodyType::Dynamic,
112            Transform::from_translation(position),
113            Box::new(Sphere::new(Point::origin(), radius)),
114            mass,
115            SVector::from_element(inertia),
116        )
117    }
118
119    pub fn is_dynamic(&self) -> bool {
120        matches!(self.body_type, BodyType::Dynamic)
121    }
122
123    pub fn is_static(&self) -> bool {
124        matches!(self.body_type, BodyType::Static)
125    }
126
127    pub fn apply_force(&mut self, force: SVector<f64, D>) {
128        if self.is_dynamic() {
129            self.force_accumulator += force;
130            self.wake();
131        }
132    }
133
134    pub fn apply_torque(&mut self, torque: Bivector<D>) {
135        if self.is_dynamic() {
136            // Need to implement AddAssign for Bivector or do manually
137            let mut current = self.torque_accumulator.clone();
138            for i in 0..D {
139                for j in (i + 1)..D {
140                    current.set(i, j, current.get(i, j) + torque.get(i, j));
141                }
142            }
143            self.torque_accumulator = current;
144            self.wake();
145        }
146    }
147
148    pub fn clear_accumulators(&mut self) {
149        self.force_accumulator = SVector::zeros();
150        self.torque_accumulator = Bivector::zero();
151    }
152
153    pub fn wake(&mut self) {
154        self.sleeping = false;
155        self.sleep_timer = 0.0;
156        self.sleep_counter = 0;
157    }
158
159    pub fn try_sleep(&mut self, threshold: f64, ticks: u32) {
160        if !self.is_dynamic() {
161            return;
162        }
163
164        let v2 = self.linear_velocity.norm_squared();
165        let w2 = self.angular_velocity.norm_squared();
166
167        if v2 < threshold * threshold && w2 < threshold * threshold {
168            self.sleep_counter += 1;
169            if self.sleep_counter >= ticks {
170                self.sleeping = true;
171                self.linear_velocity = SVector::zeros();
172                self.angular_velocity = Bivector::zero();
173            }
174        } else {
175            self.sleep_counter = 0;
176        }
177    }
178
179    pub fn position(&self) -> SVector<f64, D> {
180        self.transform.translation.0
181    }
182
183    pub fn kinetic_energy(&self) -> f64 {
184        if !self.is_dynamic() {
185            return 0.0;
186        }
187        let linear = 0.5 * self.mass * self.linear_velocity.norm_squared();
188        let i_avg = self.inertia.sum() / D as f64;
189        let angular = 0.5 * i_avg * self.angular_velocity.norm_squared();
190        linear + angular
191    }
192
193    pub fn merge_toolhead(
194        &mut self,
195        local_transform: Transform<D>,
196        tool_collider: Box<dyn Shape<D>>,
197        tool_mass: f64,
198    ) {
199        use symtropy_math::CompoundShape;
200
201        let mut compound =
202            if let Some(existing) = self.collider.as_any().downcast_ref::<CompoundShape<D>>() {
203                let mut new_c = CompoundShape::<D>::new();
204                for (tf, s) in existing.children() {
205                    new_c.add_child(tf.clone(), s.clone_box());
206                }
207                new_c
208            } else {
209                let mut new_c = CompoundShape::<D>::new();
210                new_c.add_child(Transform::identity(), self.collider.as_ref().clone_box());
211                new_c
212            };
213
214        compound.add_child(local_transform.clone(), tool_collider);
215        self.collider = Box::new(compound);
216
217        let total_mass = self.mass + tool_mass;
218        let com_offset = (local_transform.translation.0 * tool_mass) / total_mass;
219
220        let i_original = self.inertia.sum() / D as f64;
221        let i_tool = 0.4 * tool_mass * 0.1 * 0.1;
222
223        let d_original = com_offset.norm();
224        let d_tool = (local_transform.translation.0 - com_offset).norm();
225        let i_total = (i_original + self.mass * d_original * d_original)
226            + (i_tool + tool_mass * d_tool * d_tool);
227
228        self.mass = total_mass;
229        self.inv_mass = 1.0 / total_mass;
230        self.inertia = SVector::from_element(i_total);
231        self.inv_inertia = SVector::from_element(1.0 / i_total);
232
233        self.transform.translation.0 += com_offset;
234    }
235
236    pub fn world_support(&self, direction: &SVector<f64, D>) -> SVector<f64, D> {
237        let local_dir = self.transform.rotation.reverse().rotate_vector(direction);
238        let local_support = self.collider.support(&local_dir);
239        self.transform.transform_point(&Point(local_support)).0
240    }
241}
symtropy_physics/body.rs

symtropy_physics/
body.rs
