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

Add prelude + use vectors for setting linvel/translation in builders

Browse Source
This commit is contained in:
Crozet Sébastien
2021-05-25 11:00:13 +02:00
parent 47139323e0
commit 1bef66fea9
93 changed files with 1528 additions and 1259 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
src/dynamics/integration_parameters.rs
View File

@@ -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,

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

@@ -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(),
}
}

174
src/dynamics/rigid_body.rs
View File

@@ -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
}

34
src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
View File

@@ -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;

12
src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
View File

@@ -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,
)
};

24
src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
View File

@@ -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;