From 754a48b7ff6d8c58b1ee08651e60112900b60455 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Tue, 25 Aug 2020 22:10:25 +0200 Subject: First public release of Rapier. --- src/dynamics/integration_parameters.rs | 207 +++++++ src/dynamics/joint/ball_joint.rs | 34 + src/dynamics/joint/fixed_joint.rs | 33 + src/dynamics/joint/joint.rs | 112 ++++ src/dynamics/joint/joint_set.rs | 218 +++++++ src/dynamics/joint/mod.rs | 16 + src/dynamics/joint/prismatic_joint.rs | 193 ++++++ src/dynamics/joint/revolute_joint.rs | 46 ++ src/dynamics/mass_properties.rs | 206 ++++++ src/dynamics/mass_properties_ball.rs | 30 + src/dynamics/mass_properties_capsule.rs | 60 ++ src/dynamics/mass_properties_cuboid.rs | 33 + src/dynamics/mass_properties_polygon.rs | 144 +++++ src/dynamics/mod.rs | 30 + src/dynamics/rigid_body.rs | 441 +++++++++++++ src/dynamics/rigid_body_set.rs | 518 ++++++++++++++++ src/dynamics/solver/categorization.rs | 70 +++ src/dynamics/solver/delta_vel.rs | 18 + src/dynamics/solver/interaction_groups.rs | 434 +++++++++++++ src/dynamics/solver/island_solver.rs | 73 +++ .../joint_constraint/ball_position_constraint.rs | 165 +++++ .../ball_position_constraint_wide.rs | 199 ++++++ .../joint_constraint/ball_velocity_constraint.rs | 238 +++++++ .../ball_velocity_constraint_wide.rs | 329 ++++++++++ .../joint_constraint/fixed_position_constraint.rs | 139 +++++ .../joint_constraint/fixed_velocity_constraint.rs | 370 +++++++++++ .../fixed_velocity_constraint_wide.rs | 472 ++++++++++++++ .../solver/joint_constraint/joint_constraint.rs | 340 ++++++++++ .../joint_constraint/joint_position_constraint.rs | 169 +++++ src/dynamics/solver/joint_constraint/mod.rs | 65 ++ .../prismatic_position_constraint.rs | 170 +++++ .../prismatic_velocity_constraint.rs | 558 +++++++++++++++++ .../prismatic_velocity_constraint_wide.rs | 687 +++++++++++++++++++++ .../revolute_position_constraint.rs | 142 +++++ .../revolute_velocity_constraint.rs | 294 +++++++++ .../revolute_velocity_constraint_wide.rs | 397 ++++++++++++ src/dynamics/solver/mod.rs | 56 ++ src/dynamics/solver/parallel_island_solver.rs | 259 ++++++++ src/dynamics/solver/parallel_position_solver.rs | 582 +++++++++++++++++ src/dynamics/solver/parallel_velocity_solver.rs | 485 +++++++++++++++ src/dynamics/solver/position_constraint.rs | 246 ++++++++ src/dynamics/solver/position_constraint_wide.rs | 217 +++++++ src/dynamics/solver/position_ground_constraint.rs | 189 ++++++ .../solver/position_ground_constraint_wide.rs | 199 ++++++ src/dynamics/solver/position_solver.rs | 451 ++++++++++++++ src/dynamics/solver/velocity_constraint.rs | 401 ++++++++++++ src/dynamics/solver/velocity_constraint_wide.rs | 344 +++++++++++ src/dynamics/solver/velocity_ground_constraint.rs | 300 +++++++++ .../solver/velocity_ground_constraint_wide.rs | 300 +++++++++ src/dynamics/solver/velocity_solver.rs | 405 ++++++++++++ 50 files changed, 12084 insertions(+) create mode 100644 src/dynamics/integration_parameters.rs create mode 100644 src/dynamics/joint/ball_joint.rs create mode 100644 src/dynamics/joint/fixed_joint.rs create mode 100644 src/dynamics/joint/joint.rs create mode 100644 src/dynamics/joint/joint_set.rs create mode 100644 src/dynamics/joint/mod.rs create mode 100644 src/dynamics/joint/prismatic_joint.rs create mode 100644 src/dynamics/joint/revolute_joint.rs create mode 100644 src/dynamics/mass_properties.rs create mode 100644 src/dynamics/mass_properties_ball.rs create mode 100644 src/dynamics/mass_properties_capsule.rs create mode 100644 src/dynamics/mass_properties_cuboid.rs create mode 100644 src/dynamics/mass_properties_polygon.rs create mode 100644 src/dynamics/mod.rs create mode 100644 src/dynamics/rigid_body.rs create mode 100644 src/dynamics/rigid_body_set.rs create mode 100644 src/dynamics/solver/categorization.rs create mode 100644 src/dynamics/solver/delta_vel.rs create mode 100644 src/dynamics/solver/interaction_groups.rs create mode 100644 src/dynamics/solver/island_solver.rs create mode 100644 src/dynamics/solver/joint_constraint/ball_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/fixed_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/mod.rs create mode 100644 src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/revolute_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/mod.rs create mode 100644 src/dynamics/solver/parallel_island_solver.rs create mode 100644 src/dynamics/solver/parallel_position_solver.rs create mode 100644 src/dynamics/solver/parallel_velocity_solver.rs create mode 100644 src/dynamics/solver/position_constraint.rs create mode 100644 src/dynamics/solver/position_constraint_wide.rs create mode 100644 src/dynamics/solver/position_ground_constraint.rs create mode 100644 src/dynamics/solver/position_ground_constraint_wide.rs create mode 100644 src/dynamics/solver/position_solver.rs create mode 100644 src/dynamics/solver/velocity_constraint.rs create mode 100644 src/dynamics/solver/velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/velocity_ground_constraint.rs create mode 100644 src/dynamics/solver/velocity_ground_constraint_wide.rs create mode 100644 src/dynamics/solver/velocity_solver.rs (limited to 'src/dynamics') diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs new file mode 100644 index 0000000..faf22e1 --- /dev/null +++ b/src/dynamics/integration_parameters.rs @@ -0,0 +1,207 @@ +/// Parameters for a time-step of the physics engine. +#[derive(Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct IntegrationParameters { + /// The timestep (default: `1.0 / 60.0`) + dt: f32, + /// The inverse of `dt`. + inv_dt: f32, + // /// If `true` and if rapier is compiled with the `parallel` feature, this will enable rayon-based multithreading (default: `true`). + // /// + // /// This parameter is ignored if rapier is not compiled with is `parallel` feature. + // /// Refer to rayon's documentation regarding how to configure the number of threads with either + // /// `rayon::ThreadPoolBuilder::new().num_threads(4).build_global().unwrap()` or `ThreadPool::install`. + // /// Note that using only one thread with `multithreading_enabled` set to `true` will result on a slower + // /// simulation than setting `multithreading_enabled` to `false`. + // pub multithreading_enabled: bool, + /// If `true`, the world's `step` method will stop right after resolving exactly one CCD event (default: `false`). + /// This allows the user to take action during a timestep, in-between two CCD events. + 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, + /// 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, + /// 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, + /// 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, + /// Amount of penetration the engine wont attempt to correct (default: `0.001m`). + pub allowed_linear_error: f32, + /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). + pub prediction_distance: f32, + /// Amount of angular drift of joint limits the engine wont + /// attempt to correct (default: `0.001rad`). + pub allowed_angular_error: f32, + /// Maximum linear correction during one step of the non-linear position solver (default: `0.2`). + pub max_linear_correction: f32, + /// Maximum angular correction during one step of the non-linear position solver (default: `0.2`). + pub max_angular_correction: f32, + /// 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, + /// 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`). + pub max_position_iterations: usize, + /// Minimum number of dynamic bodies in each active island (default: `128`). + pub min_island_size: usize, + /// Maximum number of iterations performed by the position-based constraints solver for CCD steps (default: `10`). + /// + /// This should be sufficiently high so all penetration get resolved. For example, if CCD cause your + /// objects to stutter, that may be because the number of CCD position iterations is too low, causing + /// them to remain stuck in a penetration configuration for a few frames. + /// + /// The highest this number, the highest its computational cost. + pub max_ccd_position_iterations: usize, + /// Maximum number of substeps performed by the solver (default: `1`). + pub max_ccd_substeps: usize, + /// Controls the number of Proximity::Intersecting events generated by a trigger during CCD resolution (default: `false`). + /// + /// If false, triggers will only generate one Proximity::Intersecting event during a step, even + /// if another colliders repeatedly enters and leaves the triggers during multiple CCD substeps. + /// + /// If true, triggers will generate as many Proximity::Intersecting and Proximity::Disjoint/Proximity::WithinMargin + /// events as the number of times a collider repeatedly enters and leaves the triggers during multiple CCD substeps. + /// This is more computationally intensive. + pub multiple_ccd_substep_sensor_events_enabled: bool, + /// Whether penetration are taken into account in CCD resolution (default: `false`). + /// + /// If this is set to `false` two penetrating colliders will not be considered to have any time of impact + /// while they are penetrating. This may end up allowing some tunelling, but will avoid stuttering effect + /// when the constraints solver fails to completely separate two colliders after a CCD contact. + /// + /// If this is set to `true`, two penetrating colliders will be considered to have a time of impact + /// equal to 0 until the constraints solver manages to separate them. This will prevent tunnelling + /// almost completely, but may introduce stuttering effects when the constraints solver fails to completely + /// separate two colliders after a CCD contact. + // FIXME: this is a very binary way of handling penetration. + // We should provide a more flexible solution by letting the user choose some + // minimal amount of movement applied to an object that get stuck. + pub ccd_on_penetration_enabled: bool, +} + +impl IntegrationParameters { + /// Creates a set of integration parameters with the given values. + pub fn new( + dt: f32, + // 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, + max_velocity_iterations: usize, + max_position_iterations: usize, + max_ccd_position_iterations: usize, + max_ccd_substeps: usize, + return_after_ccd_substep: bool, + multiple_ccd_substep_sensor_events_enabled: bool, + ccd_on_penetration_enabled: bool, + ) -> Self { + IntegrationParameters { + dt, + inv_dt: if dt == 0.0 { 0.0 } else { 1.0 / dt }, + // multithreading_enabled, + erp, + joint_erp, + warmstart_coeff, + restitution_velocity_threshold, + allowed_linear_error, + allowed_angular_error, + max_linear_correction, + max_angular_correction, + prediction_distance, + max_stabilization_multiplier, + max_velocity_iterations, + max_position_iterations, + // 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. + // However we don't want it to be too small and end up with + // tons of islands, reducing SIMD parallelism opportunities. + min_island_size: 128, + max_ccd_position_iterations, + max_ccd_substeps, + return_after_ccd_substep, + multiple_ccd_substep_sensor_events_enabled, + ccd_on_penetration_enabled, + } + } + + /// The current time-stepping length. + #[inline(always)] + pub fn dt(&self) -> f32 { + self.dt + } + + /// The inverse of the time-stepping length. + /// + /// This is zero if `self.dt` is zero. + #[inline(always)] + pub fn inv_dt(&self) -> f32 { + self.inv_dt + } + + /// Sets the time-stepping length. + /// + /// This automatically recompute `self.inv_dt`. + #[inline] + pub fn set_dt(&mut self, dt: f32) { + assert!(dt >= 0.0, "The time-stepping length cannot be negative."); + self.dt = dt; + if dt == 0.0 { + self.inv_dt = 0.0 + } else { + self.inv_dt = 1.0 / dt + } + } + + /// Sets the inverse time-stepping length (i.e. the frequency). + /// + /// This automatically recompute `self.dt`. + #[inline] + pub fn set_inv_dt(&mut self, inv_dt: f32) { + self.inv_dt = inv_dt; + if inv_dt == 0.0 { + self.dt = 0.0 + } else { + self.dt = 1.0 / inv_dt + } + } +} + +impl Default for IntegrationParameters { + fn default() -> Self { + Self::new( + 1.0 / 60.0, + // true, + 0.2, + 0.2, + 1.0, + 1.0, + 0.005, + 0.001, + 0.2, + 0.2, + 0.002, + 0.2, + 4, + 1, + 10, + 1, + false, + false, + false, + ) + } +} diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs new file mode 100644 index 0000000..ec255d4 --- /dev/null +++ b/src/dynamics/joint/ball_joint.rs @@ -0,0 +1,34 @@ +use crate::math::{Point, 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, + /// Where the ball joint is attached on the first body, expressed in the first 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, +} + +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, + } + } +} diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs new file mode 100644 index 0000000..0731cfb --- /dev/null +++ b/src/dynamics/joint/fixed_joint.rs @@ -0,0 +1,33 @@ +use crate::math::{Isometry, SpacialVector}; + +#[derive(Copy, Clone)] +#[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 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, + /// 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, +} + +impl FixedJoint { + /// 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(), + } + } +} diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs new file mode 100644 index 0000000..074f802 --- /dev/null +++ b/src/dynamics/joint/joint.rs @@ -0,0 +1,112 @@ +#[cfg(feature = "dim3")] +use crate::dynamics::RevoluteJoint; +use crate::dynamics::{BallJoint, FixedJoint, JointHandle, PrismaticJoint, RigidBodyHandle}; + +#[derive(Copy, Clone)] +#[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), +} + +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, + #[cfg(feature = "dim3")] + JointParams::RevoluteJoint(_) => 3, + } + } + + /// 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 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) + } +} + +#[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))] +/// 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, +} diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs new file mode 100644 index 0000000..51d7432 --- /dev/null +++ b/src/dynamics/joint/joint_set.rs @@ -0,0 +1,218 @@ +use super::Joint; +use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex}; + +use crate::data::arena::{Arena, Index}; +use crate::dynamics::{JointParams, RigidBodyHandle, RigidBodySet}; + +/// The unique identifier of a joint added to the joint set. +pub type JointHandle = Index; +pub(crate) type JointIndex = usize; +pub(crate) type JointGraphEdge = crate::data::graph::Edge; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A set of joints that can be handled by a physics `World`. +pub struct JointSet { + 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 { + joint_ids: Arena::new(), + joint_graph: InteractionGraph::new(), + } + } + + /// 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 { + &self.joint_graph + } + + /// Is the given joint handle valid? + pub fn contains(&self, handle: JointHandle) -> bool { + self.joint_ids.contains(handle) + } + + /// Gets the joint with the given handle. + pub fn get(&self, handle: JointHandle) -> Option<&Joint> { + let id = self.joint_ids.get(handle)?; + self.joint_graph.graph.edge_weight(*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: usize) -> Option<(&Joint, JointHandle)> { + let (id, handle) = self.joint_ids.get_unknown_gen(i)?; + Some((self.joint_graph.graph.edge_weight(*id)?, 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) + } + + /// 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| &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, + bodies: &mut RigidBodySet, + 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, + #[cfg(feature = "parallel")] + constraint_index: 0, + #[cfg(feature = "parallel")] + position_constraint_index: 0, + params: joint_params.into(), + }; + + let (rb1, rb2) = bodies.get2_mut_internal(joint.body1, joint.body2); + let (rb1, rb2) = ( + rb1.expect("Attempt to attach a joint to a non-existing body."), + rb2.expect("Attempt to attach a joint to a non-existing body."), + ); + + // NOTE: the body won't have a graph index if it does not + // have any joint attached. + if !InteractionGraph::::is_graph_index_valid(rb1.joint_graph_index) { + rb1.joint_graph_index = self.joint_graph.graph.add_node(joint.body1); + } + + if !InteractionGraph::::is_graph_index_valid(rb2.joint_graph_index) { + rb2.joint_graph_index = self.joint_graph.graph.add_node(joint.body2); + } + + let id = self + .joint_graph + .add_edge(rb1.joint_graph_index, rb2.joint_graph_index, joint); + + self.joint_ids[handle] = id; + 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, + bodies: &RigidBodySet, + out: &mut Vec>, + ) { + for out_island in &mut out[..bodies.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 rb1 = &bodies[joint.body1]; + let rb2 = &bodies[joint.body2]; + + if (rb1.is_dynamic() || rb2.is_dynamic()) + && (!rb1.is_dynamic() || !rb1.is_sleeping()) + && (!rb2.is_dynamic() || !rb2.is_sleeping()) + { + let island_index = if !rb1.is_dynamic() { + rb2.active_island_id + } else { + rb1.active_island_id + }; + + out[island_index].push(i); + } + } + } + + pub(crate) fn remove_rigid_body( + &mut self, + deleted_id: RigidBodyGraphIndex, + bodies: &mut RigidBodySet, + ) { + 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 { + let to_delete_edge_id = self.joint_ids.remove(to_delete_handle).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; + } + + // Wake up the attached bodies. + bodies.wake_up(h1); + bodies.wake_up(h2); + } + + 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. + if let Some(replacement) = bodies.get_mut_internal(other) { + replacement.joint_graph_index = deleted_id; + } + } + } + } +} diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs new file mode 100644 index 0000000..b4dd60e --- /dev/null +++ b/src/dynamics/joint/mod.rs @@ -0,0 +1,16 @@ +pub use self::ball_joint::BallJoint; +pub use self::fixed_joint::FixedJoint; +pub use self::joint::{Joint, JointParams}; +pub(crate) use self::joint_set::{JointGraphEdge, JointIndex}; +pub use self::joint_set::{JointHandle, JointSet}; +pub use self::prismatic_joint::PrismaticJoint; +#[cfg(feature = "dim3")] +pub use self::revolute_joint::RevoluteJoint; + +mod ball_joint; +mod fixed_joint; +mod joint; +mod joint_set; +mod prismatic_joint; +#[cfg(feature = "dim3")] +mod revolute_joint; diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs new file mode 100644 index 0000000..a6fd558 --- /dev/null +++ b/src/dynamics/joint/prismatic_joint.rs @@ -0,0 +1,193 @@ +use crate::math::{Isometry, Point, Vector, DIM}; +use crate::utils::WBasis; +use na::Unit; +#[cfg(feature = "dim2")] +use na::Vector2; +#[cfg(feature = "dim3")] +use na::Vector5; + +#[derive(Copy, Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// 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, + /// Where the prismatic joint is attached on the second body, expressed in the local space of the second attached body. + pub local_anchor2: Point, + pub(crate) local_axis1: Unit>, + pub(crate) local_axis2: Unit>, + pub(crate) basis1: [Vector; DIM - 1], + pub(crate) basis2: [Vector; 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, + /// 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, + /// 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], + /// 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 motor_enabled: bool, + // pub target_motor_vel: f32, + // pub max_motor_impulse: f32, + // pub motor_impulse: f32, +} + +impl PrismaticJoint { + /// Creates a new prismatic joint with the given point of applications and axis, all expressed + /// in the local-space of the affected bodies. + #[cfg(feature = "dim2")] + pub fn new( + local_anchor1: Point, + local_axis1: Unit>, + local_anchor2: Point, + local_axis2: Unit>, + ) -> Self { + Self { + local_anchor1, + local_anchor2, + local_axis1, + local_axis2, + basis1: local_axis1.orthonormal_basis(), + basis2: local_axis2.orthonormal_basis(), + impulse: na::zero(), + limits_enabled: false, + limits: [-f32::MAX, f32::MAX], + limits_impulse: 0.0, + // motor_enabled: false, + // target_motor_vel: 0.0, + // max_motor_impulse: f32::MAX, + // motor_impulse: 0.0, + } + } + + /// Creates a new prismatic joint with the given point of applications and axis, all expressed + /// in the local-space of the affected bodies. + /// + /// The local tangent are vector orthogonal to the local axis. It is used to compute a basis orthonormal + /// to the joint's axis. If this tangent is set to zero, te orthonormal basis will be automatically + /// computed arbitrarily. + #[cfg(feature = "dim3")] + pub fn new( + local_anchor1: Point, + local_axis1: Unit>, + local_tangent1: Vector, + local_anchor2: Point, + local_axis2: Unit>, + local_tangent2: Vector, + ) -> Self { + let basis1 = if let Some(local_bitangent1) = + Unit::try_new(local_axis1.cross(&local_tangent1), 1.0e-3) + { + [ + local_bitangent1.into_inner(), + local_bitangent1.cross(&local_axis1), + ] + } else { + local_axis1.orthonormal_basis() + }; + + let basis2 = if let Some(local_bitangent2) = + Unit::try_new(local_axis2.cross(&local_tangent2), 2.0e-3) + { + [ + local_bitangent2.into_inner(), + local_bitangent2.cross(&local_axis2), + ] + } else { + local_axis2.orthonormal_basis() + }; + + Self { + local_anchor1, + local_anchor2, + local_axis1, + local_axis2, + basis1, + basis2, + impulse: na::zero(), + limits_enabled: false, + limits: [-f32::MAX, f32::MAX], + limits_impulse: 0.0, + // motor_enabled: false, + // target_motor_vel: 0.0, + // max_motor_impulse: f32::MAX, + // motor_impulse: 0.0, + } + } + + /// The local axis of this joint, expressed in the local-space of the first attached body. + pub fn local_axis1(&self) -> Unit> { + self.local_axis1 + } + + /// The local axis of this joint, expressed in the local-space of the second attached body. + pub fn local_axis2(&self) -> Unit> { + self.local_axis2 + } + + // FIXME: precompute this? + #[cfg(feature = "dim2")] + pub(crate) fn local_frame1(&self) -> Isometry { + use na::{Matrix2, Rotation2, UnitComplex}; + + let mat = Matrix2::from_columns(&[self.local_axis1.into_inner(), self.basis1[0]]); + let rotmat = Rotation2::from_matrix_unchecked(mat); + let rotation = UnitComplex::from_rotation_matrix(&rotmat); + let translation = self.local_anchor1.coords.into(); + Isometry::from_parts(translation, rotation) + } + + // FIXME: precompute this? + #[cfg(feature = "dim2")] + pub(crate) fn local_frame2(&self) -> Isometry { + use na::{Matrix2, Rotation2, UnitComplex}; + + let mat = Matrix2::from_columns(&[self.local_axis2.into_inner(), self.basis2[0]]); + let rotmat = Rotation2::from_matrix_unchecked(mat); + let rotation = UnitComplex::from_rotation_matrix(&rotmat); + let translation = self.local_anchor2.coords.into(); + Isometry::from_parts(translation, rotation) + } + + // FIXME: precompute this? + #[cfg(feature = "dim3")] + pub(crate) fn local_frame1(&self) -> Isometry { + use na::{Matrix3, Rotation3, UnitQuaternion}; + + let mat = Matrix3::from_columns(&[ + self.local_axis1.into_inner(), + self.basis1[0], + self.basis1[1], + ]); + let rotmat = Rotation3::from_matrix_unchecked(mat); + let rotation = UnitQuaternion::from_rotation_matrix(&rotmat); + let translation = self.local_anchor1.coords.into(); + Isometry::from_parts(translation, rotation) + } + + // FIXME: precompute this? + #[cfg(feature = "dim3")] + pub(crate) fn local_frame2(&self) -> Isometry { + use na::{Matrix3, Rotation3, UnitQuaternion}; + + let mat = Matrix3::from_columns(&[ + self.local_axis2.into_inner(), + self.basis2[0], + self.basis2[1], + ]); + let rotmat = Rotation3::from_matrix_unchecked(mat); + let rotation = UnitQuaternion::from_rotation_matrix(&rotmat); + let translation = self.local_anchor2.coords.into(); + Isometry::from_parts(translation, rotation) + } +} diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs new file mode 100644 index 0000000..cdb424b --- /dev/null +++ b/src/dynamics/joint/revolute_joint.rs @@ -0,0 +1,46 @@ +use crate::math::{Point, Vector}; +use crate::utils::WBasis; +use na::{Unit, Vector5}; + +#[derive(Copy, Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A joint that removes all relative motion between two bodies, except for the rotations along one axis. +pub struct RevoluteJoint { + /// Where the revolute joint is attached on the first body, expressed in the local space of the first attached body. + pub local_anchor1: Point, + /// Where the revolute joint is attached on the second body, expressed in the local space of the second attached body. + pub local_anchor2: Point, + /// The rotation axis of this revolute joint expressed in the local space of the first attached body. + pub local_axis1: Unit>, + /// The rotation axis of this revolute joint expressed in the local space of the second attached body. + pub local_axis2: Unit>, + /// The basis orthonormal to `local_axis1`, expressed in the local space of the first attached body. + pub basis1: [Vector; 2], + /// The basis orthonormal to `local_axis2`, expressed in the local space of the second attached body. + pub basis2: [Vector; 2], + /// The impulse applied by this joint on the first body. + /// + /// The impulse applied to the second body is given by `-impulse`. + pub impulse: Vector5, +} + +impl RevoluteJoint { + /// Creates a new revolute joint with the given point of applications and axis, all expressed + /// in the local-space of the affected bodies. + pub fn new( + local_anchor1: Point, + local_axis1: Unit>, + local_anchor2: Point, + local_axis2: Unit>, + ) -> Self { + Self { + local_anchor1, + local_anchor2, + local_axis1, + local_axis2, + basis1: local_axis1.orthonormal_basis(), + basis2: local_axis2.orthonormal_basis(), + impulse: na::zero(), + } + } +} diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs new file mode 100644 index 0000000..cc2979c --- /dev/null +++ b/src/dynamics/mass_properties.rs @@ -0,0 +1,206 @@ +use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Vector}; +use crate::utils; +use num::Zero; +use std::ops::{Add, AddAssign}; +#[cfg(feature = "dim3")] +use {na::Matrix3, std::ops::MulAssign}; + +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// The local mass properties of a rigid-body. +pub struct MassProperties { + /// The center of mass of a rigid-body expressed in its local-space. + pub local_com: Point, + /// The inverse of the mass of a rigid-body. + /// + /// If this is zero, the rigid-body is assumed to have infinite mass. + pub inv_mass: f32, + /// The inverse of the principal angular inertia of the rigid-body. + /// + /// Components set to zero are assumed to be infinite along the corresponding principal axis. + pub inv_principal_inertia_sqrt: AngVector, + #[cfg(feature = "dim3")] + /// The principal vectors of the local angular inertia tensor of the rigid-body. + pub principal_inertia_local_frame: Rotation, +} + +impl MassProperties { + #[cfg(feature = "dim2")] + pub(crate) fn new(local_com: Point, mass: f32, principal_inertia: f32) -> Self { + let inv_mass = utils::inv(mass); + let inv_principal_inertia_sqrt = utils::inv(principal_inertia.sqrt()); + Self { + local_com, + inv_mass, + inv_principal_inertia_sqrt, + } + } + + #[cfg(feature = "dim3")] + pub(crate) fn new(local_com: Point, mass: f32, principal_inertia: AngVector) -> Self { + Self::with_principal_inertia_frame(local_com, mass, principal_inertia, Rotation::identity()) + } + + #[cfg(feature = "dim3")] + pub(crate) fn with_principal_inertia_frame( + local_com: Point, + mass: f32, + principal_inertia: AngVector, + principal_inertia_local_frame: Rotation, + ) -> Self { + let inv_mass = utils::inv(mass); + let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt())); + Self { + local_com, + inv_mass, + inv_principal_inertia_sqrt, + principal_inertia_local_frame, + } + } + + /// The world-space center of mass of the rigid-body. + pub fn world_com(&self, pos: &Isometry) -> Point { + pos * self.local_com + } + + #[cfg(feature = "dim2")] + /// The world-space inverse angular inertia tensor of the rigid-body. + pub fn world_inv_inertia_sqrt(&self, _rot: &Rotation) -> AngularInertia { + self.inv_principal_inertia_sqrt + } + + #[cfg(feature = "dim3")] + /// The world-space inverse angular inertia tensor of the rigid-body. + pub fn world_inv_inertia_sqrt(&self, rot: &Rotation) -> AngularInertia { + if !self.inv_principal_inertia_sqrt.is_zero() { + let mut lhs = (rot * self.principal_inertia_local_frame) + .to_rotation_matrix() + .into_inner(); + let rhs = lhs.transpose(); + lhs.column_mut(0) + .mul_assign(self.inv_principal_inertia_sqrt.x); + lhs.column_mut(1) + .mul_assign(self.inv_principal_inertia_sqrt.y); + lhs.column_mut(2) + .mul_assign(self.inv_principal_inertia_sqrt.z); + let inertia = lhs * rhs; + AngularInertia::from_sdp_matrix(inertia) + } else { + AngularInertia::zero() + } + } + + #[cfg(feature = "dim3")] + /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axii. + pub fn reconstruct_inertia_matrix(&self) -> Matrix3 { + let principal_inertia = self.inv_principal_inertia_sqrt.map(|e| utils::inv(e * e)); + self.principal_inertia_local_frame.to_rotation_matrix() + * Matrix3::from_diagonal(&principal_inertia) + * self + .principal_inertia_local_frame + .inverse() + .to_rotation_matrix() + } + + #[cfg(feature = "dim2")] + pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector) -> f32 { + if self.inv_mass != 0.0 { + let mass = 1.0 / self.inv_mass; + let i = utils::inv(self.inv_principal_inertia_sqrt * self.inv_principal_inertia_sqrt); + i + shift.norm_squared() * mass + } else { + 0.0 + } + } + + #[cfg(feature = "dim3")] + pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector) -> Matrix3 { + if self.inv_mass != 0.0 { + let mass = 1.0 / self.inv_mass; + let matrix = self.reconstruct_inertia_matrix(); + let diag = shift.norm_squared(); + let diagm = Matrix3::from_diagonal_element(diag); + matrix + (diagm + shift * shift.transpose()) * mass + } else { + Matrix3::zeros() + } + } +} + +impl Zero for MassProperties { + fn zero() -> Self { + Self { + inv_mass: 0.0, + inv_principal_inertia_sqrt: na::zero(), + #[cfg(feature = "dim3")] + principal_inertia_local_frame: Rotation::identity(), + local_com: Point::origin(), + } + } + + fn is_zero(&self) -> bool { + *self == Self::zero() + } +} + +impl Add for MassProperties { + type Output = Self; + + #[cfg(feature = "dim2")] + fn add(self, other: MassProperties) -> Self { + if self.is_zero() { + return other; + } else if other.is_zero() { + return self; + } + + let m1 = utils::inv(self.inv_mass); + let m2 = utils::inv(other.inv_mass); + let inv_mass = utils::inv(m1 + m2); + let local_com = (self.local_com * m1 + other.local_com.coords * m2) * inv_mass; + let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); + let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); + let inertia = i1 + i2; + let inv_principal_inertia_sqrt = utils::inv(inertia.sqrt()); + + Self { + local_com, + inv_mass, + inv_principal_inertia_sqrt, + } + } + + #[cfg(feature = "dim3")] + fn add(self, other: MassProperties) -> Self { + if self.is_zero() { + return other; + } else if other.is_zero() { + return self; + } + + let m1 = utils::inv(self.inv_mass); + let m2 = utils::inv(other.inv_mass); + let inv_mass = utils::inv(m1 + m2); + let local_com = (self.local_com * m1 + other.local_com.coords * m2) * inv_mass; + let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); + let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); + let inertia = i1 + i2; + let eigen = inertia.symmetric_eigen(); + let principal_inertia_local_frame = Rotation::from_matrix(&eigen.eigenvectors); + let principal_inertia = eigen.eigenvalues; + let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt())); + + Self { + local_com, + inv_mass, + inv_principal_inertia_sqrt, + principal_inertia_local_frame, + } + } +} + +impl AddAssign for MassProperties { + fn add_assign(&mut self, rhs: MassProperties) { + *self = *self + rhs + } +} diff --git a/src/dynamics/mass_properties_ball.rs b/src/dynamics/mass_properties_ball.rs new file mode 100644 index 0000000..ac5790a --- /dev/null +++ b/src/dynamics/mass_properties_ball.rs @@ -0,0 +1,30 @@ +use crate::dynamics::MassProperties; +#[cfg(feature = "dim3")] +use crate::math::Vector; +use crate::math::{Point, PrincipalAngularInertia}; + +impl MassProperties { + pub(crate) fn ball_volume_unit_angular_inertia( + radius: f32, + ) -> (f32, PrincipalAngularInertia) { + #[cfg(feature = "dim2")] + { + let volume = std::f32::consts::PI * radius * radius; + let i = radius * radius / 2.0; + (volume, i) + } + #[cfg(feature = "dim3")] + { + let volume = std::f32::consts::PI * radius * radius * radius * 4.0 / 3.0; + let i = radius * radius * 2.0 / 5.0; + + (volume, Vector::repeat(i)) + } + } + + pub(crate) fn from_ball(density: f32, radius: f32) -> Self { + let (vol, unit_i) = Self::ball_volume_unit_angular_inertia(radius); + let mass = vol * density; + Self::new(Point::origin(), mass, unit_i * mass) + } +} diff --git a/src/dynamics/mass_properties_capsule.rs b/src/dynamics/mass_properties_capsule.rs new file mode 100644 index 0000000..5f08958 --- /dev/null +++ b/src/dynamics/mass_properties_capsule.rs @@ -0,0 +1,60 @@ +use crate::dynamics::MassProperties; +#[cfg(feature = "dim3")] +use crate::geometry::Capsule; +use crate::math::{Point, PrincipalAngularInertia, Vector}; + +impl MassProperties { + fn cylinder_y_volume_unit_inertia( + half_height: f32, + radius: f32, + ) -> (f32, PrincipalAngularInertia) { + #[cfg(feature = "dim2")] + { + Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height)) + } + + #[cfg(feature = "dim3")] + { + let volume = half_height * radius * radius * std::f32::consts::PI * 2.0; + let sq_radius = radius * radius; + let sq_height = half_height * half_height * 4.0; + let off_principal = (sq_radius * 3.0 + sq_height) / 12.0; + + let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal); + (volume, inertia) + } + } + + pub(crate) fn from_capsule(density: f32, a: Point, b: Point, radius: f32) -> Self { + let half_height = (b - a).norm() / 2.0; + let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); + let (ball_vol, ball_unit_i) = Self::ball_volume_unit_angular_inertia(radius); + let cap_vol = cyl_vol + ball_vol; + let cap_mass = cap_vol * density; + let mut cap_unit_i = cyl_unit_i + ball_unit_i; + let local_com = na::center(&a, &b); + + #[cfg(feature = "dim2")] + { + let h = half_height * 2.0; + let extra = h * h * 0.5 + h * radius * 3.0 / 8.0; + cap_unit_i += extra; + Self::new(local_com, cap_mass, cap_unit_i * cap_mass) + } + + #[cfg(feature = "dim3")] + { + let h = half_height * 2.0; + let extra = h * h * 0.5 + h * radius * 3.0 / 8.0; + cap_unit_i.x += extra; + cap_unit_i.z += extra; + let local_frame = Capsule::new(a, b, radius).rotation_wrt_y(); + Self::with_principal_inertia_frame( + local_com, + cap_mass, + cap_unit_i * cap_mass, + local_frame, + ) + } + } +} diff --git a/src/dynamics/mass_properties_cuboid.rs b/src/dynamics/mass_properties_cuboid.rs new file mode 100644 index 0000000..2d870cf --- /dev/null +++ b/src/dynamics/mass_properties_cuboid.rs @@ -0,0 +1,33 @@ +use crate::dynamics::MassProperties; +use crate::math::{Point, PrincipalAngularInertia, Vector}; + +impl MassProperties { + pub(crate) fn cuboid_volume_unit_inertia( + half_extents: Vector, + ) -> (f32, PrincipalAngularInertia) { + #[cfg(feature = "dim2")] + { + let volume = half_extents.x * half_extents.y * 4.0; + let ix = (half_extents.x * half_extents.x) / 3.0; + let iy = (half_extents.y * half_extents.y) / 3.0; + + (volume, ix + iy) + } + + #[cfg(feature = "dim3")] + { + let volume = half_extents.x * half_extents.y * half_extents.z * 8.0; + let ix = (half_extents.x * half_extents.x) / 3.0; + let iy = (half_extents.y * half_extents.y) / 3.0; + let iz = (half_extents.z * half_extents.z) / 3.0; + + (volume, Vector::new(iy + iz, ix + iz, ix + iy)) + } + } + + pub(crate) fn from_cuboid(density: f32, half_extents: Vector) -> Self { + let (vol, unit_i) = Self::cuboid_volume_unit_inertia(half_extents); + let mass = vol * density; + Self::new(Point::origin(), mass, unit_i * mass) + } +} diff --git a/src/dynamics/mass_properties_polygon.rs b/src/dynamics/mass_properties_polygon.rs new file mode 100644 index 0000000..c87e888 --- /dev/null +++ b/src/dynamics/mass_properties_polygon.rs @@ -0,0 +1,144 @@ +use crate::dynamics::MassProperties; +use crate::math::Point; + +impl MassProperties { + pub(crate) fn from_polygon(density: f32, vertices: &[Point]) -> MassProperties { + let (area, com) = convex_polygon_area_and_center_of_mass(vertices); + + if area == 0.0 { + return MassProperties::new(com, 0.0, 0.0); + } + + let mut itot = 0.0; + let factor = 1.0 / 6.0; + + let mut iterpeek = vertices.iter().peekable(); + let firstelement = *iterpeek.peek().unwrap(); // store first element to close the cycle in the end with unwrap_or + while let Some(elem) = iterpeek.next() { + let area = triangle_area(&com, elem, iterpeek.peek().unwrap_or(&firstelement)); + + // algorithm adapted from Box2D + let e1 = *elem - com; + let e2 = **(iterpeek.peek().unwrap_or(&firstelement)) - com; + + let ex1 = e1[0]; + let ey1 = e1[1]; + let ex2 = e2[0]; + let ey2 = e2[1]; + + let intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2; + let inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2; + + let ipart = factor * (intx2 + inty2); + + itot += ipart * area; + } + + Self::new(com, area * density, itot * density) + } +} + +fn convex_polygon_area_and_center_of_mass(convex_polygon: &[Point]) -> (f32, Point) { + let geometric_center = convex_polygon + .iter() + .fold(Point::origin(), |e1, e2| e1 + e2.coords) + / convex_polygon.len() as f32; + let mut res = Point::origin(); + let mut areasum = 0.0; + + let mut iterpeek = convex_polygon.iter().peekable(); + let firstelement = *iterpeek.peek().unwrap(); // Stores first element to close the cycle in the end with unwrap_or. + while let Some(elem) = iterpeek.next() { + let (a, b, c) = ( + elem, + iterpeek.peek().unwrap_or(&firstelement), + &geometric_center, + ); + let area = triangle_area(a, b, c); + let center = (a.coords + b.coords + c.coords) / 3.0; + + res += center * area; + areasum += area; + } + + if areasum == 0.0 { + (areasum, geometric_center) + } else { + (areasum, res / areasum) + } +} + +pub fn triangle_area(pa: &Point, pb: &Point, pc: &Point) -> f32 { + // Kahan's formula. + let a = na::distance(pa, pb); + let b = na::distance(pb, pc); + let c = na::distance(pc, pa); + + let (c, b, a) = sort3(&a, &b, &c); + let a = *a; + let b = *b; + let c = *c; + + let sqr = (a + (b + c)) * (c - (a - b)) * (c + (a - b)) * (a + (b - c)); + + sqr.sqrt() * 0.25 +} + +/// Sorts a set of three values in increasing order. +#[inline] +pub fn sort3<'a>(a: &'a f32, b: &'a f32, c: &'a f32) -> (&'a f32, &'a f32, &'a f32) { + let a_b = *a > *b; + let a_c = *a > *c; + let b_c = *b > *c; + + let sa; + let sb; + let sc; + + // Sort the three values. + // FIXME: move this to the utilities? + if a_b { + // a > b + if a_c { + // a > c + sc = a; + + if b_c { + // b > c + sa = c; + sb = b; + } else { + // b <= c + sa = b; + sb = c; + } + } else { + // a <= c + sa = b; + sb = a; + sc = c; + } + } else { + // a < b + if !a_c { + // a <= c + sa = a; + + if b_c { + // b > c + sb = c; + sc = b; + } else { + sb = b; + sc = c; + } + } else { + // a > c + sa = c; + sb = a; + sc = b; + } + } + + (sa, sb, sc) +} diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs new file mode 100644 index 0000000..4499d95 --- /dev/null +++ b/src/dynamics/mod.rs @@ -0,0 +1,30 @@ +//! Structures related to dynamics: bodies, joints, etc. + +pub use self::integration_parameters::IntegrationParameters; +pub(crate) use self::joint::JointIndex; +#[cfg(feature = "dim3")] +pub use self::joint::RevoluteJoint; +pub use self::joint::{ + BallJoint, FixedJoint, Joint, JointHandle, JointParams, JointSet, PrismaticJoint, +}; +pub use self::mass_properties::MassProperties; +pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder}; +pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodyMut, RigidBodySet}; +// #[cfg(not(feature = "parallel"))] +pub(crate) use self::joint::JointGraphEdge; +#[cfg(not(feature = "parallel"))] +pub(crate) use self::solver::IslandSolver; +#[cfg(feature = "parallel")] +pub(crate) use self::solver::ParallelIslandSolver; + +mod integration_parameters; +mod joint; +mod mass_properties; +mod mass_properties_ball; +mod mass_properties_capsule; +mod mass_properties_cuboid; +#[cfg(feature = "dim2")] +mod mass_properties_polygon; +mod rigid_body; +mod rigid_body_set; +mod solver; diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs new file mode 100644 index 0000000..584dc96 --- /dev/null +++ b/src/dynamics/rigid_body.rs @@ -0,0 +1,441 @@ +use crate::dynamics::MassProperties; +use crate::geometry::{ColliderHandle, InteractionGraph, RigidBodyGraphIndex}; +use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Translation, Vector}; +use crate::utils::{WCross, WDot}; +use num::Zero; + +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// The status of a body, governing the way it is affected by external forces. +pub enum BodyStatus { + /// A `BodyStatus::Dynamic` body can be affected by all external forces. + Dynamic, + /// A `BodyStatus::Static` body cannot be affected by external forces. + Static, + /// A `BodyStatus::Kinematic` body cannot be affected by any external forces but can be controlled + /// by the user at the position level while keeping realistic one-way interaction with dynamic bodies. + /// + /// One-way interaction means that a kinematic body can push a dynamic body, but a kinematic body + /// cannot be pushed by anything. In other words, the trajectory of a kinematic body can only be + /// modified by the user and is independent from any contact or joint it is involved in. + Kinematic, + // Semikinematic, // A kinematic that performs automatic CCD with the static environment toi avoid traversing it? + // Disabled, +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A rigid body. +/// +/// To create a new rigid-body, use the `RigidBodyBuilder` structure. +#[derive(Debug)] +pub struct RigidBody { + /// The world-space position of the rigid-body. + pub position: Isometry, + pub(crate) predicted_position: Isometry, + /// The local mass properties of the rigid-body. + pub mass_properties: MassProperties, + /// The world-space center of mass of the rigid-body. + pub world_com: Point, + /// The square-root of the inverse angular inertia tensor of the rigid-body. + pub world_inv_inertia_sqrt: AngularInertia, + /// The linear velocity of the rigid-body. + pub linvel: Vector, + /// The angular velocity of the rigid-body. + pub angvel: AngVector, + pub(crate) linacc: Vector, + pub(crate) angacc: AngVector, + pub(crate) colliders: Vec, + /// Whether or not this rigid-body is sleeping. + pub activation: ActivationStatus, + pub(crate) joint_graph_index: RigidBodyGraphIndex, + pub(crate) active_island_id: usize, + pub(crate) active_set_id: usize, + pub(crate) active_set_offset: usize, + pub(crate) active_set_timestamp: u32, + /// The status of the body, governing how it is affected by external forces. + pub body_status: BodyStatus, +} + +impl Clone for RigidBody { + fn clone(&self) -> Self { + Self { + colliders: Vec::new(), + joint_graph_index: RigidBodyGraphIndex::new(crate::INVALID_U32), + active_island_id: crate::INVALID_USIZE, + active_set_id: crate::INVALID_USIZE, + active_set_offset: crate::INVALID_USIZE, + active_set_timestamp: crate::INVALID_U32, + ..*self + } + } +} + +impl RigidBody { + fn new() -> Self { + Self { + position: Isometry::identity(), + predicted_position: Isometry::identity(), + mass_properties: MassProperties::zero(), + world_com: Point::origin(), + world_inv_inertia_sqrt: AngularInertia::zero(), + linvel: Vector::zeros(), + angvel: na::zero(), + linacc: Vector::zeros(), + angacc: na::zero(), + colliders: Vec::new(), + activation: ActivationStatus::new_active(), + joint_graph_index: InteractionGraph::<()>::invalid_graph_index(), + active_island_id: 0, + active_set_id: 0, + active_set_offset: 0, + active_set_timestamp: 0, + body_status: BodyStatus::Dynamic, + } + } + + pub(crate) fn integrate_accelerations(&mut self, dt: f32, gravity: Vector) { + if self.mass_properties.inv_mass != 0.0 { + self.linvel += (gravity + self.linacc) * dt; + self.angvel += self.angacc * dt; + + // Reset the accelerations. + self.linacc = na::zero(); + self.angacc = na::zero(); + } + } + + /// The handles of colliders attached to this rigid body. + pub fn colliders(&self) -> &[ColliderHandle] { + &self.colliders[..] + } + + /// Is this rigid body dynamic? + /// + /// A dynamic body can move freely and is affected by forc