From 9b5ccb95e74350d4fb3b4bc2c4c9fbf9fb4943a2 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 15 Jan 2023 11:59:15 +0100 Subject: Update dependencies --- src/dynamics/joint/multibody_joint/multibody.rs | 30 ++++++++++------------ .../joint/multibody_joint/multibody_joint.rs | 14 +++++----- src/dynamics/solver/delta_vel.rs | 10 ++++---- .../joint_generic_velocity_constraint.rs | 26 +++++++++---------- 4 files changed, 39 insertions(+), 41 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 2eb90eb..63e87e2 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -11,9 +11,7 @@ use crate::math::{ }; use crate::prelude::MultibodyJoint; use crate::utils::{IndexMut2, WAngularInertia, WCross, WCrossMatrix}; -use na::{ - self, DMatrix, DVector, DVectorSlice, DVectorSliceMut, Dynamic, OMatrix, SMatrix, SVector, LU, -}; +use na::{self, DMatrix, DVector, DVectorView, DVectorViewMut, Dyn, OMatrix, SMatrix, SVector, LU}; #[repr(C)] #[derive(Copy, Clone, Debug, Default)] @@ -54,7 +52,7 @@ fn concat_rb_mass_matrix( result[(1, 1)] = mass.y; result[(2, 2)] = mass.z; result - .fixed_slice_mut::(DIM, DIM) + .fixed_view_mut::(DIM, DIM) .copy_from(&inertia); result } @@ -72,10 +70,10 @@ pub struct Multibody { body_jacobians: Vec>, // TODO: use sparse matrices? augmented_mass: DMatrix, - inv_augmented_mass: LU, + inv_augmented_mass: LU, acc_augmented_mass: DMatrix, - acc_inv_augmented_mass: LU, + acc_inv_augmented_mass: LU, ndofs: usize, pub(crate) root_is_dynamic: bool, @@ -85,8 +83,8 @@ pub struct Multibody { * Workspaces. */ workspace: MultibodyWorkspace, - coriolis_v: Vec>, - coriolis_w: Vec>, + coriolis_v: Vec>, + coriolis_w: Vec>, i_coriolis_dt: Jacobian, } impl Default for Multibody { @@ -234,7 +232,7 @@ impl Multibody { } /// The inverse augmented mass matrix of this multibody. - pub fn inv_augmented_mass(&self) -> &LU { + pub fn inv_augmented_mass(&self) -> &LU { &self.inv_augmented_mass } @@ -547,11 +545,11 @@ impl Multibody { if self.coriolis_v.len() != self.links.len() { self.coriolis_v.resize( self.links.len(), - OMatrix::::zeros(self.ndofs), + OMatrix::::zeros(self.ndofs), ); self.coriolis_w.resize( self.links.len(), - OMatrix::::zeros(self.ndofs), + OMatrix::::zeros(self.ndofs), ); self.i_coriolis_dt = Jacobian::zeros(self.ndofs); } @@ -730,9 +728,9 @@ impl Multibody { /// The generalized velocity at the multibody_joint of the given link. #[inline] - pub(crate) fn joint_velocity(&self, link: &MultibodyLink) -> DVectorSlice { + pub(crate) fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView { let ndofs = link.joint().ndofs(); - DVectorSlice::from_slice( + DVectorView::from_slice( &self.velocities.as_slice()[link.assembly_id..link.assembly_id + ndofs], ndofs, ) @@ -740,19 +738,19 @@ impl Multibody { /// The generalized accelerations of this multibodies. #[inline] - pub fn generalized_acceleration(&self) -> DVectorSlice { + pub fn generalized_acceleration(&self) -> DVectorView { self.accelerations.rows(0, self.ndofs) } /// The generalized velocities of this multibodies. #[inline] - pub fn generalized_velocity(&self) -> DVectorSlice { + pub fn generalized_velocity(&self) -> DVectorView { self.velocities.rows(0, self.ndofs) } /// The mutable generalized velocities of this multibodies. #[inline] - pub fn generalized_velocity_mut(&mut self) -> DVectorSliceMut { + pub fn generalized_velocity_mut(&mut self) -> DVectorViewMut { self.velocities.rows_mut(0, self.ndofs) } diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs index c4d9adb..da650e6 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -4,10 +4,10 @@ use crate::dynamics::{ RigidBodyVelocity, }; use crate::math::{ - Isometry, JacobianSliceMut, Real, Rotation, SpacialVector, Translation, Vector, ANG_DIM, DIM, + Isometry, JacobianViewMut, Real, Rotation, SpacialVector, Translation, Vector, ANG_DIM, DIM, SPATIAL_DIM, }; -use na::{DVector, DVectorSliceMut}; +use na::{DVector, DVectorViewMut}; #[cfg(feature = "dim3")] use na::{UnitQuaternion, Vector3}; @@ -126,14 +126,14 @@ 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 JacobianSliceMut) { + 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); - out.fixed_slice_mut::(0, curr_free_dof) + out.fixed_view_mut::(0, curr_free_dof) .copy_from(&transformed_axis); curr_free_dof += 1; } @@ -153,7 +153,7 @@ impl MultibodyJoint { { let dof_id = (!locked_ang_bits).trailing_zeros() as usize; let rotmat = transform.to_rotation_matrix().into_inner(); - out.fixed_slice_mut::(DIM, curr_free_dof) + out.fixed_view_mut::(DIM, curr_free_dof) .copy_from(&rotmat.column(dof_id)); } } @@ -163,7 +163,7 @@ impl MultibodyJoint { #[cfg(feature = "dim3")] 3 => { let rotmat = transform.to_rotation_matrix(); - out.fixed_slice_mut::<3, 3>(3, curr_free_dof) + out.fixed_view_mut::<3, 3>(3, curr_free_dof) .copy_from(rotmat.matrix()); } _ => unreachable!(), @@ -213,7 +213,7 @@ impl MultibodyJoint { } /// Fill `out` with the non-zero entries of a damping that can be applied by default to ensure a good stability of the multibody_joint. - pub fn default_damping(&self, out: &mut DVectorSliceMut) { + pub fn default_damping(&self, out: &mut DVectorViewMut) { let locked_bits = self.data.locked_axes.bits(); let mut curr_free_dof = self.num_free_lin_dofs(); diff --git a/src/dynamics/solver/delta_vel.rs b/src/dynamics/solver/delta_vel.rs index cfdb791..2fc92f0 100644 --- a/src/dynamics/solver/delta_vel.rs +++ b/src/dynamics/solver/delta_vel.rs @@ -1,6 +1,6 @@ use crate::math::{AngVector, Vector, SPATIAL_DIM}; use crate::utils::WReal; -use na::{DVectorSlice, DVectorSliceMut, Scalar}; +use na::{DVectorView, DVectorViewMut, Scalar}; use std::ops::{AddAssign, Sub}; #[derive(Copy, Clone, Debug, Default)] @@ -20,12 +20,12 @@ impl DeltaVel { unsafe { std::mem::transmute(self) } } - pub fn as_vector_slice(&self) -> DVectorSlice { - DVectorSlice::from_slice(&self.as_slice()[..], SPATIAL_DIM) + pub fn as_vector_slice(&self) -> DVectorView { + DVectorView::from_slice(&self.as_slice()[..], SPATIAL_DIM) } - pub fn as_vector_slice_mut(&mut self) -> DVectorSliceMut { - DVectorSliceMut::from_slice(&mut self.as_mut_slice()[..], SPATIAL_DIM) + pub fn as_vector_slice_mut(&mut self) -> DVectorViewMut { + DVectorViewMut::from_slice(&mut self.as_mut_slice()[..], SPATIAL_DIM) } } diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs index 4f26a73..8c83f58 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs @@ -4,7 +4,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, Multibody}; use crate::math::{Isometry, Real, DIM}; use crate::prelude::SPATIAL_DIM; -use na::{DVector, DVectorSlice, DVectorSliceMut}; +use na::{DVector, DVectorView, DVectorViewMut}; #[derive(Debug, Copy, Clone)] pub struct JointGenericVelocityConstraint { @@ -222,7 +222,7 @@ impl JointGenericVelocityConstraint { &self, mj_lambdas: &'a [DeltaVel], generic_mj_lambdas: &'a DVector, - ) -> DVectorSlice<'a, Real> { + ) -> DVectorView<'a, Real> { if self.is_rigid_body1 { mj_lambdas[self.mj_lambda1].as_vector_slice() } else { @@ -234,7 +234,7 @@ impl JointGenericVelocityConstraint { &self, mj_lambdas: &'a mut [DeltaVel], generic_mj_lambdas: &'a mut DVector, - ) -> DVectorSliceMut<'a, Real> { + ) -> DVectorViewMut<'a, Real> { if self.is_rigid_body1 { mj_lambdas[self.mj_lambda1].as_vector_slice_mut() } else { @@ -246,7 +246,7 @@ impl JointGenericVelocityConstraint { &self, mj_lambdas: &'a [DeltaVel], generic_mj_lambdas: &'a DVector, - ) -> DVectorSlice<'a, Real> { + ) -> DVectorView<'a, Real> { if self.is_rigid_body2 { mj_lambdas[self.mj_lambda2].as_vector_slice() } else { @@ -258,7 +258,7 @@ impl JointGenericVelocityConstraint { &self, mj_lambdas: &'a mut [DeltaVel], generic_mj_lambdas: &'a mut DVector, - ) -> DVectorSliceMut<'a, Real> { + ) -> DVectorViewMut<'a, Real> { if self.is_rigid_body2 { mj_lambdas[self.mj_lambda2].as_vector_slice_mut() } else { @@ -275,11 +275,11 @@ impl JointGenericVelocityConstraint { let jacobians = jacobians.as_slice(); let mj_lambda1 = self.mj_lambda1(mj_lambdas, generic_mj_lambdas); - let j1 = DVectorSlice::from_slice(&jacobians[self.j_id1..], self.ndofs1); + let j1 = DVectorView::from_slice(&jacobians[self.j_id1..], self.ndofs1); let vel1 = j1.dot(&mj_lambda1); let mj_lambda2 = self.mj_lambda2(mj_lambdas, generic_mj_lambdas); - let j2 = DVectorSlice::from_slice(&jacobians[self.j_id2..], self.ndofs2); + let j2 = DVectorView::from_slice(&jacobians[self.j_id2..], self.ndofs2); let vel2 = j2.dot(&mj_lambda2); let dvel = self.rhs + (vel2 - vel1); @@ -292,11 +292,11 @@ impl JointGenericVelocityConstraint { self.impulse = total_impulse; let mut mj_lambda1 = self.mj_lambda1_mut(mj_lambdas, generic_mj_lambdas); - let wj1 = DVectorSlice::from_slice(&jacobians[self.wj_id1()..], self.ndofs1); + let wj1 = DVectorView::from_slice(&jacobians[self.wj_id1()..], self.ndofs1); mj_lambda1.axpy(delta_impulse, &wj1, 1.0); let mut mj_lambda2 = self.mj_lambda2_mut(mj_lambdas, generic_mj_lambdas); - let wj2 = DVectorSlice::from_slice(&jacobians[self.wj_id2()..], self.ndofs2); + let wj2 = DVectorView::from_slice(&jacobians[self.wj_id2()..], self.ndofs2); mj_lambda2.axpy(-delta_impulse, &wj2, 1.0); } @@ -506,7 +506,7 @@ impl JointGenericVelocityGroundConstraint { &self, _mj_lambdas: &'a [DeltaVel], generic_mj_lambdas: &'a DVector, - ) -> DVectorSlice<'a, Real> { + ) -> DVectorView<'a, Real> { generic_mj_lambdas.rows(self.mj_lambda2, self.ndofs2) } @@ -514,7 +514,7 @@ impl JointGenericVelocityGroundConstraint { &self, _mj_lambdas: &'a mut [DeltaVel], generic_mj_lambdas: &'a mut DVector, - ) -> DVectorSliceMut<'a, Real> { + ) -> DVectorViewMut<'a, Real> { generic_mj_lambdas.rows_mut(self.mj_lambda2, self.ndofs2) } @@ -527,7 +527,7 @@ impl JointGenericVelocityGroundConstraint { let jacobians = jacobians.as_slice(); let mj_lambda2 = self.mj_lambda2(mj_lambdas, generic_mj_lambdas); - let j2 = DVectorSlice::from_slice(&jacobians[self.j_id2..], self.ndofs2); + let j2 = DVectorView::from_slice(&jacobians[self.j_id2..], self.ndofs2); let vel2 = j2.dot(&mj_lambda2); let dvel = self.rhs + vel2; @@ -540,7 +540,7 @@ impl JointGenericVelocityGroundConstraint { self.impulse = total_impulse; let mut mj_lambda2 = self.mj_lambda2_mut(mj_lambdas, generic_mj_lambdas); - let wj2 = DVectorSlice::from_slice(&jacobians[self.wj_id2()..], self.ndofs2); + let wj2 = DVectorView::from_slice(&jacobians[self.wj_id2()..], self.ndofs2); mj_lambda2.axpy(-delta_impulse, &wj2, 1.0); } -- cgit