symtropy_physics/joints/
fixed.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//! Fixed joint: rigidly attaches two bodies (zero degrees of freedom).
5//!
6//! Both positional and rotational offsets are maintained at their initial values.
7//! Useful for building compound bodies from multiple collision shapes.
8
9use nalgebra::SVector;
10
11use crate::body::{BodyHandle, RigidBody};
12use crate::constraint::Constraint;
13
14/// Fixed joint: locks two bodies at a fixed relative position.
15///
16/// The joint maintains the initial relative offset `anchor_b - anchor_a`
17/// (in world space at the time of creation). All translational DOF are removed.
18pub struct FixedJoint<const D: usize> {
19    pub body_a: BodyHandle,
20    pub body_b: BodyHandle,
21    /// Local-space anchor on body A.
22    pub anchor_a: SVector<f64, D>,
23    /// Local-space anchor on body B.
24    pub anchor_b: SVector<f64, D>,
25    /// Constraint stiffness [0, 1]. 1.0 = rigid, <1.0 = allows some drift.
26    pub stiffness: f64,
27}
28
29impl<const D: usize> FixedJoint<D> {
30    /// Create a fixed joint connecting body A and body B at their current positions.
31    /// The anchors default to the body origins.
32    pub fn new(body_a: BodyHandle, body_b: BodyHandle) -> Self {
33        Self {
34            body_a,
35            body_b,
36            anchor_a: SVector::zeros(),
37            anchor_b: SVector::zeros(),
38            stiffness: 1.0,
39        }
40    }
41
42    /// Create with specific local-space anchors.
43    pub fn with_anchors(
44        body_a: BodyHandle,
45        body_b: BodyHandle,
46        anchor_a: SVector<f64, D>,
47        anchor_b: SVector<f64, D>,
48    ) -> Self {
49        Self {
50            body_a,
51            body_b,
52            anchor_a,
53            anchor_b,
54            stiffness: 1.0,
55        }
56    }
57}
58
59impl<const D: usize> Constraint<D> for FixedJoint<D> {
60    fn bodies(&self) -> (BodyHandle, BodyHandle) {
61        (self.body_a, self.body_b)
62    }
63
64    fn as_any(&self) -> &dyn std::any::Any {
65        self
66    }
67
68    fn as_any_mut(&mut self) -> &mut dyn std::any::Any {
69        self
70    }
71
72    fn solve(&self, body_a: &mut RigidBody<D>, body_b: &mut RigidBody<D>, _dt: f64) {
73        // Compute world-space anchor positions
74        let world_a = body_a.transform.translation.0
75            + body_a.transform.rotation.rotate_vector(&self.anchor_a);
76        let world_b = body_b.transform.translation.0
77            + body_b.transform.rotation.rotate_vector(&self.anchor_b);
78
79        // Positional error: the two anchors should coincide
80        let error = world_b - world_a;
81        let error_mag = error.norm();
82        if error_mag < 1e-15 {
83            return;
84        }
85
86        let correction = error * self.stiffness;
87        let total_inv_mass = body_a.inv_mass + body_b.inv_mass;
88        if total_inv_mass < 1e-15 {
89            return;
90        }
91
92        let ratio_a = body_a.inv_mass / total_inv_mass;
93        let ratio_b = body_b.inv_mass / total_inv_mass;
94
95        if body_a.is_dynamic() {
96            body_a.transform.translation.0 += correction * ratio_a;
97        }
98        if body_b.is_dynamic() {
99            body_b.transform.translation.0 -= correction * ratio_b;
100        }
101    }
102
103    fn solve_velocity(
104        &self,
105        body_a: &mut RigidBody<D>,
106        body_b: &mut RigidBody<D>,
107        _dt: f64,
108        _callback: Option<&mut dyn crate::world::PhysicsCallback<D>>,
109    ) {
110        // Damp all relative velocity at the joint point
111        let rel_vel = body_b.linear_velocity - body_a.linear_velocity;
112        let speed = rel_vel.norm();
113        if speed < 1e-15 {
114            return;
115        }
116
117        let total_inv = body_a.inv_mass + body_b.inv_mass;
118        if total_inv < 1e-15 {
119            return;
120        }
121
122        // Apply impulse to eliminate relative velocity (scaled by stiffness)
123        let impulse = rel_vel * (-self.stiffness / total_inv);
124        if body_a.is_dynamic() {
125            body_a.linear_velocity -= impulse * body_a.inv_mass;
126        }
127        if body_b.is_dynamic() {
128            body_b.linear_velocity += impulse * body_b.inv_mass;
129        }
130    }
131}
132
133#[cfg(test)]
134mod tests {
135    use super::*;
136    use symtropy_math::Point;
137
138    #[test]
139    fn fixed_joint_holds_position() {
140        let mut a =
141            RigidBody::<3>::dynamic_sphere(BodyHandle(0), Point::new([0.0, 0.0, 0.0]), 0.5, 1.0);
142        let mut b =
143            RigidBody::<3>::dynamic_sphere(BodyHandle(1), Point::new([3.0, 0.0, 0.0]), 0.5, 1.0);
144
145        let joint = FixedJoint::new(BodyHandle(0), BodyHandle(1));
146
147        // Pull them together over iterations
148        for _ in 0..20 {
149            joint.solve(&mut a, &mut b, 0.016);
150        }
151
152        let dist = a.transform.translation.distance(&b.transform.translation);
153        assert!(
154            dist < 0.1,
155            "fixed joint should bring bodies together, dist = {dist}"
156        );
157    }
158
159    #[test]
160    fn fixed_joint_with_anchors() {
161        let mut a =
162            RigidBody::<3>::dynamic_sphere(BodyHandle(0), Point::new([0.0, 0.0, 0.0]), 0.5, 1.0);
163        let mut b =
164            RigidBody::<3>::dynamic_sphere(BodyHandle(1), Point::new([5.0, 0.0, 0.0]), 0.5, 1.0);
165
166        // Anchor at (+1,0,0) on A should meet anchor at (-1,0,0) on B
167        // So bodies should end up 2 apart (not 0)
168        let joint = FixedJoint::with_anchors(
169            BodyHandle(0),
170            BodyHandle(1),
171            SVector::from([1.0, 0.0, 0.0]),
172            SVector::from([-1.0, 0.0, 0.0]),
173        );
174
175        for _ in 0..30 {
176            joint.solve(&mut a, &mut b, 0.016);
177        }
178
179        // The world-space anchors should coincide, meaning bodies are 2.0 apart
180        let world_a = a.transform.translation.0 + SVector::from([1.0, 0.0, 0.0]);
181        let world_b = b.transform.translation.0 + SVector::from([-1.0, 0.0, 0.0]);
182        let anchor_dist = (world_b - world_a).norm();
183        assert!(
184            anchor_dist < 0.1,
185            "fixed joint anchors should coincide, dist = {anchor_dist}"
186        );
187    }
188
189    #[test]
190    fn fixed_joint_velocity_damping() {
191        let mut a = RigidBody::<3>::dynamic_sphere(BodyHandle(0), Point::origin(), 0.5, 1.0);
192        let mut b = RigidBody::<3>::dynamic_sphere(BodyHandle(1), Point::origin(), 0.5, 1.0);
193        b.linear_velocity = SVector::from([10.0, 0.0, 0.0]);
194
195        let joint = FixedJoint::new(BodyHandle(0), BodyHandle(1));
196        joint.solve_velocity(&mut a, &mut b, 0.016);
197
198        // Relative velocity should be reduced
199        let rel = (b.linear_velocity - a.linear_velocity).norm();
200        assert!(rel < 10.0, "velocity should be damped, rel = {rel}");
201    }
202
203    #[test]
204    fn fixed_joint_4d() {
205        let mut a = RigidBody::<4>::dynamic_sphere(BodyHandle(0), Point::origin(), 0.5, 1.0);
206        let mut b = RigidBody::<4>::dynamic_sphere(
207            BodyHandle(1),
208            Point::new([2.0, 0.0, 0.0, 0.0]),
209            0.5,
210            1.0,
211        );
212
213        let joint = FixedJoint::new(BodyHandle(0), BodyHandle(1));
214
215        for _ in 0..20 {
216            joint.solve(&mut a, &mut b, 0.016);
217        }
218
219        let dist = a.transform.translation.distance(&b.transform.translation);
220        assert!(dist < 0.1, "4D fixed joint, dist = {dist}");
221    }
222}
symtropy_physics/joints/fixed.rs

symtropy_physics/joints/
fixed.rs
