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

feat: implement new "small-steps" solver + joint improvements

Browse Source
This commit is contained in:
Sébastien Crozet
2024-01-21 21:02:23 +01:00
parent 9ac3503b87
commit 9b87f06a85
76 changed files with 6672 additions and 4305 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,7 +1,7 @@
use crate::dynamics::solver::MotorParameters;
use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RopeJoint};
use crate::math::{Isometry, Point, Real, Rotation, UnitVector, Vector, SPATIAL_DIM};
use crate::utils::{WBasis, WReal};
use crate::utils::{SimdBasis, SimdRealCopy};
#[cfg(feature = "dim3")]
use crate::dynamics::SphericalJoint;
@@ -121,7 +121,7 @@ pub struct JointLimits<N> {
pub impulse: N,
}
impl<N: WReal> Default for JointLimits<N> {
impl<N: SimdRealCopy> Default for JointLimits<N> {
fn default() -> Self {
Self {
min: -N::splat(Real::MAX),
@@ -131,6 +131,16 @@ impl<N: WReal> Default for JointLimits<N> {
}
}
impl<N: SimdRealCopy> From<[N; 2]> for JointLimits<N> {
fn from(value: [N; 2]) -> Self {
Self {
min: value[0],
max: value[1],
impulse: N::splat(0.0),
}
}
}
/// A joint’s motor along one of its degrees of freedom.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
@@ -210,14 +220,23 @@ pub struct GenericJoint {
/// The degrees-of-freedoms motorised by this joint.
pub motor_axes: JointAxesMask,
/// The coupled degrees of freedom of this joint.
///
/// Note that coupling degrees of freedoms (DoF) changes the interpretation of the coupled joint’s limits and motors.
/// If multiple linear DoF are limited/motorized, only the limits/motor configuration for the first
/// coupled linear DoF is applied to all coupled linear DoF. Similarly, if multiple angular DoF are limited/motorized
/// only the limits/motor configuration for the first coupled angular DoF is applied to all coupled angular DoF.
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.
/// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF limit and `limit_axis`
/// bitmask is applied to the coupled linear (resp. angular) axes.
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.
/// Note that the motor must also be explicitly enabled by the `motor_axes` bitmask.
/// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF motor and `motor_axes`
/// bitmask is applied to the coupled linear (resp. angular) axes.
pub motors: [JointMotor; SPATIAL_DIM],
/// Are contacts between the attached rigid-bodies enabled?
pub contacts_enabled: bool,

2
src/dynamics/joint/impulse_joint/impulse_joint_set.rs
View File

@@ -214,7 +214,7 @@ impl ImpulseJointSet {
// // .map(|e| &mut e.weight)
// }
#[cfg(not(feature = "parallel"))]
// #[cfg(not(feature = "parallel"))]
pub(crate) fn joints_mut(&mut self) -> &mut [JointGraphEdge] {
&mut self.joint_graph.graph.edges[..]
}

2
src/dynamics/joint/mod.rs
View File

@@ -6,6 +6,7 @@ pub use self::multibody_joint::*;
pub use self::prismatic_joint::*;
pub use self::revolute_joint::*;
pub use self::rope_joint::*;
pub use self::spring_joint::*;
#[cfg(feature = "dim3")]
pub use self::spherical_joint::*;
@@ -21,3 +22,4 @@ mod rope_joint;
#[cfg(feature = "dim3")]
mod spherical_joint;
mod spring_joint;

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

@@ -2,7 +2,9 @@
pub use self::multibody::Multibody;
pub use self::multibody_joint::MultibodyJoint;
pub use self::multibody_joint_set::{MultibodyIndex, MultibodyJointHandle, MultibodyJointSet};
pub use self::multibody_joint_set::{
MultibodyIndex, MultibodyJointHandle, MultibodyJointSet, MultibodyLinkId,
};
pub use self::multibody_link::MultibodyLink;
pub use self::unit_multibody_joint::{unit_joint_limit_constraint, unit_joint_motor_constraint};

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

@@ -1,16 +1,13 @@
use super::multibody_link::{MultibodyLink, MultibodyLinkVec};
use super::multibody_workspace::MultibodyWorkspace;
use crate::dynamics::{
solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyHandle, RigidBodySet,
RigidBodyType, RigidBodyVelocity,
};
use crate::dynamics::{RigidBodyHandle, RigidBodySet, RigidBodyType, RigidBodyVelocity};
#[cfg(feature = "dim3")]
use crate::math::Matrix;
use crate::math::{
AngDim, AngVector, Dim, Isometry, Jacobian, Point, Real, Vector, ANG_DIM, DIM, SPATIAL_DIM,
};
use crate::prelude::MultibodyJoint;
use crate::utils::{IndexMut2, WAngularInertia, WCross, WCrossMatrix};
use crate::utils::{IndexMut2, SimdAngularInertia, SimdCross, SimdCrossMatrix};
use na::{self, DMatrix, DVector, DVectorView, DVectorViewMut, Dyn, OMatrix, SMatrix, SVector, LU};
#[repr(C)]
@@ -372,6 +369,7 @@ impl Multibody {
self.accelerations.fill(0.0);
// Eqn 42 to 45
for i in 0..self.links.len() {
let link = &self.links[i];
let rb = &bodies[link.rigid_body];
@@ -400,7 +398,7 @@ impl Multibody {
}
acc.linvel += rb.vels.angvel.gcross(rb.vels.angvel.gcross(link.shift23));
acc.linvel += self.workspace.accs[i].angvel.gcross(link.shift23);
acc.linvel += acc.angvel.gcross(link.shift23);
self.workspace.accs[i] = acc;
@@ -728,7 +726,7 @@ impl Multibody {
/// The generalized velocity at the multibody_joint of the given link.
#[inline]
pub(crate) fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView<Real> {
pub fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView<Real> {
let ndofs = link.joint().ndofs();
DVectorView::from_slice(
&self.velocities.as_slice()[link.assembly_id..link.assembly_id + ndofs],
@@ -829,8 +827,10 @@ impl Multibody {
}
}
// TODO: make a version that doesn’t write back to bodies and doesn’t update the jacobians
// (i.e. just something used by the velocity solver’s small steps).
/// Apply forward-kinematics to this multibody and its related rigid-bodies.
pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_mass_props: bool) {
pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_rb_mass_props: bool) {
// Special case for the root, which has no parent.
{
let link = &mut self.links[0];
@@ -839,7 +839,7 @@ impl Multibody {
if let Some(rb) = bodies.get_mut_internal(link.rigid_body) {
rb.pos.next_position = link.local_to_world;
if update_mass_props {
if update_rb_mass_props {
rb.mprops.update_world_mass_properties(&link.local_to_world);
}
}
@@ -873,7 +873,7 @@ impl Multibody {
"A rigid-body that is not at the root of a multibody must be dynamic."
);
if update_mass_props {
if update_rb_mass_props {
link_rb
.mprops
.update_world_mass_properties(&link.local_to_world);
@@ -951,40 +951,4 @@ impl Multibody {
.sum();
(num_constraints, num_constraints)
}
#[inline]
pub(crate) fn generate_internal_constraints(
&self,
params: &IntegrationParameters,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
out: &mut Vec<AnyJointVelocityConstraint>,
mut insert_at: Option<usize>,
) {
if !cfg!(feature = "parallel") {
let num_constraints: usize = self
.links
.iter()
.map(|l| l.joint().num_velocity_constraints())
.sum();
let required_jacobian_len = *j_id + num_constraints * self.ndofs * 2;
if jacobians.nrows() < required_jacobian_len {
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
}
}
for link in self.links.iter() {
link.joint().velocity_constraints(
params,
self,
link,
0,
j_id,
jacobians,
out,
&mut insert_at,
);
}
}
}

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

@@ -1,4 +1,4 @@
use crate::dynamics::solver::AnyJointVelocityConstraint;
use crate::dynamics::solver::JointGenericOneBodyConstraint;
use crate::dynamics::{
joint, FixedJointBuilder, GenericJoint, IntegrationParameters, Multibody, MultibodyLink,
RigidBodyVelocity,
@@ -254,15 +254,15 @@ impl MultibodyJoint {
params: &IntegrationParameters,
multibody: &Multibody,
link: &MultibodyLink,
dof_id: usize,
j_id: &mut usize,
mut j_id: usize,
jacobians: &mut DVector<Real>,
constraints: &mut Vec<AnyJointVelocityConstraint>,
insert_at: &mut Option<usize>,
) {
constraints: &mut [JointGenericOneBodyConstraint],
) -> usize {
let j_id = &mut j_id;
let locked_bits = self.data.locked_axes.bits();
let limit_bits = self.data.limit_axes.bits();
let motor_bits = self.data.motor_axes.bits();
let mut num_constraints = 0;
let mut curr_free_dof = 0;
for i in 0..DIM {
@@ -281,11 +281,11 @@ impl MultibodyJoint {
&self.data.motors[i],
self.coords[i],
limits,
dof_id + curr_free_dof,
curr_free_dof,
j_id,
jacobians,
constraints,
insert_at,
&mut num_constraints,
);
}
@@ -296,11 +296,11 @@ impl MultibodyJoint {
link,
[self.data.limits[i].min, self.data.limits[i].max],
self.coords[i],
dof_id + curr_free_dof,
curr_free_dof,
j_id,
jacobians,
constraints,
insert_at,
&mut num_constraints,
);
}
curr_free_dof += 1;
@@ -331,11 +331,11 @@ impl MultibodyJoint {
link,
limits,
self.coords[i],
dof_id + curr_free_dof,
curr_free_dof,
j_id,
jacobians,
constraints,
insert_at,
&mut num_constraints,
);
Some(limits)
} else {
@@ -350,15 +350,17 @@ impl MultibodyJoint {
&self.data.motors[i],
self.coords[i],
limits,
dof_id + curr_free_dof,
curr_free_dof,
j_id,
jacobians,
constraints,
insert_at,
&mut num_constraints,
);
}
curr_free_dof += 1;
}
}
num_constraints
}
}

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

@@ -53,13 +53,24 @@ impl IndexedData for MultibodyJointHandle {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub struct MultibodyJointLink {
pub graph_id: RigidBodyGraphIndex,
/// Indexes usable to get a multibody link from a `MultibodyJointSet`.
///
/// ```rust
/// // With:
/// // multibody_joint_set: MultibodyJointSet
/// // multibody_link_id: MultibodyLinkId
/// let multibody = &multibody_joint_set[multibody_link_id.multibody];
/// let link = multibody.link(multibody_link_id.id).expect("Link not found.");
pub struct MultibodyLinkId {
pub(crate) graph_id: RigidBodyGraphIndex,
/// The multibody index to be used as `&multibody_joint_set[multibody]` to
/// retrieve the multibody reference.
pub multibody: MultibodyIndex,
/// The multibody link index to be given to [`Multibody::link`].
pub id: usize,
}
impl Default for MultibodyJointLink {
impl Default for MultibodyLinkId {
fn default() -> Self {
Self {
graph_id: RigidBodyGraphIndex::new(crate::INVALID_U32),
@@ -78,7 +89,7 @@ impl Default for MultibodyJointLink {
#[derive(Clone)]
pub struct MultibodyJointSet {
pub(crate) multibodies: Arena<Multibody>, // NOTE: a Slab would be sufficient.
pub(crate) rb2mb: Coarena<MultibodyJointLink>,
pub(crate) rb2mb: Coarena<MultibodyLinkId>,
// NOTE: this is mostly for the island extraction. So perhaps we won’t need
// that any more in the future when we improve our island builder.
pub(crate) connectivity_graph: InteractionGraph<RigidBodyHandle, ()>,
@@ -97,13 +108,22 @@ 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,
&MultibodyLinkId,
&Multibody,
&MultibodyLink,
),
> {
self.rb2mb
.iter()
.filter(|(_, link)| link.id > 0) // The first link of a rigid-body hasn’t been added by the user.
.map(|(h, link)| {
let mb = &self.multibodies[link.multibody.0];
(MultibodyJointHandle(h), mb, mb.link(link.id).unwrap())
(MultibodyJointHandle(h), link, mb, mb.link(link.id).unwrap())
})
}
@@ -118,7 +138,7 @@ impl MultibodyJointSet {
let data = data.into();
let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| {
let mb_handle = self.multibodies.insert(Multibody::with_root(body1));
MultibodyJointLink {
MultibodyLinkId {
graph_id: self.connectivity_graph.graph.add_node(body1),
multibody: MultibodyIndex(mb_handle),
id: 0,
@@ -127,7 +147,7 @@ impl MultibodyJointSet {
let link2 = self.rb2mb.get(body2.0).copied().unwrap_or_else(|| {
let mb_handle = self.multibodies.insert(Multibody::with_root(body2));
MultibodyJointLink {
MultibodyLinkId {
graph_id: self.connectivity_graph.graph.add_node(body2),
multibody: MultibodyIndex(mb_handle),
id: 0,
@@ -257,7 +277,7 @@ impl MultibodyJointSet {
/// Returns the link of this multibody attached to the given rigid-body.
///
/// Returns `None` if `rb` isn’t part of any rigid-body.
pub fn rigid_body_link(&self, rb: RigidBodyHandle) -> Option<&MultibodyJointLink> {
pub fn rigid_body_link(&self, rb: RigidBodyHandle) -> Option<&MultibodyLinkId> {
self.rb2mb.get(rb.0)
}
@@ -340,15 +360,15 @@ impl MultibodyJointSet {
// NOTE: if there is a joint between these two bodies, then
// one of the bodies must be the parent of the other.
let link1 = mb.link(id1.id)?;
let parent1 = link1.parent_id()?;
let parent1 = link1.parent_id();
if parent1 == id2.id {
if parent1 == Some(id2.id) {
Some((MultibodyJointHandle(rb1.0), mb, &link1))
} else {
let link2 = mb.link(id2.id)?;
let parent2 = link2.parent_id()?;
let parent2 = link2.parent_id();
if parent2 == id1.id {
if parent2 == Some(id1.id) {
Some((MultibodyJointHandle(rb2.0), mb, &link2))
} else {
None

51
src/dynamics/joint/multibody_joint/unit_multibody_joint.rs
View File

@@ -1,9 +1,7 @@
#![allow(missing_docs)] // For downcast.
use crate::dynamics::joint::MultibodyLink;
use crate::dynamics::solver::{
AnyJointVelocityConstraint, JointGenericVelocityGroundConstraint, WritebackId,
};
use crate::dynamics::solver::{JointGenericOneBodyConstraint, WritebackId};
use crate::dynamics::{IntegrationParameters, JointMotor, Multibody};
use crate::math::Real;
use na::DVector;
@@ -19,18 +17,16 @@ pub fn unit_joint_limit_constraint(
dof_id: usize,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
constraints: &mut Vec<AnyJointVelocityConstraint>,
insert_at: &mut Option<usize>,
constraints: &mut [JointGenericOneBodyConstraint],
insert_at: &mut usize,
) {
let ndofs = multibody.ndofs();
let joint_velocity = multibody.joint_velocity(link);
let min_enabled = curr_pos < limits[0];
let max_enabled = limits[1] < curr_pos;
let erp_inv_dt = params.joint_erp_inv_dt();
let cfm_coeff = params.joint_cfm_coeff();
let rhs_bias = ((curr_pos - limits[1]).max(0.0) - (limits[0] - curr_pos).max(0.0)) * erp_inv_dt;
let rhs_wo_bias = joint_velocity[dof_id];
let rhs_wo_bias = 0.0;
let dof_j_id = *j_id + dof_id + link.assembly_id;
jacobians.rows_mut(*j_id, ndofs * 2).fill(0.0);
@@ -46,8 +42,8 @@ pub fn unit_joint_limit_constraint(
max_enabled as u32 as Real * Real::MAX,
];
let constraint = JointGenericVelocityGroundConstraint {
mj_lambda2: multibody.solver_id,
let constraint = JointGenericOneBodyConstraint {
solver_vel2: multibody.solver_id,
ndofs2: ndofs,
j_id2: *j_id,
joint_id: usize::MAX,
@@ -61,14 +57,9 @@ pub fn unit_joint_limit_constraint(
writeback_id: WritebackId::Limit(dof_id),
};
if let Some(at) = insert_at {
constraints[*at] = AnyJointVelocityConstraint::JointGenericGroundConstraint(constraint);
*at += 1;
} else {
constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
constraint,
));
}
constraints[*insert_at] = constraint;
*insert_at += 1;
*j_id += 2 * ndofs;
}
@@ -84,13 +75,11 @@ pub fn unit_joint_motor_constraint(
dof_id: usize,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
constraints: &mut Vec<AnyJointVelocityConstraint>,
insert_at: &mut Option<usize>,
constraints: &mut [JointGenericOneBodyConstraint],
insert_at: &mut usize,
) {
let inv_dt = params.inv_dt();
let ndofs = multibody.ndofs();
let joint_velocity = multibody.joint_velocity(link);
let motor_params = motor.motor_params(params.dt);
let dof_j_id = *j_id + dof_id + link.assembly_id;
@@ -117,11 +106,10 @@ pub fn unit_joint_motor_constraint(
);
};
let dvel = joint_velocity[dof_id];
rhs_wo_bias += dvel - target_vel;
rhs_wo_bias += -target_vel;
let constraint = JointGenericVelocityGroundConstraint {
mj_lambda2: multibody.solver_id,
let constraint = JointGenericOneBodyConstraint {
solver_vel2: multibody.solver_id,
ndofs2: ndofs,
j_id2: *j_id,
joint_id: usize::MAX,
@@ -135,13 +123,8 @@ pub fn unit_joint_motor_constraint(
writeback_id: WritebackId::Limit(dof_id),
};
if let Some(at) = insert_at {
constraints[*at] = AnyJointVelocityConstraint::JointGenericGroundConstraint(constraint);
*at += 1;
} else {
constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
constraint,
));
}
constraints[*insert_at] = constraint;
*insert_at += 1;
*j_id += 2 * ndofs;
}

104
src/dynamics/joint/rope_joint.rs
View File

@@ -1,8 +1,8 @@
use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
use crate::dynamics::{JointAxis, MotorModel};
use crate::math::{Point, Real, UnitVector};
use crate::math::{Point, Real};
use super::{JointLimits, JointMotor};
use super::JointMotor;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
@@ -14,12 +14,16 @@ pub struct RopeJoint {
}
impl RopeJoint {
/// Creates a new rope joint limiting the max distance between to bodies
pub fn new() -> Self {
let data = GenericJointBuilder::new(JointAxesMask::FREE_FIXED_AXES)
/// Creates a new rope joint limiting the max distance between two bodies.
///
/// The `max_dist` must be strictly greater than 0.0.
pub fn new(max_dist: Real) -> Self {
let data = GenericJointBuilder::new(JointAxesMask::empty())
.coupled_axes(JointAxesMask::LIN_AXES)
.build();
Self { data }
let mut result = Self { data };
result.set_max_distance(max_dist);
result
}
/// The underlying generic joint.
@@ -62,30 +66,6 @@ impl RopeJoint {
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, axis: JointAxis) -> Option<&JointMotor> {
@@ -95,9 +75,6 @@ impl RopeJoint {
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
pub fn set_motor_model(&mut self, model: MotorModel) -> &mut Self {
self.data.set_motor_model(JointAxis::X, model);
self.data.set_motor_model(JointAxis::Y, model);
#[cfg(feature = "dim3")]
self.data.set_motor_model(JointAxis::Z, model);
self
}
@@ -105,11 +82,6 @@ impl RopeJoint {
pub fn set_motor_velocity(&mut self, target_vel: Real, factor: Real) -> &mut Self {
self.data
.set_motor_velocity(JointAxis::X, target_vel, factor);
self.data
.set_motor_velocity(JointAxis::Y, target_vel, factor);
#[cfg(feature = "dim3")]
self.data
.set_motor_velocity(JointAxis::Z, target_vel, factor);
self
}
@@ -122,11 +94,6 @@ impl RopeJoint {
) -> &mut Self {
self.data
.set_motor_position(JointAxis::X, target_pos, stiffness, damping);
self.data
.set_motor_position(JointAxis::Y, target_pos, stiffness, damping);
#[cfg(feature = "dim3")]
self.data
.set_motor_position(JointAxis::Z, target_pos, stiffness, damping);
self
}
@@ -140,35 +107,26 @@ impl RopeJoint {
) -> &mut Self {
self.data
.set_motor(JointAxis::X, target_pos, target_vel, stiffness, damping);
self.data
.set_motor(JointAxis::Y, target_pos, target_vel, stiffness, damping);
#[cfg(feature = "dim3")]
self.data
.set_motor(JointAxis::Y, target_pos, target_vel, stiffness, damping);
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.data.set_motor_max_force(JointAxis::Y, max_force);
#[cfg(feature = "dim3")]
self.data.set_motor_max_force(JointAxis::Z, max_force);
self
}
/// The limit maximum distance attached bodies can translate.
/// The maximum distance allowed between the attached objects.
#[must_use]
pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits<Real>> {
self.data.limits(axis)
pub fn max_distance(&self) -> Option<Real> {
self.data.limits(JointAxis::X).map(|l| l.max)
}
/// Sets the `[min,max]` limit distances attached bodies can translate.
pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self {
self.data.set_limits(JointAxis::X, limits);
self.data.set_limits(JointAxis::Y, limits);
#[cfg(feature = "dim3")]
self.data.set_limits(JointAxis::Z, limits);
/// Sets the maximum allowed distance between the attached objects.
///
/// The `max_dist` must be strictly greater than 0.0.
pub fn set_max_distance(&mut self, max_dist: Real) -> &mut Self {
self.data.set_limits(JointAxis::X, [0.0, max_dist]);
self
}
}
@@ -190,8 +148,8 @@ impl RopeJointBuilder {
/// Creates a new builder for rope joints.
///
/// This axis is expressed in the local-space of both rigid-bodies.
pub fn new() -> Self {
Self(RopeJoint::new())
pub fn new(max_dist: Real) -> Self {
Self(RopeJoint::new(max_dist))
}
/// Sets whether contacts between the attached rigid-bodies are enabled.
@@ -215,20 +173,6 @@ impl RopeJointBuilder {
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);
self
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
#[must_use]
pub fn motor_model(mut self, model: MotorModel) -> Self {
@@ -270,10 +214,12 @@ impl RopeJointBuilder {
self
}
/// Sets the `[min,max]` limit distances attached bodies can translate.
/// Sets the maximum allowed distance between the attached bodies.
///
/// The `max_dist` must be strictly greater than 0.0.
#[must_use]
pub fn limits(mut self, limits: [Real; 2]) -> Self {
self.0.set_limits(limits);
pub fn max_distance(mut self, max_dist: Real) -> Self {
self.0.set_max_distance(max_dist);
self
}

172
src/dynamics/joint/spring_joint.rs Normal file
View File

@@ -0,0 +1,172 @@
use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
use crate::dynamics::{JointAxis, MotorModel};
use crate::math::{Point, Real};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
#[repr(transparent)]
/// A spring-damper joint, applies a force proportional to the distance between two objects.
///
/// The spring is integrated implicitly, implying that an even undamped spring will still be subject to some
/// amount of numerical damping (so it will eventually come to a rest). More solver iterations, or smaller
/// timesteps, will lower the effect of numerical damping, providing a more realistic result.
pub struct SpringJoint {
/// The underlying joint data.
pub data: GenericJoint,
}
impl SpringJoint {
/// Creates a new spring joint limiting the max distance between two bodies.
///
/// The `max_dist` must be strictly greater than 0.0.
pub fn new(rest_length: Real, stiffness: Real, damping: Real) -> Self {
let data = GenericJointBuilder::new(JointAxesMask::empty())
.coupled_axes(JointAxesMask::LIN_AXES)
.motor_position(JointAxis::X, rest_length, stiffness, damping)
.motor_model(JointAxis::X, MotorModel::ForceBased)
.build();
Self { data }
}
/// The underlying generic joint.
pub fn data(&self) -> &GenericJoint {
&self.data
}
/// Are contacts between the attached rigid-bodies enabled?
pub fn contacts_enabled(&self) -> bool {
self.data.contacts_enabled
}
/// Sets whether contacts between the attached rigid-bodies are enabled.
pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self {
self.data.set_contacts_enabled(enabled);
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
}
/// Set the spring model used by this joint to reach the desired target velocity and position.
///
/// Setting this to `MotorModel::ForceBased` (which is the default value for this joint) makes the spring constants
/// (stiffness and damping) parameter understood as in the regular spring-mass-damper system. With
/// `MotorModel::AccelerationBased`, the spring constants will be automatically scaled by the attached masses,
/// making the spring more mass-independent.
pub fn set_spring_model(&mut self, model: MotorModel) -> &mut Self {
self.data.set_motor_model(JointAxis::X, model);
self
}
// /// The maximum distance allowed between the attached objects.
// #[must_use]
// pub fn rest_length(&self) -> Option<Real> {
// self.data.limits(JointAxis::X).map(|l| l.max)
// }
//
// /// Sets the maximum allowed distance between the attached objects.
// ///
// /// The `max_dist` must be strictly greater than 0.0.
// pub fn set_rest_length(&mut self, max_dist: Real) -> &mut Self {
// self.data.set_limits(JointAxis::X, [0.0, max_dist]);
// self
// }
}
impl Into<GenericJoint> for SpringJoint {
fn into(self) -> GenericJoint {
self.data
}
}
/// A [SpringJoint] joint using the builder pattern.
///
/// This builds a spring-damper joint which applies a force proportional to the distance between two objects.
/// See the documentation of [SpringJoint] for more information on its behavior.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct SpringJointBuilder(pub SpringJoint);
impl SpringJointBuilder {
/// Creates a new builder for spring joints.
///
/// This axis is expressed in the local-space of both rigid-bodies.
pub fn new(rest_length: Real, stiffness: Real, damping: Real) -> Self {
Self(SpringJoint::new(rest_length, stiffness, damping))
}
/// Sets whether contacts between the attached rigid-bodies are enabled.
#[must_use]
pub fn contacts_enabled(mut self, enabled: bool) -> Self {
self.0.set_contacts_enabled(enabled);
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
}
/// Set the spring used by this joint to reach the desired target velocity and position.
///
/// Setting this to `MotorModel::ForceBased` (which is the default value for this joint) makes the spring constants
/// (stiffness and damping) parameter understood as in the regular spring-mass-damper system. With
/// `MotorModel::AccelerationBased`, the spring constants will be automatically scaled by the attached masses,
/// making the spring more mass-independent.
#[must_use]
pub fn spring_model(mut self, model: MotorModel) -> Self {
self.0.set_spring_model(model);
self
}
// /// Sets the maximum allowed distance between the attached bodies.
// ///
// /// The `max_dist` must be strictly greater than 0.0.
// #[must_use]
// pub fn max_distance(mut self, max_dist: Real) -> Self {
// self.0.set_max_distance(max_dist);
// self
// }
/// Builds the spring joint.
#[must_use]
pub fn build(self) -> SpringJoint {
self.0
}
}
impl Into<GenericJoint> for SpringJointBuilder {
fn into(self) -> GenericJoint {
self.0.into()
}
}