From da92e5c2837b27433286cf0dd9d887fd44dda254 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sat, 27 Jan 2024 16:49:53 +0100 Subject: Fix clippy and enable clippy on CI --- .github/workflows/rapier-ci-build.yml | 6 +++ benchmarks2d/all_benchmarks2.rs | 6 +-- benchmarks3d/all_benchmarks3.rs | 4 +- benchmarks3d/keva3.rs | 12 ++--- examples2d/all_examples2.rs | 6 +-- examples2d/trimesh2.rs | 2 +- examples3d-f64/all_examples3-f64.rs | 6 +-- examples3d/all_examples3.rs | 6 +-- examples3d/ccd3.rs | 4 +- examples3d/joints3.rs | 11 ++-- examples3d/keva3.rs | 12 ++--- examples3d/rope_joints3.rs | 2 +- examples3d/vehicle_controller3.rs | 8 +-- src/control/ray_cast_vehicle_controller.rs | 10 ++-- src/data/arena.rs | 2 +- src/data/pubsub.rs | 3 +- src/dynamics/ccd/ccd_solver.rs | 45 +++++++--------- src/dynamics/ccd/toi_entry.rs | 6 ++- src/dynamics/coefficient_combine_rule.rs | 9 +--- src/dynamics/integration_parameters.rs | 3 +- src/dynamics/island_manager.rs | 2 +- src/dynamics/joint/fixed_joint.rs | 12 ++--- src/dynamics/joint/generic_joint.rs | 8 +-- .../joint/impulse_joint/impulse_joint_set.rs | 6 +-- src/dynamics/joint/motor_model.rs | 9 +--- src/dynamics/joint/multibody_joint/multibody.rs | 21 ++++---- .../joint/multibody_joint/multibody_joint_set.rs | 12 ++--- src/dynamics/joint/prismatic_joint.rs | 12 ++--- src/dynamics/joint/revolute_joint.rs | 14 ++--- src/dynamics/joint/rope_joint.rs | 12 ++--- src/dynamics/joint/spherical_joint.rs | 12 ++--- src/dynamics/joint/spring_joint.rs | 12 ++--- src/dynamics/rigid_body.rs | 58 +++++++++----------- src/dynamics/rigid_body_components.rs | 34 ++---------- src/dynamics/rigid_body_set.rs | 2 +- .../contact_constraint/contact_constraints_set.rs | 16 +++--- .../generic_one_body_constraint.rs | 4 +- .../generic_two_body_constraint.rs | 12 ++--- .../generic_two_body_constraint_element.rs | 20 +++---- .../contact_constraint/one_body_constraint.rs | 6 +-- .../one_body_constraint_element.rs | 4 +- .../contact_constraint/one_body_constraint_simd.rs | 12 ++--- .../contact_constraint/two_body_constraint.rs | 8 +-- .../two_body_constraint_element.rs | 4 +- .../contact_constraint/two_body_constraint_simd.rs | 24 +++------ src/dynamics/solver/interaction_groups.rs | 6 +-- .../joint_constraint/joint_constraints_set.rs | 12 ++--- .../joint_generic_constraint_builder.rs | 1 + .../joint_constraint/joint_velocity_constraint.rs | 48 +++++++---------- src/dynamics/solver/mod.rs | 14 ++--- src/dynamics/solver/velocity_solver.rs | 6 +-- src/geometry/broad_phase_multi_sap/broad_phase.rs | 5 +- src/geometry/broad_phase_multi_sap/mod.rs | 12 ++--- src/geometry/broad_phase_multi_sap/sap_axis.rs | 5 +- src/geometry/broad_phase_multi_sap/sap_proxy.rs | 13 ++--- src/geometry/broad_phase_multi_sap/sap_region.rs | 1 + src/geometry/collider.rs | 11 ++-- src/geometry/collider_set.rs | 4 +- src/geometry/interaction_groups.rs | 2 + src/geometry/narrow_phase.rs | 62 +++++++++++----------- src/lib.rs | 3 ++ src/pipeline/collision_pipeline.rs | 2 +- .../debug_render_pipeline/debug_render_backend.rs | 10 ++-- .../debug_render_pipeline/debug_render_pipeline.rs | 9 +++- src/pipeline/debug_render_pipeline/mod.rs | 2 +- src/pipeline/debug_render_pipeline/outlines.rs | 1 + src/pipeline/physics_hooks.rs | 2 +- src/pipeline/physics_pipeline.rs | 13 +++-- src/pipeline/query_pipeline.rs | 9 ++-- src/pipeline/user_changes.rs | 14 +++-- src/utils.rs | 2 +- src_testbed/camera2d.rs | 1 + src_testbed/camera3d.rs | 1 + src_testbed/debug_render.rs | 6 ++- src_testbed/graphics.rs | 7 +-- src_testbed/harness/mod.rs | 10 +++- src_testbed/lib.rs | 2 + src_testbed/objects/node.rs | 2 + src_testbed/physics/mod.rs | 10 +++- src_testbed/testbed.rs | 59 ++++++++++---------- src_testbed/ui.rs | 24 ++++----- 81 files changed, 421 insertions(+), 469 deletions(-) diff --git a/.github/workflows/rapier-ci-build.yml b/.github/workflows/rapier-ci-build.yml index 9328bc9..1b29d15 100644 --- a/.github/workflows/rapier-ci-build.yml +++ b/.github/workflows/rapier-ci-build.yml @@ -23,6 +23,12 @@ jobs: steps: - uses: actions/checkout@v3 - run: sudo apt-get install -y cmake libxcb-composite0-dev + - name: Clippy + run: cargo clippy + - name: Clippy rapier2d + run: cargo clippy -p rapier-examples-2d --features parallel,simd-stable + - name: Clippy rapier3d + run: cargo clippy -p rapier-examples-3d --features parallel,simd-stable - name: Build rapier2d run: cargo build --verbose -p rapier2d; - name: Build rapier3d diff --git a/benchmarks2d/all_benchmarks2.rs b/benchmarks2d/all_benchmarks2.rs index ce6aaac..fcc7b95 100644 --- a/benchmarks2d/all_benchmarks2.rs +++ b/benchmarks2d/all_benchmarks2.rs @@ -46,8 +46,8 @@ fn demo_name_from_url() -> Option { #[cfg_attr(target_arch = "wasm32", wasm_bindgen(start))] pub fn main() { let demo = demo_name_from_command_line() - .or_else(|| demo_name_from_url()) - .unwrap_or(String::new()) + .or_else(demo_name_from_url) + .unwrap_or_default() .to_camel_case(); let mut builders: Vec<(_, fn(&mut Testbed))> = vec![ @@ -66,7 +66,7 @@ pub fn main() { ]; // Lexicographic sort, with stress tests moved at the end of the list. - builders.sort_by(|a, b| match (a.0.starts_with("("), b.0.starts_with("(")) { + builders.sort_by(|a, b| match (a.0.starts_with('('), b.0.starts_with(')')) { (true, true) | (false, false) => a.0.cmp(b.0), (true, false) => Ordering::Greater, (false, true) => Ordering::Less, diff --git a/benchmarks3d/all_benchmarks3.rs b/benchmarks3d/all_benchmarks3.rs index bca730b..591886e 100644 --- a/benchmarks3d/all_benchmarks3.rs +++ b/benchmarks3d/all_benchmarks3.rs @@ -36,7 +36,7 @@ fn parse_command_line() -> Command { while let Some(arg) = args.next() { if &arg[..] == "--example" { - return Command::Run(args.next().unwrap_or(String::new())); + return Command::Run(args.next().unwrap_or_default()); } else if &arg[..] == "--list" { return Command::List; } @@ -68,7 +68,7 @@ pub fn main() { ]; // Lexicographic sort, with stress tests moved at the end of the list. - builders.sort_by(|a, b| match (a.0.starts_with("("), b.0.starts_with("(")) { + builders.sort_by(|a, b| match (a.0.starts_with('('), b.0.starts_with('(')) { (true, true) | (false, false) => a.0.cmp(b.0), (true, false) => Ordering::Greater, (false, true) => Ordering::Less, diff --git a/benchmarks3d/keva3.rs b/benchmarks3d/keva3.rs index ecd6e77..4f5cd54 100644 --- a/benchmarks3d/keva3.rs +++ b/benchmarks3d/keva3.rs @@ -7,9 +7,7 @@ pub fn build_block( colliders: &mut ColliderSet, half_extents: Vector, shift: Vector, - mut numx: usize, - numy: usize, - mut numz: usize, + (mut numx, numy, mut numz): (usize, usize, usize), ) { let dimensions = [half_extents.xyz(), half_extents.zyx()]; let block_width = 2.0 * half_extents.z * numx as f32; @@ -56,8 +54,8 @@ pub fn build_block( // Close the top. let dim = half_extents.zxy(); - for i in 0..(block_width / (dim.x as f32 * 2.0)) as usize { - for j in 0..(block_width / (dim.z as f32 * 2.0)) as usize { + for i in 0..(block_width / (dim.x * 2.0)) as usize { + for j in 0..(block_width / (dim.z * 2.0)) as usize { // Build the rigid body. let rigid_body = RigidBodyBuilder::dynamic().translation(vector![ i as f32 * dim.x * 2.0 + dim.x + shift.x, @@ -114,9 +112,7 @@ pub fn init_world(testbed: &mut Testbed) { &mut colliders, half_extents, vector![-block_width / 2.0, block_height, -block_width / 2.0], - numx, - numy, - numz, + (numx, numy, numz), ); block_height += numy as f32 * half_extents.y * 2.0 + half_extents.x * 2.0; num_blocks_built += numx * numy * numz; diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index 032e4d6..4cdbf98 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -57,8 +57,8 @@ fn demo_name_from_url() -> Option { #[cfg_attr(target_arch = "wasm32", wasm_bindgen(start))] pub fn main() { let demo = demo_name_from_command_line() - .or_else(|| demo_name_from_url()) - .unwrap_or(String::new()) + .or_else(demo_name_from_url) + .unwrap_or_default() .to_camel_case(); let mut builders: Vec<(_, fn(&mut Testbed))> = vec![ @@ -85,7 +85,7 @@ pub fn main() { ]; // Lexicographic sort, with stress tests moved at the end of the list. - builders.sort_by(|a, b| match (a.0.starts_with("("), b.0.starts_with("(")) { + builders.sort_by(|a, b| match (a.0.starts_with('('), b.0.starts_with('(')) { (true, true) | (false, false) => a.0.cmp(b.0), (true, false) => Ordering::Greater, (false, true) => Ordering::Less, diff --git a/examples2d/trimesh2.rs b/examples2d/trimesh2.rs index 219a583..2cb8410 100644 --- a/examples2d/trimesh2.rs +++ b/examples2d/trimesh2.rs @@ -104,7 +104,7 @@ pub fn init_world(testbed: &mut Testbed) { testbed.look_at(point![0.0, 20.0], 17.0); } -const RAPIER_SVG_STR: &'static str = r#" +const RAPIER_SVG_STR: &str = r#" diff --git a/examples3d-f64/all_examples3-f64.rs b/examples3d-f64/all_examples3-f64.rs index 4464d60..c381eec 100644 --- a/examples3d-f64/all_examples3-f64.rs +++ b/examples3d-f64/all_examples3-f64.rs @@ -43,15 +43,15 @@ fn demo_name_from_url() -> Option { #[cfg_attr(target_arch = "wasm32", wasm_bindgen(start))] pub fn main() { let demo = demo_name_from_command_line() - .or_else(|| demo_name_from_url()) - .unwrap_or(String::new()) + .or_else(demo_name_from_url) + .unwrap_or_default() .to_camel_case(); let mut builders: Vec<(_, fn(&mut Testbed))> = vec![("(Debug) serialized", debug_serialized3::init_world)]; // Lexicographic sort, with stress tests moved at the end of the list. - builders.sort_by(|a, b| match (a.0.starts_with("("), b.0.starts_with("(")) { + builders.sort_by(|a, b| match (a.0.starts_with('('), b.0.starts_with('(')) { (true, true) | (false, false) => a.0.cmp(b.0), (true, false) => Ordering::Greater, (false, true) => Ordering::Less, diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 1bc8419..bac826e 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -86,8 +86,8 @@ fn demo_name_from_url() -> Option { #[cfg_attr(target_arch = "wasm32", wasm_bindgen(start))] pub fn main() { let demo = demo_name_from_command_line() - .or_else(|| demo_name_from_url()) - .unwrap_or(String::new()) + .or_else(demo_name_from_url) + .unwrap_or_default() .to_camel_case(); let mut builders: Vec<(_, fn(&mut Testbed))> = vec![ @@ -157,7 +157,7 @@ pub fn main() { ]; // Lexicographic sort, with stress tests moved at the end of the list. - builders.sort_by(|a, b| match (a.0.starts_with("("), b.0.starts_with("(")) { + builders.sort_by(|a, b| match (a.0.starts_with('('), b.0.starts_with('(')) { (true, true) | (false, false) => a.0.cmp(b.0), (true, false) => Ordering::Greater, (false, true) => Ordering::Less, diff --git a/examples3d/ccd3.rs b/examples3d/ccd3.rs index 4047f4b..c1a5176 100644 --- a/examples3d/ccd3.rs +++ b/examples3d/ccd3.rs @@ -27,9 +27,9 @@ fn create_wall( colliders.insert_with_parent(collider, handle, bodies); k += 1; if k % 2 == 0 { - testbed.set_initial_body_color(handle, [255. / 255., 131. / 255., 244.0 / 255.]); + testbed.set_initial_body_color(handle, [1., 131. / 255., 244.0 / 255.]); } else { - testbed.set_initial_body_color(handle, [131. / 255., 255. / 255., 244.0 / 255.]); + testbed.set_initial_body_color(handle, [131. / 255., 1., 244.0 / 255.]); } } } diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs index 18a11e7..73f48ae 100644 --- a/examples3d/joints3.rs +++ b/examples3d/joints3.rs @@ -482,7 +482,7 @@ fn create_actuated_revolute_joints( } else if i == num - 1 { let stiffness = 200.0; let damping = 100.0; - joint = joint.motor_position(3.14 / 2.0, stiffness, damping); + joint = joint.motor_position(std::f32::consts::FRAC_PI_2, stiffness, damping); } if i == 1 { @@ -541,7 +541,7 @@ fn create_actuated_spherical_joints( colliders.insert_with_parent(collider, child_handle, bodies); if i > 0 { - let mut joint = joint_template.clone(); + let mut joint = joint_template; if i == 1 { joint = joint @@ -554,7 +554,12 @@ fn create_actuated_spherical_joints( joint = joint .motor_position(JointAxis::AngX, 0.0, stiffness, damping) .motor_position(JointAxis::AngY, 1.0, stiffness, damping) - .motor_position(JointAxis::AngZ, 3.14 / 2.0, stiffness, damping); + .motor_position( + JointAxis::AngZ, + std::f32::consts::FRAC_PI_2, + stiffness, + damping, + ); } if use_articulations { diff --git a/examples3d/keva3.rs b/examples3d/keva3.rs index ecd6e77..4f5cd54 100644 --- a/examples3d/keva3.rs +++ b/examples3d/keva3.rs @@ -7,9 +7,7 @@ pub fn build_block( colliders: &mut ColliderSet, half_extents: Vector, shift: Vector, - mut numx: usize, - numy: usize, - mut numz: usize, + (mut numx, numy, mut numz): (usize, usize, usize), ) { let dimensions = [half_extents.xyz(), half_extents.zyx()]; let block_width = 2.0 * half_extents.z * numx as f32; @@ -56,8 +54,8 @@ pub fn build_block( // Close the top. let dim = half_extents.zxy(); - for i in 0..(block_width / (dim.x as f32 * 2.0)) as usize { - for j in 0..(block_width / (dim.z as f32 * 2.0)) as usize { + for i in 0..(block_width / (dim.x * 2.0)) as usize { + for j in 0..(block_width / (dim.z * 2.0)) as usize { // Build the rigid body. let rigid_body = RigidBodyBuilder::dynamic().translation(vector![ i as f32 * dim.x * 2.0 + dim.x + shift.x, @@ -114,9 +112,7 @@ pub fn init_world(testbed: &mut Testbed) { &mut colliders, half_extents, vector![-block_width / 2.0, block_height, -block_width / 2.0], - numx, - numy, - numz, + (numx, numy, numz), ); block_height += numy as f32 * half_extents.y * 2.0 + half_extents.x * 2.0; num_blocks_built += numx * numy * numz; diff --git a/examples3d/rope_joints3.rs b/examples3d/rope_joints3.rs index 9696682..11f0716 100644 --- a/examples3d/rope_joints3.rs +++ b/examples3d/rope_joints3.rs @@ -67,7 +67,7 @@ pub fn init_world(testbed: &mut Testbed) { let collider = ColliderBuilder::cuboid(0.15, 0.3, 0.15); colliders.insert_with_parent(collider, character_handle, &mut bodies); - testbed.set_initial_body_color(character_handle, [255. / 255., 131. / 255., 244.0 / 255.]); + testbed.set_initial_body_color(character_handle, [1., 131. / 255., 244.0 / 255.]); /* * Tethered Ball diff --git a/examples3d/vehicle_controller3.rs b/examples3d/vehicle_controller3.rs index 1228c82..846d9fe 100644 --- a/examples3d/vehicle_controller3.rs +++ b/examples3d/vehicle_controller3.rs @@ -32,9 +32,11 @@ pub fn init_world(testbed: &mut Testbed) { let collider = ColliderBuilder::cuboid(hw * 2.0, hh, hw).density(100.0); colliders.insert_with_parent(collider, vehicle_handle, &mut bodies); - let mut tuning = WheelTuning::default(); - tuning.suspension_stiffness = 100.0; - tuning.suspension_damping = 10.0; + let tuning = WheelTuning { + suspension_stiffness: 100.0, + suspension_damping: 10.0, + ..WheelTuning::default() + }; let mut vehicle = DynamicRayCastVehicleController::new(vehicle_handle); let wheel_positions = [ point![hw * 1.5, -hh, hw], diff --git a/src/control/ray_cast_vehicle_controller.rs b/src/control/ray_cast_vehicle_controller.rs index 30979e1..00b11eb 100644 --- a/src/control/ray_cast_vehicle_controller.rs +++ b/src/control/ray_cast_vehicle_controller.rs @@ -582,7 +582,7 @@ impl DynamicRayCastVehicleController { wheel.side_impulse = resolve_single_bilateral( &bodies[self.chassis], &wheel.raycast_info.contact_point_ws, - &ground_body, + ground_body, &wheel.raycast_info.contact_point_ws, &self.axle[i], ); @@ -664,11 +664,9 @@ impl DynamicRayCastVehicleController { if sliding { for wheel in &mut self.wheels { - if wheel.side_impulse != 0.0 { - if wheel.skid_info < 1.0 { - wheel.forward_impulse *= wheel.skid_info; - wheel.side_impulse *= wheel.skid_info; - } + if wheel.side_impulse != 0.0 && wheel.skid_info < 1.0 { + wheel.forward_impulse *= wheel.skid_info; + wheel.side_impulse *= wheel.skid_info; } } } diff --git a/src/data/arena.rs b/src/data/arena.rs index 3332539..57653da 100644 --- a/src/data/arena.rs +++ b/src/data/arena.rs @@ -269,7 +269,7 @@ impl Arena { self.free_list_head = next_free; self.len += 1; Some(Index { - index: i as u32, + index: i, generation: self.generation, }) } diff --git a/src/data/pubsub.rs b/src/data/pubsub.rs index 8c5f41c..619521e 100644 --- a/src/data/pubsub.rs +++ b/src/data/pubsub.rs @@ -106,8 +106,7 @@ impl PubSub { /// Read the i-th message not yet read by the given subsciber. pub fn read_ith(&self, sub: &Subscription, i: usize) -> Option<&T> { let cursor = &self.cursors[sub.id as usize]; - self.messages - .get(cursor.next(self.deleted_messages) as usize + i) + self.messages.get(cursor.next(self.deleted_messages) + i) } /// Get the messages not yet read by the given subscriber. diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index 9b06a80..79c4495 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -52,32 +52,27 @@ impl CCDSolver { /// /// The `impacts` should be the result of a previous call to `self.predict_next_impacts`. pub fn clamp_motions(&self, dt: Real, bodies: &mut RigidBodySet, impacts: &PredictedImpacts) { - match impacts { - PredictedImpacts::Impacts(tois) => { - for (handle, toi) in tois { - let rb = bodies.index_mut_internal(*handle); - let local_com = &rb.mprops.local_mprops.local_com; - - let min_toi = (rb.ccd.ccd_thickness - * 0.15 - * crate::utils::inv(rb.ccd.max_point_velocity(&rb.integrated_vels))) - .min(dt); - // println!( - // "Min toi: {}, Toi: {}, thick: {}, max_vel: {}", - // min_toi, - // toi, - // rb.ccd.ccd_thickness, - // rb.ccd.max_point_velocity(&rb.integrated_vels) - // ); - let new_pos = rb.integrated_vels.integrate( - toi.max(min_toi), - &rb.pos.position, - &local_com, - ); - rb.pos.next_position = new_pos; - } + if let PredictedImpacts::Impacts(tois) = impacts { + for (handle, toi) in tois { + let rb = bodies.index_mut_internal(*handle); + let local_com = &rb.mprops.local_mprops.local_com; + + let min_toi = (rb.ccd.ccd_thickness + * 0.15 + * crate::utils::inv(rb.ccd.max_point_velocity(&rb.integrated_vels))) + .min(dt); + // println!( + // "Min toi: {}, Toi: {}, thick: {}, max_vel: {}", + // min_toi, + // toi, + // rb.ccd.ccd_thickness, + // rb.ccd.max_point_velocity(&rb.integrated_vels) + // ); + let new_pos = + rb.integrated_vels + .integrate(toi.max(min_toi), &rb.pos.position, local_com); + rb.pos.next_position = new_pos; } - _ => {} } } diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs index 6f5a47d..719f3c5 100644 --- a/src/dynamics/ccd/toi_entry.rs +++ b/src/dynamics/ccd/toi_entry.rs @@ -169,13 +169,15 @@ impl TOIEntry { impl PartialOrd for TOIEntry { fn partial_cmp(&self, other: &Self) -> Option { - (-self.toi).partial_cmp(&(-other.toi)) + Some(self.cmp(other)) } } impl Ord for TOIEntry { fn cmp(&self, other: &Self) -> std::cmp::Ordering { - self.partial_cmp(other).unwrap() + (-self.toi) + .partial_cmp(&(-other.toi)) + .unwrap_or(std::cmp::Ordering::Equal) } } diff --git a/src/dynamics/coefficient_combine_rule.rs b/src/dynamics/coefficient_combine_rule.rs index 1016d09..9f99b7d 100644 --- a/src/dynamics/coefficient_combine_rule.rs +++ b/src/dynamics/coefficient_combine_rule.rs @@ -7,10 +7,11 @@ use crate::math::Real; /// Each collider has its combination rule of type /// `CoefficientCombineRule`. And the rule /// actually used is given by `max(first_combine_rule as usize, second_combine_rule as usize)`. -#[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[derive(Default, Copy, Clone, Debug, PartialEq, Eq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub enum CoefficientCombineRule { /// The two coefficients are averaged. + #[default] Average = 0, /// The smallest coefficient is chosen. Min, @@ -20,12 +21,6 @@ pub enum CoefficientCombineRule { Max, } -impl Default for CoefficientCombineRule { - fn default() -> Self { - CoefficientCombineRule::Average - } -} - impl CoefficientCombineRule { pub(crate) fn combine(coeff1: Real, coeff2: Real, rule_value1: u8, rule_value2: u8) -> Real { let effective_rule = rule_value1.max(rule_value2); diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index bcb823b..13b3fde 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -71,8 +71,7 @@ impl IntegrationParameters { /// NOTE: this resets [`Self::erp`], [`Self::damping_ratio`], [`Self::joint_erp`], /// [`Self::joint_damping_ratio`] to their former default values. pub fn switch_to_standard_pgs_solver(&mut self) { - self.num_internal_pgs_iterations = - self.num_solver_iterations.get() * self.num_internal_pgs_iterations; + self.num_internal_pgs_iterations *= self.num_solver_iterations.get(); self.num_solver_iterations = NonZeroUsize::new(1).unwrap(); self.erp = 0.8; self.damping_ratio = 0.25; diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index 92b9a3e..8f18941 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -134,7 +134,7 @@ impl IslandManager { } #[inline(always)] - pub(crate) fn iter_active_bodies<'a>(&'a self) -> impl Iterator + 'a { + pub(crate) fn iter_active_bodies(&self) -> impl Iterator + '_ { self.active_dynamic_set .iter() .copied() diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs index 75c8228..2bb2f64 100644 --- a/src/dynamics/joint/fixed_joint.rs +++ b/src/dynamics/joint/fixed_joint.rs @@ -84,9 +84,9 @@ impl FixedJoint { } } -impl Into for FixedJoint { - fn into(self) -> GenericJoint { - self.data +impl From for GenericJoint { + fn from(val: FixedJoint) -> GenericJoint { + val.data } } @@ -143,8 +143,8 @@ impl FixedJointBuilder { } } -impl Into for FixedJointBuilder { - fn into(self) -> GenericJoint { - self.0.into() +impl From for GenericJoint { + fn from(val: FixedJointBuilder) -> GenericJoint { + val.0.into() } } diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 83e27be..76a7fe1 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -1,3 +1,5 @@ +#![allow(clippy::bad_bit_mask)] // Clippy will complain about the bitmasks due to JointAxesMask::FREE_FIXED_AXES being 0. + use crate::dynamics::solver::MotorParameters; use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RopeJoint}; use crate::math::{Isometry, Point, Real, Rotation, UnitVector, Vector, SPATIAL_DIM}; @@ -704,8 +706,8 @@ impl GenericJointBuilder { } } -impl Into for GenericJointBuilder { - fn into(self) -> GenericJoint { - self.0 +impl From for GenericJoint { + fn from(val: GenericJointBuilder) -> GenericJoint { + val.0 } } diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index 67f8949..3f9835e 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -105,8 +105,8 @@ impl ImpulseJointSet { } /// Iterates through all the impulse joints attached to the given rigid-body. - pub fn map_attached_joints_mut<'a>( - &'a mut self, + pub fn map_attached_joints_mut( + &mut self, body: RigidBodyHandle, mut f: impl FnMut(RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, &mut ImpulseJoint), ) { @@ -282,7 +282,7 @@ impl ImpulseJointSet { &self, islands: &IslandManager, bodies: &RigidBodySet, - out: &mut Vec>, + out: &mut [Vec], ) { for out_island in &mut out[..islands.num_islands()] { out_island.clear(); diff --git a/src/dynamics/joint/motor_model.rs b/src/dynamics/joint/motor_model.rs index e8ffffa..74b8dd3 100644 --- a/src/dynamics/joint/motor_model.rs +++ b/src/dynamics/joint/motor_model.rs @@ -1,23 +1,18 @@ use crate::math::Real; /// The spring-like model used for constraints resolution. -#[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[derive(Default, Copy, Clone, Debug, PartialEq, Eq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub enum MotorModel { /// The solved spring-like equation is: /// `acceleration = stiffness * (pos - target_pos) + damping * (vel - target_vel)` + #[default] AccelerationBased, /// The solved spring-like equation is: /// `force = stiffness * (pos - target_pos) + damping * (vel - target_vel)` ForceBased, } -impl Default for MotorModel { - fn default() -> Self { - MotorModel::AccelerationBased - } -} - impl MotorModel { /// Combines the coefficients used for solving the spring equation. /// diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index c65a6ff..617d447 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -127,8 +127,9 @@ impl Multibody { let mut link_id2new_id = vec![usize::MAX; self.links.len()]; for (i, mut link) in self.links.0.into_iter().enumerate() { - let is_new_root = (!joint_only && (i == 0 || link.parent_internal_id == to_remove)) - || (joint_only && (i == 0 || i == to_remove)); + let is_new_root = i == 0 + || !joint_only && link.parent_internal_id == to_remove + || joint_only && i == to_remove; if !joint_only && i == to_remove { continue; @@ -492,7 +493,7 @@ impl Multibody { parent_to_world = parent_link.local_to_world; let (link_j, parent_j) = self.body_jacobians.index_mut_const(i, parent_id); - link_j.copy_from(&parent_j); + link_j.copy_from(parent_j); { let mut link_j_v = link_j.fixed_rows_mut::(0); @@ -602,12 +603,12 @@ impl Multibody { let (coriolis_v, parent_coriolis_v) = self.coriolis_v.index_mut2(i, parent_id); let (coriolis_w, parent_coriolis_w) = self.coriolis_w.index_mut2(i, parent_id); - coriolis_v.copy_from(&parent_coriolis_v); - coriolis_w.copy_from(&parent_coriolis_w); + coriolis_v.copy_from(parent_coriolis_v); + coriolis_w.copy_from(parent_coriolis_w); // [c1 - c0].gcross() * (JDot + JDot/u * qdot)" let shift_cross_tr = link.shift02.gcross_matrix_tr(); - coriolis_v.gemm(1.0, &shift_cross_tr, &parent_coriolis_w, 1.0); + coriolis_v.gemm(1.0, &shift_cross_tr, parent_coriolis_w, 1.0); // JDot (but the 2.0 originates from the sum of two identical terms in JDot and JDot/u * gdot) let dvel_cross = (rb.vels.angvel.gcross(link.shift02) @@ -663,7 +664,7 @@ impl Multibody { { // [c3 - c2].gcross() * (JDot + JDot/u * qdot) let shift_cross_tr = link.shift23.gcross_matrix_tr(); - coriolis_v.gemm(1.0, &shift_cross_tr, &coriolis_w, 1.0); + coriolis_v.gemm(1.0, &shift_cross_tr, coriolis_w, 1.0); // JDot let dvel_cross = rb.vels.angvel.gcross(link.shift23).gcross_matrix_tr(); @@ -696,16 +697,16 @@ impl Multibody { { let mut i_coriolis_dt_w = self.i_coriolis_dt.fixed_rows_mut::(DIM); // NOTE: this is just an axpy, but on row columns. - i_coriolis_dt_w.zip_apply(&coriolis_w, |o, x| *o = x * dt * rb_inertia); + i_coriolis_dt_w.zip_apply(coriolis_w, |o, x| *o = x * dt * rb_inertia); } #[cfg(feature = "dim3")] { let mut i_coriolis_dt_w = self.i_coriolis_dt.fixed_rows_mut::(DIM); - i_coriolis_dt_w.gemm(dt, &rb_inertia, &coriolis_w, 0.0); + i_coriolis_dt_w.gemm(dt, &rb_inertia, coriolis_w, 0.0); } self.acc_augmented_mass - .gemm_tr(1.0, &rb_j, &self.i_coriolis_dt, 1.0); + .gemm_tr(1.0, rb_j, &self.i_coriolis_dt, 1.0); } /* diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index 748de3b..fa0ffdb 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -393,10 +393,10 @@ impl MultibodyJointSet { /// Iterate through the handles of all the rigid-bodies attached to this rigid-body /// by a multibody_joint. - pub fn attached_bodies<'a>( - &'a self, + pub fn attached_bodies( + &self, body: RigidBodyHandle, - ) -> impl Iterator + 'a { + ) -> impl Iterator + '_ { self.rb2mb .get(body.0) .into_iter() @@ -406,10 +406,10 @@ impl MultibodyJointSet { /// Iterate through the handles of all the rigid-bodies attached to this rigid-body /// by an enabled multibody_joint. - pub fn bodies_attached_with_enabled_joint<'a>( - &'a self, + pub fn bodies_attached_with_enabled_joint( + &self, body: RigidBodyHandle, - ) -> impl Iterator + 'a { + ) -> impl Iterator + '_ { self.attached_bodies(body).filter(move |other| { if let Some((_, _, link)) = self.joint_between(body, *other) { link.joint.data.is_enabled() diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs index 1e7a016..d0f3a7d 100644 --- a/src/dynamics/joint/prismatic_joint.rs +++ b/src/dynamics/joint/prismatic_joint.rs @@ -152,9 +152,9 @@ impl PrismaticJoint { } } -impl Into for PrismaticJoint { - fn into(self) -> GenericJoint { - self.data +impl From for GenericJoint { + fn from(val: PrismaticJoint) -> GenericJoint { + val.data } } @@ -263,8 +263,8 @@ impl PrismaticJointBuilder { } } -impl Into for PrismaticJointBuilder { - fn into(self) -> GenericJoint { - self.0.into() +impl From for GenericJoint { + fn from(val: PrismaticJointBuilder) -> GenericJoint { + val.0.into() } } diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs index 9400ba8..ba27351 100644 --- a/src/dynamics/joint/revolute_joint.rs +++ b/src/dynamics/joint/revolute_joint.rs @@ -17,6 +17,7 @@ pub struct RevoluteJoint { impl RevoluteJoint { /// Creates a new revolute joint allowing only relative rotations. #[cfg(feature = "dim2")] + #[allow(clippy::new_without_default)] // For symmetry with 3D which can’t have a Default impl. pub fn new() -> Self { let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES); Self { data: data.build() } @@ -137,9 +138,9 @@ impl RevoluteJoint { } } -impl Into for RevoluteJoint { - fn into(self) -> GenericJoint { - self.data +impl From for GenericJoint { + fn from(val: RevoluteJoint) -> GenericJoint { + val.data } } @@ -153,6 +154,7 @@ pub struct RevoluteJointBuilder(pub RevoluteJoint); impl RevoluteJointBuilder { /// Creates a new revolute joint builder. #[cfg(feature = "dim2")] + #[allow(clippy::new_without_default)] // For symmetry with 3D which can’t have a Default impl. pub fn new() -> Self { Self(RevoluteJoint::new()) } @@ -241,8 +243,8 @@ impl RevoluteJointBuilder { } } -impl Into for RevoluteJointBuilder { - fn into(self) -> GenericJoint { - self.0.into() +impl From for GenericJoint { + fn from(val: RevoluteJointBuilder) -> GenericJoint { + val.0.into() } } diff --git a/src/dynamics/joint/rope_joint.rs b/src/dynamics/joint/rope_joint.rs index 08c90e5..64a66ea 100644 --- a/src/dynamics/joint/rope_joint.rs +++ b/src/dynamics/joint/rope_joint.rs @@ -134,9 +134,9 @@ impl RopeJoint { } } -impl Into for RopeJoint { - fn into(self) -> GenericJoint { - self.data +impl From for GenericJoint { + fn from(val: RopeJoint) -> GenericJoint { + val.data } } @@ -231,8 +231,8 @@ impl RopeJointBuilder { } } -impl Into for RopeJointBuilder { - fn into(self) -> GenericJoint { - self.0.into() +impl From for GenericJoint { + fn from(val: RopeJointBuilder) -> GenericJoint { + val.0.into() } } diff --git a/src/dynamics/joint/spherical_joint.rs b/src/dynamics/joint/spherical_joint.rs index 6f2dc39..dcbf5fb 100644 --- a/src/dynamics/joint/spherical_joint.rs +++ b/src/dynamics/joint/spherical_joint.rs @@ -164,9 +164,9 @@ impl SphericalJoint { } } -impl Into for SphericalJoint { - fn into(self) -> GenericJoint { - self.data +impl From for GenericJoint { + fn from(val: SphericalJoint) -> GenericJoint { + val.data } } @@ -288,8 +288,8 @@ impl SphericalJointBuilder { } } -impl Into for SphericalJointBuilder { - fn into(self) -> GenericJoint { - self.0.into() +impl From for GenericJoint { + fn from(val: SphericalJointBuilder) -> GenericJoint { + val.0.into() } } diff --git a/src/dynamics/joint/spring_joint.rs b/src/dynamics/joint/spring_joint.rs index d9a849a..f5cb5bd 100644 --- a/src/dynamics/joint/spring_joint.rs +++ b/src/dynamics/joint/spring_joint.rs @@ -94,9 +94,9 @@ impl SpringJoint { // } } -impl Into for SpringJoint { - fn into(self) -> GenericJoint { - self.data +impl From for GenericJoint { + fn from(val: SpringJoint) -> GenericJoint { + val.data } } @@ -165,8 +165,8 @@ impl SpringJointBuilder { } } -impl Into for SpringJointBuilder { - fn into(self) -> GenericJoint { - self.0.into() +impl From for GenericJoint { + fn from(val: SpringJointBuilder) -> GenericJoint { + val.0.into() } } diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index b462f8b..b27b58e 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -300,16 +300,16 @@ impl RigidBody { wake_up: bool, ) { #[cfg(feature = "dim2")] - if self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_X) == !allow_translation_x - && self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_Y) == !allow_translation_y + if self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_X) != allow_translation_x + && self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_Y) != allow_translation_y { // Nothing to change. return; } #[cfg(feature = "dim3")] - if self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_X) == !allow_translation_x - && self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_Y) == !allow_translation_y - && self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_Z) == !allow_translation_z + if self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_X) != allow_translation_x + && self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_Y) != allow_translation_y + && self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_Z) != allow_translation_z { // Nothing to change. return; @@ -850,13 +850,11 @@ impl RigidBody { /// /// This does nothing on non-dynamic bodies. pub fn add_force(&mut self, force: Vector, wake_up: bool) { - if !force.is_zero() { - if self.body_type == RigidBodyType::Dynamic { - self.forces.user_force += force; + if !force.is_zero() && self.body_type == RigidBodyType::Dynamic { + self.forces.user_force += force; - if wake_up { - self.wake_up(true); - } + if wake_up { + self.wake_up(true); } } } @@ -866,13 +864,11 @@ impl RigidBody { /// This does nothing on non-dynamic bodies. #[cfg(feature = "dim2")] pub fn add_torque(&mut self, torque: Real, wake_up: bool) { - if !torque.is_zero() { - if self.body_type == RigidBodyType::Dynamic { - self.forces.user_torque += torque; + if !torque.is_zero() && self.body_type == RigidBodyType::Dynamic { + self.forces.user_torque += torque; - if wake_up { - self.wake_up(true); - } + if wake_up { + self.wake_up(true); } } } @@ -882,13 +878,11 @@ impl RigidBody { /// This does nothing on non-dynamic bodies. #[cfg(feature = "dim3")] pub fn add_torque(&mut self, torque: Vector, wake_up: bool) { - if !torque.is_zero() { - if self.body_type == RigidBodyType::Dynamic { - self.forces.user_torque += torque; + if !torque.is_zero() && self.body_type == RigidBodyType::Dynamic { + self.forces.user_torque += torque; - if wake_up { - self.wake_up(true); - } + if wake_up { + self.wake_up(true); } } } @@ -897,14 +891,12 @@ impl RigidBody { /// /// This does nothing on non-dynamic bodies. pub fn add_force_at_point(&mut self, force: Vector, point: Point, wake_up: bool) { - if !force.is_zero() { - if self.body_type == RigidBodyType::Dynamic { - self.forces.user_force += force; - self.forces.user_torque += (point - self.mprops.world_com).gcross(force); + if !force.is_zero() && self.body_type == RigidBodyType::Dynamic { + self.forces.user_force += force; + self.forces.user_torque += (point - self.mprops.world_com).gcross(force); - if wake_up { - self.wake_up(true); - } + if wake_up { + self.wake_up(true); } } } @@ -1379,8 +1371,8 @@ impl RigidBodyBuilder { } } -impl Into for RigidBodyBuilder { - fn into(self) -> RigidBody { - self.build() +impl From for RigidBody { + fn from(val: RigidBodyBuilder) -> RigidBody { + val.build() } } diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index caff27e..2291742 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -399,7 +399,7 @@ impl RigidBodyMassProps { /// Update the world-space mass properties of `self`, taking into account the new position. pub fn update_world_mass_properties(&mut self, position: &Isometry) { - self.world_com = self.local_mprops.world_com(&position); + self.world_com = self.local_mprops.world_com(position); self.effective_inv_mass = Vector::repeat(self.local_mprops.inv_mass); self.effective_world_inv_inertia_sqrt = self.local_mprops.world_inv_inertia_sqrt(&position.rotation); @@ -707,7 +707,6 @@ impl std::ops::Add for RigidBodyVelocity { } impl std::ops::AddAssign for RigidBodyVelocity { - #[must_use] fn add_assign(&mut self, rhs: Self) { self.linvel += rhs.linvel; self.angvel += rhs.angvel; @@ -788,7 +787,7 @@ impl RigidBodyForces { gravity: &Vector, mass: &Vector, ) { - self.force = self.user_force + gravity.component_mul(&mass) * self.gravity_scale; + self.force = self.user_force + gravity.component_mul(mass) * self.gravity_scale; self.torque = self.user_torque; } @@ -877,7 +876,7 @@ impl RigidBodyCcd { } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Clone, Debug, Copy, PartialEq, Eq, Hash)] +#[derive(Default, Clone, Debug, Copy, PartialEq, Eq, Hash)] /// Internal identifiers used by the physics engine. pub struct RigidBodyIds { pub(crate) active_island_id: usize, @@ -886,19 +885,8 @@ pub struct RigidBodyIds { pub(crate) active_set_timestamp: u32, } -impl Default for RigidBodyIds { - fn default() -> Self { - Self { - active_island_id: 0, - active_set_id: 0, - active_set_offset: 0, - active_set_timestamp: 0, - } - } -} - #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Clone, Debug, PartialEq, Eq)] +#[derive(Default, Clone, Debug, PartialEq, Eq)] /// The set of colliders attached to this rigid-bodies. /// /// This should not be modified manually unless you really know what @@ -906,12 +894,6 @@ impl Default for RigidBodyIds { /// to a game engine using its component-based interface). pub struct RigidBodyColliders(pub Vec); -impl Default for RigidBodyColliders { - fn default() -> Self { - Self(vec![]) - } -} - impl RigidBodyColliders { /// Detach a collider from this rigid-body. pub fn detach_collider( @@ -987,16 +969,10 @@ impl RigidBodyColliders { } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Clone, Debug, Copy, PartialEq, Eq, PartialOrd, Ord, Hash)] +#[derive(Default, Clone, Debug, Copy, PartialEq, Eq, PartialOrd, Ord, Hash)] /// The dominance groups of a rigid-body. pub struct RigidBodyDominance(pub i8); -impl Default for RigidBodyDominance { - fn default() -> Self { - RigidBodyDominance(0) - } -} - impl RigidBodyDominance { /// The actual dominance group of this rigid-body, after taking into account its type. pub fn effective_group(&self, status: &RigidBodyType) -> i16 { diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index cd8f30a..2356364 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -44,7 +44,7 @@ impl RigidBodySet { } pub(crate) fn take_modified(&mut self) -> Vec { - std::mem::replace(&mut self.modified_bodies, vec![]) + std::mem::take(&mut self.modified_bodies) } /// The number of rigid bodies on this set. diff --git a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs index 4d02270..b5fdb6d 100644 --- a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs +++ b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs @@ -192,7 +192,7 @@ impl ContactConstraintsSet { .interaction_groups .simd_interactions .chunks_exact(SIMD_WIDTH) - .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[i[0]]).num_constraints) + .map(|i| ConstraintsCounts::from_contacts(manifolds_all[i[0]]).num_constraints) .sum(); unsafe { @@ -211,7 +211,7 @@ impl ContactConstraintsSet { .chunks_exact(SIMD_WIDTH) { let num_to_add = - ConstraintsCounts::from_contacts(&manifolds_all[manifolds_i[0]]).num_constraints; + 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]]]; @@ -238,7 +238,7 @@ impl ContactConstraintsSet { .interaction_groups .nongrouped_interactions .iter() - .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints) + .map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints) .sum(); unsafe { @@ -278,7 +278,7 @@ impl ContactConstraintsSet { let total_num_constraints = self .generic_two_body_interactions .iter() - .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints) + .map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints) .sum(); self.generic_velocity_constraints_builder.resize( @@ -321,7 +321,7 @@ impl ContactConstraintsSet { let total_num_constraints = self .generic_one_body_interactions .iter() - .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints) + .map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints) .sum(); self.generic_velocity_one_body_constraints_builder.resize( total_num_constraints, @@ -362,7 +362,7 @@ impl ContactConstraintsSet { .one_body_interaction_groups .simd_interactions .chunks_exact(SIMD_WIDTH) - .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[i[0]]).num_constraints) + .map(|i| ConstraintsCounts::from_contacts(manifolds_all[i[0]]).num_constraints) .sum(); unsafe { @@ -384,7 +384,7 @@ impl ContactConstraintsSet { .chunks_exact(SIMD_WIDTH) { let num_to_add = - ConstraintsCounts::from_contacts(&manifolds_all[manifolds_i[0]]).num_constraints; + 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( @@ -408,7 +408,7 @@ impl ContactConstraintsSet { .one_body_interaction_groups .nongrouped_interactions .iter() - .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints) + .map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints) .sum(); unsafe { diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs index 7c39eab..7b1f8ea 100644 --- a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs @@ -59,7 +59,7 @@ impl GenericOneBodyConstraintBuilder { let rb1 = handle1 .map(|h| SolverBody::from(&bodies[h])) - .unwrap_or_else(SolverBody::default); + .unwrap_or_default(); let rb2 = &bodies[handle2.unwrap()]; let (vels2, mprops2) = (&rb2.vels, &rb2.mprops); @@ -265,7 +265,7 @@ impl GenericOneBodyConstraint { solve_restitution: bool, solve_friction: bool, ) { - let solver_vel2 = self.inner.solver_vel2 as usize; + let solver_vel2 = self.inner.solver_vel2; let elements = &mut self.inner.elements[..self.inner.num_contacts as usize]; OneBodyConstraintElement::generic_solve_group( diff --git a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs index a4924fe..073f585 100644 --- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs @@ -382,15 +382,15 @@ impl GenericTwoBodyConstraint { solve_friction: bool, ) { let mut solver_vel1 = if self.generic_constraint_mask & 0b01 == 0 { - GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1 as usize]) + GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1]) } else { - GenericRhs::GenericId(self.inner.solver_vel1 as usize) + GenericRhs::GenericId(self.inner.solver_vel1) }; let mut solver_vel2 = if self.generic_constraint_mask & 0b10 == 0 { - GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2 as usize]) + GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2]) } else { - GenericRhs::GenericId(self.inner.solver_vel2 as usize) + GenericRhs::GenericId(self.inner.solver_vel2) }; let elements = &mut self.inner.elements[..self.inner.num_contacts as usize]; @@ -415,11 +415,11 @@ impl GenericTwoBodyConstraint { ); if let GenericRhs::SolverVel(solver_vel1) = solver_vel1 { - solver_vels[self.inner.solver_vel1 as usize] = solver_vel1; + solver_vels[self.inner.solver_vel1] = solver_vel1; } if let GenericRhs::SolverVel(solver_vel2) = solver_vel2 { - solver_vels[self.inner.solver_vel2 as usize] = solver_vel2; + solver_vels[self.inner.solver_vel2] = solver_vel2; } } diff --git a/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs index f3bd3a0..40740a0 100644 --- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs @@ -114,7 +114,7 @@ impl TwoBodyConstraintTangentPart { j_id1, ndofs1, jacobians, - &tangents1[0], + tangents1[0], &self.gcross1[0], solver_vels, ) + solver_vel2.dvel( @@ -135,7 +135,7 @@ impl TwoBodyConstraintTangentPart { ndofs1, dlambda, jacobians, - &tangents1[0], + tangents1[0], &self.gcross1[0], solver_vels, im1, @@ -158,7 +158,7 @@ impl TwoBodyConstraintTangentPart { j_id1, ndofs1, jacobians, - &tangents1[0], + tangents1[0], &self.gcross1[0], solver_vels, ) + solver_vel2.dvel( @@ -173,7 +173,7 @@ impl TwoBodyConstraintTangentPart { j_id1 + j_step, ndofs1, jacobians, - &tangents1[1], + tangents1[1], &self.gcross1[1], solver_vels, ) + solver_vel2.dvel( @@ -199,7 +199,7 @@ impl TwoBodyConstraintTangentPart { ndofs1, dlambda[0], jacobians, - &tangents1[0], + tangents1[0], &self.gcross1[0], solver_vels, im1, @@ -209,7 +209,7 @@ impl TwoBodyConstraintTangentPart { ndofs1, dlambda[1], jacobians, - &tangents1[1], + tangents1[1], &self.gcross1[1], solver_vels, im1, @@ -258,7 +258,7 @@ impl TwoBodyConstraintNormalPart { let j_id1 = j_id1(j_id, ndofs1, ndofs2); let j_id2 = j_id2(j_id, ndofs1, ndofs2); - let dvel = solver_vel1.dvel(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, solver_vels) + 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; @@ -271,7 +271,7 @@ impl TwoBodyConstraintNormalPart { ndofs1, dlambda, jacobians, - &dir1, + dir1, &self.gcross1, solver_vels, im1, @@ -323,7 +323,7 @@ impl TwoBodyConstraintElement { cfm_factor, nrm_j_id, jacobians, - &dir1, + dir1, im1, im2, ndofs1, @@ -339,7 +339,7 @@ impl TwoBodyConstraintElement { // Solve friction. if solve_friction { #[cfg(feature = "dim3")] - let tangents1 = [tangent1, &dir1.cross(&tangent1)]; + let tangents1 = [tangent1, &dir1.cross(tangent1)]; #[cfg(feature = "dim2")] let tangents1 = [&dir1.orthonormal_vector()]; let mut tng_j_id = tangent_j_id(j_id, ndofs1, ndofs2); diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint.rs b/src/dynamics/solver/contact_constraint/one_body_constraint.rs index 57931cd..be108a4 100644 --- a/src/dynamics/solver/contact_constraint/one_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint.rs @@ -76,7 +76,7 @@ impl OneBodyConstraintBuilder { let rb1 = handle1 .map(|h| SolverBody::from(&bodies[h])) - .unwrap_or_else(SolverBody::default); + .unwrap_or_default(); let rb2 = &bodies[handle2.unwrap()]; let vels2 = &rb2.vels; @@ -334,7 +334,7 @@ impl OneBodyConstraint { solve_normal: bool, solve_friction: bool, ) { - let mut solver_vel2 = solver_vels[self.solver_vel2 as usize]; + let mut solver_vel2 = solver_vels[self.solver_vel2]; OneBodyConstraintElement::solve_group( self.cfm_factor, @@ -349,7 +349,7 @@ impl OneBodyConstraint { solve_friction, ); - solver_vels[self.solver_vel2 as usize] = solver_vel2; + solver_vels[self.solver_vel2] = solver_vel2; } // FIXME: duplicated code. This is exactly the same as in the two-body velocity constraint. diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs index c5c0944..d9ff7f4 100644 --- a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs @@ -204,7 +204,7 @@ impl OneBodyConstraintElement { AngVector: SimdDot, Result = N>, { #[cfg(feature = "dim3")] - let tangents1 = [tangent1, &dir1.cross(&tangent1)]; + let tangents1 = [tangent1, &dir1.cross(tangent1)]; #[cfg(feature = "dim2")] let tangents1 = [&dir1.orthonormal_vector()]; @@ -213,7 +213,7 @@ impl OneBodyConstraintElement { for element in elements.iter_mut() { element .normal_part - .solve(cfm_factor, &dir1, im2, solver_vel2); + .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); diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs index 33e0d77..cd2ea56 100644 --- a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs @@ -308,12 +308,8 @@ impl OneBodyConstraintSimd { solve_friction: bool, ) { let mut solver_vel2 = SolverVel { - linear: Vector::from(gather![ - |ii| solver_vels[self.solver_vel2[ii] as usize].linear - ]), - angular: AngVector::from(gather![ - |ii| solver_vels[self.solver_vel2[ii] as usize].angular - ]), + linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]), + angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]), }; OneBodyConstraintElement::solve_group( @@ -330,8 +326,8 @@ impl OneBodyConstraintSimd { ); for ii in 0..SIMD_WIDTH { - 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); + solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii); + solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii); } } diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint.rs b/src/dynamics/solver/contact_constraint/two_body_constraint.rs index 9b552ca..0a0ebd6 100644 --- a/src/dynamics/solver/contact_constraint/two_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/two_body_constraint.rs @@ -380,8 +380,8 @@ impl TwoBodyConstraint { 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]; + let mut solver_vel1 = solver_vels[self.solver_vel1]; + let mut solver_vel2 = solver_vels[self.solver_vel2]; TwoBodyConstraintElement::solve_group( self.cfm_factor, @@ -398,8 +398,8 @@ impl TwoBodyConstraint { solve_friction, ); - solver_vels[self.solver_vel1 as usize] = solver_vel1; - solver_vels[self.solver_vel2 as usize] = solver_vel2; + solver_vels[self.solver_vel1] = solver_vel1; + solver_vels[self.solver_vel2] = solver_vel2; } pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs index a8c0037..8c720b9 100644 --- a/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs @@ -243,7 +243,7 @@ impl TwoBodyConstraintElement { AngVector: SimdDot, Result = N>, { #[cfg(feature = "dim3")] - let tangents1 = [tangent1, &dir1.cross(&tangent1)]; + let tangents1 = [tangent1, &dir1.cross(tangent1)]; #[cfg(feature = "dim2")] let tangents1 = [&dir1.orthonormal_vector()]; @@ -252,7 +252,7 @@ impl TwoBodyConstraintElement { for element in elements.iter_mut() { element .normal_part - .solve(cfm_factor, &dir1, im1, im2, solver_vel1, solver_vel2); + .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); diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs index b9f3e52..ee176d6 100644 --- a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs +++ b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs @@ -292,21 +292,13 @@ impl TwoBodyConstraintSimd { solve_friction: bool, ) { let mut solver_vel1 = SolverVel { - linear: Vector::from(gather![ -