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

Fix warnings and add comments.

Browse Source
This commit is contained in:
Sébastien Crozet
2022-03-19 16:10:49 +01:00
committed by Sébastien Crozet
parent e2e6fc7871
commit db6a8c526d
23 changed files with 391 additions and 131 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
src/dynamics/joint/fixed_joint.rs
View File

@@ -4,6 +4,7 @@ use crate::math::{Isometry, Point, Real};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
#[repr(transparent)]
/// A fixed joint, locks all relative motion between two bodies.
pub struct FixedJoint {
data: GenericJoint,
}
@@ -15,47 +16,56 @@ impl Default for FixedJoint {
}
impl FixedJoint {
/// Creates a new fixed joint.
#[must_use]
pub fn new() -> Self {
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_FIXED_AXES).build();
Self { data }
}
/// The joint’s frame, expressed in the first rigid-body’s local-space.
#[must_use]
pub fn local_frame1(&self) -> &Isometry<Real> {
&self.data.local_frame1
}
/// Sets the joint’s frame, expressed in the first rigid-body’s local-space.
pub fn set_local_frame1(&mut self, local_frame: Isometry<Real>) -> &mut Self {
self.data.set_local_frame1(local_frame);
self
}
/// The joint’s frame, expressed in the second rigid-body’s local-space.
#[must_use]
pub fn local_frame2(&self) -> &Isometry<Real> {
&self.data.local_frame2
}
/// Sets joint’s frame, expressed in the second rigid-body’s local-space.
pub fn set_local_frame2(&mut self, local_frame: Isometry<Real>) -> &mut Self {
self.data.set_local_frame2(local_frame);
self
}
/// The joint’s anchor, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_anchor1(&self) -> Point<Real> {
self.data.local_anchor1()
}
/// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
self.data.set_local_anchor1(anchor1);
self
}
/// The joint’s anchor, expressed in the local-space of the second rigid-body.
#[must_use]
pub fn local_anchor2(&self) -> Point<Real> {
self.data.local_anchor2()
}
/// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
self.data.set_local_anchor2(anchor2);
self
@@ -68,39 +78,46 @@ impl Into<GenericJoint> for FixedJoint {
}
}
/// Create fixed joints using the builder pattern.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq, Default)]
pub struct FixedJointBuilder(FixedJoint);
impl FixedJointBuilder {
/// Creates a new builder for fixed joints.
pub fn new() -> Self {
Self(FixedJoint::new())
}
/// Sets the joint’s frame, expressed in the first rigid-body’s local-space.
#[must_use]
pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
self.0.set_local_frame1(local_frame);
self
}
/// Sets joint’s frame, expressed in the second rigid-body’s local-space.
#[must_use]
pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
self.0.set_local_frame2(local_frame);
self
}
/// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.0.set_local_anchor1(anchor1);
self
}
/// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.0.set_local_anchor2(anchor2);
self
}
/// Build the fixed joint.
#[must_use]
pub fn build(self) -> FixedJoint {
self.0

98
src/dynamics/joint/generic_joint.rs
View File

@@ -8,55 +8,91 @@ use crate::dynamics::SphericalJoint;
#[cfg(feature = "dim3")]
bitflags::bitflags! {
/// A bit mask identifying multiple degrees of freedom of a joint.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct JointAxesMask: u8 {
/// The translational degree of freedom along the local X axis of a joint.
const X = 1 << 0;
/// The translational degree of freedom along the local Y axis of a joint.
const Y = 1 << 1;
/// The translational degree of freedom along the local Z axis of a joint.
const Z = 1 << 2;
/// The angular degree of freedom along the local X axis of a joint.
const ANG_X = 1 << 3;
/// The angular degree of freedom along the local Y axis of a joint.
const ANG_Y = 1 << 4;
/// The angular degree of freedom along the local Z axis of a joint.
const ANG_Z = 1 << 5;
/// The set of degrees of freedom locked by a revolute joint.
const LOCKED_REVOLUTE_AXES = Self::X.bits | Self::Y.bits | Self::Z.bits | Self::ANG_Y.bits | Self::ANG_Z.bits;
/// The set of degrees of freedom locked by a prismatic joint.
const LOCKED_PRISMATIC_AXES = Self::Y.bits | Self::Z.bits | Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits;
/// The set of degrees of freedom locked by a fixed joint.
const LOCKED_FIXED_AXES = Self::X.bits | Self::Y.bits | Self::Z.bits | Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits;
/// The set of degrees of freedom locked by a spherical joint.
const LOCKED_SPHERICAL_AXES = Self::X.bits | Self::Y.bits | Self::Z.bits;
/// The set of degrees of freedom left free by a revolute joint.
const FREE_REVOLUTE_AXES = Self::ANG_X.bits;
/// The set of degrees of freedom left free by a prismatic joint.
const FREE_PRISMATIC_AXES = Self::X.bits;
/// The set of degrees of freedom left free by a fixed joint.
const FREE_FIXED_AXES = 0;
/// The set of degrees of freedom left free by a spherical joint.
const FREE_SPHERICAL_AXES = Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits;
/// The set of all translational degrees of freedom.
const LIN_AXES = Self::X.bits() | Self::Y.bits() | Self::Z.bits();
/// The set of all angular degrees of freedom.
const ANG_AXES = Self::ANG_X.bits() | Self::ANG_Y.bits() | Self::ANG_Z.bits();
}
}
#[cfg(feature = "dim2")]
bitflags::bitflags! {
/// A bit mask identifying multiple degrees of freedom of a joint.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct JointAxesMask: u8 {
/// The translational degree of freedom along the local X axis of a joint.
const X = 1 << 0;
/// The translational degree of freedom along the local Y axis of a joint.
const Y = 1 << 1;
/// The angular degree of freedom of a joint.
const ANG_X = 1 << 2;
/// The set of degrees of freedom locked by a revolute joint.
const LOCKED_REVOLUTE_AXES = Self::X.bits | Self::Y.bits;
/// The set of degrees of freedom locked by a prismatic joint.
const LOCKED_PRISMATIC_AXES = Self::Y.bits | Self::ANG_X.bits;
/// The set of degrees of freedom locked by a fixed joint.
const LOCKED_FIXED_AXES = Self::X.bits | Self::Y.bits | Self::ANG_X.bits;
/// The set of degrees of freedom left free by a revolute joint.
const FREE_REVOLUTE_AXES = Self::ANG_X.bits;
/// The set of degrees of freedom left free by a prismatic joint.
const FREE_PRISMATIC_AXES = Self::X.bits;
/// The set of degrees of freedom left free by a fixed joint.
const FREE_FIXED_AXES = 0;
/// The set of all translational degrees of freedom.
const LIN_AXES = Self::X.bits() | Self::Y.bits();
/// The set of all angular degrees of freedom.
const ANG_AXES = Self::ANG_X.bits();
}
}
/// Identifiers of degrees of freedoms of a joint.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
pub enum JointAxis {
/// The translational degree of freedom along the joint’s local X axis.
X = 0,
/// The translational degree of freedom along the joint’s local Y axis.
Y,
/// The translational degree of freedom along the joint’s local Z axis.
#[cfg(feature = "dim3")]
Z,
/// The rotational degree of freedom along the joint’s local X axis.
AngX,
/// The rotational degree of freedom along the joint’s local Y axis.
#[cfg(feature = "dim3")]
AngY,
/// The rotational degree of freedom along the joint’s local Z axis.
#[cfg(feature = "dim3")]
AngZ,
}
@@ -67,11 +103,15 @@ impl From<JointAxis> for JointAxesMask {
}
}
/// The limits of a joint along one of its degrees of freedom.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct JointLimits<N> {
/// The minimum bound of the joint limit.
pub min: N,
/// The maximum bound of the joint limit.
pub max: N,
/// The impulse applied to enforce the joint’s limit.
pub impulse: N,
}
@@ -85,15 +125,23 @@ impl<N: WReal> Default for JointLimits<N> {
}
}
/// A joint’s motor along one of its degrees of freedom.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct JointMotor {
/// The target velocity of the motor.
pub target_vel: Real,
/// The target position of the motor.
pub target_pos: Real,
/// The stiffness coefficient of the motor’s spring-like equation.
pub stiffness: Real,
/// The damping coefficient of the motor’s spring-like equation.
pub damping: Real,
/// The maximum force this motor can deliver.
pub max_force: Real,
/// The impulse applied by this motor.
pub impulse: Real,
/// The spring-like model used for simulating this motor.
pub model: MotorModel,
}
@@ -130,14 +178,27 @@ impl JointMotor {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
/// A generic joint.
pub struct GenericJoint {
/// The joint’s frame, expressed in the first rigid-body’s local-space.
pub local_frame1: Isometry<Real>,
/// The joint’s frame, expressed in the second rigid-body’s local-space.
pub local_frame2: Isometry<Real>,
/// The degrees-of-freedoms locked by this joint.
pub locked_axes: JointAxesMask,
/// The degrees-of-freedoms limited by this joint.
pub limit_axes: JointAxesMask,
/// The degrees-of-freedoms motorised by this joint.
pub motor_axes: JointAxesMask,
/// The coupled degrees of freedom of this joint.
pub coupled_axes: JointAxesMask,
/// The limits, along each degrees of freedoms of this joint.
///
/// Note that the limit must also be explicitly enabled by the `limit_axes` bitmask.
pub limits: [JointLimits<Real>; SPATIAL_DIM],
/// The motors, along each degrees of freedoms of this joint.
///
/// Note that the mostor must also be explicitly enabled by the `motors` bitmask.
pub motors: [JointMotor; SPATIAL_DIM],
}
@@ -157,11 +218,13 @@ impl Default for GenericJoint {
}
impl GenericJoint {
/// Creates a new generic joint that locks the specified degrees of freedom.
#[must_use]
pub fn new(locked_axes: JointAxesMask) -> Self {
*Self::default().lock_axes(locked_axes)
}
#[cfg(feature = "simd-is-enabled")]
/// Can this joint use SIMD-accelerated constraint formulations?
pub(crate) fn supports_simd_constraints(&self) -> bool {
self.limit_axes.is_empty() && self.motor_axes.is_empty()
@@ -187,61 +250,73 @@ impl GenericJoint {
}
}
/// Add the specified axes to the set of axes locked by this joint.
pub fn lock_axes(&mut self, axes: JointAxesMask) -> &mut Self {
self.locked_axes |= axes;
self
}
/// Sets the joint’s frame, expressed in the first rigid-body’s local-space.
pub fn set_local_frame1(&mut self, local_frame: Isometry<Real>) -> &mut Self {
self.local_frame1 = local_frame;
self
}
/// Sets the joint’s frame, expressed in the second rigid-body’s local-space.
pub fn set_local_frame2(&mut self, local_frame: Isometry<Real>) -> &mut Self {
self.local_frame2 = local_frame;
self
}
/// The principal (local X) axis of this joint, expressed in the first rigid-body’s local-space.
#[must_use]
pub fn local_axis1(&self) -> UnitVector<Real> {
self.local_frame1 * Vector::x_axis()
}
/// Sets the principal (local X) axis of this joint, expressed in the first rigid-body’s local-space.
pub fn set_local_axis1(&mut self, local_axis: UnitVector<Real>) -> &mut Self {
self.local_frame1.rotation = Self::complete_ang_frame(local_axis);
self
}
/// The principal (local X) axis of this joint, expressed in the second rigid-body’s local-space.
#[must_use]
pub fn local_axis2(&self) -> UnitVector<Real> {
self.local_frame2 * Vector::x_axis()
}
/// Sets the principal (local X) axis of this joint, expressed in the second rigid-body’s local-space.
pub fn set_local_axis2(&mut self, local_axis: UnitVector<Real>) -> &mut Self {
self.local_frame2.rotation = Self::complete_ang_frame(local_axis);
self
}
/// The anchor of this joint, expressed in the first rigid-body’s local-space.
#[must_use]
pub fn local_anchor1(&self) -> Point<Real> {
self.local_frame1.translation.vector.into()
}
/// Sets anchor of this joint, expressed in the first rigid-body’s local-space.
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
self.local_frame1.translation.vector = anchor1.coords;
self
}
/// The anchor of this joint, expressed in the second rigid-body’s local-space.
#[must_use]
pub fn local_anchor2(&self) -> Point<Real> {
self.local_frame2.translation.vector.into()
}
/// Sets anchor of this joint, expressed in the second rigid-body’s local-space.
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
self.local_frame2.translation.vector = anchor2.coords;
self
}
/// The joint limits along the specified axis.
#[must_use]
pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits<Real>> {
let i = axis as usize;
@@ -252,6 +327,7 @@ impl GenericJoint {
}
}
/// Sets the joint limits along the specified axis.
pub fn set_limits(&mut self, axis: JointAxis, limits: [Real; 2]) -> &mut Self {
let i = axis as usize;
self.limit_axes |= axis.into();
@@ -260,6 +336,7 @@ impl GenericJoint {
self
}
/// The spring-like motor model along the specified axis of this joint.
#[must_use]
pub fn motor_model(&self, axis: JointAxis) -> Option<MotorModel> {
let i = axis as usize;
@@ -303,11 +380,13 @@ impl GenericJoint {
self.set_motor(axis, target_pos, 0.0, stiffness, damping)
}
/// Sets the maximum force the motor can deliver along the specified axis.
pub fn set_motor_max_force(&mut self, axis: JointAxis, max_force: Real) -> &mut Self {
self.motors[axis as usize].max_force = max_force;
self
}
/// The motor affecting the joint’s degree of freedom along the specified axis.
#[must_use]
pub fn motor(&self, axis: JointAxis) -> Option<&JointMotor> {
let i = axis as usize;
@@ -339,6 +418,7 @@ impl GenericJoint {
macro_rules! joint_conversion_methods(
($as_joint: ident, $as_joint_mut: ident, $Joint: ty, $axes: expr) => {
/// Converts the joint to its specific variant, if it is one.
#[must_use]
pub fn $as_joint(&self) -> Option<&$Joint> {
if self.locked_axes == $axes {
@@ -350,6 +430,7 @@ macro_rules! joint_conversion_methods(
}
}
/// Converts the joint to its specific mutable variant, if it is one.
#[must_use]
pub fn $as_joint_mut(&mut self) -> Option<&mut $Joint> {
if self.locked_axes == $axes {
@@ -392,63 +473,74 @@ impl GenericJoint {
);
}
/// Create generic joints using the builder pattern.
#[derive(Copy, Clone, Debug)]
pub struct GenericJointBuilder(GenericJoint);
impl GenericJointBuilder {
/// Creates a new generic joint builder.
#[must_use]
pub fn new(locked_axes: JointAxesMask) -> Self {
Self(GenericJoint::new(locked_axes))
}
/// Sets the degrees of freedom locked by the joint.
#[must_use]
pub fn lock_axes(mut self, axes: JointAxesMask) -> Self {
self.0.lock_axes(axes);
pub fn locked_axes(mut self, axes: JointAxesMask) -> Self {
self.0.locked_axes = axes;
self
}
/// Sets the joint’s frame, expressed in the first rigid-body’s local-space.
#[must_use]
pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
self.0.set_local_frame1(local_frame);
self
}
/// Sets the joint’s frame, expressed in the second rigid-body’s local-space.
#[must_use]
pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
self.0.set_local_frame2(local_frame);
self
}
/// Sets the principal (local X) axis of this joint, expressed in the first rigid-body’s local-space.
#[must_use]
pub fn local_axis1(mut self, local_axis: UnitVector<Real>) -> Self {
self.0.set_local_axis1(local_axis);
self
}
/// Sets the principal (local X) axis of this joint, expressed in the second rigid-body’s local-space.
#[must_use]
pub fn local_axis2(mut self, local_axis: UnitVector<Real>) -> Self {
self.0.set_local_axis2(local_axis);
self
}
/// Sets the anchor of this joint, expressed in the first rigid-body’s local-space.
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.0.set_local_anchor1(anchor1);
self
}
/// Sets the anchor of this joint, expressed in the second rigid-body’s local-space.
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.0.set_local_anchor2(anchor2);
self
}
/// Sets the joint limits along the specified axis.
#[must_use]
pub fn limits(mut self, axis: JointAxis, limits: [Real; 2]) -> Self {
self.0.set_limits(axis, limits);
self
}
/// Sets the coupled degrees of freedom for this joint’s limits and motor.
#[must_use]
pub fn coupled_axes(mut self, axes: JointAxesMask) -> Self {
self.0.coupled_axes = axes;
@@ -498,12 +590,14 @@ impl GenericJointBuilder {
self
}
/// Sets the maximum force the motor can deliver along the specified axis.
#[must_use]
pub fn motor_max_force(mut self, axis: JointAxis, max_force: Real) -> Self {
self.0.set_motor_max_force(axis, max_force);
self
}
/// Builds the generic joint.
#[must_use]
pub fn build(self) -> GenericJoint {
self.0

5
src/dynamics/joint/impulse_joint/impulse_joint.rs
View File

@@ -3,14 +3,17 @@ use crate::math::{Real, SpacialVector};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug, PartialEq)]
/// A joint attached to two bodies.
/// An impulse-based joint attached to two bodies.
pub struct ImpulseJoint {
/// Handle to the first body attached to this joint.
pub body1: RigidBodyHandle,
/// Handle to the second body attached to this joint.
pub body2: RigidBodyHandle,
/// The joint’s description.
pub data: GenericJoint,
/// The impulses applied by this joint.
pub impulses: SpacialVector<Real>,
// A joint needs to know its handle to simplify its removal.

41
src/dynamics/joint/multibody_joint/multibody.rs
View File

@@ -120,7 +120,7 @@ impl Multibody {
}
}
pub fn with_root(handle: RigidBodyHandle) -> Self {
pub(crate) fn with_root(handle: RigidBodyHandle) -> Self {
let mut mb = Multibody::new();
mb.root_is_dynamic = true;
let joint = MultibodyJoint::free(Isometry::identity());
@@ -128,7 +128,7 @@ impl Multibody {
mb
}
pub fn remove_link(self, to_remove: usize, joint_only: bool) -> Vec<Multibody> {
pub(crate) fn remove_link(self, to_remove: usize, joint_only: bool) -> Vec<Multibody> {
let mut result = vec![];
let mut link2mb = vec![usize::MAX; self.links.len()];
let mut link_id2new_id = vec![usize::MAX; self.links.len()];
@@ -187,7 +187,7 @@ impl Multibody {
result
}
pub fn append(&mut self, mut rhs: Multibody, parent: usize, joint: MultibodyJoint) {
pub(crate) fn append(&mut self, mut rhs: Multibody, parent: usize, joint: MultibodyJoint) {
let rhs_root_ndofs = rhs.links[0].joint.ndofs();
let rhs_copy_shift = self.ndofs + rhs_root_ndofs;
let rhs_copy_ndofs = rhs.ndofs - rhs_root_ndofs;
@@ -235,6 +235,7 @@ impl Multibody {
self.workspace.resize(self.links.len(), self.ndofs);
}
/// The inverse augmented mass matrix of this multibody.
pub fn inv_augmented_mass(&self) -> &LU<Real, Dynamic, Dynamic> {
&self.inv_augmented_mass
}
@@ -298,7 +299,7 @@ impl Multibody {
&mut self.damping
}
pub fn add_link(
pub(crate) fn add_link(
&mut self,
parent: Option<usize>, // FIXME: should be a RigidBodyHandle?
dof: MultibodyJoint,
@@ -368,7 +369,7 @@ impl Multibody {
.extend((0..num_jacobians).map(|_| Jacobian::zeros(0)));
}
pub fn update_acceleration<Bodies>(&mut self, bodies: &Bodies)
pub(crate) fn update_acceleration<Bodies>(&mut self, bodies: &Bodies)
where
Bodies: ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyForces>
@@ -451,7 +452,7 @@ impl Multibody {
}
/// Computes the constant terms of the dynamics.
pub fn update_dynamics<Bodies>(&mut self, dt: Real, bodies: &mut Bodies)
pub(crate) fn update_dynamics<Bodies>(&mut self, dt: Real, bodies: &mut Bodies)
where
Bodies: ComponentSetMut<RigidBodyVelocity> + ComponentSet<RigidBodyMassProps>,
{
@@ -756,36 +757,40 @@ impl Multibody {
)
}
/// The generalized accelerations of this multibodies.
#[inline]
pub fn generalized_acceleration(&self) -> DVectorSlice<Real> {
self.accelerations.rows(0, self.ndofs)
}
/// The generalized velocities of this multibodies.
#[inline]
pub fn generalized_velocity(&self) -> DVectorSlice<Real> {
self.velocities.rows(0, self.ndofs)
}
/// The mutable generalized velocities of this multibodies.
#[inline]
pub fn generalized_velocity_mut(&mut self) -> DVectorSliceMut<Real> {
self.velocities.rows_mut(0, self.ndofs)
}
#[inline]
pub fn integrate(&mut self, dt: Real) {
pub(crate) fn integrate(&mut self, dt: Real) {
for rb in self.links.iter_mut() {
rb.joint
.integrate(dt, &self.velocities.as_slice()[rb.assembly_id..])
}
}
/// Apply displacements, in generalized coordinates, to this multibody.
pub fn apply_displacements(&mut self, disp: &[Real]) {
for link in self.links.iter_mut() {
link.joint.apply_displacement(&disp[link.assembly_id..])
}
}
pub fn update_root_type<Bodies>(&mut self, bodies: &mut Bodies)
pub(crate) fn update_root_type<Bodies>(&mut self, bodies: &mut Bodies)
where
Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyPosition>,
{
@@ -851,6 +856,7 @@ impl Multibody {
}
}
/// Apply forward-kinematics to this multibody and its related rigid-bodies.
pub fn forward_kinematics<Bodies>(&mut self, bodies: &mut Bodies, update_mass_props: bool)
where
Bodies: ComponentSet<RigidBodyType>
@@ -917,12 +923,13 @@ impl Multibody {
self.update_body_jacobians();
}
/// The total number of freedoms of this multibody.
#[inline]
pub fn ndofs(&self) -> usize {
self.ndofs
}
pub fn fill_jacobians(
pub(crate) fn fill_jacobians(
&self,
link_id: usize,
unit_force: Vector<Real>,
@@ -964,14 +971,16 @@ impl Multibody {
(j.dot(&invm_j), j.dot(&self.generalized_velocity()))
}
#[inline]
pub fn has_active_internal_constraints(&self) -> bool {
self.links()
.any(|link| link.joint().num_velocity_constraints() != 0)
}
// #[cfg(feature = "parallel")]
// #[inline]
// pub(crate) fn has_active_internal_constraints(&self) -> bool {
// self.links()
// .any(|link| link.joint().num_velocity_constraints() != 0)
// }
#[cfg(feature = "parallel")]
#[inline]
pub fn num_active_internal_constraints_and_jacobian_lines(&self) -> (usize, usize) {
pub(crate) fn num_active_internal_constraints_and_jacobian_lines(&self) -> (usize, usize) {
let num_constraints: usize = self
.links
.iter()
@@ -981,7 +990,7 @@ impl Multibody {
}
#[inline]
pub fn generate_internal_constraints(
pub(crate) fn generate_internal_constraints(
&self,
params: &IntegrationParameters,
j_id: &mut usize,

9
src/dynamics/joint/multibody_joint/multibody_joint.rs
View File

@@ -13,13 +13,16 @@ use na::{UnitQuaternion, Vector3};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug)]
/// An joint attached to two bodies based on the reduced coordinates formalism.
pub struct MultibodyJoint {
/// The joint’s description.
pub data: GenericJoint,
pub(crate) coords: SpacialVector<Real>,
pub(crate) joint_rot: Rotation<Real>,
}
impl MultibodyJoint {
/// Creates a new multibody joint from its description.
pub fn new(data: GenericJoint) -> Self {
Self {
data,
@@ -45,9 +48,9 @@ impl MultibodyJoint {
self.joint_rot = pos.rotation;
}
pub fn local_joint_rot(&self) -> &Rotation<Real> {
&self.joint_rot
}
// pub(crate) fn local_joint_rot(&self) -> &Rotation<Real> {
// &self.joint_rot
// }
fn num_free_lin_dofs(&self) -> usize {
let locked_bits = self.data.locked_axes.bits();

4
src/dynamics/joint/multibody_joint/multibody_joint_set.rs
View File

@@ -97,6 +97,7 @@ impl MultibodyJointSet {
}
}
/// Iterates through all the multibody joints from this set.
pub fn iter(&self) -> impl Iterator<Item = (MultibodyJointHandle, &Multibody, &MultibodyLink)> {
self.rb2mb
.iter()
@@ -246,7 +247,8 @@ impl MultibodyJointSet {
}
}
pub fn remove_articulations_attached_to_rigid_body<Bodies>(
/// Removes all the multibody joints attached to a rigid-body.
pub fn remove_joints_attached_to_rigid_body<Bodies>(
&mut self,
rb_to_remove: RigidBodyHandle,
islands: &mut IslandManager,

13
src/dynamics/joint/multibody_joint/multibody_link.rs
View File

@@ -18,12 +18,13 @@ pub struct MultibodyLink {
/*
* Change at each time step.
*/
/// The multibody joint of this link.
pub joint: MultibodyJoint,
// TODO: should this be removed in favor of the rigid-body position?
pub local_to_world: Isometry<Real>,
pub local_to_parent: Isometry<Real>,
pub shift02: Vector<Real>,
pub shift23: Vector<Real>,
pub(crate) local_to_world: Isometry<Real>,
pub(crate) local_to_parent: Isometry<Real>,
pub(crate) shift02: Vector<Real>,
pub(crate) shift23: Vector<Real>,
/// The velocity added by the joint, in world-space.
pub(crate) joint_velocity: RigidBodyVelocity,
@@ -56,10 +57,12 @@ impl MultibodyLink {
}
}
/// The multibody joint of this link.
pub fn joint(&self) -> &MultibodyJoint {
&self.joint
}
/// The handle of the rigid-body of this link.
pub fn rigid_body_handle(&self) -> RigidBodyHandle {
self.rigid_body
}
@@ -86,11 +89,13 @@ impl MultibodyLink {
}
}
/// The world-space transform of the rigid-body attached to this link.
#[inline]
pub fn local_to_world(&self) -> &Isometry<Real> {
&self.local_to_world
}
/// The position of the rigid-body attached to this link relative to its parent.
#[inline]
pub fn local_to_parent(&self) -> &Isometry<Real> {
&self.local_to_parent

35
src/dynamics/joint/prismatic_joint.rs
View File

@@ -7,11 +7,15 @@ use super::{JointLimits, JointMotor};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
#[repr(transparent)]
/// A prismatic joint, locks all relative motion between two bodies except for translation along the joint’s principal axis.
pub struct PrismaticJoint {
data: GenericJoint,
}
impl PrismaticJoint {
/// Creates a new prismatic joint allowing only relative translations along the specified axis.
///
/// This axis is expressed in the local-space of both rigid-bodies.
pub fn new(axis: UnitVector<Real>) -> Self {
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_PRISMATIC_AXES)
.local_axis1(axis)
@@ -20,46 +24,60 @@ impl PrismaticJoint {
Self { data }
}
/// The underlying generic joint.
pub fn data(&self) -> &GenericJoint {
&self.data
}
/// The joint’s anchor, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_anchor1(&self) -> Point<Real> {
self.data.local_anchor1()
}
/// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
self.data.set_local_anchor1(anchor1);
self
}
/// The joint’s anchor, expressed in the local-space of the second rigid-body.
#[must_use]
pub fn local_anchor2(&self) -> Point<Real> {
self.data.local_anchor2()
}
/// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
self.data.set_local_anchor2(anchor2);
self
}
/// The principal axis of the joint, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_axis1(&self) -> UnitVector<Real> {
self.data.local_axis1()
}
/// Sets the principal axis of the joint, expressed in the local-space of the first rigid-body.
pub fn set_local_axis1(&mut self, axis1: UnitVector<Real>) -> &mut Self {
self.data.set_local_axis1(axis1);
self
}
/// The principal axis of the joint, expressed in the local-space of the second rigid-body.
#[must_use]
pub fn local_axis2(&self) -> UnitVector<Real> {
self.data.local_axis2()
}
/// Sets the principal axis of the joint, expressed in the local-space of the second rigid-body.
pub fn set_local_axis2(&mut self, axis2: UnitVector<Real>) -> &mut Self {
self.data.set_local_axis2(axis2);
self
}
/// The motor affecting the joint’s translational degree of freedom.
#[must_use]
pub fn motor(&self) -> Option<&JointMotor> {
self.data.motor(JointAxis::X)
@@ -103,16 +121,19 @@ impl PrismaticJoint {
self
}
/// Sets the maximum force the motor can deliver.
pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self {
self.data.set_motor_max_force(JointAxis::X, max_force);
self
}
/// The limit distance attached bodies can translate along the joint’s principal axis.
#[must_use]
pub fn limits(&self) -> Option<&JointLimits<Real>> {
self.data.limits(JointAxis::X)
}
/// Sets the `[min,max]` limit distances attached bodies can translate along the joint’s principal axis.
pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self {
self.data.set_limits(JointAxis::X, limits);
self
@@ -125,31 +146,42 @@ impl Into<GenericJoint> for PrismaticJoint {
}
}
/// Create prismatic joints using the builder pattern.
///
/// A prismatic joint locks all relative motion except for translations along the joint’s principal axis.
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct PrismaticJointBuilder(PrismaticJoint);
impl PrismaticJointBuilder {
/// Creates a new builder for prismatic joints.
///
/// This axis is expressed in the local-space of both rigid-bodies.
pub fn new(axis: UnitVector<Real>) -> Self {
Self(PrismaticJoint::new(axis))
}
/// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.0.set_local_anchor1(anchor1);
self
}
/// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.0.set_local_anchor2(anchor2);
self
}
/// Sets the principal axis of the joint, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_axis1(mut self, axis1: UnitVector<Real>) -> Self {
self.0.set_local_axis1(axis1);
self
}
/// Sets the principal axis of the joint, expressed in the local-space of the second rigid-body.
#[must_use]
pub fn local_axis2(mut self, axis2: UnitVector<Real>) -> Self {
self.0.set_local_axis2(axis2);
@@ -190,18 +222,21 @@ impl PrismaticJointBuilder {
self
}
/// Sets the maximum force the motor can deliver.
#[must_use]
pub fn motor_max_force(mut self, max_force: Real) -> Self {
self.0.set_motor_max_force(max_force);
self
}
/// Sets the `[min,max]` limit distances attached bodies can translate along the joint’s principal axis.
#[must_use]
pub fn limits(mut self, limits: [Real; 2]) -> Self {
self.0.set_limits(limits);
self
}
/// Builds the prismatic joint.
#[must_use]
pub fn build(self) -> PrismaticJoint {
self.0

26
src/dynamics/joint/revolute_joint.rs
View File

@@ -8,17 +8,22 @@ use crate::math::UnitVector;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
#[repr(transparent)]
/// A revolute joint, locks all relative motion except for rotation along the joint’s principal axis.
pub struct RevoluteJoint {
data: GenericJoint,
}
impl RevoluteJoint {
/// Creates a new revolute joint allowing only relative rotations.
#[cfg(feature = "dim2")]
pub fn new() -> Self {
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES);
Self { data: data.build() }
}
/// Creates a new revolute joint allowing only relative rotations along the specified axis.
///
/// This axis is expressed in the local-space of both rigid-bodies.
#[cfg(feature = "dim3")]
pub fn new(axis: UnitVector<Real>) -> Self {
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES)
@@ -28,30 +33,36 @@ impl RevoluteJoint {
Self { data }
}
/// The underlying generic joint.
pub fn data(&self) -> &GenericJoint {
&self.data
}
/// The joint’s anchor, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_anchor1(&self) -> Point<Real> {
self.data.local_anchor1()
}
/// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
self.data.set_local_anchor1(anchor1);
self
}
/// The joint’s anchor, expressed in the local-space of the second rigid-body.
#[must_use]
pub fn local_anchor2(&self) -> Point<Real> {
self.data.local_anchor2()
}
/// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
self.data.set_local_anchor2(anchor2);
self
}
/// The motor affecting the joint’s rotational degree of freedom.
#[must_use]
pub fn motor(&self) -> Option<&JointMotor> {
self.data.motor(JointAxis::AngX)
@@ -95,16 +106,19 @@ impl RevoluteJoint {
self
}
/// Sets the maximum force the motor can deliver.
pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self {
self.data.set_motor_max_force(JointAxis::AngX, max_force);
self
}
/// The limit angle attached bodies can translate along the joint’s principal axis.
#[must_use]
pub fn limits(&self) -> Option<&JointLimits<Real>> {
self.data.limits(JointAxis::AngX)
}
/// Sets the `[min,max]` limit angle attached bodies can translate along the joint’s principal axis.
pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self {
self.data.set_limits(JointAxis::AngX, limits);
self
@@ -117,27 +131,36 @@ impl Into<GenericJoint> for RevoluteJoint {
}
}
/// Create revolute joints using the builder pattern.
///
/// A revolute joint locks all relative motion except for rotations along the joint’s principal axis.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct RevoluteJointBuilder(RevoluteJoint);
impl RevoluteJointBuilder {
/// Creates a new revolute joint builder.
#[cfg(feature = "dim2")]
pub fn new() -> Self {
Self(RevoluteJoint::new())
}
/// Creates a new revolute joint builder, allowing only relative rotations along the specified axis.
///
/// This axis is expressed in the local-space of both rigid-bodies.
#[cfg(feature = "dim3")]
pub fn new(axis: UnitVector<Real>) -> Self {
Self(RevoluteJoint::new(axis))
}
/// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.0.set_local_anchor1(anchor1);
self
}
/// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.0.set_local_anchor2(anchor2);
@@ -178,18 +201,21 @@ impl RevoluteJointBuilder {
self
}
/// Sets the maximum force the motor can deliver.
#[must_use]
pub fn motor_max_force(mut self, max_force: Real) -> Self {
self.0.set_motor_max_force(max_force);
self
}
/// Sets the `[min,max]` limit angles attached bodies can rotate along the joint’s principal axis.
#[must_use]
pub fn limits(mut self, limits: [Real; 2]) -> Self {
self.0.set_limits(limits);
self
}
/// Builds the revolute joint.
#[must_use]
pub fn build(self) -> RevoluteJoint {
self.0

42
src/dynamics/joint/spherical_joint.rs
View File

@@ -1,10 +1,13 @@
use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
use crate::dynamics::{JointAxis, MotorModel};
use crate::dynamics::{JointAxis, JointMotor, MotorModel};
use crate::math::{Point, Real};
use super::JointLimits;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
#[repr(transparent)]
/// A spherical joint, locks all relative translations between two bodies.
pub struct SphericalJoint {
data: GenericJoint,
}
@@ -16,25 +19,47 @@ impl Default for SphericalJoint {
}
impl SphericalJoint {
/// Creates a new spherical joint locking all relative translations between two bodies.
pub fn new() -> Self {
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_SPHERICAL_AXES).build();
Self { data }
}
/// The underlying generic joint.
pub fn data(&self) -> &GenericJoint {
&self.data
}
/// The joint’s anchor, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_anchor1(&self) -> Point<Real> {
self.data.local_anchor1()
}
/// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
self.data.set_local_anchor1(anchor1);
self
}
/// The joint’s anchor, expressed in the local-space of the second rigid-body.
#[must_use]
pub fn local_anchor2(&self) -> Point<Real> {
self.data.local_anchor2()
}
/// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
self.data.set_local_anchor2(anchor2);
self
}
/// The motor affecting the joint’s rotational degree of freedom along the specified axis.
#[must_use]
pub fn motor(&self, axis: JointAxis) -> Option<&JointMotor> {
self.data.motor(axis)
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
pub fn set_motor_model(&mut self, axis: JointAxis, model: MotorModel) -> &mut Self {
self.data.set_motor_model(axis, model);
@@ -79,11 +104,19 @@ impl SphericalJoint {
self
}
/// Sets the maximum force the motor can deliver along the specified axis.
pub fn set_motor_max_force(&mut self, axis: JointAxis, max_force: Real) -> &mut Self {
self.data.set_motor_max_force(axis, max_force);
self
}
/// The limit distance attached bodies can translate along the specified axis.
#[must_use]
pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits<Real>> {
self.data.limits(axis)
}
/// Sets the `[min,max]` limit angles attached bodies can translate along the joint’s principal axis.
pub fn set_limits(&mut self, axis: JointAxis, limits: [Real; 2]) -> &mut Self {
self.data.set_limits(axis, limits);
self
@@ -96,6 +129,7 @@ impl Into<GenericJoint> for SphericalJoint {
}
}
/// Create spherical joints using the builder pattern.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct SphericalJointBuilder(SphericalJoint);
@@ -107,16 +141,19 @@ impl Default for SphericalJointBuilder {
}
impl SphericalJointBuilder {
/// Creates a new builder for spherical joints.
pub fn new() -> Self {
Self(SphericalJoint::new())
}
/// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.0.set_local_anchor1(anchor1);
self
}
/// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.0.set_local_anchor2(anchor2);
@@ -166,18 +203,21 @@ impl SphericalJointBuilder {
self
}
/// Sets the maximum force the motor can deliver along the specified axis.
#[must_use]
pub fn motor_max_force(mut self, axis: JointAxis, max_force: Real) -> Self {
self.0.set_motor_max_force(axis, max_force);
self
}
/// Sets the `[min,max]` limit distances attached bodies can rotate along the specified axis.
#[must_use]
pub fn limits(mut self, axis: JointAxis, limits: [Real; 2]) -> Self {
self.0.set_limits(axis, limits);
self
}
/// Builds the spherical joint.
#[must_use]
pub fn build(self) -> SphericalJoint {
self.0