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

Add support of 64-bits reals.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-01-04 15:14:25 +01:00
parent a1aa8855f7
commit aa61fe65e3
55 changed files with 656 additions and 518 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,7 +1,7 @@
use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
#[cfg(feature = "dim2")]
use crate::math::SdpMatrix;
use crate::math::{AngularInertia, Isometry, Point, Rotation};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
#[derive(Debug)]
@@ -9,17 +9,17 @@ pub(crate) struct BallPositionConstraint {
position1: usize,
position2: usize,
local_com1: Point<f32>,
local_com2: Point<f32>,
local_com1: Point<Real>,
local_com2: Point<Real>,
im1: f32,
im2: f32,
im1: Real,
im2: Real,
ii1: AngularInertia<f32>,
ii2: AngularInertia<f32>,
ii1: AngularInertia<Real>,
ii2: AngularInertia<Real>,
local_anchor1: Point<f32>,
local_anchor2: Point<f32>,
local_anchor1: Point<Real>,
local_anchor2: Point<Real>,
}
impl BallPositionConstraint {
@@ -38,7 +38,7 @@ impl BallPositionConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position1 = positions[self.position1 as usize];
let mut position2 = positions[self.position2 as usize];
@@ -95,11 +95,11 @@ impl BallPositionConstraint {
#[derive(Debug)]
pub(crate) struct BallPositionGroundConstraint {
position2: usize,
anchor1: Point<f32>,
im2: f32,
ii2: AngularInertia<f32>,
local_anchor2: Point<f32>,
local_com2: Point<f32>,
anchor1: Point<Real>,
im2: Real,
ii2: AngularInertia<Real>,
local_anchor2: Point<Real>,
local_com2: Point<Real>,
}
impl BallPositionGroundConstraint {
@@ -133,7 +133,7 @@ impl BallPositionGroundConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position2 = positions[self.position2 as usize];
let anchor2 = position2 * self.local_anchor2;

6
src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
View File

@@ -1,7 +1,7 @@
use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
#[cfg(feature = "dim2")]
use crate::math::SdpMatrix;
use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdReal, SIMD_WIDTH};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, SimdReal, SIMD_WIDTH};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use simba::simd::SimdValue;
@@ -60,7 +60,7 @@ impl WBallPositionConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position1 = Isometry::from(array![|ii| positions[self.position1[ii]]; SIMD_WIDTH]);
let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]);
@@ -164,7 +164,7 @@ impl WBallPositionGroundConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]);
let anchor2 = position2 * self.local_anchor2;

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

