From 754a48b7ff6d8c58b1ee08651e60112900b60455 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Tue, 25 Aug 2020 22:10:25 +0200 Subject: First public release of Rapier. --- src/dynamics/solver/categorization.rs | 70 +++ src/dynamics/solver/delta_vel.rs | 18 + src/dynamics/solver/interaction_groups.rs | 434 +++++++++++++ src/dynamics/solver/island_solver.rs | 73 +++ .../joint_constraint/ball_position_constraint.rs | 165 +++++ .../ball_position_constraint_wide.rs | 199 ++++++ .../joint_constraint/ball_velocity_constraint.rs | 238 +++++++ .../ball_velocity_constraint_wide.rs | 329 ++++++++++ .../joint_constraint/fixed_position_constraint.rs | 139 +++++ .../joint_constraint/fixed_velocity_constraint.rs | 370 +++++++++++ .../fixed_velocity_constraint_wide.rs | 472 ++++++++++++++ .../solver/joint_constraint/joint_constraint.rs | 340 ++++++++++ .../joint_constraint/joint_position_constraint.rs | 169 +++++ src/dynamics/solver/joint_constraint/mod.rs | 65 ++ .../prismatic_position_constraint.rs | 170 +++++ .../prismatic_velocity_constraint.rs | 558 +++++++++++++++++ .../prismatic_velocity_constraint_wide.rs | 687 +++++++++++++++++++++ .../revolute_position_constraint.rs | 142 +++++ .../revolute_velocity_constraint.rs | 294 +++++++++ .../revolute_velocity_constraint_wide.rs | 397 ++++++++++++ src/dynamics/solver/mod.rs | 56 ++ src/dynamics/solver/parallel_island_solver.rs | 259 ++++++++ src/dynamics/solver/parallel_position_solver.rs | 582 +++++++++++++++++ src/dynamics/solver/parallel_velocity_solver.rs | 485 +++++++++++++++ src/dynamics/solver/position_constraint.rs | 246 ++++++++ src/dynamics/solver/position_constraint_wide.rs | 217 +++++++ src/dynamics/solver/position_ground_constraint.rs | 189 ++++++ .../solver/position_ground_constraint_wide.rs | 199 ++++++ src/dynamics/solver/position_solver.rs | 451 ++++++++++++++ src/dynamics/solver/velocity_constraint.rs | 401 ++++++++++++ src/dynamics/solver/velocity_constraint_wide.rs | 344 +++++++++++ src/dynamics/solver/velocity_ground_constraint.rs | 300 +++++++++ .../solver/velocity_ground_constraint_wide.rs | 300 +++++++++ src/dynamics/solver/velocity_solver.rs | 405 ++++++++++++ 34 files changed, 9763 insertions(+) create mode 100644 src/dynamics/solver/categorization.rs create mode 100644 src/dynamics/solver/delta_vel.rs create mode 100644 src/dynamics/solver/interaction_groups.rs create mode 100644 src/dynamics/solver/island_solver.rs create mode 100644 src/dynamics/solver/joint_constraint/ball_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/fixed_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/mod.rs create mode 100644 src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/revolute_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/mod.rs create mode 100644 src/dynamics/solver/parallel_island_solver.rs create mode 100644 src/dynamics/solver/parallel_position_solver.rs create mode 100644 src/dynamics/solver/parallel_velocity_solver.rs create mode 100644 src/dynamics/solver/position_constraint.rs create mode 100644 src/dynamics/solver/position_constraint_wide.rs create mode 100644 src/dynamics/solver/position_ground_constraint.rs create mode 100644 src/dynamics/solver/position_ground_constraint_wide.rs create mode 100644 src/dynamics/solver/position_solver.rs create mode 100644 src/dynamics/solver/velocity_constraint.rs create mode 100644 src/dynamics/solver/velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/velocity_ground_constraint.rs create mode 100644 src/dynamics/solver/velocity_ground_constraint_wide.rs create mode 100644 src/dynamics/solver/velocity_solver.rs (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs new file mode 100644 index 0000000..9ddf791 --- /dev/null +++ b/src/dynamics/solver/categorization.rs @@ -0,0 +1,70 @@ +use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet}; +use crate::geometry::{ContactManifold, ContactManifoldIndex, KinematicsCategory}; + +pub(crate) fn categorize_position_contacts( + bodies: &RigidBodySet, + manifolds: &[&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + out_point_point_ground: &mut Vec, + out_plane_point_ground: &mut Vec, + out_point_point: &mut Vec, + out_plane_point: &mut Vec, +) { + for manifold_i in manifold_indices { + let manifold = &manifolds[*manifold_i]; + let rb1 = &bodies[manifold.body_pair.body1]; + let rb2 = &bodies[manifold.body_pair.body2]; + + if !rb1.is_dynamic() || !rb2.is_dynamic() { + match manifold.kinematics.category { + KinematicsCategory::PointPoint => out_point_point_ground.push(*manifold_i), + KinematicsCategory::PlanePoint => out_plane_point_ground.push(*manifold_i), + } + } else { + match manifold.kinematics.category { + KinematicsCategory::PointPoint => out_point_point.push(*manifold_i), + KinematicsCategory::PlanePoint => out_plane_point.push(*manifold_i), + } + } + } +} + +pub(crate) fn categorize_velocity_contacts( + bodies: &RigidBodySet, + manifolds: &[&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + out_ground: &mut Vec, + out_not_ground: &mut Vec, +) { + for manifold_i in manifold_indices { + let manifold = &manifolds[*manifold_i]; + let rb1 = &bodies[manifold.body_pair.body1]; + let rb2 = &bodies[manifold.body_pair.body2]; + + if !rb1.is_dynamic() || !rb2.is_dynamic() { + out_ground.push(*manifold_i) + } else { + out_not_ground.push(*manifold_i) + } + } +} + +pub(crate) fn categorize_joints( + bodies: &RigidBodySet, + joints: &[JointGraphEdge], + joint_indices: &[JointIndex], + ground_joints: &mut Vec, + nonground_joints: &mut Vec, +) { + for joint_i in joint_indices { + let joint = &joints[*joint_i].weight; + let rb1 = &bodies[joint.body1]; + let rb2 = &bodies[joint.body2]; + + if !rb1.is_dynamic() || !rb2.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 new file mode 100644 index 0000000..c4a424b --- /dev/null +++ b/src/dynamics/solver/delta_vel.rs @@ -0,0 +1,18 @@ +use crate::math::{AngVector, Vector}; +use na::{Scalar, SimdRealField}; + +#[derive(Copy, Clone, Debug)] +//#[repr(align(64))] +pub(crate) struct DeltaVel { + pub linear: Vector, + pub angular: AngVector, +} + +impl DeltaVel { + pub fn zero() -> Self { + Self { + linear: na::zero(), + angular: na::zero(), + } + } +} diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs new file mode 100644 index 0000000..04acaaf --- /dev/null +++ b/src/dynamics/solver/interaction_groups.rs @@ -0,0 +1,434 @@ +use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +#[cfg(feature = "simd-is-enabled")] +use { + crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH}, + vec_map::VecMap, +}; + +pub(crate) trait PairInteraction { + fn body_pair(&self) -> BodyPair; +} + +impl<'a> PairInteraction for &'a mut ContactManifold { + fn body_pair(&self) -> BodyPair { + self.body_pair + } +} + +impl<'a> PairInteraction for JointGraphEdge { + fn body_pair(&self) -> BodyPair { + BodyPair::new(self.weight.body1, self.weight.body2) + } +} + +#[cfg(feature = "parallel")] +pub(crate) struct ParallelInteractionGroups { + bodies_color: Vec, // Workspace. + interaction_indices: Vec, // Workspace. + interaction_colors: Vec, // Workspace. + sorted_interactions: Vec, + groups: Vec, +} + +#[cfg(feature = "parallel")] +impl ParallelInteractionGroups { + pub fn new() -> Self { + Self { + bodies_color: Vec::new(), + interaction_indices: Vec::new(), + interaction_colors: Vec::new(), + sorted_interactions: Vec::new(), + groups: Vec::new(), + } + } + + pub fn group(&self, i: usize) -> &[usize] { + let range = self.groups[i]..self.groups[i + 1]; + &self.sorted_interactions[range] + } + pub fn num_groups(&self) -> usize { + self.groups.len() - 1 + } + + pub fn group_interactions( + &mut self, + island_id: usize, + bodies: &RigidBodySet, + interactions: &[Interaction], + interaction_indices: &[usize], + ) { + let num_island_bodies = bodies.active_island(island_id).len(); + self.bodies_color.clear(); + self.interaction_indices.clear(); + self.groups.clear(); + self.sorted_interactions.clear(); + self.interaction_colors.clear(); + + let mut color_len = [0; 128]; + self.bodies_color.resize(num_island_bodies, 0u128); + self.interaction_indices + .extend_from_slice(interaction_indices); + self.interaction_colors.resize(interaction_indices.len(), 0); + let bcolors = &mut self.bodies_color; + + for (interaction_id, color) in self + .interaction_indices + .iter() + .zip(self.interaction_colors.iter_mut()) + { + let body_pair = interactions[*interaction_id].body_pair(); + let rb1 = &bodies[body_pair.body1]; + let rb2 = &bodies[body_pair.body2]; + + match (rb1.is_static(), rb2.is_static()) { + (false, false) => { + let color_mask = + bcolors[rb1.active_set_offset] | bcolors[rb2.active_set_offset]; + *color = (!color_mask).trailing_zeros() as usize; + color_len[*color] += 1; + bcolors[rb1.active_set_offset] |= 1 << *color; + bcolors[rb2.active_set_offset] |= 1 << *color; + } + (true, false) => { + let color_mask = bcolors[rb2.active_set_offset]; + *color = (!color_mask).trailing_zeros() as usize; + color_len[*color] += 1; + bcolors[rb2.active_set_offset] |= 1 << *color; + } + (false, true) => { + let color_mask = bcolors[rb1.active_set_offset]; + *color = (!color_mask).trailing_zeros() as usize; + color_len[*color] += 1; + bcolors[rb1.active_set_offset] |= 1 << *color; + } + (true, true) => unreachable!(), + } + } + + let mut sort_offsets = [0; 128]; + let mut last_offset = 0; + + for i in 0..128 { + if color_len[i] == 0 { + break; + } + + self.groups.push(last_offset); + sort_offsets[i] = last_offset; + last_offset += color_len[i]; + } + + self.sorted_interactions + .resize(interaction_indices.len(), 0); + + for (interaction_id, color) in interaction_indices + .iter() + .zip(self.interaction_colors.iter()) + { + self.sorted_interactions[sort_offsets[*color]] = *interaction_id; + sort_offsets[*color] += 1; + } + + self.groups.push(self.sorted_interactions.len()); + } +} + +pub(crate) struct InteractionGroups { + #[cfg(feature = "simd-is-enabled")] + buckets: VecMap<([usize; SIMD_WIDTH], usize)>, + #[cfg(feature = "simd-is-enabled")] + body_masks: Vec, + #[cfg(feature = "simd-is-enabled")] + pub grouped_interactions: Vec, + pub nongrouped_interactions: Vec, +} + +impl InteractionGroups { + pub fn new() -> Self { + Self { + #[cfg(feature = "simd-is-enabled")] + buckets: VecMap::new(), + #[cfg(feature = "simd-is-enabled")] + body_masks: Vec::new(), + #[cfg(feature = "simd-is-enabled")] + grouped_interactions: Vec::new(), + nongrouped_interactions: Vec::new(), + } + } + + // FIXME: there is a lot of duplicated code with group_manifolds here. + // But we don't refactor just now because we may end up with distinct + // grouping strategies in the future. + #[cfg(not(feature = "simd-is-enabled"))] + pub fn group_joints( + &mut self, + _island_id: usize, + _bodies: &RigidBodySet, + _interactions: &[JointGraphEdge], + interaction_indices: &[JointIndex], + ) { + self.nongrouped_interactions + .extend_from_slice(interaction_indices); + } + + #[cfg(feature = "simd-is-enabled")] + pub fn group_joints( + &mut self, + island_id: usize, + bodies: &RigidBodySet, + interactions: &[JointGraphEdge], + interaction_indices: &[JointIndex], + ) { + // NOTE: in 3D we have up to 10 different joint types. + // In 2D we only have 5 joint types. + #[cfg(feature = "dim3")] + const NUM_JOINT_TYPES: usize = 10; + #[cfg(feature = "dim2")] + const NUM_JOINT_TYPES: usize = 5; + + // The j-th bit of joint_type_conflicts[i] indicates that the + // j-th bucket contains a joint with a type different than `i`. + let mut joint_type_conflicts = [0u128; NUM_JOINT_TYPES]; + + // Note: each bit of a body mask indicates what bucket already contains + // a constraints involving this body. + // FIXME: currently, this is a bit overconservative because when a bucket + // is full, we don't clear the corresponding body mask bit. This may result + // in less grouped constraints. + self.body_masks + .resize(bodies.active_island(island_id).len(), 0u128); + + // NOTE: each bit of the occupied mask indicates what bucket already + // contains at least one constraint. + let mut occupied_mask = 0u128; + + for interaction_i in interaction_indices { + let interaction = &interactions[*interaction_i].weight; + let body1 = &bodies[interaction.body1]; + let body2 = &bodies[interaction.body2]; + let is_static1 = !body1.is_dynamic(); + let is_static2 = !body2.is_dynamic(); + + if is_static1 && is_static2 { + continue; + } + + let ijoint = interaction.params.type_id(); + let i1 = body1.active_set_offset; + let i2 = body2.active_set_offset; + let conflicts = + self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint]; + let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts. + let conflictfree_occupied_targets = conflictfree_targets & occupied_mask; + + let target_index = if conflictfree_occupied_targets != 0 { + // Try to fill partial WContacts first. + conflictfree_occupied_targets.trailing_zeros() + } else { + conflictfree_targets.trailing_zeros() + }; + + if target_index == 128 { + // The interaction conflicts with every bucket we can manage. + // So push it in an nongrouped interaction list that won't be combined with + // any other interactions. + self.nongrouped_interactions.push(*interaction_i); + continue; + } + + let target_mask_bit = 1 << target_index; + + let bucket = self + .buckets + .entry(target_index as usize) + .or_insert_with(|| ([0; SIMD_WIDTH], 0)); + + if bucket.1 == SIMD_LAST_INDEX { + // We completed our group. + (bucket.0)[SIMD_LAST_INDEX] = *interaction_i; + self.grouped_interactions.extend_from_slice(&bucket.0); + bucket.1 = 0; + occupied_mask &= !target_mask_bit; + + for k in 0..NUM_JOINT_TYPES { + joint_type_conflicts[k] &= !target_mask_bit; + } + } else { + (bucket.0)[bucket.1] = *interaction_i; + bucket.1 += 1; + occupied_mask |= target_mask_bit; + + for k in 0..ijoint { + joint_type_conflicts[k] |= target_mask_bit; + } + for k in ijoint + 1..NUM_JOINT_TYPES { + joint_type_conflicts[k] |= target_mask_bit; + } + } + + // NOTE: static bodies don't transmit forces. Therefore they don't + // imply any interaction conflicts. + if !is_static1 { + self.body_masks[i1] |= target_mask_bit; + } + + if !is_static2 { + self.body_masks[i2] |= target_mask_bit; + } + } + + self.nongrouped_interactions.extend( + self.buckets + .values() + .flat_map(|e| e.0.iter().take(e.1).copied()), + ); + self.buckets.clear(); + self.body_masks.iter_mut().for_each(|e| *e = 0); + + assert!( + self.grouped_interactions.len() % SIMD_WIDTH == 0, + "Invalid SIMD contact grouping." + ); + + // println!( + // "Num grouped interactions: {}, nongrouped: {}", + // self.grouped_interactions.len(), + // self.nongrouped_interactions.len() + // ); + } + + pub fn clear_groups(&mut self) { + #[cfg(feature = "simd-is-enabled")] + self.grouped_interactions.clear(); + self.nongrouped_interactions.clear(); + } + + #[cfg(not(feature = "simd-is-enabled"))] + pub fn group_manifolds( + &mut self, + _island_id: usize, + _bodies: &RigidBodySet, + _interactions: &[&mut ContactManifold], + interaction_indices: &[ContactManifoldIndex], + ) { + self.nongrouped_interactions + .extend_from_slice(interaction_indices); + } + + #[cfg(feature = "simd-is-enabled")] + pub fn group_manifolds( + &mut self, + island_id: usize, + bodies: &RigidBodySet, + interactions: &[&mut ContactManifold], + interaction_indices: &[ContactManifoldIndex], + ) { + // Note: each bit of a body mask indicates what bucket already contains + // a constraints involving this body. + // FIXME: currently, this is a bit overconservative because when a bucket + // is full, we don't clear the corresponding body mask bit. This may result + // in less grouped contacts. + // NOTE: body_masks and buckets are already cleared/zeroed at the end of each sort loop. + self.body_masks + .resize(bodies.active_island(island_id).len(), 0u128); + + // NOTE: each bit of the occupied mask indicates what bucket already + // contains at least one constraint. + let mut occupied_mask = 0u128; + let max_interaction_points = interaction_indices + .iter() + .map(|i| interactions[*i].num_active_contacts()) + .max() + .unwrap_or(1); + + // FIXME: find a way to reduce the number of iteration. + // There must be a way to iterate just once on every interaction indices + // instead of MAX_MANIFOLD_POINTS times. + for k in 1..=max_interaction_points { + for interaction_i in interaction_indices { + let interaction = &interactions[*interaction_i]; + + // FIXME: how could we avoid iterating + // on each interaction at every iteration on k? + if interaction.num_active_contacts() != k { + continue; + } + + let body1 = &bodies[interaction.body_pair.body1]; + let body2 = &bodies[interaction.body_pair.body2]; + let is_static1 = !body1.is_dynamic(); + let is_static2 = !body2.is_dynamic(); + + // FIXME: don't generate interactions between static bodies in the first place. + if is_static1 && is_static2 { + continue; + } + + let i1 = body1.active_set_offset; + let i2 = body2.active_set_offset; + let conflicts = self.body_masks[i1] | self.body_masks[i2]; + let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts. + let conflictfree_occupied_targets = conflictfree_targets & occupied_mask; + + let target_index = if conflictfree_occupied_targets != 0 { + // Try to fill partial WContacts first. + conflictfree_occupied_targets.trailing_zeros() + } else { + conflictfree_targets.trailing_zeros() + }; + + if target_index == 128 { + // The interaction conflicts with every bucket we can manage. + // So push it in an nongrouped interaction list that won't be combined with + // any other interactions. + self.nongrouped_interactions.push(*interaction_i); + continue; + } + + let target_mask_bit = 1 << target_index; + + let bucket = self + .buckets + .entry(target_index as usize) + .or_insert_with(|| ([0; SIMD_WIDTH], 0)); + + if bucket.1 == SIMD_LAST_INDEX { + // We completed our group. + (bucket.0)[SIMD_LAST_INDEX] = *interaction_i; + self.grouped_interactions.extend_from_slice(&bucket.0); + bucket.1 = 0; + occupied_mask = occupied_mask & (!target_mask_bit); + } else { + (bucket.0)[bucket.1] = *interaction_i; + bucket.1 += 1; + occupied_mask = occupied_mask | target_mask_bit; + } + + // NOTE: static bodies don't transmit forces. Therefore they don't + // imply any interaction conflicts. + if !is_static1 { + self.body_masks[i1] |= target_mask_bit; + } + + if !is_static2 { + self.body_masks[i2] |= target_mask_bit; + } + } + + self.nongrouped_interactions.extend( + self.buckets + .values() + .flat_map(|e| e.0.iter().take(e.1).copied()), + ); + self.buckets.clear(); + self.body_masks.iter_mut().for_each(|e| *e = 0); + occupied_mask = 0u128; + } + + assert!( + self.grouped_interactions.len() % SIMD_WIDTH == 0, + "Invalid SIMD contact grouping." + ); + } +} diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs new file mode 100644 index 0000000..7ce142a --- /dev/null +++ b/src/dynamics/solver/island_solver.rs @@ -0,0 +1,73 @@ +use super::{PositionSolver, VelocitySolver}; +use crate::counters::Counters; +use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; + +pub struct IslandSolver { + velocity_solver: VelocitySolver, + position_solver: PositionSolver, +} + +impl IslandSolver { + pub fn new() -> Self { + Self { + velocity_solver: VelocitySolver::new(), + position_solver: PositionSolver::new(), + } + } + + pub fn solve_island( + &mut self, + island_id: usize, + counters: &mut Counters, + params: &IntegrationParameters, + bodies: &mut RigidBodySet, + manifolds: &mut [&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + joints: &mut [JointGraphEdge], + joint_indices: &[JointIndex], + ) { + if manifold_indices.len() != 0 || joint_indices.len() != 0 { + counters.solver.velocity_assembly_time.resume(); + self.velocity_solver.init_constraints( + island_id, + params, + bodies, + manifolds, + &manifold_indices, + joints, + &joint_indices, + ); + counters.solver.velocity_assembly_time.pause(); + + counters.solver.velocity_resolution_time.resume(); + self.velocity_solver + .solve_constraints(island_id, params, bodies, manifolds, joints); + counters.solver.velocity_resolution_time.pause(); + + counters.solver.position_assembly_time.resume(); + self.position_solver.init_constraints( + island_id, + params, + bodies, + manifolds, + &manifold_indices, + joints, + &joint_indices, + ); + counters.solver.position_assembly_time.pause(); + } + + counters.solver.velocity_update_time.resume(); + bodies + .foreach_active_island_body_mut_internal(island_id, |_, rb| rb.integrate(params.dt())); + counters.solver.velocity_update_time.pause(); + + if manifold_indices.len() != 0 || joint_indices.len() != 0 { + counters.solver.position_resolution_time.resume(); + self.position_solver + .solve_constraints(island_id, params, bodies); + counters.solver.position_resolution_time.pause(); + } + } +} diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs new file mode 100644 index 0000000..21a537e --- /dev/null +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -0,0 +1,165 @@ +use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; +#[cfg(feature = "dim2")] +use crate::math::SdpMatrix; +use crate::math::{AngularInertia, Isometry, Point, Rotation}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; + +#[derive(Debug)] +pub(crate) struct BallPositionConstraint { + position1: usize, + position2: usize, + + local_com1: Point, + local_com2: Point, + + im1: f32, + im2: f32, + + ii1: AngularInertia, + ii2: AngularInertia, + + local_anchor1: Point, + local_anchor2: Point, +} + +impl BallPositionConstraint { + pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &BallJoint) -> Self { + Self { + local_com1: rb1.mass_properties.local_com, + local_com2: rb2.mass_properties.local_com, + im1: rb1.mass_properties.inv_mass, + im2: rb2.mass_properties.inv_mass, + ii1: rb1.world_inv_inertia_sqrt.squared(), + ii2: rb2.world_inv_inertia_sqrt.squared(), + local_anchor1: cparams.local_anchor1, + local_anchor2: cparams.local_anchor2, + position1: rb1.active_set_offset, + position2: rb2.active_set_offset, + } + } + + 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; + + positions[self.position1 as usize] = position1; + positions[self.position2 as usize] = position2; + } +} + +#[derive(Debug)] +pub(crate) struct BallPositionGroundConstraint { + position2: usize, + anchor1: Point, + im2: f32, + ii2: AngularInertia, + local_anchor2: Point, + local_com2: Point, +} + +impl BallPositionGroundConstraint { + pub fn from_params( + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &BallJoint, + flipped: bool, + ) -> Self { + 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: rb1.predicted_position * cparams.local_anchor2, + im2: rb2.mass_properties.inv_mass, + ii2: rb2.world_inv_inertia_sqrt.squared(), + local_anchor2: cparams.local_anchor1, + position2: rb2.active_set_offset, + local_com2: rb2.mass_properties.local_com, + } + } else { + Self { + anchor1: rb1.predicted_position * cparams.local_anchor1, + im2: rb2.mass_properties.inv_mass, + ii2: rb2.world_inv_inertia_sqrt.squared(), + local_anchor2: cparams.local_anchor2, + position2: rb2.active_set_offset, + local_com2: rb2.mass_properties.local_com, + } + } + } + + 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; + 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 new file mode 100644 index 0000000..c552d57 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -0,0 +1,199 @@ +use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; +#[cfg(feature = "dim2")] +use crate::math::SdpMatrix; +use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdFloat, 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: SimdFloat, + im2: SimdFloat, + + ii1: AngularInertia, + ii2: AngularInertia, + + local_anchor1: Point, + local_anchor2: Point, +} + +impl WBallPositionConstraint { + pub fn from_params( + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&BallJoint; SIMD_WIDTH], + ) -> Self { + let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]); + let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]); + let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1 = AngularInertia::::from( + array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ) + .squared(); + let ii2 = AngularInertia::::from( + array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ) + .squared(); + let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); + let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); + let position1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + 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(array![|ii| positions[self.position1[ii]]; SIMD_WIDTH]); + let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]); + + 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 * SimdFloat::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: SimdFloat, + ii2: AngularInertia, + local_anchor2: Point, + local_com2: Point, +} + +impl WBallPositionGroundConstraint { + pub fn from_params( + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&BallJoint; SIMD_WIDTH], + flipped: [bool; SIMD_WIDTH], + ) -> Self { + let position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]); + let anchor1 = position1 + * Point::from(array![|ii| if flipped[ii] { + cparams[ii].local_anchor2 + } else { + cparams[ii].local_anchor1 + }; SIMD_WIDTH]); + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2 = AngularInertia::::from( + array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ) + .squared(); + let local_anchor2 = Point::from(array![|ii| if flipped[ii] { + cparams[ii].local_anchor1 + } else { + cparams[ii].local_anchor2 + }; SIMD_WIDTH]); + let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]); + + Self { + anchor1, + im2, + ii2, + local_anchor2, + position2, + local_com2, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]); + + 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 * SimdFloat::splat(params.joint_erp)); + position2.translation.vector -= impulse * self.im2; + + let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse)); + position2.rotation = Rotation::new(angle2) * position2.rotation; + + for ii in 0..SIMD_WIDTH { + positions[self.position2[ii]] = position2.extract(ii); + } + } +} diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs new file mode 100644 index 0000000..97ba244 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -0,0 +1,238 @@ +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, +}; +use crate::math::{SdpMatrix, Vector}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; + +#[derive(Debug)] +pub(crate) struct BallVelocityConstraint { + mj_lambda1: usize, + mj_lambda2: usize, + + joint_id: JointIndex, + + rhs: Vector, + pub(crate) impulse: Vector, + + gcross1: Vector, + gcross2: Vector, + + inv_lhs: SdpMatrix, + + im1: f32, + im2: f32, +} + +impl BallVelocityConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &BallJoint, + ) -> Self { + let anchor1 = rb1.position * cparams.local_anchor1 - rb1.world_com; + let anchor2 = rb2.position * cparams.local_anchor2 - rb2.world_com; + + let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1); + let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2); + let im1 = rb1.mass_properties.inv_mass; + let im2 = rb2.mass_properties.inv_mass; + + let rhs = -(vel1 - vel2); + let lhs; + + let cmat1 = anchor1.gcross_matrix(); + let cmat2 = anchor2.gcross_matrix(); + + #[cfg(feature = "dim3")] + { + lhs = rb2 + .world_inv_inertia_sqrt + .squared() + .quadform(&cmat2) + .add_diagonal(im2) + + rb1 + .world_inv_inertia_sqrt + .squared() + .quadform(&cmat1) + .add_diagonal(im1); + } + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + { + let ii1 = rb1.world_inv_inertia_sqrt.squared(); + let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2; + let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2; + let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2; + lhs = SdpMatrix::new(m11, m12, m22) + } + + let gcross1 = rb1.world_inv_inertia_sqrt.transform_lin_vector(anchor1); + let gcross2 = rb2.world_inv_inertia_sqrt.transform_lin_vector(anchor2); + + let inv_lhs = lhs.inverse_unchecked(); + + BallVelocityConstraint { + joint_id, + mj_lambda1: rb1.active_set_offset, + mj_lambda2: rb2.active_set_offset, + im1, + im2, + impulse: cparams.impulse * params.warmstart_coeff, + gcross1, + gcross2, + rhs, + inv_lhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + mj_lambda1.linear += self.im1 * self.impulse; + mj_lambda1.angular += self.gcross1.gcross(self.impulse); + mj_lambda2.linear -= self.im2 * self.impulse; + mj_lambda2.angular -= self.gcross2.gcross(self.impulse); + + mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let vel1 = mj_lambda1.linear + mj_lambda1.angular.gcross(self.gcross1); + let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); + let dvel = -vel1 + vel2 + self.rhs; + + let impulse = self.inv_lhs * dvel; + self.impulse += impulse; + + mj_lambda1.linear += self.im1 * impulse; + mj_lambda1.angular += self.gcross1.gcross(impulse); + + mj_lambda2.linear -= self.im2 * impulse; + mj_lambda2.angular -= self.gcross2.gcross(impulse); + + mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + let joint = &mut joints_all[self.joint_id].weight; + if let JointParams::BallJoint(ball) = &mut joint.params { + ball.impulse = self.impulse + } + } +} + +#[derive(Debug)] +pub(crate) struct BallVelocityGroundConstraint { + mj_lambda2: usize, + joint_id: JointIndex, + rhs: Vector, + impulse: Vector, + gcross2: Vector, + inv_lhs: SdpMatrix, + im2: f32, +} + +impl BallVelocityGroundConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &BallJoint, + flipped: bool, + ) -> Self { + let (anchor1, anchor2) = if flipped { + ( + rb1.position * cparams.local_anchor2 - rb1.world_com, + rb2.position * cparams.local_anchor1 - rb2.world_com, + ) + } else { + ( + rb1.position * cparams.local_anchor1 - rb1.world_com, + rb2.position * cparams.local_anchor2 - rb2.world_com, + ) + }; + + let im2 = rb2.mass_properties.inv_mass; + let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1); + let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2); + let rhs = vel2 - vel1; + + let cmat2 = anchor2.gcross_matrix(); + let gcross2 = rb2.world_inv_inertia_sqrt.transform_lin_vector(anchor2); + + let lhs; + + #[cfg(feature = "dim3")] + { + lhs = rb2 + .world_inv_inertia_sqrt + .squared() + .quadform(&cmat2) + .add_diagonal(im2); + } + + #[cfg(feature = "dim2")] + { + let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let m11 = im2 + cmat2.x * cmat2.x * ii2; + let m12 = cmat2.x * cmat2.y * ii2; + let m22 = im2 + cmat2.y * cmat2.y * ii2; + lhs = SdpMatrix::new(m11, m12, m22) + } + + let inv_lhs = lhs.inverse_unchecked(); + + BallVelocityGroundConstraint { + joint_id, + mj_lambda2: rb2.active_set_offset, + im2, + impulse: cparams.impulse * params.warmstart_coeff, + gcross2, + rhs, + inv_lhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + mj_lambda2.linear -= self.im2 * self.impulse; + mj_lambda2.angular -= self.gcross2.gcross(self.impulse); + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); + let dvel = vel2 + self.rhs; + + let impulse = self.inv_lhs * dvel; + self.impulse += impulse; + + mj_lambda2.linear -= self.im2 * impulse; + mj_lambda2.angular -= self.gcross2.gcross(impulse); + + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + // FIXME: duplicated code with the non-ground constraint. + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + let joint = &mut joints_all[self.joint_id].weight; + if let JointParams::BallJoint(ball) = &mut joint.params { + ball.impulse = self.impulse + } + } +} diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs new file mode 100644 index 0000000..b96f3b8 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs @@ -0,0 +1,329 @@ +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, +}; +use crate::math::{ + AngVector, AngularInertia, Isometry, Point, SdpMatrix, SimdFloat, Vector, SIMD_WIDTH, +}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +use simba::simd::SimdValue; + +#[derive(Debug)] +pub(crate) struct WBallVelocityConstraint { + mj_lambda1: [usize; SIMD_WIDTH], + mj_lambda2: [usize; SIMD_WIDTH], + + joint_id: [JointIndex; SIMD_WIDTH], + + rhs: Vector, + pub(crate) impulse: Vector, + + gcross1: Vector, + gcross2: Vector, + + inv_lhs: SdpMatrix, + + im1: SimdFloat, + im2: SimdFloat, +} + +impl WBallVelocityConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&BallJoint; SIMD_WIDTH], + ) -> Self { + let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1_sqrt = AngularInertia::::from( + array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + + let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( + array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); + let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); + let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + + let anchor1 = position1 * local_anchor1 - world_com1; + let anchor2 = position2 * local_anchor2 - world_com2; + + let vel1: Vector = linvel1 + angvel1.gcross(anchor1); + let vel2: Vector = linvel2 + angvel2.gcross(anchor2); + let rhs = -(vel1 - vel2); + let lhs; + + let cmat1 = anchor1.gcross_matrix(); + let cmat2 = anchor2.gcross_matrix(); + + #[cfg(feature = "dim3")] + { + lhs = ii2_sqrt.squared().quadform(&cmat2).add_diagonal(im2) + + ii1_sqrt.squared().quadform(&cmat1).add_diagonal(im1); + } + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + { + let ii1 = ii1_sqrt.squared(); + let ii2 = ii2_sqrt.squared(); + let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2; + let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2; + let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2; + lhs = SdpMatrix::new(m11, m12, m22) + } + + let gcross1 = ii1_sqrt.transform_lin_vector(anchor1); + let gcross2 = ii2_sqrt.transform_lin_vector(anchor2); + + let inv_lhs = lhs.inverse_unchecked(); + + WBallVelocityConstraint { + joint_id, + mj_lambda1, + mj_lambda2, + im1, + im2, + impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + gcross1, + gcross2, + rhs, + inv_lhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + mj_lambda1.linear += self.impulse * self.im1; + mj_lambda1.angular += self.gcross1.gcross(self.impulse); + mj_lambda2.linear -= self.impulse * self.im2; + mj_lambda2.angular -= self.gcross2.gcross(self.impulse); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); + mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); + } + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1: DeltaVel = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + let mut mj_lambda2: DeltaVel = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let vel1 = mj_lambda1.linear + mj_lambda1.angular.gcross(self.gcross1); + let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); + let dvel = -vel1 + vel2 + self.rhs; + + let impulse = self.inv_lhs * dvel; + self.impulse += impulse; + + mj_lambda1.linear += impulse * self.im1; + mj_lambda1.angular += self.gcross1.gcross(impulse); + + mj_lambda2.linear -= impulse * self.im2; + mj_lambda2.angular -= self.gcross2.gcross(impulse); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); + mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); + } + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + for ii in 0..SIMD_WIDTH { + let joint = &mut joints_all[self.joint_id[ii]].weight; + if let JointParams::BallJoint(ball) = &mut joint.params { + ball.impulse = self.impulse.extract(ii) + } + } + } +} + +#[derive(Debug)] +pub(crate) struct WBallVelocityGroundConstraint { + mj_lambda2: [usize; SIMD_WIDTH], + joint_id: [JointIndex; SIMD_WIDTH], + rhs: Vector, + pub(crate) impulse: Vector, + gcross2: Vector, + inv_lhs: SdpMatrix, + im2: SimdFloat, +} + +impl WBallVelocityGroundConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&BallJoint; SIMD_WIDTH], + flipped: [bool; SIMD_WIDTH], + ) -> Self { + let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + let local_anchor1 = Point::from( + array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], + ); + + let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( + array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let local_anchor2 = Point::from( + array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH], + ); + let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + + let anchor1 = position1 * local_anchor1 - world_com1; + let anchor2 = position2 * local_anchor2 - world_com2; + + let vel1: Vector = linvel1 + angvel1.gcross(anchor1); + let vel2: Vector = linvel2 + angvel2.gcross(anchor2); + let rhs = vel2 - vel1; + let lhs; + + let cmat2 = anchor2.gcross_matrix(); + let gcross2 = ii2_sqrt.transform_lin_vector(anchor2); + + #[cfg(feature = "dim3")] + { + lhs = ii2_sqrt.squared().quadform(&cmat2).add_diagonal(im2); + } + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + { + let ii2 = ii2_sqrt.squared(); + let m11 = im2 + cmat2.x * cmat2.x * ii2; + let m12 = cmat2.x * cmat2.y * ii2; + let m22 = im2 + cmat2.y * cmat2.y * ii2; + lhs = SdpMatrix::new(m11, m12, m22) + } + + let inv_lhs = lhs.inverse_unchecked(); + + WBallVelocityGroundConstraint { + joint_id, + mj_lambda2, + im2, + impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + gcross2, + rhs, + inv_lhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + mj_lambda2.linear -= self.impulse * self.im2; + mj_lambda2.angular -= self.gcross2.gcross(self.impulse); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2: DeltaVel = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); + let dvel = vel2 + self.rhs; + + let impulse = self.inv_lhs * dvel; + self.impulse += impulse; + + mj_lambda2.linear -= impulse * self.im2; + mj_lambda2.angular -= self.gcross2.gcross(impulse); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + for ii in 0..SIMD_WIDTH { + let joint = &mut joints_all[self.joint_id[ii]].weight; + if let JointParams::BallJoint(ball) = &mut joint.params { + ball.impulse = self.impulse.extract(ii) + } + } + } +} diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs new file mode 100644 index 0000000..24001cd --- /dev/null +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs @@ -0,0 +1,139 @@ +use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody}; +use crate::math::{AngularInertia, Isometry, Point, Rotation}; +use crate::utils::WAngularInertia; + +#[derive(Debug)] +pub(crate) struct FixedPositionConstraint { + position1: usize, + position2: usize, + local_anchor1: Isometry, + local_anchor2: Isometry, + local_com1: Point, + local_com2: Point, + im1: f32, + im2: f32, + ii1: AngularInertia, + ii2: AngularInertia, + + lin_inv_lhs: f32, + ang_inv_lhs: AngularInertia, +} + +impl FixedPositionConstraint { + pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &FixedJoint) -> Self { + let ii1 = rb1.world_inv_inertia_sqrt.squared(); + let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let im1 = rb1.mass_propert