From aa61fe65e3ff0289ecab57b4053a3410cf6d4a87 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 4 Jan 2021 15:14:25 +0100 Subject: Add support of 64-bits reals. --- src/dynamics/joint/ball_joint.rs | 16 ++++----- src/dynamics/joint/fixed_joint.rs | 10 +++--- src/dynamics/joint/prismatic_joint.rs | 68 +++++++++++++++++------------------ src/dynamics/joint/revolute_joint.rs | 24 ++++++------- 4 files changed, 59 insertions(+), 59 deletions(-) (limited to 'src/dynamics/joint') 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, + 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, + 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, + 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 { + 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, + local_anchor1: Point, + local_anchor2: Point, + impulse: Vector, ) -> 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, + 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, + 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 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 { + pub fn new(local_anchor1: Isometry, local_anchor2: Isometry) -> Self { Self { local_anchor1, local_anchor2, 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, + 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], + 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, + 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, + 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], + 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, - local_axis1: Unit>, - local_anchor2: Point, - local_axis2: Unit>, + local_anchor1: Point, + local_axis1: Unit>, + local_anchor2: Point, + local_axis2: Unit>, ) -> 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, - local_axis1: Unit>, - local_tangent1: Vector, - local_anchor2: Point, - local_axis2: Unit>, - local_tangent2: Vector, + 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) @@ -116,28 +116,28 @@ impl PrismaticJoint { basis2, 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, } } /// The local axis of this joint, expressed in the local-space of the first attached body. - pub fn local_axis1(&self) -> Unit> { + 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> { + pub fn local_axis2(&self) -> Unit> { self.local_axis2 } // FIXME: precompute this? #[cfg(feature = "dim2")] - pub(crate) fn local_frame1(&self) -> Isometry { + 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]]); @@ -149,7 +149,7 @@ impl PrismaticJoint { // FIXME: precompute this? #[cfg(feature = "dim2")] - pub(crate) fn local_frame2(&self) -> Isometry { + 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]]); @@ -161,7 +161,7 @@ impl PrismaticJoint { // FIXME: precompute this? #[cfg(feature = "dim3")] - pub(crate) fn local_frame1(&self) -> Isometry { + pub(crate) fn local_frame1(&self) -> Isometry { use na::{Matrix3, Rotation3, UnitQuaternion}; let mat = Matrix3::from_columns(&[ @@ -177,7 +177,7 @@ impl PrismaticJoint { // FIXME: precompute this? #[cfg(feature = "dim3")] - pub(crate) fn local_frame2(&self) -> Isometry { + pub(crate) fn local_frame2(&self) -> Isometry { use na::{Matrix3, Rotation3, UnitQuaternion}; let mat = Matrix3::from_columns(&[ diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs index cdb424b..ad7db0d 100644 --- a/src/dynamics/joint/revolute_joint.rs +++ b/src/dynamics/joint/revolute_joint.rs @@ -1,4 +1,4 @@ -use crate::math::{Point, Vector}; +use crate::math::{Point, Real, Vector}; use crate::utils::WBasis; use na::{Unit, Vector5}; @@ -7,31 +7,31 @@ use na::{Unit, Vector5}; /// 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, + 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, + 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>, + 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>, + pub local_axis2: Unit>, /// The basis orthonormal to `local_axis1`, expressed in the local space of the first attached body. - pub basis1: [Vector; 2], + pub basis1: [Vector; 2], /// The basis orthonormal to `local_axis2`, expressed in the local space of the second attached body. - pub basis2: [Vector; 2], + 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, + 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>, + local_anchor1: Point, + local_axis1: Unit>, + local_anchor2: Point, + local_axis2: Unit>, ) -> Self { Self { local_anchor1, -- cgit From 0ade350b5f4b6e7c0c4116e1f4f2b30ab86b7e52 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Wed, 20 Jan 2021 16:33:42 +0100 Subject: Use newtypes for collider, rigid-body and joint handles. --- src/dynamics/joint/joint_set.rs | 65 ++++++++++++++++++++++++++++------------- 1 file changed, 44 insertions(+), 21 deletions(-) (limited to 'src/dynamics/joint') diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs index 5144d97..50c88a2 100644 --- a/src/dynamics/joint/joint_set.rs +++ b/src/dynamics/joint/joint_set.rs @@ -1,11 +1,34 @@ 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 { + pub fn into_raw_parts(self) -> (usize, u64) { + self.0.into_raw_parts() + } + + 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; @@ -13,7 +36,7 @@ pub(crate) type JointGraphEdge = crate::data::graph::Edge; /// 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, + joint_graph: InteractionGraph, } impl JointSet { @@ -25,29 +48,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 { + 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) + 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 +80,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 +138,7 @@ impl JointSet { let joint = Joint { body1, body2, - handle, + handle: JointHandle(handle), #[cfg(feature = "parallel")] constraint_index: 0, #[cfg(feature = "parallel")] @@ -133,11 +154,13 @@ impl JointSet { // 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) { + 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) { + if !InteractionGraph::::is_graph_index_valid(rb2.joint_graph_index) + { rb2.joint_graph_index = self.joint_graph.graph.add_node(joint.body2); } @@ -146,7 +169,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 +214,7 @@ impl JointSet { bodies: &mut RigidBodySet, wake_up: bool, ) -> Option { - 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 +230,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 +241,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 +252,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. -- cgit From 8ff2bcc3ec666805aceedaa477bde89f2a577d1c Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Wed, 27 Jan 2021 14:20:14 +0100 Subject: Add all the missing docs. --- src/dynamics/joint/joint_set.rs | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/dynamics/joint') diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs index 50c88a2..a87532a 100644 --- a/src/dynamics/joint/joint_set.rs +++ b/src/dynamics/joint/joint_set.rs @@ -12,10 +12,12 @@ use crate::dynamics::{JointParams, RigidBodyHandle, RigidBodySet}; 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)) } -- cgit