From ecd308338b189ab569816a38a03e3f8b89669dde Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 17 Mar 2024 21:20:18 +0100 Subject: feat: start experimenting with a glam/bevy version --- src/dynamics/joint/fixed_joint.rs | 26 ++--- src/dynamics/joint/generic_joint.rs | 65 ++++++------ src/dynamics/joint/impulse_joint/impulse_joint.rs | 4 +- .../joint/impulse_joint/impulse_joint_set.rs | 107 +++++++++++++------- src/dynamics/joint/motor_model.rs | 2 +- src/dynamics/joint/multibody_joint/multibody.rs | 80 +++++++-------- .../joint/multibody_joint/multibody_joint.rs | 47 ++++----- .../joint/multibody_joint/multibody_joint_set.rs | 111 +++++++++++++++------ .../joint/multibody_joint/multibody_link.rs | 28 +++--- .../joint/multibody_joint/multibody_workspace.rs | 8 +- .../joint/multibody_joint/unit_multibody_joint.rs | 2 +- src/dynamics/joint/prismatic_joint.rs | 30 +++--- src/dynamics/joint/revolute_joint.rs | 21 ++-- src/dynamics/joint/rope_joint.rs | 14 +-- src/dynamics/joint/spherical_joint.rs | 26 ++--- src/dynamics/joint/spring_joint.rs | 14 +-- 16 files changed, 327 insertions(+), 258 deletions(-) (limited to 'src/dynamics/joint') diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs index 2bb2f64..f5a0250 100644 --- a/src/dynamics/joint/fixed_joint.rs +++ b/src/dynamics/joint/fixed_joint.rs @@ -1,5 +1,5 @@ use crate::dynamics::{GenericJoint, GenericJointBuilder, JointAxesMask}; -use crate::math::{Isometry, Point, Real}; +use crate::math::*; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] @@ -37,48 +37,48 @@ impl FixedJoint { /// The joint’s frame, expressed in the first rigid-body’s local-space. #[must_use] - pub fn local_frame1(&self) -> &Isometry { + pub fn local_frame1(&self) -> &Isometry { &self.data.local_frame1 } /// Sets the joint’s frame, expressed in the first rigid-body’s local-space. - pub fn set_local_frame1(&mut self, local_frame: Isometry) -> &mut Self { + pub fn set_local_frame1(&mut self, local_frame: Isometry) -> &mut Self { self.data.set_local_frame1(local_frame); self } /// The joint’s frame, expressed in the second rigid-body’s local-space. #[must_use] - pub fn local_frame2(&self) -> &Isometry { + pub fn local_frame2(&self) -> &Isometry { &self.data.local_frame2 } /// Sets joint’s frame, expressed in the second rigid-body’s local-space. - pub fn set_local_frame2(&mut self, local_frame: Isometry) -> &mut Self { + pub fn set_local_frame2(&mut self, local_frame: Isometry) -> &mut Self { self.data.set_local_frame2(local_frame); self } /// The joint’s anchor, expressed in the local-space of the first rigid-body. #[must_use] - pub fn local_anchor1(&self) -> Point { + pub fn local_anchor1(&self) -> Point { self.data.local_anchor1() } /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body. - pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { + pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { self.data.set_local_anchor1(anchor1); self } /// The joint’s anchor, expressed in the local-space of the second rigid-body. #[must_use] - pub fn local_anchor2(&self) -> Point { + pub fn local_anchor2(&self) -> Point { self.data.local_anchor2() } /// Sets the joint’s anchor, expressed in the local-space of the second rigid-body. - pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { + pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { self.data.set_local_anchor2(anchor2); self } @@ -110,28 +110,28 @@ impl FixedJointBuilder { /// Sets the joint’s frame, expressed in the first rigid-body’s local-space. #[must_use] - pub fn local_frame1(mut self, local_frame: Isometry) -> Self { + pub fn local_frame1(mut self, local_frame: Isometry) -> Self { self.0.set_local_frame1(local_frame); self } /// Sets joint’s frame, expressed in the second rigid-body’s local-space. #[must_use] - pub fn local_frame2(mut self, local_frame: Isometry) -> Self { + pub fn local_frame2(mut self, local_frame: Isometry) -> Self { self.0.set_local_frame2(local_frame); self } /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body. #[must_use] - pub fn local_anchor1(mut self, anchor1: Point) -> Self { + pub fn local_anchor1(mut self, anchor1: Point) -> Self { self.0.set_local_anchor1(anchor1); self } /// Sets the joint’s anchor, expressed in the local-space of the second rigid-body. #[must_use] - pub fn local_anchor2(mut self, anchor2: Point) -> Self { + pub fn local_anchor2(mut self, anchor2: Point) -> Self { self.0.set_local_anchor2(anchor2); self } diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 76a7fe1..8773b2c 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -2,7 +2,7 @@ use crate::dynamics::solver::MotorParameters; use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RopeJoint}; -use crate::math::{Isometry, Point, Real, Rotation, UnitVector, Vector, SPATIAL_DIM}; +use crate::math::*; use crate::utils::{SimdBasis, SimdRealCopy}; #[cfg(feature = "dim3")] @@ -212,9 +212,9 @@ pub enum JointEnabled { /// A generic joint. pub struct GenericJoint { /// The joint’s frame, expressed in the first rigid-body’s local-space. - pub local_frame1: Isometry, + pub local_frame1: Isometry, /// The joint’s frame, expressed in the second rigid-body’s local-space. - pub local_frame2: Isometry, + pub local_frame2: Isometry, /// The degrees-of-freedoms locked by this joint. pub locked_axes: JointAxesMask, /// The degrees-of-freedoms limited by this joint. @@ -280,23 +280,20 @@ impl GenericJoint { } #[doc(hidden)] - pub fn complete_ang_frame(axis: UnitVector) -> Rotation { + pub fn complete_ang_frame(axis: UnitVector) -> Rotation { let basis = axis.orthonormal_basis(); #[cfg(feature = "dim2")] { - use na::{Matrix2, Rotation2, UnitComplex}; - let mat = Matrix2::from_columns(&[axis.into_inner(), basis[0]]); - let rotmat = Rotation2::from_matrix_unchecked(mat); - UnitComplex::from_rotation_matrix(&rotmat) + let mat = Matrix::from_columns(&[axis.into_inner(), basis[0]]); + Rotation::from_matrix_unchecked(mat) } #[cfg(feature = "dim3")] { - use na::{Matrix3, Rotation3, UnitQuaternion}; - let mat = Matrix3::from_columns(&[axis.into_inner(), basis[0], basis[1]]); - let rotmat = Rotation3::from_matrix_unchecked(mat); - UnitQuaternion::from_rotation_matrix(&rotmat) + let mat = Matrix::from_columns(&[axis.into_inner(), basis[0], basis[1]]); + let rotmat = RotationMatrix::from_matrix_unchecked(mat); + Rotation::from_rotation_matrix(&rotmat) } } @@ -328,62 +325,62 @@ impl GenericJoint { } /// Sets the joint’s frame, expressed in the first rigid-body’s local-space. - pub fn set_local_frame1(&mut self, local_frame: Isometry) -> &mut Self { + pub fn set_local_frame1(&mut self, local_frame: Isometry) -> &mut Self { self.local_frame1 = local_frame; self } /// Sets the joint’s frame, expressed in the second rigid-body’s local-space. - pub fn set_local_frame2(&mut self, local_frame: Isometry) -> &mut Self { + pub fn set_local_frame2(&mut self, local_frame: Isometry) -> &mut Self { self.local_frame2 = local_frame; self } /// The principal (local X) axis of this joint, expressed in the first rigid-body’s local-space. #[must_use] - pub fn local_axis1(&self) -> UnitVector { - self.local_frame1 * Vector::x_axis() + pub fn local_axis1(&self) -> UnitVector { + self.local_frame1.rotation * Vector::x_axis() } /// Sets the principal (local X) axis of this joint, expressed in the first rigid-body’s local-space. - pub fn set_local_axis1(&mut self, local_axis: UnitVector) -> &mut Self { + pub fn set_local_axis1(&mut self, local_axis: UnitVector) -> &mut Self { self.local_frame1.rotation = Self::complete_ang_frame(local_axis); self } /// The principal (local X) axis of this joint, expressed in the second rigid-body’s local-space. #[must_use] - pub fn local_axis2(&self) -> UnitVector { - self.local_frame2 * Vector::x_axis() + pub fn local_axis2(&self) -> UnitVector { + self.local_frame2.rotation * Vector::x_axis() } /// Sets the principal (local X) axis of this joint, expressed in the second rigid-body’s local-space. - pub fn set_local_axis2(&mut self, local_axis: UnitVector) -> &mut Self { + pub fn set_local_axis2(&mut self, local_axis: UnitVector) -> &mut Self { self.local_frame2.rotation = Self::complete_ang_frame(local_axis); self } /// The anchor of this joint, expressed in the first rigid-body’s local-space. #[must_use] - pub fn local_anchor1(&self) -> Point { - self.local_frame1.translation.vector.into() + pub fn local_anchor1(&self) -> Point { + self.local_frame1.translation.into_vector().into() } /// Sets anchor of this joint, expressed in the first rigid-body’s local-space. - pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { - self.local_frame1.translation.vector = anchor1.coords; + pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { + *self.local_frame1.translation.as_vector_mut() = anchor1.into_vector(); self } /// The anchor of this joint, expressed in the second rigid-body’s local-space. #[must_use] - pub fn local_anchor2(&self) -> Point { - self.local_frame2.translation.vector.into() + pub fn local_anchor2(&self) -> Point { + self.local_frame2.translation.into_vector().into() } /// Sets anchor of this joint, expressed in the second rigid-body’s local-space. - pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { - self.local_frame2.translation.vector = anchor2.coords; + pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { + *self.local_frame2.translation.as_vector_mut() = anchor2.into_vector(); self } @@ -589,42 +586,42 @@ impl GenericJointBuilder { /// Sets the joint’s frame, expressed in the first rigid-body’s local-space. #[must_use] - pub fn local_frame1(mut self, local_frame: Isometry) -> Self { + pub fn local_frame1(mut self, local_frame: Isometry) -> Self { self.0.set_local_frame1(local_frame); self } /// Sets the joint’s frame, expressed in the second rigid-body’s local-space. #[must_use] - pub fn local_frame2(mut self, local_frame: Isometry) -> Self { + pub fn local_frame2(mut self, local_frame: Isometry) -> Self { self.0.set_local_frame2(local_frame); self } /// Sets the principal (local X) axis of this joint, expressed in the first rigid-body’s local-space. #[must_use] - pub fn local_axis1(mut self, local_axis: UnitVector) -> Self { + pub fn local_axis1(mut self, local_axis: UnitVector) -> Self { self.0.set_local_axis1(local_axis); self } /// Sets the principal (local X) axis of this joint, expressed in the second rigid-body’s local-space. #[must_use] - pub fn local_axis2(mut self, local_axis: UnitVector) -> Self { + pub fn local_axis2(mut self, local_axis: UnitVector) -> Self { self.0.set_local_axis2(local_axis); self } /// Sets the anchor of this joint, expressed in the first rigid-body’s local-space. #[must_use] - pub fn local_anchor1(mut self, anchor1: Point) -> Self { + pub fn local_anchor1(mut self, anchor1: Point) -> Self { self.0.set_local_anchor1(anchor1); self } /// Sets the anchor of this joint, expressed in the second rigid-body’s local-space. #[must_use] - pub fn local_anchor2(mut self, anchor2: Point) -> Self { + pub fn local_anchor2(mut self, anchor2: Point) -> Self { self.0.set_local_anchor2(anchor2); self } diff --git a/src/dynamics/joint/impulse_joint/impulse_joint.rs b/src/dynamics/joint/impulse_joint/impulse_joint.rs index 8d35c35..2be6223 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint.rs @@ -1,5 +1,5 @@ use crate::dynamics::{GenericJoint, ImpulseJointHandle, RigidBodyHandle}; -use crate::math::{Real, SpacialVector}; +use crate::math::*; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone, Debug, PartialEq)] @@ -14,7 +14,7 @@ pub struct ImpulseJoint { pub data: GenericJoint, /// The impulses applied by this joint. - pub impulses: SpacialVector, + pub impulses: SpatialVector, // A joint needs to know its handle to simplify its removal. pub(crate) handle: ImpulseJointHandle, diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index 3f9835e..7d4e651 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -2,17 +2,30 @@ use super::ImpulseJoint; use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex}; use crate::data::arena::Arena; -use crate::data::Coarena; +use crate::data::{Coarena, Index}; use crate::dynamics::{GenericJoint, IslandManager, RigidBodyHandle, RigidBodySet}; +#[cfg(feature = "bevy")] +use bevy::prelude::{Component, Reflect, ReflectComponent}; + /// The unique identifier of a joint added to the joint set. /// The unique identifier of a collider added to a collider set. #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg(not(feature = "bevy"))] #[repr(transparent)] pub struct ImpulseJointHandle(pub crate::data::arena::Index); +#[cfg(feature = "bevy")] +pub type ImpulseJointHandle = bevy::prelude::Entity; + +#[cfg(not(feature = "bevy"))] impl ImpulseJointHandle { + pub const PLACEHOLDER: Self = Self(Index::from_raw_parts( + crate::INVALID_U32, + crate::INVALID_U32, + )); + /// Converts this handle into its (index, generation) components. pub fn into_raw_parts(self) -> (u32, u32) { self.0.into_raw_parts() @@ -25,10 +38,21 @@ impl ImpulseJointHandle { /// An always-invalid joint handle. pub fn invalid() -> Self { - Self(crate::data::arena::Index::from_raw_parts( - crate::INVALID_U32, - crate::INVALID_U32, - )) + Self::PLACEHOLDER + } +} + +#[cfg(not(feature = "bevy"))] +impl From for crate::data::arena::Index { + fn from(value: ImpulseJointHandle) -> Self { + value.0 + } +} + +#[cfg(not(feature = "bevy"))] +impl From for ImpulseJointHandle { + fn from(value: Index) -> Self { + Self(value) } } @@ -40,7 +64,10 @@ pub(crate) type JointGraphEdge = crate::data::graph::Edge; /// A set of impulse_joints that can be handled by a physics `World`. pub struct ImpulseJointSet { rb_graph_ids: Coarena, + #[cfg(not(feature = "bevy"))] joint_ids: Arena, // Map joint handles to edge ids on the graph. + #[cfg(feature = "bevy")] + joint_ids: crate::data::EntityArena, // Map joint handles to edge ids on the graph. joint_graph: InteractionGraph, pub(crate) to_wake_up: Vec, // A set of rigid-body handles to wake-up during the next timestep. } @@ -48,12 +75,7 @@ pub struct ImpulseJointSet { impl ImpulseJointSet { /// Creates a new empty set of impulse_joints. pub fn new() -> Self { - Self { - rb_graph_ids: Coarena::new(), - joint_ids: Arena::new(), - joint_graph: InteractionGraph::new(), - to_wake_up: vec![], - } + Self::default() } /// The number of impulse_joints on this set. @@ -78,8 +100,8 @@ impl ImpulseJointSet { body2: RigidBodyHandle, ) -> impl Iterator { self.rb_graph_ids - .get(body1.0) - .zip(self.rb_graph_ids.get(body2.0)) + .get(body1.into()) + .zip(self.rb_graph_ids.get(body2.into())) .into_iter() .flat_map(move |(id1, id2)| self.joint_graph.interaction_pair(*id1, *id2).into_iter()) .map(|inter| (inter.2.handle, inter.2)) @@ -98,7 +120,7 @@ impl ImpulseJointSet { ), > { self.rb_graph_ids - .get(body.0) + .get(body.into()) .into_iter() .flat_map(move |id| self.joint_graph.interactions_with(*id)) .map(|inter| (inter.0, inter.1, inter.2.handle, inter.2)) @@ -110,11 +132,14 @@ impl ImpulseJointSet { body: RigidBodyHandle, mut f: impl FnMut(RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, &mut ImpulseJoint), ) { - self.rb_graph_ids.get(body.0).into_iter().for_each(|id| { - for inter in self.joint_graph.interactions_with_mut(*id) { - (f)(inter.0, inter.1, inter.3.handle, inter.3) - } - }) + self.rb_graph_ids + .get(body.into()) + .into_iter() + .for_each(|id| { + for inter in self.joint_graph.interactions_with_mut(*id) { + (f)(inter.0, inter.1, inter.3.handle, inter.3) + } + }) } /// Iterates through all the enabled impulse joints attached to the given rigid-body. @@ -135,18 +160,18 @@ impl ImpulseJointSet { /// Is the given joint handle valid? pub fn contains(&self, handle: ImpulseJointHandle) -> bool { - self.joint_ids.contains(handle.0) + self.joint_ids.contains(handle.into()) } /// Gets the joint with the given handle. pub fn get(&self, handle: ImpulseJointHandle) -> Option<&ImpulseJoint> { - let id = self.joint_ids.get(handle.0)?; + let id = self.joint_ids.get(handle.into())?; self.joint_graph.graph.edge_weight(*id) } /// Gets a mutable reference to the joint with the given handle. pub fn get_mut(&mut self, handle: ImpulseJointHandle) -> Option<&mut ImpulseJoint> { - let id = self.joint_ids.get(handle.0)?; + let id = self.joint_ids.get(handle.into())?; self.joint_graph.graph.edge_weight_mut(*id) } @@ -159,6 +184,7 @@ impl ImpulseJointSet { /// /// Using this is discouraged in favor of `self.get(handle)` which does not /// suffer form the ABA problem. + #[cfg(not(feature = "bevy"))] pub fn get_unknown_gen(&self, i: u32) -> Option<(&ImpulseJoint, ImpulseJointHandle)> { let (id, handle) = self.joint_ids.get_unknown_gen(i)?; Some(( @@ -176,6 +202,7 @@ impl ImpulseJointSet { /// /// Using this is discouraged in favor of `self.get_mut(handle)` which does not /// suffer form the ABA problem. + #[cfg(not(feature = "bevy"))] pub fn get_unknown_gen_mut( &mut self, i: u32, @@ -231,39 +258,45 @@ impl ImpulseJointSet { /// automatically woken up during the next timestep. pub fn insert( &mut self, + #[cfg(feature = "bevy")] handle: RigidBodyHandle, body1: RigidBodyHandle, body2: RigidBodyHandle, data: impl Into, wake_up: bool, ) -> ImpulseJointHandle { let data = data.into(); + + #[cfg(not(feature = "bevy"))] let handle = self.joint_ids.insert(0.into()); + #[cfg(feature = "bevy")] + self.joint_ids.insert(handle, 0.into()); + let joint = ImpulseJoint { body1, body2, data, impulses: na::zero(), - handle: ImpulseJointHandle(handle), + handle: ImpulseJointHandle::from(handle), }; let default_id = InteractionGraph::<(), ()>::invalid_graph_index(); let mut graph_index1 = *self .rb_graph_ids - .ensure_element_exist(joint.body1.0, default_id); + .ensure_element_exist(joint.body1.into(), default_id); let mut graph_index2 = *self .rb_graph_ids - .ensure_element_exist(joint.body2.0, default_id); + .ensure_element_exist(joint.body2.into(), default_id); // NOTE: the body won't have a graph index if it does not // have any joint attached. if !InteractionGraph::::is_graph_index_valid(graph_index1) { graph_index1 = self.joint_graph.graph.add_node(joint.body1); - self.rb_graph_ids.insert(joint.body1.0, graph_index1); + self.rb_graph_ids.insert(joint.body1.into(), graph_index1); } if !InteractionGraph::::is_graph_index_valid(graph_index2) { graph_index2 = self.joint_graph.graph.add_node(joint.body2); - self.rb_graph_ids.insert(joint.body2.0, graph_index2); + self.rb_graph_ids.insert(joint.body2.into(), graph_index2); } self.joint_ids[handle] = self.joint_graph.add_edge(graph_index1, graph_index2, joint); @@ -273,7 +306,7 @@ impl ImpulseJointSet { self.to_wake_up.push(body2); } - ImpulseJointHandle(handle) + ImpulseJointHandle::from(handle) } /// Retrieve all the enabled impulse joints happening between two active bodies. @@ -315,7 +348,7 @@ impl ImpulseJointSet { /// If `wake_up` is set to `true`, then the bodies attached to this joint will be /// automatically woken up. pub fn remove(&mut self, handle: ImpulseJointHandle, wake_up: bool) -> Option { - let id = self.joint_ids.remove(handle.0)?; + let id = self.joint_ids.remove(handle.into())?; let endpoints = self.joint_graph.graph.edge_endpoints(id)?; if wake_up { @@ -330,7 +363,7 @@ impl ImpulseJointSet { let removed_joint = self.joint_graph.graph.remove_edge(id); if let Some(edge) = self.joint_graph.graph.edge_weight(id) { - self.joint_ids[edge.handle.0] = id; + self.joint_ids[edge.handle.into()] = id; } removed_joint @@ -347,10 +380,10 @@ impl ImpulseJointSet { ) -> Vec { let mut deleted = vec![]; - if let Some(deleted_id) = self - .rb_graph_ids - .remove(handle.0, InteractionGraph::<(), ()>::invalid_graph_index()) - { + if let Some(deleted_id) = self.rb_graph_ids.remove( + handle.into(), + InteractionGraph::<(), ()>::invalid_graph_index(), + ) { if InteractionGraph::<(), ()>::is_graph_index_valid(deleted_id) { // We have to delete each joint one by one in order to: // - Wake-up the attached bodies. @@ -363,12 +396,12 @@ impl ImpulseJointSet { .collect(); for (h1, h2, to_delete_handle) in to_delete { deleted.push(to_delete_handle); - let to_delete_edge_id = self.joint_ids.remove(to_delete_handle.0).unwrap(); + let to_delete_edge_id = self.joint_ids.remove(to_delete_handle.into()).unwrap(); self.joint_graph.graph.remove_edge(to_delete_edge_id); // Update the id of the edge which took the place of the deleted one. if let Some(j) = self.joint_graph.graph.edge_weight_mut(to_delete_edge_id) { - self.joint_ids[j.handle.0] = to_delete_edge_id; + self.joint_ids[j.handle.into()] = to_delete_edge_id; } // Wake up the attached bodies. @@ -379,7 +412,7 @@ impl ImpulseJointSet { if let Some(other) = self.joint_graph.remove_node(deleted_id) { // One rigid-body joint graph index may have been invalidated // so we need to update it. - self.rb_graph_ids.insert(other.0, deleted_id); + self.rb_graph_ids.insert(other.into(), deleted_id); } } } diff --git a/src/dynamics/joint/motor_model.rs b/src/dynamics/joint/motor_model.rs index 74b8dd3..65e1181 100644 --- a/src/dynamics/joint/motor_model.rs +++ b/src/dynamics/joint/motor_model.rs @@ -1,4 +1,4 @@ -use crate::math::Real; +use crate::math::*; /// The spring-like model used for constraints resolution. #[derive(Default, Copy, Clone, Debug, PartialEq, Eq)] diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 617d447..753fa8f 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -1,11 +1,7 @@ use super::multibody_link::{MultibodyLink, MultibodyLinkVec}; use super::multibody_workspace::MultibodyWorkspace; -use crate::dynamics::{RigidBodyHandle, RigidBodySet, RigidBodyType, RigidBodyVelocity}; -#[cfg(feature = "dim3")] -use crate::math::Matrix; -use crate::math::{ - AngDim, AngVector, Dim, Isometry, Jacobian, Point, Real, Vector, ANG_DIM, DIM, SPATIAL_DIM, -}; +use crate::dynamics::{RigidBodyHandle, RigidBodySet, RigidBodyType, Velocity}; +use crate::math::*; use crate::prelude::MultibodyJoint; use crate::utils::{IndexMut2, SimdAngularInertia, SimdCross, SimdCrossMatrix}; use na::{self, DMatrix, DVector, DVectorView, DVectorViewMut, Dyn, OMatrix, SMatrix, SVector, LU}; @@ -13,12 +9,12 @@ use na::{self, DMatrix, DVector, DVectorView, DVectorViewMut, Dyn, OMatrix, SMat #[repr(C)] #[derive(Copy, Clone, Debug, Default)] struct Force { - linear: Vector, - angular: AngVector, + linear: Vector, + angular: AngVector, } impl Force { - fn new(linear: Vector, angular: AngVector) -> Self { + fn new(linear: Vector, angular: AngVector) -> Self { Self { linear, angular } } @@ -28,10 +24,7 @@ impl Force { } #[cfg(feature = "dim2")] -fn concat_rb_mass_matrix( - mass: Vector, - inertia: Real, -) -> SMatrix { +fn concat_rb_mass_matrix(mass: Vector, inertia: Real) -> SMatrix { let mut result = SMatrix::::zeros(); result[(0, 0)] = mass.x; result[(1, 1)] = mass.y; @@ -40,17 +33,14 @@ fn concat_rb_mass_matrix( } #[cfg(feature = "dim3")] -fn concat_rb_mass_matrix( - mass: Vector, - inertia: Matrix, -) -> SMatrix { +fn concat_rb_mass_matrix(mass: Vector, inertia: Matrix) -> SMatrix { let mut result = SMatrix::::zeros(); result[(0, 0)] = mass.x; result[(1, 1)] = mass.y; result[(2, 2)] = mass.z; result .fixed_view_mut::(DIM, DIM) - .copy_from(&inertia); + .copy_from(&inertia.into()); result } @@ -375,7 +365,7 @@ impl Multibody { let link = &self.links[i]; let rb = &bodies[link.rigid_body]; - let mut acc = RigidBodyVelocity::zero(); + let mut acc = Velocity::zero(); if i != 0 { let parent_id = link.parent_internal_id; @@ -388,7 +378,7 @@ impl Multibody { acc.linvel += 2.0 * parent_rb.vels.angvel.gcross(link.joint_velocity.linvel); #[cfg(feature = "dim3")] { - acc.angvel += parent_rb.vels.angvel.cross(&link.joint_velocity.angvel); + acc.angvel += parent_rb.vels.angvel.cross(link.joint_velocity.angvel); } acc.linvel += parent_rb @@ -411,7 +401,7 @@ impl Multibody { #[cfg(feature = "dim3")] { - gyroscopic = rb.vels.angvel.cross(&(rb_inertia * rb.vels.angvel)); + gyroscopic = rb.vels.angvel.cross(rb_inertia * rb.vels.angvel); } #[cfg(feature = "dim2")] { @@ -500,7 +490,7 @@ impl Multibody { let parent_j_w = parent_j.fixed_rows::(DIM); let shift_tr = (link.shift02).gcross_matrix_tr(); - link_j_v.gemm(1.0, &shift_tr, &parent_j_w, 1.0); + link_j_v.gemm(1.0, &shift_tr.into(), &parent_j_w, 1.0); } } else { self.body_jacobians[i].fill(0.0); @@ -522,7 +512,7 @@ impl Multibody { let (mut link_j_v, link_j_w) = link_j.rows_range_pair_mut(0..DIM, DIM..DIM + ANG_DIM); let shift_tr = link.shift23.gcross_matrix_tr(); - link_j_v.gemm(1.0, &shift_tr, &link_j_w, 1.0); + link_j_v.gemm(1.0, &shift_tr.into(), &link_j_w, 1.0); } } } @@ -608,27 +598,27 @@ impl Multibody { // [c1 - c0].gcross() * (JDot + JDot/u * qdot)" let shift_cross_tr = link.shift02.gcross_matrix_tr(); - coriolis_v.gemm(1.0, &shift_cross_tr, parent_coriolis_w, 1.0); + coriolis_v.gemm(1.0, &shift_cross_tr.into(), parent_coriolis_w, 1.0); // JDot (but the 2.0 originates from the sum of two identical terms in JDot and JDot/u * gdot) let dvel_cross = (rb.vels.angvel.gcross(link.shift02) + 2.0 * link.joint_velocity.linvel) .gcross_matrix_tr(); - coriolis_v.gemm(1.0, &dvel_cross, &parent_j_w, 1.0); + coriolis_v.gemm(1.0, &dvel_cross.into(), &parent_j_w, 1.0); // JDot/u * qdot coriolis_v.gemm( 1.0, - &link.joint_velocity.linvel.gcross_matrix_tr(), + &link.joint_velocity.linvel.gcross_matrix_tr().into(), &parent_j_w, 1.0, ); - coriolis_v.gemm(1.0, &(parent_w * shift_cross_tr), &parent_j_w, 1.0); + coriolis_v.gemm(1.0, &(parent_w * shift_cross_tr).into(), &parent_j_w, 1.0); #[cfg(feature = "dim3")] { let vel_wrt_joint_w = link.joint_velocity.angvel.gcross_matrix(); - coriolis_w.gemm(-1.0, &vel_wrt_joint_w, &parent_j_w, 1.0); + coriolis_w.gemm(-1.0, &vel_wrt_joint_w.into(), &parent_j_w, 1.0); } // JDot (but the 2.0 originates from the sum of two identical terms in JDot and JDot/u * gdot) @@ -644,13 +634,13 @@ impl Multibody { ); let rb_joint_j_v = rb_joint_j.fixed_rows::(0); - coriolis_v_part.gemm(2.0, &parent_w, &rb_joint_j_v, 1.0); + coriolis_v_part.gemm(2.0, &parent_w.into(), &rb_joint_j_v, 1.0); #[cfg(feature = "dim3")] { let rb_joint_j_w = rb_joint_j.fixed_rows::(DIM); let mut coriolis_w_part = coriolis_w.columns_mut(link.assembly_id, ndofs); - coriolis_w_part.gemm(1.0, &parent_w, &rb_joint_j_w, 1.0); + coriolis_w_part.gemm(1.0, &parent_w.into(), &rb_joint_j_w, 1.0); } } } else { @@ -664,16 +654,16 @@ impl Multibody { { // [c3 - c2].gcross() * (JDot + JDot/u * qdot) let shift_cross_tr = link.shift23.gcross_matrix_tr(); - coriolis_v.gemm(1.0, &shift_cross_tr, coriolis_w, 1.0); + coriolis_v.gemm(1.0, &shift_cross_tr.into(), coriolis_w, 1.0); // JDot let dvel_cross = rb.vels.angvel.gcross(link.shift23).gcross_matrix_tr(); - coriolis_v.gemm(1.0, &dvel_cross, &rb_j_w, 1.0); + coriolis_v.gemm(1.0, &dvel_cross.into(), &rb_j_w, 1.0); // JDot/u * qdot coriolis_v.gemm( 1.0, - &(rb.vels.angvel.gcross_matrix() * shift_cross_tr), + &(rb.vels.angvel.gcross_matrix() * shift_cross_tr).into(), &rb_j_w, 1.0, ); @@ -690,7 +680,7 @@ impl Multibody { i_coriolis_dt_v.copy_from(coriolis_v); i_coriolis_dt_v .column_iter_mut() - .for_each(|mut c| c.component_mul_assign(&(rb_mass * dt))); + .for_each(|mut c| c.component_mul_assign(&(rb_mass * dt).into())); } #[cfg(feature = "dim2")] @@ -702,7 +692,7 @@ impl Multibody { #[cfg(feature = "dim3")] { let mut i_coriolis_dt_w = self.i_coriolis_dt.fixed_rows_mut::(DIM); - i_coriolis_dt_w.gemm(dt, &rb_inertia, coriolis_w, 0.0); + i_coriolis_dt_w.gemm(dt, &rb_inertia.into(), coriolis_w, 0.0); } self.acc_augmented_mass @@ -856,10 +846,15 @@ impl Multibody { { let parent_rb = &bodies[parent_link.rigid_body]; let link_rb = &bodies[link.rigid_body]; - let c0 = parent_link.local_to_world * parent_rb.mprops.local_mprops.local_com; - let c2 = link.local_to_world - * Point::from(link.joint.data.local_frame2.translation.vector); - let c3 = link.local_to_world * link_rb.mprops.local_mprops.local_com; + let c0 = parent_link + .local_to_world + .transform_point(&parent_rb.mprops.local_mprops.local_com); + let c2 = link.local_to_world.transform_point(&Point::from( + link.joint.data.local_frame2.translation.into_vector(), + )); + let c3 = link + .local_to_world + .transform_point(&link_rb.mprops.local_mprops.local_com); link.shift02 = c2 - c0; link.shift23 = c3 - c2; @@ -896,7 +891,7 @@ impl Multibody { pub(crate) fn fill_jacobians( &self, link_id: usize, - unit_force: Vector, + unit_force: Vector, unit_torque: SVector, j_id: &mut usize, jacobians: &mut DVector, @@ -908,10 +903,7 @@ impl Multibody { let wj_id = *j_id + self.ndofs; let force = Force { linear: unit_force, - #[cfg(feature = "dim2")] - angular: unit_torque[0], - #[cfg(feature = "dim3")] - angular: unit_torque, + angular: unit_torque.into(), }; let link = &self.links[link_id]; diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs index 11ea890..491bdf7 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -1,12 +1,9 @@ use crate::dynamics::solver::JointGenericOneBodyConstraint; use crate::dynamics::{ joint, FixedJointBuilder, GenericJoint, IntegrationParameters, Multibody, MultibodyLink, - RigidBodyVelocity, -}; -use crate::math::{ - Isometry, JacobianViewMut, Real, Rotation, SpacialVector, Translation, Vector, ANG_DIM, DIM, - SPATIAL_DIM, + Velocity, }; +use crate::math::*; use na::{DVector, DVectorViewMut}; #[cfg(feature = "dim3")] use na::{UnitQuaternion, Vector3}; @@ -17,8 +14,8 @@ use na::{UnitQuaternion, Vector3}; pub struct MultibodyJoint { /// The joint’s description. pub data: GenericJoint, - pub(crate) coords: SpacialVector, - pub(crate) joint_rot: Rotation, + pub(crate) coords: SpatialVector, + pub(crate) joint_rot: Rotation, } impl MultibodyJoint { @@ -31,24 +28,24 @@ impl MultibodyJoint { } } - pub(crate) fn free(pos: Isometry) -> Self { + pub(crate) fn free(pos: Isometry) -> Self { let mut result = Self::new(GenericJoint::default()); result.set_free_pos(pos); result } - pub(crate) fn fixed(pos: Isometry) -> Self { + pub(crate) fn fixed(pos: Isometry) -> Self { Self::new(FixedJointBuilder::new().local_frame1(pos).build().into()) } - pub(crate) fn set_free_pos(&mut self, pos: Isometry) { + pub(crate) fn set_free_pos(&mut self, pos: Isometry) { self.coords .fixed_rows_mut::(0) - .copy_from(&pos.translation.vector); + .copy_from(&pos.translation.into_vector().into()); self.joint_rot = pos.rotation; } - // pub(crate) fn local_joint_rot(&self) -> &Rotation { + // pub(crate) fn local_joint_rot(&self) -> &Rotation { // &self.joint_rot // } @@ -63,7 +60,7 @@ impl MultibodyJoint { } /// The position of the multibody link containing this multibody_joint relative to its parent. - pub fn body_to_parent(&self) -> Isometry { + pub fn body_to_parent(&self) -> Isometry { let locked_bits = self.data.locked_axes.bits(); let mut transform = self.joint_rot * self.data.local_frame2.inverse(); @@ -97,12 +94,12 @@ impl MultibodyJoint { self.coords[DIM + dof_id] += vels[curr_free_dof] * dt; #[cfg(feature = "dim2")] { - self.joint_rot = Rotation::new(self.coords[DIM + dof_id]); + self.joint_rot = Rotation::from_scaled_axis(self.coords[DIM + dof_id].into()); } #[cfg(feature = "dim3")] { self.joint_rot = Rotation::from_axis_angle( - &Vector::ith_axis(dof_id), + Vector::ith_axis(dof_id), self.coords[DIM + dof_id], ); } @@ -112,8 +109,8 @@ impl MultibodyJoint { } #[cfg(feature = "dim3")] 3 => { - let angvel = Vector3::from_row_slice(&vels[curr_free_dof..curr_free_dof + 3]); - let disp = UnitQuaternion::new_eps(angvel * dt, 0.0); + let angvel = Vector::from_row_slice(&vels[curr_free_dof..curr_free_dof + 3]); + let disp = Rotation::new_eps(angvel * dt, 0.0); self.joint_rot = disp * self.joint_rot; self.coords[3] += angvel[0] * dt; self.coords[4] += angvel[1] * dt; @@ -129,15 +126,15 @@ impl MultibodyJoint { } /// Sets in `out` the non-zero entries of the multibody_joint jacobian transformed by `transform`. - pub fn jacobian(&self, transform: &Rotation, out: &mut JacobianViewMut) { + pub fn jacobian(&self, transform: &Rotation, out: &mut JacobianViewMut) { let locked_bits = self.data.locked_axes.bits(); let mut curr_free_dof = 0; for i in 0..DIM { if (locked_bits & (1 << i)) == 0 { - let transformed_axis = transform * Vector::ith(i, 1.0); + let transformed_axis = *transform * Vector::ith(i, 1.0); out.fixed_view_mut::(0, curr_free_dof) - .copy_from(&transformed_axis); + .copy_from(&transformed_axis.into()); curr_free_dof += 1; } } @@ -157,7 +154,7 @@ impl MultibodyJoint { let dof_id = (!locked_ang_bits).trailing_zeros() as usize; let rotmat = transform.to_rotation_matrix().into_inner(); out.fixed_view_mut::(DIM, curr_free_dof) - .copy_from(&rotmat.column(dof_id)); + .copy_from(&na::Vector3::from(rotmat.column(dof_id))); } } 2 => { @@ -167,7 +164,7 @@ impl MultibodyJoint { 3 => { let rotmat = transform.to_rotation_matrix(); out.fixed_view_mut::<3, 3>(3, curr_free_dof) - .copy_from(rotmat.matrix()); + .copy_from(&rotmat.into_inner().into()); } _ => unreachable!(), } @@ -175,9 +172,9 @@ impl MultibodyJoint { /// Multiply the multibody_joint jacobian by generalized velocities to obtain the /// relative velocity of the multibody link containing this multibody_joint. - pub fn jacobian_mul_coordinates(&self, acc: &[Real]) -> RigidBodyVelocity { + pub fn jacobian_mul_coordinates(&self, acc: &[Real]) -> Velocity { let locked_bits = self.data.locked_axes.bits(); - let mut result = RigidBodyVelocity::zero(); + let mut result = Velocity::zero(); let mut curr_free_dof = 0; for i in 0..DIM { @@ -207,7 +204,7 @@ impl MultibodyJoint { } #[cfg(feature = "dim3")] 3 => { - let angvel = Vector3::from_row_slice(&acc[curr_free_dof..curr_free_dof + 3]); + let angvel = Vector::from_row_slice(&acc[curr_free_dof..curr_free_dof + 3]); result.angvel += angvel; } _ => unreachable!(), diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index fa0ffdb..e8d957a 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -1,22 +1,57 @@ use crate::data::{Arena, Coarena, Index}; use crate::dynamics::joint::MultibodyLink; use crate::dynamics::{GenericJoint, Multibody, MultibodyJoint, RigidBodyHandle}; -use crate::geometry::{InteractionGraph, RigidBodyGraphIndex}; +use crate::geometry::{ColliderHandle, InteractionGraph, RigidBodyGraphIndex}; use crate::parry::partitioning::IndexedData; +#[cfg(feature = "bevy")] +use bevy::prelude::{Component, Reflect, ReflectComponent}; + /// The unique handle of an multibody_joint added to a `MultibodyJointSet`. #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr(feature = "bevy", derive(Component))] +#[cfg(not(feature = "bevy"))] #[repr(transparent)] pub struct MultibodyJointHandle(pub Index); +#[cfg(feature = "bevy")] +pub type MultibodyJointHandle = bevy::prelude::Entity; + +#[cfg(not(feature = "bevy"))] +impl From for crate::data::arena::Index { + fn from(value: MultibodyJointHandle) -> Self { + value.0 + } +} + +#[cfg(not(feature = "bevy"))] +impl From for MultibodyJointHandle { + fn from(value: Index) -> Self { + Self(value) + } +} + +#[cfg(not(feature = "bevy"))] +impl From for MultibodyJointHandle { + fn from(value: RigidBodyHandle) -> Self { + Self(value.0) + } +} + /// The temporary index of a multibody added to a `MultibodyJointSet`. #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[repr(transparent)] pub struct MultibodyIndex(pub Index); +#[cfg(not(feature = "bevy"))] impl MultibodyJointHandle { + pub const PLACEHOLDER: Self = Self(Index::from_raw_parts( + crate::INVALID_U32, + crate::INVALID_U32, + )); + /// Converts this handle into its (index, generation) components. pub fn into_raw_parts(self) -> (u32, u32) { self.0.into_raw_parts() @@ -36,12 +71,14 @@ impl MultibodyJointHandle { } } +#[cfg(not(feature = "bevy"))] impl Default for MultibodyJointHandle { fn default() -> Self { Self::invalid() } } +#[cfg(not(feature = "bevy"))] impl IndexedData for MultibodyJointHandle { fn default() -> Self { Self(IndexedData::default()) @@ -51,6 +88,13 @@ impl IndexedData for MultibodyJointHandle { } } +#[cfg(not(feature = "bevy"))] +impl From for RigidBodyHandle { + fn from(value: MultibodyJointHandle) -> Self { + RigidBodyHandle(value.0) + } +} + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq, Eq)] /// Indexes usable to get a multibody link from a `MultibodyJointSet`. @@ -123,7 +167,12 @@ impl MultibodyJointSet { .filter(|(_, link)| link.id > 0) // The first link of a rigid-body hasn’t been added by the user. .map(|(h, link)| { let mb = &self.multibodies[link.multibody.0]; - (MultibodyJointHandle(h), link, mb, mb.link(link.id).unwrap()) + ( + MultibodyJointHandle::from(h), + link, + mb, + mb.link(link.id).unwrap(), + ) }) } @@ -136,7 +185,7 @@ impl MultibodyJointSet { wake_up: bool, ) -> Option { let data = data.into(); - let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| { + let link1 = self.rb2mb.get(body1.into()).copied().unwrap_or_else(|| { let mb_handle = self.multibodies.insert(Multibody::with_root(body1)); MultibodyLinkId { graph_id: self.connectivity_graph.graph.add_node(body1), @@ -145,7 +194,7 @@ impl MultibodyJointSet { } }); - let link2 = self.rb2mb.get(body2.0).copied().unwrap_or_else(|| { + let link2 = self.rb2mb.get(body2.into()).copied().unwrap_or_else(|| { let mb_handle = self.multibodies.insert(Multibody::with_root(body2)); MultibodyLinkId { graph_id: self.connectivity_graph.graph.add_node(body2), @@ -162,14 +211,14 @@ impl MultibodyJointSet { self.connectivity_graph .graph .add_edge(link1.graph_id, link2.graph_id, ()); - self.rb2mb.insert(body1.0, link1); - self.rb2mb.insert(body2.0, link2); + self.rb2mb.insert(body1.into(), link1); + self.rb2mb.insert(body2.into(), link2); let mb2 = self.multibodies.remove(link2.multibody.0).unwrap(); let multibody1 = &mut self.multibodies[link1.multibody.0]; for mb_link2 in mb2.links() { - let link = self.rb2mb.get_mut(mb_link2.rigid_body.0).unwrap(); + let link = self.rb2mb.get_mut(mb_link2.rigid_body.into()).unwrap(); link.multibody = link1.multibody; link.id += multibody1.num_links(); } @@ -184,24 +233,24 @@ impl MultibodyJointSet { // Because each rigid-body can only have one parent link, // we can use the second rigid-body’s handle as the multibody_joint’s // handle. - Some(MultibodyJointHandle(body2.0)) + Some(MultibodyJointHandle::from(body2)) } /// Removes an multibody_joint from this set. pub fn remove(&mut self, handle: MultibodyJointHandle, wake_up: bool) { - if let Some(removed) = self.rb2mb.get(handle.0).copied() { + if let Some(removed) = self.rb2mb.get(handle.into()).copied() { let multibody = self.multibodies.remove(removed.multibody.0).unwrap(); // Remove the edge from the connectivity graph. if let Some(parent_link) = multibody.link(removed.id).unwrap().parent_id() { let parent_rb = multibody.link(parent_link).unwrap().rigid_body; self.connectivity_graph.remove_edge( - self.rb2mb.get(parent_rb.0).unwrap().graph_id, + self.rb2mb.get(parent_rb.into()).unwrap().graph_id, removed.graph_id, ); if wake_up { - self.to_wake_up.push(RigidBodyHandle(handle.0)); + self.to_wake_up.push(RigidBodyHandle::from(handle)); self.to_wake_up.push(parent_rb); } @@ -215,12 +264,12 @@ impl MultibodyJointSet { if multibody.num_links() == 1 { // We don’t have any multibody_joint attached to this body, remove it. if let Some(other) = self.connectivity_graph.remove_node(removed.graph_id) { - self.rb2mb.get_mut(other.0).unwrap().graph_id = removed.graph_id; + self.rb2mb.get_mut(other.into()).unwrap().graph_id = removed.graph_id; } } else { let mb_id = self.multibodies.insert(multibody); for link in self.multibodies[mb_id].links() { - let ids = self.rb2mb.get_mut(link.rigid_body.0).unwrap(); + let ids = self.rb2mb.get_mut(link.rigid_body.into()).unwrap(); ids.multibody = MultibodyIndex(mb_id); ids.id = link.internal_id; } @@ -232,7 +281,7 @@ impl MultibodyJointSet { /// Removes all the multibody_joints from the multibody the given rigid-body is part of. pub fn remove_multibody_articulations(&mut self, handle: RigidBodyHandle, wake_up: bool) { - if let Some(removed) = self.rb2mb.get(handle.0).copied() { + if let Some(removed) = self.rb2mb.get(handle.into()).copied() { // Remove the multibody. let multibody = self.multibodies.remove(removed.multibody.0).unwrap(); for link in multibody.links() { @@ -243,10 +292,13 @@ impl MultibodyJointSet { } // Remove the rigid-body <-> multibody mapping for this link. - let removed = self.rb2mb.remove(rb_handle.0, Default::default()).unwrap(); + let removed = self + .rb2mb + .remove(rb_handle.into(), Default::default()) + .unwrap(); // Remove the node (and all it’s edges) from the connectivity graph. if let Some(other) = self.connectivity_graph.remove_node(removed.graph_id) { - self.rb2mb.get_mut(other.0).unwrap().graph_id = removed.graph_id; + self.rb2mb.get_mut(other.into()).unwrap().graph_id = removed.graph_id; } } } @@ -255,14 +307,14 @@ impl MultibodyJointSet { /// Removes all the multibody joints attached to a rigid-body. pub fn remove_joints_attached_to_rigid_body(&mut self, rb_to_remove: RigidBodyHandle) { // TODO: optimize this. - if let Some(link_to_remove) = self.rb2mb.get(rb_to_remove.0).copied() { + if let Some(link_to_remove) = self.rb2mb.get(rb_to_remove.into()).copied() { let mut articulations_to_remove = vec![]; for (rb1, rb2, _) in self .connectivity_graph .interactions_with(link_to_remove.graph_id) { // There is a multibody_joint handle is equal to the second rigid-body’s handle. - articulations_to_remove.push(MultibodyJointHandle(rb2.0)); + articulations_to_remove.push(MultibodyJointHandle::from(rb2)); self.to_wake_up.push(rb1); self.to_wake_up.push(rb2); @@ -278,7 +330,7 @@ impl MultibodyJointSet { /// /// Returns `None` if `rb` isn’t part of any rigid-body. pub fn rigid_body_link(&self, rb: RigidBodyHandle) -> Option<&MultibodyLinkId> { - self.rb2mb.get(rb.0) + self.rb2mb.get(rb.into()) } /// Gets a reference to a multibody, based on its temporary index. @@ -296,14 +348,14 @@ impl MultibodyJointSet { /// Gets a reference to the multibody identified by its `handle`. pub fn get(&self, handle: MultibodyJointHandle) -> Option<(&Multibody, usize)> { - let link = self.rb2mb.get(handle.0)?; + let link = self.rb2mb.get(handle.into())?; let multibody = self.multibodies.get(link.multibody.0)?; Some((multibody, link.id)) } /// Gets a mutable reference to the multibody identified by its `handle`. pub fn get_mut(&mut self, handle: MultibodyJointHandle) -> Option<(&mut Multibody, usize)> { - let link = self.rb2mb.get(handle.0)?; + let link = self.rb2mb.get(handle.into())?; let multibody = self.multibodies.get_mut(link.multibody.0)?; Some((multibody, link.id)) } @@ -316,7 +368,7 @@ impl MultibodyJointSet { handle: MultibodyJointHandle, ) -> Option<(&mut Multibody, usize)> { // TODO: modification tracking? - let link = self.rb2mb.get(handle.0)?; + let link = self.rb2mb.get(handle.into())?; let multibody = self.multibodies.get_mut(link.multibody.0)?; Some((multibody, link.id)) } @@ -330,6 +382,7 @@ impl MultibodyJointSet { /// /// Using this is discouraged in favor of `self.get(handle)` which does not /// suffer form the ABA problem. + #[cfg(not(feature = "bevy"))] pub fn get_unknown_gen(&self, i: u32) -> Option<(&Multibody, usize, MultibodyJointHandle)> { let link = self.rb2mb.get_unknown_gen(i)?; let gen = self.rb2mb.get_gen(i)?; @@ -347,8 +400,8 @@ impl MultibodyJointSet { rb1: RigidBodyHandle, rb2: RigidBodyHandle, ) -> Option<(MultibodyJointHandle, &Multibody, &MultibodyLink)> { - let id1 = self.rb2mb.get(rb1.0)?; - let id2 = self.rb2mb.get(rb2.0)?; + let id1 = self.rb2mb.get(rb1.into())?; + let id2 = self.rb2mb.get(rb2.into())?; // Both bodies must be part of the same multibody. if id1.multibody != id2.multibody { @@ -363,13 +416,13 @@ impl MultibodyJointSet { let parent1 = link1.parent_id(); if parent1 == Some(id2.id) { - Some((MultibodyJointHandle(rb1.0), mb, &link1)) + Some((MultibodyJointHandle::from(rb1), mb, &link1)) } else { let link2 = mb.link(id2.id)?; let parent2 = link2.parent_id(); if parent2 == Some(id1.id) { - Some((MultibodyJointHandle(rb2.0), mb, &link2)) + Some((MultibodyJointHandle::from(rb2), mb, &link2)) } else { None } @@ -382,12 +435,12 @@ impl MultibodyJointSet { rb: RigidBodyHandle, ) -> impl Iterator + '_ { self.rb2mb - .get(rb.0) + .get(rb.into()) .into_iter() .flat_map(move |link| self.connectivity_graph.interactions_with(link.graph_id)) .map(|inter| { // NOTE: the joint handle is always equal to the handle of the second rigid-body. - (inter.0, inter.1, MultibodyJointHandle(inter.1 .0)) + (inter.0, inter.1, MultibodyJointHandle::from(inter.1)) }) } @@ -398,7 +451,7 @@ impl MultibodyJointSet { body: RigidBodyHandle, ) -> impl Iterator + '_ { self.rb2mb - .get(body.0) + .get(body.into()) .into_iter() .flat_map(move |id| self.connectivity_graph.interactions_with(id.graph_id)) .map(move |inter| crate::utils::select_other((inter.0, inter.1), body)) diff --git a/src/dynamics/joint/multibody_joint/multibody_link.rs b/src/dynamics/joint/multibody_joint/multibody_link.rs index 7336a6c..932120b 100644 --- a/src/dynamics/joint/multibody_joint/multibody_link.rs +++ b/src/dynamics/joint/multibody_joint/multibody_link.rs @@ -1,8 +1,8 @@ use std::ops::{Deref, DerefMut}; use crate::dynamics::{MultibodyJoint, RigidBodyHandle}; -use crate::math::{Isometry, Real, Vector}; -use crate::prelude::RigidBodyVelocity; +use crate::math::*; +use crate::prelude::Velocity; /// One link of a multibody. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -21,13 +21,13 @@ pub struct MultibodyLink { /// The multibody joint of this link. pub joint: MultibodyJoint, // TODO: should this be removed in favor of the rigid-body position? - pub(crate) local_to_world: Isometry, - pub(crate) local_to_parent: Isometry, - pub(crate) shift02: Vector, - pub(crate) shift23: Vector, + pub(crate) local_to_world: Isometry, + pub(crate) local_to_parent: Isometry, + pub(crate) shift02: Vector, + pub(crate) shift23: Vector, /// The velocity added by the joint, in world-space. - pub(crate) joint_velocity: RigidBodyVelocity, + pub(crate) joint_velocity: Velocity, } impl MultibodyLink { @@ -38,10 +38,10 @@ impl MultibodyLink { assembly_id: usize, parent_internal_id: usize, joint: MultibodyJoint, - local_to_world: Isometry, - local_to_parent: Isometry, + local_to_world: Isometry, + local_to_parent: Isometry, ) -> Self { - let joint_velocity = RigidBodyVelocity::zero(); + let joint_velocity = Velocity::zero(); MultibodyLink { internal_id, @@ -50,8 +50,8 @@ impl MultibodyLink { joint, local_to_world, local_to_parent, - shift02: na::zero(), - shift23: na::zero(), + shift02: Default::default(), + shift23: Default::default(), joint_velocity, rigid_body, } @@ -91,13 +91,13 @@ impl MultibodyLink { /// The world-space transform of the rigid-body attached to this link. #[inline] - pub fn local_to_world(&self) -> &Isometry { + pub fn local_to_world(&self) -> &Isometry { &self.local_to_world } /// The position of the rigid-body attached to this link relative to its parent. #[inline] - pub fn local_to_parent(&self) -> &Isometry { + pub fn local_to_parent(&self) -> &Isometry { &self.local_to_parent } } diff --git a/src/dynamics/joint/multibody_joint/multibody_workspace.rs b/src/dynamics/joint/multibody_joint/multibody_workspace.rs index 767d751..5732329 100644 --- a/src/dynamics/joint/multibody_joint/multibody_workspace.rs +++ b/src/dynamics/joint/multibody_joint/multibody_workspace.rs @@ -1,12 +1,12 @@ -use crate::dynamics::RigidBodyVelocity; -use crate::math::Real; +use crate::dynamics::Velocity; +use crate::math::*; use na::DVector; /// A temporary workspace for various updates of the multibody. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone)] pub(crate) struct MultibodyWorkspace { - pub accs: Vec, + pub accs: Vec, pub ndofs_vec: DVector, } @@ -21,7 +21,7 @@ impl MultibodyWorkspace { /// Resize the workspace so it is enough for `nlinks` links. pub fn resize(&mut self, nlinks: usize, ndofs: usize) { - self.accs.resize(nlinks, RigidBodyVelocity::zero()); + self.accs.resize(nlinks, Velocity::zero()); self.ndofs_vec = DVector::zeros(ndofs) } } diff --git a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs index d6efd12..e46024f 100644 --- a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs @@ -3,7 +3,7 @@ use crate::dynamics::joint::MultibodyLink; use crate::dynamics::solver::{JointGenericOneBodyConstraint, WritebackId}; use crate::dynamics::{IntegrationParameters, JointMotor, Multibody}; -use crate::math::Real; +use crate::math::*; use na::DVector; /// Initializes and generate the velocity constraints applicable to the multibody links attached diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs index d0f3a7d..732d261 100644 --- a/src/dynamics/joint/prismatic_joint.rs +++ b/src/dynamics/joint/prismatic_joint.rs @@ -1,6 +1,6 @@ use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask}; use crate::dynamics::{JointAxis, MotorModel}; -use crate::math::{Point, Real, UnitVector}; +use crate::math::*; use super::{JointLimits, JointMotor}; @@ -17,7 +17,7 @@ impl PrismaticJoint { /// Creates a new prismatic joint allowing only relative translations along the specified axis. /// /// This axis is expressed in the local-space of both rigid-bodies. - pub fn new(axis: UnitVector) -> Self { + pub fn new(axis: UnitVector) -> Self { let data = GenericJointBuilder::new(JointAxesMask::LOCKED_PRISMATIC_AXES) .local_axis1(axis) .local_axis2(axis) @@ -43,48 +43,48 @@ impl PrismaticJoint { /// The joint’s anchor, expressed in the local-space of the first rigid-body. #[must_use] - pub fn local_anchor1(&self) -> Point { + pub fn local_anchor1(&self) -> Point { self.data.local_anchor1() } /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body. - pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { + pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { self.data.set_local_anchor1(anchor1); self } /// The joint’s anchor, expressed in the local-space of the second rigid-body. #[must_use] - pub fn local_anchor2(&self) -> Point { + pub fn local_anchor2(&self) -> Point { self.data.local_anchor2() } /// Sets the joint’s anchor, expressed in the local-space of the second rigid-body. - pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { + pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { self.data.set_local_anchor2(anchor2); self } /// The principal axis of the joint, expressed in the local-space of the first rigid-body. #[must_use] - pub fn local_axis1(&self) -> UnitVector { + pub fn local_axis1(&self) -> UnitVector { self.data.local_axis1() } /// Sets the principal axis of the joint, expressed in the local-space of the first rigid-body. - pub fn set_local_axis1(&mut self, axis1: UnitVector) -> &mut Self { + pub fn set_local_axis1(&mut self, axis1: UnitVector) -> &mut Self { self.data.set_local_axis1(axis1); self } /// The principal axis of the joint, expressed in the local-space of the second rigid-body. #[must_use] - pub fn local_axis2(&self) -> UnitVector { + pub fn local_axis2(&self) -> UnitVector { self.data.local_axis2() } /// Sets the principal axis of the joint, expressed in the local-space of the second rigid-body. - pub fn set_local_axis2(&mut self, axis2: UnitVector) -> &mut Self { + pub fn set_local_axis2(&mut self, axis2: UnitVector) -> &mut Self { self.data.set_local_axis2(axis2); self } @@ -169,7 +169,7 @@ impl PrismaticJointBuilder { /// Creates a new builder for prismatic joints. /// /// This axis is expressed in the local-space of both rigid-bodies. - pub fn new(axis: UnitVector) -> Self { + pub fn new(axis: UnitVector) -> Self { Self(PrismaticJoint::new(axis)) } @@ -182,28 +182,28 @@ impl PrismaticJointBuilder { /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body. #[must_use] - pub fn local_anchor1(mut self, anchor1: Point) -> Self { + pub fn local_anchor1(mut self, anchor1: Point) -> Self {