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

Refactor the parallel solver code the same way we did with the non-parallel solver.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-01-04 17:59:51 +01:00
parent aa61fe65e3
commit c28b14d31c
14 changed files with 691 additions and 873 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -4,8 +4,15 @@ use super::{
};
#[cfg(feature = "dim3")]
use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
use super::{WRevolutePositionConstraint, WRevolutePositionGroundConstraint};
#[cfg(feature = "simd-is-enabled")]
use super::{WBallPositionConstraint, WBallPositionGroundConstraint};
use super::{
WBallPositionConstraint, WBallPositionGroundConstraint, WFixedPositionConstraint,
WFixedPositionGroundConstraint, WPrismaticPositionConstraint,
WPrismaticPositionGroundConstraint,
};
use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet};
#[cfg(feature = "simd-is-enabled")]
use crate::math::SIMD_WIDTH;
@@ -20,12 +27,24 @@ pub(crate) enum AnyJointPositionConstraint {
WBallGroundConstraint(WBallPositionGroundConstraint),
FixedJoint(FixedPositionConstraint),
FixedGroundConstraint(FixedPositionGroundConstraint),
#[cfg(feature = "simd-is-enabled")]
WFixedJoint(WFixedPositionConstraint),
#[cfg(feature = "simd-is-enabled")]
WFixedGroundConstraint(WFixedPositionGroundConstraint),
PrismaticJoint(PrismaticPositionConstraint),
PrismaticGroundConstraint(PrismaticPositionGroundConstraint),
#[cfg(feature = "simd-is-enabled")]
WPrismaticJoint(WPrismaticPositionConstraint),
#[cfg(feature = "simd-is-enabled")]
WPrismaticGroundConstraint(WPrismaticPositionGroundConstraint),
#[cfg(feature = "dim3")]
RevoluteJoint(RevolutePositionConstraint),
#[cfg(feature = "dim3")]
RevoluteGroundConstraint(RevolutePositionGroundConstraint),
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
WRevoluteJoint(WRevolutePositionConstraint),
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
WRevoluteGroundConstraint(WRevolutePositionGroundConstraint),
#[allow(dead_code)] // The Empty variant is only used with parallel code.
Empty,
}
@@ -71,21 +90,38 @@ impl AnyJointPositionConstraint {
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Option<Self> {
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];
match &joints[0].params {
JointParams::BallJoint(_) => {
let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
Some(AnyJointPositionConstraint::WBallJoint(
WBallPositionConstraint::from_params(rbs1, rbs2, joints),
AnyJointPositionConstraint::WBallJoint(WBallPositionConstraint::from_params(
rbs1, rbs2, joints,
))
}
JointParams::FixedJoint(_) => None,
JointParams::PrismaticJoint(_) => None,
JointParams::FixedJoint(_) => {
let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
AnyJointPositionConstraint::WFixedJoint(WFixedPositionConstraint::from_params(
rbs1, rbs2, joints,
))
}
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
AnyJointPositionConstraint::WPrismaticJoint(
WPrismaticPositionConstraint::from_params(rbs1, rbs2, joints),
)
}
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(_) => None,
JointParams::RevoluteJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
AnyJointPositionConstraint::WRevoluteJoint(
WRevolutePositionConstraint::from_params(rbs1, rbs2, joints),
)
}
}
}
@@ -118,10 +154,7 @@ impl AnyJointPositionConstraint {
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint_ground(
joints: [&Joint; SIMD_WIDTH],
bodies: &RigidBodySet,
) -> Option<Self> {
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];
let mut flipped = [false; SIMD_WIDTH];
@@ -136,14 +169,31 @@ impl AnyJointPositionConstraint {
match &joints[0].params {
JointParams::BallJoint(_) => {
let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
Some(AnyJointPositionConstraint::WBallGroundConstraint(
AnyJointPositionConstraint::WBallGroundConstraint(
WBallPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
))
)
}
JointParams::FixedJoint(_) => {
let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
AnyJointPositionConstraint::WFixedGroundConstraint(
WFixedPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)
}
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
AnyJointPositionConstraint::WPrismaticGroundConstraint(
WPrismaticPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)
}
JointParams::FixedJoint(_) => None,
JointParams::PrismaticJoint(_) => None,
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(_) => None,
JointParams::RevoluteJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
AnyJointPositionConstraint::WRevoluteGroundConstraint(
WRevolutePositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)
}
}
}
@@ -157,12 +207,24 @@ impl AnyJointPositionConstraint {
AnyJointPositionConstraint::WBallGroundConstraint(c) => c.solve(params, positions),
AnyJointPositionConstraint::FixedJoint(c) => c.solve(params, positions),
AnyJointPositionConstraint::FixedGroundConstraint(c) => c.solve(params, positions),
#[cfg(feature = "simd-is-enabled")]
AnyJointPositionConstraint::WFixedJoint(c) => c.solve(params, positions),
#[cfg(feature = "simd-is-enabled")]
AnyJointPositionConstraint::WFixedGroundConstraint(c) => c.solve(params, positions),
AnyJointPositionConstraint::PrismaticJoint(c) => c.solve(params, positions),
AnyJointPositionConstraint::PrismaticGroundConstraint(c) => c.solve(params, positions),
#[cfg(feature = "simd-is-enabled")]
AnyJointPositionConstraint::WPrismaticJoint(c) => c.solve(params, positions),
#[cfg(feature = "simd-is-enabled")]
AnyJointPositionConstraint::WPrismaticGroundConstraint(c) => c.solve(params, positions),
#[cfg(feature = "dim3")]
AnyJointPositionConstraint::RevoluteJoint(c) => c.solve(params, positions),
#[cfg(feature = "dim3")]
AnyJointPositionConstraint::RevoluteGroundConstraint(c) => c.solve(params, positions),
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
AnyJointPositionConstraint::WRevoluteJoint(c) => c.solve(params, positions),
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
AnyJointPositionConstraint::WRevoluteGroundConstraint(c) => c.solve(params, positions),
AnyJointPositionConstraint::Empty => unreachable!(),
}
}