"""Physics joints: pin and hinge constraints.
Private leaf module: import via the ``simvx.core.physics`` facade.
"""
import math
import numpy as np
from ..math.types import Quat, Vec2, Vec3
from ..nodes_2d.node2d import Node2D
from ..nodes_3d.node3d import Node3D
from ._material import BodyMode
from ._server import PhysicsServer
__all__ = [
"Joint2D",
"Joint3D",
"PinJoint2D",
"PinJoint3D",
"HingeJoint3D",
]
# ============================================================================
# Joints: Constraint solving
# ============================================================================
[docs]
class Joint2D(Node2D):
"""Base class for 2D physics joints.
Connects two physics bodies with a constraint. Registers with the
PhysicsServer so constraints are solved during the physics step,
between velocity integration and position integration.
Attributes:
body_a: First connected body (set after adding to tree).
body_b: Second connected body (set after adding to tree).
"""
def __init__(self, body_a=None, body_b=None, **kwargs):
super().__init__(**kwargs)
self.body_a = body_a
self.body_b = body_b
def _solve_constraint(self, dt: float) -> None:
"""Override to implement velocity-level constraint solving."""
pass
def _position_correct(self, dt: float) -> None:
"""Override to implement post-integration position projection."""
pass
[docs]
def on_enter_tree(self) -> None:
PhysicsServer.get().add_joint(self)
[docs]
def on_exit_tree(self) -> None:
PhysicsServer.get().remove_joint(self)
[docs]
class Joint3D(Node3D):
"""Base class for 3D physics joints.
Connects two physics bodies with a constraint. Registers with the
PhysicsServer so constraints are solved during the physics step.
"""
def __init__(self, body_a=None, body_b=None, **kwargs):
super().__init__(**kwargs)
self.body_a = body_a
self.body_b = body_b
def _solve_constraint(self, dt: float) -> None:
"""Override to implement velocity-level constraint solving."""
pass
def _position_correct(self, dt: float) -> None:
"""Override to implement post-integration position projection."""
pass
[docs]
def on_enter_tree(self) -> None:
PhysicsServer.get().add_joint(self)
[docs]
def on_exit_tree(self) -> None:
PhysicsServer.get().remove_joint(self)
[docs]
class PinJoint2D(Joint2D):
"""Pin joint -- constrains two 2D bodies to maintain a fixed distance.
Uses velocity-level constraint solving with Baumgarte stabilization.
Solved by the PhysicsServer during its step, not in physics_process.
Attributes:
distance: Rest distance between anchors. 0 = compute from initial positions.
stiffness: Constraint stiffness [0..1]. 1 = rigid, lower = springy.
damping: Velocity damping factor for the constraint.
bias_factor: Baumgarte stabilization factor (higher = faster correction).
"""
def __init__(
self, body_a=None, body_b=None, distance: float = 0.0,
stiffness: float = 1.0, damping: float = 0.5, bias_factor: float = 0.3, **kwargs,
):
super().__init__(body_a=body_a, body_b=body_b, **kwargs)
self._rest_distance = distance
self.stiffness = stiffness
self.damping = damping
self.bias_factor = bias_factor
self._initialized = False
def _ensure_init(self):
if self._initialized or not self.body_a or not self.body_b:
return
if self._rest_distance <= 0:
diff = self.body_a.position - self.body_b.position
self._rest_distance = max(diff.length(), 1e-6)
self._initialized = True
def _solve_constraint(self, dt: float) -> None:
self._ensure_init()
if self._rest_distance <= 0:
return
pos_a, pos_b = self.body_a.position, self.body_b.position
diff = pos_b - pos_a
dist = diff.length()
if dist < 1e-10:
return
normal = diff * (1.0 / dist)
error = dist - self._rest_distance
mode_a = getattr(self.body_a, "mode", BodyMode.DYNAMIC)
mode_b = getattr(self.body_b, "mode", BodyMode.DYNAMIC)
mass_a = getattr(self.body_a, "mass", 1.0) if mode_a == BodyMode.DYNAMIC else 0.0
mass_b = getattr(self.body_b, "mass", 1.0) if mode_b == BodyMode.DYNAMIC else 0.0
inv_a = (1.0 / mass_a) if mass_a > 0 else 0.0
inv_b = (1.0 / mass_b) if mass_b > 0 else 0.0
total_inv = inv_a + inv_b
if total_inv == 0:
return
# Velocity-level constraint: correct relative velocity along constraint axis
vel_a = self.body_a.linear_velocity if hasattr(self.body_a, "linear_velocity") else Vec2()
vel_b = self.body_b.linear_velocity if hasattr(self.body_b, "linear_velocity") else Vec2()
rel_vel = Vec2(vel_b.x - vel_a.x, vel_b.y - vel_a.y)
vn = rel_vel.dot(normal)
# Baumgarte bias: feeds position error back into velocity constraint
bias = self.bias_factor * error / max(dt, 1e-6)
# Impulse magnitude: corrects velocity along normal + bias for position error
impulse_mag = -(vn + bias) * self.stiffness / total_inv
impulse = normal * impulse_mag
if mode_a == BodyMode.DYNAMIC:
self.body_a.linear_velocity = Vec2(vel_a.x - impulse.x * inv_a, vel_a.y - impulse.y * inv_a)
if mode_b == BodyMode.DYNAMIC:
self.body_b.linear_velocity = Vec2(vel_b.x + impulse.x * inv_b, vel_b.y + impulse.y * inv_b)
[docs]
class PinJoint3D(Joint3D):
"""Pin joint -- constrains two 3D bodies to maintain a fixed distance.
Uses velocity-level constraint solving with Baumgarte stabilization.
Solved by the PhysicsServer during its step.
Attributes:
distance: Rest distance. 0 = auto-compute from initial positions.
stiffness: Constraint stiffness [0..1].
damping: Velocity damping along the constraint axis.
bias_factor: Baumgarte stabilization factor.
"""
def __init__(
self, body_a=None, body_b=None, distance: float = 0.0,
stiffness: float = 1.0, damping: float = 0.5, bias_factor: float = 0.3, **kwargs,
):
super().__init__(body_a=body_a, body_b=body_b, **kwargs)
self._rest_distance = distance
self.stiffness = stiffness
self.damping = damping
self.bias_factor = bias_factor
self._initialized = False
def _ensure_init(self):
if self._initialized or not self.body_a or not self.body_b:
return
if self._rest_distance <= 0:
diff = self.body_a.position - self.body_b.position
self._rest_distance = max(diff.length(), 1e-6)
self._initialized = True
def _solve_constraint(self, dt: float) -> None:
self._ensure_init()
if self._rest_distance <= 0:
return
pos_a, pos_b = self.body_a.position, self.body_b.position
diff = pos_b - pos_a
dist = diff.length()
if dist < 1e-10:
return
normal = diff * (1.0 / dist)
error = dist - self._rest_distance
mode_a = getattr(self.body_a, "mode", BodyMode.DYNAMIC)
mode_b = getattr(self.body_b, "mode", BodyMode.DYNAMIC)
mass_a = getattr(self.body_a, "mass", 1.0) if mode_a == BodyMode.DYNAMIC else 0.0
mass_b = getattr(self.body_b, "mass", 1.0) if mode_b == BodyMode.DYNAMIC else 0.0
inv_a = (1.0 / mass_a) if mass_a > 0 else 0.0
inv_b = (1.0 / mass_b) if mass_b > 0 else 0.0
total_inv = inv_a + inv_b
if total_inv == 0:
return
# Velocity-level constraint with Baumgarte bias
vel_a = self.body_a.linear_velocity if hasattr(self.body_a, "linear_velocity") else Vec3()
vel_b = self.body_b.linear_velocity if hasattr(self.body_b, "linear_velocity") else Vec3()
rel_vel = vel_b - vel_a
vn = rel_vel.dot(normal)
bias = self.bias_factor * error / max(dt, 1e-6)
impulse_mag = -(vn + bias) * self.stiffness / total_inv
impulse = normal * impulse_mag
if mode_a == BodyMode.DYNAMIC:
self.body_a.linear_velocity = vel_a - impulse * inv_a
if mode_b == BodyMode.DYNAMIC:
self.body_b.linear_velocity = vel_b + impulse * inv_b
[docs]
class HingeJoint3D(Joint3D):
"""Hinge joint -- constrains two 3D bodies to rotate around a shared axis.
Maintains a fixed distance in the hinge plane and restricts motion to
rotation around a single axis. Two-phase solver:
1. _solve_constraint (pre-integration): velocity-level Baumgarte correction
constrains velocity to be tangential and in the hinge plane.
2. _position_correct (post-integration): hard position projection snaps
the body back to the correct distance on the constraint manifold.
Attributes:
axis: Hinge rotation axis in world space (normalized).
distance: Rest distance between body centres.
stiffness: Position constraint stiffness.
damping: Velocity damping.
bias_factor: Baumgarte stabilization factor.
angular_limit_min: Minimum angle in degrees (0 = no limit).
angular_limit_max: Maximum angle in degrees (0 = no limit).
"""
def __init__(
self, body_a=None, body_b=None, axis=None, distance: float = 0.0,
stiffness: float = 1.0, damping: float = 0.5, bias_factor: float = 0.3,
angular_limit_min: float = 0.0, angular_limit_max: float = 0.0, **kwargs,
):
super().__init__(body_a=body_a, body_b=body_b, **kwargs)
self.axis = Vec3(axis) if axis else Vec3(0, 1, 0)
self._rest_distance = distance
self.stiffness = stiffness
self.damping = damping
self.bias_factor = bias_factor
self.angular_limit_min = angular_limit_min
self.angular_limit_max = angular_limit_max
self._reference_angle: float = 0.0
self._initialized = False
def _ensure_init(self):
if self._initialized or not self.body_a or not self.body_b:
return
if self._rest_distance <= 0:
diff = self.body_a.position - self.body_b.position
self._rest_distance = max(diff.length(), 1e-6)
# Capture reference angle for angular limits
rel_q = self.body_a.rotation.inverse() * self.body_b.rotation
dot = rel_q.x * self.axis.x + rel_q.y * self.axis.y + rel_q.z * self.axis.z
self._reference_angle = 2.0 * math.atan2(dot, rel_q.w)
self._initialized = True
[docs]
def reset(self) -> None:
"""Reset cached initialisation state.
Forces the next physics tick to recapture the reference angle and the
initial lever-arm direction from current body positions/rotations.
Call this after teleporting either body or resetting their orientation
so the hinge does not retain a stale rest configuration.
"""
self._initialized = False
if hasattr(self, "_init_dir"):
del self._init_dir
def _axis_n(self) -> np.ndarray:
"""Return normalized hinge axis as numpy array."""
a = np.array([self.axis.x, self.axis.y, self.axis.z], dtype=np.float32)
length = float(np.linalg.norm(a))
return a / length if length > 1e-10 else a
def _inv_masses(self):
"""Return (mode_a, mode_b, inv_a, inv_b, total_inv)."""
mode_a = getattr(self.body_a, "mode", BodyMode.DYNAMIC)
mode_b = getattr(self.body_b, "mode", BodyMode.DYNAMIC)
mass_a = getattr(self.body_a, "mass", 1.0) if mode_a == BodyMode.DYNAMIC else 0.0
mass_b = getattr(self.body_b, "mass", 1.0) if mode_b == BodyMode.DYNAMIC else 0.0
inv_a = (1.0 / mass_a) if mass_a > 0 else 0.0
inv_b = (1.0 / mass_b) if mass_b > 0 else 0.0
return mode_a, mode_b, inv_a, inv_b, inv_a + inv_b
def _solve_constraint(self, dt: float) -> None:
"""Velocity-level constraint: project velocity to be tangential and in the hinge plane."""
self._ensure_init()
mode_a, mode_b, inv_a, inv_b, total_inv = self._inv_masses()
if total_inv == 0:
return
axis_n = self._axis_n()
pos_a = np.array([self.body_a.position.x, self.body_a.position.y, self.body_a.position.z], dtype=np.float32)
pos_b = np.array([self.body_b.position.x, self.body_b.position.y, self.body_b.position.z], dtype=np.float32)
diff = pos_b - pos_a
# Lever arm in hinge plane (remove axis component)
along_axis = float(np.dot(diff, axis_n))
diff_planar = diff - axis_n * along_axis
planar_dist = float(np.linalg.norm(diff_planar))
# --- Constrain velocity of dynamic bodies ---
if mode_b == BodyMode.DYNAMIC and hasattr(self.body_b, "linear_velocity"):
vel_b = self.body_b.linear_velocity
vel_arr = np.array([vel_b.x, vel_b.y, vel_b.z], dtype=np.float32)
if planar_dist > 1e-10:
radial = diff_planar / planar_dist
# Remove radial component (keep only tangential)
vel_arr -= radial * float(np.dot(vel_arr, radial))
# Remove axis component (body stays in hinge plane)
vel_arr -= axis_n * float(np.dot(vel_arr, axis_n))
# Baumgarte bias: feed distance error back as radial velocity correction
if self._rest_distance > 0 and planar_dist > 1e-10:
dist_error = planar_dist - self._rest_distance
bias = self.bias_factor * dist_error / max(dt, 1e-6)
radial = diff_planar / planar_dist
vel_arr -= radial * bias * (inv_b / total_inv)
# Baumgarte bias for axis drift (push back towards hinge plane)
if abs(along_axis) > 1e-10:
axis_bias = self.bias_factor * along_axis / max(dt, 1e-6)
vel_arr -= axis_n * axis_bias * (inv_b / total_inv)
self.body_b.linear_velocity = Vec3(float(vel_arr[0]), float(vel_arr[1]), float(vel_arr[2]))
if mode_a == BodyMode.DYNAMIC and hasattr(self.body_a, "linear_velocity"):
vel_a = self.body_a.linear_velocity
vel_arr = np.array([vel_a.x, vel_a.y, vel_a.z], dtype=np.float32)
if planar_dist > 1e-10:
radial = diff_planar / planar_dist
vel_arr -= radial * float(np.dot(vel_arr, radial))
vel_arr -= axis_n * float(np.dot(vel_arr, axis_n))
if self._rest_distance > 0 and planar_dist > 1e-10:
dist_error = planar_dist - self._rest_distance
bias = self.bias_factor * dist_error / max(dt, 1e-6)
radial = diff_planar / planar_dist
vel_arr += radial * bias * (inv_a / total_inv)
if abs(along_axis) > 1e-10:
axis_bias = self.bias_factor * along_axis / max(dt, 1e-6)
vel_arr += axis_n * axis_bias * (inv_a / total_inv)
self.body_a.linear_velocity = Vec3(float(vel_arr[0]), float(vel_arr[1]), float(vel_arr[2]))
# --- Angular constraint: project angular velocity onto hinge axis only ---
for body, mode in [(self.body_b, mode_b), (self.body_a, mode_a)]:
if mode != BodyMode.DYNAMIC:
continue
if not hasattr(body, "angular_velocity") or not isinstance(body.angular_velocity, Vec3):
continue
av = body.angular_velocity
av_arr = np.array([av.x, av.y, av.z], dtype=np.float32)
along_av = float(np.dot(av_arr, axis_n))
body.angular_velocity = Vec3(*(axis_n * along_av))
# --- Angular limits (degrees, 0 = disabled) ---
if self.angular_limit_min != 0 or self.angular_limit_max != 0:
self._enforce_angular_limits(mode_a, mode_b, inv_a, inv_b, total_inv, axis_n)
def _position_correct(self, dt: float) -> None:
"""Post-integration position projection: snap body back to hinge constraint manifold.
This runs AFTER position integration, so it gets the final word on where
the body actually ends up. Standard practice in all real physics engines.
"""
self._ensure_init()
mode_a, mode_b, inv_a, inv_b, total_inv = self._inv_masses()
if total_inv == 0:
return
axis_n = self._axis_n()
pos_a = np.array([self.body_a.position.x, self.body_a.position.y, self.body_a.position.z], dtype=np.float32)
pos_b = np.array([self.body_b.position.x, self.body_b.position.y, self.body_b.position.z], dtype=np.float32)
diff = pos_b - pos_a
# Decompose into axis and planar components
along_axis = float(np.dot(diff, axis_n))
diff_planar = diff - axis_n * along_axis
planar_dist = float(np.linalg.norm(diff_planar))
if self._rest_distance <= 0:
return
# --- 1. Project to correct distance in hinge plane ---
if planar_dist > 1e-10:
radial = diff_planar / planar_dist
target_planar = radial * self._rest_distance
else:
# Degenerate: body exactly on axis. Pick an arbitrary direction in the plane.
perp = np.array([1.0, 0.0, 0.0], dtype=np.float32)
if abs(float(np.dot(perp, axis_n))) > 0.9:
perp = np.array([0.0, 0.0, 1.0], dtype=np.float32)
perp = perp - axis_n * float(np.dot(perp, axis_n))
perp = perp / float(np.linalg.norm(perp))
target_planar = perp * self._rest_distance
# Target position: anchor + planar offset (no axis component: stays in hinge plane)
target_b = pos_a + target_planar
correction = target_b - pos_b
# Distribute correction by inverse mass ratio
if mode_b == BodyMode.DYNAMIC:
new_b = pos_b + correction * (inv_b / total_inv)
self.body_b.position = Vec3(float(new_b[0]), float(new_b[1]), float(new_b[2]))
if mode_a == BodyMode.DYNAMIC:
new_a = pos_a - correction * (inv_a / total_inv)
self.body_a.position = Vec3(float(new_a[0]), float(new_a[1]), float(new_a[2]))
# --- 2. Set body rotation from orbital angle ---
# The hinge fully determines rotation: the body's Y-axis rotation must
# match the angle of the lever arm in the hinge plane.
if mode_b == BodyMode.DYNAMIC and hasattr(self.body_b, "rotation"):
corrected_b = np.array([self.body_b.position.x, self.body_b.position.y, self.body_b.position.z], dtype=np.float32)
corrected_a = np.array([self.body_a.position.x, self.body_a.position.y, self.body_a.position.z], dtype=np.float32)
arm = corrected_b - corrected_a
arm_planar = arm - axis_n * float(np.dot(arm, axis_n))
arm_len = float(np.linalg.norm(arm_planar))
if arm_len > 1e-10:
# Compute angle between current arm and initial arm direction
# Initial arm is along the direction at rest_distance when joint was created
if not hasattr(self, "_init_dir"):
self._init_dir = arm_planar / arm_len
init_dir = self._init_dir
cur_dir = arm_planar / arm_len
# Signed angle between init_dir and cur_dir around the hinge axis
cos_a = float(np.dot(init_dir, cur_dir))
cross = np.cross(init_dir, cur_dir)
sin_a = float(np.dot(cross, axis_n))
angle = math.atan2(sin_a, cos_a)
self.body_b.rotation = Quat.from_axis_angle(self.axis, angle)
# --- 3. Correct velocity to match the position correction ---
# After snapping position, the radial velocity component is now invalid.
# Re-project velocity to be tangential to the corrected lever arm.
for body, mode, inv_m in [(self.body_b, mode_b, inv_b), (self.body_a, mode_a, inv_a)]:
if mode != BodyMode.DYNAMIC or not hasattr(body, "linear_velocity"):
continue
vel = body.linear_velocity
vel_arr = np.array([vel.x, vel.y, vel.z], dtype=np.float32)
# Recompute lever arm from corrected positions
bp = np.array([body.position.x, body.position.y, body.position.z], dtype=np.float32)
ap = np.array([self.body_a.position.x, self.body_a.position.y, self.body_a.position.z], dtype=np.float32)
arm = bp - ap if body is self.body_b else ap - bp
arm_planar = arm - axis_n * float(np.dot(arm, axis_n))
arm_len = float(np.linalg.norm(arm_planar))
if arm_len > 1e-10:
radial = arm_planar / arm_len
vel_arr -= radial * float(np.dot(vel_arr, radial))
vel_arr -= axis_n * float(np.dot(vel_arr, axis_n))
body.linear_velocity = Vec3(float(vel_arr[0]), float(vel_arr[1]), float(vel_arr[2]))
def _enforce_angular_limits(self, mode_a, mode_b, inv_a, inv_b, total_inv, axis_n):
"""Clamp relative rotation to angular limit range."""
rel_q = self.body_a.rotation.inverse() * self.body_b.rotation
dot = rel_q.x * self.axis.x + rel_q.y * self.axis.y + rel_q.z * self.axis.z
twist_angle = 2.0 * math.atan2(dot, rel_q.w)
twist_angle = math.atan2(math.sin(twist_angle), math.cos(twist_angle))
current_angle = twist_angle - self._reference_angle
current_angle = math.atan2(math.sin(current_angle), math.cos(current_angle))
limit_min_rad = math.radians(self.angular_limit_min)
limit_max_rad = math.radians(self.angular_limit_max)
if current_angle < limit_min_rad:
correction = limit_min_rad - current_angle
elif current_angle > limit_max_rad:
correction = limit_max_rad - current_angle
else:
return
if abs(correction) < 1e-6:
return
axis_vec = self.axis
if mode_a == BodyMode.DYNAMIC:
self.body_a.rotation = self.body_a.rotation.rotate(axis_vec, -correction * inv_a / total_inv)
if mode_b == BodyMode.DYNAMIC:
self.body_b.rotation = self.body_b.rotation.rotate(axis_vec, correction * inv_b / total_inv)
# Damp axis-aligned angular velocity that pushes further past the limit.
# Velocity that's *leaving* the limit is preserved so the body can return.
# `damping` doubles as a restitution coefficient in [0, 1]:
# 0.0 -> hard stop (push-through velocity zeroed)
# 1.0 -> elastic bounce (push-through velocity reflected)
restitution = max(0.0, min(1.0, float(self.damping)))
violating_sign = 1.0 if correction < 0 else -1.0 # direction the body was moving past the limit
for body, mode in [(self.body_a, mode_a), (self.body_b, mode_b)]:
if mode != BodyMode.DYNAMIC:
continue
av = body.angular_velocity
av_arr = np.array([av.x, av.y, av.z], dtype=np.float32)
along = float(np.dot(av_arr, axis_n))
if along * violating_sign <= 0.0:
continue # velocity is already leaving the limit; leave it alone
new_along = -restitution * along
body.angular_velocity = Vec3(*(av_arr - axis_n * (along - new_along)))