nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add params.velocity_based_erp_inv_dt() helper

Browse Source
This commit is contained in:
Emil Ernerfeldt
2021-02-17 19:08:43 +01:00
parent 21247a1236
commit 4162aed2a0
5 changed files with 29 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/dynamics/integration_parameters.rs
View File

@@ -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 {

9
src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
View File

@@ -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();

35
src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
View File

@@ -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;
}
}

7
src/dynamics/solver/velocity_constraint.rs
View File

@@ -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,

7
src/dynamics/solver/velocity_ground_constraint.rs
View File

@@ -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,