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

Outsource the Shape trait, wquadtree, and shape types.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-12-14 15:51:43 +01:00
parent 9bf1321f8f
commit cc6d1b9730
47 changed files with 444 additions and 3363 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
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, SimdFloat, SIMD_WIDTH};
use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdReal, SIMD_WIDTH};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use simba::simd::SimdValue;
@@ -10,17 +10,17 @@ pub(crate) struct WBallPositionConstraint {
position1: [usize; SIMD_WIDTH],
position2: [usize; SIMD_WIDTH],
local_com1: Point<SimdFloat>,
local_com2: Point<SimdFloat>,
local_com1: Point<SimdReal>,
local_com2: Point<SimdReal>,
im1: SimdFloat,
im2: SimdFloat,
im1: SimdReal,
im2: SimdReal,
ii1: AngularInertia<SimdFloat>,
ii2: AngularInertia<SimdFloat>,
ii1: AngularInertia<SimdReal>,
ii2: AngularInertia<SimdReal>,
local_anchor1: Point<SimdFloat>,
local_anchor2: Point<SimdFloat>,
local_anchor1: Point<SimdReal>,
local_anchor2: Point<SimdReal>,
}
impl WBallPositionConstraint {
@@ -31,13 +31,13 @@ impl WBallPositionConstraint {
) -> Self {
let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]);
let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]);
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1 = AngularInertia::<SimdFloat>::from(
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1 = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
)
.squared();
let ii2 = AngularInertia::<SimdFloat>::from(
let ii2 = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
)
.squared();
@@ -97,7 +97,7 @@ impl WBallPositionConstraint {
};
let inv_lhs = lhs.inverse_unchecked();
let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp));
let impulse = inv_lhs * -(err * SimdReal::splat(params.joint_erp));
position1.translation.vector += impulse * self.im1;
position2.translation.vector -= impulse * self.im2;
@@ -120,11 +120,11 @@ impl WBallPositionConstraint {
#[derive(Debug)]
pub(crate) struct WBallPositionGroundConstraint {
position2: [usize; SIMD_WIDTH],
anchor1: Point<SimdFloat>,
im2: SimdFloat,
ii2: AngularInertia<SimdFloat>,
local_anchor2: Point<SimdFloat>,
local_com2: Point<SimdFloat>,
anchor1: Point<SimdReal>,
im2: SimdReal,
ii2: AngularInertia<SimdReal>,
local_anchor2: Point<SimdReal>,
local_com2: Point<SimdReal>,
}
impl WBallPositionGroundConstraint {
@@ -141,8 +141,8 @@ impl WBallPositionGroundConstraint {
} else {
cparams[ii].local_anchor1
}; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2 = AngularInertia::<SimdFloat>::from(
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2 = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
)
.squared();
@@ -186,7 +186,7 @@ impl WBallPositionGroundConstraint {
};
let inv_lhs = lhs.inverse_unchecked();
let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp));
let impulse = inv_lhs * -(err * SimdReal::splat(params.joint_erp));
position2.translation.vector -= impulse * self.im2;
let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse));

