nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update dependencies

Browse Source
This commit is contained in:
Sébastien Crozet
2023-01-15 11:59:15 +01:00
parent 56aa0f5e73
commit 9b5ccb95e7
19 changed files with 116 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
src/dynamics/joint/multibody_joint/multibody.rs
View File

@@ -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)
}

14
src/dynamics/joint/multibody_joint/multibody_joint.rs
View File

@@ -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();