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

4
src/dynamics/solver/delta_vel.rs
View File

@@ -1,9 +1,9 @@
use crate::math::{AngVector, Vector};
use crate::math::{AngVector, Real, Vector};
use na::{Scalar, SimdRealField};
#[derive(Copy, Clone, Debug)]
//#[repr(align(64))]
pub(crate) struct DeltaVel<N: Scalar> {
pub(crate) struct DeltaVel<N: Scalar + Copy> {
pub linear: Vector<N>,
pub angular: AngVector<N>,
}

2
src/dynamics/solver/interaction_groups.rs
View File

@@ -2,7 +2,7 @@ use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[cfg(feature = "simd-is-enabled")]
use {
crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH},
crate::math::{Real, SIMD_LAST_INDEX, SIMD_WIDTH},
vec_map::VecMap,
};

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],

10
src/dynamics/solver/parallel_island_solver.rs
View File

@@ -2,7 +2,7 @@ use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver};
use crate::dynamics::solver::ParallelPositionSolver;
use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::Isometry;
use crate::math::{Isometry, Real};
use crate::utils::WAngularInertia;
use rayon::Scope;
use std::sync::atomic::{AtomicUsize, Ordering};
@@ -119,8 +119,8 @@ impl ThreadContext {
}
pub struct ParallelIslandSolver {
mj_lambdas: Vec<DeltaVel<f32>>,
positions: Vec<Isometry<f32>>,
mj_lambdas: Vec<DeltaVel<Real>>,
positions: Vec<Isometry<Real>>,
parallel_groups: ParallelInteractionGroups,
parallel_joint_groups: ParallelInteractionGroups,
parallel_velocity_solver: ParallelVelocitySolver,
@@ -199,9 +199,9 @@ impl ParallelIslandSolver {
scope.spawn(move |_| {
// Transmute *mut -> &mut
let mj_lambdas: &mut Vec<DeltaVel<f32>> =
let mj_lambdas: &mut Vec<DeltaVel<Real>> =
unsafe { std::mem::transmute(mj_lambdas.load(Ordering::Relaxed)) };
let positions: &mut Vec<Isometry<f32>> =
let positions: &mut Vec<Isometry<Real>> =
unsafe { std::mem::transmute(positions.load(Ordering::Relaxed)) };
let bodies: &mut RigidBodySet =
unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };

4
src/dynamics/solver/parallel_position_solver.rs
View File

@@ -4,7 +4,7 @@ use crate::dynamics::solver::categorization::{categorize_joints, categorize_posi
use crate::dynamics::solver::{InteractionGroups, PositionConstraint, PositionGroundConstraint};
use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet};
use crate::geometry::ContactManifold;
use crate::math::Isometry;
use crate::math::{Isometry, Real};
#[cfg(feature = "simd-is-enabled")]
use crate::{
dynamics::solver::{WPositionConstraint, WPositionGroundConstraint},
@@ -500,7 +500,7 @@ impl ParallelPositionSolver {
&mut self,
thread: &ThreadContext,
params: &IntegrationParameters,
positions: &mut [Isometry<f32>],
positions: &mut [Isometry<Real>],
) {
if self.part.constraint_descs.len() == 0 {
return;

2
src/dynamics/solver/parallel_velocity_solver.rs
View File

@@ -317,7 +317,7 @@ impl ParallelVelocitySolver {
params: &IntegrationParameters,
manifolds_all: &mut [&mut ContactManifold],
joints_all: &mut [JointGraphEdge],
mj_lambdas: &mut [DeltaVel<f32>],
mj_lambdas: &mut [DeltaVel<Real>],
) {
if self.part.constraint_descs.len() == 0 && self.joint_part.constraint_descs.len() == 0 {
return;

26
src/dynamics/solver/position_constraint.rs
View File

@@ -4,7 +4,7 @@ use crate::dynamics::solver::{WPositionConstraint, WPositionGroundConstraint};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::geometry::ContactManifold;
use crate::math::{
AngularInertia, Isometry, Point, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS,
AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS,
};
use crate::utils::{WAngularInertia, WCross, WDot};
@@ -20,7 +20,7 @@ pub(crate) enum AnyPositionConstraint {
}
impl AnyPositionConstraint {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
match self {
#[cfg(feature = "simd-is-enabled")]
AnyPositionConstraint::GroupedGround(c) => c.solve(params, positions),
@@ -37,17 +37,17 @@ pub(crate) struct PositionConstraint {
pub rb1: usize,
pub rb2: usize,
// NOTE: the points are relative to the center of masses.
pub local_p1: [Point<f32>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<f32>; MAX_MANIFOLD_POINTS],
pub dists: [f32; MAX_MANIFOLD_POINTS],
pub local_n1: Vector<f32>,
pub local_p1: [Point<Real>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<Real>; MAX_MANIFOLD_POINTS],
pub dists: [Real; MAX_MANIFOLD_POINTS],
pub local_n1: Vector<Real>,
pub num_contacts: u8,
pub im1: f32,
pub im2: f32,
pub ii1: AngularInertia<f32>,
pub ii2: AngularInertia<f32>,
pub erp: f32,
pub max_linear_correction: f32,
pub im1: Real,
pub im2: Real,
pub ii1: AngularInertia<Real>,
pub ii2: AngularInertia<Real>,
pub erp: Real,
pub max_linear_correction: Real,
}
impl PositionConstraint {
@@ -112,7 +112,7 @@ impl PositionConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
// FIXME: can we avoid most of the multiplications by pos1/pos2?
// Compute jacobians.
let mut pos1 = positions[self.rb1];

6
src/dynamics/solver/position_constraint_wide.rs
View File

@@ -2,8 +2,8 @@ use super::AnyPositionConstraint;
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::geometry::ContactManifold;
use crate::math::{
AngularInertia, Isometry, Point, Rotation, SimdReal, Translation, Vector, MAX_MANIFOLD_POINTS,
SIMD_WIDTH,
AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Translation, Vector,
MAX_MANIFOLD_POINTS, SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WCross, WDot};
@@ -94,7 +94,7 @@ impl WPositionConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
// FIXME: can we avoid most of the multiplications by pos1/pos2?
// Compute jacobians.
let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]);

20
src/dynamics/solver/position_ground_constraint.rs
View File

@@ -2,22 +2,22 @@ use super::AnyPositionConstraint;
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::geometry::ContactManifold;
use crate::math::{
AngularInertia, Isometry, Point, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS,
AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS,
};
use crate::utils::{WAngularInertia, WCross, WDot};
pub(crate) struct PositionGroundConstraint {
pub rb2: usize,
// NOTE: the points are relative to the center of masses.
pub p1: [Point<f32>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<f32>; MAX_MANIFOLD_POINTS],
pub dists: [f32; MAX_MANIFOLD_POINTS],
pub n1: Vector<f32>,
pub p1: [Point<Real>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<Real>; MAX_MANIFOLD_POINTS],
pub dists: [Real; MAX_MANIFOLD_POINTS],
pub n1: Vector<Real>,
pub num_contacts: u8,
pub im2: f32,
pub ii2: AngularInertia<f32>,
pub erp: f32,
pub max_linear_correction: f32,
pub im2: Real,
pub ii2: AngularInertia<Real>,
pub erp: Real,
pub max_linear_correction: Real,
}
impl PositionGroundConstraint {
@@ -79,7 +79,7 @@ impl PositionGroundConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
// FIXME: can we avoid most of the multiplications by pos1/pos2?
// Compute jacobians.
let mut pos2 = positions[self.rb2];

6
src/dynamics/solver/position_ground_constraint_wide.rs
View File

@@ -2,8 +2,8 @@ use super::AnyPositionConstraint;
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::geometry::ContactManifold;
use crate::math::{
AngularInertia, Isometry, Point, Rotation, SimdReal, Translation, Vector, MAX_MANIFOLD_POINTS,
SIMD_WIDTH,
AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Translation, Vector,
MAX_MANIFOLD_POINTS, SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WCross, WDot};
@@ -92,7 +92,7 @@ impl WPositionGroundConstraint {
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
// FIXME: can we avoid most of the multiplications by pos1/pos2?
// Compute jacobians.
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);

4
src/dynamics/solver/position_solver.rs
View File

@@ -1,9 +1,9 @@
use super::AnyJointPositionConstraint;
use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters, RigidBodySet};
use crate::math::Isometry;
use crate::math::{Isometry, Real};
pub(crate) struct PositionSolver {
positions: Vec<Isometry<f32>>,
positions: Vec<Isometry<Real>>,
}
impl PositionSolver {

7
src/dynamics/solver/solver_constraints.rs
View File

@@ -2,11 +2,12 @@ use super::{
AnyJointVelocityConstraint, InteractionGroups, VelocityConstraint, VelocityGroundConstraint,
};
#[cfg(feature = "simd-is-enabled")]
use super::{WVelocityConstraint, WVelocityGroundConstraint};
use super::{
WPositionConstraint, WPositionGroundConstraint, WVelocityConstraint, WVelocityGroundConstraint,
};
use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
use crate::dynamics::solver::{
AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint,
PositionGroundConstraint, WPositionConstraint, WPositionGroundConstraint,
AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint, PositionGroundConstraint,
};
use crate::dynamics::{
solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,

28
src/dynamics/solver/velocity_constraint.rs
View File

@@ -4,7 +4,7 @@ use crate::dynamics::solver::VelocityGroundConstraint;
use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{AngVector, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::math::{AngVector, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
use simba::simd::SimdPartialOrd;
@@ -40,7 +40,7 @@ impl AnyVelocityConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
match self {
AnyVelocityConstraint::NongroupedGround(c) => c.warmstart(mj_lambdas),
AnyVelocityConstraint::Nongrouped(c) => c.warmstart(mj_lambdas),
@@ -52,7 +52,7 @@ impl AnyVelocityConstraint {
}
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
match self {
AnyVelocityConstraint::NongroupedGround(c) => c.solve(mj_lambdas),
AnyVelocityConstraint::Nongrouped(c) => c.solve(mj_lambdas),
@@ -79,11 +79,11 @@ impl AnyVelocityConstraint {
#[derive(Copy, Clone, Debug)]
pub(crate) struct VelocityConstraintElementPart {
pub gcross1: AngVector<f32>,
pub gcross2: AngVector<f32>,
pub rhs: f32,
pub impulse: f32,
pub r: f32,
pub gcross1: AngVector<Real>,
pub gcross2: AngVector<Real>,
pub rhs: Real,
pub impulse: Real,
pub r: Real,
}
#[cfg(not(target_arch = "wasm32"))]
@@ -117,10 +117,10 @@ impl VelocityConstraintElement {
#[derive(Copy, Clone, Debug)]
pub(crate) struct VelocityConstraint {
pub dir1: Vector<f32>, // Non-penetration force direction for the first body.
pub im1: f32,
pub im2: f32,
pub limit: f32,
pub dir1: Vector<Real>, // Non-penetration force direction for the first body.
pub im1: Real,
pub im2: Real,
pub limit: Real,
pub mj_lambda1: usize,
pub mj_lambda2: usize,
pub manifold_id: ContactManifoldIndex,
@@ -300,7 +300,7 @@ impl VelocityConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel::zero();
let mut mj_lambda2 = DeltaVel::zero();
@@ -331,7 +331,7 @@ impl VelocityConstraint {
mj_lambdas[self.mj_lambda2 as usize].angular += mj_lambda2.angular;
}
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];

6
src/dynamics/solver/velocity_constraint_wide.rs
View File

@@ -2,7 +2,7 @@ use super::{AnyVelocityConstraint, DeltaVel};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
use num::Zero;
@@ -199,7 +199,7 @@ impl WVelocityConstraint {
}
}
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],
@@ -249,7 +249,7 @@ impl WVelocityConstraint {
}
}
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],

20
src/dynamics/solver/velocity_ground_constraint.rs
View File

@@ -1,5 +1,5 @@
use super::{AnyVelocityConstraint, DeltaVel};
use crate::math::{AngVector, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::math::{AngVector, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
@@ -8,10 +8,10 @@ use simba::simd::SimdPartialOrd;
#[derive(Copy, Clone, Debug)]
pub(crate) struct VelocityGroundConstraintElementPart {
pub gcross2: AngVector<f32>,
pub rhs: f32,
pub impulse: f32,
pub r: f32,
pub gcross2: AngVector<Real>,
pub rhs: Real,
pub impulse: Real,
pub r: Real,
}
#[cfg(not(target_arch = "wasm32"))]
@@ -44,9 +44,9 @@ impl VelocityGroundConstraintElement {
#[derive(Copy, Clone, Debug)]
pub(crate) struct VelocityGroundConstraint {
pub dir1: Vector<f32>, // Non-penetration force direction for the first body.
pub im2: f32,
pub limit: f32,
pub dir1: Vector<Real>, // Non-penetration force direction for the first body.
pub im2: Real,
pub limit: Real,
pub mj_lambda2: usize,
pub manifold_id: ContactManifoldIndex,
pub manifold_contact_id: usize,
@@ -207,7 +207,7 @@ impl VelocityGroundConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel::zero();
let tangents1 = self.dir1.orthonormal_basis();
@@ -227,7 +227,7 @@ impl VelocityGroundConstraint {
mj_lambdas[self.mj_lambda2 as usize].angular += mj_lambda2.angular;
}
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];
// Solve friction.

6
src/dynamics/solver/velocity_ground_constraint_wide.rs
View File

@@ -2,7 +2,7 @@ use super::{AnyVelocityConstraint, DeltaVel};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
use num::Zero;
@@ -189,7 +189,7 @@ impl WVelocityGroundConstraint {
}
}
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],
@@ -219,7 +219,7 @@ impl WVelocityGroundConstraint {
}
}
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],

3
src/dynamics/solver/velocity_solver.rs
View File

@@ -4,10 +4,11 @@ use crate::dynamics::{
IntegrationParameters, JointGraphEdge, RigidBodySet,
};
use crate::geometry::ContactManifold;
use crate::math::Real;
use crate::utils::WAngularInertia;
pub(crate) struct VelocitySolver {
pub mj_lambdas: Vec<DeltaVel<f32>>,
pub mj_lambdas: Vec<DeltaVel<Real>>,
}
impl VelocitySolver {