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

53
src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs Normal file
View File

@@ -0,0 +1,53 @@
use super::{FixedPositionConstraint, FixedPositionGroundConstraint};
use crate::dynamics::{FixedJoint, IntegrationParameters, JointIndex, RigidBody};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector, SIMD_WIDTH};
use crate::utils::WAngularInertia;
use na::Unit;
// TODO: this does not uses SIMD optimizations yet.
#[derive(Debug)]
pub(crate) struct WFixedPositionConstraint {
constraints: [FixedPositionConstraint; SIMD_WIDTH],
}
impl WFixedPositionConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
cparams: [&FixedJoint; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| FixedPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
for constraint in &self.constraints {
constraint.solve(params, positions);
}
}
}
#[derive(Debug)]
pub(crate) struct WFixedPositionGroundConstraint {
constraints: [FixedPositionGroundConstraint; SIMD_WIDTH],
}
impl WFixedPositionGroundConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; 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],
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
for constraint in &self.constraints {
constraint.solve(params, positions);
}
}
}

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!(),
}
}

24
src/dynamics/solver/joint_constraint/mod.rs
View File

@@ -9,6 +9,10 @@ pub(self) use ball_velocity_constraint_wide::{
WBallVelocityConstraint, WBallVelocityGroundConstraint,
};
pub(self) use fixed_position_constraint::{FixedPositionConstraint, FixedPositionGroundConstraint};
#[cfg(feature = "simd-is-enabled")]
pub(self) use fixed_position_constraint_wide::{
WFixedPositionConstraint, WFixedPositionGroundConstraint,
};
pub(self) use fixed_velocity_constraint::{FixedVelocityConstraint, FixedVelocityGroundConstraint};
#[cfg(feature = "simd-is-enabled")]
pub(self) use fixed_velocity_constraint_wide::{
@@ -19,6 +23,10 @@ pub(crate) use joint_position_constraint::AnyJointPositionConstraint;
pub(self) use prismatic_position_constraint::{
PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
};
#[cfg(feature = "simd-is-enabled")]
pub(self) use prismatic_position_constraint_wide::{
WPrismaticPositionConstraint, WPrismaticPositionGroundConstraint,
};
pub(self) use prismatic_velocity_constraint::{
PrismaticVelocityConstraint, PrismaticVelocityGroundConstraint,
};
@@ -30,12 +38,15 @@ pub(self) use prismatic_velocity_constraint_wide::{
pub(self) use revolute_position_constraint::{
RevolutePositionConstraint, RevolutePositionGroundConstraint,
};
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
pub(self) use revolute_position_constraint_wide::{
WRevolutePositionConstraint, WRevolutePositionGroundConstraint,
};
#[cfg(feature = "dim3")]
pub(self) use revolute_velocity_constraint::{
RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint,
};
#[cfg(feature = "dim3")]
#[cfg(feature = "simd-is-enabled")]
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
pub(self) use revolute_velocity_constraint_wide::{
WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint,
};
@@ -47,19 +58,24 @@ mod ball_velocity_constraint;
#[cfg(feature = "simd-is-enabled")]
mod ball_velocity_constraint_wide;
mod fixed_position_constraint;
#[cfg(feature = "simd-is-enabled")]
mod fixed_position_constraint_wide;
mod fixed_velocity_constraint;
#[cfg(feature = "simd-is-enabled")]
mod fixed_velocity_constraint_wide;
mod joint_constraint;
mod joint_position_constraint;
mod prismatic_position_constraint;
#[cfg(feature = "simd-is-enabled")]
mod prismatic_position_constraint_wide;
mod prismatic_velocity_constraint;
#[cfg(feature = "simd-is-enabled")]
mod prismatic_velocity_constraint_wide;
#[cfg(feature = "dim3")]
mod revolute_position_constraint;
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
mod revolute_position_constraint_wide;
#[cfg(feature = "dim3")]
mod revolute_velocity_constraint;
#[cfg(feature = "dim3")]
#[cfg(feature = "simd-is-enabled")]
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
mod revolute_velocity_constraint_wide;

53
src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs Normal file
View File

@@ -0,0 +1,53 @@
use super::{PrismaticPositionConstraint, PrismaticPositionGroundConstraint};
use crate::dynamics::{IntegrationParameters, JointIndex, PrismaticJoint, RigidBody};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector, SIMD_WIDTH};
use crate::utils::WAngularInertia;
use na::Unit;
// TODO: this does not uses SIMD optimizations yet.
#[derive(Debug)]
pub(crate) struct WPrismaticPositionConstraint {
constraints: [PrismaticPositionConstraint; SIMD_WIDTH],
}
impl WPrismaticPositionConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
cparams: [&PrismaticJoint; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| PrismaticPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
for constraint in &self.constraints {
constraint.solve(params, positions);
}
}
}
#[derive(Debug)]
pub(crate) struct WPrismaticPositionGroundConstraint {
constraints: [PrismaticPositionGroundConstraint; SIMD_WIDTH],
}
impl WPrismaticPositionGroundConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; 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],
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
for constraint in &self.constraints {
constraint.solve(params, positions);
}
}
}

53
src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs Normal file
View File

@@ -0,0 +1,53 @@
use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
use crate::dynamics::{IntegrationParameters, JointIndex, RevoluteJoint, RigidBody};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector, SIMD_WIDTH};
use crate::utils::WAngularInertia;
use na::Unit;
// TODO: this does not uses SIMD optimizations yet.
#[derive(Debug)]
pub(crate) struct WRevolutePositionConstraint {
constraints: [RevolutePositionConstraint; SIMD_WIDTH],
}
impl WRevolutePositionConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
cparams: [&RevoluteJoint; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| RevolutePositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
for constraint in &self.constraints {
constraint.solve(params, positions);
}
}
}
#[derive(Debug)]
pub(crate) struct WRevolutePositionGroundConstraint {
constraints: [RevolutePositionGroundConstraint; SIMD_WIDTH],
}
impl WRevolutePositionGroundConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; 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],
}
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
for constraint in &self.constraints {
constraint.solve(params, positions);
}
}
}