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

Implement prismatic wide

Browse Source
This commit is contained in:
Emil Ernerfeldt
2021-02-18 13:43:33 +01:00
parent 27366e27ff
commit 89de6903dc
2 changed files with 124 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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")]
{