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

Add restorative impulse in velocity solver

Browse Source
This commit is contained in:
Emil Ernerfeldt
2021-02-15 20:52:16 +01:00
parent 1c5601c84b
commit 21247a1236
5 changed files with 125 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
src/dynamics/integration_parameters.rs
View File

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

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

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

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

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

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

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

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

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