Fix warnings and add comments.
This commit is contained in:
committed by
Sébastien Crozet
parent
e2e6fc7871
commit
db6a8c526d
@@ -13,6 +13,7 @@ impl<T> Coarena<T> {
|
|||||||
Self { data: Vec::new() }
|
Self { data: Vec::new() }
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Iterates through all the elements of this coarena.
|
||||||
pub fn iter(&self) -> impl Iterator<Item = (Index, &T)> {
|
pub fn iter(&self) -> impl Iterator<Item = (Index, &T)> {
|
||||||
self.data
|
self.data
|
||||||
.iter()
|
.iter()
|
||||||
|
|||||||
@@ -20,10 +20,7 @@ pub struct IntegrationParameters {
|
|||||||
|
|
||||||
/// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration)
|
/// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration)
|
||||||
/// will be compensated for during the velocity solve.
|
/// will be compensated for during the velocity solve.
|
||||||
/// If zero, you need to enable the positional solver.
|
/// (default `0.8`).
|
||||||
/// If non-zero, you do not need the positional solver.
|
|
||||||
/// A good non-zero value is around `0.2`.
|
|
||||||
/// (default `0.0`).
|
|
||||||
pub erp: Real,
|
pub erp: Real,
|
||||||
/// 0-1: the damping ratio used by the springs for Baumgarte constraints stabilization.
|
/// 0-1: the damping ratio used by the springs for Baumgarte constraints stabilization.
|
||||||
/// Lower values make the constraints more compliant (more "springy", allowing more visible penetrations
|
/// Lower values make the constraints more compliant (more "springy", allowing more visible penetrations
|
||||||
@@ -31,7 +28,13 @@ pub struct IntegrationParameters {
|
|||||||
/// (default `0.25`).
|
/// (default `0.25`).
|
||||||
pub damping_ratio: Real,
|
pub damping_ratio: Real,
|
||||||
|
|
||||||
|
/// 0-1: multiplier for how much of the joint violation
|
||||||
|
/// will be compensated for during the velocity solve.
|
||||||
|
/// (default `1.0`).
|
||||||
pub joint_erp: Real,
|
pub joint_erp: Real,
|
||||||
|
|
||||||
|
/// The fraction of critical damping applied to the joint for constraints regularization.
|
||||||
|
/// (default `0.25`).
|
||||||
pub joint_damping_ratio: Real,
|
pub joint_damping_ratio: Real,
|
||||||
|
|
||||||
/// Amount of penetration the engine wont attempt to correct (default: `0.001m`).
|
/// Amount of penetration the engine wont attempt to correct (default: `0.001m`).
|
||||||
@@ -131,6 +134,7 @@ impl IntegrationParameters {
|
|||||||
1.0 / (1.0 + cfm_coeff)
|
1.0 / (1.0 + cfm_coeff)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The CFM (constranits force mixing) coefficient applied to all joints for constraints regularization
|
||||||
pub fn joint_cfm_coeff(&self) -> Real {
|
pub fn joint_cfm_coeff(&self) -> Real {
|
||||||
// Compute CFM assuming a critically damped spring multiplied by the damping ratio.
|
// Compute CFM assuming a critically damped spring multiplied by the damping ratio.
|
||||||
let inv_erp_minus_one = 1.0 / self.joint_erp - 1.0;
|
let inv_erp_minus_one = 1.0 / self.joint_erp - 1.0;
|
||||||
|
|||||||
@@ -4,6 +4,7 @@ use crate::math::{Isometry, Point, Real};
|
|||||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||||
#[derive(Copy, Clone, Debug, PartialEq)]
|
#[derive(Copy, Clone, Debug, PartialEq)]
|
||||||
#[repr(transparent)]
|
#[repr(transparent)]
|
||||||
|
/// A fixed joint, locks all relative motion between two bodies.
|
||||||
pub struct FixedJoint {
|
pub struct FixedJoint {
|
||||||
data: GenericJoint,
|
data: GenericJoint,
|
||||||
}
|
}
|
||||||
@@ -15,47 +16,56 @@ impl Default for FixedJoint {
|
|||||||
}
|
}
|
||||||
|
|
||||||
impl FixedJoint {
|
impl FixedJoint {
|
||||||
|
/// Creates a new fixed joint.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn new() -> Self {
|
pub fn new() -> Self {
|
||||||
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_FIXED_AXES).build();
|
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_FIXED_AXES).build();
|
||||||
Self { data }
|
Self { data }
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The joint’s frame, expressed in the first rigid-body’s local-space.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_frame1(&self) -> &Isometry<Real> {
|
pub fn local_frame1(&self) -> &Isometry<Real> {
|
||||||
&self.data.local_frame1
|
&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 {
|
pub fn set_local_frame1(&mut self, local_frame: Isometry<Real>) -> &mut Self {
|
||||||
self.data.set_local_frame1(local_frame);
|
self.data.set_local_frame1(local_frame);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The joint’s frame, expressed in the second rigid-body’s local-space.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_frame2(&self) -> &Isometry<Real> {
|
pub fn local_frame2(&self) -> &Isometry<Real> {
|
||||||
&self.data.local_frame2
|
&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 {
|
pub fn set_local_frame2(&mut self, local_frame: Isometry<Real>) -> &mut Self {
|
||||||
self.data.set_local_frame2(local_frame);
|
self.data.set_local_frame2(local_frame);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The joint’s anchor, expressed in the local-space of the first rigid-body.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_anchor1(&self) -> Point<Real> {
|
pub fn local_anchor1(&self) -> Point<Real> {
|
||||||
self.data.local_anchor1()
|
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 {
|
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
|
||||||
self.data.set_local_anchor1(anchor1);
|
self.data.set_local_anchor1(anchor1);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The joint’s anchor, expressed in the local-space of the second rigid-body.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_anchor2(&self) -> Point<Real> {
|
pub fn local_anchor2(&self) -> Point<Real> {
|
||||||
self.data.local_anchor2()
|
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 {
|
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
|
||||||
self.data.set_local_anchor2(anchor2);
|
self.data.set_local_anchor2(anchor2);
|
||||||
self
|
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))]
|
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||||
#[derive(Copy, Clone, Debug, PartialEq, Default)]
|
#[derive(Copy, Clone, Debug, PartialEq, Default)]
|
||||||
pub struct FixedJointBuilder(FixedJoint);
|
pub struct FixedJointBuilder(FixedJoint);
|
||||||
|
|
||||||
impl FixedJointBuilder {
|
impl FixedJointBuilder {
|
||||||
|
/// Creates a new builder for fixed joints.
|
||||||
pub fn new() -> Self {
|
pub fn new() -> Self {
|
||||||
Self(FixedJoint::new())
|
Self(FixedJoint::new())
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the joint’s frame, expressed in the first rigid-body’s local-space.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
|
pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
|
||||||
self.0.set_local_frame1(local_frame);
|
self.0.set_local_frame1(local_frame);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets joint’s frame, expressed in the second rigid-body’s local-space.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
|
pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
|
||||||
self.0.set_local_frame2(local_frame);
|
self.0.set_local_frame2(local_frame);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
|
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
|
||||||
self.0.set_local_anchor1(anchor1);
|
self.0.set_local_anchor1(anchor1);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
|
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
|
||||||
self.0.set_local_anchor2(anchor2);
|
self.0.set_local_anchor2(anchor2);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Build the fixed joint.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn build(self) -> FixedJoint {
|
pub fn build(self) -> FixedJoint {
|
||||||
self.0
|
self.0
|
||||||
|
|||||||
@@ -8,55 +8,91 @@ use crate::dynamics::SphericalJoint;
|
|||||||
|
|
||||||
#[cfg(feature = "dim3")]
|
#[cfg(feature = "dim3")]
|
||||||
bitflags::bitflags! {
|
bitflags::bitflags! {
|
||||||
|
/// A bit mask identifying multiple degrees of freedom of a joint.
|
||||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||||
pub struct JointAxesMask: u8 {
|
pub struct JointAxesMask: u8 {
|
||||||
|
/// The translational degree of freedom along the local X axis of a joint.
|
||||||
const X = 1 << 0;
|
const X = 1 << 0;
|
||||||
|
/// The translational degree of freedom along the local Y axis of a joint.
|
||||||
const Y = 1 << 1;
|
const Y = 1 << 1;
|
||||||
|
/// The translational degree of freedom along the local Z axis of a joint.
|
||||||
const Z = 1 << 2;
|
const Z = 1 << 2;
|
||||||
|
/// The angular degree of freedom along the local X axis of a joint.
|
||||||
const ANG_X = 1 << 3;
|
const ANG_X = 1 << 3;
|
||||||
|
/// The angular degree of freedom along the local Y axis of a joint.
|
||||||
const ANG_Y = 1 << 4;
|
const ANG_Y = 1 << 4;
|
||||||
|
/// The angular degree of freedom along the local Z axis of a joint.
|
||||||
const ANG_Z = 1 << 5;
|
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;
|
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;
|
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;
|
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;
|
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;
|
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;
|
const FREE_PRISMATIC_AXES = Self::X.bits;
|
||||||
|
/// The set of degrees of freedom left free by a fixed joint.
|
||||||
const FREE_FIXED_AXES = 0;
|
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;
|
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();
|
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();
|
const ANG_AXES = Self::ANG_X.bits() | Self::ANG_Y.bits() | Self::ANG_Z.bits();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#[cfg(feature = "dim2")]
|
#[cfg(feature = "dim2")]
|
||||||
bitflags::bitflags! {
|
bitflags::bitflags! {
|
||||||
|
/// A bit mask identifying multiple degrees of freedom of a joint.
|
||||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||||
pub struct JointAxesMask: u8 {
|
pub struct JointAxesMask: u8 {
|
||||||
|
/// The translational degree of freedom along the local X axis of a joint.
|
||||||
const X = 1 << 0;
|
const X = 1 << 0;
|
||||||
|
/// The translational degree of freedom along the local Y axis of a joint.
|
||||||
const Y = 1 << 1;
|
const Y = 1 << 1;
|
||||||
|
/// The angular degree of freedom of a joint.
|
||||||
const ANG_X = 1 << 2;
|
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;
|
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;
|
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;
|
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;
|
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;
|
const FREE_PRISMATIC_AXES = Self::X.bits;
|
||||||
|
/// The set of degrees of freedom left free by a fixed joint.
|
||||||
const FREE_FIXED_AXES = 0;
|
const FREE_FIXED_AXES = 0;
|
||||||
|
/// The set of all translational degrees of freedom.
|
||||||
const LIN_AXES = Self::X.bits() | Self::Y.bits();
|
const LIN_AXES = Self::X.bits() | Self::Y.bits();
|
||||||
|
/// The set of all angular degrees of freedom.
|
||||||
const ANG_AXES = Self::ANG_X.bits();
|
const ANG_AXES = Self::ANG_X.bits();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Identifiers of degrees of freedoms of a joint.
|
||||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||||
#[derive(Copy, Clone, Debug, PartialEq)]
|
#[derive(Copy, Clone, Debug, PartialEq)]
|
||||||
pub enum JointAxis {
|
pub enum JointAxis {
|
||||||
|
/// The translational degree of freedom along the joint’s local X axis.
|
||||||
X = 0,
|
X = 0,
|
||||||
|
/// The translational degree of freedom along the joint’s local Y axis.
|
||||||
Y,
|
Y,
|
||||||
|
/// The translational degree of freedom along the joint’s local Z axis.
|
||||||
#[cfg(feature = "dim3")]
|
#[cfg(feature = "dim3")]
|
||||||
Z,
|
Z,
|
||||||
|
/// The rotational degree of freedom along the joint’s local X axis.
|
||||||
AngX,
|
AngX,
|
||||||
|
/// The rotational degree of freedom along the joint’s local Y axis.
|
||||||
#[cfg(feature = "dim3")]
|
#[cfg(feature = "dim3")]
|
||||||
AngY,
|
AngY,
|
||||||
|
/// The rotational degree of freedom along the joint’s local Z axis.
|
||||||
#[cfg(feature = "dim3")]
|
#[cfg(feature = "dim3")]
|
||||||
AngZ,
|
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))]
|
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||||
#[derive(Copy, Clone, Debug, PartialEq)]
|
#[derive(Copy, Clone, Debug, PartialEq)]
|
||||||
pub struct JointLimits<N> {
|
pub struct JointLimits<N> {
|
||||||
|
/// The minimum bound of the joint limit.
|
||||||
pub min: N,
|
pub min: N,
|
||||||
|
/// The maximum bound of the joint limit.
|
||||||
pub max: N,
|
pub max: N,
|
||||||
|
/// The impulse applied to enforce the joint’s limit.
|
||||||
pub impulse: N,
|
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))]
|
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||||
#[derive(Copy, Clone, Debug, PartialEq)]
|
#[derive(Copy, Clone, Debug, PartialEq)]
|
||||||
pub struct JointMotor {
|
pub struct JointMotor {
|
||||||
|
/// The target velocity of the motor.
|
||||||
pub target_vel: Real,
|
pub target_vel: Real,
|
||||||
|
/// The target position of the motor.
|
||||||
pub target_pos: Real,
|
pub target_pos: Real,
|
||||||
|
/// The stiffness coefficient of the motor’s spring-like equation.
|
||||||
pub stiffness: Real,
|
pub stiffness: Real,
|
||||||
|
/// The damping coefficient of the motor’s spring-like equation.
|
||||||
pub damping: Real,
|
pub damping: Real,
|
||||||
|
/// The maximum force this motor can deliver.
|
||||||
pub max_force: Real,
|
pub max_force: Real,
|
||||||
|
/// The impulse applied by this motor.
|
||||||
pub impulse: Real,
|
pub impulse: Real,
|
||||||
|
/// The spring-like model used for simulating this motor.
|
||||||
pub model: MotorModel,
|
pub model: MotorModel,
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -130,14 +178,27 @@ impl JointMotor {
|
|||||||
|
|
||||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||||
#[derive(Copy, Clone, Debug, PartialEq)]
|
#[derive(Copy, Clone, Debug, PartialEq)]
|
||||||
|
/// A generic joint.
|
||||||
pub struct GenericJoint {
|
pub struct GenericJoint {
|
||||||
|
/// The joint’s frame, expressed in the first rigid-body’s local-space.
|
||||||
pub local_frame1: Isometry<Real>,
|
pub local_frame1: Isometry<Real>,
|
||||||
|
/// The joint’s frame, expressed in the second rigid-body’s local-space.
|
||||||
pub local_frame2: Isometry<Real>,
|
pub local_frame2: Isometry<Real>,
|
||||||
|
/// The degrees-of-freedoms locked by this joint.
|
||||||
pub locked_axes: JointAxesMask,
|
pub locked_axes: JointAxesMask,
|
||||||
|
/// The degrees-of-freedoms limited by this joint.
|
||||||
pub limit_axes: JointAxesMask,
|
pub limit_axes: JointAxesMask,
|
||||||
|
/// The degrees-of-freedoms motorised by this joint.
|
||||||
pub motor_axes: JointAxesMask,
|
pub motor_axes: JointAxesMask,
|
||||||
|
/// The coupled degrees of freedom of this joint.
|
||||||
pub coupled_axes: JointAxesMask,
|
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],
|
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],
|
pub motors: [JointMotor; SPATIAL_DIM],
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -157,11 +218,13 @@ impl Default for GenericJoint {
|
|||||||
}
|
}
|
||||||
|
|
||||||
impl GenericJoint {
|
impl GenericJoint {
|
||||||
|
/// Creates a new generic joint that locks the specified degrees of freedom.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn new(locked_axes: JointAxesMask) -> Self {
|
pub fn new(locked_axes: JointAxesMask) -> Self {
|
||||||
*Self::default().lock_axes(locked_axes)
|
*Self::default().lock_axes(locked_axes)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#[cfg(feature = "simd-is-enabled")]
|
||||||
/// Can this joint use SIMD-accelerated constraint formulations?
|
/// Can this joint use SIMD-accelerated constraint formulations?
|
||||||
pub(crate) fn supports_simd_constraints(&self) -> bool {
|
pub(crate) fn supports_simd_constraints(&self) -> bool {
|
||||||
self.limit_axes.is_empty() && self.motor_axes.is_empty()
|
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 {
|
pub fn lock_axes(&mut self, axes: JointAxesMask) -> &mut Self {
|
||||||
self.locked_axes |= axes;
|
self.locked_axes |= axes;
|
||||||
self
|
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 {
|
pub fn set_local_frame1(&mut self, local_frame: Isometry<Real>) -> &mut Self {
|
||||||
self.local_frame1 = local_frame;
|
self.local_frame1 = local_frame;
|
||||||
self
|
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 {
|
pub fn set_local_frame2(&mut self, local_frame: Isometry<Real>) -> &mut Self {
|
||||||
self.local_frame2 = local_frame;
|
self.local_frame2 = local_frame;
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The principal (local X) axis of this joint, expressed in the first rigid-body’s local-space.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_axis1(&self) -> UnitVector<Real> {
|
pub fn local_axis1(&self) -> UnitVector<Real> {
|
||||||
self.local_frame1 * Vector::x_axis()
|
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 {
|
pub fn set_local_axis1(&mut self, local_axis: UnitVector<Real>) -> &mut Self {
|
||||||
self.local_frame1.rotation = Self::complete_ang_frame(local_axis);
|
self.local_frame1.rotation = Self::complete_ang_frame(local_axis);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The principal (local X) axis of this joint, expressed in the second rigid-body’s local-space.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_axis2(&self) -> UnitVector<Real> {
|
pub fn local_axis2(&self) -> UnitVector<Real> {
|
||||||
self.local_frame2 * Vector::x_axis()
|
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 {
|
pub fn set_local_axis2(&mut self, local_axis: UnitVector<Real>) -> &mut Self {
|
||||||
self.local_frame2.rotation = Self::complete_ang_frame(local_axis);
|
self.local_frame2.rotation = Self::complete_ang_frame(local_axis);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The anchor of this joint, expressed in the first rigid-body’s local-space.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_anchor1(&self) -> Point<Real> {
|
pub fn local_anchor1(&self) -> Point<Real> {
|
||||||
self.local_frame1.translation.vector.into()
|
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 {
|
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
|
||||||
self.local_frame1.translation.vector = anchor1.coords;
|
self.local_frame1.translation.vector = anchor1.coords;
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The anchor of this joint, expressed in the second rigid-body’s local-space.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_anchor2(&self) -> Point<Real> {
|
pub fn local_anchor2(&self) -> Point<Real> {
|
||||||
self.local_frame2.translation.vector.into()
|
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 {
|
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
|
||||||
self.local_frame2.translation.vector = anchor2.coords;
|
self.local_frame2.translation.vector = anchor2.coords;
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The joint limits along the specified axis.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits<Real>> {
|
pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits<Real>> {
|
||||||
let i = axis as usize;
|
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 {
|
pub fn set_limits(&mut self, axis: JointAxis, limits: [Real; 2]) -> &mut Self {
|
||||||
let i = axis as usize;
|
let i = axis as usize;
|
||||||
self.limit_axes |= axis.into();
|
self.limit_axes |= axis.into();
|
||||||
@@ -260,6 +336,7 @@ impl GenericJoint {
|
|||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The spring-like motor model along the specified axis of this joint.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn motor_model(&self, axis: JointAxis) -> Option<MotorModel> {
|
pub fn motor_model(&self, axis: JointAxis) -> Option<MotorModel> {
|
||||||
let i = axis as usize;
|
let i = axis as usize;
|
||||||
@@ -303,11 +380,13 @@ impl GenericJoint {
|
|||||||
self.set_motor(axis, target_pos, 0.0, stiffness, damping)
|
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 {
|
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.motors[axis as usize].max_force = max_force;
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The motor affecting the joint’s degree of freedom along the specified axis.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn motor(&self, axis: JointAxis) -> Option<&JointMotor> {
|
pub fn motor(&self, axis: JointAxis) -> Option<&JointMotor> {
|
||||||
let i = axis as usize;
|
let i = axis as usize;
|
||||||
@@ -339,6 +418,7 @@ impl GenericJoint {
|
|||||||
|
|
||||||
macro_rules! joint_conversion_methods(
|
macro_rules! joint_conversion_methods(
|
||||||
($as_joint: ident, $as_joint_mut: ident, $Joint: ty, $axes: expr) => {
|
($as_joint: ident, $as_joint_mut: ident, $Joint: ty, $axes: expr) => {
|
||||||
|
/// Converts the joint to its specific variant, if it is one.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn $as_joint(&self) -> Option<&$Joint> {
|
pub fn $as_joint(&self) -> Option<&$Joint> {
|
||||||
if self.locked_axes == $axes {
|
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]
|
#[must_use]
|
||||||
pub fn $as_joint_mut(&mut self) -> Option<&mut $Joint> {
|
pub fn $as_joint_mut(&mut self) -> Option<&mut $Joint> {
|
||||||
if self.locked_axes == $axes {
|
if self.locked_axes == $axes {
|
||||||
@@ -392,63 +473,74 @@ impl GenericJoint {
|
|||||||
);
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Create generic joints using the builder pattern.
|
||||||
#[derive(Copy, Clone, Debug)]
|
#[derive(Copy, Clone, Debug)]
|
||||||
pub struct GenericJointBuilder(GenericJoint);
|
pub struct GenericJointBuilder(GenericJoint);
|
||||||
|
|
||||||
impl GenericJointBuilder {
|
impl GenericJointBuilder {
|
||||||
|
/// Creates a new generic joint builder.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn new(locked_axes: JointAxesMask) -> Self {
|
pub fn new(locked_axes: JointAxesMask) -> Self {
|
||||||
Self(GenericJoint::new(locked_axes))
|
Self(GenericJoint::new(locked_axes))
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the degrees of freedom locked by the joint.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn lock_axes(mut self, axes: JointAxesMask) -> Self {
|
pub fn locked_axes(mut self, axes: JointAxesMask) -> Self {
|
||||||
self.0.lock_axes(axes);
|
self.0.locked_axes = axes;
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the joint’s frame, expressed in the first rigid-body’s local-space.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
|
pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
|
||||||
self.0.set_local_frame1(local_frame);
|
self.0.set_local_frame1(local_frame);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the joint’s frame, expressed in the second rigid-body’s local-space.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
|
pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
|
||||||
self.0.set_local_frame2(local_frame);
|
self.0.set_local_frame2(local_frame);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the principal (local X) axis of this joint, expressed in the first rigid-body’s local-space.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_axis1(mut self, local_axis: UnitVector<Real>) -> Self {
|
pub fn local_axis1(mut self, local_axis: UnitVector<Real>) -> Self {
|
||||||
self.0.set_local_axis1(local_axis);
|
self.0.set_local_axis1(local_axis);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the principal (local X) axis of this joint, expressed in the second rigid-body’s local-space.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_axis2(mut self, local_axis: UnitVector<Real>) -> Self {
|
pub fn local_axis2(mut self, local_axis: UnitVector<Real>) -> Self {
|
||||||
self.0.set_local_axis2(local_axis);
|
self.0.set_local_axis2(local_axis);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the anchor of this joint, expressed in the first rigid-body’s local-space.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
|
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
|
||||||
self.0.set_local_anchor1(anchor1);
|
self.0.set_local_anchor1(anchor1);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the anchor of this joint, expressed in the second rigid-body’s local-space.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
|
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
|
||||||
self.0.set_local_anchor2(anchor2);
|
self.0.set_local_anchor2(anchor2);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the joint limits along the specified axis.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn limits(mut self, axis: JointAxis, limits: [Real; 2]) -> Self {
|
pub fn limits(mut self, axis: JointAxis, limits: [Real; 2]) -> Self {
|
||||||
self.0.set_limits(axis, limits);
|
self.0.set_limits(axis, limits);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the coupled degrees of freedom for this joint’s limits and motor.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn coupled_axes(mut self, axes: JointAxesMask) -> Self {
|
pub fn coupled_axes(mut self, axes: JointAxesMask) -> Self {
|
||||||
self.0.coupled_axes = axes;
|
self.0.coupled_axes = axes;
|
||||||
@@ -498,12 +590,14 @@ impl GenericJointBuilder {
|
|||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the maximum force the motor can deliver along the specified axis.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn motor_max_force(mut self, axis: JointAxis, max_force: Real) -> Self {
|
pub fn motor_max_force(mut self, axis: JointAxis, max_force: Real) -> Self {
|
||||||
self.0.set_motor_max_force(axis, max_force);
|
self.0.set_motor_max_force(axis, max_force);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Builds the generic joint.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn build(self) -> GenericJoint {
|
pub fn build(self) -> GenericJoint {
|
||||||
self.0
|
self.0
|
||||||
|
|||||||
@@ -3,14 +3,17 @@ use crate::math::{Real, SpacialVector};
|
|||||||
|
|
||||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||||
#[derive(Clone, Debug, PartialEq)]
|
#[derive(Clone, Debug, PartialEq)]
|
||||||
/// A joint attached to two bodies.
|
/// An impulse-based joint attached to two bodies.
|
||||||
pub struct ImpulseJoint {
|
pub struct ImpulseJoint {
|
||||||
/// Handle to the first body attached to this joint.
|
/// Handle to the first body attached to this joint.
|
||||||
pub body1: RigidBodyHandle,
|
pub body1: RigidBodyHandle,
|
||||||
/// Handle to the second body attached to this joint.
|
/// Handle to the second body attached to this joint.
|
||||||
pub body2: RigidBodyHandle,
|
pub body2: RigidBodyHandle,
|
||||||
|
|
||||||
|
/// The joint’s description.
|
||||||
pub data: GenericJoint,
|
pub data: GenericJoint,
|
||||||
|
|
||||||
|
/// The impulses applied by this joint.
|
||||||
pub impulses: SpacialVector<Real>,
|
pub impulses: SpacialVector<Real>,
|
||||||
|
|
||||||
// A joint needs to know its handle to simplify its removal.
|
// A joint needs to know its handle to simplify its removal.
|
||||||
|
|||||||
@@ -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();
|
let mut mb = Multibody::new();
|
||||||
mb.root_is_dynamic = true;
|
mb.root_is_dynamic = true;
|
||||||
let joint = MultibodyJoint::free(Isometry::identity());
|
let joint = MultibodyJoint::free(Isometry::identity());
|
||||||
@@ -128,7 +128,7 @@ impl Multibody {
|
|||||||
mb
|
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 result = vec![];
|
||||||
let mut link2mb = vec![usize::MAX; self.links.len()];
|
let mut link2mb = vec![usize::MAX; self.links.len()];
|
||||||
let mut link_id2new_id = vec![usize::MAX; self.links.len()];
|
let mut link_id2new_id = vec![usize::MAX; self.links.len()];
|
||||||
@@ -187,7 +187,7 @@ impl Multibody {
|
|||||||
result
|
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_root_ndofs = rhs.links[0].joint.ndofs();
|
||||||
let rhs_copy_shift = self.ndofs + rhs_root_ndofs;
|
let rhs_copy_shift = self.ndofs + rhs_root_ndofs;
|
||||||
let rhs_copy_ndofs = rhs.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);
|
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> {
|
pub fn inv_augmented_mass(&self) -> &LU<Real, Dynamic, Dynamic> {
|
||||||
&self.inv_augmented_mass
|
&self.inv_augmented_mass
|
||||||
}
|
}
|
||||||
@@ -298,7 +299,7 @@ impl Multibody {
|
|||||||
&mut self.damping
|
&mut self.damping
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn add_link(
|
pub(crate) fn add_link(
|
||||||
&mut self,
|
&mut self,
|
||||||
parent: Option<usize>, // FIXME: should be a RigidBodyHandle?
|
parent: Option<usize>, // FIXME: should be a RigidBodyHandle?
|
||||||
dof: MultibodyJoint,
|
dof: MultibodyJoint,
|
||||||
@@ -368,7 +369,7 @@ impl Multibody {
|
|||||||
.extend((0..num_jacobians).map(|_| Jacobian::zeros(0)));
|
.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
|
where
|
||||||
Bodies: ComponentSet<RigidBodyMassProps>
|
Bodies: ComponentSet<RigidBodyMassProps>
|
||||||
+ ComponentSet<RigidBodyForces>
|
+ ComponentSet<RigidBodyForces>
|
||||||
@@ -451,7 +452,7 @@ impl Multibody {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// Computes the constant terms of the dynamics.
|
/// 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
|
where
|
||||||
Bodies: ComponentSetMut<RigidBodyVelocity> + ComponentSet<RigidBodyMassProps>,
|
Bodies: ComponentSetMut<RigidBodyVelocity> + ComponentSet<RigidBodyMassProps>,
|
||||||
{
|
{
|
||||||
@@ -756,36 +757,40 @@ impl Multibody {
|
|||||||
)
|
)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The generalized accelerations of this multibodies.
|
||||||
#[inline]
|
#[inline]
|
||||||
pub fn generalized_acceleration(&self) -> DVectorSlice<Real> {
|
pub fn generalized_acceleration(&self) -> DVectorSlice<Real> {
|
||||||
self.accelerations.rows(0, self.ndofs)
|
self.accelerations.rows(0, self.ndofs)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The generalized velocities of this multibodies.
|
||||||
#[inline]
|
#[inline]
|
||||||
pub fn generalized_velocity(&self) -> DVectorSlice<Real> {
|
pub fn generalized_velocity(&self) -> DVectorSlice<Real> {
|
||||||
self.velocities.rows(0, self.ndofs)
|
self.velocities.rows(0, self.ndofs)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The mutable generalized velocities of this multibodies.
|
||||||
#[inline]
|
#[inline]
|
||||||
pub fn generalized_velocity_mut(&mut self) -> DVectorSliceMut<Real> {
|
pub fn generalized_velocity_mut(&mut self) -> DVectorSliceMut<Real> {
|
||||||
self.velocities.rows_mut(0, self.ndofs)
|
self.velocities.rows_mut(0, self.ndofs)
|
||||||
}
|
}
|
||||||
|
|
||||||
#[inline]
|
#[inline]
|
||||||
pub fn integrate(&mut self, dt: Real) {
|
pub(crate) fn integrate(&mut self, dt: Real) {
|
||||||
for rb in self.links.iter_mut() {
|
for rb in self.links.iter_mut() {
|
||||||
rb.joint
|
rb.joint
|
||||||
.integrate(dt, &self.velocities.as_slice()[rb.assembly_id..])
|
.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]) {
|
pub fn apply_displacements(&mut self, disp: &[Real]) {
|
||||||
for link in self.links.iter_mut() {
|
for link in self.links.iter_mut() {
|
||||||
link.joint.apply_displacement(&disp[link.assembly_id..])
|
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
|
where
|
||||||
Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyPosition>,
|
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)
|
pub fn forward_kinematics<Bodies>(&mut self, bodies: &mut Bodies, update_mass_props: bool)
|
||||||
where
|
where
|
||||||
Bodies: ComponentSet<RigidBodyType>
|
Bodies: ComponentSet<RigidBodyType>
|
||||||
@@ -917,12 +923,13 @@ impl Multibody {
|
|||||||
self.update_body_jacobians();
|
self.update_body_jacobians();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The total number of freedoms of this multibody.
|
||||||
#[inline]
|
#[inline]
|
||||||
pub fn ndofs(&self) -> usize {
|
pub fn ndofs(&self) -> usize {
|
||||||
self.ndofs
|
self.ndofs
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn fill_jacobians(
|
pub(crate) fn fill_jacobians(
|
||||||
&self,
|
&self,
|
||||||
link_id: usize,
|
link_id: usize,
|
||||||
unit_force: Vector<Real>,
|
unit_force: Vector<Real>,
|
||||||
@@ -964,14 +971,16 @@ impl Multibody {
|
|||||||
(j.dot(&invm_j), j.dot(&self.generalized_velocity()))
|
(j.dot(&invm_j), j.dot(&self.generalized_velocity()))
|
||||||
}
|
}
|
||||||
|
|
||||||
#[inline]
|
// #[cfg(feature = "parallel")]
|
||||||
pub fn has_active_internal_constraints(&self) -> bool {
|
// #[inline]
|
||||||
self.links()
|
// pub(crate) fn has_active_internal_constraints(&self) -> bool {
|
||||||
.any(|link| link.joint().num_velocity_constraints() != 0)
|
// self.links()
|
||||||
}
|
// .any(|link| link.joint().num_velocity_constraints() != 0)
|
||||||
|
// }
|
||||||
|
|
||||||
|
#[cfg(feature = "parallel")]
|
||||||
#[inline]
|
#[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
|
let num_constraints: usize = self
|
||||||
.links
|
.links
|
||||||
.iter()
|
.iter()
|
||||||
@@ -981,7 +990,7 @@ impl Multibody {
|
|||||||
}
|
}
|
||||||
|
|
||||||
#[inline]
|
#[inline]
|
||||||
pub fn generate_internal_constraints(
|
pub(crate) fn generate_internal_constraints(
|
||||||
&self,
|
&self,
|
||||||
params: &IntegrationParameters,
|
params: &IntegrationParameters,
|
||||||
j_id: &mut usize,
|
j_id: &mut usize,
|
||||||
|
|||||||
@@ -13,13 +13,16 @@ use na::{UnitQuaternion, Vector3};
|
|||||||
|
|
||||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||||
#[derive(Copy, Clone, Debug)]
|
#[derive(Copy, Clone, Debug)]
|
||||||
|
/// An joint attached to two bodies based on the reduced coordinates formalism.
|
||||||
pub struct MultibodyJoint {
|
pub struct MultibodyJoint {
|
||||||
|
/// The joint’s description.
|
||||||
pub data: GenericJoint,
|
pub data: GenericJoint,
|
||||||
pub(crate) coords: SpacialVector<Real>,
|
pub(crate) coords: SpacialVector<Real>,
|
||||||
pub(crate) joint_rot: Rotation<Real>,
|
pub(crate) joint_rot: Rotation<Real>,
|
||||||
}
|
}
|
||||||
|
|
||||||
impl MultibodyJoint {
|
impl MultibodyJoint {
|
||||||
|
/// Creates a new multibody joint from its description.
|
||||||
pub fn new(data: GenericJoint) -> Self {
|
pub fn new(data: GenericJoint) -> Self {
|
||||||
Self {
|
Self {
|
||||||
data,
|
data,
|
||||||
@@ -45,9 +48,9 @@ impl MultibodyJoint {
|
|||||||
self.joint_rot = pos.rotation;
|
self.joint_rot = pos.rotation;
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn local_joint_rot(&self) -> &Rotation<Real> {
|
// pub(crate) fn local_joint_rot(&self) -> &Rotation<Real> {
|
||||||
&self.joint_rot
|
// &self.joint_rot
|
||||||
}
|
// }
|
||||||
|
|
||||||
fn num_free_lin_dofs(&self) -> usize {
|
fn num_free_lin_dofs(&self) -> usize {
|
||||||
let locked_bits = self.data.locked_axes.bits();
|
let locked_bits = self.data.locked_axes.bits();
|
||||||
|
|||||||
@@ -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)> {
|
pub fn iter(&self) -> impl Iterator<Item = (MultibodyJointHandle, &Multibody, &MultibodyLink)> {
|
||||||
self.rb2mb
|
self.rb2mb
|
||||||
.iter()
|
.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,
|
&mut self,
|
||||||
rb_to_remove: RigidBodyHandle,
|
rb_to_remove: RigidBodyHandle,
|
||||||
islands: &mut IslandManager,
|
islands: &mut IslandManager,
|
||||||
|
|||||||
@@ -18,12 +18,13 @@ pub struct MultibodyLink {
|
|||||||
/*
|
/*
|
||||||
* Change at each time step.
|
* Change at each time step.
|
||||||
*/
|
*/
|
||||||
|
/// The multibody joint of this link.
|
||||||
pub joint: MultibodyJoint,
|
pub joint: MultibodyJoint,
|
||||||
// TODO: should this be removed in favor of the rigid-body position?
|
// TODO: should this be removed in favor of the rigid-body position?
|
||||||
pub local_to_world: Isometry<Real>,
|
pub(crate) local_to_world: Isometry<Real>,
|
||||||
pub local_to_parent: Isometry<Real>,
|
pub(crate) local_to_parent: Isometry<Real>,
|
||||||
pub shift02: Vector<Real>,
|
pub(crate) shift02: Vector<Real>,
|
||||||
pub shift23: Vector<Real>,
|
pub(crate) shift23: Vector<Real>,
|
||||||
|
|
||||||
/// The velocity added by the joint, in world-space.
|
/// The velocity added by the joint, in world-space.
|
||||||
pub(crate) joint_velocity: RigidBodyVelocity,
|
pub(crate) joint_velocity: RigidBodyVelocity,
|
||||||
@@ -56,10 +57,12 @@ impl MultibodyLink {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The multibody joint of this link.
|
||||||
pub fn joint(&self) -> &MultibodyJoint {
|
pub fn joint(&self) -> &MultibodyJoint {
|
||||||
&self.joint
|
&self.joint
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The handle of the rigid-body of this link.
|
||||||
pub fn rigid_body_handle(&self) -> RigidBodyHandle {
|
pub fn rigid_body_handle(&self) -> RigidBodyHandle {
|
||||||
self.rigid_body
|
self.rigid_body
|
||||||
}
|
}
|
||||||
@@ -86,11 +89,13 @@ impl MultibodyLink {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The world-space transform of the rigid-body attached to this link.
|
||||||
#[inline]
|
#[inline]
|
||||||
pub fn local_to_world(&self) -> &Isometry<Real> {
|
pub fn local_to_world(&self) -> &Isometry<Real> {
|
||||||
&self.local_to_world
|
&self.local_to_world
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The position of the rigid-body attached to this link relative to its parent.
|
||||||
#[inline]
|
#[inline]
|
||||||
pub fn local_to_parent(&self) -> &Isometry<Real> {
|
pub fn local_to_parent(&self) -> &Isometry<Real> {
|
||||||
&self.local_to_parent
|
&self.local_to_parent
|
||||||
|
|||||||
@@ -7,11 +7,15 @@ use super::{JointLimits, JointMotor};
|
|||||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||||
#[derive(Copy, Clone, Debug, PartialEq)]
|
#[derive(Copy, Clone, Debug, PartialEq)]
|
||||||
#[repr(transparent)]
|
#[repr(transparent)]
|
||||||
|
/// A prismatic joint, locks all relative motion between two bodies except for translation along the joint’s principal axis.
|
||||||
pub struct PrismaticJoint {
|
pub struct PrismaticJoint {
|
||||||
data: GenericJoint,
|
data: GenericJoint,
|
||||||
}
|
}
|
||||||
|
|
||||||
impl PrismaticJoint {
|
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 {
|
pub fn new(axis: UnitVector<Real>) -> Self {
|
||||||
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_PRISMATIC_AXES)
|
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_PRISMATIC_AXES)
|
||||||
.local_axis1(axis)
|
.local_axis1(axis)
|
||||||
@@ -20,46 +24,60 @@ impl PrismaticJoint {
|
|||||||
Self { data }
|
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]
|
#[must_use]
|
||||||
pub fn local_anchor1(&self) -> Point<Real> {
|
pub fn local_anchor1(&self) -> Point<Real> {
|
||||||
self.data.local_anchor1()
|
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 {
|
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
|
||||||
self.data.set_local_anchor1(anchor1);
|
self.data.set_local_anchor1(anchor1);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The joint’s anchor, expressed in the local-space of the second rigid-body.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_anchor2(&self) -> Point<Real> {
|
pub fn local_anchor2(&self) -> Point<Real> {
|
||||||
self.data.local_anchor2()
|
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 {
|
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
|
||||||
self.data.set_local_anchor2(anchor2);
|
self.data.set_local_anchor2(anchor2);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The principal axis of the joint, expressed in the local-space of the first rigid-body.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_axis1(&self) -> UnitVector<Real> {
|
pub fn local_axis1(&self) -> UnitVector<Real> {
|
||||||
self.data.local_axis1()
|
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 {
|
pub fn set_local_axis1(&mut self, axis1: UnitVector<Real>) -> &mut Self {
|
||||||
self.data.set_local_axis1(axis1);
|
self.data.set_local_axis1(axis1);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The principal axis of the joint, expressed in the local-space of the second rigid-body.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_axis2(&self) -> UnitVector<Real> {
|
pub fn local_axis2(&self) -> UnitVector<Real> {
|
||||||
self.data.local_axis2()
|
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 {
|
pub fn set_local_axis2(&mut self, axis2: UnitVector<Real>) -> &mut Self {
|
||||||
self.data.set_local_axis2(axis2);
|
self.data.set_local_axis2(axis2);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The motor affecting the joint’s translational degree of freedom.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn motor(&self) -> Option<&JointMotor> {
|
pub fn motor(&self) -> Option<&JointMotor> {
|
||||||
self.data.motor(JointAxis::X)
|
self.data.motor(JointAxis::X)
|
||||||
@@ -103,16 +121,19 @@ impl PrismaticJoint {
|
|||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the maximum force the motor can deliver.
|
||||||
pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self {
|
pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self {
|
||||||
self.data.set_motor_max_force(JointAxis::X, max_force);
|
self.data.set_motor_max_force(JointAxis::X, max_force);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The limit distance attached bodies can translate along the joint’s principal axis.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn limits(&self) -> Option<&JointLimits<Real>> {
|
pub fn limits(&self) -> Option<&JointLimits<Real>> {
|
||||||
self.data.limits(JointAxis::X)
|
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 {
|
pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self {
|
||||||
self.data.set_limits(JointAxis::X, limits);
|
self.data.set_limits(JointAxis::X, limits);
|
||||||
self
|
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);
|
pub struct PrismaticJointBuilder(PrismaticJoint);
|
||||||
|
|
||||||
impl PrismaticJointBuilder {
|
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 {
|
pub fn new(axis: UnitVector<Real>) -> Self {
|
||||||
Self(PrismaticJoint::new(axis))
|
Self(PrismaticJoint::new(axis))
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
|
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
|
||||||
self.0.set_local_anchor1(anchor1);
|
self.0.set_local_anchor1(anchor1);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
|
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
|
||||||
self.0.set_local_anchor2(anchor2);
|
self.0.set_local_anchor2(anchor2);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the principal axis of the joint, expressed in the local-space of the first rigid-body.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_axis1(mut self, axis1: UnitVector<Real>) -> Self {
|
pub fn local_axis1(mut self, axis1: UnitVector<Real>) -> Self {
|
||||||
self.0.set_local_axis1(axis1);
|
self.0.set_local_axis1(axis1);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the principal axis of the joint, expressed in the local-space of the second rigid-body.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_axis2(mut self, axis2: UnitVector<Real>) -> Self {
|
pub fn local_axis2(mut self, axis2: UnitVector<Real>) -> Self {
|
||||||
self.0.set_local_axis2(axis2);
|
self.0.set_local_axis2(axis2);
|
||||||
@@ -190,18 +222,21 @@ impl PrismaticJointBuilder {
|
|||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the maximum force the motor can deliver.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn motor_max_force(mut self, max_force: Real) -> Self {
|
pub fn motor_max_force(mut self, max_force: Real) -> Self {
|
||||||
self.0.set_motor_max_force(max_force);
|
self.0.set_motor_max_force(max_force);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the `[min,max]` limit distances attached bodies can translate along the joint’s principal axis.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn limits(mut self, limits: [Real; 2]) -> Self {
|
pub fn limits(mut self, limits: [Real; 2]) -> Self {
|
||||||
self.0.set_limits(limits);
|
self.0.set_limits(limits);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Builds the prismatic joint.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn build(self) -> PrismaticJoint {
|
pub fn build(self) -> PrismaticJoint {
|
||||||
self.0
|
self.0
|
||||||
|
|||||||
@@ -8,17 +8,22 @@ use crate::math::UnitVector;
|
|||||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||||
#[derive(Copy, Clone, Debug, PartialEq)]
|
#[derive(Copy, Clone, Debug, PartialEq)]
|
||||||
#[repr(transparent)]
|
#[repr(transparent)]
|
||||||
|
/// A revolute joint, locks all relative motion except for rotation along the joint’s principal axis.
|
||||||
pub struct RevoluteJoint {
|
pub struct RevoluteJoint {
|
||||||
data: GenericJoint,
|
data: GenericJoint,
|
||||||
}
|
}
|
||||||
|
|
||||||
impl RevoluteJoint {
|
impl RevoluteJoint {
|
||||||
|
/// Creates a new revolute joint allowing only relative rotations.
|
||||||
#[cfg(feature = "dim2")]
|
#[cfg(feature = "dim2")]
|
||||||
pub fn new() -> Self {
|
pub fn new() -> Self {
|
||||||
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES);
|
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES);
|
||||||
Self { data: data.build() }
|
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")]
|
#[cfg(feature = "dim3")]
|
||||||
pub fn new(axis: UnitVector<Real>) -> Self {
|
pub fn new(axis: UnitVector<Real>) -> Self {
|
||||||
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES)
|
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES)
|
||||||
@@ -28,30 +33,36 @@ impl RevoluteJoint {
|
|||||||
Self { data }
|
Self { data }
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The underlying generic joint.
|
||||||
pub fn data(&self) -> &GenericJoint {
|
pub fn data(&self) -> &GenericJoint {
|
||||||
&self.data
|
&self.data
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The joint’s anchor, expressed in the local-space of the first rigid-body.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_anchor1(&self) -> Point<Real> {
|
pub fn local_anchor1(&self) -> Point<Real> {
|
||||||
self.data.local_anchor1()
|
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 {
|
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
|
||||||
self.data.set_local_anchor1(anchor1);
|
self.data.set_local_anchor1(anchor1);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The joint’s anchor, expressed in the local-space of the second rigid-body.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_anchor2(&self) -> Point<Real> {
|
pub fn local_anchor2(&self) -> Point<Real> {
|
||||||
self.data.local_anchor2()
|
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 {
|
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
|
||||||
self.data.set_local_anchor2(anchor2);
|
self.data.set_local_anchor2(anchor2);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The motor affecting the joint’s rotational degree of freedom.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn motor(&self) -> Option<&JointMotor> {
|
pub fn motor(&self) -> Option<&JointMotor> {
|
||||||
self.data.motor(JointAxis::AngX)
|
self.data.motor(JointAxis::AngX)
|
||||||
@@ -95,16 +106,19 @@ impl RevoluteJoint {
|
|||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the maximum force the motor can deliver.
|
||||||
pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self {
|
pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self {
|
||||||
self.data.set_motor_max_force(JointAxis::AngX, max_force);
|
self.data.set_motor_max_force(JointAxis::AngX, max_force);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The limit angle attached bodies can translate along the joint’s principal axis.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn limits(&self) -> Option<&JointLimits<Real>> {
|
pub fn limits(&self) -> Option<&JointLimits<Real>> {
|
||||||
self.data.limits(JointAxis::AngX)
|
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 {
|
pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self {
|
||||||
self.data.set_limits(JointAxis::AngX, limits);
|
self.data.set_limits(JointAxis::AngX, limits);
|
||||||
self
|
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))]
|
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||||
#[derive(Copy, Clone, Debug, PartialEq)]
|
#[derive(Copy, Clone, Debug, PartialEq)]
|
||||||
pub struct RevoluteJointBuilder(RevoluteJoint);
|
pub struct RevoluteJointBuilder(RevoluteJoint);
|
||||||
|
|
||||||
impl RevoluteJointBuilder {
|
impl RevoluteJointBuilder {
|
||||||
|
/// Creates a new revolute joint builder.
|
||||||
#[cfg(feature = "dim2")]
|
#[cfg(feature = "dim2")]
|
||||||
pub fn new() -> Self {
|
pub fn new() -> Self {
|
||||||
Self(RevoluteJoint::new())
|
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")]
|
#[cfg(feature = "dim3")]
|
||||||
pub fn new(axis: UnitVector<Real>) -> Self {
|
pub fn new(axis: UnitVector<Real>) -> Self {
|
||||||
Self(RevoluteJoint::new(axis))
|
Self(RevoluteJoint::new(axis))
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
|
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
|
||||||
self.0.set_local_anchor1(anchor1);
|
self.0.set_local_anchor1(anchor1);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
|
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
|
||||||
self.0.set_local_anchor2(anchor2);
|
self.0.set_local_anchor2(anchor2);
|
||||||
@@ -178,18 +201,21 @@ impl RevoluteJointBuilder {
|
|||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the maximum force the motor can deliver.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn motor_max_force(mut self, max_force: Real) -> Self {
|
pub fn motor_max_force(mut self, max_force: Real) -> Self {
|
||||||
self.0.set_motor_max_force(max_force);
|
self.0.set_motor_max_force(max_force);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the `[min,max]` limit angles attached bodies can rotate along the joint’s principal axis.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn limits(mut self, limits: [Real; 2]) -> Self {
|
pub fn limits(mut self, limits: [Real; 2]) -> Self {
|
||||||
self.0.set_limits(limits);
|
self.0.set_limits(limits);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Builds the revolute joint.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn build(self) -> RevoluteJoint {
|
pub fn build(self) -> RevoluteJoint {
|
||||||
self.0
|
self.0
|
||||||
|
|||||||
@@ -1,10 +1,13 @@
|
|||||||
use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
|
use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
|
||||||
use crate::dynamics::{JointAxis, MotorModel};
|
use crate::dynamics::{JointAxis, JointMotor, MotorModel};
|
||||||
use crate::math::{Point, Real};
|
use crate::math::{Point, Real};
|
||||||
|
|
||||||
|
use super::JointLimits;
|
||||||
|
|
||||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||||
#[derive(Copy, Clone, Debug, PartialEq)]
|
#[derive(Copy, Clone, Debug, PartialEq)]
|
||||||
#[repr(transparent)]
|
#[repr(transparent)]
|
||||||
|
/// A spherical joint, locks all relative translations between two bodies.
|
||||||
pub struct SphericalJoint {
|
pub struct SphericalJoint {
|
||||||
data: GenericJoint,
|
data: GenericJoint,
|
||||||
}
|
}
|
||||||
@@ -16,25 +19,47 @@ impl Default for SphericalJoint {
|
|||||||
}
|
}
|
||||||
|
|
||||||
impl SphericalJoint {
|
impl SphericalJoint {
|
||||||
|
/// Creates a new spherical joint locking all relative translations between two bodies.
|
||||||
pub fn new() -> Self {
|
pub fn new() -> Self {
|
||||||
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_SPHERICAL_AXES).build();
|
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_SPHERICAL_AXES).build();
|
||||||
Self { data }
|
Self { data }
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// The underlying generic joint.
|
||||||
pub fn data(&self) -> &GenericJoint {
|
pub fn data(&self) -> &GenericJoint {
|
||||||
&self.data
|
&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 {
|
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
|
||||||
self.data.set_local_anchor1(anchor1);
|
self.data.set_local_anchor1(anchor1);
|
||||||
self
|
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 {
|
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
|
||||||
self.data.set_local_anchor2(anchor2);
|
self.data.set_local_anchor2(anchor2);
|
||||||
self
|
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.
|
/// 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 {
|
pub fn set_motor_model(&mut self, axis: JointAxis, model: MotorModel) -> &mut Self {
|
||||||
self.data.set_motor_model(axis, model);
|
self.data.set_motor_model(axis, model);
|
||||||
@@ -79,11 +104,19 @@ impl SphericalJoint {
|
|||||||
self
|
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 {
|
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.data.set_motor_max_force(axis, max_force);
|
||||||
self
|
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 {
|
pub fn set_limits(&mut self, axis: JointAxis, limits: [Real; 2]) -> &mut Self {
|
||||||
self.data.set_limits(axis, limits);
|
self.data.set_limits(axis, limits);
|
||||||
self
|
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))]
|
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||||
#[derive(Copy, Clone, Debug, PartialEq)]
|
#[derive(Copy, Clone, Debug, PartialEq)]
|
||||||
pub struct SphericalJointBuilder(SphericalJoint);
|
pub struct SphericalJointBuilder(SphericalJoint);
|
||||||
@@ -107,16 +141,19 @@ impl Default for SphericalJointBuilder {
|
|||||||
}
|
}
|
||||||
|
|
||||||
impl SphericalJointBuilder {
|
impl SphericalJointBuilder {
|
||||||
|
/// Creates a new builder for spherical joints.
|
||||||
pub fn new() -> Self {
|
pub fn new() -> Self {
|
||||||
Self(SphericalJoint::new())
|
Self(SphericalJoint::new())
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
|
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
|
||||||
self.0.set_local_anchor1(anchor1);
|
self.0.set_local_anchor1(anchor1);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
|
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
|
||||||
self.0.set_local_anchor2(anchor2);
|
self.0.set_local_anchor2(anchor2);
|
||||||
@@ -166,18 +203,21 @@ impl SphericalJointBuilder {
|
|||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the maximum force the motor can deliver along the specified axis.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn motor_max_force(mut self, axis: JointAxis, max_force: Real) -> Self {
|
pub fn motor_max_force(mut self, axis: JointAxis, max_force: Real) -> Self {
|
||||||
self.0.set_motor_max_force(axis, max_force);
|
self.0.set_motor_max_force(axis, max_force);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the `[min,max]` limit distances attached bodies can rotate along the specified axis.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn limits(mut self, axis: JointAxis, limits: [Real; 2]) -> Self {
|
pub fn limits(mut self, axis: JointAxis, limits: [Real; 2]) -> Self {
|
||||||
self.0.set_limits(axis, limits);
|
self.0.set_limits(axis, limits);
|
||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Builds the spherical joint.
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn build(self) -> SphericalJoint {
|
pub fn build(self) -> SphericalJoint {
|
||||||
self.0
|
self.0
|
||||||
|
|||||||
@@ -133,6 +133,7 @@ impl RigidBody {
|
|||||||
self.rb_dominance.effective_group(&self.rb_type)
|
self.rb_dominance.effective_group(&self.rb_type)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the axes along which this rigid-body cannot translate or rotate.
|
||||||
#[inline]
|
#[inline]
|
||||||
pub fn set_locked_axes(&mut self, locked_axes: LockedAxes, wake_up: bool) {
|
pub fn set_locked_axes(&mut self, locked_axes: LockedAxes, wake_up: bool) {
|
||||||
if locked_axes != self.rb_mprops.flags {
|
if locked_axes != self.rb_mprops.flags {
|
||||||
@@ -995,6 +996,7 @@ impl RigidBodyBuilder {
|
|||||||
self
|
self
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Sets the axes along which this rigid-body cannot translate or rotate.
|
||||||
pub fn locked_axes(mut self, locked_axes: LockedAxes) -> Self {
|
pub fn locked_axes(mut self, locked_axes: LockedAxes) -> Self {
|
||||||
self.mprops_flags = locked_axes;
|
self.mprops_flags = locked_axes;
|
||||||
self
|
self
|
||||||
|
|||||||
@@ -639,9 +639,9 @@ pub struct RigidBodyForces {
|
|||||||
/// Gravity is multiplied by this scaling factor before it's
|
/// Gravity is multiplied by this scaling factor before it's
|
||||||
/// applied to this rigid-body.
|
/// applied to this rigid-body.
|
||||||
pub gravity_scale: Real,
|
pub gravity_scale: Real,
|
||||||
// Forces applied by the user.
|
/// Forces applied by the user.
|
||||||
pub user_force: Vector<Real>,
|
pub user_force: Vector<Real>,
|
||||||
// Torque applied by the user.
|
/// Torque applied by the user.
|
||||||
pub user_torque: AngVector<Real>,
|
pub user_torque: AngVector<Real>,
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -168,7 +168,7 @@ impl RigidBodySet {
|
|||||||
* Remove impulse_joints attached to this rigid-body.
|
* Remove impulse_joints attached to this rigid-body.
|
||||||
*/
|
*/
|
||||||
impulse_joints.remove_joints_attached_to_rigid_body(handle, islands, self);
|
impulse_joints.remove_joints_attached_to_rigid_body(handle, islands, self);
|
||||||
multibody_joints.remove_articulations_attached_to_rigid_body(handle, islands, self);
|
multibody_joints.remove_joints_attached_to_rigid_body(handle, islands, self);
|
||||||
|
|
||||||
Some(rb)
|
Some(rb)
|
||||||
}
|
}
|
||||||
@@ -260,6 +260,13 @@ impl RigidBodySet {
|
|||||||
})
|
})
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Update colliders positions after rigid-bodies moved.
|
||||||
|
///
|
||||||
|
/// When a rigid-body moves, the positions of the colliders attached to it need to be updated.
|
||||||
|
/// This update is generally automatically done at the beggining and the end of each simulation
|
||||||
|
/// step with `PhysicsPipeline::step`. If the positions need to be updated without running a
|
||||||
|
/// simulation step (for example when using the `QueryPipeline` alone), this method can be called
|
||||||
|
/// manually.
|
||||||
pub fn propagate_modified_body_positions_to_colliders(&self, colliders: &mut ColliderSet) {
|
pub fn propagate_modified_body_positions_to_colliders(&self, colliders: &mut ColliderSet) {
|
||||||
for body in self.modified_bodies.iter().filter_map(|h| self.get(*h)) {
|
for body in self.modified_bodies.iter().filter_map(|h| self.get(*h)) {
|
||||||
if body.changes.contains(RigidBodyChanges::POSITION) {
|
if body.changes.contains(RigidBodyChanges::POSITION) {
|
||||||
|
|||||||
@@ -1,14 +1,17 @@
|
|||||||
use crate::data::ComponentSet;
|
use crate::data::ComponentSet;
|
||||||
#[cfg(feature = "parallel")]
|
use crate::dynamics::{IslandManager, JointGraphEdge, JointIndex, RigidBodyIds};
|
||||||
use crate::dynamics::RigidBodyHandle;
|
|
||||||
use crate::dynamics::{IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet, RigidBodyIds};
|
|
||||||
use crate::geometry::{ContactManifold, ContactManifoldIndex};
|
use crate::geometry::{ContactManifold, ContactManifoldIndex};
|
||||||
|
|
||||||
#[cfg(feature = "simd-is-enabled")]
|
#[cfg(feature = "simd-is-enabled")]
|
||||||
use {
|
use {
|
||||||
crate::data::BundleSet,
|
crate::data::BundleSet,
|
||||||
crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH},
|
crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH},
|
||||||
vec_map::VecMap,
|
vec_map::VecMap,
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#[cfg(feature = "parallel")]
|
||||||
|
use crate::dynamics::{MultibodyJointSet, RigidBodyHandle};
|
||||||
|
|
||||||
#[cfg(feature = "parallel")]
|
#[cfg(feature = "parallel")]
|
||||||
pub(crate) trait PairInteraction {
|
pub(crate) trait PairInteraction {
|
||||||
fn body_pair(&self) -> (Option<RigidBodyHandle>, Option<RigidBodyHandle>);
|
fn body_pair(&self) -> (Option<RigidBodyHandle>, Option<RigidBodyHandle>);
|
||||||
@@ -195,16 +198,16 @@ impl InteractionGroups {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#[cfg(not(feature = "parallel"))]
|
// #[cfg(not(feature = "parallel"))]
|
||||||
pub fn clear(&mut self) {
|
// pub fn clear(&mut self) {
|
||||||
#[cfg(feature = "simd-is-enabled")]
|
// #[cfg(feature = "simd-is-enabled")]
|
||||||
{
|
// {
|
||||||
self.buckets.clear();
|
// self.buckets.clear();
|
||||||
self.body_masks.clear();
|
// self.body_masks.clear();
|
||||||
self.grouped_interactions.clear();
|
// self.grouped_interactions.clear();
|
||||||
}
|
// }
|
||||||
self.nongrouped_interactions.clear();
|
// self.nongrouped_interactions.clear();
|
||||||
}
|
// }
|
||||||
|
|
||||||
// TODO: there is a lot of duplicated code with group_manifolds here.
|
// TODO: there is a lot of duplicated code with group_manifolds here.
|
||||||
// But we don't refactor just now because we may end up with distinct
|
// But we don't refactor just now because we may end up with distinct
|
||||||
|
|||||||
@@ -7,15 +7,19 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
|
|||||||
};
|
};
|
||||||
use crate::dynamics::solver::DeltaVel;
|
use crate::dynamics::solver::DeltaVel;
|
||||||
use crate::dynamics::{
|
use crate::dynamics::{
|
||||||
ImpulseJoint, IntegrationParameters, JointAxesMask, JointGraphEdge, JointIndex, RigidBodyIds,
|
ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds,
|
||||||
RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
|
RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
|
||||||
};
|
};
|
||||||
#[cfg(feature = "simd-is-enabled")]
|
|
||||||
use crate::math::{Isometry, SimdReal, SIMD_WIDTH};
|
|
||||||
use crate::math::{Real, SPATIAL_DIM};
|
use crate::math::{Real, SPATIAL_DIM};
|
||||||
use crate::prelude::MultibodyJointSet;
|
use crate::prelude::MultibodyJointSet;
|
||||||
use na::DVector;
|
use na::DVector;
|
||||||
|
|
||||||
|
#[cfg(feature = "simd-is-enabled")]
|
||||||
|
use crate::math::{Isometry, SimdReal, SIMD_WIDTH};
|
||||||
|
|
||||||
|
#[cfg(feature = "parallel")]
|
||||||
|
use crate::dynamics::JointAxesMask;
|
||||||
|
|
||||||
pub enum AnyJointVelocityConstraint {
|
pub enum AnyJointVelocityConstraint {
|
||||||
JointConstraint(JointVelocityConstraint<Real, 1>),
|
JointConstraint(JointVelocityConstraint<Real, 1>),
|
||||||
JointGroundConstraint(JointVelocityGroundConstraint<Real, 1>),
|
JointGroundConstraint(JointVelocityGroundConstraint<Real, 1>),
|
||||||
|
|||||||
@@ -5,9 +5,12 @@ use crate::dynamics::solver::joint_constraint::SolverBody;
|
|||||||
use crate::dynamics::solver::MotorParameters;
|
use crate::dynamics::solver::MotorParameters;
|
||||||
use crate::dynamics::{IntegrationParameters, JointIndex, JointLimits};
|
use crate::dynamics::{IntegrationParameters, JointIndex, JointLimits};
|
||||||
use crate::math::{AngVector, Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM};
|
use crate::math::{AngVector, Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM};
|
||||||
use crate::utils::{IndexMut2, WBasis, WCross, WCrossMatrix, WDot, WQuat, WReal};
|
use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal};
|
||||||
use na::SMatrix;
|
use na::SMatrix;
|
||||||
|
|
||||||
|
#[cfg(feature = "dim3")]
|
||||||
|
use crate::utils::WBasis;
|
||||||
|
|
||||||
#[derive(Debug, Copy, Clone)]
|
#[derive(Debug, Copy, Clone)]
|
||||||
pub struct JointVelocityConstraintBuilder<N: WReal> {
|
pub struct JointVelocityConstraintBuilder<N: WReal> {
|
||||||
pub basis: Matrix<N>,
|
pub basis: Matrix<N>,
|
||||||
@@ -660,79 +663,76 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn motor_linear_coupled_ground<const LANES: usize>(
|
// pub fn motor_linear_coupled_ground<const LANES: usize>(
|
||||||
&self,
|
// &self,
|
||||||
_joint_id: [JointIndex; LANES],
|
// _joint_id: [JointIndex; LANES],
|
||||||
_body1: &SolverBody<N, LANES>,
|
// _body1: &SolverBody<N, LANES>,
|
||||||
_body2: &SolverBody<N, LANES>,
|
// _body2: &SolverBody<N, LANES>,
|
||||||
_motor_coupled_axes: u8,
|
// _motor_coupled_axes: u8,
|
||||||
_motors: &[MotorParameters<N>],
|
// _motors: &[MotorParameters<N>],
|
||||||
_limited_coupled_axes: u8,
|
// _limited_coupled_axes: u8,
|
||||||
_limits: &[JointLimits<N>],
|
// _limits: &[JointLimits<N>],
|
||||||
_writeback_id: WritebackId,
|
// _writeback_id: WritebackId,
|
||||||
) -> JointVelocityGroundConstraint<N, LANES> {
|
// ) -> JointVelocityGroundConstraint<N, LANES> {
|
||||||
todo!()
|
// let zero = N::zero();
|
||||||
/*
|
// let mut lin_jac = Vector::zeros();
|
||||||
let zero = N::zero();
|
// let mut ang_jac1: AngVector<N> = na::zero();
|
||||||
let mut lin_jac = Vector::zeros();
|
// let mut ang_jac2: AngVector<N> = na::zero();
|
||||||
let mut ang_jac1: AngVector<N> = na::zero();
|
// let mut limit = N::zero();
|
||||||
let mut ang_jac2: AngVector<N> = na::zero();
|
|
||||||
let mut limit = N::zero();
|
|
||||||
|
|
||||||
for i in 0..DIM {
|
// for i in 0..DIM {
|
||||||
if limited_coupled_axes & (1 << i) != 0 {
|
// if limited_coupled_axes & (1 << i) != 0 {
|
||||||
let coeff = self.basis.column(i).dot(&self.lin_err);
|
// let coeff = self.basis.column(i).dot(&self.lin_err);
|
||||||
lin_jac += self.basis.column(i) * coeff;
|
// lin_jac += self.basis.column(i) * coeff;
|
||||||
#[cfg(feature = "dim2")]
|
// #[cfg(feature = "dim2")]
|
||||||
{
|
// {
|
||||||
ang_jac1 += self.cmat1_basis[i] * coeff;
|
// ang_jac1 += self.cmat1_basis[i] * coeff;
|
||||||
ang_jac2 += self.cmat2_basis[i] * coeff;
|
// ang_jac2 += self.cmat2_basis[i] * coeff;
|
||||||
}
|
// }
|
||||||
#[cfg(feature = "dim3")]
|
// #[cfg(feature = "dim3")]
|
||||||
{
|
// {
|
||||||
ang_jac1 += self.cmat1_basis.column(i) * coeff;
|
// ang_jac1 += self.cmat1_basis.column(i) * coeff;
|
||||||
ang_jac2 += self.cmat2_basis.column(i) * coeff;
|
// ang_jac2 += self.cmat2_basis.column(i) * coeff;
|
||||||
}
|
// }
|
||||||
limit += limits[i].max * limits[i].max;
|
// limit += limits[i].max * limits[i].max;
|
||||||
}
|
// }
|
||||||
}
|
// }
|
||||||
|
|
||||||
limit = limit.simd_sqrt();
|
// limit = limit.simd_sqrt();
|
||||||
let dist = lin_jac.norm();
|
// let dist = lin_jac.norm();
|
||||||
let inv_dist = crate::utils::simd_inv(dist);
|
// let inv_dist = crate::utils::simd_inv(dist);
|
||||||
lin_jac *= inv_dist;
|
// lin_jac *= inv_dist;
|
||||||
ang_jac1 *= inv_dist;
|
// ang_jac1 *= inv_dist;
|
||||||
ang_jac2 *= inv_dist;
|
// ang_jac2 *= inv_dist;
|
||||||
|
|
||||||
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
|
// let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
|
||||||
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
|
// + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
|
||||||
let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt());
|
// let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt());
|
||||||
|
|
||||||
ang_jac2 = body2.sqrt_ii * ang_jac2;
|
// ang_jac2 = body2.sqrt_ii * ang_jac2;
|
||||||
|
|
||||||
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
|
// let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
|
||||||
let cfm_coeff = N::splat(params.joint_cfm_coeff());
|
// let cfm_coeff = N::splat(params.joint_cfm_coeff());
|
||||||
let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt;
|
// let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt;
|
||||||
let rhs = rhs_wo_bias + rhs_bias;
|
// let rhs = rhs_wo_bias + rhs_bias;
|
||||||
let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)];
|
// let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)];
|
||||||
|
|
||||||
JointVelocityGroundConstraint {
|
// JointVelocityGroundConstraint {
|
||||||
joint_id,
|
// joint_id,
|
||||||
mj_lambda2: body2.mj_lambda,
|
// mj_lambda2: body2.mj_lambda,
|
||||||
im2: body2.im,
|
// im2: body2.im,
|
||||||
impulse: N::zero(),
|
// impulse: N::zero(),
|
||||||
impulse_bounds,
|
// impulse_bounds,
|
||||||
lin_jac,
|
// lin_jac,
|
||||||
ang_jac2,
|
// ang_jac2,
|
||||||
inv_lhs: N::zero(), // Will be set during ortogonalization.
|
// inv_lhs: N::zero(), // Will be set during ortogonalization.
|
||||||
cfm_coeff,
|
// cfm_coeff,
|
||||||
cfm_gain: N::zero(),
|
// cfm_gain: N::zero(),
|
||||||
rhs,
|
// rhs,
|
||||||
rhs_wo_bias,
|
// rhs_wo_bias,
|
||||||
writeback_id,
|
// writeback_id,
|
||||||
}
|
// }
|
||||||
*/
|
// }
|
||||||
}
|
|
||||||
|
|
||||||
pub fn lock_linear_ground<const LANES: usize>(
|
pub fn lock_linear_ground<const LANES: usize>(
|
||||||
&self,
|
&self,
|
||||||
|
|||||||
@@ -43,15 +43,15 @@ impl<VelocityConstraint> SolverConstraints<VelocityConstraint> {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn clear(&mut self) {
|
// pub fn clear(&mut self) {
|
||||||
self.not_ground_interactions.clear();
|
// self.not_ground_interactions.clear();
|
||||||
self.ground_interactions.clear();
|
// self.ground_interactions.clear();
|
||||||
self.generic_not_ground_interactions.clear();
|
// self.generic_not_ground_interactions.clear();
|
||||||
self.generic_ground_interactions.clear();
|
// self.generic_ground_interactions.clear();
|
||||||
self.interaction_groups.clear();
|
// self.interaction_groups.clear();
|
||||||
self.ground_interaction_groups.clear();
|
// self.ground_interaction_groups.clear();
|
||||||
self.velocity_constraints.clear();
|
// self.velocity_constraints.clear();
|
||||||
}
|
// }
|
||||||
}
|
}
|
||||||
|
|
||||||
impl SolverConstraints<AnyVelocityConstraint> {
|
impl SolverConstraints<AnyVelocityConstraint> {
|
||||||
|
|||||||
@@ -74,6 +74,7 @@ impl ContactPair {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Clears all the contacts of this contact pair.
|
||||||
pub fn clear(&mut self) {
|
pub fn clear(&mut self) {
|
||||||
self.manifolds.clear();
|
self.manifolds.clear();
|
||||||
self.has_any_active_contact = false;
|
self.has_any_active_contact = false;
|
||||||
|
|||||||
@@ -11,7 +11,7 @@
|
|||||||
//! User documentation for Rapier is on [the official Rapier site](https://rapier.rs/docs/).
|
//! User documentation for Rapier is on [the official Rapier site](https://rapier.rs/docs/).
|
||||||
|
|
||||||
#![deny(bare_trait_objects)]
|
#![deny(bare_trait_objects)]
|
||||||
#![allow(missing_docs)] // FIXME: deny that
|
#![warn(missing_docs)] // FIXME: deny that
|
||||||
|
|
||||||
#[cfg(all(feature = "dim2", feature = "f32"))]
|
#[cfg(all(feature = "dim2", feature = "f32"))]
|
||||||
pub extern crate parry2d as parry;
|
pub extern crate parry2d as parry;
|
||||||
|
|||||||
@@ -367,8 +367,6 @@ impl PhysicsPipeline {
|
|||||||
{
|
{
|
||||||
// Set the rigid-bodies and kinematic bodies to their final position.
|
// Set the rigid-bodies and kinematic bodies to their final position.
|
||||||
for handle in islands.iter_active_bodies() {
|
for handle in islands.iter_active_bodies() {
|
||||||
let status: &RigidBodyType = bodies.index(handle.0);
|
|
||||||
|
|
||||||
bodies.map_mut_internal(handle.0, |poss: &mut RigidBodyPosition| {
|
bodies.map_mut_internal(handle.0, |poss: &mut RigidBodyPosition| {
|
||||||
poss.position = poss.next_position
|
poss.position = poss.next_position
|
||||||
});
|
});
|
||||||
|
|||||||
@@ -15,6 +15,9 @@ use {
|
|||||||
num::One,
|
num::One,
|
||||||
};
|
};
|
||||||
|
|
||||||
|
/// The trait for real numbers used by Rapier.
|
||||||
|
///
|
||||||
|
/// This includes `f32`, `f64` and their related SIMD types.
|
||||||
pub trait WReal: SimdRealField<Element = Real> + Copy {}
|
pub trait WReal: SimdRealField<Element = Real> + Copy {}
|
||||||
impl WReal for Real {}
|
impl WReal for Real {}
|
||||||
impl WReal for SimdReal {}
|
impl WReal for SimdReal {}
|
||||||
@@ -422,9 +425,12 @@ impl WCross<Vector2<SimdReal>> for Vector2<SimdReal> {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Trait implemented by quaternions.
|
||||||
pub trait WQuat<N> {
|
pub trait WQuat<N> {
|
||||||
|
/// The result of quaternion differentiation.
|
||||||
type Result;
|
type Result;
|
||||||
|
|
||||||
|
/// Compute the differential of `inv(q1) * q2`.
|
||||||
fn diff_conj1_2(&self, rhs: &Self) -> Self::Result;
|
fn diff_conj1_2(&self, rhs: &Self) -> Self::Result;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user