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

Split rigid-bodies and colliders into multiple components

Browse Source
This commit is contained in:
Crozet Sébastien
2021-04-26 17:59:25 +02:00
parent aaf80bfa87
commit c32da78f2a
91 changed files with 5969 additions and 3653 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,4 +1,6 @@
use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
use crate::dynamics::{
BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
#[cfg(feature = "dim2")]
use crate::math::SdpMatrix;
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation};
@@ -23,18 +25,25 @@ pub(crate) struct BallPositionConstraint {
}
impl BallPositionConstraint {
pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &BallJoint) -> Self {
pub fn from_params(
rb1: (&RigidBodyMassProps, &RigidBodyIds),
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &BallJoint,
) -> Self {
let (mprops1, ids1) = rb1;
let (mprops2, ids2) = rb2;
Self {
local_com1: rb1.mass_properties.local_com,
local_com2: rb2.mass_properties.local_com,
im1: rb1.effective_inv_mass,
im2: rb2.effective_inv_mass,
ii1: rb1.effective_world_inv_inertia_sqrt.squared(),
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
local_com1: mprops1.mass_properties.local_com,
local_com2: mprops2.mass_properties.local_com,
im1: mprops1.effective_inv_mass,
im2: mprops2.effective_inv_mass,
ii1: mprops1.effective_world_inv_inertia_sqrt.squared(),
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
local_anchor1: cparams.local_anchor1,
local_anchor2: cparams.local_anchor2,
position1: rb1.active_set_offset,
position2: rb2.active_set_offset,
position1: ids1.active_set_offset,
position2: ids2.active_set_offset,
}
}
@@ -104,31 +113,34 @@ pub(crate) struct BallPositionGroundConstraint {
impl BallPositionGroundConstraint {
pub fn from_params(
rb1: &RigidBody,
rb2: &RigidBody,
rb1: &RigidBodyPosition,
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &BallJoint,
flipped: bool,
) -> Self {
let poss1 = rb1;
let (mprops2, ids2) = rb2;
if flipped {
// Note the only thing that is flipped here
// are the local_anchors. The rb1 and rb2 have
// already been flipped by the caller.
Self {
anchor1: rb1.next_position * cparams.local_anchor2,
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
anchor1: poss1.next_position * cparams.local_anchor2,
im2: mprops2.effective_inv_mass,
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
local_anchor2: cparams.local_anchor1,
position2: rb2.active_set_offset,
local_com2: rb2.mass_properties.local_com,
position2: ids2.active_set_offset,
local_com2: mprops2.mass_properties.local_com,
}
} else {
Self {
anchor1: rb1.next_position * cparams.local_anchor1,
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
anchor1: poss1.next_position * cparams.local_anchor1,
im2: mprops2.effective_inv_mass,
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
local_anchor2: cparams.local_anchor2,
position2: rb2.active_set_offset,
local_com2: rb2.mass_properties.local_com,
position2: ids2.active_set_offset,
local_com2: mprops2.mass_properties.local_com,
}
}
}

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

@@ -1,4 +1,6 @@
use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
use crate::dynamics::{
BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
#[cfg(feature = "dim2")]
use crate::math::SdpMatrix;
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, SimdReal, SIMD_WIDTH};
@@ -25,26 +27,35 @@ pub(crate) struct WBallPositionConstraint {
impl WBallPositionConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&BallJoint; SIMD_WIDTH],
) -> 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 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1 = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
)
let (mprops1, ids1) = rbs1;
let (mprops2, ids2) = rbs2;
let local_com1 = Point::from(gather![|ii| mprops1[ii].mass_properties.local_com]);
let local_com2 = Point::from(gather![|ii| mprops2[ii].mass_properties.local_com]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii1 = AngularInertia::<SimdReal>::from(gather![
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
])
.squared();
let ii2 = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
)
let ii2 = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
])
.squared();
let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
let position1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]);
let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]);
let position1 = gather![|ii| ids1[ii].active_set_offset];
let position2 = gather![|ii| ids2[ii].active_set_offset];
Self {
local_com1,
@@ -61,8 +72,8 @@ impl WBallPositionConstraint {
}
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]);
let mut position1 = Isometry::from(gather![|ii| positions[self.position1[ii]]]);
let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]);
let anchor1 = position1 * self.local_anchor1;
let anchor2 = position2 * self.local_anchor2;
@@ -129,30 +140,36 @@ pub(crate) struct WBallPositionGroundConstraint {
impl WBallPositionGroundConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: [&RigidBodyPosition; SIMD_WIDTH],
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&BallJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].next_position; SIMD_WIDTH]);
let poss1 = rbs1;
let (mprops2, ids2) = rbs2;
let position1 = Isometry::from(gather![|ii| poss1[ii].next_position]);
let anchor1 = position1
* Point::from(array![|ii| if flipped[ii] {
* Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor2
} else {
cparams[ii].local_anchor1
}; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2 = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
)
}]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2 = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
])
.squared();
let local_anchor2 = Point::from(array![|ii| if flipped[ii] {
let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor1
} else {
cparams[ii].local_anchor2
}; SIMD_WIDTH]);
let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]);
}]);
let position2 = gather![|ii| ids2[ii].active_set_offset];
let local_com2 = Point::from(gather![|ii| mprops2[ii].mass_properties.local_com]);
Self {
anchor1,
@@ -165,7 +182,7 @@ impl WBallPositionGroundConstraint {
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]);
let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]);
let anchor2 = position2 * self.local_anchor2;
let com2 = position2 * self.local_com2;

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

