diff options
| -rw-r--r-- | src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs | 52 | ||||
| -rw-r--r-- | src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs | 178 |
2 files changed, 116 insertions, 114 deletions
diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index 33b8b40..d5a0f3d 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -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)); } } 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 ec2d5b7..a9d95e5 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -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(); - - if limits_enabled { + // 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); + + 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)); - - 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); + + let min_enabled = dist.simd_lt(min_limit); + let max_enabled = dist.simd_gt(max_limit); + + 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); } } |