diff options
Diffstat (limited to 'src/dynamics/solver/joint_constraint')
3 files changed, 25 insertions, 36 deletions
diff --git a/src/dynamics/solver/joint_constraint/joint_constraints_set.rs b/src/dynamics/solver/joint_constraint/joint_constraints_set.rs index 21b6459..64c1e83 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraints_set.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraints_set.rs @@ -399,7 +399,7 @@ impl JointConstraintsSet { ) { for builder in &mut self.generic_velocity_constraints_builder { builder.update( - ¶ms, + params, multibodies, solver_bodies, &mut self.generic_jacobians, @@ -409,7 +409,7 @@ impl JointConstraintsSet { for builder in &mut self.generic_velocity_one_body_constraints_builder { builder.update( - ¶ms, + params, multibodies, solver_bodies, &mut self.generic_jacobians, @@ -418,17 +418,17 @@ impl JointConstraintsSet { } for builder in &mut self.velocity_constraints_builder { - builder.update(¶ms, solver_bodies, &mut self.velocity_constraints); + builder.update(params, 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); + builder.update(params, solver_bodies, &mut self.simd_velocity_constraints); } for builder in &mut self.velocity_one_body_constraints_builder { builder.update( - ¶ms, + params, solver_bodies, &mut self.velocity_one_body_constraints, ); @@ -437,7 +437,7 @@ impl JointConstraintsSet { #[cfg(feature = "simd-is-enabled")] for builder in &mut self.simd_velocity_one_body_constraints_builder { builder.update( - ¶ms, + params, solver_bodies, &mut self.simd_velocity_one_body_constraints, ); diff --git a/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs index 5523962..34256a2 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs @@ -219,6 +219,7 @@ impl JointGenericTwoBodyConstraintBuilder { } #[derive(Copy, Clone)] +#[allow(clippy::large_enum_variant)] pub enum JointGenericOneBodyConstraintBuilder { Internal(JointGenericVelocityOneBodyInternalConstraintBuilder), External(JointGenericVelocityOneBodyExternalConstraintBuilder), diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs index 274e94e..e184e3d 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs @@ -332,13 +332,13 @@ impl JointTwoBodyConstraint<Real, 1> { } pub fn solve(&mut self, solver_vels: &mut [SolverVel<Real>]) { - let mut solver_vel1 = solver_vels[self.solver_vel1[0] as usize]; - let mut solver_vel2 = solver_vels[self.solver_vel2[0] as usize]; + let mut solver_vel1 = solver_vels[self.solver_vel1[0]]; + let mut solver_vel2 = solver_vels[self.solver_vel2[0]]; self.solve_generic(&mut solver_vel1, &mut solver_vel2); - solver_vels[self.solver_vel1[0] as usize] = solver_vel1; - solver_vels[self.solver_vel2[0] as usize] = solver_vel2; + solver_vels[self.solver_vel1[0]] = solver_vel1; + solver_vels[self.solver_vel2[0]] = solver_vel2; } pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { @@ -399,29 +399,21 @@ impl JointTwoBodyConstraint<SimdReal, SIMD_WIDTH> { pub fn solve(&mut self, solver_vels: &mut [SolverVel<Real>]) { let mut solver_vel1 = SolverVel { - linear: Vector::from(gather![ - |ii| solver_vels[self.solver_vel1[ii] as usize].linear - ]), - angular: AngVector::from(gather![ - |ii| solver_vels[self.solver_vel1[ii] as usize].angular - ]), + linear: Vector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].linear]), + angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].angular]), }; 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]), }; self.solve_generic(&mut solver_vel1, &mut solver_vel2); for ii in 0..SIMD_WIDTH { - 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); + solver_vels[self.solver_vel1[ii]].linear = solver_vel1.linear.extract(ii); + solver_vels[self.solver_vel1[ii]].angular = solver_vel1.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); } } @@ -682,9 +674,9 @@ impl JointOneBodyConstraint<Real, 1> { } pub fn solve(&mut self, solver_vels: &mut [SolverVel<Real>]) { - let mut solver_vel2 = solver_vels[self.solver_vel2[0] as usize]; + let mut solver_vel2 = solver_vels[self.solver_vel2[0]]; self.solve_generic(&mut solver_vel2); - solver_vels[self.solver_vel2[0] as usize] = solver_vel2; + solver_vels[self.solver_vel2[0]] = solver_vel2; } pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { @@ -751,19 +743,15 @@ impl JointOneBodyConstraint<SimdReal, SIMD_WIDTH> { pub fn solve(&mut self, solver_vels: &mut [SolverVel<Real>]) { 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]), }; self.solve_generic(&mut solver_vel2); 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); } } |