aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs16
-rw-r--r--src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs130
2 files changed, 124 insertions, 22 deletions
diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
index 440b8ac..c4b905b 100644
--- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
@@ -159,11 +159,9 @@ impl PrismaticVelocityConstraint {
let frame2 = rb2.position * joint.local_frame2();
let ang_err = frame2.rotation * frame1.rotation.inverse();
- if limit_err < joint.limits[0] {
- linear_err += *axis1 * (limit_err - joint.limits[0]);
- } else if limit_err > joint.limits[1] {
- linear_err += *axis1 * (limit_err - joint.limits[1]);
- }
+ let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]);
+ linear_err +=
+ *axis1 * ((limit_err - max_limit).max(0.0) - (min_limit - limit_err).max(0.0));
#[cfg(feature = "dim2")]
{
@@ -585,11 +583,9 @@ impl PrismaticVelocityGroundConstraint {
let ang_err = frame2.rotation * frame1.rotation.inverse();
- if limit_err < joint.limits[0] {
- linear_err += *axis1 * (limit_err - joint.limits[0]);
- } else if limit_err > joint.limits[1] {
- linear_err += *axis1 * (limit_err - joint.limits[1]);
- }
+ let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]);
+ linear_err +=
+ *axis1 * ((limit_err - max_limit).max(0.0) - (min_limit - limit_err).max(0.0));
#[cfg(feature = "dim2")]
{
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 442393d..e48148d 100644
--- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
@@ -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;
@@ -180,13 +183,63 @@ 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 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 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();
+
+ 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;
+ }
+
+ #[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.
@@ -197,7 +250,7 @@ impl WPrismaticVelocityConstraint {
let mut limits_inv_lhs = na::zero();
let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
- if limits_enabled.any() {
+ if limits_enabled {
let danchor = anchor2 - anchor1;
let dist = danchor.dot(&axis1);
@@ -553,21 +606,74 @@ 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 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 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();
+
+ 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;
+ }
+
+ #[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]);
- if limits_enabled.any() {
+ if limits_enabled {
let danchor = anchor2 - anchor1;
let dist = danchor.dot(&axis1);
generated by cgit (git 2.34.1) at 2026-02-03 09:23:26 +0000