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

Complete the implementation of non-simd joint motor for the revolute joint.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-02-19 15:21:25 +01:00
parent a1ddda5077
commit e9f17f32e8
14 changed files with 483 additions and 343 deletions
Download Patch File Download Diff File Expand all files Collapse all files

84
src/dynamics/solver/joint_constraint/joint_constraint.rs
View File

@@ -14,9 +14,9 @@ use super::{
#[cfg(feature = "dim3")]
#[cfg(feature = "simd-is-enabled")]
use super::{WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint};
use crate::dynamics::solver::joint_constraint::generic_velocity_constraint::{
GenericVelocityConstraint, GenericVelocityGroundConstraint,
};
// use crate::dynamics::solver::joint_constraint::generic_velocity_constraint::{
// GenericVelocityConstraint, GenericVelocityGroundConstraint,
// };
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet,
@@ -38,12 +38,12 @@ pub(crate) enum AnyJointVelocityConstraint {
WFixedConstraint(WFixedVelocityConstraint),
#[cfg(feature = "simd-is-enabled")]
WFixedGroundConstraint(WFixedVelocityGroundConstraint),
GenericConstraint(GenericVelocityConstraint),
GenericGroundConstraint(GenericVelocityGroundConstraint),
#[cfg(feature = "simd-is-enabled")]
WGenericConstraint(WGenericVelocityConstraint),
#[cfg(feature = "simd-is-enabled")]
WGenericGroundConstraint(WGenericVelocityGroundConstraint),
// GenericConstraint(GenericVelocityConstraint),
// GenericGroundConstraint(GenericVelocityGroundConstraint),
// #[cfg(feature = "simd-is-enabled")]
// WGenericConstraint(WGenericVelocityConstraint),
// #[cfg(feature = "simd-is-enabled")]
// WGenericGroundConstraint(WGenericVelocityGroundConstraint),
PrismaticConstraint(PrismaticVelocityConstraint),
PrismaticGroundConstraint(PrismaticVelocityGroundConstraint),
#[cfg(feature = "simd-is-enabled")]
@@ -89,9 +89,9 @@ impl AnyJointVelocityConstraint {
JointParams::PrismaticJoint(p) => AnyJointVelocityConstraint::PrismaticConstraint(
PrismaticVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
),
JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericConstraint(
GenericVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
),
// JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericConstraint(
// GenericVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
// ),
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteConstraint(
RevoluteVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
@@ -167,11 +167,11 @@ impl AnyJointVelocityConstraint {
JointParams::FixedJoint(p) => AnyJointVelocityConstraint::FixedGroundConstraint(
FixedVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped),
),
JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericGroundConstraint(
GenericVelocityGroundConstraint::from_params(
params, joint_id, rb1, rb2, p, flipped,
),
),
// JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericGroundConstraint(
// GenericVelocityGroundConstraint::from_params(
// params, joint_id, rb1, rb2, p, flipped,
// ),
// ),
JointParams::PrismaticJoint(p) => {
AnyJointVelocityConstraint::PrismaticGroundConstraint(
PrismaticVelocityGroundConstraint::from_params(
@@ -180,10 +180,8 @@ impl AnyJointVelocityConstraint {
)
}
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteGroundConstraint(
RevoluteVelocityGroundConstraint::from_params(
params, joint_id, rb1, rb2, p, flipped,
),
JointParams::RevoluteJoint(p) => RevoluteVelocityGroundConstraint::from_params(
params, joint_id, rb1, rb2, p, flipped,
),
}
}
@@ -267,12 +265,12 @@ impl AnyJointVelocityConstraint {
AnyJointVelocityConstraint::WFixedConstraint(c) => c.warmstart(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.warmstart(mj_lambdas),
AnyJointVelocityConstraint::GenericConstraint(c) => c.warmstart(mj_lambdas),
AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.warmstart(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WGenericConstraint(c) => c.warmstart(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.warmstart(mj_lambdas),
// AnyJointVelocityConstraint::GenericConstraint(c) => c.warmstart(mj_lambdas),
// AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.warmstart(mj_lambdas),
// #[cfg(feature = "simd-is-enabled")]
// AnyJointVelocityConstraint::WGenericConstraint(c) => c.warmstart(mj_lambdas),
// #[cfg(feature = "simd-is-enabled")]
// AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.warmstart(mj_lambdas),
AnyJointVelocityConstraint::PrismaticConstraint(c) => c.warmstart(mj_lambdas),
AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.warmstart(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
@@ -307,12 +305,12 @@ impl AnyJointVelocityConstraint {
AnyJointVelocityConstraint::WFixedConstraint(c) => c.solve(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.solve(mj_lambdas),
AnyJointVelocityConstraint::GenericConstraint(c) => c.solve(mj_lambdas),
AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.solve(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WGenericConstraint(c) => c.solve(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.solve(mj_lambdas),
// AnyJointVelocityConstraint::GenericConstraint(c) => c.solve(mj_lambdas),
// AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.solve(mj_lambdas),
// #[cfg(feature = "simd-is-enabled")]
// AnyJointVelocityConstraint::WGenericConstraint(c) => c.solve(mj_lambdas),
// #[cfg(feature = "simd-is-enabled")]
// AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.solve(mj_lambdas),
AnyJointVelocityConstraint::PrismaticConstraint(c) => c.solve(mj_lambdas),
AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.solve(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
@@ -355,16 +353,16 @@ impl AnyJointVelocityConstraint {
AnyJointVelocityConstraint::WFixedGroundConstraint(c) => {
c.writeback_impulses(joints_all)
}
AnyJointVelocityConstraint::GenericConstraint(c) => c.writeback_impulses(joints_all),
AnyJointVelocityConstraint::GenericGroundConstraint(c) => {
c.writeback_impulses(joints_all)
}
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WGenericConstraint(c) => c.writeback_impulses(joints_all),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WGenericGroundConstraint(c) => {
c.writeback_impulses(joints_all)
}
// AnyJointVelocityConstraint::GenericConstraint(c) => c.writeback_impulses(joints_all),
// AnyJointVelocityConstraint::GenericGroundConstraint(c) => {
// c.writeback_impulses(joints_all)
// }
// #[cfg(feature = "simd-is-enabled")]
// AnyJointVelocityConstraint::WGenericConstraint(c) => c.writeback_impulses(joints_all),
// #[cfg(feature = "simd-is-enabled")]
// AnyJointVelocityConstraint::WGenericGroundConstraint(c) => {
// c.writeback_impulses(joints_all)
// }
AnyJointVelocityConstraint::PrismaticConstraint(c) => c.writeback_impulses(joints_all),
AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => {
c.writeback_impulses(joints_all)