aboutsummaryrefslogtreecommitdiff
path: root/src/dynamics
diff options
context:
space:
mode:
Diffstat (limited to 'src/dynamics')
-rw-r--r--src/dynamics/integration_parameters.rs13
-rw-r--r--src/dynamics/joint/fixed_joint.rs10
-rw-r--r--src/dynamics/rigid_body.rs174
-rw-r--r--src/dynamics/solver/joint_constraint/fixed_position_constraint.rs34
-rw-r--r--src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs12
-rw-r--r--src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs24
6 files changed, 189 insertions, 78 deletions
diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index e039bfc..4725319 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -10,7 +10,7 @@ pub struct IntegrationParameters {
///
/// When CCD with multiple substeps is enabled, the timestep is subdivided
/// into smaller pieces. This timestep subdivision won't generate timestep
- /// lengths smaller than `min_dt`.
+ /// lengths smaller than `min_ccd_dt`.
///
/// Setting this to a large value will reduce the opportunity to performing
/// CCD substepping, resulting in potentially more time dropped by the
@@ -18,17 +18,6 @@ pub struct IntegrationParameters {
/// to numerical instabilities.
pub min_ccd_dt: Real,
- // /// If `true` and if rapier is compiled with the `parallel` feature, this will enable rayon-based multithreading (default: `true`).
- // ///
- // /// This parameter is ignored if rapier is not compiled with is `parallel` feature.
- // /// Refer to rayon's documentation regarding how to configure the number of threads with either
- // /// `rayon::ThreadPoolBuilder::new().num_threads(4).build_global().unwrap()` or `ThreadPool::install`.
- // /// Note that using only one thread with `multithreading_enabled` set to `true` will result on a slower
- // /// simulation than setting `multithreading_enabled` to `false`.
- // pub multithreading_enabled: bool,
- // /// If `true`, the world's `step` method will stop right after resolving exactly one CCD event (default: `false`).
- // /// This allows the user to take action during a timestep, in-between two CCD events.
- // pub return_after_ccd_substep: bool,
/// The Error Reduction Parameter in `[0, 1]` is the proportion of
/// the positional error to be corrected at each time step (default: `0.2`).
pub erp: Real,
diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs
index 2917757..3f87e8d 100644
--- a/src/dynamics/joint/fixed_joint.rs
+++ b/src/dynamics/joint/fixed_joint.rs
@@ -8,10 +8,10 @@ use crate::math::{Isometry, Real, SpacialVector};
pub struct FixedJoint {
/// The frame of reference for the first body affected by this joint, expressed in the local frame
/// of the first body.
- pub local_anchor1: Isometry<Real>,
+ pub local_frame1: Isometry<Real>,
/// The frame of reference for the second body affected by this joint, expressed in the local frame
/// of the first body.
- pub local_anchor2: Isometry<Real>,
+ pub local_frame2: Isometry<Real>,
/// The impulse applied to the first body affected by this joint.
///
/// The impulse applied to the second body affected by this joint is given by `-impulse`.
@@ -23,10 +23,10 @@ pub struct FixedJoint {
impl FixedJoint {
/// Creates a new fixed joint from the frames of reference of both bodies.
- pub fn new(local_anchor1: Isometry<Real>, local_anchor2: Isometry<Real>) -> Self {
+ pub fn new(local_frame1: Isometry<Real>, local_frame2: Isometry<Real>) -> Self {
Self {
- local_anchor1,
- local_anchor2,
+ local_frame1,
+ local_frame2,
impulse: SpacialVector::zeros(),
}
}
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 824ec92..d53ff98 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -124,6 +124,72 @@ impl RigidBody {
self.rb_dominance.effective_group(&self.rb_type)
}
+ #[inline]
+ /// Locks or unlocks all the rotations of this rigid-body.
+ pub fn lock_rotations(&mut self, locked: bool, wake_up: bool) {
+ if self.is_dynamic() {
+ if wake_up {
+ self.wake_up(true);
+ }
+
+ self.rb_mprops
+ .flags
+ .set(RigidBodyMassPropsFlags::ROTATION_LOCKED_X, locked);
+ self.rb_mprops
+ .flags
+ .set(RigidBodyMassPropsFlags::ROTATION_LOCKED_Y, locked);
+ self.rb_mprops
+ .flags
+ .set(RigidBodyMassPropsFlags::ROTATION_LOCKED_Z, locked);
+ self.update_world_mass_properties();
+ }
+ }
+
+ #[inline]
+ /// Locks or unlocks rotations of this rigid-body along each cartesian axes.
+ pub fn restrict_rotations(
+ &mut self,
+ allow_rotations_x: bool,
+ allow_rotations_y: bool,
+ allow_rotations_z: bool,
+ wake_up: bool,
+ ) {
+ if self.is_dynamic() {
+ if wake_up {
+ self.wake_up(true);
+ }
+
+ self.rb_mprops.flags.set(
+ RigidBodyMassPropsFlags::ROTATION_LOCKED_X,
+ allow_rotations_x,
+ );
+ self.rb_mprops.flags.set(
+ RigidBodyMassPropsFlags::ROTATION_LOCKED_Y,
+ allow_rotations_y,
+ );
+ self.rb_mprops.flags.set(
+ RigidBodyMassPropsFlags::ROTATION_LOCKED_Z,
+ allow_rotations_z,
+ );
+ self.update_world_mass_properties();
+ }
+ }
+
+ #[inline]
+ /// Locks or unlocks all the rotations of this rigid-body.
+ pub fn lock_translations(&mut self, locked: bool, wake_up: bool) {
+ if self.is_dynamic() {
+ if wake_up {
+ self.wake_up(true);
+ }
+
+ self.rb_mprops
+ .flags
+ .set(RigidBodyMassPropsFlags::TRANSLATION_LOCKED, locked);
+ self.update_world_mass_properties();
+ }
+ }
+
/// Are the translations of this rigid-body locked?
pub fn is_translation_locked(&self) -> bool {
self.rb_mprops
@@ -251,6 +317,16 @@ impl RigidBody {
self.rb_forces.gravity_scale = scale;
}
+ /// The dominance group of this rigid-body.
+ pub fn dominance_group(&self) -> i8 {
+ self.rb_dominance.0
+ }
+
+ /// The dominance group of this rigid-body.
+ pub fn set_dominance_group(&mut self, dominance: i8) {
+ self.rb_dominance.0 = dominance
+ }
+
/// Adds a collider to this rigid-body.
// TODO ECS: we keep this public for now just to simply our experiments on bevy_rapier.
pub fn add_collider(
@@ -279,9 +355,10 @@ impl RigidBody {
if let Some(i) = self.rb_colliders.0.iter().position(|e| *e == handle) {
self.changes.set(RigidBodyChanges::COLLIDERS, true);
self.rb_colliders.0.swap_remove(i);
+
let mass_properties = coll
.mass_properties()
- .transform_by(coll.position_wrt_parent());
+ .transform_by(coll.position_wrt_parent().unwrap());
self.rb_mprops.mass_properties -= mass_properties;
self.update_world_mass_properties();
}
@@ -384,6 +461,45 @@ impl RigidBody {
&self.rb_pos.position
}
+ /// The translational part of this rigid-body's position.
+ #[inline]
+ pub fn translation(&self) -> Vector<Real> {
+ self.rb_pos.position.translation.vector
+ }
+
+ /// Sets the translational part of this rigid-body's position.
+ #[inline]
+ pub fn set_translation(&mut self, translation: Vector<Real>, wake_up: bool) {
+ self.changes.insert(RigidBodyChanges::POSITION);
+ self.rb_pos.position.translation.vector = translation;
+ self.rb_pos.next_position.translation.vector = translation;
+
+ // TODO: Do we really need to check that the body isn't dynamic?
+ if wake_up && self.is_dynamic() {
+ self.wake_up(true)
+ }
+ }
+
+ /// The translational part of this rigid-body's position.
+ #[inline]
+ pub fn rotation(&self) -> Rotation<Real> {
+ self.rb_pos.position.rotation
+ }
+
+ /// Sets the rotational part of this rigid-body's position.
+ #[inline]
+ pub fn set_rotation(&mut self, rotation: AngVector<Real>, wake_up: bool) {
+ self.changes.insert(RigidBodyChanges::POSITION);
+ let rotation = Rotation::new(rotation);
+ self.rb_pos.position.rotation = rotation;
+ self.rb_pos.next_position.rotation = rotation;
+
+ // TODO: Do we really need to check that the body isn't dynamic?
+ if wake_up && self.is_dynamic() {
+ self.wake_up(true)
+ }
+ }
+
/// Sets the position and `next_kinematic_position` of this rigid body.
///
/// This will teleport the rigid-body to the specified position/orientation,
@@ -404,6 +520,20 @@ impl RigidBody {
}
}
+ /// If this rigid body is kinematic, sets its future translation after the next timestep integration.
+ pub fn set_next_kinematic_rotation(&mut self, rotation: AngVector<Real>) {
+ if self.is_kinematic() {
+ self.rb_pos.next_position.rotation = Rotation::new(rotation);
+ }
+ }
+
+ /// If this rigid body is kinematic, sets its future orientation after the next timestep integration.
+ pub fn set_next_kinematic_translation(&mut self, rotation: Rotation<Real>) {
+ if self.is_kinematic() {
+ self.rb_pos.next_position.rotation = rotation;
+ }
+ }
+
/// If this rigid body is kinematic, sets its future position after the next timestep integration.
pub fn set_next_kinematic_position(&mut self, pos: Isometry<Real>) {
if self.is_kinematic() {
@@ -411,6 +541,17 @@ impl RigidBody {
}
}
+ /// Predicts the next position of this rigid-body, by integrating its velocity and forces
+ /// by a time of `dt`.
+ pub fn predict_position_using_velocity_and_forces(&self, dt: Real) -> Isometry<Real> {
+ self.rb_pos.integrate_forces_and_velocities(
+ dt,
+ &self.rb_forces,
+ &self.rb_vels,
+ &self.rb_mprops,
+ )
+ }
+
pub(crate) fn update_world_mass_properties(&mut self) {
self.rb_mprops
.update_world_mass_properties(&self.rb_pos.position);
@@ -618,8 +759,8 @@ impl RigidBodyBuilder {
}
/// Sets the scale applied to the gravity force affecting the rigid-body to be created.
- pub fn gravity_scale(mut self, x: Real) -> Self {
- self.gravity_scale = x;
+ pub fn gravity_scale(mut self, scale_factor: Real) -> Self {
+ self.gravity_scale = scale_factor;
self
}
@@ -630,19 +771,8 @@ impl RigidBodyBuilder {
}
/// Sets the initial translation of the rigid-body to be created.
- #[cfg(feature = "dim2")]
- pub fn translation(mut self, x: Real, y: Real) -> Self {
- self.position.translation.x = x;
- self.position.translation.y = y;
- self
- }
-
- /// Sets the initial translation of the rigid-body to be created.
- #[cfg(feature = "dim3")]
- pub fn translation(mut self, x: Real, y: Real, z: Real) -> Self {
- self.position.translation.x = x;
- self.position.translation.y = y;
- self.position.translation.z = z;
+ pub fn translation(mut self, translation: Vector<Real>) -> Self {
+ self.position.translation.vector = translation;
self
}
@@ -811,16 +941,8 @@ impl RigidBodyBuilder {
}
/// Sets the initial linear velocity of the rigid-body to be created.
- #[cfg(feature = "dim2")]
- pub fn linvel(mut self, x: Real, y: Real) -> Self {
- self.linvel = Vector::new(x, y);
- self
- }
-
- /// Sets the initial linear velocity of the rigid-body to be created.
- #[cfg(feature = "dim3")]
- pub fn linvel(mut self, x: Real, y: Real, z: Real) -> Self {
- self.linvel = Vector::new(x, y, z);
+ pub fn linvel(mut self, linvel: Vector<Real>) -> Self {
+ self.linvel = linvel;
self
}
diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
index 239138f..f8945c3 100644
--- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
@@ -8,8 +8,8 @@ use crate::utils::WAngularInertia;
pub(crate) struct FixedPositionConstraint {
position1: usize,
position2: usize,
- local_anchor1: Isometry<Real>,
- local_anchor2: Isometry<Real>,
+ local_frame1: Isometry<Real>,
+ local_frame2: Isometry<Real>,
local_com1: Point<Real>,
local_com2: Point<Real>,
im1: Real,
@@ -38,8 +38,8 @@ impl FixedPositionConstraint {
let ang_inv_lhs = (ii1 + ii2).inverse();
Self {
- local_anchor1: cparams.local_anchor1,
- local_anchor2: cparams.local_anchor2,
+ local_frame1: cparams.local_frame1,
+ local_frame2: cparams.local_frame2,
position1: ids1.active_set_offset,
position2: ids2.active_set_offset,
im1,
@@ -58,8 +58,8 @@ impl FixedPositionConstraint {
let mut position2 = positions[self.position2 as usize];
// Angular correction.
- let anchor1 = position1 * self.local_anchor1;
- let anchor2 = position2 * self.local_anchor2;
+ let anchor1 = position1 * self.local_frame1;
+ let anchor2 = position2 * self.local_frame2;
let ang_err = anchor2.rotation * anchor1.rotation.inverse();
#[cfg(feature = "dim3")]
let ang_impulse = self
@@ -75,8 +75,8 @@ impl FixedPositionConstraint {
Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation;
// Linear correction.
- let anchor1 = position1 * Point::from(self.local_anchor1.translation.vector);
- let anchor2 = position2 * Point::from(self.local_anchor2.translation.vector);
+ let anchor1 = position1 * Point::from(self.local_frame1.translation.vector);
+ let anchor2 = position2 * Point::from(self.local_frame2.translation.vector);
let err = anchor2 - anchor1;
let impulse = err * (self.lin_inv_lhs * params.joint_erp);
position1.translation.vector += self.im1 * impulse;
@@ -91,7 +91,7 @@ impl FixedPositionConstraint {
pub(crate) struct FixedPositionGroundConstraint {
position2: usize,
anchor1: Isometry<Real>,
- local_anchor2: Isometry<Real>,
+ local_frame2: Isometry<Real>,
local_com2: Point<Real>,
im2: Real,
ii2: AngularInertia<Real>,
@@ -109,19 +109,19 @@ impl FixedPositionGroundConstraint {
let (mprops2, ids2) = rb2;
let anchor1;
- let local_anchor2;
+ let local_frame2;
if flipped {
- anchor1 = poss1.next_position * cparams.local_anchor2;
- local_anchor2 = cparams.local_anchor1;
+ anchor1 = poss1.next_position * cparams.local_frame2;
+ local_frame2 = cparams.local_frame1;
} else {
- anchor1 = poss1.next_position * cparams.local_anchor1;
- local_anchor2 = cparams.local_anchor2;
+ anchor1 = poss1.next_position * cparams.local_frame1;
+ local_frame2 = cparams.local_frame2;
};
Self {
anchor1,
- local_anchor2,
+ local_frame2,
position2: ids2.active_set_offset,
im2: mprops2.effective_inv_mass,
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
@@ -134,13 +134,13 @@ impl FixedPositionGroundConstraint {
let mut position2 = positions[self.position2 as usize];
// Angular correction.
- let anchor2 = position2 * self.local_anchor2;
+ let anchor2 = position2 * self.local_frame2;
let ang_err = anchor2.rotation * self.anchor1.rotation.inverse();
position2.rotation = ang_err.powf(-params.joint_erp) * position2.rotation;
// Linear correction.
let anchor1 = Point::from(self.anchor1.translation.vector);
- let anchor2 = position2 * Point::from(self.local_anchor2.translation.vector);
+ let anchor2 = position2 * Point::from(self.local_frame2.translation.vector);
let err = anchor2 - anchor1;
// NOTE: no need to divide by im2 just to multiply right after.
let impulse = err * params.joint_erp;
diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
index a0c0739..8bfc1a6 100644
--- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
@@ -63,8 +63,8 @@ impl FixedVelocityConstraint {
let (poss1, vels1, mprops1, ids1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
- let anchor1 = poss1.position * cparams.local_anchor1;
- let anchor2 = poss2.position * cparams.local_anchor2;
+ let anchor1 = poss1.position * cparams.local_frame1;
+ let anchor2 = poss2.position * cparams.local_frame2;
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
@@ -280,13 +280,13 @@ impl FixedVelocityGroundConstraint {
let (anchor1, anchor2) = if flipped {
(
- poss1.position * cparams.local_anchor2,
- poss2.position * cparams.local_anchor1,
+ poss1.position * cparams.local_frame2,
+ poss2.position * cparams.local_frame1,
)
} else {
(
- poss1.position * cparams.local_anchor1,
- poss2.position * cparams.local_anchor2,
+ poss1.position * cparams.local_frame1,
+ poss2.position * cparams.local_frame2,
)
};
diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
index 84e1fca..0421d49 100644
--- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
@@ -91,12 +91,12 @@ impl WFixedVelocityConstraint {
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
- let local_anchor1 = Isometry::from(gather![|ii| cparams[ii].local_anchor1]);
- let local_anchor2 = Isometry::from(gather![|ii| cparams[ii].local_anchor2]);
+ let local_frame1 = Isometry::from(gather![|ii| cparams[ii].local_frame1]);
+ let local_frame2 = Isometry::from(gather![|ii| cparams[ii].local_frame2]);
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
- let anchor1 = position1 * local_anchor1;
- let anchor2 = position2 * local_anchor2;
+ let anchor1 = position1 * local_frame1;
+ let anchor2 = position2 * local_frame2;
let ii1 = ii1_sqrt.squared();
let ii2 = ii2_sqrt.squared();
let r1 = anchor1.translation.vector - world_com1.coords;
@@ -363,20 +363,20 @@ impl WFixedVelocityGroundConstraint {
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
- let local_anchor1 = Isometry::from(gather![|ii| if flipped[ii] {
- cparams[ii].local_anchor2
+ let local_frame1 = Isometry::from(gather![|ii| if flipped[ii] {
+ cparams[ii].local_frame2
} else {
- cparams[ii].local_anchor1
+ cparams[ii].local_frame1
}]);
- let local_anchor2 = Isometry::from(gather![|ii| if flipped[ii] {
- cparams[ii].local_anchor1
+ let local_frame2 = Isometry::from(gather![|ii| if flipped[ii] {
+ cparams[ii].local_frame1
} else {
- cparams[ii].local_anchor2
+ cparams[ii].local_frame2
}]);
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
- let anchor1 = position1 * local_anchor1;
- let anchor2 = position2 * local_anchor2;
+ let anchor1 = position1 * local_frame1;
+ let anchor2 = position2 * local_frame2;
let ii2 = ii2_sqrt.squared();
let r1 = anchor1.translation.vector - world_com1.coords;
let r2 = anchor2.translation.vector - world_com2.coords;
generated by cgit (git 2.34.1) at 2026-02-03 20:54:14 +0000