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,

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

@@ -2,7 +2,7 @@ use super::AnyPositionConstraint;
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::geometry::{ContactManifold, KinematicsCategory};
use crate::math::{
AngularInertia, Isometry, Point, Rotation, SimdFloat, Translation, Vector, MAX_MANIFOLD_POINTS,
AngularInertia, Isometry, Point, Rotation, SimdReal, Translation, Vector, MAX_MANIFOLD_POINTS,
SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WCross, WDot};
@@ -14,16 +14,16 @@ pub(crate) struct WPositionConstraint {
pub rb1: [usize; SIMD_WIDTH],
pub rb2: [usize; SIMD_WIDTH],
// NOTE: the points are relative to the center of masses.
pub local_p1: [Point<SimdFloat>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<SimdFloat>; MAX_MANIFOLD_POINTS],
pub local_n1: Vector<SimdFloat>,
pub radius: SimdFloat,
pub im1: SimdFloat,
pub im2: SimdFloat,
pub ii1: AngularInertia<SimdFloat>,
pub ii2: AngularInertia<SimdFloat>,
pub erp: SimdFloat,
pub max_linear_correction: SimdFloat,
pub local_p1: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
pub local_n1: Vector<SimdReal>,
pub radius: SimdReal,
pub im1: SimdReal,
pub im2: SimdReal,
pub ii1: AngularInertia<SimdReal>,
pub ii2: AngularInertia<SimdReal>,
pub erp: SimdReal,
pub max_linear_correction: SimdReal,
pub num_contacts: u8,
}
@@ -38,18 +38,18 @@ impl WPositionConstraint {
let rbs1 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH];
let rbs2 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH];
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let sqrt_ii1: AngularInertia<SimdFloat> =
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let sqrt_ii1: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let sqrt_ii2: AngularInertia<SimdFloat> =
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let sqrt_ii2: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
let local_n1 = Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]);
let local_n2 = Vector::from(array![|ii| manifolds[ii].local_n2; SIMD_WIDTH]);
let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
let radius1 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
let radius2 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]);
let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]);
@@ -57,7 +57,7 @@ impl WPositionConstraint {
let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/;
let radius = radius1 + radius2 /*- SimdReal::splat(params.allowed_linear_error)*/;
for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
@@ -74,8 +74,8 @@ impl WPositionConstraint {
im2,
ii1: sqrt_ii1.squared(),
ii2: sqrt_ii2.squared(),
erp: SimdFloat::splat(params.erp),
max_linear_correction: SimdFloat::splat(params.max_linear_correction),
erp: SimdReal::splat(params.erp),
max_linear_correction: SimdReal::splat(params.max_linear_correction),
num_contacts: num_points as u8,
};
@@ -119,7 +119,7 @@ impl WPositionConstraint {
// Compute jacobians.
let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]);
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
let allowed_err = SimdFloat::splat(params.allowed_linear_error);
let allowed_err = SimdReal::splat(params.allowed_linear_error);
let target_dist = self.radius - allowed_err;
for k in 0..self.num_contacts as usize {
@@ -133,7 +133,7 @@ impl WPositionConstraint {
let dist = sqdist.simd_sqrt();
let n = dpos / dist;
let err = ((dist - target_dist) * self.erp)
.simd_clamp(-self.max_linear_correction, SimdFloat::zero());
.simd_clamp(-self.max_linear_correction, SimdReal::zero());
let dp1 = p1.coords - pos1.translation.vector;
let dp2 = p2.coords - pos2.translation.vector;
@@ -173,7 +173,7 @@ impl WPositionConstraint {
// Compute jacobians.
let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]);
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
let allowed_err = SimdFloat::splat(params.allowed_linear_error);
let allowed_err = SimdReal::splat(params.allowed_linear_error);
let target_dist = self.radius - allowed_err;
for k in 0..self.num_contacts as usize {
@@ -188,7 +188,7 @@ impl WPositionConstraint {
// NOTE: only works for the point-point case.
let p1 = p2 - n1 * dist;
let err = ((dist - target_dist) * self.erp)
.simd_clamp(-self.max_linear_correction, SimdFloat::zero());
.simd_clamp(-self.max_linear_correction, SimdReal::zero());
let dp1 = p1.coords - pos1.translation.vector;
let dp2 = p2.coords - pos2.translation.vector;

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

@@ -2,7 +2,7 @@ use super::AnyPositionConstraint;
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::geometry::{ContactManifold, KinematicsCategory};
use crate::math::{
AngularInertia, Isometry, Point, Rotation, SimdFloat, Translation, Vector, MAX_MANIFOLD_POINTS,
AngularInertia, Isometry, Point, Rotation, SimdReal, Translation, Vector, MAX_MANIFOLD_POINTS,
SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WCross, WDot};
@@ -13,14 +13,14 @@ use simba::simd::{SimdBool as _, SimdComplexField, SimdPartialOrd, SimdValue};
pub(crate) struct WPositionGroundConstraint {
pub rb2: [usize; SIMD_WIDTH],
// NOTE: the points are relative to the center of masses.
pub p1: [Point<SimdFloat>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<SimdFloat>; MAX_MANIFOLD_POINTS],
pub n1: Vector<SimdFloat>,
pub radius: SimdFloat,
pub im2: SimdFloat,
pub ii2: AngularInertia<SimdFloat>,
pub erp: SimdFloat,
pub max_linear_correction: SimdFloat,
pub p1: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
pub n1: Vector<SimdReal>,
pub radius: SimdReal,
pub im2: SimdReal,
pub ii2: AngularInertia<SimdReal>,
pub erp: SimdReal,
pub max_linear_correction: SimdReal,
pub num_contacts: u8,
}
@@ -45,8 +45,8 @@ impl WPositionGroundConstraint {
}
}
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let sqrt_ii2: AngularInertia<SimdFloat> =
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let sqrt_ii2: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
let local_n1 = Vector::from(
@@ -63,15 +63,15 @@ impl WPositionGroundConstraint {
array![|ii| if flipped[ii] { manifolds[ii].data.delta1 } else { manifolds[ii].data.delta2 }; SIMD_WIDTH],
);
let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
let radius1 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
let radius2 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
let coll_pos1 =
delta1 * Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]);
let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/;
let radius = radius1 + radius2 /*- SimdReal::splat(params.allowed_linear_error)*/;
let n1 = coll_pos1 * local_n1;
@@ -87,8 +87,8 @@ impl WPositionGroundConstraint {
radius,
im2,
ii2: sqrt_ii2.squared(),
erp: SimdFloat::splat(params.erp),
max_linear_correction: SimdFloat::splat(params.max_linear_correction),
erp: SimdReal::splat(params.erp),
max_linear_correction: SimdReal::splat(params.max_linear_correction),
num_contacts: num_points as u8,
};
@@ -131,7 +131,7 @@ impl WPositionGroundConstraint {
// 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]);
let allowed_err = SimdFloat::splat(params.allowed_linear_error);
let allowed_err = SimdReal::splat(params.allowed_linear_error);
let target_dist = self.radius - allowed_err;
for k in 0..self.num_contacts as usize {
@@ -145,7 +145,7 @@ impl WPositionGroundConstraint {
let dist = sqdist.simd_sqrt();
let n = dpos / dist;
let err = ((dist - target_dist) * self.erp)
.simd_clamp(-self.max_linear_correction, SimdFloat::zero());
.simd_clamp(-self.max_linear_correction, SimdReal::zero());
let dp2 = p2.coords - pos2.translation.vector;
let gcross2 = -dp2.gcross(n);
let ii_gcross2 = self.ii2.transform_vector(gcross2);
@@ -173,7 +173,7 @@ impl WPositionGroundConstraint {
// 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]);
let allowed_err = SimdFloat::splat(params.allowed_linear_error);
let allowed_err = SimdReal::splat(params.allowed_linear_error);
let target_dist = self.radius - allowed_err;
for k in 0..self.num_contacts as usize {
@@ -186,7 +186,7 @@ impl WPositionGroundConstraint {
// NOTE: this condition does not seem to be useful perfomancewise?
if dist.simd_lt(target_dist).any() {
let err = ((dist - target_dist) * self.erp)
.simd_clamp(-self.max_linear_correction, SimdFloat::zero());
.simd_clamp(-self.max_linear_correction, SimdReal::zero());
let dp2 = p2.coords - pos2.translation.vector;
let gcross2 = -dp2.gcross(n1);

67
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, Isometry, Point, SimdFloat, Vector, DIM, MAX_MANIFOLD_POINTS,
AngVector, AngularInertia, Isometry, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS,
SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
@@ -11,11 +11,11 @@ use simba::simd::{SimdPartialOrd, SimdValue};
#[derive(Copy, Clone, Debug)]
pub(crate) struct WVelocityConstraintElementPart {
pub gcross1: AngVector<SimdFloat>,
pub gcross2: AngVector<SimdFloat>,
pub rhs: SimdFloat,
pub impulse: SimdFloat,
pub r: SimdFloat,
pub gcross1: AngVector<SimdReal>,
pub gcross2: AngVector<SimdReal>,
pub rhs: SimdReal,
pub impulse: SimdReal,
pub r: SimdReal,
}
impl WVelocityConstraintElementPart {
@@ -23,9 +23,9 @@ impl WVelocityConstraintElementPart {
Self {
gcross1: AngVector::zero(),
gcross2: AngVector::zero(),
rhs: SimdFloat::zero(),
impulse: SimdFloat::zero(),
r: SimdFloat::zero(),
rhs: SimdReal::zero(),
impulse: SimdReal::zero(),
r: SimdReal::zero(),
}
}
}
@@ -47,12 +47,12 @@ impl WVelocityConstraintElement {
#[derive(Copy, Clone, Debug)]
pub(crate) struct WVelocityConstraint {
pub dir1: Vector<SimdFloat>, // Non-penetration force direction for the first body.
pub dir1: Vector<SimdReal>, // Non-penetration force direction for the first body.
pub elements: [WVelocityConstraintElement; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
pub im1: SimdFloat,
pub im2: SimdFloat,
pub limit: SimdFloat,
pub im1: SimdReal,
pub im2: SimdReal,
pub limit: SimdReal,
pub mj_lambda1: [usize; SIMD_WIDTH],
pub mj_lambda2: [usize; SIMD_WIDTH],
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
@@ -68,29 +68,29 @@ impl WVelocityConstraint {
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
) {
let inv_dt = SimdFloat::splat(params.inv_dt());
let inv_dt = SimdReal::splat(params.inv_dt());
let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]);
let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]);
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1: AngularInertia<SimdFloat> =
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; 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 pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdFloat> =
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; 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 pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
@@ -103,14 +103,13 @@ impl WVelocityConstraint {
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let friction = SimdFloat::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]);
let restitution = SimdFloat::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]);
let restitution_velocity_threshold =
SimdFloat::splat(params.restitution_velocity_threshold);
let friction = SimdReal::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]);
let restitution = SimdReal::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]);
let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold);
let warmstart_multiplier =
SimdFloat::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff);
SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
@@ -137,10 +136,10 @@ impl WVelocityConstraint {
let p2 = coll_pos2
* Point::from(array![|ii| manifold_points[ii][k].local_p2; SIMD_WIDTH]);
let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let impulse =
SimdFloat::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
let dp1 = p1 - world_com1;
let dp2 = p2 - world_com2;
@@ -153,13 +152,13 @@ impl WVelocityConstraint {
let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1));
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
let r = SimdFloat::splat(1.0)
let r = SimdReal::splat(1.0)
/ (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
let mut rhs = (vel1 - vel2).dot(&force_dir1);
let use_restitution = rhs.simd_le(-restitution_velocity_threshold);
let rhs_with_restitution = rhs + rhs * restitution;
rhs = rhs_with_restitution.select(use_restitution, rhs);
rhs += dist.simd_max(SimdFloat::zero()) * inv_dt;
rhs += dist.simd_max(SimdReal::zero()) * inv_dt;
constraint.elements[k].normal_part = WVelocityConstraintElementPart {
gcross1,
@@ -175,17 +174,17 @@ impl WVelocityConstraint {
for j in 0..DIM - 1 {
#[cfg(feature = "dim2")]
let impulse = SimdFloat::from(
let impulse = SimdReal::from(
array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH],
);
#[cfg(feature = "dim3")]
let impulse = SimdFloat::from(
let impulse = SimdReal::from(
array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH],
);
let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
let r = SimdFloat::splat(1.0)
let r = SimdReal::splat(1.0)
/ (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
let rhs = (vel1 - vel2).dot(&tangents1[j]);
@@ -309,7 +308,7 @@ impl WVelocityConstraint {
- self.dir1.dot(&mj_lambda2.linear)
+ elt.gcross2.gdot(mj_lambda2.angular)
+ elt.rhs;
let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdFloat::zero());
let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdReal::zero());
let dlambda = new_impulse - elt.impulse;
elt.impulse = new_impulse;

59
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, Isometry, Point, SimdFloat, Vector, DIM, MAX_MANIFOLD_POINTS,
AngVector, AngularInertia, Isometry, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS,
SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
@@ -11,19 +11,19 @@ use simba::simd::{SimdPartialOrd, SimdValue};
#[derive(Copy, Clone, Debug)]
pub(crate) struct WVelocityGroundConstraintElementPart {
pub gcross2: AngVector<SimdFloat>,
pub rhs: SimdFloat,
pub impulse: SimdFloat,
pub r: SimdFloat,
pub gcross2: AngVector<SimdReal>,
pub rhs: SimdReal,
pub impulse: SimdReal,
pub r: SimdReal,
}
impl WVelocityGroundConstraintElementPart {
pub fn zero() -> Self {
Self {
gcross2: AngVector::zero(),
rhs: SimdFloat::zero(),
impulse: SimdFloat::zero(),
r: SimdFloat::zero(),
rhs: SimdReal::zero(),
impulse: SimdReal::zero(),
r: SimdReal::zero(),
}
}
}
@@ -45,11 +45,11 @@ impl WVelocityGroundConstraintElement {
#[derive(Copy, Clone, Debug)]
pub(crate) struct WVelocityGroundConstraint {
pub dir1: Vector<SimdFloat>, // Non-penetration force direction for the first body.
pub dir1: Vector<SimdReal>, // Non-penetration force direction for the first body.
pub elements: [WVelocityGroundConstraintElement; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
pub im2: SimdFloat,
pub limit: SimdFloat,
pub im2: SimdReal,
pub limit: SimdReal,
pub mj_lambda2: [usize; SIMD_WIDTH],
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
pub manifold_contact_id: usize,
@@ -64,7 +64,7 @@ impl WVelocityGroundConstraint {
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
) {
let inv_dt = SimdFloat::splat(params.inv_dt());
let inv_dt = SimdReal::splat(params.inv_dt());
let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
let mut flipped = [false; SIMD_WIDTH];
@@ -76,15 +76,15 @@ impl WVelocityGroundConstraint {
}
}
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdFloat> =
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; 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 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 pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
@@ -109,14 +109,13 @@ impl WVelocityGroundConstraint {
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let friction = SimdFloat::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]);
let restitution = SimdFloat::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]);
let restitution_velocity_threshold =
SimdFloat::splat(params.restitution_velocity_threshold);
let friction = SimdReal::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]);
let restitution = SimdReal::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]);
let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold);
let warmstart_multiplier =
SimdFloat::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff);
SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
@@ -143,10 +142,10 @@ impl WVelocityGroundConstraint {
array![|ii| if flipped[ii] { manifold_points[ii][k].local_p1 } else { manifold_points[ii][k].local_p2 }; SIMD_WIDTH],
);
let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let impulse =
SimdFloat::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
let dp1 = p1 - world_com1;
let dp2 = p2 - world_com2;
@@ -157,12 +156,12 @@ impl WVelocityGroundConstraint {
{
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
let r = SimdFloat::splat(1.0) / (im2 + gcross2.gdot(gcross2));
let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2));
let mut rhs = (vel1 - vel2).dot(&force_dir1);
let use_restitution = rhs.simd_le(-restitution_velocity_threshold);
let rhs_with_restitution = rhs + rhs * restitution;
rhs = rhs_with_restitution.select(use_restitution, rhs);
rhs += dist.simd_max(SimdFloat::zero()) * inv_dt;
rhs += dist.simd_max(SimdReal::zero()) * inv_dt;
constraint.elements[k].normal_part = WVelocityGroundConstraintElementPart {
gcross2,
@@ -177,16 +176,16 @@ impl WVelocityGroundConstraint {
for j in 0..DIM - 1 {
#[cfg(feature = "dim2")]
let impulse = SimdFloat::from(
let impulse = SimdReal::from(
array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH],
);
#[cfg(feature = "dim3")]
let impulse = SimdFloat::from(
let impulse = SimdReal::from(
array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH],
);
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
let r = SimdFloat::splat(1.0) / (im2 + gcross2.gdot(gcross2));
let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2));
let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]);
constraint.elements[k].tangent_parts[j] =
@@ -274,7 +273,7 @@ impl WVelocityGroundConstraint {
let elt = &mut self.elements[i].normal_part;
let dimpulse =
-self.dir1.dot(&mj_lambda2.linear) + elt.gcross2.gdot(mj_lambda2.angular) + elt.rhs;
let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdFloat::zero());
let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdReal::zero());
let dlambda = new_impulse - elt.impulse;
elt.impulse = new_impulse;