diff options
Diffstat (limited to 'src/dynamics')
5 files changed, 29 insertions, 35 deletions
diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 96d9eaf..8c0f26c 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -181,6 +181,12 @@ impl IntegrationParameters { self.dt = 1.0 / inv_dt } } + + /// Convenience: `velocity_based_erp / dt` + #[inline] + pub(crate) fn velocity_based_erp_inv_dt(&self) -> Real { + self.velocity_based_erp * self.inv_dt() + } } impl Default for IntegrationParameters { diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs index 33dc5ba..508ac65 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -50,9 +50,8 @@ impl BallVelocityConstraint { let im1 = rb1.effective_inv_mass; let im2 = rb2.effective_inv_mass; - let mut rhs = params.velocity_solve_fraction * (vel2 - vel1); - - rhs += params.velocity_based_erp * params.inv_dt() * (anchor_world2 - anchor_world1); + let rhs = (vel2 - vel1) * params.velocity_solve_fraction + + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt(); let lhs; let cmat1 = anchor1.gcross_matrix(); @@ -293,9 +292,9 @@ impl BallVelocityGroundConstraint { let im2 = rb2.effective_inv_mass; let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1); let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2); - let mut rhs = params.velocity_solve_fraction * (vel2 - vel1); - rhs += params.velocity_based_erp * params.inv_dt() * (anchor_world2 - anchor_world1); + let rhs = (vel2 - vel1) * params.velocity_solve_fraction + + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt(); let cmat2 = anchor2.gcross_matrix(); diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs index cf710b0..db33fde 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs @@ -112,26 +112,23 @@ impl FixedVelocityConstraint { lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, ); - if params.velocity_based_erp != 0.0 { + let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); + if velocity_based_erp_inv_dt != 0.0 { let error = anchor2 * anchor1.inverse(); let lin_err = error.translation.vector; #[cfg(feature = "dim2")] { let ang_err = error.rotation.angle(); - rhs += params.velocity_based_erp - * params.inv_dt() - * Vector3::new(lin_err.x, lin_err.y, ang_err); + rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt; } #[cfg(feature = "dim3")] { let ang_err = error.rotation.scaled_axis(); - rhs += params.velocity_based_erp - * params.inv_dt() - * Vector6::new( - lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z, - ); + rhs += Vector6::new( + lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z, + ) * velocity_based_erp_inv_dt; } } @@ -326,16 +323,14 @@ impl FixedVelocityGroundConstraint { lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, ); - if params.velocity_based_erp != 0.0 { + let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); + if velocity_based_erp_inv_dt != 0.0 { // let error = anchor2 * anchor1.inverse(); // let lin_err = error.translation.vector; // let ang_err = error.rotation; // Doesn't quite do what it should - // let target_pos = anchor1.lerp_slerp( - // &anchor2, - // params.velocity_based_erp * params.inv_dt(), - // ); + // let target_pos = anchor1.lerp_slerp(&anchor2, velocity_based_erp_inv_dt); // let error = target_pos * anchor1.inverse(); // let lin_err = error.translation.vector; @@ -345,19 +340,15 @@ impl FixedVelocityGroundConstraint { #[cfg(feature = "dim2")] { let ang_err = ang_err.angle(); - rhs += params.velocity_based_erp - * params.inv_dt() - * Vector3::new(lin_err.x, lin_err.y, ang_err); + rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt; } #[cfg(feature = "dim3")] { let ang_err = ang_err.scaled_axis(); - rhs += params.velocity_based_erp - * params.inv_dt() - * Vector6::new( - lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z, - ); + rhs += Vector6::new( + lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z, + ) * velocity_based_erp_inv_dt; } } diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index dff8ff0..b7c257a 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -147,6 +147,8 @@ impl VelocityConstraint { assert_eq!(manifold.data.relative_dominance, 0); let inv_dt = params.inv_dt(); + let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); + let rb1 = &bodies[manifold.data.body_pair.body1]; let rb2 = &bodies[manifold.data.body_pair.body2]; let mj_lambda1 = rb1.active_set_offset; @@ -250,10 +252,7 @@ impl VelocityConstraint { * (vel1 - vel2).dot(&force_dir1); rhs += manifold_point.dist.max(0.0) * inv_dt; rhs *= params.velocity_solve_fraction; - rhs += is_resting - * params.velocity_based_erp - * inv_dt - * manifold_point.dist.min(0.0); + rhs += is_resting * velocity_based_erp_inv_dt * manifold_point.dist.min(0.0); constraint.elements[k].normal_part = VelocityConstraintElementPart { gcross1, diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index b3d4eeb..5d29510 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -64,6 +64,8 @@ impl VelocityGroundConstraint { push: bool, ) { let inv_dt = params.inv_dt(); + let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); + let mut rb1 = &bodies[manifold.data.body_pair.body1]; let mut rb2 = &bodies[manifold.data.body_pair.body2]; let flipped = manifold.data.relative_dominance < 0; @@ -162,10 +164,7 @@ impl VelocityGroundConstraint { * (vel1 - vel2).dot(&force_dir1); rhs += manifold_point.dist.max(0.0) * inv_dt; rhs *= params.velocity_solve_fraction; - rhs += is_resting - * params.velocity_based_erp - * inv_dt - * manifold_point.dist.min(0.0); + rhs += is_resting * velocity_based_erp_inv_dt * manifold_point.dist.min(0.0); constraint.elements[k].normal_part = VelocityGroundConstraintElementPart { gcross2, |