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/ccd/ccd_solver.rs | 55 +- src/dynamics/ccd/toi_entry.rs | 12 +- src/dynamics/coefficient_combine_rule.rs | 10 +- src/dynamics/integration_parameters.rs | 2 +- src/dynamics/island_manager.rs | 10 +- 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 +- src/dynamics/rigid_body.rs | 178 ++-- src/dynamics/rigid_body_components.rs | 302 +++--- src/dynamics/rigid_body_set.rs | 61 +- src/dynamics/solver/categorization.rs | 4 +- .../contact_constraint/contact_constraints_set.rs | 14 +- .../generic_one_body_constraint.rs | 28 +- .../generic_one_body_constraint_element.rs | 17 +- .../generic_two_body_constraint.rs | 48 +- .../generic_two_body_constraint_element.rs | 75 +- .../contact_constraint/one_body_constraint.rs | 64 +- .../one_body_constraint_element.rs | 107 +- .../contact_constraint/one_body_constraint_simd.rs | 86 +- .../contact_constraint/two_body_constraint.rs | 112 +- .../two_body_constraint_element.rs | 143 ++- .../contact_constraint/two_body_constraint_simd.rs | 88 +- src/dynamics/solver/island_solver.rs | 2 +- .../joint_constraint/any_joint_constraint.rs | 2 +- .../joint_constraint/joint_constraint_builder.rs | 1096 +------------------- .../joint_constraint/joint_constraint_helper.rs | 141 +++ .../joint_constraint_helper_simd.rs | 19 + .../joint_constraint_helper_template.rs | 966 +++++++++++++++++ .../joint_constraint/joint_generic_constraint.rs | 24 +- .../joint_generic_constraint_builder.rs | 110 +- .../joint_constraint/joint_velocity_constraint.rs | 106 +- src/dynamics/solver/joint_constraint/mod.rs | 8 +- src/dynamics/solver/parallel_island_solver.rs | 2 +- src/dynamics/solver/parallel_solver_constraints.rs | 2 +- src/dynamics/solver/parallel_velocity_solver.rs | 2 +- src/dynamics/solver/solver_body.rs | 39 +- src/dynamics/solver/solver_constraints_set.rs | 2 +- src/dynamics/solver/solver_vel.rs | 25 +- src/dynamics/solver/velocity_solver.rs | 14 +- 53 files changed, 2351 insertions(+), 2210 deletions(-) create mode 100644 src/dynamics/solver/joint_constraint/joint_constraint_helper.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_constraint_helper_simd.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_constraint_helper_template.rs (limited to 'src/dynamics') diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index 79c4495..064c044 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -1,7 +1,7 @@ use super::TOIEntry; use crate::dynamics::{IslandManager, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ColliderParent, ColliderSet, CollisionEvent, NarrowPhase}; -use crate::math::Real; +use crate::math::*; use crate::parry::utils::SortedPair; use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode}; use crate::prelude::{ActiveEvents, CollisionEventFlags}; @@ -92,14 +92,14 @@ impl CCDSolver { for handle in islands.active_dynamic_bodies() { let rb = bodies.index_mut_internal(*handle); - if rb.ccd.ccd_enabled { + if rb.ccd.enabled { let forces = if include_forces { Some(&rb.forces) } else { None }; let moving_fast = rb.ccd.is_moving_fast(dt, &rb.integrated_vels, forces); - rb.ccd.ccd_active = moving_fast; + rb.ccd.active = moving_fast; ccd_active = ccd_active || moving_fast; } } @@ -129,7 +129,7 @@ impl CCDSolver { for handle in islands.active_dynamic_bodies() { let rb1 = &bodies[*handle]; - if rb1.ccd.ccd_active { + if rb1.ccd.active { let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities( dt, &rb1.forces, @@ -161,10 +161,7 @@ impl CCDSolver { } if pairs_seen - .insert( - SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0), - (), - ) + .insert(SortedPair::new(ch1.index(), ch2.index()), ()) .is_none() { let co1 = &colliders[*ch1]; @@ -253,7 +250,7 @@ impl CCDSolver { for handle in islands.active_dynamic_bodies() { let rb1 = &bodies[*handle]; - if rb1.ccd.ccd_active { + if rb1.ccd.active { let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities( dt, &rb1.forces, @@ -261,8 +258,8 @@ impl CCDSolver { &rb1.mprops, ); - for ch1 in &rb1.colliders.0 { - let co1 = &colliders[*ch1]; + for ch1 in rb1.colliders.0.iter().copied() { + let co1 = &colliders[ch1]; let co_parent1 = co1 .parent .as_ref() @@ -275,19 +272,16 @@ impl CCDSolver { self.query_pipeline .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| { - if *ch1 == *ch2 { + if ch1 == *ch2 { // Ignore self-intersection. return true; } if pairs_seen - .insert( - SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0), - (), - ) + .insert(SortedPair::new(ch1.index(), ch2.index()), ()) .is_none() { - let co1 = &colliders[*ch1]; + let co1 = &colliders[ch1]; let co2 = &colliders[*ch2]; let bh1 = co1.parent.map(|p| p.handle); @@ -301,7 +295,7 @@ impl CCDSolver { } let smallest_dist = narrow_phase - .contact_pair(*ch1, *ch2) + .contact_pair(ch1, *ch2) .and_then(|p| p.find_deepest_contact()) .map(|c| c.1.dist) .unwrap_or(0.0); @@ -311,7 +305,7 @@ impl CCDSolver { if let Some(toi) = TOIEntry::try_from_colliders( self.query_pipeline.query_dispatcher(), - *ch1, + ch1, *ch2, co1, co2, @@ -362,12 +356,10 @@ impl CCDSolver { let rb2 = toi.b2.and_then(|b| bodies.get(b)); let mut colliders_to_check = Vec::new(); - let should_freeze1 = rb1.is_some() - && rb1.unwrap().ccd.ccd_active - && !frozen.contains_key(&toi.b1.unwrap()); - let should_freeze2 = rb2.is_some() - && rb2.unwrap().ccd.ccd_active - && !frozen.contains_key(&toi.b2.unwrap()); + let should_freeze1 = + rb1.is_some() && rb1.unwrap().ccd.active && !frozen.contains_key(&toi.b1.unwrap()); + let should_freeze2 = + rb2.is_some() && rb2.unwrap().ccd.active && !frozen.contains_key(&toi.b2.unwrap()); if !should_freeze1 && !should_freeze2 { continue; @@ -400,8 +392,8 @@ impl CCDSolver { // NOTE: the 1 and 2 indices (e.g., `ch1`, `ch2`) bellow are unrelated to the // ones we used above. - for ch1 in &colliders_to_check { - let co1 = &colliders[*ch1]; + for ch1 in colliders_to_check.iter().copied() { + let co1 = &colliders[ch1]; let co1_parent = co1.parent.as_ref().unwrap(); let rb1 = &bodies[co1_parent.handle]; @@ -428,23 +420,22 @@ impl CCDSolver { let rb1 = bh1.and_then(|h| bodies.get(h)); let rb2 = bh2.and_then(|h| bodies.get(h)); - if (frozen1.is_some() || !rb1.map(|b| b.ccd.ccd_active).unwrap_or(false)) - && (frozen2.is_some() - || !rb2.map(|b| b.ccd.ccd_active).unwrap_or(false)) + if (frozen1.is_some() || !rb1.map(|b| b.ccd.active).unwrap_or(false)) + && (frozen2.is_some() || !rb2.map(|b| b.ccd.active).unwrap_or(false)) { // We already did a resweep. return true; } let smallest_dist = narrow_phase - .contact_pair(*ch1, *ch2) + .contact_pair(ch1, *ch2) .and_then(|p| p.find_deepest_contact()) .map(|c| c.1.dist) .unwrap_or(0.0); if let Some(toi) = TOIEntry::try_from_colliders( self.query_pipeline.query_dispatcher(), - *ch1, + ch1, *ch2, co1, co2, diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs index 719f3c5..3f4f5f2 100644 --- a/src/dynamics/ccd/toi_entry.rs +++ b/src/dynamics/ccd/toi_entry.rs @@ -1,6 +1,6 @@ use crate::dynamics::{RigidBody, RigidBodyHandle}; use crate::geometry::{Collider, ColliderHandle}; -use crate::math::Real; +use crate::math::*; use parry::query::{NonlinearRigidMotion, QueryDispatcher}; #[derive(Copy, Clone, Debug)] @@ -57,13 +57,13 @@ impl TOIEntry { } let linvel1 = frozen1.is_none() as u32 as Real - * rb1.map(|b| b.integrated_vels.linvel).unwrap_or(na::zero()); + * rb1.map(|b| b.integrated_vels.linvel).unwrap_or_default(); let linvel2 = frozen2.is_none() as u32 as Real - * rb2.map(|b| b.integrated_vels.linvel).unwrap_or(na::zero()); + * rb2.map(|b| b.integrated_vels.linvel).unwrap_or_default(); let angvel1 = frozen1.is_none() as u32 as Real - * rb1.map(|b| b.integrated_vels.angvel).unwrap_or(na::zero()); + * rb1.map(|b| b.integrated_vels.angvel).unwrap_or_default(); let angvel2 = frozen2.is_none() as u32 as Real - * rb2.map(|b| b.integrated_vels.angvel).unwrap_or(na::zero()); + * rb2.map(|b| b.integrated_vels.angvel).unwrap_or_default(); #[cfg(feature = "dim2")] let vel12 = (linvel2 - linvel1).norm() @@ -154,7 +154,7 @@ impl TOIEntry { } fn body_motion(rb: &RigidBody) -> NonlinearRigidMotion { - if rb.ccd.ccd_active { + if rb.ccd.active { NonlinearRigidMotion::new( rb.pos.position, rb.mprops.local_mprops.local_com, diff --git a/src/dynamics/coefficient_combine_rule.rs b/src/dynamics/coefficient_combine_rule.rs index 9f99b7d..84094bf 100644 --- a/src/dynamics/coefficient_combine_rule.rs +++ b/src/dynamics/coefficient_combine_rule.rs @@ -1,4 +1,7 @@ -use crate::math::Real; +use crate::math::*; + +#[cfg(feature = "bevy")] +use bevy::prelude::{Component, Reflect, ReflectComponent}; /// Rules used to combine two coefficients. /// @@ -9,6 +12,11 @@ use crate::math::Real; /// actually used is given by `max(first_combine_rule as usize, second_combine_rule as usize)`. #[derive(Default, Copy, Clone, Debug, PartialEq, Eq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "bevy", + derive(Component, Reflect), + reflect(Component, PartialEq) +)] pub enum CoefficientCombineRule { /// The two coefficients are averaged. #[default] diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 13b3fde..ea290fc 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -1,4 +1,4 @@ -use crate::math::Real; +use crate::math::*; use std::num::NonZeroUsize; /// Parameters for a time-step of the physics engine. diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index 8f18941..1e720fc 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -1,9 +1,9 @@ use crate::dynamics::{ - ImpulseJointSet, MultibodyJointSet, RigidBodyActivation, RigidBodyChanges, RigidBodyColliders, - RigidBodyHandle, RigidBodyIds, RigidBodySet, RigidBodyType, RigidBodyVelocity, + ImpulseJointSet, MultibodyJointSet, RigidBodyChanges, RigidBodyColliders, RigidBodyHandle, + RigidBodyIds, RigidBodySet, RigidBodyType, SleepState, Velocity, }; use crate::geometry::{ColliderSet, NarrowPhase}; -use crate::math::Real; +use crate::math::*; use crate::utils::SimdDot; /// Structure responsible for maintaining the set of active rigid-bodies, and @@ -317,14 +317,14 @@ impl IslandManager { for handle in &self.can_sleep { let rb = bodies.index_mut_internal(*handle); if rb.activation.sleeping { - rb.vels = RigidBodyVelocity::zero(); + rb.vels = Velocity::zero(); rb.activation.sleep(); } } } } -fn update_energy(activation: &mut RigidBodyActivation, sq_linvel: Real, sq_angvel: Real, dt: Real) { +fn update_energy(activation: &mut SleepState, sq_linvel: Real, sq_angvel: Real, dt: Real) { if sq_linvel < activation.linear_threshold * activation.linear_threshold.abs() && sq_angvel < activation.angular_threshold * activation.angular_threshold.abs() { 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 m