symtropy_physics/
constraint.rs

1// Copyright (C) 2024-2026 Tristan Stoltz / Luminous Dynamics
2// SPDX-License-Identifier: AGPL-3.0-or-later
3// Commercial licensing: see COMMERCIAL_LICENSE.md at repository root
4//! Constraint trait for ND joints and connections.
5
6use crate::body::{BodyHandle, RigidBody};
7
8/// A constraint between two rigid bodies.
9pub trait Constraint<const D: usize>: Send + Sync {
10    /// The two bodies this constraint connects.
11    fn bodies(&self) -> (BodyHandle, BodyHandle);
12
13    /// Solve the constraint by applying positional corrections.
14    /// Called multiple times per frame for iterative convergence.
15    fn solve(&self, body_a: &mut RigidBody<D>, body_b: &mut RigidBody<D>, dt: f64);
16
17    /// Optional velocity-level correction (impulse-based, Fix 7).
18    /// Called after position solve for hybrid PBD+impulse dynamics.
19    /// Default: no-op (pure PBD).
20    fn solve_velocity(&self, _body_a: &mut RigidBody<D>, _body_b: &mut RigidBody<D>, _dt: f64) {}
21}
22
23/// Distance constraint: keeps two bodies at a fixed distance.
24pub struct DistanceConstraint<const D: usize> {
25    pub body_a: BodyHandle,
26    pub body_b: BodyHandle,
27    pub rest_length: f64,
28    pub stiffness: f64,
29}
30
31impl<const D: usize> Constraint<D> for DistanceConstraint<D> {
32    fn bodies(&self) -> (BodyHandle, BodyHandle) {
33        (self.body_a, self.body_b)
34    }
35
36    fn solve(&self, body_a: &mut RigidBody<D>, body_b: &mut RigidBody<D>, _dt: f64) {
37        let delta = body_b.transform.translation.0 - body_a.transform.translation.0;
38        let dist = delta.norm();
39        if dist < 1e-15 {
40            return;
41        }
42
43        let error = dist - self.rest_length;
44        let correction = delta / dist * error * self.stiffness;
45
46        let total_inv_mass = body_a.inv_mass + body_b.inv_mass;
47        if total_inv_mass < 1e-15 {
48            return;
49        }
50
51        let ratio_a = body_a.inv_mass / total_inv_mass;
52        let ratio_b = body_b.inv_mass / total_inv_mass;
53
54        if body_a.is_dynamic() {
55            body_a.transform.translation.0 += correction * ratio_a;
56        }
57        if body_b.is_dynamic() {
58            body_b.transform.translation.0 -= correction * ratio_b;
59        }
60    }
61
62    fn solve_velocity(&self, body_a: &mut RigidBody<D>, body_b: &mut RigidBody<D>, _dt: f64) {
63        let delta = body_b.transform.translation.0 - body_a.transform.translation.0;
64        let dist = delta.norm();
65        if dist < 1e-15 { return; }
66        let normal = delta / dist;
67
68        let rel_vel = body_b.linear_velocity - body_a.linear_velocity;
69        let vel_along = rel_vel.dot(&normal);
70
71        let total_inv = body_a.inv_mass + body_b.inv_mass;
72        if total_inv < 1e-15 { return; }
73        let impulse = -vel_along / total_inv * self.stiffness;
74
75        let impulse_vec = normal * impulse;
76        if body_a.is_dynamic() {
77            body_a.linear_velocity -= impulse_vec * body_a.inv_mass;
78        }
79        if body_b.is_dynamic() {
80            body_b.linear_velocity += impulse_vec * body_b.inv_mass;
81        }
82    }
83}
84
85#[cfg(test)]
86mod tests {
87    use super::*;
88    use symtropy_math::Point;
89
90    #[test]
91    fn distance_constraint_corrects() {
92        let mut a = RigidBody::<3>::dynamic_sphere(BodyHandle(0), Point::new([0.0, 0.0, 0.0]), 1.0, 1.0);
93        let mut b = RigidBody::<3>::dynamic_sphere(BodyHandle(1), Point::new([5.0, 0.0, 0.0]), 1.0, 1.0);
94
95        let constraint = DistanceConstraint {
96            body_a: BodyHandle(0),
97            body_b: BodyHandle(1),
98            rest_length: 3.0,
99            stiffness: 1.0,
100        };
101
102        // Bodies are 5 apart, rest length is 3 — should pull them together
103        for _ in 0..10 {
104            constraint.solve(&mut a, &mut b, 0.01);
105        }
106
107        let dist = a.transform.translation.distance(&b.transform.translation);
108        assert!((dist - 3.0).abs() < 0.1, "dist = {dist}, expected ~3.0");
109    }
110}
symtropy_physics/constraint.rs

symtropy_physics/
constraint.rs
