diff options
Diffstat (limited to 'src/dynamics')
4 files changed, 39 insertions, 41 deletions
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::<ANG_DIM, ANG_DIM>(DIM, DIM) + .fixed_view_mut::<ANG_DIM, ANG_DIM>(DIM, DIM) .copy_from(&inertia); result } @@ -72,10 +70,10 @@ pub struct Multibody { body_jacobians: Vec<Jacobian<Real>>, // TODO: use sparse matrices? augmented_mass: DMatrix<Real>, - inv_augmented_mass: LU<Real, Dynamic, Dynamic>, + inv_augmented_mass: LU<Real, Dyn, Dyn>, acc_augmented_mass: DMatrix<Real>, - acc_inv_augmented_mass: LU<Real, Dynamic, Dynamic>, + acc_inv_augmented_mass: LU<Real, Dyn, Dyn>, ndofs: usize, pub(crate) root_is_dynamic: bool, @@ -85,8 +83,8 @@ pub struct Multibody { * Workspaces. */ workspace: MultibodyWorkspace, - coriolis_v: Vec<OMatrix<Real, Dim, Dynamic>>, - coriolis_w: Vec<OMatrix<Real, AngDim, Dynamic>>, + coriolis_v: Vec<OMatrix<Real, Dim, Dyn>>, + coriolis_w: Vec<OMatrix<Real, AngDim, Dyn>>, i_coriolis_dt: Jacobian<Real>, } 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<Real, Dynamic, Dynamic> { + pub fn inv_augmented_mass(&self) -> &LU<Real, Dyn, Dyn> { &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::<Real, Dim, Dynamic>::zeros(self.ndofs), + OMatrix::<Real, Dim, Dyn>::zeros(self.ndofs), ); self.coriolis_w.resize( self.links.len(), - OMatrix::<Real, AngDim, Dynamic>::zeros(self.ndofs), + OMatrix::<Real, AngDim, Dyn>::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<Real> { + pub(crate) fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView<Real> { 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<Real> { + pub fn generalized_acceleration(&self) -> DVectorView<Real> { self.accelerations.rows(0, self.ndofs) } /// The generalized velocities of this multibodies. #[inline] - pub fn generalized_velocity(&self) -> DVectorSlice<Real> { + pub fn generalized_velocity(&self) -> DVectorView<Real> { self.velocities.rows(0, self.ndofs) } /// The mutable generalized velocities of this multibodies. #[inline] - pub fn generalized_velocity_mut(&mut self) -> DVectorSliceMut<Real> { + pub fn generalized_velocity_mut(&mut self) -> DVectorViewMut<Real> { 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<Real>, out: &mut JacobianSliceMut<Real>) { + pub fn jacobian(&self, transform: &Rotation<Real>, out: &mut JacobianViewMut<Real>) { 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::<DIM, 1>(0, curr_free_dof) + out.fixed_view_mut::<DIM, 1>(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::<ANG_DIM, 1>(DIM, curr_free_dof) + out.fixed_view_mut::<ANG_DIM, 1>(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<Real>) { + pub fn default_damping(&self, out: &mut DVectorViewMut<Real>) { 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<N: Scalar + Copy> DeltaVel<N> { unsafe { std::mem::transmute(self) } } - pub fn as_vector_slice(&self) -> DVectorSlice<N> { - DVectorSlice::from_slice(&self.as_slice()[..], SPATIAL_DIM) + pub fn as_vector_slice(&self) -> DVectorView<N> { + DVectorView::from_slice(&self.as_slice()[..], SPATIAL_DIM) } - pub fn as_vector_slice_mut(&mut self) -> DVectorSliceMut<N> { - DVectorSliceMut::from_slice(&mut self.as_mut_slice()[..], SPATIAL_DIM) + pub fn as_vector_slice_mut(&mut self) -> DVectorViewMut<N> { + 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<Real>], generic_mj_lambdas: &'a DVector<Real>, - ) -> 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<Real>], generic_mj_lambdas: &'a mut DVector<Real>, - ) -> 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<Real>], generic_mj_lambdas: &'a DVector<Real>, - ) -> 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<Real>], generic_mj_lambdas: &'a mut DVector<Real>, - ) -> 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<Real>], generic_mj_lambdas: &'a DVector<Real>, - ) -> 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<Real>], generic_mj_lambdas: &'a mut DVector<Real>, - ) -> 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); } |