symtropy_physics/
constraint.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//! 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 + std::any::Any {
10    /// The two bodies this constraint connects.
11    fn bodies(&self) -> (BodyHandle, BodyHandle);
12
13    /// Cast to std::any::Any for downcasting in tests/lab.
14    fn as_any(&self) -> &dyn std::any::Any;
15
16    /// Cast to std::any::Any for downcasting in tests/lab.
17    fn as_any_mut(&mut self) -> &mut dyn std::any::Any;
18
19    /// Solve the constraint by applying positional corrections.
20    /// Called multiple times per frame for iterative convergence.
21    fn solve(&self, body_a: &mut RigidBody<D>, body_b: &mut RigidBody<D>, dt: f64);
22
23    /// Optional velocity-level correction (impulse-based, Fix 7).
24    /// Called after position solve for hybrid PBD+impulse dynamics.
25    /// Default: no-op (pure PBD).
26    fn solve_velocity(
27        &self,
28        _body_a: &mut RigidBody<D>,
29        _body_b: &mut RigidBody<D>,
30        _dt: f64,
31        _callback: Option<&mut dyn crate::world::PhysicsCallback<D>>,
32    ) {
33    }
34}
35
36/// Distance constraint: keeps two bodies at a fixed distance.
37pub struct DistanceConstraint<const D: usize> {
38    pub body_a: BodyHandle,
39    pub body_b: BodyHandle,
40    pub rest_length: f64,
41    pub stiffness: f64,
42}
43
44impl<const D: usize> Constraint<D> for DistanceConstraint<D> {
45    fn bodies(&self) -> (BodyHandle, BodyHandle) {
46        (self.body_a, self.body_b)
47    }
48
49    fn as_any(&self) -> &dyn std::any::Any {
50        self
51    }
52
53    fn as_any_mut(&mut self) -> &mut dyn std::any::Any {
54        self
55    }
56
57    fn solve(&self, body_a: &mut RigidBody<D>, body_b: &mut RigidBody<D>, _dt: f64) {
58        let delta = body_b.transform.translation.0 - body_a.transform.translation.0;
59        let dist = delta.norm();
60        if dist < 1e-15 {
61            return;
62        }
63
64        let error = dist - self.rest_length;
65        let correction = delta / dist * error * self.stiffness;
66
67        let total_inv_mass = body_a.inv_mass + body_b.inv_mass;
68        if total_inv_mass < 1e-15 {
69            return;
70        }
71
72        let ratio_a = body_a.inv_mass / total_inv_mass;
73        let ratio_b = body_b.inv_mass / total_inv_mass;
74
75        if body_a.is_dynamic() {
76            body_a.transform.translation.0 += correction * ratio_a;
77        }
78        if body_b.is_dynamic() {
79            body_b.transform.translation.0 -= correction * ratio_b;
80        }
81    }
82
83    fn solve_velocity(
84        &self,
85        body_a: &mut RigidBody<D>,
86        body_b: &mut RigidBody<D>,
87        _dt: f64,
88        _callback: Option<&mut dyn crate::world::PhysicsCallback<D>>,
89    ) {
90        let delta = body_b.transform.translation.0 - body_a.transform.translation.0;
91        let dist = delta.norm();
92        if dist < 1e-15 {
93            return;
94        }
95        let normal = delta / dist;
96
97        let rel_vel = body_b.linear_velocity - body_a.linear_velocity;
98        let vel_along = rel_vel.dot(&normal);
99
100        let total_inv = body_a.inv_mass + body_b.inv_mass;
101        if total_inv < 1e-15 {
102            return;
103        }
104        let impulse = -vel_along / total_inv * self.stiffness;
105
106        let impulse_vec = normal * impulse;
107        if body_a.is_dynamic() {
108            body_a.linear_velocity -= impulse_vec * body_a.inv_mass;
109        }
110        if body_b.is_dynamic() {
111            body_b.linear_velocity += impulse_vec * body_b.inv_mass;
112        }
113    }
114}
115
116#[cfg(test)]
117mod tests {
118    use super::*;
119    use symtropy_math::Point;
120
121    #[test]
122    fn distance_constraint_corrects() {
123        let mut a =
124            RigidBody::<3>::dynamic_sphere(BodyHandle(0), Point::new([0.0, 0.0, 0.0]), 1.0, 1.0);
125        let mut b =
126            RigidBody::<3>::dynamic_sphere(BodyHandle(1), Point::new([5.0, 0.0, 0.0]), 1.0, 1.0);
127
128        let constraint = DistanceConstraint {
129            body_a: BodyHandle(0),
130            body_b: BodyHandle(1),
131            rest_length: 3.0,
132            stiffness: 1.0,
133        };
134
135        // Bodies are 5 apart, rest length is 3 — should pull them together
136        for _ in 0..10 {
137            constraint.solve(&mut a, &mut b, 0.01);
138        }
139
140        let dist = a.transform.translation.distance(&b.transform.translation);
141        assert!((dist - 3.0).abs() < 0.1, "dist = {dist}, expected ~3.0");
142    }
143}
symtropy_physics/constraint.rs

symtropy_physics/
constraint.rs
