From 0703e5527fd95d86bb6621e61dbcb1a6e7f9329a Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 16 Jan 2022 16:40:59 +0100 Subject: Fix some solver issues - Fix the wrong codepath taken by the solver for contacts involving a collider without parent. - Properly adress the non-linear treatment of the friction direction - Simplify the sleeping strategy - Add an impulse resolution multiplier --- src/dynamics/solver/categorization.rs | 16 +- src/dynamics/solver/delta_vel.rs | 13 +- src/dynamics/solver/generic_velocity_constraint.rs | 60 ++++- .../solver/generic_velocity_constraint_element.rs | 29 ++- .../solver/generic_velocity_ground_constraint.rs | 242 +++++++++++++++++++++ .../generic_velocity_ground_constraint_element.rs | 141 ++++++++++++ src/dynamics/solver/island_solver.rs | 5 +- src/dynamics/solver/mod.rs | 3 + src/dynamics/solver/solver_constraints.rs | 56 ++++- src/dynamics/solver/velocity_constraint.rs | 24 +- src/dynamics/solver/velocity_constraint_element.rs | 39 ++-- src/dynamics/solver/velocity_constraint_wide.rs | 27 ++- src/dynamics/solver/velocity_ground_constraint.rs | 22 +- .../solver/velocity_ground_constraint_element.rs | 40 ++-- .../solver/velocity_ground_constraint_wide.rs | 23 +- src/dynamics/solver/velocity_solver.rs | 4 +- 16 files changed, 650 insertions(+), 94 deletions(-) create mode 100644 src/dynamics/solver/generic_velocity_ground_constraint.rs create mode 100644 src/dynamics/solver/generic_velocity_ground_constraint_element.rs (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index c5b2601..0083388 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -9,8 +9,8 @@ pub(crate) fn categorize_contacts( manifold_indices: &[ContactManifoldIndex], out_ground: &mut Vec, out_not_ground: &mut Vec, - out_generic: &mut Vec, - _unused: &mut Vec, // Unused but useful to simplify the parallel code. + out_generic_ground: &mut Vec, + out_generic_not_ground: &mut Vec, ) { for manifold_i in manifold_indices { let manifold = &manifolds[*manifold_i]; @@ -18,15 +18,19 @@ pub(crate) fn categorize_contacts( if manifold .data .rigid_body1 - .map(|rb| multibody_joints.rigid_body_link(rb)) + .and_then(|h| multibody_joints.rigid_body_link(h)) .is_some() || manifold .data - .rigid_body1 - .map(|rb| multibody_joints.rigid_body_link(rb)) + .rigid_body2 + .and_then(|h| multibody_joints.rigid_body_link(h)) .is_some() { - out_generic.push(*manifold_i); + if manifold.data.relative_dominance != 0 { + out_generic_ground.push(*manifold_i); + } else { + out_generic_not_ground.push(*manifold_i); + } } else if manifold.data.relative_dominance != 0 { out_ground.push(*manifold_i) } else { diff --git a/src/dynamics/solver/delta_vel.rs b/src/dynamics/solver/delta_vel.rs index 9160f2e..697fd24 100644 --- a/src/dynamics/solver/delta_vel.rs +++ b/src/dynamics/solver/delta_vel.rs @@ -1,7 +1,7 @@ use crate::math::{AngVector, Vector, SPATIAL_DIM}; use na::{DVectorSlice, DVectorSliceMut}; use na::{Scalar, SimdRealField}; -use std::ops::AddAssign; +use std::ops::{AddAssign, Sub}; #[derive(Copy, Clone, Debug)] #[repr(C)] @@ -44,3 +44,14 @@ impl AddAssign for DeltaVel { self.angular += rhs.angular; } } + +impl Sub for DeltaVel { + type Output = Self; + + fn sub(self, rhs: Self) -> Self { + DeltaVel { + linear: self.linear - rhs.linear, + angular: self.angular - rhs.angular, + } + } +} diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs index 8c93511..f1ab0ea 100644 --- a/src/dynamics/solver/generic_velocity_constraint.rs +++ b/src/dynamics/solver/generic_velocity_constraint.rs @@ -9,10 +9,60 @@ use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::{WAngularInertia, WCross, WDot}; use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart}; +use crate::dynamics::solver::GenericVelocityGroundConstraint; #[cfg(feature = "dim2")] use crate::utils::WBasis; use na::DVector; +#[derive(Copy, Clone, Debug)] +pub(crate) enum AnyGenericVelocityConstraint { + NongroupedGround(GenericVelocityGroundConstraint), + Nongrouped(GenericVelocityConstraint), +} + +impl AnyGenericVelocityConstraint { + pub fn solve( + &mut self, + jacobians: &DVector, + mj_lambdas: &mut [DeltaVel], + generic_mj_lambdas: &mut DVector, + solve_restitution: bool, + solve_friction: bool, + ) { + match self { + AnyGenericVelocityConstraint::Nongrouped(c) => c.solve( + jacobians, + mj_lambdas, + generic_mj_lambdas, + solve_restitution, + solve_friction, + ), + AnyGenericVelocityConstraint::NongroupedGround(c) => c.solve( + jacobians, + generic_mj_lambdas, + solve_restitution, + solve_friction, + ), + } + } + + pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { + match self { + AnyGenericVelocityConstraint::Nongrouped(c) => c.writeback_impulses(manifolds_all), + AnyGenericVelocityConstraint::NongroupedGround(c) => { + c.writeback_impulses(manifolds_all) + } + } + } + + pub fn remove_bias_from_rhs(&mut self) { + match self { + AnyGenericVelocityConstraint::Nongrouped(c) => c.remove_bias_from_rhs(), + AnyGenericVelocityConstraint::NongroupedGround(c) => c.remove_bias_from_rhs(), + } + } +} + #[derive(Copy, Clone, Debug)] pub(crate) struct GenericVelocityConstraint { // We just build the generic constraint on top of the velocity constraint, @@ -31,7 +81,7 @@ impl GenericVelocityConstraint { manifold: &ContactManifold, bodies: &Bodies, multibodies: &MultibodyJointSet, - out_constraints: &mut Vec, + out_constraints: &mut Vec, jacobians: &mut DVector, jacobian_id: &mut usize, push: bool, @@ -293,6 +343,9 @@ impl GenericVelocityConstraint { (vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]); constraint.elements[k].tangent_part.rhs[j] = rhs; + // FIXME: in 3D, we should take into account gcross[0].dot(gcross[1]) + // in lhs. See the corresponding code on the `velocity_constraint.rs` + // file. constraint.elements[k].tangent_part.r[j] = r; } } @@ -316,9 +369,10 @@ impl GenericVelocityConstraint { }; if push { - out_constraints.push(constraint); + out_constraints.push(AnyGenericVelocityConstraint::Nongrouped(constraint)); } else { - out_constraints[manifold.data.constraint_index + _l] = constraint; + out_constraints[manifold.data.constraint_index + _l] = + AnyGenericVelocityConstraint::Nongrouped(constraint); } } } diff --git a/src/dynamics/solver/generic_velocity_constraint_element.rs b/src/dynamics/solver/generic_velocity_constraint_element.rs index c31bbfb..e75dd01 100644 --- a/src/dynamics/solver/generic_velocity_constraint_element.rs +++ b/src/dynamics/solver/generic_velocity_constraint_element.rs @@ -41,7 +41,7 @@ fn tangent_j_id(j_id: usize, ndofs1: usize, ndofs2: usize) -> usize { impl GenericRhs { #[inline(always)] - fn dimpulse( + fn dvel( &self, j_id: usize, ndofs: usize, @@ -110,14 +110,14 @@ impl VelocityConstraintTangentPart { #[cfg(feature = "dim2")] { - let dimpulse_0 = mj_lambda1.dimpulse( + let dvel_0 = mj_lambda1.dvel( j_id1, ndofs1, jacobians, &tangents1[0], &self.gcross1[0], mj_lambdas, - ) + mj_lambda2.dimpulse( + ) + mj_lambda2.dvel( j_id2, ndofs2, jacobians, @@ -126,7 +126,7 @@ impl VelocityConstraintTangentPart { mj_lambdas, ) + self.rhs[0]; - let new_impulse = (self.impulse[0] - self.r[0] * dimpulse_0).simd_clamp(-limit, limit); + let new_impulse = (self.impulse[0] - self.r[0] * dvel_0).simd_clamp(-limit, limit); let dlambda = new_impulse - self.impulse[0]; self.impulse[0] = new_impulse; @@ -154,14 +154,14 @@ impl VelocityConstraintTangentPart { #[cfg(feature = "dim3")] { - let dimpulse_0 = mj_lambda1.dimpulse( + let dvel_0 = mj_lambda1.dvel( j_id1, ndofs1, jacobians, &tangents1[0], &self.gcross1[0], mj_lambdas, - ) + mj_lambda2.dimpulse( + ) + mj_lambda2.dvel( j_id2, ndofs2, jacobians, @@ -169,14 +169,14 @@ impl VelocityConstraintTangentPart { &self.gcross2[0], mj_lambdas, ) + self.rhs[0]; - let dimpulse_1 = mj_lambda1.dimpulse( + let dvel_1 = mj_lambda1.dvel( j_id1 + j_step, ndofs1, jacobians, &tangents1[1], &self.gcross1[1], mj_lambdas, - ) + mj_lambda2.dimpulse( + ) + mj_lambda2.dvel( j_id2 + j_step, ndofs2, jacobians, @@ -186,8 +186,8 @@ impl VelocityConstraintTangentPart { ) + self.rhs[1]; let new_impulse = na::Vector2::new( - self.impulse[0] - self.r[0] * dimpulse_0, - self.impulse[1] - self.r[1] * dimpulse_1, + self.impulse[0] - self.r[0] * dvel_0, + self.impulse[1] - self.r[1] * dvel_1, ); let new_impulse = new_impulse.cap_magnitude(limit); @@ -257,12 +257,11 @@ impl VelocityConstraintNormalPart { let j_id1 = j_id1(j_id, ndofs1, ndofs2); let j_id2 = j_id2(j_id, ndofs1, ndofs2); - let dimpulse = - mj_lambda1.dimpulse(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, mj_lambdas) - + mj_lambda2.dimpulse(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas) - + self.rhs; + let dvel = mj_lambda1.dvel(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, mj_lambdas) + + mj_lambda2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas) + + self.rhs; - let new_impulse = (self.impulse - self.r * dimpulse).max(0.0); + let new_impulse = (self.impulse - self.r * dvel).max(0.0); let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs new file mode 100644 index 0000000..c9b2c3f --- /dev/null +++ b/src/dynamics/solver/generic_velocity_ground_constraint.rs @@ -0,0 +1,242 @@ +use crate::data::{BundleSet, ComponentSet}; +use crate::dynamics::solver::VelocityGroundConstraint; +use crate::dynamics::{ + IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodyType, + RigidBodyVelocity, +}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS}; +use crate::utils::WCross; + +use super::{VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart}; +use crate::dynamics::solver::AnyGenericVelocityConstraint; +#[cfg(feature = "dim2")] +use crate::utils::WBasis; +use na::DVector; + +#[derive(Copy, Clone, Debug)] +pub(crate) struct GenericVelocityGroundConstraint { + // We just build the generic constraint on top of the velocity constraint, + // adding some information we can use in the generic case. + pub velocity_constraint: VelocityGroundConstraint, + pub j_id: usize, + pub ndofs2: usize, +} + +impl GenericVelocityGroundConstraint { + pub fn generate( + params: &IntegrationParameters, + manifold_id: ContactManifoldIndex, + manifold: &ContactManifold, + bodies: &Bodies, + multibodies: &MultibodyJointSet, + out_constraints: &mut Vec, + jacobians: &mut DVector, + jacobian_id: &mut usize, + push: bool, + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { + let inv_dt = params.inv_dt(); + let erp_inv_dt = params.erp_inv_dt(); + + let mut handle1 = manifold.data.rigid_body1; + let mut handle2 = manifold.data.rigid_body2; + let flipped = manifold.data.relative_dominance < 0; + + let (force_dir1, flipped_multiplier) = if flipped { + std::mem::swap(&mut handle1, &mut handle2); + (manifold.data.normal, -1.0) + } else { + (-manifold.data.normal, 1.0) + }; + + let (rb_vels1, world_com1) = if let Some(handle1) = handle1 { + let (vels1, mprops1): (&RigidBodyVelocity, &RigidBodyMassProps) = + bodies.index_bundle(handle1.0); + (*vels1, mprops1.world_com) + } else { + (RigidBodyVelocity::zero(), Point::origin()) + }; + + let (rb_vels2, rb_mprops2): (&RigidBodyVelocity, &RigidBodyMassProps) = + bodies.index_bundle(handle2.unwrap().0); + + let (mb2, link_id2) = handle2 + .and_then(|h| multibodies.rigid_body_link(h)) + .map(|m| (&multibodies[m.multibody], m.id)) + .unwrap(); + let mj_lambda2 = mb2.solver_id; + + #[cfg(feature = "dim2")] + let tangents1 = force_dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let tangents1 = super::compute_tangent_contact_directions( + &force_dir1, + &rb_vels1.linvel, + &rb_vels2.linvel, + ); + + let multibodies_ndof = mb2.ndofs(); + // For each solver contact we generate DIM constraints, and each constraints appends + // the multibodies jacobian and weighted jacobians + let required_jacobian_len = + *jacobian_id + manifold.data.solver_contacts.len() * multibodies_ndof * 2 * DIM; + + if jacobians.nrows() < required_jacobian_len { + jacobians.resize_vertically_mut(required_jacobian_len, 0.0); + } + + for (_l, manifold_points) in manifold + .data + .solver_contacts + .chunks(MAX_MANIFOLD_POINTS) + .enumerate() + { + let chunk_j_id = *jacobian_id; + let mut constraint = VelocityGroundConstraint { + dir1: force_dir1, + #[cfg(feature = "dim3")] + tangent1: tangents1[0], + elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], + im2: rb_mprops2.effective_inv_mass, + limit: 0.0, + mj_lambda2, + manifold_id, + manifold_contact_id: [0; MAX_MANIFOLD_POINTS], + num_contacts: manifold_points.len() as u8, + }; + + for k in 0..manifold_points.len() { + let manifold_point = &manifold_points[k]; + let dp1 = manifold_point.point - world_com1; + let dp2 = manifold_point.point - rb_mprops2.world_com; + + let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(dp1); + let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(dp2); + + constraint.limit = manifold_point.friction; + constraint.manifold_contact_id[k] = manifold_point.contact_id; + + // Normal part. + { + let torque_dir2 = dp2.gcross(-force_dir1); + let inv_r2 = mb2 + .fill_jacobians( + link_id2, + -force_dir1, + #[cfg(feature = "dim2")] + na::vector!(torque_dir2), + #[cfg(feature = "dim3")] + torque_dir2, + jacobian_id, + jacobians, + ) + .0; + + let r = crate::utils::inv(inv_r2); + + let is_bouncy = manifold_point.is_bouncy() as u32 as Real; + let is_resting = 1.0 - is_bouncy; + + let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) + * (vel1 - vel2).dot(&force_dir1); + rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt; + rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction; + let rhs_bias = + /* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0); + + constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart { + gcross2: na::zero(), // Unused for generic constraints. + rhs: rhs_wo_bias + rhs_bias, + rhs_wo_bias, + impulse: na::zero(), + r, + }; + } + + // Tangent parts. + { + constraint.elements[k].tangent_part.impulse = na::zero(); + + for j in 0..DIM - 1 { + let torque_dir2 = dp2.gcross(-tangents1[j]); + let inv_r2 = mb2 + .fill_jacobians( + link_id2, + -tangents1[j], + #[cfg(feature = "dim2")] + na::vector![torque_dir2], + #[cfg(feature = "dim3")] + torque_dir2, + jacobian_id, + jacobians, + ) + .0; + + let r = crate::utils::inv(inv_r2); + + let rhs = (vel1 - vel2 + + flipped_multiplier * manifold_point.tangent_velocity) + .dot(&tangents1[j]); + + constraint.elements[k].tangent_part.rhs[j] = rhs; + + // FIXME: in 3D, we should take into account gcross[0].dot(gcross[1]) + // in lhs. See the corresponding code on the `velocity_constraint.rs` + // file. + constraint.elements[k].tangent_part.r[j] = r; + } + } + } + + let constraint = GenericVelocityGroundConstraint { + velocity_constraint: constraint, + j_id: chunk_j_id, + ndofs2: mb2.ndofs(), + }; + + if push { + out_constraints.push(AnyGenericVelocityConstraint::NongroupedGround(constraint)); + } else { + out_constraints[manifold.data.constraint_index + _l] = + AnyGenericVelocityConstraint::NongroupedGround(constraint); + } + } + } + + pub fn solve( + &mut self, + jacobians: &DVector, + generic_mj_lambdas: &mut DVector, + solve_restitution: bool, + solve_friction: bool, + ) { + let mj_lambda2 = self.velocity_constraint.mj_lambda2 as usize; + + let elements = &mut self.velocity_constraint.elements + [..self.velocity_constraint.num_contacts as usize]; + VelocityGroundConstraintElement::generic_solve_group( + elements, + jacobians, + self.velocity_constraint.limit, + self.ndofs2, + self.j_id, + mj_lambda2, + generic_mj_lambdas, + solve_restitution, + solve_friction, + ); + } + + pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { + self.velocity_constraint.writeback_impulses(manifolds_all); + } + + pub fn remove_bias_from_rhs(&mut self) { + self.velocity_constraint.remove_bias_from_rhs(); + } +} diff --git a/src/dynamics/solver/generic_velocity_ground_constraint_element.rs b/src/dynamics/solver/generic_velocity_ground_constraint_element.rs new file mode 100644 index 0000000..80c97ab --- /dev/null +++ b/src/dynamics/solver/generic_velocity_ground_constraint_element.rs @@ -0,0 +1,141 @@ +use crate::dynamics::solver::{ + VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart, + VelocityGroundConstraintTangentPart, +}; +use crate::math::{Real, DIM}; +use na::DVector; +#[cfg(feature = "dim2")] +use na::SimdPartialOrd; + +impl VelocityGroundConstraintTangentPart { + #[inline] + pub fn generic_solve( + &mut self, + j_id2: usize, + jacobians: &DVector, + ndofs2: usize, + limit: Real, + mj_lambda2: usize, + mj_lambdas: &mut DVector, + ) { + #[cfg(feature = "dim2")] + { + let dvel_0 = jacobians + .rows(j_id2, ndofs2) + .dot(&mj_lambdas.rows(mj_lambda2, ndofs2)) + + self.rhs[0]; + + let new_impulse = (self.impulse[0] - self.r[0] * dvel_0).simd_clamp(-limit, limit); + let dlambda = new_impulse - self.impulse[0]; + self.impulse[0] = new_impulse; + + mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy( + dlambda, + &jacobians.rows(j_id2 + ndofs2, ndofs2), + 1.0, + ); + } + + #[cfg(feature = "dim3")] + { + let j_step = ndofs2 * 2; + let dvel_0 = jacobians + .rows(j_id2, ndofs2) + .dot(&mj_lambdas.rows(mj_lambda2, ndofs2)) + + self.rhs[0]; + let dvel_1 = jacobians + .rows(j_id2 + j_step, ndofs2) + .dot(&mj_lambdas.rows(mj_lambda2, ndofs2)) + + self.rhs[1]; + + let new_impulse = na::Vector2::new( + self.impulse[0] - self.r[0] * dvel_0, + self.impulse[1] - self.r[1] * dvel_1, + ); + let new_impulse = new_impulse.cap_magnitude(limit); + + let dlambda = new_impulse - self.impulse; + self.impulse = new_impulse; + + mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy( + dlambda[0], + &jacobians.rows(j_id2 + ndofs2, ndofs2), + 1.0, + ); + mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy( + dlambda[1], + &jacobians.rows(j_id2 + j_step + ndofs2, ndofs2), + 1.0, + ); + } + } +} + +impl VelocityGroundConstraintNormalPart { + #[inline] + pub fn generic_solve( + &mut self, + j_id2: usize, + jacobians: &DVector, + ndofs2: usize, + mj_lambda2: usize, + mj_lambdas: &mut DVector, + ) { + let dvel = jacobians + .rows(j_id2, ndofs2) + .dot(&mj_lambdas.rows(mj_lambda2, ndofs2)) + + self.rhs; + + let new_impulse = (self.impulse - self.r * dvel).max(0.0); + let dlambda = new_impulse - self.impulse; + self.impulse = new_impulse; + + mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy( + dlambda, + &jacobians.rows(j_id2 + ndofs2, ndofs2), + 1.0, + ); + } +} + +impl VelocityGroundConstraintElement { + #[inline] + pub fn generic_solve_group( + elements: &mut [Self], + jacobians: &DVector, + limit: Real, + ndofs2: usize, + // Jacobian index of the first constraint. + j_id: usize, + mj_lambda2: usize, + mj_lambdas: &mut DVector, + solve_restitution: bool, + solve_friction: bool, + ) { + let j_step = ndofs2 * 2 * DIM; + + // Solve penetration. + if solve_restitution { + let mut nrm_j_id = j_id; + + for element in elements.iter_mut() { + element + .normal_part + .generic_solve(nrm_j_id, jacobians, ndofs2, mj_lambda2, mj_lambdas); + nrm_j_id += j_step; + } + } + + // Solve friction. + if solve_friction { + let mut tng_j_id = j_id + ndofs2 * 2; + + for element in elements.iter_mut() { + let limit = limit * element.normal_part.impulse; + let part = &mut element.tangent_part; + part.generic_solve(tng_j_id, jacobians, ndofs2, limit, mj_lambda2, mj_lambdas); + tng_j_id += j_step; + } + } + } +} diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index aeb2698..9f82182 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -2,7 +2,8 @@ use super::VelocitySolver; use crate::counters::Counters; use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; use crate::dynamics::solver::{ - AnyJointVelocityConstraint, AnyVelocityConstraint, GenericVelocityConstraint, SolverConstraints, + AnyGenericVelocityConstraint, AnyJointVelocityConstraint, AnyVelocityConstraint, + SolverConstraints, }; use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces, @@ -13,7 +14,7 @@ use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::prelude::{MultibodyJointSet, RigidBodyActivation}; pub struct IslandSolver { - contact_constraints: SolverConstraints, + contact_constraints: SolverConstraints, joint_constraints: SolverConstraints, velocity_solver: VelocitySolver, } diff --git a/src/dynamics/solver/mod.rs b/src/dynamics/solver/mod.rs index 7607c28..3ffa94c 100644 --- a/src/dynamics/solver/mod.rs +++ b/src/dynamics/solver/mod.rs @@ -13,6 +13,7 @@ pub(self) use self::velocity_solver::VelocitySolver; pub(self) use delta_vel::DeltaVel; pub(self) use generic_velocity_constraint::*; pub(self) use generic_velocity_constraint_element::*; +pub(self) use generic_velocity_ground_constraint::*; pub(self) use interaction_groups::*; pub(crate) use joint_constraint::MotorParameters; pub use joint_constraint::*; @@ -29,6 +30,8 @@ mod categorization; mod delta_vel; mod generic_velocity_constraint; mod generic_velocity_constraint_element; +mod generic_velocity_ground_constraint; +mod generic_velocity_ground_constraint_element; mod interaction_groups; #[cfg(not(feature = "parallel"))] mod island_solver; diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs index cfd7575..2e92a5e 100644 --- a/src/dynamics/solver/solver_constraints.rs +++ b/src/dynamics/solver/solver_constraints.rs @@ -5,6 +5,8 @@ use super::{ use super::{WVelocityConstraint, WVelocityGroundConstraint}; use crate::data::ComponentSet; use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints}; +use crate::dynamics::solver::generic_velocity_ground_constraint::GenericVelocityGroundConstraint; +use crate::dynamics::solver::AnyGenericVelocityConstraint; use crate::dynamics::solver::GenericVelocityConstraint; use crate::dynamics::{ solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, @@ -58,7 +60,7 @@ impl } } -impl SolverConstraints { +impl SolverConstraints { pub fn init_constraint_groups( &mut self, island_id: usize, @@ -82,8 +84,8 @@ impl SolverConstraints { manifold_indices, &mut self.ground_interactions, &mut self.not_ground_interactions, - &mut self.generic_not_ground_interactions, &mut self.generic_ground_interactions, + &mut self.generic_not_ground_interactions, ); self.interaction_groups.clear_groups(); @@ -141,18 +143,32 @@ impl SolverConstraints { manifold_indices, ); + let mut jacobian_id = 0; #[cfg(feature = "simd-is-enabled")] { self.compute_grouped_constraints(params, bodies, manifolds); } self.compute_nongrouped_constraints(params, bodies, manifolds); - self.compute_generic_constraints(params, bodies, multibody_joints, manifolds); + self.compute_generic_constraints( + params, + bodies, + multibody_joints, + manifolds, + &mut jacobian_id, + ); #[cfg(feature = "simd-is-enabled")] { self.compute_grouped_ground_constraints(params, bodies, manifolds); } self.compute_nongrouped_ground_constraints(params, bodies, manifolds); + self.compute_generic_ground_constraints( + params, + bodies, + multibody_joints, + manifolds, + &mut jacobian_id, + ); } #[cfg(feature = "simd-is-enabled")] @@ -215,6 +231,7 @@ impl SolverConstraints { bodies: &Bodies, multibody_joints: &MultibodyJointSet, manifolds_all: &[&mut ContactManifold], + jacobian_id: &mut usize, ) where Bodies: ComponentSet + ComponentSet @@ -222,7 +239,6 @@ impl SolverConstraints { + ComponentSet + ComponentSet, { - let mut jacobian_id = 0; for manifold_i in &self.generic_not_ground_interactions { let manifold = &manifolds_all[*manifold_i]; GenericVelocityConstraint::generate( @@ -233,7 +249,37 @@ impl SolverConstraints { multibody_joints, &mut self.generic_velocity_constraints, &mut self.generic_jacobians, - &mut jacobian_id, + jacobian_id, + true, + ); + } + } + + fn compute_generic_ground_constraints( + &mut self, + params: &IntegrationParameters, + bodies: &Bodies, + multibody_joints: &MultibodyJointSet, + manifolds_all: &[&mut ContactManifold], + jacobian_id: &mut usize, + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { + for manifold_i in &self.generic_ground_interactions { + let manifold = &manifolds_all[*manifold_i]; + GenericVelocityGroundConstraint::generate( + params, + *manifold_i, + manifold, + bodies, + multibody_joints, + &mut self.generic_velocity_constraints, + &mut self.generic_jacobians, + jacobian_id, true, ); } diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 0a841b8..6a95492 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -236,7 +236,7 @@ impl VelocityConstraint { .transform_vector(dp2.gcross(-force_dir1)); let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass; - let r = 1.0 + let r = params.delassus_inv_factor / (force_dir1.dot(&imsum.component_mul(&force_dir1)) + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); @@ -246,8 +246,7 @@ impl VelocityConstraint { let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1); - rhs_wo_bias += - (manifold_point.dist + params.allowed_linear_error).max(0.0) * inv_dt; + rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt; rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction; let rhs_bias = /* is_resting * */ erp_inv_dt @@ -275,17 +274,26 @@ impl VelocityConstraint { .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-tangents1[j])); let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass; - let r = 1.0 - / (tangents1[j].dot(&imsum.component_mul(&tangents1[j])) - + gcross1.gdot(gcross1) - + gcross2.gdot(gcross2)); + let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j])) + + gcross1.gdot(gcross1) + + gcross2.gdot(gcross2); let rhs = (vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]); constraint.elements[k].tangent_part.gcross1[j] = gcross1; constraint.elements[k].tangent_part.gcross2[j] = gcross2; constraint.elements[k].tangent_part.rhs[j] = rhs; - constraint.elements[k].tangent_part.r[j] = r; + constraint.elements[k].tangent_part.r[j] = + if cfg!(feature = "dim2") { 1.0 / r } else { r }; + } + + #[cfg(feature = "dim3")] + { + constraint.elements[k].tangent_part.r[2] = 2.0 + * (constraint.elements[k].tangent_part.gcross1[0] + .gdot(constraint.elements[k].tangent_part.gcross1[1]) + + constraint.elements[k].tangent_part.gcross2[0] + .gdot(constraint.elements[k].tangent_part.gcross2[1])); } } } diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs index e153626..b0f8087 100644 --- a/src/dynamics/solver/velocity_constraint_element.rs +++ b/src/dynamics/solver/velocity_constraint_element.rs @@ -12,7 +12,10 @@ pub(crate) struct VelocityConstraintTangentPart { pub impulse: na::Vector1, #[cfg(feature = "dim3")] pub impulse: na::Vector2, - pub r: [N; DIM - 1], + #[cfg(feature = "dim2")] + pub r: [N; 1], + #[cfg(feature = "dim3")] + pub r: [N; DIM], } impl VelocityConstraintTangentPart { @@ -22,7 +25,10 @@ impl VelocityConstraintTangentPart { gcross2: [na::zero(); DIM - 1], rhs: [na::zero(); DIM - 1], impulse: na::zero(), - r: [na::zero(); DIM - 1], + #[cfg(feature = "dim2")] + r: [na::zero(); 1], + #[cfg(feature = "dim3")] + r: [na::zero(); DIM], } } @@ -41,12 +47,12 @@ impl VelocityConstraintTangentPart { { #[cfg(feature = "dim2")] { - let dimpulse = tangents1[0].dot(&mj_lambda1.linear) + let dvel = tangents1[0].dot(&mj_lambda1.linear) + self.gcross1[0].gdot(mj_lambda1.angular) - tangents1[0].dot(&mj_lambda2.linear) + self.gcross2[0].gdot(mj_lambda2.angular) + self.rhs[0]; - let new_impulse = (self.impulse[0] - self.r[0] * dimpulse).simd_clamp(-limit, limit); + let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit); let dlambda = new_impulse - self.impulse[0]; self.impulse[0] = new_impulse; @@ -59,25 +65,30 @@ impl VelocityConstraintTangentPart { #[cfg(feature = "dim3")] { - let dimpulse_0 = tangents1[0].dot(&mj_lambda1.linear) + let dvel_0 = tangents1[0].dot(&mj_lambda1.linear) + self.gcross1[0].gdot(mj_lambda1.angular) - tangents1[0].dot(&mj_lambda2.linear) + self.gcross2[0].gdot(mj_lambda2.angular) + self.rhs[0]; - let dimpulse_1 = tangents1[1].dot(&mj_lambda1.linear) + let dvel_1 = tangents1[1].dot(&mj_lambda1.linear) + self.gcross1[1].gdot(mj_lambda1.angular) - tangents1[1].dot(&mj_lambda2.linear) + self.gcross2[1].gdot(mj_lambda2.angular) + self.rhs[1]; - let new_impulse = na::Vector2::new( - self.impulse[0] - self.r[0] * dimpulse_0, - self.impulse[1] - self.r[1] * dimpulse_1, - ); + let dvel_00 = dvel_0 * dvel_0; + let dvel_11 = dvel_1 * dvel_1; + let dvel_01 = dvel_0 * dvel_1; + let inv_lhs = (dvel_00 + dvel_11) + * crate::utils::simd_inv( + dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2], + ); + let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1]; + let new_impulse = self.impulse - delta_impulse; let new_impulse = { let _disable_fe_except = - crate::utils::DisableFloatingPointExceptionsFlags:: - disable_floating_point_exceptions(); + crate::utils::DisableFloatingPointExceptionsFlags:: + disable_floating_point_exceptions(); new_impulse.simd_cap_magnitude(limit) }; @@ -128,11 +139,11 @@ impl VelocityConstraintNormalPart { ) where AngVector: WDot, Result = N>, { - let dimpulse = dir1.dot(&mj_lambda1.linear) + self.gcross1.gdot(mj_lambda1.angular) + let dvel = dir1.dot(&mj_lambda1.linear) + self.gcross1.gdot(mj_lambda1.angular) - dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs; - let new_impulse = (self.impulse - self.r * dimpulse).simd_max(N::zero()); + let new_impulse = (self.impulse - self.r * dvel).simd_max(N::zero()); let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index f072ad8..7fcb7f4 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -49,6 +49,7 @@ impl WVelocityConstraint { let inv_dt = SimdReal::splat(params.inv_dt()); let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction); let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); + let delassus_inv_factor = SimdReal::splat(params.delassus_inv_factor); let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()]; @@ -136,14 +137,14 @@ impl WVelocityConstraint { let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); let imsum = im1 + im2; - let r = SimdReal::splat(1.0) + let r = delassus_inv_factor / (force_dir1.dot(&imsum.component_mul(&force_dir1)) + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); let projected_velocity = (vel1 - vel2).dot(&force_dir1); let mut rhs_wo_bias = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity; - rhs_wo_bias += (dist + allowed_lin_err).simd_max(SimdReal::zero()) * inv_dt; + rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt; rhs_wo_bias *= is_bouncy + is_resting * velocity_solve_fraction; let rhs_bias = (dist + allowed_lin_err).simd_min(SimdReal::zero()) * (erp_inv_dt/* * is_resting */); @@ -165,16 +166,28 @@ impl WVelocityConstraint { let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j])); let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); let imsum = im1 + im2; - let r = SimdReal::splat(1.0) - / (tangents1[j].dot(&imsum.component_mul(&tangents1[j])) - + gcross1.gdot(gcross1) - + gcross2.gdot(gcross2)); + let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j])) + + gcross1.gdot(gcross1) + + gcross2.gdot(gcross2); let rhs = (vel1 - vel2 + tangent_velocity).dot(&tangents1[j]); constraint.elements[k].tangent_part.gcross1[j] = gcross1; constraint.elements[k].tangent_part.gcross2[j] = gcross2; constraint.elements[k].tangent_part.rhs[j] = rhs; - constraint.elements[k].tangent_part.r[j] = r; + constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") { + SimdReal::splat(1.0) / r + } else { + r + }; + } + + #[cfg(feature = "dim3")] + { + constraint.elements[k].tangent_part.r[2] = SimdReal::splat(2.0) + * (constraint.elements[k].tangent_part.gcross1[0] + .gdot(constraint.elements[k].tangent_part.gcross1[1]) + + constraint.elements[k].tangent_part.gcross2[0] + .gdot(constraint.elements[k].tangent_part.gcross2[1])); } } diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index bd60633..76de3f9 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -153,7 +153,7 @@ impl VelocityGroundConstraint { .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-force_dir1)); - let r = 1.0 + let r = params.delassus_inv_factor / (force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) + gcross2.gdot(gcross2)); @@ -162,8 +162,7 @@ impl VelocityGroundConstraint { let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1); - rhs_wo_bias += - (manifold_point.dist + params.allowed_linear_error).max(0.0) * inv_dt; + rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt; rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction; let rhs_bias = /* is_resting * */ erp_inv_dt @@ -186,17 +185,24 @@ impl VelocityGroundConstraint { let gcross2 = mprops2 .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-tangents1[j])); - let r = 1.0 - / (tangents1[j] - .dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j])) - + gcross2.gdot(gcross2)); + let r = tangents1[j] + .dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j])) + + gcross2.gdot(gcross2); let rhs = (vel1 - vel2 + flipped_multiplier * manifold_point.tangent_velocity) .dot(&tangents1[j]); constraint.elements[k].tangent_part.gcross2[j] = gcross2; constraint.elements[k].tangent_part.rhs[j] = rhs; - constraint.elements[k].tangent_part.r[j] = r; + constraint.elements[k].tangent_part.r[j] = + if cfg!(feature = "dim2") { 1.0 / r } else { r }; + } + + #[cfg(feature = "dim3")] + { + constraint.elements[k].tangent_part.r[2] = 2.0 + * constraint.elements[k].tangent_part.gcross2[0] + .gdot(constraint.elements[k].tangent_part.gcross2[1]); } } } diff --git a/src/dynamics/solver/velocity_ground_constraint_element.rs b/src/dynamics/solver/velocity_ground_constraint_element.rs index 4d4d3c3..a843905 100644 --- a/src/dynamics/solver/velocity_ground_constraint_element.rs +++ b/src/dynamics/solver/velocity_ground_constraint_element.rs @@ -11,17 +11,22 @@ pub(crate) struct VelocityGroundConstraintTangentPart { pub impulse: na::Vector1, #[cfg(feature = "dim3")] pub impulse: na::Vector2, - pub r: [N; DIM - 1], + #[cfg(feature = "dim2")] + pub r: [N; 1], + #[cfg(feature = "dim3")] + pub r: [N; DIM], } impl VelocityGroundConstraintTangentPart { - #[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))] fn zero() -> Self { Self { gcross2: [na::zero(); DIM - 1], rhs: [na::zero(); DIM - 1], impulse: na::zero(), - r: [na::zero(); DIM - 1], + #[cfg(feature = "dim2")] + r: [na::zero(); 1], + #[cfg(feature = "dim3")] + r: [na::zero(); DIM], } } @@ -38,10 +43,10 @@ impl VelocityGroundConstraintTangentPart { { #[cfg(feature = "dim2")] { - let dimpulse = -tangents1[0].dot(&mj_lambda2.linear) + let dvel = -tangents1[0].dot(&mj_lambda2.linear) + self.gcross2[0].gdot(mj_lambda2.angular) + self.rhs[0]; - let new_impulse = (self.impulse[0] - self.r[0] * dimpulse).simd_clamp(-limit, limit); + let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit); let dlambda = new_impulse - self.impulse[0]; self.impulse[0] = new_impulse; @@ -51,17 +56,22 @@ impl VelocityGroundConstraintTangentPart { #[cfg(feature = "dim3")] { - let dimpulse_0 = -tangents1[0].dot(&mj_lambda2.linear) + let dvel_0 = -tangents1[0].dot(&mj_lambda2.linear) + self.gcross2[0].gdot(mj_lambda2.angular) + self.rhs[0]; - let dimpulse_1 = -tangents1[1].dot(&mj_lambda2.linear) + let dvel_1 = -tangents1[1].dot(&mj_lambda2.linear) + self.gcross2[1].gdot(mj_lambda2.angular) + self.rhs[1]; - let new_impulse = na::Vector2::new( - self.impulse[0] - self.r[0] * dimpulse_0, - self.impulse[1] - self.r[1] * dimpulse_1, - ); + let dvel_00 = dvel_0 * dvel_0; + let dvel_11 = dvel_1 * dvel_1; + let dvel_01 = dvel_0 * dvel_1; + let inv_lhs = (dvel_00 + dvel_11) + * crate::utils::simd_inv( + dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2], + ); + let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1]; + let new_impulse = self.impulse - delta_impulse; let new_impulse = { let _disable_fe_except = crate::utils::DisableFloatingPointExceptionsFlags:: @@ -69,7 +79,6 @@ impl VelocityGroundConstraintTangentPart { new_impulse.simd_cap_magnitude(limit) }; let dlambda = new_impulse - self.impulse; - self.impulse = new_impulse; mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda[0] @@ -89,7 +98,6 @@ pub(crate) struct VelocityGroundConstraintNormalPart { } impl VelocityGroundConstraintNormalPart { - #[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))] fn zero() -> Self { Self { gcross2: na::zero(), @@ -105,9 +113,8 @@ impl VelocityGroundConstraintNormalPart { where AngVector: WDot, Result = N>, { - let dimpulse = - -dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs; - let new_impulse = (self.impulse - self.r * dimpulse).simd_max(N::zero()); + let dvel = -dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs; + let new_impulse = (self.impulse - self.r * dvel).simd_max(N::zero()); let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; @@ -123,7 +130,6 @@ pub(crate) struct VelocityGroundConstraintElement { } impl VelocityGroundConstraintElement { - #[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))] pub fn zero() -> Self { Self { normal_part: VelocityGroundConstraintNormalPart::zero(), diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 2e4812b..4771469 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -44,6 +44,7 @@ impl WVelocityGroundConstraint { let inv_dt = SimdReal::splat(params.inv_dt()); let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction); let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); + let delassus_inv_factor = SimdReal::splat(params.delassus_inv_factor); let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1]; @@ -142,12 +143,12 @@ impl WVelocityGroundConstraint { { let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); - let r = SimdReal::splat(1.0) + let r = delassus_inv_factor / (force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2)); let projected_velocity = (vel1 - vel2).dot(&force_dir1); let mut rhs_wo_bias = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity; - rhs_wo_bias += (dist + allowed_lin_err).simd_max(SimdReal::zero()) * inv_dt; + rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt; rhs_wo_bias *= is_bouncy + is_resting * velocity_solve_fraction; let rhs_bias = (dist + allowed_lin_err).simd_min(SimdReal::zero()) * (erp_inv_dt/* * is_resting */); @@ -166,14 +167,24 @@ impl WVelocityGroundConstraint { for j in 0..DIM - 1 { let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); - let r = SimdReal::splat(1.0) - / (tangents1[j].dot(&im2.component_mul(&tangents1[j])) - + gcross2.gdot(gcross2)); + let r = + tangents1[j].dot(&im2.component_mul(&tangents1[j])) + gcross2.gdot(gcross2); let rhs = (vel1 - vel2 + tangent_velocity * flipped_sign).dot(&tangents1[j]); constraint.elements[k].tangent_part.gcross2[j] = gcross2; - constraint.elements[k].tangent_part.r[j] = r; constraint.elements[k].tangent_part.rhs[j] = rhs; + constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") { + SimdReal::splat(1.0) / r + } else { + r + }; + } + + #[cfg(feature = "dim3")] + { + constraint.elements[k].tangent_part.r[2] = SimdReal::splat(2.0) + * constraint.elements[k].tangent_part.gcross2[0] + .gdot(constraint.elements[k].tangent_part.gcross2[1]); } } diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 91b6be9..1cc43ac 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -1,6 +1,6 @@ use super::AnyJointVelocityConstraint; use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; -use crate::dynamics::solver::GenericVelocityConstraint; +use crate::dynamics::solver::AnyGenericVelocityConstraint; use crate::dynamics::{ solver::{AnyVelocityConstraint, DeltaVel}, IntegrationParameters, JointGraphEdge, MultibodyJointSet, RigidBodyForces, RigidBodyType, @@ -36,7 +36,7 @@ impl VelocitySolver { manifolds_all: &mut [&mut ContactManifold], joints_all: &mut [JointGraphEdge], contact_constraints: &mut [AnyVelocityConstraint], - generic_contact_constraints: &mut [GenericVelocityConstraint], + generic_contact_constraints: &mut [AnyGenericVelocityConstraint], generic_contact_jacobians: &DVector, joint_constraints: &mut [AnyJointVelocityConstraint], generic_joint_jacobians: &DVector, -- cgit