diff --git a/examples2d/one_way_platforms2.rs b/examples2d/one_way_platforms2.rs index dd50201da..d16d0cc9b 100644 --- a/examples2d/one_way_platforms2.rs +++ b/examples2d/one_way_platforms2.rs @@ -90,7 +90,7 @@ pub fn init_world(testbed: &mut Testbed) { * depending on their position. */ testbed.add_callback(move |graphics, physics, _, run_state| { - if run_state.timestep_id % 50 == 0 && physics.bodies.len() <= 7 { + if run_state.timestep_id % 200 == 0 && physics.bodies.len() <= 7 { // Spawn a new cube. let collider = ColliderBuilder::cuboid(1.5, 2.0); let body = RigidBodyBuilder::dynamic().translation(vector![20.0, 10.0]); diff --git a/examples2d/rope_joints2.rs b/examples2d/rope_joints2.rs index 451dd1d31..ec9b201e6 100644 --- a/examples2d/rope_joints2.rs +++ b/examples2d/rope_joints2.rs @@ -51,9 +51,7 @@ pub fn init_world(testbed: &mut Testbed) { let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, child_handle, &mut bodies); - let joint = RopeJointBuilder::new() - .local_anchor2(point![0.0, 0.0]) - .limits([2.0, 2.0]); + let joint = RopeJointBuilder::new(2.0).local_anchor2(point![0.0, 0.0]); impulse_joints.insert(character_handle, child_handle, joint, true); /* diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index ad4d226f9..851bc8af2 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -34,7 +34,10 @@ mod heightfield3; mod joints3; // mod joints3; mod character_controller3; +mod debug_chain_high_mass_ratio3; +mod debug_cube_high_mass_ratio3; mod debug_internal_edges3; +mod debug_long_chain3; mod joint_motor_position3; mod keva3; mod locked_rotations3; @@ -45,8 +48,10 @@ mod primitives3; mod restitution3; mod rope_joints3; mod sensor3; +mod spring_joints3; mod trimesh3; mod vehicle_controller3; +mod vehicle_joints3; fn demo_name_from_command_line() -> Option { let mut args = std::env::args(); @@ -105,8 +110,10 @@ pub fn main() { ("Restitution", restitution3::init_world), ("Rope Joints", rope_joints3::init_world), ("Sensor", sensor3::init_world), + ("Spring Joints", spring_joints3::init_world), ("TriMesh", trimesh3::init_world), ("Vehicle controller", vehicle_controller3::init_world), + ("Vehicle joints", vehicle_joints3::init_world), ("Keva tower", keva3::init_world), ("Newton cradle", newton_cradle3::init_world), ("(Debug) multibody_joints", debug_articulations3::init_world), @@ -122,6 +129,15 @@ pub fn main() { ), ("(Debug) friction", debug_friction3::init_world), ("(Debug) internal edges", debug_internal_edges3::init_world), + ("(Debug) long chain", debug_long_chain3::init_world), + ( + "(Debug) high mass ratio: chain", + debug_chain_high_mass_ratio3::init_world, + ), + ( + "(Debug) high mass ratio: cube", + debug_cube_high_mass_ratio3::init_world, + ), ("(Debug) triangle", debug_triangle3::init_world), ("(Debug) trimesh", debug_trimesh3::init_world), ("(Debug) cylinder", debug_cylinder3::init_world), diff --git a/examples3d/debug_chain_high_mass_ratio3.rs b/examples3d/debug_chain_high_mass_ratio3.rs new file mode 100644 index 000000000..809311eb7 --- /dev/null +++ b/examples3d/debug_chain_high_mass_ratio3.rs @@ -0,0 +1,73 @@ +use rapier3d::prelude::*; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + let use_articulations = false; + + /* + * Create a chain with a very heavy ball at the end. + */ + let num = 17; + let rad = 0.2; + + let mut body_handles = Vec::new(); + + for i in 0..num { + let fi = i as f32; + + let status = if i == 0 { + RigidBodyType::Fixed + } else { + RigidBodyType::Dynamic + }; + + let ball_rad = if i == num - 1 { rad * 10.0 } else { rad }; + let shift1 = rad * 1.1; + let shift2 = ball_rad + rad * 0.1; + let z = if i == 0 { + 0.0 + } else { + (fi - 1.0) * 2.0 * shift1 + shift1 + shift2 + }; + + let rigid_body = RigidBodyBuilder::new(status) + .translation(vector![0.0, 0.0, z]) + .additional_solver_iterations(16); + let child_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::ball(ball_rad); + colliders.insert_with_parent(collider, child_handle, &mut bodies); + + // Vertical joint. + if i > 0 { + let parent_handle = *body_handles.last().unwrap(); + let joint = if i == 1 { + SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift1 * 2.0]) + } else { + SphericalJointBuilder::new() + .local_anchor1(point![0.0, 0.0, shift1]) + .local_anchor2(point![0.0, 0.0, -shift2]) + }; + + if use_articulations { + multibody_joints.insert(parent_handle, child_handle, joint, true); + } else { + impulse_joints.insert(parent_handle, child_handle, joint, true); + } + } + + body_handles.push(child_handle); + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![10.0, 10.0, 10.0], Point::origin()); +} diff --git a/examples3d/debug_cube_high_mass_ratio3.rs b/examples3d/debug_cube_high_mass_ratio3.rs new file mode 100644 index 000000000..ffaf7932b --- /dev/null +++ b/examples3d/debug_cube_high_mass_ratio3.rs @@ -0,0 +1,96 @@ +use rapier3d::prelude::*; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + let num_levels = 4; + let stick_len = 2.0; + let stick_rad = 0.2; + + /* + * Floor. + */ + let floor_body = + RigidBodyBuilder::fixed().translation(vector![0.0, -stick_len - stick_rad, 0.0]); + let floor_handle = bodies.insert(floor_body); + let floor_cube = ColliderBuilder::cuboid(stick_len, stick_len, stick_len); + colliders.insert_with_parent(floor_cube, floor_handle, &mut bodies); + + /* + * Create a stack of capsule with a very heavy cube on top. + */ + for i in 0..num_levels { + let fi = i as f32; + + let body = RigidBodyBuilder::dynamic().translation(vector![ + 0.0, + fi * stick_rad * 4.0, + -(stick_len / 2.0 - stick_rad) + ]); + let handle = bodies.insert(body); + let capsule = ColliderBuilder::cuboid(stick_len / 2.0, stick_rad, stick_rad); + colliders.insert_with_parent(capsule, handle, &mut bodies); + + let body = RigidBodyBuilder::dynamic().translation(vector![ + 0.0, + fi * stick_rad * 4.0, + (stick_len / 2.0 - stick_rad) + ]); + let handle = bodies.insert(body); + let capsule = ColliderBuilder::cuboid(stick_len / 2.0, stick_rad, stick_rad); + colliders.insert_with_parent(capsule, handle, &mut bodies); + + let body = RigidBodyBuilder::dynamic().translation(vector![ + -(stick_len / 2.0 - stick_rad), + (fi + 0.5) * stick_rad * 4.0, + 0.0 + ]); + let handle = bodies.insert(body); + let capsule = ColliderBuilder::cuboid(stick_rad, stick_rad, stick_len / 2.0); + colliders.insert_with_parent(capsule, handle, &mut bodies); + + let body = RigidBodyBuilder::dynamic().translation(vector![ + (stick_len / 2.0 - stick_rad), + (fi + 0.5) * stick_rad * 4.0, + 0.0 + ]); + let handle = bodies.insert(body); + let capsule = ColliderBuilder::cuboid(stick_rad, stick_rad, stick_len / 2.0); + colliders.insert_with_parent(capsule, handle, &mut bodies); + } + + /* + * Big cube on top. + */ + let cube_len = stick_len * 2.0; + let floor_body = RigidBodyBuilder::dynamic() + .translation(vector![ + 0.0, + cube_len / 2.0 + (num_levels as f32 - 0.25) * stick_rad * 4.0, + 0.0 + ]) + .additional_solver_iterations(36); + let floor_handle = bodies.insert(floor_body); + let floor_cube = ColliderBuilder::cuboid(cube_len / 2.0, cube_len / 2.0, cube_len / 2.0); + colliders.insert_with_parent(floor_cube, floor_handle, &mut bodies); + + let small_mass = + MassProperties::from_cuboid(1.0, vector![stick_rad, stick_rad, stick_len / 2.0]).mass(); + let big_mass = + MassProperties::from_cuboid(1.0, vector![cube_len / 2.0, cube_len / 2.0, cube_len / 2.0]) + .mass(); + println!("debug_cube_high_mass_ratio3: small stick mass: {small_mass}, big cube mass: {big_mass}, mass_ratio: {}", big_mass / small_mass); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![10.0, 10.0, 10.0], Point::origin()); +} diff --git a/examples3d/debug_long_chain3.rs b/examples3d/debug_long_chain3.rs new file mode 100644 index 000000000..e2bb9905c --- /dev/null +++ b/examples3d/debug_long_chain3.rs @@ -0,0 +1,63 @@ +use rapier3d::prelude::*; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + let use_articulations = false; + + /* + * Create the long chain. + */ + let num = 100; + let rad = 0.2; + let shift = rad * 2.2; + + let mut body_handles = Vec::new(); + + for i in 0..num { + let fi = i as f32; + + let status = if i == 0 { + RigidBodyType::Fixed + } else { + RigidBodyType::Dynamic + }; + + let rigid_body = RigidBodyBuilder::new(status).translation(vector![0.0, 0.0, fi * shift]); + let child_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::ball(rad); + colliders.insert_with_parent(collider, child_handle, &mut bodies); + + // Vertical joint. + if i > 0 { + let parent_handle = *body_handles.last().unwrap(); + let joint = if i == 1 { + SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift]) + } else { + SphericalJointBuilder::new() + .local_anchor1(point![0.0, 0.0, shift / 2.0]) + .local_anchor2(point![0.0, 0.0, -shift / 2.0]) + }; + + if use_articulations { + multibody_joints.insert(parent_handle, child_handle, joint, true); + } else { + impulse_joints.insert(parent_handle, child_handle, joint, true); + } + } + + body_handles.push(child_handle); + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![10.0, 10.0, 10.0], Point::origin()); +} diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs index dc4deedca..18a11e72a 100644 --- a/examples3d/joints3.rs +++ b/examples3d/joints3.rs @@ -20,7 +20,6 @@ fn create_coupled_joints( let joint1 = GenericJointBuilder::new(JointAxesMask::empty()) .limits(JointAxis::X, [-3.0, 3.0]) .limits(JointAxis::Y, [0.0, 3.0]) - .limits(JointAxis::Z, [0.0, 3.0]) .coupled_axes(JointAxesMask::Y | JointAxesMask::Z); if use_articulations { diff --git a/examples3d/one_way_platforms3.rs b/examples3d/one_way_platforms3.rs index 3b2715a18..1a5704c7b 100644 --- a/examples3d/one_way_platforms3.rs +++ b/examples3d/one_way_platforms3.rs @@ -90,7 +90,7 @@ pub fn init_world(testbed: &mut Testbed) { * depending on their position. */ testbed.add_callback(move |graphics, physics, _, run_state| { - if run_state.timestep_id % 50 == 0 && physics.bodies.len() <= 7 { + if run_state.timestep_id % 200 == 0 && physics.bodies.len() <= 7 { // Spawn a new cube. let collider = ColliderBuilder::cuboid(1.0, 2.0, 1.5); let body = RigidBodyBuilder::dynamic().translation(vector![0.0, 6.0, 20.0]); diff --git a/examples3d/platform3.rs b/examples3d/platform3.rs index d42cb76a5..9025afa0b 100644 --- a/examples3d/platform3.rs +++ b/examples3d/platform3.rs @@ -29,12 +29,12 @@ pub fn init_world(testbed: &mut Testbed) { let shift = rad * 2.0; let centerx = shift * num as f32 / 2.0; - let centery = shift / 2.0 + 3.04; let centerz = shift * num as f32 / 2.0; for i in 0usize..num { for j in 0usize..num { for k in 0usize..num { + let centery = if j >= num / 2 { 5.0 } else { 3.0 }; let x = i as f32 * shift - centerx; let y = j as f32 * shift + centery; let z = k as f32 * shift - centerz; @@ -51,11 +51,8 @@ pub fn init_world(testbed: &mut Testbed) { /* * Setup a velocity-based kinematic rigid body. */ - let platform_body = RigidBodyBuilder::kinematic_velocity_based().translation(vector![ - 0.0, - 1.5 + 0.8, - -10.0 * rad - ]); + let platform_body = + RigidBodyBuilder::kinematic_velocity_based().translation(vector![0.0, 1.5 + 0.8, 0.0]); let velocity_based_platform_handle = bodies.insert(platform_body); let collider = ColliderBuilder::cuboid(rad * 10.0, rad, rad * 10.0); colliders.insert_with_parent(collider, velocity_based_platform_handle, &mut bodies); @@ -65,8 +62,8 @@ pub fn init_world(testbed: &mut Testbed) { */ let platform_body = RigidBodyBuilder::kinematic_position_based().translation(vector![ 0.0, - 2.0 + 1.5 + 0.8, - -10.0 * rad + 3.0 + 1.5 + 0.8, + 0.0 ]); let position_based_platform_handle = bodies.insert(platform_body); let collider = ColliderBuilder::cuboid(rad * 10.0, rad, rad * 10.0); @@ -75,22 +72,17 @@ pub fn init_world(testbed: &mut Testbed) { /* * Setup a callback to control the platform. */ - let mut count = 0; testbed.add_callback(move |_, physics, _, run_state| { - count += 1; - if count % 100 > 50 { - return; - } - let velocity = vector![ 0.0, - (run_state.time * 5.0).sin(), - run_state.time.sin() * 5.0 + (run_state.time * 2.0).sin(), + run_state.time.sin() * 2.0 ]; // Update the velocity-based kinematic body by setting its velocity. if let Some(platform) = physics.bodies.get_mut(velocity_based_platform_handle) { platform.set_linvel(velocity, true); + platform.set_angvel(vector![0.0, 0.2, 0.0], true); } // Update the position-based kinematic body by setting its next position. diff --git a/examples3d/rope_joints3.rs b/examples3d/rope_joints3.rs index 329364b82..96966826a 100644 --- a/examples3d/rope_joints3.rs +++ b/examples3d/rope_joints3.rs @@ -80,9 +80,7 @@ pub fn init_world(testbed: &mut Testbed) { let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, child_handle, &mut bodies); - let joint = RopeJointBuilder::new() - .local_anchor2(point![0.0, 0.0, 0.0]) - .limits([2.0, 2.0]); + let joint = RopeJointBuilder::new(2.0); impulse_joints.insert(character_handle, child_handle, joint, true); /* diff --git a/examples3d/spring_joints3.rs b/examples3d/spring_joints3.rs new file mode 100644 index 000000000..333640241 --- /dev/null +++ b/examples3d/spring_joints3.rs @@ -0,0 +1,64 @@ +use rapier3d::prelude::*; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + /* + * Fixed ground to attach one end of the joints. + */ + let rigid_body = RigidBodyBuilder::fixed(); + let ground_handle = bodies.insert(rigid_body); + + /* + * Spring joints with a variety of spring parameters. + * The middle one has uses critical damping. + */ + let num = 30; + let radius = 0.5; + let mass = Ball::new(radius).mass_properties(1.0).mass(); + let stiffness = 1.0e3; + let critical_damping = 2.0 * (stiffness * mass).sqrt(); + for i in 0..=num { + let x_pos = -6.0 + 1.5 * i as f32; + let ball_pos = point![x_pos, 4.5, 0.0]; + let rigid_body = RigidBodyBuilder::dynamic() + .translation(ball_pos.coords) + .can_sleep(false); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::ball(radius); + colliders.insert_with_parent(collider, handle, &mut bodies); + + let damping_ratio = i as f32 / (num as f32 / 2.0); + let damping = damping_ratio * critical_damping; + let joint = SpringJointBuilder::new(0.0, stiffness, damping) + .local_anchor1(ball_pos - Vector::y() * 3.0); + impulse_joints.insert(ground_handle, handle, joint, true); + + // Box that will fall on to of the springed balls, makes the simulation funier to watch. + let rigid_body = + RigidBodyBuilder::dynamic().translation(ball_pos.coords + Vector::y() * 5.0); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(radius, radius, radius).density(100.0); + colliders.insert_with_parent(collider, handle, &mut bodies); + } + + /* + * Set up the testbed. + */ + testbed.set_world_with_params( + bodies, + colliders, + impulse_joints, + multibody_joints, + vector![0.0, -9.81, 0.0], + (), + ); + testbed.look_at(point![15.0, 5.0, 42.0], point![13.0, 1.0, 1.0]); +} diff --git a/examples3d/vehicle_joints3.rs b/examples3d/vehicle_joints3.rs new file mode 100644 index 000000000..ddd672c2f --- /dev/null +++ b/examples3d/vehicle_joints3.rs @@ -0,0 +1,227 @@ +use rapier3d::prelude::*; +use rapier_testbed3d::{KeyCode, Testbed}; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + /* + * Ground. + */ + let ground_size = Vector::new(60.0, 0.4, 60.0); + let nsubdivs = 100; + + let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| { + -(i as f32 * ground_size.x / (nsubdivs as f32) / 2.0).cos() + - (j as f32 * ground_size.z / (nsubdivs as f32) / 2.0).cos() + }); + + let collider = ColliderBuilder::heightfield(heights, ground_size) + .translation(vector![-7.0, 0.0, 0.0]) + .friction(1.0); + colliders.insert(collider); + + /* + * Vehicle we will control manually, simulated using joints. + * Strongly inspired from https://github.com/h3r2tic/cornell-mcray/blob/main/src/car.rs (MIT/Apache license). + */ + const CAR_GROUP: Group = Group::GROUP_1; + + let wheel_params = [ + vector![0.6874, 0.2783, -0.7802], + vector![-0.6874, 0.2783, -0.7802], + vector![0.64, 0.2783, 1.0254], + vector![-0.64, 0.2783, 1.0254], + ]; + // TODO: lower center of mass? + // let mut center_of_mass = wheel_params.iter().sum().unwrap() / 4.0; + // center_of_mass.y = 0.0; + + let suspension_height = 0.12; + let max_steering_angle = 35.0f32.to_radians(); + let drive_strength = 1.0; + let wheel_radius = 0.28; + let car_position = point![0.0, wheel_radius + suspension_height, 0.0]; + let body_position_in_car_space = vector![0.0, 0.4739, 0.0]; + + let body_position = car_position + body_position_in_car_space; + + let body_co = ColliderBuilder::cuboid(0.65, 0.3, 0.9) + .density(100.0) + .collision_groups(InteractionGroups::new(CAR_GROUP, !CAR_GROUP)); + let body_rb = RigidBodyBuilder::dynamic() + .position(body_position.into()) + .build(); + let body_handle = bodies.insert(body_rb); + colliders.insert_with_parent(body_co, body_handle, &mut bodies); + + let mut steering_joints = vec![]; + let mut motor_joints = vec![]; + + for (wheel_id, wheel_pos_in_car_space) in wheel_params.into_iter().enumerate() { + let is_front = wheel_id >= 2; + let wheel_center = car_position + wheel_pos_in_car_space; + + let axle_mass_props = MassProperties::from_ball(100.0, wheel_radius); + let axle_rb = RigidBodyBuilder::dynamic() + .position(wheel_center.into()) + .additional_mass_properties(axle_mass_props); + let axle_handle = bodies.insert(axle_rb); + + // This is a fake cylinder collider that we add only because our testbed can + // only render colliders. Setting it as sensor makes it show up as wireframe. + let wheel_fake_co = ColliderBuilder::cylinder(wheel_radius / 2.0, wheel_radius) + .rotation(Vector::z() * std::f32::consts::FRAC_PI_2) + .sensor(true) + .density(0.0) + .collision_groups(InteractionGroups::none()); + + // The actual wheel collider. Simulating the wheel as a ball is interesting as it + // is mathematically simpler than a cylinder and cheaper to compute for collision-detection. + let wheel_co = ColliderBuilder::ball(wheel_radius) + .density(100.0) + .collision_groups(InteractionGroups::new(CAR_GROUP, !CAR_GROUP)) + .friction(1.0); + let wheel_rb = RigidBodyBuilder::dynamic().position(wheel_center.into()); + let wheel_handle = bodies.insert(wheel_rb); + colliders.insert_with_parent(wheel_co, wheel_handle, &mut bodies); + colliders.insert_with_parent(wheel_fake_co, wheel_handle, &mut bodies); + + let suspension_attachment_in_body_space = + wheel_pos_in_car_space - body_position_in_car_space; + + // Suspension between the body and the axle. + let mut locked_axes = + JointAxesMask::X | JointAxesMask::Z | JointAxesMask::ANG_X | JointAxesMask::ANG_Z; + if !is_front { + locked_axes |= JointAxesMask::ANG_Y; + } + + let mut suspension_joint = GenericJointBuilder::new(locked_axes) + .limits(JointAxis::Y, [0.0, suspension_height]) + .motor_position(JointAxis::Y, 0.0, 1.0e4, 1.0e3) + .local_anchor1(suspension_attachment_in_body_space.into()); + + if is_front { + suspension_joint = + suspension_joint.limits(JointAxis::AngY, [-max_steering_angle, max_steering_angle]); + } + + let body_axle_joint_handle = + impulse_joints.insert(body_handle, axle_handle, suspension_joint, true); + + // Joint between the axle and the wheel. + let wheel_joint = RevoluteJointBuilder::new(Vector::x_axis()); + let wheel_joint_handle = + impulse_joints.insert(axle_handle, wheel_handle, wheel_joint, true); + + if is_front { + steering_joints.push(body_axle_joint_handle); + motor_joints.push(wheel_joint_handle); + } + } + + /* + * Callback to control the wheels motors. + */ + testbed.add_callback(move |gfx, physics, _, _| { + let Some(gfx) = gfx else { return }; + + let mut thrust = 0.0; + let mut steering = 0.0; + let mut boost = 1.0; + + for key in gfx.keys().get_pressed() { + match *key { + KeyCode::Right => { + steering = -1.0; + } + KeyCode::Left => { + steering = 1.0; + } + KeyCode::Up => { + thrust = -drive_strength; + } + KeyCode::Down => { + thrust = drive_strength; + } + KeyCode::ShiftRight => { + boost = 1.5; + } + _ => {} + } + } + + if thrust != 0.0 || steering != 0.0 { + physics.bodies.get_mut(body_handle).unwrap().wake_up(true); + } + + // Apply steering to the axles. + for steering_handle in &steering_joints { + let steering_joint = physics.impulse_joints.get_mut(*steering_handle).unwrap(); + steering_joint.data.set_motor_position( + JointAxis::AngY, + max_steering_angle * steering, + 1.0e4, + 1.0e3, + ); + } + + // Apply thrust. + // Pseudo-differential adjusting speed of engines depending on steering arc + // Higher values result in more drifty behavior. + let differential_strength = 0.5; + let sideways_shift = (max_steering_angle * steering).sin() * differential_strength; + let speed_diff = if sideways_shift > 0.0 { + f32::hypot(1.0, sideways_shift) + } else { + 1.0 / f32::hypot(1.0, sideways_shift) + }; + + let ms = [1.0 / speed_diff, speed_diff]; + for (motor_handle, &ms) in motor_joints.iter().copied().zip(ms.iter()) { + let motor_joint = physics.impulse_joints.get_mut(motor_handle).unwrap(); + motor_joint.data.set_motor_velocity( + JointAxis::AngX, + -30.0 * thrust * ms * boost, + 1.0e2, + ); + } + }); + + /* + * Create some cubes on the ground. + */ + // let num = 8; + // let rad = 0.1; + // + // let shift = rad * 2.0; + // let centerx = shift * (num / 2) as f32; + // let centery = rad; + // + // for j in 0usize..1 { + // for k in 0usize..4 { + // for i in 0..num { + // let x = i as f32 * shift - centerx; + // let y = j as f32 * shift + centery; + // let z = k as f32 * shift + centerx; + // + // let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y, z]); + // let handle = bodies.insert(rigid_body); + // let collider = ColliderBuilder::cuboid(rad, rad, rad); + // colliders.insert_with_parent(collider, handle, &mut bodies); + // } + // } + // } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point!(10.0, 10.0, 10.0), Point::origin()); +} diff --git a/src/control/ray_cast_vehicle_controller.rs b/src/control/ray_cast_vehicle_controller.rs index 1e2da51f8..30979e1b9 100644 --- a/src/control/ray_cast_vehicle_controller.rs +++ b/src/control/ray_cast_vehicle_controller.rs @@ -4,7 +4,7 @@ use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ColliderHandle, ColliderSet, Ray}; use crate::math::{Point, Real, Rotation, Vector}; use crate::pipeline::{QueryFilter, QueryPipeline}; -use crate::utils::{WCross, WDot}; +use crate::utils::{SimdCross, SimdDot}; /// A character controller to simulate vehicles using ray-casting for the wheels. pub struct DynamicRayCastVehicleController { diff --git a/src/dynamics/ccd/mod.rs b/src/dynamics/ccd/mod.rs index 84807fafa..a73ca850e 100644 --- a/src/dynamics/ccd/mod.rs +++ b/src/dynamics/ccd/mod.rs @@ -1,3 +1,6 @@ +// TODO: not sure why it complains about PredictedImpacts being unused, +// making it private or pub(crate) triggers a different error. +#[allow(unused_imports)] pub use self::ccd_solver::{CCDSolver, PredictedImpacts}; pub use self::toi_entry::TOIEntry; diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 6d3acc545..d07527f80 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -1,4 +1,5 @@ use crate::math::Real; +use std::num::NonZeroUsize; /// Parameters for a time-step of the physics engine. #[derive(Copy, Clone, Debug)] @@ -43,15 +44,10 @@ pub struct IntegrationParameters { pub max_penetration_correction: Real, /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). pub prediction_distance: Real, - /// Maximum number of iterations performed to solve non-penetration and joint constraints (default: `4`). - pub max_velocity_iterations: usize, - /// Maximum number of iterations performed to solve friction constraints (default: `8`). - pub max_velocity_friction_iterations: usize, - /// Maximum number of iterations performed to remove the energy introduced by penetration corrections (default: `1`). - pub max_stabilization_iterations: usize, - /// If `false`, friction and non-penetration constraints will be solved in the same loop. Otherwise, - /// non-penetration constraints are solved first, and friction constraints are solved after (default: `true`). - pub interleave_restitution_and_friction_resolution: bool, + /// Number of iterations performed to solve friction constraints at solver iteration (default: `2`). + pub num_friction_iteration_per_solver_iteration: usize, + /// The number of solver iterations run by the constraints solver for calculating forces (default: `4`). + pub num_solver_iterations: NonZeroUsize, /// Minimum number of dynamic bodies in each active island (default: `128`). pub min_island_size: usize, /// Maximum number of substeps performed by the solver (default: `1`). @@ -151,17 +147,15 @@ impl Default for IntegrationParameters { Self { dt: 1.0 / 60.0, min_ccd_dt: 1.0 / 60.0 / 100.0, - erp: 0.8, - damping_ratio: 0.25, + erp: 0.6, + damping_ratio: 1.0, joint_erp: 1.0, joint_damping_ratio: 1.0, - allowed_linear_error: 0.001, // 0.005 + allowed_linear_error: 0.001, max_penetration_correction: Real::MAX, prediction_distance: 0.002, - max_velocity_iterations: 4, - max_velocity_friction_iterations: 8, - max_stabilization_iterations: 1, - interleave_restitution_and_friction_resolution: true, // Enabling this makes a big difference for 2D stability. + num_friction_iteration_per_solver_iteration: 2, + num_solver_iterations: NonZeroUsize::new(4).unwrap(), // TODO: what is the optimal value for min_island_size? // It should not be too big so that we don't end up with // huge islands that don't fit in cache. diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index ef5d50a80..43ac4ec26 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -4,7 +4,7 @@ use crate::dynamics::{ }; use crate::geometry::{ColliderSet, NarrowPhase}; use crate::math::Real; -use crate::utils::WDot; +use crate::utils::SimdDot; /// Structure responsible for maintaining the set of active rigid-bodies, and /// putting non-moving rigid-bodies to sleep to save computation times. @@ -14,6 +14,7 @@ pub struct IslandManager { pub(crate) active_dynamic_set: Vec, pub(crate) active_kinematic_set: Vec, pub(crate) active_islands: Vec, + pub(crate) active_islands_additional_solver_iterations: Vec, active_set_timestamp: u32, #[cfg_attr(feature = "serde-serialize", serde(skip))] can_sleep: Vec, // Workspace. @@ -28,6 +29,7 @@ impl IslandManager { active_dynamic_set: vec![], active_kinematic_set: vec![], active_islands: vec![], + active_islands_additional_solver_iterations: vec![], active_set_timestamp: 0, can_sleep: vec![], stack: vec![], @@ -127,6 +129,10 @@ impl IslandManager { &self.active_dynamic_set[island_range] } + pub(crate) fn active_island_additional_solver_iterations(&self, island_id: usize) -> usize { + self.active_islands_additional_solver_iterations[island_id] + } + #[inline(always)] pub(crate) fn iter_active_bodies<'a>(&'a self) -> impl Iterator + 'a { self.active_dynamic_set @@ -136,6 +142,7 @@ impl IslandManager { } #[cfg(feature = "parallel")] + #[allow(dead_code)] // That will likely be useful when we re-introduce intra-island parallelism. pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range { self.active_islands[island_id]..self.active_islands[island_id + 1] } @@ -232,12 +239,21 @@ impl IslandManager { // let t = instant::now(); // Propagation of awake state and awake island computation through the // traversal of the interaction graph. + self.active_islands_additional_solver_iterations.clear(); self.active_islands.clear(); self.active_islands.push(0); // The max avoid underflow when the stack is empty. let mut island_marker = self.stack.len().max(1) - 1; + // NOTE: islands containing a body with non-standard number of iterations won’t + // be merged with another island, unless another island with standard + // iterations number already started before and got continued due to the + // `min_island_size`. That could be avoided by pushing bodies with non-standard + // iterations on top of the stack (and other bodies on the back). Not sure it’s + // worth it though. + let mut additional_solver_iterations = 0; + while let Some(handle) = self.stack.pop() { let rb = bodies.index_mut_internal(handle); @@ -248,16 +264,23 @@ impl IslandManager { } if self.stack.len() < island_marker { - if self.active_dynamic_set.len() - *self.active_islands.last().unwrap() - >= min_island_size + if additional_solver_iterations != rb.additional_solver_iterations + || self.active_dynamic_set.len() - *self.active_islands.last().unwrap() + >= min_island_size { // We are starting a new island. + self.active_islands_additional_solver_iterations + .push(additional_solver_iterations); self.active_islands.push(self.active_dynamic_set.len()); + additional_solver_iterations = 0; } island_marker = self.stack.len(); } + additional_solver_iterations = + additional_solver_iterations.max(rb.additional_solver_iterations); + // Transmit the active state to all the rigid-bodies with colliders // in contact or joined with this collider. push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack); @@ -281,6 +304,8 @@ impl IslandManager { self.active_dynamic_set.push(handle); } + self.active_islands_additional_solver_iterations + .push(additional_solver_iterations); self.active_islands.push(self.active_dynamic_set.len()); // println!( // "Extraction: {}, num islands: {}", diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 5096e150a..83e27beaa 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -1,7 +1,7 @@ use crate::dynamics::solver::MotorParameters; use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RopeJoint}; use crate::math::{Isometry, Point, Real, Rotation, UnitVector, Vector, SPATIAL_DIM}; -use crate::utils::{WBasis, WReal}; +use crate::utils::{SimdBasis, SimdRealCopy}; #[cfg(feature = "dim3")] use crate::dynamics::SphericalJoint; @@ -121,7 +121,7 @@ pub struct JointLimits { pub impulse: N, } -impl Default for JointLimits { +impl Default for JointLimits { fn default() -> Self { Self { min: -N::splat(Real::MAX), @@ -131,6 +131,16 @@ impl Default for JointLimits { } } +impl From<[N; 2]> for JointLimits { + fn from(value: [N; 2]) -> Self { + Self { + min: value[0], + max: value[1], + impulse: N::splat(0.0), + } + } +} + /// A joint’s motor along one of its degrees of freedom. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] @@ -210,14 +220,23 @@ pub struct GenericJoint { /// The degrees-of-freedoms motorised by this joint. pub motor_axes: JointAxesMask, /// The coupled degrees of freedom of this joint. + /// + /// Note that coupling degrees of freedoms (DoF) changes the interpretation of the coupled joint’s limits and motors. + /// If multiple linear DoF are limited/motorized, only the limits/motor configuration for the first + /// coupled linear DoF is applied to all coupled linear DoF. Similarly, if multiple angular DoF are limited/motorized + /// only the limits/motor configuration for the first coupled angular DoF is applied to all coupled angular DoF. pub coupled_axes: JointAxesMask, /// The limits, along each degrees of freedoms of this joint. /// /// Note that the limit must also be explicitly enabled by the `limit_axes` bitmask. + /// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF limit and `limit_axis` + /// bitmask is applied to the coupled linear (resp. angular) axes. pub limits: [JointLimits; SPATIAL_DIM], /// The motors, along each degrees of freedoms of this joint. /// - /// Note that the mostor must also be explicitly enabled by the `motors` bitmask. + /// Note that the motor must also be explicitly enabled by the `motor_axes` bitmask. + /// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF motor and `motor_axes` + /// bitmask is applied to the coupled linear (resp. angular) axes. pub motors: [JointMotor; SPATIAL_DIM], /// Are contacts between the attached rigid-bodies enabled? pub contacts_enabled: bool, diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index f21f950f7..67f894957 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -214,7 +214,8 @@ impl ImpulseJointSet { // // .map(|e| &mut e.weight) // } - #[cfg(not(feature = "parallel"))] + // #[cfg(not(feature = "parallel"))] + #[allow(dead_code)] // That will likely be useful when we re-introduce intra-island parallelism. pub(crate) fn joints_mut(&mut self) -> &mut [JointGraphEdge] { &mut self.joint_graph.graph.edges[..] } diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs index 93cb0ab41..423d4c285 100644 --- a/src/dynamics/joint/mod.rs +++ b/src/dynamics/joint/mod.rs @@ -6,6 +6,7 @@ pub use self::multibody_joint::*; pub use self::prismatic_joint::*; pub use self::revolute_joint::*; pub use self::rope_joint::*; +pub use self::spring_joint::*; #[cfg(feature = "dim3")] pub use self::spherical_joint::*; @@ -21,3 +22,4 @@ mod rope_joint; #[cfg(feature = "dim3")] mod spherical_joint; +mod spring_joint; diff --git a/src/dynamics/joint/multibody_joint/mod.rs b/src/dynamics/joint/multibody_joint/mod.rs index a7013503e..c78900415 100644 --- a/src/dynamics/joint/multibody_joint/mod.rs +++ b/src/dynamics/joint/multibody_joint/mod.rs @@ -2,7 +2,9 @@ pub use self::multibody::Multibody; pub use self::multibody_joint::MultibodyJoint; -pub use self::multibody_joint_set::{MultibodyIndex, MultibodyJointHandle, MultibodyJointSet}; +pub use self::multibody_joint_set::{ + MultibodyIndex, MultibodyJointHandle, MultibodyJointSet, MultibodyLinkId, +}; pub use self::multibody_link::MultibodyLink; pub use self::unit_multibody_joint::{unit_joint_limit_constraint, unit_joint_motor_constraint}; diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 63e87e2bd..c65a6fffc 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -1,16 +1,13 @@ use super::multibody_link::{MultibodyLink, MultibodyLinkVec}; use super::multibody_workspace::MultibodyWorkspace; -use crate::dynamics::{ - solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyHandle, RigidBodySet, - RigidBodyType, RigidBodyVelocity, -}; +use crate::dynamics::{RigidBodyHandle, RigidBodySet, RigidBodyType, RigidBodyVelocity}; #[cfg(feature = "dim3")] use crate::math::Matrix; use crate::math::{ AngDim, AngVector, Dim, Isometry, Jacobian, Point, Real, Vector, ANG_DIM, DIM, SPATIAL_DIM, }; use crate::prelude::MultibodyJoint; -use crate::utils::{IndexMut2, WAngularInertia, WCross, WCrossMatrix}; +use crate::utils::{IndexMut2, SimdAngularInertia, SimdCross, SimdCrossMatrix}; use na::{self, DMatrix, DVector, DVectorView, DVectorViewMut, Dyn, OMatrix, SMatrix, SVector, LU}; #[repr(C)] @@ -372,6 +369,7 @@ impl Multibody { self.accelerations.fill(0.0); + // Eqn 42 to 45 for i in 0..self.links.len() { let link = &self.links[i]; let rb = &bodies[link.rigid_body]; @@ -400,7 +398,7 @@ impl Multibody { } acc.linvel += rb.vels.angvel.gcross(rb.vels.angvel.gcross(link.shift23)); - acc.linvel += self.workspace.accs[i].angvel.gcross(link.shift23); + acc.linvel += acc.angvel.gcross(link.shift23); self.workspace.accs[i] = acc; @@ -728,7 +726,7 @@ impl Multibody { /// The generalized velocity at the multibody_joint of the given link. #[inline] - pub(crate) fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView { + pub fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView { let ndofs = link.joint().ndofs(); DVectorView::from_slice( &self.velocities.as_slice()[link.assembly_id..link.assembly_id + ndofs], @@ -829,8 +827,10 @@ impl Multibody { } } + // TODO: make a version that doesn’t write back to bodies and doesn’t update the jacobians + // (i.e. just something used by the velocity solver’s small steps). /// Apply forward-kinematics to this multibody and its related rigid-bodies. - pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_mass_props: bool) { + pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_rb_mass_props: bool) { // Special case for the root, which has no parent. { let link = &mut self.links[0]; @@ -839,7 +839,7 @@ impl Multibody { if let Some(rb) = bodies.get_mut_internal(link.rigid_body) { rb.pos.next_position = link.local_to_world; - if update_mass_props { + if update_rb_mass_props { rb.mprops.update_world_mass_properties(&link.local_to_world); } } @@ -873,7 +873,7 @@ impl Multibody { "A rigid-body that is not at the root of a multibody must be dynamic." ); - if update_mass_props { + if update_rb_mass_props { link_rb .mprops .update_world_mass_properties(&link.local_to_world); @@ -943,6 +943,7 @@ impl Multibody { #[cfg(feature = "parallel")] #[inline] + #[allow(dead_code)] // That will likely be useful when we re-introduce intra-island parallelism. pub(crate) fn num_active_internal_constraints_and_jacobian_lines(&self) -> (usize, usize) { let num_constraints: usize = self .links @@ -951,40 +952,4 @@ impl Multibody { .sum(); (num_constraints, num_constraints) } - - #[inline] - pub(crate) fn generate_internal_constraints( - &self, - params: &IntegrationParameters, - j_id: &mut usize, - jacobians: &mut DVector, - out: &mut Vec, - mut insert_at: Option, - ) { - if !cfg!(feature = "parallel") { - let num_constraints: usize = self - .links - .iter() - .map(|l| l.joint().num_velocity_constraints()) - .sum(); - - let required_jacobian_len = *j_id + num_constraints * self.ndofs * 2; - if jacobians.nrows() < required_jacobian_len { - jacobians.resize_vertically_mut(required_jacobian_len, 0.0); - } - } - - for link in self.links.iter() { - link.joint().velocity_constraints( - params, - self, - link, - 0, - j_id, - jacobians, - out, - &mut insert_at, - ); - } - } } diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs index da650e609..62fc43427 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -1,4 +1,4 @@ -use crate::dynamics::solver::AnyJointVelocityConstraint; +use crate::dynamics::solver::JointGenericOneBodyConstraint; use crate::dynamics::{ joint, FixedJointBuilder, GenericJoint, IntegrationParameters, Multibody, MultibodyLink, RigidBodyVelocity, @@ -254,15 +254,15 @@ impl MultibodyJoint { params: &IntegrationParameters, multibody: &Multibody, link: &MultibodyLink, - dof_id: usize, - j_id: &mut usize, + mut j_id: usize, jacobians: &mut DVector, - constraints: &mut Vec, - insert_at: &mut Option, - ) { + constraints: &mut [JointGenericOneBodyConstraint], + ) -> usize { + let j_id = &mut j_id; let locked_bits = self.data.locked_axes.bits(); let limit_bits = self.data.limit_axes.bits(); let motor_bits = self.data.motor_axes.bits(); + let mut num_constraints = 0; let mut curr_free_dof = 0; for i in 0..DIM { @@ -281,11 +281,11 @@ impl MultibodyJoint { &self.data.motors[i], self.coords[i], limits, - dof_id + curr_free_dof, + curr_free_dof, j_id, jacobians, constraints, - insert_at, + &mut num_constraints, ); } @@ -296,11 +296,11 @@ impl MultibodyJoint { link, [self.data.limits[i].min, self.data.limits[i].max], self.coords[i], - dof_id + curr_free_dof, + curr_free_dof, j_id, jacobians, constraints, - insert_at, + &mut num_constraints, ); } curr_free_dof += 1; @@ -331,11 +331,11 @@ impl MultibodyJoint { link, limits, self.coords[i], - dof_id + curr_free_dof, + curr_free_dof, j_id, jacobians, constraints, - insert_at, + &mut num_constraints, ); Some(limits) } else { @@ -350,15 +350,17 @@ impl MultibodyJoint { &self.data.motors[i], self.coords[i], limits, - dof_id + curr_free_dof, + curr_free_dof, j_id, jacobians, constraints, - insert_at, + &mut num_constraints, ); } curr_free_dof += 1; } } + + num_constraints } } diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index a4b338af9..748de3b7d 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -53,13 +53,24 @@ impl IndexedData for MultibodyJointHandle { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq, Eq)] -pub struct MultibodyJointLink { - pub graph_id: RigidBodyGraphIndex, +/// Indexes usable to get a multibody link from a `MultibodyJointSet`. +/// +/// ```.skip +/// // With: +/// // multibody_joint_set: MultibodyJointSet +/// // multibody_link_id: MultibodyLinkId +/// let multibody = &multibody_joint_set[multibody_link_id.multibody]; +/// let link = multibody.link(multibody_link_id.id).expect("Link not found."); +pub struct MultibodyLinkId { + pub(crate) graph_id: RigidBodyGraphIndex, + /// The multibody index to be used as `&multibody_joint_set[multibody]` to + /// retrieve the multibody reference. pub multibody: MultibodyIndex, + /// The multibody link index to be given to [`Multibody::link`]. pub id: usize, } -impl Default for MultibodyJointLink { +impl Default for MultibodyLinkId { fn default() -> Self { Self { graph_id: RigidBodyGraphIndex::new(crate::INVALID_U32), @@ -78,7 +89,7 @@ impl Default for MultibodyJointLink { #[derive(Clone)] pub struct MultibodyJointSet { pub(crate) multibodies: Arena, // NOTE: a Slab would be sufficient. - pub(crate) rb2mb: Coarena, + pub(crate) rb2mb: Coarena, // NOTE: this is mostly for the island extraction. So perhaps we won’t need // that any more in the future when we improve our island builder. pub(crate) connectivity_graph: InteractionGraph, @@ -97,13 +108,22 @@ impl MultibodyJointSet { } /// Iterates through all the multibody joints from this set. - pub fn iter(&self) -> impl Iterator { + pub fn iter( + &self, + ) -> impl Iterator< + Item = ( + MultibodyJointHandle, + &MultibodyLinkId, + &Multibody, + &MultibodyLink, + ), + > { self.rb2mb .iter() .filter(|(_, link)| link.id > 0) // The first link of a rigid-body hasn’t been added by the user. .map(|(h, link)| { let mb = &self.multibodies[link.multibody.0]; - (MultibodyJointHandle(h), mb, mb.link(link.id).unwrap()) + (MultibodyJointHandle(h), link, mb, mb.link(link.id).unwrap()) }) } @@ -118,7 +138,7 @@ impl MultibodyJointSet { let data = data.into(); let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| { let mb_handle = self.multibodies.insert(Multibody::with_root(body1)); - MultibodyJointLink { + MultibodyLinkId { graph_id: self.connectivity_graph.graph.add_node(body1), multibody: MultibodyIndex(mb_handle), id: 0, @@ -127,7 +147,7 @@ impl MultibodyJointSet { let link2 = self.rb2mb.get(body2.0).copied().unwrap_or_else(|| { let mb_handle = self.multibodies.insert(Multibody::with_root(body2)); - MultibodyJointLink { + MultibodyLinkId { graph_id: self.connectivity_graph.graph.add_node(body2), multibody: MultibodyIndex(mb_handle), id: 0, @@ -257,7 +277,7 @@ impl MultibodyJointSet { /// Returns the link of this multibody attached to the given rigid-body. /// /// Returns `None` if `rb` isn’t part of any rigid-body. - pub fn rigid_body_link(&self, rb: RigidBodyHandle) -> Option<&MultibodyJointLink> { + pub fn rigid_body_link(&self, rb: RigidBodyHandle) -> Option<&MultibodyLinkId> { self.rb2mb.get(rb.0) } @@ -340,15 +360,15 @@ impl MultibodyJointSet { // NOTE: if there is a joint between these two bodies, then // one of the bodies must be the parent of the other. let link1 = mb.link(id1.id)?; - let parent1 = link1.parent_id()?; + let parent1 = link1.parent_id(); - if parent1 == id2.id { + if parent1 == Some(id2.id) { Some((MultibodyJointHandle(rb1.0), mb, &link1)) } else { let link2 = mb.link(id2.id)?; - let parent2 = link2.parent_id()?; + let parent2 = link2.parent_id(); - if parent2 == id1.id { + if parent2 == Some(id1.id) { Some((MultibodyJointHandle(rb2.0), mb, &link2)) } else { None diff --git a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs index a1ec483a8..d6efd12d6 100644 --- a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs @@ -1,9 +1,7 @@ #![allow(missing_docs)] // For downcast. use crate::dynamics::joint::MultibodyLink; -use crate::dynamics::solver::{ - AnyJointVelocityConstraint, JointGenericVelocityGroundConstraint, WritebackId, -}; +use crate::dynamics::solver::{JointGenericOneBodyConstraint, WritebackId}; use crate::dynamics::{IntegrationParameters, JointMotor, Multibody}; use crate::math::Real; use na::DVector; @@ -19,18 +17,16 @@ pub fn unit_joint_limit_constraint( dof_id: usize, j_id: &mut usize, jacobians: &mut DVector, - constraints: &mut Vec, - insert_at: &mut Option, + constraints: &mut [JointGenericOneBodyConstraint], + insert_at: &mut usize, ) { let ndofs = multibody.ndofs(); - let joint_velocity = multibody.joint_velocity(link); - let min_enabled = curr_pos < limits[0]; let max_enabled = limits[1] < curr_pos; let erp_inv_dt = params.joint_erp_inv_dt(); let cfm_coeff = params.joint_cfm_coeff(); let rhs_bias = ((curr_pos - limits[1]).max(0.0) - (limits[0] - curr_pos).max(0.0)) * erp_inv_dt; - let rhs_wo_bias = joint_velocity[dof_id]; + let rhs_wo_bias = 0.0; let dof_j_id = *j_id + dof_id + link.assembly_id; jacobians.rows_mut(*j_id, ndofs * 2).fill(0.0); @@ -46,8 +42,8 @@ pub fn unit_joint_limit_constraint( max_enabled as u32 as Real * Real::MAX, ]; - let constraint = JointGenericVelocityGroundConstraint { - mj_lambda2: multibody.solver_id, + let constraint = JointGenericOneBodyConstraint { + solver_vel2: multibody.solver_id, ndofs2: ndofs, j_id2: *j_id, joint_id: usize::MAX, @@ -61,14 +57,9 @@ pub fn unit_joint_limit_constraint( writeback_id: WritebackId::Limit(dof_id), }; - if let Some(at) = insert_at { - constraints[*at] = AnyJointVelocityConstraint::JointGenericGroundConstraint(constraint); - *at += 1; - } else { - constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint( - constraint, - )); - } + constraints[*insert_at] = constraint; + *insert_at += 1; + *j_id += 2 * ndofs; } @@ -84,13 +75,11 @@ pub fn unit_joint_motor_constraint( dof_id: usize, j_id: &mut usize, jacobians: &mut DVector, - constraints: &mut Vec, - insert_at: &mut Option, + constraints: &mut [JointGenericOneBodyConstraint], + insert_at: &mut usize, ) { let inv_dt = params.inv_dt(); let ndofs = multibody.ndofs(); - let joint_velocity = multibody.joint_velocity(link); - let motor_params = motor.motor_params(params.dt); let dof_j_id = *j_id + dof_id + link.assembly_id; @@ -117,11 +106,10 @@ pub fn unit_joint_motor_constraint( ); }; - let dvel = joint_velocity[dof_id]; - rhs_wo_bias += dvel - target_vel; + rhs_wo_bias += -target_vel; - let constraint = JointGenericVelocityGroundConstraint { - mj_lambda2: multibody.solver_id, + let constraint = JointGenericOneBodyConstraint { + solver_vel2: multibody.solver_id, ndofs2: ndofs, j_id2: *j_id, joint_id: usize::MAX, @@ -135,13 +123,8 @@ pub fn unit_joint_motor_constraint( writeback_id: WritebackId::Limit(dof_id), }; - if let Some(at) = insert_at { - constraints[*at] = AnyJointVelocityConstraint::JointGenericGroundConstraint(constraint); - *at += 1; - } else { - constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint( - constraint, - )); - } + constraints[*insert_at] = constraint; + *insert_at += 1; + *j_id += 2 * ndofs; } diff --git a/src/dynamics/joint/rope_joint.rs b/src/dynamics/joint/rope_joint.rs index 3b7317c97..44d4809c7 100644 --- a/src/dynamics/joint/rope_joint.rs +++ b/src/dynamics/joint/rope_joint.rs @@ -1,8 +1,8 @@ use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask}; use crate::dynamics::{JointAxis, MotorModel}; -use crate::math::{Point, Real, UnitVector}; +use crate::math::{Point, Real}; -use super::{JointLimits, JointMotor}; +use super::JointMotor; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] @@ -14,12 +14,16 @@ pub struct RopeJoint { } impl RopeJoint { - /// Creates a new rope joint limiting the max distance between to bodies - pub fn new() -> Self { - let data = GenericJointBuilder::new(JointAxesMask::FREE_FIXED_AXES) + /// Creates a new rope joint limiting the max distance between two bodies. + /// + /// The `max_dist` must be strictly greater than 0.0. + pub fn new(max_dist: Real) -> Self { + let data = GenericJointBuilder::new(JointAxesMask::empty()) .coupled_axes(JointAxesMask::LIN_AXES) .build(); - Self { data } + let mut result = Self { data }; + result.set_max_distance(max_dist); + result } /// The underlying generic joint. @@ -62,30 +66,6 @@ impl RopeJoint { self } - /// The principal axis of the joint, expressed in the local-space of the first rigid-body. - #[must_use] - pub fn local_axis1(&self) -> UnitVector { - self.data.local_axis1() - } - - /// Sets the principal axis of the joint, expressed in the local-space of the first rigid-body. - pub fn set_local_axis1(&mut self, axis1: UnitVector) -> &mut Self { - self.data.set_local_axis1(axis1); - self - } - - /// The principal axis of the joint, expressed in the local-space of the second rigid-body. - #[must_use] - pub fn local_axis2(&self) -> UnitVector { - self.data.local_axis2() - } - - /// Sets the principal axis of the joint, expressed in the local-space of the second rigid-body. - pub fn set_local_axis2(&mut self, axis2: UnitVector) -> &mut Self { - self.data.set_local_axis2(axis2); - self - } - /// The motor affecting the joint’s translational degree of freedom. #[must_use] pub fn motor(&self, axis: JointAxis) -> Option<&JointMotor> { @@ -95,9 +75,6 @@ impl RopeJoint { /// Set the spring-like model used by the motor to reach the desired target velocity and position. pub fn set_motor_model(&mut self, model: MotorModel) -> &mut Self { self.data.set_motor_model(JointAxis::X, model); - self.data.set_motor_model(JointAxis::Y, model); - #[cfg(feature = "dim3")] - self.data.set_motor_model(JointAxis::Z, model); self } @@ -105,11 +82,6 @@ impl RopeJoint { pub fn set_motor_velocity(&mut self, target_vel: Real, factor: Real) -> &mut Self { self.data .set_motor_velocity(JointAxis::X, target_vel, factor); - self.data - .set_motor_velocity(JointAxis::Y, target_vel, factor); - #[cfg(feature = "dim3")] - self.data - .set_motor_velocity(JointAxis::Z, target_vel, factor); self } @@ -122,11 +94,6 @@ impl RopeJoint { ) -> &mut Self { self.data .set_motor_position(JointAxis::X, target_pos, stiffness, damping); - self.data - .set_motor_position(JointAxis::Y, target_pos, stiffness, damping); - #[cfg(feature = "dim3")] - self.data - .set_motor_position(JointAxis::Z, target_pos, stiffness, damping); self } @@ -140,35 +107,26 @@ impl RopeJoint { ) -> &mut Self { self.data .set_motor(JointAxis::X, target_pos, target_vel, stiffness, damping); - self.data - .set_motor(JointAxis::Y, target_pos, target_vel, stiffness, damping); - #[cfg(feature = "dim3")] - self.data - .set_motor(JointAxis::Y, target_pos, target_vel, stiffness, damping); self } /// Sets the maximum force the motor can deliver. pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self { self.data.set_motor_max_force(JointAxis::X, max_force); - self.data.set_motor_max_force(JointAxis::Y, max_force); - #[cfg(feature = "dim3")] - self.data.set_motor_max_force(JointAxis::Z, max_force); self } - /// The limit maximum distance attached bodies can translate. + /// The maximum distance allowed between the attached objects. #[must_use] - pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits> { - self.data.limits(axis) + pub fn max_distance(&self) -> Option { + self.data.limits(JointAxis::X).map(|l| l.max) } - /// Sets the `[min,max]` limit distances attached bodies can translate. - pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self { - self.data.set_limits(JointAxis::X, limits); - self.data.set_limits(JointAxis::Y, limits); - #[cfg(feature = "dim3")] - self.data.set_limits(JointAxis::Z, limits); + /// Sets the maximum allowed distance between the attached objects. + /// + /// The `max_dist` must be strictly greater than 0.0. + pub fn set_max_distance(&mut self, max_dist: Real) -> &mut Self { + self.data.set_limits(JointAxis::X, [0.0, max_dist]); self } } @@ -190,8 +148,8 @@ impl RopeJointBuilder { /// Creates a new builder for rope joints. /// /// This axis is expressed in the local-space of both rigid-bodies. - pub fn new() -> Self { - Self(RopeJoint::new()) + pub fn new(max_dist: Real) -> Self { + Self(RopeJoint::new(max_dist)) } /// Sets whether contacts between the attached rigid-bodies are enabled. @@ -215,20 +173,6 @@ impl RopeJointBuilder { self } - /// Sets the principal axis of the joint, expressed in the local-space of the first rigid-body. - #[must_use] - pub fn local_axis1(mut self, axis1: UnitVector) -> Self { - self.0.set_local_axis1(axis1); - self - } - - /// Sets the principal axis of the joint, expressed in the local-space of the second rigid-body. - #[must_use] - pub fn local_axis2(mut self, axis2: UnitVector) -> Self { - self.0.set_local_axis2(axis2); - self - } - /// Set the spring-like model used by the motor to reach the desired target velocity and position. #[must_use] pub fn motor_model(mut self, model: MotorModel) -> Self { @@ -270,10 +214,12 @@ impl RopeJointBuilder { self } - /// Sets the `[min,max]` limit distances attached bodies can translate. + /// Sets the maximum allowed distance between the attached bodies. + /// + /// The `max_dist` must be strictly greater than 0.0. #[must_use] - pub fn limits(mut self, limits: [Real; 2]) -> Self { - self.0.set_limits(limits); + pub fn max_distance(mut self, max_dist: Real) -> Self { + self.0.set_max_distance(max_dist); self } diff --git a/src/dynamics/joint/spring_joint.rs b/src/dynamics/joint/spring_joint.rs new file mode 100644 index 000000000..542775177 --- /dev/null +++ b/src/dynamics/joint/spring_joint.rs @@ -0,0 +1,172 @@ +use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask}; +use crate::dynamics::{JointAxis, MotorModel}; +use crate::math::{Point, Real}; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +#[repr(transparent)] +/// A spring-damper joint, applies a force proportional to the distance between two objects. +/// +/// The spring is integrated implicitly, implying that an even undamped spring will still be subject to some +/// amount of numerical damping (so it will eventually come to a rest). More solver iterations, or smaller +/// timesteps, will lower the effect of numerical damping, providing a more realistic result. +pub struct SpringJoint { + /// The underlying joint data. + pub data: GenericJoint, +} + +impl SpringJoint { + /// Creates a new spring joint limiting the max distance between two bodies. + /// + /// The `max_dist` must be strictly greater than 0.0. + pub fn new(rest_length: Real, stiffness: Real, damping: Real) -> Self { + let data = GenericJointBuilder::new(JointAxesMask::empty()) + .coupled_axes(JointAxesMask::LIN_AXES) + .motor_position(JointAxis::X, rest_length, stiffness, damping) + .motor_model(JointAxis::X, MotorModel::ForceBased) + .build(); + Self { data } + } + + /// The underlying generic joint. + pub fn data(&self) -> &GenericJoint { + &self.data + } + + /// Are contacts between the attached rigid-bodies enabled? + pub fn contacts_enabled(&self) -> bool { + self.data.contacts_enabled + } + + /// Sets whether contacts between the attached rigid-bodies are enabled. + pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self { + self.data.set_contacts_enabled(enabled); + self + } + + /// The joint’s anchor, expressed in the local-space of the first rigid-body. + #[must_use] + pub fn local_anchor1(&self) -> Point { + self.data.local_anchor1() + } + + /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body. + pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { + self.data.set_local_anchor1(anchor1); + self + } + + /// The joint’s anchor, expressed in the local-space of the second rigid-body. + #[must_use] + pub fn local_anchor2(&self) -> Point { + self.data.local_anchor2() + } + + /// Sets the joint’s anchor, expressed in the local-space of the second rigid-body. + pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { + self.data.set_local_anchor2(anchor2); + self + } + + /// Set the spring model used by this joint to reach the desired target velocity and position. + /// + /// Setting this to `MotorModel::ForceBased` (which is the default value for this joint) makes the spring constants + /// (stiffness and damping) parameter understood as in the regular spring-mass-damper system. With + /// `MotorModel::AccelerationBased`, the spring constants will be automatically scaled by the attached masses, + /// making the spring more mass-independent. + pub fn set_spring_model(&mut self, model: MotorModel) -> &mut Self { + self.data.set_motor_model(JointAxis::X, model); + self + } + + // /// The maximum distance allowed between the attached objects. + // #[must_use] + // pub fn rest_length(&self) -> Option { + // self.data.limits(JointAxis::X).map(|l| l.max) + // } + // + // /// Sets the maximum allowed distance between the attached objects. + // /// + // /// The `max_dist` must be strictly greater than 0.0. + // pub fn set_rest_length(&mut self, max_dist: Real) -> &mut Self { + // self.data.set_limits(JointAxis::X, [0.0, max_dist]); + // self + // } +} + +impl Into for SpringJoint { + fn into(self) -> GenericJoint { + self.data + } +} + +/// A [SpringJoint] joint using the builder pattern. +/// +/// This builds a spring-damper joint which applies a force proportional to the distance between two objects. +/// See the documentation of [SpringJoint] for more information on its behavior. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct SpringJointBuilder(pub SpringJoint); + +impl SpringJointBuilder { + /// Creates a new builder for spring joints. + /// + /// This axis is expressed in the local-space of both rigid-bodies. + pub fn new(rest_length: Real, stiffness: Real, damping: Real) -> Self { + Self(SpringJoint::new(rest_length, stiffness, damping)) + } + + /// Sets whether contacts between the attached rigid-bodies are enabled. + #[must_use] + pub fn contacts_enabled(mut self, enabled: bool) -> Self { + self.0.set_contacts_enabled(enabled); + self + } + + /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body. + #[must_use] + pub fn local_anchor1(mut self, anchor1: Point) -> Self { + self.0.set_local_anchor1(anchor1); + self + } + + /// Sets the joint’s anchor, expressed in the local-space of the second rigid-body. + #[must_use] + pub fn local_anchor2(mut self, anchor2: Point) -> Self { + self.0.set_local_anchor2(anchor2); + self + } + + /// Set the spring used by this joint to reach the desired target velocity and position. + /// + /// Setting this to `MotorModel::ForceBased` (which is the default value for this joint) makes the spring constants + /// (stiffness and damping) parameter understood as in the regular spring-mass-damper system. With + /// `MotorModel::AccelerationBased`, the spring constants will be automatically scaled by the attached masses, + /// making the spring more mass-independent. + #[must_use] + pub fn spring_model(mut self, model: MotorModel) -> Self { + self.0.set_spring_model(model); + self + } + + // /// Sets the maximum allowed distance between the attached bodies. + // /// + // /// The `max_dist` must be strictly greater than 0.0. + // #[must_use] + // pub fn max_distance(mut self, max_dist: Real) -> Self { + // self.0.set_max_distance(max_dist); + // self + // } + + /// Builds the spring joint. + #[must_use] + pub fn build(self) -> SpringJoint { + self.0 + } +} + +impl Into for SpringJointBuilder { + fn into(self) -> GenericJoint { + self.0.into() + } +} diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 04e5b03a4..0f9a019ad 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -8,10 +8,10 @@ pub(crate) use self::joint::JointGraphEdge; pub(crate) use self::joint::JointIndex; pub use self::joint::*; pub use self::rigid_body_components::*; -#[cfg(not(feature = "parallel"))] +// #[cfg(not(feature = "parallel"))] pub(crate) use self::solver::IslandSolver; -#[cfg(feature = "parallel")] -pub(crate) use self::solver::ParallelIslandSolver; +// #[cfg(feature = "parallel")] +// pub(crate) use self::solver::ParallelIslandSolver; pub use parry::mass_properties::MassProperties; pub use self::rigid_body::{RigidBody, RigidBodyBuilder}; diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 064087c5e..9469dc70d 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -7,7 +7,7 @@ use crate::geometry::{ ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition, ColliderSet, ColliderShape, }; use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector}; -use crate::utils::WCross; +use crate::utils::SimdCross; use num::Zero; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -36,6 +36,7 @@ pub struct RigidBody { /// The dominance group this rigid-body is part of. pub(crate) dominance: RigidBodyDominance, pub(crate) enabled: bool, + pub(crate) additional_solver_iterations: usize, /// User-defined data associated to this rigid-body. pub user_data: u128, } @@ -64,6 +65,7 @@ impl RigidBody { dominance: RigidBodyDominance::default(), enabled: true, user_data: 0, + additional_solver_iterations: 0, } } @@ -72,6 +74,27 @@ impl RigidBody { self.ids = Default::default(); } + /// Set the additional number of solver iterations run for this rigid-body and + /// everything interacting with it. + /// + /// See [`Self::set_additional_solver_iterations`] for additional information. + pub fn additional_solver_iterations(&self) -> usize { + self.additional_solver_iterations + } + + /// Set the additional number of solver iterations run for this rigid-body and + /// everything interacting with it. + /// + /// Increasing this number will help improve simulation accuracy on this rigid-body + /// and every rigid-body interacting directly or indirectly with it (through joints + /// or contacts). This implies a performance hit. + /// + /// The default value is 0, meaning [`IntegrationParameters::num_solver_iterations`] will + /// be used as number of solver iterations for this body. + pub fn set_additional_solver_iterations(&mut self, additional_iterations: usize) { + self.additional_solver_iterations = additional_iterations; + } + /// The activation status of this rigid-body. pub fn activation(&self) -> &RigidBodyActivation { &self.activation @@ -1030,6 +1053,11 @@ pub struct RigidBodyBuilder { pub enabled: bool, /// An arbitrary user-defined 128-bit integer associated to the rigid-bodies built by this builder. pub user_data: u128, + /// The additional number of solver iterations run for this rigid-body and + /// everything interacting with it. + /// + /// See [`RigidBody::set_additional_solver_iterations`] for additional information. + pub additional_solver_iterations: usize, } impl RigidBodyBuilder { @@ -1051,6 +1079,7 @@ impl RigidBodyBuilder { dominance_group: 0, enabled: true, user_data: 0, + additional_solver_iterations: 0, } } @@ -1090,6 +1119,15 @@ impl RigidBodyBuilder { Self::new(RigidBodyType::Dynamic) } + /// Sets the additional number of solver iterations run for this rigid-body and + /// everything interacting with it. + /// + /// See [`RigidBody::set_additional_solver_iterations`] for additional information. + pub fn additional_solver_iterations(mut self, additional_iterations: usize) -> Self { + self.additional_solver_iterations = additional_iterations; + self + } + /// Sets the scale applied to the gravity force affecting the rigid-body to be created. pub fn gravity_scale(mut self, scale_factor: Real) -> Self { self.gravity_scale = scale_factor; @@ -1311,6 +1349,7 @@ impl RigidBodyBuilder { rb.vels.angvel = self.angvel; rb.body_type = self.body_type; rb.user_data = self.user_data; + rb.additional_solver_iterations = self.additional_solver_iterations; if self.additional_mass_properties != RigidBodyAdditionalMassProps::MassProps(MassProperties::zero()) diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index 1a5ca1dc5..caff27e7e 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -7,7 +7,7 @@ use crate::math::{ AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, }; use crate::parry::partitioning::IndexedData; -use crate::utils::{WAngularInertia, WCross, WDot}; +use crate::utils::{SimdAngularInertia, SimdCross, SimdDot}; use num::Zero; /// The unique handle of a rigid body added to a `RigidBodySet`. @@ -307,11 +307,52 @@ impl RigidBodyMassProps { self.effective_inv_mass.map(crate::utils::inv) } + /// The square root of the effective world-space angular inertia (that takes the potential rotation locking into account) of + /// this rigid-body. + #[must_use] + pub fn effective_angular_inertia_sqrt(&self) -> AngularInertia { + #[allow(unused_mut)] // mut needed in 3D. + let mut ang_inertia = self.effective_world_inv_inertia_sqrt; + + // Make the matrix invertible. + #[cfg(feature = "dim3")] + { + if self.flags.contains(LockedAxes::ROTATION_LOCKED_X) { + ang_inertia.m11 = 1.0; + } + if self.flags.contains(LockedAxes::ROTATION_LOCKED_Y) { + ang_inertia.m22 = 1.0; + } + if self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) { + ang_inertia.m33 = 1.0; + } + } + + #[allow(unused_mut)] // mut needed in 3D. + let mut result = ang_inertia.inverse(); + + // Remove the locked axes again. + #[cfg(feature = "dim3")] + { + if self.flags.contains(LockedAxes::ROTATION_LOCKED_X) { + result.m11 = 0.0; + } + if self.flags.contains(LockedAxes::ROTATION_LOCKED_Y) { + result.m22 = 0.0; + } + if self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) { + result.m33 = 0.0; + } + } + + result + } + /// The effective world-space angular inertia (that takes the potential rotation locking into account) of /// this rigid-body. #[must_use] pub fn effective_angular_inertia(&self) -> AngularInertia { - self.effective_world_inv_inertia_sqrt.squared().inverse() + self.effective_angular_inertia_sqrt().squared() } /// Recompute the mass-properties of this rigid-bodies based on its currently attached colliders. diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index d11097153..cef9c9e2e 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -6,10 +6,10 @@ pub(crate) fn categorize_contacts( multibody_joints: &MultibodyJointSet, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], - out_ground: &mut Vec, - out_not_ground: &mut Vec, - out_generic_ground: &mut Vec, - out_generic_not_ground: &mut Vec, + out_one_body: &mut Vec, + out_two_body: &mut Vec, + out_generic_one_body: &mut Vec, + out_generic_two_body: &mut Vec, ) { for manifold_i in manifold_indices { let manifold = &manifolds[*manifold_i]; @@ -26,14 +26,14 @@ pub(crate) fn categorize_contacts( .is_some() { if manifold.data.relative_dominance != 0 { - out_generic_ground.push(*manifold_i); + out_generic_one_body.push(*manifold_i); } else { - out_generic_not_ground.push(*manifold_i); + out_generic_two_body.push(*manifold_i); } } else if manifold.data.relative_dominance != 0 { - out_ground.push(*manifold_i) + out_one_body.push(*manifold_i) } else { - out_not_ground.push(*manifold_i) + out_two_body.push(*manifold_i) } } } @@ -43,10 +43,10 @@ pub(crate) fn categorize_joints( multibody_joints: &MultibodyJointSet, impulse_joints: &[JointGraphEdge], joint_indices: &[JointIndex], - ground_joints: &mut Vec, - nonground_joints: &mut Vec, - generic_ground_joints: &mut Vec, - generic_nonground_joints: &mut Vec, + one_body_joints: &mut Vec, + two_body_joints: &mut Vec, + generic_one_body_joints: &mut Vec, + generic_two_body_joints: &mut Vec, ) { for joint_i in joint_indices { let joint = &impulse_joints[*joint_i].weight; @@ -57,14 +57,14 @@ pub(crate) fn categorize_joints( || multibody_joints.rigid_body_link(joint.body2).is_some() { if !rb1.is_dynamic() || !rb2.is_dynamic() { - generic_ground_joints.push(*joint_i); + generic_one_body_joints.push(*joint_i); } else { - generic_nonground_joints.push(*joint_i); + generic_two_body_joints.push(*joint_i); } } else if !rb1.is_dynamic() || !rb2.is_dynamic() { - ground_joints.push(*joint_i); + one_body_joints.push(*joint_i); } else { - nonground_joints.push(*joint_i); + two_body_joints.push(*joint_i); } } } diff --git a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs new file mode 100644 index 000000000..4d0227064 --- /dev/null +++ b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs @@ -0,0 +1,528 @@ +use crate::dynamics::solver::categorization::categorize_contacts; +use crate::dynamics::solver::contact_constraint::{ + GenericOneBodyConstraint, GenericOneBodyConstraintBuilder, GenericTwoBodyConstraint, + GenericTwoBodyConstraintBuilder, OneBodyConstraint, OneBodyConstraintBuilder, + TwoBodyConstraint, TwoBodyConstraintBuilder, +}; +use crate::dynamics::solver::solver_body::SolverBody; +use crate::dynamics::solver::solver_vel::SolverVel; +use crate::dynamics::solver::{reset_buffer, ConstraintTypes, SolverConstraintsSet}; +use crate::dynamics::{ + ImpulseJoint, IntegrationParameters, IslandManager, JointAxesMask, MultibodyJointSet, + RigidBodySet, +}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::math::{Real, MAX_MANIFOLD_POINTS}; +use na::DVector; +use parry::math::DIM; + +#[cfg(feature = "simd-is-enabled")] +use { + crate::dynamics::solver::contact_constraint::{ + OneBodyConstraintSimd, SimdOneBodyConstraintBuilder, TwoBodyConstraintBuilderSimd, + TwoBodyConstraintSimd, + }, + crate::math::SIMD_WIDTH, +}; + +pub struct ConstraintsCounts { + pub num_constraints: usize, + pub num_jacobian_lines: usize, +} + +impl ConstraintsCounts { + pub fn from_contacts(manifold: &ContactManifold) -> Self { + let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0; + Self { + num_constraints: manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + + rest as usize, + num_jacobian_lines: manifold.data.solver_contacts.len() * DIM, + } + } + + pub fn from_joint(joint: &ImpulseJoint) -> Self { + let joint = &joint.data; + let locked_axes = joint.locked_axes.bits(); + let motor_axes = joint.motor_axes.bits() & !locked_axes; + let limit_axes = joint.limit_axes.bits() & !locked_axes; + let coupled_axes = joint.coupled_axes.bits(); + + let num_constraints = (motor_axes & !coupled_axes).count_ones() as usize + + ((motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize + + ((motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize + + locked_axes.count_ones() as usize + + (limit_axes & !coupled_axes).count_ones() as usize + + ((limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize + + ((limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize; + Self { + num_constraints, + num_jacobian_lines: num_constraints, + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct ContactConstraintTypes; + +impl ConstraintTypes for ContactConstraintTypes { + type OneBody = OneBodyConstraint; + type TwoBodies = TwoBodyConstraint; + type GenericOneBody = GenericOneBodyConstraint; + type GenericTwoBodies = GenericTwoBodyConstraint; + #[cfg(feature = "simd-is-enabled")] + type SimdOneBody = OneBodyConstraintSimd; + #[cfg(feature = "simd-is-enabled")] + type SimdTwoBodies = TwoBodyConstraintSimd; + + type BuilderOneBody = OneBodyConstraintBuilder; + type BuilderTwoBodies = TwoBodyConstraintBuilder; + type GenericBuilderOneBody = GenericOneBodyConstraintBuilder; + type GenericBuilderTwoBodies = GenericTwoBodyConstraintBuilder; + #[cfg(feature = "simd-is-enabled")] + type SimdBuilderOneBody = SimdOneBodyConstraintBuilder; + #[cfg(feature = "simd-is-enabled")] + type SimdBuilderTwoBodies = TwoBodyConstraintBuilderSimd; +} + +pub type ContactConstraintsSet = SolverConstraintsSet; + +impl ContactConstraintsSet { + pub fn init_constraint_groups( + &mut self, + island_id: usize, + islands: &IslandManager, + bodies: &RigidBodySet, + multibody_joints: &MultibodyJointSet, + manifolds: &[&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + ) { + self.two_body_interactions.clear(); + self.one_body_interactions.clear(); + self.generic_two_body_interactions.clear(); + self.generic_one_body_interactions.clear(); + + categorize_contacts( + bodies, + multibody_joints, + manifolds, + manifold_indices, + &mut self.one_body_interactions, + &mut self.two_body_interactions, + &mut self.generic_one_body_interactions, + &mut self.generic_two_body_interactions, + ); + + self.interaction_groups.clear_groups(); + self.interaction_groups.group_manifolds( + island_id, + islands, + bodies, + manifolds, + &self.two_body_interactions, + ); + + self.one_body_interaction_groups.clear_groups(); + self.one_body_interaction_groups.group_manifolds( + island_id, + islands, + bodies, + manifolds, + &self.one_body_interactions, + ); + + // NOTE: uncomment this do disable SIMD contact resolution. + // self.interaction_groups + // .nongrouped_interactions + // .append(&mut self.interaction_groups.simd_interactions); + // self.one_body_interaction_groups + // .nongrouped_interactions + // .append(&mut self.one_body_interaction_groups.simd_interactions); + } + + pub fn init( + &mut self, + island_id: usize, + islands: &IslandManager, + bodies: &RigidBodySet, + multibody_joints: &MultibodyJointSet, + manifolds: &[&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + ) { + self.clear_constraints(); + self.clear_builders(); + + self.init_constraint_groups( + island_id, + islands, + bodies, + multibody_joints, + manifolds, + manifold_indices, + ); + + let mut jacobian_id = 0; + + #[cfg(feature = "simd-is-enabled")] + { + self.simd_compute_constraints(bodies, manifolds); + } + self.compute_constraints(bodies, manifolds); + self.compute_generic_constraints(bodies, multibody_joints, manifolds, &mut jacobian_id); + + #[cfg(feature = "simd-is-enabled")] + { + self.simd_compute_one_body_constraints(bodies, manifolds); + } + self.compute_one_body_constraints(bodies, manifolds); + self.compute_generic_one_body_constraints( + bodies, + multibody_joints, + manifolds, + &mut jacobian_id, + ); + } + + #[cfg(feature = "simd-is-enabled")] + fn simd_compute_constraints( + &mut self, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + ) { + let total_num_constraints = self + .interaction_groups + .simd_interactions + .chunks_exact(SIMD_WIDTH) + .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[i[0]]).num_constraints) + .sum(); + + unsafe { + reset_buffer( + &mut self.simd_velocity_constraints_builder, + total_num_constraints, + ); + reset_buffer(&mut self.simd_velocity_constraints, total_num_constraints); + } + + let mut curr_start = 0; + + for manifolds_i in self + .interaction_groups + .simd_interactions + .chunks_exact(SIMD_WIDTH) + { + let num_to_add = + ConstraintsCounts::from_contacts(&manifolds_all[manifolds_i[0]]).num_constraints; + let manifold_id = gather![|ii| manifolds_i[ii]]; + let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]]; + + TwoBodyConstraintBuilderSimd::generate( + manifold_id, + manifolds, + bodies, + &mut self.simd_velocity_constraints_builder[curr_start..], + &mut self.simd_velocity_constraints[curr_start..], + ); + + curr_start += num_to_add; + } + + assert_eq!(curr_start, total_num_constraints); + } + + fn compute_constraints( + &mut self, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + ) { + let total_num_constraints = self + .interaction_groups + .nongrouped_interactions + .iter() + .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints) + .sum(); + + unsafe { + reset_buffer( + &mut self.velocity_constraints_builder, + total_num_constraints, + ); + reset_buffer(&mut self.velocity_constraints, total_num_constraints); + } + + let mut curr_start = 0; + + for manifold_i in &self.interaction_groups.nongrouped_interactions { + let manifold = &manifolds_all[*manifold_i]; + let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints; + + TwoBodyConstraintBuilder::generate( + *manifold_i, + manifold, + bodies, + &mut self.velocity_constraints_builder[curr_start..], + &mut self.velocity_constraints[curr_start..], + ); + + curr_start += num_to_add; + } + assert_eq!(curr_start, total_num_constraints); + } + + fn compute_generic_constraints( + &mut self, + bodies: &RigidBodySet, + multibody_joints: &MultibodyJointSet, + manifolds_all: &[&mut ContactManifold], + jacobian_id: &mut usize, + ) { + let total_num_constraints = self + .generic_two_body_interactions + .iter() + .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints) + .sum(); + + self.generic_velocity_constraints_builder.resize( + total_num_constraints, + GenericTwoBodyConstraintBuilder::invalid(), + ); + self.generic_velocity_constraints + .resize(total_num_constraints, GenericTwoBodyConstraint::invalid()); + + let mut curr_start = 0; + + for manifold_i in &self.generic_two_body_interactions { + let manifold = &manifolds_all[*manifold_i]; + let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints; + + GenericTwoBodyConstraintBuilder::generate( + *manifold_i, + manifold, + bodies, + multibody_joints, + &mut self.generic_velocity_constraints_builder[curr_start..], + &mut self.generic_velocity_constraints[curr_start..], + &mut self.generic_jacobians, + jacobian_id, + ); + + curr_start += num_to_add; + } + + assert_eq!(curr_start, total_num_constraints); + } + + fn compute_generic_one_body_constraints( + &mut self, + bodies: &RigidBodySet, + multibody_joints: &MultibodyJointSet, + manifolds_all: &[&mut ContactManifold], + jacobian_id: &mut usize, + ) { + let total_num_constraints = self + .generic_one_body_interactions + .iter() + .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints) + .sum(); + self.generic_velocity_one_body_constraints_builder.resize( + total_num_constraints, + GenericOneBodyConstraintBuilder::invalid(), + ); + self.generic_velocity_one_body_constraints + .resize(total_num_constraints, GenericOneBodyConstraint::invalid()); + + let mut curr_start = 0; + + for manifold_i in &self.generic_one_body_interactions { + let manifold = &manifolds_all[*manifold_i]; + let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints; + + GenericOneBodyConstraintBuilder::generate( + *manifold_i, + manifold, + bodies, + multibody_joints, + &mut self.generic_velocity_one_body_constraints_builder[curr_start..], + &mut self.generic_velocity_one_body_constraints[curr_start..], + &mut self.generic_jacobians, + jacobian_id, + ); + + curr_start += num_to_add; + } + assert_eq!(curr_start, total_num_constraints); + } + + #[cfg(feature = "simd-is-enabled")] + fn simd_compute_one_body_constraints( + &mut self, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + ) { + let total_num_constraints = self + .one_body_interaction_groups + .simd_interactions + .chunks_exact(SIMD_WIDTH) + .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[i[0]]).num_constraints) + .sum(); + + unsafe { + reset_buffer( + &mut self.simd_velocity_one_body_constraints_builder, + total_num_constraints, + ); + reset_buffer( + &mut self.simd_velocity_one_body_constraints, + total_num_constraints, + ); + } + + let mut curr_start = 0; + + for manifolds_i in self + .one_body_interaction_groups + .simd_interactions + .chunks_exact(SIMD_WIDTH) + { + let num_to_add = + ConstraintsCounts::from_contacts(&manifolds_all[manifolds_i[0]]).num_constraints; + let manifold_id = gather![|ii| manifolds_i[ii]]; + let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]]; + SimdOneBodyConstraintBuilder::generate( + manifold_id, + manifolds, + bodies, + &mut self.simd_velocity_one_body_constraints_builder[curr_start..], + &mut self.simd_velocity_one_body_constraints[curr_start..], + ); + curr_start += num_to_add; + } + assert_eq!(curr_start, total_num_constraints); + } + + fn compute_one_body_constraints( + &mut self, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + ) { + let total_num_constraints = self + .one_body_interaction_groups + .nongrouped_interactions + .iter() + .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints) + .sum(); + + unsafe { + reset_buffer( + &mut self.velocity_one_body_constraints_builder, + total_num_constraints, + ); + reset_buffer( + &mut self.velocity_one_body_constraints, + total_num_constraints, + ); + } + + let mut curr_start = 0; + + for manifold_i in &self.one_body_interaction_groups.nongrouped_interactions { + let manifold = &manifolds_all[*manifold_i]; + let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints; + + OneBodyConstraintBuilder::generate( + *manifold_i, + manifold, + bodies, + &mut self.velocity_one_body_constraints_builder[curr_start..], + &mut self.velocity_one_body_constraints[curr_start..], + ); + + curr_start += num_to_add; + } + assert_eq!(curr_start, total_num_constraints); + } + + pub fn solve_restitution( + &mut self, + solver_vels: &mut [SolverVel], + generic_solver_vels: &mut DVector, + ) { + let (jac, constraints) = self.iter_constraints_mut(); + for mut c in constraints { + c.solve_restitution(jac, solver_vels, generic_solver_vels); + } + } + + pub fn solve_restitution_wo_bias( + &mut self, + solver_vels: &mut [SolverVel], + generic_solver_vels: &mut DVector, + ) { + let (jac, constraints) = self.iter_constraints_mut(); + for mut c in constraints { + c.remove_bias(); + c.solve_restitution(jac, solver_vels, generic_solver_vels); + } + } + + pub fn solve_friction( + &mut self, + solver_vels: &mut [SolverVel], + generic_solver_vels: &mut DVector, + ) { + let (jac, constraints) = self.iter_constraints_mut(); + for mut c in constraints { + c.solve_friction(jac, solver_vels, generic_solver_vels); + } + } + + pub fn writeback_impulses(&mut self, manifolds_all: &mut [&mut ContactManifold]) { + let (_, constraints) = self.iter_constraints_mut(); + for mut c in constraints { + c.writeback_impulses(manifolds_all); + } + } + + pub fn update( + &mut self, + params: &IntegrationParameters, + small_step_id: usize, + multibodies: &MultibodyJointSet, + solver_bodies: &[SolverBody], + ) { + macro_rules! update_contacts( + ($builders: ident, $constraints: ident) => { + for (builder, constraint) in self.$builders.iter().zip(self.$constraints.iter_mut()) { + builder.update( + ¶ms, + small_step_id as Real * params.dt, + solver_bodies, + multibodies, + constraint, + ); + } + } + ); + + update_contacts!( + generic_velocity_constraints_builder, + generic_velocity_constraints + ); + update_contacts!(velocity_constraints_builder, velocity_constraints); + #[cfg(feature = "simd-is-enabled")] + update_contacts!(simd_velocity_constraints_builder, simd_velocity_constraints); + + update_contacts!( + generic_velocity_one_body_constraints_builder, + generic_velocity_one_body_constraints + ); + update_contacts!( + velocity_one_body_constraints_builder, + velocity_one_body_constraints + ); + #[cfg(feature = "simd-is-enabled")] + update_contacts!( + simd_velocity_one_body_constraints_builder, + simd_velocity_one_body_constraints + ); + } +} diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs new file mode 100644 index 000000000..7c39eab3a --- /dev/null +++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs @@ -0,0 +1,292 @@ +use crate::dynamics::solver::OneBodyConstraint; +use crate::dynamics::{ + IntegrationParameters, MultibodyJointSet, MultibodyLinkId, RigidBodySet, RigidBodyVelocity, +}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS}; +use crate::utils::SimdCross; + +use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart}; +use crate::dynamics::solver::solver_body::SolverBody; +use crate::dynamics::solver::{ContactPointInfos, OneBodyConstraintBuilder}; +#[cfg(feature = "dim2")] +use crate::utils::SimdBasis; +use na::DVector; + +#[derive(Copy, Clone)] +pub(crate) struct GenericOneBodyConstraintBuilder { + link2: MultibodyLinkId, + ccd_thickness: Real, + inner: OneBodyConstraintBuilder, +} + +impl GenericOneBodyConstraintBuilder { + pub fn invalid() -> Self { + Self { + link2: MultibodyLinkId::default(), + ccd_thickness: 0.0, + inner: OneBodyConstraintBuilder::invalid(), + } + } + + pub fn generate( + manifold_id: ContactManifoldIndex, + manifold: &ContactManifold, + bodies: &RigidBodySet, + multibodies: &MultibodyJointSet, + out_builders: &mut [GenericOneBodyConstraintBuilder], + out_constraints: &mut [GenericOneBodyConstraint], + jacobians: &mut DVector, + jacobian_id: &mut usize, + ) { + let mut handle1 = manifold.data.rigid_body1; + let mut handle2 = manifold.data.rigid_body2; + let flipped = manifold.data.relative_dominance < 0; + + let (force_dir1, flipped_multiplier) = if flipped { + std::mem::swap(&mut handle1, &mut handle2); + (manifold.data.normal, -1.0) + } else { + (-manifold.data.normal, 1.0) + }; + + let (vels1, world_com1) = if let Some(handle1) = handle1 { + let rb1 = &bodies[handle1]; + (rb1.vels, rb1.mprops.world_com) + } else { + (RigidBodyVelocity::zero(), Point::origin()) + }; + + let rb1 = handle1 + .map(|h| SolverBody::from(&bodies[h])) + .unwrap_or_else(SolverBody::default); + + let rb2 = &bodies[handle2.unwrap()]; + let (vels2, mprops2) = (&rb2.vels, &rb2.mprops); + + let link2 = *multibodies.rigid_body_link(handle2.unwrap()).unwrap(); + let (mb2, link_id2) = (&multibodies[link2.multibody], link2.id); + let solver_vel2 = mb2.solver_id; + + #[cfg(feature = "dim2")] + let tangents1 = force_dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let tangents1 = + super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel); + + let multibodies_ndof = mb2.ndofs(); + // For each solver contact we generate DIM constraints, and each constraints appends + // the multibodies jacobian and weighted jacobians + let required_jacobian_len = + *jacobian_id + manifold.data.solver_contacts.len() * multibodies_ndof * 2 * DIM; + + if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") { + jacobians.resize_vertically_mut(required_jacobian_len, 0.0); + } + + for (l, manifold_points) in manifold + .data + .solver_contacts + .chunks(MAX_MANIFOLD_POINTS) + .enumerate() + { + let chunk_j_id = *jacobian_id; + + let builder = &mut out_builders[l]; + let constraint = &mut out_constraints[l]; + + builder.inner.rb1 = rb1; + builder.inner.vels1 = vels1; + + constraint.inner.dir1 = force_dir1; + constraint.inner.im2 = mprops2.effective_inv_mass; + constraint.inner.solver_vel2 = solver_vel2; + constraint.inner.manifold_id = manifold_id; + constraint.inner.num_contacts = manifold_points.len() as u8; + #[cfg(feature = "dim3")] + { + constraint.inner.tangent1 = tangents1[0]; + } + + for k in 0..manifold_points.len() { + let manifold_point = &manifold_points[k]; + let point = manifold_point.point; + let dp1 = point - world_com1; + let dp2 = point - mprops2.world_com; + + let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); + let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); + + constraint.inner.limit = manifold_point.friction; + constraint.inner.manifold_contact_id[k] = manifold_point.contact_id; + + // Normal part. + let normal_rhs_wo_bias; + { + let torque_dir2 = dp2.gcross(-force_dir1); + let inv_r2 = mb2 + .fill_jacobians( + link_id2, + -force_dir1, + #[cfg(feature = "dim2")] + na::vector!(torque_dir2), + #[cfg(feature = "dim3")] + torque_dir2, + jacobian_id, + jacobians, + ) + .0; + + let r = crate::utils::inv(inv_r2); + + let is_bouncy = manifold_point.is_bouncy() as u32 as Real; + + let proj_vel1 = vel1.dot(&force_dir1); + let proj_vel2 = vel2.dot(&force_dir1); + let dvel = proj_vel1 - proj_vel2; + // NOTE: we add proj_vel1 since it’s not accessible through solver_vel. + normal_rhs_wo_bias = + proj_vel1 + (is_bouncy * manifold_point.restitution) * dvel; + + constraint.inner.elements[k].normal_part = OneBodyConstraintNormalPart { + gcross2: na::zero(), // Unused for generic constraints. + rhs: na::zero(), + rhs_wo_bias: na::zero(), + impulse: na::zero(), + total_impulse: na::zero(), + r, + }; + } + + // Tangent parts. + { + constraint.inner.elements[k].tangent_part.impulse = na::zero(); + + for j in 0..DIM - 1 { + let torque_dir2 = dp2.gcross(-tangents1[j]); + let inv_r2 = mb2 + .fill_jacobians( + link_id2, + -tangents1[j], + #[cfg(feature = "dim2")] + na::vector![torque_dir2], + #[cfg(feature = "dim3")] + torque_dir2, + jacobian_id, + jacobians, + ) + .0; + + let r = crate::utils::inv(inv_r2); + + let rhs_wo_bias = (vel1 + + flipped_multiplier * manifold_point.tangent_velocity) + .dot(&tangents1[j]); + + constraint.inner.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias; + constraint.inner.elements[k].tangent_part.rhs[j] = rhs_wo_bias; + + // FIXME: in 3D, we should take into account gcross[0].dot(gcross[1]) + // in lhs. See the corresponding code on the `velocity_constraint.rs` + // file. + constraint.inner.elements[k].tangent_part.r[j] = r; + } + } + + // Builder. + let infos = ContactPointInfos { + local_p1: rb1.position.inverse_transform_point(&manifold_point.point), + local_p2: rb2 + .pos + .position + .inverse_transform_point(&manifold_point.point), + tangent_vel: manifold_point.tangent_velocity, + dist: manifold_point.dist, + normal_rhs_wo_bias, + }; + + builder.link2 = link2; + builder.ccd_thickness = rb2.ccd.ccd_thickness; + builder.inner.infos[k] = infos; + constraint.inner.manifold_contact_id[k] = manifold_point.contact_id; + } + + constraint.j_id = chunk_j_id; + constraint.ndofs2 = mb2.ndofs(); + } + } + + pub fn update( + &self, + params: &IntegrationParameters, + solved_dt: Real, + _solver_bodies: &[SolverBody], + multibodies: &MultibodyJointSet, + constraint: &mut GenericOneBodyConstraint, + ) { + // We don’t update jacobians so the update is mostly identical to the non-generic velocity constraint. + let pos2 = &multibodies[self.link2.multibody] + .link(self.link2.id) + .unwrap() + .local_to_world; + + self.inner.update_with_positions( + params, + solved_dt, + pos2, + self.ccd_thickness, + &mut constraint.inner, + ); + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct GenericOneBodyConstraint { + // We just build the generic constraint on top of the velocity constraint, + // adding some information we can use in the generic case. + pub inner: OneBodyConstraint, + pub j_id: usize, + pub ndofs2: usize, +} + +impl GenericOneBodyConstraint { + pub fn invalid() -> Self { + Self { + inner: OneBodyConstraint::invalid(), + j_id: usize::MAX, + ndofs2: usize::MAX, + } + } + + pub fn solve( + &mut self, + jacobians: &DVector, + generic_solver_vels: &mut DVector, + solve_restitution: bool, + solve_friction: bool, + ) { + let solver_vel2 = self.inner.solver_vel2 as usize; + + let elements = &mut self.inner.elements[..self.inner.num_contacts as usize]; + OneBodyConstraintElement::generic_solve_group( + self.inner.cfm_factor, + elements, + jacobians, + self.inner.limit, + self.ndofs2, + self.j_id, + solver_vel2, + generic_solver_vels, + solve_restitution, + solve_friction, + ); + } + + pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { + self.inner.writeback_impulses(manifolds_all); + } + + pub fn remove_cfm_and_bias_from_rhs(&mut self) { + self.inner.remove_cfm_and_bias_from_rhs(); + } +} diff --git a/src/dynamics/solver/generic_velocity_ground_constraint_element.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs similarity index 74% rename from src/dynamics/solver/generic_velocity_ground_constraint_element.rs rename to src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs index 750811cde..1003a9a06 100644 --- a/src/dynamics/solver/generic_velocity_ground_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs @@ -1,13 +1,12 @@ use crate::dynamics::solver::{ - VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart, - VelocityGroundConstraintTangentPart, + OneBodyConstraintElement, OneBodyConstraintNormalPart, OneBodyConstraintTangentPart, }; use crate::math::{Real, DIM}; use na::DVector; #[cfg(feature = "dim2")] use na::SimdPartialOrd; -impl VelocityGroundConstraintTangentPart { +impl OneBodyConstraintTangentPart { #[inline] pub fn generic_solve( &mut self, @@ -15,21 +14,21 @@ impl VelocityGroundConstraintTangentPart { jacobians: &DVector, ndofs2: usize, limit: Real, - mj_lambda2: usize, - mj_lambdas: &mut DVector, + solver_vel2: usize, + solver_vels: &mut DVector, ) { #[cfg(feature = "dim2")] { let dvel_0 = jacobians .rows(j_id2, ndofs2) - .dot(&mj_lambdas.rows(mj_lambda2, ndofs2)) + .dot(&solver_vels.rows(solver_vel2, ndofs2)) + self.rhs[0]; let new_impulse = (self.impulse[0] - self.r[0] * dvel_0).simd_clamp(-limit, limit); let dlambda = new_impulse - self.impulse[0]; self.impulse[0] = new_impulse; - mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy( + solver_vels.rows_mut(solver_vel2, ndofs2).axpy( dlambda, &jacobians.rows(j_id2 + ndofs2, ndofs2), 1.0, @@ -41,11 +40,11 @@ impl VelocityGroundConstraintTangentPart { let j_step = ndofs2 * 2; let dvel_0 = jacobians .rows(j_id2, ndofs2) - .dot(&mj_lambdas.rows(mj_lambda2, ndofs2)) + .dot(&solver_vels.rows(solver_vel2, ndofs2)) + self.rhs[0]; let dvel_1 = jacobians .rows(j_id2 + j_step, ndofs2) - .dot(&mj_lambdas.rows(mj_lambda2, ndofs2)) + .dot(&solver_vels.rows(solver_vel2, ndofs2)) + self.rhs[1]; let new_impulse = na::Vector2::new( @@ -57,12 +56,12 @@ impl VelocityGroundConstraintTangentPart { let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; - mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy( + solver_vels.rows_mut(solver_vel2, ndofs2).axpy( dlambda[0], &jacobians.rows(j_id2 + ndofs2, ndofs2), 1.0, ); - mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy( + solver_vels.rows_mut(solver_vel2, ndofs2).axpy( dlambda[1], &jacobians.rows(j_id2 + j_step + ndofs2, ndofs2), 1.0, @@ -71,7 +70,7 @@ impl VelocityGroundConstraintTangentPart { } } -impl VelocityGroundConstraintNormalPart { +impl OneBodyConstraintNormalPart { #[inline] pub fn generic_solve( &mut self, @@ -79,19 +78,19 @@ impl VelocityGroundConstraintNormalPart { j_id2: usize, jacobians: &DVector, ndofs2: usize, - mj_lambda2: usize, - mj_lambdas: &mut DVector, + solver_vel2: usize, + solver_vels: &mut DVector, ) { let dvel = jacobians .rows(j_id2, ndofs2) - .dot(&mj_lambdas.rows(mj_lambda2, ndofs2)) + .dot(&solver_vels.rows(solver_vel2, ndofs2)) + self.rhs; let new_impulse = cfm_factor * (self.impulse - self.r * dvel).max(0.0); let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; - mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy( + solver_vels.rows_mut(solver_vel2, ndofs2).axpy( dlambda, &jacobians.rows(j_id2 + ndofs2, ndofs2), 1.0, @@ -99,7 +98,7 @@ impl VelocityGroundConstraintNormalPart { } } -impl VelocityGroundConstraintElement { +impl OneBodyConstraintElement { #[inline] pub fn generic_solve_group( cfm_factor: Real, @@ -109,8 +108,8 @@ impl VelocityGroundConstraintElement { ndofs2: usize, // Jacobian index of the first constraint. j_id: usize, - mj_lambda2: usize, - mj_lambdas: &mut DVector, + solver_vel2: usize, + solver_vels: &mut DVector, solve_restitution: bool, solve_friction: bool, ) { @@ -122,7 +121,12 @@ impl VelocityGroundConstraintElement { for element in elements.iter_mut() { element.normal_part.generic_solve( - cfm_factor, nrm_j_id, jacobians, ndofs2, mj_lambda2, mj_lambdas, + cfm_factor, + nrm_j_id, + jacobians, + ndofs2, + solver_vel2, + solver_vels, ); nrm_j_id += j_step; } @@ -135,7 +139,7 @@ impl VelocityGroundConstraintElement { for element in elements.iter_mut() { let limit = limit * element.normal_part.impulse; let part = &mut element.tangent_part; - part.generic_solve(tng_j_id, jacobians, ndofs2, limit, mj_lambda2, mj_lambdas); + part.generic_solve(tng_j_id, jacobians, ndofs2, limit, solver_vel2, solver_vels); tng_j_id += j_step; } } diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs similarity index 58% rename from src/dynamics/solver/generic_velocity_constraint.rs rename to src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs index ed8c569a4..a4924fe68 100644 --- a/src/dynamics/solver/generic_velocity_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs @@ -1,43 +1,45 @@ -use crate::dynamics::solver::{GenericRhs, VelocityConstraint}; +use crate::dynamics::solver::{GenericRhs, TwoBodyConstraint}; use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS}; -use crate::utils::{WAngularInertia, WCross, WDot}; +use crate::utils::{SimdAngularInertia, SimdCross, SimdDot}; -use super::{ - AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart, -}; +use super::{TwoBodyConstraintBuilder, TwoBodyConstraintElement, TwoBodyConstraintNormalPart}; +use crate::dynamics::solver::solver_body::SolverBody; +use crate::dynamics::solver::{ContactPointInfos, SolverVel}; +use crate::prelude::RigidBodyHandle; #[cfg(feature = "dim2")] -use crate::utils::WBasis; +use crate::utils::SimdBasis; use na::DVector; -#[derive(Copy, Clone, Debug)] -pub(crate) struct GenericVelocityConstraint { - // We just build the generic constraint on top of the velocity constraint, - // adding some information we can use in the generic case. - pub velocity_constraint: VelocityConstraint, - pub j_id: usize, - pub ndofs1: usize, - pub ndofs2: usize, - pub generic_constraint_mask: u8, +#[derive(Copy, Clone)] +pub(crate) struct GenericTwoBodyConstraintBuilder { + handle1: RigidBodyHandle, + handle2: RigidBodyHandle, + ccd_thickness: Real, + inner: TwoBodyConstraintBuilder, } -impl GenericVelocityConstraint { +impl GenericTwoBodyConstraintBuilder { + pub fn invalid() -> Self { + Self { + handle1: RigidBodyHandle::invalid(), + handle2: RigidBodyHandle::invalid(), + ccd_thickness: Real::MAX, + inner: TwoBodyConstraintBuilder::invalid(), + } + } + pub fn generate( - params: &IntegrationParameters, manifold_id: ContactManifoldIndex, manifold: &ContactManifold, bodies: &RigidBodySet, multibodies: &MultibodyJointSet, - out_constraints: &mut Vec, + out_builders: &mut [GenericTwoBodyConstraintBuilder], + out_constraints: &mut [GenericTwoBodyConstraint], jacobians: &mut DVector, jacobian_id: &mut usize, - insert_at: Option, ) { - let cfm_factor = params.cfm_factor(); - let inv_dt = params.inv_dt(); - let erp_inv_dt = params.erp_inv_dt(); - let handle1 = manifold.data.rigid_body1.unwrap(); let handle2 = manifold.data.rigid_body2.unwrap(); @@ -46,7 +48,6 @@ impl GenericVelocityConstraint { let (vels1, mprops1, type1) = (&rb1.vels, &rb1.mprops, &rb1.body_type); let (vels2, mprops2, type2) = (&rb2.vels, &rb2.mprops, &rb2.body_type); - let ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness; let multibody1 = multibodies .rigid_body_link(handle1) @@ -54,14 +55,14 @@ impl GenericVelocityConstraint { let multibody2 = multibodies .rigid_body_link(handle2) .map(|m| (&multibodies[m.multibody], m.id)); - let mj_lambda1 = multibody1 + let solver_vel1 = multibody1 .map(|mb| mb.0.solver_id) .unwrap_or(if type1.is_dynamic() { rb1.ids.active_set_offset } else { 0 }); - let mj_lambda2 = multibody2 + let solver_vel2 = multibody2 .map(|mb| mb.0.solver_id) .unwrap_or(if type2.is_dynamic() { rb2.ids.active_set_offset @@ -87,50 +88,50 @@ impl GenericVelocityConstraint { jacobians.resize_vertically_mut(required_jacobian_len, 0.0); } - for (_l, manifold_points) in manifold + for (l, manifold_points) in manifold .data .solver_contacts .chunks(MAX_MANIFOLD_POINTS) .enumerate() { let chunk_j_id = *jacobian_id; - let mut is_fast_contact = false; - let mut constraint = VelocityConstraint { - dir1: force_dir1, - #[cfg(feature = "dim3")] - tangent1: tangents1[0], - elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], - im1: if type1.is_dynamic() { - mprops1.effective_inv_mass - } else { - na::zero() - }, - im2: if type2.is_dynamic() { - mprops2.effective_inv_mass - } else { - na::zero() - }, - cfm_factor, - limit: 0.0, - mj_lambda1, - mj_lambda2, - manifold_id, - manifold_contact_id: [0; MAX_MANIFOLD_POINTS], - num_contacts: manifold_points.len() as u8, + + let builder = &mut out_builders[l]; + let constraint = &mut out_constraints[l]; + constraint.inner.dir1 = force_dir1; + constraint.inner.im1 = if type1.is_dynamic() { + mprops1.effective_inv_mass + } else { + na::zero() }; + constraint.inner.im2 = if type2.is_dynamic() { + mprops2.effective_inv_mass + } else { + na::zero() + }; + constraint.inner.solver_vel1 = solver_vel1; + constraint.inner.solver_vel2 = solver_vel2; + constraint.inner.manifold_id = manifold_id; + constraint.inner.num_contacts = manifold_points.len() as u8; + #[cfg(feature = "dim3")] + { + constraint.inner.tangent1 = tangents1[0]; + } for k in 0..manifold_points.len() { let manifold_point = &manifold_points[k]; - let dp1 = manifold_point.point - mprops1.world_com; - let dp2 = manifold_point.point - mprops2.world_com; + let point = manifold_point.point; + let dp1 = point - mprops1.world_com; + let dp2 = point - mprops2.world_com; let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); - constraint.limit = manifold_point.friction; - constraint.manifold_contact_id[k] = manifold_point.contact_id; + constraint.inner.limit = manifold_point.friction; + constraint.inner.manifold_contact_id[k] = manifold_point.contact_id; // Normal part. + let normal_rhs_wo_bias; { let torque_dir1 = dp1.gcross(force_dir1); let torque_dir2 = dp2.gcross(-force_dir1); @@ -191,23 +192,16 @@ impl GenericVelocityConstraint { let r = crate::utils::inv(inv_r1 + inv_r2); let is_bouncy = manifold_point.is_bouncy() as u32 as Real; - let is_resting = 1.0 - is_bouncy; - - let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) - * (vel1 - vel2).dot(&force_dir1); - rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt; - rhs_wo_bias *= is_bouncy + is_resting; - let rhs_bias = - /* is_resting * */ erp_inv_dt * manifold_point.dist.clamp(-params.max_penetration_correction, 0.0); - let rhs = rhs_wo_bias + rhs_bias; - is_fast_contact = is_fast_contact || (-rhs * params.dt > ccd_thickness * 0.5); + normal_rhs_wo_bias = + (is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1); - constraint.elements[k].normal_part = VelocityConstraintNormalPart { + constraint.inner.elements[k].normal_part = TwoBodyConstraintNormalPart { gcross1, gcross2, - rhs, - rhs_wo_bias, + rhs: na::zero(), + rhs_wo_bias: na::zero(), + total_impulse: na::zero(), impulse: na::zero(), r, }; @@ -215,7 +209,7 @@ impl GenericVelocityConstraint { // Tangent parts. { - constraint.elements[k].tangent_part.impulse = na::zero(); + constraint.inner.elements[k].tangent_part.impulse = na::zero(); for j in 0..DIM - 1 { let torque_dir1 = dp1.gcross(tangents1[j]); @@ -226,7 +220,7 @@ impl GenericVelocityConstraint { } else { na::zero() }; - constraint.elements[k].tangent_part.gcross1[j] = gcross1; + constraint.inner.elements[k].tangent_part.gcross1[j] = gcross1; let torque_dir2 = dp2.gcross(-tangents1[j]); let gcross2 = if type2.is_dynamic() { @@ -236,7 +230,7 @@ impl GenericVelocityConstraint { } else { na::zero() }; - constraint.elements[k].tangent_part.gcross2[j] = gcross2; + constraint.inner.elements[k].tangent_part.gcross2[j] = gcross2; let inv_r1 = if let Some((mb1, link_id1)) = multibody1.as_ref() { mb1.fill_jacobians( @@ -277,23 +271,43 @@ impl GenericVelocityConstraint { }; let r = crate::utils::inv(inv_r1 + inv_r2); + let rhs_wo_bias = manifold_point.tangent_velocity.dot(&tangents1[j]); - let rhs = - (vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]); + constraint.inner.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias; + constraint.inner.elements[k].tangent_part.rhs[j] = rhs_wo_bias; - constraint.elements[k].tangent_part.rhs[j] = rhs; - // FIXME: in 3D, we should take into account gcross[0].dot(gcross[1]) + // TODO: in 3D, we should take into account gcross[0].dot(gcross[1]) // in lhs. See the corresponding code on the `velocity_constraint.rs` // file. - constraint.elements[k].tangent_part.r[j] = r; + constraint.inner.elements[k].tangent_part.r[j] = r; } } - } - constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor }; + // Builder. + let infos = ContactPointInfos { + local_p1: rb1 + .pos + .position + .inverse_transform_point(&manifold_point.point), + local_p2: rb2 + .pos + .position + .inverse_transform_point(&manifold_point.point), + tangent_vel: manifold_point.tangent_velocity, + dist: manifold_point.dist, + normal_rhs_wo_bias, + }; + + builder.handle1 = handle1; + builder.handle2 = handle2; + builder.ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness; + builder.inner.infos[k] = infos; + constraint.inner.manifold_contact_id[k] = manifold_point.contact_id; + } let ndofs1 = multibody1.map(|mb| mb.0.ndofs()).unwrap_or(0); let ndofs2 = multibody2.map(|mb| mb.0.ndofs()).unwrap_or(0); + // NOTE: we use the generic constraint for non-dynamic bodies because this will // reduce all ops to nothing because its ndofs will be zero. let generic_constraint_mask = (multibody1.is_some() as u8) @@ -301,78 +315,119 @@ impl GenericVelocityConstraint { | (!type1.is_dynamic() as u8) | ((!type2.is_dynamic() as u8) << 1); - let constraint = GenericVelocityConstraint { - velocity_constraint: constraint, - j_id: chunk_j_id, - ndofs1, - ndofs2, - generic_constraint_mask, - }; + constraint.j_id = chunk_j_id; + constraint.ndofs1 = ndofs1; + constraint.ndofs2 = ndofs2; + constraint.generic_constraint_mask = generic_constraint_mask; + } + } - if let Some(at) = insert_at { - out_constraints[at + _l] = AnyVelocityConstraint::NongroupedGeneric(constraint); - } else { - out_constraints.push(AnyVelocityConstraint::NongroupedGeneric(constraint)); - } + pub fn update( + &self, + params: &IntegrationParameters, + solved_dt: Real, + bodies: &[SolverBody], + multibodies: &MultibodyJointSet, + constraint: &mut GenericTwoBodyConstraint, + ) { + // We don’t update jacobians so the update is mostly identical to the non-generic velocity constraint. + let pos1 = multibodies + .rigid_body_link(self.handle1) + .map(|m| &multibodies[m.multibody].link(m.id).unwrap().local_to_world) + .unwrap_or_else(|| &bodies[constraint.inner.solver_vel1].position); + let pos2 = multibodies + .rigid_body_link(self.handle2) + .map(|m| &multibodies[m.multibody].link(m.id).unwrap().local_to_world) + .unwrap_or_else(|| &bodies[constraint.inner.solver_vel2].position); + + self.inner.update_with_positions( + params, + solved_dt, + pos1, + pos2, + self.ccd_thickness, + &mut constraint.inner, + ); + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct GenericTwoBodyConstraint { + // We just build the generic constraint on top of the velocity constraint, + // adding some information we can use in the generic case. + pub inner: TwoBodyConstraint, + pub j_id: usize, + pub ndofs1: usize, + pub ndofs2: usize, + pub generic_constraint_mask: u8, +} + +impl GenericTwoBodyConstraint { + pub fn invalid() -> Self { + Self { + inner: TwoBodyConstraint::invalid(), + j_id: usize::MAX, + ndofs1: usize::MAX, + ndofs2: usize::MAX, + generic_constraint_mask: u8::MAX, } } pub fn solve( &mut self, jacobians: &DVector, - mj_lambdas: &mut [DeltaVel], - generic_mj_lambdas: &mut DVector, + solver_vels: &mut [SolverVel], + generic_solver_vels: &mut DVector, solve_restitution: bool, solve_friction: bool, ) { - let mut mj_lambda1 = if self.generic_constraint_mask & 0b01 == 0 { - GenericRhs::DeltaVel(mj_lambdas[self.velocity_constraint.mj_lambda1 as usize]) + let mut solver_vel1 = if self.generic_constraint_mask & 0b01 == 0 { + GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1 as usize]) } else { - GenericRhs::GenericId(self.velocity_constraint.mj_lambda1 as usize) + GenericRhs::GenericId(self.inner.solver_vel1 as usize) }; - let mut mj_lambda2 = if self.generic_constraint_mask & 0b10 == 0 { - GenericRhs::DeltaVel(mj_lambdas[self.velocity_constraint.mj_lambda2 as usize]) + let mut solver_vel2 = if self.generic_constraint_mask & 0b10 == 0 { + GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2 as usize]) } else { - GenericRhs::GenericId(self.velocity_constraint.mj_lambda2 as usize) + GenericRhs::GenericId(self.inner.solver_vel2 as usize) }; - let elements = &mut self.velocity_constraint.elements - [..self.velocity_constraint.num_contacts as usize]; - VelocityConstraintElement::generic_solve_group( - self.velocity_constraint.cfm_factor, + let elements = &mut self.inner.elements[..self.inner.num_contacts as usize]; + TwoBodyConstraintElement::generic_solve_group( + self.inner.cfm_factor, elements, jacobians, - &self.velocity_constraint.dir1, + &self.inner.dir1, #[cfg(feature = "dim3")] - &self.velocity_constraint.tangent1, - &self.velocity_constraint.im1, - &self.velocity_constraint.im2, - self.velocity_constraint.limit, + &self.inner.tangent1, + &self.inner.im1, + &self.inner.im2, + self.inner.limit, self.ndofs1, self.ndofs2, self.j_id, - &mut mj_lambda1, - &mut mj_lambda2, - generic_mj_lambdas, + &mut solver_vel1, + &mut solver_vel2, + generic_solver_vels, solve_restitution, solve_friction, ); - if let GenericRhs::DeltaVel(mj_lambda1) = mj_lambda1 { - mj_lambdas[self.velocity_constraint.mj_lambda1 as usize] = mj_lambda1; + if let GenericRhs::SolverVel(solver_vel1) = solver_vel1 { + solver_vels[self.inner.solver_vel1 as usize] = solver_vel1; } - if let GenericRhs::DeltaVel(mj_lambda2) = mj_lambda2 { - mj_lambdas[self.velocity_constraint.mj_lambda2 as usize] = mj_lambda2; + if let GenericRhs::SolverVel(solver_vel2) = solver_vel2 { + solver_vels[self.inner.solver_vel2 as usize] = solver_vel2; } } pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { - self.velocity_constraint.writeback_impulses(manifolds_all); + self.inner.writeback_impulses(manifolds_all); } pub fn remove_cfm_and_bias_from_rhs(&mut self) { - self.velocity_constraint.remove_cfm_and_bias_from_rhs(); + self.inner.remove_cfm_and_bias_from_rhs(); } } diff --git a/src/dynamics/solver/generic_velocity_constraint_element.rs b/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs similarity index 71% rename from src/dynamics/solver/generic_velocity_constraint_element.rs rename to src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs index 19fba43c3..f3bd3a0e3 100644 --- a/src/dynamics/solver/generic_velocity_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs @@ -1,15 +1,15 @@ -use super::DeltaVel; +use crate::dynamics::solver::SolverVel; use crate::dynamics::solver::{ - VelocityConstraintElement, VelocityConstraintNormalPart, VelocityConstraintTangentPart, + TwoBodyConstraintElement, TwoBodyConstraintNormalPart, TwoBodyConstraintTangentPart, }; use crate::math::{AngVector, Real, Vector, DIM}; -use crate::utils::WDot; +use crate::utils::SimdDot; use na::DVector; #[cfg(feature = "dim2")] -use {crate::utils::WBasis, na::SimdPartialOrd}; +use {crate::utils::SimdBasis, na::SimdPartialOrd}; pub(crate) enum GenericRhs { - DeltaVel(DeltaVel), + SolverVel(SolverVel), GenericId(usize), } @@ -48,13 +48,13 @@ impl GenericRhs { jacobians: &DVector, dir: &Vector, gcross: &AngVector, - mj_lambdas: &DVector, + solver_vels: &DVector, ) -> Real { match self { - GenericRhs::DeltaVel(rhs) => dir.dot(&rhs.linear) + gcross.gdot(rhs.angular), - GenericRhs::GenericId(mj_lambda) => { + GenericRhs::SolverVel(rhs) => dir.dot(&rhs.linear) + gcross.gdot(rhs.angular), + GenericRhs::GenericId(solver_vel) => { let j = jacobians.rows(j_id, ndofs); - let rhs = mj_lambdas.rows(*mj_lambda, ndofs); + let rhs = solver_vels.rows(*solver_vel, ndofs); j.dot(&rhs) } } @@ -69,25 +69,25 @@ impl GenericRhs { jacobians: &DVector, dir: &Vector, gcross: &AngVector, - mj_lambdas: &mut DVector, + solver_vels: &mut DVector, inv_mass: &Vector, ) { match self { - GenericRhs::DeltaVel(rhs) => { + GenericRhs::SolverVel(rhs) => { rhs.linear += dir.component_mul(inv_mass) * impulse; rhs.angular += gcross * impulse; } - GenericRhs::GenericId(mj_lambda) => { + GenericRhs::GenericId(solver_vel) => { let wj_id = j_id + ndofs; let wj = jacobians.rows(wj_id, ndofs); - let mut rhs = mj_lambdas.rows_mut(*mj_lambda, ndofs); + let mut rhs = solver_vels.rows_mut(*solver_vel, ndofs); rhs.axpy(impulse, &wj, 1.0); } } } } -impl VelocityConstraintTangentPart { +impl TwoBodyConstraintTangentPart { #[inline] pub fn generic_solve( &mut self, @@ -99,9 +99,9 @@ impl VelocityConstraintTangentPart { ndofs1: usize, ndofs2: usize, limit: Real, - mj_lambda1: &mut GenericRhs, - mj_lambda2: &mut GenericRhs, - mj_lambdas: &mut DVector, + solver_vel1: &mut GenericRhs, + solver_vel2: &mut GenericRhs, + solver_vels: &mut DVector, ) { let j_id1 = j_id1(j_id, ndofs1, ndofs2); let j_id2 = j_id2(j_id, ndofs1, ndofs2); @@ -110,79 +110,79 @@ impl VelocityConstraintTangentPart { #[cfg(feature = "dim2")] { - let dvel_0 = mj_lambda1.dvel( + let dvel_0 = solver_vel1.dvel( j_id1, ndofs1, jacobians, &tangents1[0], &self.gcross1[0], - mj_lambdas, - ) + mj_lambda2.dvel( + solver_vels, + ) + solver_vel2.dvel( j_id2, ndofs2, jacobians, &-tangents1[0], &self.gcross2[0], - mj_lambdas, + solver_vels, ) + self.rhs[0]; let new_impulse = (self.impulse[0] - self.r[0] * dvel_0).simd_clamp(-limit, limit); let dlambda = new_impulse - self.impulse[0]; self.impulse[0] = new_impulse; - mj_lambda1.apply_impulse( + solver_vel1.apply_impulse( j_id1, ndofs1, dlambda, jacobians, &tangents1[0], &self.gcross1[0], - mj_lambdas, + solver_vels, im1, ); - mj_lambda2.apply_impulse( + solver_vel2.apply_impulse( j_id2, ndofs2, dlambda, jacobians, &-tangents1[0], &self.gcross2[0], - mj_lambdas, + solver_vels, im2, ); } #[cfg(feature = "dim3")] { - let dvel_0 = mj_lambda1.dvel( + let dvel_0 = solver_vel1.dvel( j_id1, ndofs1, jacobians, &tangents1[0], &self.gcross1[0], - mj_lambdas, - ) + mj_lambda2.dvel( + solver_vels, + ) + solver_vel2.dvel( j_id2, ndofs2, jacobians, &-tangents1[0], &self.gcross2[0], - mj_lambdas, + solver_vels, ) + self.rhs[0]; - let dvel_1 = mj_lambda1.dvel( + let dvel_1 = solver_vel1.dvel( j_id1 + j_step, ndofs1, jacobians, &tangents1[1], &self.gcross1[1], - mj_lambdas, - ) + mj_lambda2.dvel( + solver_vels, + ) + solver_vel2.dvel( j_id2 + j_step, ndofs2, jacobians, &-tangents1[1], &self.gcross2[1], - mj_lambdas, + solver_vels, ) + self.rhs[1]; let new_impulse = na::Vector2::new( @@ -194,52 +194,52 @@ impl VelocityConstraintTangentPart { let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; - mj_lambda1.apply_impulse( + solver_vel1.apply_impulse( j_id1, ndofs1, dlambda[0], jacobians, &tangents1[0], &self.gcross1[0], - mj_lambdas, + solver_vels, im1, ); - mj_lambda1.apply_impulse( + solver_vel1.apply_impulse( j_id1 + j_step, ndofs1, dlambda[1], jacobians, &tangents1[1], &self.gcross1[1], - mj_lambdas, + solver_vels, im1, ); - mj_lambda2.apply_impulse( + solver_vel2.apply_impulse( j_id2, ndofs2, dlambda[0], jacobians, &-tangents1[0], &self.gcross2[0], - mj_lambdas, + solver_vels, im2, ); - mj_lambda2.apply_impulse( + solver_vel2.apply_impulse( j_id2 + j_step, ndofs2, dlambda[1], jacobians, &-tangents1[1], &self.gcross2[1], - mj_lambdas, + solver_vels, im2, ); } } } -impl VelocityConstraintNormalPart { +impl TwoBodyConstraintNormalPart { #[inline] pub fn generic_solve( &mut self, @@ -251,45 +251,45 @@ impl VelocityConstraintNormalPart { im2: &Vector, ndofs1: usize, ndofs2: usize, - mj_lambda1: &mut GenericRhs, - mj_lambda2: &mut GenericRhs, - mj_lambdas: &mut DVector, + solver_vel1: &mut GenericRhs, + solver_vel2: &mut GenericRhs, + solver_vels: &mut DVector, ) { let j_id1 = j_id1(j_id, ndofs1, ndofs2); let j_id2 = j_id2(j_id, ndofs1, ndofs2); - let dvel = mj_lambda1.dvel(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, mj_lambdas) - + mj_lambda2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas) + let dvel = solver_vel1.dvel(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, solver_vels) + + solver_vel2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, solver_vels) + self.rhs; let new_impulse = cfm_factor * (self.impulse - self.r * dvel).max(0.0); let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; - mj_lambda1.apply_impulse( + solver_vel1.apply_impulse( j_id1, ndofs1, dlambda, jacobians, &dir1, &self.gcross1, - mj_lambdas, + solver_vels, im1, ); - mj_lambda2.apply_impulse( + solver_vel2.apply_impulse( j_id2, ndofs2, dlambda, jacobians, &-dir1, &self.gcross2, - mj_lambdas, + solver_vels, im2, ); } } -impl VelocityConstraintElement { +impl TwoBodyConstraintElement { #[inline] pub fn generic_solve_group( cfm_factor: Real, @@ -306,9 +306,9 @@ impl VelocityConstraintElement { ndofs2: usize, // Jacobian index of the first constraint. j_id: usize, - mj_lambda1: &mut GenericRhs, - mj_lambda2: &mut GenericRhs, - mj_lambdas: &mut DVector, + solver_vel1: &mut GenericRhs, + solver_vel2: &mut GenericRhs, + solver_vels: &mut DVector, solve_restitution: bool, solve_friction: bool, ) { @@ -320,8 +320,17 @@ impl VelocityConstraintElement { for element in elements.iter_mut() { element.normal_part.generic_solve( - cfm_factor, nrm_j_id, jacobians, &dir1, im1, im2, ndofs1, ndofs2, mj_lambda1, - mj_lambda2, mj_lambdas, + cfm_factor, + nrm_j_id, + jacobians, + &dir1, + im1, + im2, + ndofs1, + ndofs2, + solver_vel1, + solver_vel2, + solver_vels, ); nrm_j_id += j_step; } @@ -339,8 +348,17 @@ impl VelocityConstraintElement { let limit = limit * element.normal_part.impulse; let part = &mut element.tangent_part; part.generic_solve( - tng_j_id, jacobians, tangents1, im1, im2, ndofs1, ndofs2, limit, mj_lambda1, - mj_lambda2, mj_lambdas, + tng_j_id, + jacobians, + tangents1, + im1, + im2, + ndofs1, + ndofs2, + limit, + solver_vel1, + solver_vel2, + solver_vels, ); tng_j_id += j_step; } diff --git a/src/dynamics/solver/contact_constraint/mod.rs b/src/dynamics/solver/contact_constraint/mod.rs new file mode 100644 index 000000000..09099dd8a --- /dev/null +++ b/src/dynamics/solver/contact_constraint/mod.rs @@ -0,0 +1,29 @@ +pub(crate) use generic_one_body_constraint::*; +// pub(crate) use generic_one_body_constraint_element::*; +pub(crate) use contact_constraints_set::{ + ConstraintsCounts, ContactConstraintTypes, ContactConstraintsSet, +}; +pub(crate) use generic_two_body_constraint::*; +pub(crate) use generic_two_body_constraint_element::*; +pub(crate) use one_body_constraint::*; +pub(crate) use one_body_constraint_element::*; +#[cfg(feature = "simd-is-enabled")] +pub(crate) use one_body_constraint_simd::*; +pub(crate) use two_body_constraint::*; +pub(crate) use two_body_constraint_element::*; +#[cfg(feature = "simd-is-enabled")] +pub(crate) use two_body_constraint_simd::*; + +mod contact_constraints_set; +mod generic_one_body_constraint; +mod generic_one_body_constraint_element; +mod generic_two_body_constraint; +mod generic_two_body_constraint_element; +mod one_body_constraint; +mod one_body_constraint_element; +#[cfg(feature = "simd-is-enabled")] +mod one_body_constraint_simd; +mod two_body_constraint; +mod two_body_constraint_element; +#[cfg(feature = "simd-is-enabled")] +mod two_body_constraint_simd; diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint.rs b/src/dynamics/solver/contact_constraint/one_body_constraint.rs new file mode 100644 index 000000000..57931cdd6 --- /dev/null +++ b/src/dynamics/solver/contact_constraint/one_body_constraint.rs @@ -0,0 +1,382 @@ +use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart}; +use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; +#[cfg(feature = "dim2")] +use crate::utils::SimdBasis; +use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot, SimdRealCopy}; +use parry::math::Isometry; + +use crate::dynamics::solver::solver_body::SolverBody; +use crate::dynamics::solver::SolverVel; +use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, RigidBodyVelocity}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; + +// TODO: move this struct somewhere else. +#[derive(Copy, Clone, Debug)] +pub struct ContactPointInfos { + pub tangent_vel: Vector, + pub local_p1: Point, + pub local_p2: Point, + pub dist: N, + pub normal_rhs_wo_bias: N, +} + +impl Default for ContactPointInfos { + fn default() -> Self { + Self { + tangent_vel: Vector::zeros(), + local_p1: Point::origin(), + local_p2: Point::origin(), + dist: N::zero(), + normal_rhs_wo_bias: N::zero(), + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct OneBodyConstraintBuilder { + // PERF: only store what’s necessary for the bias updates instead of the complete solver body. + pub rb1: SolverBody, + pub vels1: RigidBodyVelocity, + pub infos: [ContactPointInfos; MAX_MANIFOLD_POINTS], +} + +impl OneBodyConstraintBuilder { + pub fn invalid() -> Self { + Self { + rb1: SolverBody::default(), + vels1: RigidBodyVelocity::zero(), + infos: [ContactPointInfos::default(); MAX_MANIFOLD_POINTS], + } + } + + pub fn generate( + manifold_id: ContactManifoldIndex, + manifold: &ContactManifold, + bodies: &RigidBodySet, + out_builders: &mut [OneBodyConstraintBuilder], + out_constraints: &mut [OneBodyConstraint], + ) { + let mut handle1 = manifold.data.rigid_body1; + let mut handle2 = manifold.data.rigid_body2; + let flipped = manifold.data.relative_dominance < 0; + + let (force_dir1, flipped_multiplier) = if flipped { + std::mem::swap(&mut handle1, &mut handle2); + (manifold.data.normal, -1.0) + } else { + (-manifold.data.normal, 1.0) + }; + + let (vels1, world_com1) = if let Some(handle1) = handle1 { + let rb1 = &bodies[handle1]; + (rb1.vels, rb1.mprops.world_com) + } else { + (RigidBodyVelocity::zero(), Point::origin()) + }; + + let rb1 = handle1 + .map(|h| SolverBody::from(&bodies[h])) + .unwrap_or_else(SolverBody::default); + + let rb2 = &bodies[handle2.unwrap()]; + let vels2 = &rb2.vels; + let mprops2 = &rb2.mprops; + + #[cfg(feature = "dim2")] + let tangents1 = force_dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let tangents1 = + super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel); + + let solver_vel2 = rb2.ids.active_set_offset; + + for (l, manifold_points) in manifold + .data + .solver_contacts + .chunks(MAX_MANIFOLD_POINTS) + .enumerate() + { + let builder = &mut out_builders[l]; + let constraint = &mut out_constraints[l]; + + builder.rb1 = rb1; + builder.vels1 = vels1; + + constraint.dir1 = force_dir1; + constraint.im2 = mprops2.effective_inv_mass; + constraint.solver_vel2 = solver_vel2; + constraint.manifold_id = manifold_id; + constraint.num_contacts = manifold_points.len() as u8; + #[cfg(feature = "dim3")] + { + constraint.tangent1 = tangents1[0]; + } + + for k in 0..manifold_points.len() { + let manifold_point = &manifold_points[k]; + + let dp2 = manifold_point.point - mprops2.world_com; + let dp1 = manifold_point.point - world_com1; + let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); + let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); + + constraint.limit = manifold_point.friction; + constraint.manifold_contact_id[k] = manifold_point.contact_id; + + // Normal part. + let normal_rhs_wo_bias; + { + let gcross2 = mprops2 + .effective_world_inv_inertia_sqrt + .transform_vector(dp2.gcross(-force_dir1)); + + let projected_mass = utils::inv( + force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) + + gcross2.gdot(gcross2), + ); + + let is_bouncy = manifold_point.is_bouncy() as u32 as Real; + + let proj_vel1 = vel1.dot(&force_dir1); + let proj_vel2 = vel2.dot(&force_dir1); + let dvel = proj_vel1 - proj_vel2; + // NOTE: we add proj_vel1 since it’s not accessible through solver_vel. + normal_rhs_wo_bias = + proj_vel1 + (is_bouncy * manifold_point.restitution) * dvel; + + constraint.elements[k].normal_part = OneBodyConstraintNormalPart { + gcross2, + rhs: na::zero(), + rhs_wo_bias: na::zero(), + impulse: na::zero(), + total_impulse: na::zero(), + r: projected_mass, + }; + } + + // Tangent parts. + { + constraint.elements[k].tangent_part.impulse = na::zero(); + + for j in 0..DIM - 1 { + let gcross2 = mprops2 + .effective_world_inv_inertia_sqrt + .transform_vector(dp2.gcross(-tangents1[j])); + let r = tangents1[j] + .dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j])) + + gcross2.gdot(gcross2); + let rhs_wo_bias = (vel1 + + flipped_multiplier * manifold_point.tangent_velocity) + .dot(&tangents1[j]); + + constraint.elements[k].tangent_part.gcross2[j] = gcross2; + constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias; + constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias; + constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") { + utils::inv(r) + } else { + r + }; + } + + #[cfg(feature = "dim3")] + { + constraint.elements[k].tangent_part.r[2] = 2.0 + * constraint.elements[k].tangent_part.gcross2[0] + .gdot(constraint.elements[k].tangent_part.gcross2[1]); + } + } + + // Builder. + { + let local_p1 = rb1.position.inverse_transform_point(&manifold_point.point); + let local_p2 = rb2 + .pos + .position + .inverse_transform_point(&manifold_point.point); + let infos = ContactPointInfos { + local_p1, + local_p2, + tangent_vel: flipped_multiplier * manifold_point.tangent_velocity, + dist: manifold_point.dist, + normal_rhs_wo_bias, + }; + + builder.infos[k] = infos; + } + } + } + } + + pub fn update( + &self, + params: &IntegrationParameters, + solved_dt: Real, + bodies: &[SolverBody], + _multibodies: &MultibodyJointSet, + constraint: &mut OneBodyConstraint, + ) { + let rb2 = &bodies[constraint.solver_vel2]; + self.update_with_positions( + params, + solved_dt, + &rb2.position, + rb2.ccd_thickness, + constraint, + ) + } + + // TODO: this code is SOOOO similar to TwoBodyConstraint::update. + // In fact the only differences are types and the `rb1` and ignoring its ccd thickness. + pub fn update_with_positions( + &self, + params: &IntegrationParameters, + solved_dt: Real, + rb2_pos: &Isometry, + ccd_thickness: Real, + constraint: &mut OneBodyConstraint, + ) { + let cfm_factor = params.cfm_factor(); + let inv_dt = params.inv_dt(); + let erp_inv_dt = params.erp_inv_dt(); + + let all_infos = &self.infos[..constraint.num_contacts as usize]; + let all_elements = &mut constraint.elements[..constraint.num_contacts as usize]; + let rb1 = &self.rb1; + // Integrate the velocity of the static rigid-body, if it’s kinematic. + let new_pos1 = self + .vels1 + .integrate(solved_dt, &rb1.position, &rb1.local_com); + let mut is_fast_contact = false; + + #[cfg(feature = "dim2")] + let tangents1 = constraint.dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let tangents1 = [ + constraint.tangent1, + constraint.dir1.cross(&constraint.tangent1), + ]; + + for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) { + // NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface. + let p1 = new_pos1 * info.local_p1 + info.tangent_vel * solved_dt; + let p2 = rb2_pos * info.local_p2; + let dist = info.dist + (p1 - p2).dot(&constraint.dir1); + + // Normal part. + { + let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt; + let rhs_bias = erp_inv_dt + * (dist + params.allowed_linear_error) + .clamp(-params.max_penetration_correction, 0.0); + let new_rhs = rhs_wo_bias + rhs_bias; + let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse; + is_fast_contact = is_fast_contact || (-new_rhs * params.dt > ccd_thickness * 0.5); + + element.normal_part.rhs_wo_bias = rhs_wo_bias; + element.normal_part.rhs = new_rhs; + element.normal_part.total_impulse = total_impulse; + element.normal_part.impulse = na::zero(); + } + + // Tangent part. + { + element.tangent_part.total_impulse += element.tangent_part.impulse; + element.tangent_part.impulse = na::zero(); + + for j in 0..DIM - 1 { + let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt; + element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias; + } + } + } + + constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor }; + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct OneBodyConstraint { + pub solver_vel2: usize, + pub dir1: Vector, // Non-penetration force direction for the first body. + #[cfg(feature = "dim3")] + pub tangent1: Vector, // One of the friction force directions. + pub im2: Vector, + pub cfm_factor: Real, + pub limit: Real, + pub elements: [OneBodyConstraintElement; MAX_MANIFOLD_POINTS], + + pub manifold_id: ContactManifoldIndex, + pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS], + pub num_contacts: u8, +} + +impl OneBodyConstraint { + pub fn invalid() -> Self { + Self { + solver_vel2: usize::MAX, + dir1: Vector::zeros(), + #[cfg(feature = "dim3")] + tangent1: Vector::zeros(), + im2: Vector::zeros(), + cfm_factor: 0.0, + limit: 0.0, + elements: [OneBodyConstraintElement::zero(); MAX_MANIFOLD_POINTS], + manifold_id: ContactManifoldIndex::MAX, + manifold_contact_id: [u8::MAX; MAX_MANIFOLD_POINTS], + num_contacts: u8::MAX, + } + } + + pub fn solve( + &mut self, + solver_vels: &mut [SolverVel], + solve_normal: bool, + solve_friction: bool, + ) { + let mut solver_vel2 = solver_vels[self.solver_vel2 as usize]; + + OneBodyConstraintElement::solve_group( + self.cfm_factor, + &mut self.elements[..self.num_contacts as usize], + &self.dir1, + #[cfg(feature = "dim3")] + &self.tangent1, + &self.im2, + self.limit, + &mut solver_vel2, + solve_normal, + solve_friction, + ); + + solver_vels[self.solver_vel2 as usize] = solver_vel2; + } + + // FIXME: duplicated code. This is exactly the same as in the two-body velocity constraint. + pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { + let manifold = &mut manifolds_all[self.manifold_id]; + + for k in 0..self.num_contacts as usize { + let contact_id = self.manifold_contact_id[k]; + let active_contact = &mut manifold.points[contact_id as usize]; + active_contact.data.impulse = self.elements[k].normal_part.impulse; + + #[cfg(feature = "dim2")] + { + active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0]; + } + #[cfg(feature = "dim3")] + { + active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse; + } + } + } + + pub fn remove_cfm_and_bias_from_rhs(&mut self) { + self.cfm_factor = 1.0; + for elt in &mut self.elements { + elt.normal_part.rhs = elt.normal_part.rhs_wo_bias; + elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias; + } + } +} diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs new file mode 100644 index 000000000..c5c094442 --- /dev/null +++ b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs @@ -0,0 +1,232 @@ +use crate::dynamics::solver::SolverVel; +use crate::math::{AngVector, Vector, DIM}; +use crate::utils::{SimdBasis, SimdDot, SimdRealCopy}; + +#[derive(Copy, Clone, Debug)] +pub(crate) struct OneBodyConstraintTangentPart { + pub gcross2: [AngVector; DIM - 1], + pub rhs: [N; DIM - 1], + pub rhs_wo_bias: [N; DIM - 1], + #[cfg(feature = "dim2")] + pub impulse: na::Vector1, + #[cfg(feature = "dim3")] + pub impulse: na::Vector2, + #[cfg(feature = "dim2")] + pub total_impulse: na::Vector1, + #[cfg(feature = "dim3")] + pub total_impulse: na::Vector2, + #[cfg(feature = "dim2")] + pub r: [N; 1], + #[cfg(feature = "dim3")] + pub r: [N; DIM], +} + +impl OneBodyConstraintTangentPart { + fn zero() -> Self { + Self { + gcross2: [na::zero(); DIM - 1], + rhs: [na::zero(); DIM - 1], + rhs_wo_bias: [na::zero(); DIM - 1], + impulse: na::zero(), + total_impulse: na::zero(), + #[cfg(feature = "dim2")] + r: [na::zero(); 1], + #[cfg(feature = "dim3")] + r: [na::zero(); DIM], + } + } + + #[inline] + pub fn apply_limit( + &mut self, + tangents1: [&Vector; DIM - 1], + im2: &Vector, + limit: N, + solver_vel2: &mut SolverVel, + ) where + AngVector: SimdDot, Result = N>, + { + #[cfg(feature = "dim2")] + { + let new_impulse = self.impulse[0].simd_clamp(-limit, limit); + let dlambda = new_impulse - self.impulse[0]; + self.impulse[0] = new_impulse; + + solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda; + solver_vel2.angular += self.gcross2[0] * dlambda; + } + + #[cfg(feature = "dim3")] + { + let new_impulse = self.impulse; + let new_impulse = { + let _disable_fe_except = + crate::utils::DisableFloatingPointExceptionsFlags:: + disable_floating_point_exceptions(); + new_impulse.simd_cap_magnitude(limit) + }; + let dlambda = new_impulse - self.impulse; + self.impulse = new_impulse; + + solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0] + + tangents1[1].component_mul(im2) * -dlambda[1]; + solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1]; + } + } + + #[inline] + pub fn solve( + &mut self, + tangents1: [&Vector; DIM - 1], + im2: &Vector, + limit: N, + solver_vel2: &mut SolverVel, + ) where + AngVector: SimdDot, Result = N>, + { + #[cfg(feature = "dim2")] + { + let dvel = -tangents1[0].dot(&solver_vel2.linear) + + self.gcross2[0].gdot(solver_vel2.angular) + + self.rhs[0]; + let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit); + let dlambda = new_impulse - self.impulse[0]; + self.impulse[0] = new_impulse; + + solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda; + solver_vel2.angular += self.gcross2[0] * dlambda; + } + + #[cfg(feature = "dim3")] + { + let dvel_0 = -tangents1[0].dot(&solver_vel2.linear) + + self.gcross2[0].gdot(solver_vel2.angular) + + self.rhs[0]; + let dvel_1 = -tangents1[1].dot(&solver_vel2.linear) + + self.gcross2[1].gdot(solver_vel2.angular) + + self.rhs[1]; + + let dvel_00 = dvel_0 * dvel_0; + let dvel_11 = dvel_1 * dvel_1; + let dvel_01 = dvel_0 * dvel_1; + let inv_lhs = (dvel_00 + dvel_11) + * crate::utils::simd_inv( + dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2], + ); + let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1]; + let new_impulse = self.impulse - delta_impulse; + let new_impulse = { + let _disable_fe_except = + crate::utils::DisableFloatingPointExceptionsFlags:: + disable_floating_point_exceptions(); + new_impulse.simd_cap_magnitude(limit) + }; + let dlambda = new_impulse - self.impulse; + self.impulse = new_impulse; + + solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0] + + tangents1[1].component_mul(im2) * -dlambda[1]; + solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1]; + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct OneBodyConstraintNormalPart { + pub gcross2: AngVector, + pub rhs: N, + pub rhs_wo_bias: N, + pub impulse: N, + pub total_impulse: N, + pub r: N, +} + +impl OneBodyConstraintNormalPart { + fn zero() -> Self { + Self { + gcross2: na::zero(), + rhs: na::zero(), + rhs_wo_bias: na::zero(), + impulse: na::zero(), + total_impulse: na::zero(), + r: na::zero(), + } + } + + #[inline] + pub fn solve( + &mut self, + cfm_factor: N, + dir1: &Vector, + im2: &Vector, + solver_vel2: &mut SolverVel, + ) where + AngVector: SimdDot, Result = N>, + { + let dvel = + -dir1.dot(&solver_vel2.linear) + self.gcross2.gdot(solver_vel2.angular) + self.rhs; + let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero()); + let dlambda = new_impulse - self.impulse; + self.impulse = new_impulse; + + solver_vel2.linear += dir1.component_mul(im2) * -dlambda; + solver_vel2.angular += self.gcross2 * dlambda; + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct OneBodyConstraintElement { + pub normal_part: OneBodyConstraintNormalPart, + pub tangent_part: OneBodyConstraintTangentPart, +} + +impl OneBodyConstraintElement { + pub fn zero() -> Self { + Self { + normal_part: OneBodyConstraintNormalPart::zero(), + tangent_part: OneBodyConstraintTangentPart::zero(), + } + } + + #[inline] + pub fn solve_group( + cfm_factor: N, + elements: &mut [Self], + dir1: &Vector, + #[cfg(feature = "dim3")] tangent1: &Vector, + im2: &Vector, + limit: N, + solver_vel2: &mut SolverVel, + solve_normal: bool, + solve_friction: bool, + ) where + Vector: SimdBasis, + AngVector: SimdDot, Result = N>, + { + #[cfg(feature = "dim3")] + let tangents1 = [tangent1, &dir1.cross(&tangent1)]; + #[cfg(feature = "dim2")] + let tangents1 = [&dir1.orthonormal_vector()]; + + // Solve penetration. + if solve_normal { + for element in elements.iter_mut() { + element + .normal_part + .solve(cfm_factor, &dir1, im2, solver_vel2); + let limit = limit * element.normal_part.impulse; + let part = &mut element.tangent_part; + part.apply_limit(tangents1, im2, limit, solver_vel2); + } + } + + // Solve friction. + if solve_friction { + for element in elements.iter_mut() { + let limit = limit * element.normal_part.impulse; + let part = &mut element.tangent_part; + part.solve(tangents1, im2, limit, solver_vel2); + } + } + } +} diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs similarity index 52% rename from src/dynamics/solver/velocity_ground_constraint_wide.rs rename to src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs index 25bb30d93..33e0d77f9 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs @@ -1,52 +1,38 @@ -use super::{ - AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement, - VelocityGroundConstraintNormalPart, -}; +use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart}; +use crate::dynamics::solver::solver_body::SolverBody; +use crate::dynamics::solver::{ContactPointInfos, SolverVel}; use crate::dynamics::{ - IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity, + IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodySet, + RigidBodyVelocity, }; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ - AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, + AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, + SIMD_WIDTH, }; #[cfg(feature = "dim2")] -use crate::utils::WBasis; -use crate::utils::{self, WAngularInertia, WCross, WDot}; +use crate::utils::SimdBasis; +use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot}; use num::Zero; use parry::math::SimdBool; use simba::simd::{SimdPartialOrd, SimdValue}; #[derive(Copy, Clone, Debug)] -pub(crate) struct WVelocityGroundConstraint { - pub dir1: Vector, // Non-penetration force direction for the first body. - #[cfg(feature = "dim3")] - pub tangent1: Vector, // One of the friction force directions. - pub elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS], - pub num_contacts: u8, - pub im2: Vector, - pub cfm_factor: SimdReal, - pub limit: SimdReal, - pub mj_lambda2: [usize; SIMD_WIDTH], - pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], - pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS], +pub(crate) struct SimdOneBodyConstraintBuilder { + // PERF: only store what’s needed, and store it in simd form. + rb1: [SolverBody; SIMD_WIDTH], + vels1: [RigidBodyVelocity; SIMD_WIDTH], + infos: [ContactPointInfos; MAX_MANIFOLD_POINTS], } -impl WVelocityGroundConstraint { +impl SimdOneBodyConstraintBuilder { pub fn generate( - params: &IntegrationParameters, manifold_id: [ContactManifoldIndex; SIMD_WIDTH], manifolds: [&ContactManifold; SIMD_WIDTH], bodies: &RigidBodySet, - out_constraints: &mut Vec, - insert_at: Option, + out_builders: &mut [SimdOneBodyConstraintBuilder], + out_constraints: &mut [OneBodyConstraintSimd], ) { - let cfm_factor = SimdReal::splat(params.cfm_factor()); - let dt = SimdReal::splat(params.dt); - let inv_dt = SimdReal::splat(params.inv_dt()); - let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); - let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); - let max_penetration_correction = SimdReal::splat(params.max_penetration_correction); - let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1]; let mut handles2 = gather![|ii| manifolds[ii].data.rigid_body2]; let mut flipped = [1.0; SIMD_WIDTH]; @@ -58,23 +44,26 @@ impl WVelocityGroundConstraint { } } + let rb1: [SolverBody; SIMD_WIDTH] = gather![|ii| { + handles1[ii] + .map(|h| SolverBody::from(&bodies[h])) + .unwrap_or_else(SolverBody::default) + }]; + let vels1: [RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| { handles1[ii] .map(|h| bodies[h].vels) - .unwrap_or_else(RigidBodyVelocity::zero) + .unwrap_or_else(RigidBodyVelocity::default) }]; - let world_com1 = Point::from(gather![|ii| { - handles1[ii] - .map(|h| bodies[h].mprops.world_com) - .unwrap_or_else(Point::origin) - }]); + + let world_com1 = Point::from(gather![|ii| { rb1[ii].world_com }]); + let poss1 = Isometry::from(gather![|ii| rb1[ii].position]); let bodies2 = gather![|ii| &bodies[handles2[ii].unwrap()]]; let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies2[ii].vels]; let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies2[ii].ids]; let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies2[ii].mprops]; - let ccd_thickness = SimdReal::from(gather![|ii| bodies2[ii].ccd.ccd_thickness]); let flipped_sign = SimdReal::from(flipped); @@ -88,12 +77,13 @@ impl WVelocityGroundConstraint { let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); + let poss2 = Isometry::from(gather![|ii| bodies2[ii].pos.position]); let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); let normal1 = Vector::from(gather![|ii| manifolds[ii].data.normal]); let force_dir1 = normal1 * -flipped_sign; - let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; + let solver_vel2 = gather![|ii| ids2[ii].active_set_offset]; let num_active_contacts = manifolds[0].data.num_active_contacts(); @@ -106,20 +96,21 @@ impl WVelocityGroundConstraint { let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]]; let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); - let mut is_fast_contact = SimdBool::splat(false); - let mut constraint = WVelocityGroundConstraint { - dir1: force_dir1, - #[cfg(feature = "dim3")] - tangent1: tangents1[0], - elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], - im2, - cfm_factor, - limit: SimdReal::splat(0.0), - mj_lambda2, - manifold_id, - manifold_contact_id: [[0; SIMD_WIDTH]; MAX_MANIFOLD_POINTS], - num_contacts: num_points as u8, - }; + let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS]; + let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS]; + + builder.rb1 = rb1; + builder.vels1 = vels1; + + constraint.dir1 = force_dir1; + constraint.im2 = im2; + constraint.solver_vel2 = solver_vel2; + constraint.manifold_id = manifold_id; + constraint.num_contacts = num_points as u8; + #[cfg(feature = "dim3")] + { + constraint.tangent1 = tangents1[0]; + } for k in 0..num_points { let friction = SimdReal::from(gather![|ii| manifold_points[ii][k].friction]); @@ -127,9 +118,10 @@ impl WVelocityGroundConstraint { let is_bouncy = SimdReal::from(gather![ |ii| manifold_points[ii][k].is_bouncy() as u32 as Real ]); - let is_resting = SimdReal::splat(1.0) - is_bouncy; - let point = Point::from(gather![|ii| manifold_points[ii][k].point]); + let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]); + let point = Point::from(gather![|ii| manifold_points[ii][k].point]); + let tangent_velocity = Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]); @@ -143,6 +135,7 @@ impl WVelocityGroundConstraint { constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id]; // Normal part. + let normal_rhs_wo_bias; { let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); @@ -150,24 +143,18 @@ impl WVelocityGroundConstraint { force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2), ); - let projected_velocity = (vel1 - vel2).dot(&force_dir1); - let mut rhs_wo_bias = - (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity; - rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt; - rhs_wo_bias *= is_bouncy + is_resting; - let rhs_bias = (dist + allowed_lin_err) - .simd_clamp(-max_penetration_correction, SimdReal::zero()) - * (erp_inv_dt/* * is_resting */); - - let rhs = rhs_wo_bias + rhs_bias; - is_fast_contact = - is_fast_contact | (-rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5)); + let projected_vel1 = vel1.dot(&force_dir1); + let projected_vel2 = vel2.dot(&force_dir1); + let projected_velocity = projected_vel1 - projected_vel2; + normal_rhs_wo_bias = + (is_bouncy * restitution) * projected_velocity + projected_vel1; // Add projected_vel1 since it’s not accessible through solver_vel. - constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart { + constraint.elements[k].normal_part = OneBodyConstraintNormalPart { gcross2, - rhs, - rhs_wo_bias, + rhs: na::zero(), + rhs_wo_bias: na::zero(), impulse: na::zero(), + total_impulse: na::zero(), r: projected_mass, }; } @@ -179,10 +166,11 @@ impl WVelocityGroundConstraint { let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); let r = tangents1[j].dot(&im2.component_mul(&tangents1[j])) + gcross2.gdot(gcross2); - let rhs = (vel1 - vel2 + tangent_velocity * flipped_sign).dot(&tangents1[j]); + let rhs_wo_bias = (vel1 + tangent_velocity * flipped_sign).dot(&tangents1[j]); constraint.elements[k].tangent_part.gcross2[j] = gcross2; - constraint.elements[k].tangent_part.rhs[j] = rhs; + constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias; + constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias; constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") { utils::simd_inv(r) } else { @@ -196,33 +184,139 @@ impl WVelocityGroundConstraint { * constraint.elements[k].tangent_part.gcross2[0] .gdot(constraint.elements[k].tangent_part.gcross2[1]); } + + // Builder. + { + let local_p1 = poss1.inverse_transform_point(&point); + let local_p2 = poss2.inverse_transform_point(&point); + let infos = ContactPointInfos { + local_p1, + local_p2, + tangent_vel: tangent_velocity * flipped_sign, + dist, + normal_rhs_wo_bias, + }; + + builder.infos[k] = infos; + } + } + } + } + + // TODO: this code is SOOOO similar to TwoBodyConstraintSimd::update. + // In fact the only differences are types and the `rb1` and ignoring its ccd thickness. + pub fn update( + &self, + params: &IntegrationParameters, + solved_dt: Real, + bodies: &[SolverBody], + _multibodies: &MultibodyJointSet, + constraint: &mut OneBodyConstraintSimd, + ) { + let cfm_factor = SimdReal::splat(params.cfm_factor()); + let dt = SimdReal::splat(params.dt); + let inv_dt = SimdReal::splat(params.inv_dt()); + let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); + let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); + let max_penetration_correction = SimdReal::splat(params.max_penetration_correction); + + let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]]; + let ccd_thickness = SimdReal::from(gather![|ii| rb2[ii].ccd_thickness]); + let poss2 = Isometry::from(gather![|ii| rb2[ii].position]); + + let all_infos = &self.infos[..constraint.num_contacts as usize]; + let all_elements = &mut constraint.elements[..constraint.num_contacts as usize]; + + // Integrate the velocity of the static rigid-body, if it’s kinematic. + let new_pos1 = Isometry::from(gather![|ii| self.vels1[ii].integrate( + solved_dt, + &self.rb1[ii].position, + &self.rb1[ii].local_com + )]); + + #[cfg(feature = "dim2")] + let tangents1 = constraint.dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let tangents1 = [ + constraint.tangent1, + constraint.dir1.cross(&constraint.tangent1), + ]; + + let mut is_fast_contact = SimdBool::splat(false); + let solved_dt = SimdReal::splat(solved_dt); + + for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) { + // NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface. + let p1 = new_pos1 * info.local_p1 + info.tangent_vel * solved_dt; + let p2 = poss2 * info.local_p2; + let dist = info.dist + (p1 - p2).dot(&constraint.dir1); + + // Normal part. + { + let rhs_wo_bias = + info.normal_rhs_wo_bias + dist.simd_max(SimdReal::zero()) * inv_dt; + let rhs_bias = (dist + allowed_lin_err) + .simd_clamp(-max_penetration_correction, SimdReal::zero()) + * erp_inv_dt; + let new_rhs = rhs_wo_bias + rhs_bias; + let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse; + is_fast_contact = + is_fast_contact | (-new_rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5)); + + element.normal_part.rhs_wo_bias = rhs_wo_bias; + element.normal_part.rhs = new_rhs; + element.normal_part.total_impulse = total_impulse; + element.normal_part.impulse = na::zero(); } - constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor); + // tangent parts. + { + element.tangent_part.total_impulse += element.tangent_part.impulse; + element.tangent_part.impulse = na::zero(); - if let Some(at) = insert_at { - out_constraints[at + l / MAX_MANIFOLD_POINTS] = - AnyVelocityConstraint::GroupedGround(constraint); - } else { - out_constraints.push(AnyVelocityConstraint::GroupedGround(constraint)); + for j in 0..DIM - 1 { + let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt; + element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias; + } } } + + constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor); } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct OneBodyConstraintSimd { + pub dir1: Vector, // Non-penetration force direction for the first body. + #[cfg(feature = "dim3")] + pub tangent1: Vector, // One of the friction force directions. + pub elements: [OneBodyConstraintElement; MAX_MANIFOLD_POINTS], + pub num_contacts: u8, + pub im2: Vector, + pub cfm_factor: SimdReal, + pub limit: SimdReal, + pub solver_vel2: [usize; SIMD_WIDTH], + pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], + pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS], +} +impl OneBodyConstraintSimd { pub fn solve( &mut self, - mj_lambdas: &mut [DeltaVel], + solver_vels: &mut [SolverVel], solve_normal: bool, solve_friction: bool, ) { - let mut mj_lambda2 = DeltaVel { - linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + let mut solver_vel2 = SolverVel { + linear: Vector::from(gather![ + |ii| solver_vels[self.solver_vel2[ii] as usize].linear + ]), angular: AngVector::from(gather![ - |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + |ii| solver_vels[self.solver_vel2[ii] as usize].angular ]), }; - VelocityGroundConstraintElement::solve_group( + OneBodyConstraintElement::solve_group( self.cfm_factor, &mut self.elements[..self.num_contacts as usize], &self.dir1, @@ -230,18 +324,18 @@ impl WVelocityGroundConstraint { &self.tangent1, &self.im2, self.limit, - &mut mj_lambda2, + &mut solver_vel2, solve_normal, solve_friction, ); for ii in 0..SIMD_WIDTH { - mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); - mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + solver_vels[self.solver_vel2[ii] as usize].linear = solver_vel2.linear.extract(ii); + solver_vels[self.solver_vel2[ii] as usize].angular = solver_vel2.angular.extract(ii); } } - // FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint. + // FIXME: duplicated code. This is exactly the same as in the two-body velocity constraint. pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { for k in 0..self.num_contacts as usize { let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into(); @@ -272,6 +366,7 @@ impl WVelocityGroundConstraint { self.cfm_factor = SimdReal::splat(1.0); for elt in &mut self.elements { elt.normal_part.rhs = elt.normal_part.rhs_wo_bias; + elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias; } } } diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint.rs b/src/dynamics/solver/contact_constraint/two_body_constraint.rs new file mode 100644 index 000000000..9b552cad7 --- /dev/null +++ b/src/dynamics/solver/contact_constraint/two_body_constraint.rs @@ -0,0 +1,470 @@ +use super::{ContactConstraintTypes, ContactPointInfos}; +use crate::dynamics::solver::SolverVel; +use crate::dynamics::solver::{AnyConstraintMut, SolverBody}; + +use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::math::{Isometry, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; +use crate::utils::{self, SimdAngularInertia, SimdBasis, SimdCross, SimdDot}; +use na::DVector; + +use super::{TwoBodyConstraintElement, TwoBodyConstraintNormalPart}; + +impl<'a> AnyConstraintMut<'a, ContactConstraintTypes> { + pub fn remove_bias(&mut self) { + match self { + Self::OneBody(c) => c.remove_cfm_and_bias_from_rhs(), + Self::TwoBodies(c) => c.remove_cfm_and_bias_from_rhs(), + Self::GenericOneBody(c) => c.remove_cfm_and_bias_from_rhs(), + Self::GenericTwoBodies(c) => c.remove_cfm_and_bias_from_rhs(), + #[cfg(feature = "simd-is-enabled")] + Self::SimdOneBody(c) => c.remove_cfm_and_bias_from_rhs(), + #[cfg(feature = "simd-is-enabled")] + Self::SimdTwoBodies(c) => c.remove_cfm_and_bias_from_rhs(), + } + } + + pub fn solve_restitution( + &mut self, + generic_jacobians: &DVector, + solver_vels: &mut [SolverVel], + generic_solver_vels: &mut DVector, + ) { + match self { + Self::OneBody(c) => c.solve(solver_vels, true, false), + Self::TwoBodies(c) => c.solve(solver_vels, true, false), + Self::GenericOneBody(c) => c.solve(generic_jacobians, generic_solver_vels, true, false), + Self::GenericTwoBodies(c) => c.solve( + generic_jacobians, + solver_vels, + generic_solver_vels, + true, + false, + ), + #[cfg(feature = "simd-is-enabled")] + Self::SimdOneBody(c) => c.solve(solver_vels, true, false), + #[cfg(feature = "simd-is-enabled")] + Self::SimdTwoBodies(c) => c.solve(solver_vels, true, false), + } + } + + pub fn solve_friction( + &mut self, + generic_jacobians: &DVector, + solver_vels: &mut [SolverVel], + generic_solver_vels: &mut DVector, + ) { + match self { + Self::OneBody(c) => c.solve(solver_vels, false, true), + Self::TwoBodies(c) => c.solve(solver_vels, false, true), + Self::GenericOneBody(c) => c.solve(generic_jacobians, generic_solver_vels, false, true), + Self::GenericTwoBodies(c) => c.solve( + generic_jacobians, + solver_vels, + generic_solver_vels, + false, + true, + ), + #[cfg(feature = "simd-is-enabled")] + Self::SimdOneBody(c) => c.solve(solver_vels, false, true), + #[cfg(feature = "simd-is-enabled")] + Self::SimdTwoBodies(c) => c.solve(solver_vels, false, true), + } + } + + pub fn writeback_impulses(&mut self, manifolds_all: &mut [&mut ContactManifold]) { + match self { + Self::OneBody(c) => c.writeback_impulses(manifolds_all), + Self::TwoBodies(c) => c.writeback_impulses(manifolds_all), + Self::GenericOneBody(c) => c.writeback_impulses(manifolds_all), + Self::GenericTwoBodies(c) => c.writeback_impulses(manifolds_all), + #[cfg(feature = "simd-is-enabled")] + Self::SimdOneBody(c) => c.writeback_impulses(manifolds_all), + #[cfg(feature = "simd-is-enabled")] + Self::SimdTwoBodies(c) => c.writeback_impulses(manifolds_all), + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct TwoBodyConstraint { + pub dir1: Vector, // Non-penetration force direction for the first body. + #[cfg(feature = "dim3")] + pub tangent1: Vector, // One of the friction force directions. + pub im1: Vector, + pub im2: Vector, + pub cfm_factor: Real, + pub limit: Real, + pub solver_vel1: usize, + pub solver_vel2: usize, + pub manifold_id: ContactManifoldIndex, + pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS], + pub num_contacts: u8, + pub elements: [TwoBodyConstraintElement; MAX_MANIFOLD_POINTS], +} + +impl TwoBodyConstraint { + pub fn invalid() -> Self { + Self { + dir1: Vector::zeros(), + #[cfg(feature = "dim3")] + tangent1: Vector::zeros(), + im1: Vector::zeros(), + im2: Vector::zeros(), + cfm_factor: 0.0, + limit: 0.0, + solver_vel1: usize::MAX, + solver_vel2: usize::MAX, + manifold_id: ContactManifoldIndex::MAX, + manifold_contact_id: [u8::MAX; MAX_MANIFOLD_POINTS], + num_contacts: u8::MAX, + elements: [TwoBodyConstraintElement::zero(); MAX_MANIFOLD_POINTS], + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct TwoBodyConstraintBuilder { + pub infos: [ContactPointInfos; MAX_MANIFOLD_POINTS], +} + +impl TwoBodyConstraintBuilder { + pub fn invalid() -> Self { + Self { + infos: [ContactPointInfos::default(); MAX_MANIFOLD_POINTS], + } + } + + pub fn generate( + manifold_id: ContactManifoldIndex, + manifold: &ContactManifold, + bodies: &RigidBodySet, + out_builders: &mut [TwoBodyConstraintBuilder], + out_constraints: &mut [TwoBodyConstraint], + ) { + assert_eq!(manifold.data.relative_dominance, 0); + + let handle1 = manifold.data.rigid_body1.unwrap(); + let handle2 = manifold.data.rigid_body2.unwrap(); + + let rb1 = &bodies[handle1]; + let (vels1, mprops1) = (&rb1.vels, &rb1.mprops); + let rb2 = &bodies[handle2]; + let (vels2, mprops2) = (&rb2.vels, &rb2.mprops); + + let solver_vel1 = rb1.ids.active_set_offset; + let solver_vel2 = rb2.ids.active_set_offset; + let force_dir1 = -manifold.data.normal; + + #[cfg(feature = "dim2")] + let tangents1 = force_dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let tangents1 = + super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel); + + for (l, manifold_points) in manifold + .data + .solver_contacts + .chunks(MAX_MANIFOLD_POINTS) + .enumerate() + { + let builder = &mut out_builders[l]; + let constraint = &mut out_constraints[l]; + constraint.dir1 = force_dir1; + constraint.im1 = mprops1.effective_inv_mass; + constraint.im2 = mprops2.effective_inv_mass; + constraint.solver_vel1 = solver_vel1; + constraint.solver_vel2 = solver_vel2; + constraint.manifold_id = manifold_id; + constraint.num_contacts = manifold_points.len() as u8; + #[cfg(feature = "dim3")] + { + constraint.tangent1 = tangents1[0]; + } + + for k in 0..manifold_points.len() { + let manifold_point = &manifold_points[k]; + let point = manifold_point.point; + + let dp1 = point - mprops1.world_com; + let dp2 = point - mprops2.world_com; + + let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); + let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); + + constraint.limit = manifold_point.friction; + constraint.manifold_contact_id[k] = manifold_point.contact_id; + + // Normal part. + let normal_rhs_wo_bias; + { + let gcross1 = mprops1 + .effective_world_inv_inertia_sqrt + .transform_vector(dp1.gcross(force_dir1)); + let gcross2 = mprops2 + .effective_world_inv_inertia_sqrt + .transform_vector(dp2.gcross(-force_dir1)); + + let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass; + let projected_mass = utils::inv( + force_dir1.dot(&imsum.component_mul(&force_dir1)) + + gcross1.gdot(gcross1) + + gcross2.gdot(gcross2), + ); + + let is_bouncy = manifold_point.is_bouncy() as u32 as Real; + + normal_rhs_wo_bias = + (is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1); + + constraint.elements[k].normal_part = TwoBodyConstraintNormalPart { + gcross1, + gcross2, + rhs: na::zero(), + rhs_wo_bias: na::zero(), + impulse: na::zero(), + total_impulse: na::zero(), + r: projected_mass, + }; + } + + // Tangent parts. + { + constraint.elements[k].tangent_part.impulse = na::zero(); + + for j in 0..DIM - 1 { + let gcross1 = mprops1 + .effective_world_inv_inertia_sqrt + .transform_vector(dp1.gcross(tangents1[j])); + let gcross2 = mprops2 + .effective_world_inv_inertia_sqrt + .transform_vector(dp2.gcross(-tangents1[j])); + let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass; + let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j])) + + gcross1.gdot(gcross1) + + gcross2.gdot(gcross2); + let rhs_wo_bias = manifold_point.tangent_velocity.dot(&tangents1[j]); + + constraint.elements[k].tangent_part.gcross1[j] = gcross1; + constraint.elements[k].tangent_part.gcross2[j] = gcross2; + constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias; + constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias; + constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") { + utils::inv(r) + } else { + r + }; + } + + #[cfg(feature = "dim3")] + { + constraint.elements[k].tangent_part.r[2] = 2.0 + * (constraint.elements[k].tangent_part.gcross1[0] + .gdot(constraint.elements[k].tangent_part.gcross1[1]) + + constraint.elements[k].tangent_part.gcross2[0] + .gdot(constraint.elements[k].tangent_part.gcross2[1])); + } + } + + // Builder. + let infos = ContactPointInfos { + local_p1: rb1 + .pos + .position + .inverse_transform_point(&manifold_point.point), + local_p2: rb2 + .pos + .position + .inverse_transform_point(&manifold_point.point), + tangent_vel: manifold_point.tangent_velocity, + dist: manifold_point.dist, + normal_rhs_wo_bias, + }; + + builder.infos[k] = infos; + constraint.manifold_contact_id[k] = manifold_point.contact_id; + } + } + } + + pub fn update( + &self, + params: &IntegrationParameters, + solved_dt: Real, + bodies: &[SolverBody], + _multibodies: &MultibodyJointSet, + constraint: &mut TwoBodyConstraint, + ) { + let rb1 = &bodies[constraint.solver_vel1]; + let rb2 = &bodies[constraint.solver_vel2]; + let ccd_thickness = rb1.ccd_thickness + rb2.ccd_thickness; + self.update_with_positions( + params, + solved_dt, + &rb1.position, + &rb2.position, + ccd_thickness, + constraint, + ) + } + + // Used by both generic and non-generic builders.. + pub fn update_with_positions( + &self, + params: &IntegrationParameters, + solved_dt: Real, + rb1_pos: &Isometry, + rb2_pos: &Isometry, + ccd_thickness: Real, + constraint: &mut TwoBodyConstraint, + ) { + let cfm_factor = params.cfm_factor(); + let inv_dt = params.inv_dt(); + let erp_inv_dt = params.erp_inv_dt(); + + let all_infos = &self.infos[..constraint.num_contacts as usize]; + let all_elements = &mut constraint.elements[..constraint.num_contacts as usize]; + + let mut is_fast_contact = false; + + #[cfg(feature = "dim2")] + let tangents1 = constraint.dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let tangents1 = [ + constraint.tangent1, + constraint.dir1.cross(&constraint.tangent1), + ]; + + for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) { + // Tangent velocity is equivalent to the first body’s surface moving artificially. + let p1 = rb1_pos * info.local_p1 + info.tangent_vel * solved_dt; + let p2 = rb2_pos * info.local_p2; + let dist = info.dist + (p1 - p2).dot(&constraint.dir1); + + // Normal part. + { + let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt; + let rhs_bias = erp_inv_dt + * (dist + params.allowed_linear_error) + .clamp(-params.max_penetration_correction, 0.0); + let new_rhs = rhs_wo_bias + rhs_bias; + let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse; + is_fast_contact = is_fast_contact || (-new_rhs * params.dt > ccd_thickness * 0.5); + + element.normal_part.rhs_wo_bias = rhs_wo_bias; + element.normal_part.rhs = new_rhs; + element.normal_part.total_impulse = total_impulse; + element.normal_part.impulse = na::zero(); + } + + // Tangent part. + { + element.tangent_part.total_impulse += element.tangent_part.impulse; + element.tangent_part.impulse = na::zero(); + + for j in 0..DIM - 1 { + let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt; + element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias; + } + } + } + + constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor }; + } +} + +impl TwoBodyConstraint { + pub fn solve( + &mut self, + solver_vels: &mut [SolverVel], + solve_normal: bool, + solve_friction: bool, + ) { + let mut solver_vel1 = solver_vels[self.solver_vel1 as usize]; + let mut solver_vel2 = solver_vels[self.solver_vel2 as usize]; + + TwoBodyConstraintElement::solve_group( + self.cfm_factor, + &mut self.elements[..self.num_contacts as usize], + &self.dir1, + #[cfg(feature = "dim3")] + &self.tangent1, + &self.im1, + &self.im2, + self.limit, + &mut solver_vel1, + &mut solver_vel2, + solve_normal, + solve_friction, + ); + + solver_vels[self.solver_vel1 as usize] = solver_vel1; + solver_vels[self.solver_vel2 as usize] = solver_vel2; + } + + pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { + let manifold = &mut manifolds_all[self.manifold_id]; + + for k in 0..self.num_contacts as usize { + let contact_id = self.manifold_contact_id[k]; + let active_contact = &mut manifold.points[contact_id as usize]; + active_contact.data.impulse = self.elements[k].normal_part.impulse; + + #[cfg(feature = "dim2")] + { + active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0]; + } + #[cfg(feature = "dim3")] + { + active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse; + } + } + } + + pub fn remove_cfm_and_bias_from_rhs(&mut self) { + self.cfm_factor = 1.0; + for elt in &mut self.elements { + elt.normal_part.rhs = elt.normal_part.rhs_wo_bias; + // elt.normal_part.impulse = elt.normal_part.total_impulse; + + elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias; + // elt.tangent_part.impulse = elt.tangent_part.total_impulse; + } + } +} + +#[inline(always)] +#[cfg(feature = "dim3")] +pub(crate) fn compute_tangent_contact_directions( + force_dir1: &Vector, + linvel1: &Vector, + linvel2: &Vector, +) -> [Vector; DIM - 1] +where + N: utils::SimdRealCopy, + Vector: SimdBasis, +{ + use na::SimdValue; + + // Compute the tangent direction. Pick the direction of + // the linear relative velocity, if it is not too small. + // Otherwise use a fallback direction. + let relative_linvel = linvel1 - linvel2; + let mut tangent_relative_linvel = + relative_linvel - force_dir1 * (force_dir1.dot(&relative_linvel)); + + let tangent_linvel_norm = { + let _disable_fe_except = + crate::utils::DisableFloatingPointExceptionsFlags::disable_floating_point_exceptions(); + tangent_relative_linvel.normalize_mut() + }; + + const THRESHOLD: Real = 1.0e-4; + let use_fallback = tangent_linvel_norm.simd_lt(N::splat(THRESHOLD)); + let tangent_fallback = force_dir1.orthonormal_vector(); + + let tangent1 = tangent_fallback.select(use_fallback, tangent_relative_linvel); + let bitangent1 = force_dir1.cross(&tangent1); + + [tangent1, bitangent1] +} diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs new file mode 100644 index 000000000..a8c003784 --- /dev/null +++ b/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs @@ -0,0 +1,271 @@ +use crate::dynamics::solver::SolverVel; +use crate::math::{AngVector, Vector, DIM}; +use crate::utils::{SimdBasis, SimdDot, SimdRealCopy}; + +#[derive(Copy, Clone, Debug)] +pub(crate) struct TwoBodyConstraintTangentPart { + pub gcross1: [AngVector; DIM - 1], + pub gcross2: [AngVector; DIM - 1], + pub rhs: [N; DIM - 1], + pub rhs_wo_bias: [N; DIM - 1], + #[cfg(feature = "dim2")] + pub impulse: na::Vector1, + #[cfg(feature = "dim3")] + pub impulse: na::Vector2, + #[cfg(feature = "dim2")] + pub total_impulse: na::Vector1, + #[cfg(feature = "dim3")] + pub total_impulse: na::Vector2, + #[cfg(feature = "dim2")] + pub r: [N; 1], + #[cfg(feature = "dim3")] + pub r: [N; DIM], +} + +impl TwoBodyConstraintTangentPart { + fn zero() -> Self { + Self { + gcross1: [na::zero(); DIM - 1], + gcross2: [na::zero(); DIM - 1], + rhs: [na::zero(); DIM - 1], + rhs_wo_bias: [na::zero(); DIM - 1], + impulse: na::zero(), + total_impulse: na::zero(), + #[cfg(feature = "dim2")] + r: [na::zero(); 1], + #[cfg(feature = "dim3")] + r: [na::zero(); DIM], + } + } + + #[inline] + pub fn apply_limit( + &mut self, + tangents1: [&Vector; DIM - 1], + im1: &Vector, + im2: &Vector, + limit: N, + solver_vel1: &mut SolverVel, + solver_vel2: &mut SolverVel, + ) where + AngVector: SimdDot, Result = N>, + { + #[cfg(feature = "dim2")] + { + let new_impulse = self.impulse[0].simd_clamp(-limit, limit); + let dlambda = new_impulse - self.impulse[0]; + self.impulse[0] = new_impulse; + + solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda; + solver_vel1.angular += self.gcross1[0] * dlambda; + + solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda; + solver_vel2.angular += self.gcross2[0] * dlambda; + } + + #[cfg(feature = "dim3")] + { + let new_impulse = self.impulse; + let new_impulse = { + let _disable_fe_except = + crate::utils::DisableFloatingPointExceptionsFlags:: + disable_floating_point_exceptions(); + new_impulse.simd_cap_magnitude(limit) + }; + + let dlambda = new_impulse - self.impulse; + self.impulse = new_impulse; + + solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda[0] + + tangents1[1].component_mul(im1) * dlambda[1]; + solver_vel1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1]; + + solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0] + + tangents1[1].component_mul(im2) * -dlambda[1]; + solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1]; + } + } + + #[inline] + pub fn solve( + &mut self, + tangents1: [&Vector; DIM - 1], + im1: &Vector, + im2: &Vector, + limit: N, + solver_vel1: &mut SolverVel, + solver_vel2: &mut SolverVel, + ) where + AngVector: SimdDot, Result = N>, + { + #[cfg(feature = "dim2")] + { + let dvel = tangents1[0].dot(&solver_vel1.linear) + + self.gcross1[0].gdot(solver_vel1.angular) + - tangents1[0].dot(&solver_vel2.linear) + + self.gcross2[0].gdot(solver_vel2.angular) + + self.rhs[0]; + let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit); + let dlambda = new_impulse - self.impulse[0]; + self.impulse[0] = new_impulse; + + solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda; + solver_vel1.angular += self.gcross1[0] * dlambda; + + solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda; + solver_vel2.angular += self.gcross2[0] * dlambda; + } + + #[cfg(feature = "dim3")] + { + let dvel_0 = tangents1[0].dot(&solver_vel1.linear) + + self.gcross1[0].gdot(solver_vel1.angular) + - tangents1[0].dot(&solver_vel2.linear) + + self.gcross2[0].gdot(solver_vel2.angular) + + self.rhs[0]; + let dvel_1 = tangents1[1].dot(&solver_vel1.linear) + + self.gcross1[1].gdot(solver_vel1.angular) + - tangents1[1].dot(&solver_vel2.linear) + + self.gcross2[1].gdot(solver_vel2.angular) + + self.rhs[1]; + + let dvel_00 = dvel_0 * dvel_0; + let dvel_11 = dvel_1 * dvel_1; + let dvel_01 = dvel_0 * dvel_1; + let inv_lhs = (dvel_00 + dvel_11) + * crate::utils::simd_inv( + dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2], + ); + let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1]; + let new_impulse = self.impulse - delta_impulse; + let new_impulse = { + let _disable_fe_except = + crate::utils::DisableFloatingPointExceptionsFlags:: + disable_floating_point_exceptions(); + new_impulse.simd_cap_magnitude(limit) + }; + + let dlambda = new_impulse - self.impulse; + self.impulse = new_impulse; + + solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda[0] + + tangents1[1].component_mul(im1) * dlambda[1]; + solver_vel1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1]; + + solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0] + + tangents1[1].component_mul(im2) * -dlambda[1]; + solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1]; + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct TwoBodyConstraintNormalPart { + pub gcross1: AngVector, + pub gcross2: AngVector, + pub rhs: N, + pub rhs_wo_bias: N, + pub impulse: N, + pub total_impulse: N, + pub r: N, +} + +impl TwoBodyConstraintNormalPart { + fn zero() -> Self { + Self { + gcross1: na::zero(), + gcross2: na::zero(), + rhs: na::zero(), + rhs_wo_bias: na::zero(), + impulse: na::zero(), + total_impulse: na::zero(), + r: na::zero(), + } + } + + #[inline] + pub fn solve( + &mut self, + cfm_factor: N, + dir1: &Vector, + im1: &Vector, + im2: &Vector, + solver_vel1: &mut SolverVel, + solver_vel2: &mut SolverVel, + ) where + AngVector: SimdDot, Result = N>, + { + let dvel = dir1.dot(&solver_vel1.linear) + self.gcross1.gdot(solver_vel1.angular) + - dir1.dot(&solver_vel2.linear) + + self.gcross2.gdot(solver_vel2.angular) + + self.rhs; + let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero()); + let dlambda = new_impulse - self.impulse; + self.impulse = new_impulse; + + solver_vel1.linear += dir1.component_mul(im1) * dlambda; + solver_vel1.angular += self.gcross1 * dlambda; + + solver_vel2.linear += dir1.component_mul(im2) * -dlambda; + solver_vel2.angular += self.gcross2 * dlambda; + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct TwoBodyConstraintElement { + pub normal_part: TwoBodyConstraintNormalPart, + pub tangent_part: TwoBodyConstraintTangentPart, +} + +impl TwoBodyConstraintElement { + pub fn zero() -> Self { + Self { + normal_part: TwoBodyConstraintNormalPart::zero(), + tangent_part: TwoBodyConstraintTangentPart::zero(), + } + } + + #[inline] + pub fn solve_group( + cfm_factor: N, + elements: &mut [Self], + dir1: &Vector, + #[cfg(feature = "dim3")] tangent1: &Vector, + im1: &Vector, + im2: &Vector, + limit: N, + solver_vel1: &mut SolverVel, + solver_vel2: &mut SolverVel, + solve_normal: bool, + solve_friction: bool, + ) where + Vector: SimdBasis, + AngVector: SimdDot, Result = N>, + { + #[cfg(feature = "dim3")] + let tangents1 = [tangent1, &dir1.cross(&tangent1)]; + #[cfg(feature = "dim2")] + let tangents1 = [&dir1.orthonormal_vector()]; + + // Solve penetration. + if solve_normal { + for element in elements.iter_mut() { + element + .normal_part + .solve(cfm_factor, &dir1, im1, im2, solver_vel1, solver_vel2); + let limit = limit * element.normal_part.impulse; + let part = &mut element.tangent_part; + part.apply_limit(tangents1, im1, im2, limit, solver_vel1, solver_vel2); + } + } + + // Solve friction. + if solve_friction { + for element in elements.iter_mut() { + let limit = limit * element.normal_part.impulse; + let part = &mut element.tangent_part; + part.solve(tangents1, im1, im2, limit, solver_vel1, solver_vel2); + } + } + } +} diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs similarity index 57% rename from src/dynamics/solver/velocity_constraint_wide.rs rename to src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs index b8e0a7ec8..b9f3e525d 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs @@ -1,57 +1,39 @@ -use super::{ - AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart, -}; +use super::{TwoBodyConstraintElement, TwoBodyConstraintNormalPart}; +use crate::dynamics::solver::solver_body::SolverBody; +use crate::dynamics::solver::{ContactPointInfos, SolverVel}; use crate::dynamics::{ - IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity, + IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodySet, + RigidBodyVelocity, }; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ - AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, + AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, + SIMD_WIDTH, }; #[cfg(feature = "dim2")] -use crate::utils::WBasis; -use crate::utils::{self, WAngularInertia, WCross, WDot}; +use crate::utils::SimdBasis; +use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot}; use num::Zero; use parry::math::SimdBool; use simba::simd::{SimdPartialOrd, SimdValue}; #[derive(Copy, Clone, Debug)] -pub(crate) struct WVelocityConstraint { - pub dir1: Vector, // Non-penetration force direction for the first body. - #[cfg(feature = "dim3")] - pub tangent1: Vector, // One of the friction force directions. - pub elements: [VelocityConstraintElement; MAX_MANIFOLD_POINTS], - pub num_contacts: u8, - pub im1: Vector, - pub im2: Vector, - pub cfm_factor: SimdReal, - pub limit: SimdReal, - pub mj_lambda1: [usize; SIMD_WIDTH], - pub mj_lambda2: [usize; SIMD_WIDTH], - pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], - pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS], +pub(crate) struct TwoBodyConstraintBuilderSimd { + infos: [super::ContactPointInfos; MAX_MANIFOLD_POINTS], } -impl WVelocityConstraint { +impl TwoBodyConstraintBuilderSimd { pub fn generate( - params: &IntegrationParameters, manifold_id: [ContactManifoldIndex; SIMD_WIDTH], manifolds: [&ContactManifold; SIMD_WIDTH], bodies: &RigidBodySet, - out_constraints: &mut Vec, - insert_at: Option, + out_builders: &mut [TwoBodyConstraintBuilderSimd], + out_constraints: &mut [TwoBodyConstraintSimd], ) { for ii in 0..SIMD_WIDTH { assert_eq!(manifolds[ii].data.relative_dominance, 0); } - let cfm_factor = SimdReal::splat(params.cfm_factor()); - let dt = SimdReal::splat(params.dt); - let inv_dt = SimdReal::splat(params.inv_dt()); - let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); - let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); - let max_penetration_correction = SimdReal::splat(params.max_penetration_correction); - let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()]; let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()]; @@ -62,9 +44,8 @@ impl WVelocityConstraint { let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].mprops]; let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].mprops]; - let ccd_thickness1 = SimdReal::from(gather![|ii| bodies[handles1[ii]].ccd.ccd_thickness]); - let ccd_thickness2 = SimdReal::from(gather![|ii| bodies[handles2[ii]].ccd.ccd_thickness]); - let ccd_thickness = ccd_thickness1 + ccd_thickness2; + let poss1 = Isometry::from(gather![|ii| bodies[handles1[ii]].pos.position]); + let poss2 = Isometry::from(gather![|ii| bodies[handles2[ii]].pos.position]); let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); let im1 = Vector::from(gather![|ii| mprops1[ii].effective_inv_mass]); @@ -84,8 +65,8 @@ impl WVelocityConstraint { let force_dir1 = -Vector::from(gather![|ii| manifolds[ii].data.normal]); - let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset]; - let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; + let solver_vel1 = gather![|ii| ids1[ii].active_set_offset]; + let solver_vel2 = gather![|ii| ids2[ii].active_set_offset]; let num_active_contacts = manifolds[0].data.num_active_contacts(); @@ -99,22 +80,20 @@ impl WVelocityConstraint { gather![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]]; let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); - let mut is_fast_contact = SimdBool::splat(false); - let mut constraint = WVelocityConstraint { - dir1: force_dir1, - #[cfg(feature = "dim3")] - tangent1: tangents1[0], - elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], - im1, - im2, - cfm_factor, - limit: SimdReal::splat(0.0), - mj_lambda1, - mj_lambda2, - manifold_id, - manifold_contact_id: [[0; SIMD_WIDTH]; MAX_MANIFOLD_POINTS], - num_contacts: num_points as u8, - }; + let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS]; + let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS]; + + constraint.dir1 = force_dir1; + constraint.im1 = im1; + constraint.im2 = im2; + constraint.solver_vel1 = solver_vel1; + constraint.solver_vel2 = solver_vel2; + constraint.manifold_id = manifold_id; + constraint.num_contacts = num_points as u8; + #[cfg(feature = "dim3")] + { + constraint.tangent1 = tangents1[0]; + } for k in 0..num_points { let friction = SimdReal::from(gather![|ii| manifold_points[ii][k].friction]); @@ -122,9 +101,10 @@ impl WVelocityConstraint { let is_bouncy = SimdReal::from(gather![ |ii| manifold_points[ii][k].is_bouncy() as u32 as Real ]); - let is_resting = SimdReal::splat(1.0) - is_bouncy; - let point = Point::from(gather![|ii| manifold_points[ii][k].point]); + let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]); + let point = Point::from(gather![|ii| manifold_points[ii][k].point]); + let tangent_velocity = Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]); let dp1 = point - world_com1; @@ -137,6 +117,7 @@ impl WVelocityConstraint { constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id]; // Normal part. + let normal_rhs_wo_bias; { let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1)); let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); @@ -149,24 +130,15 @@ impl WVelocityConstraint { ); let projected_velocity = (vel1 - vel2).dot(&force_dir1); - let mut rhs_wo_bias = - (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity; - rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt; - rhs_wo_bias *= is_bouncy + is_resting; - let rhs_bias = (dist + allowed_lin_err) - .simd_clamp(-max_penetration_correction, SimdReal::zero()) - * (erp_inv_dt/* * is_resting */); - - let rhs = rhs_wo_bias + rhs_bias; - is_fast_contact = - is_fast_contact | (-rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5)); - - constraint.elements[k].normal_part = VelocityConstraintNormalPart { + normal_rhs_wo_bias = is_bouncy * restitution * projected_velocity; + + constraint.elements[k].normal_part = TwoBodyConstraintNormalPart { gcross1, gcross2, - rhs: rhs_wo_bias + rhs_bias, - rhs_wo_bias, + rhs: na::zero(), + rhs_wo_bias: na::zero(), impulse: SimdReal::splat(0.0), + total_impulse: SimdReal::splat(0.0), r: projected_mass, }; } @@ -181,11 +153,12 @@ impl WVelocityConstraint { let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j])) + gcross1.gdot(gcross1) + gcross2.gdot(gcross2); - let rhs = (vel1 - vel2 + tangent_velocity).dot(&tangents1[j]); + let rhs_wo_bias = tangent_velocity.dot(&tangents1[j]); constraint.elements[k].tangent_part.gcross1[j] = gcross1; constraint.elements[k].tangent_part.gcross2[j] = gcross2; - constraint.elements[k].tangent_part.rhs[j] = rhs; + constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias; + constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias; constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") { utils::simd_inv(r) } else { @@ -201,40 +174,142 @@ impl WVelocityConstraint { + constraint.elements[k].tangent_part.gcross2[0] .gdot(constraint.elements[k].tangent_part.gcross2[1])); } + + // Builder. + let infos = ContactPointInfos { + local_p1: poss1.inverse_transform_point(&point), + local_p2: poss2.inverse_transform_point(&point), + tangent_vel: tangent_velocity, + dist, + normal_rhs_wo_bias, + }; + + builder.infos[k] = infos; + } + } + } + + pub fn update( + &self, + params: &IntegrationParameters, + solved_dt: Real, + bodies: &[SolverBody], + _multibodies: &MultibodyJointSet, + constraint: &mut TwoBodyConstraintSimd, + ) { + let cfm_factor = SimdReal::splat(params.cfm_factor()); + let dt = SimdReal::splat(params.dt); + let inv_dt = SimdReal::splat(params.inv_dt()); + let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); + let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); + let max_penetration_correction = SimdReal::splat(params.max_penetration_correction); + + let rb1 = gather![|ii| &bodies[constraint.solver_vel1[ii]]]; + let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]]; + + let ccd_thickness = SimdReal::from(gather![|ii| rb1[ii].ccd_thickness]) + + SimdReal::from(gather![|ii| rb2[ii].ccd_thickness]); + + let poss1 = Isometry::from(gather![|ii| rb1[ii].position]); + let poss2 = Isometry::from(gather![|ii| rb2[ii].position]); + + let all_infos = &self.infos[..constraint.num_contacts as usize]; + let all_elements = &mut constraint.elements[..constraint.num_contacts as usize]; + + #[cfg(feature = "dim2")] + let tangents1 = constraint.dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let tangents1 = [ + constraint.tangent1, + constraint.dir1.cross(&constraint.tangent1), + ]; + + let mut is_fast_contact = SimdBool::splat(false); + let solved_dt = SimdReal::splat(solved_dt); + + for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) { + // NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface. + let p1 = poss1 * info.local_p1 + info.tangent_vel * solved_dt; + let p2 = poss2 * info.local_p2; + let dist = info.dist + (p1 - p2).dot(&constraint.dir1); + + // Normal part. + { + let rhs_wo_bias = + info.normal_rhs_wo_bias + dist.simd_max(SimdReal::zero()) * inv_dt; + let rhs_bias = (dist + allowed_lin_err) + .simd_clamp(-max_penetration_correction, SimdReal::zero()) + * erp_inv_dt; + let new_rhs = rhs_wo_bias + rhs_bias; + let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse; + is_fast_contact = + is_fast_contact | (-new_rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5)); + + element.normal_part.rhs_wo_bias = rhs_wo_bias; + element.normal_part.rhs = new_rhs; + element.normal_part.total_impulse = total_impulse; + element.normal_part.impulse = na::zero(); } - constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor); + // tangent parts. + { + element.tangent_part.total_impulse += element.tangent_part.impulse; + element.tangent_part.impulse = na::zero(); - if let Some(at) = insert_at { - out_constraints[at + l / MAX_MANIFOLD_POINTS] = - AnyVelocityConstraint::Grouped(constraint); - } else { - out_constraints.push(AnyVelocityConstraint::Grouped(constraint)); + for j in 0..DIM - 1 { + let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt; + element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias; + } } } + + constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor); } +} +#[derive(Copy, Clone, Debug)] +pub(crate) struct TwoBodyConstraintSimd { + pub dir1: Vector, // Non-penetration force direction for the first body. + #[cfg(feature = "dim3")] + pub tangent1: Vector, // One of the friction force directions. + pub elements: [TwoBodyConstraintElement; MAX_MANIFOLD_POINTS], + pub num_contacts: u8, + pub im1: Vector, + pub im2: Vector, + pub cfm_factor: SimdReal, + pub limit: SimdReal, + pub solver_vel1: [usize; SIMD_WIDTH], + pub solver_vel2: [usize; SIMD_WIDTH], + pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], + pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS], +} + +impl TwoBodyConstraintSimd { pub fn solve( &mut self, - mj_lambdas: &mut [DeltaVel], + solver_vels: &mut [SolverVel], solve_normal: bool, solve_friction: bool, ) { - let mut mj_lambda1 = DeltaVel { - linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + let mut solver_vel1 = SolverVel { + linear: Vector::from(gather![ + |ii| solver_vels[self.solver_vel1[ii] as usize].linear + ]), angular: AngVector::from(gather![ - |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + |ii| solver_vels[self.solver_vel1[ii] as usize].angular ]), }; - let mut mj_lambda2 = DeltaVel { - linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + let mut solver_vel2 = SolverVel { + linear: Vector::from(gather![ + |ii| solver_vels[self.solver_vel2[ii] as usize].linear + ]), angular: AngVector::from(gather![ - |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + |ii| solver_vels[self.solver_vel2[ii] as usize].angular ]), }; - VelocityConstraintElement::solve_group( + TwoBodyConstraintElement::solve_group( self.cfm_factor, &mut self.elements[..self.num_contacts as usize], &self.dir1, @@ -243,19 +318,19 @@ impl WVelocityConstraint { &self.im1, &self.im2, self.limit, - &mut mj_lambda1, - &mut mj_lambda2, + &mut solver_vel1, + &mut solver_vel2, solve_normal, solve_friction, ); for ii in 0..SIMD_WIDTH { - mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); - mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); + solver_vels[self.solver_vel1[ii] as usize].linear = solver_vel1.linear.extract(ii); + solver_vels[self.solver_vel1[ii] as usize].angular = solver_vel1.angular.extract(ii); } for ii in 0..SIMD_WIDTH { - mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); - mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + solver_vels[self.solver_vel2[ii] as usize].linear = solver_vel2.linear.extract(ii); + solver_vels[self.solver_vel2[ii] as usize].angular = solver_vel2.angular.extract(ii); } } @@ -289,6 +364,7 @@ impl WVelocityConstraint { self.cfm_factor = SimdReal::splat(1.0); for elt in &mut self.elements { elt.normal_part.rhs = elt.normal_part.rhs_wo_bias; + elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias; } } } diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs deleted file mode 100644 index 91bb9fb7d..000000000 --- a/src/dynamics/solver/generic_velocity_ground_constraint.rs +++ /dev/null @@ -1,240 +0,0 @@ -use crate::dynamics::solver::VelocityGroundConstraint; -use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, RigidBodyVelocity}; -use crate::geometry::{ContactManifold, ContactManifoldIndex}; -use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS}; -use crate::utils::WCross; - -use super::{ - AnyVelocityConstraint, VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart, -}; -#[cfg(feature = "dim2")] -use crate::utils::WBasis; -use na::DVector; - -#[derive(Copy, Clone, Debug)] -pub(crate) struct GenericVelocityGroundConstraint { - // We just build the generic constraint on top of the velocity constraint, - // adding some information we can use in the generic case. - pub velocity_constraint: VelocityGroundConstraint, - pub j_id: usize, - pub ndofs2: usize, -} - -impl GenericVelocityGroundConstraint { - pub fn generate( - params: &IntegrationParameters, - manifold_id: ContactManifoldIndex, - manifold: &ContactManifold, - bodies: &RigidBodySet, - multibodies: &MultibodyJointSet, - out_constraints: &mut Vec, - jacobians: &mut DVector, - jacobian_id: &mut usize, - insert_at: Option, - ) { - let cfm_factor = params.cfm_factor(); - let inv_dt = params.inv_dt(); - let erp_inv_dt = params.erp_inv_dt(); - - let mut handle1 = manifold.data.rigid_body1; - let mut handle2 = manifold.data.rigid_body2; - let flipped = manifold.data.relative_dominance < 0; - - let (force_dir1, flipped_multiplier) = if flipped { - std::mem::swap(&mut handle1, &mut handle2); - (manifold.data.normal, -1.0) - } else { - (-manifold.data.normal, 1.0) - }; - - let (vels1, world_com1) = if let Some(handle1) = handle1 { - let rb1 = &bodies[handle1]; - (rb1.vels, rb1.mprops.world_com) - } else { - (RigidBodyVelocity::zero(), Point::origin()) - }; - - let rb2 = &bodies[handle2.unwrap()]; - let (vels2, mprops2) = (&rb2.vels, &rb2.mprops); - - let (mb2, link_id2) = handle2 - .and_then(|h| multibodies.rigid_body_link(h)) - .map(|m| (&multibodies[m.multibody], m.id)) - .unwrap(); - let mj_lambda2 = mb2.solver_id; - - #[cfg(feature = "dim2")] - let tangents1 = force_dir1.orthonormal_basis(); - #[cfg(feature = "dim3")] - let tangents1 = - super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel); - - let multibodies_ndof = mb2.ndofs(); - // For each solver contact we generate DIM constraints, and each constraints appends - // the multibodies jacobian and weighted jacobians - let required_jacobian_len = - *jacobian_id + manifold.data.solver_contacts.len() * multibodies_ndof * 2 * DIM; - - if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") { - jacobians.resize_vertically_mut(required_jacobian_len, 0.0); - } - - for (_l, manifold_points) in manifold - .data - .solver_contacts - .chunks(MAX_MANIFOLD_POINTS) - .enumerate() - { - let chunk_j_id = *jacobian_id; - let mut is_fast_contact = false; - let mut constraint = VelocityGroundConstraint { - dir1: force_dir1, - #[cfg(feature = "dim3")] - tangent1: tangents1[0], - elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], - im2: mprops2.effective_inv_mass, - cfm_factor, - limit: 0.0, - mj_lambda2, - manifold_id, - manifold_contact_id: [0; MAX_MANIFOLD_POINTS], - num_contacts: manifold_points.len() as u8, - }; - - for k in 0..manifold_points.len() { - let manifold_point = &manifold_points[k]; - let dp1 = manifold_point.point - world_com1; - let dp2 = manifold_point.point - mprops2.world_com; - - let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); - let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); - - constraint.limit = manifold_point.friction; - constraint.manifold_contact_id[k] = manifold_point.contact_id; - - // Normal part. - { - let torque_dir2 = dp2.gcross(-force_dir1); - let inv_r2 = mb2 - .fill_jacobians( - link_id2, - -force_dir1, - #[cfg(feature = "dim2")] - na::vector!(torque_dir2), - #[cfg(feature = "dim3")] - torque_dir2, - jacobian_id, - jacobians, - ) - .0; - - let r = crate::utils::inv(inv_r2); - - let is_bouncy = manifold_point.is_bouncy() as u32 as Real; - let is_resting = 1.0 - is_bouncy; - - let dvel = (vel1 - vel2).dot(&force_dir1); - let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) * dvel; - rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt; - rhs_wo_bias *= is_bouncy + is_resting; - let rhs_bias = - /* is_resting * */ erp_inv_dt * manifold_point.dist.clamp(-params.max_penetration_correction, 0.0); - - let rhs = rhs_wo_bias + rhs_bias; - is_fast_contact = - is_fast_contact || (-rhs * params.dt > rb2.ccd.ccd_thickness * 0.5); - - constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart { - gcross2: na::zero(), // Unused for generic constraints. - rhs, - rhs_wo_bias, - impulse: na::zero(), - r, - }; - } - - // Tangent parts. - { - constraint.elements[k].tangent_part.impulse = na::zero(); - - for j in 0..DIM - 1 { - let torque_dir2 = dp2.gcross(-tangents1[j]); - let inv_r2 = mb2 - .fill_jacobians( - link_id2, - -tangents1[j], - #[cfg(feature = "dim2")] - na::vector![torque_dir2], - #[cfg(feature = "dim3")] - torque_dir2, - jacobian_id, - jacobians, - ) - .0; - - let r = crate::utils::inv(inv_r2); - - let rhs = (vel1 - vel2 - + flipped_multiplier * manifold_point.tangent_velocity) - .dot(&tangents1[j]); - - constraint.elements[k].tangent_part.rhs[j] = rhs; - - // FIXME: in 3D, we should take into account gcross[0].dot(gcross[1]) - // in lhs. See the corresponding code on the `velocity_constraint.rs` - // file. - constraint.elements[k].tangent_part.r[j] = r; - } - } - } - - constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor }; - - let constraint = GenericVelocityGroundConstraint { - velocity_constraint: constraint, - j_id: chunk_j_id, - ndofs2: mb2.ndofs(), - }; - - if let Some(at) = insert_at { - out_constraints[at + _l] = - AnyVelocityConstraint::NongroupedGenericGround(constraint); - } else { - out_constraints.push(AnyVelocityConstraint::NongroupedGenericGround(constraint)); - } - } - } - - pub fn solve( - &mut self, - jacobians: &DVector, - generic_mj_lambdas: &mut DVector, - solve_restitution: bool, - solve_friction: bool, - ) { - let mj_lambda2 = self.velocity_constraint.mj_lambda2 as usize; - - let elements = &mut self.velocity_constraint.elements - [..self.velocity_constraint.num_contacts as usize]; - VelocityGroundConstraintElement::generic_solve_group( - self.velocity_constraint.cfm_factor, - elements, - jacobians, - self.velocity_constraint.limit, - self.ndofs2, - self.j_id, - mj_lambda2, - generic_mj_lambdas, - solve_restitution, - solve_friction, - ); - } - - pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { - self.velocity_constraint.writeback_impulses(manifolds_all); - } - - pub fn remove_cfm_and_bias_from_rhs(&mut self) { - self.velocity_constraint.remove_cfm_and_bias_from_rhs(); - } -} diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index d2e45ebaf..d654a597d 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -32,6 +32,7 @@ impl<'a> PairInteraction for JointGraphEdge { } #[cfg(feature = "parallel")] +#[allow(dead_code)] // That will likely be useful when we re-introduce intra-island parallelism. pub(crate) struct ParallelInteractionGroups { bodies_color: Vec, // Workspace. interaction_indices: Vec, // Workspace. @@ -41,6 +42,7 @@ pub(crate) struct ParallelInteractionGroups { } #[cfg(feature = "parallel")] +#[allow(dead_code)] // That will likely be useful when we re-introduce intra-island parallelism. impl ParallelInteractionGroups { pub fn new() -> Self { Self { @@ -171,7 +173,7 @@ pub(crate) struct InteractionGroups { #[cfg(feature = "simd-is-enabled")] body_masks: Vec, #[cfg(feature = "simd-is-enabled")] - pub grouped_interactions: Vec, + pub simd_interactions: Vec, pub nongrouped_interactions: Vec, } @@ -183,7 +185,7 @@ impl InteractionGroups { #[cfg(feature = "simd-is-enabled")] body_masks: Vec::new(), #[cfg(feature = "simd-is-enabled")] - grouped_interactions: Vec::new(), + simd_interactions: Vec::new(), nongrouped_interactions: Vec::new(), } } @@ -194,7 +196,7 @@ impl InteractionGroups { // { // self.buckets.clear(); // self.body_masks.clear(); - // self.grouped_interactions.clear(); + // self.simd_interactions.clear(); // } // self.nongrouped_interactions.clear(); // } @@ -300,7 +302,7 @@ impl InteractionGroups { if bucket.1 == SIMD_LAST_INDEX { // We completed our group. (bucket.0)[SIMD_LAST_INDEX] = *interaction_i; - self.grouped_interactions.extend_from_slice(&bucket.0); + self.simd_interactions.extend_from_slice(&bucket.0); bucket.1 = 0; occupied_mask &= !target_mask_bit; @@ -340,20 +342,20 @@ impl InteractionGroups { self.body_masks.iter_mut().for_each(|e| *e = 0); assert!( - self.grouped_interactions.len() % SIMD_WIDTH == 0, + self.simd_interactions.len() % SIMD_WIDTH == 0, "Invalid SIMD contact grouping." ); // println!( // "Num grouped interactions: {}, nongrouped: {}", - // self.grouped_interactions.len(), + // self.simd_interactions.len(), // self.nongrouped_interactions.len() // ); } pub fn clear_groups(&mut self) { #[cfg(feature = "simd-is-enabled")] - self.grouped_interactions.clear(); + self.simd_interactions.clear(); self.nongrouped_interactions.clear(); } @@ -466,7 +468,7 @@ impl InteractionGroups { if bucket.1 == SIMD_LAST_INDEX { // We completed our group. (bucket.0)[SIMD_LAST_INDEX] = *interaction_i; - self.grouped_interactions.extend_from_slice(&bucket.0); + self.simd_interactions.extend_from_slice(&bucket.0); bucket.1 = 0; occupied_mask = occupied_mask & (!target_mask_bit); } else { @@ -497,7 +499,7 @@ impl InteractionGroups { } assert!( - self.grouped_interactions.len() % SIMD_WIDTH == 0, + self.simd_interactions.len() % SIMD_WIDTH == 0, "Invalid SIMD contact grouping." ); } diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index edd17f85c..2953f9832 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -1,16 +1,15 @@ -use super::VelocitySolver; +use super::{JointConstraintsSet, VelocitySolver}; use crate::counters::Counters; -use crate::dynamics::solver::{ - AnyJointVelocityConstraint, AnyVelocityConstraint, SolverConstraints, -}; +use crate::dynamics::solver::contact_constraint::ContactConstraintsSet; use crate::dynamics::IslandManager; use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::prelude::MultibodyJointSet; +use parry::math::Real; pub struct IslandSolver { - contact_constraints: SolverConstraints, - joint_constraints: SolverConstraints, + contact_constraints: ContactConstraintsSet, + joint_constraints: JointConstraintsSet, velocity_solver: VelocitySolver, } @@ -23,8 +22,8 @@ impl Default for IslandSolver { impl IslandSolver { pub fn new() -> Self { Self { - contact_constraints: SolverConstraints::new(), - joint_constraints: SolverConstraints::new(), + contact_constraints: ContactConstraintsSet::new(), + joint_constraints: JointConstraintsSet::new(), velocity_solver: VelocitySolver::new(), } } @@ -33,57 +32,71 @@ impl IslandSolver { &mut self, island_id: usize, counters: &mut Counters, - params: &IntegrationParameters, + base_params: &IntegrationParameters, islands: &IslandManager, bodies: &mut RigidBodySet, manifolds: &mut [&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], impulse_joints: &mut [JointGraphEdge], joint_indices: &[JointIndex], - multibody_joints: &mut MultibodyJointSet, + multibodies: &mut MultibodyJointSet, ) { - // Init the solver id for multibody_joints. - // We need that for building the constraints. - let mut solver_id = 0; - for (_, multibody) in multibody_joints.multibodies.iter_mut() { - multibody.solver_id = solver_id; - solver_id += multibody.ndofs(); - } + counters.solver.velocity_resolution_time.resume(); + let num_solver_iterations = base_params.num_solver_iterations.get() + + islands.active_island_additional_solver_iterations(island_id); - counters.solver.velocity_assembly_time.resume(); - self.contact_constraints.init( + let mut params = *base_params; + params.dt /= num_solver_iterations as Real; + params.damping_ratio /= num_solver_iterations as Real; + // params.joint_damping_ratio /= num_solver_iterations as Real; + + /* + * + * Bellow this point, the `params` is using the "small step" settings. + * + */ + // INIT + self.velocity_solver + .init_solver_velocities_and_solver_bodies( + ¶ms, + island_id, + islands, + bodies, + multibodies, + ); + self.velocity_solver.init_constraints( island_id, - params, islands, bodies, - multibody_joints, + multibodies, manifolds, manifold_indices, - ); - self.joint_constraints.init( - island_id, - params, - islands, - bodies, - multibody_joints, impulse_joints, joint_indices, + &mut self.contact_constraints, + &mut self.joint_constraints, ); - counters.solver.velocity_assembly_time.pause(); - counters.solver.velocity_resolution_time.resume(); - self.velocity_solver.solve( - island_id, - params, + // SOLVE + self.velocity_solver.solve_constraints( + ¶ms, + num_solver_iterations, + bodies, + multibodies, + &mut self.contact_constraints, + &mut self.joint_constraints, + ); + + // WRITEBACK + self.joint_constraints.writeback_impulses(impulse_joints); + self.contact_constraints.writeback_impulses(manifolds); + self.velocity_solver.writeback_bodies( + base_params, + num_solver_iterations, islands, + island_id, bodies, - multibody_joints, - manifolds, - impulse_joints, - &mut self.contact_constraints.velocity_constraints, - &self.contact_constraints.generic_jacobians, - &mut self.joint_constraints.velocity_constraints, - &self.joint_constraints.generic_jacobians, + multibodies, ); counters.solver.velocity_resolution_time.pause(); } diff --git a/src/dynamics/solver/joint_constraint/any_joint_constraint.rs b/src/dynamics/solver/joint_constraint/any_joint_constraint.rs new file mode 100644 index 000000000..8cbd438be --- /dev/null +++ b/src/dynamics/solver/joint_constraint/any_joint_constraint.rs @@ -0,0 +1,97 @@ +use crate::dynamics::solver::joint_constraint::joint_generic_constraint::{ + JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint, +}; +use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{ + JointOneBodyConstraint, JointTwoBodyConstraint, +}; +use crate::dynamics::solver::{AnyConstraintMut, ConstraintTypes}; +use crate::dynamics::JointGraphEdge; +use crate::math::Real; +use na::DVector; + +use crate::dynamics::solver::joint_constraint::joint_generic_constraint_builder::{ + JointGenericOneBodyConstraintBuilder, JointGenericTwoBodyConstraintBuilder, +}; +use crate::dynamics::solver::solver_vel::SolverVel; +#[cfg(feature = "simd-is-enabled")] +use crate::{ + dynamics::solver::joint_constraint::joint_constraint_builder::{ + JointOneBodyConstraintBuilderSimd, JointTwoBodyConstraintBuilderSimd, + }, + math::{SimdReal, SIMD_WIDTH}, +}; + +use crate::dynamics::solver::joint_constraint::joint_constraint_builder::{ + JointOneBodyConstraintBuilder, JointTwoBodyConstraintBuilder, +}; + +pub struct JointConstraintTypes; + +impl<'a> AnyConstraintMut<'a, JointConstraintTypes> { + pub fn remove_bias(&mut self) { + match self { + Self::OneBody(c) => c.remove_bias_from_rhs(), + Self::TwoBodies(c) => c.remove_bias_from_rhs(), + Self::GenericOneBody(c) => c.remove_bias_from_rhs(), + Self::GenericTwoBodies(c) => c.remove_bias_from_rhs(), + #[cfg(feature = "simd-is-enabled")] + Self::SimdOneBody(c) => c.remove_bias_from_rhs(), + #[cfg(feature = "simd-is-enabled")] + Self::SimdTwoBodies(c) => c.remove_bias_from_rhs(), + } + } + + pub fn solve( + &mut self, + generic_jacobians: &DVector, + solver_vels: &mut [SolverVel], + generic_solver_vels: &mut DVector, + ) { + match self { + Self::OneBody(c) => c.solve(solver_vels), + Self::TwoBodies(c) => c.solve(solver_vels), + Self::GenericOneBody(c) => c.solve(generic_jacobians, solver_vels, generic_solver_vels), + Self::GenericTwoBodies(c) => { + c.solve(generic_jacobians, solver_vels, generic_solver_vels) + } + #[cfg(feature = "simd-is-enabled")] + Self::SimdOneBody(c) => c.solve(solver_vels), + #[cfg(feature = "simd-is-enabled")] + Self::SimdTwoBodies(c) => c.solve(solver_vels), + } + } + + pub fn writeback_impulses(&mut self, joints_all: &mut [JointGraphEdge]) { + match self { + Self::OneBody(c) => c.writeback_impulses(joints_all), + Self::TwoBodies(c) => c.writeback_impulses(joints_all), + Self::GenericOneBody(c) => c.writeback_impulses(joints_all), + Self::GenericTwoBodies(c) => c.writeback_impulses(joints_all), + #[cfg(feature = "simd-is-enabled")] + Self::SimdOneBody(c) => c.writeback_impulses(joints_all), + #[cfg(feature = "simd-is-enabled")] + Self::SimdTwoBodies(c) => c.writeback_impulses(joints_all), + } + } +} + +impl ConstraintTypes for JointConstraintTypes { + type OneBody = JointOneBodyConstraint; + type TwoBodies = JointTwoBodyConstraint; + type GenericOneBody = JointGenericOneBodyConstraint; + type GenericTwoBodies = JointGenericTwoBodyConstraint; + + #[cfg(feature = "simd-is-enabled")] + type SimdOneBody = JointOneBodyConstraint; + #[cfg(feature = "simd-is-enabled")] + type SimdTwoBodies = JointTwoBodyConstraint; + + type BuilderOneBody = JointOneBodyConstraintBuilder; + type BuilderTwoBodies = JointTwoBodyConstraintBuilder; + type GenericBuilderOneBody = JointGenericOneBodyConstraintBuilder; + type GenericBuilderTwoBodies = JointGenericTwoBodyConstraintBuilder; + #[cfg(feature = "simd-is-enabled")] + type SimdBuilderOneBody = JointOneBodyConstraintBuilderSimd; + #[cfg(feature = "simd-is-enabled")] + type SimdBuilderTwoBodies = JointTwoBodyConstraintBuilderSimd; +} diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs deleted file mode 100644 index 962602f19..000000000 --- a/src/dynamics/solver/joint_constraint/joint_constraint.rs +++ /dev/null @@ -1,540 +0,0 @@ -use crate::dynamics::solver::joint_constraint::joint_generic_velocity_constraint::{ - JointGenericVelocityConstraint, JointGenericVelocityGroundConstraint, -}; -use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{ - JointVelocityConstraint, JointVelocityGroundConstraint, SolverBody, -}; -use crate::dynamics::solver::DeltaVel; -use crate::dynamics::{ - ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, -}; -use crate::math::{Real, SPATIAL_DIM}; -use crate::prelude::MultibodyJointSet; -use na::DVector; - -#[cfg(feature = "simd-is-enabled")] -use crate::math::{Isometry, SimdReal, SIMD_WIDTH}; - -#[cfg(feature = "parallel")] -use crate::dynamics::JointAxesMask; - -pub enum AnyJointVelocityConstraint { - JointConstraint(JointVelocityConstraint), - JointGroundConstraint(JointVelocityGroundConstraint), - JointGenericConstraint(JointGenericVelocityConstraint), - JointGenericGroundConstraint(JointGenericVelocityGroundConstraint), - #[cfg(feature = "simd-is-enabled")] - JointConstraintSimd(JointVelocityConstraint), - #[cfg(feature = "simd-is-enabled")] - JointGroundConstraintSimd(JointVelocityGroundConstraint), - Empty, -} - -impl AnyJointVelocityConstraint { - #[cfg(feature = "parallel")] - pub fn num_active_constraints_and_jacobian_lines(joint: &ImpulseJoint) -> (usize, usize) { - let joint = &joint.data; - let locked_axes = joint.locked_axes.bits(); - let motor_axes = joint.motor_axes.bits() & !locked_axes; - let limit_axes = joint.limit_axes.bits() & !locked_axes; - let coupled_axes = joint.coupled_axes.bits(); - - let num_constraints = (motor_axes & !coupled_axes).count_ones() as usize - + ((motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize - + ((motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize - + locked_axes.count_ones() as usize - + (limit_axes & !coupled_axes).count_ones() as usize - + ((limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize - + ((limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize; - (num_constraints, num_constraints) - } - - pub fn from_joint( - params: &IntegrationParameters, - joint_id: JointIndex, - joint: &ImpulseJoint, - bodies: &RigidBodySet, - multibodies: &MultibodyJointSet, - j_id: &mut usize, - jacobians: &mut DVector, - out: &mut Vec, - insert_at: Option, - ) { - let local_frame1 = joint.data.local_frame1; - let local_frame2 = joint.data.local_frame2; - let rb1 = &bodies[joint.body1]; - let rb2 = &bodies[joint.body2]; - let frame1 = rb1.pos.position * local_frame1; - let frame2 = rb2.pos.position * local_frame2; - - let body1 = SolverBody { - linvel: rb1.vels.linvel, - angvel: rb1.vels.angvel, - im: rb1.mprops.effective_inv_mass, - sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt, - world_com: rb1.mprops.world_com, - mj_lambda: [rb1.ids.active_set_offset], - }; - let body2 = SolverBody { - linvel: rb2.vels.linvel, - angvel: rb2.vels.angvel, - im: rb2.mprops.effective_inv_mass, - sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt, - world_com: rb2.mprops.world_com, - mj_lambda: [rb2.ids.active_set_offset], - }; - - let mb1 = multibodies - .rigid_body_link(joint.body1) - .map(|link| (&multibodies[link.multibody], link.id)); - let mb2 = multibodies - .rigid_body_link(joint.body2) - .map(|link| (&multibodies[link.multibody], link.id)); - - if mb1.is_some() || mb2.is_some() { - let multibodies_ndof = mb1.map(|m| m.0.ndofs()).unwrap_or(SPATIAL_DIM) - + mb2.map(|m| m.0.ndofs()).unwrap_or(SPATIAL_DIM); - - if multibodies_ndof == 0 { - // Both multibodies are fixed, don’t generate any constraint. - return; - } - - // For each solver contact we generate up to SPATIAL_DIM constraints, and each - // constraints appends the multibodies jacobian and weighted jacobians. - // Also note that for impulse_joints, the rigid-bodies will also add their jacobians - // to the generic DVector. - // TODO: is this count correct when we take both motors and limits into account? - let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM; - - if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") { - jacobians.resize_vertically_mut(required_jacobian_len, 0.0); - } - - // TODO: find a way to avoid the temporary buffer. - let mut out_tmp = [JointGenericVelocityConstraint::invalid(); 12]; - let out_tmp_len = JointGenericVelocityConstraint::lock_axes( - params, - joint_id, - &body1, - &body2, - mb1, - mb2, - &frame1, - &frame2, - &joint.data, - jacobians, - j_id, - &mut out_tmp, - ); - - if let Some(at) = insert_at { - for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() { - out[at + i] = AnyJointVelocityConstraint::JointGenericConstraint(c); - } - } else { - for c in out_tmp.into_iter().take(out_tmp_len) { - out.push(AnyJointVelocityConstraint::JointGenericConstraint(c)); - } - } - } else { - // TODO: find a way to avoid the temporary buffer. - let mut out_tmp = [JointVelocityConstraint::invalid(); 12]; - let out_tmp_len = JointVelocityConstraint::::lock_axes( - params, - joint_id, - &body1, - &body2, - &frame1, - &frame2, - &joint.data, - &mut out_tmp, - ); - - if let Some(at) = insert_at { - for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() { - out[at + i] = AnyJointVelocityConstraint::JointConstraint(c); - } - } else { - for c in out_tmp.into_iter().take(out_tmp_len) { - out.push(AnyJointVelocityConstraint::JointConstraint(c)); - } - } - } - } - - #[cfg(feature = "simd-is-enabled")] - pub fn from_wide_joint( - params: &IntegrationParameters, - joint_id: [JointIndex; SIMD_WIDTH], - impulse_joints: [&ImpulseJoint; SIMD_WIDTH], - bodies: &RigidBodySet, - out: &mut Vec, - insert_at: Option, - ) { - use crate::dynamics::{ - RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, - }; - - let rbs1: ( - [&RigidBodyPosition; SIMD_WIDTH], - [&RigidBodyVelocity; SIMD_WIDTH], - [&RigidBodyMassProps; SIMD_WIDTH], - [&RigidBodyIds; SIMD_WIDTH], - ) = ( - gather![|ii| &bodies[impulse_joints[ii].body1].pos], - gather![|ii| &bodies[impulse_joints[ii].body1].vels], - gather![|ii| &bodies[impulse_joints[ii].body1].mprops], - gather![|ii| &bodies[impulse_joints[ii].body1].ids], - ); - let rbs2: ( - [&RigidBodyPosition; SIMD_WIDTH], - [&RigidBodyVelocity; SIMD_WIDTH], - [&RigidBodyMassProps; SIMD_WIDTH], - [&RigidBodyIds; SIMD_WIDTH], - ) = ( - gather![|ii| &bodies[impulse_joints[ii].body2].pos], - gather![|ii| &bodies[impulse_joints[ii].body2].vels], - gather![|ii| &bodies[impulse_joints[ii].body2].mprops], - gather![|ii| &bodies[impulse_joints[ii].body2].ids], - ); - - let (rb_pos1, rb_vel1, rb_mprops1, rb_ids1) = rbs1; - let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rbs2; - let pos1: Isometry = gather![|ii| rb_pos1[ii].position].into(); - let pos2: Isometry = gather![|ii| rb_pos2[ii].position].into(); - - let local_frame1: Isometry = - gather![|ii| impulse_joints[ii].data.local_frame1].into(); - let local_frame2: Isometry = - gather![|ii| impulse_joints[ii].data.local_frame2].into(); - - let frame1 = pos1 * local_frame1; - let frame2 = pos2 * local_frame2; - - let body1: SolverBody = SolverBody { - linvel: gather![|ii| rb_vel1[ii].linvel].into(), - angvel: gather![|ii| rb_vel1[ii].angvel].into(), - im: gather![|ii| rb_mprops1[ii].effective_inv_mass].into(), - sqrt_ii: gather![|ii| rb_mprops1[ii].effective_world_inv_inertia_sqrt].into(), - world_com: gather![|ii| rb_mprops1[ii].world_com].into(), - mj_lambda: gather![|ii| rb_ids1[ii].active_set_offset], - }; - let body2: SolverBody = SolverBody { - linvel: gather![|ii| rb_vel2[ii].linvel].into(), - angvel: gather![|ii| rb_vel2[ii].angvel].into(), - im: gather![|ii| rb_mprops2[ii].effective_inv_mass].into(), - sqrt_ii: gather![|ii| rb_mprops2[ii].effective_world_inv_inertia_sqrt].into(), - world_com: gather![|ii| rb_mprops2[ii].world_com].into(), - mj_lambda: gather![|ii| rb_ids2[ii].active_set_offset], - }; - - // TODO: find a way to avoid the temporary buffer. - let mut out_tmp = [JointVelocityConstraint::invalid(); 12]; - let out_tmp_len = JointVelocityConstraint::::lock_axes( - params, - joint_id, - &body1, - &body2, - &frame1, - &frame2, - impulse_joints[0].data.locked_axes.bits(), - &mut out_tmp, - ); - - if let Some(at) = insert_at { - for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() { - out[at + i] = AnyJointVelocityConstraint::JointConstraintSimd(c); - } - } else { - for c in out_tmp.into_iter().take(out_tmp_len) { - out.push(AnyJointVelocityConstraint::JointConstraintSimd(c)); - } - } - } - - pub fn from_joint_ground( - params: &IntegrationParameters, - joint_id: JointIndex, - joint: &ImpulseJoint, - bodies: &RigidBodySet, - multibodies: &MultibodyJointSet, - j_id: &mut usize, - jacobians: &mut DVector, - out: &mut Vec, - insert_at: Option, - ) { - let mut handle1 = joint.body1; - let mut handle2 = joint.body2; - let flipped = !bodies[handle2].is_dynamic(); - - let (local_frame1, local_frame2) = if flipped { - std::mem::swap(&mut handle1, &mut handle2); - (joint.data.local_frame2, joint.data.local_frame1) - } else { - (joint.data.local_frame1, joint.data.local_frame2) - }; - - let rb1 = &bodies[handle1]; - let rb2 = &bodies[handle2]; - - let frame1 = rb1.pos.position * local_frame1; - let frame2 = rb2.pos.position * local_frame2; - - let body1 = SolverBody { - linvel: rb1.vels.linvel, - angvel: rb1.vels.angvel, - im: rb1.mprops.effective_inv_mass, - sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt, - world_com: rb1.mprops.world_com, - mj_lambda: [crate::INVALID_USIZE], - }; - let body2 = SolverBody { - linvel: rb2.vels.linvel, - angvel: rb2.vels.angvel, - im: rb2.mprops.effective_inv_mass, - sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt, - world_com: rb2.mprops.world_com, - mj_lambda: [rb2.ids.active_set_offset], - }; - - if let Some(mb2) = multibodies - .rigid_body_link(handle2) - .map(|link| (&multibodies[link.multibody], link.id)) - { - let multibodies_ndof = mb2.0.ndofs(); - - if multibodies_ndof == 0 { - // The multibody is fixed, don’t generate any constraint. - return; - } - - // For each solver contact we generate up to SPATIAL_DIM constraints, and each - // constraints appends the multibodies jacobian and weighted jacobians. - // Also note that for impulse_joints, the rigid-bodies will also add their jacobians - // to the generic DVector. - // TODO: is this count correct when we take both motors and limits into account? - let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM; - - if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") { - jacobians.resize_vertically_mut(required_jacobian_len, 0.0); - } - - // TODO: find a way to avoid the temporary buffer. - let mut out_tmp = [JointGenericVelocityGroundConstraint::invalid(); 12]; - let out_tmp_len = JointGenericVelocityGroundConstraint::lock_axes( - params, - joint_id, - &body1, - &body2, - mb2, - &frame1, - &frame2, - &joint.data, - jacobians, - j_id, - &mut out_tmp, - ); - - if let Some(at) = insert_at { - for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() { - out[at + i] = AnyJointVelocityConstraint::JointGenericGroundConstraint(c); - } - } else { - for c in out_tmp.into_iter().take(out_tmp_len) { - out.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(c)); - } - } - } else { - // TODO: find a way to avoid the temporary buffer. - let mut out_tmp = [JointVelocityGroundConstraint::invalid(); 12]; - let out_tmp_len = JointVelocityGroundConstraint::::lock_axes( - params, - joint_id, - &body1, - &body2, - &frame1, - &frame2, - &joint.data, - &mut out_tmp, - ); - - if let Some(at) = insert_at { - for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() { - out[at + i] = AnyJointVelocityConstraint::JointGroundConstraint(c); - } - } else { - for c in out_tmp.into_iter().take(out_tmp_len) { - out.push(AnyJointVelocityConstraint::JointGroundConstraint(c)); - } - } - } - } - - #[cfg(feature = "simd-is-enabled")] - pub fn from_wide_joint_ground( - params: &IntegrationParameters, - joint_id: [JointIndex; SIMD_WIDTH], - impulse_joints: [&ImpulseJoint; SIMD_WIDTH], - bodies: &RigidBodySet, - out: &mut Vec, - insert_at: Option, - ) { - use crate::dynamics::{ - RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity, - }; - - let mut handles1 = gather![|ii| impulse_joints[ii].body1]; - let mut handles2 = gather![|ii| impulse_joints[ii].body2]; - let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].body_type]; - let mut flipped = [false; SIMD_WIDTH]; - - for ii in 0..SIMD_WIDTH { - if !status2[ii].is_dynamic() { - std::mem::swap(&mut handles1[ii], &mut handles2[ii]); - flipped[ii] = true; - } - } - - let local_frame1: Isometry = gather![|ii| if flipped[ii] { - impulse_joints[ii].data.local_frame2 - } else { - impulse_joints[ii].data.local_frame1 - }] - .into(); - let local_frame2: Isometry = gather![|ii| if flipped[ii] { - impulse_joints[ii].data.local_frame1 - } else { - impulse_joints[ii].data.local_frame2 - }] - .into(); - - let rbs1: ( - [&RigidBodyPosition; SIMD_WIDTH], - [&RigidBodyVelocity; SIMD_WIDTH], - [&RigidBodyMassProps; SIMD_WIDTH], - ) = ( - gather![|ii| &bodies[handles1[ii]].pos], - gather![|ii| &bodies[handles1[ii]].vels], - gather![|ii| &bodies[handles1[ii]].mprops], - ); - let rbs2: ( - [&RigidBodyPosition; SIMD_WIDTH], - [&RigidBodyVelocity; SIMD_WIDTH], - [&RigidBodyMassProps; SIMD_WIDTH], - [&RigidBodyIds; SIMD_WIDTH], - ) = ( - gather![|ii| &bodies[handles2[ii]].pos], - gather![|ii| &bodies[handles2[ii]].vels], - gather![|ii| &bodies[handles2[ii]].mprops], - gather![|ii| &bodies[handles2[ii]].ids], - ); - - let (rb_pos1, rb_vel1, rb_mprops1) = rbs1; - let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rbs2; - let pos1: Isometry = gather![|ii| rb_pos1[ii].position].into(); - let pos2: Isometry = gather![|ii| rb_pos2[ii].position].into(); - - let frame1 = pos1 * local_frame1; - let frame2 = pos2 * local_frame2; - - let body1: SolverBody = SolverBody { - linvel: gather![|ii| rb_vel1[ii].linvel].into(), - angvel: gather![|ii| rb_vel1[ii].angvel].into(), - im: gather![|ii| rb_mprops1[ii].effective_inv_mass].into(), - sqrt_ii: gather![|ii| rb_mprops1[ii].effective_world_inv_inertia_sqrt].into(), - world_com: gather![|ii| rb_mprops1[ii].world_com].into(), - mj_lambda: [crate::INVALID_USIZE; SIMD_WIDTH], - }; - let body2: SolverBody = SolverBody { - linvel: gather![|ii| rb_vel2[ii].linvel].into(), - angvel: gather![|ii| rb_vel2[ii].angvel].into(), - im: gather![|ii| rb_mprops2[ii].effective_inv_mass].into(), - sqrt_ii: gather![|ii| rb_mprops2[ii].effective_world_inv_inertia_sqrt].into(), - world_com: gather![|ii| rb_mprops2[ii].world_com].into(), - mj_lambda: gather![|ii| rb_ids2[ii].active_set_offset], - }; - - // TODO: find a way to avoid the temporary buffer. - let mut out_tmp = [JointVelocityGroundConstraint::invalid(); 12]; - let out_tmp_len = JointVelocityGroundConstraint::::lock_axes( - params, - joint_id, - &body1, - &body2, - &frame1, - &frame2, - impulse_joints[0].data.locked_axes.bits(), - &mut out_tmp, - ); - - if let Some(at) = insert_at { - for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() { - out[at + i] = AnyJointVelocityConstraint::JointGroundConstraintSimd(c); - } - } else { - for c in out_tmp.into_iter().take(out_tmp_len) { - out.push(AnyJointVelocityConstraint::JointGroundConstraintSimd(c)); - } - } - } - - pub fn remove_bias_from_rhs(&mut self) { - match self { - AnyJointVelocityConstraint::JointConstraint(c) => c.remove_bias_from_rhs(), - AnyJointVelocityConstraint::JointGroundConstraint(c) => c.remove_bias_from_rhs(), - #[cfg(feature = "simd-is-enabled")] - AnyJointVelocityConstraint::JointConstraintSimd(c) => c.remove_bias_from_rhs(), - #[cfg(feature = "simd-is-enabled")] - AnyJointVelocityConstraint::JointGroundConstraintSimd(c) => c.remove_bias_from_rhs(), - AnyJointVelocityConstraint::JointGenericConstraint(c) => c.remove_bias_from_rhs(), - AnyJointVelocityConstraint::JointGenericGroundConstraint(c) => c.remove_bias_from_rhs(), - AnyJointVelocityConstraint::Empty => unreachable!(), - } - } - - pub fn solve( - &mut self, - jacobians: &DVector, - mj_lambdas: &mut [DeltaVel], - generic_mj_lambdas: &mut DVector, - ) { - match self { - AnyJointVelocityConstraint::JointConstraint(c) => c.solve(mj_lambdas), - AnyJointVelocityConstraint::JointGroundConstraint(c) => c.solve(mj_lambdas), - #[cfg(feature = "simd-is-enabled")] - AnyJointVelocityConstraint::JointConstraintSimd(c) => c.solve(mj_lambdas), - #[cfg(feature = "simd-is-enabled")] - AnyJointVelocityConstraint::JointGroundConstraintSimd(c) => c.solve(mj_lambdas), - AnyJointVelocityConstraint::JointGenericConstraint(c) => { - c.solve(jacobians, mj_lambdas, generic_mj_lambdas) - } - AnyJointVelocityConstraint::JointGenericGroundConstraint(c) => { - c.solve(jacobians, mj_lambdas, generic_mj_lambdas) - } - AnyJointVelocityConstraint::Empty => unreachable!(), - } - } - - pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { - match self { - AnyJointVelocityConstraint::JointConstraint(c) => c.writeback_impulses(joints_all), - AnyJointVelocityConstraint::JointGroundConstraint(c) => { - c.writeback_impulses(joints_all) - } - #[cfg(feature = "simd-is-enabled")] - AnyJointVelocityConstraint::JointConstraintSimd(c) => c.writeback_impulses(joints_all), - #[cfg(feature = "simd-is-enabled")] - AnyJointVelocityConstraint::JointGroundConstraintSimd(c) => { - c.writeback_impulses(joints_all) - } - AnyJointVelocityConstraint::JointGenericConstraint(c) => { - c.writeback_impulses(joints_all) - } - AnyJointVelocityConstraint::JointGenericGroundConstraint(c) => { - c.writeback_impulses(joints_all) - } - AnyJointVelocityConstraint::Empty => unreachable!(), - } - } -} diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs similarity index 58% rename from src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs rename to src/dynamics/solver/joint_constraint/joint_constraint_builder.rs index f7101a1a3..00bead156 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs @@ -1,19 +1,349 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{ - JointVelocityConstraint, JointVelocityGroundConstraint, WritebackId, + JointFixedSolverBody, JointOneBodyConstraint, JointTwoBodyConstraint, WritebackId, }; -use crate::dynamics::solver::joint_constraint::SolverBody; +use crate::dynamics::solver::joint_constraint::JointSolverBody; +use crate::dynamics::solver::solver_body::SolverBody; +use crate::dynamics::solver::ConstraintsCounts; use crate::dynamics::solver::MotorParameters; -use crate::dynamics::{IntegrationParameters, JointIndex, JointLimits}; +use crate::dynamics::{GenericJoint, ImpulseJoint, IntegrationParameters, JointIndex}; use crate::math::{AngVector, Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM}; +use crate::prelude::RigidBodySet; use crate::utils; -use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal}; +use crate::utils::{IndexMut2, SimdCrossMatrix, SimdDot, SimdQuat, SimdRealCopy}; use na::SMatrix; #[cfg(feature = "dim3")] -use crate::utils::WBasis; +use crate::utils::SimdBasis; + +#[cfg(feature = "simd-is-enabled")] +use crate::math::{SimdReal, SIMD_WIDTH}; + +pub struct JointTwoBodyConstraintBuilder { + body1: usize, + body2: usize, + joint_id: JointIndex, + joint: GenericJoint, + constraint_id: usize, +} + +impl JointTwoBodyConstraintBuilder { + pub fn generate( + joint: &ImpulseJoint, + bodies: &RigidBodySet, + joint_id: JointIndex, + out_builder: &mut Self, + out_constraint_id: &mut usize, + ) { + let rb1 = &bodies[joint.body1]; + let rb2 = &bodies[joint.body2]; + *out_builder = Self { + body1: rb1.ids.active_set_offset, + body2: rb2.ids.active_set_offset, + joint_id, + joint: joint.data, + constraint_id: *out_constraint_id, + }; + + let count = ConstraintsCounts::from_joint(joint); + *out_constraint_id += count.num_constraints; + } + + pub fn update( + &self, + params: &IntegrationParameters, + bodies: &[SolverBody], + out: &mut [JointTwoBodyConstraint], + ) { + // NOTE: right now, the "update", is basically reconstructing all the + // constraints. Could we make this more incremental? + + let rb1 = &bodies[self.body1]; + let rb2 = &bodies[self.body2]; + let frame1 = rb1.position * self.joint.local_frame1; + let frame2 = rb2.position * self.joint.local_frame2; + + let joint_body1 = JointSolverBody { + im: rb1.im, + sqrt_ii: rb1.sqrt_ii, + world_com: rb1.world_com, + solver_vel: [self.body1], + }; + let joint_body2 = JointSolverBody { + im: rb2.im, + sqrt_ii: rb2.sqrt_ii, + world_com: rb2.world_com, + solver_vel: [self.body2], + }; + + JointTwoBodyConstraint::::lock_axes( + params, + self.joint_id, + &joint_body1, + &joint_body2, + &frame1, + &frame2, + &self.joint, + &mut out[self.constraint_id..], + ); + } +} + +#[cfg(feature = "simd-is-enabled")] +pub struct JointTwoBodyConstraintBuilderSimd { + body1: [usize; SIMD_WIDTH], + body2: [usize; SIMD_WIDTH], + joint_body1: JointSolverBody, + joint_body2: JointSolverBody, + joint_id: [JointIndex; SIMD_WIDTH], + local_frame1: Isometry, + local_frame2: Isometry, + locked_axes: u8, + constraint_id: usize, +} + +#[cfg(feature = "simd-is-enabled")] +impl JointTwoBodyConstraintBuilderSimd { + pub fn generate( + joint: [&ImpulseJoint; SIMD_WIDTH], + bodies: &RigidBodySet, + joint_id: [JointIndex; SIMD_WIDTH], + out_builder: &mut Self, + out_constraint_id: &mut usize, + ) { + let rb1 = gather![|ii| &bodies[joint[ii].body1]]; + let rb2 = gather![|ii| &bodies[joint[ii].body2]]; + + let body1 = gather![|ii| rb1[ii].ids.active_set_offset]; + let body2 = gather![|ii| rb2[ii].ids.active_set_offset]; + + let joint_body1 = JointSolverBody { + im: gather![|ii| rb1[ii].mprops.effective_inv_mass].into(), + sqrt_ii: gather![|ii| rb1[ii].mprops.effective_world_inv_inertia_sqrt].into(), + world_com: Point::origin(), + solver_vel: body1, + }; + let joint_body2 = JointSolverBody { + im: gather![|ii| rb2[ii].mprops.effective_inv_mass].into(), + sqrt_ii: gather![|ii| rb2[ii].mprops.effective_world_inv_inertia_sqrt].into(), + world_com: Point::origin(), + solver_vel: body2, + }; + + *out_builder = Self { + body1, + body2, + joint_body1, + joint_body2, + joint_id, + local_frame1: gather![|ii| joint[ii].data.local_frame1].into(), + local_frame2: gather![|ii| joint[ii].data.local_frame2].into(), + locked_axes: joint[0].data.locked_axes.bits(), + constraint_id: *out_constraint_id, + }; + + let count = ConstraintsCounts::from_joint(joint[0]); + *out_constraint_id += count.num_constraints; + } + + pub fn update( + &mut self, + params: &IntegrationParameters, + bodies: &[SolverBody], + out: &mut [JointTwoBodyConstraint], + ) { + // NOTE: right now, the "update", is basically reconstructing all the + // constraints. Could we make this more incremental? + + let rb1 = gather![|ii| &bodies[self.body1[ii]]]; + let rb2 = gather![|ii| &bodies[self.body2[ii]]]; + let frame1 = Isometry::from(gather![|ii| rb1[ii].position]) * self.local_frame1; + let frame2 = Isometry::from(gather![|ii| rb2[ii].position]) * self.local_frame2; + self.joint_body1.world_com = gather![|ii| rb1[ii].world_com].into(); + self.joint_body2.world_com = gather![|ii| rb2[ii].world_com].into(); + + JointTwoBodyConstraint::::lock_axes( + params, + self.joint_id, + &self.joint_body1, + &self.joint_body2, + &frame1, + &frame2, + self.locked_axes, + &mut out[self.constraint_id..], + ); + } +} + +pub struct JointOneBodyConstraintBuilder { + body1: JointFixedSolverBody, + frame1: Isometry, + body2: usize, + joint_id: JointIndex, + joint: GenericJoint, + constraint_id: usize, +} + +impl JointOneBodyConstraintBuilder { + pub fn generate( + joint: &ImpulseJoint, + bodies: &RigidBodySet, + joint_id: JointIndex, + out_builder: &mut Self, + out_constraint_id: &mut usize, + ) { + let mut joint_data = joint.data; + let mut handle1 = joint.body1; + let mut handle2 = joint.body2; + let flipped = !bodies[handle2].is_dynamic(); + + if flipped { + std::mem::swap(&mut handle1, &mut handle2); + std::mem::swap(&mut joint_data.local_frame1, &mut joint_data.local_frame2); + }; + + let rb1 = &bodies[handle1]; + let rb2 = &bodies[handle2]; + + let frame1 = rb1.pos.position * joint_data.local_frame1; + let joint_body1 = JointFixedSolverBody { + linvel: rb1.vels.linvel, + angvel: rb1.vels.angvel, + world_com: rb1.mprops.world_com, + }; + + *out_builder = Self { + body1: joint_body1, + frame1, + body2: rb2.ids.active_set_offset, + joint_id, + joint: joint_data, + constraint_id: *out_constraint_id, + }; + + let count = ConstraintsCounts::from_joint(joint); + *out_constraint_id += count.num_constraints; + } + + pub fn update( + &self, + params: &IntegrationParameters, + bodies: &[SolverBody], + out: &mut [JointOneBodyConstraint], + ) { + // NOTE: right now, the "update", is basically reconstructing all the + // constraints. Could we make this more incremental? + + let rb2 = &bodies[self.body2]; + let frame2 = rb2.position * self.joint.local_frame2; + + let joint_body2 = JointSolverBody { + im: rb2.im, + sqrt_ii: rb2.sqrt_ii, + world_com: rb2.world_com, + solver_vel: [self.body2], + }; + + JointOneBodyConstraint::::lock_axes( + params, + self.joint_id, + &self.body1, + &joint_body2, + &self.frame1, + &frame2, + &self.joint, + &mut out[self.constraint_id..], + ); + } +} + +#[cfg(feature = "simd-is-enabled")] +pub struct JointOneBodyConstraintBuilderSimd { + body1: JointFixedSolverBody, + frame1: Isometry, + body2: [usize; SIMD_WIDTH], + joint_id: [JointIndex; SIMD_WIDTH], + local_frame2: Isometry, + locked_axes: u8, + constraint_id: usize, +} + +#[cfg(feature = "simd-is-enabled")] +impl JointOneBodyConstraintBuilderSimd { + pub fn generate( + joint: [&ImpulseJoint; SIMD_WIDTH], + bodies: &RigidBodySet, + joint_id: [JointIndex; SIMD_WIDTH], + out_builder: &mut Self, + out_constraint_id: &mut usize, + ) { + let mut rb1 = gather![|ii| &bodies[joint[ii].body1]]; + let mut rb2 = gather![|ii| &bodies[joint[ii].body2]]; + let mut local_frame1 = gather![|ii| joint[ii].data.local_frame1]; + let mut local_frame2 = gather![|ii| joint[ii].data.local_frame2]; + + for ii in 0..SIMD_WIDTH { + if !rb2[ii].is_dynamic() { + std::mem::swap(&mut rb1[ii], &mut rb2[ii]); + std::mem::swap(&mut local_frame1[ii], &mut local_frame2[ii]); + } + } + + let poss1 = Isometry::from(gather![|ii| rb1[ii].pos.position]); + + let joint_body1 = JointFixedSolverBody { + linvel: gather![|ii| rb1[ii].vels.linvel].into(), + angvel: gather![|ii| rb1[ii].vels.angvel].into(), + world_com: gather![|ii| rb1[ii].mprops.world_com].into(), + }; + + *out_builder = Self { + body1: joint_body1, + body2: gather![|ii| rb2[ii].ids.active_set_offset], + joint_id, + frame1: poss1 * Isometry::from(local_frame1), + local_frame2: local_frame2.into(), + locked_axes: joint[0].data.locked_axes.bits(), + constraint_id: *out_constraint_id, + }; + + let count = ConstraintsCounts::from_joint(joint[0]); + *out_constraint_id += count.num_constraints; + } + + pub fn update( + &self, + params: &IntegrationParameters, + bodies: &[SolverBody], + out: &mut [JointOneBodyConstraint], + ) { + // NOTE: right now, the "update", is basically reconstructing all the + // constraints. Could we make this more incremental? + + let rb2 = gather![|ii| &bodies[self.body2[ii]]]; + let frame2 = Isometry::from(gather![|ii| rb2[ii].position]) * self.local_frame2; + + let joint_body2 = JointSolverBody { + im: gather![|ii| rb2[ii].im].into(), + sqrt_ii: gather![|ii| rb2[ii].sqrt_ii].into(), + world_com: gather![|ii| rb2[ii].world_com].into(), + solver_vel: self.body2, + }; + + JointOneBodyConstraint::::lock_axes( + params, + self.joint_id, + &self.body1, + &joint_body2, + &self.frame1, + &frame2, + self.locked_axes, + &mut out[self.constraint_id..], + ); + } +} #[derive(Debug, Copy, Clone)] -pub struct JointVelocityConstraintBuilder { +pub struct JointTwoBodyConstraintHelper { pub basis: Matrix, pub basis2: Matrix, // TODO: used for angular coupling. Can we avoid storing this? pub cmat1_basis: SMatrix, @@ -23,7 +353,7 @@ pub struct JointVelocityConstraintBuilder { pub ang_err: Rotation, } -impl JointVelocityConstraintBuilder { +impl JointTwoBodyConstraintHelper { pub fn new( frame1: &Isometry, frame2: &Isometry, @@ -84,12 +414,12 @@ impl JointVelocityConstraintBuilder { &self, params: &IntegrationParameters, joint_id: [JointIndex; LANES], - body1: &SolverBody, - body2: &SolverBody, + body1: &JointSolverBody, + body2: &JointSolverBody, limited_axis: usize, limits: [N; 2], writeback_id: WritebackId, - ) -> JointVelocityConstraint { + ) -> JointTwoBodyConstraint { let zero = N::zero(); let mut constraint = self.lock_linear(params, joint_id, body1, body2, limited_axis, writeback_id); @@ -116,20 +446,19 @@ impl JointVelocityConstraintBuilder { &self, params: &IntegrationParameters, joint_id: [JointIndex; LANES], - body1: &SolverBody, - body2: &SolverBody, - limited_coupled_axes: u8, - limits: &[JointLimits], + body1: &JointSolverBody, + body2: &JointSolverBody, + coupled_axes: u8, + limits: [N; 2], writeback_id: WritebackId, - ) -> JointVelocityConstraint { + ) -> JointTwoBodyConstraint { let zero = N::zero(); let mut lin_jac = Vector::zeros(); let mut ang_jac1: AngVector = na::zero(); let mut ang_jac2: AngVector = na::zero(); - let mut limit = N::zero(); for i in 0..DIM { - if limited_coupled_axes & (1 << i) != 0 { + if coupled_axes & (1 << i) != 0 { let coeff = self.basis.column(i).dot(&self.lin_err); lin_jac += self.basis.column(i) * coeff; #[cfg(feature = "dim2")] @@ -142,34 +471,32 @@ impl JointVelocityConstraintBuilder { ang_jac1 += self.cmat1_basis.column(i) * coeff; ang_jac2 += self.cmat2_basis.column(i) * coeff; } - limit += limits[i].max * limits[i].max; } } - limit = limit.simd_sqrt(); + // FIXME: handle min limit too. + let dist = lin_jac.norm(); let inv_dist = crate::utils::simd_inv(dist); lin_jac *= inv_dist; ang_jac1 *= inv_dist; ang_jac2 *= inv_dist; - let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) - + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); - let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt()); + let rhs_wo_bias = (dist - limits[1]).simd_min(zero) * N::splat(params.inv_dt()); ang_jac1 = body1.sqrt_ii * ang_jac1; ang_jac2 = body2.sqrt_ii * ang_jac2; let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); let cfm_coeff = N::splat(params.joint_cfm_coeff()); - let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt; + let rhs_bias = (dist - limits[1]).simd_max(zero) * erp_inv_dt; let rhs = rhs_wo_bias + rhs_bias; let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)]; - JointVelocityConstraint { + JointTwoBodyConstraint { joint_id, - mj_lambda1: body1.mj_lambda, - mj_lambda2: body2.mj_lambda, + solver_vel1: body1.solver_vel, + solver_vel2: body2.solver_vel, im1: body1.im, im2: body2.im, impulse: N::zero(), @@ -190,13 +517,13 @@ impl JointVelocityConstraintBuilder { &self, params: &IntegrationParameters, joint_id: [JointIndex; LANES], - body1: &SolverBody, - body2: &SolverBody, + body1: &JointSolverBody, + body2: &JointSolverBody, motor_axis: usize, motor_params: &MotorParameters, limits: Option<[N; 2]>, writeback_id: WritebackId, - ) -> JointVelocityConstraint { + ) -> JointTwoBodyConstraint { let inv_dt = N::splat(params.inv_dt()); let mut constraint = self.lock_linear(params, joint_id, body1, body2, motor_axis, writeback_id); @@ -214,9 +541,7 @@ impl JointVelocityConstraintBuilder { target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt); }; - let dvel = constraint.lin_jac.dot(&(body2.linvel - body1.linvel)) - + (constraint.ang_jac2.gdot(body2.angvel) - constraint.ang_jac1.gdot(body1.angvel)); - rhs_wo_bias += dvel - target_vel; + rhs_wo_bias += -target_vel; constraint.cfm_coeff = motor_params.cfm_coeff; constraint.cfm_gain = motor_params.cfm_gain; @@ -230,11 +555,11 @@ impl JointVelocityConstraintBuilder { &self, params: &IntegrationParameters, joint_id: [JointIndex; LANES], - body1: &SolverBody, - body2: &SolverBody, + body1: &JointSolverBody, + body2: &JointSolverBody, locked_axis: usize, writeback_id: WritebackId, - ) -> JointVelocityConstraint { + ) -> JointTwoBodyConstraint { let lin_jac = self.basis.column(locked_axis).into_owned(); #[cfg(feature = "dim2")] let mut ang_jac1 = self.cmat1_basis[locked_axis]; @@ -245,10 +570,7 @@ impl JointVelocityConstraintBuilder { #[cfg(feature = "dim3")] let mut ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned(); - let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) - + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); - let rhs_wo_bias = dvel; - + let rhs_wo_bias = N::zero(); let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); let cfm_coeff = N::splat(params.joint_cfm_coeff()); let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt; @@ -256,10 +578,10 @@ impl JointVelocityConstraintBuilder { ang_jac1 = body1.sqrt_ii * ang_jac1; ang_jac2 = body2.sqrt_ii * ang_jac2; - JointVelocityConstraint { + JointTwoBodyConstraint { joint_id, - mj_lambda1: body1.mj_lambda, - mj_lambda2: body2.mj_lambda, + solver_vel1: body1.solver_vel, + solver_vel2: body2.solver_vel, im1: body1.im, im2: body2.im, impulse: N::zero(), @@ -280,12 +602,12 @@ impl JointVelocityConstraintBuilder { &self, params: &IntegrationParameters, joint_id: [JointIndex; LANES], - body1: &SolverBody, - body2: &SolverBody, + body1: &JointSolverBody, + body2: &JointSolverBody, _limited_axis: usize, limits: [N; 2], writeback_id: WritebackId, - ) -> JointVelocityConstraint { + ) -> JointTwoBodyConstraint { let zero = N::zero(); let half = N::splat(0.5); let s_limits = [(limits[0] * half).simd_sin(), (limits[1] * half).simd_sin()]; @@ -305,9 +627,7 @@ impl JointVelocityConstraintBuilder { let ang_jac = N::one(); #[cfg(feature = "dim3")] let ang_jac = self.ang_basis.column(_limited_axis).into_owned(); - let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); - let rhs_wo_bias = dvel; - + let rhs_wo_bias = N::zero(); let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); let cfm_coeff = N::splat(params.joint_cfm_coeff()); let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero) @@ -317,10 +637,10 @@ impl JointVelocityConstraintBuilder { let ang_jac1 = body1.sqrt_ii * ang_jac; let ang_jac2 = body2.sqrt_ii * ang_jac; - JointVelocityConstraint { + JointTwoBodyConstraint { joint_id, - mj_lambda1: body1.mj_lambda, - mj_lambda2: body2.mj_lambda, + solver_vel1: body1.solver_vel, + solver_vel2: body2.solver_vel, im1: body1.im, im2: body2.im, impulse: N::zero(), @@ -340,12 +660,12 @@ impl JointVelocityConstraintBuilder { pub fn motor_angular( &self, joint_id: [JointIndex; LANES], - body1: &SolverBody, - body2: &SolverBody, + body1: &JointSolverBody, + body2: &JointSolverBody, _motor_axis: usize, motor_params: &MotorParameters, writeback_id: WritebackId, - ) -> JointVelocityConstraint { + ) -> JointTwoBodyConstraint { #[cfg(feature = "dim2")] let ang_jac = N::one(); #[cfg(feature = "dim3")] @@ -353,27 +673,24 @@ impl JointVelocityConstraintBuilder { let mut rhs_wo_bias = N::zero(); if motor_params.erp_inv_dt != N::zero() { - let half = N::splat(0.5); - #[cfg(feature = "dim2")] - let s_ang_dist = (self.ang_err.angle() * half).simd_sin(); + let ang_dist = self.ang_err.angle(); #[cfg(feature = "dim3")] - let s_ang_dist = self.ang_err.imag()[_motor_axis]; - let s_target_ang = (motor_params.target_pos * half).simd_sin(); - rhs_wo_bias += utils::smallest_abs_diff_between_sin_angles(s_ang_dist, s_target_ang) + let ang_dist = self.ang_err.imag()[_motor_axis].simd_asin() * N::splat(2.0); + let target_ang = motor_params.target_pos; + rhs_wo_bias += utils::smallest_abs_diff_between_angles(ang_dist, target_ang) * motor_params.erp_inv_dt; } - let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); - rhs_wo_bias += dvel - motor_params.target_vel; + rhs_wo_bias += -motor_params.target_vel; let ang_jac1 = body1.sqrt_ii * ang_jac; let ang_jac2 = body2.sqrt_ii * ang_jac; - JointVelocityConstraint { + JointTwoBodyConstraint { joint_id, - mj_lambda1: body1.mj_lambda, - mj_lambda2: body2.mj_lambda, + solver_vel1: body1.solver_vel, + solver_vel2: body2.solver_vel, im1: body1.im, im2: body2.im, impulse: N::zero(), @@ -394,19 +711,17 @@ impl JointVelocityConstraintBuilder { &self, params: &IntegrationParameters, joint_id: [JointIndex; LANES], - body1: &SolverBody, - body2: &SolverBody, + body1: &JointSolverBody, + body2: &JointSolverBody, _locked_axis: usize, writeback_id: WritebackId, - ) -> JointVelocityConstraint { + ) -> JointTwoBodyConstraint { #[cfg(feature = "dim2")] let ang_jac = N::one(); #[cfg(feature = "dim3")] let ang_jac = self.ang_basis.column(_locked_axis).into_owned(); - let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); - let rhs_wo_bias = dvel; - + let rhs_wo_bias = N::zero(); let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); let cfm_coeff = N::splat(params.joint_cfm_coeff()); #[cfg(feature = "dim2")] @@ -417,10 +732,10 @@ impl JointVelocityConstraintBuilder { let ang_jac1 = body1.sqrt_ii * ang_jac; let ang_jac2 = body2.sqrt_ii * ang_jac; - JointVelocityConstraint { + JointTwoBodyConstraint { joint_id, - mj_lambda1: body1.mj_lambda, - mj_lambda2: body2.mj_lambda, + solver_vel1: body1.solver_vel, + solver_vel2: body2.solver_vel, im1: body1.im, im2: body2.im, impulse: N::zero(), @@ -439,7 +754,7 @@ impl JointVelocityConstraintBuilder { /// Orthogonalize the constraints and set their inv_lhs field. pub fn finalize_constraints( - constraints: &mut [JointVelocityConstraint], + constraints: &mut [JointTwoBodyConstraint], ) { let len = constraints.len(); @@ -484,16 +799,16 @@ impl JointVelocityConstraintBuilder { } } - pub fn limit_linear_ground( + pub fn limit_linear_one_body( &self, params: &IntegrationParameters, joint_id: [JointIndex; LANES], - body1: &SolverBody, - body2: &SolverBody, + body1: &JointFixedSolverBody, + body2: &JointSolverBody, limited_axis: usize, limits: [N; 2], writeback_id: WritebackId, - ) -> JointVelocityGroundConstraint { + ) -> JointOneBodyConstraint { let zero = N::zero(); let lin_jac = self.basis.column(limited_axis).into_owned(); let dist = self.lin_err.dot(&lin_jac); @@ -512,10 +827,7 @@ impl JointVelocityConstraintBuilder { #[cfg(feature = "dim3")] let mut ang_jac2 = self.cmat2_basis.column(limited_axis).into_owned(); - let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) - + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); - let rhs_wo_bias = dvel; - + let rhs_wo_bias = -lin_jac.dot(&body1.linvel) - ang_jac1.gdot(body1.angvel); let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); let cfm_coeff = N::splat(params.joint_cfm_coeff()); let rhs_bias = @@ -523,9 +835,9 @@ impl JointVelocityConstraintBuilder { ang_jac2 = body2.sqrt_ii * ang_jac2; - JointVelocityGroundConstraint { + JointOneBodyConstraint { joint_id, - mj_lambda2: body2.mj_lambda, + solver_vel2: body2.solver_vel, im2: body2.im, impulse: zero, impulse_bounds, @@ -540,24 +852,23 @@ impl JointVelocityConstraintBuilder { } } - pub fn limit_linear_coupled_ground( + pub fn limit_linear_coupled_one_body( &self, params: &IntegrationParameters, joint_id: [JointIndex; LANES], - body1: &SolverBody, - body2: &SolverBody, - limited_coupled_axes: u8, - limits: &[JointLimits], + body1: &JointFixedSolverBody, + body2: &JointSolverBody, + coupled_axes: u8, + limits: [N; 2], writeback_id: WritebackId, - ) -> JointVelocityGroundConstraint { + ) -> JointOneBodyConstraint { let zero = N::zero(); let mut lin_jac = Vector::zeros(); let mut ang_jac1: AngVector = na::zero(); let mut ang_jac2: AngVector = na::zero(); - let mut limit = N::zero(); for i in 0..DIM { - if limited_coupled_axes & (1 << i) != 0 { + if coupled_axes & (1 << i) != 0 { let coeff = self.basis.column(i).dot(&self.lin_err); lin_jac += self.basis.column(i) * coeff; #[cfg(feature = "dim2")] @@ -570,32 +881,30 @@ impl JointVelocityConstraintBuilder { ang_jac1 += self.cmat1_basis.column(i) * coeff; ang_jac2 += self.cmat2_basis.column(i) * coeff; } - limit += limits[i].max * limits[i].max; } } - limit = limit.simd_sqrt(); let dist = lin_jac.norm(); let inv_dist = crate::utils::simd_inv(dist); lin_jac *= inv_dist; ang_jac1 *= inv_dist; ang_jac2 *= inv_dist; - let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) - + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); - let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt()); + // FIXME: handle min limit too. + let proj_vel1 = -lin_jac.dot(&body1.linvel) - ang_jac1.gdot(body1.angvel); + let rhs_wo_bias = proj_vel1 + (dist - limits[1]).simd_min(zero) * N::splat(params.inv_dt()); ang_jac2 = body2.sqrt_ii * ang_jac2; let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); let cfm_coeff = N::splat(params.joint_cfm_coeff()); - let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt; + let rhs_bias = (dist - limits[1]).simd_max(zero) * erp_inv_dt; let rhs = rhs_wo_bias + rhs_bias; let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)]; - JointVelocityGroundConstraint { + JointOneBodyConstraint { joint_id, - mj_lambda2: body2.mj_lambda, + solver_vel2: body2.solver_vel, im2: body2.im, impulse: N::zero(), impulse_bounds, @@ -610,17 +919,17 @@ impl JointVelocityConstraintBuilder { } } - pub fn motor_linear_ground( + pub fn motor_linear_one_body( &self, params: &IntegrationParameters, joint_id: [JointIndex; LANES], - body1: &SolverBody, - body2: &SolverBody, + body1: &JointFixedSolverBody, + body2: &JointSolverBody, motor_axis: usize, motor_params: &MotorParameters, limits: Option<[N; 2]>, writeback_id: WritebackId, - ) -> JointVelocityGroundConstraint { + ) -> JointOneBodyConstraint { let inv_dt = N::splat(params.inv_dt()); let lin_jac = self.basis.column(motor_axis).into_owned(); @@ -643,15 +952,87 @@ impl JointVelocityConstraintBuilder { target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt); }; - let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) - + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); - rhs_wo_bias += dvel - target_vel; + let proj_vel1 = -lin_jac.dot(&body1.linvel) - ang_jac1.gdot(body1.angvel); + rhs_wo_bias += proj_vel1 - target_vel; + + ang_jac2 = body2.sqrt_ii * ang_jac2; + + JointOneBodyConstraint { + joint_id, + solver_vel2: body2.solver_vel, + im2: body2.im, + impulse: N::zero(), + impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse], + lin_jac, + ang_jac2, + inv_lhs: N::zero(), // Will be set during ortogonalization. + cfm_coeff: motor_params.cfm_coeff, + cfm_gain: motor_params.cfm_gain, + rhs: rhs_wo_bias, + rhs_wo_bias, + writeback_id, + } + } + + pub fn motor_linear_coupled_one_body( + &self, + params: &IntegrationParameters, + joint_id: [JointIndex; LANES], + body1: &JointFixedSolverBody, + body2: &JointSolverBody, + coupled_axes: u8, + motor_params: &MotorParameters, + limits: Option<[N; 2]>, + writeback_id: WritebackId, + ) -> JointOneBodyConstraint { + let inv_dt = N::splat(params.inv_dt()); + + let mut lin_jac = Vector::zeros(); + let mut ang_jac1: AngVector = na::zero(); + let mut ang_jac2: AngVector = na::zero(); + + for i in 0..DIM { + if coupled_axes & (1 << i) != 0 { + let coeff = self.basis.column(i).dot(&self.lin_err); + lin_jac += self.basis.column(i) * coeff; + #[cfg(feature = "dim2")] + { + ang_jac1 += self.cmat1_basis[i] * coeff; + ang_jac2 += self.cmat2_basis[i] * coeff; + } + #[cfg(feature = "dim3")] + { + ang_jac1 += self.cmat1_basis.column(i) * coeff; + ang_jac2 += self.cmat2_basis.column(i) * coeff; + } + } + } + + let dist = lin_jac.norm(); + let inv_dist = crate::utils::simd_inv(dist); + lin_jac *= inv_dist; + ang_jac1 *= inv_dist; + ang_jac2 *= inv_dist; + + let mut rhs_wo_bias = N::zero(); + if motor_params.erp_inv_dt != N::zero() { + rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt; + } + + let mut target_vel = motor_params.target_vel; + if let Some(limits) = limits { + target_vel = + target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt); + }; + + let proj_vel1 = -lin_jac.dot(&body1.linvel) - ang_jac1.gdot(body1.angvel); + rhs_wo_bias += proj_vel1 - target_vel; ang_jac2 = body2.sqrt_ii * ang_jac2; - JointVelocityGroundConstraint { + JointOneBodyConstraint { joint_id, - mj_lambda2: body2.mj_lambda, + solver_vel2: body2.solver_vel, im2: body2.im, impulse: N::zero(), impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse], @@ -666,86 +1047,15 @@ impl JointVelocityConstraintBuilder { } } - // pub fn motor_linear_coupled_ground( - // &self, - // _joint_id: [JointIndex; LANES], - // _body1: &SolverBody, - // _body2: &SolverBody, - // _motor_coupled_axes: u8, - // _motors: &[MotorParameters], - // _limited_coupled_axes: u8, - // _limits: &[JointLimits], - // _writeback_id: WritebackId, - // ) -> JointVelocityGroundConstraint { - // let zero = N::zero(); - // let mut lin_jac = Vector::zeros(); - // let mut ang_jac1: AngVector = na::zero(); - // let mut ang_jac2: AngVector = na::zero(); - // let mut limit = N::zero(); - - // for i in 0..DIM { - // if limited_coupled_axes & (1 << i) != 0 { - // let coeff = self.basis.column(i).dot(&self.lin_err); - // lin_jac += self.basis.column(i) * coeff; - // #[cfg(feature = "dim2")] - // { - // ang_jac1 += self.cmat1_basis[i] * coeff; - // ang_jac2 += self.cmat2_basis[i] * coeff; - // } - // #[cfg(feature = "dim3")] - // { - // ang_jac1 += self.cmat1_basis.column(i) * coeff; - // ang_jac2 += self.cmat2_basis.column(i) * coeff; - // } - // limit += limits[i].max * limits[i].max; - // } - // } - - // limit = limit.simd_sqrt(); - // let dist = lin_jac.norm(); - // let inv_dist = crate::utils::simd_inv(dist); - // lin_jac *= inv_dist; - // ang_jac1 *= inv_dist; - // ang_jac2 *= inv_dist; - - // let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) - // + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); - // let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt()); - - // ang_jac2 = body2.sqrt_ii * ang_jac2; - - // let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - // let cfm_coeff = N::splat(params.joint_cfm_coeff()); - // let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt; - // let rhs = rhs_wo_bias + rhs_bias; - // let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)]; - - // JointVelocityGroundConstraint { - // joint_id, - // mj_lambda2: body2.mj_lambda, - // im2: body2.im, - // impulse: N::zero(), - // impulse_bounds, - // lin_jac, - // ang_jac2, - // inv_lhs: N::zero(), // Will be set during ortogonalization. - // cfm_coeff, - // cfm_gain: N::zero(), - // rhs, - // rhs_wo_bias, - // writeback_id, - // } - // } - - pub fn lock_linear_ground( + pub fn lock_linear_one_body( &self, params: &IntegrationParameters, joint_id: [JointIndex; LANES], - body1: &SolverBody, - body2: &SolverBody, + body1: &JointFixedSolverBody, + body2: &JointSolverBody, locked_axis: usize, writeback_id: WritebackId, - ) -> JointVelocityGroundConstraint { + ) -> JointOneBodyConstraint { let lin_jac = self.basis.column(locked_axis).into_owned(); let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned(); #[cfg(feature = "dim2")] @@ -753,9 +1063,7 @@ impl JointVelocityConstraintBuilder { #[cfg(feature = "dim3")] let mut ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned(); - let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) - + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); - let rhs_wo_bias = dvel; + let rhs_wo_bias = -lin_jac.dot(&body1.linvel) - ang_jac1.gdot(body1.angvel); let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); let cfm_coeff = N::splat(params.joint_cfm_coeff()); @@ -763,9 +1071,9 @@ impl JointVelocityConstraintBuilder { ang_jac2 = body2.sqrt_ii * ang_jac2; - JointVelocityGroundConstraint { + JointOneBodyConstraint { joint_id, - mj_lambda2: body2.mj_lambda, + solver_vel2: body2.solver_vel, im2: body2.im, impulse: N::zero(), impulse_bounds: [-N::splat(Real::MAX), N::splat(Real::MAX)], @@ -780,15 +1088,15 @@ impl JointVelocityConstraintBuilder { } } - pub fn motor_angular_ground( + pub fn motor_angular_one_body( &self, joint_id: [JointIndex; LANES], - body1: &SolverBody, - body2: &SolverBody, + body1: &JointFixedSolverBody, + body2: &JointSolverBody, _motor_axis: usize, motor_params: &MotorParameters, writeback_id: WritebackId, - ) -> JointVelocityGroundConstraint { + ) -> JointOneBodyConstraint { #[cfg(feature = "dim2")] let ang_jac = N::one(); #[cfg(feature = "dim3")] @@ -796,25 +1104,23 @@ impl JointVelocityConstraintBuilder { let mut rhs_wo_bias = N::zero(); if motor_params.erp_inv_dt != N::zero() { - let half = N::splat(0.5); - #[cfg(feature = "dim2")] - let s_ang_dist = (self.ang_err.angle() * half).simd_sin(); + let ang_dist = self.ang_err.angle(); #[cfg(feature = "dim3")] - let s_ang_dist = self.ang_err.imag()[_motor_axis]; - let s_target_ang = (motor_params.target_pos * half).simd_sin(); - rhs_wo_bias += utils::smallest_abs_diff_between_sin_angles(s_ang_dist, s_target_ang) + let ang_dist = self.ang_err.imag()[_motor_axis].simd_asin() * N::splat(2.0); + let target_ang = motor_params.target_pos; + rhs_wo_bias += utils::smallest_abs_diff_between_angles(ang_dist, target_ang) * motor_params.erp_inv_dt; } - let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); - rhs_wo_bias += dvel - motor_params.target_vel; + let proj_vel1 = -ang_jac.gdot(body1.angvel); + rhs_wo_bias += proj_vel1 - motor_params.target_vel; let ang_jac2 = body2.sqrt_ii * ang_jac; - JointVelocityGroundConstraint { + JointOneBodyConstraint { joint_id, - mj_lambda2: body2.mj_lambda, + solver_vel2: body2.solver_vel, im2: body2.im, impulse: N::zero(), impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse], @@ -829,16 +1135,16 @@ impl JointVelocityConstraintBuilder { } } - pub fn limit_angular_ground( + pub fn limit_angular_one_body( &self, params: &IntegrationParameters, joint_id: [JointIndex; LANES], - body1: &SolverBody, - body2: &SolverBody, + body1: &JointFixedSolverBody, + body2: &JointSolverBody, _limited_axis: usize, limits: [N; 2], writeback_id: WritebackId, - ) -> JointVelocityGroundConstraint { + ) -> JointOneBodyConstraint { let zero = N::zero(); let half = N::splat(0.5); let s_limits = [(limits[0] * half).simd_sin(), (limits[1] * half).simd_sin()]; @@ -858,8 +1164,7 @@ impl JointVelocityConstraintBuilder { let ang_jac = N::one(); #[cfg(feature = "dim3")] let ang_jac = self.ang_basis.column(_limited_axis).into_owned(); - let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); - let rhs_wo_bias = dvel; + let rhs_wo_bias = -ang_jac.gdot(body1.angvel); let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); let cfm_coeff = N::splat(params.joint_cfm_coeff()); @@ -869,9 +1174,9 @@ impl JointVelocityConstraintBuilder { let ang_jac2 = body2.sqrt_ii * ang_jac; - JointVelocityGroundConstraint { + JointOneBodyConstraint { joint_id, - mj_lambda2: body2.mj_lambda, + solver_vel2: body2.solver_vel, im2: body2.im, impulse: zero, impulse_bounds, @@ -886,22 +1191,21 @@ impl JointVelocityConstraintBuilder { } } - pub fn lock_angular_ground( + pub fn lock_angular_one_body( &self, params: &IntegrationParameters, joint_id: [JointIndex; LANES], - body1: &SolverBody, - body2: &SolverBody, + body1: &JointFixedSolverBody, + body2: &JointSolverBody, _locked_axis: usize, writeback_id: WritebackId, - ) -> JointVelocityGroundConstraint { + ) -> JointOneBodyConstraint { #[cfg(feature = "dim2")] let ang_jac = N::one(); #[cfg(feature = "dim3")] let ang_jac = self.ang_basis.column(_locked_axis).into_owned(); - let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); - let rhs_wo_bias = dvel; + let rhs_wo_bias = -ang_jac.gdot(body1.angvel); let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); let cfm_coeff = N::splat(params.joint_cfm_coeff()); @@ -912,9 +1216,9 @@ impl JointVelocityConstraintBuilder { let ang_jac2 = body2.sqrt_ii * ang_jac; - JointVelocityGroundConstraint { + JointOneBodyConstraint { joint_id, - mj_lambda2: body2.mj_lambda, + solver_vel2: body2.solver_vel, im2: body2.im, impulse: N::zero(), impulse_bounds: [-N::splat(Real::MAX), N::splat(Real::MAX)], @@ -930,8 +1234,8 @@ impl JointVelocityConstraintBuilder { } /// Orthogonalize the constraints and set their inv_lhs field. - pub fn finalize_ground_constraints( - constraints: &mut [JointVelocityGroundConstraint], + pub fn finalize_one_body_constraints( + constraints: &mut [JointOneBodyConstraint], ) { let len = constraints.len(); @@ -947,13 +1251,11 @@ impl JointVelocityConstraintBuilder { let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac)) + c_j.ang_jac2.gdot(c_j.ang_jac2); let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain; - let inv_dot_jj = crate::utils::simd_inv(dot_jj + cfm_gain); - c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs. + let inv_dot_jj = crate::utils::simd_inv(dot_jj); + c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs. c_j.cfm_gain = cfm_gain; - if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] - || c_j.cfm_gain != N::zero() - { + if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] { // Don't remove constraints with limited forces from the others // because they may not deliver the necessary forces to fulfill // the removed parts of other constraints. @@ -976,22 +1278,22 @@ impl JointVelocityConstraintBuilder { } } -impl JointVelocityConstraintBuilder { - // TODO: this method is almost identical to the ground version, except for the +impl JointTwoBodyConstraintHelper { + // TODO: this method is almost identical to the one_body version, except for the // return type. Could they share their implementation somehow? #[cfg(feature = "dim3")] pub fn limit_angular_coupled( &self, params: &IntegrationParameters, joint_id: [JointIndex; 1], - body1: &SolverBody, - body2: &SolverBody, - limited_coupled_axes: u8, - limits: &[JointLimits], + body1: &JointSolverBody, + body2: &JointSolverBody, + coupled_axes: u8, + limits: [Real; 2], writeback_id: WritebackId, - ) -> JointVelocityConstraint { - // NOTE: right now, this only supports exactly 2 coupled axes. - let ang_coupled_axes = limited_coupled_axes >> DIM; + ) -> JointTwoBodyConstraint { + // NOTE: right now, this only supports exactly 2 coupled axes. + let ang_coupled_axes = coupled_axes >> DIM; assert_eq!(ang_coupled_axes.count_ones(), 2); let not_coupled_index = ang_coupled_axes.trailing_ones() as usize; let axis1 = self.basis.column(not_coupled_index).into_owned(); @@ -1002,42 +1304,28 @@ impl JointVelocityConstraintBuilder { .axis_angle() .map(|(axis, angle)| (axis.into_inner(), angle)) .unwrap_or_else(|| (axis1.orthonormal_basis()[0], 0.0)); - let mut ang_limits = [0.0, 0.0]; - for k in 0..3 { - if (ang_coupled_axes & (1 << k)) != 0 { - let limit = &limits[DIM + k]; - ang_limits[0] += limit.min * limit.min; - ang_limits[1] += limit.max * limit.max; - } - } - - ang_limits[0] = ang_limits[0].sqrt(); - ang_limits[1] = ang_limits[1].sqrt(); - - let min_enabled = angle <= ang_limits[0]; - let max_enabled = ang_limits[1] <= angle; + let min_enabled = angle <= limits[0]; + let max_enabled = limits[1] <= angle; let impulse_bounds = [ if min_enabled { -Real::INFINITY } else { 0.0 }, if max_enabled { Real::INFINITY } else { 0.0 }, ]; - let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); - let rhs_wo_bias = dvel; + let rhs_wo_bias = 0.0; let erp_inv_dt = params.joint_erp_inv_dt(); let cfm_coeff = params.joint_cfm_coeff(); - let rhs_bias = - ((angle - ang_limits[1]).max(0.0) - (ang_limits[0] - angle).max(0.0)) * erp_inv_dt; + let rhs_bias = ((angle - limits[1]).max(0.0) - (limits[0] - angle).max(0.0)) * erp_inv_dt; let ang_jac1 = body1.sqrt_ii * ang_jac; let ang_jac2 = body2.sqrt_ii * ang_jac; - JointVelocityConstraint { + JointTwoBodyConstraint { joint_id, - mj_lambda1: body1.mj_lambda, - mj_lambda2: body2.mj_lambda, + solver_vel1: body1.solver_vel, + solver_vel2: body2.solver_vel, im1: body1.im, im2: body2.im, impulse: 0.0, @@ -1055,18 +1343,18 @@ impl JointVelocityConstraintBuilder { } #[cfg(feature = "dim3")] - pub fn limit_angular_coupled_ground( + pub fn limit_angular_coupled_one_body( &self, params: &IntegrationParameters, joint_id: [JointIndex; 1], - body1: &SolverBody, - body2: &SolverBody, - limited_coupled_axes: u8, - limits: &[JointLimits], + body1: &JointFixedSolverBody, + body2: &JointSolverBody, + coupled_axes: u8, + limits: [Real; 2], writeback_id: WritebackId, - ) -> JointVelocityGroundConstraint { - // NOTE: right now, this only supports exactly 2 coupled axes. - let ang_coupled_axes = limited_coupled_axes >> DIM; + ) -> JointOneBodyConstraint { + // NOTE: right now, this only supports exactly 2 coupled axes. + let ang_coupled_axes = coupled_axes >> DIM; assert_eq!(ang_coupled_axes.count_ones(), 2); let not_coupled_index = ang_coupled_axes.trailing_ones() as usize; let axis1 = self.basis.column(not_coupled_index).into_owned(); @@ -1077,40 +1365,26 @@ impl JointVelocityConstraintBuilder { .axis_angle() .map(|(axis, angle)| (axis.into_inner(), angle)) .unwrap_or_else(|| (axis1.orthonormal_basis()[0], 0.0)); - let mut ang_limits = [0.0, 0.0]; - for k in 0..3 { - if (ang_coupled_axes & (1 << k)) != 0 { - let limit = &limits[DIM + k]; - ang_limits[0] += limit.min * limit.min; - ang_limits[1] += limit.max * limit.max; - } - } - - ang_limits[0] = ang_limits[0].sqrt(); - ang_limits[1] = ang_limits[1].sqrt(); - - let min_enabled = angle <= ang_limits[0]; - let max_enabled = ang_limits[1] <= angle; + let min_enabled = angle <= limits[0]; + let max_enabled = limits[1] <= angle; let impulse_bounds = [ if min_enabled { -Real::INFINITY } else { 0.0 }, if max_enabled { Real::INFINITY } else { 0.0 }, ]; - let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); - let rhs_wo_bias = dvel; + let rhs_wo_bias = -ang_jac.gdot(body1.angvel); let erp_inv_dt = params.joint_erp_inv_dt(); let cfm_coeff = params.joint_cfm_coeff(); - let rhs_bias = - ((angle - ang_limits[1]).max(0.0) - (ang_limits[0] - angle).max(0.0)) * erp_inv_dt; + let rhs_bias = ((angle - limits[1]).max(0.0) - (limits[0] - angle).max(0.0)) * erp_inv_dt; let ang_jac2 = body2.sqrt_ii * ang_jac; - JointVelocityGroundConstraint { + JointOneBodyConstraint { joint_id, - mj_lambda2: body2.mj_lambda, + solver_vel2: body2.solver_vel, im2: body2.im, impulse: 0.0, impulse_bounds, diff --git a/src/dynamics/solver/joint_constraint/joint_constraints_set.rs b/src/dynamics/solver/joint_constraint/joint_constraints_set.rs new file mode 100644 index 000000000..21b645965 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/joint_constraints_set.rs @@ -0,0 +1,446 @@ +use crate::dynamics::solver::categorization::categorize_joints; +use crate::dynamics::solver::solver_body::SolverBody; +use crate::dynamics::solver::solver_vel::SolverVel; +use crate::dynamics::solver::{ + reset_buffer, JointConstraintTypes, JointGenericOneBodyConstraint, + JointGenericOneBodyConstraintBuilder, JointGenericTwoBodyConstraint, + JointGenericTwoBodyConstraintBuilder, JointGenericVelocityOneBodyExternalConstraintBuilder, + JointGenericVelocityOneBodyInternalConstraintBuilder, SolverConstraintsSet, +}; +use crate::dynamics::{ + IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet, + RigidBodySet, +}; +use na::DVector; +use parry::math::Real; + +use crate::dynamics::solver::joint_constraint::joint_constraint_builder::{ + JointOneBodyConstraintBuilder, JointTwoBodyConstraintBuilder, +}; +#[cfg(feature = "simd-is-enabled")] +use { + crate::dynamics::solver::joint_constraint::joint_constraint_builder::{ + JointOneBodyConstraintBuilderSimd, JointTwoBodyConstraintBuilderSimd, + }, + crate::math::SIMD_WIDTH, +}; + +pub type JointConstraintsSet = SolverConstraintsSet; + +impl JointConstraintsSet { + pub fn init( + &mut self, + island_id: usize, + islands: &IslandManager, + bodies: &RigidBodySet, + multibody_joints: &MultibodyJointSet, + impulse_joints: &[JointGraphEdge], + joint_constraint_indices: &[JointIndex], + ) { + // Generate constraints for impulse_joints. + self.two_body_interactions.clear(); + self.one_body_interactions.clear(); + self.generic_two_body_interactions.clear(); + self.generic_one_body_interactions.clear(); + + categorize_joints( + bodies, + multibody_joints, + impulse_joints, + joint_constraint_indices, + &mut self.one_body_interactions, + &mut self.two_body_interactions, + &mut self.generic_one_body_interactions, + &mut self.generic_two_body_interactions, + ); + + self.clear_constraints(); + self.clear_builders(); + + self.interaction_groups.clear_groups(); + self.interaction_groups.group_joints( + island_id, + islands, + bodies, + impulse_joints, + &self.two_body_interactions, + ); + + self.one_body_interaction_groups.clear_groups(); + self.one_body_interaction_groups.group_joints( + island_id, + islands, + bodies, + impulse_joints, + &self.one_body_interactions, + ); + // NOTE: uncomment this do disable SIMD joint resolution. + // self.interaction_groups + // .nongrouped_interactions + // .append(&mut self.interaction_groups.simd_interactions); + // self.one_body_interaction_groups + // .nongrouped_interactions + // .append(&mut self.one_body_interaction_groups.simd_interactions); + + let mut j_id = 0; + self.compute_joint_constraints(bodies, impulse_joints); + #[cfg(feature = "simd-is-enabled")] + { + self.simd_compute_joint_constraints(bodies, impulse_joints); + } + self.compute_generic_joint_constraints(bodies, multibody_joints, impulse_joints, &mut j_id); + + self.compute_joint_one_body_constraints(bodies, impulse_joints); + #[cfg(feature = "simd-is-enabled")] + { + self.simd_compute_joint_one_body_constraints(bodies, impulse_joints); + } + self.compute_generic_one_body_joint_constraints( + island_id, + islands, + bodies, + multibody_joints, + impulse_joints, + &mut j_id, + ); + } + + fn compute_joint_one_body_constraints( + &mut self, + bodies: &RigidBodySet, + joints_all: &[JointGraphEdge], + ) { + let total_num_builders = self + .one_body_interaction_groups + .nongrouped_interactions + .len(); + + unsafe { + reset_buffer( + &mut self.velocity_one_body_constraints_builder, + total_num_builders, + ); + } + + let mut num_constraints = 0; + for (joint_i, builder) in self + .one_body_interaction_groups + .nongrouped_interactions + .iter() + .zip(self.velocity_one_body_constraints_builder.iter_mut()) + { + let joint = &joints_all[*joint_i].weight; + JointOneBodyConstraintBuilder::generate( + joint, + bodies, + *joint_i, + builder, + &mut num_constraints, + ); + } + + unsafe { + reset_buffer(&mut self.velocity_one_body_constraints, num_constraints); + } + } + + #[cfg(feature = "simd-is-enabled")] + fn simd_compute_joint_one_body_constraints( + &mut self, + bodies: &RigidBodySet, + joints_all: &[JointGraphEdge], + ) { + let total_num_builders = + self.one_body_interaction_groups.simd_interactions.len() / SIMD_WIDTH; + + unsafe { + reset_buffer( + &mut self.simd_velocity_one_body_constraints_builder, + total_num_builders, + ); + } + + let mut num_constraints = 0; + for (joints_i, builder) in self + .one_body_interaction_groups + .simd_interactions + .chunks_exact(SIMD_WIDTH) + .zip(self.simd_velocity_one_body_constraints_builder.iter_mut()) + { + let joints_id = gather![|ii| joints_i[ii]]; + let impulse_joints = gather![|ii| &joints_all[joints_i[ii]].weight]; + JointOneBodyConstraintBuilderSimd::generate( + impulse_joints, + bodies, + joints_id, + builder, + &mut num_constraints, + ); + } + + unsafe { + reset_buffer( + &mut self.simd_velocity_one_body_constraints, + num_constraints, + ); + } + } + + fn compute_joint_constraints(&mut self, bodies: &RigidBodySet, joints_all: &[JointGraphEdge]) { + let total_num_builders = self.interaction_groups.nongrouped_interactions.len(); + + unsafe { + reset_buffer(&mut self.velocity_constraints_builder, total_num_builders); + } + + let mut num_constraints = 0; + for (joint_i, builder) in self + .interaction_groups + .nongrouped_interactions + .iter() + .zip(self.velocity_constraints_builder.iter_mut()) + { + let joint = &joints_all[*joint_i].weight; + JointTwoBodyConstraintBuilder::generate( + joint, + bodies, + *joint_i, + builder, + &mut num_constraints, + ); + } + + unsafe { + reset_buffer(&mut self.velocity_constraints, num_constraints); + } + } + + fn compute_generic_joint_constraints( + &mut self, + bodies: &RigidBodySet, + multibodies: &MultibodyJointSet, + joints_all: &[JointGraphEdge], + j_id: &mut usize, + ) { + let total_num_builders = self.generic_two_body_interactions.len(); + self.generic_velocity_constraints_builder.resize( + total_num_builders, + JointGenericTwoBodyConstraintBuilder::invalid(), + ); + + let mut num_constraints = 0; + for (joint_i, builder) in self + .generic_two_body_interactions + .iter() + .zip(self.generic_velocity_constraints_builder.iter_mut()) + { + let joint = &joints_all[*joint_i].weight; + JointGenericTwoBodyConstraintBuilder::generate( + *joint_i, + joint, + bodies, + multibodies, + builder, + j_id, + &mut self.generic_jacobians, + &mut num_constraints, + ); + } + + self.generic_velocity_constraints + .resize(num_constraints, JointGenericTwoBodyConstraint::invalid()); + } + + fn compute_generic_one_body_joint_constraints( + &mut self, + island_id: usize, + islands: &IslandManager, + bodies: &RigidBodySet, + multibodies: &MultibodyJointSet, + joints_all: &[JointGraphEdge], + j_id: &mut usize, + ) { + let mut total_num_builders = self.generic_one_body_interactions.len(); + + for handle in islands.active_island(island_id) { + if let Some(link_id) = multibodies.rigid_body_link(*handle) { + if JointGenericVelocityOneBodyInternalConstraintBuilder::num_constraints( + multibodies, + link_id, + ) > 0 + { + total_num_builders += 1; + } + } + } + + self.generic_velocity_one_body_constraints_builder.resize( + total_num_builders, + JointGenericOneBodyConstraintBuilder::invalid(), + ); + + let mut num_constraints = 0; + for (joint_i, builder) in self.generic_one_body_interactions.iter().zip( + self.generic_velocity_one_body_constraints_builder + .iter_mut(), + ) { + let joint = &joints_all[*joint_i].weight; + JointGenericVelocityOneBodyExternalConstraintBuilder::generate( + *joint_i, + joint, + bodies, + multibodies, + builder, + j_id, + &mut self.generic_jacobians, + &mut num_constraints, + ); + } + + let mut curr_builder = self.generic_one_body_interactions.len(); + for handle in islands.active_island(island_id) { + if curr_builder >= self.generic_velocity_one_body_constraints_builder.len() { + break; // No more builder need to be generated. + } + + if let Some(link_id) = multibodies.rigid_body_link(*handle) { + let prev_num_constraints = num_constraints; + JointGenericVelocityOneBodyInternalConstraintBuilder::generate( + multibodies, + link_id, + &mut self.generic_velocity_one_body_constraints_builder[curr_builder], + j_id, + &mut self.generic_jacobians, + &mut num_constraints, + ); + if num_constraints != prev_num_constraints { + curr_builder += 1; + } + } + } + + self.generic_velocity_one_body_constraints + .resize(num_constraints, JointGenericOneBodyConstraint::invalid()); + } + + #[cfg(feature = "simd-is-enabled")] + fn simd_compute_joint_constraints( + &mut self, + bodies: &RigidBodySet, + joints_all: &[JointGraphEdge], + ) { + let total_num_builders = self.interaction_groups.simd_interactions.len() / SIMD_WIDTH; + + unsafe { + reset_buffer( + &mut self.simd_velocity_constraints_builder, + total_num_builders, + ); + } + + let mut num_constraints = 0; + for (joints_i, builder) in self + .interaction_groups + .simd_interactions + .chunks_exact(SIMD_WIDTH) + .zip(self.simd_velocity_constraints_builder.iter_mut()) + { + let joints_id = gather![|ii| joints_i[ii]]; + let impulse_joints = gather![|ii| &joints_all[joints_i[ii]].weight]; + JointTwoBodyConstraintBuilderSimd::generate( + impulse_joints, + bodies, + joints_id, + builder, + &mut num_constraints, + ); + } + + unsafe { + reset_buffer(&mut self.simd_velocity_constraints, num_constraints); + } + } + + pub fn solve( + &mut self, + solver_vels: &mut [SolverVel], + generic_solver_vels: &mut DVector, + ) { + let (jac, constraints) = self.iter_constraints_mut(); + for mut c in constraints { + c.solve(jac, solver_vels, generic_solver_vels); + } + } + + pub fn solve_wo_bias( + &mut self, + solver_vels: &mut [SolverVel], + generic_solver_vels: &mut DVector, + ) { + let (jac, constraints) = self.iter_constraints_mut(); + for mut c in constraints { + c.remove_bias(); + c.solve(jac, solver_vels, generic_solver_vels); + } + } + + pub fn writeback_impulses(&mut self, joints_all: &mut [JointGraphEdge]) { + let (_, constraints) = self.iter_constraints_mut(); + for mut c in constraints { + c.writeback_impulses(joints_all); + } + } + + pub fn update( + &mut self, + params: &IntegrationParameters, + multibodies: &MultibodyJointSet, + solver_bodies: &[SolverBody], + ) { + for builder in &mut self.generic_velocity_constraints_builder { + builder.update( + ¶ms, + multibodies, + solver_bodies, + &mut self.generic_jacobians, + &mut self.generic_velocity_constraints, + ); + } + + for builder in &mut self.generic_velocity_one_body_constraints_builder { + builder.update( + ¶ms, + multibodies, + solver_bodies, + &mut self.generic_jacobians, + &mut self.generic_velocity_one_body_constraints, + ); + } + + for builder in &mut self.velocity_constraints_builder { + builder.update(¶ms, solver_bodies, &mut self.velocity_constraints); + } + + #[cfg(feature = "simd-is-enabled")] + for builder in &mut self.simd_velocity_constraints_builder { + builder.update(¶ms, solver_bodies, &mut self.simd_velocity_constraints); + } + + for builder in &mut self.velocity_one_body_constraints_builder { + builder.update( + ¶ms, + solver_bodies, + &mut self.velocity_one_body_constraints, + ); + } + + #[cfg(feature = "simd-is-enabled")] + for builder in &mut self.simd_velocity_one_body_constraints_builder { + builder.update( + ¶ms, + solver_bodies, + &mut self.simd_velocity_one_body_constraints, + ); + } + } +} diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_generic_constraint.rs similarity index 73% rename from src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs rename to src/dynamics/solver/joint_constraint/joint_generic_constraint.rs index de5bbe640..d500b48d6 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_constraint.rs @@ -1,17 +1,19 @@ -use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId; -use crate::dynamics::solver::joint_constraint::{JointVelocityConstraintBuilder, SolverBody}; -use crate::dynamics::solver::DeltaVel; +use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{ + JointFixedSolverBody, WritebackId, +}; +use crate::dynamics::solver::joint_constraint::{JointSolverBody, JointTwoBodyConstraintHelper}; +use crate::dynamics::solver::SolverVel; use crate::dynamics::{GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, Multibody}; use crate::math::{Isometry, Real, DIM}; use crate::prelude::SPATIAL_DIM; use na::{DVector, DVectorView, DVectorViewMut}; #[derive(Debug, Copy, Clone)] -pub struct JointGenericVelocityConstraint { +pub struct JointGenericTwoBodyConstraint { pub is_rigid_body1: bool, pub is_rigid_body2: bool, - pub mj_lambda1: usize, - pub mj_lambda2: usize, + pub solver_vel1: usize, + pub solver_vel2: usize, pub ndofs1: usize, pub j_id1: usize, @@ -31,31 +33,31 @@ pub struct JointGenericVelocityConstraint { pub writeback_id: WritebackId, } -impl Default for JointGenericVelocityConstraint { +impl Default for JointGenericTwoBodyConstraint { fn default() -> Self { - JointGenericVelocityConstraint::invalid() + JointGenericTwoBodyConstraint::invalid() } } -impl JointGenericVelocityConstraint { +impl JointGenericTwoBodyConstraint { pub fn invalid() -> Self { Self { is_rigid_body1: false, is_rigid_body2: false, - mj_lambda1: 0, - mj_lambda2: 0, - ndofs1: 0, - j_id1: 0, - ndofs2: 0, - j_id2: 0, - joint_id: 0, + solver_vel1: usize::MAX, + solver_vel2: usize::MAX, + ndofs1: usize::MAX, + j_id1: usize::MAX, + ndofs2: usize::MAX, + j_id2: usize::MAX, + joint_id: usize::MAX, impulse: 0.0, impulse_bounds: [-Real::MAX, Real::MAX], - inv_lhs: 0.0, - rhs: 0.0, - rhs_wo_bias: 0.0, - cfm_coeff: 0.0, - cfm_gain: 0.0, + inv_lhs: Real::MAX, + rhs: Real::MAX, + rhs_wo_bias: Real::MAX, + cfm_coeff: Real::MAX, + cfm_gain: Real::MAX, writeback_id: WritebackId::Dof(0), } } @@ -63,8 +65,8 @@ impl JointGenericVelocityConstraint { pub fn lock_axes( params: &IntegrationParameters, joint_id: JointIndex, - body1: &SolverBody, - body2: &SolverBody, + body1: &JointSolverBody, + body2: &JointSolverBody, mb1: Option<(&Multibody, usize)>, mb2: Option<(&Multibody, usize)>, frame1: &Isometry, @@ -79,7 +81,7 @@ impl JointGenericVelocityConstraint { let motor_axes = joint.motor_axes.bits(); let limit_axes = joint.limit_axes.bits(); - let builder = JointVelocityConstraintBuilder::new( + let builder = JointTwoBodyConstraintHelper::new( frame1, frame2, &body1.world_com, @@ -91,7 +93,6 @@ impl JointGenericVelocityConstraint { for i in DIM..SPATIAL_DIM { if motor_axes & (1 << i) != 0 { out[len] = builder.motor_angular_generic( - params, jacobians, j_id, joint_id, @@ -109,7 +110,6 @@ impl JointGenericVelocityConstraint { for i in 0..DIM { if motor_axes & (1 << i) != 0 { out[len] = builder.motor_linear_generic( - params, jacobians, j_id, joint_id, @@ -125,10 +125,7 @@ impl JointGenericVelocityConstraint { len += 1; } } - JointVelocityConstraintBuilder::finalize_generic_constraints( - jacobians, - &mut out[start..len], - ); + JointTwoBodyConstraintHelper::finalize_generic_constraints(jacobians, &mut out[start..len]); let start = len; for i in DIM..SPATIAL_DIM { @@ -203,10 +200,7 @@ impl JointGenericVelocityConstraint { } } - JointVelocityConstraintBuilder::finalize_generic_constraints( - jacobians, - &mut out[start..len], - ); + JointTwoBodyConstraintHelper::finalize_generic_constraints(jacobians, &mut out[start..len]); len } @@ -218,69 +212,69 @@ impl JointGenericVelocityConstraint { self.j_id2 + self.ndofs2 } - fn mj_lambda1<'a>( + fn solver_vel1<'a>( &self, - mj_lambdas: &'a [DeltaVel], - generic_mj_lambdas: &'a DVector, + solver_vels: &'a [SolverVel], + generic_solver_vels: &'a DVector, ) -> DVectorView<'a, Real> { if self.is_rigid_body1 { - mj_lambdas[self.mj_lambda1].as_vector_slice() + solver_vels[self.solver_vel1].as_vector_slice() } else { - generic_mj_lambdas.rows(self.mj_lambda1, self.ndofs1) + generic_solver_vels.rows(self.solver_vel1, self.ndofs1) } } - fn mj_lambda1_mut<'a>( + fn solver_vel1_mut<'a>( &self, - mj_lambdas: &'a mut [DeltaVel], - generic_mj_lambdas: &'a mut DVector, + solver_vels: &'a mut [SolverVel], + generic_solver_vels: &'a mut DVector, ) -> DVectorViewMut<'a, Real> { if self.is_rigid_body1 { - mj_lambdas[self.mj_lambda1].as_vector_slice_mut() + solver_vels[self.solver_vel1].as_vector_slice_mut() } else { - generic_mj_lambdas.rows_mut(self.mj_lambda1, self.ndofs1) + generic_solver_vels.rows_mut(self.solver_vel1, self.ndofs1) } } - fn mj_lambda2<'a>( + fn solver_vel2<'a>( &self, - mj_lambdas: &'a [DeltaVel], - generic_mj_lambdas: &'a DVector, + solver_vels: &'a [SolverVel], + generic_solver_vels: &'a DVector, ) -> DVectorView<'a, Real> { if self.is_rigid_body2 { - mj_lambdas[self.mj_lambda2].as_vector_slice() + solver_vels[self.solver_vel2].as_vector_slice() } else { - generic_mj_lambdas.rows(self.mj_lambda2, self.ndofs2) + generic_solver_vels.rows(self.solver_vel2, self.ndofs2) } } - fn mj_lambda2_mut<'a>( + fn solver_vel2_mut<'a>( &self, - mj_lambdas: &'a mut [DeltaVel], - generic_mj_lambdas: &'a mut DVector, + solver_vels: &'a mut [SolverVel], + generic_solver_vels: &'a mut DVector, ) -> DVectorViewMut<'a, Real> { if self.is_rigid_body2 { - mj_lambdas[self.mj_lambda2].as_vector_slice_mut() + solver_vels[self.solver_vel2].as_vector_slice_mut() } else { - generic_mj_lambdas.rows_mut(self.mj_lambda2, self.ndofs2) + generic_solver_vels.rows_mut(self.solver_vel2, self.ndofs2) } } pub fn solve( &mut self, jacobians: &DVector, - mj_lambdas: &mut [DeltaVel], - generic_mj_lambdas: &mut DVector, + solver_vels: &mut [SolverVel], + generic_solver_vels: &mut DVector, ) { let jacobians = jacobians.as_slice(); - let mj_lambda1 = self.mj_lambda1(mj_lambdas, generic_mj_lambdas); + let solver_vel1 = self.solver_vel1(solver_vels, generic_solver_vels); let j1 = DVectorView::from_slice(&jacobians[self.j_id1..], self.ndofs1); - let vel1 = j1.dot(&mj_lambda1); + let vel1 = j1.dot(&solver_vel1); - let mj_lambda2 = self.mj_lambda2(mj_lambdas, generic_mj_lambdas); + let solver_vel2 = self.solver_vel2(solver_vels, generic_solver_vels); let j2 = DVectorView::from_slice(&jacobians[self.j_id2..], self.ndofs2); - let vel2 = j2.dot(&mj_lambda2); + let vel2 = j2.dot(&solver_vel2); let dvel = self.rhs + (vel2 - vel1); let total_impulse = na::clamp( @@ -291,13 +285,13 @@ impl JointGenericVelocityConstraint { let delta_impulse = total_impulse - self.impulse; self.impulse = total_impulse; - let mut mj_lambda1 = self.mj_lambda1_mut(mj_lambdas, generic_mj_lambdas); + let mut solver_vel1 = self.solver_vel1_mut(solver_vels, generic_solver_vels); let wj1 = DVectorView::from_slice(&jacobians[self.wj_id1()..], self.ndofs1); - mj_lambda1.axpy(delta_impulse, &wj1, 1.0); + solver_vel1.axpy(delta_impulse, &wj1, 1.0); - let mut mj_lambda2 = self.mj_lambda2_mut(mj_lambdas, generic_mj_lambdas); + let mut solver_vel2 = self.solver_vel2_mut(solver_vels, generic_solver_vels); let wj2 = DVectorView::from_slice(&jacobians[self.wj_id2()..], self.ndofs2); - mj_lambda2.axpy(-delta_impulse, &wj2, 1.0); + solver_vel2.axpy(-delta_impulse, &wj2, 1.0); } pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { @@ -315,8 +309,8 @@ impl JointGenericVelocityConstraint { } #[derive(Debug, Copy, Clone)] -pub struct JointGenericVelocityGroundConstraint { - pub mj_lambda2: usize, +pub struct JointGenericOneBodyConstraint { + pub solver_vel2: usize, pub ndofs2: usize, pub j_id2: usize, @@ -333,16 +327,16 @@ pub struct JointGenericVelocityGroundConstraint { pub writeback_id: WritebackId, } -impl Default for JointGenericVelocityGroundConstraint { +impl Default for JointGenericOneBodyConstraint { fn default() -> Self { - JointGenericVelocityGroundConstraint::invalid() + JointGenericOneBodyConstraint::invalid() } } -impl JointGenericVelocityGroundConstraint { +impl JointGenericOneBodyConstraint { pub fn invalid() -> Self { Self { - mj_lambda2: crate::INVALID_USIZE, + solver_vel2: crate::INVALID_USIZE, ndofs2: 0, j_id2: crate::INVALID_USIZE, joint_id: 0, @@ -360,8 +354,8 @@ impl JointGenericVelocityGroundConstraint { pub fn lock_axes( params: &IntegrationParameters, joint_id: JointIndex, - body1: &SolverBody, - body2: &SolverBody, + body1: &JointFixedSolverBody, + body2: &JointSolverBody, mb2: (&Multibody, usize), frame1: &Isometry, frame2: &Isometry, @@ -375,7 +369,7 @@ impl JointGenericVelocityGroundConstraint { let motor_axes = joint.motor_axes.bits(); let limit_axes = joint.limit_axes.bits(); - let builder = JointVelocityConstraintBuilder::new( + let builder = JointTwoBodyConstraintHelper::new( frame1, frame2, &body1.world_com, @@ -386,13 +380,11 @@ impl JointGenericVelocityGroundConstraint { let start = len; for i in DIM..SPATIAL_DIM { if motor_axes & (1 << i) != 0 { - out[len] = builder.motor_angular_generic_ground( - params, + out[len] = builder.motor_angular_generic_one_body( jacobians, j_id, joint_id, body1, - body2, mb2, i - DIM, &joint.motors[i].motor_params(params.dt), @@ -404,13 +396,11 @@ impl JointGenericVelocityGroundConstraint { for i in 0..DIM { if motor_axes & (1 << i) != 0 { - out[len] = builder.motor_linear_generic_ground( - params, + out[len] = builder.motor_linear_generic_one_body( jacobians, j_id, joint_id, body1, - body2, mb2, // locked_ang_axes, i, @@ -421,7 +411,7 @@ impl JointGenericVelocityGroundConstraint { } } - JointVelocityConstraintBuilder::finalize_generic_constraints_ground( + JointTwoBodyConstraintHelper::finalize_generic_constraints_one_body( jacobians, &mut out[start..len], ); @@ -429,7 +419,7 @@ impl JointGenericVelocityGroundConstraint { let start = len; for i in DIM..SPATIAL_DIM { if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_angular_generic_ground( + out[len] = builder.lock_angular_generic_one_body( params, jacobians, j_id, @@ -444,7 +434,7 @@ impl JointGenericVelocityGroundConstraint { } for i in 0..DIM { if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_linear_generic_ground( + out[len] = builder.lock_linear_generic_one_body( params, jacobians, j_id, @@ -460,7 +450,7 @@ impl JointGenericVelocityGroundConstraint { for i in DIM..SPATIAL_DIM { if limit_axes & (1 << i) != 0 { - out[len] = builder.limit_angular_generic_ground( + out[len] = builder.limit_angular_generic_one_body( params, jacobians, j_id, @@ -476,7 +466,7 @@ impl JointGenericVelocityGroundConstraint { } for i in 0..DIM { if limit_axes & (1 << i) != 0 { - out[len] = builder.limit_linear_generic_ground( + out[len] = builder.limit_linear_generic_one_body( params, jacobians, j_id, @@ -491,7 +481,7 @@ impl JointGenericVelocityGroundConstraint { } } - JointVelocityConstraintBuilder::finalize_generic_constraints_ground( + JointTwoBodyConstraintHelper::finalize_generic_constraints_one_body( jacobians, &mut out[start..len], ); @@ -502,33 +492,33 @@ impl JointGenericVelocityGroundConstraint { self.j_id2 + self.ndofs2 } - fn mj_lambda2<'a>( + fn solver_vel2<'a>( &self, - _mj_lambdas: &'a [DeltaVel], - generic_mj_lambdas: &'a DVector, + _solver_vels: &'a [SolverVel], + generic_solver_vels: &'a DVector, ) -> DVectorView<'a, Real> { - generic_mj_lambdas.rows(self.mj_lambda2, self.ndofs2) + generic_solver_vels.rows(self.solver_vel2, self.ndofs2) } - fn mj_lambda2_mut<'a>( + fn solver_vel2_mut<'a>( &self, - _mj_lambdas: &'a mut [DeltaVel], - generic_mj_lambdas: &'a mut DVector, + _solver_vels: &'a mut [SolverVel], + generic_solver_vels: &'a mut DVector, ) -> DVectorViewMut<'a, Real> { - generic_mj_lambdas.rows_mut(self.mj_lambda2, self.ndofs2) + generic_solver_vels.rows_mut(self.solver_vel2, self.ndofs2) } pub fn solve( &mut self, jacobians: &DVector, - mj_lambdas: &mut [DeltaVel], - generic_mj_lambdas: &mut DVector, + solver_vels: &mut [SolverVel], + generic_solver_vels: &mut DVector, ) { let jacobians = jacobians.as_slice(); - let mj_lambda2 = self.mj_lambda2(mj_lambdas, generic_mj_lambdas); + let solver_vel2 = self.solver_vel2(solver_vels, generic_solver_vels); let j2 = DVectorView::from_slice(&jacobians[self.j_id2..], self.ndofs2); - let vel2 = j2.dot(&mj_lambda2); + let vel2 = j2.dot(&solver_vel2); let dvel = self.rhs + vel2; let total_impulse = na::clamp( @@ -539,9 +529,9 @@ impl JointGenericVelocityGroundConstraint { let delta_impulse = total_impulse - self.impulse; self.impulse = total_impulse; - let mut mj_lambda2 = self.mj_lambda2_mut(mj_lambdas, generic_mj_lambdas); + let mut solver_vel2 = self.solver_vel2_mut(solver_vels, generic_solver_vels); let wj2 = DVectorView::from_slice(&jacobians[self.wj_id2()..], self.ndofs2); - mj_lambda2.axpy(-delta_impulse, &wj2, 1.0); + solver_vel2.axpy(-delta_impulse, &wj2, 1.0); } pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs similarity index 58% rename from src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs rename to src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs index c16151d8c..552396221 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs @@ -1,29 +1,459 @@ -use crate::dynamics::solver::joint_constraint::joint_generic_velocity_constraint::{ - JointGenericVelocityConstraint, JointGenericVelocityGroundConstraint, +use crate::dynamics::solver::joint_constraint::joint_generic_constraint::{ + JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint, }; -use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId; -use crate::dynamics::solver::joint_constraint::{JointVelocityConstraintBuilder, SolverBody}; +use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{ + JointFixedSolverBody, WritebackId, +}; +use crate::dynamics::solver::joint_constraint::{JointSolverBody, JointTwoBodyConstraintHelper}; use crate::dynamics::solver::MotorParameters; -use crate::dynamics::{IntegrationParameters, JointIndex, Multibody}; +use crate::dynamics::{ + GenericJoint, ImpulseJoint, IntegrationParameters, JointIndex, Multibody, MultibodyJointSet, + MultibodyLinkId, RigidBodySet, +}; use crate::math::{Real, Vector, ANG_DIM, DIM, SPATIAL_DIM}; use crate::utils; use crate::utils::IndexMut2; -use crate::utils::WDot; +use crate::utils::SimdDot; use na::{DVector, SVector}; +use crate::dynamics::solver::solver_body::SolverBody; +use crate::dynamics::solver::ConstraintsCounts; #[cfg(feature = "dim3")] -use crate::utils::WAngularInertia; +use crate::utils::SimdAngularInertia; #[cfg(feature = "dim2")] use na::Vector1; +use parry::math::Isometry; + +#[derive(Copy, Clone)] +enum LinkOrBody { + Link(MultibodyLinkId), + Body(usize), +} + +#[derive(Copy, Clone)] +pub struct JointGenericTwoBodyConstraintBuilder { + link1: LinkOrBody, + link2: LinkOrBody, + joint_id: JointIndex, + joint: GenericJoint, + j_id: usize, + // These are solver body for both joints, except that + // the world_com is actually in local-space. + local_body1: JointSolverBody, + local_body2: JointSolverBody, + multibodies_ndof: usize, + constraint_id: usize, +} + +impl JointGenericTwoBodyConstraintBuilder { + pub fn invalid() -> Self { + Self { + link1: LinkOrBody::Body(usize::MAX), + link2: LinkOrBody::Body(usize::MAX), + joint_id: JointIndex::MAX, + joint: GenericJoint::default(), + j_id: usize::MAX, + local_body1: JointSolverBody::invalid(), + local_body2: JointSolverBody::invalid(), + multibodies_ndof: usize::MAX, + constraint_id: usize::MAX, + } + } + + pub fn generate( + joint_id: JointIndex, + joint: &ImpulseJoint, + bodies: &RigidBodySet, + multibodies: &MultibodyJointSet, + out_builder: &mut Self, + j_id: &mut usize, + jacobians: &mut DVector, + out_constraint_id: &mut usize, + ) { + let starting_j_id = *j_id; + let rb1 = &bodies[joint.body1]; + let rb2 = &bodies[joint.body2]; + + let local_body1 = JointSolverBody { + im: rb1.mprops.effective_inv_mass, + sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt, + world_com: rb1.mprops.local_mprops.local_com, + solver_vel: [rb1.ids.active_set_offset], + }; + let local_body2 = JointSolverBody { + im: rb2.mprops.effective_inv_mass, + sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt, + world_com: rb2.mprops.local_mprops.local_com, + solver_vel: [rb2.ids.active_set_offset], + }; + + let mut multibodies_ndof = 0; + let link1 = match multibodies.rigid_body_link(joint.body1) { + Some(link) => { + multibodies_ndof += multibodies[link.multibody].ndofs(); + LinkOrBody::Link(*link) + } + None => { + multibodies_ndof += SPATIAL_DIM; + LinkOrBody::Body(rb2.ids.active_set_offset) + } + }; + let link2 = match multibodies.rigid_body_link(joint.body2) { + Some(link) => { + multibodies_ndof += multibodies[link.multibody].ndofs(); + LinkOrBody::Link(*link) + } + None => { + multibodies_ndof += SPATIAL_DIM; + LinkOrBody::Body(rb2.ids.active_set_offset) + } + }; + + if multibodies_ndof == 0 { + // Both multibodies are fixed, don’t generate any constraint. + out_builder.multibodies_ndof = multibodies_ndof; + return; + } + + // For each solver contact we generate up to SPATIAL_DIM constraints, and each + // constraints appends the multibodies jacobian and weighted jacobians. + // Also note that for impulse_joints, the rigid-bodies will also add their jacobians + // to the generic DVector. + // TODO: is this count correct when we take both motors and limits into account? + let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM; + + // TODO: use a more precise increment. + *j_id += multibodies_ndof * 2 * SPATIAL_DIM; + + if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") { + jacobians.resize_vertically_mut(required_jacobian_len, 0.0); + } -impl SolverBody { + *out_builder = Self { + link1, + link2, + joint_id, + joint: joint.data, + j_id: starting_j_id, + local_body1, + local_body2, + multibodies_ndof, + constraint_id: *out_constraint_id, + }; + + *out_constraint_id += ConstraintsCounts::from_joint(joint).num_constraints; + } + + pub fn update( + &self, + params: &IntegrationParameters, + multibodies: &MultibodyJointSet, + bodies: &[SolverBody], + jacobians: &mut DVector, + out: &mut [JointGenericTwoBodyConstraint], + ) { + if self.multibodies_ndof == 0 { + // The joint is between two static bodies, no constraint needed. + return; + } + + // NOTE: right now, the "update", is basically reconstructing all the + // constraints. Could we make this more incremental? + let pos1; + let pos2; + let mb1; + let mb2; + + match self.link1 { + LinkOrBody::Link(link) => { + let mb = &multibodies[link.multibody]; + pos1 = &mb.link(link.id).unwrap().local_to_world; + mb1 = Some((mb, link.id)); + } + LinkOrBody::Body(body1) => { + pos1 = &bodies[body1].position; + mb1 = None; + } + }; + match self.link2 { + LinkOrBody::Link(link) => { + let mb = &multibodies[link.multibody]; + pos2 = &mb.link(link.id).unwrap().local_to_world; + mb2 = Some((mb, link.id)); + } + LinkOrBody::Body(body2) => { + pos2 = &bodies[body2].position; + mb2 = None; + } + }; + + let frame1 = pos1 * self.joint.local_frame1; + let frame2 = pos2 * self.joint.local_frame2; + + let joint_body1 = JointSolverBody { + world_com: pos1 * self.local_body1.world_com, // the world_com was stored in local-space. + ..self.local_body1 + }; + let joint_body2 = JointSolverBody { + world_com: pos2 * self.local_body2.world_com, // the world_com was stored in local-space. + ..self.local_body2 + }; + + let mut j_id = self.j_id; + + JointGenericTwoBodyConstraint::lock_axes( + params, + self.joint_id, + &joint_body1, + &joint_body2, + mb1, + mb2, + &frame1, + &frame2, + &self.joint, + jacobians, + &mut j_id, + &mut out[self.constraint_id..], + ); + } +} + +#[derive(Copy, Clone)] +pub enum JointGenericOneBodyConstraintBuilder { + Internal(JointGenericVelocityOneBodyInternalConstraintBuilder), + External(JointGenericVelocityOneBodyExternalConstraintBuilder), + Empty, +} + +#[derive(Copy, Clone)] +pub struct JointGenericVelocityOneBodyInternalConstraintBuilder { + link: MultibodyLinkId, + j_id: usize, + constraint_id: usize, +} + +impl JointGenericOneBodyConstraintBuilder { + pub fn invalid() -> Self { + Self::Empty + } + + pub fn update( + &self, + params: &IntegrationParameters, + multibodies: &MultibodyJointSet, + bodies: &[SolverBody], + jacobians: &mut DVector, + out: &mut [JointGenericOneBodyConstraint], + ) { + match self { + Self::Empty => {} + Self::Internal(builder) => builder.update(params, multibodies, jacobians, out), + Self::External(builder) => builder.update(params, multibodies, bodies, jacobians, out), + } + } +} + +impl JointGenericVelocityOneBodyInternalConstraintBuilder { + pub fn num_constraints(multibodies: &MultibodyJointSet, link_id: &MultibodyLinkId) -> usize { + let multibody = &multibodies[link_id.multibody]; + let link = multibody.link(link_id.id).unwrap(); + link.joint().num_velocity_constraints() + } + + pub fn generate( + multibodies: &MultibodyJointSet, + link_id: &MultibodyLinkId, + out_builder: &mut JointGenericOneBodyConstraintBuilder, + j_id: &mut usize, + jacobians: &mut DVector, + out_constraint_id: &mut usize, + ) { + let multibody = &multibodies[link_id.multibody]; + let link = multibody.link(link_id.id).unwrap(); + let num_constraints = link.joint().num_velocity_constraints(); + + if num_constraints == 0 { + return; + } + + *out_builder = JointGenericOneBodyConstraintBuilder::Internal(Self { + link: *link_id, + j_id: *j_id, + constraint_id: *out_constraint_id, + }); + + *j_id += num_constraints * multibody.ndofs() * 2; + if jacobians.nrows() < *j_id { + jacobians.resize_vertically_mut(*j_id, 0.0); + } + + *out_constraint_id += num_constraints; + } + + pub fn update( + &self, + params: &IntegrationParameters, + multibodies: &MultibodyJointSet, + jacobians: &mut DVector, + out: &mut [JointGenericOneBodyConstraint], + ) { + let mb = &multibodies[self.link.multibody]; + let link = mb.link(self.link.id).unwrap(); + link.joint().velocity_constraints( + params, + mb, + link, + self.j_id, + jacobians, + &mut out[self.constraint_id..], + ); + } +} + +#[derive(Copy, Clone)] +pub struct JointGenericVelocityOneBodyExternalConstraintBuilder { + body1: JointFixedSolverBody, + frame1: Isometry, + link2: MultibodyLinkId, + joint_id: JointIndex, + joint: GenericJoint, + j_id: usize, + flipped: bool, + constraint_id: usize, + // These are solver body for both joints, except that + // the world_com is actually in local-space. + local_body2: JointSolverBody, +} + +impl JointGenericVelocityOneBodyExternalConstraintBuilder { + pub fn generate( + joint_id: JointIndex, + joint: &ImpulseJoint, + bodies: &RigidBodySet, + multibodies: &MultibodyJointSet, + out_builder: &mut JointGenericOneBodyConstraintBuilder, + j_id: &mut usize, + jacobians: &mut DVector, + out_constraint_id: &mut usize, + ) { + let mut handle1 = joint.body1; + let mut handle2 = joint.body2; + let flipped = !bodies[handle2].is_dynamic(); + + let local_frame1 = if flipped { + std::mem::swap(&mut handle1, &mut handle2); + joint.data.local_frame2 + } else { + joint.data.local_frame1 + }; + + let rb1 = &bodies[handle1]; + let rb2 = &bodies[handle2]; + + let frame1 = rb1.pos.position * local_frame1; + + let starting_j_id = *j_id; + + let body1 = JointFixedSolverBody { + linvel: rb1.vels.linvel, + angvel: rb1.vels.angvel, + world_com: rb1.mprops.world_com, + }; + let local_body2 = JointSolverBody { + im: rb2.mprops.effective_inv_mass, + sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt, + world_com: rb2.mprops.local_mprops.local_com, + solver_vel: [rb2.ids.active_set_offset], + }; + + let link2 = *multibodies.rigid_body_link(handle2).unwrap(); + let mb2 = &multibodies[link2.multibody]; + let multibodies_ndof = mb2.ndofs(); + + if multibodies_ndof == 0 { + // The multibody is fixed, don’t generate any constraint. + *out_builder = JointGenericOneBodyConstraintBuilder::Empty; + return; + } + + // For each solver contact we generate up to SPATIAL_DIM constraints, and each + // constraints appends the multibodies jacobian and weighted jacobians. + // Also note that for impulse_joints, the rigid-bodies will also add their jacobians + // to the generic DVector. + // TODO: is this count correct when we take both motors and limits into account? + let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM; + // TODO: use a more precise increment. + *j_id += multibodies_ndof * 2 * SPATIAL_DIM; + + if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") { + jacobians.resize_vertically_mut(required_jacobian_len, 0.0); + } + + *out_builder = JointGenericOneBodyConstraintBuilder::External(Self { + body1, + link2, + joint_id, + joint: joint.data, + j_id: starting_j_id, + frame1, + local_body2, + flipped, + constraint_id: *out_constraint_id, + }); + + *out_constraint_id += ConstraintsCounts::from_joint(joint).num_constraints; + } + + pub fn update( + &self, + params: &IntegrationParameters, + multibodies: &MultibodyJointSet, + _bodies: &[SolverBody], + jacobians: &mut DVector, + out: &mut [JointGenericOneBodyConstraint], + ) { + // NOTE: right now, the "update", is basically reconstructing all the + // constraints. Could we make this more incremental? + let mb2 = &multibodies[self.link2.multibody]; + let pos2 = &mb2.link(self.link2.id).unwrap().local_to_world; + let frame2 = pos2 + * if self.flipped { + self.joint.local_frame1 + } else { + self.joint.local_frame2 + }; + + let joint_body2 = JointSolverBody { + world_com: pos2 * self.local_body2.world_com, // the world_com was stored in local-space. + ..self.local_body2 + }; + + let mut j_id = self.j_id; + + JointGenericOneBodyConstraint::lock_axes( + params, + self.joint_id, + &self.body1, + &joint_body2, + (mb2, self.link2.id), + &self.frame1, + &frame2, + &self.joint, + jacobians, + &mut j_id, + &mut out[self.constraint_id..], + ); + } +} + +impl JointSolverBody { pub fn fill_jacobians( &self, unit_force: Vector, unit_torque: SVector, j_id: &mut usize, jacobians: &mut DVector, - ) -> Real { + ) { let wj_id = *j_id + SPATIAL_DIM; jacobians .fixed_rows_mut::(*j_id) @@ -54,26 +484,24 @@ impl SolverBody { } *j_id += SPATIAL_DIM * 2; - unit_force.dot(&self.linvel) + unit_torque.gdot(self.angvel) } } -impl JointVelocityConstraintBuilder { +impl JointTwoBodyConstraintHelper { pub fn lock_jacobians_generic( &self, - _params: &IntegrationParameters, jacobians: &mut DVector, j_id: &mut usize, joint_id: JointIndex, - body1: &SolverBody, - body2: &SolverBody, + body1: &JointSolverBody, + body2: &JointSolverBody, mb1: Option<(&Multibody, usize)>, mb2: Option<(&Multibody, usize)>, writeback_id: WritebackId, lin_jac: Vector, ang_jac1: SVector, ang_jac2: SVector, - ) -> JointGenericVelocityConstraint { + ) -> JointGenericTwoBodyConstraint { let is_rigid_body1 = mb1.is_none(); let is_rigid_body2 = mb2.is_none(); @@ -81,19 +509,17 @@ impl JointVelocityConstraintBuilder { let ndofs2 = mb2.map(|(m, _)| m.ndofs()).unwrap_or(SPATIAL_DIM); let j_id1 = *j_id; - let vel1 = if let Some((mb1, link_id1)) = mb1 { - mb1.fill_jacobians(link_id1, lin_jac, ang_jac1, j_id, jacobians) - .1 + if let Some((mb1, link_id1)) = mb1 { + mb1.fill_jacobians(link_id1, lin_jac, ang_jac1, j_id, jacobians); } else { - body1.fill_jacobians(lin_jac, ang_jac1, j_id, jacobians) + body1.fill_jacobians(lin_jac, ang_jac1, j_id, jacobians); }; let j_id2 = *j_id; - let vel2 = if let Some((mb2, link_id2)) = mb2 { - mb2.fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians) - .1 + if let Some((mb2, link_id2)) = mb2 { + mb2.fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians); } else { - body2.fill_jacobians(lin_jac, ang_jac2, j_id, jacobians) + body2.fill_jacobians(lin_jac, ang_jac2, j_id, jacobians); }; if is_rigid_body1 { @@ -116,16 +542,16 @@ impl JointVelocityConstraintBuilder { j.copy_from(&wj); } - let rhs_wo_bias = vel2 - vel1; + let rhs_wo_bias = 0.0; - let mj_lambda1 = mb1.map(|m| m.0.solver_id).unwrap_or(body1.mj_lambda[0]); - let mj_lambda2 = mb2.map(|m| m.0.solver_id).unwrap_or(body2.mj_lambda[0]); + let solver_vel1 = mb1.map(|m| m.0.solver_id).unwrap_or(body1.solver_vel[0]); + let solver_vel2 = mb2.map(|m| m.0.solver_id).unwrap_or(body2.solver_vel[0]); - JointGenericVelocityConstraint { + JointGenericTwoBodyConstraint { is_rigid_body1, is_rigid_body2, - mj_lambda1, - mj_lambda2, + solver_vel1, + solver_vel2, ndofs1, j_id1, ndofs2, @@ -148,19 +574,18 @@ impl JointVelocityConstraintBuilder { jacobians: &mut DVector, j_id: &mut usize, joint_id: JointIndex, - body1: &SolverBody, - body2: &SolverBody, + body1: &JointSolverBody, + body2: &JointSolverBody, mb1: Option<(&Multibody, usize)>, mb2: Option<(&Multibody, usize)>, locked_axis: usize, writeback_id: WritebackId, - ) -> JointGenericVelocityConstraint { + ) -> JointGenericTwoBodyConstraint { let lin_jac = self.basis.column(locked_axis).into_owned(); let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned(); let ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned(); let mut c = self.lock_jacobians_generic( - params, jacobians, j_id, joint_id, @@ -186,20 +611,19 @@ impl JointVelocityConstraintBuilder { jacobians: &mut DVector, j_id: &mut usize, joint_id: JointIndex, - body1: &SolverBody, - body2: &SolverBody, + body1: &JointSolverBody, + body2: &JointSolverBody, mb1: Option<(&Multibody, usize)>, mb2: Option<(&Multibody, usize)>, limited_axis: usize, limits: [Real; 2], writeback_id: WritebackId, - ) -> JointGenericVelocityConstraint { + ) -> JointGenericTwoBodyConstraint { let lin_jac = self.basis.column(limited_axis).into_owned(); let ang_jac1 = self.cmat1_basis.column(limited_axis).into_owned(); let ang_jac2 = self.cmat2_basis.column(limited_axis).into_owned(); let mut constraint = self.lock_jacobians_generic( - params, jacobians, j_id, joint_id, @@ -230,18 +654,17 @@ impl JointVelocityConstraintBuilder { pub fn motor_linear_generic( &self, - params: &IntegrationParameters, jacobians: &mut DVector, j_id: &mut usize, joint_id: JointIndex, - body1: &SolverBody, - body2: &SolverBody, + body1: &JointSolverBody, + body2: &JointSolverBody, mb1: Option<(&Multibody, usize)>, mb2: Option<(&Multibody, usize)>, motor_axis: usize, motor_params: &MotorParameters, writeback_id: WritebackId, - ) -> JointGenericVelocityConstraint { + ) -> JointGenericTwoBodyConstraint { let lin_jac = self.basis.column(motor_axis).into_owned(); let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned(); let ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned(); @@ -255,7 +678,6 @@ impl JointVelocityConstraintBuilder { // } let mut constraint = self.lock_jacobians_generic( - params, jacobians, j_id, joint_id, @@ -275,9 +697,7 @@ impl JointVelocityConstraintBuilder { rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt; } - let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) - + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); - rhs_wo_bias += dvel - motor_params.target_vel; + rhs_wo_bias += -motor_params.target_vel; constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse]; constraint.rhs = rhs_wo_bias; @@ -293,20 +713,19 @@ impl JointVelocityConstraintBuilder { jacobians: &mut DVector, j_id: &mut usize, joint_id: JointIndex, - body1: &SolverBody, - body2: &SolverBody, + body1: &JointSolverBody, + body2: &JointSolverBody, mb1: Option<(&Multibody, usize)>, mb2: Option<(&Multibody, usize)>, _locked_axis: usize, writeback_id: WritebackId, - ) -> JointGenericVelocityConstraint { + ) -> JointGenericTwoBodyConstraint { #[cfg(feature = "dim2")] let ang_jac = Vector1::new(1.0); #[cfg(feature = "dim3")] let ang_jac = self.ang_basis.column(_locked_axis).into_owned(); let mut constraint = self.lock_jacobians_generic( - params, jacobians, j_id, joint_id, @@ -335,21 +754,20 @@ impl JointVelocityConstraintBuilder { jacobians: &mut DVector, j_id: &mut usize, joint_id: JointIndex, - body1: &SolverBody, - body2: &SolverBody, + body1: &JointSolverBody, + body2: &JointSolverBody, mb1: Option<(&Multibody, usize)>, mb2: Option<(&Multibody, usize)>, _limited_axis: usize, limits: [Real; 2], writeback_id: WritebackId, - ) -> JointGenericVelocityConstraint { + ) -> JointGenericTwoBodyConstraint { #[cfg(feature = "dim2")] let ang_jac = Vector1::new(1.0); #[cfg(feature = "dim3")] let ang_jac = self.ang_basis.column(_limited_axis).into_owned(); let mut constraint = self.lock_jacobians_generic( - params, jacobians, j_id, joint_id, @@ -386,25 +804,23 @@ impl JointVelocityConstraintBuilder { pub fn motor_angular_generic( &self, - params: &IntegrationParameters, jacobians: &mut DVector, j_id: &mut usize, joint_id: JointIndex, - body1: &SolverBody, - body2: &SolverBody, + body1: &JointSolverBody, + body2: &JointSolverBody, mb1: Option<(&Multibody, usize)>, mb2: Option<(&Multibody, usize)>, _motor_axis: usize, motor_params: &MotorParameters, writeback_id: WritebackId, - ) -> JointGenericVelocityConstraint { + ) -> JointGenericTwoBodyConstraint { #[cfg(feature = "dim2")] let ang_jac = na::Vector1::new(1.0); #[cfg(feature = "dim3")] let ang_jac = self.basis.column(_motor_axis).into_owned(); let mut constraint = self.lock_jacobians_generic( - params, jacobians, j_id, joint_id, @@ -429,8 +845,7 @@ impl JointVelocityConstraintBuilder { * motor_params.erp_inv_dt; } - let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); - rhs_wo_bias += dvel - motor_params.target_vel; + rhs_wo_bias += -motor_params.target_vel; constraint.rhs_wo_bias = rhs_wo_bias; constraint.rhs = rhs_wo_bias; @@ -442,7 +857,7 @@ impl JointVelocityConstraintBuilder { pub fn finalize_generic_constraints( jacobians: &mut DVector, - constraints: &mut [JointGenericVelocityConstraint], + constraints: &mut [JointGenericTwoBodyConstraint], ) { // TODO: orthogonalization doesn’t seem to give good results for multibodies? const ORTHOGONALIZE: bool = false; @@ -506,34 +921,30 @@ impl JointVelocityConstraintBuilder { } } -impl JointVelocityConstraintBuilder { - pub fn lock_jacobians_generic_ground( +impl JointTwoBodyConstraintHelper { + pub fn lock_jacobians_generic_one_body( &self, - _params: &IntegrationParameters, jacobians: &mut DVector, j_id: &mut usize, joint_id: JointIndex, - body1: &SolverBody, + body1: &JointFixedSolverBody, (mb2, link_id2): (&Multibody, usize), writeback_id: WritebackId, lin_jac: Vector, ang_jac1: SVector, ang_jac2: SVector, - ) -> JointGenericVelocityGroundConstraint { + ) -> JointGenericOneBodyConstraint { let ndofs2 = mb2.ndofs(); - let vel1 = lin_jac.dot(&body1.linvel) + ang_jac1.gdot(body1.angvel); - + let proj_vel1 = lin_jac.dot(&body1.linvel) + ang_jac1.gdot(body1.angvel); let j_id2 = *j_id; - let vel2 = mb2 - .fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians) - .1; - let rhs_wo_bias = vel2 - vel1; + mb2.fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians); + let rhs_wo_bias = -proj_vel1; - let mj_lambda2 = mb2.solver_id; + let solver_vel2 = mb2.solver_id; - JointGenericVelocityGroundConstraint { - mj_lambda2, + JointGenericOneBodyConstraint { + solver_vel2, ndofs2, j_id2, joint_id, @@ -548,23 +959,22 @@ impl JointVelocityConstraintBuilder { } } - pub fn lock_linear_generic_ground( + pub fn lock_linear_generic_one_body( &self, params: &IntegrationParameters, jacobians: &mut DVector, j_id: &mut usize, joint_id: JointIndex, - body1: &SolverBody, + body1: &JointFixedSolverBody, mb2: (&Multibody, usize), locked_axis: usize, writeback_id: WritebackId, - ) -> JointGenericVelocityGroundConstraint { + ) -> JointGenericOneBodyConstraint { let lin_jac = self.basis.column(locked_axis).into_owned(); let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned(); let ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned(); - let mut c = self.lock_jacobians_generic_ground( - params, + let mut c = self.lock_jacobians_generic_one_body( jacobians, j_id, joint_id, @@ -582,24 +992,23 @@ impl JointVelocityConstraintBuilder { c } - pub fn limit_linear_generic_ground( + pub fn limit_linear_generic_one_body( &self, params: &IntegrationParameters, jacobians: &mut DVector, j_id: &mut usize, joint_id: JointIndex, - body1: &SolverBody, + body1: &JointFixedSolverBody, mb2: (&Multibody, usize), limited_axis: usize, limits: [Real; 2], writeback_id: WritebackId, - ) -> JointGenericVelocityGroundConstraint { + ) -> JointGenericOneBodyConstraint { let lin_jac = self.basis.column(limited_axis).into_owned(); let ang_jac1 = self.cmat1_basis.column(limited_axis).into_owned(); let ang_jac2 = self.cmat2_basis.column(limited_axis).into_owned(); - let mut constraint = self.lock_jacobians_generic_ground( - params, + let mut constraint = self.lock_jacobians_generic_one_body( jacobians, j_id, joint_id, @@ -626,19 +1035,17 @@ impl JointVelocityConstraintBuilder { constraint } - pub fn motor_linear_generic_ground( + pub fn motor_linear_generic_one_body( &self, - params: &IntegrationParameters, jacobians: &mut DVector, j_id: &mut usize, joint_id: JointIndex, - body1: &SolverBody, - body2: &SolverBody, // TODO: we shouldn’t need this. + body1: &JointFixedSolverBody, mb2: (&Multibody, usize), motor_axis: usize, motor_params: &MotorParameters, writeback_id: WritebackId, - ) -> JointGenericVelocityGroundConstraint { + ) -> JointGenericOneBodyConstraint { let lin_jac = self.basis.column(motor_axis).into_owned(); let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned(); let ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned(); @@ -651,8 +1058,7 @@ impl JointVelocityConstraintBuilder { // constraint.ang_jac2.fill(0.0); // } - let mut constraint = self.lock_jacobians_generic_ground( - params, + let mut constraint = self.lock_jacobians_generic_one_body( jacobians, j_id, joint_id, @@ -670,9 +1076,8 @@ impl JointVelocityConstraintBuilder { rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt; } - let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) - + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); - rhs_wo_bias += dvel - motor_params.target_vel; + let proj_vel1 = -lin_jac.dot(&body1.linvel) - ang_jac1.gdot(body1.angvel); + rhs_wo_bias += proj_vel1 - motor_params.target_vel; constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse]; constraint.rhs = rhs_wo_bias; @@ -682,24 +1087,23 @@ impl JointVelocityConstraintBuilder { constraint } - pub fn lock_angular_generic_ground( + pub fn lock_angular_generic_one_body( &self, params: &IntegrationParameters, jacobians: &mut DVector, j_id: &mut usize, joint_id: JointIndex, - body1: &SolverBody, + body1: &JointFixedSolverBody, mb2: (&Multibody, usize), _locked_axis: usize, writeback_id: WritebackId, - ) -> JointGenericVelocityGroundConstraint { + ) -> JointGenericOneBodyConstraint { #[cfg(feature = "dim2")] let ang_jac = Vector1::new(1.0); #[cfg(feature = "dim3")] let ang_jac = self.ang_basis.column(_locked_axis).into_owned(); - let mut constraint = self.lock_jacobians_generic_ground( - params, + let mut constraint = self.lock_jacobians_generic_one_body( jacobians, j_id, joint_id, @@ -720,25 +1124,24 @@ impl JointVelocityConstraintBuilder { constraint } - pub fn limit_angular_generic_ground( + pub fn limit_angular_generic_one_body( &self, params: &IntegrationParameters, jacobians: &mut DVector, j_id: &mut usize, joint_id: JointIndex, - body1: &SolverBody, + body1: &JointFixedSolverBody, mb2: (&Multibody, usize), _limited_axis: usize, limits: [Real; 2], writeback_id: WritebackId, - ) -> JointGenericVelocityGroundConstraint { + ) -> JointGenericOneBodyConstraint { #[cfg(feature = "dim2")] let ang_jac = Vector1::new(1.0); #[cfg(feature = "dim3")] let ang_jac = self.ang_basis.column(_limited_axis).into_owned(); - let mut constraint = self.lock_jacobians_generic_ground( - params, + let mut constraint = self.lock_jacobians_generic_one_body( jacobians, j_id, joint_id, @@ -771,26 +1174,23 @@ impl JointVelocityConstraintBuilder { constraint } - pub fn motor_angular_generic_ground( + pub fn motor_angular_generic_one_body( &self, - params: &IntegrationParameters, jacobians: &mut DVector, j_id: &mut usize, joint_id: JointIndex, - body1: &SolverBody, - body2: &SolverBody, // TODO: we shouldn’t need this. + body1: &JointFixedSolverBody, mb2: (&Multibody, usize), _motor_axis: usize, motor_params: &MotorParameters, writeback_id: WritebackId, - ) -> JointGenericVelocityGroundConstraint { + ) -> JointGenericOneBodyConstraint { #[cfg(feature = "dim2")] let ang_jac = na::Vector1::new(1.0); #[cfg(feature = "dim3")] let ang_jac = self.basis.column(_motor_axis).into_owned(); - let mut constraint = self.lock_jacobians_generic_ground( - params, + let mut constraint = self.lock_jacobians_generic_one_body( jacobians, j_id, joint_id, @@ -813,8 +1213,8 @@ impl JointVelocityConstraintBuilder { * motor_params.erp_inv_dt; } - let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); - rhs += dvel - motor_params.target_vel; + let proj_vel1 = -ang_jac.gdot(body1.angvel); + rhs += proj_vel1 - motor_params.target_vel; constraint.rhs_wo_bias = rhs; constraint.rhs = rhs; @@ -824,9 +1224,9 @@ impl JointVelocityConstraintBuilder { constraint } - pub fn finalize_generic_constraints_ground( + pub fn finalize_generic_constraints_one_body( jacobians: &mut DVector, - constraints: &mut [JointGenericVelocityGroundConstraint], + constraints: &mut [JointGenericOneBodyConstraint], ) { // TODO: orthogonalization doesn’t seem to give good results for multibodies? const ORTHOGONALIZE: bool = false; diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs index dbf126506..274e94e72 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs @@ -1,10 +1,11 @@ -use crate::dynamics::solver::joint_constraint::JointVelocityConstraintBuilder; -use crate::dynamics::solver::DeltaVel; +use crate::dynamics::solver::joint_constraint::JointTwoBodyConstraintHelper; +use crate::dynamics::solver::SolverVel; use crate::dynamics::{ GenericJoint, IntegrationParameters, JointAxesMask, JointGraphEdge, JointIndex, }; use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, Vector, DIM, SPATIAL_DIM}; -use crate::utils::{WDot, WReal}; +use crate::num::Zero; +use crate::utils::{SimdDot, SimdRealCopy}; #[cfg(feature = "simd-is-enabled")] use { @@ -13,7 +14,7 @@ use { }; #[derive(Copy, Clone, PartialEq, Debug)] -pub struct MotorParameters { +pub struct MotorParameters { pub erp_inv_dt: N, pub cfm_coeff: N, pub cfm_gain: N, @@ -22,7 +23,7 @@ pub struct MotorParameters { pub max_impulse: N, } -impl Default for MotorParameters { +impl Default for MotorParameters { fn default() -> Self { Self { erp_inv_dt: N::zero(), @@ -47,19 +48,46 @@ pub enum WritebackId { // the solver, to avoid fetching data from the rigid-body set // every time. #[derive(Copy, Clone)] -pub struct SolverBody { - pub linvel: Vector, - pub angvel: AngVector, +pub struct JointSolverBody { pub im: Vector, pub sqrt_ii: AngularInertia, pub world_com: Point, - pub mj_lambda: [usize; LANES], + pub solver_vel: [usize; LANES], +} + +impl JointSolverBody { + pub fn invalid() -> Self { + Self { + im: Vector::zeros(), + sqrt_ii: AngularInertia::zero(), + world_com: Point::origin(), + solver_vel: [usize::MAX; LANES], + } + } +} + +#[derive(Copy, Clone)] +pub struct JointFixedSolverBody { + pub linvel: Vector, + pub angvel: AngVector, + // TODO: is this really needed? + pub world_com: Point, +} + +impl JointFixedSolverBody { + pub fn invalid() -> Self { + Self { + linvel: Vector::zeros(), + angvel: AngVector::zero(), + world_com: Point::origin(), + } + } } #[derive(Debug, Copy, Clone)] -pub struct JointVelocityConstraint { - pub mj_lambda1: [usize; LANES], - pub mj_lambda2: [usize; LANES], +pub struct JointTwoBodyConstraint { + pub solver_vel1: [usize; LANES], + pub solver_vel2: [usize; LANES], pub joint_id: [JointIndex; LANES], @@ -81,32 +109,15 @@ pub struct JointVelocityConstraint { pub writeback_id: WritebackId, } -impl JointVelocityConstraint { - pub fn invalid() -> Self { - Self { - mj_lambda1: [crate::INVALID_USIZE; LANES], - mj_lambda2: [crate::INVALID_USIZE; LANES], - joint_id: [crate::INVALID_USIZE; LANES], - impulse: N::zero(), - impulse_bounds: [N::zero(), N::zero()], - lin_jac: Vector::zeros(), - ang_jac1: na::zero(), - ang_jac2: na::zero(), - inv_lhs: N::zero(), - cfm_gain: N::zero(), - cfm_coeff: N::zero(), - rhs: N::zero(), - rhs_wo_bias: N::zero(), - im1: na::zero(), - im2: na::zero(), - writeback_id: WritebackId::Dof(0), - } - } - - pub fn solve_generic(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { - let dlinvel = self.lin_jac.dot(&(mj_lambda2.linear - mj_lambda1.linear)); +impl JointTwoBodyConstraint { + pub fn solve_generic( + &mut self, + solver_vel1: &mut SolverVel, + solver_vel2: &mut SolverVel, + ) { + let dlinvel = self.lin_jac.dot(&(solver_vel2.linear - solver_vel1.linear)); let dangvel = - self.ang_jac2.gdot(mj_lambda2.angular) - self.ang_jac1.gdot(mj_lambda1.angular); + self.ang_jac2.gdot(solver_vel2.angular) - self.ang_jac1.gdot(solver_vel1.angular); let rhs = dlinvel + dangvel + self.rhs; let total_impulse = (self.impulse + self.inv_lhs * (rhs - self.cfm_gain * self.impulse)) @@ -118,10 +129,10 @@ impl JointVelocityConstraint { let ang_impulse1 = self.ang_jac1 * delta_impulse; let ang_impulse2 = self.ang_jac2 * delta_impulse; - mj_lambda1.linear += lin_impulse.component_mul(&self.im1); - mj_lambda1.angular += ang_impulse1; - mj_lambda2.linear -= lin_impulse.component_mul(&self.im2); - mj_lambda2.angular -= ang_impulse2; + solver_vel1.linear += lin_impulse.component_mul(&self.im1); + solver_vel1.angular += ang_impulse1; + solver_vel2.linear -= lin_impulse.component_mul(&self.im2); + solver_vel2.angular -= ang_impulse2; } pub fn remove_bias_from_rhs(&mut self) { @@ -129,12 +140,12 @@ impl JointVelocityConstraint { } } -impl JointVelocityConstraint { +impl JointTwoBodyConstraint { pub fn lock_axes( params: &IntegrationParameters, joint_id: JointIndex, - body1: &SolverBody, - body2: &SolverBody, + body1: &JointSolverBody, + body2: &JointSolverBody, frame1: &Isometry, frame2: &Isometry, joint: &GenericJoint, @@ -146,7 +157,18 @@ impl JointVelocityConstraint { let limit_axes = joint.limit_axes.bits() & !locked_axes; let coupled_axes = joint.coupled_axes.bits(); - let builder = JointVelocityConstraintBuilder::new( + // The has_lin/ang_coupling test is needed to avoid shl overflow later. + let has_lin_coupling = (coupled_axes & JointAxesMask::LIN_AXES.bits()) != 0; + let first_coupled_lin_axis_id = + (coupled_axes & JointAxesMask::LIN_AXES.bits()).trailing_zeros() as usize; + + #[cfg(feature = "dim3")] + let has_ang_coupling = (coupled_axes & JointAxesMask::ANG_AXES.bits()) != 0; + #[cfg(feature = "dim3")] + let first_coupled_ang_axis_id = + (coupled_axes & JointAxesMask::ANG_AXES.bits()).trailing_zeros() as usize; + + let builder = JointTwoBodyConstraintHelper::new( frame1, frame2, &body1.world_com, @@ -195,20 +217,31 @@ impl JointVelocityConstraint { } if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 { - // TODO: coupled linear motor constraint. - // out[len] = builder.motor_linear_coupled( - // params, - // [joint_id], - // body1, - // body2, - // limit_axes & coupled_axes, - // &joint.limits, - // WritebackId::Limit(0), // TODO: writeback - // ); - // len += 1; - } - - JointVelocityConstraintBuilder::finalize_constraints(&mut out[start..len]); + // if (motor_axes & !coupled_axes) & (1 << first_coupled_lin_axis_id) != 0 { + // let limits = if limit_axes & (1 << first_coupled_lin_axis_id) != 0 { + // Some([ + // joint.limits[first_coupled_lin_axis_id].min, + // joint.limits[first_coupled_lin_axis_id].max, + // ]) + // } else { + // None + // }; + // + // out[len] = builder.motor_linear_coupled + // params, + // [joint_id], + // body1, + // body2, + // coupled_axes, + // &joint.motors[first_coupled_lin_axis_id].motor_params(params.dt), + // limits, + // WritebackId::Motor(first_coupled_lin_axis_id), + // ); + // len += 1; + // } + } + + JointTwoBodyConstraintHelper::finalize_constraints(&mut out[start..len]); let start = len; for i in DIM..SPATIAL_DIM { @@ -262,44 +295,50 @@ impl JointVelocityConstraint { } #[cfg(feature = "dim3")] - if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 { + if has_ang_coupling && (limit_axes & (1 << first_coupled_ang_axis_id)) != 0 { out[len] = builder.limit_angular_coupled( params, [joint_id], body1, body2, - limit_axes & coupled_axes, - &joint.limits, - WritebackId::Limit(0), // TODO: writeback + coupled_axes, + [ + joint.limits[first_coupled_ang_axis_id].min, + joint.limits[first_coupled_ang_axis_id].max, + ], + WritebackId::Limit(first_coupled_ang_axis_id), ); len += 1; } - if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 { + if has_lin_coupling && (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 { out[len] = builder.limit_linear_coupled( params, [joint_id], body1, body2, - limit_axes & coupled_axes, - &joint.limits, - WritebackId::Limit(0), // TODO: writeback + coupled_axes, + [ + joint.limits[first_coupled_lin_axis_id].min, + joint.limits[first_coupled_lin_axis_id].max, + ], + WritebackId::Limit(first_coupled_lin_axis_id), ); len += 1; } - JointVelocityConstraintBuilder::finalize_constraints(&mut out[start..len]); + JointTwoBodyConstraintHelper::finalize_constraints(&mut out[start..len]); len } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda1 = mj_lambdas[self.mj_lambda1[0] as usize]; - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2[0] as usize]; + pub fn solve(&mut self, solver_vels: &mut [SolverVel]) { + let mut solver_vel1 = solver_vels[self.solver_vel1[0] as usize]; + let mut solver_vel2 = solver_vels[self.solver_vel2[0] as usize]; - self.solve_generic(&mut mj_lambda1, &mut mj_lambda2); + self.solve_generic(&mut solver_vel1, &mut solver_vel2); - mj_lambdas[self.mj_lambda1[0] as usize] = mj_lambda1; - mj_lambdas[self.mj_lambda2[0] as usize] = mj_lambda2; + solver_vels[self.solver_vel1[0] as usize] = solver_vel1; + solver_vels[self.solver_vel2[0] as usize] = solver_vel2; } pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { @@ -312,18 +351,18 @@ impl JointVelocityConstraint { } } #[cfg(feature = "simd-is-enabled")] -impl JointVelocityConstraint { +impl JointTwoBodyConstraint { pub fn lock_axes( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], - body1: &SolverBody, - body2: &SolverBody, + body1: &JointSolverBody, + body2: &JointSolverBody, frame1: &Isometry, frame2: &Isometry, locked_axes: u8, out: &mut [Self], ) -> usize { - let builder = JointVelocityConstraintBuilder::new( + let builder = JointTwoBodyConstraintHelper::new( frame1, frame2, &body1.world_com, @@ -354,31 +393,35 @@ impl JointVelocityConstraint { } } - JointVelocityConstraintBuilder::finalize_constraints(&mut out[..len]); + JointTwoBodyConstraintHelper::finalize_constraints(&mut out[..len]); len } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda1 = DeltaVel { - linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + pub fn solve(&mut self, solver_vels: &mut [SolverVel]) { + let mut solver_vel1 = SolverVel { + linear: Vector::from(gather![ + |ii| solver_vels[self.solver_vel1[ii] as usize].linear + ]), angular: AngVector::from(gather![ - |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + |ii| solver_vels[self.solver_vel1[ii] as usize].angular ]), }; - let mut mj_lambda2 = DeltaVel { - linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + let mut solver_vel2 = SolverVel { + linear: Vector::from(gather![ + |ii| solver_vels[self.solver_vel2[ii] as usize].linear + ]), angular: AngVector::from(gather![ - |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + |ii| solver_vels[self.solver_vel2[ii] as usize].angular ]), }; - self.solve_generic(&mut mj_lambda1, &mut mj_lambda2); + self.solve_generic(&mut solver_vel1, &mut solver_vel2); for ii in 0..SIMD_WIDTH { - mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); - mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); - mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); - mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + solver_vels[self.solver_vel1[ii] as usize].linear = solver_vel1.linear.extract(ii); + solver_vels[self.solver_vel1[ii] as usize].angular = solver_vel1.angular.extract(ii); + solver_vels[self.solver_vel2[ii] as usize].linear = solver_vel2.linear.extract(ii); + solver_vels[self.solver_vel2[ii] as usize].angular = solver_vel2.angular.extract(ii); } } @@ -398,8 +441,8 @@ impl JointVelocityConstraint { } #[derive(Debug, Copy, Clone)] -pub struct JointVelocityGroundConstraint { - pub mj_lambda2: [usize; LANES], +pub struct JointOneBodyConstraint { + pub solver_vel2: [usize; LANES], pub joint_id: [JointIndex; LANES], @@ -419,28 +462,10 @@ pub struct JointVelocityGroundConstraint { pub writeback_id: WritebackId, } -impl JointVelocityGroundConstraint { - pub fn invalid() -> Self { - Self { - mj_lambda2: [crate::INVALID_USIZE; LANES], - joint_id: [crate::INVALID_USIZE; LANES], - impulse: N::zero(), - impulse_bounds: [N::zero(), N::zero()], - lin_jac: Vector::zeros(), - ang_jac2: na::zero(), - inv_lhs: N::zero(), - cfm_coeff: N::zero(), - cfm_gain: N::zero(), - rhs: N::zero(), - rhs_wo_bias: N::zero(), - im2: na::zero(), - writeback_id: WritebackId::Dof(0), - } - } - - pub fn solve_generic(&mut self, mj_lambda2: &mut DeltaVel) { - let dlinvel = mj_lambda2.linear; - let dangvel = mj_lambda2.angular; +impl JointOneBodyConstraint { + pub fn solve_generic(&mut self, solver_vel2: &mut SolverVel) { + let dlinvel = solver_vel2.linear; + let dangvel = solver_vel2.angular; let dvel = self.lin_jac.dot(&dlinvel) + self.ang_jac2.gdot(dangvel) + self.rhs; let total_impulse = (self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse)) @@ -451,8 +476,8 @@ impl JointVelocityGroundConstraint { let lin_impulse = self.lin_jac * delta_impulse; let ang_impulse = self.ang_jac2 * delta_impulse; - mj_lambda2.linear -= lin_impulse.component_mul(&self.im2); - mj_lambda2.angular -= ang_impulse; + solver_vel2.linear -= lin_impulse.component_mul(&self.im2); + solver_vel2.angular -= ang_impulse; } pub fn remove_bias_from_rhs(&mut self) { @@ -460,12 +485,12 @@ impl JointVelocityGroundConstraint { } } -impl JointVelocityGroundConstraint { +impl JointOneBodyConstraint { pub fn lock_axes( params: &IntegrationParameters, joint_id: JointIndex, - body1: &SolverBody, - body2: &SolverBody, + body1: &JointFixedSolverBody, + body2: &JointSolverBody, frame1: &Isometry, frame2: &Isometry, joint: &GenericJoint, @@ -477,7 +502,18 @@ impl JointVelocityGroundConstraint { let limit_axes = joint.limit_axes.bits() & !locked_axes; let coupled_axes = joint.coupled_axes.bits(); - let builder = JointVelocityConstraintBuilder::new( + // The has_lin/ang_coupling test is needed to avoid shl overflow later. + let has_lin_coupling = (coupled_axes & JointAxesMask::LIN_AXES.bits()) != 0; + let first_coupled_lin_axis_id = + (coupled_axes & JointAxesMask::LIN_AXES.bits()).trailing_zeros() as usize; + + #[cfg(feature = "dim3")] + let has_ang_coupling = (coupled_axes & JointAxesMask::ANG_AXES.bits()) != 0; + #[cfg(feature = "dim3")] + let first_coupled_ang_axis_id = + (coupled_axes & JointAxesMask::ANG_AXES.bits()).trailing_zeros() as usize; + + let builder = JointTwoBodyConstraintHelper::new( frame1, frame2, &body1.world_com, @@ -488,7 +524,7 @@ impl JointVelocityGroundConstraint { let start = len; for i in DIM..SPATIAL_DIM { if (motor_axes & !coupled_axes) & (1 << i) != 0 { - out[len] = builder.motor_angular_ground( + out[len] = builder.motor_angular_one_body( [joint_id], body1, body2, @@ -507,7 +543,7 @@ impl JointVelocityGroundConstraint { None }; - out[len] = builder.motor_linear_ground( + out[len] = builder.motor_linear_one_body( params, [joint_id], body1, @@ -521,35 +557,40 @@ impl JointVelocityGroundConstraint { } } - if (motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 { + #[cfg(feature = "dim3")] + if has_ang_coupling && (motor_axes & (1 << first_coupled_ang_axis_id)) != 0 { // TODO: coupled angular motor constraint. } - if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 { - /* - // TODO: coupled linear motor constraint. - out[len] = builder.motor_linear_coupled_ground( + if has_lin_coupling && (motor_axes & (1 << first_coupled_lin_axis_id)) != 0 { + let limits = if (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 { + Some([ + joint.limits[first_coupled_lin_axis_id].min, + joint.limits[first_coupled_lin_axis_id].max, + ]) + } else { + None + }; + + out[len] = builder.motor_linear_coupled_one_body( params, [joint_id], body1, body2, - motor_axes & coupled_axes, - &joint.motors, - limit_axes & coupled_axes, - &joint.limits, - WritebackId::Limit(0), // TODO: writeback + coupled_axes, + &joint.motors[first_coupled_lin_axis_id].motor_params(params.dt), + limits, + WritebackId::Motor(first_coupled_lin_axis_id), ); len += 1; - */ - todo!() } - JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[start..len]); + JointTwoBodyConstraintHelper::finalize_one_body_constraints(&mut out[start..len]); let start = len; for i in DIM..SPATIAL_DIM { if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_angular_ground( + out[len] = builder.lock_angular_one_body( params, [joint_id], body1, @@ -562,7 +603,7 @@ impl JointVelocityGroundConstraint { } for i in 0..DIM { if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_linear_ground( + out[len] = builder.lock_linear_one_body( params, [joint_id], body1, @@ -576,7 +617,7 @@ impl JointVelocityGroundConstraint { for i in DIM..SPATIAL_DIM { if (limit_axes & !coupled_axes) & (1 << i) != 0 { - out[len] = builder.limit_angular_ground( + out[len] = builder.limit_angular_one_body( params, [joint_id], body1, @@ -590,7 +631,7 @@ impl JointVelocityGroundConstraint { } for i in 0..DIM { if (limit_axes & !coupled_axes) & (1 << i) != 0 { - out[len] = builder.limit_linear_ground( + out[len] = builder.limit_linear_one_body( params, [joint_id], body1, @@ -604,40 +645,46 @@ impl JointVelocityGroundConstraint { } #[cfg(feature = "dim3")] - if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 { - out[len] = builder.limit_angular_coupled_ground( + if has_ang_coupling && (limit_axes & (1 << first_coupled_ang_axis_id)) != 0 { + out[len] = builder.limit_angular_coupled_one_body( params, [joint_id], body1, body2, - limit_axes & coupled_axes, - &joint.limits, - WritebackId::Limit(0), // TODO: writeback + coupled_axes, + [ + joint.limits[first_coupled_ang_axis_id].min, + joint.limits[first_coupled_ang_axis_id].max, + ], + WritebackId::Limit(first_coupled_ang_axis_id), ); len += 1; } - if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 { - out[len] = builder.limit_linear_coupled_ground( + if has_lin_coupling && (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 { + out[len] = builder.limit_linear_coupled_one_body( params, [joint_id], body1, body2, - limit_axes & coupled_axes, - &joint.limits, - WritebackId::Limit(0), // TODO: writeback + coupled_axes, + [ + joint.limits[first_coupled_lin_axis_id].min, + joint.limits[first_coupled_lin_axis_id].max, + ], + WritebackId::Limit(first_coupled_lin_axis_id), ); len += 1; } - JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[start..len]); + JointTwoBodyConstraintHelper::finalize_one_body_constraints(&mut out[start..len]); len } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2[0] as usize]; - self.solve_generic(&mut mj_lambda2); - mj_lambdas[self.mj_lambda2[0] as usize] = mj_lambda2; + pub fn solve(&mut self, solver_vels: &mut [SolverVel]) { + let mut solver_vel2 = solver_vels[self.solver_vel2[0] as usize]; + self.solve_generic(&mut solver_vel2); + solver_vels[self.solver_vel2[0] as usize] = solver_vel2; } pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { @@ -651,19 +698,19 @@ impl JointVelocityGroundConstraint { } #[cfg(feature = "simd-is-enabled")] -impl JointVelocityGroundConstraint { +impl JointOneBodyConstraint { pub fn lock_axes( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], - body1: &SolverBody, - body2: &SolverBody, + body1: &JointFixedSolverBody, + body2: &JointSolverBody, frame1: &Isometry, frame2: &Isometry, locked_axes: u8, out: &mut [Self], ) -> usize { let mut len = 0; - let builder = JointVelocityConstraintBuilder::new( + let builder = JointTwoBodyConstraintHelper::new( frame1, frame2, &body1.world_com, @@ -673,7 +720,7 @@ impl JointVelocityGroundConstraint { for i in 0..DIM { if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_linear_ground( + out[len] = builder.lock_linear_one_body( params, joint_id, body1, @@ -686,7 +733,7 @@ impl JointVelocityGroundConstraint { } for i in DIM..SPATIAL_DIM { if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_angular_ground( + out[len] = builder.lock_angular_one_body( params, joint_id, body1, @@ -698,23 +745,25 @@ impl JointVelocityGroundConstraint { } } - JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[..len]); + JointTwoBodyConstraintHelper::finalize_one_body_constraints(&mut out[..len]); len } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda2 = DeltaVel { - linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + pub fn solve(&mut self, solver_vels: &mut [SolverVel]) { + let mut solver_vel2 = SolverVel { + linear: Vector::from(gather![ + |ii| solver_vels[self.solver_vel2[ii] as usize].linear + ]), angular: AngVector::from(gather![ - |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + |ii| solver_vels[self.solver_vel2[ii] as usize].angular ]), }; - self.solve_generic(&mut mj_lambda2); + self.solve_generic(&mut solver_vel2); for ii in 0..SIMD_WIDTH { - mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); - mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + solver_vels[self.solver_vel2[ii] as usize].linear = solver_vel2.linear.extract(ii); + solver_vels[self.solver_vel2[ii] as usize].angular = solver_vel2.angular.extract(ii); } } diff --git a/src/dynamics/solver/joint_constraint/mod.rs b/src/dynamics/solver/joint_constraint/mod.rs index 503bd81e8..7d9f0c5a1 100644 --- a/src/dynamics/solver/joint_constraint/mod.rs +++ b/src/dynamics/solver/joint_constraint/mod.rs @@ -1,13 +1,18 @@ -pub use joint_velocity_constraint::{MotorParameters, SolverBody, WritebackId}; +pub use joint_velocity_constraint::{JointSolverBody, MotorParameters, WritebackId}; -pub use joint_constraint::AnyJointVelocityConstraint; -pub use joint_generic_velocity_constraint::{ - JointGenericVelocityConstraint, JointGenericVelocityGroundConstraint, +pub use any_joint_constraint::JointConstraintTypes; +pub use joint_constraint_builder::JointTwoBodyConstraintHelper; +pub use joint_constraints_set::JointConstraintsSet; +pub use joint_generic_constraint::{JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint}; +pub use joint_generic_constraint_builder::{ + JointGenericOneBodyConstraintBuilder, JointGenericTwoBodyConstraintBuilder, + JointGenericVelocityOneBodyExternalConstraintBuilder, + JointGenericVelocityOneBodyInternalConstraintBuilder, }; -pub use joint_velocity_constraint_builder::JointVelocityConstraintBuilder; -mod joint_constraint; -mod joint_generic_velocity_constraint; -mod joint_generic_velocity_constraint_builder; +mod any_joint_constraint; +mod joint_constraint_builder; +mod joint_constraints_set; +mod joint_generic_constraint; +mod joint_generic_constraint_builder; mod joint_velocity_constraint; -mod joint_velocity_constraint_builder; diff --git a/src/dynamics/solver/mod.rs b/src/dynamics/solver/mod.rs index 3ffa94c19..5b1154584 100644 --- a/src/dynamics/solver/mod.rs +++ b/src/dynamics/solver/mod.rs @@ -1,56 +1,47 @@ -#[cfg(not(feature = "parallel"))] +// #[cfg(not(feature = "parallel"))] pub(crate) use self::island_solver::IslandSolver; -#[cfg(feature = "parallel")] -pub(crate) use self::parallel_island_solver::{ParallelIslandSolver, ThreadContext}; -#[cfg(feature = "parallel")] -pub(self) use self::parallel_solver_constraints::ParallelSolverConstraints; -#[cfg(feature = "parallel")] -pub(self) use self::parallel_velocity_solver::ParallelVelocitySolver; -#[cfg(not(feature = "parallel"))] -pub(self) use self::solver_constraints::SolverConstraints; -#[cfg(not(feature = "parallel"))] +// #[cfg(feature = "parallel")] +// pub(crate) use self::parallel_island_solver::{ParallelIslandSolver, ThreadContext}; +// #[cfg(feature = "parallel")] +// pub(self) use self::parallel_solver_constraints::ParallelSolverConstraints; +// #[cfg(feature = "parallel")] +// pub(self) use self::parallel_velocity_solver::ParallelVelocitySolver; +// #[cfg(not(feature = "parallel"))] +pub(self) use self::solver_constraints_set::SolverConstraintsSet; +// #[cfg(not(feature = "parallel"))] pub(self) use self::velocity_solver::VelocitySolver; -pub(self) use delta_vel::DeltaVel; -pub(self) use generic_velocity_constraint::*; -pub(self) use generic_velocity_constraint_element::*; -pub(self) use generic_velocity_ground_constraint::*; + +pub(self) use contact_constraint::*; pub(self) use interaction_groups::*; pub(crate) use joint_constraint::MotorParameters; pub use joint_constraint::*; -pub(self) use velocity_constraint::*; -pub(self) use velocity_constraint_element::*; -#[cfg(feature = "simd-is-enabled")] -pub(self) use velocity_constraint_wide::*; -pub(self) use velocity_ground_constraint::*; -pub(self) use velocity_ground_constraint_element::*; -#[cfg(feature = "simd-is-enabled")] -pub(self) use velocity_ground_constraint_wide::*; +pub(self) use solver_body::SolverBody; +pub(self) use solver_constraints_set::{AnyConstraintMut, ConstraintTypes}; +pub(self) use solver_vel::SolverVel; mod categorization; -mod delta_vel; -mod generic_velocity_constraint; -mod generic_velocity_constraint_element; -mod generic_velocity_ground_constraint; -mod generic_velocity_ground_constraint_element; +mod contact_constraint; mod interaction_groups; -#[cfg(not(feature = "parallel"))] +// #[cfg(not(feature = "parallel"))] mod island_solver; mod joint_constraint; -#[cfg(feature = "parallel")] -mod parallel_island_solver; -#[cfg(feature = "parallel")] -mod parallel_solver_constraints; -#[cfg(feature = "parallel")] -mod parallel_velocity_solver; -#[cfg(not(feature = "parallel"))] -mod solver_constraints; -mod velocity_constraint; -mod velocity_constraint_element; -#[cfg(feature = "simd-is-enabled")] -mod velocity_constraint_wide; -mod velocity_ground_constraint; -mod velocity_ground_constraint_element; -#[cfg(feature = "simd-is-enabled")] -mod velocity_ground_constraint_wide; -#[cfg(not(feature = "parallel"))] +// #[cfg(feature = "parallel")] +// mod parallel_island_solver; +// #[cfg(feature = "parallel")] +// mod parallel_solver_constraints; +// #[cfg(feature = "parallel")] +// mod parallel_velocity_solver; +mod solver_body; +// #[cfg(not(feature = "parallel"))] +mod solver_constraints_set; +mod solver_vel; +// #[cfg(not(feature = "parallel"))] mod velocity_solver; + +// TODO: SAFETY: restrict with bytemuck::AnyBitPattern to make this safe. +pub unsafe fn reset_buffer(buffer: &mut Vec, len: usize) { + buffer.clear(); + buffer.reserve(len); + buffer.as_mut_ptr().write_bytes(u8::MAX, len); + buffer.set_len(len); +} diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index 905de463f..218d2af02 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -3,7 +3,7 @@ use std::sync::atomic::{AtomicUsize, Ordering}; use rayon::Scope; use crate::dynamics::solver::{ - AnyJointVelocityConstraint, AnyVelocityConstraint, ParallelSolverConstraints, + ContactConstraintTypes, JointConstraintTypes, ParallelSolverConstraints, }; use crate::dynamics::{ IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet, @@ -12,7 +12,7 @@ use crate::dynamics::{ use crate::geometry::{ContactManifold, ContactManifoldIndex}; use na::DVector; -use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver}; +use super::{ParallelInteractionGroups, ParallelVelocitySolver, SolverVel}; #[macro_export] #[doc(hidden)] @@ -137,8 +137,8 @@ pub struct ParallelIslandSolver { velocity_solver: ParallelVelocitySolver, parallel_groups: ParallelInteractionGroups, parallel_joint_groups: ParallelInteractionGroups, - parallel_contact_constraints: ParallelSolverConstraints, - parallel_joint_constraints: ParallelSolverConstraints, + parallel_contact_constraints: ParallelSolverConstraints, + parallel_joint_constraints: ParallelSolverConstraints, thread: ThreadContext, } @@ -247,16 +247,16 @@ impl ParallelIslandSolver { } } - if self.velocity_solver.generic_mj_lambdas.len() < solver_id { - self.velocity_solver.generic_mj_lambdas = DVector::zeros(solver_id); + if self.velocity_solver.generic_solver_vels.len() < solver_id { + self.velocity_solver.generic_solver_vels = DVector::zeros(solver_id); } else { - self.velocity_solver.generic_mj_lambdas.fill(0.0); + self.velocity_solver.generic_solver_vels.fill(0.0); } - self.velocity_solver.mj_lambdas.clear(); + self.velocity_solver.solver_vels.clear(); self.velocity_solver - .mj_lambdas - .resize(islands.active_island(island_id).len(), DeltaVel::zero()); + .solver_vels + .resize(islands.active_island(island_id).len(), SolverVel::zero()); } for _ in 0..num_task_per_island { @@ -286,16 +286,16 @@ impl ParallelIslandSolver { unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) }; let impulse_joints: &mut Vec = unsafe { std::mem::transmute(impulse_joints.load(Ordering::Relaxed)) }; - let parallel_contact_constraints: &mut ParallelSolverConstraints = unsafe { + let parallel_contact_constraints: &mut ParallelSolverConstraints = unsafe { std::mem::transmute(parallel_contact_constraints.load(Ordering::Relaxed)) }; - let parallel_joint_constraints: &mut ParallelSolverConstraints = unsafe { + let parallel_joint_constraints: &mut ParallelSolverConstraints = unsafe { std::mem::transmute(parallel_joint_constraints.load(Ordering::Relaxed)) }; enable_flush_to_zero!(); // Ensure this is enabled on each thread. - // Initialize `mj_lambdas` (per-body velocity deltas) with external accelerations (gravity etc): + // Initialize `solver_vels` (per-body velocity deltas) with external accelerations (gravity etc): { let island_range = islands.active_island_range(island_id); let active_bodies = &islands.active_dynamic_set[island_range]; @@ -309,14 +309,14 @@ impl ParallelIslandSolver { .unwrap(); if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic { - let mut mj_lambdas = velocity_solver - .generic_mj_lambdas + let mut solver_vels = velocity_solver + .generic_solver_vels .rows_mut(multibody.solver_id, multibody.ndofs()); - mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0); + solver_vels.axpy(params.dt, &multibody.accelerations, 0.0); } } else { let rb = &bodies[*handle]; - let dvel = &mut velocity_solver.mj_lambdas[rb.ids.active_set_offset]; + let dvel = &mut velocity_solver.solver_vels[rb.ids.active_set_offset]; // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied // by the square root of the inertia tensor: diff --git a/src/dynamics/solver/parallel_solver_constraints.rs b/src/dynamics/solver/parallel_solver_constraints.rs index 46f1a5758..358a99273 100644 --- a/src/dynamics/solver/parallel_solver_constraints.rs +++ b/src/dynamics/solver/parallel_solver_constraints.rs @@ -1,10 +1,9 @@ use super::ParallelInteractionGroups; -use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, ThreadContext}; +use super::{ContactConstraintTypes, JointConstraintTypes, ThreadContext}; use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints}; -use crate::dynamics::solver::generic_velocity_constraint::GenericVelocityConstraint; +use crate::dynamics::solver::generic_two_body_constraint::GenericTwoBodyConstraint; use crate::dynamics::solver::{ - GenericVelocityGroundConstraint, InteractionGroups, VelocityConstraint, - VelocityGroundConstraint, + GenericOneBodyConstraint, InteractionGroups, OneBodyConstraint, TwoBodyConstraint, }; use crate::dynamics::{ ImpulseJoint, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyIndex, @@ -14,7 +13,7 @@ use crate::geometry::ContactManifold; use crate::math::{Real, SPATIAL_DIM}; #[cfg(feature = "simd-is-enabled")] use crate::{ - dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint}, + dynamics::solver::{OneBodyConstraintSimd, TwoBodyConstraintSimd}, math::SIMD_WIDTH, }; use na::DVector; @@ -36,40 +35,40 @@ use std::sync::atomic::Ordering; // } pub(crate) enum ConstraintDesc { - NongroundNongrouped(usize), - GroundNongrouped(usize), + TwoBodyNongrouped(usize), + OneBodyNongrouped(usize), #[cfg(feature = "simd-is-enabled")] - NongroundGrouped([usize; SIMD_WIDTH]), + TwoBodyGrouped([usize; SIMD_WIDTH]), #[cfg(feature = "simd-is-enabled")] - GroundGrouped([usize; SIMD_WIDTH]), - GenericNongroundNongrouped(usize, usize), - GenericGroundNongrouped(usize, usize), + OneBodyGrouped([usize; SIMD_WIDTH]), + GenericTwoBodyNongrouped(usize, usize), + GenericOneBodyNongrouped(usize, usize), GenericMultibodyInternal(MultibodyIndex, usize), } -pub(crate) struct ParallelSolverConstraints { +pub(crate) struct ParallelSolverConstraints { pub generic_jacobians: DVector, - pub not_ground_interactions: Vec, - pub ground_interactions: Vec, - pub generic_not_ground_interactions: Vec, - pub generic_ground_interactions: Vec, + pub two_body_interactions: Vec, + pub one_body_interactions: Vec, + pub generic_two_body_interactions: Vec, + pub generic_one_body_interactions: Vec, pub interaction_groups: InteractionGroups, - pub ground_interaction_groups: InteractionGroups, - pub velocity_constraints: Vec, + pub one_body_interaction_groups: InteractionGroups, + pub velocity_constraints: Vec, pub constraint_descs: Vec<(usize, ConstraintDesc)>, pub parallel_desc_groups: Vec, } -impl ParallelSolverConstraints { +impl ParallelSolverConstraints { pub fn new() -> Self { Self { generic_jacobians: DVector::zeros(0), - not_ground_interactions: vec![], - ground_interactions: vec![], - generic_not_ground_interactions: vec![], - generic_ground_interactions: vec![], + two_body_interactions: vec![], + one_body_interactions: vec![], + generic_two_body_interactions: vec![], + generic_one_body_interactions: vec![], interaction_groups: InteractionGroups::new(), - ground_interaction_groups: InteractionGroups::new(), + one_body_interaction_groups: InteractionGroups::new(), velocity_constraints: vec![], constraint_descs: vec![], parallel_desc_groups: vec![], @@ -78,14 +77,14 @@ impl ParallelSolverConstraints { } macro_rules! impl_init_constraints_group { - ($VelocityConstraint: ty, $Interaction: ty, + ($TwoBodyConstraint: ty, $Interaction: ty, $categorize: ident, $group: ident, $body1: ident, $body2: ident, $generate_internal_constraints: expr, $num_active_constraints_and_jacobian_lines: path, $empty_velocity_constraint: expr $(, $weight: ident)*) => { - impl ParallelSolverConstraints<$VelocityConstraint> { + impl ParallelSolverConstraints<$TwoBodyConstraint> { pub fn init_constraint_groups( &mut self, island_id: usize, @@ -100,7 +99,7 @@ macro_rules! impl_init_constraints_group { let num_groups = interaction_groups.num_groups(); self.interaction_groups.clear_groups(); - self.ground_interaction_groups.clear_groups(); + self.one_body_interaction_groups.clear_groups(); self.parallel_desc_groups.clear(); self.constraint_descs.clear(); self.parallel_desc_groups.push(0); @@ -108,43 +107,43 @@ macro_rules! impl_init_constraints_group { for i in 0..num_groups { let group = interaction_groups.group(i); - self.not_ground_interactions.clear(); - self.ground_interactions.clear(); - self.generic_not_ground_interactions.clear(); - self.generic_ground_interactions.clear(); + self.two_body_interactions.clear(); + self.one_body_interactions.clear(); + self.generic_two_body_interactions.clear(); + self.generic_one_body_interactions.clear(); $categorize( bodies, multibodies, interactions, group, - &mut self.ground_interactions, - &mut self.not_ground_interactions, - &mut self.generic_ground_interactions, - &mut self.generic_not_ground_interactions, + &mut self.one_body_interactions, + &mut self.two_body_interactions, + &mut self.generic_one_body_interactions, + &mut self.generic_two_body_interactions, ); #[cfg(feature = "simd-is-enabled")] - let start_grouped = self.interaction_groups.grouped_interactions.len(); + let start_grouped = self.interaction_groups.simd_interactions.len(); let start_nongrouped = self.interaction_groups.nongrouped_interactions.len(); #[cfg(feature = "simd-is-enabled")] - let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len(); - let start_nongrouped_ground = self.ground_interaction_groups.nongrouped_interactions.len(); + let start_grouped_one_body = self.one_body_interaction_groups.simd_interactions.len(); + let start_nongrouped_one_body = self.one_body_interaction_groups.nongrouped_interactions.len(); self.interaction_groups.$group( island_id, islands, bodies, interactions, - &self.not_ground_interactions, + &self.two_body_interactions, ); - self.ground_interaction_groups.$group( + self.one_body_interaction_groups.$group( island_id, islands, bodies, interactions, - &self.ground_interactions, + &self.one_body_interactions, ); // Compute constraint indices. @@ -152,19 +151,19 @@ macro_rules! impl_init_constraints_group { let interaction = &mut interactions[*interaction_i]$(.$weight)*; self.constraint_descs.push(( total_num_constraints, - ConstraintDesc::NongroundNongrouped(*interaction_i), + ConstraintDesc::TwoBodyNongrouped(*interaction_i), )); total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0; } #[cfg(feature = "simd-is-enabled")] for interaction_i in - self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH) + self.interaction_groups.simd_interactions[start_grouped..].chunks(SIMD_WIDTH) { let interaction = &mut interactions[interaction_i[0]]$(.$weight)*; self.constraint_descs.push(( total_num_constraints, - ConstraintDesc::NongroundGrouped( + ConstraintDesc::TwoBodyGrouped( gather![|ii| interaction_i[ii]], ), )); @@ -172,25 +171,25 @@ macro_rules! impl_init_constraints_group { } for interaction_i in - &self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..] + &self.one_body_interaction_groups.nongrouped_interactions[start_nongrouped_one_body..] { let interaction = &mut interactions[*interaction_i]$(.$weight)*; self.constraint_descs.push(( total_num_constraints, - ConstraintDesc::GroundNongrouped(*interaction_i), + ConstraintDesc::OneBodyNongrouped(*interaction_i), )); total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0; } #[cfg(feature = "simd-is-enabled")] - for interaction_i in self.ground_interaction_groups.grouped_interactions - [start_grouped_ground..] + for interaction_i in self.one_body_interaction_groups.simd_interactions + [start_grouped_one_body..] .chunks(SIMD_WIDTH) { let interaction = &mut interactions[interaction_i[0]]$(.$weight)*; self.constraint_descs.push(( total_num_constraints, - ConstraintDesc::GroundGrouped( + ConstraintDesc::OneBodyGrouped( gather![|ii| interaction_i[ii]], ), )); @@ -208,11 +207,11 @@ macro_rules! impl_init_constraints_group { } }; - for interaction_i in &self.generic_not_ground_interactions[..] { + for interaction_i in &self.generic_two_body_interactions[..] { let interaction = &mut interactions[*interaction_i]$(.$weight)*; self.constraint_descs.push(( total_num_constraints, - ConstraintDesc::GenericNongroundNongrouped(*interaction_i, *j_id), + ConstraintDesc::GenericTwoBodyNongrouped(*interaction_i, *j_id), )); let (num_constraints, num_jac_lines) = $num_active_constraints_and_jacobian_lines(interaction); let ndofs1 = $body1(interaction).map(multibody_ndofs).unwrap_or(0); @@ -222,11 +221,11 @@ macro_rules! impl_init_constraints_group { total_num_constraints += num_constraints; } - for interaction_i in &self.generic_ground_interactions[..] { + for interaction_i in &self.generic_one_body_interactions[..] { let interaction = &mut interactions[*interaction_i]$(.$weight)*; self.constraint_descs.push(( total_num_constraints, - ConstraintDesc::GenericGroundNongrouped(*interaction_i, *j_id), + ConstraintDesc::GenericOneBodyNongrouped(*interaction_i, *j_id), )); let (num_constraints, num_jac_lines) = $num_active_constraints_and_jacobian_lines(interaction); @@ -289,31 +288,31 @@ fn manifold_body2(manifold: &ContactManifold) -> Option { } impl_init_constraints_group!( - AnyVelocityConstraint, + ContactConstraintTypes, &mut ContactManifold, categorize_contacts, group_manifolds, manifold_body1, manifold_body2, false, - VelocityConstraint::num_active_constraints_and_jacobian_lines, - AnyVelocityConstraint::Empty + TwoBodyConstraint::num_active_constraints_and_jacobian_lines, + ContactConstraintTypes::Empty ); impl_init_constraints_group!( - AnyJointVelocityConstraint, + JointConstraintTypes, JointGraphEdge, categorize_joints, group_joints, joint_body1, joint_body2, true, - AnyJointVelocityConstraint::num_active_constraints_and_jacobian_lines, - AnyJointVelocityConstraint::Empty, + JointConstraintTypes::num_active_constraints_and_jacobian_lines, + JointConstraintTypes::Empty, weight ); -impl ParallelSolverConstraints { +impl ParallelSolverConstraints { pub fn fill_constraints( &mut self, thread: &ThreadContext, @@ -328,33 +327,33 @@ impl ParallelSolverConstraints { let batch_size = thread.batch_size; for desc in descs[thread.constraint_initialization_index, thread.num_initialized_constraints] { match &desc.1 { - ConstraintDesc::NongroundNongrouped(manifold_id) => { + ConstraintDesc::TwoBodyNongrouped(manifold_id) => { let manifold = &*manifolds_all[*manifold_id]; - VelocityConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, Some(desc.0)); + TwoBodyConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, Some(desc.0)); } - ConstraintDesc::GroundNongrouped(manifold_id) => { + ConstraintDesc::OneBodyNongrouped(manifold_id) => { let manifold = &*manifolds_all[*manifold_id]; - VelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, Some(desc.0)); + OneBodyConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, Some(desc.0)); } #[cfg(feature = "simd-is-enabled")] - ConstraintDesc::NongroundGrouped(manifold_id) => { + ConstraintDesc::TwoBodyGrouped(manifold_id) => { let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]]; - WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0)); + TwoBodyConstraintSimd::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0)); } #[cfg(feature = "simd-is-enabled")] - ConstraintDesc::GroundGrouped(manifold_id) => { + ConstraintDesc::OneBodyGrouped(manifold_id) => { let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]]; - WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0)); + OneBodyConstraintSimd::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0)); } - ConstraintDesc::GenericNongroundNongrouped(manifold_id, j_id) => { + ConstraintDesc::GenericTwoBodyNongrouped(manifold_id, j_id) => { let mut j_id = *j_id; let manifold = &*manifolds_all[*manifold_id]; - GenericVelocityConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, Some(desc.0)); + GenericTwoBodyConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, Some(desc.0)); } - ConstraintDesc::GenericGroundNongrouped(manifold_id, j_id) => { + ConstraintDesc::GenericOneBodyNongrouped(manifold_id, j_id) => { let mut j_id = *j_id; let manifold = &*manifolds_all[*manifold_id]; - GenericVelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, Some(desc.0)); + GenericOneBodyConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, Some(desc.0)); } ConstraintDesc::GenericMultibodyInternal(..) => unreachable!() } @@ -363,7 +362,7 @@ impl ParallelSolverConstraints { } } -impl ParallelSolverConstraints { +impl ParallelSolverConstraints { pub fn fill_constraints( &mut self, thread: &ThreadContext, @@ -378,33 +377,33 @@ impl ParallelSolverConstraints { let batch_size = thread.batch_size; for desc in descs[thread.joint_constraint_initialization_index, thread.num_initialized_joint_constraints] { match &desc.1 { - ConstraintDesc::NongroundNongrouped(joint_id) => { + ConstraintDesc::TwoBodyNongrouped(joint_id) => { let joint = &joints_all[*joint_id].weight; - AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0)); + JointConstraintTypes::from_joint(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0)); } - ConstraintDesc::GroundNongrouped(joint_id) => { + ConstraintDesc::OneBodyNongrouped(joint_id) => { let joint = &joints_all[*joint_id].weight; - AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0)); + JointConstraintTypes::from_joint_one_body(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0)); } #[cfg(feature = "simd-is-enabled")] - ConstraintDesc::NongroundGrouped(joint_id) => { + ConstraintDesc::TwoBodyGrouped(joint_id) => { let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight]; - AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0)); + JointConstraintTypes::from_wide_joint(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0)); } #[cfg(feature = "simd-is-enabled")] - ConstraintDesc::GroundGrouped(joint_id) => { + ConstraintDesc::OneBodyGrouped(joint_id) => { let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight]; - AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0)); + JointConstraintTypes::from_wide_joint_one_body(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0)); } - ConstraintDesc::GenericNongroundNongrouped(joint_id, j_id) => { + ConstraintDesc::GenericTwoBodyNongrouped(joint_id, j_id) => { let mut j_id = *j_id; let joint = &joints_all[*joint_id].weight; - AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0)); + JointConstraintTypes::from_joint(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0)); } - ConstraintDesc::GenericGroundNongrouped(joint_id, j_id) => { + ConstraintDesc::GenericOneBodyNongrouped(joint_id, j_id) => { let mut j_id = *j_id; let joint = &joints_all[*joint_id].weight; - AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0)); + JointConstraintTypes::from_joint_one_body(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0)); } ConstraintDesc::GenericMultibodyInternal(multibody_id, j_id) => { let mut j_id = *j_id; diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs index 3db1cdc82..3c129763f 100644 --- a/src/dynamics/solver/parallel_velocity_solver.rs +++ b/src/dynamics/solver/parallel_velocity_solver.rs @@ -1,4 +1,4 @@ -use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadContext}; +use super::{ContactConstraintTypes, JointConstraintTypes, SolverVel, ThreadContext}; use crate::concurrent_loop; use crate::dynamics::{ solver::ParallelSolverConstraints, IntegrationParameters, IslandManager, JointGraphEdge, @@ -6,21 +6,21 @@ use crate::dynamics::{ }; use crate::geometry::ContactManifold; use crate::math::Real; -use crate::utils::WAngularInertia; +use crate::utils::SimdAngularInertia; use na::DVector; use std::sync::atomic::Ordering; pub(crate) struct ParallelVelocitySolver { - pub mj_lambdas: Vec>, - pub generic_mj_lambdas: DVector, + pub solver_vels: Vec>, + pub generic_solver_vels: DVector, } impl ParallelVelocitySolver { pub fn new() -> Self { Self { - mj_lambdas: Vec::new(), - generic_mj_lambdas: DVector::zeros(0), + solver_vels: Vec::new(), + generic_solver_vels: DVector::zeros(0), } } @@ -34,8 +34,8 @@ impl ParallelVelocitySolver { multibodies: &mut MultibodyJointSet, manifolds_all: &mut [&mut ContactManifold], joints_all: &mut [JointGraphEdge], - contact_constraints: &mut ParallelSolverConstraints, - joint_constraints: &mut ParallelSolverConstraints, + contact_constraints: &mut ParallelSolverConstraints, + joint_constraints: &mut ParallelSolverConstraints, ) { let mut start_index = thread .solve_interaction_index @@ -104,16 +104,15 @@ impl ParallelVelocitySolver { * Solve constraints. */ { - for i in 0..params.max_velocity_iterations { - let solve_friction = params.interleave_restitution_and_friction_resolution - && params.max_velocity_friction_iterations + i - >= params.max_velocity_iterations; + for i in 0..params.num_velocity_iterations_per_small_step { + let solve_friction = params.num_friction_iteration_per_solver_iteration + i + >= params.num_velocity_iterations_per_small_step; // Solve joints. solve!( joint_constraints, &joint_constraints.generic_jacobians, - &mut self.mj_lambdas, - &mut self.generic_mj_lambdas + &mut self.solver_vels, + &mut self.generic_solver_vels ); shift += joint_descs.len(); start_index -= joint_descs.len(); @@ -122,8 +121,8 @@ impl ParallelVelocitySolver { solve!( contact_constraints, &contact_constraints.generic_jacobians, - &mut self.mj_lambdas, - &mut self.generic_mj_lambdas, + &mut self.solver_vels, + &mut self.generic_solver_vels, true, false ); @@ -134,8 +133,8 @@ impl ParallelVelocitySolver { solve!( contact_constraints, &contact_constraints.generic_jacobians, - &mut self.mj_lambdas, - &mut self.generic_mj_lambdas, + &mut self.solver_vels, + &mut self.generic_solver_vels, true, false ); @@ -146,8 +145,8 @@ impl ParallelVelocitySolver { solve!( contact_constraints, &contact_constraints.generic_jacobians, - &mut self.mj_lambdas, - &mut self.generic_mj_lambdas, + &mut self.solver_vels, + &mut self.generic_solver_vels, false, true ); @@ -157,21 +156,22 @@ impl ParallelVelocitySolver { } // Solve the remaining friction iterations. - let remaining_friction_iterations = - if !params.interleave_restitution_and_friction_resolution { - params.max_velocity_friction_iterations - } else if params.max_velocity_friction_iterations > params.max_velocity_iterations { - params.max_velocity_friction_iterations - params.max_velocity_iterations - } else { - 0 - }; + let remaining_friction_iterations = if params + .num_friction_iteration_per_solver_iteration + > params.num_velocity_iterations_per_small_step + { + params.num_friction_iteration_per_solver_iteration + - params.num_velocity_iterations_per_small_step + } else { + 0 + }; for _ in 0..remaining_friction_iterations { solve!( contact_constraints, &contact_constraints.generic_jacobians, - &mut self.mj_lambdas, - &mut self.generic_mj_lambdas, + &mut self.solver_vels, + &mut self.generic_solver_vels, false, true ); @@ -194,18 +194,18 @@ impl ParallelVelocitySolver { .unwrap(); if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic { - let mj_lambdas = self - .generic_mj_lambdas + let solver_vels = self + .generic_solver_vels .rows(multibody.solver_id, multibody.ndofs()); let prev_vels = multibody.velocities.clone(); // FIXME: avoid allocations. - multibody.velocities += mj_lambdas; + multibody.velocities += solver_vels; multibody.integrate(params.dt); multibody.forward_kinematics(bodies, false); multibody.velocities = prev_vels; } } else { let rb = bodies.index_mut_internal(*handle); - let dvel = self.mj_lambdas[rb.ids.active_set_offset]; + let dvel = self.solver_vels[rb.ids.active_set_offset]; let dangvel = rb.mprops .effective_world_inv_inertia_sqrt .transform_vector(dvel.angular); @@ -252,8 +252,8 @@ impl ParallelVelocitySolver { solve!( joint_constraints, &joint_constraints.generic_jacobians, - &mut self.mj_lambdas, - &mut self.generic_mj_lambdas + &mut self.solver_vels, + &mut self.generic_solver_vels ); shift += joint_descs.len(); start_index -= joint_descs.len(); @@ -261,8 +261,8 @@ impl ParallelVelocitySolver { solve!( contact_constraints, &contact_constraints.generic_jacobians, - &mut self.mj_lambdas, - &mut self.generic_mj_lambdas, + &mut self.solver_vels, + &mut self.generic_solver_vels, true, false ); @@ -272,8 +272,8 @@ impl ParallelVelocitySolver { solve!( contact_constraints, &contact_constraints.generic_jacobians, - &mut self.mj_lambdas, - &mut self.generic_mj_lambdas, + &mut self.solver_vels, + &mut self.generic_solver_vels, false, true ); @@ -296,14 +296,14 @@ impl ParallelVelocitySolver { .unwrap(); if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic { - let mj_lambdas = self - .generic_mj_lambdas + let solver_vels = self + .generic_solver_vels .rows(multibody.solver_id, multibody.ndofs()); - multibody.velocities += mj_lambdas; + multibody.velocities += solver_vels; } } else { let rb = bodies.index_mut_internal(*handle); - let dvel = self.mj_lambdas[rb.ids.active_set_offset]; + let dvel = self.solver_vels[rb.ids.active_set_offset]; let dangvel = rb.mprops .effective_world_inv_inertia_sqrt .transform_vector(dvel.angular); diff --git a/src/dynamics/solver/solver_body.rs b/src/dynamics/solver/solver_body.rs new file mode 100644 index 000000000..297f28ad7 --- /dev/null +++ b/src/dynamics/solver/solver_body.rs @@ -0,0 +1,59 @@ +use crate::dynamics::{RigidBody, RigidBodyVelocity}; +use crate::math::{AngularInertia, Isometry, Point, Real, Vector}; +use crate::prelude::RigidBodyDamping; + +#[cfg(feature = "dim2")] +use crate::num::Zero; + +#[derive(Copy, Clone, Debug)] +pub(crate) struct SolverBody { + pub position: Isometry, + pub integrated_vels: RigidBodyVelocity, + pub im: Vector, + pub sqrt_ii: AngularInertia, + pub world_com: Point, + pub ccd_thickness: Real, + pub damping: RigidBodyDamping, + pub local_com: Point, +} + +impl Default for SolverBody { + fn default() -> Self { + Self { + position: Isometry::identity(), + integrated_vels: RigidBodyVelocity::zero(), + im: na::zero(), + sqrt_ii: AngularInertia::zero(), + world_com: Point::origin(), + ccd_thickness: 0.0, + damping: RigidBodyDamping::default(), + local_com: Point::origin(), + } + } +} + +impl SolverBody { + pub fn from(rb: &RigidBody) -> Self { + Self { + position: rb.pos.position, + integrated_vels: RigidBodyVelocity::zero(), + im: rb.mprops.effective_inv_mass, + sqrt_ii: rb.mprops.effective_world_inv_inertia_sqrt, + world_com: rb.mprops.world_com, + ccd_thickness: rb.ccd.ccd_thickness, + damping: rb.damping, + local_com: rb.mprops.local_mprops.local_com, + } + } + + pub fn copy_from(&mut self, rb: &RigidBody) { + self.position = rb.pos.position; + self.integrated_vels = RigidBodyVelocity::zero(); + self.im = rb.mprops.effective_inv_mass; + self.sqrt_ii = rb.mprops.effective_world_inv_inertia_sqrt; + self.world_com = rb.mprops.world_com; + self.ccd_thickness = rb.ccd.ccd_thickness; + self.damping = rb.damping; + self.local_com = rb.mprops.local_mprops.local_com; + } +} diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs deleted file mode 100644 index 4006a85ce..000000000 --- a/src/dynamics/solver/solver_constraints.rs +++ /dev/null @@ -1,564 +0,0 @@ -use super::{ - AnyJointVelocityConstraint, InteractionGroups, VelocityConstraint, VelocityGroundConstraint, -}; -#[cfg(feature = "simd-is-enabled")] -use super::{WVelocityConstraint, WVelocityGroundConstraint}; -use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints}; -use crate::dynamics::solver::generic_velocity_ground_constraint::GenericVelocityGroundConstraint; -use crate::dynamics::solver::GenericVelocityConstraint; -use crate::dynamics::{ - solver::AnyVelocityConstraint, IntegrationParameters, IslandManager, JointGraphEdge, - JointIndex, MultibodyJointSet, RigidBodySet, -}; -use crate::geometry::{ContactManifold, ContactManifoldIndex}; -use crate::math::Real; -#[cfg(feature = "simd-is-enabled")] -use crate::math::SIMD_WIDTH; -use na::DVector; - -pub(crate) struct SolverConstraints { - pub generic_jacobians: DVector, - pub not_ground_interactions: Vec, - pub ground_interactions: Vec, - pub generic_not_ground_interactions: Vec, - pub generic_ground_interactions: Vec, - pub interaction_groups: InteractionGroups, - pub ground_interaction_groups: InteractionGroups, - pub velocity_constraints: Vec, -} - -impl SolverConstraints { - pub fn new() -> Self { - Self { - generic_jacobians: DVector::zeros(0), - not_ground_interactions: vec![], - ground_interactions: vec![], - generic_not_ground_interactions: vec![], - generic_ground_interactions: vec![], - interaction_groups: InteractionGroups::new(), - ground_interaction_groups: InteractionGroups::new(), - velocity_constraints: vec![], - } - } - - // pub fn clear(&mut self) { - // self.not_ground_interactions.clear(); - // self.ground_interactions.clear(); - // self.generic_not_ground_interactions.clear(); - // self.generic_ground_interactions.clear(); - // self.interaction_groups.clear(); - // self.ground_interaction_groups.clear(); - // self.velocity_constraints.clear(); - // } -} - -impl SolverConstraints { - pub fn init_constraint_groups( - &mut self, - island_id: usize, - islands: &IslandManager, - bodies: &RigidBodySet, - multibody_joints: &MultibodyJointSet, - manifolds: &[&mut ContactManifold], - manifold_indices: &[ContactManifoldIndex], - ) { - self.not_ground_interactions.clear(); - self.ground_interactions.clear(); - self.generic_not_ground_interactions.clear(); - self.generic_ground_interactions.clear(); - - categorize_contacts( - bodies, - multibody_joints, - manifolds, - manifold_indices, - &mut self.ground_interactions, - &mut self.not_ground_interactions, - &mut self.generic_ground_interactions, - &mut self.generic_not_ground_interactions, - ); - - self.interaction_groups.clear_groups(); - self.interaction_groups.group_manifolds( - island_id, - islands, - bodies, - manifolds, - &self.not_ground_interactions, - ); - - self.ground_interaction_groups.clear_groups(); - self.ground_interaction_groups.group_manifolds( - island_id, - islands, - bodies, - manifolds, - &self.ground_interactions, - ); - - // NOTE: uncomment this do disable SIMD contact resolution. - // self.interaction_groups - // .nongrouped_interactions - // .append(&mut self.interaction_groups.grouped_interactions); - // self.ground_interaction_groups - // .nongrouped_interactions - // .append(&mut self.ground_interaction_groups.grouped_interactions); - } - - pub fn init( - &mut self, - island_id: usize, - params: &IntegrationParameters, - islands: &IslandManager, - bodies: &RigidBodySet, - multibody_joints: &MultibodyJointSet, - manifolds: &[&mut ContactManifold], - manifold_indices: &[ContactManifoldIndex], - ) { - self.velocity_constraints.clear(); - - self.init_constraint_groups( - island_id, - islands, - bodies, - multibody_joints, - manifolds, - manifold_indices, - ); - - let mut jacobian_id = 0; - #[cfg(feature = "simd-is-enabled")] - { - self.compute_grouped_constraints(params, bodies, manifolds); - } - self.compute_nongrouped_constraints(params, bodies, manifolds); - self.compute_generic_constraints( - params, - bodies, - multibody_joints, - manifolds, - &mut jacobian_id, - ); - - #[cfg(feature = "simd-is-enabled")] - { - self.compute_grouped_ground_constraints(params, bodies, manifolds); - } - self.compute_nongrouped_ground_constraints(params, bodies, manifolds); - self.compute_generic_ground_constraints( - params, - bodies, - multibody_joints, - manifolds, - &mut jacobian_id, - ); - } - - #[cfg(feature = "simd-is-enabled")] - fn compute_grouped_constraints( - &mut self, - params: &IntegrationParameters, - bodies: &RigidBodySet, - manifolds_all: &[&mut ContactManifold], - ) { - for manifolds_i in self - .interaction_groups - .grouped_interactions - .chunks_exact(SIMD_WIDTH) - { - let manifold_id = gather![|ii| manifolds_i[ii]]; - let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]]; - WVelocityConstraint::generate( - params, - manifold_id, - manifolds, - bodies, - &mut self.velocity_constraints, - None, - ); - } - } - - fn compute_nongrouped_constraints( - &mut self, - params: &IntegrationParameters, - bodies: &RigidBodySet, - manifolds_all: &[&mut ContactManifold], - ) { - for manifold_i in &self.interaction_groups.nongrouped_interactions { - let manifold = &manifolds_all[*manifold_i]; - VelocityConstraint::generate( - params, - *manifold_i, - manifold, - bodies, - &mut self.velocity_constraints, - None, - ); - } - } - - fn compute_generic_constraints( - &mut self, - params: &IntegrationParameters, - bodies: &RigidBodySet, - multibody_joints: &MultibodyJointSet, - manifolds_all: &[&mut ContactManifold], - jacobian_id: &mut usize, - ) { - for manifold_i in &self.generic_not_ground_interactions { - let manifold = &manifolds_all[*manifold_i]; - GenericVelocityConstraint::generate( - params, - *manifold_i, - manifold, - bodies, - multibody_joints, - &mut self.velocity_constraints, - &mut self.generic_jacobians, - jacobian_id, - None, - ); - } - } - - fn compute_generic_ground_constraints( - &mut self, - params: &IntegrationParameters, - bodies: &RigidBodySet, - multibody_joints: &MultibodyJointSet, - manifolds_all: &[&mut ContactManifold], - jacobian_id: &mut usize, - ) { - for manifold_i in &self.generic_ground_interactions { - let manifold = &manifolds_all[*manifold_i]; - GenericVelocityGroundConstraint::generate( - params, - *manifold_i, - manifold, - bodies, - multibody_joints, - &mut self.velocity_constraints, - &mut self.generic_jacobians, - jacobian_id, - None, - ); - } - } - - #[cfg(feature = "simd-is-enabled")] - fn compute_grouped_ground_constraints( - &mut self, - params: &IntegrationParameters, - bodies: &RigidBodySet, - manifolds_all: &[&mut ContactManifold], - ) { - for manifolds_i in self - .ground_interaction_groups - .grouped_interactions - .chunks_exact(SIMD_WIDTH) - { - let manifold_id = gather![|ii| manifolds_i[ii]]; - let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]]; - WVelocityGroundConstraint::generate( - params, - manifold_id, - manifolds, - bodies, - &mut self.velocity_constraints, - None, - ); - } - } - - fn compute_nongrouped_ground_constraints( - &mut self, - params: &IntegrationParameters, - bodies: &RigidBodySet, - manifolds_all: &[&mut ContactManifold], - ) { - for manifold_i in &self.ground_interaction_groups.nongrouped_interactions { - let manifold = &manifolds_all[*manifold_i]; - VelocityGroundConstraint::generate( - params, - *manifold_i, - manifold, - bodies, - &mut self.velocity_constraints, - None, - ); - } - } -} - -impl SolverConstraints { - pub fn init( - &mut self, - island_id: usize, - params: &IntegrationParameters, - islands: &IslandManager, - bodies: &RigidBodySet, - multibody_joints: &MultibodyJointSet, - impulse_joints: &[JointGraphEdge], - joint_constraint_indices: &[JointIndex], - ) { - // Generate constraints for impulse_joints. - self.not_ground_interactions.clear(); - self.ground_interactions.clear(); - self.generic_not_ground_interactions.clear(); - self.generic_ground_interactions.clear(); - - categorize_joints( - bodies, - multibody_joints, - impulse_joints, - joint_constraint_indices, - &mut self.ground_interactions, - &mut self.not_ground_interactions, - &mut self.generic_ground_interactions, - &mut self.generic_not_ground_interactions, - ); - - self.velocity_constraints.clear(); - - self.interaction_groups.clear_groups(); - self.interaction_groups.group_joints( - island_id, - islands, - bodies, - impulse_joints, - &self.not_ground_interactions, - ); - - self.ground_interaction_groups.clear_groups(); - self.ground_interaction_groups.group_joints( - island_id, - islands, - bodies, - impulse_joints, - &self.ground_interactions, - ); - // NOTE: uncomment this do disable SIMD joint resolution. - // self.interaction_groups - // .nongrouped_interactions - // .append(&mut self.interaction_groups.grouped_interactions); - // self.ground_interaction_groups - // .nongrouped_interactions - // .append(&mut self.ground_interaction_groups.grouped_interactions); - - let mut j_id = 0; - self.compute_nongrouped_joint_constraints( - params, - bodies, - multibody_joints, - impulse_joints, - &mut j_id, - ); - #[cfg(feature = "simd-is-enabled")] - { - self.compute_grouped_joint_constraints(params, bodies, impulse_joints); - } - self.compute_generic_joint_constraints( - params, - bodies, - multibody_joints, - impulse_joints, - &mut j_id, - ); - - self.compute_nongrouped_joint_ground_constraints( - params, - bodies, - multibody_joints, - impulse_joints, - ); - #[cfg(feature = "simd-is-enabled")] - { - self.compute_grouped_joint_ground_constraints(params, bodies, impulse_joints); - } - self.compute_generic_ground_joint_constraints( - params, - bodies, - multibody_joints, - impulse_joints, - &mut j_id, - ); - self.compute_articulation_constraints( - params, - island_id, - islands, - multibody_joints, - &mut j_id, - ); - } - - fn compute_articulation_constraints( - &mut self, - params: &IntegrationParameters, - island_id: usize, - islands: &IslandManager, - multibody_joints: &MultibodyJointSet, - j_id: &mut usize, - ) { - for handle in islands.active_island(island_id) { - if let Some(link) = multibody_joints.rigid_body_link(*handle) { - let multibody = multibody_joints.get_multibody(link.multibody).unwrap(); - if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic { - multibody.generate_internal_constraints( - params, - j_id, - &mut self.generic_jacobians, - &mut self.velocity_constraints, - None, - ) - } - } - } - } - - fn compute_nongrouped_joint_ground_constraints( - &mut self, - params: &IntegrationParameters, - bodies: &RigidBodySet, - multibody_joints: &MultibodyJointSet, - joints_all: &[JointGraphEdge], - ) { - let mut j_id = 0; - for joint_i in &self.ground_interaction_groups.nongrouped_interactions { - let joint = &joints_all[*joint_i].weight; - AnyJointVelocityConstraint::from_joint_ground( - params, - *joint_i, - joint, - bodies, - multibody_joints, - &mut j_id, - &mut self.generic_jacobians, - &mut self.velocity_constraints, - None, - ); - } - } - - #[cfg(feature = "simd-is-enabled")] - fn compute_grouped_joint_ground_constraints( - &mut self, - params: &IntegrationParameters, - bodies: &RigidBodySet, - joints_all: &[JointGraphEdge], - ) { - for joints_i in self - .ground_interaction_groups - .grouped_interactions - .chunks_exact(SIMD_WIDTH) - { - let joints_id = gather![|ii| joints_i[ii]]; - let impulse_joints = gather![|ii| &joints_all[joints_i[ii]].weight]; - AnyJointVelocityConstraint::from_wide_joint_ground( - params, - joints_id, - impulse_joints, - bodies, - &mut self.velocity_constraints, - None, - ); - } - } - - fn compute_nongrouped_joint_constraints( - &mut self, - params: &IntegrationParameters, - bodies: &RigidBodySet, - multibody_joints: &MultibodyJointSet, - joints_all: &[JointGraphEdge], - j_id: &mut usize, - ) { - for joint_i in &self.interaction_groups.nongrouped_interactions { - let joint = &joints_all[*joint_i].weight; - AnyJointVelocityConstraint::from_joint( - params, - *joint_i, - joint, - bodies, - multibody_joints, - j_id, - &mut self.generic_jacobians, - &mut self.velocity_constraints, - None, - ); - } - } - - fn compute_generic_joint_constraints( - &mut self, - params: &IntegrationParameters, - bodies: &RigidBodySet, - multibody_joints: &MultibodyJointSet, - joints_all: &[JointGraphEdge], - j_id: &mut usize, - ) { - for joint_i in &self.generic_not_ground_interactions { - let joint = &joints_all[*joint_i].weight; - AnyJointVelocityConstraint::from_joint( - params, - *joint_i, - joint, - bodies, - multibody_joints, - j_id, - &mut self.generic_jacobians, - &mut self.velocity_constraints, - None, - ) - } - } - - fn compute_generic_ground_joint_constraints( - &mut self, - params: &IntegrationParameters, - bodies: &RigidBodySet, - multibody_joints: &MultibodyJointSet, - joints_all: &[JointGraphEdge], - j_id: &mut usize, - ) { - for joint_i in &self.generic_ground_interactions { - let joint = &joints_all[*joint_i].weight; - AnyJointVelocityConstraint::from_joint_ground( - params, - *joint_i, - joint, - bodies, - multibody_joints, - j_id, - &mut self.generic_jacobians, - &mut self.velocity_constraints, - None, - ) - } - } - - #[cfg(feature = "simd-is-enabled")] - fn compute_grouped_joint_constraints( - &mut self, - params: &IntegrationParameters, - bodies: &RigidBodySet, - joints_all: &[JointGraphEdge], - ) { - for joints_i in self - .interaction_groups - .grouped_interactions - .chunks_exact(SIMD_WIDTH) - { - let joints_id = gather![|ii| joints_i[ii]]; - let impulse_joints = gather![|ii| &joints_all[joints_i[ii]].weight]; - AnyJointVelocityConstraint::from_wide_joint( - params, - joints_id, - impulse_joints, - bodies, - &mut self.velocity_constraints, - None, - ); - } - } -} diff --git a/src/dynamics/solver/solver_constraints_set.rs b/src/dynamics/solver/solver_constraints_set.rs new file mode 100644 index 000000000..4404241c7 --- /dev/null +++ b/src/dynamics/solver/solver_constraints_set.rs @@ -0,0 +1,241 @@ +use super::InteractionGroups; +use crate::math::Real; +use na::DVector; + +pub(crate) trait ConstraintTypes { + type OneBody; + type TwoBodies; + type GenericOneBody; + type GenericTwoBodies; + + #[cfg(feature = "simd-is-enabled")] + type SimdOneBody; + #[cfg(feature = "simd-is-enabled")] + type SimdTwoBodies; + + type BuilderOneBody; + type BuilderTwoBodies; + + type GenericBuilderOneBody; + type GenericBuilderTwoBodies; + + #[cfg(feature = "simd-is-enabled")] + type SimdBuilderOneBody; + #[cfg(feature = "simd-is-enabled")] + type SimdBuilderTwoBodies; +} + +pub enum AnyConstraintMut<'a, Constraints: ConstraintTypes> { + OneBody(&'a mut Constraints::OneBody), + TwoBodies(&'a mut Constraints::TwoBodies), + GenericOneBody(&'a mut Constraints::GenericOneBody), + GenericTwoBodies(&'a mut Constraints::GenericTwoBodies), + #[cfg(feature = "simd-is-enabled")] + SimdOneBody(&'a mut Constraints::SimdOneBody), + #[cfg(feature = "simd-is-enabled")] + SimdTwoBodies(&'a mut Constraints::SimdTwoBodies), +} + +pub(crate) struct SolverConstraintsSet { + pub generic_jacobians: DVector, + pub two_body_interactions: Vec, + pub one_body_interactions: Vec, + pub generic_two_body_interactions: Vec, + pub generic_one_body_interactions: Vec, + pub interaction_groups: InteractionGroups, + pub one_body_interaction_groups: InteractionGroups, + + pub velocity_constraints: Vec, + pub generic_velocity_constraints: Vec, + #[cfg(feature = "simd-is-enabled")] + pub simd_velocity_constraints: Vec, + pub velocity_one_body_constraints: Vec, + pub generic_velocity_one_body_constraints: Vec, + #[cfg(feature = "simd-is-enabled")] + pub simd_velocity_one_body_constraints: Vec, + + pub velocity_constraints_builder: Vec, + pub generic_velocity_constraints_builder: Vec, + #[cfg(feature = "simd-is-enabled")] + pub simd_velocity_constraints_builder: Vec, + pub velocity_one_body_constraints_builder: Vec, + pub generic_velocity_one_body_constraints_builder: Vec, + #[cfg(feature = "simd-is-enabled")] + pub simd_velocity_one_body_constraints_builder: Vec, +} + +impl SolverConstraintsSet { + pub fn new() -> Self { + Self { + generic_jacobians: DVector::zeros(0), + two_body_interactions: vec![], + one_body_interactions: vec![], + generic_two_body_interactions: vec![], + generic_one_body_interactions: vec![], + interaction_groups: InteractionGroups::new(), + one_body_interaction_groups: InteractionGroups::new(), + + velocity_constraints: vec![], + generic_velocity_constraints: vec![], + #[cfg(feature = "simd-is-enabled")] + simd_velocity_constraints: vec![], + velocity_one_body_constraints: vec![], + generic_velocity_one_body_constraints: vec![], + #[cfg(feature = "simd-is-enabled")] + simd_velocity_one_body_constraints: vec![], + + velocity_constraints_builder: vec![], + generic_velocity_constraints_builder: vec![], + #[cfg(feature = "simd-is-enabled")] + simd_velocity_constraints_builder: vec![], + velocity_one_body_constraints_builder: vec![], + generic_velocity_one_body_constraints_builder: vec![], + #[cfg(feature = "simd-is-enabled")] + simd_velocity_one_body_constraints_builder: vec![], + } + } + + #[allow(dead_code)] // Useful for debuging. + pub fn print_counts(&self) { + println!("Solver constraints:"); + println!( + "|__ two_body_interactions: {}", + self.two_body_interactions.len() + ); + println!( + "|__ one_body_interactions: {}", + self.one_body_interactions.len() + ); + println!( + "|__ generic_two_body_interactions: {}", + self.generic_two_body_interactions.len() + ); + println!( + "|__ generic_one_body_interactions: {}", + self.generic_one_body_interactions.len() + ); + + println!( + "|__ velocity_constraints: {}", + self.velocity_constraints.len() + ); + println!( + "|__ generic_velocity_constraints: {}", + self.generic_velocity_constraints.len() + ); + #[cfg(feature = "simd-is-enabled")] + println!( + "|__ simd_velocity_constraints: {}", + self.simd_velocity_constraints.len() + ); + println!( + "|__ velocity_one_body_constraints: {}", + self.velocity_one_body_constraints.len() + ); + println!( + "|__ generic_velocity_one_body_constraints: {}", + self.generic_velocity_one_body_constraints.len() + ); + #[cfg(feature = "simd-is-enabled")] + println!( + "|__ simd_velocity_one_body_constraints: {}", + self.simd_velocity_one_body_constraints.len() + ); + + println!( + "|__ velocity_constraints_builder: {}", + self.velocity_constraints_builder.len() + ); + println!( + "|__ generic_velocity_constraints_builder: {}", + self.generic_velocity_constraints_builder.len() + ); + #[cfg(feature = "simd-is-enabled")] + println!( + "|__ simd_velocity_constraints_builder: {}", + self.simd_velocity_constraints_builder.len() + ); + println!( + "|__ velocity_one_body_constraints_builder: {}", + self.velocity_one_body_constraints_builder.len() + ); + println!( + "|__ generic_velocity_one_body_constraints_builder: {}", + self.generic_velocity_one_body_constraints_builder.len() + ); + #[cfg(feature = "simd-is-enabled")] + println!( + "|__ simd_velocity_one_body_constraints_builder: {}", + self.simd_velocity_one_body_constraints_builder.len() + ); + } + + pub fn clear_constraints(&mut self) { + self.generic_jacobians.fill(0.0); + self.velocity_constraints.clear(); + self.velocity_one_body_constraints.clear(); + self.generic_velocity_constraints.clear(); + self.generic_velocity_one_body_constraints.clear(); + + #[cfg(feature = "simd-is-enabled")] + { + self.simd_velocity_constraints.clear(); + self.simd_velocity_one_body_constraints.clear(); + } + } + + pub fn clear_builders(&mut self) { + self.velocity_constraints_builder.clear(); + self.velocity_one_body_constraints_builder.clear(); + self.generic_velocity_constraints_builder.clear(); + self.generic_velocity_one_body_constraints_builder.clear(); + + #[cfg(feature = "simd-is-enabled")] + { + self.simd_velocity_constraints_builder.clear(); + self.simd_velocity_one_body_constraints_builder.clear(); + } + } + + // Returns the generic jacobians and a mutable iterator through all the constraints. + pub fn iter_constraints_mut( + &mut self, + ) -> ( + &DVector, + impl Iterator>, + ) { + let jac = &self.generic_jacobians; + let a = self + .velocity_constraints + .iter_mut() + .map(AnyConstraintMut::TwoBodies); + let b = self + .generic_velocity_constraints + .iter_mut() + .map(AnyConstraintMut::GenericTwoBodies); + #[cfg(feature = "simd-is-enabled")] + let c = self + .simd_velocity_constraints + .iter_mut() + .map(AnyConstraintMut::SimdTwoBodies); + let d = self + .velocity_one_body_constraints + .iter_mut() + .map(AnyConstraintMut::OneBody); + let e = self + .generic_velocity_one_body_constraints + .iter_mut() + .map(AnyConstraintMut::GenericOneBody); + #[cfg(feature = "simd-is-enabled")] + let f = self + .simd_velocity_one_body_constraints + .iter_mut() + .map(AnyConstraintMut::SimdOneBody); + + #[cfg(feature = "simd-is-enabled")] + return (jac, a.chain(b).chain(c).chain(d).chain(e).chain(f)); + + #[cfg(not(feature = "simd-is-enabled"))] + return (jac, a.chain(b).chain(d).chain(e)); + } +} diff --git a/src/dynamics/solver/delta_vel.rs b/src/dynamics/solver/solver_vel.rs similarity index 74% rename from src/dynamics/solver/delta_vel.rs rename to src/dynamics/solver/solver_vel.rs index 2fc92f03b..48c76b866 100644 --- a/src/dynamics/solver/delta_vel.rs +++ b/src/dynamics/solver/solver_vel.rs @@ -1,17 +1,19 @@ use crate::math::{AngVector, Vector, SPATIAL_DIM}; -use crate::utils::WReal; +use crate::utils::SimdRealCopy; use na::{DVectorView, DVectorViewMut, Scalar}; use std::ops::{AddAssign, Sub}; #[derive(Copy, Clone, Debug, Default)] #[repr(C)] //#[repr(align(64))] -pub struct DeltaVel { +pub struct SolverVel { + // The linear velocity of a solver body. pub linear: Vector, + // The angular velocity, multiplied by the inverse sqrt angular inertia, of a solver body. pub angular: AngVector, } -impl DeltaVel { +impl SolverVel { pub fn as_slice(&self) -> &[N; SPATIAL_DIM] { unsafe { std::mem::transmute(self) } } @@ -29,7 +31,7 @@ impl DeltaVel { } } -impl DeltaVel { +impl SolverVel { pub fn zero() -> Self { Self { linear: na::zero(), @@ -38,18 +40,18 @@ impl DeltaVel { } } -impl AddAssign for DeltaVel { +impl AddAssign for SolverVel { fn add_assign(&mut self, rhs: Self) { self.linear += rhs.linear; self.angular += rhs.angular; } } -impl Sub for DeltaVel { +impl Sub for SolverVel { type Output = Self; fn sub(self, rhs: Self) -> Self { - DeltaVel { + SolverVel { linear: self.linear - rhs.linear, angular: self.angular - rhs.angular, } diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs deleted file mode 100644 index a6b73845e..000000000 --- a/src/dynamics/solver/velocity_constraint.rs +++ /dev/null @@ -1,441 +0,0 @@ -use crate::dynamics::solver::{ - GenericVelocityConstraint, GenericVelocityGroundConstraint, VelocityGroundConstraint, -}; -#[cfg(feature = "simd-is-enabled")] -use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint}; -use crate::dynamics::{IntegrationParameters, RigidBodySet}; -use crate::geometry::{ContactManifold, ContactManifoldIndex}; -use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS}; -use crate::utils::{self, WAngularInertia, WBasis, WCross, WDot}; -use na::DVector; - -use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart}; - -//#[repr(align(64))] -#[derive(Copy, Clone, Debug)] -pub(crate) enum AnyVelocityConstraint { - NongroupedGround(VelocityGroundConstraint), - Nongrouped(VelocityConstraint), - #[cfg(feature = "simd-is-enabled")] - GroupedGround(WVelocityGroundConstraint), - #[cfg(feature = "simd-is-enabled")] - Grouped(WVelocityConstraint), - NongroupedGenericGround(GenericVelocityGroundConstraint), - NongroupedGeneric(GenericVelocityConstraint), - #[allow(dead_code)] // The Empty variant is only used with parallel code. - Empty, -} - -impl AnyVelocityConstraint { - #[cfg(target_arch = "wasm32")] - pub fn as_nongrouped_mut(&mut self) -> Option<&mut VelocityConstraint> { - if let AnyVelocityConstraint::Nongrouped(c) = self { - Some(c) - } else { - None - } - } - - #[cfg(target_arch = "wasm32")] - pub fn as_nongrouped_ground_mut(&mut self) -> Option<&mut VelocityGroundConstraint> { - if let AnyVelocityConstraint::NongroupedGround(c) = self { - Some(c) - } else { - None - } - } - - pub fn remove_bias_from_rhs(&mut self) { - match self { - AnyVelocityConstraint::Nongrouped(c) => c.remove_cfm_and_bias_from_rhs(), - AnyVelocityConstraint::NongroupedGround(c) => c.remove_cfm_and_bias_from_rhs(), - #[cfg(feature = "simd-is-enabled")] - AnyVelocityConstraint::Grouped(c) => c.remove_cfm_and_bias_from_rhs(), - #[cfg(feature = "simd-is-enabled")] - AnyVelocityConstraint::GroupedGround(c) => c.remove_cfm_and_bias_from_rhs(), - AnyVelocityConstraint::NongroupedGeneric(c) => c.remove_cfm_and_bias_from_rhs(), - AnyVelocityConstraint::NongroupedGenericGround(c) => c.remove_cfm_and_bias_from_rhs(), - AnyVelocityConstraint::Empty => unreachable!(), - } - } - - pub fn solve( - &mut self, - jacobians: &DVector, - mj_lambdas: &mut [DeltaVel], - generic_mj_lambdas: &mut DVector, - solve_restitution: bool, - solve_friction: bool, - ) { - match self { - AnyVelocityConstraint::NongroupedGround(c) => { - c.solve(mj_lambdas, solve_restitution, solve_friction) - } - AnyVelocityConstraint::Nongrouped(c) => { - c.solve(mj_lambdas, solve_restitution, solve_friction) - } - #[cfg(feature = "simd-is-enabled")] - AnyVelocityConstraint::GroupedGround(c) => { - c.solve(mj_lambdas, solve_restitution, solve_friction) - } - #[cfg(feature = "simd-is-enabled")] - AnyVelocityConstraint::Grouped(c) => { - c.solve(mj_lambdas, solve_restitution, solve_friction) - } - AnyVelocityConstraint::NongroupedGeneric(c) => c.solve( - jacobians, - mj_lambdas, - generic_mj_lambdas, - solve_restitution, - solve_friction, - ), - AnyVelocityConstraint::NongroupedGenericGround(c) => c.solve( - jacobians, - generic_mj_lambdas, - solve_restitution, - solve_friction, - ), - AnyVelocityConstraint::Empty => unreachable!(), - } - } - - pub fn writeback_impulses(&self, manifold_all: &mut [&mut ContactManifold]) { - match self { - AnyVelocityConstraint::NongroupedGround(c) => c.writeback_impulses(manifold_all), - AnyVelocityConstraint::Nongrouped(c) => c.writeback_impulses(manifold_all), - #[cfg(feature = "simd-is-enabled")] - AnyVelocityConstraint::GroupedGround(c) => c.writeback_impulses(manifold_all), - #[cfg(feature = "simd-is-enabled")] - AnyVelocityConstraint::Grouped(c) => c.writeback_impulses(manifold_all), - AnyVelocityConstraint::NongroupedGeneric(c) => c.writeback_impulses(manifold_all), - AnyVelocityConstraint::NongroupedGenericGround(c) => c.writeback_impulses(manifold_all), - AnyVelocityConstraint::Empty => unreachable!(), - } - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct VelocityConstraint { - pub dir1: Vector, // Non-penetration force direction for the first body. - #[cfg(feature = "dim3")] - pub tangent1: Vector, // One of the friction force directions. - pub im1: Vector, - pub im2: Vector, - pub cfm_factor: Real, - pub limit: Real, - pub mj_lambda1: usize, - pub mj_lambda2: usize, - pub manifold_id: ContactManifoldIndex, - pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS], - pub num_contacts: u8, - pub elements: [VelocityConstraintElement; MAX_MANIFOLD_POINTS], -} - -impl VelocityConstraint { - #[cfg(feature = "parallel")] - pub fn num_active_constraints_and_jacobian_lines(manifold: &ContactManifold) -> (usize, usize) { - let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0; - ( - manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + rest as usize, - manifold.data.solver_contacts.len() * DIM, - ) - } - - pub fn generate( - params: &IntegrationParameters, - manifold_id: ContactManifoldIndex, - manifold: &ContactManifold, - bodies: &RigidBodySet, - out_constraints: &mut Vec, - insert_at: Option, - ) { - assert_eq!(manifold.data.relative_dominance, 0); - - let cfm_factor = params.cfm_factor(); - let inv_dt = params.inv_dt(); - let erp_inv_dt = params.erp_inv_dt(); - - let handle1 = manifold.data.rigid_body1.unwrap(); - let handle2 = manifold.data.rigid_body2.unwrap(); - - let rb1 = &bodies[handle1]; - let (vels1, mprops1) = (&rb1.vels, &rb1.mprops); - let rb2 = &bodies[handle2]; - let (vels2, mprops2) = (&rb2.vels, &rb2.mprops); - let ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness; - - let mj_lambda1 = rb1.ids.active_set_offset; - let mj_lambda2 = rb2.ids.active_set_offset; - let force_dir1 = -manifold.data.normal; - - #[cfg(feature = "dim2")] - let tangents1 = force_dir1.orthonormal_basis(); - #[cfg(feature = "dim3")] - let tangents1 = - super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel); - - for (_l, manifold_points) in manifold - .data - .solver_contacts - .chunks(MAX_MANIFOLD_POINTS) - .enumerate() - { - let mut is_fast_contact = false; - - #[cfg(not(target_arch = "wasm32"))] - let mut constraint = VelocityConstraint { - dir1: force_dir1, - #[cfg(feature = "dim3")] - tangent1: tangents1[0], - elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], - im1: mprops1.effective_inv_mass, - im2: mprops2.effective_inv_mass, - cfm_factor, - limit: 0.0, - mj_lambda1, - mj_lambda2, - manifold_id, - manifold_contact_id: [0; MAX_MANIFOLD_POINTS], - num_contacts: manifold_points.len() as u8, - }; - - // TODO: this is a WIP optimization for WASM platforms. - // For some reasons, the compiler does not inline the `Vec::push` method - // in this method. This generates two memset and one memcpy which are both very - // expansive. - // This would likely be solved by some kind of "placement-push" (like emplace in C++). - // In the mean time, a workaround is to "push" using `.resize_with` and `::uninit()` to - // avoid spurious copying. - // Is this optimization beneficial when targeting non-WASM platforms? - // - // NOTE: impulse_joints have the same problem, but it is not easy to refactor the code that way - // for the moment. - #[cfg(target_arch = "wasm32")] - let constraint = if insert_at.is_none() { - let new_len = out_constraints.len() + 1; - unsafe { - #[allow(invalid_value)] - out_constraints.resize_with(new_len, || { - AnyVelocityConstraint::Nongrouped( - std::mem::MaybeUninit::uninit().assume_init(), - ) - }); - } - out_constraints - .last_mut() - .unwrap() - .as_nongrouped_mut() - .unwrap() - } else { - unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable. - }; - - #[cfg(target_arch = "wasm32")] - { - constraint.dir1 = force_dir1; - #[cfg(feature = "dim3")] - { - constraint.tangent1 = tangents1[0]; - } - constraint.im1 = mprops1.effective_inv_mass; - constraint.im2 = mprops2.effective_inv_mass; - constraint.cfm_factor = cfm_factor; - constraint.limit = 0.0; - constraint.mj_lambda1 = mj_lambda1; - constraint.mj_lambda2 = mj_lambda2; - constraint.manifold_id = manifold_id; - constraint.manifold_contact_id = [0; MAX_MANIFOLD_POINTS]; - constraint.num_contacts = manifold_points.len() as u8; - } - - for k in 0..manifold_points.len() { - let manifold_point = &manifold_points[k]; - let dp1 = manifold_point.point - mprops1.world_com; - let dp2 = manifold_point.point - mprops2.world_com; - - let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); - let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); - - constraint.limit = manifold_point.friction; - constraint.manifold_contact_id[k] = manifold_point.contact_id; - - // Normal part. - { - let gcross1 = mprops1 - .effective_world_inv_inertia_sqrt - .transform_vector(dp1.gcross(force_dir1)); - let gcross2 = mprops2 - .effective_world_inv_inertia_sqrt - .transform_vector(dp2.gcross(-force_dir1)); - - let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass; - let projected_mass = utils::inv( - force_dir1.dot(&imsum.component_mul(&force_dir1)) - + gcross1.gdot(gcross1) - + gcross2.gdot(gcross2), - ); - - let is_bouncy = manifold_point.is_bouncy() as u32 as Real; - let is_resting = 1.0 - is_bouncy; - - let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) - * (vel1 - vel2).dot(&force_dir1); - rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt; - rhs_wo_bias *= is_bouncy + is_resting; - let rhs_bias = /* is_resting - * */ erp_inv_dt - * (manifold_point.dist + params.allowed_linear_error).clamp(-params.max_penetration_correction, 0.0); - - let rhs = rhs_wo_bias + rhs_bias; - is_fast_contact = is_fast_contact || (-rhs * params.dt > ccd_thickness * 0.5); - - constraint.elements[k].normal_part = VelocityConstraintNormalPart { - gcross1, - gcross2, - rhs, - rhs_wo_bias, - impulse: na::zero(), - r: projected_mass, - }; - } - - // Tangent parts. - { - constraint.elements[k].tangent_part.impulse = na::zero(); - - for j in 0..DIM - 1 { - let gcross1 = mprops1 - .effective_world_inv_inertia_sqrt - .transform_vector(dp1.gcross(tangents1[j])); - let gcross2 = mprops2 - .effective_world_inv_inertia_sqrt - .transform_vector(dp2.gcross(-tangents1[j])); - let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass; - let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j])) - + gcross1.gdot(gcross1) - + gcross2.gdot(gcross2); - let rhs = - (vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]); - - constraint.elements[k].tangent_part.gcross1[j] = gcross1; - constraint.elements[k].tangent_part.gcross2[j] = gcross2; - constraint.elements[k].tangent_part.rhs[j] = rhs; - constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") { - utils::inv(r) - } else { - r - }; - } - - #[cfg(feature = "dim3")] - { - constraint.elements[k].tangent_part.r[2] = 2.0 - * (constraint.elements[k].tangent_part.gcross1[0] - .gdot(constraint.elements[k].tangent_part.gcross1[1]) - + constraint.elements[k].tangent_part.gcross2[0] - .gdot(constraint.elements[k].tangent_part.gcross2[1])); - } - } - } - - constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor }; - - #[cfg(not(target_arch = "wasm32"))] - if let Some(at) = insert_at { - out_constraints[at + _l] = AnyVelocityConstraint::Nongrouped(constraint); - } else { - out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint)); - } - } - } - - pub fn solve( - &mut self, - mj_lambdas: &mut [DeltaVel], - solve_normal: bool, - solve_friction: bool, - ) { - let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - - VelocityConstraintElement::solve_group( - self.cfm_factor, - &mut self.elements[..self.num_contacts as usize], - &self.dir1, - #[cfg(feature = "dim3")] - &self.tangent1, - &self.im1, - &self.im2, - self.limit, - &mut mj_lambda1, - &mut mj_lambda2, - solve_normal, - solve_friction, - ); - - mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; - mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; - } - - pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { - let manifold = &mut manifolds_all[self.manifold_id]; - - for k in 0..self.num_contacts as usize { - let contact_id = self.manifold_contact_id[k]; - let active_contact = &mut manifold.points[contact_id as usize]; - active_contact.data.impulse = self.elements[k].normal_part.impulse; - - #[cfg(feature = "dim2")] - { - active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0]; - } - #[cfg(feature = "dim3")] - { - active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse; - } - } - } - - pub fn remove_cfm_and_bias_from_rhs(&mut self) { - self.cfm_factor = 1.0; - for elt in &mut self.elements { - elt.normal_part.rhs = elt.normal_part.rhs_wo_bias; - } - } -} - -#[inline(always)] -#[cfg(feature = "dim3")] -pub(crate) fn compute_tangent_contact_directions( - force_dir1: &Vector, - linvel1: &Vector, - linvel2: &Vector, -) -> [Vector; DIM - 1] -where - N: utils::WReal, - Vector: WBasis, -{ - use na::SimdValue; - - // Compute the tangent direction. Pick the direction of - // the linear relative velocity, if it is not too small. - // Otherwise use a fallback direction. - let relative_linvel = linvel1 - linvel2; - let mut tangent_relative_linvel = - relative_linvel - force_dir1 * (force_dir1.dot(&relative_linvel)); - - let tangent_linvel_norm = { - let _disable_fe_except = - crate::utils::DisableFloatingPointExceptionsFlags::disable_floating_point_exceptions(); - tangent_relative_linvel.normalize_mut() - }; - - const THRESHOLD: Real = 1.0e-4; - let use_fallback = tangent_linvel_norm.simd_lt(N::splat(THRESHOLD)); - let tangent_fallback = force_dir1.orthonormal_vector(); - - let tangent1 = tangent_fallback.select(use_fallback, tangent_relative_linvel); - let bitangent1 = force_dir1.cross(&tangent1); - - [tangent1, bitangent1] -} diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs deleted file mode 100644 index 30afef5e5..000000000 --- a/src/dynamics/solver/velocity_constraint_element.rs +++ /dev/null @@ -1,211 +0,0 @@ -use super::DeltaVel; -use crate::math::{AngVector, Vector, DIM}; -use crate::utils::{WBasis, WDot, WReal}; - -#[derive(Copy, Clone, Debug)] -pub(crate) struct VelocityConstraintTangentPart { - pub gcross1: [AngVector; DIM - 1], - pub gcross2: [AngVector; DIM - 1], - pub rhs: [N; DIM - 1], - #[cfg(feature = "dim2")] - pub impulse: na::Vector1, - #[cfg(feature = "dim3")] - pub impulse: na::Vector2, - #[cfg(feature = "dim2")] - pub r: [N; 1], - #[cfg(feature = "dim3")] - pub r: [N; DIM], -} - -impl VelocityConstraintTangentPart { - fn zero() -> Self { - Self { - gcross1: [na::zero(); DIM - 1], - gcross2: [na::zero(); DIM - 1], - rhs: [na::zero(); DIM - 1], - impulse: na::zero(), - #[cfg(feature = "dim2")] - r: [na::zero(); 1], - #[cfg(feature = "dim3")] - r: [na::zero(); DIM], - } - } - - #[inline] - pub fn solve( - &mut self, - tangents1: [&Vector; DIM - 1], - im1: &Vector, - im2: &Vector, - limit: N, - mj_lambda1: &mut DeltaVel, - mj_lambda2: &mut DeltaVel, - ) where - AngVector: WDot, Result = N>, - { - #[cfg(feature = "dim2")] - { - let dvel = tangents1[0].dot(&mj_lambda1.linear) - + self.gcross1[0].gdot(mj_lambda1.angular) - - tangents1[0].dot(&mj_lambda2.linear) - + self.gcross2[0].gdot(mj_lambda2.angular) - + self.rhs[0]; - let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit); - let dlambda = new_impulse - self.impulse[0]; - self.impulse[0] = new_impulse; - - mj_lambda1.linear += tangents1[0].component_mul(im1) * dlambda; - mj_lambda1.angular += self.gcross1[0] * dlambda; - - mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda; - mj_lambda2.angular += self.gcross2[0] * dlambda; - } - - #[cfg(feature = "dim3")] - { - let dvel_0 = tangents1[0].dot(&mj_lambda1.linear) - + self.gcross1[0].gdot(mj_lambda1.angular) - - tangents1[0].dot(&mj_lambda2.linear) - + self.gcross2[0].gdot(mj_lambda2.angular) - + self.rhs[0]; - let dvel_1 = tangents1[1].dot(&mj_lambda1.linear) - + self.gcross1[1].gdot(mj_lambda1.angular) - - tangents1[1].dot(&mj_lambda2.linear) - + self.gcross2[1].gdot(mj_lambda2.angular) - + self.rhs[1]; - - let dvel_00 = dvel_0 * dvel_0; - let dvel_11 = dvel_1 * dvel_1; - let dvel_01 = dvel_0 * dvel_1; - let inv_lhs = (dvel_00 + dvel_11) - * crate::utils::simd_inv( - dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2], - ); - let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1]; - let new_impulse = self.impulse - delta_impulse; - let new_impulse = { - let _disable_fe_except = - crate::utils::DisableFloatingPointExceptionsFlags:: - disable_floating_point_exceptions(); - new_impulse.simd_cap_magnitude(limit) - }; - - let dlambda = new_impulse - self.impulse; - self.impulse = new_impulse; - - mj_lambda1.linear += tangents1[0].component_mul(im1) * dlambda[0] - + tangents1[1].component_mul(im1) * dlambda[1]; - mj_lambda1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1]; - - mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda[0] - + tangents1[1].component_mul(im2) * -dlambda[1]; - mj_lambda2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1]; - } - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct VelocityConstraintNormalPart { - pub gcross1: AngVector, - pub gcross2: AngVector, - pub rhs: N, - pub rhs_wo_bias: N, - pub impulse: N, - pub r: N, -} - -impl VelocityConstraintNormalPart { - fn zero() -> Self { - Self { - gcross1: na::zero(), - gcross2: na::zero(), - rhs: na::zero(), - rhs_wo_bias: na::zero(), - impulse: na::zero(), - r: na::zero(), - } - } - - #[inline] - pub fn solve( - &mut self, - cfm_factor: N, - dir1: &Vector, - im1: &Vector, - im2: &Vector, - mj_lambda1: &mut DeltaVel, - mj_lambda2: &mut DeltaVel, - ) where - AngVector: WDot, Result = N>, - { - let dvel = dir1.dot(&mj_lambda1.linear) + self.gcross1.gdot(mj_lambda1.angular) - - dir1.dot(&mj_lambda2.linear) - + self.gcross2.gdot(mj_lambda2.angular) - + self.rhs; - let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero()); - let dlambda = new_impulse - self.impulse; - self.impulse = new_impulse; - - mj_lambda1.linear += dir1.component_mul(im1) * dlambda; - mj_lambda1.angular += self.gcross1 * dlambda; - - mj_lambda2.linear += dir1.component_mul(im2) * -dlambda; - mj_lambda2.angular += self.gcross2 * dlambda; - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct VelocityConstraintElement { - pub normal_part: VelocityConstraintNormalPart, - pub tangent_part: VelocityConstraintTangentPart, -} - -impl VelocityConstraintElement { - pub fn zero() -> Self { - Self { - normal_part: VelocityConstraintNormalPart::zero(), - tangent_part: VelocityConstraintTangentPart::zero(), - } - } - - #[inline] - pub fn solve_group( - cfm_factor: N, - elements: &mut [Self], - dir1: &Vector, - #[cfg(feature = "dim3")] tangent1: &Vector, - im1: &Vector, - im2: &Vector, - limit: N, - mj_lambda1: &mut DeltaVel, - mj_lambda2: &mut DeltaVel, - solve_normal: bool, - solve_friction: bool, - ) where - Vector: WBasis, - AngVector: WDot, Result = N>, - { - // Solve penetration. - if solve_normal { - for element in elements.iter_mut() { - element - .normal_part - .solve(cfm_factor, &dir1, im1, im2, mj_lambda1, mj_lambda2); - } - } - - // Solve friction. - if solve_friction { - #[cfg(feature = "dim3")] - let tangents1 = [tangent1, &dir1.cross(&tangent1)]; - #[cfg(feature = "dim2")] - let tangents1 = [&dir1.orthonormal_vector()]; - - for element in elements.iter_mut() { - let limit = limit * element.normal_part.impulse; - let part = &mut element.tangent_part; - part.solve(tangents1, im1, im2, limit, mj_lambda1, mj_lambda2); - } - } - } -} diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs deleted file mode 100644 index 83bbd7efc..000000000 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ /dev/null @@ -1,281 +0,0 @@ -use super::{ - AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement, - VelocityGroundConstraintNormalPart, -}; -use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; -#[cfg(feature = "dim2")] -use crate::utils::WBasis; -use crate::utils::{self, WAngularInertia, WCross, WDot}; - -use crate::dynamics::{IntegrationParameters, RigidBodySet, RigidBodyVelocity}; -use crate::geometry::{ContactManifold, ContactManifoldIndex}; - -#[derive(Copy, Clone, Debug)] -pub(crate) struct VelocityGroundConstraint { - pub mj_lambda2: usize, - pub dir1: Vector, // Non-penetration force direction for the first body. - #[cfg(feature = "dim3")] - pub tangent1: Vector, // One of the friction force directions. - pub im2: Vector, - pub cfm_factor: Real, - pub limit: Real, - pub elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS], - - pub manifold_id: ContactManifoldIndex, - pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS], - pub num_contacts: u8, -} - -impl VelocityGroundConstraint { - pub fn generate( - params: &IntegrationParameters, - manifold_id: ContactManifoldIndex, - manifold: &ContactManifold, - bodies: &RigidBodySet, - out_constraints: &mut Vec, - insert_at: Option, - ) { - let cfm_factor = params.cfm_factor(); - let inv_dt = params.inv_dt(); - let erp_inv_dt = params.erp_inv_dt(); - - let mut handle1 = manifold.data.rigid_body1; - let mut handle2 = manifold.data.rigid_body2; - let flipped = manifold.data.relative_dominance < 0; - - let (force_dir1, flipped_multiplier) = if flipped { - std::mem::swap(&mut handle1, &mut handle2); - (manifold.data.normal, -1.0) - } else { - (-manifold.data.normal, 1.0) - }; - - let (vels1, world_com1) = if let Some(handle1) = handle1 { - let rb1 = &bodies[handle1]; - (rb1.vels, rb1.mprops.world_com) - } else { - (RigidBodyVelocity::zero(), Point::origin()) - }; - - let rb2 = &bodies[handle2.unwrap()]; - let vels2 = &rb2.vels; - let mprops2 = &rb2.mprops; - - #[cfg(feature = "dim2")] - let tangents1 = force_dir1.orthonormal_basis(); - #[cfg(feature = "dim3")] - let tangents1 = - super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel); - - let mj_lambda2 = rb2.ids.active_set_offset; - - for (_l, manifold_points) in manifold - .data - .solver_contacts - .chunks(MAX_MANIFOLD_POINTS) - .enumerate() - { - #[cfg(not(target_arch = "wasm32"))] - let mut constraint = VelocityGroundConstraint { - dir1: force_dir1, - #[cfg(feature = "dim3")] - tangent1: tangents1[0], - elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], - im2: mprops2.effective_inv_mass, - cfm_factor, - limit: 0.0, - mj_lambda2, - manifold_id, - manifold_contact_id: [0; MAX_MANIFOLD_POINTS], - num_contacts: manifold_points.len() as u8, - }; - - // TODO: this is a WIP optimization for WASM platforms. - // For some reasons, the compiler does not inline the `Vec::push` method - // in this method. This generates two memset and one memcpy which are both very - // expansive. - // This would likely be solved by some kind of "placement-push" (like emplace in C++). - // In the mean time, a workaround is to "push" using `.resize_with` and `::uninit()` to - // avoid spurious copying. - // Is this optimization beneficial when targeting non-WASM platforms? - // - // NOTE: impulse_joints have the same problem, but it is not easy to refactor the code that way - // for the moment. - #[cfg(target_arch = "wasm32")] - let constraint = if insert_at.is_none() { - let new_len = out_constraints.len() + 1; - unsafe { - #[allow(invalid_value)] - out_constraints.resize_with(new_len, || { - AnyVelocityConstraint::NongroupedGround( - std::mem::MaybeUninit::uninit().assume_init(), - ) - }); - } - out_constraints - .last_mut() - .unwrap() - .as_nongrouped_ground_mut() - .unwrap() - } else { - unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable. - }; - - #[cfg(target_arch = "wasm32")] - { - constraint.dir1 = force_dir1; - #[cfg(feature = "dim3")] - { - constraint.tangent1 = tangents1[0]; - } - constraint.im2 = mprops2.effective_inv_mass; - constraint.cfm_factor = cfm_factor; - constraint.limit = 0.0; - constraint.mj_lambda2 = mj_lambda2; - constraint.manifold_id = manifold_id; - constraint.manifold_contact_id = [0; MAX_MANIFOLD_POINTS]; - constraint.num_contacts = manifold_points.len() as u8; - } - - let mut is_fast_contact = false; - - for k in 0..manifold_points.len() { - let manifold_point = &manifold_points[k]; - let dp2 = manifold_point.point - mprops2.world_com; - let dp1 = manifold_point.point - world_com1; - let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); - let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); - - constraint.limit = manifold_point.friction; - constraint.manifold_contact_id[k] = manifold_point.contact_id; - - // Normal part. - { - let gcross2 = mprops2 - .effective_world_inv_inertia_sqrt - .transform_vector(dp2.gcross(-force_dir1)); - - let projected_mass = utils::inv( - force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) - + gcross2.gdot(gcross2), - ); - - let is_bouncy = manifold_point.is_bouncy() as u32 as Real; - let is_resting = 1.0 - is_bouncy; - - let dvel = (vel1 - vel2).dot(&force_dir1); - let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) * dvel; - rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt; - rhs_wo_bias *= is_bouncy + is_resting; - let rhs_bias = /* is_resting - * */ erp_inv_dt - * (manifold_point.dist + params.allowed_linear_error).clamp(-params.max_penetration_correction, 0.0); - - let rhs = rhs_wo_bias + rhs_bias; - is_fast_contact = - is_fast_contact || (-rhs * params.dt > rb2.ccd.ccd_thickness * 0.5); - - constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart { - gcross2, - rhs, - rhs_wo_bias, - impulse: na::zero(), - r: projected_mass, - }; - } - - // Tangent parts. - { - constraint.elements[k].tangent_part.impulse = na::zero(); - - for j in 0..DIM - 1 { - let gcross2 = mprops2 - .effective_world_inv_inertia_sqrt - .transform_vector(dp2.gcross(-tangents1[j])); - let r = tangents1[j] - .dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j])) - + gcross2.gdot(gcross2); - let rhs = (vel1 - vel2 - + flipped_multiplier * manifold_point.tangent_velocity) - .dot(&tangents1[j]); - - constraint.elements[k].tangent_part.gcross2[j] = gcross2; - constraint.elements[k].tangent_part.rhs[j] = rhs; - constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") { - utils::inv(r) - } else { - r - }; - } - - #[cfg(feature = "dim3")] - { - constraint.elements[k].tangent_part.r[2] = 2.0 - * constraint.elements[k].tangent_part.gcross2[0] - .gdot(constraint.elements[k].tangent_part.gcross2[1]); - } - } - } - - constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor }; - - #[cfg(not(target_arch = "wasm32"))] - if let Some(at) = insert_at { - out_constraints[at + _l] = AnyVelocityConstraint::NongroupedGround(constraint); - } else { - out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint)); - } - } - } - - pub fn solve( - &mut self, - mj_lambdas: &mut [DeltaVel], - solve_normal: bool, - solve_friction: bool, - ) { - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - - VelocityGroundConstraintElement::solve_group( - self.cfm_factor, - &mut self.elements[..self.num_contacts as usize], - &self.dir1, - #[cfg(feature = "dim3")] - &self.tangent1, - &self.im2, - self.limit, - &mut mj_lambda2, - solve_normal, - solve_friction, - ); - - mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; - } - - // FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint. - pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { - let manifold = &mut manifolds_all[self.manifold_id]; - - for k in 0..self.num_contacts as usize { - let contact_id = self.manifold_contact_id[k]; - let active_contact = &mut manifold.points[contact_id as usize]; - active_contact.data.impulse = self.elements[k].normal_part.impulse; - - #[cfg(feature = "dim2")] - { - active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0]; - } - #[cfg(feature = "dim3")] - { - active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse; - } - } - } - - pub fn remove_cfm_and_bias_from_rhs(&mut self) { - self.cfm_factor = 1.0; - for elt in &mut self.elements { - elt.normal_part.rhs = elt.normal_part.rhs_wo_bias; - } - } -} diff --git a/src/dynamics/solver/velocity_ground_constraint_element.rs b/src/dynamics/solver/velocity_ground_constraint_element.rs deleted file mode 100644 index 06a727a25..000000000 --- a/src/dynamics/solver/velocity_ground_constraint_element.rs +++ /dev/null @@ -1,181 +0,0 @@ -use super::DeltaVel; -use crate::math::{AngVector, Vector, DIM}; -use crate::utils::{WBasis, WDot, WReal}; - -#[derive(Copy, Clone, Debug)] -pub(crate) struct VelocityGroundConstraintTangentPart { - pub gcross2: [AngVector; DIM - 1], - pub rhs: [N; DIM - 1], - #[cfg(feature = "dim2")] - pub impulse: na::Vector1, - #[cfg(feature = "dim3")] - pub impulse: na::Vector2, - #[cfg(feature = "dim2")] - pub r: [N; 1], - #[cfg(feature = "dim3")] - pub r: [N; DIM], -} - -impl VelocityGroundConstraintTangentPart { - fn zero() -> Self { - Self { - gcross2: [na::zero(); DIM - 1], - rhs: [na::zero(); DIM - 1], - impulse: na::zero(), - #[cfg(feature = "dim2")] - r: [na::zero(); 1], - #[cfg(feature = "dim3")] - r: [na::zero(); DIM], - } - } - - #[inline] - pub fn solve( - &mut self, - tangents1: [&Vector; DIM - 1], - im2: &Vector, - limit: N, - mj_lambda2: &mut DeltaVel, - ) where - AngVector: WDot, Result = N>, - { - #[cfg(feature = "dim2")] - { - let dvel = -tangents1[0].dot(&mj_lambda2.linear) - + self.gcross2[0].gdot(mj_lambda2.angular) - + self.rhs[0]; - let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit); - let dlambda = new_impulse - self.impulse[0]; - self.impulse[0] = new_impulse; - - mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda; - mj_lambda2.angular += self.gcross2[0] * dlambda; - } - - #[cfg(feature = "dim3")] - { - let dvel_0 = -tangents1[0].dot(&mj_lambda2.linear) - + self.gcross2[0].gdot(mj_lambda2.angular) - + self.rhs[0]; - let dvel_1 = -tangents1[1].dot(&mj_lambda2.linear) - + self.gcross2[1].gdot(mj_lambda2.angular) - + self.rhs[1]; - - let dvel_00 = dvel_0 * dvel_0; - let dvel_11 = dvel_1 * dvel_1; - let dvel_01 = dvel_0 * dvel_1; - let inv_lhs = (dvel_00 + dvel_11) - * crate::utils::simd_inv( - dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2], - ); - let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1]; - let new_impulse = self.impulse - delta_impulse; - let new_impulse = { - let _disable_fe_except = - crate::utils::DisableFloatingPointExceptionsFlags:: - disable_floating_point_exceptions(); - new_impulse.simd_cap_magnitude(limit) - }; - let dlambda = new_impulse - self.impulse; - self.impulse = new_impulse; - - mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda[0] - + tangents1[1].component_mul(im2) * -dlambda[1]; - mj_lambda2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1]; - } - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct VelocityGroundConstraintNormalPart { - pub gcross2: AngVector, - pub rhs: N, - pub rhs_wo_bias: N, - pub impulse: N, - pub r: N, -} - -impl VelocityGroundConstraintNormalPart { - fn zero() -> Self { - Self { - gcross2: na::zero(), - rhs: na::zero(), - rhs_wo_bias: na::zero(), - impulse: na::zero(), - r: na::zero(), - } - } - - #[inline] - pub fn solve( - &mut self, - cfm_factor: N, - dir1: &Vector, - im2: &Vector, - mj_lambda2: &mut DeltaVel, - ) where - AngVector: WDot, Result = N>, - { - let dvel = -dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs; - let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero()); - let dlambda = new_impulse - self.impulse; - self.impulse = new_impulse; - - mj_lambda2.linear += dir1.component_mul(im2) * -dlambda; - mj_lambda2.angular += self.gcross2 * dlambda; - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct VelocityGroundConstraintElement { - pub normal_part: VelocityGroundConstraintNormalPart, - pub tangent_part: VelocityGroundConstraintTangentPart, -} - -impl VelocityGroundConstraintElement { - pub fn zero() -> Self { - Self { - normal_part: VelocityGroundConstraintNormalPart::zero(), - tangent_part: VelocityGroundConstraintTangentPart::zero(), - } - } - - #[inline] - pub fn solve_group( - cfm_factor: N, - elements: &mut [Self], - dir1: &Vector, - #[cfg(feature = "dim3")] tangent1: &Vector, - im2: &Vector, - limit: N, - mj_lambda2: &mut DeltaVel, - solve_normal: bool, - solve_friction: bool, - ) where - Vector: WBasis, - AngVector: WDot, Result = N>, - { - // Solve penetration. - if solve_normal { - for element in elements.iter_mut() { - element - .normal_part - .solve(cfm_factor, &dir1, im2, mj_lambda2); - } - } - - // Solve friction. - if solve_friction { - #[cfg(feature = "dim3")] - let tangents1 = [tangent1, &dir1.cross(&tangent1)]; - #[cfg(feature = "dim2")] - let tangents1 = [&dir1.orthonormal_vector()]; - - for element in elements.iter_mut() { - let limit = limit * element.normal_part.impulse; - let part = &mut element.tangent_part; - part.solve(tangents1, im2, limit, mj_lambda2); - } - } - } -} diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index d15ea68cc..8e3ebcd16 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -1,48 +1,99 @@ -use super::AnyJointVelocityConstraint; +use super::{JointConstraintTypes, SolverConstraintsSet}; +use crate::dynamics::solver::solver_body::SolverBody; use crate::dynamics::{ - solver::{AnyVelocityConstraint, DeltaVel}, - IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodySet, + solver::{ContactConstraintTypes, SolverVel}, + IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet, + MultibodyLinkId, RigidBodySet, }; -use crate::geometry::ContactManifold; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::Real; -use crate::utils::WAngularInertia; +use crate::prelude::RigidBodyVelocity; +use crate::utils::SimdAngularInertia; use na::DVector; pub(crate) struct VelocitySolver { - pub mj_lambdas: Vec>, - pub generic_mj_lambdas: DVector, + pub solver_bodies: Vec, + pub solver_vels: Vec>, + pub solver_vels_increment: Vec>, + pub generic_solver_vels: DVector, + pub generic_solver_vels_increment: DVector, + pub multibody_roots: Vec, } impl VelocitySolver { pub fn new() -> Self { Self { - mj_lambdas: Vec::new(), - generic_mj_lambdas: DVector::zeros(0), + solver_bodies: Vec::new(), + solver_vels: Vec::new(), + solver_vels_increment: Vec::new(), + generic_solver_vels: DVector::zeros(0), + generic_solver_vels_increment: DVector::zeros(0), + multibody_roots: Vec::new(), } } - pub fn solve( - &mut self, + pub fn init_constraints( + &self, island_id: usize, - params: &IntegrationParameters, islands: &IslandManager, bodies: &mut RigidBodySet, multibodies: &mut MultibodyJointSet, manifolds_all: &mut [&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], joints_all: &mut [JointGraphEdge], - contact_constraints: &mut [AnyVelocityConstraint], - generic_contact_jacobians: &DVector, - joint_constraints: &mut [AnyJointVelocityConstraint], - generic_joint_jacobians: &DVector, + joint_indices: &[JointIndex], + contact_constraints: &mut SolverConstraintsSet, + joint_constraints: &mut SolverConstraintsSet, ) { - self.mj_lambdas.clear(); - self.mj_lambdas - .resize(islands.active_island(island_id).len(), DeltaVel::zero()); + contact_constraints.init( + island_id, + islands, + bodies, + multibodies, + manifolds_all, + manifold_indices, + ); - let total_multibodies_ndofs = multibodies.multibodies.iter().map(|m| m.1.ndofs()).sum(); - self.generic_mj_lambdas = DVector::zeros(total_multibodies_ndofs); + joint_constraints.init( + island_id, + islands, + bodies, + multibodies, + joints_all, + joint_indices, + ); + } - // Initialize delta-velocities (`mj_lambdas`) with external forces (gravity etc): + pub fn init_solver_velocities_and_solver_bodies( + &mut self, + params: &IntegrationParameters, + island_id: usize, + islands: &IslandManager, + bodies: &mut RigidBodySet, + multibodies: &mut MultibodyJointSet, + ) { + self.multibody_roots.clear(); + self.solver_bodies.clear(); + self.solver_bodies.resize( + islands.active_island(island_id).len(), + SolverBody::default(), + ); + + self.solver_vels_increment.clear(); + self.solver_vels_increment + .resize(islands.active_island(island_id).len(), SolverVel::zero()); + self.solver_vels.clear(); + self.solver_vels + .resize(islands.active_island(island_id).len(), SolverVel::zero()); + + /* + * Initialize solver bodies and delta-velocities (`solver_vels_increment`) with external forces (gravity etc): + * NOTE: we compute this only once by neglecting changes of mass matrices. + */ + + // Assign solver ids to multibodies, and collect the relevant roots. + // And init solver_vels for rigidb-bodies. + let mut multibody_solver_id = 0; for handle in islands.active_island(island_id) { if let Some(link) = multibodies.rigid_body_link(*handle).copied() { let multibody = multibodies @@ -50,196 +101,213 @@ impl VelocitySolver { .unwrap(); if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic { - let mut mj_lambdas = self - .generic_mj_lambdas - .rows_mut(multibody.solver_id, multibody.ndofs()); - mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0); + multibody.solver_id = multibody_solver_id; + multibody_solver_id += multibody.ndofs(); + self.multibody_roots.push(link); } } else { let rb = &bodies[*handle]; - let dvel = &mut self.mj_lambdas[rb.ids.active_set_offset]; + let solver_vel = &mut self.solver_vels[rb.ids.active_set_offset]; + let solver_vel_incr = &mut self.solver_vels_increment[rb.ids.active_set_offset]; + let solver_body = &mut self.solver_bodies[rb.ids.active_set_offset]; + solver_body.copy_from(rb); // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied // by the square root of the inertia tensor: - dvel.angular += + solver_vel_incr.angular = rb.mprops.effective_world_inv_inertia_sqrt * rb.forces.torque * params.dt; - dvel.linear += + solver_vel_incr.linear = rb.forces.force.component_mul(&rb.mprops.effective_inv_mass) * params.dt; + + solver_vel.linear = rb.vels.linvel; + // PERF: can we avoid the call to effective_angular_inertia_sqrt? + solver_vel.angular = rb.mprops.effective_angular_inertia_sqrt() * rb.vels.angvel; } } - /* - * Solve constraints. - */ - for i in 0..params.max_velocity_iterations { - let solve_friction = params.interleave_restitution_and_friction_resolution - && params.max_velocity_friction_iterations + i >= params.max_velocity_iterations; - - for constraint in &mut *joint_constraints { - constraint.solve( - generic_joint_jacobians, - &mut self.mj_lambdas[..], - &mut self.generic_mj_lambdas, - ); - } + // PERF: don’t reallocate at each iteration. + self.generic_solver_vels_increment = DVector::zeros(multibody_solver_id); + self.generic_solver_vels = DVector::zeros(multibody_solver_id); - for constraint in &mut *contact_constraints { - constraint.solve( - generic_contact_jacobians, - &mut self.mj_lambdas[..], - &mut self.generic_mj_lambdas, - true, - false, - ); - } + // init solver_vels for multibodies. + for link in &self.multibody_roots { + let multibody = multibodies + .get_multibody_mut_internal(link.multibody) + .unwrap(); + multibody.update_dynamics(params.dt, bodies); + multibody.update_acceleration(bodies); - if solve_friction { - for constraint in &mut *contact_constraints { - constraint.solve( - generic_contact_jacobians, - &mut self.mj_lambdas[..], - &mut self.generic_mj_lambdas, - false, - true, - ); - } - } + let mut solver_vels_incr = self + .generic_solver_vels_increment + .rows_mut(multibody.solver_id, multibody.ndofs()); + let mut solver_vels = self + .generic_solver_vels + .rows_mut(multibody.solver_id, multibody.ndofs()); + + solver_vels_incr.axpy(params.dt, &multibody.accelerations, 0.0); + solver_vels.copy_from(&multibody.velocities); } + } - let remaining_friction_iterations = - if !params.interleave_restitution_and_friction_resolution { - params.max_velocity_friction_iterations - } else if params.max_velocity_friction_iterations > params.max_velocity_iterations { - params.max_velocity_friction_iterations - params.max_velocity_iterations - } else { - 0 - }; + pub fn solve_constraints( + &mut self, + params: &IntegrationParameters, + num_solver_iterations: usize, + bodies: &mut RigidBodySet, + multibodies: &mut MultibodyJointSet, + contact_constraints: &mut SolverConstraintsSet, + joint_constraints: &mut SolverConstraintsSet, + ) { + for small_step_id in 0..num_solver_iterations { + let is_last_small_step = small_step_id == num_solver_iterations - 1; - for _ in 0..remaining_friction_iterations { - for constraint in &mut *contact_constraints { - constraint.solve( - generic_contact_jacobians, - &mut self.mj_lambdas[..], - &mut self.generic_mj_lambdas, - false, - true, - ); + for (solver_vels, incr) in self + .solver_vels + .iter_mut() + .zip(self.solver_vels_increment.iter()) + { + solver_vels.linear += incr.linear; + solver_vels.angular += incr.angular; } - } - // Integrate positions. - for handle in islands.active_island(island_id) { - if let Some(link) = multibodies.rigid_body_link(*handle).copied() { - let multibody = multibodies - .get_multibody_mut_internal(link.multibody) - .unwrap(); + self.generic_solver_vels += &self.generic_solver_vels_increment; - if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic { - let mj_lambdas = self - .generic_mj_lambdas - .rows(multibody.solver_id, multibody.ndofs()); - let prev_vels = multibody.velocities.clone(); // FIXME: avoid allocations. - multibody.velocities += mj_lambdas; - multibody.integrate(params.dt); - multibody.forward_kinematics(bodies, false); - multibody.velocities = prev_vels; - } + /* + * Update & solve constraints. + */ + joint_constraints.update(¶ms, multibodies, &self.solver_bodies); + contact_constraints.update(¶ms, small_step_id, multibodies, &self.solver_bodies); + + joint_constraints.solve(&mut self.solver_vels, &mut self.generic_solver_vels); + contact_constraints + .solve_restitution(&mut self.solver_vels, &mut self.generic_solver_vels); + + let num_friction_iterations = if is_last_small_step { + params.num_friction_iteration_per_solver_iteration * num_solver_iterations + - (num_solver_iterations - 1) } else { - let rb = bodies.index_mut_internal(*handle); + 1 + }; - let dvel = self.mj_lambdas[rb.ids.active_set_offset]; - let dangvel = rb - .mprops - .effective_world_inv_inertia_sqrt - .transform_vector(dvel.angular); - - // Update positions. - let mut new_pos = rb.pos; - let mut new_vels = rb.vels; - new_vels.linvel += dvel.linear; - new_vels.angvel += dangvel; - new_vels = new_vels.apply_damping(params.dt, &rb.damping); - new_pos.next_position = new_vels.integrate( - params.dt, - &rb.pos.position, - &rb.mprops.local_mprops.local_com, - ); - rb.integrated_vels = new_vels; - rb.pos = new_pos; + for _ in 0..num_friction_iterations { + contact_constraints + .solve_friction(&mut self.solver_vels, &mut self.generic_solver_vels); } - } - for joint in &mut *joint_constraints { - joint.remove_bias_from_rhs(); + /* + * Integrate positions. + */ + self.integrate_positions(¶ms, is_last_small_step, bodies, multibodies); + + /* + * Resolution without bias. + */ + joint_constraints.solve_wo_bias(&mut self.solver_vels, &mut self.generic_solver_vels); + contact_constraints + .solve_restitution_wo_bias(&mut self.solver_vels, &mut self.generic_solver_vels); } - for constraint in &mut *contact_constraints { - constraint.remove_bias_from_rhs(); + } + + pub fn integrate_positions( + &mut self, + params: &IntegrationParameters, + is_last_small_step: bool, + bodies: &mut RigidBodySet, + multibodies: &mut MultibodyJointSet, + ) { + // Integrate positions. + for (solver_vels, solver_body) in self.solver_vels.iter().zip(self.solver_bodies.iter_mut()) + { + let linvel = solver_vels.linear; + let angvel = solver_body.sqrt_ii.transform_vector(solver_vels.angular); + + let mut new_vels = RigidBodyVelocity { linvel, angvel }; + new_vels = new_vels.apply_damping(params.dt, &solver_body.damping); + let new_pos = + new_vels.integrate(params.dt, &solver_body.position, &solver_body.local_com); + solver_body.integrated_vels += new_vels; + solver_body.position = new_pos; + solver_body.world_com = new_pos * solver_body.local_com; } - for _ in 0..params.max_stabilization_iterations { - for constraint in &mut *joint_constraints { - constraint.solve( - generic_joint_jacobians, - &mut self.mj_lambdas[..], - &mut self.generic_mj_lambdas, - ); - } + // Integrate multibody positions. + for link in &self.multibody_roots { + let multibody = multibodies + .get_multibody_mut_internal(link.multibody) + .unwrap(); + let solver_vels = self + .generic_solver_vels + .rows(multibody.solver_id, multibody.ndofs()); + multibody.velocities.copy_from(&solver_vels); + multibody.integrate(params.dt); + // PERF: we could have a mode where it doesn’t write back to the `bodies` yet. + multibody.forward_kinematics(bodies, !is_last_small_step); - for constraint in &mut *contact_constraints { - constraint.solve( - generic_contact_jacobians, - &mut self.mj_lambdas[..], - &mut self.generic_mj_lambdas, - true, - false, - ); - } + if !is_last_small_step { + // These are very expensive and not needed if we don’t + // have to run another step. + multibody.update_dynamics(params.dt, bodies); + multibody.update_acceleration(bodies); - for constraint in &mut *contact_constraints { - constraint.solve( - generic_contact_jacobians, - &mut self.mj_lambdas[..], - &mut self.generic_mj_lambdas, - false, - true, - ); + let mut solver_vels_incr = self + .generic_solver_vels_increment + .rows_mut(multibody.solver_id, multibody.ndofs()); + solver_vels_incr.axpy(params.dt, &multibody.accelerations, 0.0); } } + } - // Update velocities. + pub fn writeback_bodies( + &mut self, + params: &IntegrationParameters, + num_solver_iterations: usize, + islands: &IslandManager, + island_id: usize, + bodies: &mut RigidBodySet, + multibodies: &mut MultibodyJointSet, + ) { for handle in islands.active_island(island_id) { - if let Some(link) = multibodies.rigid_body_link(*handle).copied() { + let link = if self.multibody_roots.is_empty() { + None + } else { + multibodies.rigid_body_link(*handle).copied() + }; + + if let Some(link) = link { let multibody = multibodies .get_multibody_mut_internal(link.multibody) .unwrap(); if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic { - let mj_lambdas = self - .generic_mj_lambdas + let solver_vels = self + .generic_solver_vels .rows(multibody.solver_id, multibody.ndofs()); - multibody.velocities += mj_lambdas; + multibody.velocities.copy_from(&solver_vels); } } else { let rb = bodies.index_mut_internal(*handle); - let dvel = self.mj_lambdas[rb.ids.active_set_offset]; - let dangvel = rb - .mprops - .effective_world_inv_inertia_sqrt - .transform_vector(dvel.angular); - - rb.vels.linvel += dvel.linear; - rb.vels.angvel += dangvel; - rb.vels = rb.vels.apply_damping(params.dt, &rb.damping); - } - } + let solver_body = &self.solver_bodies[rb.ids.active_set_offset]; + let solver_vels = &self.solver_vels[rb.ids.active_set_offset]; - // Write impulses back into the manifold structures. - for constraint in &*joint_constraints { - constraint.writeback_impulses(joints_all); - } + let dangvel = solver_body.sqrt_ii.transform_vector(solver_vels.angular); - for constraint in &*contact_constraints { - constraint.writeback_impulses(manifolds_all); + let mut new_vels = RigidBodyVelocity { + linvel: solver_vels.linear, + angvel: dangvel, + }; + new_vels = new_vels.apply_damping(params.dt, &solver_body.damping); + + // NOTE: using integrated_vels instead of interpolation is interesting for + // high angular velocities. However, it is a bit inexact due to the + // solver integrating at intermediate sub-steps. Should we just switch + // to interpolation? + rb.integrated_vels.linvel = + solver_body.integrated_vels.linvel / num_solver_iterations as Real; + rb.integrated_vels.angvel = + solver_body.integrated_vels.angvel / num_solver_iterations as Real; + rb.vels = new_vels; + rb.pos.next_position = solver_body.position; + } } } } diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index 3bef40da2..44730a028 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -283,7 +283,7 @@ pub struct ContactManifoldData { pub struct SolverContact { /// The index of the manifold contact used to generate this solver contact. pub(crate) contact_id: u8, - /// The world-space contact point. + /// The contact point in world-space. pub point: Point, /// The distance between the two original contacts points along the contact normal. /// If negative, this is measures the penetration depth. diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 4500284db..8b5f0c906 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -927,6 +927,7 @@ impl NarrowPhase { for manifold in &mut pair.manifolds { let world_pos1 = manifold.subshape_pos1.prepend_to(&co1.pos); + let world_pos2 = manifold.subshape_pos2.prepend_to(&co2.pos); manifold.data.solver_contacts.clear(); manifold.data.rigid_body1 = co1.parent.map(|p| p.handle); manifold.data.rigid_body2 = co2.parent.map(|p| p.handle); @@ -944,10 +945,13 @@ impl NarrowPhase { if contact.dist < prediction_distance { // Generate the solver contact. + let world_pt1 = world_pos1 * contact.local_p1; + let world_pt2 = world_pos2 * contact.local_p2; + let effective_point = na::center(&world_pt1, &world_pt2); + let solver_contact = SolverContact { contact_id: contact_id as u8, - point: world_pos1 * contact.local_p1 - + manifold.data.normal * contact.dist / 2.0, + point: effective_point, dist: contact.dist, friction, restitution, diff --git a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs index f214b8cf3..706c810c2 100644 --- a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs +++ b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs @@ -8,7 +8,7 @@ use crate::geometry::{Cone, Cylinder}; use crate::math::{Isometry, Point, Real, Vector, DIM}; use crate::pipeline::debug_render_pipeline::debug_render_backend::DebugRenderObject; use crate::pipeline::debug_render_pipeline::DebugRenderStyle; -use crate::utils::WBasis; +use crate::utils::SimdBasis; use std::any::TypeId; use std::collections::HashMap; @@ -221,7 +221,7 @@ impl DebugRenderPipeline { } if self.mode.contains(DebugRenderMode::MULTIBODY_JOINTS) { - for (handle, multibody, link) in multibody_joints.iter() { + for (handle, _, multibody, link) in multibody_joints.iter() { let anc_color = self.style.multibody_joint_anchor_color; let sep_color = self.style.multibody_joint_separation_color; let parent = multibody.link(link.parent_id().unwrap()).unwrap(); diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 74232961a..5ff78fd70 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -1,14 +1,14 @@ //! Physics pipeline structures. use crate::counters::Counters; -#[cfg(not(feature = "parallel"))] +// #[cfg(not(feature = "parallel"))] use crate::dynamics::IslandSolver; +#[cfg(feature = "parallel")] +use crate::dynamics::JointGraphEdge; use crate::dynamics::{ CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, RigidBodyChanges, RigidBodyHandle, RigidBodyPosition, RigidBodyType, }; -#[cfg(feature = "parallel")] -use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; use crate::geometry::{ BroadPhase, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair, ContactManifoldIndex, NarrowPhase, TemporaryInteractionIndex, @@ -206,19 +206,14 @@ impl PhysicsPipeline { self.counters.stages.update_time.resume(); for handle in islands.active_dynamic_bodies() { + // TODO: should that be moved to the solver (just like we moved + // the multibody dynamics update) since it depends on dt? let rb = bodies.index_mut_internal(*handle); rb.mprops.update_world_mass_properties(&rb.pos.position); let effective_mass = rb.mprops.effective_mass(); rb.forces .compute_effective_force_and_torque(&gravity, &effective_mass); } - - for multibody in &mut multibody_joints.multibodies { - multibody - .1 - .update_dynamics(integration_parameters.dt, bodies); - multibody.1.update_acceleration(bodies); - } self.counters.stages.update_time.pause(); self.counters.stages.solver_time.resume(); @@ -263,7 +258,11 @@ impl PhysicsPipeline { let manifold_indices = &self.manifold_indices[..]; let joint_constraint_indices = &self.joint_constraint_indices[..]; - rayon::scope(|scope| { + // PERF: right now, we are only doing islands-based parallelism. + // Intra-island parallelism (that hasn’t been ported to the new + // solver yet) will be supported in the future. + self.counters.solver.velocity_resolution_time.resume(); + rayon::scope(|_scope| { enable_flush_to_zero!(); solvers @@ -280,13 +279,14 @@ impl PhysicsPipeline { std::mem::transmute(multibody_joints.load(Ordering::Relaxed)) }; + let mut counters = Counters::new(false); solver.init_and_solve( - scope, island_id, - islands, + &mut counters, integration_parameters, + islands, bodies, - manifolds, + &mut manifolds[..], &manifold_indices[island_id], impulse_joints, &joint_constraint_indices[island_id], @@ -294,6 +294,7 @@ impl PhysicsPipeline { ) }); }); + self.counters.solver.velocity_resolution_time.pause(); } // Generate contact force events if needed. diff --git a/src/utils.rs b/src/utils.rs index 90401a48d..c2894b900 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -18,9 +18,9 @@ use { /// The trait for real numbers used by Rapier. /// /// This includes `f32`, `f64` and their related SIMD types. -pub trait WReal: SimdRealField + Copy {} -impl WReal for Real {} -impl WReal for SimdReal {} +pub trait SimdRealCopy: SimdRealField + Copy {} +impl SimdRealCopy for Real {} +impl SimdRealCopy for SimdReal {} const INV_EPSILON: Real = 1.0e-20; @@ -32,19 +32,19 @@ pub(crate) fn inv(val: Real) -> Real { } } -pub(crate) fn simd_inv(val: N) -> N { +pub(crate) fn simd_inv(val: N) -> N { let eps = N::splat(INV_EPSILON); N::zero().select(val.simd_gt(-eps) & val.simd_lt(eps), N::one() / val) } /// Trait to copy the sign of each component of one scalar/vector/matrix to another. -pub trait WSign: Sized { +pub trait SimdSign: Sized { // See SIMD implementations of copy_sign there: https://stackoverflow.com/a/57872652 /// Copy the sign of each component of `self` to the corresponding component of `to`. fn copy_sign_to(self, to: Rhs) -> Rhs; } -impl WSign for Real { +impl SimdSign for Real { fn copy_sign_to(self, to: Self) -> Self { const MINUS_ZERO: Real = -0.0; let signbit = MINUS_ZERO.to_bits(); @@ -52,13 +52,13 @@ impl WSign for Real { } } -impl> WSign> for N { +impl> SimdSign> for N { fn copy_sign_to(self, to: Vector2) -> Vector2 { Vector2::new(self.copy_sign_to(to.x), self.copy_sign_to(to.y)) } } -impl> WSign> for N { +impl> SimdSign> for N { fn copy_sign_to(self, to: Vector3) -> Vector3 { Vector3::new( self.copy_sign_to(to.x), @@ -68,13 +68,13 @@ impl> WSign> for N { } } -impl> WSign> for Vector2 { +impl> SimdSign> for Vector2 { fn copy_sign_to(self, to: Vector2) -> Vector2 { Vector2::new(self.x.copy_sign_to(to.x), self.y.copy_sign_to(to.y)) } } -impl> WSign> for Vector3 { +impl> SimdSign> for Vector3 { fn copy_sign_to(self, to: Vector3) -> Vector3 { Vector3::new( self.x.copy_sign_to(to.x), @@ -84,20 +84,20 @@ impl> WSign> for Vector3 { } } -impl WSign for SimdReal { +impl SimdSign for SimdReal { fn copy_sign_to(self, to: SimdReal) -> SimdReal { to.simd_copysign(self) } } -pub(crate) trait WComponent: Sized { +pub(crate) trait SimdComponent: Sized { type Element; fn min_component(self) -> Self::Element; fn max_component(self) -> Self::Element; } -impl WComponent for Real { +impl SimdComponent for Real { type Element = Real; fn min_component(self) -> Self::Element { @@ -108,7 +108,7 @@ impl WComponent for Real { } } -impl WComponent for SimdReal { +impl SimdComponent for SimdReal { type Element = Real; fn min_component(self) -> Self::Element { @@ -120,7 +120,7 @@ impl WComponent for SimdReal { } /// Trait to compute the orthonormal basis of a vector. -pub trait WBasis: Sized { +pub trait SimdBasis: Sized { /// The type of the array of orthonormal vectors. type Basis; /// Computes the vectors which, when combined with `self`, form an orthonormal basis. @@ -129,7 +129,7 @@ pub trait WBasis: Sized { fn orthonormal_vector(self) -> Self; } -impl WBasis for Vector2 { +impl SimdBasis for Vector2 { type Basis = [Vector2; 1]; fn orthonormal_basis(self) -> [Vector2; 1] { [Vector2::new(-self.y, self.x)] @@ -139,7 +139,7 @@ impl WBasis for Vector2 { } } -impl> WBasis for Vector3 { +impl> SimdBasis for Vector3 { type Basis = [Vector3; 2]; // Robust and branchless implementation from Pixar: // https://graphics.pixar.com/library/OrthonormalB/paper.pdf @@ -166,14 +166,14 @@ impl> WBasis for Vector3 { } } -pub(crate) trait WVec: Sized { +pub(crate) trait SimdVec: Sized { type Element; fn horizontal_inf(&self) -> Self::Element; fn horizontal_sup(&self) -> Self::Element; } -impl WVec for Vector2 +impl SimdVec for Vector2 where N::Element: Scalar, { @@ -188,7 +188,7 @@ where } } -impl WVec for Point2 +impl SimdVec for Point2 where N::Element: Scalar, { @@ -203,7 +203,7 @@ where } } -impl WVec for Vector3 +impl SimdVec for Vector3 where N::Element: Scalar, { @@ -226,7 +226,7 @@ where } } -impl WVec for Point3 +impl SimdVec for Point3 where N::Element: Scalar, { @@ -249,7 +249,7 @@ where } } -pub(crate) trait WCrossMatrix: Sized { +pub(crate) trait SimdCrossMatrix: Sized { type CrossMat; type CrossMatTr; @@ -257,7 +257,7 @@ pub(crate) trait WCrossMatrix: Sized { fn gcross_matrix_tr(self) -> Self::CrossMatTr; } -impl WCrossMatrix for Vector3 { +impl SimdCrossMatrix for Vector3 { type CrossMat = Matrix3; type CrossMatTr = Matrix3; @@ -282,7 +282,7 @@ impl WCrossMatrix for Vector3 { } } -impl WCrossMatrix for Vector2 { +impl SimdCrossMatrix for Vector2 { type CrossMat = RowVector2; type CrossMatTr = Vector2; @@ -295,7 +295,7 @@ impl WCrossMatrix for Vector2 { Vector2::new(-self.y, self.x) } } -impl WCrossMatrix for Real { +impl SimdCrossMatrix for Real { type CrossMat = Matrix2; type CrossMatTr = Matrix2; @@ -310,7 +310,7 @@ impl WCrossMatrix for Real { } } -impl WCrossMatrix for SimdReal { +impl SimdCrossMatrix for SimdReal { type CrossMat = Matrix2; type CrossMatTr = Matrix2; @@ -325,12 +325,12 @@ impl WCrossMatrix for SimdReal { } } -pub(crate) trait WCross: Sized { +pub(crate) trait SimdCross: Sized { type Result; fn gcross(&self, rhs: Rhs) -> Self::Result; } -impl WCross> for Vector3 { +impl SimdCross> for Vector3 { type Result = Self; fn gcross(&self, rhs: Vector3) -> Self::Result { @@ -338,7 +338,7 @@ impl WCross> for Vector3 { } } -impl WCross> for Vector2 { +impl SimdCross> for Vector2 { type Result = Real; fn gcross(&self, rhs: Vector2) -> Self::Result { @@ -346,7 +346,7 @@ impl WCross> for Vector2 { } } -impl WCross> for Real { +impl SimdCross> for Real { type Result = Vector2; fn gcross(&self, rhs: Vector2) -> Self::Result { @@ -354,12 +354,12 @@ impl WCross> for Real { } } -pub(crate) trait WDot: Sized { +pub(crate) trait SimdDot: Sized { type Result; fn gdot(&self, rhs: Rhs) -> Self::Result; } -impl WDot> for Vector3 { +impl SimdDot> for Vector3 { type Result = N; fn gdot(&self, rhs: Vector3) -> Self::Result { @@ -367,7 +367,7 @@ impl WDot> for Vector3 { } } -impl WDot> for Vector2 { +impl SimdDot> for Vector2 { type Result = N; fn gdot(&self, rhs: Vector2) -> Self::Result { @@ -375,7 +375,7 @@ impl WDot> for Vector2 { } } -impl WDot> for N { +impl SimdDot> for N { type Result = N; fn gdot(&self, rhs: Vector1) -> Self::Result { @@ -383,7 +383,7 @@ impl WDot> for N { } } -impl WDot for N { +impl SimdDot for N { type Result = N; fn gdot(&self, rhs: N) -> Self::Result { @@ -391,7 +391,7 @@ impl WDot for N { } } -impl WDot for Vector1 { +impl SimdDot for Vector1 { type Result = N; fn gdot(&self, rhs: N) -> Self::Result { @@ -399,7 +399,7 @@ impl WDot for Vector1 { } } -impl WCross> for Vector3 { +impl SimdCross> for Vector3 { type Result = Vector3; fn gcross(&self, rhs: Self) -> Self::Result { @@ -407,7 +407,7 @@ impl WCross> for Vector3 { } } -impl WCross> for SimdReal { +impl SimdCross> for SimdReal { type Result = Vector2; fn gcross(&self, rhs: Vector2) -> Self::Result { @@ -415,7 +415,7 @@ impl WCross> for SimdReal { } } -impl WCross> for Vector2 { +impl SimdCross> for Vector2 { type Result = SimdReal; fn gcross(&self, rhs: Self) -> Self::Result { @@ -426,7 +426,7 @@ impl WCross> for Vector2 { } /// Trait implemented by quaternions. -pub trait WQuat { +pub trait SimdQuat { /// The result of quaternion differentiation. type Result; @@ -434,7 +434,7 @@ pub trait WQuat { fn diff_conj1_2(&self, rhs: &Self) -> Self::Result; } -impl WQuat for UnitComplex { +impl SimdQuat for UnitComplex { type Result = Matrix1; fn diff_conj1_2(&self, rhs: &Self) -> Self::Result { @@ -443,7 +443,7 @@ impl WQuat for UnitComplex { } } -impl WQuat for UnitQuaternion { +impl SimdQuat for UnitQuaternion { type Result = Matrix3; fn diff_conj1_2(&self, rhs: &Self) -> Self::Result { @@ -461,7 +461,7 @@ impl WQuat for UnitQuaternion { } } -pub(crate) trait WAngularInertia { +pub(crate) trait SimdAngularInertia { type AngVector; type LinVector; type AngMatrix; @@ -473,7 +473,7 @@ pub(crate) trait WAngularInertia { fn into_matrix(self) -> Self::AngMatrix; } -impl WAngularInertia for N { +impl SimdAngularInertia for N { type AngVector = N; type LinVector = Vector2; type AngMatrix = N; @@ -502,7 +502,7 @@ impl WAngularInertia for N { } } -impl WAngularInertia for SdpMatrix3 { +impl SimdAngularInertia for SdpMatrix3 { type AngVector = Vector3; type LinVector = Vector3; type AngMatrix = Matrix3; @@ -566,7 +566,7 @@ impl WAngularInertia for SdpMatrix3 { } } -impl WAngularInertia for SdpMatrix3 { +impl SimdAngularInertia for SdpMatrix3 { type AngVector = Vector3; type LinVector = Vector3; type AngMatrix = Matrix3; @@ -668,6 +668,7 @@ impl FlushToZeroDenormalsAreZeroFlags { any(target_arch = "x86", target_arch = "x86_64"), target_feature = "sse" ))] + #[allow(deprecated)] // will address that later. pub fn flush_denormal_to_zero() -> Self { unsafe { #[cfg(target_arch = "x86")] @@ -691,6 +692,7 @@ impl FlushToZeroDenormalsAreZeroFlags { target_feature = "sse" ))] impl Drop for FlushToZeroDenormalsAreZeroFlags { + #[allow(deprecated)] // will address that later. fn drop(&mut self) { #[cfg(target_arch = "x86")] unsafe { @@ -806,7 +808,7 @@ impl IndexMut2 for [T] { } /// Calculate the difference with smallest absolute value between the two given values. -pub fn smallest_abs_diff_between_sin_angles(a: N, b: N) -> N { +pub fn smallest_abs_diff_between_sin_angles(a: N, b: N) -> N { // Select the smallest path among the two angles to reach the target. let s_err = a - b; let sgn = s_err.simd_signum(); @@ -814,3 +816,13 @@ pub fn smallest_abs_diff_between_sin_angles(a: N, b: N) -> N { let s_err_is_smallest = s_err.simd_abs().simd_lt(s_err_complement.simd_abs()); s_err.select(s_err_is_smallest, s_err_complement) } + +/// Calculate the difference with smallest absolute value between the two given angles. +pub fn smallest_abs_diff_between_angles(a: N, b: N) -> N { + // Select the smallest path among the two angles to reach the target. + let s_err = a - b; + let sgn = s_err.simd_signum(); + let s_err_complement = s_err - sgn * N::simd_two_pi(); + let s_err_is_smallest = s_err.simd_abs().simd_lt(s_err_complement.simd_abs()); + s_err.select(s_err_is_smallest, s_err_complement) +} diff --git a/src_testbed/box2d_backend.rs b/src_testbed/box2d_backend.rs index 9cb4b5f6b..b9bc97ed9 100644 --- a/src_testbed/box2d_backend.rs +++ b/src_testbed/box2d_backend.rs @@ -223,11 +223,8 @@ impl Box2dWorld { pub fn step(&mut self, counters: &mut Counters, params: &IntegrationParameters) { counters.step_started(); - self.world.step( - params.dt, - params.max_velocity_iterations as i32, - params.max_stabilization_iterations as i32, - ); + self.world + .step(params.dt, params.num_solver_iterations.get() as i32, 1); counters.step_completed(); } diff --git a/src_testbed/lib.rs b/src_testbed/lib.rs index 46364cbb3..12339e586 100644 --- a/src_testbed/lib.rs +++ b/src_testbed/lib.rs @@ -12,6 +12,7 @@ pub use crate::harness::plugin::HarnessPlugin; pub use crate::physics::PhysicsState; pub use crate::plugin::TestbedPlugin; pub use crate::testbed::{Testbed, TestbedApp, TestbedGraphics, TestbedState}; +pub use bevy::prelude::KeyCode; #[cfg(all(feature = "dim2", feature = "other-backends"))] mod box2d_backend; diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index 208426cba..7070c8163 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -209,10 +209,9 @@ impl PhysxWorld { actor.set_linear_velocity(&linvel, true); actor.set_angular_velocity(&angvel, true); actor.set_solver_iteration_counts( - // Use our number of velocity iterations as their number of position iterations. - integration_parameters.max_velocity_iterations.max(1) as u32, - // Use our number of velocity stabilization iterations as their number of velocity iterations. - integration_parameters.max_stabilization_iterations.max(1) as u32, + // Use our number of solver iterations as their number of position iterations. + integration_parameters.num_solver_iterations.get() as u32, + 1, ); rapier2dynamic.insert(rapier_handle, actor); diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 9b08e90b1..aac73e8f5 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -1,5 +1,6 @@ use std::env; use std::mem; +use std::num::NonZeroUsize; use bevy::prelude::*; @@ -147,6 +148,7 @@ pub struct TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f> { #[allow(dead_code)] // Dead in 2D but not in 3D. camera_transform: GlobalTransform, camera: &'a mut OrbitCamera, + keys: &'a Input, } pub struct Testbed<'a, 'b, 'c, 'd, 'e, 'f> { @@ -284,11 +286,7 @@ impl TestbedApp { self.harness .physics .integration_parameters - .max_velocity_iterations = 4; - self.harness - .physics - .integration_parameters - .max_stabilization_iterations = 1; + .num_solver_iterations = NonZeroUsize::new(4).unwrap(); // Init world. let mut testbed = Testbed { @@ -458,6 +456,10 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f> { colliders, ) } + + pub fn keys(&self) -> &Input { + &*self.keys + } } impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> { @@ -910,7 +912,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> { .multibody_joints .iter() .next() - .map(|a| a.2.rigid_body_handle()); + .map(|(_, _, _, link)| link.rigid_body_handle()); if let Some(to_delete) = to_delete { self.harness .physics @@ -1107,6 +1109,7 @@ fn update_testbed( components: &mut gfx_components, camera_transform: *cameras.single().1, camera: &mut cameras.single_mut().2, + keys: &*keys, }; let mut testbed = Testbed { @@ -1200,6 +1203,7 @@ fn update_testbed( components: &mut gfx_components, camera_transform: *cameras.single().1, camera: &mut cameras.single_mut().2, + keys: &*keys, }; let mut testbed = Testbed { @@ -1351,6 +1355,7 @@ fn update_testbed( components: &mut gfx_components, camera_transform: *cameras.single().1, camera: &mut cameras.single_mut().2, + keys: &*keys, }; harness.step_with_graphics(Some(&mut testbed_graphics)); diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index fee4abcb9..3a51b320f 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -1,5 +1,6 @@ use rapier::counters::Counters; use rapier::math::Real; +use std::num::NonZeroUsize; use crate::debug_render::DebugRenderPipelineResource; use crate::harness::Harness; @@ -98,43 +99,25 @@ pub fn update_ui( let integration_parameters = &mut harness.physics.integration_parameters; - ui.checkbox( - &mut integration_parameters.interleave_restitution_and_friction_resolution, - "interleave friction resolution", - ); - if state.selected_backend == PHYSX_BACKEND_PATCH_FRICTION || state.selected_backend == PHYSX_BACKEND_TWO_FRICTION_DIR { - ui.add( - Slider::new(&mut integration_parameters.max_velocity_iterations, 1..=200) - .text("pos. iters."), - ); - ui.add( - Slider::new( - &mut integration_parameters.max_stabilization_iterations, - 1..=200, - ) - .text("vel. iters."), - ); + let mut num_iterations = integration_parameters.num_solver_iterations.get(); + ui.add(Slider::new(&mut num_iterations, 1..=40).text("pos. iters.")); + integration_parameters.num_solver_iterations = + NonZeroUsize::new(num_iterations).unwrap(); } else { - ui.add( - Slider::new(&mut integration_parameters.max_velocity_iterations, 1..=200) - .text("vel. rest. iters."), - ); - ui.add( - Slider::new( - &mut integration_parameters.max_velocity_friction_iterations, - 1..=200, - ) - .text("vel. frict. iters."), - ); + let mut num_iterations = integration_parameters.num_solver_iterations.get(); + ui.add(Slider::new(&mut num_iterations, 1..=40).text("num solver iters.")); + integration_parameters.num_solver_iterations = + NonZeroUsize::new(num_iterations).unwrap(); + ui.add( Slider::new( - &mut integration_parameters.max_stabilization_iterations, - 1..=200, + &mut integration_parameters.num_friction_iteration_per_solver_iteration, + 1..=40, ) - .text("vel. stab. iters."), + .text("frict. iters. per solver iters."), ); }