@@ -1,6 +1,7 @@
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{AngVector, AngularInertia, Real, SdpMatrix, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
@@ -36,19 +37,32 @@ impl BallVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
joint: &BallJoint,
) -> Self {
let anchor_world1 = rb1.position * joint.local_anchor1;
let anchor_world2 = rb2.position * joint.local_anchor2;
let anchor1 = anchor_world1 - rb1.world_com;
let anchor2 = anchor_world2 - rb2.world_com;
let (poss1, vels1, mprops1, ids1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
let anchor_world1 = poss1.position * joint.local_anchor1;
let anchor_world2 = poss2.position * joint.local_anchor2;
let anchor1 = anchor_world1 - mprops1.world_com;
let anchor2 = anchor_world2 - mprops2.world_com;
let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1);
let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2);
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let rhs = (vel2 - vel1) * params.velocity_solve_fraction
+ (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
@@ -59,12 +73,12 @@ impl BallVelocityConstraint {
#[cfg(feature = "dim3")]
{
lhs = rb2
lhs = mprops2
.effective_world_inv_inertia_sqrt
.squared()
.quadform(&cmat2)
.add_diagonal(im2)
+ rb1
+ mprops1
.effective_world_inv_inertia_sqrt
.squared()
.quadform(&cmat1)
@@ -75,8 +89,8 @@ impl BallVelocityConstraint {
// it's just easier that way.
#[cfg(feature = "dim2")]
{
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2;
let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2;
let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2;
@@ -100,8 +114,8 @@ impl BallVelocityConstraint {
);
if stiffness != 0.0 {
let dpos = rb2.position.rotation
* (rb1.position.rotation * joint.motor_target_pos).inverse();
let dpos = poss2.position.rotation
* (poss1.position.rotation * joint.motor_target_pos).inverse();
#[cfg(feature = "dim2")]
{
motor_rhs += dpos.angle() * stiffness;
@@ -113,15 +127,15 @@ impl BallVelocityConstraint {
}
if damping != 0.0 {
let curr_vel = rb2.angvel - rb1.angvel;
let curr_vel = vels2.angvel - vels1.angvel;
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
#[cfg(feature = "dim2")]
if stiffness != 0.0 || damping != 0.0 {
motor_inv_lhs = if keep_lhs {
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
Some(gamma / (ii1 + ii2))
} else {
Some(gamma)
@@ -132,8 +146,8 @@ impl BallVelocityConstraint {
#[cfg(feature = "dim3")]
if stiffness != 0.0 || damping != 0.0 {
motor_inv_lhs = if keep_lhs {
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
Some((ii1 + ii2).inverse_unchecked() * gamma)
} else {
Some(SdpMatrix::diagonal(gamma))
@@ -151,8 +165,8 @@ impl BallVelocityConstraint {
BallVelocityConstraint {
joint_id,
mj_lambda1: rb1.active_set_offset,
mj_lambda2: rb2.active_set_offset,
mj_lambda1: ids1.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im1,
im2,
impulse: joint.impulse * params.warmstart_coeff,
@@ -164,8 +178,8 @@ impl BallVelocityConstraint {
motor_impulse,
motor_inv_lhs,
motor_max_impulse: joint.motor_max_impulse,
ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
}
}
@@ -269,29 +283,37 @@ impl BallVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
joint: &BallJoint,
flipped: bool,
) -> Self {
let (poss1, vels1, mprops1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let (anchor_world1, anchor_world2) = if flipped {
(
rb1.position * joint.local_anchor2,
rb2.position * joint.local_anchor1,
poss1.position * joint.local_anchor2,
poss2.position * joint.local_anchor1,
)
} else {
(
rb1.position * joint.local_anchor1,
rb2.position * joint.local_anchor2,
poss1.position * joint.local_anchor1,
poss2.position * joint.local_anchor2,
)
};
let anchor1 = anchor_world1 - rb1.world_com;
let anchor2 = anchor_world2 - rb2.world_com;
let anchor1 = anchor_world1 - mprops1.world_com;
let anchor2 = anchor_world2 - mprops2.world_com;
let im2 = rb2.effective_inv_mass;
let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
let im2 = mprops2.effective_inv_mass;
let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1);
let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2);
let rhs = (vel2 - vel1) * params.velocity_solve_fraction
+ (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
@@ -302,7 +324,7 @@ impl BallVelocityGroundConstraint {
#[cfg(feature = "dim3")]
{
lhs = rb2
lhs = mprops2
.effective_world_inv_inertia_sqrt
.squared()
.quadform(&cmat2)
@@ -311,7 +333,7 @@ impl BallVelocityGroundConstraint {
#[cfg(feature = "dim2")]
{
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let m11 = im2 + cmat2.x * cmat2.x * ii2;
let m12 = cmat2.x * cmat2.y * ii2;
let m22 = im2 + cmat2.y * cmat2.y * ii2;
@@ -335,8 +357,8 @@ impl BallVelocityGroundConstraint {
);
if stiffness != 0.0 {
let dpos = rb2.position.rotation
* (rb1.position.rotation * joint.motor_target_pos).inverse();
let dpos = poss2.position.rotation
* (poss1.position.rotation * joint.motor_target_pos).inverse();
#[cfg(feature = "dim2")]
{
motor_rhs += dpos.angle() * stiffness;
@@ -348,14 +370,14 @@ impl BallVelocityGroundConstraint {
}
if damping != 0.0 {
let curr_vel = rb2.angvel - rb1.angvel;
let curr_vel = vels2.angvel - vels1.angvel;
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
#[cfg(feature = "dim2")]
if stiffness != 0.0 || damping != 0.0 {
motor_inv_lhs = if keep_lhs {
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
Some(gamma / ii2)
} else {
Some(gamma)
@@ -366,7 +388,7 @@ impl BallVelocityGroundConstraint {
#[cfg(feature = "dim3")]
if stiffness != 0.0 || damping != 0.0 {
motor_inv_lhs = if keep_lhs {
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
Some(ii2.inverse_unchecked() * gamma)
} else {
Some(SdpMatrix::diagonal(gamma))
@@ -384,7 +406,7 @@ impl BallVelocityGroundConstraint {
BallVelocityGroundConstraint {
joint_id,
mj_lambda2: rb2.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im2,
impulse: joint.impulse * params.warmstart_coeff,
r2: anchor2,
@@ -394,7 +416,7 @@ impl BallVelocityGroundConstraint {
motor_impulse,
motor_inv_lhs,
motor_max_impulse: joint.motor_max_impulse,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
}
}

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

@@ -1,6 +1,7 @@
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, SdpMatrix, SimdReal, Vector, SIMD_WIDTH,
@@ -34,33 +35,46 @@ impl WBallVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&BallJoint; SIMD_WIDTH],
) -> 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::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let (poss1, vels1, mprops1, ids1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
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::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]);
let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]);
let impulse = Vector::from(gather![|ii| cparams[ii].impulse]);
let anchor_world1 = position1 * local_anchor1;
let anchor_world2 = position2 * local_anchor2;
@@ -114,20 +128,16 @@ impl WBallVelocityConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
mj_lambda1.linear += self.impulse * self.im1;
@@ -147,20 +157,16 @@ impl WBallVelocityConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
@@ -214,33 +220,49 @@ impl WBallVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&BallJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> 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::<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],
);
let (poss1, vels1, mprops1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
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::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let local_anchor1 = Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor2
} else {
cparams[ii].local_anchor1
}]);
let local_anchor2 = Point::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
);
let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor1
} else {
cparams[ii].local_anchor2
}]);
let impulse = Vector::from(gather![|ii| cparams[ii].impulse]);
let anchor_world1 = position1 * local_anchor1;
let anchor_world2 = position2 * local_anchor2;
@@ -287,12 +309,10 @@ impl WBallVelocityGroundConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
mj_lambda2.linear -= self.impulse * self.im2;
@@ -306,12 +326,10 @@ impl WBallVelocityGroundConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

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

@@ -1,4 +1,6 @@
use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody};
use crate::dynamics::{
FixedJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation};
use crate::utils::WAngularInertia;
@@ -20,25 +22,32 @@ pub(crate) struct FixedPositionConstraint {
}
impl FixedPositionConstraint {
pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &FixedJoint) -> Self {
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
pub fn from_params(
rb1: (&RigidBodyMassProps, &RigidBodyIds),
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &FixedJoint,
) -> Self {
let (mprops1, ids1) = rb1;
let (mprops2, ids2) = rb2;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let lin_inv_lhs = 1.0 / (im1 + im2);
let ang_inv_lhs = (ii1 + ii2).inverse();
Self {
local_anchor1: cparams.local_anchor1,
local_anchor2: cparams.local_anchor2,
position1: rb1.active_set_offset,
position2: rb2.active_set_offset,
position1: ids1.active_set_offset,
position2: ids2.active_set_offset,
im1,
im2,
ii1,
ii2,
local_com1: rb1.mass_properties.local_com,
local_com2: rb2.mass_properties.local_com,
local_com1: mprops1.mass_properties.local_com,
local_com2: mprops2.mass_properties.local_com,
lin_inv_lhs,
ang_inv_lhs,
}
@@ -91,29 +100,32 @@ pub(crate) struct FixedPositionGroundConstraint {
impl FixedPositionGroundConstraint {
pub fn from_params(
rb1: &RigidBody,
rb2: &RigidBody,
rb1: &RigidBodyPosition,
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &FixedJoint,
flipped: bool,
) -> Self {
let poss1 = rb1;
let (mprops2, ids2) = rb2;
let anchor1;
let local_anchor2;
if flipped {
anchor1 = rb1.next_position * cparams.local_anchor2;
anchor1 = poss1.next_position * cparams.local_anchor2;
local_anchor2 = cparams.local_anchor1;
} else {
anchor1 = rb1.next_position * cparams.local_anchor1;
anchor1 = poss1.next_position * cparams.local_anchor1;
local_anchor2 = cparams.local_anchor2;
};
Self {
anchor1,
local_anchor2,
position2: rb2.active_set_offset,
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
local_com2: rb2.mass_properties.local_com,
position2: ids2.active_set_offset,
im2: mprops2.effective_inv_mass,
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
local_com2: mprops2.mass_properties.local_com,
impulse: 0.0,
}
}

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

@@ -1,5 +1,7 @@
use super::{FixedPositionConstraint, FixedPositionGroundConstraint};
use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody};
use crate::dynamics::{
FixedJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
use crate::math::{Isometry, Real, SIMD_WIDTH};
// TODO: this does not uses SIMD optimizations yet.
@@ -10,12 +12,22 @@ pub(crate) struct WFixedPositionConstraint {
impl WFixedPositionConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&FixedJoint; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| FixedPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
constraints: gather![|ii| FixedPositionConstraint::from_params(
(rbs1.0[ii], rbs1.1[ii]),
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii]
)],
}
}
@@ -33,13 +45,21 @@ pub(crate) struct WFixedPositionGroundConstraint {
impl WFixedPositionGroundConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: [&RigidBodyPosition; SIMD_WIDTH],
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&FixedJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| FixedPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
constraints: gather![|ii| FixedPositionGroundConstraint::from_params(
rbs1[ii],
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii],
flipped[ii]
)],
}
}

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

@@ -1,6 +1,7 @@
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{AngularInertia, Real, SpacialVector, Vector, DIM};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
@@ -45,18 +46,31 @@ impl FixedVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
cparams: &FixedJoint,
) -> Self {
let anchor1 = rb1.position * cparams.local_anchor1;
let anchor2 = rb2.position * cparams.local_anchor2;
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1.translation.vector - rb1.world_com.coords;
let r2 = anchor2.translation.vector - rb2.world_com.coords;
let (poss1, vels1, mprops1, ids1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let anchor1 = poss1.position * cparams.local_anchor1;
let anchor2 = poss2.position * cparams.local_anchor2;
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1.translation.vector - mprops1.world_com.coords;
let r2 = anchor2.translation.vector - mprops2.world_com.coords;
let rmat1 = r1.gcross_matrix();
let rmat2 = r2.gcross_matrix();
@@ -99,8 +113,9 @@ impl FixedVelocityConstraint {
#[cfg(feature = "dim3")]
let inv_lhs = lhs.cholesky().expect("Singular system.").inverse();
let lin_dvel = -rb1.linvel - rb1.angvel.gcross(r1) + rb2.linvel + rb2.angvel.gcross(r2);
let ang_dvel = -rb1.angvel + rb2.angvel;
let lin_dvel =
-vels1.linvel - vels1.angvel.gcross(r1) + vels2.linvel + vels2.angvel.gcross(r2);
let ang_dvel = -vels1.angvel + vels2.angvel;
#[cfg(feature = "dim2")]
let mut rhs =
@@ -133,14 +148,14 @@ impl FixedVelocityConstraint {
FixedVelocityConstraint {
joint_id,
mj_lambda1: rb1.active_set_offset,
mj_lambda2: rb2.active_set_offset,
mj_lambda1: ids1.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im1,
im2,
ii1,
ii2,
ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
impulse: cparams.impulse * params.warmstart_coeff,
inv_lhs,
r1,
@@ -250,28 +265,36 @@ impl FixedVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
cparams: &FixedJoint,
flipped: bool,
) -> Self {
let (poss1, vels1, mprops1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let (anchor1, anchor2) = if flipped {
(
rb1.position * cparams.local_anchor2,
rb2.position * cparams.local_anchor1,
poss1.position * cparams.local_anchor2,
poss2.position * cparams.local_anchor1,
)
} else {
(
rb1.position * cparams.local_anchor1,
rb2.position * cparams.local_anchor2,
poss1.position * cparams.local_anchor1,
poss2.position * cparams.local_anchor2,
)
};
let r1 = anchor1.translation.vector - rb1.world_com.coords;
let r1 = anchor1.translation.vector - mprops1.world_com.coords;
let im2 = rb2.effective_inv_mass;
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2.translation.vector - rb2.world_com.coords;
let im2 = mprops2.effective_inv_mass;
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2.translation.vector - mprops2.world_com.coords;
let rmat2 = r2.gcross_matrix();
#[allow(unused_mut)] // For 2D.
@@ -310,8 +333,9 @@ impl FixedVelocityGroundConstraint {
#[cfg(feature = "dim3")]
let inv_lhs = lhs.cholesky().expect("Singular system.").inverse();
let lin_dvel = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1);
let ang_dvel = rb2.angvel - rb1.angvel;
let lin_dvel =
vels2.linvel + vels2.angvel.gcross(r2) - vels1.linvel - vels1.angvel.gcross(r1);
let ang_dvel = vels2.angvel - vels1.angvel;
#[cfg(feature = "dim2")]
let mut rhs =
@@ -343,10 +367,10 @@ impl FixedVelocityGroundConstraint {
FixedVelocityGroundConstraint {
joint_id,
mj_lambda2: rb2.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im2,
ii2,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
impulse: cparams.impulse * params.warmstart_coeff,
inv_lhs,
r2,

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

@@ -2,7 +2,8 @@ use simba::simd::SimdValue;
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{
AngVector, AngularInertia, CrossMatrix, Isometry, Point, Real, SimdReal, SpacialVector, Vector,
@@ -53,33 +54,46 @@ impl WFixedVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&FixedJoint; SIMD_WIDTH],
) -> 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::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let (poss1, vels1, mprops1, ids1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
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::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor1 = Isometry::from(gather![|ii| cparams[ii].local_anchor1]);
let local_anchor2 = Isometry::from(gather![|ii| cparams[ii].local_anchor2]);
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
@@ -157,8 +171,7 @@ impl WFixedVelocityConstraint {
#[cfg(feature = "dim3")]
{
let ang_err =
Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
rhs += Vector6::new(
lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
) * velocity_based_erp_inv_dt;
@@ -185,20 +198,16 @@ impl WFixedVelocityConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.impulse.fixed_rows::<DIM>(0).into_owned();
@@ -229,20 +238,16 @@ impl WFixedVelocityConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
@@ -326,33 +331,49 @@ impl WFixedVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&FixedJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> 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::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let (poss1, vels1, mprops1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
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::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let local_anchor1 = Isometry::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
);
let local_anchor2 = Isometry::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
);
let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor1 = Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor2
} else {
cparams[ii].local_anchor1
}]);
let local_anchor2 = Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor1
} else {
cparams[ii].local_anchor2
}]);
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
@@ -423,8 +444,7 @@ impl WFixedVelocityGroundConstraint {
#[cfg(feature = "dim3")]
{
let ang_err =
Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
rhs += Vector6::new(
lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
) * velocity_based_erp_inv_dt;
@@ -446,12 +466,10 @@ impl WFixedVelocityGroundConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.impulse.fixed_rows::<DIM>(0).into_owned();
@@ -473,12 +491,10 @@ impl WFixedVelocityGroundConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

2
src/dynamics/solver/joint_constraint/generic_position_constraint.rs
View File

@@ -1,6 +1,6 @@
use super::{GenericVelocityConstraint, GenericVelocityGroundConstraint};
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody};
use crate::dynamics::{GenericJoint, IntegrationParameters};
use crate::math::{
AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector,
DIM,

13
src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs
View File

@@ -15,7 +15,11 @@ impl WGenericPositionConstraint {
cparams: [&GenericJoint; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| GenericPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
constraints: gather![|ii| GenericPositionConstraint::from_params(
rbs1[ii],
rbs2[ii],
cparams[ii]
)],
}
}
@@ -39,7 +43,12 @@ impl WGenericPositionGroundConstraint {
flipped: [bool; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| GenericPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
constraints: gather![|ii| GenericPositionGroundConstraint::from_params(
rbs1[ii],
rbs2[ii],
cparams[ii],
flipped[ii]
)],
}
}

16
src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs
View File

@@ -50,8 +50,8 @@ impl GenericVelocityConstraint {
rb1: &RigidBody,
rb2: &RigidBody,
) -> SpatialVector<Real> {
let lin_dvel = basis1.inverse_transform_vector(&(-rb1.linvel - rb1.angvel.gcross(*r1)))
+ basis2.inverse_transform_vector(&(rb2.linvel + rb2.angvel.gcross(*r2)));
let lin_dvel = basis1.inverse_transform_vector(&(-rb1.linvel() - rb1.angvel().gcross(*r1)))
+ basis2.inverse_transform_vector(&(rb2.linvel() + rb2.angvel().gcross(*r2)));
let ang_dvel = basis1.inverse_transform_vector(&-rb1.angvel)
+ basis2.inverse_transform_vector(&rb2.angvel);
@@ -203,8 +203,8 @@ impl GenericVelocityConstraint {
rb2: &RigidBody,
joint: &GenericJoint,
) -> Self {
let anchor1 = rb1.position * joint.local_anchor1;
let anchor2 = rb2.position * joint.local_anchor2;
let anchor1 = rb1.position() * joint.local_anchor1;
let anchor2 = rb2.position() * joint.local_anchor2;
let basis1 = anchor1.rotation;
let basis2 = anchor2.rotation;
let im1 = rb1.effective_inv_mass;
@@ -405,13 +405,13 @@ impl GenericVelocityGroundConstraint {
) -> Self {
let (anchor1, anchor2) = if flipped {
(
rb1.position * joint.local_anchor2,
rb2.position * joint.local_anchor1,
rb1.position() * joint.local_anchor2,
rb2.position() * joint.local_anchor1,
)
} else {
(
rb1.position * joint.local_anchor1,
rb2.position * joint.local_anchor2,
rb1.position() * joint.local_anchor1,
rb2.position() * joint.local_anchor2,
)
};

146
src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs
View File

@@ -57,29 +57,29 @@ impl WGenericVelocityConstraint {
rbs2: [&RigidBody; SIMD_WIDTH],
cparams: [&GenericJoint; SIMD_WIDTH],
) -> 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::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| rbs1[ii].position]);
let linvel1 = Vector::from(gather![|ii| *rbs1[ii].linvel()]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| *rbs1[ii].angvel()]);
let world_com1 = Point::from(gather![|ii| rbs1[ii].world_com]);
let im1 = SimdReal::from(gather![|ii| rbs1[ii].effective_inv_mass]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| rbs1[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda1 = gather![|ii| rbs1[ii].active_set_offset];
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::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position2 = Isometry::from(gather![|ii| rbs2[ii].position]);
let linvel2 = Vector::from(gather![|ii| *rbs2[ii].linvel()]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| *rbs2[ii].angvel()]);
let world_com2 = Point::from(gather![|ii| rbs2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| rbs2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| rbs2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| rbs2[ii].active_set_offset];
let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let local_anchor1 = Isometry::from(gather![|ii| cparams[ii].local_anchor1]);
let local_anchor2 = Isometry::from(gather![|ii| cparams[ii].local_anchor2]);
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
@@ -160,20 +160,16 @@ impl WGenericVelocityConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
@@ -204,20 +200,16 @@ impl WGenericVelocityConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
@@ -306,28 +298,32 @@ impl WGenericVelocityGroundConstraint {
cparams: [&GenericJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> 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::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let position1 = Isometry::from(gather![|ii| rbs1[ii].position]);
let linvel1 = Vector::from(gather![|ii| *rbs1[ii].linvel()]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| *rbs1[ii].angvel()]);
let world_com1 = Point::from(gather![|ii| rbs1[ii].world_com]);
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::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position2 = Isometry::from(gather![|ii| rbs2[ii].position]);
let linvel2 = Vector::from(gather![|ii| *rbs2[ii].linvel()]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| *rbs2[ii].angvel()]);
let world_com2 = Point::from(gather![|ii| rbs2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| rbs2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| rbs2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| rbs2[ii].active_set_offset];
let local_anchor1 = Isometry::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
);
let local_anchor2 = Isometry::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
);
let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let local_anchor1 = Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor2
} else {
cparams[ii].local_anchor1
}]);
let local_anchor2 = Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor1
} else {
cparams[ii].local_anchor2
}]);
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
@@ -395,12 +391,10 @@ impl WGenericVelocityGroundConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
@@ -422,12 +416,10 @@ impl WGenericVelocityGroundConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

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

@@ -16,9 +16,11 @@ use super::{WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint};
// use crate::dynamics::solver::joint_constraint::generic_velocity_constraint::{
// GenericVelocityConstraint, GenericVelocityGroundConstraint,
// };
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet,
IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
};
use crate::math::Real;
#[cfg(feature = "simd-is-enabled")]
@@ -69,14 +71,30 @@ impl AnyJointVelocityConstraint {
1
}
pub fn from_joint(
pub fn from_joint<Bodies>(
params: &IntegrationParameters,
joint_id: JointIndex,
joint: &Joint,
bodies: &RigidBodySet,
) -> Self {
let rb1 = &bodies[joint.body1];
let rb2 = &bodies[joint.body2];
bodies: &Bodies,
) -> Self
where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let rb1 = (
bodies.index(joint.body1.0),
bodies.index(joint.body1.0),
bodies.index(joint.body1.0),
bodies.index(joint.body1.0),
);
let rb2 = (
bodies.index(joint.body2.0),
bodies.index(joint.body2.0),
bodies.index(joint.body2.0),
bodies.index(joint.body2.0),
);
match &joint.params {
JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallConstraint(
@@ -99,45 +117,59 @@ impl AnyJointVelocityConstraint {
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint(
pub fn from_wide_joint<Bodies>(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
joints: [&Joint; SIMD_WIDTH],
bodies: &RigidBodySet,
) -> Self {
let rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
let rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
bodies: &Bodies,
) -> Self
where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let rbs1 = (
gather![|ii| bodies.index(joints[ii].body1.0)],
gather![|ii| bodies.index(joints[ii].body1.0)],
gather![|ii| bodies.index(joints[ii].body1.0)],
gather![|ii| bodies.index(joints[ii].body1.0)],
);
let rbs2 = (
gather![|ii| bodies.index(joints[ii].body2.0)],
gather![|ii| bodies.index(joints[ii].body2.0)],
gather![|ii| bodies.index(joints[ii].body2.0)],
gather![|ii| bodies.index(joints[ii].body2.0)],
);
match &joints[0].params {
JointParams::BallJoint(_) => {
let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
AnyJointVelocityConstraint::WBallConstraint(WBallVelocityConstraint::from_params(
params, joint_id, rbs1, rbs2, joints,
))
}
JointParams::FixedJoint(_) => {
let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
AnyJointVelocityConstraint::WFixedConstraint(WFixedVelocityConstraint::from_params(
params, joint_id, rbs1, rbs2, joints,
))
}
// JointParams::GenericJoint(_) => {
// let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
// let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
// AnyJointVelocityConstraint::WGenericConstraint(
// WGenericVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
// )
// }
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
AnyJointVelocityConstraint::WPrismaticConstraint(
WPrismaticVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
)
}
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
AnyJointVelocityConstraint::WRevoluteConstraint(
WRevoluteVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
)
@@ -145,20 +177,31 @@ impl AnyJointVelocityConstraint {
}
}
pub fn from_joint_ground(
pub fn from_joint_ground<Bodies>(
params: &IntegrationParameters,
joint_id: JointIndex,
joint: &Joint,
bodies: &RigidBodySet,
) -> Self {
let mut rb1 = &bodies[joint.body1];
let mut rb2 = &bodies[joint.body2];
let flipped = !rb2.is_dynamic();
bodies: &Bodies,
) -> Self
where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let mut handle1 = joint.body1;
let mut handle2 = joint.body2;
let status2: &RigidBodyType = bodies.index(handle2.0);
let flipped = !status2.is_dynamic();
if flipped {
std::mem::swap(&mut rb1, &mut rb2);
std::mem::swap(&mut handle1, &mut handle2);
}
let rb1 = bodies.index_bundle(handle1.0);
let rb2 = bodies.index_bundle(handle2.0);
match &joint.params {
JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallGroundConstraint(
BallVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped),
@@ -186,26 +229,46 @@ impl AnyJointVelocityConstraint {
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint_ground(
pub fn from_wide_joint_ground<Bodies>(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
joints: [&Joint; SIMD_WIDTH],
bodies: &RigidBodySet,
) -> Self {
let mut rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
let mut rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
bodies: &Bodies,
) -> Self
where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let mut handles1 = gather![|ii| joints[ii].body1];
let mut handles2 = gather![|ii| joints[ii].body2];
let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
let mut flipped = [false; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH {
if !rbs2[ii].is_dynamic() {
std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
if !status2[ii].is_dynamic() {
std::mem::swap(&mut handles1[ii], &mut handles2[ii]);
flipped[ii] = true;
}
}
let rbs1 = (
gather![|ii| bodies.index(handles1[ii].0)],
gather![|ii| bodies.index(handles1[ii].0)],
gather![|ii| bodies.index(handles1[ii].0)],
);
let rbs2 = (
gather![|ii| bodies.index(handles2[ii].0)],
gather![|ii| bodies.index(handles2[ii].0)],
gather![|ii| bodies.index(handles2[ii].0)],
gather![|ii| bodies.index(handles2[ii].0)],
);
match &joints[0].params {
JointParams::BallJoint(_) => {
let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
AnyJointVelocityConstraint::WBallGroundConstraint(
WBallVelocityGroundConstraint::from_params(
params, joint_id, rbs1, rbs2, joints, flipped,
@@ -213,7 +276,7 @@ impl AnyJointVelocityConstraint {
)
}
JointParams::FixedJoint(_) => {
let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
AnyJointVelocityConstraint::WFixedGroundConstraint(
WFixedVelocityGroundConstraint::from_params(
params, joint_id, rbs1, rbs2, joints, flipped,
@@ -221,7 +284,7 @@ impl AnyJointVelocityConstraint {
)
}
// JointParams::GenericJoint(_) => {
// let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
// let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
// AnyJointVelocityConstraint::WGenericGroundConstraint(
// WGenericVelocityGroundConstraint::from_params(
// params, joint_id, rbs1, rbs2, joints, flipped,
@@ -229,8 +292,7 @@ impl AnyJointVelocityConstraint {
// )
// }
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
AnyJointVelocityConstraint::WPrismaticGroundConstraint(
WPrismaticVelocityGroundConstraint::from_params(
params, joint_id, rbs1, rbs2, joints, flipped,
@@ -239,8 +301,7 @@ impl AnyJointVelocityConstraint {
}
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
AnyJointVelocityConstraint::WRevoluteGroundConstraint(
WRevoluteVelocityGroundConstraint::from_params(
params, joint_id, rbs1, rbs2, joints, flipped,

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

@@ -13,7 +13,11 @@ use super::{
WFixedPositionGroundConstraint, WPrismaticPositionConstraint,
WPrismaticPositionGroundConstraint,
};
use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet};
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::{
IntegrationParameters, Joint, JointParams, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
RigidBodyType,
};
#[cfg(feature = "simd-is-enabled")]
use crate::math::SIMD_WIDTH;
use crate::math::{Isometry, Real};
@@ -56,9 +60,12 @@ pub(crate) enum AnyJointPositionConstraint {
}
impl AnyJointPositionConstraint {
pub fn from_joint(joint: &Joint, bodies: &RigidBodySet) -> Self {
let rb1 = &bodies[joint.body1];
let rb2 = &bodies[joint.body2];
pub fn from_joint<Bodies>(joint: &Joint, bodies: &Bodies) -> Self
where
Bodies: ComponentSet<RigidBodyMassProps> + ComponentSet<RigidBodyIds>,
{
let rb1 = bodies.index_bundle(joint.body1.0);
let rb2 = bodies.index_bundle(joint.body2.0);
match &joint.params {
JointParams::BallJoint(p) => AnyJointPositionConstraint::BallJoint(
@@ -81,40 +88,47 @@ impl AnyJointPositionConstraint {
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Self {
let rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
let rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
pub fn from_wide_joint<Bodies>(joints: [&Joint; SIMD_WIDTH], bodies: &Bodies) -> Self
where
Bodies: ComponentSet<RigidBodyMassProps> + ComponentSet<RigidBodyIds>,
{
let rbs1 = (
gather![|ii| bodies.index(joints[ii].body1.0)],
gather![|ii| bodies.index(joints[ii].body1.0)],
);
let rbs2 = (
gather![|ii| bodies.index(joints[ii].body2.0)],
gather![|ii| bodies.index(joints[ii].body2.0)],
);
match &joints[0].params {
JointParams::BallJoint(_) => {
let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
AnyJointPositionConstraint::WBallJoint(WBallPositionConstraint::from_params(
rbs1, rbs2, joints,
))
}
JointParams::FixedJoint(_) => {
let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
AnyJointPositionConstraint::WFixedJoint(WFixedPositionConstraint::from_params(
rbs1, rbs2, joints,
))
}
// JointParams::GenericJoint(_) => {
// let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
// let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
// AnyJointPositionConstraint::WGenericJoint(WGenericPositionConstraint::from_params(
// rbs1, rbs2, joints,
// ))
// }
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
AnyJointPositionConstraint::WPrismaticJoint(
WPrismaticPositionConstraint::from_params(rbs1, rbs2, joints),
)
}
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
AnyJointPositionConstraint::WRevoluteJoint(
WRevolutePositionConstraint::from_params(rbs1, rbs2, joints),
)
@@ -122,15 +136,26 @@ impl AnyJointPositionConstraint {
}
}
pub fn from_joint_ground(joint: &Joint, bodies: &RigidBodySet) -> Self {
let mut rb1 = &bodies[joint.body1];
let mut rb2 = &bodies[joint.body2];
let flipped = !rb2.is_dynamic();
pub fn from_joint_ground<Bodies>(joint: &Joint, bodies: &Bodies) -> Self
where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let mut handle1 = joint.body1;
let mut handle2 = joint.body2;
let status2: &RigidBodyType = bodies.index(handle2.0);
let flipped = !status2.is_dynamic();
if flipped {
std::mem::swap(&mut rb1, &mut rb2);
std::mem::swap(&mut handle1, &mut handle2);
}
let rb1 = bodies.index(handle1.0);
let rb2 = (bodies.index(handle2.0), bodies.index(handle2.0));
match &joint.params {
JointParams::BallJoint(p) => AnyJointPositionConstraint::BallGroundConstraint(
BallPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
@@ -154,48 +179,60 @@ impl AnyJointPositionConstraint {
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint_ground(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Self {
let mut rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
let mut rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
pub fn from_wide_joint_ground<Bodies>(joints: [&Joint; SIMD_WIDTH], bodies: &Bodies) -> Self
where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let mut handles1 = gather![|ii| joints[ii].body1];
let mut handles2 = gather![|ii| joints[ii].body2];
let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
let mut flipped = [false; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH {
if !rbs2[ii].is_dynamic() {
std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
if !status2[ii].is_dynamic() {
std::mem::swap(&mut handles1[ii], &mut handles2[ii]);
flipped[ii] = true;
}
}
let rbs1 = gather![|ii| bodies.index(handles1[ii].0)];
let rbs2 = (
gather![|ii| bodies.index(handles2[ii].0)],
gather![|ii| bodies.index(handles2[ii].0)],
);
match &joints[0].params {
JointParams::BallJoint(_) => {
let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
AnyJointPositionConstraint::WBallGroundConstraint(
WBallPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)
}
JointParams::FixedJoint(_) => {
let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
AnyJointPositionConstraint::WFixedGroundConstraint(
WFixedPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)
}
// JointParams::GenericJoint(_) => {
// let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
// let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
// AnyJointPositionConstraint::WGenericGroundConstraint(
// WGenericPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
// )
// }
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
AnyJointPositionConstraint::WPrismaticGroundConstraint(
WPrismaticPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)
}
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
AnyJointPositionConstraint::WRevoluteGroundConstraint(
WRevolutePositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)

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

@@ -1,4 +1,6 @@
use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody};
use crate::dynamics::{
IntegrationParameters, PrismaticJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector};
use crate::utils::WAngularInertia;
use na::Unit;
@@ -27,11 +29,18 @@ pub(crate) struct PrismaticPositionConstraint {
}
impl PrismaticPositionConstraint {
pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &PrismaticJoint) -> Self {
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
pub fn from_params(
rb1: (&RigidBodyMassProps, &RigidBodyIds),
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &PrismaticJoint,
) -> Self {
let (mprops1, ids1) = rb1;
let (mprops2, ids2) = rb2;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let lin_inv_lhs = 1.0 / (im1 + im2);
let ang_inv_lhs = (ii1 + ii2).inverse();
@@ -46,8 +55,8 @@ impl PrismaticPositionConstraint {
local_frame2: cparams.local_frame2(),
local_axis1: cparams.local_axis1,
local_axis2: cparams.local_axis2,
position1: rb1.active_set_offset,
position2: rb2.active_set_offset,
position1: ids1.active_set_offset,
position2: ids2.active_set_offset,
limits: cparams.limits,
}
}
@@ -108,25 +117,28 @@ pub(crate) struct PrismaticPositionGroundConstraint {
impl PrismaticPositionGroundConstraint {
pub fn from_params(
rb1: &RigidBody,
rb2: &RigidBody,
rb1: &RigidBodyPosition,
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &PrismaticJoint,
flipped: bool,
) -> Self {
let poss1 = rb1;
let (_, ids2) = rb2;
let frame1;
let local_frame2;
let axis1;
let local_axis2;
if flipped {
frame1 = rb1.next_position * cparams.local_frame2();
frame1 = poss1.next_position * cparams.local_frame2();
local_frame2 = cparams.local_frame1();
axis1 = rb1.next_position * cparams.local_axis2;
axis1 = poss1.next_position * cparams.local_axis2;
local_axis2 = cparams.local_axis1;
} else {
frame1 = rb1.next_position * cparams.local_frame1();
frame1 = poss1.next_position * cparams.local_frame1();
local_frame2 = cparams.local_frame2();
axis1 = rb1.next_position * cparams.local_axis1;
axis1 = poss1.next_position * cparams.local_axis1;
local_axis2 = cparams.local_axis2;
};
@@ -135,7 +147,7 @@ impl PrismaticPositionGroundConstraint {
local_frame2,
axis1,
local_axis2,
position2: rb2.active_set_offset,
position2: ids2.active_set_offset,
limits: cparams.limits,
}
}

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

@@ -1,5 +1,7 @@
use super::{PrismaticPositionConstraint, PrismaticPositionGroundConstraint};
use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody};
use crate::dynamics::{
IntegrationParameters, PrismaticJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
use crate::math::{Isometry, Real, SIMD_WIDTH};
// TODO: this does not uses SIMD optimizations yet.
@@ -10,12 +12,22 @@ pub(crate) struct WPrismaticPositionConstraint {
impl WPrismaticPositionConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&PrismaticJoint; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| PrismaticPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
constraints: gather![|ii| PrismaticPositionConstraint::from_params(
(rbs1.0[ii], rbs1.1[ii]),
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii]
)],
}
}
@@ -33,13 +45,21 @@ pub(crate) struct WPrismaticPositionGroundConstraint {
impl WPrismaticPositionGroundConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: [&RigidBodyPosition; SIMD_WIDTH],
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&PrismaticJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| PrismaticPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
constraints: gather![|ii| PrismaticPositionGroundConstraint::from_params(
rbs1[ii],
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii],
flipped[ii]
)],
}
}

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

@@ -1,6 +1,7 @@
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{AngularInertia, Real, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot};
@@ -74,32 +75,45 @@ impl PrismaticVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
joint: &PrismaticJoint,
) -> Self {
let (poss1, vels1, mprops1, ids1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
// Linear part.
let anchor1 = rb1.position * joint.local_anchor1;
let anchor2 = rb2.position * joint.local_anchor2;
let axis1 = rb1.position * joint.local_axis1;
let axis2 = rb2.position * joint.local_axis2;
let anchor1 = poss1.position * joint.local_anchor1;
let anchor2 = poss2.position * joint.local_anchor2;
let axis1 = poss1.position * joint.local_axis1;
let axis2 = poss2.position * joint.local_axis2;
#[cfg(feature = "dim2")]
let basis1 = rb1.position * joint.basis1[0];
let basis1 = poss1.position * joint.basis1[0];
#[cfg(feature = "dim3")]
let basis1 = Matrix3x2::from_columns(&[
rb1.position * joint.basis1[0],
rb1.position * joint.basis1[1],
poss1.position * joint.basis1[0],
poss1.position * joint.basis1[1],
]);
let im1 = rb1.effective_inv_mass;
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - rb1.world_com;
let im1 = mprops1.effective_inv_mass;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - mprops1.world_com;
let r1_mat = r1.gcross_matrix();
let im2 = rb2.effective_inv_mass;
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2 - rb2.world_com;
let im2 = mprops2.effective_inv_mass;
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2 - mprops2.world_com;
let r2_mat = r2.gcross_matrix();
#[allow(unused_mut)] // For 2D.
@@ -131,8 +145,8 @@ impl PrismaticVelocityConstraint {
lhs = SdpMatrix2::new(m11, m12, m22);
}
let anchor_linvel1 = rb1.linvel + rb1.angvel.gcross(r1);
let anchor_linvel2 = rb2.linvel + rb2.angvel.gcross(r2);
let anchor_linvel1 = vels1.linvel + vels1.angvel.gcross(r1);
let anchor_linvel2 = vels2.linvel + vels2.angvel.gcross(r2);
// NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
// for which a textbook inverse is still efficient.
@@ -142,7 +156,7 @@ impl PrismaticVelocityConstraint {
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
let angvel_err = rb2.angvel - rb1.angvel;
let angvel_err = vels2.angvel - vels1.angvel;
#[cfg(feature = "dim2")]
let mut rhs = Vector2::new(linvel_err.x, angvel_err) * params.velocity_solve_fraction;
@@ -159,8 +173,8 @@ impl PrismaticVelocityConstraint {
if velocity_based_erp_inv_dt != 0.0 {
let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
let frame1 = rb1.position * joint.local_frame1();
let frame2 = rb2.position * joint.local_frame2();
let frame1 = poss1.position * joint.local_frame1();
let frame2 = poss2.position * joint.local_frame2();
let ang_err = frame2.rotation * frame1.rotation.inverse();
#[cfg(feature = "dim2")]
@@ -195,9 +209,9 @@ impl PrismaticVelocityConstraint {
}
if damping != 0.0 {
let curr_vel = rb2.linvel.dot(&axis2) + rb2.angvel.gdot(gcross2)
- rb1.linvel.dot(&axis1)
- rb1.angvel.gdot(gcross1);
let curr_vel = vels2.linvel.dot(&axis2) + vels2.angvel.gdot(gcross2)
- vels1.linvel.dot(&axis1)
- vels1.angvel.gdot(gcross1);
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
@@ -266,12 +280,12 @@ impl PrismaticVelocityConstraint {
PrismaticVelocityConstraint {
joint_id,
mj_lambda1: rb1.active_set_offset,
mj_lambda2: rb2.active_set_offset,
mj_lambda1: ids1.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im1,
ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
im2,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
impulse: joint.impulse * params.warmstart_coeff,
limits_active,
limits_impulse: limits_impulse * params.warmstart_coeff,
@@ -501,11 +515,19 @@ impl PrismaticVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
joint: &PrismaticJoint,
flipped: bool,
) -> Self {
let (poss1, vels1, mprops1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let anchor2;
let anchor1;
let axis2;
@@ -513,35 +535,35 @@ impl PrismaticVelocityGroundConstraint {
let basis1;
if flipped {
anchor2 = rb2.position * joint.local_anchor1;
anchor1 = rb1.position * joint.local_anchor2;
axis2 = rb2.position * joint.local_axis1;
axis1 = rb1.position * joint.local_axis2;
anchor2 = poss2.position * joint.local_anchor1;
anchor1 = poss1.position * joint.local_anchor2;
axis2 = poss2.position * joint.local_axis1;
axis1 = poss1.position * joint.local_axis2;
#[cfg(feature = "dim2")]
{
basis1 = rb1.position * joint.basis2[0];
basis1 = poss1.position * joint.basis2[0];
}
#[cfg(feature = "dim3")]
{
basis1 = Matrix3x2::from_columns(&[
rb1.position * joint.basis2[0],
rb1.position * joint.basis2[1],
poss1.position * joint.basis2[0],
poss1.position * joint.basis2[1],
]);
}
} else {
anchor2 = rb2.position * joint.local_anchor2;
anchor1 = rb1.position * joint.local_anchor1;
axis2 = rb2.position * joint.local_axis2;
axis1 = rb1.position * joint.local_axis1;
anchor2 = poss2.position * joint.local_anchor2;
anchor1 = poss1.position * joint.local_anchor1;
axis2 = poss2.position * joint.local_axis2;
axis1 = poss1.position * joint.local_axis1;
#[cfg(feature = "dim2")]
{
basis1 = rb1.position * joint.basis1[0];
basis1 = poss1.position * joint.basis1[0];
}
#[cfg(feature = "dim3")]
{
basis1 = Matrix3x2::from_columns(&[
rb1.position * joint.basis1[0],
rb1.position * joint.basis1[1],
poss1.position * joint.basis1[0],
poss1.position * joint.basis1[1],
]);
}
};
@@ -560,10 +582,10 @@ impl PrismaticVelocityGroundConstraint {
// simplifications of the computation without introducing
// much instabilities.
let im2 = rb2.effective_inv_mass;
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - rb1.world_com;
let r2 = anchor2 - rb2.world_com;
let im2 = mprops2.effective_inv_mass;
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - mprops1.world_com;
let r2 = anchor2 - mprops2.world_com;
let r2_mat = r2.gcross_matrix();
#[allow(unused_mut)] // For 2D.
@@ -592,8 +614,8 @@ impl PrismaticVelocityGroundConstraint {
lhs = SdpMatrix2::new(m11, m12, m22);
}
let anchor_linvel1 = rb1.linvel + rb1.angvel.gcross(r1);
let anchor_linvel2 = rb2.linvel + rb2.angvel.gcross(r2);
let anchor_linvel1 = vels1.linvel + vels1.angvel.gcross(r1);
let anchor_linvel2 = vels2.linvel + vels2.angvel.gcross(r2);
// NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
// for which a textbook inverse is still efficient.
@@ -603,7 +625,7 @@ impl PrismaticVelocityGroundConstraint {
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
let angvel_err = rb2.angvel - rb1.angvel;
let angvel_err = vels2.angvel - vels1.angvel;
#[cfg(feature = "dim2")]
let mut rhs = Vector2::new(linvel_err.x, angvel_err) * params.velocity_solve_fraction;
@@ -622,11 +644,11 @@ impl PrismaticVelocityGroundConstraint {
let (frame1, frame2);
if flipped {
frame1 = rb1.position * joint.local_frame2();
frame2 = rb2.position * joint.local_frame1();
frame1 = poss1.position * joint.local_frame2();
frame2 = poss2.position * joint.local_frame1();
} else {
frame1 = rb1.position * joint.local_frame1();
frame2 = rb2.position * joint.local_frame2();
frame1 = poss1.position * joint.local_frame1();
frame2 = poss2.position * joint.local_frame2();
}
let ang_err = frame2.rotation * frame1.rotation.inverse();
@@ -660,7 +682,7 @@ impl PrismaticVelocityGroundConstraint {
}
if damping != 0.0 {
let curr_vel = rb2.linvel.dot(&axis2) - rb1.linvel.dot(&axis1);
let curr_vel = vels2.linvel.dot(&axis2) - vels1.linvel.dot(&axis1);
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
@@ -714,9 +736,9 @@ impl PrismaticVelocityGroundConstraint {
PrismaticVelocityGroundConstraint {
joint_id,
mj_lambda2: rb2.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im2,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
impulse: joint.impulse * params.warmstart_coeff,
limits_active,
limits_forcedir2,

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

@@ -2,7 +2,8 @@ use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH,
@@ -71,47 +72,60 @@ impl WPrismaticVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&PrismaticJoint; SIMD_WIDTH],
) -> 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::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let (poss1, vels1, mprops1, ids1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
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::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
let local_axis1 = Vector::from(array![|ii| *cparams[ii].local_axis1; SIMD_WIDTH]);
let local_axis2 = Vector::from(array![|ii| *cparams[ii].local_axis2; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]);
let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]);
let local_axis1 = Vector::from(gather![|ii| *cparams[ii].local_axis1]);
let local_axis2 = Vector::from(gather![|ii| *cparams[ii].local_axis2]);
#[cfg(feature = "dim2")]
let local_basis1 = [Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH])];
let local_basis1 = [Vector::from(gather![|ii| cparams[ii].basis1[0]])];
#[cfg(feature = "dim3")]
let local_basis1 = [
Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH]),
Vector::from(array![|ii| cparams[ii].basis1[1]; SIMD_WIDTH]),
Vector::from(gather![|ii| cparams[ii].basis1[0]]),
Vector::from(gather![|ii| cparams[ii].basis1[1]]),
];
#[cfg(feature = "dim2")]
let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let impulse = Vector2::from(gather![|ii| cparams[ii].impulse]);
#[cfg(feature = "dim3")]
let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let impulse = Vector5::from(gather![|ii| cparams[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
@@ -207,8 +221,8 @@ impl WPrismaticVelocityConstraint {
let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
let local_frame1 = Isometry::from(array![|ii| cparams[ii].local_frame1(); SIMD_WIDTH]);
let local_frame2 = Isometry::from(array![|ii| cparams[ii].local_frame2(); SIMD_WIDTH]);
let local_frame1 = Isometry::from(gather![|ii| cparams[ii].local_frame1()]);
let local_frame2 = Isometry::from(gather![|ii| cparams[ii].local_frame2()]);
let frame1 = position1 * local_frame1;
let frame2 = position2 * local_frame2;
@@ -221,8 +235,7 @@ impl WPrismaticVelocityConstraint {
#[cfg(feature = "dim3")]
{
let ang_err =
Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
* velocity_based_erp_inv_dt;
}
@@ -237,15 +250,15 @@ impl WPrismaticVelocityConstraint {
let mut limits_inv_lhs = zero;
let mut limits_impulse_limits = (zero, zero);
let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
let limits_enabled = SimdBool::from(gather![|ii| cparams[ii].limits_enabled]);
if limits_enabled.any() {
let danchor = anchor2 - anchor1;
let dist = danchor.dot(&axis1);
// TODO: we should allow predictive constraint activation.
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 min_limit = SimdReal::from(gather![|ii| cparams[ii].limits[0]]);
let max_limit = SimdReal::from(gather![|ii| cparams[ii].limits[1]]);
let min_enabled = dist.simd_lt(min_limit);
let max_enabled = dist.simd_gt(max_limit);
@@ -265,10 +278,9 @@ impl WPrismaticVelocityConstraint {
- (min_limit - dist).simd_max(zero))
* SimdReal::splat(velocity_based_erp_inv_dt);
limits_impulse =
SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH])
.simd_max(limits_impulse_limits.0)
.simd_min(limits_impulse_limits.1);
limits_impulse = SimdReal::from(gather![|ii| cparams[ii].limits_impulse])
.simd_max(limits_impulse_limits.0)
.simd_min(limits_impulse_limits.1);
limits_inv_lhs = SimdReal::splat(1.0)
/ (im1
@@ -303,20 +315,16 @@ impl WPrismaticVelocityConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
@@ -428,20 +436,16 @@ impl WPrismaticVelocityConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
@@ -510,59 +514,85 @@ impl WPrismaticVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&PrismaticJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> 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::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let (poss1, vels1, mprops1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
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::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
#[cfg(feature = "dim2")]
let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let impulse = Vector2::from(gather![|ii| cparams[ii].impulse]);
#[cfg(feature = "dim3")]
let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let impulse = Vector5::from(gather![|ii| cparams[ii].impulse]);
let local_anchor1 = Point::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
);
let local_anchor2 = Point::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
);
let local_axis1 = Vector::from(
array![|ii| if flipped[ii] { *cparams[ii].local_axis2 } else { *cparams[ii].local_axis1 }; SIMD_WIDTH],
);
let local_axis2 = Vector::from(
array![|ii| if flipped[ii] { *cparams[ii].local_axis1 } else { *cparams[ii].local_axis2 }; SIMD_WIDTH],
);
let local_anchor1 = Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor2
} else {
cparams[ii].local_anchor1
}]);
let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor1
} else {
cparams[ii].local_anchor2
}]);
let local_axis1 = Vector::from(gather![|ii| if flipped[ii] {
*cparams[ii].local_axis2
} else {
*cparams[ii].local_axis1
}]);
let local_axis2 = Vector::from(gather![|ii| if flipped[ii] {
*cparams[ii].local_axis1
} else {
*cparams[ii].local_axis2
}]);
#[cfg(feature = "dim2")]
let basis1 = position1
* Vector::from(
array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
);
* Vector::from(gather![|ii| if flipped[ii] {
cparams[ii].basis2[0]
} else {
cparams[ii].basis1[0]
}]);
#[cfg(feature = "dim3")]
let basis1 = Matrix3x2::from_columns(&[
position1
* Vector::from(
array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
),
* Vector::from(gather![|ii| if flipped[ii] {
cparams[ii].basis2[0]
} else {
cparams[ii].basis1[0]
}]),
position1
* Vector::from(
array![|ii| if flipped[ii] { cparams[ii].basis2[1] } else { cparams[ii].basis1[1] }; SIMD_WIDTH],
),
* Vector::from(gather![|ii| if flipped[ii] {
cparams[ii].basis2[1]
} else {
cparams[ii].basis1[1]
}]),
]);
let anchor1 = position1 * local_anchor1;
@@ -634,13 +664,17 @@ impl WPrismaticVelocityGroundConstraint {
let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
let frame1 = position1
* Isometry::from(
array![|ii| if flipped[ii] { cparams[ii].local_frame2() } else { cparams[ii].local_frame1() }; SIMD_WIDTH],
);
* Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_frame2()
} else {
cparams[ii].local_frame1()
}]);
let frame2 = position2
* Isometry::from(
array![|ii| if flipped[ii] { cparams[ii].local_frame1() } else { cparams[ii].local_frame2() }; SIMD_WIDTH],
);
* Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_frame1()
} else {
cparams[ii].local_frame2()
}]);
let ang_err = frame2.rotation * frame1.rotation.inverse();
@@ -651,8 +685,7 @@ impl WPrismaticVelocityGroundConstraint {
#[cfg(feature = "dim3")]
{
let ang_err =
Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
* velocity_based_erp_inv_dt;
}
@@ -666,14 +699,14 @@ impl WPrismaticVelocityGroundConstraint {
let mut limits_impulse = zero;
let mut limits_impulse_limits = (zero, zero);
let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
let limits_enabled = SimdBool::from(gather![|ii| cparams[ii].limits_enabled]);
if limits_enabled.any() {
let danchor = anchor2 - anchor1;
let dist = danchor.dot(&axis1);
// TODO: we should allow predictive constraint activation.
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 min_limit = SimdReal::from(gather![|ii| cparams[ii].limits[0]]);
let max_limit = SimdReal::from(gather![|ii| cparams[ii].limits[1]]);
let min_enabled = dist.simd_lt(min_limit);
let max_enabled = dist.simd_gt(max_limit);
@@ -690,10 +723,9 @@ impl WPrismaticVelocityGroundConstraint {
- (min_limit - dist).simd_max(zero))
* SimdReal::splat(velocity_based_erp_inv_dt);
limits_impulse =
SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH])
.simd_max(limits_impulse_limits.0)
.simd_min(limits_impulse_limits.1);
limits_impulse = SimdReal::from(gather![|ii| cparams[ii].limits_impulse])
.simd_max(limits_impulse_limits.0)
.simd_min(limits_impulse_limits.1);
}
}
@@ -718,12 +750,10 @@ impl WPrismaticVelocityGroundConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
@@ -791,12 +821,10 @@ impl WPrismaticVelocityGroundConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
self.solve_dofs(&mut mj_lambda2);

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

@@ -1,4 +1,6 @@
use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody};
use crate::dynamics::{
IntegrationParameters, RevoluteJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use na::Unit;
@@ -29,11 +31,18 @@ pub(crate) struct RevolutePositionConstraint {
}
impl RevolutePositionConstraint {
pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &RevoluteJoint) -> Self {
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
pub fn from_params(
rb1: (&RigidBodyMassProps, &RigidBodyIds),
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &RevoluteJoint,
) -> Self {
let (mprops1, ids1) = rb1;
let (mprops2, ids2) = rb2;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let ang_inv_lhs = (ii1 + ii2).inverse();
Self {
@@ -42,14 +51,14 @@ impl RevolutePositionConstraint {
ii1,
ii2,
ang_inv_lhs,
local_com1: rb1.mass_properties.local_com,
local_com2: rb2.mass_properties.local_com,
local_com1: mprops1.mass_properties.local_com,
local_com2: mprops2.mass_properties.local_com,
local_anchor1: cparams.local_anchor1,
local_anchor2: cparams.local_anchor2,
local_axis1: cparams.local_axis1,
local_axis2: cparams.local_axis2,
position1: rb1.active_set_offset,
position2: rb2.active_set_offset,
position1: ids1.active_set_offset,
position2: ids2.active_set_offset,
local_basis1: cparams.basis1,
local_basis2: cparams.basis2,
}
@@ -132,11 +141,14 @@ pub(crate) struct RevolutePositionGroundConstraint {
impl RevolutePositionGroundConstraint {
pub fn from_params(
rb1: &RigidBody,
rb2: &RigidBody,
rb1: &RigidBodyPosition,
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &RevoluteJoint,
flipped: bool,
) -> Self {
let poss1 = rb1;
let (mprops2, ids2) = rb2;
let anchor1;
let local_anchor2;
let axis1;
@@ -145,23 +157,23 @@ impl RevolutePositionGroundConstraint {
let local_basis2;
if flipped {
anchor1 = rb1.next_position * cparams.local_anchor2;
anchor1 = poss1.next_position * cparams.local_anchor2;
local_anchor2 = cparams.local_anchor1;
axis1 = rb1.next_position * cparams.local_axis2;
axis1 = poss1.next_position * cparams.local_axis2;
local_axis2 = cparams.local_axis1;
basis1 = [
rb1.next_position * cparams.basis2[0],
rb1.next_position * cparams.basis2[1],
poss1.next_position * cparams.basis2[0],
poss1.next_position * cparams.basis2[1],
];
local_basis2 = cparams.basis1;
} else {
anchor1 = rb1.next_position * cparams.local_anchor1;
anchor1 = poss1.next_position * cparams.local_anchor1;
local_anchor2 = cparams.local_anchor2;
axis1 = rb1.next_position * cparams.local_axis1;
axis1 = poss1.next_position * cparams.local_axis1;
local_axis2 = cparams.local_axis2;
basis1 = [
rb1.next_position * cparams.basis1[0],
rb1.next_position * cparams.basis1[1],
poss1.next_position * cparams.basis1[0],
poss1.next_position * cparams.basis1[1],
];
local_basis2 = cparams.basis2;
};
@@ -169,12 +181,12 @@ impl RevolutePositionGroundConstraint {
Self {
anchor1,
local_anchor2,
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
local_com2: rb2.mass_properties.local_com,
im2: mprops2.effective_inv_mass,
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
local_com2: mprops2.mass_properties.local_com,
axis1,
local_axis2,
position2: rb2.active_set_offset,
position2: ids2.active_set_offset,
basis1,
local_basis2,
}

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

@@ -1,5 +1,7 @@
use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody};
use crate::dynamics::{
IntegrationParameters, RevoluteJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
use crate::math::{Isometry, Real, SIMD_WIDTH};
// TODO: this does not uses SIMD optimizations yet.
@@ -10,12 +12,22 @@ pub(crate) struct WRevolutePositionConstraint {
impl WRevolutePositionConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&RevoluteJoint; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| RevolutePositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
constraints: gather![|ii| RevolutePositionConstraint::from_params(
(rbs1.0[ii], rbs1.1[ii]),
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii]
)],
}
}
@@ -33,13 +45,21 @@ pub(crate) struct WRevolutePositionGroundConstraint {
impl WRevolutePositionGroundConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: [&RigidBodyPosition; SIMD_WIDTH],
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&RevoluteJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| RevolutePositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
constraints: gather![|ii| RevolutePositionGroundConstraint::from_params(
rbs1[ii],
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii],
flipped[ii]
)],
}
}

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

@@ -1,6 +1,7 @@
use crate::dynamics::solver::{AnyJointVelocityConstraint, DeltaVel};
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{AngularInertia, Real, Rotation, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
@@ -43,34 +44,47 @@ impl RevoluteVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
joint: &RevoluteJoint,
) -> Self {
let (poss1, vels1, mprops1, ids1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
// Linear part.
let anchor1 = rb1.position * joint.local_anchor1;
let anchor2 = rb2.position * joint.local_anchor2;
let anchor1 = poss1.position * joint.local_anchor1;
let anchor2 = poss2.position * joint.local_anchor2;
let basis1 = Matrix3x2::from_columns(&[
rb1.position * joint.basis1[0],
rb1.position * joint.basis1[1],
poss1.position * joint.basis1[0],
poss1.position * joint.basis1[1],
]);
let basis2 = Matrix3x2::from_columns(&[
rb2.position * joint.basis2[0],
rb2.position * joint.basis2[1],
poss2.position * joint.basis2[0],
poss2.position * joint.basis2[1],
]);
let basis_projection2 = basis2 * basis2.transpose();
let basis2 = basis_projection2 * basis1;
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - rb1.world_com;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - mprops1.world_com;
let r1_mat = r1.gcross_matrix();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2 - rb2.world_com;
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2 - mprops2.world_com;
let r2_mat = r2.gcross_matrix();
let mut lhs = Matrix5::zeros();
@@ -90,8 +104,8 @@ impl RevoluteVelocityConstraint {
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
let linvel_err =
(rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
let angvel_err = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
(vels2.linvel + vels2.angvel.gcross(r2)) - (vels1.linvel + vels1.angvel.gcross(r1));
let angvel_err = basis2.tr_mul(&vels2.angvel) - basis1.tr_mul(&vels1.angvel);
let mut rhs = Vector5::new(
linvel_err.x,
@@ -105,8 +119,8 @@ impl RevoluteVelocityConstraint {
if velocity_based_erp_inv_dt != 0.0 {
let lin_err = anchor2 - anchor1;
let axis1 = rb1.position * joint.local_axis1;
let axis2 = rb2.position * joint.local_axis2;
let axis1 = poss1.position * joint.local_axis1;
let axis2 = poss2.position * joint.local_axis2;
let axis_error = axis1.cross(&axis2);
let ang_err = (basis2.tr_mul(&axis_error) + basis1.tr_mul(&axis_error)) * 0.5;
@@ -118,8 +132,8 @@ impl RevoluteVelocityConstraint {
/*
* Motor.
*/
let motor_axis1 = rb1.position * *joint.local_axis1;
let motor_axis2 = rb2.position * *joint.local_axis2;
let motor_axis1 = poss1.position * *joint.local_axis1;
let motor_axis2 = poss2.position * *joint.local_axis2;
let mut motor_rhs = 0.0;
let mut motor_inv_lhs = 0.0;
let mut motor_angle = 0.0;
@@ -132,12 +146,12 @@ impl RevoluteVelocityConstraint {
);
if stiffness != 0.0 {
motor_angle = joint.estimate_motor_angle(&rb1.position, &rb2.position);
motor_angle = joint.estimate_motor_angle(&poss1.position, &poss2.position);
motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness;
}
if damping != 0.0 {
let curr_vel = rb2.angvel.dot(&motor_axis2) - rb1.angvel.dot(&motor_axis1);
let curr_vel = vels2.angvel.dot(&motor_axis2) - vels1.angvel.dot(&motor_axis1);
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
@@ -171,14 +185,14 @@ impl RevoluteVelocityConstraint {
RevoluteVelocityConstraint {
joint_id,
mj_lambda1: rb1.active_set_offset,
mj_lambda2: rb2.active_set_offset,
mj_lambda1: ids1.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im1,
ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
basis1,
basis2,
im2,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
impulse,
inv_lhs,
rhs,
@@ -330,11 +344,19 @@ impl RevoluteVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
joint: &RevoluteJoint,
flipped: bool,
) -> AnyJointVelocityConstraint {
let (poss1, vels1, mprops1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let anchor2;
let anchor1;
let axis1;
@@ -343,39 +365,39 @@ impl RevoluteVelocityGroundConstraint {
let basis2;
if flipped {
axis1 = rb1.position * *joint.local_axis2;
axis2 = rb2.position * *joint.local_axis1;
anchor1 = rb1.position * joint.local_anchor2;
anchor2 = rb2.position * joint.local_anchor1;
axis1 = poss1.position * *joint.local_axis2;
axis2 = poss2.position * *joint.local_axis1;
anchor1 = poss1.position * joint.local_anchor2;
anchor2 = poss2.position * joint.local_anchor1;
basis1 = Matrix3x2::from_columns(&[
rb1.position * joint.basis2[0],
rb1.position * joint.basis2[1],
poss1.position * joint.basis2[0],
poss1.position * joint.basis2[1],
]);
basis2 = Matrix3x2::from_columns(&[
rb2.position * joint.basis1[0],
rb2.position * joint.basis1[1],
poss2.position * joint.basis1[0],
poss2.position * joint.basis1[1],
]);
} else {
axis1 = rb1.position * *joint.local_axis1;
axis2 = rb2.position * *joint.local_axis2;
anchor1 = rb1.position * joint.local_anchor1;
anchor2 = rb2.position * joint.local_anchor2;
axis1 = poss1.position * *joint.local_axis1;
axis2 = poss2.position * *joint.local_axis2;
anchor1 = poss1.position * joint.local_anchor1;
anchor2 = poss2.position * joint.local_anchor2;
basis1 = Matrix3x2::from_columns(&[
rb1.position * joint.basis1[0],
rb1.position * joint.basis1[1],
poss1.position * joint.basis1[0],
poss1.position * joint.basis1[1],
]);
basis2 = Matrix3x2::from_columns(&[
rb2.position * joint.basis2[0],
rb2.position * joint.basis2[1],
poss2.position * joint.basis2[0],
poss2.position * joint.basis2[1],
]);
};
let basis_projection2 = basis2 * basis2.transpose();
let basis2 = basis_projection2 * basis1;
let im2 = rb2.effective_inv_mass;
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - rb1.world_com;
let r2 = anchor2 - rb2.world_com;
let im2 = mprops2.effective_inv_mass;
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - mprops1.world_com;
let r2 = anchor2 - mprops2.world_com;
let r2_mat = r2.gcross_matrix();
let mut lhs = Matrix5::zeros();
@@ -393,8 +415,8 @@ impl RevoluteVelocityGroundConstraint {
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
let linvel_err =
(rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
let angvel_err = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
(vels2.linvel + vels2.angvel.gcross(r2)) - (vels1.linvel + vels1.angvel.gcross(r1));
let angvel_err = basis2.tr_mul(&vels2.angvel) - basis1.tr_mul(&vels1.angvel);
let mut rhs = Vector5::new(
linvel_err.x,
linvel_err.y,
@@ -409,11 +431,11 @@ impl RevoluteVelocityGroundConstraint {
let (axis1, axis2);
if flipped {
axis1 = rb1.position * joint.local_axis2;
axis2 = rb2.position * joint.local_axis1;
axis1 = poss1.position * joint.local_axis2;
axis2 = poss2.position * joint.local_axis1;
} else {
axis1 = rb1.position * joint.local_axis1;
axis2 = rb2.position * joint.local_axis2;
axis1 = poss1.position * joint.local_axis1;
axis2 = poss2.position * joint.local_axis2;
}
let axis_error = axis1.cross(&axis2);
let ang_err = basis2.tr_mul(&axis_error);
@@ -437,12 +459,12 @@ impl RevoluteVelocityGroundConstraint {
);
if stiffness != 0.0 {
motor_angle = joint.estimate_motor_angle(&rb1.position, &rb2.position);
motor_angle = joint.estimate_motor_angle(&poss1.position, &poss2.position);
motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness;
}
if damping != 0.0 {
let curr_vel = rb2.angvel.dot(&axis2) - rb1.angvel.dot(&axis1);
let curr_vel = vels2.angvel.dot(&axis2) - vels1.angvel.dot(&axis1);
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
@@ -460,9 +482,9 @@ impl RevoluteVelocityGroundConstraint {
let result = RevoluteVelocityGroundConstraint {
joint_id,
mj_lambda2: rb2.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im2,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
impulse: joint.impulse * params.warmstart_coeff,
basis2,
inv_lhs,

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

@@ -2,7 +2,8 @@ use simba::simd::SimdValue;
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Vector, SIMD_WIDTH,
@@ -39,41 +40,54 @@ impl WRevoluteVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
joints: [&RevoluteJoint; SIMD_WIDTH],
) -> 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::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let (poss1, vels1, mprops1, ids1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
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::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
let local_anchor1 = Point::from(array![|ii| joints[ii].local_anchor1; SIMD_WIDTH]);
let local_anchor2 = Point::from(array![|ii| joints[ii].local_anchor2; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor1 = Point::from(gather![|ii| joints[ii].local_anchor1]);
let local_anchor2 = Point::from(gather![|ii| joints[ii].local_anchor2]);
let local_basis1 = [
Vector::from(array![|ii| joints[ii].basis1[0]; SIMD_WIDTH]),
Vector::from(array![|ii| joints[ii].basis1[1]; SIMD_WIDTH]),
Vector::from(gather![|ii| joints[ii].basis1[0]]),
Vector::from(gather![|ii| joints[ii].basis1[1]]),
];
let local_basis2 = [
Vector::from(array![|ii| joints[ii].basis2[0]; SIMD_WIDTH]),
Vector::from(array![|ii| joints[ii].basis2[1]; SIMD_WIDTH]),
Vector::from(gather![|ii| joints[ii].basis2[0]]),
Vector::from(gather![|ii| joints[ii].basis2[1]]),
];
let impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]);
let impulse = Vector5::from(gather![|ii| joints[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
@@ -124,10 +138,8 @@ impl WRevoluteVelocityConstraint {
let lin_err = anchor2 - anchor1;
let local_axis1 =
Unit::<Vector<_>>::from(array![|ii| joints[ii].local_axis1; SIMD_WIDTH]);
let local_axis2 =
Unit::<Vector<_>>::from(array![|ii| joints[ii].local_axis2; SIMD_WIDTH]);
let local_axis1 = Unit::<Vector<_>>::from(gather![|ii| joints[ii].local_axis1]);
let local_axis2 = Unit::<Vector<_>>::from(gather![|ii| joints[ii].local_axis2]);
let axis1 = position1 * local_axis1;
let axis2 = position2 * local_axis2;
@@ -150,12 +162,12 @@ impl WRevoluteVelocityConstraint {
let warmstart_coeff = SimdReal::splat(params.warmstart_coeff);
let mut impulse = impulse * warmstart_coeff;
let axis1 = array![|ii| rbs1[ii].position * *joints[ii].local_axis1; SIMD_WIDTH];
let rotated_impulse = Vector::from(array![|ii| {
let axis1 = gather![|ii| poss1[ii].position * *joints[ii].local_axis1];
let rotated_impulse = Vector::from(gather![|ii| {
let axis_rot = Rotation::rotation_between(&joints[ii].prev_axis1, &axis1[ii])
.unwrap_or_else(Rotation::identity);
axis_rot * joints[ii].world_ang_impulse
}; SIMD_WIDTH]);
}]);
let rotated_basis_impulse = basis1.tr_mul(&rotated_impulse);
impulse[3] = rotated_basis_impulse.x * warmstart_coeff;
@@ -182,20 +194,16 @@ impl WRevoluteVelocityConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse1 = self.impulse.fixed_rows::<3>(0).into_owned();
@@ -225,20 +233,16 @@ impl WRevoluteVelocityConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
@@ -314,52 +318,76 @@ impl WRevoluteVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
joints: [&RevoluteJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> 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::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let (poss1, vels1, mprops1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
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::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]);
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let local_anchor1 = Point::from(
array![|ii| if flipped[ii] { joints[ii].local_anchor2 } else { joints[ii].local_anchor1 }; SIMD_WIDTH],
);
let local_anchor2 = Point::from(
array![|ii| if flipped[ii] { joints[ii].local_anchor1 } else { joints[ii].local_anchor2 }; SIMD_WIDTH],
);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let impulse = Vector5::from(gather![|ii| joints[ii].impulse]);
let local_anchor1 = Point::from(gather![|ii| if flipped[ii] {
joints[ii].local_anchor2
} else {
joints[ii].local_anchor1
}]);
let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
joints[ii].local_anchor1
} else {
joints[ii].local_anchor2
}]);
let basis1 = Matrix3x2::from_columns(&[
position1
* Vector::from(
array![|ii| if flipped[ii] { joints[ii].basis2[0] } else { joints[ii].basis1[0] }; SIMD_WIDTH],
),
* Vector::from(gather![|ii| if flipped[ii] {
joints[ii].basis2[0]
} else {
joints[ii].basis1[0]
}]),
position1
* Vector::from(
array![|ii| if flipped[ii] { joints[ii].basis2[1] } else { joints[ii].basis1[1] }; SIMD_WIDTH],
),
* Vector::from(gather![|ii| if flipped[ii] {
joints[ii].basis2[1]
} else {
joints[ii].basis1[1]
}]),
]);
let basis2 = Matrix3x2::from_columns(&[
position2
* Vector::from(
array![|ii| if flipped[ii] { joints[ii].basis1[0] } else { joints[ii].basis2[0] }; SIMD_WIDTH],
),
* Vector::from(gather![|ii| if flipped[ii] {
joints[ii].basis1[0]
} else {
joints[ii].basis2[0]
}]),
position2
* Vector::from(
array![|ii| if flipped[ii] { joints[ii].basis1[1] } else { joints[ii].basis2[1] }; SIMD_WIDTH],
),
* Vector::from(gather![|ii| if flipped[ii] {
joints[ii].basis1[1]
} else {
joints[ii].basis2[1]
}]),
]);
let basis_projection2 = basis2 * basis2.transpose();
let basis2 = basis_projection2 * basis1;
@@ -403,12 +431,16 @@ impl WRevoluteVelocityGroundConstraint {
let lin_err = anchor2 - anchor1;
let local_axis1 = Unit::<Vector<_>>::from(
array![|ii| if flipped[ii] { joints[ii].local_axis2 } else { joints[ii].local_axis1 }; SIMD_WIDTH],
);
let local_axis2 = Unit::<Vector<_>>::from(
array![|ii| if flipped[ii] { joints[ii].local_axis1 } else { joints[ii].local_axis2 }; SIMD_WIDTH],
);
let local_axis1 = Unit::<Vector<_>>::from(gather![|ii| if flipped[ii] {
joints[ii].local_axis2
} else {
joints[ii].local_axis1
}]);
let local_axis2 = Unit::<Vector<_>>::from(gather![|ii| if flipped[ii] {
joints[ii].local_axis1
} else {
joints[ii].local_axis2
}]);
let axis1 = position1 * local_axis1;
let axis2 = position2 * local_axis2;
@@ -434,12 +466,10 @@ impl WRevoluteVelocityGroundConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.impulse.fixed_rows::<3>(0).into_owned();
@@ -458,12 +488,10 @@ impl WRevoluteVelocityGroundConstraint {
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],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);