From 21247a123691de7a5f53454c7edba838af83c594 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Mon, 15 Feb 2021 20:52:16 +0100 Subject: Add restorative impulse in velocity solver --- src/dynamics/integration_parameters.rs | 22 ++++-- .../joint_constraint/ball_velocity_constraint.rs | 29 +++++--- .../joint_constraint/fixed_velocity_constraint.rs | 78 +++++++++++++++++++--- src/dynamics/solver/velocity_constraint.rs | 15 +++-- src/dynamics/solver/velocity_ground_constraint.rs | 15 +++-- 5 files changed, 125 insertions(+), 34 deletions(-) (limited to 'src') 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, }; } -- cgit From 4162aed2a04767e6bac8332b8f8fbacb42f1f1a3 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Wed, 17 Feb 2021 19:08:43 +0100 Subject: Add params.velocity_based_erp_inv_dt() helper --- src/dynamics/integration_parameters.rs | 6 ++++ .../joint_constraint/ball_velocity_constraint.rs | 9 +++--- .../joint_constraint/fixed_velocity_constraint.rs | 35 ++++++++-------------- src/dynamics/solver/velocity_constraint.rs | 7 ++--- src/dynamics/solver/velocity_ground_constraint.rs | 7 ++--- 5 files changed, 29 insertions(+), 35 deletions(-) (limited to 'src') 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, -- cgit From 3f26b46196531cfca10ae487bb242e8bb3f84f65 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Wed, 17 Feb 2021 19:15:52 +0100 Subject: Implement ball wide --- .../joint_constraint/ball_velocity_constraint_wide.rs | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) (limited to 'src') 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(); -- cgit From ede4f0f7703dee4fc6a53e6da7775f7455a0b94e Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Wed, 17 Feb 2021 19:38:57 +0100 Subject: cleanup --- .../joint_constraint/fixed_velocity_constraint.rs | 18 ++++++++---------- src/dynamics/solver/velocity_constraint.rs | 2 +- src/dynamics/solver/velocity_ground_constraint.rs | 2 +- 3 files changed, 10 insertions(+), 12 deletions(-) (limited to 'src') 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 { -- cgit From d3f39a9bab028493ee9cd1a3b105ed6616727e03 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Wed, 17 Feb 2021 19:40:02 +0100 Subject: Implemented prismatic narrow. Needs testing and close review --- .../prismatic_velocity_constraint.rs | 91 ++++++++++++++++++++-- 1 file changed, 83 insertions(+), 8 deletions(-) (limited to 'src') 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. -- cgit From 48708d9a76b4868f77599c0fc6f303fd194dbb88 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Wed, 17 Feb 2021 20:22:41 +0100 Subject: Implement revolute narrow --- .../prismatic_velocity_constraint.rs | 28 +++++----- .../revolute_velocity_constraint.rs | 64 +++++++++++++++++++--- 2 files changed, 70 insertions(+), 22 deletions(-) (limited to 'src') 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. -- cgit From 27366e27ff1274b4c8d8542eaa0b24f449165555 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Thu, 18 Feb 2021 10:40:42 +0100 Subject: Implement fixed wide --- .../joint_constraint/fixed_velocity_constraint.rs | 9 --- .../fixed_velocity_constraint_wide.rs | 65 +++++++++++++++++++--- 2 files changed, 58 insertions(+), 16 deletions(-) (limited to 'src') 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, -- cgit From 89de6903dcdc1f0dfa4314cf99dd19fa179eb756 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Thu, 18 Feb 2021 13:43:33 +0100 Subject: Implement prismatic wide --- .../prismatic_velocity_constraint.rs | 16 +-- .../prismatic_velocity_constraint_wide.rs | 130 +++++++++++++++++++-- 2 files changed, 124 insertions(+), 22 deletions(-) (limited to 'src') 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); -- cgit From 48afbac6cefbcf1ad6ab663ce15c503bb549cb69 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Thu, 18 Feb 2021 18:18:47 +0100 Subject: 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(-) (limited to 'src') 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, -- cgit From b94cdfa782d322d54cc174cd820f85766954d11b Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Fri, 19 Feb 2021 10:24:00 +0100 Subject: Implement ground wide --- src/dynamics/solver/velocity_constraint_wide.rs | 12 ++++++++++-- src/dynamics/solver/velocity_ground_constraint_wide.rs | 12 ++++++++++-- 2 files changed, 20 insertions(+), 4 deletions(-) (limited to 'src') 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