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

Start experimenting with a generic joint implementation for joint drives.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-02-10 11:56:51 +01:00
parent 3be8669206
commit 5b80c4efbf
14 changed files with 1350 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,6 +1,7 @@
use super::{
BallPositionConstraint, BallPositionGroundConstraint, FixedPositionConstraint,
FixedPositionGroundConstraint, PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
FixedPositionGroundConstraint, GenericPositionConstraint, GenericPositionGroundConstraint,
PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
};
#[cfg(feature = "dim3")]
use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
@@ -10,8 +11,8 @@ use super::{WRevolutePositionConstraint, WRevolutePositionGroundConstraint};
#[cfg(feature = "simd-is-enabled")]
use super::{
WBallPositionConstraint, WBallPositionGroundConstraint, WFixedPositionConstraint,
WFixedPositionGroundConstraint, WPrismaticPositionConstraint,
WPrismaticPositionGroundConstraint,
WFixedPositionGroundConstraint, WGenericPositionConstraint, WGenericPositionGroundConstraint,
WPrismaticPositionConstraint, WPrismaticPositionGroundConstraint,
};
use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet};
#[cfg(feature = "simd-is-enabled")]
@@ -31,6 +32,12 @@ pub(crate) enum AnyJointPositionConstraint {
WFixedJoint(WFixedPositionConstraint),
#[cfg(feature = "simd-is-enabled")]
WFixedGroundConstraint(WFixedPositionGroundConstraint),
GenericJoint(GenericPositionConstraint),
GenericGroundConstraint(GenericPositionGroundConstraint),
#[cfg(feature = "simd-is-enabled")]
WGenericJoint(WGenericPositionConstraint),
#[cfg(feature = "simd-is-enabled")]
WGenericGroundConstraint(WGenericPositionGroundConstraint),
PrismaticJoint(PrismaticPositionConstraint),
PrismaticGroundConstraint(PrismaticPositionGroundConstraint),
#[cfg(feature = "simd-is-enabled")]
@@ -61,6 +68,9 @@ impl AnyJointPositionConstraint {
JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedJoint(
FixedPositionConstraint::from_params(rb1, rb2, p),
),
JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericJoint(
GenericPositionConstraint::from_params(rb1, rb2, p),
),
JointParams::PrismaticJoint(p) => AnyJointPositionConstraint::PrismaticJoint(
PrismaticPositionConstraint::from_params(rb1, rb2, p),
),
@@ -89,6 +99,12 @@ impl AnyJointPositionConstraint {
rbs1, rbs2, joints,
))
}
JointParams::GenericJoint(_) => {
let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
AnyJointPositionConstraint::WGenericJoint(WGenericPositionConstraint::from_params(
rbs1, rbs2, joints,
))
}
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
@@ -123,6 +139,9 @@ impl AnyJointPositionConstraint {
JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedGroundConstraint(
FixedPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
),
JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericGroundConstraint(
GenericPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
),
JointParams::PrismaticJoint(p) => {
AnyJointPositionConstraint::PrismaticGroundConstraint(
PrismaticPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
@@ -161,6 +180,12 @@ impl AnyJointPositionConstraint {
WFixedPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)
}
JointParams::GenericJoint(_) => {
let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
AnyJointPositionConstraint::WGenericGroundConstraint(
WGenericPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)
}
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
@@ -193,6 +218,12 @@ impl AnyJointPositionConstraint {
AnyJointPositionConstraint::WFixedJoint(c) => c.solve(params, positions),
#[cfg(feature = "simd-is-enabled")]
AnyJointPositionConstraint::WFixedGroundConstraint(c) => c.solve(params, positions),
AnyJointPositionConstraint::GenericJoint(c) => c.solve(params, positions),
AnyJointPositionConstraint::GenericGroundConstraint(c) => c.solve(params, positions),
#[cfg(feature = "simd-is-enabled")]
AnyJointPositionConstraint::WGenericJoint(c) => c.solve(params, positions),
#[cfg(feature = "simd-is-enabled")]
AnyJointPositionConstraint::WGenericGroundConstraint(c) => c.solve(params, positions),
AnyJointPositionConstraint::PrismaticJoint(c) => c.solve(params, positions),
AnyJointPositionConstraint::PrismaticGroundConstraint(c) => c.solve(params, positions),
#[cfg(feature = "simd-is-enabled")]