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

Merge pull request #116 from EmbarkStudios/corrective-velocity-solve

Browse Source 
Optional violation correction in velocity solver
This commit is contained in:
Sébastien Crozet
2021-03-02 15:54:16 +01:00
committed by GitHub
parent 449ed996d8 115bae172d
commit a74097b4c6
13 changed files with 640 additions and 179 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
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()
}
}
@@ -173,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 {
@@ -183,6 +197,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,

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

@@ -40,17 +40,20 @@ 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 rhs = (vel2 - vel1) * params.velocity_solve_fraction
+ (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
let lhs;
let cmat1 = anchor1.gcross_matrix();
let cmat2 = anchor2.gcross_matrix();
@@ -271,22 +274,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 rhs = (vel2 - vel1) * params.velocity_solve_fraction
+ (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
let cmat2 = anchor2.gcross_matrix();

18
src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
View File

@@ -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<SimdReal> = linvel1 + angvel1.gcross(anchor1);
let vel2: Vector<SimdReal> = 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<SimdReal> = linvel1 + angvel1.gcross(anchor1);
let vel2: Vector<SimdReal> = 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();

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

@@ -103,12 +103,33 @@ 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 =
Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * params.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,
);
) * 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.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 = 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;
}
}
FixedVelocityConstraint {
joint_id,
@@ -293,11 +314,32 @@ 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 =
Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * params.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,
);
) * 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.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 = 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;
}
}
FixedVelocityGroundConstraint {
joint_id,

65
src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
View File

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

205
src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
View File

@@ -48,10 +48,15 @@ pub(crate) struct PrismaticVelocityConstraint {
motor_inv_lhs: Real,
motor_max_impulse: Real,
limits_active: bool,
limits_impulse: Real,
limits_forcedirs: Option<(Vector<Real>, Vector<Real>)>,
/// World-coordinate direction of the limit force on rb2.
/// The force direction on rb1 is opposite (Newton's third law)..
limits_forcedir2: Vector<Real>,
limits_rhs: Real,
limits_inv_lhs: Real,
/// min/max applied impulse due to limits
limits_impulse_limits: (Real, Real),
#[cfg(feature = "dim2")]
basis1: Vector2<Real>,
@@ -135,13 +140,39 @@ 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 linear_err = basis1.tr_mul(&(anchor2 - anchor1));
let frame1 = rb1.position * joint.local_frame1();
let frame2 = rb2.position * joint.local_frame2();
let ang_err = frame2.rotation * frame1.rotation.inverse();
#[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.
@@ -176,31 +207,35 @@ impl PrismaticVelocityConstraint {
joint.motor_max_impulse,
);
/*
* Setup limit constraint.
*/
let mut limits_forcedirs = None;
// 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 = 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, 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 min_enabled = dist < min_limit;
let max_enabled = max_limit < dist;
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 {
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;
if dist < joint.limits[0] || dist > joint.limits[1] {
let gcross1 = r1.gcross(*axis1);
let gcross2 = r2.gcross(*axis2);
limits_inv_lhs = crate::utils::inv(
@@ -208,6 +243,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);
}
}
@@ -220,10 +260,12 @@ 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_forcedirs,
limits_forcedir2,
limits_rhs,
limits_inv_lhs,
limits_impulse_limits,
motor_rhs,
motor_inv_lhs,
motor_impulse,
@@ -263,10 +305,9 @@ 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 {
let limit_impulse1 = limits_forcedir1 * self.limits_impulse;
let limit_impulse2 = limits_forcedir2 * self.limits_impulse;
if self.limits_active {
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
@@ -313,14 +354,19 @@ impl PrismaticVelocityConstraint {
}
fn solve_limits(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
if self.limits_active {
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 * self.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;
@@ -396,8 +442,12 @@ pub(crate) struct PrismaticVelocityGroundConstraint {
#[cfg(feature = "dim3")]
impulse: Vector5<Real>,
limits_active: bool,
limits_forcedir2: Vector<Real>,
limits_impulse: Real,
limits_rhs: Real,
/// min/max applied impulse due to limits
limits_impulse_limits: (Real, Real),
axis2: Vector<Real>,
motor_impulse: Real,
@@ -409,7 +459,6 @@ pub(crate) struct PrismaticVelocityGroundConstraint {
basis1: Vector2<Real>,
#[cfg(feature = "dim3")]
basis1: Matrix3x2<Real>,
limits_forcedir2: Option<Vector<Real>>,
im2: Real,
ii2_sqrt: AngularInertia<Real>,
@@ -520,13 +569,45 @@ 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 linear_err = basis1.tr_mul(&(anchor2 - anchor1));
let (frame1, frame2);
if flipped {
frame1 = rb1.position * joint.local_frame2();
frame2 = rb2.position * joint.local_frame1();
} else {
frame1 = rb1.position * joint.local_frame1();
frame2 = rb2.position * joint.local_frame2();
}
let ang_err = frame2.rotation * frame1.rotation.inverse();
#[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.
@@ -564,25 +645,37 @@ impl PrismaticVelocityGroundConstraint {
/*
* Setup limit constraint.
*/
let mut limits_forcedir2 = None;
let mut limits_active = false;
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 min_enabled = dist < min_limit;
let max_enabled = max_limit < dist;
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 {
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);
}
}
@@ -592,7 +685,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,
@@ -602,8 +699,6 @@ impl PrismaticVelocityGroundConstraint {
rhs,
r2,
axis2: axis2.into_inner(),
limits_forcedir2,
limits_rhs,
}
}
@@ -625,9 +720,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;
}
@@ -657,16 +750,20 @@ impl PrismaticVelocityGroundConstraint {
}
fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
if let Some(limits_forcedir2) = self.limits_forcedir2 {
if self.limits_active {
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);
}
}

233
src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
View File

@@ -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;
@@ -45,10 +48,12 @@ pub(crate) struct WPrismaticVelocityConstraint {
#[cfg(feature = "dim2")]
impulse: Vector2<SimdReal>,
limits_active: bool,
limits_impulse: SimdReal,
limits_forcedirs: Option<(Vector<SimdReal>, Vector<SimdReal>)>,
limits_forcedir2: Vector<SimdReal>,
limits_rhs: SimdReal,
limits_inv_lhs: SimdReal,
limits_impulse_limits: (SimdReal, SimdReal),
#[cfg(feature = "dim2")]
basis1: Vector2<SimdReal>,
@@ -180,46 +185,91 @@ 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 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 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]);
let frame1 = position1 * local_frame1;
let frame2 = position2 * local_frame2;
let ang_err = frame2.rotation * frame1.rotation.inverse();
#[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 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 = zero;
let mut limits_impulse_limits = (zero, zero);
/*
* 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();
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.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 {
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_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 = SimdReal::splat(1.0)
/ (im1
+ im2
@@ -236,11 +286,13 @@ 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_forcedirs,
limits_forcedir2,
limits_rhs,
limits_inv_lhs,
limits_impulse_limits,
basis1,
inv_lhs,
rhs,
@@ -284,9 +336,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_active {
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
@@ -348,15 +400,19 @@ impl WPrismaticVelocityConstraint {
mj_lambda1: &mut DeltaVel<SimdReal>,
mj_lambda2: &mut DeltaVel<SimdReal>,
) {
if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
if self.limits_active {
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).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;
@@ -434,15 +490,17 @@ pub(crate) struct WPrismaticVelocityGroundConstraint {
#[cfg(feature = "dim3")]
impulse: Vector5<SimdReal>,
limits_active: bool,
limits_forcedir2: Vector<SimdReal>,
limits_impulse: SimdReal,
limits_rhs: SimdReal,
limits_impulse_limits: (SimdReal, SimdReal),
axis2: Vector<SimdReal>,
#[cfg(feature = "dim2")]
basis1: Vector2<SimdReal>,
#[cfg(feature = "dim3")]
basis1: Matrix3x2<SimdReal>,
limits_forcedir2: Option<Vector<SimdReal>>,
im2: SimdReal,
ii2_sqrt: AngularInertia<SimdReal>,
@@ -553,40 +611,89 @@ 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 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 linear_err = basis1.tr_mul(&(anchor2 - anchor1));
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();
#[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]);
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]);
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 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.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 {
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);
}
}
@@ -596,14 +703,16 @@ impl WPrismaticVelocityGroundConstraint {
im2,
ii2_sqrt,
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
limits_active,
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,
}
}
@@ -628,9 +737,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);
@@ -663,18 +770,22 @@ impl WPrismaticVelocityGroundConstraint {
}
fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<SimdReal>) {
if let Some(limits_forcedir2) = self.limits_forcedir2 {
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);
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 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;
mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
mj_lambda2.linear += self.limits_forcedir2 * (self.im2 * dimpulse);
}
}

64
src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
View File

@@ -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 axis_error = axis1.cross(&axis2);
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;
}
/*
* Motor.
@@ -371,9 +392,35 @@ 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 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;
}
/*
* Motor part.
@@ -472,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<Real>) {
if self.motor_inv_lhs != 0.0 {
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

72
src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs
View File

@@ -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, Unit, Vector5, U2, U3};
#[derive(Debug)]
pub(crate) struct WRevoluteVelocityConstraint {
@@ -107,9 +107,38 @@ 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 local_axis1 =
Unit::<Vector<_>>::from(array![|ii| joints[ii].local_axis1; SIMD_WIDTH]);
let local_axis2 =
Unit::<Vector<_>>::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)) * 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;
}
/*
* Adjust the warmstart impulse.
@@ -357,9 +386,38 @@ 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 local_axis1 = Unit::<Vector<_>>::from(
array![|ii| if flipped[ii] { joints[ii].local_axis2 } else { joints[ii].local_axis1 }; SIMD_WIDTH],
);
let local_axis2 = Unit::<Vector<_>>::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;
}
WRevoluteVelocityGroundConstraint {
joint_id,

14
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;
@@ -244,17 +246,19 @@ 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 *= 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 {
gcross1,
gcross2,
rhs,
impulse,
impulse: manifold_point.data.impulse * warmstart_coeff,
r,
};
}

12
src/dynamics/solver/velocity_constraint_wide.rs
View File

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

14
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;
@@ -156,16 +158,18 @@ 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 *= 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 {
gcross2,
rhs,
impulse,
impulse: manifold_point.data.impulse * warmstart_coeff,
r,
};
}

12
src/dynamics/solver/velocity_ground_constraint_wide.rs
View File

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