oxiphysics_python/world_api/constraints.rs
1// Copyright 2026 COOLJAPAN OU (Team KitaSan)
2// SPDX-License-Identifier: Apache-2.0
3
4//! Constraint builders and constraint resolution.
5
6#![allow(missing_docs)]
7
8use super::PyPhysicsWorld;
9
10// ===========================================================================
11// Constraint Builders
12// ===========================================================================
13
14/// Type of constraint between two bodies.
15#[derive(Debug, Clone, Copy, PartialEq, Eq)]
16#[allow(dead_code)]
17pub enum ConstraintType {
18 /// Distance constraint: maintain a fixed distance between two anchor points.
19 Distance,
20 /// Point-to-point (ball-socket): anchor points coincide.
21 PointToPoint,
22 /// Hinge: bodies rotate around a shared axis.
23 Hinge,
24 /// Slider: bodies translate along a shared axis.
25 Slider,
26}
27
28/// A constraint between two rigid bodies.
29///
30/// Constraints are stored in the world and resolved during each step.
31/// Currently implemented as soft position-level corrections.
32#[derive(Debug, Clone)]
33#[allow(dead_code)]
34pub struct PyConstraint {
35 /// Unique handle.
36 pub handle: u32,
37 /// Type of constraint.
38 pub constraint_type: ConstraintType,
39 /// Handle of the first body.
40 pub body_a: u32,
41 /// Handle of the second body.
42 pub body_b: u32,
43 /// Anchor point on body_a in local coordinates (or world if no body).
44 pub anchor_a: [f64; 3],
45 /// Anchor point on body_b in local coordinates.
46 pub anchor_b: [f64; 3],
47 /// Target distance (for Distance constraints).
48 pub target_distance: f64,
49 /// Hinge axis in world space (for Hinge constraints).
50 pub axis: [f64; 3],
51 /// Constraint stiffness (0..1, 1 = rigid).
52 pub stiffness: f64,
53 /// Whether the constraint is currently active.
54 pub enabled: bool,
55}
56
57impl PyConstraint {
58 /// Create a distance constraint between two bodies.
59 pub fn distance(
60 handle: u32,
61 body_a: u32,
62 body_b: u32,
63 anchor_a: [f64; 3],
64 anchor_b: [f64; 3],
65 distance: f64,
66 ) -> Self {
67 Self {
68 handle,
69 constraint_type: ConstraintType::Distance,
70 body_a,
71 body_b,
72 anchor_a,
73 anchor_b,
74 target_distance: distance,
75 axis: [0.0, 1.0, 0.0],
76 stiffness: 0.5,
77 enabled: true,
78 }
79 }
80
81 /// Create a point-to-point constraint (ball-socket joint).
82 pub fn point_to_point(handle: u32, body_a: u32, body_b: u32, pivot: [f64; 3]) -> Self {
83 Self {
84 handle,
85 constraint_type: ConstraintType::PointToPoint,
86 body_a,
87 body_b,
88 anchor_a: pivot,
89 anchor_b: pivot,
90 target_distance: 0.0,
91 axis: [0.0, 1.0, 0.0],
92 stiffness: 0.5,
93 enabled: true,
94 }
95 }
96
97 /// Create a hinge constraint around `axis` at `pivot`.
98 pub fn hinge(handle: u32, body_a: u32, body_b: u32, pivot: [f64; 3], axis: [f64; 3]) -> Self {
99 Self {
100 handle,
101 constraint_type: ConstraintType::Hinge,
102 body_a,
103 body_b,
104 anchor_a: pivot,
105 anchor_b: pivot,
106 target_distance: 0.0,
107 axis,
108 stiffness: 0.5,
109 enabled: true,
110 }
111 }
112
113 /// Set constraint stiffness (builder pattern).
114 pub fn with_stiffness(mut self, s: f64) -> Self {
115 self.stiffness = s.clamp(0.0, 1.0);
116 self
117 }
118
119 /// Enable or disable the constraint.
120 pub fn set_enabled(&mut self, enabled: bool) {
121 self.enabled = enabled;
122 }
123}
124
125/// Extension: constraint storage and resolution on `PyPhysicsWorld`.
126impl PyPhysicsWorld {
127 /// Add a constraint to the world. Returns the constraint handle.
128 ///
129 /// Note: constraint handles are auto-incremented independently of body handles.
130 pub fn add_constraint(&mut self, mut c: PyConstraint) -> u32 {
131 let h = self.next_constraint_handle();
132 c.handle = h;
133 self.constraints.push(c);
134 h
135 }
136
137 /// Remove a constraint by handle. Returns `true` if found.
138 pub fn remove_constraint(&mut self, handle: u32) -> bool {
139 let before = self.constraints.len();
140 self.constraints.retain(|c| c.handle != handle);
141 self.constraints.len() < before
142 }
143
144 /// Return the number of active constraints.
145 pub fn constraint_count(&self) -> usize {
146 self.constraints.len()
147 }
148
149 /// Resolve all enabled constraints (soft position correction).
150 ///
151 /// Called internally by `step()` after contact resolution.
152 pub(super) fn resolve_constraints(&mut self) {
153 // Clone to avoid borrow issues
154 let constraints: Vec<PyConstraint> = self.constraints.clone();
155 for c in &constraints {
156 if !c.enabled {
157 continue;
158 }
159 match c.constraint_type {
160 ConstraintType::Distance | ConstraintType::PointToPoint => {
161 let (pa, pb, inv_ma, inv_mb, static_a, static_b) = {
162 let ba = match self.get_body(c.body_a) {
163 Some(b) => b,
164 None => continue,
165 };
166 let bb = match self.get_body(c.body_b) {
167 Some(b) => b,
168 None => continue,
169 };
170 (
171 ba.position,
172 bb.position,
173 ba.inv_mass(),
174 bb.inv_mass(),
175 ba.is_static,
176 bb.is_static,
177 )
178 };
179 let diff = [pb[0] - pa[0], pb[1] - pa[1], pb[2] - pa[2]];
180 let dist = (diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2]).sqrt();
181 let target = if c.constraint_type == ConstraintType::PointToPoint {
182 0.0
183 } else {
184 c.target_distance
185 };
186 if dist < 1e-12 {
187 continue;
188 }
189 let error = dist - target;
190 if error.abs() < 1e-8 {
191 continue;
192 }
193 let n = [diff[0] / dist, diff[1] / dist, diff[2] / dist];
194 let total_inv_m = inv_ma + inv_mb;
195 if total_inv_m < 1e-15 {
196 continue;
197 }
198 let correction = error * c.stiffness / total_inv_m;
199 if !static_a && let Some(ba) = self.get_body_mut(c.body_a) {
200 ba.position[0] += n[0] * correction * inv_ma;
201 ba.position[1] += n[1] * correction * inv_ma;
202 ba.position[2] += n[2] * correction * inv_ma;
203 }
204 if !static_b && let Some(bb) = self.get_body_mut(c.body_b) {
205 bb.position[0] -= n[0] * correction * inv_mb;
206 bb.position[1] -= n[1] * correction * inv_mb;
207 bb.position[2] -= n[2] * correction * inv_mb;
208 }
209 }
210 ConstraintType::Hinge | ConstraintType::Slider => {
211 // Simplified: treat as point-to-point for now
212 // A full implementation would project motion onto the axis
213 }
214 }
215 }
216 }
217
218 /// Compute the next available constraint handle.
219 fn next_constraint_handle(&self) -> u32 {
220 self.constraints
221 .iter()
222 .map(|c| c.handle)
223 .max()
224 .map(|h| h + 1)
225 .unwrap_or(0)
226 }
227}