1use crate::Rotation2;
2use ferravex_math::{Quat, Vec3};
3use std::ops::{Index, IndexMut};
4
5#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)]
6pub struct RigidBodyHandle {
7 pub(crate) index: usize,
8 pub(crate) generation: u32,
9}
10#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)]
11pub struct ColliderHandle {
12 pub(crate) index: usize,
13 pub(crate) generation: u32,
14}
15#[derive(Clone, Debug)]
16pub(crate) struct Slot<T> {
17 pub(crate) generation: u32,
18 pub(crate) value: Option<T>,
19}
20#[derive(Clone, Debug)]
21pub struct BodySet<B> {
22 slots: Vec<Slot<B>>,
23 len: usize,
24}
25#[derive(Clone, Debug)]
26pub struct ColliderSet {
27 slots: Vec<Slot<Collider>>,
28 len: usize,
29}
30macro_rules! arena {
31 ($set:ident $(<$generic:ident>)?,$item:ty,$handle:ident) => {
32 impl$(<$generic>)? Default for $set$(<$generic>)? {fn default()->Self {Self{slots:Vec::new(),len:0}}}
33 impl$(<$generic>)? $set$(<$generic>)? {
34 pub fn len(&self)->usize {self.len}
35 pub fn is_empty(&self)->bool {self.len==0}
36 pub fn get(&self,h:$handle)->Option<&$item> {let s=self.slots.get(h.index)?; (s.generation==h.generation).then_some(s.value.as_ref()).flatten()}
37 pub fn get_mut(&mut self,h:$handle)->Option<&mut $item> {let s=self.slots.get_mut(h.index)?; (s.generation==h.generation).then_some(s.value.as_mut()).flatten()}
38 pub fn iter(&self)->impl Iterator<Item=($handle,&$item)> {self.slots.iter().enumerate().filter_map(|(index,s)|s.value.as_ref().map(|v|($handle{index,generation:s.generation},v)))}
39 pub fn iter_mut(&mut self)->impl Iterator<Item=($handle,&mut $item)> {self.slots.iter_mut().enumerate().filter_map(|(index,s)|s.value.as_mut().map(|v|($handle{index,generation:s.generation},v)))}
40 pub(crate) fn insert(&mut self,value:$item)->$handle {
41 self.len+=1;
42 for(index,s) in self.slots.iter_mut().enumerate() {if s.value.is_none()&&s.generation<u32::MAX {s.value=Some(value);return $handle{index,generation:s.generation};}}
43 let index=self.slots.len();self.slots.push(Slot{generation:0,value:Some(value)});$handle{index,generation:0}
44 }
45 pub(crate) fn remove(&mut self,h:$handle)->Option<$item> {let s=self.slots.get_mut(h.index)?;if s.generation!=h.generation {return None;} let v=s.value.take()?;s.generation=s.generation.saturating_add(1);self.len-=1;Some(v)}
46 }
47 impl$(<$generic>)? Index<$handle> for $set$(<$generic>)? {type Output=$item;fn index(&self,h:$handle)->&$item {self.get(h).expect("stale physics handle")}}
48 impl$(<$generic>)? IndexMut<$handle> for $set$(<$generic>)? {fn index_mut(&mut self,h:$handle)->&mut $item {self.get_mut(h).expect("stale physics handle")}}
49 }
50}
51pub(crate) use arena;
52arena!(BodySet<B>, B, RigidBodyHandle);
53arena!(ColliderSet, Collider, ColliderHandle);
54#[derive(Clone, Copy, Debug)]
55pub(crate) enum Shape {
56 Ball(f32),
57 Box(Vec3),
58 #[cfg(feature = "dim2")]
59 Capsule {
60 a: Vec3,
61 b: Vec3,
62 radius: f32,
63 },
64 #[cfg(feature = "dim3")]
65 Cylinder {
66 half_height: f32,
67 radius: f32,
68 },
69}
70impl Shape {
71 pub fn validate(self) {
72 match self {
73 Self::Ball(r) => assert!(r.is_finite() && r > 0.),
74 Self::Box(h) => assert!(h.is_finite() && h.x > 0. && h.y > 0. && h.z > 0.),
75 #[cfg(feature = "dim2")]
76 Self::Capsule { a, b, radius } => {
77 assert!(a.is_finite() && b.is_finite() && radius.is_finite() && radius > 0.)
78 }
79 #[cfg(feature = "dim3")]
80 Self::Cylinder {
81 half_height,
82 radius,
83 } => assert!(
84 half_height.is_finite() && half_height > 0. && radius.is_finite() && radius > 0.
85 ),
86 }
87 }
88}
89#[derive(Clone, Debug)]
90pub struct Collider {
91 pub(crate) shape: Shape,
92 pub(crate) local_translation: Vec3,
93 pub(crate) local_rotation: Quat,
94 pub(crate) mass_dirty: bool,
95 pub(crate) parent: Option<RigidBodyHandle>,
96 pub(crate) density: f32,
97 pub(crate) restitution: f32,
98 pub(crate) friction: f32,
99 pub(crate) sensor: bool,
100 pub(crate) membership: u32,
101 pub(crate) filter: u32,
102 pub(crate) revision: u64,
103 pub(crate) dirty: bool,
104}
105impl Collider {
106 pub(crate) fn new(shape: Shape) -> Self {
107 Self {
108 shape,
109 local_translation: Vec3::ZERO,
110 local_rotation: Quat::IDENTITY,
111 mass_dirty: false,
112 parent: None,
113 density: 1.,
114 restitution: 0.,
115 friction: 0.6,
116 sensor: false,
117 membership: u32::MAX,
118 filter: u32::MAX,
119 revision: 0,
120 dirty: false,
121 }
122 }
123 pub(crate) fn valid(&self, planar: bool) -> bool {
124 self.local_translation.is_finite()
125 && self.local_translation.abs().max_element() <= 1e6
126 && self.local_rotation.is_finite()
127 && (self.local_rotation.length_squared() - 1.).abs() < 0.001
128 && self.density.is_finite()
129 && (0. ..=1e6).contains(&self.density)
130 && (!planar
131 || self.local_translation.z == 0.
132 && (self.local_rotation * Vec3::Z - Vec3::Z).length_squared() < 1e-6)
133 }
134 pub fn local_translation(&self) -> Vec3 {
135 self.local_translation
136 }
137 pub fn local_rotation(&self) -> Quat {
138 self.local_rotation
139 }
140 pub fn density(&self) -> f32 {
141 self.density
142 }
143 pub fn set_density(&mut self, density: f32) {
144 assert!(density.is_finite() && (0. ..=1e6).contains(&density));
145 if self.density != density {
146 self.density = density;
147 self.touch_mass();
148 }
149 }
150 pub(crate) fn touch_mass(&mut self) {
151 self.revision = self
152 .revision
153 .checked_add(1)
154 .expect("collider revision exhausted");
155 self.dirty = true;
156 self.mass_dirty = true;
157 }
158 pub(crate) fn pose(&self, body: &Core) -> crate::geometry::Pose {
159 crate::geometry::Pose {
160 position: body.position + body.rotation * self.local_translation,
161 rotation: body.rotation * self.local_rotation,
162 planar: body.planar,
163 }
164 }
165 pub fn set_sensor(&mut self, sensor: bool) {
166 if self.sensor != sensor {
167 self.revision = self
168 .revision
169 .checked_add(1)
170 .expect("collider revision exhausted");
171 self.dirty = true;
172 self.sensor = sensor;
173 }
174 }
175 pub fn set_collision_layers(&mut self, membership: u32, filter: u32) {
176 if (self.membership, self.filter) != (membership, filter) {
177 self.revision = self
178 .revision
179 .checked_add(1)
180 .expect("collider revision exhausted");
181 self.dirty = true;
182 self.membership = membership;
183 self.filter = filter;
184 }
185 }
186 pub fn parent(&self) -> Option<RigidBodyHandle> {
187 self.parent
188 }
189 pub fn is_sensor(&self) -> bool {
190 self.sensor
191 }
192 pub fn set_restitution(&mut self, v: f32) {
193 assert!(v.is_finite() && (0.0..=1.0).contains(&v));
194 if self.restitution != v {
195 self.revision = self
196 .revision
197 .checked_add(1)
198 .expect("collider revision exhausted");
199 self.dirty = true;
200 }
201 self.restitution = v;
202 }
203 pub fn set_friction(&mut self, v: f32) {
204 assert!(v.is_finite() && v >= 0.);
205 if self.friction != v {
206 self.revision = self
207 .revision
208 .checked_add(1)
209 .expect("collider revision exhausted");
210 self.dirty = true;
211 }
212 self.friction = v;
213 }
214}
215#[derive(Clone, Copy, Debug, PartialEq, Eq)]
216pub(crate) enum Kind {
217 Fixed,
218 Dynamic,
219 Kinematic,
220}
221#[derive(Clone, Debug)]
222#[doc(hidden)]
223pub struct Core {
224 pub(crate) kind: Kind,
225 pub(crate) planar: bool,
226 pub(crate) position: Vec3,
227 pub(crate) rotation: Quat,
228 pub(crate) rotation2: Rotation2,
229 pub(crate) velocity: Vec3,
230 pub(crate) angular_velocity: Vec3,
231 pub(crate) mass: f32,
232 pub(crate) inv_mass: f32,
233 pub(crate) inv_inertia: crate::mass::Tensor,
234 pub(crate) local_com: Vec3,
235 pub(crate) force: Vec3,
236 pub(crate) torque: Vec3,
237 pub(crate) gravity_scale: f32,
238 pub(crate) linear_damping: f32,
239 pub(crate) angular_damping: f32,
240 pub(crate) ccd: bool,
241 pub(crate) sleeping: bool,
242 pub(crate) quiet: f32,
243 pub(crate) revision: u64,
244 pub(crate) solver_dirty: bool,
245 pub(crate) wake_requested: bool,
246 pub(crate) next_position: Option<Vec3>,
247 pub(crate) next_rotation: Option<Quat>,
248 pub(crate) colliders: Vec<ColliderHandle>,
249}
250#[doc(hidden)]
251pub trait Access {
252 fn core(&self) -> &Core;
253 fn core_mut(&mut self) -> &mut Core;
254}
255impl Core {
256 pub(crate) fn new(kind: Kind, planar: bool) -> Self {
257 Self {
258 kind,
259 planar,
260 position: Vec3::ZERO,
261 rotation: Quat::IDENTITY,
262 rotation2: Rotation2::default(),
263 velocity: Vec3::ZERO,
264 angular_velocity: Vec3::ZERO,
265 mass: 0.,
266 inv_mass: 0.,
267 inv_inertia: crate::mass::Tensor::ZERO,
268 local_com: Vec3::ZERO,
269 force: Vec3::ZERO,
270 torque: Vec3::ZERO,
271 gravity_scale: 1.,
272 linear_damping: 0.,
273 angular_damping: 0.,
274 ccd: false,
275 sleeping: false,
276 quiet: 0.,
277 revision: 0,
278 solver_dirty: false,
279 wake_requested: false,
280 next_position: None,
281 next_rotation: None,
282 colliders: Vec::with_capacity(1),
283 }
284 }
285 pub(crate) fn invalidate_contacts(&mut self) {
286 self.revision = self
287 .revision
288 .checked_add(1)
289 .expect("body revision exhausted");
290 self.solver_dirty = true;
291 }
292 pub(crate) fn wake(&mut self) {
293 self.wake_requested = true;
294 self.wake_internal();
295 }
296 pub(crate) fn wake_internal(&mut self) {
297 self.sleeping = false;
298 self.quiet = 0.;
299 }
300 pub(crate) fn set_rotation(&mut self, q: Quat) {
301 assert!(q.is_finite());
302 self.rotation = q.normalize();
303 let axis = self.rotation * Vec3::X;
304 self.rotation2 = Rotation2::from_angle(axis.y.atan2(axis.x));
305 }
306 pub(crate) fn inverse_inertia(&self, v: Vec3) -> Vec3 {
307 self.rotation * self.inv_inertia.apply(self.rotation.conjugate() * v)
308 }
309 pub(crate) fn contact_inverse_inertia(&self, v: Vec3) -> Vec3 {
310 if self.sleeping {
311 Vec3::ZERO
312 } else {
313 self.inverse_inertia(v)
314 }
315 }
316 pub(crate) fn contact_inv_mass(&self) -> f32 {
317 if self.sleeping { 0. } else { self.inv_mass }
318 }
319 pub(crate) fn point_velocity(&self, p: Vec3) -> Vec3 {
320 self.velocity + self.angular_velocity.cross(p - self.world_com())
321 }
322 pub(crate) fn impulse(&mut self, j: Vec3, p: Vec3) {
323 if self.kind == Kind::Dynamic && !self.sleeping {
324 self.velocity += j * self.inv_mass;
325 self.angular_velocity += self.inverse_inertia((p - self.world_com()).cross(j));
326 }
327 }
328 pub(crate) fn world_com(&self) -> Vec3 {
329 self.position + self.rotation * self.local_com
330 }
331 pub(crate) fn set_mass_properties(&mut self, properties: crate::mass::Mass) {
332 let old = self.world_com();
333 self.local_com = properties.center;
334 self.mass = properties.mass;
335 self.inv_mass = 1. / properties.mass;
336 self.inv_inertia = properties.inverse;
337 self.velocity += self.angular_velocity.cross(self.world_com() - old);
338 self.invalidate_contacts();
339 self.wake();
340 }
341 pub(crate) fn integrate(&mut self, dt: f32) {
342 let center = self.world_com() + self.velocity * dt;
343 self.set_rotation(Quat::from_scaled_axis(self.angular_velocity * dt) * self.rotation);
344 self.position = center - self.rotation * self.local_com;
345 }
346 pub(crate) fn position_impulse(&mut self, linear: Vec3, angular: Vec3) {
347 if self.kind != Kind::Dynamic || self.sleeping {
348 return;
349 }
350 let center = self.world_com() + linear * self.inv_mass;
351 let delta = self.inverse_inertia(angular).clamp_length_max(0.2);
352 if delta.length_squared() > 1e-20 {
353 self.set_rotation(Quat::from_scaled_axis(delta) * self.rotation);
354 }
355 self.position = center - self.rotation * self.local_com;
356 }
357}
358impl<B> BodySet<B> {
359 pub(crate) fn two_mut(&mut self, a: RigidBodyHandle, b: RigidBodyHandle) -> (&mut B, &mut B) {
360 assert!(a.index != b.index);
361 let (sa, sb) = if a.index < b.index {
362 let (l, r) = self.slots.split_at_mut(b.index);
363 (&mut l[a.index], &mut r[0])
364 } else {
365 let (l, r) = self.slots.split_at_mut(a.index);
366 (&mut r[0], &mut l[b.index])
367 };
368 assert_eq!(sa.generation, a.generation);
369 assert_eq!(sb.generation, b.generation);
370 (sa.value.as_mut().unwrap(), sb.value.as_mut().unwrap())
371 }
372}