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

32
src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
View File

@@ -2,12 +2,12 @@ use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
};
use crate::math::{AngularInertia, Dim, Real, SpacialVector, Vector};
use crate::math::{AngularInertia, Real, SpacialVector, Vector, DIM};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
#[cfg(feature = "dim2")]
use na::{Matrix3, Vector3};
#[cfg(feature = "dim3")]
use na::{Matrix6, Vector6, U3};
use na::{Matrix6, Vector6};
#[derive(Debug)]
pub(crate) struct FixedVelocityConstraint {
@@ -73,10 +73,10 @@ impl FixedVelocityConstraint {
// Note that Cholesky only reads the lower-triangular part of the matrix
// so we don't need to fill lhs01.
lhs = Matrix6::zeros();
lhs.fixed_slice_mut::<U3, U3>(0, 0)
lhs.fixed_slice_mut::<3, 3>(0, 0)
.copy_from(&lhs00.into_matrix());
lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11);
lhs.fixed_slice_mut::<3, 3>(3, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<3, 3>(3, 3).copy_from(&lhs11);
}
// In 2D we just unroll the computation because
@@ -153,11 +153,11 @@ impl FixedVelocityConstraint {
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.impulse.fixed_rows::<Dim>(0).into_owned();
let lin_impulse = self.impulse.fixed_rows::<DIM>(0).into_owned();
#[cfg(feature = "dim2")]
let ang_impulse = self.impulse[2];
#[cfg(feature = "dim3")]
let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned();
let ang_impulse = self.impulse.fixed_rows::<3>(3).into_owned();
mj_lambda1.linear += self.im1 * lin_impulse;
mj_lambda1.angular += self
@@ -194,11 +194,11 @@ impl FixedVelocityConstraint {
let impulse = self.inv_lhs * rhs;
self.impulse += impulse;
let lin_impulse = impulse.fixed_rows::<Dim>(0).into_owned();
let lin_impulse = impulse.fixed_rows::<DIM>(0).into_owned();
#[cfg(feature = "dim2")]
let ang_impulse = impulse[2];
#[cfg(feature = "dim3")]
let ang_impulse = impulse.fixed_rows::<U3>(3).into_owned();
let ang_impulse = impulse.fixed_rows::<3>(3).into_owned();
mj_lambda1.linear += self.im1 * lin_impulse;
mj_lambda1.angular += self
@@ -286,10 +286,10 @@ impl FixedVelocityGroundConstraint {
// Note that Cholesky only reads the lower-triangular part of the matrix
// so we don't need to fill lhs01.
lhs = Matrix6::zeros();
lhs.fixed_slice_mut::<U3, U3>(0, 0)
lhs.fixed_slice_mut::<3, 3>(0, 0)
.copy_from(&lhs00.into_matrix());
lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11);
lhs.fixed_slice_mut::<3, 3>(3, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<3, 3>(3, 3).copy_from(&lhs11);
}
// In 2D we just unroll the computation because
@@ -357,11 +357,11 @@ impl FixedVelocityGroundConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
let lin_impulse = self.impulse.fixed_rows::<DIM>(0).into_owned();
#[cfg(feature = "dim2")]
let ang_impulse = self.impulse[2];
#[cfg(feature = "dim3")]
let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned();
let ang_impulse = self.impulse.fixed_rows::<3>(3).into_owned();
mj_lambda2.linear -= self.im2 * lin_impulse;
mj_lambda2.angular -= self
@@ -388,11 +388,11 @@ impl FixedVelocityGroundConstraint {
let impulse = self.inv_lhs * rhs;
self.impulse += impulse;
let lin_impulse = impulse.fixed_rows::<Dim>(0).into_owned();
let lin_impulse = impulse.fixed_rows::<DIM>(0).into_owned();
#[cfg(feature = "dim2")]
let ang_impulse = impulse[2];
#[cfg(feature = "dim3")]
let ang_impulse = impulse.fixed_rows::<U3>(3).into_owned();
let ang_impulse = impulse.fixed_rows::<3>(3).into_owned();
mj_lambda2.linear -= self.im2 * lin_impulse;
mj_lambda2.angular -= self

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

@@ -5,12 +5,12 @@ use crate::dynamics::{
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
};
use crate::math::{
AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, Real, SimdReal, SpacialVector,
Vector, SIMD_WIDTH,
AngVector, AngularInertia, CrossMatrix, Isometry, Point, Real, SimdReal, SpacialVector, Vector,
DIM, SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
#[cfg(feature = "dim3")]
use na::{Cholesky, Matrix6, Vector3, Vector6, U3};
use na::{Cholesky, Matrix6, Vector3, Vector6};
#[cfg(feature = "dim2")]
use {
na::{Matrix3, Vector3},
@@ -103,10 +103,10 @@ impl WFixedVelocityConstraint {
// Note that Cholesky only reads the lower-triangular part of the matrix
// so we don't need to fill lhs01.
lhs = Matrix6::zeros();
lhs.fixed_slice_mut::<U3, U3>(0, 0)
lhs.fixed_slice_mut::<3, 3>(0, 0)
.copy_from(&lhs00.into_matrix());
lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11);
lhs.fixed_slice_mut::<3, 3>(3, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<3, 3>(3, 3).copy_from(&lhs11);
}
// In 2D we just unroll the computation because
@@ -201,11 +201,11 @@ impl WFixedVelocityConstraint {
),
};
let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
let lin_impulse = self.impulse.fixed_rows::<DIM>(0).into_owned();
#[cfg(feature = "dim2")]
let ang_impulse = self.impulse[2];
#[cfg(feature = "dim3")]
let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned();
let ang_impulse = self.impulse.fixed_rows::<3>(3).into_owned();
mj_lambda1.linear += lin_impulse * self.im1;
mj_lambda1.angular += self
@@ -262,11 +262,11 @@ impl WFixedVelocityConstraint {
let impulse = self.inv_lhs * rhs;
self.impulse += impulse;
let lin_impulse = impulse.fixed_rows::<Dim>(0).into_owned();
let lin_impulse = impulse.fixed_rows::<DIM>(0).into_owned();
#[cfg(feature = "dim2")]
let ang_impulse = impulse[2];
#[cfg(feature = "dim3")]
let ang_impulse = impulse.fixed_rows::<U3>(3).into_owned();
let ang_impulse = impulse.fixed_rows::<3>(3).into_owned();
mj_lambda1.linear += lin_impulse * self.im1;
mj_lambda1.angular += self
@@ -371,10 +371,10 @@ impl WFixedVelocityGroundConstraint {
let lhs11 = ii2.into_matrix();
lhs = Matrix6::zeros();
lhs.fixed_slice_mut::<U3, U3>(0, 0)
lhs.fixed_slice_mut::<3, 3>(0, 0)
.copy_from(&lhs00.into_matrix());
lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11);
lhs.fixed_slice_mut::<3, 3>(3, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<3, 3>(3, 3).copy_from(&lhs11);
}
// In 2D we just unroll the computation because
@@ -454,11 +454,11 @@ impl WFixedVelocityGroundConstraint {
),
};
let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
let lin_impulse = self.impulse.fixed_rows::<DIM>(0).into_owned();
#[cfg(feature = "dim2")]
let ang_impulse = self.impulse[2];
#[cfg(feature = "dim3")]
let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned();
let ang_impulse = self.impulse.fixed_rows::<3>(3).into_owned();
mj_lambda2.linear -= lin_impulse * self.im2;
mj_lambda2.angular -= self
@@ -494,11 +494,11 @@ impl WFixedVelocityGroundConstraint {
let impulse = self.inv_lhs * rhs;
self.impulse += impulse;
let lin_impulse = impulse.fixed_rows::<Dim>(0).into_owned();
let lin_impulse = impulse.fixed_rows::<DIM>(0).into_owned();
#[cfg(feature = "dim2")]
let ang_impulse = impulse[2];
#[cfg(feature = "dim3")]
let ang_impulse = impulse.fixed_rows::<U3>(3).into_owned();
let ang_impulse = impulse.fixed_rows::<3>(3).into_owned();
mj_lambda2.linear -= lin_impulse * self.im2;
mj_lambda2.angular -= self

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

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

@@ -10,7 +10,7 @@ use crate::math::{
use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot};
#[cfg(feature = "dim3")]
use na::{Cholesky, Matrix3x2, Matrix5, Vector3, Vector5, U2, U3};
use na::{Cholesky, Matrix3x2, Matrix5, Vector3, Vector5};
#[cfg(feature = "dim2")]
use {
@@ -19,10 +19,10 @@ 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 WPrismaticVelocityConstraint {
@@ -159,10 +159,10 @@ impl WPrismaticVelocityConstraint {
+ 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")]
@@ -319,11 +319,11 @@ impl WPrismaticVelocityConstraint {
),
};
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 += lin_impulse * self.im1;
mj_lambda1.angular += self
@@ -378,11 +378,11 @@ impl WPrismaticVelocityConstraint {
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 += lin_impulse * self.im1;
mj_lambda1.angular += self
@@ -586,10 +586,10 @@ impl WPrismaticVelocityGroundConstraint {
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")]
@@ -726,11 +726,11 @@ impl WPrismaticVelocityGroundConstraint {
),
};
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 -= lin_impulse * self.im2;
mj_lambda2.angular -= self
@@ -757,11 +757,11 @@ impl WPrismaticVelocityGroundConstraint {
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 -= lin_impulse * self.im2;
mj_lambda2.angular -= self

40
src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
View File

@@ -4,7 +4,7 @@ use crate::dynamics::{
};
use crate::math::{AngularInertia, Real, Rotation, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use na::{Cholesky, Matrix3x2, Matrix5, UnitQuaternion, Vector5, U2, U3};
use na::{Cholesky, Matrix3x2, Matrix5, UnitQuaternion, Vector5};
#[derive(Debug)]
pub(crate) struct RevoluteVelocityConstraint {
@@ -82,10 +82,10 @@ impl RevoluteVelocityConstraint {
// Note that Cholesky won't read the upper-right part
// of lhs so we don't have to fill it.
lhs.fixed_slice_mut::<U3, U3>(0, 0)
lhs.fixed_slice_mut::<3, 3>(0, 0)
.copy_from(&lhs00.into_matrix());
lhs.fixed_slice_mut::<U2, U3>(3, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<U2, U2>(3, 3).copy_from(&lhs11);
lhs.fixed_slice_mut::<2, 3>(3, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<2, 2>(3, 3).copy_from(&lhs11);
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
@@ -198,10 +198,10 @@ impl RevoluteVelocityConstraint {
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
let lin_impulse1 = self.impulse.fixed_rows::<U3>(0).into_owned();
let lin_impulse2 = self.impulse.fixed_rows::<U3>(0).into_owned();
let ang_impulse1 = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
let ang_impulse2 = self.basis2 * self.impulse.fixed_rows::<U2>(3).into_owned();
let lin_impulse1 = self.impulse.fixed_rows::<3>(0).into_owned();
let lin_impulse2 = self.impulse.fixed_rows::<3>(0).into_owned();
let ang_impulse1 = self.basis1 * self.impulse.fixed_rows::<2>(3).into_owned();
let ang_impulse2 = self.basis2 * self.impulse.fixed_rows::<2>(3).into_owned();
mj_lambda1.linear += self.im1 * lin_impulse1;
mj_lambda1.angular += self
@@ -240,10 +240,10 @@ impl RevoluteVelocityConstraint {
Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
let impulse = self.inv_lhs * rhs;
self.impulse += impulse;
let lin_impulse1 = impulse.fixed_rows::<U3>(0).into_owned();
let lin_impulse2 = impulse.fixed_rows::<U3>(0).into_owned();
let ang_impulse1 = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
let ang_impulse2 = self.basis2 * impulse.fixed_rows::<U2>(3).into_owned();
let lin_impulse1 = impulse.fixed_rows::<3>(0).into_owned();
let lin_impulse2 = impulse.fixed_rows::<3>(0).into_owned();
let ang_impulse1 = self.basis1 * impulse.fixed_rows::<2>(3).into_owned();
let ang_impulse2 = self.basis2 * impulse.fixed_rows::<2>(3).into_owned();
mj_lambda1.linear += self.im1 * lin_impulse1;
mj_lambda1.angular += self
@@ -291,7 +291,7 @@ impl RevoluteVelocityConstraint {
let joint = &mut joints_all[self.joint_id].weight;
if let JointParams::RevoluteJoint(revolute) = &mut joint.params {
revolute.impulse = self.impulse;
let rot_part = self.impulse.fixed_rows::<U2>(3).into_owned();
let rot_part = self.impulse.fixed_rows::<2>(3).into_owned();
revolute.world_ang_impulse = self.basis1 * rot_part;
revolute.prev_axis1 = self.motor_axis1;
revolute.motor_last_angle = self.motor_angle;
@@ -385,10 +385,10 @@ impl RevoluteVelocityGroundConstraint {
// Note that cholesky won't read the upper-right part
// of lhs so we don't have to fill it.
lhs.fixed_slice_mut::<U3, U3>(0, 0)
lhs.fixed_slice_mut::<3, 3>(0, 0)
.copy_from(&lhs00.into_matrix());
lhs.fixed_slice_mut::<U2, U3>(3, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<U2, U2>(3, 3).copy_from(&lhs11);
lhs.fixed_slice_mut::<2, 3>(3, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<2, 2>(3, 3).copy_from(&lhs11);
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
@@ -482,8 +482,8 @@ impl RevoluteVelocityGroundConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
let ang_impulse = self.basis2 * self.impulse.fixed_rows::<U2>(3).into_owned();
let lin_impulse = self.impulse.fixed_rows::<3>(0).into_owned();
let ang_impulse = self.basis2 * self.impulse.fixed_rows::<2>(3).into_owned();
mj_lambda2.linear -= self.im2 * lin_impulse;
mj_lambda2.angular -= self
@@ -511,8 +511,8 @@ impl RevoluteVelocityGroundConstraint {
Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
let impulse = self.inv_lhs * rhs;
self.impulse += impulse;
let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
let ang_impulse = self.basis2 * impulse.fixed_rows::<U2>(3).into_owned();
let lin_impulse = impulse.fixed_rows::<3>(0).into_owned();
let ang_impulse = self.basis2 * impulse.fixed_rows::<2>(3).into_owned();
mj_lambda2.linear -= self.im2 * lin_impulse;
mj_lambda2.angular -= self

40
src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs
View File

@@ -8,7 +8,7 @@ use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Vector, SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use na::{Cholesky, Matrix3x2, Matrix5, Unit, Vector5, U2, U3};
use na::{Cholesky, Matrix3x2, Matrix5, Unit, Vector5};
#[derive(Debug)]
pub(crate) struct WRevoluteVelocityConstraint {
@@ -100,10 +100,10 @@ impl WRevoluteVelocityConstraint {
// Note that Cholesky won't read the upper-right part
// of lhs so we don't have to fill it.
lhs.fixed_slice_mut::<U3, U3>(0, 0)
lhs.fixed_slice_mut::<3, 3>(0, 0)
.copy_from(&lhs00.into_matrix());
lhs.fixed_slice_mut::<U2, U3>(3, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<U2, U2>(3, 3).copy_from(&lhs11);
lhs.fixed_slice_mut::<2, 3>(3, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<2, 2>(3, 3).copy_from(&lhs11);
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
@@ -198,10 +198,10 @@ impl WRevoluteVelocityConstraint {
),
};
let lin_impulse1 = self.impulse.fixed_rows::<U3>(0).into_owned();
let lin_impulse2 = self.impulse.fixed_rows::<U3>(0).into_owned();
let ang_impulse1 = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
let ang_impulse2 = self.basis2 * self.impulse.fixed_rows::<U2>(3).into_owned();
let lin_impulse1 = self.impulse.fixed_rows::<3>(0).into_owned();
let lin_impulse2 = self.impulse.fixed_rows::<3>(0).into_owned();
let ang_impulse1 = self.basis1 * self.impulse.fixed_rows::<2>(3).into_owned();
let ang_impulse2 = self.basis2 * self.impulse.fixed_rows::<2>(3).into_owned();
mj_lambda1.linear += lin_impulse1 * self.im1;
mj_lambda1.angular += self
@@ -251,10 +251,10 @@ impl WRevoluteVelocityConstraint {
Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
let impulse = self.inv_lhs * rhs;
self.impulse += impulse;
let lin_impulse1 = impulse.fixed_rows::<U3>(0).into_owned();
let lin_impulse2 = impulse.fixed_rows::<U3>(0).into_owned();
let ang_impulse1 = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
let ang_impulse2 = self.basis2 * impulse.fixed_rows::<U2>(3).into_owned();
let lin_impulse1 = impulse.fixed_rows::<3>(0).into_owned();
let lin_impulse2 = impulse.fixed_rows::<3>(0).into_owned();
let ang_impulse1 = self.basis1 * impulse.fixed_rows::<2>(3).into_owned();
let ang_impulse2 = self.basis2 * impulse.fixed_rows::<2>(3).into_owned();
mj_lambda1.linear += lin_impulse1 * self.im1;
mj_lambda1.angular += self
@@ -277,7 +277,7 @@ impl WRevoluteVelocityConstraint {
}
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
let rot_part = self.impulse.fixed_rows::<U2>(3).into_owned();
let rot_part = self.impulse.fixed_rows::<2>(3).into_owned();
let world_ang_impulse = self.basis1 * rot_part;
for ii in 0..SIMD_WIDTH {
@@ -379,10 +379,10 @@ impl WRevoluteVelocityGroundConstraint {
// Note that cholesky won't read the upper-right part
// of lhs so we don't have to fill it.
lhs.fixed_slice_mut::<U3, U3>(0, 0)
lhs.fixed_slice_mut::<3, 3>(0, 0)
.copy_from(&lhs00.into_matrix());
lhs.fixed_slice_mut::<U2, U3>(3, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<U2, U2>(3, 3).copy_from(&lhs11);
lhs.fixed_slice_mut::<2, 3>(3, 0).copy_from(&lhs10);
lhs.fixed_slice_mut::<2, 2>(3, 3).copy_from(&lhs11);
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
@@ -442,8 +442,8 @@ impl WRevoluteVelocityGroundConstraint {
),
};
let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
let ang_impulse = self.basis2 * self.impulse.fixed_rows::<U2>(3).into_owned();
let lin_impulse = self.impulse.fixed_rows::<3>(0).into_owned();
let ang_impulse = self.basis2 * self.impulse.fixed_rows::<2>(3).into_owned();
mj_lambda2.linear -= lin_impulse * self.im2;
mj_lambda2.angular -= self
@@ -473,8 +473,8 @@ impl WRevoluteVelocityGroundConstraint {
Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
let impulse = self.inv_lhs * rhs;
self.impulse += impulse;
let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
let ang_impulse = self.basis2 * impulse.fixed_rows::<U2>(3).into_owned();
let lin_impulse = impulse.fixed_rows::<3>(0).into_owned();
let ang_impulse = self.basis2 * impulse.fixed_rows::<2>(3).into_owned();
mj_lambda2.linear -= lin_impulse * self.im2;
mj_lambda2.angular -= self