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

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),
)