diff options
3 files changed, 441 insertions, 494 deletions
diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 2aa9a51..fa35520 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -1,4 +1,4 @@ -use crate::dynamics::RevoluteJoint; +use crate::dynamics::{BallJoint, FixedJoint, PrismaticJoint, RevoluteJoint}; use crate::math::{Isometry, Real, SpacialVector, SPATIAL_DIM}; use crate::na::{Rotation3, UnitQuaternion}; @@ -24,11 +24,16 @@ pub struct GenericJoint { pub min_position: SpacialVector<Real>, pub max_position: SpacialVector<Real>, - pub target_velocity: SpacialVector<Real>, - /// The maximum negative impulse the joint can apply on each DoF. Must be <= 0.0 - pub max_negative_impulse: SpacialVector<Real>, + pub min_velocity: SpacialVector<Real>, + pub max_velocity: SpacialVector<Real>, + /// The minimum negative impulse the joint can apply on each DoF. Must be <= 0.0 + pub min_impulse: SpacialVector<Real>, /// The maximum positive impulse the joint can apply on each DoF. Must be >= 0.0 - pub max_positive_impulse: SpacialVector<Real>, + pub max_impulse: SpacialVector<Real>, + /// The minimum negative position impulse the joint can apply on each DoF. Must be <= 0.0 + pub min_pos_impulse: SpacialVector<Real>, + /// The maximum positive position impulse the joint can apply on each DoF. Must be >= 0.0 + pub max_pos_impulse: SpacialVector<Real>, } impl GenericJoint { @@ -40,25 +45,78 @@ impl GenericJoint { impulse: SpacialVector::zeros(), min_position: SpacialVector::zeros(), max_position: SpacialVector::zeros(), - target_velocity: SpacialVector::zeros(), - max_negative_impulse: SpacialVector::repeat(-Real::MAX), - max_positive_impulse: SpacialVector::repeat(Real::MAX), + min_velocity: SpacialVector::zeros(), + max_velocity: SpacialVector::zeros(), + min_impulse: SpacialVector::repeat(-Real::MAX), + max_impulse: SpacialVector::repeat(Real::MAX), + min_pos_impulse: SpacialVector::repeat(-Real::MAX), + max_pos_impulse: SpacialVector::repeat(Real::MAX), } } + + pub fn free_dof(&mut self, dof: u8) { + self.min_position[dof as usize] = -Real::MAX; + self.max_position[dof as usize] = Real::MAX; + self.min_velocity[dof as usize] = -Real::MAX; + self.max_velocity[dof as usize] = Real::MAX; + self.min_impulse[dof as usize] = 0.0; + self.max_impulse[dof as usize] = 0.0; + self.min_pos_impulse[dof as usize] = 0.0; + self.max_pos_impulse[dof as usize] = 0.0; + } + + pub fn set_dof_limits(&mut self, dof: u8, min: Real, max: Real) { + self.min_position[dof as usize] = min; + self.max_position[dof as usize] = max; + } } impl From<RevoluteJoint> for GenericJoint { fn from(joint: RevoluteJoint) -> Self { - let basis1 = [joint.local_axis1, joint.basis1[0], joint.basis1[1]]; - let basis2 = [joint.local_axis2, joint.basis2[0], joint.basis2[1]]; - let quat1 = UnitQuaternion::from_basis_unchecked(&basis1[..]); - let quat2 = UnitQuaternion::from_basis_unchecked(&basis2[..]); + let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]]; + let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]]; + let quat1 = UnitQuaternion::from_basis_unchecked(&basis1); + let quat2 = UnitQuaternion::from_basis_unchecked(&basis2); let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1); let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2); let mut result = Self::new(local_anchor1, local_anchor2); - result.min_position[3] = -Real::MAX; - result.max_position[3] = Real::MAX; + result.free_dof(3); result } } + +impl From<BallJoint> for GenericJoint { + fn from(joint: BallJoint) -> Self { + let local_anchor1 = Isometry::new(joint.local_anchor1.coords, na::zero()); + let local_anchor2 = Isometry::new(joint.local_anchor2.coords, na::zero()); + + let mut result = Self::new(local_anchor1, local_anchor2); + result.free_dof(3); + result.free_dof(4); + result.free_dof(5); + result + } +} + +impl From<PrismaticJoint> for GenericJoint { + fn from(joint: PrismaticJoint) -> Self { + let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]]; + let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]]; + let quat1 = UnitQuaternion::from_basis_unchecked(&basis1); + let quat2 = UnitQuaternion::from_basis_unchecked(&basis2); + let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1); + let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2); + + let mut result = Self::new(local_anchor1, local_anchor2); + result.free_dof(0); + result.set_dof_limits(0, joint.limits[0], joint.limits[1]); + result + } +} + +impl From<FixedJoint> for GenericJoint { + fn from(joint: FixedJoint) -> Self { + Self::new(joint.local_anchor1, joint.local_anchor2) + } +} diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs index 1e80311..9d74bf3 100644 --- a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs @@ -48,57 +48,7 @@ impl GenericPositionConstraint { } pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) { - let mut position1 = positions[self.position1 as usize]; - let mut position2 = positions[self.position2 as usize]; - - let anchor1 = position1 * self.local_anchor1; - let anchor2 = position2 * self.local_anchor2; - let r1 = Point::from(anchor1.translation.vector) - position1 * self.local_com1; - let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2; - - let delta_pos = Isometry::from_parts( - anchor2.translation * anchor1.translation.inverse(), - anchor2.rotation * anchor1.rotation.inverse(), - ); - - let mass_matrix = GenericVelocityConstraint::compute_mass_matrix( - &self.joint, - self.im1, - self.im2, - self.ii1, - self.ii2, - r1, - r2, - false, - ); - - let lin_dpos = delta_pos.translation.vector; - let ang_dpos = delta_pos.rotation.scaled_axis(); - let dpos = Vector6::new( - lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, - ); - let err = dpos - - dpos - .sup(&self.joint.min_position) - .inf(&self.joint.max_position); - let impulse = mass_matrix * err; - let lin_impulse = impulse.xyz(); - let ang_impulse = Vector3::new(impulse[3], impulse[4], impulse[5]); - - position1.rotation = Rotation::new( - self.ii1 - .transform_vector(ang_impulse + r1.gcross(lin_impulse)), - ) * position1.rotation; - position2.rotation = Rotation::new( - self.ii2 - .transform_vector(-ang_impulse - r2.gcross(lin_impulse)), - ) * position2.rotation; - - position1.translation.vector += self.im1 * lin_impulse; - position2.translation.vector -= self.im2 * lin_impulse; - - positions[self.position1 as usize] = position1; - positions[self.position2 as usize] = position2; + return; } pub fn solve2( @@ -152,43 +102,7 @@ impl GenericPositionGroundConstraint { } pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) { - let mut position2 = positions[self.position2 as usize]; - - let anchor2 = position2 * self.local_anchor2; - let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2; - - let delta_pos = Isometry::from_parts( - anchor2.translation * self.anchor1.translation.inverse(), - anchor2.rotation * self.anchor1.rotation.inverse(), - ); - let mass_matrix = GenericVelocityGroundConstraint::compute_mass_matrix( - &self.joint, - self.im2, - self.ii2, - r2, - false, - ); - - let lin_dpos = delta_pos.translation.vector; - let ang_dpos = delta_pos.rotation.scaled_axis(); - let dpos = Vector6::new( - lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, - ); - let err = dpos - - dpos - .sup(&self.joint.min_position) - .inf(&self.joint.max_position); - let impulse = mass_matrix * err; - let lin_impulse = impulse.xyz(); - let ang_impulse = Vector3::new(impulse[3], impulse[4], impulse[5]); - - position2.rotation = Rotation::new( - self.ii2 - .transform_vector(-ang_impulse - r2.gcross(lin_impulse)), - ) * position2.rotation; - position2.translation.vector -= self.im2 * lin_impulse; - - positions[self.position2 as usize] = position2; + return; } pub fn solve2( diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs index db8010b..fe20e36 100644 --- a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs @@ -2,7 +2,8 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; -use crate::math::{AngularInertia, Dim, Isometry, Real, SpacialVector, Vector, DIM}; +use crate::math::{AngularInertia, Dim, Isometry, Real, Rotation, SpacialVector, Vector, DIM}; +use crate::na::UnitQuaternion; use crate::parry::math::{AngDim, SpatialVector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[cfg(feature = "dim2")] @@ -17,23 +18,10 @@ pub(crate) struct GenericVelocityConstraint { joint_id: JointIndex, - impulse: SpacialVector<Real>, - pos_impulse: SpacialVector<Real>, - - max_positive_impulse: SpatialVector<Real>, - max_negative_impulse: SpatialVector<Real>, - - #[cfg(feature = "dim3")] - inv_lhs: Matrix6<Real>, // FIXME: replace by Cholesky. #[cfg(feature = "dim3")] - rhs: Vector6<Real>, - + inv_lhs: Matrix6<Real>, // TODO: replace by Cholesky? #[cfg(feature = "dim2")] - inv_lhs: Matrix3<Real>, // FIXME: replace by Cholesky. - #[cfg(feature = "dim2")] - rhs: Vector3<Real>, - - pos_rhs: Vector6<Real>, + inv_lhs: Matrix3<Real>, im1: Real, im2: Real, @@ -46,79 +34,147 @@ pub(crate) struct GenericVelocityConstraint { r1: Vector<Real>, r2: Vector<Real>, + rot2: Rotation<Real>, + + vel: GenericConstraintPart, + pos: GenericConstraintPart, } impl GenericVelocityConstraint { #[inline(always)] - pub fn compute_mass_matrix( - joint: &GenericJoint, + pub fn compute_delassus_matrix( im1: Real, im2: Real, ii1: AngularInertia<Real>, ii2: AngularInertia<Real>, r1: Vector<Real>, r2: Vector<Real>, - velocity_solver: bool, + rot2: Rotation<Real>, ) -> Matrix6<Real> { - let rmat1 = r1.gcross_matrix(); - let rmat2 = r2.gcross_matrix(); - - #[allow(unused_mut)] // For 2D - let mut lhs; + let rotmat = rot2.to_rotation_matrix().into_inner(); + let rmat1 = r1.gcross_matrix() * rotmat; + let rmat2 = r2.gcross_matrix() * rotmat; #[cfg(feature = "dim3")] { - let lhs00 = + let del00 = ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2); - let lhs10 = ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2); - let lhs11 = (ii1 + ii2).into_matrix(); + let del10 = + rotmat.transpose() * (ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2)); + let del11 = (ii1 + ii2).quadform(&rotmat).into_matrix(); // Note that Cholesky only reads the lower-triangular part of the matrix - // so we don't need to fill lhs01. - lhs = Matrix6::zeros(); - lhs.fixed_slice_mut::<U3, U3>(0, 0) - .copy_from(&lhs00.into_matrix()); - lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10); - lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11); - - // Adjust the mass matrix to take force limits into account. - // If a DoF has a force limit, then we need to make its - // constraint independent from the others because otherwise - // the force clamping will cause errors to propagate in the - // other constraints. - if velocity_solver { - for i in 0..6 { - if joint.max_negative_impulse[i] > -Real::MAX - || joint.max_positive_impulse[i] < Real::MAX - { - let diag = lhs[(i, i)]; - lhs.column_mut(i).fill(0.0); - lhs.row_mut(i).fill(0.0); - lhs[(i, i)] = diag; - } - } - } + // so we don't need to fill del01. + let mut del = Matrix6::zeros(); + del.fixed_slice_mut::<U3, U3>(0, 0) + .copy_from(&del00.into_matrix()); + del.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&del10); + del.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&del11); + del } // In 2D we just unroll the computation because // it's just easier that way. #[cfg(feature = "dim2")] { + panic!("Take the rotmat into account."); let m11 = im1 + im2 + rmat1.x * rmat1.x * ii1 + rmat2.x * rmat2.x * ii2; let m12 = rmat1.x * rmat1.y * ii1 + rmat2.x * rmat2.y * ii2; let m22 = im1 + im2 + rmat1.y * rmat1.y * ii1 + rmat2.y * rmat2.y * ii2; let m13 = rmat1.x * ii1 + rmat2.x * ii2; let m23 = rmat1.y * ii1 + rmat2.y * ii2; let m33 = ii1 + ii2; - lhs = Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33) + Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33) } + } + + pub fn compute_velocity_error( + min_velocity: &SpatialVector<Real>, + max_velocity: &SpatialVector<Real>, + r1: &Vector<Real>, + r2: &Vector<Real>, + _anchor1: &Isometry<Real>, + anchor2: &Isometry<Real>, + rb1: &RigidBody, + rb2: &RigidBody, + ) -> SpatialVector<Real> { + let lin_dvel = -rb1.linvel - rb1.angvel.gcross(*r1) + rb2.linvel + rb2.angvel.gcross(*r2); + let ang_dvel = -rb1.angvel + rb2.angvel; + + let lin_dvel2 = anchor2.inverse_transform_vector(&lin_dvel); + let ang_dvel2 = anchor2.inverse_transform_vector(&ang_dvel); + + dbg!(lin_dvel); + dbg!(lin_dvel2); + + let min_linvel = min_velocity.xyz(); + let min_angvel = min_velocity.fixed_rows::<AngDim>(DIM).into_owned(); + let max_linvel = max_velocity.xyz(); + let max_angvel = max_velocity.fixed_rows::<AngDim>(DIM).into_owned(); + let lin_rhs = lin_dvel2 - lin_dvel2.sup(&min_linvel).inf(&max_linvel); + let ang_rhs = ang_dvel2 - ang_dvel2.sup(&min_angvel).inf(&max_angvel); - // NOTE: we don't use Cholesky in 2D because we only have a 3x3 matrix - // for which a textbook inverse is still efficient. #[cfg(feature = "dim2")] - return lhs.try_inverse().expect("Singular system."); + return Vector3::new(lin_rhs.x, lin_rhs.y, ang_rhs); + #[cfg(feature = "dim3")] - return lhs.cholesky().expect("Singular system.").inverse(); + return Vector6::new( + lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y, ang_rhs.z, + ); + } + + pub fn compute_position_error( + joint: &GenericJoint, + anchor1: &Isometry<Real>, + anchor2: &Isometry<Real>, + ) -> SpatialVector<Real> { + let delta_pos = Isometry::from_parts( + anchor2.translation * anchor1.translation.inverse(), + anchor2.rotation * anchor1.rotation.inverse(), + ); + let lin_dpos = anchor2.inverse_transform_vector(&delta_pos.translation.vector); + let ang_dpos = anchor2.inverse_transform_vector(&delta_pos.rotation.scaled_axis()); + + let dpos = Vector6::new( + lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, + ); + + let error = dpos - dpos.sup(&joint.min_position).inf(&joint.max_position); + let error_code = + (error[3] == 0.0) as usize + (error[4] == 0.0) as usize + (error[5] == 0.0) as usize; + + match error_code { + 0 => error, + 1 => { + let constrained_axis = error.rows(3, 3).iamin(); + let axis1 = anchor1 + .rotation + .to_rotation_matrix() + .into_inner() + .column(constrained_axis) + .into_owned(); + let axis2 = anchor2 + .rotation + .to_rotation_matrix() + .into_inner() + .column(constrained_axis) + .into_owned(); + let rot_cross = UnitQuaternion::rotation_between(&axis1, &axis2) + .unwrap_or(UnitQuaternion::identity()); + let ang_dpos = anchor2.inverse_transform_vector(&rot_cross.scaled_axis()); + let dpos = Vector6::new( + lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, + ); + + dpos - dpos.sup(&joint.min_position).inf(&joint.max_position) + } + 2 => { + // TODO + error + } + 3 => error, + _ => unreachable!(), + } } pub fn from_params( @@ -136,48 +192,67 @@ impl GenericVelocityConstraint { let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); let r1 = anchor1.translation.vector - rb1.world_com.coords; let r2 = anchor2.translation.vector - rb2.world_com.coords; + let mut min_impulse = joint.min_impulse; + let mut max_impulse = joint.max_impulse; + let mut min_pos_impulse = joint.min_pos_impulse; + let mut max_pos_impulse = joint.max_pos_impulse; + let mut min_velocity = joint.min_velocity; + let mut max_velocity = joint.max_velocity; + + let pos_rhs = Self::compute_position_error(joint, &anchor1, &anchor2) + * params.inv_dt() + * params.joint_erp; + + for i in 0..6 { + if pos_rhs[i] < 0.0 { + min_impulse[i] = -Real::MAX; + min_pos_impulse[i] = -Real::MAX; + min_velocity[i] = 0.0; + } + if pos_rhs[i] > 0.0 { + max_impulse[i] = Real::MAX; + max_pos_impulse[i] = Real::MAX; + max_velocity[i] = 0.0; + } + } - let lin_dvel = -rb1.linvel - rb1.angvel.gcross(r1) + rb2.linvel + rb2.angvel.gcross(r2); - let ang_dvel = -rb1.angvel + rb2.angvel; + let rhs = Self::compute_velocity_error( + &min_velocity, + &max_velocity, + &r1, + &r2, + &anchor1, + &anchor2, + rb1, + rb2, + ); - let inv_lhs = Self::compute_mass_matrix(joint, im1, im2, ii1, ii2, r1, r2, true); + let mut delassus = + Self::compute_delassus_matrix(im1, im2, ii1, ii2, r1, r2, anchor2.rotation); + + // Adjust the Delassus matrix to take force limits into account. + // If a DoF has a force limit, then we need to make its + // constraint independent from the others because otherwise + // the force clamping will cause errors to propagate in the + // other constraints. + for i in 0..6 { + if min_impulse[i] > -Real::MAX && max_impulse[i] < Real::MAX { + let diag = delassus[(i, i)]; + delassus.column_mut(i).fill(0.0); + delassus.row_mut(i).fill(0.0); + delassus[(i, i)] = diag; + } + } + // NOTE: we don't use Cholesky in 2D because we only have a 3x3 matrix. #[cfg(feature = "dim2")] - let dvel = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); - + let inv_lhs = delassus.try_inverse().expect("Singular system."); #[cfg(feature = "dim3")] - let dvel = Vector6::new( - lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, - ); - - let target_linvel = anchor2 * joint.target_velocity.xyz(); - let target_angvel = anchor2 * joint.target_velocity.fixed_rows::<AngDim>(DIM).into_owned(); - let target_vel = Vector6::new( - target_linvel.x, - target_linvel.y, - target_linvel.z, - target_angvel.x, - target_angvel.y, - target_angvel.z, - ); - - let rhs = dvel - dvel.sup(&target_vel).inf(&target_vel); - - let delta_pos = Isometry::from_parts( - anchor2.translation * anchor1.translation.inverse(), - anchor2.rotation * anchor1.rotation.inverse(), - ); - let lin_dpos = delta_pos.translation.vector; - let ang_dpos = delta_pos.rotation.scaled_axis(); - let dpos = Vector6::new( - lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, - ); - let err = dpos - dpos.sup(&joint.min_position).inf(&joint.max_position); - let pos_rhs = err * params.inv_dt() * params.joint_erp; + let inv_lhs = delassus.cholesky().expect("Singular system.").inverse(); let impulse = (joint.impulse * params.warmstart_coeff) - .inf(&joint.max_positive_impulse) - .sup(&joint.max_negative_impulse); + .inf(&max_impulse) + .sup(&min_impulse); GenericVelocityConstraint { joint_id, @@ -189,15 +264,22 @@ impl GenericVelocityConstraint { ii2, ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, - impulse, - pos_impulse: na::zero(), - max_positive_impulse: joint.max_positive_impulse, - max_negative_impulse: joint.max_negative_impulse, inv_lhs, r1, r2, - rhs, - pos_rhs, + rot2: anchor2.rotation, + vel: GenericConstraintPart { + impulse, + min_impulse, + max_impulse, + rhs, + }, + pos: GenericConstraintPart { + impulse: na::zero(), + min_impulse: min_pos_impulse, + max_impulse: max_pos_impulse, + rhs: pos_rhs, + }, } } @@ -205,11 +287,11 @@ impl GenericVelocityConstraint { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned(); + let lin_impulse = self.rot2 * self.vel.impulse.fixed_rows::<Dim>(0).into_owned(); #[cfg(feature = "dim2")] - let ang_impulse = self.impulse[2]; + let ang_impulse = self.rot2 * self.vel.impulse[2]; #[cfg(feature = "dim3")] - let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned(); + let ang_impulse = self.rot2 * self.vel.impulse.fixed_rows::<U3>(3).into_owned(); mj_lambda1.linear += self.im1 * lin_impulse; mj_lambda1.angular += self @@ -227,48 +309,6 @@ impl GenericVelocityConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) { return; - let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - - let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); - let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - - let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1) - + mj_lambda2.linear - + ang_vel2.gcross(self.r2); - let dangvel = -ang_vel1 + ang_vel2; - - #[cfg(feature = "dim2")] - let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; - #[cfg(feature = "dim3")] - let dvel = Vector6::new( - dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, - ) + self.rhs; - - let new_impulse = (self.impulse + self.inv_lhs * dvel) - .sup(&self.max_negative_impulse) - .inf(&self.max_positive_impulse); - let effective_impulse = new_impulse - self.impulse; - self.impulse = new_impulse; - - let lin_impulse = effective_impulse.fixed_rows::<Dim>(0).into_owned(); - #[cfg(feature = "dim2")] - let ang_impulse = effective_impulse[2]; - #[cfg(feature = "dim3")] - let ang_impulse = effective_impulse.fixed_rows::<U3>(3).into_owned(); - - mj_lambda1.linear += self.im1 * lin_impulse; - mj_lambda1.angular += self - .ii1_sqrt - .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); - - mj_lambda2.linear -= self.im2 * lin_impulse; - mj_lambda2.angular -= self - .ii2_sqrt - .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); - - mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; - mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } pub fn solve2( @@ -281,82 +321,10 @@ impl GenericVelocityConstraint { let mut mj_lambda_pos1 = mj_lambdas_pos[self.mj_lambda1 as usize]; let mut mj_lambda_pos2 = mj_lambdas_pos[self.mj_lambda2 as usize]; - /* - * Solve velocity. - */ - let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); - let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - - let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1) - + mj_lambda2.linear - + ang_vel2.gcross(self.r2); - let dangvel = -ang_vel1 + ang_vel2; - - #[cfg(feature = "dim2")] - let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; - #[cfg(feature = "dim3")] - let dvel = Vector6::new( - dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, - ) + self.rhs; - - let new_impulse = (self.impulse + self.inv_lhs * dvel) - .sup(&self.max_negative_impulse) - .inf(&self.max_positive_impulse); - let effective_impulse = new_impulse - self.impulse; - self.impulse = new_impulse; - - let lin_impulse = effective_impulse.fixed_rows::<Dim>(0).into_owned(); - #[cfg(feature = "dim2")] - let ang_impulse = effective_impulse[2]; - #[cfg(feature = "dim3")] - let ang_impulse = effective_impulse.fixed_rows::<U3>(3).into_owned(); - - mj_lambda1.linear += self.im1 * lin_impulse; - mj_lambda1.angular += self - .ii1_sqrt - .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); - - mj_lambda2.linear -= self.im2 * lin_impulse; - mj_lambda2.angular -= self - .ii2_sqrt - .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); - - /* - * Solve positions. - */ - - let ang_pos1 = self.ii1_sqrt.transform_vector(mj_lambda_pos1.angular); - let ang_pos2 = self.ii2_sqrt.transform_vector(mj_lambda_pos2.angular); - - let dlinpos = -mj_lambda_pos1.linear - ang_pos1.gcross(self.r1) - + mj_lambda_pos2.linear - + ang_pos2.gcross(self.r2); - let dangpos = -ang_pos1 + ang_pos2; - - #[cfg(feature = "dim3")] - let dpos = Vector6::new( - dlinpos.x, dlinpos.y, dlinpos.z, dangpos.x, dangpos.y, dangpos.z, - ) + self.pos_rhs; - - let new_impulse = self.pos_impulse + self.inv_lhs * dpos; - let effective_impulse = new_impulse - self.pos_impulse; - self.pos_impulse = new_impulse; - - let lin_impulse = effective_impulse.fixed_rows::<Dim>(0).into_owned(); - #[cfg(feature = "dim2")] - let ang_impulse = effective_impulse[2]; - #[cfg(feature = "dim3")] - let ang_impulse = effective_impulse.fixed_rows::<U3>(3).into_owned(); - - mj_lambda_pos1.linear += self.im1 * lin_impulse; - mj_lambda_pos1.angular += self - .ii1_sqrt - .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); - - mj_lambda_pos2.linear -= self.im2 * lin_impulse; - mj_lambda_pos2.angular -= self - .ii2_sqrt - .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + self.vel.impulse = self.vel.solve(self, &mut mj_lambda1, &mut mj_lambda2); + self.pos.impulse = self + .pos + .solve(self, &mut mj_lambda_pos1, &mut mj_lambda_pos2); mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; @@ -367,7 +335,7 @@ impl GenericVelocityConstraint { pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { let joint = &mut joints_all[self.joint_id].weight; if let JointParams::GenericJoint(fixed) = &mut joint.params { - fixed.impulse = self.impulse; + fixed.impulse = self.vel.impulse; } } } @@ -378,94 +346,61 @@ pub(crate) struct GenericVelocityGroundConstraint { joint_id: JointIndex, - impulse: SpacialVector<Real>, - pos_impulse: SpacialVector<Real>, - - max_positive_impulse: SpatialVector<Real>, - max_negative_impulse: SpatialVector<Real>, - - #[cfg(feature = "dim3")] - inv_lhs: Matrix6<Real>, // FIXME: replace by Cholesky. #[cfg(feature = "dim3")] - rhs: Vector6<Real>, - + inv_lhs: Matrix6<Real>, // TODO: replace by Cholesky? #[cfg(feature = "dim2")] - inv_lhs: Matrix3<Real>, // FIXME: replace by Cholesky. - #[cfg(feature = "dim2")] - rhs: Vector3<Real>, - - pos_rhs: Vector6<Real>, + inv_lhs: Matrix3<Real>, im2: Real, ii2: AngularInertia<Real>, ii2_sqrt: AngularInertia<Real>, r2: Vector<Real>, + rot2: Rotation<Real>, + + vel: GenericConstraintPart, + pos: GenericConstraintPart, } impl GenericVelocityGroundConstraint { #[inline(always)] - pub fn compute_mass_matrix( - joint: &GenericJoint, + pub fn compute_delassus_matrix( im2: Real, ii2: AngularInertia<Real>, r2: Vector<Real>, - velocity_solver: bool, + rot2: Rotation<Real>, ) -> Matrix6<Real> { - let rmat2 = r2.gcross_matrix(); - - #[allow(unused_mut)] // For 2D. - let mut lhs; + let rotmat2 = rot2.to_rotation_matrix().into_inner(); + let rmat2 = r2.gcross_matrix() * rotmat2; #[cfg(feature = "dim3")] { - let lhs00 = ii2.quadform(&rmat2).add_diagonal(im2); - let lhs10 = ii2.transform_matrix(&rmat2); - let lhs11 = ii2.into_matrix(); + let del00 = ii2.quadform(&rmat2).add_diagonal(im2); + let del10 = rotmat2.transpose() * ii2.transform_matrix(&rmat2); + let del11 = ii2.quadform(&rotmat2).into_matrix(); // Note that Cholesky only reads the lower-triangular part of the matrix // so we don't need to fill lhs01. - lhs = Matrix6::zeros(); - lhs.fixed_slice_mut::<U3, U3>(0, 0) - .copy_from(&lhs00.into_matrix()); - lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10); - lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11); - - // Adjust the mass matrix to take force limits into account. - // If a DoF has a force limit, then we need to make its - // constraint independent from the others because otherwise - // the force clamping will cause errors to propagate in the - // other constraints. - if velocity_solver { - for i in 0..6 { - if joint.max_negative_impulse[i] > -Real::MAX - || joint.max_positive_impulse[i] < Real::MAX - { - let diag = lhs[(i, i)]; - lhs.column_mut(i).fill(0.0); - lhs.row_mut(i).fill(0.0); - lhs[(i, i)] = diag; - } - } - } + let mut del = Matrix6::zeros(); + del.fixed_slice_mut::<U3, U3>(0, 0) + .copy_from(&del00.into_matrix()); + del.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&del10); + del.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&del11); + del } // In 2D we just unroll the computation because // it's just easier that way. #[cfg(feature = "dim2")] { + panic!("Properly handle the rotmat2"); let m11 = im2 + rmat2.x * rmat2.x * ii2; let m12 = rmat2.x * rmat2.y * ii2; let m22 = im2 + rmat2.y * rmat2.y * ii2; let m13 = rmat2.x * ii2; let m23 = rmat2.y * ii2; let m33 = ii2; - lhs = Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33) + Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33) } - - #[cfg(feature = "dim2")] - return lhs.try_inverse().expect("Singular system."); - #[cfg(feature = "dim3")] - return lhs.cholesky().expect("Singular system.").inverse(); } pub fn from_params( @@ -488,50 +423,70 @@ impl GenericVelocityGroundConstraint { ) }; - let r1 = anchor1.translation.vector - rb1.world_com.coords; let im2 = rb2.effective_inv_mass; let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let r1 = anchor1.translation.vector - rb1.world_com.coords; let r2 = anchor2.translation.vector - rb2.world_com.coords; + let mut min_impulse = joint.min_impulse; + let mut max_impulse = joint.max_impulse; + let mut min_pos_impulse = joint.min_pos_impulse; + let mut max_pos_impulse = joint.max_pos_impulse; + let mut min_velocity = joint.min_velocity; + let mut max_velocity = joint.max_velocity; + + let pos_rhs = GenericVelocityConstraint::compute_position_error(joint, &anchor1, &anchor2) + * params.inv_dt() + * params.joint_erp; + + for i in 0..6 { + if pos_rhs[i] < 0.0 { + min_impulse[i] = -Real::MAX; + min_pos_impulse[i] = -Real::MAX; + min_velocity[i] = 0.0; + } + if pos_rhs[i] > 0.0 { + max_impulse[i] = Real::MAX; + max_pos_impulse[i] = Real::MAX; + max_velocity[i] = 0.0; + } + } - let inv_lhs = Self::compute_mass_matrix(joint, im2, ii2, r2, true); + let rhs = GenericVelocityConstraint::compute_velocity_error( + &min_velocity, + &max_velocity, + &r1, + &r2, + &anchor1, + &anchor2, + rb1, + rb2, + ); - let lin_dvel = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1); - let ang_dvel = rb2.angvel - rb1.angvel; + let mut delassus = Self::compute_delassus_matrix(im2, ii2, r2, anchor2.rotation); + + // Adjust the Delassus matrix to take force limits into account. + // If a DoF has a force limit, then we need to make its + // constraint independent from the others because otherwise + // the force clamping will cause errors to propagate in the + // other constraints. + for i in 0..6 { + if min_impulse[i] > -Real::MAX && max_impulse[i] < Real::MAX { + let diag = delassus[(i, i)]; + delassus.column_mut(i).fill(0.0); + delassus.row_mut(i).fill(0.0); + delassus[(i, i)] = diag; + } + } + // NOTE: we don't use Chol |