diff options
Diffstat (limited to 'src/dynamics')
56 files changed, 2410 insertions, 4045 deletions
diff --git a/src/dynamics/coefficient_combine_rule.rs b/src/dynamics/coefficient_combine_rule.rs new file mode 100644 index 0000000..5e8b4a0 --- /dev/null +++ b/src/dynamics/coefficient_combine_rule.rs @@ -0,0 +1,34 @@ +use crate::math::Real; + +/// Rules used to combine two coefficients. +/// +/// This is used to determine the effective restitution and +/// friction coefficients for a contact between two colliders. +/// Each collider has its combination rule of type +/// `CoefficientCombineRule`. And the rule +/// actually used is given by `max(first_combine_rule as usize, second_combine_rule as usize)`. +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub enum CoefficientCombineRule { + /// The two coefficients are averaged. + Average = 0, + /// The smallest coefficient is chosen. + Min, + /// The two coefficients are multiplied. + Multiply, + /// The greatest coefficient is chosen. + Max, +} + +impl CoefficientCombineRule { + pub(crate) fn combine(coeff1: Real, coeff2: Real, rule_value1: u8, rule_value2: u8) -> Real { + let effective_rule = rule_value1.max(rule_value2); + + match effective_rule { + 0 => (coeff1 + coeff1) / 2.0, + 1 => coeff1.min(coeff2), + 2 => coeff1 * coeff2, + _ => coeff1.max(coeff2), + } + } +} diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 56b5801..0d4d3b6 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -1,9 +1,11 @@ +use crate::math::Real; + /// Parameters for a time-step of the physics engine. #[derive(Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct IntegrationParameters { /// The timestep length (default: `1.0 / 60.0`) - pub dt: f32, + pub dt: Real, // /// If `true` and if rapier is compiled with the `parallel` feature, this will enable rayon-based multithreading (default: `true`). // /// @@ -18,31 +20,31 @@ pub struct IntegrationParameters { pub return_after_ccd_substep: bool, /// 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: f32, + 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: f32, + 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: f32, + pub warmstart_coeff: Real, /// Contacts at points where the involved bodies have a relative /// velocity smaller than this threshold wont be affected by the restitution force (default: `1.0`). - pub restitution_velocity_threshold: f32, + pub restitution_velocity_threshold: Real, /// Amount of penetration the engine wont attempt to correct (default: `0.005m`). - pub allowed_linear_error: f32, + pub allowed_linear_error: Real, /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). - pub prediction_distance: f32, + pub prediction_distance: Real, /// Amount of angular drift of joint limits the engine wont /// attempt to correct (default: `0.001rad`). - pub allowed_angular_error: f32, + pub allowed_angular_error: Real, /// Maximum linear correction during one step of the non-linear position solver (default: `0.2`). - pub max_linear_correction: f32, + pub max_linear_correction: Real, /// Maximum angular correction during one step of the non-linear position solver (default: `0.2`). - pub max_angular_correction: f32, + pub max_angular_correction: Real, /// Maximum nonlinear SOR-prox scaling parameter when the constraint /// correction direction is close to the kernel of the involved multibody's /// jacobian (default: `0.2`). - pub max_stabilization_multiplier: f32, + pub max_stabilization_multiplier: Real, /// Maximum number of iterations performed by the velocity constraints solver (default: `4`). pub max_velocity_iterations: usize, /// Maximum number of iterations performed by the position-based constraints solver (default: `1`). @@ -88,18 +90,18 @@ 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: f32, + dt: Real, // multithreading_enabled: bool, - erp: f32, - joint_erp: f32, - warmstart_coeff: f32, - restitution_velocity_threshold: f32, - allowed_linear_error: f32, - allowed_angular_error: f32, - max_linear_correction: f32, - max_angular_correction: f32, - prediction_distance: f32, - max_stabilization_multiplier: f32, + erp: Real, + joint_erp: Real, + warmstart_coeff: Real, + restitution_velocity_threshold: Real, + allowed_linear_error: Real, + allowed_angular_error: Real, + max_linear_correction: Real, + max_angular_correction: Real, + prediction_distance: Real, + max_stabilization_multiplier: Real, max_velocity_iterations: usize, max_position_iterations: usize, max_ccd_position_iterations: usize, @@ -140,7 +142,7 @@ impl IntegrationParameters { /// The current time-stepping length. #[inline(always)] #[deprecated = "You can just read the `IntegrationParams::dt` value directly"] - pub fn dt(&self) -> f32 { + pub fn dt(&self) -> Real { self.dt } @@ -148,7 +150,7 @@ impl IntegrationParameters { /// /// This is zero if `self.dt` is zero. #[inline(always)] - pub fn inv_dt(&self) -> f32 { + pub fn inv_dt(&self) -> Real { if self.dt == 0.0 { 0.0 } else { @@ -159,7 +161,7 @@ impl IntegrationParameters { /// Sets the time-stepping length. #[inline] #[deprecated = "You can just set the `IntegrationParams::dt` value directly"] - pub fn set_dt(&mut self, dt: f32) { + pub fn set_dt(&mut self, dt: Real) { assert!(dt >= 0.0, "The time-stepping length cannot be negative."); self.dt = dt; } @@ -168,7 +170,7 @@ impl IntegrationParameters { /// /// This automatically recompute `self.dt`. #[inline] - pub fn set_inv_dt(&mut self, inv_dt: f32) { + pub fn set_inv_dt(&mut self, inv_dt: Real) { if inv_dt == 0.0 { self.dt = 0.0 } else { diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs index ec255d4..82e2a10 100644 --- a/src/dynamics/joint/ball_joint.rs +++ b/src/dynamics/joint/ball_joint.rs @@ -1,29 +1,29 @@ -use crate::math::{Point, Vector}; +use crate::math::{Point, Real, Vector}; #[derive(Copy, Clone)] #[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<f32>, + pub local_anchor1: Point<Real>, /// Where the ball joint is attached on the first body, expressed in the first body local frame. - pub local_anchor2: Point<f32>, + pub local_anchor2: Point<Real>, /// The impulse applied by this joint on the first body. /// /// The impulse applied to the second body is given by `-impulse`. - pub impulse: Vector<f32>, + pub impulse: Vector<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<f32>, local_anchor2: Point<f32>) -> Self { + pub fn new(local_anchor1: Point<Real>, local_anchor2: Point<Real>) -> Self { Self::with_impulse(local_anchor1, local_anchor2, Vector::zeros()) } pub(crate) fn with_impulse( - local_anchor1: Point<f32>, - local_anchor2: Point<f32>, - impulse: Vector<f32>, + local_anchor1: Point<Real>, + local_anchor2: Point<Real>, + impulse: Vector<Real>, ) -> Self { Self { local_anchor1, diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs index 0731cfb..359e14a 100644 --- a/src/dynamics/joint/fixed_joint.rs +++ b/src/dynamics/joint/fixed_joint.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, SpacialVector}; +use crate::math::{Isometry, Real, SpacialVector}; #[derive(Copy, Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -8,22 +8,22 @@ use crate::math::{Isometry, SpacialVector}; 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_anchor1: Isometry<f32>, + pub local_anchor1: Isometry<Real>, /// 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<f32>, + pub local_anchor2: Isometry<Real>, /// 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<f32>, + pub impulse: SpacialVector<Real>, } impl FixedJoint { /// Creates a new fixed joint from the frames of reference of both bodies. - pub fn new(local_anchor1: Isometry<f32>, local_anchor2: Isometry<f32>) -> Self { + pub fn new(local_anchor1: Isometry<Real>, local_anchor2: Isometry<Real>) -> Self { Self { local_anchor1, local_anchor2, diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs index 5144d97..a87532a 100644 --- a/src/dynamics/joint/joint_set.rs +++ b/src/dynamics/joint/joint_set.rs @@ -1,11 +1,36 @@ use super::Joint; use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex}; -use crate::data::arena::{Arena, Index}; +use crate::data::arena::Arena; use crate::dynamics::{JointParams, RigidBodyHandle, RigidBodySet}; /// The unique identifier of a joint added to the joint set. -pub type JointHandle = Index; +/// 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) crate::data::arena::Index); + +impl JointHandle { + /// Converts this handle into its (index, generation) components. + pub fn into_raw_parts(self) -> (usize, u64) { + self.0.into_raw_parts() + } + + /// Reconstructs an handle from its (index, generation) components. + pub fn from_raw_parts(id: usize, generation: u64) -> 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_USIZE, + crate::INVALID_U64, + )) + } +} + pub(crate) type JointIndex = usize; pub(crate) type JointGraphEdge = crate::data::graph::Edge<Joint>; @@ -13,7 +38,7 @@ pub(crate) type JointGraphEdge = crate::data::graph::Edge<Joint>; /// A set of joints that can be handled by a physics `World`. pub struct JointSet { joint_ids: Arena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph. - joint_graph: InteractionGraph<Joint>, + joint_graph: InteractionGraph<RigidBodyHandle, Joint>, } impl JointSet { @@ -25,29 +50,24 @@ impl JointSet { } } - /// An always-invalid joint handle. - pub fn invalid_handle() -> JointHandle { - JointHandle::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64) - } - /// The number of joints on this set. pub fn len(&self) -> usize { self.joint_graph.graph.edges.len() } /// Retrieve the joint graph where edges are joints and nodes are rigid body handles. - pub fn joint_graph(&self) -> &InteractionGraph<Joint> { + pub fn joint_graph(&self) -> &InteractionGraph<RigidBodyHandle, Joint> { &self.joint_graph } /// Is the given joint handle valid? pub fn contains(&self, handle: JointHandle) -> bool { - self.joint_ids.contains(handle) + 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)?; + let id = self.joint_ids.get(handle.0)?; self.joint_graph.graph.edge_weight(*id) } @@ -62,7 +82,10 @@ impl JointSet { /// suffer form the ABA problem. pub fn get_unknown_gen(&self, i: usize) -> Option<(&Joint, JointHandle)> { let (id, handle) = self.joint_ids.get_unknown_gen(i)?; - Some((self.joint_graph.graph.edge_weight(*id)?, handle)) + Some(( + self.joint_graph.graph.edge_weight(*id)?, + JointHandle(handle), + )) } /// Iterates through all the joint on this set. @@ -117,7 +140,7 @@ impl JointSet { let joint = Joint { body1, body2, - handle, + handle: JointHandle(handle), #[cfg(feature = "parallel")] constraint_index: 0, #[cfg(feature = "parallel")] @@ -133,11 +156,13 @@ impl JointSet { // NOTE: the body won't have a graph index if it does not // have any joint attached. - if !InteractionGraph::<Joint>::is_graph_index_valid(rb1.joint_graph_index) { + if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(rb1.joint_graph_index) + { rb1.joint_graph_index = self.joint_graph.graph.add_node(joint.body1); } - if !InteractionGraph::<Joint>::is_graph_index_valid(rb2.joint_graph_index) { + if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(rb2.joint_graph_index) + { rb2.joint_graph_index = self.joint_graph.graph.add_node(joint.body2); } @@ -146,7 +171,7 @@ impl JointSet { .add_edge(rb1.joint_graph_index, rb2.joint_graph_index, joint); self.joint_ids[handle] = id; - handle + JointHandle(handle) } /// Retrieve all the joints happening between two active bodies. @@ -191,7 +216,7 @@ impl JointSet { bodies: &mut RigidBodySet, wake_up: bool, ) -> Option<Joint> { - let id = self.joint_ids.remove(handle)?; + let id = self.joint_ids.remove(handle.0)?; let endpoints = self.joint_graph.graph.edge_endpoints(id)?; if wake_up { @@ -207,7 +232,7 @@ impl JointSet { 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] = id; + self.joint_ids[edge.handle.0] = id; } removed_joint @@ -218,7 +243,7 @@ impl JointSet { deleted_id: RigidBodyGraphIndex, bodies: &mut RigidBodySet, ) { - if InteractionGraph::<()>::is_graph_index_valid(deleted_id) { + 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. @@ -229,12 +254,12 @@ impl JointSet { .map(|e| (e.0, e.1, e.2.handle)) .collect(); for (h1, h2, to_delete_handle) in to_delete { - let to_delete_edge_id = self.joint_ids.remove(to_delete_handle).unwrap(); + 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] = to_delete_edge_id; + self.joint_ids[j.handle.0] = to_delete_edge_id; } // Wake up the attached bodies. diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs index a6fd558..174ce79 100644 --- a/src/dynamics/joint/prismatic_joint.rs +++ b/src/dynamics/joint/prismatic_joint.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Point, Vector, DIM}; +use crate::math::{Isometry, Point, Real, Vector, DIM}; use crate::utils::WBasis; use na::Unit; #[cfg(feature = "dim2")] @@ -11,35 +11,35 @@ use na::Vector5; /// A joint that removes all relative motion between two bodies, except for the translations along one axis. pub struct PrismaticJoint { /// Where the prismatic joint is attached on the first body, expressed in the local space of the first attached body. - pub local_anchor1: Point<f32>, + pub local_anchor1: Point<Real>, /// Where the prismatic joint is attached on the second body, expressed in the local space of the second attached body. - pub local_anchor2: Point<f32>, - pub(crate) local_axis1: Unit<Vector<f32>>, - pub(crate) local_axis2: Unit<Vector<f32>>, - pub(crate) basis1: [Vector<f32>; DIM - 1], - pub(crate) basis2: [Vector<f32>; DIM - 1], + pub local_anchor2: Point<Real>, + pub(crate) local_axis1: Unit<Vector<Real>>, + pub(crate) local_axis2: Unit<Vector<Real>>, + pub(crate) basis1: [Vector<Real>; DIM - 1], + pub(crate) basis2: [Vector<Real>; DIM - 1], /// The impulse applied by this joint on the first body. /// /// The impulse applied to the second body is given by `-impulse`. #[cfg(feature = "dim3")] - pub impulse: Vector5<f32>, + pub impulse: Vector5<Real>, /// The impulse applied by this joint on the first body. /// /// The impulse applied to the second body is given by `-impulse`. #[cfg(feature = "dim2")] - pub impulse: Vector2<f32>, + pub impulse: Vector2<Real>, /// Whether or not this joint should enforce translational limits along its axis. pub limits_enabled: bool, /// The min an max relative position of the attached bodies along this joint's axis. - pub limits: [f32; 2], + pub limits: [Real; 2], /// The impulse applied by this joint on the first body to enforce the position limit along this joint's axis. /// /// The impulse applied to the second body is given by `-impulse`. - pub limits_impulse: f32, + pub limits_impulse: Real, // pub motor_enabled: bool, - // pub target_motor_vel: f32, - // pub max_motor_impulse: f32, - // pub motor_impulse: f32, + // pub target_motor_vel: Real, + // pub max_motor_impulse: Real, + // pub motor_impulse: Real, } impl PrismaticJoint { @@ -47,10 +47,10 @@ impl PrismaticJoint { /// in the local-space of the affected bodies. #[cfg(feature = "dim2")] pub fn new( - local_anchor1: Point<f32>, - local_axis1: Unit<Vector<f32>>, - local_anchor2: Point<f32>, - local_axis2: Unit<Vector<f32>>, + local_anchor1: Point<Real>, + local_axis1: Unit<Vector<Real>>, + local_anchor2: Point<Real>, + local_axis2: Unit<Vector<Real>>, ) -> Self { Self { local_anchor1, @@ -61,11 +61,11 @@ impl PrismaticJoint { basis2: local_axis2.orthonormal_basis(), impulse: na::zero(), limits_enabled: false, - limits: [-f32::MAX, f32::MAX], + limits: [-Real::MAX, Real::MAX], limits_impulse: 0.0, // motor_enabled: false, // target_motor_vel: 0.0, - // max_motor_impulse: f32::MAX, + // max_motor_impulse: Real::MAX, // motor_impulse: 0.0, } } @@ -78,12 +78,12 @@ impl PrismaticJoint { /// computed arbitrarily. #[cfg(feature = "dim3")] pub fn new( - local_anchor1: Point<f32>, - local_axis1: Unit<Vector<f32>>, - local_tangent1: Vector<f32>, - local_anchor2: Point<f32>, - local_axis2: Unit<Vector<f32>>, - local_tangent2: Vector<f32>, + local_anchor1: Point<Real>, + local_axis1: Unit<Vector<Real>>, + |