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

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,