From 21247a123691de7a5f53454c7edba838af83c594 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Mon, 15 Feb 2021 20:52:16 +0100 Subject: [PATCH 01/18] Add restorative impulse in velocity solver --- src/dynamics/integration_parameters.rs | 22 ++++-- .../ball_velocity_constraint.rs | 29 ++++--- .../fixed_velocity_constraint.rs | 78 +++++++++++++++++-- src/dynamics/solver/velocity_constraint.rs | 15 ++-- .../solver/velocity_ground_constraint.rs | 15 ++-- 5 files changed, 125 insertions(+), 34 deletions(-) diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index caad9b5..96d9eaf 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -27,6 +27,19 @@ pub struct IntegrationParameters { /// Each cached impulse are multiplied by this coefficient in `[0, 1]` /// when they are re-used to initialize the solver (default `1.0`). pub warmstart_coeff: Real, + + /// 0-1: how much of the velocity to dampen out in the constraint solver? + /// (default `1.0`). + pub velocity_solve_fraction: Real, + + /// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration) + /// will be compensated for during the velocity solve. + /// If zero, you need to enable the positional solver. + /// If non-zero, you do not need the positional solver. + /// A good non-zero value is around `0.2`. + /// (default `0.0`). + pub velocity_based_erp: Real, + /// Amount of penetration the engine wont attempt to correct (default: `0.005m`). pub allowed_linear_error: Real, /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). @@ -121,17 +134,12 @@ impl IntegrationParameters { max_stabilization_multiplier, max_velocity_iterations, max_position_iterations, - // FIXME: 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. - // However we don't want it to be too small and end up with - // tons of islands, reducing SIMD parallelism opportunities. - min_island_size: 128, max_ccd_position_iterations, max_ccd_substeps, return_after_ccd_substep, multiple_ccd_substep_sensor_events_enabled, ccd_on_penetration_enabled, + ..Default::default() } } @@ -183,6 +191,8 @@ impl Default for IntegrationParameters { return_after_ccd_substep: false, erp: 0.2, joint_erp: 0.2, + velocity_solve_fraction: 1.0, + velocity_based_erp: 0.0, warmstart_coeff: 1.0, allowed_linear_error: 0.005, prediction_distance: 0.002, diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs index a110fbb..33dc5ba 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -40,17 +40,21 @@ impl BallVelocityConstraint { rb2: &RigidBody, joint: &BallJoint, ) -> Self { - let anchor1 = rb1.position * joint.local_anchor1 - rb1.world_com; - let anchor2 = rb2.position * joint.local_anchor2 - rb2.world_com; + let anchor_world1 = rb1.position * joint.local_anchor1; + let anchor_world2 = rb2.position * joint.local_anchor2; + let anchor1 = anchor_world1 - rb1.world_com; + let anchor2 = anchor_world2 - rb2.world_com; let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1); let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2); let im1 = rb1.effective_inv_mass; let im2 = rb2.effective_inv_mass; - let rhs = -(vel1 - vel2); - let lhs; + let mut rhs = params.velocity_solve_fraction * (vel2 - vel1); + rhs += params.velocity_based_erp * params.inv_dt() * (anchor_world2 - anchor_world1); + + let lhs; let cmat1 = anchor1.gcross_matrix(); let cmat2 = anchor2.gcross_matrix(); @@ -271,22 +275,27 @@ impl BallVelocityGroundConstraint { joint: &BallJoint, flipped: bool, ) -> Self { - let (anchor1, anchor2) = if flipped { + let (anchor_world1, anchor_world2) = if flipped { ( - rb1.position * joint.local_anchor2 - rb1.world_com, - rb2.position * joint.local_anchor1 - rb2.world_com, + rb1.position * joint.local_anchor2, + rb2.position * joint.local_anchor1, ) } else { ( - rb1.position * joint.local_anchor1 - rb1.world_com, - rb2.position * joint.local_anchor2 - rb2.world_com, + rb1.position * joint.local_anchor1, + rb2.position * joint.local_anchor2, ) }; + let anchor1 = anchor_world1 - rb1.world_com; + let anchor2 = anchor_world2 - rb2.world_com; + let im2 = rb2.effective_inv_mass; let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1); let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2); - let rhs = vel2 - vel1; + let mut rhs = params.velocity_solve_fraction * (vel2 - vel1); + + rhs += params.velocity_based_erp * params.inv_dt() * (anchor_world2 - anchor_world1); 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 2868d4b..cf710b0 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs @@ -103,12 +103,37 @@ impl FixedVelocityConstraint { let ang_dvel = -rb1.angvel + rb2.angvel; #[cfg(feature = "dim2")] - let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + let mut rhs = + params.velocity_solve_fraction * Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); #[cfg(feature = "dim3")] - let rhs = Vector6::new( - lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, - ); + let mut rhs = params.velocity_solve_fraction + * Vector6::new( + 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 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); + } + + #[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, + ); + } + } FixedVelocityConstraint { joint_id, @@ -293,11 +318,48 @@ impl FixedVelocityGroundConstraint { let ang_dvel = rb2.angvel - rb1.angvel; #[cfg(feature = "dim2")] - let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + let mut rhs = + params.velocity_solve_fraction * Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); #[cfg(feature = "dim3")] - let rhs = Vector6::new( - lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, - ); + let mut rhs = params.velocity_solve_fraction + * Vector6::new( + 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 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 error = target_pos * anchor1.inverse(); + // let lin_err = error.translation.vector; + + let lin_err = anchor2.translation.vector - anchor1.translation.vector; + let ang_err = anchor2.rotation * anchor1.rotation.inverse(); + + #[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); + } + + #[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, + ); + } + } FixedVelocityGroundConstraint { joint_id, diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index abc46c9..dff8ff0 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -244,17 +244,22 @@ impl VelocityConstraint { + gcross2.gdot(gcross2)); let is_bouncy = manifold_point.is_bouncy() as u32 as Real; - let rhs = (1.0 + is_bouncy * manifold_point.restitution) - * (vel1 - vel2).dot(&force_dir1) - + manifold_point.dist.max(0.0) * inv_dt; + let is_resting = 1.0 - is_bouncy; - let impulse = manifold_point.data.impulse * warmstart_coeff; + let mut rhs = (1.0 + is_bouncy * manifold_point.restitution) + * (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); constraint.elements[k].normal_part = VelocityConstraintElementPart { gcross1, gcross2, rhs, - impulse, + impulse: manifold_point.data.impulse * warmstart_coeff, r, }; } diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index a3a5563..b3d4eeb 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -156,16 +156,21 @@ impl VelocityGroundConstraint { let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2)); let is_bouncy = manifold_point.is_bouncy() as u32 as Real; - let rhs = (1.0 + is_bouncy * manifold_point.restitution) - * (vel1 - vel2).dot(&force_dir1) - + manifold_point.dist.max(0.0) * inv_dt; + let is_resting = 1.0 - is_bouncy; - let impulse = manifold_point.data.impulse * warmstart_coeff; + let mut rhs = (1.0 + is_bouncy * manifold_point.restitution) + * (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); constraint.elements[k].normal_part = VelocityGroundConstraintElementPart { gcross2, rhs, - impulse, + impulse: manifold_point.data.impulse * warmstart_coeff, r, }; } From 4162aed2a04767e6bac8332b8f8fbacb42f1f1a3 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Wed, 17 Feb 2021 19:08:43 +0100 Subject: [PATCH 02/18] Add params.velocity_based_erp_inv_dt() helper --- src/dynamics/integration_parameters.rs | 6 ++++ .../ball_velocity_constraint.rs | 9 +++-- .../fixed_velocity_constraint.rs | 35 +++++++------------ src/dynamics/solver/velocity_constraint.rs | 7 ++-- .../solver/velocity_ground_constraint.rs | 7 ++-- 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, From 3f26b46196531cfca10ae487bb242e8bb3f84f65 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Wed, 17 Feb 2021 19:15:52 +0100 Subject: [PATCH 03/18] Implement ball wide --- .../ball_velocity_constraint_wide.rs | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs index 95b0bb5..ff5e001 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs @@ -62,12 +62,15 @@ impl WBallVelocityConstraint { let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); - let anchor1 = position1 * local_anchor1 - world_com1; - let anchor2 = position2 * local_anchor2 - world_com2; + let anchor_world1 = position1 * local_anchor1; + let anchor_world2 = position2 * local_anchor2; + let anchor1 = anchor_world1 - world_com1; + let anchor2 = anchor_world2 - world_com2; let vel1: Vector = linvel1 + angvel1.gcross(anchor1); let vel2: Vector = linvel2 + angvel2.gcross(anchor2); - let rhs = -(vel1 - vel2); + let rhs = (vel2 - vel1) * SimdReal::splat(params.velocity_solve_fraction) + + (anchor_world2 - anchor_world1) * SimdReal::splat(params.velocity_based_erp_inv_dt()); let lhs; let cmat1 = anchor1.gcross_matrix(); @@ -239,12 +242,15 @@ impl WBallVelocityGroundConstraint { ); let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); - let anchor1 = position1 * local_anchor1 - world_com1; - let anchor2 = position2 * local_anchor2 - world_com2; + let anchor_world1 = position1 * local_anchor1; + let anchor_world2 = position2 * local_anchor2; + let anchor1 = anchor_world1 - world_com1; + let anchor2 = anchor_world2 - world_com2; let vel1: Vector = linvel1 + angvel1.gcross(anchor1); let vel2: Vector = linvel2 + angvel2.gcross(anchor2); - let rhs = vel2 - vel1; + let rhs = (vel2 - vel1) * SimdReal::splat(params.velocity_solve_fraction) + + (anchor_world2 - anchor_world1) * SimdReal::splat(params.velocity_based_erp_inv_dt()); let lhs; let cmat2 = anchor2.gcross_matrix(); From ede4f0f7703dee4fc6a53e6da7775f7455a0b94e Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Wed, 17 Feb 2021 19:38:57 +0100 Subject: [PATCH 04/18] cleanup --- .../fixed_velocity_constraint.rs | 18 ++++++++---------- src/dynamics/solver/velocity_constraint.rs | 2 +- .../solver/velocity_ground_constraint.rs | 2 +- 3 files changed, 10 insertions(+), 12 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs index db33fde..dc7569c 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs @@ -104,13 +104,12 @@ impl FixedVelocityConstraint { #[cfg(feature = "dim2")] let mut rhs = - params.velocity_solve_fraction * Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * params.velocity_solve_fraction; #[cfg(feature = "dim3")] - let mut rhs = params.velocity_solve_fraction - * Vector6::new( - lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, - ); + let mut rhs = Vector6::new( + lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, + ) * params.velocity_solve_fraction; let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); if velocity_based_erp_inv_dt != 0.0 { @@ -316,12 +315,11 @@ impl FixedVelocityGroundConstraint { #[cfg(feature = "dim2")] let mut rhs = - params.velocity_solve_fraction * Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * params.velocity_solve_fraction; #[cfg(feature = "dim3")] - let mut rhs = params.velocity_solve_fraction - * Vector6::new( - lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, - ); + let mut rhs = Vector6::new( + lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, + ) * params.velocity_solve_fraction; let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); if velocity_based_erp_inv_dt != 0.0 { diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index b7c257a..243d7d7 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -251,7 +251,7 @@ impl VelocityConstraint { let mut rhs = (1.0 + is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1); rhs += manifold_point.dist.max(0.0) * inv_dt; - rhs *= params.velocity_solve_fraction; + rhs *= is_bouncy + is_resting * params.velocity_solve_fraction; rhs += is_resting * velocity_based_erp_inv_dt * manifold_point.dist.min(0.0); constraint.elements[k].normal_part = VelocityConstraintElementPart { diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 5d29510..4e195cd 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -163,7 +163,7 @@ impl VelocityGroundConstraint { let mut rhs = (1.0 + is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1); rhs += manifold_point.dist.max(0.0) * inv_dt; - rhs *= params.velocity_solve_fraction; + rhs *= is_bouncy + is_resting * params.velocity_solve_fraction; rhs += is_resting * velocity_based_erp_inv_dt * manifold_point.dist.min(0.0); constraint.elements[k].normal_part = VelocityGroundConstraintElementPart { From d3f39a9bab028493ee9cd1a3b105ed6616727e03 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Wed, 17 Feb 2021 19:40:02 +0100 Subject: [PATCH 05/18] Implemented prismatic narrow. Needs testing and close review --- .../prismatic_velocity_constraint.rs | 91 +++++++++++++++++-- 1 file changed, 83 insertions(+), 8 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index ae3f3e2..9f19ace 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -135,13 +135,47 @@ impl PrismaticVelocityConstraint { #[cfg(feature = "dim3")] let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); - let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1)); - let ang_rhs = rb2.angvel - rb1.angvel; + let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1)); + let angvel_err = rb2.angvel - rb1.angvel; #[cfg(feature = "dim2")] - let rhs = Vector2::new(lin_rhs.x, ang_rhs); + let mut rhs = Vector2::new(linvel_err.x, angvel_err) * params.velocity_solve_fraction; #[cfg(feature = "dim3")] - let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z); + let mut rhs = Vector5::new( + linvel_err.x, + linvel_err.y, + angvel_err.x, + angvel_err.y, + angvel_err.z, + ) * params.velocity_solve_fraction; + + let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); + if velocity_based_erp_inv_dt != 0.0 { + let dpos = anchor2 - anchor1; + let limit_err = dpos.dot(&axis1); + let mut linear_err = dpos - *axis1 * limit_err; + + let frame1 = rb1.position * cparams.local_frame1(); + let frame2 = rb2.position * cparams.local_frame2(); + let ang_err = frame2.rotation * frame1.rotation.inverse(); + + if limit_err < cparams.limits[0] { + linear_err += *axis1 * (limit_err - cparams.limits[0]); + } else if limit_err > cparams.limits[1] { + linear_err += *axis1 * (limit_err - cparams.limits[1]); + } + + #[cfg(feature = "dim2")] + { + rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt; + } + #[cfg(feature = "dim3")] + { + let ang_err = ang_err.scaled_axis(); + rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z) + * velocity_based_erp_inv_dt; + } + } /* * Setup motor. @@ -520,13 +554,54 @@ impl PrismaticVelocityGroundConstraint { #[cfg(feature = "dim3")] let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); - let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1)); - let ang_rhs = rb2.angvel - rb1.angvel; + let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1)); + let angvel_err = rb2.angvel - rb1.angvel; #[cfg(feature = "dim2")] - let rhs = Vector2::new(lin_rhs.x, ang_rhs); + let mut rhs = Vector2::new(linvel_err.x, angvel_err) * params.velocity_solve_fraction; #[cfg(feature = "dim3")] - let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z); + let mut rhs = Vector5::new( + linvel_err.x, + linvel_err.y, + angvel_err.x, + angvel_err.y, + angvel_err.z, + ) * params.velocity_solve_fraction; + + let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); + if velocity_based_erp_inv_dt != 0.0 { + let (frame1, frame2); + if flipped { + frame1 = rb1.position * cparams.local_frame2(); + frame2 = rb2.position * cparams.local_frame1(); + } else { + frame1 = rb1.position * cparams.local_frame1(); + frame2 = rb2.position * cparams.local_frame2(); + } + + let dpos = anchor2 - anchor1; + let limit_err = dpos.dot(&axis1); + let mut linear_err = dpos - *axis1 * limit_err; + + let ang_err = frame2.rotation * frame1.rotation.inverse(); + + if limit_err < cparams.limits[0] { + linear_err += *axis1 * (limit_err - cparams.limits[0]); + } else if limit_err > cparams.limits[1] { + linear_err += *axis1 * (limit_err - cparams.limits[1]); + } + + #[cfg(feature = "dim2")] + { + rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt; + } + #[cfg(feature = "dim3")] + { + let ang_err = ang_err.scaled_axis(); + rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z) + * velocity_based_erp_inv_dt; + } + } /* * Setup motor. From 48708d9a76b4868f77599c0fc6f303fd194dbb88 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Wed, 17 Feb 2021 20:22:41 +0100 Subject: [PATCH 06/18] Implement revolute narrow --- .../prismatic_velocity_constraint.rs | 28 ++++---- .../revolute_velocity_constraint.rs | 64 ++++++++++++++++--- 2 files changed, 70 insertions(+), 22 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index 9f19ace..440b8ac 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -155,14 +155,14 @@ impl PrismaticVelocityConstraint { let limit_err = dpos.dot(&axis1); let mut linear_err = dpos - *axis1 * limit_err; - let frame1 = rb1.position * cparams.local_frame1(); - let frame2 = rb2.position * cparams.local_frame2(); + let frame1 = rb1.position * joint.local_frame1(); + let frame2 = rb2.position * joint.local_frame2(); let ang_err = frame2.rotation * frame1.rotation.inverse(); - if limit_err < cparams.limits[0] { - linear_err += *axis1 * (limit_err - cparams.limits[0]); - } else if limit_err > cparams.limits[1] { - linear_err += *axis1 * (limit_err - cparams.limits[1]); + if limit_err < joint.limits[0] { + linear_err += *axis1 * (limit_err - joint.limits[0]); + } else if limit_err > joint.limits[1] { + linear_err += *axis1 * (limit_err - joint.limits[1]); } #[cfg(feature = "dim2")] @@ -572,11 +572,11 @@ impl PrismaticVelocityGroundConstraint { if velocity_based_erp_inv_dt != 0.0 { let (frame1, frame2); if flipped { - frame1 = rb1.position * cparams.local_frame2(); - frame2 = rb2.position * cparams.local_frame1(); + frame1 = rb1.position * joint.local_frame2(); + frame2 = rb2.position * joint.local_frame1(); } else { - frame1 = rb1.position * cparams.local_frame1(); - frame2 = rb2.position * cparams.local_frame2(); + frame1 = rb1.position * joint.local_frame1(); + frame2 = rb2.position * joint.local_frame2(); } let dpos = anchor2 - anchor1; @@ -585,10 +585,10 @@ impl PrismaticVelocityGroundConstraint { let ang_err = frame2.rotation * frame1.rotation.inverse(); - if limit_err < cparams.limits[0] { - linear_err += *axis1 * (limit_err - cparams.limits[0]); - } else if limit_err > cparams.limits[1] { - linear_err += *axis1 * (limit_err - cparams.limits[1]); + if limit_err < joint.limits[0] { + linear_err += *axis1 * (limit_err - joint.limits[0]); + } else if limit_err > joint.limits[1] { + linear_err += *axis1 * (limit_err - joint.limits[1]); } #[cfg(feature = "dim2")] diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index 61cb720..46f375b 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -3,9 +3,8 @@ use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody, }; use crate::math::{AngularInertia, Real, Rotation, Vector}; -use crate::na::UnitQuaternion; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; -use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; +use na::{Cholesky, Matrix3x2, Matrix5, UnitQuaternion, Vector5, U2, U3}; #[derive(Debug)] pub(crate) struct RevoluteVelocityConstraint { @@ -90,9 +89,31 @@ impl RevoluteVelocityConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); - let lin_rhs = (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1)); - let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel); - let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); + let linvel_err = + (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1)); + let angvel_err = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel); + + let mut rhs = Vector5::new( + linvel_err.x, + linvel_err.y, + linvel_err.z, + angvel_err.x, + angvel_err.y, + ) * params.velocity_solve_fraction; + + let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); + if velocity_based_erp_inv_dt != 0.0 { + let lin_err = anchor2 - anchor1; + + let axis1 = rb1.position * joint.local_axis1; + let axis2 = rb2.position * joint.local_axis2; + let ang_err = + Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); + let ang_err = ang_err.scaled_axis(); + + rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y) + * velocity_based_erp_inv_dt; + } /* * Motor. @@ -371,9 +392,36 @@ impl RevoluteVelocityGroundConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); - let lin_rhs = (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1)); - let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel); - let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); + let linvel_err = + (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1)); + let angvel_err = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel); + let mut rhs = Vector5::new( + linvel_err.x, + linvel_err.y, + linvel_err.z, + angvel_err.x, + angvel_err.y, + ) * params.velocity_solve_fraction; + + let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); + if velocity_based_erp_inv_dt != 0.0 { + let lin_err = anchor2 - anchor1; + + let (axis1, axis2); + if flipped { + axis1 = rb1.position * joint.local_axis2; + axis2 = rb2.position * joint.local_axis1; + } else { + axis1 = rb1.position * joint.local_axis1; + axis2 = rb2.position * joint.local_axis2; + } + let ang_err = + Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); + let ang_err = ang_err.scaled_axis(); + + rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y) + * velocity_based_erp_inv_dt; + } /* * Motor part. From 27366e27ff1274b4c8d8542eaa0b24f449165555 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Thu, 18 Feb 2021 10:40:42 +0100 Subject: [PATCH 07/18] Implement fixed wide --- .../fixed_velocity_constraint.rs | 9 --- .../fixed_velocity_constraint_wide.rs | 65 +++++++++++++++++-- 2 files changed, 58 insertions(+), 16 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs index dc7569c..a169687 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs @@ -323,15 +323,6 @@ impl FixedVelocityGroundConstraint { 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, velocity_based_erp_inv_dt); - // let error = target_pos * anchor1.inverse(); - // let lin_err = error.translation.vector; - let lin_err = anchor2.translation.vector - anchor1.translation.vector; let ang_err = anchor2.rotation * anchor1.rotation.inverse(); diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs index c423272..6b90343 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs @@ -10,7 +10,7 @@ use crate::math::{ }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[cfg(feature = "dim3")] -use na::{Cholesky, Matrix6, Vector6, U3}; +use na::{Cholesky, Matrix6, Vector3, Vector6, U3}; #[cfg(feature = "dim2")] use { na::{Matrix3, Vector3}, @@ -132,13 +132,38 @@ impl WFixedVelocityConstraint { let lin_dvel = -linvel1 - angvel1.gcross(r1) + linvel2 + angvel2.gcross(r2); let ang_dvel = -angvel1 + angvel2; + let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction); + #[cfg(feature = "dim2")] - let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + let mut rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * velocity_solve_fraction; #[cfg(feature = "dim3")] - let rhs = Vector6::new( + let mut rhs = Vector6::new( lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, - ); + ) * velocity_solve_fraction; + + let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); + if velocity_based_erp_inv_dt != 0.0 { + let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt); + + let lin_err = anchor2.translation.vector - anchor1.translation.vector; + let ang_err = anchor2.rotation * anchor1.rotation.inverse(); + + #[cfg(feature = "dim2")] + { + let ang_err = ang_err.angle(); + rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt; + } + + #[cfg(feature = "dim3")] + { + let ang_err = + Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]); + 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; + } + } WFixedVelocityConstraint { joint_id, @@ -373,12 +398,38 @@ impl WFixedVelocityGroundConstraint { let lin_dvel = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1); let ang_dvel = angvel2 - angvel1; + let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction); + #[cfg(feature = "dim2")] - let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + let mut rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * velocity_solve_fraction; + #[cfg(feature = "dim3")] - let rhs = Vector6::new( + let mut rhs = Vector6::new( lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, - ); + ) * velocity_solve_fraction; + + let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); + if velocity_based_erp_inv_dt != 0.0 { + let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt); + + let lin_err = anchor2.translation.vector - anchor1.translation.vector; + let ang_err = anchor2.rotation * anchor1.rotation.inverse(); + + #[cfg(feature = "dim2")] + { + let ang_err = ang_err.angle(); + rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt; + } + + #[cfg(feature = "dim3")] + { + let ang_err = + Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]); + 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; + } + } WFixedVelocityGroundConstraint { joint_id, From 89de6903dcdc1f0dfa4314cf99dd19fa179eb756 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Thu, 18 Feb 2021 13:43:33 +0100 Subject: [PATCH 08/18] Implement prismatic wide --- .../prismatic_velocity_constraint.rs | 16 +-- .../prismatic_velocity_constraint_wide.rs | 130 ++++++++++++++++-- 2 files changed, 124 insertions(+), 22 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index 440b8ac..c4b905b 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -159,11 +159,9 @@ impl PrismaticVelocityConstraint { let frame2 = rb2.position * joint.local_frame2(); let ang_err = frame2.rotation * frame1.rotation.inverse(); - if limit_err < joint.limits[0] { - linear_err += *axis1 * (limit_err - joint.limits[0]); - } else if limit_err > joint.limits[1] { - linear_err += *axis1 * (limit_err - joint.limits[1]); - } + let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]); + linear_err += + *axis1 * ((limit_err - max_limit).max(0.0) - (min_limit - limit_err).max(0.0)); #[cfg(feature = "dim2")] { @@ -585,11 +583,9 @@ impl PrismaticVelocityGroundConstraint { let ang_err = frame2.rotation * frame1.rotation.inverse(); - if limit_err < joint.limits[0] { - linear_err += *axis1 * (limit_err - joint.limits[0]); - } else if limit_err > joint.limits[1] { - linear_err += *axis1 * (limit_err - joint.limits[1]); - } + let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]); + linear_err += + *axis1 * ((limit_err - max_limit).max(0.0) - (min_limit - limit_err).max(0.0)); #[cfg(feature = "dim2")] { diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs index 442393d..e48148d 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -8,8 +8,10 @@ use crate::math::{ AngVector, AngularInertia, Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot}; + #[cfg(feature = "dim3")] -use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; +use na::{Cholesky, Matrix3x2, Matrix5, Vector3, Vector5, U2, U3}; + #[cfg(feature = "dim2")] use { na::{Matrix2, Vector2}, @@ -18,6 +20,7 @@ use { #[cfg(feature = "dim2")] type LinImpulseDim = na::U1; + #[cfg(feature = "dim3")] type LinImpulseDim = na::U2; @@ -180,13 +183,63 @@ impl WPrismaticVelocityConstraint { #[cfg(feature = "dim3")] let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); - let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1)); - let ang_rhs = angvel2 - angvel1; + let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1)); + let angvel_err = angvel2 - angvel1; + + let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction); #[cfg(feature = "dim2")] - let rhs = Vector2::new(lin_rhs.x, ang_rhs); + let mut rhs = Vector2::new(linvel_err.x, angvel_err) * velocity_solve_fraction; #[cfg(feature = "dim3")] - let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z); + let mut rhs = Vector5::new( + linvel_err.x, + linvel_err.y, + angvel_err.x, + angvel_err.y, + angvel_err.z, + ) * velocity_solve_fraction; + + let limits_enabled = + SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]).any(); + + let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); + if velocity_based_erp_inv_dt != 0.0 { + let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt); + + let dpos = anchor2 - anchor1; + let limit_err = dpos.dot(&axis1); + let mut linear_err = dpos - axis1 * limit_err; + + let local_frame1 = Isometry::from(array![|ii| cparams[ii].local_frame1(); SIMD_WIDTH]); + let local_frame2 = Isometry::from(array![|ii| cparams[ii].local_frame2(); SIMD_WIDTH]); + + let frame1 = position1 * local_frame1; + let frame2 = position2 * local_frame2; + let ang_err = frame2.rotation * frame1.rotation.inverse(); + + if limits_enabled { + let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]); + let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]); + + let zero: SimdReal = na::zero(); + linear_err += axis1 + * ((limit_err - max_limit).simd_max(zero) + - (min_limit - limit_err).simd_max(zero)); + } + + #[cfg(feature = "dim2")] + { + rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt; + } + + #[cfg(feature = "dim3")] + { + let ang_err = + Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]); + rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z) + * velocity_based_erp_inv_dt; + } + } /* * Setup limit constraint. @@ -197,7 +250,7 @@ impl WPrismaticVelocityConstraint { let mut limits_inv_lhs = na::zero(); let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]); - if limits_enabled.any() { + if limits_enabled { let danchor = anchor2 - anchor1; let dist = danchor.dot(&axis1); @@ -553,21 +606,74 @@ impl WPrismaticVelocityGroundConstraint { #[cfg(feature = "dim3")] let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); - let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1)); - let ang_rhs = angvel2 - angvel1; + let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1)); + let angvel_err = angvel2 - angvel1; + + let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction); #[cfg(feature = "dim2")] - let rhs = Vector2::new(lin_rhs.x, ang_rhs); + let mut rhs = Vector2::new(linvel_err.x, angvel_err) * velocity_solve_fraction; #[cfg(feature = "dim3")] - let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z); + let mut rhs = Vector5::new( + linvel_err.x, + linvel_err.y, + angvel_err.x, + angvel_err.y, + angvel_err.z, + ) * velocity_solve_fraction; + + let limits_enabled = + SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]).any(); + + let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); + if velocity_based_erp_inv_dt != 0.0 { + let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt); + + let dpos = anchor2 - anchor1; + let limit_err = dpos.dot(&axis1); + let mut linear_err = dpos - axis1 * limit_err; + + let frame1 = position1 + * Isometry::from( + array![|ii| if flipped[ii] { cparams[ii].local_frame2() } else { cparams[ii].local_frame1() }; SIMD_WIDTH], + ); + let frame2 = position2 + * Isometry::from( + array![|ii| if flipped[ii] { cparams[ii].local_frame1() } else { cparams[ii].local_frame2() }; SIMD_WIDTH], + ); + + let ang_err = frame2.rotation * frame1.rotation.inverse(); + + if limits_enabled { + let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]); + let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]); + + let zero: SimdReal = na::zero(); + linear_err += axis1 + * ((limit_err - max_limit).simd_max(zero) + - (min_limit - limit_err).simd_max(zero)); + } + + #[cfg(feature = "dim2")] + { + rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt; + } + + #[cfg(feature = "dim3")] + { + let ang_err = + Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]); + rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z) + * velocity_based_erp_inv_dt; + } + } // Setup limit constraint. let mut limits_forcedir2 = None; let mut limits_rhs = na::zero(); let mut limits_impulse = na::zero(); - let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]); - if limits_enabled.any() { + if limits_enabled { let danchor = anchor2 - anchor1; let dist = danchor.dot(&axis1); From 48afbac6cefbcf1ad6ab663ce15c503bb549cb69 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Thu, 18 Feb 2021 18:18:47 +0100 Subject: [PATCH 09/18] Implement revolute wide --- .../prismatic_velocity_constraint_wide.rs | 1 - .../revolute_velocity_constraint.rs | 1 + .../revolute_velocity_constraint_wide.rs | 60 ++++++++++++++++--- 3 files changed, 54 insertions(+), 8 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs index e48148d..ec2d5b7 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -248,7 +248,6 @@ impl WPrismaticVelocityConstraint { let mut limits_rhs = na::zero(); let mut limits_impulse = na::zero(); let mut limits_inv_lhs = na::zero(); - let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]); if limits_enabled { let danchor = anchor2 - anchor1; diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index 46f375b..15869e2 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -107,6 +107,7 @@ impl RevoluteVelocityConstraint { let axis1 = rb1.position * joint.local_axis1; let axis2 = rb2.position * joint.local_axis2; + let ang_err = Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); let ang_err = ang_err.scaled_axis(); diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs index 7750d98..4404dd1 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs @@ -8,7 +8,7 @@ use crate::math::{ AngVector, AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Vector, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; -use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; +use na::{Cholesky, Matrix3x2, Matrix5, Vector3, Vector5, U2, U3}; #[derive(Debug)] pub(crate) struct WRevoluteVelocityConstraint { @@ -107,9 +107,32 @@ impl WRevoluteVelocityConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); - let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1); - let ang_rhs = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1); - let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); + let linvel_err = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1); + let angvel_err = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1); + + let mut rhs = Vector5::new( + linvel_err.x, + linvel_err.y, + linvel_err.z, + angvel_err.x, + angvel_err.y, + ) * SimdReal::splat(params.velocity_solve_fraction); + + let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); + if velocity_based_erp_inv_dt != 0.0 { + let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt); + + let lin_err = anchor2 - anchor1; + + let ang_err = Vector3::::from(array![|ii| { + let axis1 = rbs1[ii].position * joints[ii].local_axis1; + let axis2 = rbs2[ii].position * joints[ii].local_axis2; + Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity).scaled_axis() + }; SIMD_WIDTH]); + + rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y) + * velocity_based_erp_inv_dt; + } /* * Adjust the warmstart impulse. @@ -357,9 +380,32 @@ impl WRevoluteVelocityGroundConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); - let lin_rhs = (linvel2 + angvel2.gcross(r2)) - (linvel1 + angvel1.gcross(r1)); - let ang_rhs = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1); - let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); + let linvel_err = (linvel2 + angvel2.gcross(r2)) - (linvel1 + angvel1.gcross(r1)); + let angvel_err = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1); + + let mut rhs = Vector5::new( + linvel_err.x, + linvel_err.y, + linvel_err.z, + angvel_err.x, + angvel_err.y, + ) * SimdReal::splat(params.velocity_solve_fraction); + + let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); + if velocity_based_erp_inv_dt != 0.0 { + let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt); + + let lin_err = anchor2 - anchor1; + + let ang_err = Vector3::::from(array![|ii| { + let axis1 = rbs1[ii].position * if flipped[ii] { joints[ii].local_axis2 } else { joints[ii].local_axis1 }; + let axis2 = rbs2[ii].position * if flipped[ii] { joints[ii].local_axis1 } else { joints[ii].local_axis2 }; + Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity).scaled_axis() + }; SIMD_WIDTH]); + + rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y) + * velocity_based_erp_inv_dt; + } WRevoluteVelocityGroundConstraint { joint_id, From b94cdfa782d322d54cc174cd820f85766954d11b Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Fri, 19 Feb 2021 10:24:00 +0100 Subject: [PATCH 10/18] Implement ground wide --- src/dynamics/solver/velocity_constraint_wide.rs | 12 ++++++++++-- .../solver/velocity_ground_constraint_wide.rs | 12 ++++++++++-- 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 82a764e..d97602c 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -72,6 +72,9 @@ impl WVelocityConstraint { } let inv_dt = SimdReal::splat(params.inv_dt()); + let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction); + let velocity_based_erp_inv_dt = SimdReal::splat(params.velocity_based_erp_inv_dt()); + let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; @@ -132,6 +135,7 @@ impl WVelocityConstraint { let is_bouncy = SimdReal::from( array![|ii| manifold_points[ii][k].is_bouncy() as u32 as Real; SIMD_WIDTH], ); + let is_resting = SimdReal::splat(1.0) - is_bouncy; let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]); let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); let tangent_velocity = @@ -158,8 +162,12 @@ impl WVelocityConstraint { let r = SimdReal::splat(1.0) / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); let projected_velocity = (vel1 - vel2).dot(&force_dir1); - let rhs = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity - + dist.simd_max(SimdReal::zero()) * inv_dt; + let mut rhs = + (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity; + rhs += dist.simd_max(SimdReal::zero()) * inv_dt; + rhs *= is_bouncy + is_resting * velocity_solve_fraction; + rhs += + dist.simd_min(SimdReal::zero()) * (velocity_based_erp_inv_dt * is_resting); constraint.elements[k].normal_part = WVelocityConstraintElementPart { gcross1, diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 5339d8a..8e9f3a1 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -64,6 +64,9 @@ impl WVelocityGroundConstraint { push: bool, ) { let inv_dt = SimdReal::splat(params.inv_dt()); + let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction); + let velocity_based_erp_inv_dt = SimdReal::splat(params.velocity_based_erp_inv_dt()); + let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; let mut flipped = [1.0; SIMD_WIDTH]; @@ -124,6 +127,7 @@ impl WVelocityGroundConstraint { let is_bouncy = SimdReal::from( array![|ii| manifold_points[ii][k].is_bouncy() as u32 as Real; SIMD_WIDTH], ); + let is_resting = SimdReal::splat(1.0) - is_bouncy; let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]); let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); let tangent_velocity = @@ -147,8 +151,12 @@ impl WVelocityGroundConstraint { let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2)); let projected_velocity = (vel1 - vel2).dot(&force_dir1); - let rhs = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity - + dist.simd_max(SimdReal::zero()) * inv_dt; + let mut rhs = + (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity; + rhs += dist.simd_max(SimdReal::zero()) * inv_dt; + rhs *= is_bouncy + is_resting * velocity_solve_fraction; + rhs += + dist.simd_min(SimdReal::zero()) * (velocity_based_erp_inv_dt * is_resting); constraint.elements[k].normal_part = WVelocityGroundConstraintElementPart { gcross2, From 59796e47670797de29db93e9800f039fe4951654 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Fri, 19 Feb 2021 14:41:11 +0100 Subject: [PATCH 11/18] fix --- .../solver/joint_constraint/fixed_velocity_constraint.rs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs index a169687..5782d86 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs @@ -113,18 +113,18 @@ impl FixedVelocityConstraint { 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 lin_err = anchor2.translation.vector - anchor1.translation.vector; + let ang_err = anchor2.rotation * anchor1.rotation.inverse(); #[cfg(feature = "dim2")] { - let ang_err = error.rotation.angle(); + let ang_err = ang_err.angle(); 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(); + let ang_err = ang_err.scaled_axis(); 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; From 4ee09a8bc9e17d43e1dae75fbd02b069c7211637 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Tue, 23 Feb 2021 17:03:32 +0100 Subject: [PATCH 12/18] Fix the narrow pismatic velocity constraint --- .../prismatic_velocity_constraint.rs | 142 +++++++++++------- 1 file changed, 87 insertions(+), 55 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index c4b905b..33b8b40 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -49,9 +49,13 @@ pub(crate) struct PrismaticVelocityConstraint { motor_max_impulse: Real, limits_impulse: Real, - limits_forcedirs: Option<(Vector, Vector)>, + /// World-coordinate direction of the limit force on rb2. + /// The force direction on rb1 is opposite (Newton's third law).. + limits_forcedir2: Vector, limits_rhs: Real, - limits_inv_lhs: Real, + limits_inv_lhs: Option, + /// min/max applied impulse due to limits + limits_impulse_limits: (Real, Real), #[cfg(feature = "dim2")] basis1: Vector2, @@ -152,17 +156,12 @@ impl PrismaticVelocityConstraint { let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); if velocity_based_erp_inv_dt != 0.0 { let dpos = anchor2 - anchor1; - let limit_err = dpos.dot(&axis1); - let mut linear_err = dpos - *axis1 * limit_err; + let linear_err = basis1.tr_mul(&dpos); let frame1 = rb1.position * joint.local_frame1(); let frame2 = rb2.position * joint.local_frame2(); let ang_err = frame2.rotation * frame1.rotation.inverse(); - let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]); - linear_err += - *axis1 * ((limit_err - max_limit).max(0.0) - (min_limit - limit_err).max(0.0)); - #[cfg(feature = "dim2")] { rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt; @@ -211,35 +210,48 @@ impl PrismaticVelocityConstraint { /* * Setup limit constraint. */ - let mut limits_forcedirs = None; + let limits_forcedir2 = axis2.into_inner(); // hopefully axis1 is colinear with axis2 let mut limits_rhs = 0.0; let mut limits_impulse = 0.0; - let mut limits_inv_lhs = 0.0; + let mut limits_inv_lhs = None; + let mut limits_impulse_limits = (0.0, 0.0); if joint.limits_enabled { let danchor = anchor2 - anchor1; let dist = danchor.dot(&axis1); - // TODO: we should allow both limits to be active at - // the same time, and allow predictive constraint activation. - if dist < joint.limits[0] { - limits_forcedirs = Some((-axis1.into_inner(), axis2.into_inner())); - limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1); - limits_impulse = joint.limits_impulse; - } else if dist > joint.limits[1] { - limits_forcedirs = Some((axis1.into_inner(), -axis2.into_inner())); - limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1); - limits_impulse = joint.limits_impulse; + // TODO: we should allow predictive constraint activation. + + let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]); + let below_min = dist < min_limit; + let above_max = max_limit < dist; + + if below_min { + limits_impulse_limits.1 = Real::INFINITY; + } + if above_max { + limits_impulse_limits.0 = -Real::INFINITY; } - if dist < joint.limits[0] || dist > joint.limits[1] { + if below_min || above_max { + limits_impulse = joint + .limits_impulse + .max(limits_impulse_limits.0) + .min(limits_impulse_limits.1); + + limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) + * params.velocity_solve_fraction; + + limits_rhs += velocity_based_erp_inv_dt + * ((dist - max_limit).max(0.0) - (min_limit - dist).max(0.0)); + let gcross1 = r1.gcross(*axis1); let gcross2 = r2.gcross(*axis2); - limits_inv_lhs = crate::utils::inv( + limits_inv_lhs = Some(crate::utils::inv( im1 + im2 + gcross1.gdot(ii1.transform_vector(gcross1)) + gcross2.gdot(ii2.transform_vector(gcross2)), - ); + )); } } @@ -253,9 +265,10 @@ impl PrismaticVelocityConstraint { ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, impulse: joint.impulse * params.warmstart_coeff, limits_impulse: limits_impulse * params.warmstart_coeff, - limits_forcedirs, + limits_forcedir2, limits_rhs, limits_inv_lhs, + limits_impulse_limits, motor_rhs, motor_inv_lhs, motor_impulse, @@ -295,10 +308,11 @@ impl PrismaticVelocityConstraint { mj_lambda2.linear -= self.motor_axis2 * (self.im2 * self.motor_impulse); // Warmstart limits. - if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { + if self.limits_inv_lhs.is_some() { + let limits_forcedir1 = -self.limits_forcedir2; + let limits_forcedir2 = self.limits_forcedir2; let limit_impulse1 = limits_forcedir1 * self.limits_impulse; let limit_impulse2 = limits_forcedir2 * self.limits_impulse; - mj_lambda1.linear += self.im1 * limit_impulse1; mj_lambda1.angular += self .ii1_sqrt @@ -345,14 +359,19 @@ impl PrismaticVelocityConstraint { } fn solve_limits(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { - if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { + if let Some(limits_inv_lhs) = self.limits_inv_lhs { + let limits_forcedir1 = -self.limits_forcedir2; + let limits_forcedir2 = self.limits_forcedir2; + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1))) + self.limits_rhs; - let new_impulse = (self.limits_impulse - lin_dvel * self.limits_inv_lhs).max(0.0); + let new_impulse = (self.limits_impulse - lin_dvel * limits_inv_lhs) + .max(self.limits_impulse_limits.0) + .min(self.limits_impulse_limits.1); let dimpulse = new_impulse - self.limits_impulse; self.limits_impulse = new_impulse; @@ -428,8 +447,11 @@ pub(crate) struct PrismaticVelocityGroundConstraint { #[cfg(feature = "dim3")] impulse: Vector5, + limits_forcedir2: Vector, limits_impulse: Real, limits_rhs: Real, + /// min/max applied impulse due to limits + limits_impulse_limits: (Real, Real), axis2: Vector, motor_impulse: Real, @@ -441,7 +463,6 @@ pub(crate) struct PrismaticVelocityGroundConstraint { basis1: Vector2, #[cfg(feature = "dim3")] basis1: Matrix3x2, - limits_forcedir2: Option>, im2: Real, ii2_sqrt: AngularInertia, @@ -578,15 +599,9 @@ impl PrismaticVelocityGroundConstraint { } let dpos = anchor2 - anchor1; - let limit_err = dpos.dot(&axis1); - let mut linear_err = dpos - *axis1 * limit_err; + let linear_err = basis1.tr_mul(&dpos); let ang_err = frame2.rotation * frame1.rotation.inverse(); - - let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]); - linear_err += - *axis1 * ((limit_err - max_limit).max(0.0) - (min_limit - limit_err).max(0.0)); - #[cfg(feature = "dim2")] { rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt; @@ -635,25 +650,39 @@ impl PrismaticVelocityGroundConstraint { /* * Setup limit constraint. */ - let mut limits_forcedir2 = None; + let limits_forcedir2 = axis2.into_inner(); let mut limits_rhs = 0.0; let mut limits_impulse = 0.0; + let mut limits_impulse_limits = (0.0, 0.0); if joint.limits_enabled { let danchor = anchor2 - anchor1; let dist = danchor.dot(&axis1); - // TODO: we should allow both limits to be active at - // the same time. - // TODO: allow predictive constraint activation. - if dist < joint.limits[0] { - limits_forcedir2 = Some(axis2.into_inner()); - limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1); - limits_impulse = joint.limits_impulse; - } else if dist > joint.limits[1] { - limits_forcedir2 = Some(-axis2.into_inner()); - limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1); - limits_impulse = joint.limits_impulse; + // TODO: we should allow predictive constraint activation. + + let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]); + let below_min = dist < min_limit; + let above_max = max_limit < dist; + + if below_min { + limits_impulse_limits.1 = Real::INFINITY; + } + if above_max { + limits_impulse_limits.0 = -Real::INFINITY; + } + + if below_min || above_max { + limits_impulse = joint + .limits_impulse + .max(limits_impulse_limits.0) + .min(limits_impulse_limits.1); + + limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) + * params.velocity_solve_fraction; + + limits_rhs += velocity_based_erp_inv_dt + * ((dist - max_limit).max(0.0) - (min_limit - dist).max(0.0)); } } @@ -675,6 +704,7 @@ impl PrismaticVelocityGroundConstraint { axis2: axis2.into_inner(), limits_forcedir2, limits_rhs, + limits_impulse_limits, } } @@ -696,9 +726,7 @@ impl PrismaticVelocityGroundConstraint { mj_lambda2.linear -= self.axis2 * (self.im2 * self.motor_impulse); // Warmstart limits. - if let Some(limits_forcedir2) = self.limits_forcedir2 { - mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse); - } + mj_lambda2.linear += self.limits_forcedir2 * (self.im2 * self.limits_impulse); mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } @@ -728,16 +756,20 @@ impl PrismaticVelocityGroundConstraint { } fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel) { - if let Some(limits_forcedir2) = self.limits_forcedir2 { + if self.limits_impulse_limits != (0.0, 0.0) { let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) + let lin_dvel = self + .limits_forcedir2 + .dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) + self.limits_rhs; - let new_impulse = (self.limits_impulse - lin_dvel / self.im2).max(0.0); + let new_impulse = (self.limits_impulse - lin_dvel / self.im2) + .max(self.limits_impulse_limits.0) + .min(self.limits_impulse_limits.1); let dimpulse = new_impulse - self.limits_impulse; self.limits_impulse = new_impulse; - mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse); + mj_lambda2.linear += self.limits_forcedir2 * (self.im2 * dimpulse); } } From 9bbb0815397c48f6fa19421a127a77447f279a4b Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Tue, 23 Feb 2021 17:41:03 +0100 Subject: [PATCH 13/18] fix narrow revolute velocity --- .../joint_constraint/revolute_velocity_constraint.rs | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index 15869e2..839cddc 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -108,9 +108,8 @@ impl RevoluteVelocityConstraint { let axis1 = rb1.position * joint.local_axis1; let axis2 = rb2.position * joint.local_axis2; - let ang_err = - Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); - let ang_err = ang_err.scaled_axis(); + let axis_error = axis1.cross(&axis2); + let ang_err = basis2.tr_mul(&axis_error) - basis1.tr_mul(&axis_error); rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y) * velocity_based_erp_inv_dt; @@ -416,9 +415,8 @@ impl RevoluteVelocityGroundConstraint { axis1 = rb1.position * joint.local_axis1; axis2 = rb2.position * joint.local_axis2; } - let ang_err = - Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); - let ang_err = ang_err.scaled_axis(); + let axis_error = axis1.cross(&axis2); + let ang_err = basis2.tr_mul(&axis_error); rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y) * velocity_based_erp_inv_dt; @@ -521,6 +519,7 @@ impl RevoluteVelocityGroundConstraint { .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); } + fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel) { if self.motor_inv_lhs != 0.0 { let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); From a0824772c9f5ef690c40db87abf165d41656404a Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Tue, 23 Feb 2021 17:49:33 +0100 Subject: [PATCH 14/18] fix the revolute wide --- .../revolute_velocity_constraint_wide.rs | 33 ++++++++++++------- 1 file changed, 22 insertions(+), 11 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs index 4404dd1..ad25101 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs @@ -8,7 +8,7 @@ use crate::math::{ AngVector, AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Vector, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; -use na::{Cholesky, Matrix3x2, Matrix5, Vector3, Vector5, U2, U3}; +use na::{Cholesky, Matrix3x2, Matrix5, Unit, Vector5, U2, U3}; #[derive(Debug)] pub(crate) struct WRevoluteVelocityConstraint { @@ -124,11 +124,16 @@ impl WRevoluteVelocityConstraint { let lin_err = anchor2 - anchor1; - let ang_err = Vector3::::from(array![|ii| { - let axis1 = rbs1[ii].position * joints[ii].local_axis1; - let axis2 = rbs2[ii].position * joints[ii].local_axis2; - Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity).scaled_axis() - }; SIMD_WIDTH]); + let local_axis1 = + Unit::>::from(array![|ii| joints[ii].local_axis1; SIMD_WIDTH]); + let local_axis2 = + Unit::>::from(array![|ii| joints[ii].local_axis2; SIMD_WIDTH]); + + let axis1 = position1 * local_axis1; + let axis2 = position2 * local_axis2; + + let axis_error = axis1.cross(&axis2); + let ang_err = basis2.tr_mul(&axis_error) - basis1.tr_mul(&axis_error); rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y) * velocity_based_erp_inv_dt; @@ -397,11 +402,17 @@ impl WRevoluteVelocityGroundConstraint { let lin_err = anchor2 - anchor1; - let ang_err = Vector3::::from(array![|ii| { - let axis1 = rbs1[ii].position * if flipped[ii] { joints[ii].local_axis2 } else { joints[ii].local_axis1 }; - let axis2 = rbs2[ii].position * if flipped[ii] { joints[ii].local_axis1 } else { joints[ii].local_axis2 }; - Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity).scaled_axis() - }; SIMD_WIDTH]); + let local_axis1 = Unit::>::from( + array![|ii| if flipped[ii] { joints[ii].local_axis2 } else { joints[ii].local_axis1 }; SIMD_WIDTH], + ); + let local_axis2 = Unit::>::from( + array![|ii| if flipped[ii] { joints[ii].local_axis1 } else { joints[ii].local_axis2 }; SIMD_WIDTH], + ); + let axis1 = position1 * local_axis1; + let axis2 = position2 * local_axis2; + + let axis_error = axis1.cross(&axis2); + let ang_err = basis2.tr_mul(&axis_error) - basis1.tr_mul(&axis_error); rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y) * velocity_based_erp_inv_dt; From 4ef7b1cefebb678d6d673c7f5d9a1a86bc4b0e80 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Tue, 23 Feb 2021 18:28:47 +0100 Subject: [PATCH 15/18] Fix primatic wide --- .../prismatic_velocity_constraint.rs | 52 +++--- .../prismatic_velocity_constraint_wide.rs | 172 +++++++++--------- 2 files changed, 113 insertions(+), 111 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index 33b8b40..d5a0f3d 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -223,27 +223,22 @@ impl PrismaticVelocityConstraint { // TODO: we should allow predictive constraint activation. let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]); - let below_min = dist < min_limit; - let above_max = max_limit < dist; + let min_enabled = dist < min_limit; + let max_enabled = max_limit < dist; - if below_min { + if min_enabled { limits_impulse_limits.1 = Real::INFINITY; } - if above_max { + if max_enabled { limits_impulse_limits.0 = -Real::INFINITY; } - if below_min || above_max { - limits_impulse = joint - .limits_impulse - .max(limits_impulse_limits.0) - .min(limits_impulse_limits.1); - + if min_enabled || max_enabled { limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) * params.velocity_solve_fraction; - limits_rhs += velocity_based_erp_inv_dt - * ((dist - max_limit).max(0.0) - (min_limit - dist).max(0.0)); + limits_rhs += ((dist - max_limit).max(0.0) - (min_limit - dist).max(0.0)) + * velocity_based_erp_inv_dt; let gcross1 = r1.gcross(*axis1); let gcross2 = r2.gcross(*axis2); @@ -252,6 +247,11 @@ impl PrismaticVelocityConstraint { + gcross1.gdot(ii1.transform_vector(gcross1)) + gcross2.gdot(ii2.transform_vector(gcross2)), )); + + limits_impulse = joint + .limits_impulse + .max(limits_impulse_limits.0) + .min(limits_impulse_limits.1); } } @@ -589,6 +589,9 @@ impl PrismaticVelocityGroundConstraint { let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); if velocity_based_erp_inv_dt != 0.0 { + let dpos = anchor2 - anchor1; + let linear_err = basis1.tr_mul(&dpos); + let (frame1, frame2); if flipped { frame1 = rb1.position * joint.local_frame2(); @@ -598,9 +601,6 @@ impl PrismaticVelocityGroundConstraint { frame2 = rb2.position * joint.local_frame2(); } - let dpos = anchor2 - anchor1; - let linear_err = basis1.tr_mul(&dpos); - let ang_err = frame2.rotation * frame1.rotation.inverse(); #[cfg(feature = "dim2")] { @@ -662,27 +662,27 @@ impl PrismaticVelocityGroundConstraint { // TODO: we should allow predictive constraint activation. let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]); - let below_min = dist < min_limit; - let above_max = max_limit < dist; + let min_enabled = dist < min_limit; + let max_enabled = max_limit < dist; - if below_min { + if min_enabled { limits_impulse_limits.1 = Real::INFINITY; } - if above_max { + if max_enabled { limits_impulse_limits.0 = -Real::INFINITY; } - if below_min || above_max { + if min_enabled || max_enabled { + limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) + * params.velocity_solve_fraction; + + limits_rhs += ((dist - max_limit).max(0.0) - (min_limit - dist).max(0.0)) + * velocity_based_erp_inv_dt; + limits_impulse = joint .limits_impulse .max(limits_impulse_limits.0) .min(limits_impulse_limits.1); - - limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) - * params.velocity_solve_fraction; - - limits_rhs += velocity_based_erp_inv_dt - * ((dist - max_limit).max(0.0) - (min_limit - dist).max(0.0)); } } diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs index ec2d5b7..a9d95e5 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -49,9 +49,10 @@ pub(crate) struct WPrismaticVelocityConstraint { impulse: Vector2, limits_impulse: SimdReal, - limits_forcedirs: Option<(Vector, Vector)>, + limits_forcedir2: Vector, limits_rhs: SimdReal, - limits_inv_lhs: SimdReal, + limits_inv_lhs: Option, + limits_impulse_limits: (SimdReal, SimdReal), #[cfg(feature = "dim2")] basis1: Vector2, @@ -199,16 +200,12 @@ impl WPrismaticVelocityConstraint { angvel_err.z, ) * velocity_solve_fraction; - let limits_enabled = - SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]).any(); - let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); if velocity_based_erp_inv_dt != 0.0 { let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt); let dpos = anchor2 - anchor1; - let limit_err = dpos.dot(&axis1); - let mut linear_err = dpos - axis1 * limit_err; + let linear_err = basis1.tr_mul(&dpos); let local_frame1 = Isometry::from(array![|ii| cparams[ii].local_frame1(); SIMD_WIDTH]); let local_frame2 = Isometry::from(array![|ii| cparams[ii].local_frame2(); SIMD_WIDTH]); @@ -217,16 +214,6 @@ impl WPrismaticVelocityConstraint { let frame2 = position2 * local_frame2; let ang_err = frame2.rotation * frame1.rotation.inverse(); - if limits_enabled { - let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]); - let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]); - - let zero: SimdReal = na::zero(); - linear_err += axis1 - * ((limit_err - max_limit).simd_max(zero) - - (min_limit - limit_err).simd_max(zero)); - } - #[cfg(feature = "dim2")] { rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt; @@ -241,42 +228,53 @@ impl WPrismaticVelocityConstraint { } } - /* - * Setup limit constraint. - */ - let mut limits_forcedirs = None; - let mut limits_rhs = na::zero(); - let mut limits_impulse = na::zero(); - let mut limits_inv_lhs = na::zero(); + // Setup limit constraint. + let zero: SimdReal = na::zero(); + let limits_forcedir2 = axis2; // hopefully axis1 is colinear with axis2 + let mut limits_rhs = zero; + let mut limits_impulse = zero; + let mut limits_inv_lhs = None; + let mut limits_impulse_limits = (zero, zero); - if limits_enabled { + let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]); + if limits_enabled.any() { let danchor = anchor2 - anchor1; let dist = danchor.dot(&axis1); - // FIXME: we should allow both limits to be active at - // the same time + allow predictive constraint activation. + // TODO: we should allow predictive constraint activation. + let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]); let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]); - let lim_impulse = SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]); let min_enabled = dist.simd_lt(min_limit); let max_enabled = dist.simd_gt(max_limit); - let _0: SimdReal = na::zero(); - let _1: SimdReal = na::one(); - let sign = _1.select(min_enabled, (-_1).select(max_enabled, _0)); - if sign != _0 { + limits_impulse_limits.1 = SimdReal::splat(Real::INFINITY).select(min_enabled, zero); + limits_impulse_limits.0 = SimdReal::splat(-Real::INFINITY).select(max_enabled, zero); + + if (min_enabled | max_enabled).any() { let gcross1 = r1.gcross(axis1); let gcross2 = r2.gcross(axis2); - limits_forcedirs = Some((axis1 * -sign, axis2 * sign)); - limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) * sign; - limits_impulse = lim_impulse.select(min_enabled | max_enabled, _0); - limits_inv_lhs = SimdReal::splat(1.0) - / (im1 - + im2 - + gcross1.gdot(ii1.transform_vector(gcross1)) - + gcross2.gdot(ii2.transform_vector(gcross2))); + limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) + * velocity_solve_fraction; + + limits_rhs += ((dist - max_limit).simd_max(zero) + - (min_limit - dist).simd_max(zero)) + * SimdReal::splat(velocity_based_erp_inv_dt); + + limits_impulse = + SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]) + .simd_max(limits_impulse_limits.0) + .simd_min(limits_impulse_limits.1); + + limits_inv_lhs = Some( + SimdReal::splat(1.0) + / (im1 + + im2 + + gcross1.gdot(ii1.transform_vector(gcross1)) + + gcross2.gdot(ii2.transform_vector(gcross2))), + ); } } @@ -290,9 +288,10 @@ impl WPrismaticVelocityConstraint { ii2_sqrt, impulse: impulse * SimdReal::splat(params.warmstart_coeff), limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff), - limits_forcedirs, + limits_forcedir2, limits_rhs, limits_inv_lhs, + limits_impulse_limits, basis1, inv_lhs, rhs, @@ -336,9 +335,9 @@ impl WPrismaticVelocityConstraint { .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); // Warmstart limits. - if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { - let limit_impulse1 = limits_forcedir1 * self.limits_impulse; - let limit_impulse2 = limits_forcedir2 * self.limits_impulse; + if self.limits_inv_lhs.is_some() { + let limit_impulse1 = -self.limits_forcedir2 * self.limits_impulse; + let limit_impulse2 = self.limits_forcedir2 * self.limits_impulse; mj_lambda1.linear += limit_impulse1 * self.im1; mj_lambda1.angular += self @@ -400,7 +399,10 @@ impl WPrismaticVelocityConstraint { mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel, ) { - if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { + if let Some(limits_inv_lhs) = self.limits_inv_lhs { + let limits_forcedir1 = -self.limits_forcedir2; + let limits_forcedir2 = self.limits_forcedir2; + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); @@ -408,7 +410,7 @@ impl WPrismaticVelocityConstraint { + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1))) + self.limits_rhs; let new_impulse = - (self.limits_impulse - lin_dvel * self.limits_inv_lhs).simd_max(na::zero()); + (self.limits_impulse - lin_dvel * limits_inv_lhs).simd_max(na::zero()); let dimpulse = new_impulse - self.limits_impulse; self.limits_impulse = new_impulse; @@ -486,15 +488,17 @@ pub(crate) struct WPrismaticVelocityGroundConstraint { #[cfg(feature = "dim3")] impulse: Vector5, + limits_enabled: bool, + limits_forcedir2: Vector, limits_impulse: SimdReal, limits_rhs: SimdReal, + limits_impulse_limits: (SimdReal, SimdReal), axis2: Vector, #[cfg(feature = "dim2")] basis1: Vector2, #[cfg(feature = "dim3")] basis1: Matrix3x2, - limits_forcedir2: Option>, im2: SimdReal, ii2_sqrt: AngularInertia, @@ -621,16 +625,12 @@ impl WPrismaticVelocityGroundConstraint { angvel_err.z, ) * velocity_solve_fraction; - let limits_enabled = - SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]).any(); - let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); if velocity_based_erp_inv_dt != 0.0 { let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt); let dpos = anchor2 - anchor1; - let limit_err = dpos.dot(&axis1); - let mut linear_err = dpos - axis1 * limit_err; + let linear_err = basis1.tr_mul(&dpos); let frame1 = position1 * Isometry::from( @@ -643,16 +643,6 @@ impl WPrismaticVelocityGroundConstraint { let ang_err = frame2.rotation * frame1.rotation.inverse(); - if limits_enabled { - let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]); - let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]); - - let zero: SimdReal = na::zero(); - linear_err += axis1 - * ((limit_err - max_limit).simd_max(zero) - - (min_limit - limit_err).simd_max(zero)); - } - #[cfg(feature = "dim2")] { rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt; @@ -668,30 +658,40 @@ impl WPrismaticVelocityGroundConstraint { } // Setup limit constraint. - let mut limits_forcedir2 = None; - let mut limits_rhs = na::zero(); - let mut limits_impulse = na::zero(); + let zero: SimdReal = na::zero(); + let limits_forcedir2 = axis2; // hopefully axis1 is colinear with axis2 + let mut limits_rhs = zero; + let mut limits_impulse = zero; + let mut limits_impulse_limits = (zero, zero); + let limits_enabled = + SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]).any(); if limits_enabled { let danchor = anchor2 - anchor1; let dist = danchor.dot(&axis1); - // FIXME: we should allow both limits to be active at - // the same time + allow predictive constraint activation. + // TODO: we should allow predictive constraint activation. let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]); let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]); - let lim_impulse = SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]); - let use_min = dist.simd_lt(min_limit); - let use_max = dist.simd_gt(max_limit); - let _0: SimdReal = na::zero(); - let _1: SimdReal = na::one(); - let sign = _1.select(use_min, (-_1).select(use_max, _0)); + let min_enabled = dist.simd_lt(min_limit); + let max_enabled = dist.simd_gt(max_limit); - if sign != _0 { - limits_forcedir2 = Some(axis2 * sign); - limits_rhs = anchor_linvel2.dot(&axis2) * sign - anchor_linvel1.dot(&axis1) * sign; - limits_impulse = lim_impulse.select(use_min | use_max, _0); + limits_impulse_limits.1 = SimdReal::splat(Real::INFINITY).select(min_enabled, zero); + limits_impulse_limits.0 = SimdReal::splat(-Real::INFINITY).select(max_enabled, zero); + + if (min_enabled | max_enabled).any() { + limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) + * velocity_solve_fraction; + + limits_rhs += ((dist - max_limit).simd_max(zero) + - (min_limit - dist).simd_max(zero)) + * SimdReal::splat(velocity_based_erp_inv_dt); + + limits_impulse = + SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]) + .simd_max(limits_impulse_limits.0) + .simd_min(limits_impulse_limits.1); } } @@ -701,14 +701,16 @@ impl WPrismaticVelocityGroundConstraint { im2, ii2_sqrt, impulse: impulse * SimdReal::splat(params.warmstart_coeff), + limits_enabled, + limits_forcedir2, + limits_rhs, limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff), + limits_impulse_limits, basis1, inv_lhs, rhs, r2, axis2, - limits_forcedir2, - limits_rhs, } } @@ -733,9 +735,7 @@ impl WPrismaticVelocityGroundConstraint { .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); - if let Some(limits_forcedir2) = self.limits_forcedir2 { - mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse); - } + mj_lambda2.linear += self.limits_forcedir2 * (self.im2 * self.limits_impulse); for ii in 0..SIMD_WIDTH { mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); @@ -768,18 +768,20 @@ impl WPrismaticVelocityGroundConstraint { } fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel) { - if let Some(limits_forcedir2) = self.limits_forcedir2 { + if self.limits_enabled { // FIXME: the transformation by ii2_sqrt could be avoided by // reusing some computations above. let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) + let lin_dvel = self + .limits_forcedir2 + .dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) + self.limits_rhs; let new_impulse = (self.limits_impulse - lin_dvel / self.im2).simd_max(na::zero()); let dimpulse = new_impulse - self.limits_impulse; self.limits_impulse = new_impulse; - mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse); + mj_lambda2.linear += self.limits_forcedir2 * (self.im2 * dimpulse); } } From 54eae9bb7b3070c5a6895da82c90008681475f73 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Tue, 23 Feb 2021 19:26:47 +0100 Subject: [PATCH 16/18] simplify prismatic limits --- .../prismatic_velocity_constraint.rs | 38 ++++++++------ .../prismatic_velocity_constraint_wide.rs | 50 +++++++++++-------- 2 files changed, 50 insertions(+), 38 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index d5a0f3d..2bd5b5c 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -48,12 +48,13 @@ pub(crate) struct PrismaticVelocityConstraint { motor_inv_lhs: Real, motor_max_impulse: Real, + limits_active: bool, limits_impulse: Real, /// World-coordinate direction of the limit force on rb2. /// The force direction on rb1 is opposite (Newton's third law).. limits_forcedir2: Vector, limits_rhs: Real, - limits_inv_lhs: Option, + limits_inv_lhs: Real, /// min/max applied impulse due to limits limits_impulse_limits: (Real, Real), @@ -207,13 +208,12 @@ impl PrismaticVelocityConstraint { joint.motor_max_impulse, ); - /* - * Setup limit constraint. - */ + // Setup limit constraint. + let mut limits_active = false; let limits_forcedir2 = axis2.into_inner(); // hopefully axis1 is colinear with axis2 let mut limits_rhs = 0.0; let mut limits_impulse = 0.0; - let mut limits_inv_lhs = None; + let mut limits_inv_lhs = 0.0; let mut limits_impulse_limits = (0.0, 0.0); if joint.limits_enabled { @@ -233,7 +233,8 @@ impl PrismaticVelocityConstraint { limits_impulse_limits.0 = -Real::INFINITY; } - if min_enabled || max_enabled { + limits_active = min_enabled || max_enabled; + if limits_active { limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) * params.velocity_solve_fraction; @@ -242,11 +243,11 @@ impl PrismaticVelocityConstraint { let gcross1 = r1.gcross(*axis1); let gcross2 = r2.gcross(*axis2); - limits_inv_lhs = Some(crate::utils::inv( + limits_inv_lhs = crate::utils::inv( im1 + im2 + gcross1.gdot(ii1.transform_vector(gcross1)) + gcross2.gdot(ii2.transform_vector(gcross2)), - )); + ); limits_impulse = joint .limits_impulse @@ -264,6 +265,7 @@ impl PrismaticVelocityConstraint { im2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, impulse: joint.impulse * params.warmstart_coeff, + limits_active, limits_impulse: limits_impulse * params.warmstart_coeff, limits_forcedir2, limits_rhs, @@ -308,7 +310,7 @@ impl PrismaticVelocityConstraint { mj_lambda2.linear -= self.motor_axis2 * (self.im2 * self.motor_impulse); // Warmstart limits. - if self.limits_inv_lhs.is_some() { + if self.limits_active { let limits_forcedir1 = -self.limits_forcedir2; let limits_forcedir2 = self.limits_forcedir2; let limit_impulse1 = limits_forcedir1 * self.limits_impulse; @@ -359,7 +361,7 @@ impl PrismaticVelocityConstraint { } fn solve_limits(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { - if let Some(limits_inv_lhs) = self.limits_inv_lhs { + if self.limits_active { let limits_forcedir1 = -self.limits_forcedir2; let limits_forcedir2 = self.limits_forcedir2; @@ -369,7 +371,7 @@ impl PrismaticVelocityConstraint { let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1))) + self.limits_rhs; - let new_impulse = (self.limits_impulse - lin_dvel * limits_inv_lhs) + let new_impulse = (self.limits_impulse - lin_dvel * self.limits_inv_lhs) .max(self.limits_impulse_limits.0) .min(self.limits_impulse_limits.1); let dimpulse = new_impulse - self.limits_impulse; @@ -447,6 +449,7 @@ pub(crate) struct PrismaticVelocityGroundConstraint { #[cfg(feature = "dim3")] impulse: Vector5, + limits_active: bool, limits_forcedir2: Vector, limits_impulse: Real, limits_rhs: Real, @@ -650,6 +653,7 @@ impl PrismaticVelocityGroundConstraint { /* * Setup limit constraint. */ + let mut limits_active = false; let limits_forcedir2 = axis2.into_inner(); let mut limits_rhs = 0.0; let mut limits_impulse = 0.0; @@ -672,7 +676,8 @@ impl PrismaticVelocityGroundConstraint { limits_impulse_limits.0 = -Real::INFINITY; } - if min_enabled || max_enabled { + limits_active = min_enabled || max_enabled; + if limits_active { limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) * params.velocity_solve_fraction; @@ -692,7 +697,11 @@ impl PrismaticVelocityGroundConstraint { im2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, impulse: joint.impulse * params.warmstart_coeff, + limits_active, + limits_forcedir2, limits_impulse: limits_impulse * params.warmstart_coeff, + limits_rhs, + limits_impulse_limits, motor_rhs, motor_inv_lhs, motor_impulse, @@ -702,9 +711,6 @@ impl PrismaticVelocityGroundConstraint { rhs, r2, axis2: axis2.into_inner(), - limits_forcedir2, - limits_rhs, - limits_impulse_limits, } } @@ -756,7 +762,7 @@ impl PrismaticVelocityGroundConstraint { } fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel) { - if self.limits_impulse_limits != (0.0, 0.0) { + if self.limits_active { let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let lin_dvel = self diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs index a9d95e5..5186ee7 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -48,10 +48,11 @@ pub(crate) struct WPrismaticVelocityConstraint { #[cfg(feature = "dim2")] impulse: Vector2, + limits_active: bool, limits_impulse: SimdReal, limits_forcedir2: Vector, limits_rhs: SimdReal, - limits_inv_lhs: Option, + limits_inv_lhs: SimdReal, limits_impulse_limits: (SimdReal, SimdReal), #[cfg(feature = "dim2")] @@ -231,9 +232,10 @@ impl WPrismaticVelocityConstraint { // Setup limit constraint. let zero: SimdReal = na::zero(); let limits_forcedir2 = axis2; // hopefully axis1 is colinear with axis2 + let mut limits_active = false; let mut limits_rhs = zero; let mut limits_impulse = zero; - let mut limits_inv_lhs = None; + let mut limits_inv_lhs = zero; let mut limits_impulse_limits = (zero, zero); let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]); @@ -252,7 +254,8 @@ impl WPrismaticVelocityConstraint { limits_impulse_limits.1 = SimdReal::splat(Real::INFINITY).select(min_enabled, zero); limits_impulse_limits.0 = SimdReal::splat(-Real::INFINITY).select(max_enabled, zero); - if (min_enabled | max_enabled).any() { + limits_active = (min_enabled | max_enabled).any(); + if limits_active { let gcross1 = r1.gcross(axis1); let gcross2 = r2.gcross(axis2); @@ -268,13 +271,11 @@ impl WPrismaticVelocityConstraint { .simd_max(limits_impulse_limits.0) .simd_min(limits_impulse_limits.1); - limits_inv_lhs = Some( - SimdReal::splat(1.0) - / (im1 - + im2 - + gcross1.gdot(ii1.transform_vector(gcross1)) - + gcross2.gdot(ii2.transform_vector(gcross2))), - ); + limits_inv_lhs = SimdReal::splat(1.0) + / (im1 + + im2 + + gcross1.gdot(ii1.transform_vector(gcross1)) + + gcross2.gdot(ii2.transform_vector(gcross2))); } } @@ -286,6 +287,7 @@ impl WPrismaticVelocityConstraint { ii1_sqrt, im2, ii2_sqrt, + limits_active, impulse: impulse * SimdReal::splat(params.warmstart_coeff), limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff), limits_forcedir2, @@ -335,7 +337,7 @@ impl WPrismaticVelocityConstraint { .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); // Warmstart limits. - if self.limits_inv_lhs.is_some() { + if self.limits_active { let limit_impulse1 = -self.limits_forcedir2 * self.limits_impulse; let limit_impulse2 = self.limits_forcedir2 * self.limits_impulse; @@ -399,7 +401,7 @@ impl WPrismaticVelocityConstraint { mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel, ) { - if let Some(limits_inv_lhs) = self.limits_inv_lhs { + if self.limits_active { let limits_forcedir1 = -self.limits_forcedir2; let limits_forcedir2 = self.limits_forcedir2; @@ -409,8 +411,9 @@ impl WPrismaticVelocityConstraint { let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1))) + self.limits_rhs; - let new_impulse = - (self.limits_impulse - lin_dvel * limits_inv_lhs).simd_max(na::zero()); + let new_impulse = (self.limits_impulse - lin_dvel * self.limits_inv_lhs) + .simd_max(self.limits_impulse_limits.0) + .simd_min(self.limits_impulse_limits.1); let dimpulse = new_impulse - self.limits_impulse; self.limits_impulse = new_impulse; @@ -488,7 +491,7 @@ pub(crate) struct WPrismaticVelocityGroundConstraint { #[cfg(feature = "dim3")] impulse: Vector5, - limits_enabled: bool, + limits_active: bool, limits_forcedir2: Vector, limits_impulse: SimdReal, limits_rhs: SimdReal, @@ -660,13 +663,13 @@ impl WPrismaticVelocityGroundConstraint { // Setup limit constraint. let zero: SimdReal = na::zero(); let limits_forcedir2 = axis2; // hopefully axis1 is colinear with axis2 + let mut limits_active = false; let mut limits_rhs = zero; let mut limits_impulse = zero; let mut limits_impulse_limits = (zero, zero); - let limits_enabled = - SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]).any(); - if limits_enabled { + let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]); + if limits_enabled.any() { let danchor = anchor2 - anchor1; let dist = danchor.dot(&axis1); @@ -680,7 +683,8 @@ impl WPrismaticVelocityGroundConstraint { limits_impulse_limits.1 = SimdReal::splat(Real::INFINITY).select(min_enabled, zero); limits_impulse_limits.0 = SimdReal::splat(-Real::INFINITY).select(max_enabled, zero); - if (min_enabled | max_enabled).any() { + limits_active = (min_enabled | max_enabled).any(); + if limits_active { limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) * velocity_solve_fraction; @@ -701,7 +705,7 @@ impl WPrismaticVelocityGroundConstraint { im2, ii2_sqrt, impulse: impulse * SimdReal::splat(params.warmstart_coeff), - limits_enabled, + limits_active, limits_forcedir2, limits_rhs, limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff), @@ -768,7 +772,7 @@ impl WPrismaticVelocityGroundConstraint { } fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel) { - if self.limits_enabled { + if self.limits_active { // FIXME: the transformation by ii2_sqrt could be avoided by // reusing some computations above. let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); @@ -777,7 +781,9 @@ impl WPrismaticVelocityGroundConstraint { .limits_forcedir2 .dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) + self.limits_rhs; - let new_impulse = (self.limits_impulse - lin_dvel / self.im2).simd_max(na::zero()); + let new_impulse = (self.limits_impulse - lin_dvel / self.im2) + .simd_max(self.limits_impulse_limits.0) + .simd_min(self.limits_impulse_limits.1); let dimpulse = new_impulse - self.limits_impulse; self.limits_impulse = new_impulse; From f517601e17089ba2af3a4909acc217076c3dcb11 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Tue, 23 Feb 2021 19:47:20 +0100 Subject: [PATCH 17/18] Final cleanup --- .../prismatic_velocity_constraint.rs | 28 ++++++------------- .../prismatic_velocity_constraint_wide.rs | 10 +++---- 2 files changed, 12 insertions(+), 26 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index 2bd5b5c..9445896 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -156,8 +156,7 @@ impl PrismaticVelocityConstraint { let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); if velocity_based_erp_inv_dt != 0.0 { - let dpos = anchor2 - anchor1; - let linear_err = basis1.tr_mul(&dpos); + let linear_err = basis1.tr_mul(&(anchor2 - anchor1)); let frame1 = rb1.position * joint.local_frame1(); let frame2 = rb2.position * joint.local_frame2(); @@ -226,12 +225,8 @@ impl PrismaticVelocityConstraint { let min_enabled = dist < min_limit; let max_enabled = max_limit < dist; - if min_enabled { - limits_impulse_limits.1 = Real::INFINITY; - } - if max_enabled { - limits_impulse_limits.0 = -Real::INFINITY; - } + limits_impulse_limits.0 = if max_enabled { -Real::INFINITY } else { 0.0 }; + limits_impulse_limits.1 = if min_enabled { Real::INFINITY } else { 0.0 }; limits_active = min_enabled || max_enabled; if limits_active { @@ -311,10 +306,8 @@ impl PrismaticVelocityConstraint { // Warmstart limits. if self.limits_active { - let limits_forcedir1 = -self.limits_forcedir2; - let limits_forcedir2 = self.limits_forcedir2; - let limit_impulse1 = limits_forcedir1 * self.limits_impulse; - let limit_impulse2 = limits_forcedir2 * self.limits_impulse; + let limit_impulse1 = -self.limits_forcedir2 * self.limits_impulse; + let limit_impulse2 = self.limits_forcedir2 * self.limits_impulse; mj_lambda1.linear += self.im1 * limit_impulse1; mj_lambda1.angular += self .ii1_sqrt @@ -592,8 +585,7 @@ impl PrismaticVelocityGroundConstraint { let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); if velocity_based_erp_inv_dt != 0.0 { - let dpos = anchor2 - anchor1; - let linear_err = basis1.tr_mul(&dpos); + let linear_err = basis1.tr_mul(&(anchor2 - anchor1)); let (frame1, frame2); if flipped { @@ -669,12 +661,8 @@ impl PrismaticVelocityGroundConstraint { let min_enabled = dist < min_limit; let max_enabled = max_limit < dist; - if min_enabled { - limits_impulse_limits.1 = Real::INFINITY; - } - if max_enabled { - limits_impulse_limits.0 = -Real::INFINITY; - } + limits_impulse_limits.0 = if max_enabled { -Real::INFINITY } else { 0.0 }; + limits_impulse_limits.1 = if min_enabled { Real::INFINITY } else { 0.0 }; limits_active = min_enabled || max_enabled; if limits_active { diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs index 5186ee7..f21acee 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -205,8 +205,7 @@ impl WPrismaticVelocityConstraint { if velocity_based_erp_inv_dt != 0.0 { let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt); - let dpos = anchor2 - anchor1; - let linear_err = basis1.tr_mul(&dpos); + let linear_err = basis1.tr_mul(&(anchor2 - anchor1)); let local_frame1 = Isometry::from(array![|ii| cparams[ii].local_frame1(); SIMD_WIDTH]); let local_frame2 = Isometry::from(array![|ii| cparams[ii].local_frame2(); SIMD_WIDTH]); @@ -251,8 +250,8 @@ impl WPrismaticVelocityConstraint { let min_enabled = dist.simd_lt(min_limit); let max_enabled = dist.simd_gt(max_limit); - limits_impulse_limits.1 = SimdReal::splat(Real::INFINITY).select(min_enabled, zero); limits_impulse_limits.0 = SimdReal::splat(-Real::INFINITY).select(max_enabled, zero); + limits_impulse_limits.1 = SimdReal::splat(Real::INFINITY).select(min_enabled, zero); limits_active = (min_enabled | max_enabled).any(); if limits_active { @@ -632,8 +631,7 @@ impl WPrismaticVelocityGroundConstraint { if velocity_based_erp_inv_dt != 0.0 { let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt); - let dpos = anchor2 - anchor1; - let linear_err = basis1.tr_mul(&dpos); + let linear_err = basis1.tr_mul(&(anchor2 - anchor1)); let frame1 = position1 * Isometry::from( @@ -680,8 +678,8 @@ impl WPrismaticVelocityGroundConstraint { let min_enabled = dist.simd_lt(min_limit); let max_enabled = dist.simd_gt(max_limit); - limits_impulse_limits.1 = SimdReal::splat(Real::INFINITY).select(min_enabled, zero); limits_impulse_limits.0 = SimdReal::splat(-Real::INFINITY).select(max_enabled, zero); + limits_impulse_limits.1 = SimdReal::splat(Real::INFINITY).select(min_enabled, zero); limits_active = (min_enabled | max_enabled).any(); if limits_active { From 115bae172d1f6bc8f26f6e499100ca6b437f3c87 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Thu, 25 Feb 2021 10:16:10 +0100 Subject: [PATCH 18/18] fix the body-body revolute angle velocity erp --- .../solver/joint_constraint/revolute_velocity_constraint.rs | 2 +- .../joint_constraint/revolute_velocity_constraint_wide.rs | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index 839cddc..23cd6b0 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -109,7 +109,7 @@ impl RevoluteVelocityConstraint { let axis2 = rb2.position * joint.local_axis2; let axis_error = axis1.cross(&axis2); - let ang_err = basis2.tr_mul(&axis_error) - basis1.tr_mul(&axis_error); + let ang_err = (basis2.tr_mul(&axis_error) + basis1.tr_mul(&axis_error)) * 0.5; rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y) * velocity_based_erp_inv_dt; diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs index ad25101..81e41dc 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs @@ -133,7 +133,8 @@ impl WRevoluteVelocityConstraint { let axis2 = position2 * local_axis2; let axis_error = axis1.cross(&axis2); - let ang_err = basis2.tr_mul(&axis_error) - basis1.tr_mul(&axis_error); + let ang_err = + (basis2.tr_mul(&axis_error) + basis1.tr_mul(&axis_error)) * SimdReal::splat(0.5); rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y) * velocity_based_erp_inv_dt;