@@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
};
use crate::math::{SdpMatrix, Vector};
use crate::math::{Real, SdpMatrix, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
#[derive(Debug)]
@@ -12,16 +12,16 @@ pub(crate) struct BallVelocityConstraint {
joint_id: JointIndex,
rhs: Vector<f32>,
pub(crate) impulse: Vector<f32>,
rhs: Vector<Real>,
pub(crate) impulse: Vector<Real>,
gcross1: Vector<f32>,
gcross2: Vector<f32>,
gcross1: Vector<Real>,
gcross2: Vector<Real>,
inv_lhs: SdpMatrix<f32>,
inv_lhs: SdpMatrix<Real>,
im1: f32,
im2: f32,
im1: Real,
im2: Real,
}
impl BallVelocityConstraint {
@@ -91,7 +91,7 @@ impl BallVelocityConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
@@ -104,7 +104,7 @@ impl BallVelocityConstraint {
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
@@ -137,11 +137,11 @@ impl BallVelocityConstraint {
pub(crate) struct BallVelocityGroundConstraint {
mj_lambda2: usize,
joint_id: JointIndex,
rhs: Vector<f32>,
impulse: Vector<f32>,
gcross2: Vector<f32>,
inv_lhs: SdpMatrix<f32>,
im2: f32,
rhs: Vector<Real>,
impulse: Vector<Real>,
gcross2: Vector<Real>,
inv_lhs: SdpMatrix<Real>,
im2: Real,
}
impl BallVelocityGroundConstraint {
@@ -206,14 +206,14 @@ impl BallVelocityGroundConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
mj_lambda2.linear -= self.im2 * self.impulse;
mj_lambda2.angular -= self.gcross2.gcross(self.impulse);
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2);

10
src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
View File

@@ -3,7 +3,7 @@ use crate::dynamics::{
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, SdpMatrix, SimdReal, Vector, SIMD_WIDTH,
AngVector, AngularInertia, Isometry, Point, Real, SdpMatrix, SimdReal, Vector, SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use simba::simd::SimdValue;
@@ -107,7 +107,7 @@ impl WBallVelocityConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
@@ -140,7 +140,7 @@ impl WBallVelocityConstraint {
}
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
@@ -274,7 +274,7 @@ impl WBallVelocityGroundConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
@@ -293,7 +293,7 @@ impl WBallVelocityGroundConstraint {
}
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],

38
src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
View File

@@ -1,22 +1,22 @@
use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody};
use crate::math::{AngularInertia, Isometry, Point, Rotation};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation};
use crate::utils::WAngularInertia;
#[derive(Debug)]
pub(crate) struct FixedPositionConstraint {
position1: usize,
position2: usize,
local_anchor1: Isometry<f32>,
local_anchor2: Isometry<f32>,
local_com1: Point<f32>,
local_com2: Point<f32>,
im1: f32,
im2: f32,
ii1: AngularInertia<f32>,
ii2: AngularInertia<f32>,
local_anchor1: Isometry<Real>,
local_anchor2: Isometry<Real>,
local_com1: Point<Real>,
local_com2: Point<Real>,
im1: Real,
im2: Real,
ii1: AngularInertia<Real>,
ii2: AngularInertia<Real>,
lin_inv_lhs: f32,
ang_inv_lhs: AngularInertia<f32>,
lin_inv_lhs: Real,
ang_inv_lhs: AngularInertia<Real>,
}
impl FixedPositionConstraint {
@@ -44,7 +44,7 @@ impl FixedPositionConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position1 = positions[self.position1 as usize];
let mut position2 = positions[self.position2 as usize];
@@ -81,12 +81,12 @@ impl FixedPositionConstraint {
#[derive(Debug)]
pub(crate) struct FixedPositionGroundConstraint {
position2: usize,
anchor1: Isometry<f32>,
local_anchor2: Isometry<f32>,
local_com2: Point<f32>,
im2: f32,
ii2: AngularInertia<f32>,
impulse: f32,
anchor1: Isometry<Real>,
local_anchor2: Isometry<Real>,
local_com2: Point<Real>,
im2: Real,
ii2: AngularInertia<Real>,
impulse: Real,
}
impl FixedPositionGroundConstraint {
@@ -118,7 +118,7 @@ impl FixedPositionGroundConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position2 = positions[self.position2 as usize];
// Angular correction.

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

@@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
};
use crate::math::{AngularInertia, Dim, SpacialVector, Vector};
use crate::math::{AngularInertia, Dim, Real, SpacialVector, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
#[cfg(feature = "dim2")]
use na::{Matrix3, Vector3};
@@ -16,29 +16,29 @@ pub(crate) struct FixedVelocityConstraint {
joint_id: JointIndex,
impulse: SpacialVector<f32>,
impulse: SpacialVector<Real>,
#[cfg(feature = "dim3")]
inv_lhs: Matrix6<f32>, // FIXME: replace by Cholesky.
inv_lhs: Matrix6<Real>, // FIXME: replace by Cholesky.
#[cfg(feature = "dim3")]
rhs: Vector6<f32>,
rhs: Vector6<Real>,
#[cfg(feature = "dim2")]
inv_lhs: Matrix3<f32>, // FIXME: replace by Cholesky.
inv_lhs: Matrix3<Real>, // FIXME: replace by Cholesky.
#[cfg(feature = "dim2")]
rhs: Vector3<f32>,
rhs: Vector3<Real>,
im1: f32,
im2: f32,
im1: Real,
im2: Real,
ii1: AngularInertia<f32>,
ii2: AngularInertia<f32>,
ii1: AngularInertia<Real>,
ii2: AngularInertia<Real>,
ii1_sqrt: AngularInertia<f32>,
ii2_sqrt: AngularInertia<f32>,
ii1_sqrt: AngularInertia<Real>,
ii2_sqrt: AngularInertia<Real>,
r1: Vector<f32>,
r2: Vector<f32>,
r1: Vector<Real>,
r2: Vector<Real>,
}
impl FixedVelocityConstraint {
@@ -128,7 +128,7 @@ impl FixedVelocityConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
@@ -152,7 +152,7 @@ impl FixedVelocityConstraint {
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
@@ -207,22 +207,22 @@ pub(crate) struct FixedVelocityGroundConstraint {
joint_id: JointIndex,
impulse: SpacialVector<f32>,
impulse: SpacialVector<Real>,
#[cfg(feature = "dim3")]
inv_lhs: Matrix6<f32>, // FIXME: replace by Cholesky.
inv_lhs: Matrix6<Real>, // FIXME: replace by Cholesky.
#[cfg(feature = "dim3")]
rhs: Vector6<f32>,
rhs: Vector6<Real>,
#[cfg(feature = "dim2")]
inv_lhs: Matrix3<f32>, // FIXME: replace by Cholesky.
inv_lhs: Matrix3<Real>, // FIXME: replace by Cholesky.
#[cfg(feature = "dim2")]
rhs: Vector3<f32>,
rhs: Vector3<Real>,
im2: f32,
ii2: AngularInertia<f32>,
ii2_sqrt: AngularInertia<f32>,
r2: Vector<f32>,
im2: Real,
ii2: AngularInertia<Real>,
ii2_sqrt: AngularInertia<Real>,
r2: Vector<Real>,
}
impl FixedVelocityGroundConstraint {
@@ -312,7 +312,7 @@ impl FixedVelocityGroundConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
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();
@@ -329,7 +329,7 @@ impl FixedVelocityGroundConstraint {
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

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

@@ -5,8 +5,8 @@ use crate::dynamics::{
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
};
use crate::math::{
AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, SimdReal, SpacialVector, Vector,
SIMD_WIDTH,
AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, Real, SimdReal, SpacialVector,
Vector, SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
#[cfg(feature = "dim3")]
@@ -158,7 +158,7 @@ impl WFixedVelocityConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
@@ -202,7 +202,7 @@ impl WFixedVelocityConstraint {
}
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
@@ -393,7 +393,7 @@ impl WFixedVelocityGroundConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
@@ -420,7 +420,7 @@ impl WFixedVelocityGroundConstraint {
}
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],

5
src/dynamics/solver/joint_constraint/joint_constraint.rs
View File

@@ -17,6 +17,7 @@ use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet,
};
use crate::math::Real;
#[cfg(feature = "simd-is-enabled")]
use crate::math::SIMD_WIDTH;
@@ -220,7 +221,7 @@ impl AnyJointVelocityConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
match self {
AnyJointVelocityConstraint::BallConstraint(c) => c.warmstart(mj_lambdas),
AnyJointVelocityConstraint::BallGroundConstraint(c) => c.warmstart(mj_lambdas),
@@ -254,7 +255,7 @@ impl AnyJointVelocityConstraint {
}
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
match self {
AnyJointVelocityConstraint::BallConstraint(c) => c.solve(mj_lambdas),
AnyJointVelocityConstraint::BallGroundConstraint(c) => c.solve(mj_lambdas),

4
src/dynamics/solver/joint_constraint/joint_position_constraint.rs
View File

@@ -7,9 +7,9 @@ use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
#[cfg(feature = "simd-is-enabled")]
use super::{WBallPositionConstraint, WBallPositionGroundConstraint};
use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet};
use crate::math::Isometry;
#[cfg(feature = "simd-is-enabled")]
use crate::math::SIMD_WIDTH;
use crate::math::{Isometry, Real};
pub(crate) enum AnyJointPositionConstraint {
BallJoint(BallPositionConstraint),
@@ -147,7 +147,7 @@ impl AnyJointPositionConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
match self {
AnyJointPositionConstraint::BallJoint(c) => c.solve(params, positions),
AnyJointPositionConstraint::BallGroundConstraint(c) => c.solve(params, positions),

38
src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs
View File

@@ -1,5 +1,5 @@
use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody};
use crate::math::{AngularInertia, Isometry, Point, Rotation, Vector};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector};
use crate::utils::WAngularInertia;
use na::Unit;
@@ -8,22 +8,22 @@ pub(crate) struct PrismaticPositionConstraint {
position1: usize,
position2: usize,
im1: f32,
im2: f32,
im1: Real,
im2: Real,
ii1: AngularInertia<f32>,
ii2: AngularInertia<f32>,
ii1: AngularInertia<Real>,
ii2: AngularInertia<Real>,
lin_inv_lhs: f32,
ang_inv_lhs: AngularInertia<f32>,
lin_inv_lhs: Real,
ang_inv_lhs: AngularInertia<Real>,
limits: [f32; 2],
limits: [Real; 2],
local_frame1: Isometry<f32>,
local_frame2: Isometry<f32>,
local_frame1: Isometry<Real>,
local_frame2: Isometry<Real>,
local_axis1: Unit<Vector<f32>>,
local_axis2: Unit<Vector<f32>>,
local_axis1: Unit<Vector<Real>>,
local_axis2: Unit<Vector<Real>>,
}
impl PrismaticPositionConstraint {
@@ -52,7 +52,7 @@ impl PrismaticPositionConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position1 = positions[self.position1 as usize];
let mut position2 = positions[self.position2 as usize];
@@ -99,11 +99,11 @@ impl PrismaticPositionConstraint {
#[derive(Debug)]
pub(crate) struct PrismaticPositionGroundConstraint {
position2: usize,
frame1: Isometry<f32>,
local_frame2: Isometry<f32>,
axis1: Unit<Vector<f32>>,
local_axis2: Unit<Vector<f32>>,
limits: [f32; 2],
frame1: Isometry<Real>,
local_frame2: Isometry<Real>,
axis1: Unit<Vector<Real>>,
local_axis2: Unit<Vector<Real>>,
limits: [Real; 2],
}
impl PrismaticPositionGroundConstraint {
@@ -140,7 +140,7 @@ impl PrismaticPositionGroundConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position2 = positions[self.position2 as usize];
// Angular correction.

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

@@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
};
use crate::math::{AngularInertia, Vector};
use crate::math::{AngularInertia, Real, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
#[cfg(feature = "dim3")]
use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
@@ -24,37 +24,37 @@ pub(crate) struct PrismaticVelocityConstraint {
joint_id: JointIndex,
r1: Vector<f32>,
r2: Vector<f32>,
r1: Vector<Real>,
r2: Vector<Real>,
#[cfg(feature = "dim3")]
inv_lhs: Matrix5<f32>,
inv_lhs: Matrix5<Real>,
#[cfg(feature = "dim3")]
rhs: Vector5<f32>,
rhs: Vector5<Real>,
#[cfg(feature = "dim3")]
impulse: Vector5<f32>,
impulse: Vector5<Real>,
#[cfg(feature = "dim2")]
inv_lhs: Matrix2<f32>,
inv_lhs: Matrix2<Real>,
#[cfg(feature = "dim2")]
rhs: Vector2<f32>,
rhs: Vector2<Real>,
#[cfg(feature = "dim2")]
impulse: Vector2<f32>,
impulse: Vector2<Real>,
limits_impulse: f32,
limits_forcedirs: Option<(Vector<f32>, Vector<f32>)>,
limits_rhs: f32,
limits_impulse: Real,
limits_forcedirs: Option<(Vector<Real>, Vector<Real>)>,
limits_rhs: Real,
#[cfg(feature = "dim2")]
basis1: Vector2<f32>,
basis1: Vector2<Real>,
#[cfg(feature = "dim3")]
basis1: Matrix3x2<f32>,
basis1: Matrix3x2<Real>,
im1: f32,
im2: f32,
im1: Real,
im2: Real,
ii1_sqrt: AngularInertia<f32>,
ii2_sqrt: AngularInertia<f32>,
ii1_sqrt: AngularInertia<Real>,
ii2_sqrt: AngularInertia<Real>,
}
impl PrismaticVelocityConstraint {
@@ -191,7 +191,7 @@ impl PrismaticVelocityConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
@@ -220,7 +220,7 @@ impl PrismaticVelocityConstraint {
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
@@ -295,34 +295,34 @@ pub(crate) struct PrismaticVelocityGroundConstraint {
joint_id: JointIndex,
r2: Vector<f32>,
r2: Vector<Real>,
#[cfg(feature = "dim2")]
inv_lhs: Matrix2<f32>,
inv_lhs: Matrix2<Real>,
#[cfg(feature = "dim2")]
rhs: Vector2<f32>,
rhs: Vector2<Real>,
#[cfg(feature = "dim2")]
impulse: Vector2<f32>,
impulse: Vector2<Real>,
#[cfg(feature = "dim3")]
inv_lhs: Matrix5<f32>,
inv_lhs: Matrix5<Real>,
#[cfg(feature = "dim3")]
rhs: Vector5<f32>,
rhs: Vector5<Real>,
#[cfg(feature = "dim3")]
impulse: Vector5<f32>,
impulse: Vector5<Real>,
limits_impulse: f32,
limits_rhs: f32,
limits_impulse: Real,
limits_rhs: Real,
axis2: Vector<f32>,
axis2: Vector<Real>,
#[cfg(feature = "dim2")]
basis1: Vector2<f32>,
basis1: Vector2<Real>,
#[cfg(feature = "dim3")]
basis1: Matrix3x2<f32>,
limits_forcedir2: Option<Vector<f32>>,
basis1: Matrix3x2<Real>,
limits_forcedir2: Option<Vector<Real>>,
im2: f32,
ii2_sqrt: AngularInertia<f32>,
im2: Real,
ii2_sqrt: AngularInertia<Real>,
}
impl PrismaticVelocityGroundConstraint {
@@ -478,7 +478,7 @@ impl PrismaticVelocityGroundConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
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();
@@ -499,7 +499,7 @@ impl PrismaticVelocityGroundConstraint {
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
/*

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

@@ -5,7 +5,7 @@ use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, SimdBool, SimdReal, Vector, SIMD_WIDTH,
AngVector, AngularInertia, Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
#[cfg(feature = "dim3")]
@@ -236,7 +236,7 @@ impl WPrismaticVelocityConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
@@ -285,7 +285,7 @@ impl WPrismaticVelocityConstraint {
}
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
@@ -585,7 +585,7 @@ impl WPrismaticVelocityGroundConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
@@ -616,7 +616,7 @@ impl WPrismaticVelocityGroundConstraint {
}
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],

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

@@ -1,5 +1,5 @@
use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody};
use crate::math::{AngularInertia, Isometry, Point, Rotation, Vector};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector};
use crate::utils::WAngularInertia;
use na::Unit;
@@ -8,20 +8,20 @@ pub(crate) struct RevolutePositionConstraint {
position1: usize,
position2: usize,
im1: f32,
im2: f32,
im1: Real,
im2: Real,
ii1: AngularInertia<f32>,
ii2: AngularInertia<f32>,
ii1: AngularInertia<Real>,
ii2: AngularInertia<Real>,
lin_inv_lhs: f32,
ang_inv_lhs: AngularInertia<f32>,
lin_inv_lhs: Real,
ang_inv_lhs: AngularInertia<Real>,
local_anchor1: Point<f32>,
local_anchor2: Point<f32>,
local_anchor1: Point<Real>,
local_anchor2: Point<Real>,
local_axis1: Unit<Vector<f32>>,
local_axis2: Unit<Vector<f32>>,
local_axis1: Unit<Vector<Real>>,
local_axis2: Unit<Vector<Real>>,
}
impl RevolutePositionConstraint {
@@ -49,7 +49,7 @@ impl RevolutePositionConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position1 = positions[self.position1 as usize];
let mut position2 = positions[self.position2 as usize];
@@ -83,10 +83,10 @@ impl RevolutePositionConstraint {
#[derive(Debug)]
pub(crate) struct RevolutePositionGroundConstraint {
position2: usize,
anchor1: Point<f32>,
local_anchor2: Point<f32>,
axis1: Unit<Vector<f32>>,
local_axis2: Unit<Vector<f32>>,
anchor1: Point<Real>,
local_anchor2: Point<Real>,
axis1: Unit<Vector<Real>>,
local_axis2: Unit<Vector<Real>>,
}
impl RevolutePositionGroundConstraint {
@@ -122,7 +122,7 @@ impl RevolutePositionGroundConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position2 = positions[self.position2 as usize];
let axis2 = position2 * self.local_axis2;

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

@@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
};
use crate::math::{AngularInertia, Vector};
use crate::math::{AngularInertia, Real, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
@@ -13,20 +13,20 @@ pub(crate) struct RevoluteVelocityConstraint {
joint_id: JointIndex,
r1: Vector<f32>,
r2: Vector<f32>,
r1: Vector<Real>,
r2: Vector<Real>,
inv_lhs: Matrix5<f32>,
rhs: Vector5<f32>,
impulse: Vector5<f32>,
inv_lhs: Matrix5<Real>,
rhs: Vector5<Real>,
impulse: Vector5<Real>,
basis1: Matrix3x2<f32>,
basis1: Matrix3x2<Real>,
im1: f32,
im2: f32,
im1: Real,
im2: Real,
ii1_sqrt: AngularInertia<f32>,
ii2_sqrt: AngularInertia<f32>,
ii1_sqrt: AngularInertia<Real>,
ii2_sqrt: AngularInertia<Real>,
}
impl RevoluteVelocityConstraint {
@@ -99,7 +99,7 @@ impl RevoluteVelocityConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
@@ -120,7 +120,7 @@ impl RevoluteVelocityConstraint {
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
@@ -165,17 +165,17 @@ pub(crate) struct RevoluteVelocityGroundConstraint {
joint_id: JointIndex,
r2: Vector<f32>,
r2: Vector<Real>,
inv_lhs: Matrix5<f32>,
rhs: Vector5<f32>,
impulse: Vector5<f32>,
inv_lhs: Matrix5<Real>,
rhs: Vector5<Real>,
impulse: Vector5<Real>,
basis1: Matrix3x2<f32>,
basis1: Matrix3x2<Real>,
im2: f32,
im2: Real,
ii2_sqrt: AngularInertia<f32>,
ii2_sqrt: AngularInertia<Real>,
}
impl RevoluteVelocityGroundConstraint {
@@ -249,7 +249,7 @@ impl RevoluteVelocityGroundConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
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();
@@ -263,7 +263,7 @@ impl RevoluteVelocityGroundConstraint {
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

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

@@ -4,7 +4,7 @@ use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
};
use crate::math::{AngVector, AngularInertia, Isometry, Point, SimdReal, Vector, SIMD_WIDTH};
use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, SIMD_WIDTH};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
@@ -123,7 +123,7 @@ impl WRevoluteVelocityConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
@@ -164,7 +164,7 @@ impl WRevoluteVelocityConstraint {
}
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
@@ -330,7 +330,7 @@ impl WRevoluteVelocityGroundConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
@@ -354,7 +354,7 @@ impl WRevoluteVelocityGroundConstraint {
}
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],