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

Final cleanup

Browse Source
This commit is contained in:
Emil Ernerfeldt
2021-02-23 19:47:20 +01:00
parent 54eae9bb7b
commit f517601e17
2 changed files with 12 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -156,8 +156,7 @@ impl PrismaticVelocityConstraint {
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 linear_err = basis1.tr_mul(&(anchor2 - anchor1));
let frame1 = rb1.position * joint.local_frame1();
let frame2 = rb2.position * joint.local_frame2();
@@ -226,12 +225,8 @@ impl PrismaticVelocityConstraint {
let min_enabled = dist < min_limit;
let max_enabled = max_limit < dist;
if min_enabled {
limits_impulse_limits.1 = Real::INFINITY;
}
if max_enabled {
limits_impulse_limits.0 = -Real::INFINITY;
}
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 {
@@ -311,10 +306,8 @@ impl PrismaticVelocityConstraint {
// Warmstart limits.
if self.limits_active {
let limits_forcedir1 = -self.limits_forcedir2;
let limits_forcedir2 = self.limits_forcedir2;
let limit_impulse1 = limits_forcedir1 * self.limits_impulse;
let limit_impulse2 = limits_forcedir2 * self.limits_impulse;
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
@@ -592,8 +585,7 @@ 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 linear_err = basis1.tr_mul(&(anchor2 - anchor1));
let (frame1, frame2);
if flipped {
@@ -669,12 +661,8 @@ impl PrismaticVelocityGroundConstraint {
let min_enabled = dist < min_limit;
let max_enabled = max_limit < dist;
if min_enabled {
limits_impulse_limits.1 = Real::INFINITY;
}
if max_enabled {
limits_impulse_limits.0 = -Real::INFINITY;
}
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 {

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

@@ -205,8 +205,7 @@ impl WPrismaticVelocityConstraint {
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 linear_err = basis1.tr_mul(&dpos);
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]);
@@ -251,8 +250,8 @@ impl WPrismaticVelocityConstraint {
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);
limits_impulse_limits.1 = SimdReal::splat(Real::INFINITY).select(min_enabled, zero);
limits_active = (min_enabled | max_enabled).any();
if limits_active {
@@ -632,8 +631,7 @@ impl WPrismaticVelocityGroundConstraint {
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 linear_err = basis1.tr_mul(&dpos);
let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
let frame1 = position1
* Isometry::from(
@@ -680,8 +678,8 @@ impl WPrismaticVelocityGroundConstraint {
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);
limits_impulse_limits.1 = SimdReal::splat(Real::INFINITY).select(min_enabled, zero);
limits_active = (min_enabled | max_enabled).any();
if limits_active {