1#![allow(clippy::bad_bit_mask)] use crate::dynamics::solver::MotorParameters;
4use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RopeJoint};
5use crate::math::{Isometry, Point, Real, Rotation, SPATIAL_DIM, UnitVector, Vector};
6use crate::utils::{SimdBasis, SimdRealCopy};
7
8#[cfg(feature = "dim3")]
9use crate::dynamics::SphericalJoint;
10
11#[cfg(feature = "dim3")]
12bitflags::bitflags! {
13 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
15 #[derive(Copy, Clone, PartialEq, Eq, Debug)]
16 pub struct JointAxesMask: u8 {
17 const LIN_X = 1 << 0;
19 const LIN_Y = 1 << 1;
21 const LIN_Z = 1 << 2;
23 const ANG_X = 1 << 3;
25 const ANG_Y = 1 << 4;
27 const ANG_Z = 1 << 5;
29 const LOCKED_REVOLUTE_AXES = Self::LIN_X.bits() | Self::LIN_Y.bits() | Self::LIN_Z.bits() | Self::ANG_Y.bits() | Self::ANG_Z.bits();
31 const LOCKED_PRISMATIC_AXES = Self::LIN_Y.bits() | Self::LIN_Z.bits() | Self::ANG_X.bits() | Self::ANG_Y.bits() | Self::ANG_Z.bits();
33 const LOCKED_FIXED_AXES = Self::LIN_X.bits() | Self::LIN_Y.bits() | Self::LIN_Z.bits() | Self::ANG_X.bits() | Self::ANG_Y.bits() | Self::ANG_Z.bits();
35 const LOCKED_SPHERICAL_AXES = Self::LIN_X.bits() | Self::LIN_Y.bits() | Self::LIN_Z.bits();
37 const FREE_REVOLUTE_AXES = Self::ANG_X.bits();
39 const FREE_PRISMATIC_AXES = Self::LIN_X.bits();
41 const FREE_FIXED_AXES = 0;
43 const FREE_SPHERICAL_AXES = Self::ANG_X.bits() | Self::ANG_Y.bits() | Self::ANG_Z.bits();
45 const LIN_AXES = Self::LIN_X.bits() | Self::LIN_Y.bits() | Self::LIN_Z.bits();
47 const ANG_AXES = Self::ANG_X.bits() | Self::ANG_Y.bits() | Self::ANG_Z.bits();
49 }
50}
51
52#[cfg(feature = "dim2")]
53bitflags::bitflags! {
54 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
56 #[derive(Copy, Clone, PartialEq, Eq, Debug)]
57 pub struct JointAxesMask: u8 {
58 const LIN_X = 1 << 0;
60 const LIN_Y = 1 << 1;
62 const ANG_X = 1 << 2;
64 const LOCKED_REVOLUTE_AXES = Self::LIN_X.bits() | Self::LIN_Y.bits();
66 const LOCKED_PRISMATIC_AXES = Self::LIN_Y.bits() | Self::ANG_X.bits();
68 const LOCKED_FIXED_AXES = Self::LIN_X.bits() | Self::LIN_Y.bits() | Self::ANG_X.bits();
70 const FREE_REVOLUTE_AXES = Self::ANG_X.bits();
72 const FREE_PRISMATIC_AXES = Self::LIN_X.bits();
74 const FREE_FIXED_AXES = 0;
76 const LIN_AXES = Self::LIN_X.bits() | Self::LIN_Y.bits();
78 const ANG_AXES = Self::ANG_X.bits();
80 }
81}
82
83impl Default for JointAxesMask {
84 fn default() -> Self {
85 Self::empty()
86 }
87}
88
89#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
91#[derive(Copy, Clone, Debug, PartialEq)]
92pub enum JointAxis {
93 LinX = 0,
95 LinY,
97 #[cfg(feature = "dim3")]
99 LinZ,
100 AngX,
102 #[cfg(feature = "dim3")]
104 AngY,
105 #[cfg(feature = "dim3")]
107 AngZ,
108}
109
110impl From<JointAxis> for JointAxesMask {
111 fn from(axis: JointAxis) -> Self {
112 JointAxesMask::from_bits(1 << axis as usize).unwrap()
113 }
114}
115
116#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
118#[derive(Copy, Clone, Debug, PartialEq)]
119pub struct JointLimits<N> {
120 pub min: N,
122 pub max: N,
124 pub impulse: N,
126}
127
128impl<N: SimdRealCopy> Default for JointLimits<N> {
129 fn default() -> Self {
130 Self {
131 min: -N::splat(Real::MAX),
132 max: N::splat(Real::MAX),
133 impulse: N::splat(0.0),
134 }
135 }
136}
137
138impl<N: SimdRealCopy> From<[N; 2]> for JointLimits<N> {
139 fn from(value: [N; 2]) -> Self {
140 Self {
141 min: value[0],
142 max: value[1],
143 impulse: N::splat(0.0),
144 }
145 }
146}
147
148#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
150#[derive(Copy, Clone, Debug, PartialEq)]
151pub struct JointMotor {
152 pub target_vel: Real,
154 pub target_pos: Real,
156 pub stiffness: Real,
158 pub damping: Real,
160 pub max_force: Real,
162 pub impulse: Real,
164 pub model: MotorModel,
166}
167
168impl Default for JointMotor {
169 fn default() -> Self {
170 Self {
171 target_pos: 0.0,
172 target_vel: 0.0,
173 stiffness: 0.0,
174 damping: 0.0,
175 max_force: Real::MAX,
176 impulse: 0.0,
177 model: MotorModel::AccelerationBased,
178 }
179 }
180}
181
182impl JointMotor {
183 pub(crate) fn motor_params(&self, dt: Real) -> MotorParameters<Real> {
184 let (erp_inv_dt, cfm_coeff, cfm_gain) =
185 self.model
186 .combine_coefficients(dt, self.stiffness, self.damping);
187 MotorParameters {
188 erp_inv_dt,
189 cfm_coeff,
190 cfm_gain,
191 target_pos: self.target_pos,
193 target_vel: self.target_vel,
194 max_impulse: self.max_force * dt,
195 }
196 }
197}
198
199#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
200#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
201pub enum JointEnabled {
203 Enabled,
205 DisabledByAttachedBody,
208 Disabled,
210}
211
212#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
213#[derive(Copy, Clone, Debug, PartialEq)]
214pub struct GenericJoint {
216 pub local_frame1: Isometry<Real>,
218 pub local_frame2: Isometry<Real>,
220 pub locked_axes: JointAxesMask,
222 pub limit_axes: JointAxesMask,
224 pub motor_axes: JointAxesMask,
226 pub coupled_axes: JointAxesMask,
233 pub limits: [JointLimits<Real>; SPATIAL_DIM],
239 pub motors: [JointMotor; SPATIAL_DIM],
245 pub contacts_enabled: bool,
247 pub enabled: JointEnabled,
249 pub user_data: u128,
251}
252
253impl Default for GenericJoint {
254 fn default() -> Self {
255 Self {
256 local_frame1: Isometry::identity(),
257 local_frame2: Isometry::identity(),
258 locked_axes: JointAxesMask::empty(),
259 limit_axes: JointAxesMask::empty(),
260 motor_axes: JointAxesMask::empty(),
261 coupled_axes: JointAxesMask::empty(),
262 limits: [JointLimits::default(); SPATIAL_DIM],
263 motors: [JointMotor::default(); SPATIAL_DIM],
264 contacts_enabled: true,
265 enabled: JointEnabled::Enabled,
266 user_data: 0,
267 }
268 }
269}
270
271impl GenericJoint {
272 #[must_use]
274 pub fn new(locked_axes: JointAxesMask) -> Self {
275 *Self::default().lock_axes(locked_axes)
276 }
277
278 #[cfg(feature = "simd-is-enabled")]
279 pub(crate) fn supports_simd_constraints(&self) -> bool {
281 self.limit_axes.is_empty() && self.motor_axes.is_empty()
282 }
283
284 #[doc(hidden)]
285 pub fn complete_ang_frame(axis: UnitVector<Real>) -> Rotation<Real> {
286 let basis = axis.orthonormal_basis();
287
288 #[cfg(feature = "dim2")]
289 {
290 use na::{Matrix2, Rotation2, UnitComplex};
291 let mat = Matrix2::from_columns(&[axis.into_inner(), basis[0]]);
292 let rotmat = Rotation2::from_matrix_unchecked(mat);
293 UnitComplex::from_rotation_matrix(&rotmat)
294 }
295
296 #[cfg(feature = "dim3")]
297 {
298 use na::{Matrix3, Rotation3, UnitQuaternion};
299 let mat = Matrix3::from_columns(&[axis.into_inner(), basis[0], basis[1]]);
300 let rotmat = Rotation3::from_matrix_unchecked(mat);
301 UnitQuaternion::from_rotation_matrix(&rotmat)
302 }
303 }
304
305 pub fn is_enabled(&self) -> bool {
307 self.enabled == JointEnabled::Enabled
308 }
309
310 pub fn set_enabled(&mut self, enabled: bool) {
312 match self.enabled {
313 JointEnabled::Enabled | JointEnabled::DisabledByAttachedBody => {
314 if !enabled {
315 self.enabled = JointEnabled::Disabled;
316 }
317 }
318 JointEnabled::Disabled => {
319 if enabled {
320 self.enabled = JointEnabled::Enabled;
321 }
322 }
323 }
324 }
325
326 pub fn lock_axes(&mut self, axes: JointAxesMask) -> &mut Self {
328 self.locked_axes |= axes;
329 self
330 }
331
332 pub fn set_local_frame1(&mut self, local_frame: Isometry<Real>) -> &mut Self {
334 self.local_frame1 = local_frame;
335 self
336 }
337
338 pub fn set_local_frame2(&mut self, local_frame: Isometry<Real>) -> &mut Self {
340 self.local_frame2 = local_frame;
341 self
342 }
343
344 #[must_use]
346 pub fn local_axis1(&self) -> UnitVector<Real> {
347 self.local_frame1 * Vector::x_axis()
348 }
349
350 pub fn set_local_axis1(&mut self, local_axis: UnitVector<Real>) -> &mut Self {
352 self.local_frame1.rotation = Self::complete_ang_frame(local_axis);
353 self
354 }
355
356 #[must_use]
358 pub fn local_axis2(&self) -> UnitVector<Real> {
359 self.local_frame2 * Vector::x_axis()
360 }
361
362 pub fn set_local_axis2(&mut self, local_axis: UnitVector<Real>) -> &mut Self {
364 self.local_frame2.rotation = Self::complete_ang_frame(local_axis);
365 self
366 }
367
368 #[must_use]
370 pub fn local_anchor1(&self) -> Point<Real> {
371 self.local_frame1.translation.vector.into()
372 }
373
374 pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
376 self.local_frame1.translation.vector = anchor1.coords;
377 self
378 }
379
380 #[must_use]
382 pub fn local_anchor2(&self) -> Point<Real> {
383 self.local_frame2.translation.vector.into()
384 }
385
386 pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
388 self.local_frame2.translation.vector = anchor2.coords;
389 self
390 }
391
392 pub fn contacts_enabled(&self) -> bool {
394 self.contacts_enabled
395 }
396
397 pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self {
399 self.contacts_enabled = enabled;
400 self
401 }
402
403 #[must_use]
405 pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits<Real>> {
406 let i = axis as usize;
407 if self.limit_axes.contains(axis.into()) {
408 Some(&self.limits[i])
409 } else {
410 None
411 }
412 }
413
414 pub fn set_limits(&mut self, axis: JointAxis, limits: [Real; 2]) -> &mut Self {
416 let i = axis as usize;
417 self.limit_axes |= axis.into();
418 self.limits[i].min = limits[0];
419 self.limits[i].max = limits[1];
420 self
421 }
422
423 #[must_use]
425 pub fn motor_model(&self, axis: JointAxis) -> Option<MotorModel> {
426 let i = axis as usize;
427 if self.motor_axes.contains(axis.into()) {
428 Some(self.motors[i].model)
429 } else {
430 None
431 }
432 }
433
434 pub fn set_motor_model(&mut self, axis: JointAxis, model: MotorModel) -> &mut Self {
436 self.motors[axis as usize].model = model;
437 self
438 }
439
440 pub fn set_motor_velocity(
442 &mut self,
443 axis: JointAxis,
444 target_vel: Real,
445 factor: Real,
446 ) -> &mut Self {
447 self.set_motor(
448 axis,
449 self.motors[axis as usize].target_pos,
450 target_vel,
451 0.0,
452 factor,
453 )
454 }
455
456 pub fn set_motor_position(
458 &mut self,
459 axis: JointAxis,
460 target_pos: Real,
461 stiffness: Real,
462 damping: Real,
463 ) -> &mut Self {
464 self.set_motor(axis, target_pos, 0.0, stiffness, damping)
465 }
466
467 pub fn set_motor_max_force(&mut self, axis: JointAxis, max_force: Real) -> &mut Self {
469 self.motors[axis as usize].max_force = max_force;
470 self
471 }
472
473 #[must_use]
475 pub fn motor(&self, axis: JointAxis) -> Option<&JointMotor> {
476 let i = axis as usize;
477 if self.motor_axes.contains(axis.into()) {
478 Some(&self.motors[i])
479 } else {
480 None
481 }
482 }
483
484 pub fn set_motor(
486 &mut self,
487 axis: JointAxis,
488 target_pos: Real,
489 target_vel: Real,
490 stiffness: Real,
491 damping: Real,
492 ) -> &mut Self {
493 self.motor_axes |= axis.into();
494 let i = axis as usize;
495 self.motors[i].target_vel = target_vel;
496 self.motors[i].target_pos = target_pos;
497 self.motors[i].stiffness = stiffness;
498 self.motors[i].damping = damping;
499 self
500 }
501
502 pub fn flip(&mut self) {
504 std::mem::swap(&mut self.local_frame1, &mut self.local_frame2);
505
506 let coupled_bits = self.coupled_axes.bits();
507
508 for dim in 0..SPATIAL_DIM {
509 if coupled_bits & (1 << dim) == 0 {
510 let limit = self.limits[dim];
511 self.limits[dim].min = -limit.max;
512 self.limits[dim].max = -limit.min;
513 }
514
515 self.motors[dim].target_vel = -self.motors[dim].target_vel;
516 self.motors[dim].target_pos = -self.motors[dim].target_pos;
517 }
518 }
519}
520
521macro_rules! joint_conversion_methods(
522 ($as_joint: ident, $as_joint_mut: ident, $Joint: ty, $axes: expr) => {
523 #[must_use]
525 pub fn $as_joint(&self) -> Option<&$Joint> {
526 if self.locked_axes == $axes {
527 Some(unsafe { std::mem::transmute::<&Self, &$Joint>(self) })
530 } else {
531 None
532 }
533 }
534
535 #[must_use]
537 pub fn $as_joint_mut(&mut self) -> Option<&mut $Joint> {
538 if self.locked_axes == $axes {
539 Some(unsafe { std::mem::transmute::<&mut Self, &mut $Joint>(self) })
542 } else {
543 None
544 }
545 }
546 }
547);
548
549impl GenericJoint {
550 joint_conversion_methods!(
551 as_revolute,
552 as_revolute_mut,
553 RevoluteJoint,
554 JointAxesMask::LOCKED_REVOLUTE_AXES
555 );
556 joint_conversion_methods!(
557 as_fixed,
558 as_fixed_mut,
559 FixedJoint,
560 JointAxesMask::LOCKED_FIXED_AXES
561 );
562 joint_conversion_methods!(
563 as_prismatic,
564 as_prismatic_mut,
565 PrismaticJoint,
566 JointAxesMask::LOCKED_PRISMATIC_AXES
567 );
568 joint_conversion_methods!(
569 as_rope,
570 as_rope_mut,
571 RopeJoint,
572 JointAxesMask::FREE_FIXED_AXES
573 );
574
575 #[cfg(feature = "dim3")]
576 joint_conversion_methods!(
577 as_spherical,
578 as_spherical_mut,
579 SphericalJoint,
580 JointAxesMask::LOCKED_SPHERICAL_AXES
581 );
582}
583
584#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
586#[derive(Copy, Clone, Debug, PartialEq)]
587pub struct GenericJointBuilder(pub GenericJoint);
588
589impl GenericJointBuilder {
590 #[must_use]
592 pub fn new(locked_axes: JointAxesMask) -> Self {
593 Self(GenericJoint::new(locked_axes))
594 }
595
596 #[must_use]
598 pub fn locked_axes(mut self, axes: JointAxesMask) -> Self {
599 self.0.locked_axes = axes;
600 self
601 }
602
603 #[must_use]
605 pub fn contacts_enabled(mut self, enabled: bool) -> Self {
606 self.0.contacts_enabled = enabled;
607 self
608 }
609
610 #[must_use]
612 pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
613 self.0.set_local_frame1(local_frame);
614 self
615 }
616
617 #[must_use]
619 pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
620 self.0.set_local_frame2(local_frame);
621 self
622 }
623
624 #[must_use]
626 pub fn local_axis1(mut self, local_axis: UnitVector<Real>) -> Self {
627 self.0.set_local_axis1(local_axis);
628 self
629 }
630
631 #[must_use]
633 pub fn local_axis2(mut self, local_axis: UnitVector<Real>) -> Self {
634 self.0.set_local_axis2(local_axis);
635 self
636 }
637
638 #[must_use]
640 pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
641 self.0.set_local_anchor1(anchor1);
642 self
643 }
644
645 #[must_use]
647 pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
648 self.0.set_local_anchor2(anchor2);
649 self
650 }
651
652 #[must_use]
654 pub fn limits(mut self, axis: JointAxis, limits: [Real; 2]) -> Self {
655 self.0.set_limits(axis, limits);
656 self
657 }
658
659 #[must_use]
661 pub fn coupled_axes(mut self, axes: JointAxesMask) -> Self {
662 self.0.coupled_axes = axes;
663 self
664 }
665
666 #[must_use]
668 pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self {
669 self.0.set_motor_model(axis, model);
670 self
671 }
672
673 #[must_use]
675 pub fn motor_velocity(mut self, axis: JointAxis, target_vel: Real, factor: Real) -> Self {
676 self.0.set_motor_velocity(axis, target_vel, factor);
677 self
678 }
679
680 #[must_use]
682 pub fn motor_position(
683 mut self,
684 axis: JointAxis,
685 target_pos: Real,
686 stiffness: Real,
687 damping: Real,
688 ) -> Self {
689 self.0
690 .set_motor_position(axis, target_pos, stiffness, damping);
691 self
692 }
693
694 #[must_use]
696 pub fn set_motor(
697 mut self,
698 axis: JointAxis,
699 target_pos: Real,
700 target_vel: Real,
701 stiffness: Real,
702 damping: Real,
703 ) -> Self {
704 self.0
705 .set_motor(axis, target_pos, target_vel, stiffness, damping);
706 self
707 }
708
709 #[must_use]
711 pub fn motor_max_force(mut self, axis: JointAxis, max_force: Real) -> Self {
712 self.0.set_motor_max_force(axis, max_force);
713 self
714 }
715
716 pub fn user_data(mut self, data: u128) -> Self {
718 self.0.user_data = data;
719 self
720 }
721
722 #[must_use]
724 pub fn build(self) -> GenericJoint {
725 self.0
726 }
727}
728
729impl From<GenericJointBuilder> for GenericJoint {
730 fn from(val: GenericJointBuilder) -> GenericJoint {
731 val.0
732 }
733}