64
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, SimdFloat, Vector, SIMD_WIDTH,
AngVector, AngularInertia, Isometry, Point, SdpMatrix, SimdReal, Vector, SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use simba::simd::SimdValue;
@@ -15,16 +15,16 @@ pub(crate) struct WBallVelocityConstraint {
joint_id: [JointIndex; SIMD_WIDTH],
rhs: Vector<SimdFloat>,
pub(crate) impulse: Vector<SimdFloat>,
rhs: Vector<SimdReal>,
pub(crate) impulse: Vector<SimdReal>,
gcross1: Vector<SimdFloat>,
gcross2: Vector<SimdFloat>,
gcross1: Vector<SimdReal>,
gcross2: Vector<SimdReal>,
inv_lhs: SdpMatrix<SimdFloat>,
inv_lhs: SdpMatrix<SimdReal>,
im1: SimdFloat,
im2: SimdFloat,
im1: SimdReal,
im2: SimdReal,
}
impl WBallVelocityConstraint {
@@ -37,20 +37,20 @@ impl WBallVelocityConstraint {
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdFloat>::from(
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdFloat>::from(
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -62,8 +62,8 @@ impl WBallVelocityConstraint {
let anchor1 = position1 * local_anchor1 - world_com1;
let anchor2 = position2 * local_anchor2 - world_com2;
let vel1: Vector<SimdFloat> = linvel1 + angvel1.gcross(anchor1);
let vel2: Vector<SimdFloat> = linvel2 + angvel2.gcross(anchor2);
let vel1: Vector<SimdReal> = linvel1 + angvel1.gcross(anchor1);
let vel2: Vector<SimdReal> = linvel2 + angvel2.gcross(anchor2);
let rhs = -(vel1 - vel2);
let lhs;
@@ -99,7 +99,7 @@ impl WBallVelocityConstraint {
mj_lambda2,
im1,
im2,
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
gcross1,
gcross2,
rhs,
@@ -141,7 +141,7 @@ impl WBallVelocityConstraint {
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
let mut mj_lambda1: DeltaVel<SimdFloat> = DeltaVel {
let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
@@ -149,7 +149,7 @@ impl WBallVelocityConstraint {
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
};
let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
@@ -195,11 +195,11 @@ impl WBallVelocityConstraint {
pub(crate) struct WBallVelocityGroundConstraint {
mj_lambda2: [usize; SIMD_WIDTH],
joint_id: [JointIndex; SIMD_WIDTH],
rhs: Vector<SimdFloat>,
pub(crate) impulse: Vector<SimdFloat>,
gcross2: Vector<SimdFloat>,
inv_lhs: SdpMatrix<SimdFloat>,
im2: SimdFloat,
rhs: Vector<SimdReal>,
pub(crate) impulse: Vector<SimdReal>,
gcross2: Vector<SimdReal>,
inv_lhs: SdpMatrix<SimdReal>,
im2: SimdReal,
}
impl WBallVelocityGroundConstraint {
@@ -213,7 +213,7 @@ impl WBallVelocityGroundConstraint {
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let local_anchor1 = Point::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
@@ -221,10 +221,10 @@ impl WBallVelocityGroundConstraint {
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdFloat>::from(
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -237,8 +237,8 @@ impl WBallVelocityGroundConstraint {
let anchor1 = position1 * local_anchor1 - world_com1;
let anchor2 = position2 * local_anchor2 - world_com2;
let vel1: Vector<SimdFloat> = linvel1 + angvel1.gcross(anchor1);
let vel2: Vector<SimdFloat> = linvel2 + angvel2.gcross(anchor2);
let vel1: Vector<SimdReal> = linvel1 + angvel1.gcross(anchor1);
let vel2: Vector<SimdReal> = linvel2 + angvel2.gcross(anchor2);
let rhs = vel2 - vel1;
let lhs;
@@ -267,7 +267,7 @@ impl WBallVelocityGroundConstraint {
joint_id,
mj_lambda2,
im2,
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
gcross2,
rhs,
inv_lhs,
@@ -294,7 +294,7 @@ impl WBallVelocityGroundConstraint {
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),

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

@@ -5,7 +5,7 @@ use crate::dynamics::{
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
};
use crate::math::{
AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, SimdFloat, SpacialVector, Vector,
AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, SimdReal, SpacialVector, Vector,
SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
@@ -24,29 +24,29 @@ pub(crate) struct WFixedVelocityConstraint {
joint_id: [JointIndex; SIMD_WIDTH],
impulse: SpacialVector<SimdFloat>,
impulse: SpacialVector<SimdReal>,
#[cfg(feature = "dim3")]
inv_lhs: Matrix6<SimdFloat>, // FIXME: replace by Cholesky.
inv_lhs: Matrix6<SimdReal>, // FIXME: replace by Cholesky.
#[cfg(feature = "dim3")]
rhs: Vector6<SimdFloat>,
rhs: Vector6<SimdReal>,
#[cfg(feature = "dim2")]
inv_lhs: Matrix3<SimdFloat>,
inv_lhs: Matrix3<SimdReal>,
#[cfg(feature = "dim2")]
rhs: Vector3<SimdFloat>,
rhs: Vector3<SimdReal>,
im1: SimdFloat,
im2: SimdFloat,
im1: SimdReal,
im2: SimdReal,
ii1: AngularInertia<SimdFloat>,
ii2: AngularInertia<SimdFloat>,
ii1: AngularInertia<SimdReal>,
ii2: AngularInertia<SimdReal>,
ii1_sqrt: AngularInertia<SimdFloat>,
ii2_sqrt: AngularInertia<SimdFloat>,
ii1_sqrt: AngularInertia<SimdReal>,
ii2_sqrt: AngularInertia<SimdReal>,
r1: Vector<SimdFloat>,
r2: Vector<SimdFloat>,
r1: Vector<SimdReal>,
r2: Vector<SimdReal>,
}
impl WFixedVelocityConstraint {
@@ -59,20 +59,20 @@ impl WFixedVelocityConstraint {
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdFloat>::from(
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdFloat>::from(
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -150,7 +150,7 @@ impl WFixedVelocityConstraint {
ii2,
ii1_sqrt,
ii2_sqrt,
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
inv_lhs,
r1,
r2,
@@ -203,7 +203,7 @@ impl WFixedVelocityConstraint {
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
let mut mj_lambda1: DeltaVel<SimdFloat> = DeltaVel {
let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
@@ -211,7 +211,7 @@ impl WFixedVelocityConstraint {
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
};
let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
@@ -279,22 +279,22 @@ pub(crate) struct WFixedVelocityGroundConstraint {
joint_id: [JointIndex; SIMD_WIDTH],
impulse: SpacialVector<SimdFloat>,
impulse: SpacialVector<SimdReal>,
#[cfg(feature = "dim3")]
inv_lhs: Matrix6<SimdFloat>, // FIXME: replace by Cholesky.
inv_lhs: Matrix6<SimdReal>, // FIXME: replace by Cholesky.
#[cfg(feature = "dim3")]
rhs: Vector6<SimdFloat>,
rhs: Vector6<SimdReal>,
#[cfg(feature = "dim2")]
inv_lhs: Matrix3<SimdFloat>,
inv_lhs: Matrix3<SimdReal>,
#[cfg(feature = "dim2")]
rhs: Vector3<SimdFloat>,
rhs: Vector3<SimdReal>,
im2: SimdFloat,
ii2: AngularInertia<SimdFloat>,
ii2_sqrt: AngularInertia<SimdFloat>,
r2: Vector<SimdFloat>,
im2: SimdReal,
ii2: AngularInertia<SimdReal>,
ii2_sqrt: AngularInertia<SimdReal>,
r2: Vector<SimdReal>,
}
impl WFixedVelocityGroundConstraint {
@@ -308,15 +308,15 @@ impl WFixedVelocityGroundConstraint {
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdFloat>::from(
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -386,7 +386,7 @@ impl WFixedVelocityGroundConstraint {
im2,
ii2,
ii2_sqrt,
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
inv_lhs,
r2,
rhs,
@@ -421,7 +421,7 @@ impl WFixedVelocityGroundConstraint {
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),

114
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, SimdFloat, Vector, SIMD_WIDTH,
AngVector, AngularInertia, Isometry, Point, SimdBool, SimdReal, Vector, SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
#[cfg(feature = "dim3")]
@@ -28,37 +28,37 @@ pub(crate) struct WPrismaticVelocityConstraint {
joint_id: [JointIndex; SIMD_WIDTH],
r1: Vector<SimdFloat>,
r2: Vector<SimdFloat>,
r1: Vector<SimdReal>,
r2: Vector<SimdReal>,
#[cfg(feature = "dim3")]
inv_lhs: Matrix5<SimdFloat>,
inv_lhs: Matrix5<SimdReal>,
#[cfg(feature = "dim3")]
rhs: Vector5<SimdFloat>,
rhs: Vector5<SimdReal>,
#[cfg(feature = "dim3")]
impulse: Vector5<SimdFloat>,
impulse: Vector5<SimdReal>,
#[cfg(feature = "dim2")]
inv_lhs: Matrix2<SimdFloat>,
inv_lhs: Matrix2<SimdReal>,
#[cfg(feature = "dim2")]
rhs: Vector2<SimdFloat>,
rhs: Vector2<SimdReal>,
#[cfg(feature = "dim2")]
impulse: Vector2<SimdFloat>,
impulse: Vector2<SimdReal>,
limits_impulse: SimdFloat,
limits_forcedirs: Option<(Vector<SimdFloat>, Vector<SimdFloat>)>,
limits_rhs: SimdFloat,
limits_impulse: SimdReal,
limits_forcedirs: Option<(Vector<SimdReal>, Vector<SimdReal>)>,
limits_rhs: SimdReal,
#[cfg(feature = "dim2")]
basis1: Vector2<SimdFloat>,
basis1: Vector2<SimdReal>,
#[cfg(feature = "dim3")]
basis1: Matrix3x2<SimdFloat>,
basis1: Matrix3x2<SimdReal>,
im1: SimdFloat,
im2: SimdFloat,
im1: SimdReal,
im2: SimdReal,
ii1_sqrt: AngularInertia<SimdFloat>,
ii2_sqrt: AngularInertia<SimdFloat>,
ii1_sqrt: AngularInertia<SimdReal>,
ii2_sqrt: AngularInertia<SimdReal>,
}
impl WPrismaticVelocityConstraint {
@@ -71,20 +71,20 @@ impl WPrismaticVelocityConstraint {
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdFloat>::from(
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdFloat>::from(
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -199,14 +199,14 @@ impl WPrismaticVelocityConstraint {
// FIXME: we should allow both limits to be active at
// the same time + allow predictive constraint activation.
let min_limit = SimdFloat::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
let max_limit = SimdFloat::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
let lim_impulse = SimdFloat::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]);
let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
let lim_impulse = SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]);
let min_enabled = dist.simd_lt(min_limit);
let max_enabled = dist.simd_gt(max_limit);
let _0: SimdFloat = na::zero();
let _1: SimdFloat = na::one();
let _0: SimdReal = na::zero();
let _1: SimdReal = na::one();
let sign = _1.select(min_enabled, (-_1).select(max_enabled, _0));
if sign != _0 {
@@ -224,8 +224,8 @@ impl WPrismaticVelocityConstraint {
ii1_sqrt,
im2,
ii2_sqrt,
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
limits_impulse: limits_impulse * SimdFloat::splat(params.warmstart_coeff),
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff),
limits_forcedirs,
limits_rhs,
basis1,
@@ -383,34 +383,34 @@ pub(crate) struct WPrismaticVelocityGroundConstraint {
joint_id: [JointIndex; SIMD_WIDTH],
r2: Vector<SimdFloat>,
r2: Vector<SimdReal>,
#[cfg(feature = "dim2")]
inv_lhs: Matrix2<SimdFloat>,
inv_lhs: Matrix2<SimdReal>,
#[cfg(feature = "dim2")]
rhs: Vector2<SimdFloat>,
rhs: Vector2<SimdReal>,
#[cfg(feature = "dim2")]
impulse: Vector2<SimdFloat>,
impulse: Vector2<SimdReal>,
#[cfg(feature = "dim3")]
inv_lhs: Matrix5<SimdFloat>,
inv_lhs: Matrix5<SimdReal>,
#[cfg(feature = "dim3")]
rhs: Vector5<SimdFloat>,
rhs: Vector5<SimdReal>,
#[cfg(feature = "dim3")]
impulse: Vector5<SimdFloat>,
impulse: Vector5<SimdReal>,
limits_impulse: SimdFloat,
limits_rhs: SimdFloat,
limits_impulse: SimdReal,
limits_rhs: SimdReal,
axis2: Vector<SimdFloat>,
axis2: Vector<SimdReal>,
#[cfg(feature = "dim2")]
basis1: Vector2<SimdFloat>,
basis1: Vector2<SimdReal>,
#[cfg(feature = "dim3")]
basis1: Matrix3x2<SimdFloat>,
limits_forcedir2: Option<Vector<SimdFloat>>,
basis1: Matrix3x2<SimdReal>,
limits_forcedir2: Option<Vector<SimdReal>>,
im2: SimdFloat,
ii2_sqrt: AngularInertia<SimdFloat>,
im2: SimdReal,
ii2_sqrt: AngularInertia<SimdReal>,
}
impl WPrismaticVelocityGroundConstraint {
@@ -424,15 +424,15 @@ impl WPrismaticVelocityGroundConstraint {
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdFloat>::from(
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -551,14 +551,14 @@ impl WPrismaticVelocityGroundConstraint {
// FIXME: we should allow both limits to be active at
// the same time + allow predictive constraint activation.
let min_limit = SimdFloat::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
let max_limit = SimdFloat::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
let lim_impulse = SimdFloat::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]);
let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
let lim_impulse = SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]);
let use_min = dist.simd_lt(min_limit);
let use_max = dist.simd_gt(max_limit);
let _0: SimdFloat = na::zero();
let _1: SimdFloat = na::one();
let _0: SimdReal = na::zero();
let _1: SimdReal = na::one();
let sign = _1.select(use_min, (-_1).select(use_max, _0));
if sign != _0 {
@@ -573,8 +573,8 @@ impl WPrismaticVelocityGroundConstraint {
mj_lambda2,
im2,
ii2_sqrt,
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
limits_impulse: limits_impulse * SimdFloat::splat(params.warmstart_coeff),
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff),
basis1,
inv_lhs,
rhs,

60
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, SimdFloat, Vector, SIMD_WIDTH};
use crate::math::{AngVector, AngularInertia, Isometry, Point, SimdReal, Vector, SIMD_WIDTH};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
@@ -15,20 +15,20 @@ pub(crate) struct WRevoluteVelocityConstraint {
joint_id: [JointIndex; SIMD_WIDTH],
r1: Vector<SimdFloat>,
r2: Vector<SimdFloat>,
r1: Vector<SimdReal>,
r2: Vector<SimdReal>,
inv_lhs: Matrix5<SimdFloat>,
rhs: Vector5<SimdFloat>,
impulse: Vector5<SimdFloat>,
inv_lhs: Matrix5<SimdReal>,
rhs: Vector5<SimdReal>,
impulse: Vector5<SimdReal>,
basis1: Matrix3x2<SimdFloat>,
basis1: Matrix3x2<SimdReal>,
im1: SimdFloat,
im2: SimdFloat,
im1: SimdReal,
im2: SimdReal,
ii1_sqrt: AngularInertia<SimdFloat>,
ii2_sqrt: AngularInertia<SimdFloat>,
ii1_sqrt: AngularInertia<SimdReal>,
ii2_sqrt: AngularInertia<SimdReal>,
}
impl WRevoluteVelocityConstraint {
@@ -41,20 +41,20 @@ impl WRevoluteVelocityConstraint {
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdFloat>::from(
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdFloat>::from(
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -115,7 +115,7 @@ impl WRevoluteVelocityConstraint {
basis1,
im2,
ii2_sqrt,
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
inv_lhs,
rhs,
r1,
@@ -231,17 +231,17 @@ pub(crate) struct WRevoluteVelocityGroundConstraint {
joint_id: [JointIndex; SIMD_WIDTH],
r2: Vector<SimdFloat>,
r2: Vector<SimdReal>,
inv_lhs: Matrix5<SimdFloat>,
rhs: Vector5<SimdFloat>,
impulse: Vector5<SimdFloat>,
inv_lhs: Matrix5<SimdReal>,
rhs: Vector5<SimdReal>,
impulse: Vector5<SimdReal>,
basis1: Matrix3x2<SimdFloat>,
basis1: Matrix3x2<SimdReal>,
im2: SimdFloat,
im2: SimdReal,
ii2_sqrt: AngularInertia<SimdFloat>,
ii2_sqrt: AngularInertia<SimdReal>,
}
impl WRevoluteVelocityGroundConstraint {
@@ -255,15 +255,15 @@ impl WRevoluteVelocityGroundConstraint {
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdFloat>::from(
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -322,7 +322,7 @@ impl WRevoluteVelocityGroundConstraint {
mj_lambda2,
im2,
ii2_sqrt,
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
basis1,
inv_lhs,
rhs,