ferravex_physics/
joints.rs

1//! Bounded, generational connections between two bodies. Anchors are relative to
2//! authored body origins, so compound center-of-mass edits do not move a pivot.
3use crate::PhysicsError;
4use crate::body::{RigidBodyHandle, Slot};
5use ferravex_math::{Quat, Vec3};
6use std::ops::{Index, IndexMut};
7pub const MAX_JOINTS: usize = 4096;
8#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)]
9pub struct JointHandle {
10    pub(crate) index: usize,
11    pub(crate) generation: u32,
12}
13#[derive(Clone, Copy, Debug, PartialEq)]
14pub enum JointKind {
15    Fixed,
16    Hinge {
17        axis1: Vec3,
18        axis2: Vec3,
19    },
20    Distance {
21        length: f32,
22    },
23    /// Force = -stiffness * extension - damping * relative speed, solved implicitly.
24    Spring {
25        length: f32,
26        stiffness: f32,
27        damping: f32,
28    },
29}
30#[derive(Clone, Copy, Debug, PartialEq)]
31pub struct JointMotor {
32    pub target_velocity: f32,
33    pub target_position: Option<f32>,
34    pub stiffness: f32,
35    pub max_speed: f32,
36    pub max_torque: f32,
37}
38impl JointMotor {
39    pub fn velocity(speed: f32, max_torque: f32) -> Self {
40        Self {
41            target_velocity: speed,
42            target_position: None,
43            stiffness: 0.,
44            max_speed: speed.abs(),
45            max_torque,
46        }
47    }
48    pub fn position(angle: f32, stiffness: f32, max_speed: f32, max_torque: f32) -> Self {
49        Self {
50            target_velocity: 0.,
51            target_position: Some(angle),
52            stiffness,
53            max_speed,
54            max_torque,
55        }
56    }
57    fn valid(self) -> bool {
58        self.target_velocity.is_finite()
59            && self.target_velocity.abs() <= 1000.
60            && self
61                .target_position
62                .is_none_or(|p| p.is_finite() && p.abs() <= std::f32::consts::PI)
63            && [self.stiffness, self.max_speed, self.max_torque]
64                .iter()
65                .all(|v| v.is_finite() && (0. ..=1e9).contains(v))
66            && self.max_speed <= 1000.
67    }
68}
69#[derive(Clone, Copy, Debug, PartialEq)]
70pub struct JointDesc {
71    pub kind: JointKind,
72    pub local_anchor1: Vec3,
73    pub local_anchor2: Vec3,
74    /// None captures the bodies' relative orientation at insertion.
75    pub reference_rotation: Option<Quat>,
76    pub contacts_enabled: bool,
77    pub enabled: bool,
78    /// Twist range relative to the captured orientation, in radians [-pi, pi].
79    pub limits: Option<[f32; 2]>,
80    pub motor: Option<JointMotor>,
81}
82impl JointDesc {
83    pub fn new(kind: JointKind) -> Self {
84        Self {
85            kind,
86            local_anchor1: Vec3::ZERO,
87            local_anchor2: Vec3::ZERO,
88            reference_rotation: None,
89            contacts_enabled: false,
90            enabled: true,
91            limits: None,
92            motor: None,
93        }
94    }
95    pub(crate) fn valid(self, planar: bool) -> bool {
96        let scalar = |v: f32| v.is_finite() && (0. ..=1e6).contains(&v);
97        let hinge = matches!(self.kind, JointKind::Hinge { .. });
98        [self.local_anchor1, self.local_anchor2]
99            .iter()
100            .all(|p| p.is_finite() && p.abs().max_element() <= 1e6 && (!planar || p.z == 0.))
101            && self.reference_rotation.is_none_or(|q| {
102                q.is_finite()
103                    && (q.length_squared() - 1.).abs() < 0.001
104                    && (!planar || (q * Vec3::Z - Vec3::Z).length_squared() < 1e-6)
105            })
106            && match self.kind {
107                JointKind::Fixed => true,
108                JointKind::Distance { length } => scalar(length),
109                JointKind::Spring {
110                    length,
111                    stiffness,
112                    damping,
113                } => scalar(length) && scalar(stiffness) && stiffness > 0. && scalar(damping),
114                JointKind::Hinge { axis1, axis2 } => [axis1, axis2].iter().all(|a| {
115                    a.is_finite()
116                        && (a.length_squared() - 1.).abs() < 0.001
117                        && (!planar || a.distance_squared(Vec3::Z) < 1e-6)
118                }),
119            }
120            && self.limits.is_none_or(|[lo, hi]| {
121                hinge
122                    && lo.is_finite()
123                    && hi.is_finite()
124                    && lo <= hi
125                    && lo >= -std::f32::consts::PI
126                    && hi <= std::f32::consts::PI
127            })
128            && self.motor.is_none_or(|m| hinge && m.valid())
129    }
130}
131#[derive(Clone, Copy, Debug, Default)]
132pub(crate) struct CachedRow {
133    pub impulse: f32,
134    pub axis: Vec3,
135}
136#[derive(Clone, Debug)]
137pub struct Joint {
138    pub(crate) a: RigidBodyHandle,
139    pub(crate) b: RigidBodyHandle,
140    pub(crate) desc: JointDesc,
141    pub(crate) dirty: bool,
142    pub(crate) revisions: [u64; 2],
143    pub(crate) dt: f32,
144    pub(crate) cache: [CachedRow; 8],
145}
146impl Joint {
147    pub fn bodies(&self) -> [RigidBodyHandle; 2] {
148        [self.a, self.b]
149    }
150    pub fn description(&self) -> JointDesc {
151        self.desc
152    }
153    pub fn set_motor(&mut self, motor: Option<JointMotor>) -> Result<(), PhysicsError> {
154        let desc = JointDesc { motor, ..self.desc };
155        self.set_description(desc)
156    }
157    pub fn set_limits(&mut self, limits: Option<[f32; 2]>) -> Result<(), PhysicsError> {
158        self.set_description(JointDesc {
159            limits,
160            ..self.desc
161        })
162    }
163    pub fn set_enabled(&mut self, enabled: bool) {
164        if self.desc.enabled != enabled {
165            self.desc.enabled = enabled;
166            self.dirty = true;
167        }
168    }
169    pub fn set_contacts_enabled(&mut self, enabled: bool) {
170        if self.desc.contacts_enabled != enabled {
171            self.desc.contacts_enabled = enabled;
172            self.dirty = true;
173        }
174    }
175    fn set_description(&mut self, desc: JointDesc) -> Result<(), PhysicsError> {
176        // Dimension-specific validation was performed on insertion; these setters
177        // only change dimension-independent motor/limit fields.
178        if !desc.valid(false) {
179            return Err(PhysicsError::InvalidJoint);
180        }
181        if self.desc != desc {
182            self.desc = desc;
183            self.dirty = true;
184        }
185        Ok(())
186    }
187}
188#[derive(Clone, Debug)]
189pub struct JointSet {
190    slots: Vec<Slot<Joint>>,
191    len: usize,
192}
193crate::body::arena!(JointSet, Joint, JointHandle);
ferravex_physics/joints.rs

ferravex_physics/
joints.rs
