From f74b8401ad9ef50b8cdbf1f43a2b21f6c42b0ebc Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 2 Jan 2022 14:47:40 +0100 Subject: Implement multibody joints and the new solver --- src/dynamics/solver/categorization.rs | 36 +- src/dynamics/solver/delta_vel.rs | 24 +- src/dynamics/solver/generic_velocity_constraint.rs | 377 +++++++++ .../solver/generic_velocity_constraint_element.rs | 348 +++++++++ src/dynamics/solver/interaction_groups.rs | 13 +- src/dynamics/solver/island_solver.rs | 151 ++-- .../joint_constraint/ball_position_constraint.rs | 266 ------- .../ball_position_constraint_wide.rs | 216 ------ .../joint_constraint/ball_velocity_constraint.rs | 660 ---------------- .../ball_velocity_constraint_wide.rs | 359 --------- .../joint_constraint/fixed_position_constraint.rs | 151 ---- .../fixed_position_constraint_wide.rs | 71 -- .../joint_constraint/fixed_velocity_constraint.rs | 436 ----------- .../fixed_velocity_constraint_wide.rs | 539 ------------- .../generic_position_constraint.rs | 346 --------- .../generic_position_constraint_wide.rs | 60 -- .../generic_velocity_constraint.rs | 706 ----------------- .../generic_velocity_constraint_wide.rs | 464 ----------- .../solver/joint_constraint/joint_constraint.rs | 710 +++++++++-------- .../joint_generic_velocity_constraint.rs | 529 +++++++++++++ .../joint_generic_velocity_constraint_builder.rs | 853 ++++++++++++++++++++ .../joint_constraint/joint_position_constraint.rs | 280 ------- .../joint_constraint/joint_velocity_constraint.rs | 608 +++++++++++++++ .../joint_velocity_constraint_builder.rs | 699 +++++++++++++++++ src/dynamics/solver/joint_constraint/mod.rs | 107 +-- .../prismatic_position_constraint.rs | 182 ----- .../prismatic_position_constraint_wide.rs | 71 -- .../prismatic_velocity_constraint.rs | 859 --------------------- .../prismatic_velocity_constraint_wide.rs | 848 -------------------- .../revolute_position_constraint.rs | 295 ------- .../revolute_position_constraint_wide.rs | 71 -- .../revolute_velocity_constraint.rs | 750 ------------------ .../revolute_velocity_constraint_wide.rs | 527 ------------- src/dynamics/solver/mod.rs | 27 +- src/dynamics/solver/parallel_island_solver.rs | 156 +--- src/dynamics/solver/parallel_position_solver.rs | 107 --- src/dynamics/solver/parallel_solver_constraints.rs | 101 ++- src/dynamics/solver/parallel_velocity_solver.rs | 111 +-- src/dynamics/solver/position_constraint.rs | 168 ---- src/dynamics/solver/position_constraint_wide.rs | 157 ---- src/dynamics/solver/position_ground_constraint.rs | 121 --- .../solver/position_ground_constraint_wide.rs | 143 ---- src/dynamics/solver/position_solver.rs | 57 -- src/dynamics/solver/solver_constraints.rs | 317 ++++++-- src/dynamics/solver/velocity_constraint.rs | 131 ++-- src/dynamics/solver/velocity_constraint_element.rs | 114 +-- src/dynamics/solver/velocity_constraint_wide.rs | 110 +-- src/dynamics/solver/velocity_ground_constraint.rs | 77 +- .../solver/velocity_ground_constraint_element.rs | 81 +- .../solver/velocity_ground_constraint_wide.rs | 93 +-- src/dynamics/solver/velocity_solver.rs | 222 +++++- 51 files changed, 4686 insertions(+), 10219 deletions(-) create mode 100644 src/dynamics/solver/generic_velocity_constraint.rs create mode 100644 src/dynamics/solver/generic_velocity_constraint_element.rs delete mode 100644 src/dynamics/solver/joint_constraint/ball_position_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/fixed_position_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/generic_position_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs delete mode 100644 src/dynamics/solver/joint_constraint/joint_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs delete mode 100644 src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/revolute_position_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs delete mode 100644 src/dynamics/solver/parallel_position_solver.rs delete mode 100644 src/dynamics/solver/position_constraint.rs delete mode 100644 src/dynamics/solver/position_constraint_wide.rs delete mode 100644 src/dynamics/solver/position_ground_constraint.rs delete mode 100644 src/dynamics/solver/position_ground_constraint_wide.rs delete mode 100644 src/dynamics/solver/position_solver.rs (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index 366a5b3..c5b2601 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -1,18 +1,33 @@ use crate::data::ComponentSet; -use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodyType}; +use crate::dynamics::{JointGraphEdge, JointIndex, MultibodyJointSet, RigidBodyType}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; pub(crate) fn categorize_contacts( _bodies: &impl ComponentSet, // Unused but useful to simplify the parallel code. + multibody_joints: &MultibodyJointSet, manifolds: &[&mut ContactManifold], 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. ) { for manifold_i in manifold_indices { let manifold = &manifolds[*manifold_i]; - if manifold.data.relative_dominance != 0 { + if manifold + .data + .rigid_body1 + .map(|rb| multibody_joints.rigid_body_link(rb)) + .is_some() + || manifold + .data + .rigid_body1 + .map(|rb| multibody_joints.rigid_body_link(rb)) + .is_some() + { + out_generic.push(*manifold_i); + } else if manifold.data.relative_dominance != 0 { out_ground.push(*manifold_i) } else { out_not_ground.push(*manifold_i) @@ -22,17 +37,28 @@ pub(crate) fn categorize_contacts( pub(crate) fn categorize_joints( bodies: &impl ComponentSet, - joints: &[JointGraphEdge], + multibody_joints: &MultibodyJointSet, + impulse_joints: &[JointGraphEdge], joint_indices: &[JointIndex], ground_joints: &mut Vec, nonground_joints: &mut Vec, + generic_ground_joints: &mut Vec, + generic_nonground_joints: &mut Vec, ) { for joint_i in joint_indices { - let joint = &joints[*joint_i].weight; + let joint = &impulse_joints[*joint_i].weight; let status1 = bodies.index(joint.body1.0); let status2 = bodies.index(joint.body2.0); - if !status1.is_dynamic() || !status2.is_dynamic() { + if multibody_joints.rigid_body_link(joint.body1).is_some() + || multibody_joints.rigid_body_link(joint.body2).is_some() + { + if !status1.is_dynamic() || !status2.is_dynamic() { + generic_ground_joints.push(*joint_i); + } else { + generic_nonground_joints.push(*joint_i); + } + } else if !status1.is_dynamic() || !status2.is_dynamic() { ground_joints.push(*joint_i); } else { nonground_joints.push(*joint_i); diff --git a/src/dynamics/solver/delta_vel.rs b/src/dynamics/solver/delta_vel.rs index b457020..9160f2e 100644 --- a/src/dynamics/solver/delta_vel.rs +++ b/src/dynamics/solver/delta_vel.rs @@ -1,14 +1,34 @@ -use crate::math::{AngVector, Vector}; +use crate::math::{AngVector, Vector, SPATIAL_DIM}; +use na::{DVectorSlice, DVectorSliceMut}; use na::{Scalar, SimdRealField}; use std::ops::AddAssign; #[derive(Copy, Clone, Debug)] +#[repr(C)] //#[repr(align(64))] -pub(crate) struct DeltaVel { +pub struct DeltaVel { pub linear: Vector, pub angular: AngVector, } +impl DeltaVel { + pub fn as_slice(&self) -> &[N; SPATIAL_DIM] { + unsafe { std::mem::transmute(self) } + } + + pub fn as_mut_slice(&mut self) -> &mut [N; SPATIAL_DIM] { + unsafe { std::mem::transmute(self) } + } + + pub fn as_vector_slice(&self) -> DVectorSlice { + DVectorSlice::from_slice(&self.as_slice()[..], SPATIAL_DIM) + } + + pub fn as_vector_slice_mut(&mut self) -> DVectorSliceMut { + DVectorSliceMut::from_slice(&mut self.as_mut_slice()[..], SPATIAL_DIM) + } +} + impl DeltaVel { pub fn zero() -> Self { Self { diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs new file mode 100644 index 0000000..fb06335 --- /dev/null +++ b/src/dynamics/solver/generic_velocity_constraint.rs @@ -0,0 +1,377 @@ +use crate::data::{BundleSet, ComponentSet}; +use crate::dynamics::solver::{GenericRhs, VelocityConstraint}; +use crate::dynamics::{ + IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodyType, + RigidBodyVelocity, +}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS}; +use crate::utils::{WAngularInertia, WCross, WDot}; + +use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart}; +#[cfg(feature = "dim2")] +use crate::utils::WBasis; +use na::DVector; + +#[derive(Copy, Clone, Debug)] +pub(crate) struct GenericVelocityConstraint { + // 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: VelocityConstraint, + pub j_id: usize, + pub ndofs1: usize, + pub ndofs2: usize, + pub generic_constraint_mask: u8, +} + +impl GenericVelocityConstraint { + 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 handle1 = manifold.data.rigid_body1.unwrap(); + let handle2 = manifold.data.rigid_body2.unwrap(); + let (rb_ids1, rb_vels1, rb_mprops1, rb_type1): ( + &RigidBodyIds, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyType, + ) = bodies.index_bundle(handle1.0); + let (rb_ids2, rb_vels2, rb_mprops2, rb_type2): ( + &RigidBodyIds, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyType, + ) = bodies.index_bundle(handle2.0); + + let multibody1 = multibodies + .rigid_body_link(handle1) + .map(|m| (&multibodies[m.multibody], m.id)); + let multibody2 = multibodies + .rigid_body_link(handle2) + .map(|m| (&multibodies[m.multibody], m.id)); + let mj_lambda1 = multibody1 + .map(|mb| mb.0.solver_id) + .unwrap_or(if rb_type1.is_dynamic() { + rb_ids1.active_set_offset + } else { + 0 + }); + let mj_lambda2 = multibody2 + .map(|mb| mb.0.solver_id) + .unwrap_or(if rb_type2.is_dynamic() { + rb_ids2.active_set_offset + } else { + 0 + }); + let force_dir1 = -manifold.data.normal; + + #[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 = multibody1.map(|m| m.0.ndofs()).unwrap_or(0) + + multibody2.map(|m| m.0.ndofs()).unwrap_or(0); + // 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 = VelocityConstraint { + dir1: force_dir1, + #[cfg(feature = "dim3")] + tangent1: tangents1[0], + elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], + im1: if rb_type1.is_dynamic() { + rb_mprops1.effective_inv_mass + } else { + 0.0 + }, + im2: if rb_type2.is_dynamic() { + rb_mprops2.effective_inv_mass + } else { + 0.0 + }, + limit: 0.0, + mj_lambda1, + 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 - rb_mprops1.world_com; + 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_dir1 = dp1.gcross(force_dir1); + let torque_dir2 = dp2.gcross(-force_dir1); + + let gcross1 = if rb_type1.is_dynamic() { + rb_mprops1 + .effective_world_inv_inertia_sqrt + .transform_vector(torque_dir1) + } else { + na::zero() + }; + let gcross2 = if rb_type2.is_dynamic() { + rb_mprops2 + .effective_world_inv_inertia_sqrt + .transform_vector(torque_dir2) + } else { + na::zero() + }; + + let inv_r1 = if let Some((mb1, link_id1)) = multibody1.as_ref() { + mb1.fill_jacobians( + *link_id1, + force_dir1, + #[cfg(feature = "dim2")] + na::vector!(torque_dir1), + #[cfg(feature = "dim3")] + torque_dir1, + jacobian_id, + jacobians, + ) + .0 + } else if rb_type1.is_dynamic() { + rb_mprops1.effective_inv_mass + gcross1.gdot(gcross1) + } else { + 0.0 + }; + + let inv_r2 = if let Some((mb2, link_id2)) = multibody2.as_ref() { + mb2.fill_jacobians( + *link_id2, + -force_dir1, + #[cfg(feature = "dim2")] + na::vector!(torque_dir2), + #[cfg(feature = "dim3")] + torque_dir2, + jacobian_id, + jacobians, + ) + .0 + } else if rb_type2.is_dynamic() { + rb_mprops2.effective_inv_mass + gcross2.gdot(gcross2) + } else { + 0.0 + }; + + let r = crate::utils::inv(inv_r1 + 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 = VelocityConstraintNormalPart { + gcross1, + gcross2, + 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_dir1 = dp1.gcross(tangents1[j]); + let gcross1 = if rb_type1.is_dynamic() { + rb_mprops1 + .effective_world_inv_inertia_sqrt + .transform_vector(torque_dir1) + } else { + na::zero() + }; + constraint.elements[k].tangent_part.gcross1[j] = gcross1; + + let torque_dir2 = dp2.gcross(-tangents1[j]); + let gcross2 = if rb_type2.is_dynamic() { + rb_mprops2 + .effective_world_inv_inertia_sqrt + .transform_vector(torque_dir2) + } else { + na::zero() + }; + constraint.elements[k].tangent_part.gcross2[j] = gcross2; + + let inv_r1 = if let Some((mb1, link_id1)) = multibody1.as_ref() { + mb1.fill_jacobians( + *link_id1, + tangents1[j], + #[cfg(feature = "dim2")] + na::vector![torque_dir1], + #[cfg(feature = "dim3")] + torque_dir1, + jacobian_id, + jacobians, + ) + .0 + } else if rb_type1.is_dynamic() { + rb_mprops1.effective_inv_mass + gcross1.gdot(gcross1) + } else { + 0.0 + }; + + let inv_r2 = if let Some((mb2, link_id2)) = multibody2.as_ref() { + mb2.fill_jacobians( + *link_id2, + -tangents1[j], + #[cfg(feature = "dim2")] + na::vector![torque_dir2], + #[cfg(feature = "dim3")] + torque_dir2, + jacobian_id, + jacobians, + ) + .0 + } else if rb_type2.is_dynamic() { + rb_mprops2.effective_inv_mass + gcross2.gdot(gcross2) + } else { + 0.0 + }; + + let r = crate::utils::inv(inv_r1 + inv_r2); + + let rhs = + (vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]); + + constraint.elements[k].tangent_part.rhs[j] = rhs; + constraint.elements[k].tangent_part.r[j] = r; + } + } + } + + let ndofs1 = multibody1.map(|mb| mb.0.ndofs()).unwrap_or(0); + let ndofs2 = multibody2.map(|mb| mb.0.ndofs()).unwrap_or(0); + // NOTE: we use the generic constraint for non-dynamic bodies because this will + // reduce all ops to nothing because its ndofs will be zero. + let generic_constraint_mask = (multibody1.is_some() as u8) + | ((multibody2.is_some() as u8) << 1) + | (!rb_type1.is_dynamic() as u8) + | ((!rb_type2.is_dynamic() as u8) << 1); + + let constraint = GenericVelocityConstraint { + velocity_constraint: constraint, + j_id: chunk_j_id, + ndofs1, + ndofs2, + generic_constraint_mask, + }; + + if push { + out_constraints.push(constraint); + } else { + out_constraints[manifold.data.constraint_index + _l] = constraint; + } + } + } + + pub fn solve( + &mut self, + jacobians: &DVector, + mj_lambdas: &mut [DeltaVel], + generic_mj_lambdas: &mut DVector, + solve_restitution: bool, + solve_friction: bool, + ) { + let mut mj_lambda1 = if self.generic_constraint_mask & 0b01 == 0 { + GenericRhs::DeltaVel(mj_lambdas[self.velocity_constraint.mj_lambda1 as usize]) + } else { + GenericRhs::GenericId(self.velocity_constraint.mj_lambda1 as usize) + }; + + let mut mj_lambda2 = if self.generic_constraint_mask & 0b10 == 0 { + GenericRhs::DeltaVel(mj_lambdas[self.velocity_constraint.mj_lambda2 as usize]) + } else { + GenericRhs::GenericId(self.velocity_constraint.mj_lambda2 as usize) + }; + + let elements = &mut self.velocity_constraint.elements + [..self.velocity_constraint.num_contacts as usize]; + VelocityConstraintElement::generic_solve_group( + elements, + jacobians, + &self.velocity_constraint.dir1, + #[cfg(feature = "dim3")] + &self.velocity_constraint.tangent1, + self.velocity_constraint.im1, + self.velocity_constraint.im2, + self.velocity_constraint.limit, + self.ndofs1, + self.ndofs2, + self.j_id, + &mut mj_lambda1, + &mut mj_lambda2, + generic_mj_lambdas, + solve_restitution, + solve_friction, + ); + + if let GenericRhs::DeltaVel(mj_lambda1) = mj_lambda1 { + mj_lambdas[self.velocity_constraint.mj_lambda1 as usize] = mj_lambda1; + } + + if let GenericRhs::DeltaVel(mj_lambda2) = mj_lambda2 { + mj_lambdas[self.velocity_constraint.mj_lambda2 as usize] = mj_lambda2; + } + } + + 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_constraint_element.rs b/src/dynamics/solver/generic_velocity_constraint_element.rs new file mode 100644 index 0000000..150be8b --- /dev/null +++ b/src/dynamics/solver/generic_velocity_constraint_element.rs @@ -0,0 +1,348 @@ +use super::DeltaVel; +use crate::dynamics::solver::{ + VelocityConstraintElement, VelocityConstraintNormalPart, VelocityConstraintTangentPart, +}; +use crate::math::{AngVector, Real, Vector, DIM}; +use crate::utils::WDot; +use na::DVector; +#[cfg(feature = "dim2")] +use {crate::utils::WBasis, na::SimdPartialOrd}; + +pub(crate) enum GenericRhs { + DeltaVel(DeltaVel), + GenericId(usize), +} + +// Offset between the jacobians of two consecutive constraints. +#[inline(always)] +fn j_step(ndofs1: usize, ndofs2: usize) -> usize { + (ndofs1 + ndofs2) * 2 +} + +#[inline(always)] +fn j_id1(j_id: usize, _ndofs1: usize, _ndofs2: usize) -> usize { + j_id +} + +#[inline(always)] +fn j_id2(j_id: usize, ndofs1: usize, _ndofs2: usize) -> usize { + j_id + ndofs1 * 2 +} + +#[inline(always)] +fn normal_j_id(j_id: usize, _ndofs1: usize, _ndofs2: usize) -> usize { + j_id +} + +#[inline(always)] +fn tangent_j_id(j_id: usize, ndofs1: usize, ndofs2: usize) -> usize { + j_id + (ndofs1 + ndofs2) * 2 +} + +impl GenericRhs { + #[inline(always)] + fn dimpulse( + &self, + j_id: usize, + ndofs: usize, + jacobians: &DVector, + dir: &Vector, + gcross: &AngVector, + mj_lambdas: &DVector, + ) -> Real { + match self { + GenericRhs::DeltaVel(rhs) => dir.dot(&rhs.linear) + gcross.gdot(rhs.angular), + GenericRhs::GenericId(mj_lambda) => { + let j = jacobians.rows(j_id, ndofs); + let rhs = mj_lambdas.rows(*mj_lambda, ndofs); + j.dot(&rhs) + } + } + } + + #[inline(always)] + fn apply_impulse( + &mut self, + j_id: usize, + ndofs: usize, + impulse: Real, + jacobians: &DVector, + dir: &Vector, + gcross: &AngVector, + mj_lambdas: &mut DVector, + inv_mass: Real, + ) { + match self { + GenericRhs::DeltaVel(rhs) => { + rhs.linear += dir * (inv_mass * impulse); + rhs.angular += gcross * impulse; + } + GenericRhs::GenericId(mj_lambda) => { + let wj_id = j_id + ndofs; + let wj = jacobians.rows(wj_id, ndofs); + let mut rhs = mj_lambdas.rows_mut(*mj_lambda, ndofs); + rhs.axpy(impulse, &wj, 1.0); + } + } + } +} + +impl VelocityConstraintTangentPart { + #[inline] + pub fn generic_solve( + &mut self, + j_id: usize, + jacobians: &DVector, + tangents1: [&Vector; DIM - 1], + im1: Real, + im2: Real, + ndofs1: usize, + ndofs2: usize, + limit: Real, + mj_lambda1: &mut GenericRhs, + mj_lambda2: &mut GenericRhs, + mj_lambdas: &mut DVector, + ) { + let j_id1 = j_id1(j_id, ndofs1, ndofs2); + let j_id2 = j_id2(j_id, ndofs1, ndofs2); + #[cfg(feature = "dim3")] + let j_step = j_step(ndofs1, ndofs2); + + #[cfg(feature = "dim2")] + { + let dimpulse_0 = mj_lambda1.dimpulse( + j_id1, + ndofs1, + jacobians, + &tangents1[0], + &self.gcross1[0], + mj_lambdas, + ) + mj_lambda2.dimpulse( + j_id2, + ndofs2, + jacobians, + &-tangents1[0], + &self.gcross2[0], + mj_lambdas, + ) + self.rhs[0]; + + let new_impulse = (self.impulse[0] - self.r[0] * dimpulse_0).simd_clamp(-limit, limit); + let dlambda = new_impulse - self.impulse[0]; + self.impulse[0] = new_impulse; + + mj_lambda1.apply_impulse( + j_id1, + ndofs1, + dlambda, + jacobians, + &tangents1[0], + &self.gcross1[0], + mj_lambdas, + im1, + ); + mj_lambda2.apply_impulse( + j_id2, + ndofs2, + dlambda, + jacobians, + &-tangents1[0], + &self.gcross2[0], + mj_lambdas, + im2, + ); + } + + #[cfg(feature = "dim3")] + { + let dimpulse_0 = mj_lambda1.dimpulse( + j_id1, + ndofs1, + jacobians, + &tangents1[0], + &self.gcross1[0], + mj_lambdas, + ) + mj_lambda2.dimpulse( + j_id2, + ndofs2, + jacobians, + &-tangents1[0], + &self.gcross2[0], + mj_lambdas, + ) + self.rhs[0]; + let dimpulse_1 = mj_lambda1.dimpulse( + j_id1 + j_step, + ndofs1, + jacobians, + &tangents1[1], + &self.gcross1[1], + mj_lambdas, + ) + mj_lambda2.dimpulse( + j_id2 + j_step, + ndofs2, + jacobians, + &-tangents1[1], + &self.gcross2[1], + mj_lambdas, + ) + 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 new_impulse = new_impulse.cap_magnitude(limit); + + let dlambda = new_impulse - self.impulse; + self.impulse = new_impulse; + + mj_lambda1.apply_impulse( + j_id1, + ndofs1, + dlambda[0], + jacobians, + &tangents1[0], + &self.gcross1[0], + mj_lambdas, + im1, + ); + mj_lambda1.apply_impulse( + j_id1 + j_step, + ndofs1, + dlambda[1], + jacobians, + &tangents1[1], + &self.gcross1[1], + mj_lambdas, + im1, + ); + + mj_lambda2.apply_impulse( + j_id2, + ndofs2, + dlambda[0], + jacobians, + &-tangents1[0], + &self.gcross2[0], + mj_lambdas, + im2, + ); + mj_lambda2.apply_impulse( + j_id2 + j_step, + ndofs2, + dlambda[1], + jacobians, + &-tangents1[1], + &self.gcross2[1], + mj_lambdas, + im2, + ); + } + } +} + +impl VelocityConstraintNormalPart { + #[inline] + pub fn generic_solve( + &mut self, + j_id: usize, + jacobians: &DVector, + dir1: &Vector, + im1: Real, + im2: Real, + ndofs1: usize, + ndofs2: usize, + mj_lambda1: &mut GenericRhs, + mj_lambda2: &mut GenericRhs, + mj_lambdas: &mut DVector, + ) { + 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 new_impulse = (self.impulse - self.r * dimpulse).max(0.0); + let dlambda = new_impulse - self.impulse; + self.impulse = new_impulse; + + mj_lambda1.apply_impulse( + j_id1, + ndofs1, + dlambda, + jacobians, + &dir1, + &self.gcross1, + mj_lambdas, + im1, + ); + mj_lambda2.apply_impulse( + j_id2, + ndofs2, + dlambda, + jacobians, + &-dir1, + &self.gcross2, + mj_lambdas, + im2, + ); + } +} + +impl VelocityConstraintElement { + #[inline] + pub fn generic_solve_group( + elements: &mut [Self], + jacobians: &DVector, + dir1: &Vector, + #[cfg(feature = "dim3")] tangent1: &Vector, + im1: Real, + im2: Real, + limit: Real, + // ndofs is 0 for a non-multibody body, or a multibody with zero + // degrees of freedom. + ndofs1: usize, + ndofs2: usize, + // Jacobian index of the first constraint. + j_id: usize, + mj_lambda1: &mut GenericRhs, + mj_lambda2: &mut GenericRhs, + mj_lambdas: &mut DVector, + solve_restitution: bool, + solve_friction: bool, + ) { + let j_step = j_step(ndofs1, ndofs2) * DIM; + + // Solve penetration. + if solve_restitution { + let mut nrm_j_id = normal_j_id(j_id, ndofs1, ndofs2); + + for element in elements.iter_mut() { + element.normal_part.generic_solve( + nrm_j_id, jacobians, &dir1, im1, im2, ndofs1, ndofs2, mj_lambda1, mj_lambda2, + mj_lambdas, + ); + nrm_j_id += j_step; + } + } + + // Solve friction. + if solve_friction { + #[cfg(feature = "dim3")] + let tangents1 = [tangent1, &dir1.cross(&tangent1)]; + #[cfg(feature = "dim2")] + let tangents1 = [&dir1.orthonormal_vector()]; + let mut tng_j_id = tangent_j_id(j_id, ndofs1, ndofs2); + + 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, tangents1, im1, im2, ndofs1, ndofs2, limit, mj_lambda1, + mj_lambda2, mj_lambdas, + ); + tng_j_id += j_step; + } + } + } +} diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index c6baea1..67cc805 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -216,12 +216,13 @@ impl InteractionGroups { ) where Bodies: ComponentSet + ComponentSet, { - // NOTE: in 3D we have up to 10 different joint types. - // In 2D we only have 5 joint types. + // TODO: right now, we only sort based on the axes locked by the joint. + // We could also take motors and limits into account in the future (most of + // the SIMD constraints generation for motors and limits is already implemented). #[cfg(feature = "dim3")] - const NUM_JOINT_TYPES: usize = 10; + const NUM_JOINT_TYPES: usize = 64; #[cfg(feature = "dim2")] - const NUM_JOINT_TYPES: usize = 5; + const NUM_JOINT_TYPES: usize = 8; // The j-th bit of joint_type_conflicts[i] indicates that the // j-th bucket contains a joint with a type different than `i`. @@ -254,13 +255,13 @@ impl InteractionGroups { continue; } - if !interaction.supports_simd_constraints() { + if !interaction.data.supports_simd_constraints() { // This joint does not support simd constraints yet. self.nongrouped_interactions.push(*interaction_i); continue; } - let ijoint = interaction.params.type_id(); + let ijoint = interaction.data.locked_axes.bits() as usize; let i1 = ids1.active_set_offset; let i2 = ids2.active_set_offset; let conflicts = diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index 912fe1d..a862480 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -1,9 +1,8 @@ -use super::{PositionSolver, VelocitySolver}; +use super::VelocitySolver; use crate::counters::Counters; use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; use crate::dynamics::solver::{ - AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint, - AnyVelocityConstraint, SolverConstraints, + AnyJointVelocityConstraint, AnyVelocityConstraint, GenericVelocityConstraint, SolverConstraints, }; use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces, @@ -11,12 +10,12 @@ use crate::dynamics::{ }; use crate::dynamics::{IslandManager, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::prelude::{MultibodyJointSet, RigidBodyActivation}; pub struct IslandSolver { - contact_constraints: SolverConstraints, - joint_constraints: SolverConstraints, + contact_constraints: SolverConstraints, + joint_constraints: SolverConstraints, velocity_solver: VelocitySolver, - position_solver: PositionSolver, } impl Default for IslandSolver { @@ -31,33 +30,10 @@ impl IslandSolver { contact_constraints: SolverConstraints::new(), joint_constraints: SolverConstraints::new(), velocity_solver: VelocitySolver::new(), - position_solver: PositionSolver::new(), } } - pub fn solve_position_constraints( - &mut self, - island_id: usize, - islands: &IslandManager, - counters: &mut Counters, - params: &IntegrationParameters, - bodies: &mut Bodies, - ) where - Bodies: ComponentSet + ComponentSetMut, - { - counters.solver.position_resolution_time.resume(); - self.position_solver.solve( - island_id, - params, - islands, - bodies, - &self.contact_constraints.position_constraints, - &self.joint_constraints.position_constraints, - ); - counters.solver.position_resolution_time.pause(); - } - - pub fn init_constraints_and_solve_velocity_constraints( + pub fn init_and_solve( &mut self, island_id: usize, counters: &mut Counters, @@ -66,31 +42,64 @@ impl IslandSolver { bodies: &mut Bodies, manifolds: &mut [&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], - joints: &mut [JointGraphEdge], + impulse_joints: &mut [JointGraphEdge], joint_indices: &[JointIndex], + multibody_joints: &mut MultibodyJointSet, ) where Bodies: ComponentSet + ComponentSetMut + ComponentSetMut - + ComponentSet + + ComponentSetMut + + ComponentSetMut + ComponentSet + ComponentSet + ComponentSet, { - let has_constraints = manifold_indices.len() != 0 || joint_indices.len() != 0; + let mut has_constraints = manifold_indices.len() != 0 || joint_indices.len() != 0; + if !has_constraints { + // Check if the multibody_joints have internal constraints. + for handle in islands.active_island(island_id) { + if let Some(link) = multibody_joints.rigid_body_link(*handle) { + let multibody = multibody_joints.get_multibody(link.multibody).unwrap(); + + if (link.id == 0 || link.id == 1 && !multibody.root_is_dynamic) + && multibody.has_active_internal_constraints() + { + has_constraints = true; + break; + } + } + } + } if has_constraints { + // Init the solver id for multibody_joints. + // We need that for building the constraints. + let mut solver_id = 0; + for (_, multibody) in multibody_joints.multibodies.iter_mut() { + multibody.solver_id = solver_id; + solver_id += multibody.ndofs(); + } + counters.solver.velocity_assembly_time.resume(); self.contact_constraints.init( island_id, params, islands, bodies, + multibody_joints, manifolds, manifold_indices, ); - self.joint_constraints - .init(island_id, params, islands, bodies, joints, joint_indices); + self.joint_constraints.init( + island_id, + params, + islands, + bodies, + multibody_joints, + impulse_joints, + joint_indices, + ); counters.solver.velocity_assembly_time.pause(); counters.solver.velocity_resolution_time.resume(); @@ -99,57 +108,53 @@ impl IslandSolver { params, islands, bodies, + multibody_joints, manifolds, - joints, + impulse_joints, &mut self.contact_constraints.velocity_constraints, + &mut self.contact_constraints.generic_velocity_constraints, + &self.contact_constraints.generic_jacobians, &mut self.joint_constraints.velocity_constraints, + &self.joint_constraints.generic_jacobians, ); counters.solver.velocity_resolution_time.pause(); - - counters.solver.velocity_update_time.resume(); - - for handle in islands.active_island(island_id) { - let (poss, vels, damping, mprops): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyDamping, - &RigidBodyMassProps, - ) = bodies.index_bundle(handle.0); - - let mut new_poss = *poss; - let new_vels = vels.apply_damping(params.dt, damping); - new_poss.next_position = - vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com); - - bodies.set_internal(handle.0, new_vels); - bodies.set_internal(handle.0, new_poss); - } - - counters.solver.velocity_update_time.pause(); } else { self.contact_constraints.clear(); self.joint_constraints.clear(); counters.solver.velocity_update_time.resume(); for handle in islands.active_island(island_id) { - // Since we didn't run the velocity solver we need to integrate the accelerations here - let (poss, vels, forces, damping, mprops): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyForces, - &RigidBodyDamping, - &RigidBodyMassProps, - ) = bodies.index_bundle(handle.0); + if let Some(link) = multibody_joints.rigid_body_link(*handle).copied() { + let multibody = multibody_joints + .get_multibody_mut_internal(link.multibody) + .unwrap(); + + if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic { + let accels = &multibody.accelerations; + multibody.velocities.axpy(params.dt, accels, 1.0); + multibody.integrate_next(params.dt); + multibody.forward_kinematics_next(bodies, false); + } + } else { + // Since we didn't run the velocity solver we need to integrate the accelerations here + let (poss, vels, forces, damping, mprops): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyForces, + &RigidBodyDamping, + &RigidBodyMassProps, + ) = bodies.index_bundle(handle.0); - let mut new_poss = *poss; - let new_vels = forces - .integrate(params.dt, vels, mprops) - .apply_damping(params.dt, &damping); - new_poss.next_position = - vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com); + let mut new_poss = *poss; + let new_vels = forces + .integrate(params.dt, vels, mprops) + .apply_damping(params.dt, &damping); + new_poss.next_position = + vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com); - bodies.set_internal(handle.0, new_vels); - bodies.set_internal(handle.0, new_poss); + bodies.set_internal(handle.0, new_vels); + bodies.set_internal(handle.0, new_poss); + } } counters.solver.velocity_update_time.pause(); } diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs deleted file mode 100644 index 0dfdb86..0000000 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ /dev/null @@ -1,266 +0,0 @@ -use crate::dynamics::{ - BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, -}; -#[cfg(feature = "dim2")] -use crate::math::SdpMatrix; -use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, UnitVector}; -use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; - -#[derive(Debug)] -pub(crate) struct BallPositionConstraint { - position1: usize, - position2: usize, - - local_com1: Point, - local_com2: Point, - - im1: Real, - im2: Real, - - ii1: AngularInertia, - ii2: AngularInertia, - inv_ii1_ii2: AngularInertia, - - local_anchor1: Point, - local_anchor2: Point, - - limits_enabled: bool, - limits_angle: Real, - limits_local_axis1: UnitVector, - limits_local_axis2: UnitVector, -} - -impl BallPositionConstraint { - pub fn from_params( - rb1: (&RigidBodyMassProps, &RigidBodyIds), - rb2: (&RigidBodyMassProps, &RigidBodyIds), - cparams: &BallJoint, - ) -> Self { - let (mprops1, ids1) = rb1; - let (mprops2, ids2) = rb2; - - let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); - let inv_ii1_ii2 = (ii1 + ii2).inverse(); - - Self { - local_com1: mprops1.local_mprops.local_com, - local_com2: mprops2.local_mprops.local_com, - im1: mprops1.effective_inv_mass, - im2: mprops2.effective_inv_mass, - ii1, - ii2, - inv_ii1_ii2, - local_anchor1: cparams.local_anchor1, - local_anchor2: cparams.local_anchor2, - position1: ids1.active_set_offset, - position2: ids2.active_set_offset, - limits_enabled: cparams.limits_enabled, - limits_angle: cparams.limits_angle, - limits_local_axis1: cparams.limits_local_axis1, - limits_local_axis2: cparams.limits_local_axis2, - } - } - - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { - 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 com1 = position1 * self.local_com1; - let com2 = position2 * self.local_com2; - - let err = anchor1 - anchor2; - - let centered_anchor1 = anchor1 - com1; - let centered_anchor2 = anchor2 - com2; - - let cmat1 = centered_anchor1.gcross_matrix(); - let cmat2 = centered_anchor2.gcross_matrix(); - - // NOTE: the -cmat1 is just a simpler way of doing cmat1.transpose() - // because it is anti-symmetric. - #[cfg(feature = "dim3")] - let lhs = self.ii1.quadform(&cmat1).add_diagonal(self.im1) - + self.ii2.quadform(&cmat2).add_diagonal(self.im2); - - // In 2D we just unroll the computation because - // it's just easier that way. It is also - // faster because in 2D lhs will be symmetric. - #[cfg(feature = "dim2")] - let lhs = { - let m11 = - self.im1 + self.im2 + cmat1.x * cmat1.x * self.ii1 + cmat2.x * cmat2.x * self.ii2; - let m12 = cmat1.x * cmat1.y * self.ii1 + cmat2.x * cmat2.y * self.ii2; - let m22 = - self.im1 + self.im2 + cmat1.y * cmat1.y * self.ii1 + cmat2.y * cmat2.y * self.ii2; - SdpMatrix::new(m11, m12, m22) - }; - - let inv_lhs = lhs.inverse_unchecked(); - let impulse = inv_lhs * -(err * params.joint_erp); - - position1.translation.vector += self.im1 * impulse; - position2.translation.vector -= self.im2 * impulse; - - let angle1 = self.ii1.transform_vector(centered_anchor1.gcross(impulse)); - let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse)); - - position1.rotation = Rotation::new(angle1) * position1.rotation; - position2.rotation = Rotation::new(angle2) * position2.rotation; - - /* - * Limits part. - */ - if self.limits_enabled { - let axis1 = position1 * self.limits_local_axis1; - let axis2 = position2 * self.limits_local_axis2; - - #[cfg(feature = "dim2")] - let axis_angle = Rotation::rotation_between_axis(&axis2, &axis1).axis_angle(); - #[cfg(feature = "dim3")] - let axis_angle = - Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle()); - - // TODO: handle the case where dot(axis1, axis2) = -1.0 - if let Some((axis, angle)) = axis_angle { - if angle >= self.limits_angle { - #[cfg(feature = "dim2")] - let axis = axis[0]; - #[cfg(feature = "dim3")] - let axis = axis.into_inner(); - let ang_error = angle - self.limits_angle; - let ang_impulse = self - .inv_ii1_ii2 - .transform_vector(axis * ang_error * params.joint_erp); - - position1.rotation = - Rotation::new(self.ii1.transform_vector(-ang_impulse)) * position1.rotation; - position2.rotation = - Rotation::new(self.ii2.transform_vector(ang_impulse)) * position2.rotation; - } - } - } - - positions[self.position1 as usize] = position1; - positions[self.position2 as usize] = position2; - } -} - -#[derive(Debug)] -pub(crate) struct BallPositionGroundConstraint { - position2: usize, - anchor1: Point, - im2: Real, - ii2: AngularInertia, - local_anchor2: Point, - local_com2: Point, - - limits_enabled: bool, - limits_angle: Real, - limits_axis1: UnitVector, - limits_local_axis2: UnitVector, -} - -impl BallPositionGroundConstraint { - pub fn from_params( - rb1: &RigidBodyPosition, - rb2: (&RigidBodyMassProps, &RigidBodyIds), - cparams: &BallJoint, - flipped: bool, - ) -> Self { - let poss1 = rb1; - let (mprops2, ids2) = rb2; - - if flipped { - // Note the only thing that is flipped here - // are the local_anchors. The rb1 and rb2 have - // already been flipped by the caller. - Self { - anchor1: poss1.next_position * cparams.local_anchor2, - im2: mprops2.effective_inv_mass, - ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), - local_anchor2: cparams.local_anchor1, - position2: ids2.active_set_offset, - local_com2: mprops2.local_mprops.local_com, - limits_enabled: cparams.limits_enabled, - limits_angle: cparams.limits_angle, - limits_axis1: poss1.next_position * cparams.limits_local_axis2, - limits_local_axis2: cparams.limits_local_axis1, - } - } else { - Self { - anchor1: poss1.next_position * cparams.local_anchor1, - im2: mprops2.effective_inv_mass, - ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), - local_anchor2: cparams.local_anchor2, - position2: ids2.active_set_offset, - local_com2: mprops2.local_mprops.local_com, - limits_enabled: cparams.limits_enabled, - limits_angle: cparams.limits_angle, - limits_axis1: poss1.next_position * cparams.limits_local_axis1, - limits_local_axis2: cparams.limits_local_axis2, - } - } - } - - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { - let mut position2 = positions[self.position2 as usize]; - - let anchor2 = position2 * self.local_anchor2; - let com2 = position2 * self.local_com2; - - let err = self.anchor1 - anchor2; - let centered_anchor2 = anchor2 - com2; - let cmat2 = centered_anchor2.gcross_matrix(); - - #[cfg(feature = "dim3")] - let lhs = self.ii2.quadform(&cmat2).add_diagonal(self.im2); - - #[cfg(feature = "dim2")] - let lhs = { - let m11 = self.im2 + cmat2.x * cmat2.x * self.ii2; - let m12 = cmat2.x * cmat2.y * self.ii2; - let m22 = self.im2 + cmat2.y * cmat2.y * self.ii2; - SdpMatrix::new(m11, m12, m22) - }; - - let inv_lhs = lhs.inverse_unchecked(); - let impulse = inv_lhs * -(err * params.joint_erp); - position2.translation.vector -= self.im2 * impulse; - - let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse)); - position2.rotation = Rotation::new(angle2) * position2.rotation; - - /* - * Limits part. - */ - if self.limits_enabled { - let axis2 = position2 * self.limits_local_axis2; - - #[cfg(feature = "dim2")] - let axis_angle = - Rotation::rotation_between_axis(&axis2, &self.limits_axis1).axis_angle(); - #[cfg(feature = "dim3")] - let axis_angle = Rotation::rotation_between_axis(&axis2, &self.limits_axis1) - .and_then(|r| r.axis_angle()); - - // TODO: handle the case where dot(axis1, axis2) = -1.0 - if let Some((axis, angle)) = axis_angle { - if angle >= self.limits_angle { - #[cfg(feature = "dim2")] - let axis = axis[0]; - #[cfg(feature = "dim3")] - let axis = axis.into_inner(); - let ang_error = angle - self.limits_angle; - let ang_correction = axis * ang_error * params.joint_erp; - position2.rotation = Rotation::new(ang_correction) * position2.rotation; - } - } - } - - positions[self.position2 as usize] = position2; - } -} diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs deleted file mode 100644 index ea462ed..0000000 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ /dev/null @@ -1,216 +0,0 @@ -use crate::dynamics::{ - BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, -}; -#[cfg(feature = "dim2")] -use crate::math::SdpMatrix; -use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, SimdReal, SIMD_WIDTH}; -use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; -use simba::simd::SimdValue; - -#[derive(Debug)] -pub(crate) struct WBallPositionConstraint { - position1: [usize; SIMD_WIDTH], - position2: [usize; SIMD_WIDTH], - - local_com1: Point, - local_com2: Point, - - im1: SimdReal, - im2: SimdReal, - - ii1: AngularInertia, - ii2: AngularInertia, - - local_anchor1: Point, - local_anchor2: Point, -} - -impl WBallPositionConstraint { - pub fn from_params( - rbs1: ( - [&RigidBodyMassProps; SIMD_WIDTH], - [&RigidBodyIds; SIMD_WIDTH], - ), - rbs2: ( - [&RigidBodyMassProps; SIMD_WIDTH], - [&RigidBodyIds; SIMD_WIDTH], - ), - cparams: [&BallJoint; SIMD_WIDTH], - ) -> Self { - let (mprops1, ids1) = rbs1; - let (mprops2, ids2) = rbs2; - - let local_com1 = Point::from(gather![|ii| mprops1[ii].local_mprops.local_com]); - let local_com2 = Point::from(gather![|ii| mprops2[ii].local_mprops.local_com]); - let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]); - let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); - let ii1 = AngularInertia::::from(gather![ - |ii| mprops1[ii].effective_world_inv_inertia_sqrt - ]) - .squared(); - let ii2 = AngularInertia::::from(gather![ - |ii| mprops2[ii].effective_world_inv_inertia_sqrt - ]) - .squared(); - let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]); - let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]); - let position1 = gather![|ii| ids1[ii].active_set_offset]; - let position2 = gather![|ii| ids2[ii].active_set_offset]; - - Self { - local_com1, - local_com2, - im1, - im2, - ii1, - ii2, - local_anchor1, - local_anchor2, - position1, - position2, - } - } - - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { - let mut position1 = Isometry::from(gather![|ii| positions[self.position1[ii]]]); - let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]); - - let anchor1 = position1 * self.local_anchor1; - let anchor2 = position2 * self.local_anchor2; - - let com1 = position1 * self.local_com1; - let com2 = position2 * self.local_com2; - - let err = anchor1 - anchor2; - - let centered_anchor1 = anchor1 - com1; - let centered_anchor2 = anchor2 - com2; - - let cmat1 = centered_anchor1.gcross_matrix(); - let cmat2 = centered_anchor2.gcross_matrix(); - - // NOTE: the -cmat1 is just a simpler way of doing cmat1.transpose() - // because it is anti-symmetric. - #[cfg(feature = "dim3")] - let lhs = self.ii1.quadform(&cmat1).add_diagonal(self.im1) - + self.ii2.quadform(&cmat2).add_diagonal(self.im2); - - // In 2D we just unroll the computation because - // it's just easier that way. - #[cfg(feature = "dim2")] - let lhs = { - let m11 = - self.im1 + self.im2 + cmat1.x * cmat1.x * self.ii1 + cmat2.x * cmat2.x * self.ii2; - let m12 = cmat1.x * cmat1.y * self.ii1 + cmat2.x * cmat2.y * self.ii2; - let m22 = - self.im1 + self.im2 + cmat1.y * cmat1.y * self.ii1 + cmat2.y * cmat2.y * self.ii2; - SdpMatrix::new(m11, m12, m22) - }; - - let inv_lhs = lhs.inverse_unchecked(); - let impulse = inv_lhs * -(err * SimdReal::splat(params.joint_erp)); - - position1.translation.vector += impulse * self.im1; - position2.translation.vector -= impulse * self.im2; - - let angle1 = self.ii1.transform_vector(centered_anchor1.gcross(impulse)); - let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse)); - - position1.rotation = Rotation::new(angle1) * position1.rotation; - position2.rotation = Rotation::new(angle2) * position2.rotation; - - for ii in 0..SIMD_WIDTH { - positions[self.position1[ii]] = position1.extract(ii); - } - for ii in 0..SIMD_WIDTH { - positions[self.position2[ii]] = position2.extract(ii); - } - } -} - -#[derive(Debug)] -pub(crate) struct WBallPositionGroundConstraint { - position2: [usize; SIMD_WIDTH], - anchor1: Point, - im2: SimdReal, - ii2: AngularInertia, - local_anchor2: Point, - local_com2: Point, -} - -impl WBallPositionGroundConstraint { - pub fn from_params( - rbs1: [&RigidBodyPosition; SIMD_WIDTH], - rbs2: ( - [&RigidBodyMassProps; SIMD_WIDTH], - [&RigidBodyIds; SIMD_WIDTH], - ), - cparams: [&BallJoint; SIMD_WIDTH], - flipped: [bool; SIMD_WIDTH], - ) -> Self { - let poss1 = rbs1; - let (mprops2, ids2) = rbs2; - - let position1 = Isometry::from(gather![|ii| poss1[ii].next_position]); - let anchor1 = position1 - * Point::from(gather![|ii| if flipped[ii] { - cparams[ii].local_anchor2 - } else { - cparams[ii].local_anchor1 - }]); - let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective