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

Fix primatic wide

Browse Source
This commit is contained in:
Emil Ernerfeldt
2021-02-23 18:28:47 +01:00
parent a0824772c9
commit 4ef7b1cefe
2 changed files with 113 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -223,27 +223,22 @@ impl PrismaticVelocityConstraint {
// TODO: we should allow predictive constraint activation.
let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]);
let below_min = dist < min_limit;
let above_max = max_limit < dist;
let min_enabled = dist < min_limit;
let max_enabled = max_limit < dist;
if below_min {
if min_enabled {
limits_impulse_limits.1 = Real::INFINITY;
}
if above_max {
if max_enabled {
limits_impulse_limits.0 = -Real::INFINITY;
}
if below_min || above_max {
limits_impulse = joint
.limits_impulse
.max(limits_impulse_limits.0)
.min(limits_impulse_limits.1);
if min_enabled || max_enabled {
limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1))
* params.velocity_solve_fraction;
limits_rhs += velocity_based_erp_inv_dt
* ((dist - max_limit).max(0.0) - (min_limit - dist).max(0.0));
limits_rhs += ((dist - max_limit).max(0.0) - (min_limit - dist).max(0.0))
* velocity_based_erp_inv_dt;
let gcross1 = r1.gcross(*axis1);
let gcross2 = r2.gcross(*axis2);
@@ -252,6 +247,11 @@ impl PrismaticVelocityConstraint {
+ gcross1.gdot(ii1.transform_vector(gcross1))
+ gcross2.gdot(ii2.transform_vector(gcross2)),
));
limits_impulse = joint
.limits_impulse
.max(limits_impulse_limits.0)
.min(limits_impulse_limits.1);
}
}
@@ -589,6 +589,9 @@ impl PrismaticVelocityGroundConstraint {
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
if velocity_based_erp_inv_dt != 0.0 {
let dpos = anchor2 - anchor1;
let linear_err = basis1.tr_mul(&dpos);
let (frame1, frame2);
if flipped {
frame1 = rb1.position * joint.local_frame2();
@@ -598,9 +601,6 @@ impl PrismaticVelocityGroundConstraint {
frame2 = rb2.position * joint.local_frame2();
}
let dpos = anchor2 - anchor1;
let linear_err = basis1.tr_mul(&dpos);
let ang_err = frame2.rotation * frame1.rotation.inverse();
#[cfg(feature = "dim2")]
{
@@ -662,27 +662,27 @@ impl PrismaticVelocityGroundConstraint {
// TODO: we should allow predictive constraint activation.
let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]);
let below_min = dist < min_limit;
let above_max = max_limit < dist;
let min_enabled = dist < min_limit;
let max_enabled = max_limit < dist;
if below_min {
if min_enabled {
limits_impulse_limits.1 = Real::INFINITY;
}
if above_max {
if max_enabled {
limits_impulse_limits.0 = -Real::INFINITY;
}
if below_min || above_max {
if min_enabled || max_enabled {
limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1))
* params.velocity_solve_fraction;
limits_rhs += ((dist - max_limit).max(0.0) - (min_limit - dist).max(0.0))
* velocity_based_erp_inv_dt;
limits_impulse = joint
.limits_impulse
.max(limits_impulse_limits.0)
.min(limits_impulse_limits.1);
limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1))
* params.velocity_solve_fraction;
limits_rhs += velocity_based_erp_inv_dt
* ((dist - max_limit).max(0.0) - (min_limit - dist).max(0.0));
}
}

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

@@ -49,9 +49,10 @@ pub(crate) struct WPrismaticVelocityConstraint {
impulse: Vector2<SimdReal>,
limits_impulse: SimdReal,
limits_forcedirs: Option<(Vector<SimdReal>, Vector<SimdReal>)>,
limits_forcedir2: Vector<SimdReal>,
limits_rhs: SimdReal,
limits_inv_lhs: SimdReal,
limits_inv_lhs: Option<SimdReal>,
limits_impulse_limits: (SimdReal, SimdReal),
#[cfg(feature = "dim2")]
basis1: Vector2<SimdReal>,
@@ -199,16 +200,12 @@ impl WPrismaticVelocityConstraint {
angvel_err.z,
) * velocity_solve_fraction;
let limits_enabled =
SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]).any();
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
if velocity_based_erp_inv_dt != 0.0 {
let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
let dpos = anchor2 - anchor1;
let limit_err = dpos.dot(&axis1);
let mut linear_err = dpos - axis1 * limit_err;
let linear_err = basis1.tr_mul(&dpos);
let local_frame1 = Isometry::from(array![|ii| cparams[ii].local_frame1(); SIMD_WIDTH]);
let local_frame2 = Isometry::from(array![|ii| cparams[ii].local_frame2(); SIMD_WIDTH]);
@@ -217,16 +214,6 @@ impl WPrismaticVelocityConstraint {
let frame2 = position2 * local_frame2;
let ang_err = frame2.rotation * frame1.rotation.inverse();
if limits_enabled {
let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
let zero: SimdReal = na::zero();
linear_err += axis1
* ((limit_err - max_limit).simd_max(zero)
- (min_limit - limit_err).simd_max(zero));
}
#[cfg(feature = "dim2")]
{
rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt;
@@ -241,42 +228,53 @@ impl WPrismaticVelocityConstraint {
}
}
/*
* Setup limit constraint.
*/
let mut limits_forcedirs = None;
let mut limits_rhs = na::zero();
let mut limits_impulse = na::zero();
let mut limits_inv_lhs = na::zero();
// Setup limit constraint.
let zero: SimdReal = na::zero();
let limits_forcedir2 = axis2; // hopefully axis1 is colinear with axis2
let mut limits_rhs = zero;
let mut limits_impulse = zero;
let mut limits_inv_lhs = None;
let mut limits_impulse_limits = (zero, zero);
if limits_enabled {
let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
if limits_enabled.any() {
let danchor = anchor2 - anchor1;
let dist = danchor.dot(&axis1);
// FIXME: we should allow both limits to be active at
// the same time + allow predictive constraint activation.
// TODO: we should allow predictive constraint activation.
let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
let lim_impulse = SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]);
let min_enabled = dist.simd_lt(min_limit);
let max_enabled = dist.simd_gt(max_limit);
let _0: SimdReal = na::zero();
let _1: SimdReal = na::one();
let sign = _1.select(min_enabled, (-_1).select(max_enabled, _0));
if sign != _0 {
limits_impulse_limits.1 = SimdReal::splat(Real::INFINITY).select(min_enabled, zero);
limits_impulse_limits.0 = SimdReal::splat(-Real::INFINITY).select(max_enabled, zero);
if (min_enabled | max_enabled).any() {
let gcross1 = r1.gcross(axis1);
let gcross2 = r2.gcross(axis2);
limits_forcedirs = Some((axis1 * -sign, axis2 * sign));
limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) * sign;
limits_impulse = lim_impulse.select(min_enabled | max_enabled, _0);
limits_inv_lhs = SimdReal::splat(1.0)
/ (im1
+ im2
+ gcross1.gdot(ii1.transform_vector(gcross1))
+ gcross2.gdot(ii2.transform_vector(gcross2)));
limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1))
* velocity_solve_fraction;
limits_rhs += ((dist - max_limit).simd_max(zero)
- (min_limit - dist).simd_max(zero))
* SimdReal::splat(velocity_based_erp_inv_dt);
limits_impulse =
SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH])
.simd_max(limits_impulse_limits.0)
.simd_min(limits_impulse_limits.1);
limits_inv_lhs = Some(
SimdReal::splat(1.0)
/ (im1
+ im2
+ gcross1.gdot(ii1.transform_vector(gcross1))
+ gcross2.gdot(ii2.transform_vector(gcross2))),
);
}
}
@@ -290,9 +288,10 @@ impl WPrismaticVelocityConstraint {
ii2_sqrt,
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff),
limits_forcedirs,
limits_forcedir2,
limits_rhs,
limits_inv_lhs,
limits_impulse_limits,
basis1,
inv_lhs,
rhs,
@@ -336,9 +335,9 @@ impl WPrismaticVelocityConstraint {
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
// Warmstart limits.
if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
let limit_impulse1 = limits_forcedir1 * self.limits_impulse;
let limit_impulse2 = limits_forcedir2 * self.limits_impulse;
if self.limits_inv_lhs.is_some() {
let limit_impulse1 = -self.limits_forcedir2 * self.limits_impulse;
let limit_impulse2 = self.limits_forcedir2 * self.limits_impulse;
mj_lambda1.linear += limit_impulse1 * self.im1;
mj_lambda1.angular += self
@@ -400,7 +399,10 @@ impl WPrismaticVelocityConstraint {
mj_lambda1: &mut DeltaVel<SimdReal>,
mj_lambda2: &mut DeltaVel<SimdReal>,
) {
if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
if let Some(limits_inv_lhs) = self.limits_inv_lhs {
let limits_forcedir1 = -self.limits_forcedir2;
let limits_forcedir2 = self.limits_forcedir2;
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
@@ -408,7 +410,7 @@ impl WPrismaticVelocityConstraint {
+ limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1)))
+ self.limits_rhs;
let new_impulse =
(self.limits_impulse - lin_dvel * self.limits_inv_lhs).simd_max(na::zero());
(self.limits_impulse - lin_dvel * limits_inv_lhs).simd_max(na::zero());
let dimpulse = new_impulse - self.limits_impulse;
self.limits_impulse = new_impulse;
@@ -486,15 +488,17 @@ pub(crate) struct WPrismaticVelocityGroundConstraint {
#[cfg(feature = "dim3")]
impulse: Vector5<SimdReal>,
limits_enabled: 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>,
@@ -621,16 +625,12 @@ impl WPrismaticVelocityGroundConstraint {
angvel_err.z,
) * velocity_solve_fraction;
let limits_enabled =
SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]).any();
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
if velocity_based_erp_inv_dt != 0.0 {
let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
let dpos = anchor2 - anchor1;
let limit_err = dpos.dot(&axis1);
let mut linear_err = dpos - axis1 * limit_err;
let linear_err = basis1.tr_mul(&dpos);
let frame1 = position1
* Isometry::from(
@@ -643,16 +643,6 @@ impl WPrismaticVelocityGroundConstraint {
let ang_err = frame2.rotation * frame1.rotation.inverse();
if limits_enabled {
let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
let zero: SimdReal = na::zero();
linear_err += axis1
* ((limit_err - max_limit).simd_max(zero)
- (min_limit - limit_err).simd_max(zero));
}
#[cfg(feature = "dim2")]
{
rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt;
@@ -668,30 +658,40 @@ impl WPrismaticVelocityGroundConstraint {
}
// Setup limit constraint.
let mut limits_forcedir2 = None;
let mut limits_rhs = na::zero();
let mut limits_impulse = na::zero();
let zero: SimdReal = na::zero();
let limits_forcedir2 = axis2; // hopefully axis1 is colinear with axis2
let mut limits_rhs = zero;
let mut limits_impulse = zero;
let mut limits_impulse_limits = (zero, zero);
let limits_enabled =
SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]).any();
if limits_enabled {
let danchor = anchor2 - anchor1;
let dist = danchor.dot(&axis1);
// FIXME: we should allow both limits to be active at
// the same time + allow predictive constraint activation.
// TODO: we should allow predictive constraint activation.
let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
let lim_impulse = SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]);
let use_min = dist.simd_lt(min_limit);
let use_max = dist.simd_gt(max_limit);
let _0: SimdReal = na::zero();
let _1: SimdReal = na::one();
let sign = _1.select(use_min, (-_1).select(use_max, _0));
let min_enabled = dist.simd_lt(min_limit);
let max_enabled = dist.simd_gt(max_limit);
if sign != _0 {
limits_forcedir2 = Some(axis2 * sign);
limits_rhs = anchor_linvel2.dot(&axis2) * sign - anchor_linvel1.dot(&axis1) * sign;
limits_impulse = lim_impulse.select(use_min | use_max, _0);
limits_impulse_limits.1 = SimdReal::splat(Real::INFINITY).select(min_enabled, zero);
limits_impulse_limits.0 = SimdReal::splat(-Real::INFINITY).select(max_enabled, zero);
if (min_enabled | max_enabled).any() {
limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1))
* velocity_solve_fraction;
limits_rhs += ((dist - max_limit).simd_max(zero)
- (min_limit - dist).simd_max(zero))
* SimdReal::splat(velocity_based_erp_inv_dt);
limits_impulse =
SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH])
.simd_max(limits_impulse_limits.0)
.simd_min(limits_impulse_limits.1);
}
}
@@ -701,14 +701,16 @@ impl WPrismaticVelocityGroundConstraint {
im2,
ii2_sqrt,
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
limits_enabled,
limits_forcedir2,
limits_rhs,
limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff),
limits_impulse_limits,
basis1,
inv_lhs,
rhs,
r2,
axis2,
limits_forcedir2,
limits_rhs,
}
}
@@ -733,9 +735,7 @@ impl WPrismaticVelocityGroundConstraint {
.ii2_sqrt
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
if let Some(limits_forcedir2) = self.limits_forcedir2 {
mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse);
}
mj_lambda2.linear += self.limits_forcedir2 * (self.im2 * self.limits_impulse);
for ii in 0..SIMD_WIDTH {
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
@@ -768,18 +768,20 @@ impl WPrismaticVelocityGroundConstraint {
}
fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<SimdReal>) {
if let Some(limits_forcedir2) = self.limits_forcedir2 {
if self.limits_enabled {
// FIXME: the transformation by ii2_sqrt could be avoided by
// reusing some computations above.
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
let lin_dvel = self
.limits_forcedir2
.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
+ self.limits_rhs;
let new_impulse = (self.limits_impulse - lin_dvel / self.im2).simd_max(na::zero());
let dimpulse = new_impulse - self.limits_impulse;
self.limits_impulse = new_impulse;
mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
mj_lambda2.linear += self.limits_forcedir2 * (self.im2 * dimpulse);
}
}