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/joint/ball_joint.rs | 148 --- src/dynamics/joint/fixed_joint.rs | 73 +- src/dynamics/joint/generic_joint.rs | 144 --- src/dynamics/joint/impulse_joint/impulse_joint.rs | 20 + .../joint/impulse_joint/impulse_joint_set.rs | 356 +++++++ src/dynamics/joint/impulse_joint/mod.rs | 6 + src/dynamics/joint/joint.rs | 143 --- src/dynamics/joint/joint_data.rs | 270 ++++++ src/dynamics/joint/joint_set.rs | 359 ------- src/dynamics/joint/mod.rs | 27 +- src/dynamics/joint/motor_model.rs | 61 ++ src/dynamics/joint/multibody_joint/mod.rs | 15 + src/dynamics/joint/multibody_joint/multibody.rs | 1021 ++++++++++++++++++++ .../joint/multibody_joint/multibody_joint.rs | 571 +++++++++++ .../joint/multibody_joint/multibody_joint_set.rs | 352 +++++++ .../joint/multibody_joint/multibody_link.rs | 173 ++++ .../joint/multibody_joint/multibody_workspace.rs | 27 + .../joint/multibody_joint/unit_multibody_joint.rs | 122 +++ src/dynamics/joint/prismatic_joint.rs | 289 ++---- src/dynamics/joint/revolute_joint.rs | 220 ++--- src/dynamics/joint/spherical_joint.rs | 91 ++ src/dynamics/joint/spring_model.rs | 65 -- 22 files changed, 3285 insertions(+), 1268 deletions(-) delete mode 100644 src/dynamics/joint/ball_joint.rs delete mode 100644 src/dynamics/joint/generic_joint.rs create mode 100644 src/dynamics/joint/impulse_joint/impulse_joint.rs create mode 100644 src/dynamics/joint/impulse_joint/impulse_joint_set.rs create mode 100644 src/dynamics/joint/impulse_joint/mod.rs delete mode 100644 src/dynamics/joint/joint.rs create mode 100644 src/dynamics/joint/joint_data.rs delete mode 100644 src/dynamics/joint/joint_set.rs create mode 100644 src/dynamics/joint/motor_model.rs create mode 100644 src/dynamics/joint/multibody_joint/mod.rs create mode 100644 src/dynamics/joint/multibody_joint/multibody.rs create mode 100644 src/dynamics/joint/multibody_joint/multibody_joint.rs create mode 100644 src/dynamics/joint/multibody_joint/multibody_joint_set.rs create mode 100644 src/dynamics/joint/multibody_joint/multibody_link.rs create mode 100644 src/dynamics/joint/multibody_joint/multibody_workspace.rs create mode 100644 src/dynamics/joint/multibody_joint/unit_multibody_joint.rs create mode 100644 src/dynamics/joint/spherical_joint.rs delete mode 100644 src/dynamics/joint/spring_model.rs (limited to 'src/dynamics/joint') diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs deleted file mode 100644 index 0b626c0..0000000 --- a/src/dynamics/joint/ball_joint.rs +++ /dev/null @@ -1,148 +0,0 @@ -use crate::dynamics::SpringModel; -use crate::math::{Point, Real, Rotation, UnitVector, Vector}; - -#[derive(Copy, Clone, PartialEq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// A joint that removes all relative linear motion between a pair of points on two bodies. -pub struct BallJoint { - /// Where the ball joint is attached on the first body, expressed in the first body local frame. - pub local_anchor1: Point, - /// Where the ball joint is attached on the second body, expressed in the second body local frame. - pub local_anchor2: Point, - /// The impulse applied by this joint on the first body. - /// - /// The impulse applied to the second body is given by `-impulse`. - pub impulse: Vector, - - /// The target relative angular velocity the motor will attempt to reach. - #[cfg(feature = "dim2")] - pub motor_target_vel: Real, - /// The target relative angular velocity the motor will attempt to reach. - #[cfg(feature = "dim3")] - pub motor_target_vel: Vector, - /// The target angular position of this joint, expressed as an axis-angle. - pub motor_target_pos: Rotation, - /// The motor's stiffness. - /// See the documentation of `SpringModel` for more information on this parameter. - pub motor_stiffness: Real, - /// The motor's damping. - /// See the documentation of `SpringModel` for more information on this parameter. - pub motor_damping: Real, - /// The maximal impulse the motor is able to deliver. - pub motor_max_impulse: Real, - /// The angular impulse applied by the motor. - #[cfg(feature = "dim2")] - pub motor_impulse: Real, - /// The angular impulse applied by the motor. - #[cfg(feature = "dim3")] - pub motor_impulse: Vector, - /// The spring-like model used by the motor to reach the target velocity and . - pub motor_model: SpringModel, - - /// Are the limits enabled for this joint? - pub limits_enabled: bool, - /// The axis of the limit cone for this joint, if the local-space of the first body. - pub limits_local_axis1: UnitVector, - /// The axis of the limit cone for this joint, if the local-space of the first body. - pub limits_local_axis2: UnitVector, - /// The maximum angle allowed between the two limit axes in world-space. - pub limits_angle: Real, - /// The impulse applied to enforce joint limits. - pub limits_impulse: Real, -} - -impl BallJoint { - /// Creates a new Ball joint from two anchors given on the local spaces of the respective bodies. - pub fn new(local_anchor1: Point, local_anchor2: Point) -> Self { - Self::with_impulse(local_anchor1, local_anchor2, Vector::zeros()) - } - - pub(crate) fn with_impulse( - local_anchor1: Point, - local_anchor2: Point, - impulse: Vector, - ) -> Self { - Self { - local_anchor1, - local_anchor2, - impulse, - motor_target_vel: na::zero(), - motor_target_pos: Rotation::identity(), - motor_stiffness: 0.0, - motor_damping: 0.0, - motor_impulse: na::zero(), - motor_max_impulse: Real::MAX, - motor_model: SpringModel::default(), - limits_enabled: false, - limits_local_axis1: Vector::x_axis(), - limits_local_axis2: Vector::x_axis(), - limits_angle: Real::MAX, - limits_impulse: 0.0, - } - } - - /// Can a SIMD constraint be used for resolving this joint? - pub fn supports_simd_constraints(&self) -> bool { - // SIMD ball constraints don't support motors and limits right now. - !self.limits_enabled - && (self.motor_max_impulse == 0.0 - || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0)) - } - - /// Set the spring-like model used by the motor to reach the desired target velocity and position. - pub fn configure_motor_model(&mut self, model: SpringModel) { - self.motor_model = model; - } - - /// Sets the target velocity and velocity correction factor this motor. - #[cfg(feature = "dim2")] - pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) { - self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor) - } - - /// Sets the target velocity and velocity correction factor this motor. - #[cfg(feature = "dim3")] - pub fn configure_motor_velocity(&mut self, target_vel: Vector, factor: Real) { - self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor) - } - - /// Sets the target orientation this motor needs to reach. - pub fn configure_motor_position( - &mut self, - target_pos: Rotation, - stiffness: Real, - damping: Real, - ) { - self.configure_motor(target_pos, na::zero(), stiffness, damping) - } - - /// Sets the target orientation this motor needs to reach. - #[cfg(feature = "dim2")] - pub fn configure_motor( - &mut self, - target_pos: Rotation, - target_vel: Real, - stiffness: Real, - damping: Real, - ) { - self.motor_target_vel = target_vel; - self.motor_target_pos = target_pos; - self.motor_stiffness = stiffness; - self.motor_damping = damping; - } - - /// Configure both the target orientation and target velocity of the motor. - #[cfg(feature = "dim3")] - pub fn configure_motor( - &mut self, - target_pos: Rotation, - target_vel: Vector, - stiffness: Real, - damping: Real, - ) { - self.motor_target_vel = target_vel; - self.motor_target_pos = target_pos; - self.motor_stiffness = stiffness; - self.motor_damping = damping; - } -} diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs index 6424750..10c4d7e 100644 --- a/src/dynamics/joint/fixed_joint.rs +++ b/src/dynamics/joint/fixed_joint.rs @@ -1,38 +1,55 @@ -use crate::math::{Isometry, Real, SpacialVector}; +use crate::dynamics::{JointAxesMask, JointData}; +use crate::math::{Isometry, Point, Real}; -#[derive(Copy, Clone, PartialEq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// A joint that prevents all relative movement between two bodies. -/// -/// Given two frames of references, this joint aims to ensure these frame always coincide in world-space. +#[derive(Copy, Clone, Debug, PartialEq)] pub struct FixedJoint { - /// The frame of reference for the first body affected by this joint, expressed in the local frame - /// of the first body. - pub local_frame1: Isometry, - /// The frame of reference for the second body affected by this joint, expressed in the local frame - /// of the first body. - pub local_frame2: Isometry, - /// The impulse applied to the first body affected by this joint. - /// - /// The impulse applied to the second body affected by this joint is given by `-impulse`. - /// This combines both linear and angular impulses: - /// - In 2D, `impulse.xy()` gives the linear impulse, and `impulse.z` the angular impulse. - /// - In 3D, `impulse.xyz()` gives the linear impulse, and `(impulse[3], impulse[4], impulse[5])` the angular impulse. - pub impulse: SpacialVector, + data: JointData, } impl FixedJoint { - /// Creates a new fixed joint from the frames of reference of both bodies. - pub fn new(local_frame1: Isometry, local_frame2: Isometry) -> Self { - Self { - local_frame1, - local_frame2, - impulse: SpacialVector::zeros(), - } + pub fn new() -> Self { + #[cfg(feature = "dim2")] + let mask = JointAxesMask::X | JointAxesMask::Y | JointAxesMask::ANG_X; + #[cfg(feature = "dim3")] + let mask = JointAxesMask::X + | JointAxesMask::Y + | JointAxesMask::Z + | JointAxesMask::ANG_X + | JointAxesMask::ANG_Y + | JointAxesMask::ANG_Z; + + let data = JointData::default().lock_axes(mask); + Self { data } + } + + #[must_use] + pub fn local_frame1(mut self, local_frame: Isometry) -> Self { + self.data = self.data.local_frame1(local_frame); + self + } + + #[must_use] + pub fn local_frame2(mut self, local_frame: Isometry) -> Self { + self.data = self.data.local_frame2(local_frame); + self } - /// Can a SIMD constraint be used for resolving this joint? - pub fn supports_simd_constraints(&self) -> bool { - true + #[must_use] + pub fn local_anchor1(mut self, anchor1: Point) -> Self { + self.data = self.data.local_anchor1(anchor1); + self + } + + #[must_use] + pub fn local_anchor2(mut self, anchor2: Point) -> Self { + self.data = self.data.local_anchor2(anchor2); + self + } +} + +impl Into for FixedJoint { + fn into(self) -> JointData { + self.data } } diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs deleted file mode 100644 index 78f1e84..0000000 --- a/src/dynamics/joint/generic_joint.rs +++ /dev/null @@ -1,144 +0,0 @@ -use crate::dynamics::{BallJoint, FixedJoint, PrismaticJoint, RevoluteJoint}; -use crate::math::{Isometry, Real, SpacialVector}; -use crate::na::{Rotation3, UnitQuaternion}; - -#[derive(Copy, Clone, Debug, PartialEq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// A joint that prevents all relative movement between two bodies. -/// -/// Given two frames of references, this joint aims to ensure these frame always coincide in world-space. -pub struct GenericJoint { - /// The frame of reference for the first body affected by this joint, expressed in the local frame - /// of the first body. - pub local_anchor1: Isometry, - /// The frame of reference for the second body affected by this joint, expressed in the local frame - /// of the first body. - pub local_anchor2: Isometry, - /// The impulse applied to the first body affected by this joint. - /// - /// The impulse applied to the second body affected by this joint is given by `-impulse`. - /// This combines both linear and angular impulses: - /// - In 2D, `impulse.xy()` gives the linear impulse, and `impulse.z` the angular impulse. - /// - In 3D, `impulse.xyz()` gives the linear impulse, and `(impulse[3], impulse[4], impulse[5])` the angular impulse. - pub impulse: SpacialVector, - - pub min_position: SpacialVector, - pub max_position: SpacialVector, - pub min_velocity: SpacialVector, - pub max_velocity: SpacialVector, - /// The minimum negative impulse the joint can apply on each DoF. Must be <= 0.0 - pub min_impulse: SpacialVector, - /// The maximum positive impulse the joint can apply on each DoF. Must be >= 0.0 - pub max_impulse: SpacialVector, - /// The minimum negative position impulse the joint can apply on each DoF. Must be <= 0.0 - pub min_pos_impulse: SpacialVector, - /// The maximum positive position impulse the joint can apply on each DoF. Must be >= 0.0 - pub max_pos_impulse: SpacialVector, -} - -impl GenericJoint { - /// Creates a new fixed joint from the frames of reference of both bodies. - pub fn new(local_anchor1: Isometry, local_anchor2: Isometry) -> Self { - Self { - local_anchor1, - local_anchor2, - impulse: SpacialVector::zeros(), - min_position: SpacialVector::zeros(), - max_position: SpacialVector::zeros(), - min_velocity: SpacialVector::zeros(), - max_velocity: SpacialVector::zeros(), - min_impulse: SpacialVector::repeat(-Real::MAX), - max_impulse: SpacialVector::repeat(Real::MAX), - min_pos_impulse: SpacialVector::repeat(-Real::MAX), - max_pos_impulse: SpacialVector::repeat(Real::MAX), - } - } - - pub fn set_dof_vel(&mut self, dof: u8, target_vel: Real, max_force: Real) { - self.min_velocity[dof as usize] = target_vel; - self.max_velocity[dof as usize] = target_vel; - self.min_impulse[dof as usize] = -max_force; - self.max_impulse[dof as usize] = max_force; - } - - pub fn free_dof(&mut self, dof: u8) { - self.min_position[dof as usize] = -Real::MAX; - self.max_position[dof as usize] = Real::MAX; - self.min_velocity[dof as usize] = -Real::MAX; - self.max_velocity[dof as usize] = Real::MAX; - self.min_impulse[dof as usize] = 0.0; - self.max_impulse[dof as usize] = 0.0; - self.min_pos_impulse[dof as usize] = 0.0; - self.max_pos_impulse[dof as usize] = 0.0; - } - - pub fn set_dof_limits(&mut self, dof: u8, min: Real, max: Real) { - self.min_position[dof as usize] = min; - self.max_position[dof as usize] = max; - } -} - -impl From for GenericJoint { - fn from(joint: RevoluteJoint) -> Self { - let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]]; - let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]]; - let quat1 = UnitQuaternion::from_basis_unchecked(&basis1); - let quat2 = UnitQuaternion::from_basis_unchecked(&basis2); - let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1); - let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2); - - let mut result = Self::new(local_anchor1, local_anchor2); - result.free_dof(3); - - if joint.motor_damping != 0.0 { - result.set_dof_vel(3, joint.motor_target_vel, joint.motor_max_impulse); - } - - result.impulse[0] = joint.impulse[0]; - result.impulse[1] = joint.impulse[1]; - result.impulse[2] = joint.impulse[2]; - result.impulse[3] = joint.motor_impulse; - result.impulse[4] = joint.impulse[3]; - result.impulse[5] = joint.impulse[4]; - - result - } -} - -impl From for GenericJoint { - fn from(joint: BallJoint) -> Self { - let local_anchor1 = Isometry::new(joint.local_anchor1.coords, na::zero()); - let local_anchor2 = Isometry::new(joint.local_anchor2.coords, na::zero()); - - let mut result = Self::new(local_anchor1, local_anchor2); - result.impulse[0] = joint.impulse[0]; - result.impulse[1] = joint.impulse[1]; - result.impulse[2] = joint.impulse[2]; - result.free_dof(3); - result.free_dof(4); - result.free_dof(5); - result - } -} - -impl From for GenericJoint { - fn from(joint: PrismaticJoint) -> Self { - let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]]; - let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]]; - let quat1 = UnitQuaternion::from_basis_unchecked(&basis1); - let quat2 = UnitQuaternion::from_basis_unchecked(&basis2); - let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1); - let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2); - - let mut result = Self::new(local_anchor1, local_anchor2); - result.free_dof(0); - result.set_dof_limits(0, joint.limits[0], joint.limits[1]); - result - } -} - -impl From for GenericJoint { - fn from(joint: FixedJoint) -> Self { - Self::new(joint.local_anchor1, joint.local_anchor2) - } -} diff --git a/src/dynamics/joint/impulse_joint/impulse_joint.rs b/src/dynamics/joint/impulse_joint/impulse_joint.rs new file mode 100644 index 0000000..a12c533 --- /dev/null +++ b/src/dynamics/joint/impulse_joint/impulse_joint.rs @@ -0,0 +1,20 @@ +use crate::dynamics::{JointData, JointHandle, RigidBodyHandle}; +use crate::math::{Real, SpacialVector}; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Debug, PartialEq)] +/// A joint attached to two bodies. +pub struct ImpulseJoint { + /// Handle to the first body attached to this joint. + pub body1: RigidBodyHandle, + /// Handle to the second body attached to this joint. + pub body2: RigidBodyHandle, + + pub data: JointData, + pub impulses: SpacialVector, + + // A joint needs to know its handle to simplify its removal. + pub(crate) handle: JointHandle, + #[cfg(feature = "parallel")] + pub(crate) constraint_index: usize, +} diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs new file mode 100644 index 0000000..183b668 --- /dev/null +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -0,0 +1,356 @@ +use super::ImpulseJoint; +use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex}; + +use crate::data::arena::Arena; +use crate::data::{BundleSet, Coarena, ComponentSet, ComponentSetMut}; +use crate::dynamics::{IslandManager, RigidBodyActivation, RigidBodyIds, RigidBodyType}; +use crate::dynamics::{JointData, RigidBodyHandle}; + +/// The unique identifier of a joint added to the joint set. +/// The unique identifier of a collider added to a collider set. +#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[repr(transparent)] +pub struct JointHandle(pub crate::data::arena::Index); + +impl JointHandle { + /// Converts this handle into its (index, generation) components. + pub fn into_raw_parts(self) -> (u32, u32) { + self.0.into_raw_parts() + } + + /// Reconstructs an handle from its (index, generation) components. + pub fn from_raw_parts(id: u32, generation: u32) -> Self { + Self(crate::data::arena::Index::from_raw_parts(id, generation)) + } + + /// An always-invalid joint handle. + pub fn invalid() -> Self { + Self(crate::data::arena::Index::from_raw_parts( + crate::INVALID_U32, + crate::INVALID_U32, + )) + } +} + +pub(crate) type JointIndex = usize; +pub(crate) type JointGraphEdge = crate::data::graph::Edge; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Default)] +/// A set of impulse_joints that can be handled by a physics `World`. +pub struct ImpulseJointSet { + rb_graph_ids: Coarena, + joint_ids: Arena, // Map joint handles to edge ids on the graph. + joint_graph: InteractionGraph, +} + +impl ImpulseJointSet { + /// Creates a new empty set of impulse_joints. + pub fn new() -> Self { + Self { + rb_graph_ids: Coarena::new(), + joint_ids: Arena::new(), + joint_graph: InteractionGraph::new(), + } + } + + /// The number of impulse_joints on this set. + pub fn len(&self) -> usize { + self.joint_graph.graph.edges.len() + } + + /// `true` if there are no impulse_joints in this set. + pub fn is_empty(&self) -> bool { + self.joint_graph.graph.edges.is_empty() + } + + /// Retrieve the joint graph where edges are impulse_joints and nodes are rigid body handles. + pub fn joint_graph(&self) -> &InteractionGraph { + &self.joint_graph + } + + /// Iterates through all the impulse_joints attached to the given rigid-body. + pub fn joints_with<'a>( + &'a self, + body: RigidBodyHandle, + ) -> impl Iterator { + self.rb_graph_ids + .get(body.0) + .into_iter() + .flat_map(move |id| self.joint_graph.interactions_with(*id)) + } + + /// Is the given joint handle valid? + pub fn contains(&self, handle: JointHandle) -> bool { + self.joint_ids.contains(handle.0) + } + + /// Gets the joint with the given handle. + pub fn get(&self, handle: JointHandle) -> Option<&ImpulseJoint> { + let id = self.joint_ids.get(handle.0)?; + self.joint_graph.graph.edge_weight(*id) + } + + /// Gets a mutable reference to the joint with the given handle. + pub fn get_mut(&mut self, handle: JointHandle) -> Option<&mut ImpulseJoint> { + let id = self.joint_ids.get(handle.0)?; + self.joint_graph.graph.edge_weight_mut(*id) + } + + /// Gets the joint with the given handle without a known generation. + /// + /// This is useful when you know you want the joint at position `i` but + /// don't know what is its current generation number. Generation numbers are + /// used to protect from the ABA problem because the joint position `i` + /// are recycled between two insertion and a removal. + /// + /// Using this is discouraged in favor of `self.get(handle)` which does not + /// suffer form the ABA problem. + pub fn get_unknown_gen(&self, i: u32) -> Option<(&ImpulseJoint, JointHandle)> { + let (id, handle) = self.joint_ids.get_unknown_gen(i)?; + Some(( + self.joint_graph.graph.edge_weight(*id)?, + JointHandle(handle), + )) + } + + /// Gets a mutable reference to the joint with the given handle without a known generation. + /// + /// This is useful when you know you want the joint at position `i` but + /// don't know what is its current generation number. Generation numbers are + /// used to protect from the ABA problem because the joint position `i` + /// are recycled between two insertion and a removal. + /// + /// Using this is discouraged in favor of `self.get_mut(handle)` which does not + /// suffer form the ABA problem. + pub fn get_unknown_gen_mut(&mut self, i: u32) -> Option<(&mut ImpulseJoint, JointHandle)> { + let (id, handle) = self.joint_ids.get_unknown_gen(i)?; + Some(( + self.joint_graph.graph.edge_weight_mut(*id)?, + JointHandle(handle), + )) + } + + /// Iterates through all the joint on this set. + pub fn iter(&self) -> impl Iterator { + self.joint_graph + .graph + .edges + .iter() + .map(|e| (e.weight.handle, &e.weight)) + } + + /// Iterates mutably through all the joint on this set. + pub fn iter_mut(&mut self) -> impl Iterator { + self.joint_graph + .graph + .edges + .iter_mut() + .map(|e| (e.weight.handle, &mut e.weight)) + } + + // /// The set of impulse_joints as an array. + // pub(crate) fn impulse_joints(&self) -> &[JointGraphEdge] { + // // self.joint_graph + // // .graph + // // .edges + // // .iter_mut() + // // .map(|e| &mut e.weight) + // } + + #[cfg(not(feature = "parallel"))] + pub(crate) fn joints_mut(&mut self) -> &mut [JointGraphEdge] { + &mut self.joint_graph.graph.edges[..] + } + + #[cfg(feature = "parallel")] + pub(crate) fn joints_vec_mut(&mut self) -> &mut Vec { + &mut self.joint_graph.graph.edges + } + + /// Inserts a new joint into this set and retrieve its handle. + pub fn insert( + &mut self, + body1: RigidBodyHandle, + body2: RigidBodyHandle, + data: impl Into, + ) -> JointHandle { + let data = data.into(); + let handle = self.joint_ids.insert(0.into()); + let joint = ImpulseJoint { + body1, + body2, + data, + impulses: na::zero(), + handle: JointHandle(handle), + #[cfg(feature = "parallel")] + constraint_index: 0, + }; + + let default_id = InteractionGraph::<(), ()>::invalid_graph_index(); + let mut graph_index1 = *self + .rb_graph_ids + .ensure_element_exist(joint.body1.0, default_id); + let mut graph_index2 = *self + .rb_graph_ids + .ensure_element_exist(joint.body2.0, default_id); + + // NOTE: the body won't have a graph index if it does not + // have any joint attached. + if !InteractionGraph::::is_graph_index_valid(graph_index1) { + graph_index1 = self.joint_graph.graph.add_node(joint.body1); + self.rb_graph_ids.insert(joint.body1.0, graph_index1); + } + + if !InteractionGraph::::is_graph_index_valid(graph_index2) { + graph_index2 = self.joint_graph.graph.add_node(joint.body2); + self.rb_graph_ids.insert(joint.body2.0, graph_index2); + } + + self.joint_ids[handle] = self.joint_graph.add_edge(graph_index1, graph_index2, joint); + JointHandle(handle) + } + + /// Retrieve all the impulse_joints happening between two active bodies. + // NOTE: this is very similar to the code from NarrowPhase::select_active_interactions. + pub(crate) fn select_active_interactions( + &self, + islands: &IslandManager, + bodies: &Bodies, + out: &mut Vec>, + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet, + { + for out_island in &mut out[..islands.num_islands()] { + out_island.clear(); + } + + // FIXME: don't iterate through all the interactions. + for (i, edge) in self.joint_graph.graph.edges.iter().enumerate() { + let joint = &edge.weight; + + let (status1, activation1, ids1): ( + &RigidBodyType, + &RigidBodyActivation, + &RigidBodyIds, + ) = bodies.index_bundle(joint.body1.0); + let (status2, activation2, ids2): ( + &RigidBodyType, + &RigidBodyActivation, + &RigidBodyIds, + ) = bodies.index_bundle(joint.body2.0); + + if (status1.is_dynamic() || status2.is_dynamic()) + && (!status1.is_dynamic() || !activation1.sleeping) + && (!status2.is_dynamic() || !activation2.sleeping) + { + let island_index = if !status1.is_dynamic() { + ids2.active_island_id + } else { + ids1.active_island_id + }; + + out[island_index].push(i); + } + } + } + + /// Removes a joint from this set. + /// + /// If `wake_up` is set to `true`, then the bodies attached to this joint will be + /// automatically woken up. + pub fn remove( + &mut self, + handle: JointHandle, + islands: &mut IslandManager, + bodies: &mut Bodies, + wake_up: bool, + ) -> Option + where + Bodies: ComponentSetMut + + ComponentSet + + ComponentSetMut, + { + let id = self.joint_ids.remove(handle.0)?; + let endpoints = self.joint_graph.graph.edge_endpoints(id)?; + + if wake_up { + // Wake-up the bodies attached to this joint. + if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.0) { + islands.wake_up(bodies, *rb_handle, true); + } + if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.1) { + islands.wake_up(bodies, *rb_handle, true); + } + } + + let removed_joint = self.joint_graph.graph.remove_edge(id); + + if let Some(edge) = self.joint_graph.graph.edge_weight(id) { + self.joint_ids[edge.handle.0] = id; + } + + removed_joint + } + + /// Deletes all the impulse_joints attached to the given rigid-body. + /// + /// The provided rigid-body handle is not required to identify a rigid-body that + /// is still contained by the `bodies` component set. + /// Returns the (now invalid) handles of the removed impulse_joints. + pub fn remove_joints_attached_to_rigid_body( + &mut self, + handle: RigidBodyHandle, + islands: &mut IslandManager, + bodies: &mut Bodies, + ) -> Vec + where + Bodies: ComponentSetMut + + ComponentSet + + ComponentSetMut, + { + let mut deleted = vec![]; + + if let Some(deleted_id) = self + .rb_graph_ids + .remove(handle.0, InteractionGraph::<(), ()>::invalid_graph_index()) + { + if InteractionGraph::<(), ()>::is_graph_index_valid(deleted_id) { + // We have to delete each joint one by one in order to: + // - Wake-up the attached bodies. + // - Update our Handle -> graph edge mapping. + // Delete the node. + let to_delete: Vec<_> = self + .joint_graph + .interactions_with(deleted_id) + .map(|e| (e.0, e.1, e.2.handle)) + .collect(); + for (h1, h2, to_delete_handle) in to_delete { + deleted.push(to_delete_handle); + let to_delete_edge_id = self.joint_ids.remove(to_delete_handle.0).unwrap(); + self.joint_graph.graph.remove_edge(to_delete_edge_id); + + // Update the id of the edge which took the place of the deleted one. + if let Some(j) = self.joint_graph.graph.edge_weight_mut(to_delete_edge_id) { + self.joint_ids[j.handle.0] = to_delete_edge_id; + } + + // Wake up the attached bodies. + islands.wake_up(bodies, h1, true); + islands.wake_up(bodies, h2, true); + } + + if let Some(other) = self.joint_graph.remove_node(deleted_id) { + // One rigid-body joint graph index may have been invalidated + // so we need to update it. + self.rb_graph_ids.insert(other.0, deleted_id); + } + } + } + + deleted + } +} diff --git a/src/dynamics/joint/impulse_joint/mod.rs b/src/dynamics/joint/impulse_joint/mod.rs new file mode 100644 index 0000000..2afe078 --- /dev/null +++ b/src/dynamics/joint/impulse_joint/mod.rs @@ -0,0 +1,6 @@ +pub use self::impulse_joint::ImpulseJoint; +pub use self::impulse_joint_set::{ImpulseJointSet, JointHandle}; +pub(crate) use self::impulse_joint_set::{JointGraphEdge, JointIndex}; + +mod impulse_joint; +mod impulse_joint_set; diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs deleted file mode 100644 index 0c2c864..0000000 --- a/src/dynamics/joint/joint.rs +++ /dev/null @@ -1,143 +0,0 @@ -#[cfg(feature = "dim3")] -use crate::dynamics::RevoluteJoint; -use crate::dynamics::{BallJoint, FixedJoint, JointHandle, PrismaticJoint, RigidBodyHandle}; - -#[derive(Copy, Clone, PartialEq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// An enum grouping all possible types of joints. -pub enum JointParams { - /// A Ball joint that removes all relative linear degrees of freedom between the affected bodies. - BallJoint(BallJoint), - /// A fixed joint that removes all relative degrees of freedom between the affected bodies. - FixedJoint(FixedJoint), - /// A prismatic joint that removes all degrees of degrees of freedom between the affected - /// bodies except for the translation along one axis. - PrismaticJoint(PrismaticJoint), - #[cfg(feature = "dim3")] - /// A revolute joint that removes all degrees of degrees of freedom between the affected - /// bodies except for the translation along one axis. - RevoluteJoint(RevoluteJoint), - // GenericJoint(GenericJoint), -} - -impl JointParams { - /// An integer identifier for each type of joint. - pub fn type_id(&self) -> usize { - match self { - JointParams::BallJoint(_) => 0, - JointParams::FixedJoint(_) => 1, - JointParams::PrismaticJoint(_) => 2, - // JointParams::GenericJoint(_) => 3, - #[cfg(feature = "dim3")] - JointParams::RevoluteJoint(_) => 4, - } - } - - /// Gets a reference to the underlying ball joint, if `self` is one. - pub fn as_ball_joint(&self) -> Option<&BallJoint> { - if let JointParams::BallJoint(j) = self { - Some(j) - } else { - None - } - } - - /// Gets a reference to the underlying fixed joint, if `self` is one. - pub fn as_fixed_joint(&self) -> Option<&FixedJoint> { - if let JointParams::FixedJoint(j) = self { - Some(j) - } else { - None - } - } - - // /// Gets a reference to the underlying generic joint, if `self` is one. - // pub fn as_generic_joint(&self) -> Option<&GenericJoint> { - // if let JointParams::GenericJoint(j) = self { - // Some(j) - // } else { - // None - // } - // } - - /// Gets a reference to the underlying prismatic joint, if `self` is one. - pub fn as_prismatic_joint(&self) -> Option<&PrismaticJoint> { - if let JointParams::PrismaticJoint(j) = self { - Some(j) - } else { - None - } - } - - /// Gets a reference to the underlying revolute joint, if `self` is one. - #[cfg(feature = "dim3")] - pub fn as_revolute_joint(&self) -> Option<&RevoluteJoint> { - if let JointParams::RevoluteJoint(j) = self { - Some(j) - } else { - None - } - } -} - -impl From for JointParams { - fn from(j: BallJoint) -> Self { - JointParams::BallJoint(j) - } -} - -impl From for JointParams { - fn from(j: FixedJoint) -> Self { - JointParams::FixedJoint(j) - } -} - -// impl From for JointParams { -// fn from(j: GenericJoint) -> Self { -// JointParams::GenericJoint(j) -// } -// } - -#[cfg(feature = "dim3")] -impl From for JointParams { - fn from(j: RevoluteJoint) -> Self { - JointParams::RevoluteJoint(j) - } -} - -impl From for JointParams { - fn from(j: PrismaticJoint) -> Self { - JointParams::PrismaticJoint(j) - } -} - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Clone)] -/// A joint attached to two bodies. -pub struct Joint { - /// Handle to the first body attached to this joint. - pub body1: RigidBodyHandle, - /// Handle to the second body attached to this joint. - pub body2: RigidBodyHandle, - // A joint needs to know its handle to simplify its removal. - pub(crate) handle: JointHandle, - #[cfg(feature = "parallel")] - pub(crate) constraint_index: usize, - #[cfg(feature = "parallel")] - pub(crate) position_constraint_index: usize, - /// The joint geometric parameters and impulse. - pub params: JointParams, -} - -impl Joint { - /// Can this joint use SIMD-accelerated constraint formulations? - pub fn supports_simd_constraints(&self) -> bool { - match &self.params { - JointParams::PrismaticJoint(joint) => joint.supports_simd_constraints(), - JointParams::FixedJoint(joint) => joint.supports_simd_constraints(), - JointParams::BallJoint(joint) => joint.supports_simd_constraints(), - #[cfg(feature = "dim3")] - JointParams::RevoluteJoint(joint) => joint.supports_simd_constraints(), - } - } -} diff --git a/src/dynamics/joint/joint_data.rs b/src/dynamics/joint/joint_data.rs new file mode 100644 index 0000000..35d832d --- /dev/null +++ b/src/dynamics/joint/joint_data.rs @@ -0,0 +1,270 @@ +use crate::dynamics::solver::MotorParameters; +use crate::dynamics::MotorModel; +use crate::math::{Isometry, Point, Real, Rotation, UnitVector, SPATIAL_DIM}; +use crate::utils::WBasis; + +#[cfg(feature = "dim3")] +bitflags::bitflags! { + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] + pub struct JointAxesMask: u8 { + const FREE = 0; + const X = 1 << 0; + const Y = 1 << 1; + const Z = 1 << 2; + const ANG_X = 1 << 3; + const ANG_Y = 1 << 4; + const ANG_Z = 1 << 5; + } +} + +#[cfg(feature = "dim2")] +bitflags::bitflags! { + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] + pub struct JointAxesMask: u8 { + const FREE = 0; + const X = 1 << 0; + const Y = 1 << 1; + const ANG_X = 1 << 2; + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub enum JointAxis { + X = 0, + Y, + #[cfg(feature = "dim3")] + Z, + AngX, + #[cfg(feature = "dim3")] + AngY, + #[cfg(feature = "dim3")] + AngZ, +} + +impl From for JointAxesMask { + fn from(axis: JointAxis) -> Self { + JointAxesMask::from_bits(1 << axis as usize).unwrap() + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct JointLimits { + pub min: Real, + pub max: Real, + pub impulse: Real, +} + +impl Default for JointLimits { + fn default() -> Self { + Self { + min: -Real::MAX, + max: Real::MAX, + impulse: 0.0, + } + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct JointMotor { + pub target_vel: Real, + pub target_pos: Real, + pub stiffness: Real, + pub damping: Real, + pub max_impulse: Real, + pub impulse: Real, + pub model: MotorModel, +} + +impl Default for JointMotor { + fn default() -> Self { + Self { + target_pos: 0.0, + target_vel: 0.0, + stiffness: 0.0, + damping: 0.0, + max_impulse: Real::MAX, + impulse: 0.0, + model: MotorModel::VelocityBased, + } + } +} + +impl JointMotor { + pub(crate) fn motor_params(&self, dt: Real) -> MotorParameters { + let (stiffness, damping, gamma, _keep_lhs) = + self.model + .combine_coefficients(dt, self.stiffness, self.damping); + MotorParameters { + stiffness, + damping, + gamma, + // keep_lhs, + target_pos: self.target_pos, + target_vel: self.target_vel, + max_impulse: self.max_impulse, + } + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct JointData { + pub local_frame1: Isometry, + pub local_frame2: Isometry, + pub locked_axes: JointAxesMask, + pub limit_axes: JointAxesMask, + pub motor_axes: JointAxesMask, + pub limits: [JointLimits; SPATIAL_DIM], + pub motors: [JointMotor; SPATIAL_DIM], +} + +impl Default for JointData { + fn default() -> Self { + Self { + local_frame1: Isometry::identity(), + local_frame2: Isometry::identity(), + locked_axes: JointAxesMask::FREE, + limit_axes: JointAxesMask::FREE, + motor_axes: JointAxesMask::FREE, + limits: [JointLimits::default(); SPATIAL_DIM], + motors: [JointMotor::default(); SPATIAL_DIM], + } + } +} + +impl JointData { + #[must_use] + pub fn new(locked_axes: JointAxesMask) -> Self { + Self::default().lock_axes(locked_axes) + } + + /// Can this joint use SIMD-accelerated constraint formulations? + pub fn supports_simd_constraints(&self) -> bool { + self.limit_axes.is_empty() && self.motor_axes.is_empty() + } + + #[must_use] + pub fn lock_axes(mut self, axes: JointAxesMask) -> Self { + self.locked_axes |= axes; + self + } + + fn complete_ang_frame(axis: UnitVector) -> Rotation { + let basis = axis.orthonormal_basis(); + + #[cfg(feature = "dim2")] + { + use na::{Matrix2, Rotation2, UnitComplex}; + let mat = Matrix2::from_columns(&[axis.into_inner(), basis[0]]); + let rotmat = Rotation2::from_matrix_unchecked(mat); + UnitComplex::from_rotation_matrix(&rotmat) + } + + #[cfg(feature = "dim3")] + { + use na::{Matrix3, Rotation3, UnitQuaternion}; + let mat = Matrix3::from_columns(&[axis.into_inner(), basis[0], basis[1]]); + let rotmat = Rotation3::from_matrix_unchecked(mat); + UnitQuaternion::from_rotation_matrix(&rotmat) + } + } + + #[must_use] + pub fn local_frame1(mut self, local_frame: Isometry) -> Self { + self.local_frame1 = local_frame; + self + } + + #[must_use] + pub fn local_frame2(mut self, local_frame: Isometry) -> Self { + self.local_frame2 = local_frame; + self + } + + #[must_use] + pub fn local_axis1(mut self, local_axis: UnitVector) -> Self { + self.local_frame1.rotation = Self::complete_ang_frame(local_axis); + self + } + + #[must_use] + pub fn local_axis2(mut self, local_axis: UnitVector) -> Self { + self.local_frame2.rotation = Self::complete_ang_frame(local_axis); + self + } + + #[must_use] + pub fn local_anchor1(mut self, anchor1: Point) -> Self { + self.local_frame1.translation.vector = anchor1.coords; + self + } + + #[must_use] + pub fn local_anchor2(mut self, anchor2: Point) -> Self { + self.local_frame2.translation.vector = anchor2.coords; + self + } + + #[must_use] + pub fn limit_axis(mut self, axis: JointAxis, limits: [Real; 2]) -> Self { + let i = axis as usize; + self.limit_axes |= axis.into(); + self.limits[i].min = limits[0]; + self.limits[i].max = limits[1]; + self + } + + /// Set the spring-like model used by the motor to reach the desired target velocity and position. + pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self { + self.motors[axis as usize].model = model; + self + } + + /// Sets the target velocity this motor needs to reach. + pub fn motor_velocity(self, axis: JointAxis, target_vel: Real, factor: Real) -> Self { + self.motor_axis( + axis, + self.motors[axis as usize].target_pos, + target_vel, + 0.0, + factor, + ) + } + + /// Sets the target angle this motor needs to reach. + pub fn motor_position( + self, + axis: JointAxis, + target_pos: Real, + stiffness: Real, + damping: Real, + ) -> Self { + self.motor_axis(axis, target_pos, 0.0, stiffness, damping) + } + + /// Configure both the target angle and target velocity of the motor. + pub fn motor_axis( + mut self, + axis: JointAxis, + target_pos: Real, + target_vel: Real, + stiffness: Real, + damping: Real, + ) -> Self { + self.motor_axes |= axis.into(); + let i = axis as usize; + self.motors[i].target_vel = target_vel; + self.motors[i].target_pos = target_pos; + self.motors[i].stiffness = stiffness; + self.motors[i].damping = damping; + self + } + + pub fn motor_max_impulse(mut self, axis: JointAxis, max_impulse: Real) -> Self { + self.motors[axis as usize].max_impulse = max_impulse; + self + } +} diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs deleted file mode 100644 index aba2aa8..0000000 --- a/src/dynamics/joint/joint_set.rs +++ /dev/null @@ -1,359 +0,0 @@ -use super::Joint; -use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex}; - -use crate::data::arena::Arena; -use crate::data::{BundleSet, Coarena, ComponentSet, ComponentSetMut}; -use crate::dynamics::{IslandManager, RigidBodyActivation, RigidBodyIds, RigidBodyType}; -use crate::dynamics::{JointParams, RigidBodyHandle}; - -/// The unique identifier of a joint added to the joint set. -/// The unique identifier of a collider added to a collider set. -#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[repr(transparent)] -pub struct JointHandle(pub crate::data::arena::Index); - -impl JointHandle { - /// Converts this handle into its (index, generation) components. - pub fn into_raw_parts(self) -> (u32, u32) { - self.0.into_raw_parts() - } - - /// Reconstructs an handle from its (index, generation) components. - pub fn from_raw_parts(id: u32, generation: u32) -> Self { - Self(crate::data::arena::Index::from_raw_parts(id, generation)) - } - - /// An always-invalid joint handle. - pub fn invalid() -> Self { - Self(crate::data::arena::Index::from_raw_parts( - crate::INVALID_U32, - crate::INVALID_U32, - )) - } -} - -pub(crate) type JointIndex = usize; -pub(crate) type JointGraphEdge = crate::data::graph::Edge; - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Clone, Default)] -/// A set of joints that can be handled by a physics `World`. -pub struct JointSet { - rb_graph_ids: Coarena, - joint_ids: Arena, // Map joint handles to edge ids on the graph. - joint_graph: InteractionGraph, -} - -impl JointSet { - /// Creates a new empty set of joints. - pub fn new() -> Self { - Self { - rb_graph_ids: Coarena::new(), - joint_ids: Arena::new(), - joint_graph: InteractionGraph::new(), - } - } - - /// The number of joints on this set. - pub fn len(&self) -> usize { - self.joint_graph.graph.edges.len() - } - - /// `true` if there are no joints in this set. - pub fn is_empty(&self) -> bool { - self.joint_graph.graph.edges.is_empty() - } - - /// Retrieve the joint graph where edges are joints and nodes are rigid body handles. - pub fn joint_graph(&self) -> &InteractionGraph { - &self.joint_graph - } - - /// Iterates through all the joitns attached to the given rigid-body. - pub fn joints_with<'a>( - &'a self, - body: RigidBodyHandle, - ) -> impl Iterator { - self.rb_graph_ids - .get(body.0) - .into_iter() - .flat_map(move |id| self.joint_graph.interactions_with(*id)) - } - - /// Is the given joint handle valid? - pub fn contains(&self, handle: JointHandle) -> bool { - self.joint_ids.contains(handle.0) - } - - /// Gets the joint with the given handle. - pub fn get(&self, handle: JointHandle) -> Option<&Joint> { - let id = self.joint_ids.get(handle.0)?; - self.joint_graph.graph.edge_weight(*id) - } - - /// Gets a mutable reference to the joint with the given handle. - pub fn get_mut(&mut self, handle: JointHandle) -> Option<&mut Joint> { - let id = self.joint_ids.get(handle.0)?; - self.joint_graph.graph.edge_weight_mut(*id) - } - - /// Gets the joint with the given handle without a known generation. - /// - /// This is useful when you know you want the joint at position `i` but - /// don't know what is its current generation number. Generation numbers are - /// used to protect from the ABA problem because the joint position `i` - /// are recycled between two insertion and a removal. - /// - /// Using this is discouraged in favor of `self.get(handle)` which does not - /// suffer form the ABA problem. - pub fn get_unknown_gen(&self, i: u32) -> Option<(&Joint, JointHandle)> { - let (id, handle) = self.joint_ids.get_unknown_gen(i)?; - Some(( - self.joint_graph.graph.edge_weight(*id)?, - JointHandle(handle), - )) - } - - /// Gets a mutable reference to the joint with the given handle without a known generation. - /// - /// This is useful when you know you want the joint at position `i` but - /// don't know what is its current generation number. Generation numbers are - /// used to protect from the ABA problem because the joint position `i` - /// are recycled between two insertion and a removal. - /// - /// Using this is discouraged in favor of `self.get_mut(handle)` which does not - /// suffer form the ABA problem. - pub fn get_unknown_gen_mut(&mut self, i: u32) -> Option<(&mut Joint, JointHandle)> { - let (id, handle) = self.joint_ids.get_unknown_gen(i)?; - Some(( - self.joint_graph.graph.edge_weight_mut(*id)?, - JointHandle(handle), - )) - } - - /// Iterates through all the joint on this set. - pub fn iter(&self) -> impl Iterator { - self.joint_graph - .graph - .edges - .iter() - .map(|e| (e.weight.handle, &e.weight)) - } - - /// Iterates mutably through all the joint on this set. - pub fn iter_mut(&mut self) -> impl Iterator { - self.joint_graph - .graph - .edges - .iter_mut() - .map(|e| (e.weight.handle, &mut e.weight)) - } - - // /// The set of joints as an array. - // pub(crate) fn joints(&self) -> &[JointGraphEdge] { - // // self.joint_graph - // // .graph - // // .edges - // // .iter_mut() - // // .map(|e| &mut e.weight) - // } - - #[cfg(not(feature = "parallel"))] - pub(crate) fn joints_mut(&mut self) -> &mut [JointGraphEdge] { - &mut self.joint_graph.graph.edges[..] - } - - #[cfg(feature = "parallel")] - pub(crate) fn joints_vec_mut(&mut self) -> &mut Vec { - &mut self.joint_graph.graph.edges - } - - /// Inserts a new joint into this set and retrieve its handle. - pub fn insert( - &mut self, - body1: RigidBodyHandle, - body2: RigidBodyHandle, - joint_params: J, - ) -> JointHandle - where - J: Into, - { - let handle = self.joint_ids.insert(0.into()); - let joint = Joint { - body1, - body2, - handle: JointHandle(handle), - #[cfg(feature = "parallel")] - constraint_index: 0, - #[cfg(feature = "parallel")] - position_constraint_index: 0, - params: joint_params.into(), - }; - - let default_id = InteractionGraph::<(), ()>::invalid_graph_index(); - let mut graph_index1 = *self - .rb_graph_ids - .ensure_element_exist(joint.body1.0, default_id); - let mut graph_index2 = *self - .rb_graph_ids - .ensure_element_exist(joint.body2.0, default_id); - - // NOTE: the body won't have a graph index if it does not - // have any joint attached. - if !InteractionGraph::::is_graph_index_valid(graph_index1) { - graph_index1 = self.joint_graph.graph.add_node(joint.body1); - self.rb_graph_ids.insert(joint.body1.0, graph_index1); - } - - if !InteractionGraph::::is_graph_index_valid(graph_index2) { - graph_index2 = self.joint_graph.graph.add_node(joint.body2); - self.rb_graph_ids.insert(joint.body2.0, graph_index2); - } - - self.joint_ids[handle] = self.joint_graph.add_edge(graph_index1, graph_index2, joint); - JointHandle(handle) - } - - /// Retrieve all the joints happening between two active bodies. - // NOTE: this is very similar to the code from NarrowPhase::select_active_interactions. - pub(crate) fn select_active_interactions( - &self, - islands: &IslandManager, - bodies: &Bodies, - out: &mut Vec>, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet, - { - for out_island in &mut out[..islands.num_islands()] { - out_island.clear(); - } - - // FIXME: don't iterate through all the interactions. - for (i, edge) in self.joint_graph.graph.edges.iter().enumerate() { - let joint = &edge.weight; - - let (status1, activation1, ids1): ( - &RigidBodyType, - &RigidBodyActivation, - &RigidBodyIds, - ) = bodies.index_bundle(joint.body1.0); - let (status2, activation2, ids2): ( - &RigidBodyType, - &RigidBodyActivation, - &RigidBodyIds, - ) = bodies.index_bundle(joint.body2.0); - - if (status1.is_dynamic() || status2.is_dynamic()) - && (!status1.is_dynamic() || !activation1.sleeping) - && (!status2.is_dynamic() || !activation2.sleeping) - { - let island_index = if !status1.is_dynamic() { - ids2.active_island_id - } else { - ids1.active_island_id - }; - - out[island_index].push(i); - } - } - } - - /// Removes a joint from this set. - /// - /// If `wake_up` is set to `true`, then the bodies attached to this joint will be - /// automatically woken up. - pub fn remove( - &mut self, - handle: JointHandle, - islands: &mut IslandManager, - bodies: &mut Bodies, - wake_up: bool, - ) -> Option - where - Bodies: ComponentSetMut - + ComponentSet - + ComponentSetMut, - { - let id = self.joint_ids.remove(handle.0)?; - let endpoints = self.joint_graph.graph.edge_endpoints(id)?; - - if wake_up { - // Wake-up the bodies attached to this joint. - if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.0) { - islands.wake_up(bodies, *rb_handle, true); - } - if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.1) { - islands.wake_up(bodies, *rb_handle, true); - } - } - - let removed_joint = self.joint_graph.graph.remove_edge(id); - - if let Some(edge) = self.joint_graph.graph.edge_weight(id) { - self.joint_ids[edge.handle.0] = id; - } - - removed_joint - } - - /// Deletes all the joints attached to the given rigid-body. - /// - /// The provided rigid-body handle is not required to identify a rigid-body that - /// is still contained by the `bodies` component set. - /// Returns the (now invalid) handles of the removed joints. - pub fn remove_joints_attached_to_rigid_body( - &mut self, - handle: RigidBodyHandle, - islands: &mut IslandManager, - bodies: &mut Bodies, - ) -> Vec - where - Bodies: ComponentSetMut - + ComponentSet - + ComponentSetMut, - { - let mut deleted = vec![]; - - if let Some(deleted_id) = self - .rb_graph_ids - .remove(handle.0, InteractionGraph::<(), ()>::invalid_graph_index()) - { - if InteractionGraph::<(), ()>::is_graph_index_valid(deleted_id) { - // We have to delete each joint one by one in order to: - // - Wake-up the attached bodies. - // - Update our Handle -> graph edge mapping. - // Delete the node. - let to_delete: Vec<_> = self - .joint_graph - .interactions_with(deleted_id) - .map(|e| (e.0, e.1, e.2.handle)) - .collect(); - for (h1, h2, to_delete_handle) in to_delete { - deleted.push(to_delete_handle); - let to_delete_edge_id = self.joint_ids.remove(to_delete_handle.0).unwrap(); - self.joint_graph.graph.remove_edge(to_delete_edge_id); - - // Update the id of the edge which took the place of the deleted one. - if let Some(j) = self.joint_graph.graph.edge_weight_mut(to_delete_edge_id) { - self.joint_ids[j.handle.0] = to_delete_edge_id; - } - - // Wake up the attached bodies. - islands.wake_up(bodies, h1, true); - islands.wake_up(bodies, h2, true); - } - - if let Some(other) = self.joint_graph.remove_node(deleted_id) { - // One rigid-body joint graph index may have been invalidated - // so we need to update it. - self.rb_graph_ids.insert(other.0, deleted_id); - } - } - } - - deleted - } -} diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs index 72a7483..6594b83 100644 --- a/src/dynamics/joint/mod.rs +++ b/src/dynamics/joint/mod.rs @@ -1,20 +1,21 @@ -pub use self::ball_joint::BallJoint; pub use self::fixed_joint::FixedJoint; -// pub use self::generic_joint::GenericJoint; -pub use self::joint::{Joint, JointParams}; -pub(crate) use self::joint_set::{JointGraphEdge, JointIndex}; -pub use self::joint_set::{JointHandle, JointSet}; +pub use self::impulse_joint::*; +pub use self::joint_data::*; +pub use self::motor_model::MotorModel; +pub use self::multibody_joint::*; pub use self::prismatic_joint::PrismaticJoint; -#[cfg(feature = "dim3")] pub use self::revolute_joint::RevoluteJoint; -pub use self::spring_model::SpringModel; -mod ball_joint; +#[cfg(feature = "dim3")] +pub use self::spherical_joint::SphericalJoint; + mod fixed_joint; -// mod generic_joint; -mod joint; -mod joint_set; +mod impulse_joint; +mod joint_data; +mod motor_model; +mod multibody_joint; mod prismatic_joint; -#[cfg(feature = "dim3")] mod revolute_joint; -mod spring_model; + +#[cfg(feature = "dim3")] +mod spherical_joint; diff --git a/src/dynamics/joint/motor_model.rs b/src/dynamics/joint/motor_model.rs new file mode 100644 index 0000000..e5a6549 --- /dev/null +++ b/src/dynamics/joint/motor_model.rs @@ -0,0 +1,61 @@ +use crate::math::Real; + +/// The spring-like model used for constraints resolution. +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub enum MotorModel { + /// The solved spring-like equation is: + /// `delta_velocity(t + dt) = stiffness / dt * (target_pos - pos(t)) + damping * (target_vel - vel(t))` + /// + /// Here the `stiffness` is the ratio of position error to be solved at each timestep (like + /// a velocity-based ERP), and the `damping` is the ratio of velocity error to be solved at + /// each timestep. + VelocityBased, + /// The solved spring-like equation is: + /// `acceleration(t + dt) = stiffness * (target_pos - pos(t)) + damping * (target_vel - vel(t))` + AccelerationBased, + // /// The solved spring-like equation is: + // /// `force(t + dt) = stiffness * (target_pos - pos(t + dt)) + damping * (target_vel - vel(t + dt))` + // ForceBased, +} + +impl Default for MotorModel { + fn default() -> Self { + MotorModel::VelocityBased + } +} + +impl MotorModel { + /// Combines the coefficients used for solving the spring equation. + /// + /// Returns the new coefficients (stiffness, damping, gamma, keep_inv_lhs) + /// coefficients for the equivalent impulse-based equation. These new + /// coefficients must be used in the following way: + /// - `rhs = (stiffness * pos_err + damping * vel_err) / gamma`. + /// - `new_inv_lhs = gamma * if keep_inv_lhs { inv_lhs } else { 1.0 }`. + /// Note that the returned `gamma` will be zero if both `stiffness` and `damping` are zero. + pub fn combine_coefficients( + self, + dt: Real, + stiffness: Real, + damping: Real, + ) -> (Real, Real, Real, bool) { + match self { + MotorModel::VelocityBased => (stiffness * crate::utils::inv(dt), damping, 1.0, true), + MotorModel::AccelerationBased => { + let effective_stiffness = stiffness * dt; + let effective_damping = damping * dt; + // TODO: Using gamma behaves very badly for some reasons. + // Maybe I got the formulation wrong, so let's keep it to 1.0 for now, + // and get back to this later. + // let gamma = effective_stiffness * dt + effective_damping; + (effective_stiffness, effective_damping, 1.0, true) + } // MotorModel::ForceBased => { + // let effective_stiffness = stiffness * dt; + // let effective_damping = damping * dt; + // let gamma = effective_stiffness * dt + effective_damping; + // (effective_stiffness, effective_damping, g