diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs | 28 | ||||
| -rw-r--r-- | src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs | 64 |
2 files changed, 70 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 9f19ace..440b8ac 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -155,14 +155,14 @@ impl PrismaticVelocityConstraint { let limit_err = dpos.dot(&axis1); let mut linear_err = dpos - *axis1 * limit_err; - let frame1 = rb1.position * cparams.local_frame1(); - let frame2 = rb2.position * cparams.local_frame2(); + let frame1 = rb1.position * joint.local_frame1(); + let frame2 = rb2.position * joint.local_frame2(); let ang_err = frame2.rotation * frame1.rotation.inverse(); - if limit_err < cparams.limits[0] { - linear_err += *axis1 * (limit_err - cparams.limits[0]); - } else if limit_err > cparams.limits[1] { - linear_err += *axis1 * (limit_err - cparams.limits[1]); + 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]); } #[cfg(feature = "dim2")] @@ -572,11 +572,11 @@ impl PrismaticVelocityGroundConstraint { if velocity_based_erp_inv_dt != 0.0 { let (frame1, frame2); if flipped { - frame1 = rb1.position * cparams.local_frame2(); - frame2 = rb2.position * cparams.local_frame1(); + frame1 = rb1.position * joint.local_frame2(); + frame2 = rb2.position * joint.local_frame1(); } else { - frame1 = rb1.position * cparams.local_frame1(); - frame2 = rb2.position * cparams.local_frame2(); + frame1 = rb1.position * joint.local_frame1(); + frame2 = rb2.position * joint.local_frame2(); } let dpos = anchor2 - anchor1; @@ -585,10 +585,10 @@ impl PrismaticVelocityGroundConstraint { let ang_err = frame2.rotation * frame1.rotation.inverse(); - if limit_err < cparams.limits[0] { - linear_err += *axis1 * (limit_err - cparams.limits[0]); - } else if limit_err > cparams.limits[1] { - linear_err += *axis1 * (limit_err - cparams.limits[1]); + 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]); } #[cfg(feature = "dim2")] diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index 61cb720..46f375b 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -3,9 +3,8 @@ use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody, }; use crate::math::{AngularInertia, Real, Rotation, Vector}; -use crate::na::UnitQuaternion; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; -use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; +use na::{Cholesky, Matrix3x2, Matrix5, UnitQuaternion, Vector5, U2, U3}; #[derive(Debug)] pub(crate) struct RevoluteVelocityConstraint { @@ -90,9 +89,31 @@ impl RevoluteVelocityConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); - let lin_rhs = (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1)); - let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel); - let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); + let linvel_err = + (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1)); + let angvel_err = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel); + + let mut rhs = Vector5::new( + linvel_err.x, + linvel_err.y, + linvel_err.z, + angvel_err.x, + angvel_err.y, + ) * params.velocity_solve_fraction; + + let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); + if velocity_based_erp_inv_dt != 0.0 { + let lin_err = anchor2 - anchor1; + + let axis1 = rb1.position * joint.local_axis1; + let axis2 = rb2.position * joint.local_axis2; + let ang_err = + Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); + let ang_err = ang_err.scaled_axis(); + + rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y) + * velocity_based_erp_inv_dt; + } /* * Motor. @@ -371,9 +392,36 @@ impl RevoluteVelocityGroundConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); - let lin_rhs = (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1)); - let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel); - let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); + let linvel_err = + (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1)); + let angvel_err = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel); + let mut rhs = Vector5::new( + linvel_err.x, + linvel_err.y, + linvel_err.z, + angvel_err.x, + angvel_err.y, + ) * params.velocity_solve_fraction; + + let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); + if velocity_based_erp_inv_dt != 0.0 { + let lin_err = anchor2 - anchor1; + + let (axis1, axis2); + if flipped { + axis1 = rb1.position * joint.local_axis2; + axis2 = rb2.position * joint.local_axis1; + } else { + axis1 = rb1.position * joint.local_axis1; + axis2 = rb2.position * joint.local_axis2; + } + let ang_err = + Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); + let ang_err = ang_err.scaled_axis(); + + rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y) + * velocity_based_erp_inv_dt; + } /* * Motor part. |