From 4d5e87f67ca740928a5145c1637380f1f69933dc Mon Sep 17 00:00:00 2001 From: stainlu Date: Thu, 9 Apr 2026 01:56:20 +0800 Subject: [PATCH] Add joint limits and motors to physics system Extend revolute joints with angle limits and optional motors, and distance joints with min/max length limits. This enables constrained hinges (e.g. door stops, elbow joints) and motor-driven rotation (e.g. wheels, powered mechanisms). - JointMotor struct drives revolute joints toward a target angular velocity - Distance limits define a free range; no correction within [min, max] - Angle limits clamp revolute rotation via position-level correction - solve() now receives rotations for angle-limit computation - solve_velocity() applies clamped motor impulses per timestep - Builder methods: with_angle_limits, with_motor, with_distance_limits - 16 unit tests covering limits, motors, convergence, and builder panics Co-Authored-By: Claude Sonnet 4.6 --- crates/euca-physics/src/joints.rs | 439 ++++++++++++++++++++++++++- crates/euca-physics/src/lib.rs | 2 +- crates/euca-physics/src/systems.rs | 62 +++- crates/euca-render/src/renderer.rs | 81 ++--- crates/euca-rhi/src/metal_backend.rs | 4 +- 5 files changed, 530 insertions(+), 58 deletions(-) diff --git a/crates/euca-physics/src/joints.rs b/crates/euca-physics/src/joints.rs index 8b75267..f8bc149 100644 --- a/crates/euca-physics/src/joints.rs +++ b/crates/euca-physics/src/joints.rs @@ -4,7 +4,16 @@ //! Resolved during the constraint solver iterations alongside contact constraints. use euca_ecs::Entity; -use euca_math::Vec3; +use euca_math::{Quat, Vec3}; + +/// Motor that drives a revolute joint toward a target angular velocity. +#[derive(Clone, Copy, Debug)] +pub struct JointMotor { + /// Target angular velocity in radians per second. + pub target_velocity: f32, + /// Maximum torque the motor can exert (N-m). + pub max_torque: f32, +} /// A joint connecting two entities. #[derive(Clone, Debug)] @@ -28,6 +37,10 @@ pub enum JointKind { anchor_a: Vec3, anchor_b: Vec3, rest_length: f32, + /// Minimum allowed distance (`None` = can compress to zero). + min_length: Option, + /// Maximum allowed distance (`None` = can stretch infinitely). + max_length: Option, }, /// Ball-and-socket: constrains position but allows free rotation. /// Anchors are local-space attachment points. @@ -37,6 +50,12 @@ pub enum JointKind { anchor_a: Vec3, anchor_b: Vec3, axis: Vec3, + /// Lower angle limit in radians (`None` = no limit). + min_angle: Option, + /// Upper angle limit in radians (`None` = no limit). + max_angle: Option, + /// Optional motor driving the joint. + motor: Option, }, } @@ -56,6 +75,8 @@ impl Joint { anchor_a, anchor_b, rest_length, + min_length: None, + max_length: None, }, stiffness: 1.0, } @@ -91,6 +112,9 @@ impl Joint { anchor_a, anchor_b, axis, + min_angle: None, + max_angle: None, + motor: None, }, stiffness: 1.0, } @@ -102,12 +126,76 @@ impl Joint { self } - /// Compute the position correction for this joint given current positions. - /// Returns (correction_a, correction_b) — position deltas to apply. + /// Set angle limits on a revolute joint (radians). + /// + /// # Panics + /// Panics if this joint is not `JointKind::Revolute`. + pub fn with_angle_limits(mut self, min: f32, max: f32) -> Self { + match &mut self.kind { + JointKind::Revolute { + min_angle, + max_angle, + .. + } => { + *min_angle = Some(min); + *max_angle = Some(max); + } + _ => panic!("with_angle_limits called on non-Revolute joint"), + } + self + } + + /// Attach a motor to a revolute joint. + /// + /// # Panics + /// Panics if this joint is not `JointKind::Revolute`. + pub fn with_motor(mut self, target_velocity: f32, max_torque: f32) -> Self { + match &mut self.kind { + JointKind::Revolute { motor, .. } => { + *motor = Some(JointMotor { + target_velocity, + max_torque, + }); + } + _ => panic!("with_motor called on non-Revolute joint"), + } + self + } + + /// Set distance limits on a distance joint. + /// + /// # Panics + /// Panics if this joint is not `JointKind::Distance`. + pub fn with_distance_limits(mut self, min: f32, max: f32) -> Self { + match &mut self.kind { + JointKind::Distance { + min_length, + max_length, + .. + } => { + *min_length = Some(min); + *max_length = Some(max); + } + _ => panic!("with_distance_limits called on non-Distance joint"), + } + self + } + + /// Returns `true` if this joint has a motor attached. + pub fn has_motor(&self) -> bool { + matches!(&self.kind, JointKind::Revolute { motor: Some(_), .. }) + } + + /// Compute the position correction for this joint given current positions + /// and rotations. + /// + /// Returns `(correction_a, correction_b)` -- position deltas to apply. pub fn solve( &self, pos_a: Vec3, pos_b: Vec3, + rot_a: Quat, + rot_b: Quat, is_a_dynamic: bool, is_b_dynamic: bool, ) -> (Vec3, Vec3) { @@ -116,6 +204,8 @@ impl Joint { anchor_a, anchor_b, rest_length, + min_length, + max_length, } => { let world_a = pos_a + *anchor_a; let world_b = pos_b + *anchor_b; @@ -124,7 +214,26 @@ impl Joint { if dist < 1e-8 { return (Vec3::ZERO, Vec3::ZERO); } - let error = dist - rest_length; + + // Determine target distance based on limits. + let target = if min_length.is_some() || max_length.is_some() { + // With limits: free range between min and max. + let lo = min_length.unwrap_or(0.0); + let hi = max_length.unwrap_or(f32::MAX); + if dist < lo { + lo + } else if dist > hi { + hi + } else { + // Within free range -- no correction. + return (Vec3::ZERO, Vec3::ZERO); + } + } else { + // No limits: maintain rest length. + *rest_length + }; + + let error = dist - target; let dir = delta * (1.0 / dist); let correction = dir * error * self.stiffness; @@ -142,6 +251,9 @@ impl Joint { anchor_a, anchor_b, axis, + min_angle, + max_angle, + .. } => { // Position constraint: same as ball-and-socket let world_a = pos_a + *anchor_a; @@ -152,12 +264,110 @@ impl Joint { // (allow movement along the axis, correct perpendicular error) let axis_component = *axis * delta.dot(*axis); let perp_error = delta - axis_component; - let correction = perp_error * self.stiffness; + let mut correction = perp_error * self.stiffness; + + // Angle limit enforcement via position-level correction. + if min_angle.is_some() || max_angle.is_some() { + let angle = relative_hinge_angle(rot_a, rot_b, *axis); + let lo = min_angle.unwrap_or(-std::f32::consts::PI); + let hi = max_angle.unwrap_or(std::f32::consts::PI); + let angle_error = if angle < lo { + lo - angle + } else if angle > hi { + hi - angle + } else { + 0.0 + }; + if angle_error.abs() > 1e-6 { + // Convert angular error into a positional correction + // using the anchor offset as the lever arm. + let arm = world_b - pos_b; + let arm_len = arm.length(); + let lever = arm_len.max(0.01); + let linear_error = angle_error * lever; + let lever_dir = if arm_len > 1e-6 { + arm.normalize() + } else { + perpendicular_to(*axis) + }; + let cross = axis.cross(lever_dir); + let correction_dir = if cross.length() > 1e-6 { + cross.normalize() + } else { + Vec3::ZERO + }; + correction = correction + correction_dir * linear_error * self.stiffness; + } + } distribute_correction(correction, is_a_dynamic, is_b_dynamic) } } } + + /// Compute velocity-level corrections for motor-driven joints. + /// + /// For revolute joints with a motor, returns the angular impulses to apply to + /// body A and body B. Returns `(Vec3::ZERO, Vec3::ZERO)` if no motor is set. + pub fn solve_velocity( + &self, + angular_vel_a: Vec3, + angular_vel_b: Vec3, + is_a_dynamic: bool, + is_b_dynamic: bool, + dt: f32, + ) -> (Vec3, Vec3) { + match &self.kind { + JointKind::Revolute { axis, motor, .. } => { + let motor = match motor { + Some(m) => m, + None => return (Vec3::ZERO, Vec3::ZERO), + }; + let current_rel_vel = (angular_vel_b - angular_vel_a).dot(*axis); + let vel_error = motor.target_velocity - current_rel_vel; + let impulse_magnitude = (vel_error * self.stiffness) + .clamp(-motor.max_torque * dt, motor.max_torque * dt); + let impulse = *axis * impulse_magnitude; + // Motor drives B toward target relative to A: B gets positive + // impulse along axis, A gets the reaction (opposite sign). + distribute_angular_impulse(impulse, is_a_dynamic, is_b_dynamic) + } + _ => (Vec3::ZERO, Vec3::ZERO), + } + } +} + +/// Compute the relative rotation angle between two bodies projected onto a hinge axis. +/// +/// Returns the signed angle (in radians) of `rot_b` relative to `rot_a` around `axis`. +fn relative_hinge_angle(rot_a: Quat, rot_b: Quat, axis: Vec3) -> f32 { + let rel = rot_a.inverse() * rot_b; + // Extract the angle around the hinge axis from the quaternion's vector part. + let proj = Vec3::new(rel.x, rel.y, rel.z).dot(axis); + proj.atan2(rel.w) * 2.0 +} + +/// Return a unit vector perpendicular to the given axis. +fn perpendicular_to(axis: Vec3) -> Vec3 { + let candidate = if axis.x.abs() < 0.9 { Vec3::X } else { Vec3::Y }; + axis.cross(candidate).normalize() +} + +/// Distribute an angular impulse between two bodies. +/// +/// Body B receives the impulse (driven), body A receives the reaction (negated). +/// When both are dynamic, the impulse is split equally. +fn distribute_angular_impulse( + impulse: Vec3, + is_a_dynamic: bool, + is_b_dynamic: bool, +) -> (Vec3, Vec3) { + match (is_a_dynamic, is_b_dynamic) { + (true, true) => (impulse * (-0.5), impulse * 0.5), + (true, false) => (impulse * (-1.0), Vec3::ZERO), + (false, true) => (Vec3::ZERO, impulse), + (false, false) => (Vec3::ZERO, Vec3::ZERO), + } } /// Distribute a correction between two bodies based on their dynamic status. @@ -173,17 +383,23 @@ fn distribute_correction(correction: Vec3, is_a_dynamic: bool, is_b_dynamic: boo #[cfg(test)] mod tests { use super::*; + use std::f32::consts::{FRAC_PI_4, PI}; fn make_entity(index: u32) -> Entity { Entity::from_raw(index, 0) } + // Identity rotation shorthand for tests that don't involve rotation. + const ID: Quat = Quat::IDENTITY; + + // ── Existing tests (unchanged behavior) ──────────────────────────────── + #[test] fn distance_joint_at_rest() { let joint = Joint::distance(make_entity(0), make_entity(1), Vec3::ZERO, Vec3::ZERO, 5.0); - // Bodies are exactly rest_length apart — no correction - let (ca, cb) = joint.solve(Vec3::ZERO, Vec3::new(5.0, 0.0, 0.0), true, true); + // Bodies are exactly rest_length apart -- no correction + let (ca, cb) = joint.solve(Vec3::ZERO, Vec3::new(5.0, 0.0, 0.0), ID, ID, true, true); assert!(ca.length() < 1e-5); assert!(cb.length() < 1e-5); } @@ -192,8 +408,8 @@ mod tests { fn distance_joint_stretched() { let joint = Joint::distance(make_entity(0), make_entity(1), Vec3::ZERO, Vec3::ZERO, 5.0); - // Bodies are 10 apart but rest length is 5 — should pull together - let (ca, cb) = joint.solve(Vec3::ZERO, Vec3::new(10.0, 0.0, 0.0), true, true); + // Bodies are 10 apart but rest length is 5 -- should pull together + let (ca, cb) = joint.solve(Vec3::ZERO, Vec3::new(10.0, 0.0, 0.0), ID, ID, true, true); assert!(ca.x > 0.0, "A should move toward B"); assert!(cb.x < 0.0, "B should move toward A"); } @@ -209,7 +425,7 @@ mod tests { // Anchors should meet: A at (0,0,0)+anchor = (1,0,0), B at (3,0,0)+anchor = (2,0,0) // Error = B_anchor - A_anchor = (2,0,0)-(1,0,0) = (1,0,0) - let (ca, cb) = joint.solve(Vec3::ZERO, Vec3::new(3.0, 0.0, 0.0), true, true); + let (ca, cb) = joint.solve(Vec3::ZERO, Vec3::new(3.0, 0.0, 0.0), ID, ID, true, true); assert!(ca.x > 0.0); assert!(cb.x < 0.0); } @@ -218,9 +434,208 @@ mod tests { fn static_body_doesnt_move() { let joint = Joint::distance(make_entity(0), make_entity(1), Vec3::ZERO, Vec3::ZERO, 5.0); - // A is static — only B should receive correction - let (ca, cb) = joint.solve(Vec3::ZERO, Vec3::new(10.0, 0.0, 0.0), false, true); + // A is static -- only B should receive correction + let (ca, cb) = joint.solve(Vec3::ZERO, Vec3::new(10.0, 0.0, 0.0), ID, ID, false, true); assert!(ca.length() < 1e-5, "Static body shouldn't move"); assert!(cb.x < 0.0, "Dynamic body should be pulled"); } + + // ── Distance joint with limits ───────────────────────────────────────── + + #[test] + fn distance_joint_with_limits_at_rest() { + // Within the [3, 7] free range at distance 5 -- no correction. + let joint = Joint::distance(make_entity(0), make_entity(1), Vec3::ZERO, Vec3::ZERO, 5.0) + .with_distance_limits(3.0, 7.0); + + let (ca, cb) = joint.solve(Vec3::ZERO, Vec3::new(5.0, 0.0, 0.0), ID, ID, true, true); + assert!(ca.length() < 1e-5); + assert!(cb.length() < 1e-5); + } + + #[test] + fn distance_joint_stretched_beyond_max() { + // Distance 10.0 exceeds max_length 7.0 -- should correct toward 7.0. + let joint = Joint::distance(make_entity(0), make_entity(1), Vec3::ZERO, Vec3::ZERO, 5.0) + .with_distance_limits(3.0, 7.0); + + let (ca, cb) = joint.solve(Vec3::ZERO, Vec3::new(10.0, 0.0, 0.0), ID, ID, true, true); + assert!(ca.x > 0.0, "A should move toward B"); + assert!(cb.x < 0.0, "B should move toward A"); + // Correction magnitude should correspond to error of 3.0 (10-7), not 5.0 (10-5). + let total = (ca - cb).length(); + assert!( + (total - 3.0).abs() < 1e-4, + "total correction should be 3.0, got {total}" + ); + } + + #[test] + fn distance_joint_compressed_below_min() { + // Distance 2.0 is below min_length 3.0 -- should push apart. + let joint = Joint::distance(make_entity(0), make_entity(1), Vec3::ZERO, Vec3::ZERO, 5.0) + .with_distance_limits(3.0, 7.0); + + let (ca, cb) = joint.solve(Vec3::ZERO, Vec3::new(2.0, 0.0, 0.0), ID, ID, true, true); + assert!(ca.x < 0.0, "A should move away from B"); + assert!(cb.x > 0.0, "B should move away from A"); + } + + #[test] + fn distance_joint_without_limits_preserves_behavior() { + // No limits set -- behaves exactly as rest_length constraint. + let joint = Joint::distance(make_entity(0), make_entity(1), Vec3::ZERO, Vec3::ZERO, 5.0); + + let (ca, cb) = joint.solve(Vec3::ZERO, Vec3::new(10.0, 0.0, 0.0), ID, ID, true, true); + let total = (ca - cb).length(); + assert!( + (total - 5.0).abs() < 1e-4, + "should correct full error to rest_length" + ); + } + + // ── Revolute joint with angle limits ─────────────────────────────────── + + #[test] + fn revolute_angle_within_limits_no_correction() { + // Both anchors at origin, bodies co-located -- positional error is zero. + let joint = Joint::revolute( + make_entity(0), + make_entity(1), + Vec3::ZERO, + Vec3::ZERO, + Vec3::Y, + ) + .with_angle_limits(-FRAC_PI_4, FRAC_PI_4); + + // Both at identity -- relative angle is 0, well within limits. + let (ca, cb) = joint.solve(Vec3::ZERO, Vec3::ZERO, ID, ID, true, true); + // No positional error and angle is within limits -- zero correction. + assert!(ca.length() < 1e-4); + assert!(cb.length() < 1e-4); + } + + #[test] + fn revolute_angle_past_limit_gets_corrected() { + // B is rotated 90 degrees around Y, but limit is [-pi/4, pi/4]. + let joint = Joint::revolute( + make_entity(0), + make_entity(1), + Vec3::ZERO, + Vec3::new(1.0, 0.0, 0.0), + Vec3::Y, + ) + .with_angle_limits(-FRAC_PI_4, FRAC_PI_4); + + let rot_b = Quat::from_axis_angle(Vec3::Y, std::f32::consts::FRAC_PI_2); + let (ca, cb) = joint.solve(Vec3::ZERO, Vec3::new(1.0, 0.0, 0.0), ID, rot_b, true, true); + // Some angular correction should be produced. + let total = (ca - cb).length(); + assert!( + total > 1e-3, + "should produce correction for angle past limit" + ); + } + + // ── Revolute motor ───────────────────────────────────────────────────── + + #[test] + fn revolute_motor_applies_impulse() { + let joint = Joint::revolute( + make_entity(0), + make_entity(1), + Vec3::ZERO, + Vec3::new(1.0, 0.0, 0.0), + Vec3::Y, + ) + .with_motor(5.0, 100.0); + + // Both bodies at rest -- motor drives toward target_velocity 5.0 rad/s. + let dt = 1.0 / 60.0; + let (imp_a, imp_b) = joint.solve_velocity(Vec3::ZERO, Vec3::ZERO, true, true, dt); + // Impulse should be along the hinge axis (Y). + assert!( + imp_a.y.abs() > 1e-5 || imp_b.y.abs() > 1e-5, + "motor should produce Y-axis impulse" + ); + // A gets negative (reaction), B gets positive (driven). + assert!(imp_a.y < 0.0, "body A receives reaction impulse"); + assert!(imp_b.y > 0.0, "body B receives drive impulse"); + } + + #[test] + fn motor_respects_max_torque() { + let joint = Joint::revolute( + make_entity(0), + make_entity(1), + Vec3::ZERO, + Vec3::new(1.0, 0.0, 0.0), + Vec3::Y, + ) + .with_motor(1000.0, 2.0); // Huge velocity difference, tiny max torque. + + let dt = 1.0 / 60.0; + let (imp_a, imp_b) = joint.solve_velocity(Vec3::ZERO, Vec3::ZERO, true, true, dt); + let total = (imp_a - imp_b).length(); + // The total impulse magnitude must be <= max_torque * dt = 2.0 / 60.0. + let max_impulse = 2.0 * dt; + assert!( + total <= max_impulse + 1e-5, + "impulse {total} should not exceed max_torque * dt = {max_impulse}" + ); + } + + #[test] + fn motor_converges_over_time() { + let joint = Joint::revolute( + make_entity(0), + make_entity(1), + Vec3::ZERO, + Vec3::new(1.0, 0.0, 0.0), + Vec3::Y, + ) + .with_motor(5.0, 100.0); + + let dt = 1.0 / 60.0; + let mut ang_vel_b = Vec3::ZERO; + for _ in 0..120 { + let (_, imp_b) = joint.solve_velocity(Vec3::ZERO, ang_vel_b, false, true, dt); + ang_vel_b = ang_vel_b + imp_b; + } + // After 2 seconds at 60 Hz with plenty of torque, should approach target. + let rel_vel = ang_vel_b.dot(Vec3::Y); + assert!( + (rel_vel - 5.0).abs() < 0.5, + "relative velocity {rel_vel} should approach target 5.0" + ); + } + + // ── Builder panic tests ──────────────────────────────────────────────── + + #[test] + #[should_panic(expected = "with_angle_limits called on non-Revolute joint")] + fn angle_limits_on_distance_panics() { + Joint::distance(make_entity(0), make_entity(1), Vec3::ZERO, Vec3::ZERO, 5.0) + .with_angle_limits(-PI, PI); + } + + #[test] + #[should_panic(expected = "with_motor called on non-Revolute joint")] + fn motor_on_distance_panics() { + Joint::distance(make_entity(0), make_entity(1), Vec3::ZERO, Vec3::ZERO, 5.0) + .with_motor(1.0, 1.0); + } + + #[test] + #[should_panic(expected = "with_distance_limits called on non-Distance joint")] + fn distance_limits_on_revolute_panics() { + Joint::revolute( + make_entity(0), + make_entity(1), + Vec3::ZERO, + Vec3::ZERO, + Vec3::Y, + ) + .with_distance_limits(1.0, 5.0); + } } diff --git a/crates/euca-physics/src/lib.rs b/crates/euca-physics/src/lib.rs index 70314d7..4ebefe7 100644 --- a/crates/euca-physics/src/lib.rs +++ b/crates/euca-physics/src/lib.rs @@ -19,7 +19,7 @@ pub use components::{ CachedColliderShape, Collider, ColliderShape, CollisionEvent, Gravity, Mass, PhysicsBody, RigidBodyType, SLEEP_THRESHOLD, Sleeping, Velocity, layers_interact, }; -pub use joints::{Joint, JointKind}; +pub use joints::{Joint, JointKind, JointMotor}; pub use raycast::{ OverlapHit, Ray, RayHit, SweepHit, WorldRayHit, overlap_sphere, raycast_aabb, raycast_collider, raycast_sphere, raycast_world, sweep_sphere, diff --git a/crates/euca-physics/src/systems.rs b/crates/euca-physics/src/systems.rs index 7fa57cb..39f9982 100644 --- a/crates/euca-physics/src/systems.rs +++ b/crates/euca-physics/src/systems.rs @@ -49,7 +49,7 @@ fn physics_step_single(world: &mut World, dt: f32, gravity: Vec3) { sync_cached_shapes(world); apply_gravity(world, gravity, dt); integrate_positions(world, dt); - resolve_collisions_and_joints(world); + resolve_collisions_and_joints(world, dt); update_sleep_states(world); } @@ -70,14 +70,14 @@ pub fn sync_cached_shapes(world: &mut World) { } } -fn resolve_collisions_and_joints(world: &mut World) { +fn resolve_collisions_and_joints(world: &mut World, dt: f32) { // Collect joints (if any) let joints = world .resource::() .map(|j| j.joints.clone()) .unwrap_or_default(); - resolve_collisions_with_joints(world, &joints); + resolve_collisions_with_joints(world, &joints, dt); } fn apply_gravity(world: &mut World, gravity: Vec3, dt: f32) { @@ -702,7 +702,7 @@ fn solve_island( } } -fn resolve_collisions_with_joints(world: &mut World, joints: &[crate::joints::Joint]) { +fn resolve_collisions_with_joints(world: &mut World, joints: &[crate::joints::Joint], dt: f32) { // ── Iterative constraint solver ── // Collect bodies once, iterate position corrections in-place, // write back to world at the end. @@ -842,6 +842,14 @@ fn resolve_collisions_with_joints(world: &mut World, joints: &[crate::joints::Jo .map(|(i, b)| (b.entity, i)) .collect(); + // Read current rotations for joint entities (needed for angle limits). + let entity_rotations: std::collections::HashMap = joints + .iter() + .flat_map(|j| [j.entity_a, j.entity_b]) + .filter_map(|e| world.get::(e).map(|lt| (e, lt.0.rotation))) + .collect(); + + // Position solving iterations. for _iter in 0..SOLVER_ITERATIONS { for joint in joints { let idx_a = entity_to_idx.get(&joint.entity_a).copied(); @@ -856,7 +864,16 @@ fn resolve_collisions_with_joints(world: &mut World, joints: &[crate::joints::Jo None => continue, }; - let (ca, cb) = joint.solve(pos_a, pos_b, is_a_dyn, is_b_dyn); + let rot_a = entity_rotations + .get(&joint.entity_a) + .copied() + .unwrap_or(euca_math::Quat::IDENTITY); + let rot_b = entity_rotations + .get(&joint.entity_b) + .copied() + .unwrap_or(euca_math::Quat::IDENTITY); + + let (ca, cb) = joint.solve(pos_a, pos_b, rot_a, rot_b, is_a_dyn, is_b_dyn); if let Some(i) = idx_a { bodies[i].pos = bodies[i].pos + ca; @@ -866,6 +883,41 @@ fn resolve_collisions_with_joints(world: &mut World, joints: &[crate::joints::Jo } } } + + // Velocity solving: apply motor impulses (skip joints without motors). + for joint in joints.iter().filter(|j| j.has_motor()) { + let idx_a = entity_to_idx.get(&joint.entity_a).copied(); + let idx_b = entity_to_idx.get(&joint.entity_b).copied(); + + let is_a_dyn = idx_a + .map(|i| bodies[i].body_type == RigidBodyType::Dynamic) + .unwrap_or(false); + let is_b_dyn = idx_b + .map(|i| bodies[i].body_type == RigidBodyType::Dynamic) + .unwrap_or(false); + + let ang_vel_a = world + .get::(joint.entity_a) + .map(|v| v.angular) + .unwrap_or(Vec3::ZERO); + let ang_vel_b = world + .get::(joint.entity_b) + .map(|v| v.angular) + .unwrap_or(Vec3::ZERO); + + let (imp_a, imp_b) = joint.solve_velocity(ang_vel_a, ang_vel_b, is_a_dyn, is_b_dyn, dt); + + if imp_a.length_squared() > 1e-12 + && let Some(vel) = world.get_mut::(joint.entity_a) + { + vel.angular = vel.angular + imp_a; + } + if imp_b.length_squared() > 1e-12 + && let Some(vel) = world.get_mut::(joint.entity_b) + { + vel.angular = vel.angular + imp_b; + } + } } // Write solved positions back to world diff --git a/crates/euca-render/src/renderer.rs b/crates/euca-render/src/renderer.rs index b2fe66d..b007225 100644 --- a/crates/euca-render/src/renderer.rs +++ b/crates/euca-render/src/renderer.rs @@ -2316,7 +2316,11 @@ impl Renderer { self.metalfx_low_res_depth_view.as_ref().unwrap(), ) } else { - (&self.msaa_hdr_view, Some(resolve_target), &self.depth_texture) + ( + &self.msaa_hdr_view, + Some(resolve_target), + &self.depth_texture, + ) }; { @@ -2540,43 +2544,43 @@ impl Renderer { &self.metalfx_output, ) { - let (jitter_x, jitter_y) = (camera.jitter[0], camera.jitter[1]); - rhi.encode_metalfx_upscale( - encoder, - upscaler.as_ref(), - low_color, - low_depth, - &self.velocity_textures.velocity_texture, - output_tex, - jitter_x, - jitter_y, - self.metalfx_reset_history, - ); - self.metalfx_reset_history = false; + let (jitter_x, jitter_y) = (camera.jitter[0], camera.jitter[1]); + rhi.encode_metalfx_upscale( + encoder, + upscaler.as_ref(), + low_color, + low_depth, + &self.velocity_textures.velocity_texture, + output_tex, + jitter_x, + jitter_y, + self.metalfx_reset_history, + ); + self.metalfx_reset_history = false; - // Blit MetalFX output into the post-process ping buffer so - // downstream passes (TAA, motion blur, DoF) read the upscaled image. - let (sw, sh) = rhi.surface_size(); - rhi.copy_texture_to_texture( - encoder, - &euca_rhi::TexelCopyTextureInfo { - texture: output_tex, - mip_level: 0, - origin: euca_rhi::Origin3d { x: 0, y: 0, z: 0 }, - aspect: euca_rhi::TextureAspect::All, - }, - &euca_rhi::TexelCopyTextureInfo { - texture: self.post_process_stack.ping_texture(), - mip_level: 0, - origin: euca_rhi::Origin3d { x: 0, y: 0, z: 0 }, - aspect: euca_rhi::TextureAspect::All, - }, - euca_rhi::Extent3d { - width: sw, - height: sh, - depth_or_array_layers: 1, - }, - ); + // Blit MetalFX output into the post-process ping buffer so + // downstream passes (TAA, motion blur, DoF) read the upscaled image. + let (sw, sh) = rhi.surface_size(); + rhi.copy_texture_to_texture( + encoder, + &euca_rhi::TexelCopyTextureInfo { + texture: output_tex, + mip_level: 0, + origin: euca_rhi::Origin3d { x: 0, y: 0, z: 0 }, + aspect: euca_rhi::TextureAspect::All, + }, + &euca_rhi::TexelCopyTextureInfo { + texture: self.post_process_stack.ping_texture(), + mip_level: 0, + origin: euca_rhi::Origin3d { x: 0, y: 0, z: 0 }, + aspect: euca_rhi::TextureAspect::All, + }, + euca_rhi::Extent3d { + width: sw, + height: sh, + depth_or_array_layers: 1, + }, + ); } // GPU compute particles: update (compute dispatch) then draw (render pass). @@ -2985,8 +2989,7 @@ impl Renderer { | euca_rhi::TextureUsages::TEXTURE_BINDING, view_formats: &[], }); - let output_view = - rhi.create_texture_view(&output, &euca_rhi::TextureViewDesc::default()); + let output_view = rhi.create_texture_view(&output, &euca_rhi::TextureViewDesc::default()); // Create the MetalFX temporal scaler (panics on unsupported hardware). let upscaler = rhi.create_temporal_upscaler( diff --git a/crates/euca-rhi/src/metal_backend.rs b/crates/euca-rhi/src/metal_backend.rs index c10a7cf..512bfa3 100644 --- a/crates/euca-rhi/src/metal_backend.rs +++ b/crates/euca-rhi/src/metal_backend.rs @@ -1584,7 +1584,9 @@ impl RenderDevice for MetalDevice { reset: bool, ) { if let Some(scaler) = upscaler.downcast_ref::() { - scaler.encode(encoder, color, depth, motion, output, jitter_x, jitter_y, reset); + scaler.encode( + encoder, color, depth, motion, output, jitter_x, jitter_y, reset, + ); } else { log::warn!("encode_metalfx_upscale: upscaler is not a MetalFXUpscaler — skipping"); }