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/counters/mod.rs | 4 +- src/data/coarena.rs | 12 +- src/data/graph.rs | 52 +- src/dynamics/integration_parameters.rs | 96 +- src/dynamics/island_manager.rs | 13 +- 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 -- src/dynamics/mod.rs | 15 +- src/dynamics/rigid_body_components.rs | 168 +++- src/dynamics/rigid_body_set.rs | 18 +- src/dynamics/solver/categorization.rs | 36 +- src/dynamics/solver/delta_vel.rs | 24 +- src/dynamics/solver/generic_velocity_constraint.rs | 377 ++++++++ .../solver/generic_velocity_constraint_element.rs | 348 +++++++ src/dynamics/solver/interaction_groups.rs | 13 +- src/dynamics/solver/island_solver.rs | 151 +-- .../joint_constraint/ball_position_constraint.rs | 266 ----- .../ball_position_constraint_wide.rs | 216 ----- .../joint_constraint/ball_velocity_constraint.rs | 660 ------------- .../ball_velocity_constraint_wide.rs | 359 ------- .../joint_constraint/fixed_position_constraint.rs | 151 --- .../fixed_position_constraint_wide.rs | 71 -- .../joint_constraint/fixed_velocity_constraint.rs | 436 --------- .../fixed_velocity_constraint_wide.rs | 539 ----------- .../generic_position_constraint.rs | 346 ------- .../generic_position_constraint_wide.rs | 60 -- .../generic_velocity_constraint.rs | 706 -------------- .../generic_velocity_constraint_wide.rs | 464 --------- .../solver/joint_constraint/joint_constraint.rs | 710 ++++++++------ .../joint_generic_velocity_constraint.rs | 529 ++++++++++ .../joint_generic_velocity_constraint_builder.rs | 853 ++++++++++++++++ .../joint_constraint/joint_position_constraint.rs | 280 ------ .../joint_constraint/joint_velocity_constraint.rs | 608 ++++++++++++ .../joint_velocity_constraint_builder.rs | 699 ++++++++++++++ src/dynamics/solver/joint_constraint/mod.rs | 107 +- .../prismatic_position_constraint.rs | 182 ---- .../prismatic_position_constraint_wide.rs | 71 -- .../prismatic_velocity_constraint.rs | 859 ---------------- .../prismatic_velocity_constraint_wide.rs | 848 ---------------- .../revolute_position_constraint.rs | 295 ------ .../revolute_position_constraint_wide.rs | 71 -- .../revolute_velocity_constraint.rs | 750 -------------- .../revolute_velocity_constraint_wide.rs | 527 ---------- src/dynamics/solver/mod.rs | 27 +- src/dynamics/solver/parallel_island_solver.rs | 156 +-- src/dynamics/solver/parallel_position_solver.rs | 107 -- src/dynamics/solver/parallel_solver_constraints.rs | 101 +- src/dynamics/solver/parallel_velocity_solver.rs | 111 +-- src/dynamics/solver/position_constraint.rs | 168 ---- src/dynamics/solver/position_constraint_wide.rs | 157 --- src/dynamics/solver/position_ground_constraint.rs | 121 --- .../solver/position_ground_constraint_wide.rs | 143 --- src/dynamics/solver/position_solver.rs | 57 -- src/dynamics/solver/solver_constraints.rs | 317 ++++-- src/dynamics/solver/velocity_constraint.rs | 131 +-- src/dynamics/solver/velocity_constraint_element.rs | 114 +-- src/dynamics/solver/velocity_constraint_wide.rs | 110 +-- src/dynamics/solver/velocity_ground_constraint.rs | 77 +- .../solver/velocity_ground_constraint_element.rs | 81 +- .../solver/velocity_ground_constraint_wide.rs | 93 +- src/dynamics/solver/velocity_solver.rs | 222 ++++- src/geometry/broad_phase_multi_sap/broad_phase.rs | 6 +- src/geometry/contact_pair.rs | 55 +- src/geometry/narrow_phase.rs | 6 +- src/lib.rs | 50 +- src/pipeline/physics_pipeline.rs | 160 ++- src/utils.rs | 226 +++-- 87 files changed, 8467 insertions(+), 11872 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 create mode 100644 src/dynamics/solver/generic_velocity_constraint.rs create mode 100644 src/dynamics/solver/generic_velocity_constraint_element.rs delete mode 100644 src/dynamics/solver/joint_constraint/ball_position_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/fixed_position_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/generic_position_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs delete mode 100644 src/dynamics/solver/joint_constraint/joint_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs delete mode 100644 src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/revolute_position_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs delete mode 100644 src/dynamics/solver/parallel_position_solver.rs delete mode 100644 src/dynamics/solver/position_constraint.rs delete mode 100644 src/dynamics/solver/position_constraint_wide.rs delete mode 100644 src/dynamics/solver/position_ground_constraint.rs delete mode 100644 src/dynamics/solver/position_ground_constraint_wide.rs delete mode 100644 src/dynamics/solver/position_solver.rs (limited to 'src') diff --git a/src/counters/mod.rs b/src/counters/mod.rs index 4d4d05d..324696f 100644 --- a/src/counters/mod.rs +++ b/src/counters/mod.rs @@ -14,7 +14,7 @@ mod solver_counters; mod stages_counters; mod timer; -/// Aggregation of all the performances counters tracked by nphysics. +/// Aggregation of all the performances counters tracked by rapier. #[derive(Clone, Copy)] pub struct Counters { /// Whether thi counter is enabled or not. @@ -34,7 +34,7 @@ pub struct Counters { } impl Counters { - /// Create a new set of counters initialized to wero. + /// Create a new set of counters initialized to zero. pub fn new(enabled: bool) -> Self { Counters { enabled, diff --git a/src/data/coarena.rs b/src/data/coarena.rs index 124d85a..1f01c05 100644 --- a/src/data/coarena.rs +++ b/src/data/coarena.rs @@ -13,6 +13,14 @@ impl Coarena { Self { data: Vec::new() } } + pub fn iter(&self) -> impl Iterator { + self.data + .iter() + .enumerate() + .filter(|(_, elt)| elt.0 != u32::MAX) + .map(|(i, elt)| (Index::from_raw_parts(i as u32, elt.0), &elt.1)) + } + /// Gets a specific element from the coarena without specifying its generation number. /// /// It is strongly encouraged to use `Coarena::get` instead of this method because this method @@ -23,12 +31,12 @@ impl Coarena { /// Deletes an element for the coarena and returns its value. /// - /// We can't really remove an element from the coarena. So instead of actually removing - /// it, this method will reset the value to the given `removed_value`. + /// This method will reset the value to the given `removed_value`. pub fn remove(&mut self, index: Index, removed_value: T) -> Option { let (i, g) = index.into_raw_parts(); let data = self.data.get_mut(i as usize)?; if g == data.0 { + data.0 = u32::MAX; // invalidate the generation number. Some(std::mem::replace(&mut data.1, removed_value)) } else { None diff --git a/src/data/graph.rs b/src/data/graph.rs index de958c3..2dc7fbf 100644 --- a/src/data/graph.rs +++ b/src/data/graph.rs @@ -644,19 +644,24 @@ impl<'a, E> Iterator for Edges<'a, E> { // For iterate_over, "both" is represented as None. // For reverse, "no" is represented as None. - let (iterate_over, reverse) = (None, Some(self.direction.opposite())); + let (iterate_over, _reverse) = (None, Some(self.direction.opposite())); if iterate_over.unwrap_or(Direction::Outgoing) == Direction::Outgoing { let i = self.next[0].index(); - if let Some(Edge { node, weight, next }) = self.edges.get(i) { + if let Some(Edge { + node: _node, + weight, + next, + }) = self.edges.get(i) + { self.next[0] = next[0]; return Some(EdgeReference { index: EdgeIndex(i as u32), - node: if reverse == Some(Direction::Outgoing) { - swap_pair(*node) - } else { - *node - }, + // node: if reverse == Some(Direction::Outgoing) { + // swap_pair(*node) + // } else { + // *node + // }, weight, }); } @@ -674,11 +679,11 @@ impl<'a, E> Iterator for Edges<'a, E> { return Some(EdgeReference { index: edge_index, - node: if reverse == Some(Direction::Incoming) { - swap_pair(*node) - } else { - *node - }, + // node: if reverse == Some(Direction::Incoming) { + // swap_pair(*node) + // } else { + // *node + // }, weight, }); } @@ -688,10 +693,10 @@ impl<'a, E> Iterator for Edges<'a, E> { } } -fn swap_pair(mut x: [T; 2]) -> [T; 2] { - x.swap(0, 1); - x -} +// fn swap_pair(mut x: [T; 2]) -> [T; 2] { +// x.swap(0, 1); +// x +// } impl<'a, E> Clone for Edges<'a, E> { fn clone(&self) -> Self { @@ -742,24 +747,11 @@ impl IndexMut for Graph { } } -/// A “walker” object that can be used to step through the edge list of a node. -/// -/// Created with [`.detach()`](struct.Neighbors.html#method.detach). -/// -/// The walker does not borrow from the graph, so it lets you step through -/// neighbors or incident edges while also mutating graph weights, as -/// in the following example: -#[derive(Clone)] -pub struct WalkNeighbors { - skip_start: NodeIndex, - next: [EdgeIndex; 2], -} - /// Reference to a `Graph` edge. #[derive(Debug)] pub struct EdgeReference<'a, E: 'a> { index: EdgeIndex, - node: [NodeIndex; 2], + // node: [NodeIndex; 2], weight: &'a E, } diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 4725319..d998eec 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -18,18 +18,6 @@ pub struct IntegrationParameters { /// to numerical instabilities. pub min_ccd_dt: Real, - /// The Error Reduction Parameter in `[0, 1]` is the proportion of - /// the positional error to be corrected at each time step (default: `0.2`). - pub erp: Real, - /// The Error Reduction Parameter for joints in `[0, 1]` is the proportion of - /// the positional error to be corrected at each time step (default: `0.2`). - pub joint_erp: Real, - /// Each cached impulse are multiplied by this coefficient in `[0, 1]` - /// when they are re-used to initialize the solver (default `1.0`). - pub warmstart_coeff: Real, - /// Correction factor to avoid large warmstart impulse after a strong impact (default `10.0`). - pub warmstart_correction_slope: Real, - /// 0-1: how much of the velocity to dampen out in the constraint solver? /// (default `1.0`). pub velocity_solve_fraction: Real, @@ -40,23 +28,21 @@ pub struct IntegrationParameters { /// If non-zero, you do not need the positional solver. /// A good non-zero value is around `0.2`. /// (default `0.0`). - pub velocity_based_erp: Real, + pub erp: Real, - /// Amount of penetration the engine wont attempt to correct (default: `0.005m`). + /// Amount of penetration the engine wont attempt to correct (default: `0.001m`). pub allowed_linear_error: Real, /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). pub prediction_distance: Real, - /// Amount of angular drift of joint limits the engine wont - /// attempt to correct (default: `0.001rad`). - pub allowed_angular_error: Real, - /// Maximum linear correction during one step of the non-linear position solver (default: `0.2`). - pub max_linear_correction: Real, - /// Maximum angular correction during one step of the non-linear position solver (default: `0.2`). - pub max_angular_correction: Real, - /// Maximum number of iterations performed by the velocity constraints solver (default: `4`). + /// Maximum number of iterations performed to solve non-penetration and joint constraints (default: `4`). pub max_velocity_iterations: usize, - /// Maximum number of iterations performed by the position-based constraints solver (default: `1`). - pub max_position_iterations: usize, + /// Maximum number of iterations performed to solve friction constraints (default: `8`). + pub max_velocity_friction_iterations: usize, + /// Maximum number of iterations performed to remove the energy introduced by penetration corrections (default: `1`). + pub max_stabilization_iterations: usize, + /// If `false`, friction and non-penetration constraints will be solved in the same loop. Otherwise, + /// non-penetration constraints are solved first, and friction constraints are solved after (default: `true`). + pub interleave_restitution_and_friction_resolution: bool, /// Minimum number of dynamic bodies in each active island (default: `128`). pub min_island_size: usize, /// Maximum number of substeps performed by the solver (default: `1`). @@ -64,46 +50,6 @@ pub struct IntegrationParameters { } impl IntegrationParameters { - /// Creates a set of integration parameters with the given values. - #[deprecated = "Use `IntegrationParameters { dt: 60.0, ..Default::default() }` instead"] - pub fn new( - dt: Real, - erp: Real, - joint_erp: Real, - warmstart_coeff: Real, - allowed_linear_error: Real, - allowed_angular_error: Real, - max_linear_correction: Real, - max_angular_correction: Real, - prediction_distance: Real, - max_velocity_iterations: usize, - max_position_iterations: usize, - max_ccd_substeps: usize, - ) -> Self { - IntegrationParameters { - dt, - erp, - joint_erp, - warmstart_coeff, - allowed_linear_error, - allowed_angular_error, - max_linear_correction, - max_angular_correction, - prediction_distance, - max_velocity_iterations, - max_position_iterations, - max_ccd_substeps, - ..Default::default() - } - } - - /// The current time-stepping length. - #[inline(always)] - #[deprecated = "You can just read the `IntegrationParams::dt` value directly"] - pub fn dt(&self) -> Real { - self.dt - } - /// The inverse of the time-stepping length, i.e. the steps per seconds (Hz). /// /// This is zero if `self.dt` is zero. @@ -136,10 +82,10 @@ impl IntegrationParameters { } } - /// Convenience: `velocity_based_erp / dt` + /// Convenience: `erp / dt` #[inline] - pub(crate) fn velocity_based_erp_inv_dt(&self) -> Real { - self.velocity_based_erp * self.inv_dt() + pub(crate) fn erp_inv_dt(&self) -> Real { + self.erp * self.inv_dt() } } @@ -148,20 +94,14 @@ impl Default for IntegrationParameters { Self { dt: 1.0 / 60.0, min_ccd_dt: 1.0 / 60.0 / 100.0, - // multithreading_enabled: true, - erp: 0.2, - joint_erp: 0.2, velocity_solve_fraction: 1.0, - velocity_based_erp: 0.0, - warmstart_coeff: 1.0, - warmstart_correction_slope: 10.0, - allowed_linear_error: 0.005, + erp: 0.8, + allowed_linear_error: 0.001, // 0.005 prediction_distance: 0.002, - allowed_angular_error: 0.001, - max_linear_correction: 0.2, - max_angular_correction: 0.2, max_velocity_iterations: 4, - max_position_iterations: 1, + max_velocity_friction_iterations: 8, + max_stabilization_iterations: 1, + interleave_restitution_and_friction_resolution: true, // Enabling this makes a big difference for 2D stability. // FIXME: what is the optimal value for min_island_size? // It should not be too big so that we don't end up with // huge islands that don't fit in cache. diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index 10ac0f8..55b47c9 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -1,7 +1,7 @@ use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption}; use crate::dynamics::{ - JointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyHandle, RigidBodyIds, - RigidBodyType, RigidBodyVelocity, + ImpulseJointSet, MultibodyJointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyHandle, + RigidBodyIds, RigidBodyType, RigidBodyVelocity, }; use crate::geometry::{ColliderParent, NarrowPhase}; use crate::math::Real; @@ -175,7 +175,8 @@ impl IslandManager { bodies: &mut Bodies, colliders: &Colliders, narrow_phase: &NarrowPhase, - joints: &JointSet, + impulse_joints: &ImpulseJointSet, + multibody_joints: &MultibodyJointSet, min_island_size: usize, ) where Bodies: ComponentSetMut @@ -302,11 +303,15 @@ impl IslandManager { // in contact or joined with this collider. push_contacting_bodies(rb_colliders, colliders, narrow_phase, &mut self.stack); - for inter in joints.joints_with(handle) { + for inter in impulse_joints.joints_with(handle) { let other = crate::utils::select_other((inter.0, inter.1), handle); self.stack.push(other); } + for other in multibody_joints.attached_bodies(handle) { + self.stack.push(other); + } + bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| { activation.wake_up(false); }); 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; +