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

Switch to nalgebra 0.26

Browse Source
This commit is contained in:
Crozet Sébastien
2021-04-12 17:22:18 +02:00
parent be6a61815d
commit da9c3db5e8
19 changed files with 146 additions and 133 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
View File

@@ -5,7 +5,7 @@ use crate::dynamics::{
use crate::math::{AngularInertia, Real, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot};
#[cfg(feature = "dim3")]
use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
use na::{Cholesky, Matrix3x2, Matrix5, Vector5};
#[cfg(feature = "dim2")]
use {
na::{Matrix2, Vector2},
@@ -13,9 +13,9 @@ use {
};
#[cfg(feature = "dim2")]
type LinImpulseDim = na::U1;
const LIN_IMPULSE_DIM: usize = 1;
#[cfg(feature = "dim3")]
type LinImpulseDim = na::U2;
const LIN_IMPULSE_DIM: usize = 2;
#[derive(Debug)]
pub(crate) struct PrismaticVelocityConstraint {
@@ -114,10 +114,10 @@ impl PrismaticVelocityConstraint {
+ ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2);
let lhs10 = ii1 * r1_mat_b1 + ii2 * r2_mat_b1;
let lhs11 = (ii1 + ii2).into_matrix();
lhs.fixed_slice_mut::<U2, U2>(0, 0)
lhs.fixed_slice_mut::<2, 2>(0, 0)
.copy_from(&lhs00.into_matrix());
lhs.fixed_slice_mut::<U3, U2>(2, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<U3, U3>(2, 2).copy_from(&lhs11);
lhs.fixed_slice_mut::<3, 2>(2, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<3, 3>(2, 2).copy_from(&lhs11);
}
#[cfg(feature = "dim2")]
@@ -284,11 +284,11 @@ impl PrismaticVelocityConstraint {
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
#[cfg(feature = "dim2")]
let ang_impulse = self.impulse.y;
#[cfg(feature = "dim3")]
let ang_impulse = self.impulse.fixed_rows::<U3>(2).into_owned();
let ang_impulse = self.impulse.fixed_rows::<3>(2).into_owned();
mj_lambda1.linear += self.im1 * lin_impulse;
mj_lambda1.angular += self
@@ -336,11 +336,11 @@ impl PrismaticVelocityConstraint {
Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs;
let impulse = self.inv_lhs * rhs;
self.impulse += impulse;
let lin_impulse = self.basis1 * impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
let lin_impulse = self.basis1 * impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
#[cfg(feature = "dim2")]
let ang_impulse = impulse.y;
#[cfg(feature = "dim3")]
let ang_impulse = impulse.fixed_rows::<U3>(2).into_owned();
let ang_impulse = impulse.fixed_rows::<3>(2).into_owned();
mj_lambda1.linear += self.im1 * lin_impulse;
mj_lambda1.angular += self
@@ -544,10 +544,10 @@ impl PrismaticVelocityGroundConstraint {
let lhs00 = ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2);
let lhs10 = ii2 * r2_mat_b1;
let lhs11 = ii2.into_matrix();
lhs.fixed_slice_mut::<U2, U2>(0, 0)
lhs.fixed_slice_mut::<2, 2>(0, 0)
.copy_from(&lhs00.into_matrix());
lhs.fixed_slice_mut::<U3, U2>(2, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<U3, U3>(2, 2).copy_from(&lhs11);
lhs.fixed_slice_mut::<3, 2>(2, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<3, 3>(2, 2).copy_from(&lhs11);
}
#[cfg(feature = "dim2")]
@@ -705,11 +705,11 @@ impl PrismaticVelocityGroundConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
#[cfg(feature = "dim2")]
let ang_impulse = self.impulse.y;
#[cfg(feature = "dim3")]
let ang_impulse = self.impulse.fixed_rows::<U3>(2).into_owned();
let ang_impulse = self.impulse.fixed_rows::<3>(2).into_owned();
mj_lambda2.linear -= self.im2 * lin_impulse;
mj_lambda2.angular -= self
@@ -737,11 +737,11 @@ impl PrismaticVelocityGroundConstraint {
Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs;
let impulse = self.inv_lhs * rhs;
self.impulse += impulse;
let lin_impulse = self.basis1 * impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
let lin_impulse = self.basis1 * impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
#[cfg(feature = "dim2")]
let ang_impulse = impulse.y;
#[cfg(feature = "dim3")]
let ang_impulse = impulse.fixed_rows::<U3>(2).into_owned();
let ang_impulse = impulse.fixed_rows::<3>(2).into_owned();
mj_lambda2.linear -= self.im2 * lin_impulse;
mj_lambda2.angular -= self