diff options
| -rw-r--r-- | src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs | 565 |
1 files changed, 218 insertions, 347 deletions
diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs index ba3efbe..63ffdfd 100644 --- a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs @@ -6,10 +6,10 @@ use crate::math::{AngularInertia, Dim, Isometry, Real, Rotation, SpacialVector, use crate::na::UnitQuaternion; use crate::parry::math::{AngDim, SpatialVector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +#[cfg(feature = "dim3")] +use na::{Matrix3, Matrix6, Vector3, Vector6, U3}; #[cfg(feature = "dim2")] use na::{Matrix3, Vector3}; -#[cfg(feature = "dim3")] -use na::{Matrix6, Vector6, U3}; #[derive(Debug)] pub(crate) struct GenericVelocityConstraint { @@ -18,10 +18,8 @@ pub(crate) struct GenericVelocityConstraint { joint_id: JointIndex, - #[cfg(feature = "dim3")] - inv_lhs: Matrix6<Real>, // TODO: replace by Cholesky? - #[cfg(feature = "dim2")] - inv_lhs: Matrix3<Real>, + inv_lhs_lin: Matrix3<Real>, + inv_lhs_ang: Matrix3<Real>, im1: Real, im2: Real, @@ -41,53 +39,6 @@ pub(crate) struct GenericVelocityConstraint { } impl GenericVelocityConstraint { - #[inline(always)] - pub fn compute_delassus_matrix( - im1: Real, - im2: Real, - ii1: AngularInertia<Real>, - ii2: AngularInertia<Real>, - r1: Vector<Real>, - r2: Vector<Real>, - basis: Rotation<Real>, - ) -> Matrix6<Real> { - let rotmat = basis.to_rotation_matrix().into_inner(); - let rmat1 = r1.gcross_matrix() * rotmat; - let rmat2 = r2.gcross_matrix() * rotmat; - - #[cfg(feature = "dim3")] - { - let del00 = - ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2); - 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 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; - Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33) - } - } - pub fn compute_velocity_error( min_velocity: &SpatialVector<Real>, max_velocity: &SpatialVector<Real>, @@ -244,36 +195,6 @@ impl GenericVelocityConstraint { delassus.try_inverse().unwrap() } - pub fn invert_delassus_matrix( - min_impulse: &Vector6<Real>, - max_impulse: &Vector6<Real>, - dependant_set_mask: &mut Vector6<Real>, - mut delassus: Matrix6<Real>, - ) -> Matrix6<Real> { - // 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. - dependant_set_mask.fill(1.0); - - 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; - dependant_set_mask[i] = 0.0; - } - } - - // NOTE: we don't use Cholesky in 2D because we only have a 3x3 matrix. - #[cfg(feature = "dim2")] - return delassus.try_inverse().expect("Singular system."); - #[cfg(feature = "dim3")] - return delassus.cholesky().expect("Singular system.").inverse(); - } - pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, @@ -317,19 +238,42 @@ impl GenericVelocityConstraint { let rhs = Self::compute_velocity_error(&min_velocity, &max_velocity, &r1, &r2, &basis, rb1, rb2); + let rhs_lin = rhs.xyz(); + let rhs_ang = rhs.fixed_rows::<Dim>(DIM).into(); - let mut delassus = Self::compute_delassus_matrix(im1, im2, ii1, ii2, r1, r2, basis); - let inv_lhs = Self::invert_delassus_matrix( - &min_impulse, - &max_impulse, - &mut dependant_set_mask, - delassus, + // TODO: we should keep the SdpMatrix3 type. + let rotmat = basis.to_rotation_matrix().into_inner(); + let rmat1 = r1.gcross_matrix() * rotmat; + let rmat2 = r2.gcross_matrix() * rotmat; + let delassus00 = (ii1.quadform(&rmat1).add_diagonal(im1) + + ii2.quadform(&rmat2).add_diagonal(im2)) + .into_matrix(); + let delassus11 = (ii1.quadform(&rotmat) + ii2.quadform(&rotmat)).into_matrix(); + + let inv_lhs_lin = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse.xyz(), + &max_pos_impulse.xyz(), + &mut Vector3::zeros(), + delassus00, + ); + let inv_lhs_ang = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(), + &max_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(), + &mut Vector3::zeros(), + delassus11, ); let impulse = (joint.impulse * params.warmstart_coeff) .inf(&max_impulse) .sup(&min_impulse); + let lin_impulse = impulse.xyz(); + let ang_impulse = impulse.fixed_rows::<Dim>(DIM).into_owned(); + let min_lin_impulse = min_impulse.xyz(); + let min_ang_impulse = min_impulse.fixed_rows::<Dim>(DIM).into_owned(); + let max_lin_impulse = max_impulse.xyz(); + let max_ang_impulse = max_impulse.fixed_rows::<Dim>(DIM).into_owned(); + GenericVelocityConstraint { joint_id, mj_lambda1: rb1.active_set_offset, @@ -340,16 +284,21 @@ impl GenericVelocityConstraint { ii2, ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, - inv_lhs, + inv_lhs_lin, + inv_lhs_ang, r1, r2, basis, dependant_set_mask, vel: GenericConstraintPart { - impulse, - min_impulse, - max_impulse, - rhs, + lin_impulse, + ang_impulse, + min_lin_impulse, + min_ang_impulse, + max_lin_impulse, + max_ang_impulse, + rhs_lin, + rhs_ang, }, } } @@ -358,11 +307,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.basis * self.vel.impulse.fixed_rows::<Dim>(0).into_owned(); + let lin_impulse = self.basis * self.vel.lin_impulse; #[cfg(feature = "dim2")] let ang_impulse = self.basis * self.vel.impulse[2]; #[cfg(feature = "dim3")] - let ang_impulse = self.basis * self.vel.impulse.fixed_rows::<U3>(3).into_owned(); + let ang_impulse = self.basis * self.vel.ang_impulse; mj_lambda1.linear += self.im1 * lin_impulse; mj_lambda1.angular += self @@ -382,7 +331,9 @@ impl GenericVelocityConstraint { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - self.vel.impulse = self.vel.solve(self, &mut mj_lambda1, &mut mj_lambda2); + let (lin_imp, ang_imp) = self.vel.solve(self, &mut mj_lambda1, &mut mj_lambda2); + self.vel.lin_impulse = lin_imp; + self.vel.ang_impulse = ang_imp; mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; @@ -391,7 +342,15 @@ 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.vel.impulse; + let impulse = Vector6::new( + self.vel.lin_impulse.x, + self.vel.lin_impulse.y, + self.vel.lin_impulse.z, + self.vel.ang_impulse.x, + self.vel.ang_impulse.y, + self.vel.ang_impulse.z, + ); + fixed.impulse = impulse; } } } @@ -402,10 +361,8 @@ pub(crate) struct GenericVelocityGroundConstraint { joint_id: JointIndex, - #[cfg(feature = "dim3")] - inv_lhs: Matrix6<Real>, // TODO: replace by Cholesky? - #[cfg(feature = "dim2")] - inv_lhs: Matrix3<Real>, + inv_lhs_lin: Matrix3<Real>, + inv_lhs_ang: Matrix3<Real>, im2: Real, ii2: AngularInertia<Real>, @@ -520,33 +477,58 @@ impl GenericVelocityGroundConstraint { rb1, rb2, ); + let rhs_lin = rhs.xyz(); + let rhs_ang = rhs.fixed_rows::<Dim>(DIM).into_owned(); - let delassus = Self::compute_delassus_matrix(im2, ii2, r2, basis); - let inv_lhs = GenericVelocityConstraint::invert_delassus_matrix( - &min_impulse, - &max_impulse, - &mut dependant_set_mask, - delassus, + // TODO: we should keep the SdpMatrix3 type. + let rotmat = basis.to_rotation_matrix().into_inner(); + let rmat2 = r2.gcross_matrix() * rotmat; + let delassus00 = ii2.quadform(&rmat2).add_diagonal(im2).into_matrix(); + let delassus11 = ii2.quadform(&rotmat).into_matrix(); + + let inv_lhs_lin = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse.xyz(), + &max_pos_impulse.xyz(), + &mut Vector3::zeros(), + delassus00, + ); + let inv_lhs_ang = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(), + &max_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(), + &mut Vector3::zeros(), + delassus11, ); let impulse = (joint.impulse * params.warmstart_coeff) .inf(&max_impulse) .sup(&min_impulse); + let lin_impulse = impulse.xyz(); + let ang_impulse = impulse.fixed_rows::<Dim>(DIM).into_owned(); + let min_lin_impulse = min_impulse.xyz(); + let min_ang_impulse = min_impulse.fixed_rows::<Dim>(DIM).into_owned(); + let max_lin_impulse = max_impulse.xyz(); + let max_ang_impulse = max_impulse.fixed_rows::<Dim>(DIM).into_owned(); + GenericVelocityGroundConstraint { joint_id, mj_lambda2: rb2.active_set_offset, im2, ii2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, - inv_lhs, + inv_lhs_lin, + inv_lhs_ang, r2, basis, vel: GenericConstraintPart { - impulse, - min_impulse, - max_impulse, - rhs, + lin_impulse, + ang_impulse, + min_lin_impulse, + min_ang_impulse, + max_lin_impulse, + max_ang_impulse, + rhs_lin, + rhs_ang, }, dependant_set_mask, } @@ -555,11 +537,11 @@ impl GenericVelocityGroundConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - let lin_impulse = self.basis * self.vel.impulse.fixed_rows::<Dim>(0).into_owned(); + let lin_impulse = self.basis * self.vel.lin_impulse; #[cfg(feature = "dim2")] let ang_impulse = self.basis * self.vel.impulse[2]; #[cfg(feature = "dim3")] - let ang_impulse = self.basis * self.vel.impulse.fixed_rows::<U3>(3).into_owned(); + let ang_impulse = self.basis * self.vel.ang_impulse; mj_lambda2.linear -= self.im2 * lin_impulse; mj_lambda2.angular -= self @@ -571,7 +553,11 @@ impl GenericVelocityGroundConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - self.vel.impulse = self.vel.solve_ground(self, &mut mj_lambda2); + + let (lin_imp, ang_imp) = self.vel.solve_ground(self, &mut mj_lambda2); + self.vel.lin_impulse = lin_imp; + self.vel.ang_impulse = ang_imp; + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } @@ -579,268 +565,153 @@ impl GenericVelocityGroundConstraint { 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.vel.impulse; + let impulse = Vector6::new( + self.vel.lin_impulse.x, + self.vel.lin_impulse.y, + self.vel.lin_impulse.z, + self.vel.ang_impulse.x, + self.vel.ang_impulse.y, + self.vel.ang_impulse.z, + ); + fixed.impulse = impulse; } } } #[derive(Debug)] struct GenericConstraintPart { - impulse: SpacialVector<Real>, - max_impulse: SpatialVector<Real>, - min_impulse: SpatialVector<Real>, - - #[cfg(feature = "dim3")] - rhs: Vector6<Real>, - #[cfg(feature = "dim2")] - rhs: Vector3<Real>, + lin_impulse: Vector3<Real>, + max_lin_impulse: Vector3<Real>, + min_lin_impulse: Vector3<Real>, + rhs_lin: Vector3<Real>, + + ang_impulse: Vector3<Real>, + max_ang_impulse: Vector3<Real>, + min_ang_impulse: Vector3<Real>, + rhs_ang: Vector3<Real>, } impl GenericConstraintPart { - fn solve_ground( + fn solve( &self, - parent: &GenericVelocityGroundConstraint, + parent: &GenericVelocityConstraint, + mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>, - ) -> SpatialVector<Real> { - let mut new_impulse = SpacialVector::zeros(); - - for i in 0..3 { - // Solve the independent 1D constraints. - if parent.dependant_set_mask[i] == 0.0 - && (self.min_impulse[i] != 0.0 || self.max_impulse[i] != 0.0) - { - let constraint_axis = parent.basis * Vector::ith(i % 3, 1.0); - let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); - - let dvel = if i < 3 { - (mj_lambda2.linear + ang_vel2.gcross(parent.r2)).dot(&constraint_axis) - + self.rhs[i] - } else { - ang_vel2.dot(&constraint_axis) + self.rhs[i] - }; - - new_impulse[i] = na::clamp( - self.impulse[i] + parent.inv_lhs[(i, i)] * dvel, - self.min_impulse[i], - self.max_impulse[i], - ); - - let effective_impulse = new_impulse[i] - self.impulse[i]; - let impulse = constraint_axis * effective_impulse; - - if i < 3 { - mj_lambda2.linear -= parent.im2 * impulse; - mj_lambda2.angular -= - parent.ii2_sqrt.transform_vector(parent.r2.gcross(impulse)); - } else { - mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(impulse); - } - } + ) -> (Vector3<Real>, Vector3<Real>) { + let new_lin_impulse; + let new_ang_impulse; + + /* + * + * Solve translations. + * + */ + { + let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dvel = parent.basis.inverse_transform_vector( + &(-mj_lambda1.linear - ang_vel1.gcross(parent.r1) + + mj_lambda2.linear + + ang_vel2.gcross(parent.r2)), + ); + + let err = dvel + self.rhs_lin; + + new_lin_impulse = (self.lin_impulse + parent.inv_lhs_lin * err) + .sup(&self.min_lin_impulse) + .inf(&self.max_lin_impulse); + let effective_impulse = parent.basis * (new_lin_impulse - self.lin_impulse); + + mj_lambda1.linear += parent.im1 * effective_impulse; + mj_lambda1.angular += parent + .ii1_sqrt + .transform_vector(parent.r1.gcross(effective_impulse)); + + mj_lambda2.linear -= parent.im2 * effective_impulse; + mj_lambda2.angular -= parent + .ii2_sqrt + .transform_vector(parent.r2.gcross(effective_impulse)); } - let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); - - let dlinvel = parent - .basis - .inverse_transform_vector(&(mj_lambda2.linear + ang_vel2.gcross(parent.r2))); - let dangvel = parent.basis.inverse_transform_vector(&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; - - new_impulse += - (self.impulse + parent.inv_lhs * dvel).component_mul(&parent.dependant_set_mask); - let effective_impulse = - (new_impulse - self.impulse).component_mul(&parent.dependant_set_mask); - - let lin_impulse = parent.basis * effective_impulse.fixed_rows::<Dim>(0).into_owned(); - #[cfg(feature = "dim2")] - let ang_impulse = parent.basis * effective_impulse[2]; - #[cfg(feature = "dim3")] - let ang_impulse = parent.basis * effective_impulse.fixed_rows::<U3>(3).into_owned(); + /* + * + * Solve rotations. + * + */ + { + let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); - mj_lambda2.linear -= parent.im2 * lin_impulse; - mj_lambda2.angular -= parent - .ii2_sqrt - .transform_vector(ang_impulse + parent.r2.gcross(lin_impulse)); - - for i in 3..6 { - // Solve the independent 1D constraints. - if parent.dependant_set_mask[i] == 0.0 - && (self.min_impulse[i] != 0.0 || self.max_impulse[i] != 0.0) - { - let constraint_axis = parent.basis * Vector::ith(i % 3, 1.0); - let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); - - let dvel = if i < 3 { - (mj_lambda2.linear + ang_vel2.gcross(parent.r2)).dot(&constraint_axis) - + self.rhs[i] - } else { - ang_vel2.dot(&constraint_axis) + self.rhs[i] - }; - - let inv_lhs = parent.inv_lhs[(i, i)]; - - new_impulse[i] = na::clamp( - self.impulse[i] + inv_lhs * dvel, - self.min_impulse[i], - self.max_impulse[i], - ); + let dvel = parent + .basis + .inverse_transform_vector(&(ang_vel2 - ang_vel1)); + let err = dvel + self.rhs_ang; - let effective_impulse = new_impulse[i] - self.impulse[i]; - let impulse = constraint_axis * effective_impulse; + new_ang_impulse = (self.ang_impulse + parent.inv_lhs_ang * err) + .sup(&self.min_ang_impulse) + .inf(&self.max_ang_impulse); + let effective_impulse = parent.basis * (new_ang_impulse - self.ang_impulse); - if i < 3 { - mj_lambda2.linear -= parent.im2 * impulse; - mj_lambda2.angular -= - parent.ii2_sqrt.transform_vector(parent.r2.gcross(impulse)); - } else { - mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(impulse); - } - } + mj_lambda1.angular += parent.ii1_sqrt.transform_vector(effective_impulse); + mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse); } - new_impulse + (new_lin_impulse, new_ang_impulse) } - fn solve( + fn solve_ground( &self, - parent: &GenericVelocityConstraint, - mj_lambda1: &mut DeltaVel<Real>, + parent: &GenericVelocityGroundConstraint, mj_lambda2: &mut DeltaVel<Real>, - ) -> SpatialVector<Real> { - let mut new_impulse = SpacialVector::zeros(); - - for i in 0..3 { - // Solve the independent 1D constraints. - if parent.dependant_set_mask[i] == 0.0 - && (self.min_impulse[i] != 0.0 || self.max_impulse[i] != 0.0) - { - let constraint_axis = parent.basis * Vector::ith(i % 3, 1.0); - let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); - let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular); - - let dvel = if i < 3 { - (mj_lambda2.linear + ang_vel2.gcross(parent.r2) - - mj_lambda1.linear - - ang_vel1.gcross(parent.r1)) - .dot(&constraint_axis) - + self.rhs[i] - } else { - (ang_vel2 - ang_vel1).dot(&constraint_axis) + self.rhs[i] - }; - - new_impulse[i] = na::clamp( - self.impulse[i] + parent.inv_lhs[(i, i)] * dvel, - self.min_impulse[i], - self.max_impulse[i], - ); - - let effective_impulse = new_impulse[i] - self.impulse[i]; - let impulse = constraint_axis * effective_impulse; - - if i < 3 { - mj_lambda1.linear += parent.im1 * impulse; - mj_lambda1.angular += - parent.ii1_sqrt.transform_vector(parent.r1.gcross(impulse)); - mj_lambda2.linear -= parent.im2 * impulse; - mj_lambda2.angular -= - parent.ii2_sqrt.transform_vector(parent.r2.gcross(impulse)); - } else { - mj_lambda1.angular += parent.ii1_sqrt.transform_vector(impulse); - mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(impulse); - } - } - } + ) -> (Vector3<Real>, Vector3<Real>) { + let new_lin_impulse; + let new_ang_impulse; + + /* + * + * Solve translations. + * + */ + { + let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); - let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular); - let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); + let dvel = parent + .basis + .inverse_transform_vector(&(mj_lambda2.linear + ang_vel2.gcross(parent.r2))); - let dlinvel = parent.basis.inverse_transform_vector( - &(-mj_lambda1.linear - ang_vel1.gcross(parent.r1) - + mj_lambda2.linear - + ang_vel2.gcross(parent.r2)), - ); - let dangvel = parent - .basis - .inverse_transform_vector(&(-ang_vel1 + ang_vel2)); + let err = dvel + self.rhs_lin; - #[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; + new_lin_impulse = (self.lin_impulse + parent.inv_lhs_lin * err) + .sup(&self.min_lin_impulse) + .inf(&self.max_lin_impulse); + let effective_impulse = parent.basis * (new_lin_impulse - self.lin_impulse); - new_impulse += - (self.impulse + parent.inv_lhs * dvel).component_mul(&parent.dependant_set_mask); - let effective_impulse = - (new_impulse - self.impulse).component_mul(&parent.dependant_set_mask); + mj_lambda2.linear -= parent.im2 * effective_impulse; + mj_lambda2.angular -= parent + .ii2_sqrt + .transform_vector(parent.r2.gcross(effective_impulse)); + } - let lin_impulse = parent.basis * effective_impulse.fixed_rows::<Dim>(0).into_owned(); - #[cfg(feature = "dim2")] - let ang_impulse = parent.basis * effective_impulse[2]; - #[cfg(feature = "dim3")] - let ang_impulse = parent.basis * effective_impulse.fixed_rows::<U3>(3).into_owned(); + /* + * + * Solve rotations. + * + */ + { + let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); - mj_lambda1.linear += parent.im1 * lin_impulse; - mj_lambda1.angular += parent - .ii1_sqrt - .transform_vector(ang_impulse + parent.r1.gcross(lin_impulse)); + let dvel = parent.basis.inverse_transform_vector(&ang_vel2); + let err = dvel + self.rhs_ang; - mj_lambda2.linear -= parent.im2 * lin_impulse; - mj_lambda2.angular -= parent - .ii2_sqrt - .transform_vector(ang_impulse + parent.r2.gcross(lin_impulse)); - - for i in 3..6 { - // Solve the independent 1D constraints. - if parent.dependant_set_mask[i] == 0.0 - && (self.min_impulse[i] != 0.0 || self.max_impulse[i] != 0.0) - { - let constraint_axis = parent.basis * Vector::ith(i % 3, 1.0); - let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); - let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular); - - let dvel = if i < 3 { - (mj_lambda2.linear + ang_vel2.gcross(parent.r2) - - mj_lambda1.linear - - ang_vel1.gcross(parent.r1)) - .dot(&constraint_axis) - + self.rhs[i] - } else { - (ang_vel2 - ang_vel1).dot(&constraint_axis) + self.rhs[i] - }; - - let inv_lhs = parent.inv_lhs[(i, i)]; - - new_impulse[i] = na::clamp( - self.impulse[i] + inv_lhs * dvel, - self.min_impulse[i], - self.max_impulse[i], - ); + new_ang_impulse = (self.ang_impulse + parent.inv_lhs_ang * err) + .sup(&self.min_ang_impulse) + .inf(&self.max_ang_impulse); + let effective_impulse = parent.basis * (new_ang_impulse - self.ang_impulse); - let effective_impulse = new_impulse[i] - self.impulse[i]; - let impulse = constraint_axis * effective_impulse; - - if i < 3 { - mj_lambda1.linear += parent.im1 * impulse; - mj_lambda1.angular += - parent.ii1_sqrt.transform_vector(parent.r1.gcross(impulse)); - mj_lambda2.linear -= parent.im2 * impulse; - mj_lambda2.angular -= - parent.ii2_sqrt.transform_vector(parent.r2.gcross(impulse)); - } else { - mj_lambda1.angular += parent.ii1_sqrt.transform_vector(impulse); - mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(impulse); - } - } + mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse); } - new_impulse + (new_lin_impulse, new_ang_impulse) } } |