From aa61fe65e3ff0289ecab57b4053a3410cf6d4a87 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 4 Jan 2021 15:14:25 +0100 Subject: Add support of 64-bits reals. --- src/dynamics/rigid_body.rs | 122 +++++++++++++++++++++++---------------------- 1 file changed, 62 insertions(+), 60 deletions(-) (limited to 'src/dynamics/rigid_body.rs') diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 5128b6f..683cca8 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -2,7 +2,9 @@ use crate::dynamics::MassProperties; use crate::geometry::{ Collider, ColliderHandle, ColliderSet, InteractionGraph, RigidBodyGraphIndex, }; -use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Translation, Vector}; +use crate::math::{ + AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, +}; use crate::utils::{self, WCross, WDot}; use num::Zero; @@ -54,24 +56,24 @@ bitflags::bitflags! { #[derive(Debug, Clone)] pub struct RigidBody { /// The world-space position of the rigid-body. - pub(crate) position: Isometry, - pub(crate) predicted_position: Isometry, + pub(crate) position: Isometry, + pub(crate) predicted_position: Isometry, /// The local mass properties of the rigid-body. pub(crate) mass_properties: MassProperties, /// The world-space center of mass of the rigid-body. - pub world_com: Point, + pub world_com: Point, /// The square-root of the inverse angular inertia tensor of the rigid-body. - pub world_inv_inertia_sqrt: AngularInertia, + pub world_inv_inertia_sqrt: AngularInertia, /// The linear velocity of the rigid-body. - pub(crate) linvel: Vector, + pub(crate) linvel: Vector, /// The angular velocity of the rigid-body. - pub(crate) angvel: AngVector, + pub(crate) angvel: AngVector, /// Damping factor for gradually slowing down the translational motion of the rigid-body. - pub linear_damping: f32, + pub linear_damping: Real, /// Damping factor for gradually slowing down the angular motion of the rigid-body. - pub angular_damping: f32, - pub(crate) linacc: Vector, - pub(crate) angacc: AngVector, + pub angular_damping: Real, + pub(crate) linacc: Vector, + pub(crate) angacc: AngVector, pub(crate) colliders: Vec, /// Whether or not this rigid-body is sleeping. pub activation: ActivationStatus, @@ -125,7 +127,7 @@ impl RigidBody { self.active_set_timestamp = 0; } - pub(crate) fn integrate_accelerations(&mut self, dt: f32, gravity: Vector) { + pub(crate) fn integrate_accelerations(&mut self, dt: Real, gravity: Vector) { if self.mass_properties.inv_mass != 0.0 { self.linvel += (gravity + self.linacc) * dt; self.angvel += self.angacc * dt; @@ -184,7 +186,7 @@ impl RigidBody { /// The mass of this rigid body. /// /// Returns zero if this rigid body has an infinite mass. - pub fn mass(&self) -> f32 { + pub fn mass(&self) -> Real { utils::inv(self.mass_properties.inv_mass) } @@ -193,7 +195,7 @@ impl RigidBody { /// If this rigid-body is kinematic this value is set by the `set_next_kinematic_position` /// method and is used for estimating the kinematic body velocity at the next timestep. /// For non-kinematic bodies, this value is currently unspecified. - pub fn predicted_position(&self) -> &Isometry { + pub fn predicted_position(&self) -> &Isometry { &self.predicted_position } @@ -311,13 +313,13 @@ impl RigidBody { !self.linvel.is_zero() || !self.angvel.is_zero() } - fn integrate_velocity(&self, dt: f32) -> Isometry { + fn integrate_velocity(&self, dt: Real) -> Isometry { let com = &self.position * self.mass_properties.local_com; let shift = Translation::from(com.coords); shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse() } - pub(crate) fn integrate(&mut self, dt: f32) { + pub(crate) fn integrate(&mut self, dt: Real) { // TODO: do we want to apply damping before or after the velocity integration? self.linvel *= 1.0 / (1.0 + dt * self.linear_damping); self.angvel *= 1.0 / (1.0 + dt * self.angular_damping); @@ -326,19 +328,19 @@ impl RigidBody { } /// The linear velocity of this rigid-body. - pub fn linvel(&self) -> &Vector { + pub fn linvel(&self) -> &Vector { &self.linvel } /// The angular velocity of this rigid-body. #[cfg(feature = "dim2")] - pub fn angvel(&self) -> f32 { + pub fn angvel(&self) -> Real { self.angvel } /// The angular velocity of this rigid-body. #[cfg(feature = "dim3")] - pub fn angvel(&self) -> &Vector { + pub fn angvel(&self) -> &Vector { &self.angvel } @@ -346,7 +348,7 @@ impl RigidBody { /// /// If `wake_up` is `true` then the rigid-body will be woken up if it was /// put to sleep because it did not move for a while. - pub fn set_linvel(&mut self, linvel: Vector, wake_up: bool) { + pub fn set_linvel(&mut self, linvel: Vector, wake_up: bool) { self.linvel = linvel; if self.is_dynamic() && wake_up { @@ -359,7 +361,7 @@ impl RigidBody { /// If `wake_up` is `true` then the rigid-body will be woken up if it was /// put to sleep because it did not move for a while. #[cfg(feature = "dim2")] - pub fn set_angvel(&mut self, angvel: f32, wake_up: bool) { + pub fn set_angvel(&mut self, angvel: Real, wake_up: bool) { self.angvel = angvel; if self.is_dynamic() && wake_up { @@ -372,7 +374,7 @@ impl RigidBody { /// If `wake_up` is `true` then the rigid-body will be woken up if it was /// put to sleep because it did not move for a while. #[cfg(feature = "dim3")] - pub fn set_angvel(&mut self, angvel: Vector, wake_up: bool) { + pub fn set_angvel(&mut self, angvel: Vector, wake_up: bool) { self.angvel = angvel; if self.is_dynamic() && wake_up { @@ -381,7 +383,7 @@ impl RigidBody { } /// The world-space position of this rigid-body. - pub fn position(&self) -> &Isometry { + pub fn position(&self) -> &Isometry { &self.position } @@ -394,7 +396,7 @@ impl RigidBody { /// /// If `wake_up` is `true` then the rigid-body will be woken up if it was /// put to sleep because it did not move for a while. - pub fn set_position(&mut self, pos: Isometry, wake_up: bool) { + pub fn set_position(&mut self, pos: Isometry, wake_up: bool) { self.changes.insert(RigidBodyChanges::POSITION); self.set_position_internal(pos); @@ -404,7 +406,7 @@ impl RigidBody { } } - pub(crate) fn set_position_internal(&mut self, pos: Isometry) { + pub(crate) fn set_position_internal(&mut self, pos: Isometry) { self.position = pos; // TODO: update the predicted position for dynamic bodies too? @@ -414,13 +416,13 @@ impl RigidBody { } /// 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) { + pub fn set_next_kinematic_position(&mut self, pos: Isometry) { if self.is_kinematic() { self.predicted_position = pos; } } - pub(crate) fn compute_velocity_from_predicted_position(&mut self, inv_dt: f32) { + pub(crate) fn compute_velocity_from_predicted_position(&mut self, inv_dt: Real) { let dpos = self.predicted_position * self.position.inverse(); #[cfg(feature = "dim2")] { @@ -433,7 +435,7 @@ impl RigidBody { self.linvel = dpos.translation.vector * inv_dt; } - pub(crate) fn update_predicted_position(&mut self, dt: f32) { + pub(crate) fn update_predicted_position(&mut self, dt: Real) { self.predicted_position = self.integrate_velocity(dt) * self.position; } @@ -448,7 +450,7 @@ impl RigidBody { * Application of forces/impulses. */ /// Applies a force at the center-of-mass of this rigid-body. - pub fn apply_force(&mut self, force: Vector, wake_up: bool) { + pub fn apply_force(&mut self, force: Vector, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { self.linacc += force * self.mass_properties.inv_mass; @@ -459,7 +461,7 @@ impl RigidBody { } /// Applies an impulse at the center-of-mass of this rigid-body. - pub fn apply_impulse(&mut self, impulse: Vector, wake_up: bool) { + pub fn apply_impulse(&mut self, impulse: Vector, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { self.linvel += impulse * self.mass_properties.inv_mass; @@ -471,7 +473,7 @@ impl RigidBody { /// Applies a torque at the center-of-mass of this rigid-body. #[cfg(feature = "dim2")] - pub fn apply_torque(&mut self, torque: f32, wake_up: bool) { + pub fn apply_torque(&mut self, torque: Real, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque); @@ -483,7 +485,7 @@ impl RigidBody { /// Applies a torque at the center-of-mass of this rigid-body. #[cfg(feature = "dim3")] - pub fn apply_torque(&mut self, torque: Vector, wake_up: bool) { + pub fn apply_torque(&mut self, torque: Vector, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque); @@ -495,7 +497,7 @@ impl RigidBody { /// Applies an impulsive torque at the center-of-mass of this rigid-body. #[cfg(feature = "dim2")] - pub fn apply_torque_impulse(&mut self, torque_impulse: f32, wake_up: bool) { + pub fn apply_torque_impulse(&mut self, torque_impulse: Real, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { self.angvel += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse); @@ -508,7 +510,7 @@ impl RigidBody { /// Applies an impulsive torque at the center-of-mass of this rigid-body. #[cfg(feature = "dim3")] - pub fn apply_torque_impulse(&mut self, torque_impulse: Vector, wake_up: bool) { + pub fn apply_torque_impulse(&mut self, torque_impulse: Vector, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { self.angvel += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse); @@ -520,7 +522,7 @@ impl RigidBody { } /// Applies a force at the given world-space point of this rigid-body. - pub fn apply_force_at_point(&mut self, force: Vector, point: Point, wake_up: bool) { + pub fn apply_force_at_point(&mut self, force: Vector, point: Point, wake_up: bool) { let torque = (point - self.world_com).gcross(force); self.apply_force(force, wake_up); self.apply_torque(torque, wake_up); @@ -529,8 +531,8 @@ impl RigidBody { /// Applies an impulse at the given world-space point of this rigid-body. pub fn apply_impulse_at_point( &mut self, - impulse: Vector, - point: Point, + impulse: Vector, + point: Point, wake_up: bool, ) { let torque_impulse = (point - self.world_com).gcross(impulse); @@ -539,7 +541,7 @@ impl RigidBody { } /// The velocity of the given world-space point on this rigid-body. - pub fn velocity_at_point(&self, point: &Point) -> Vector { + pub fn velocity_at_point(&self, point: &Point) -> Vector { let dpt = point - self.world_com; self.linvel + self.angvel.gcross(dpt) } @@ -547,11 +549,11 @@ impl RigidBody { /// A builder for rigid-bodies. pub struct RigidBodyBuilder { - position: Isometry, - linvel: Vector, - angvel: AngVector, - linear_damping: f32, - angular_damping: f32, + position: Isometry, + linvel: Vector, + angvel: AngVector, + linear_damping: Real, + angular_damping: Real, body_status: BodyStatus, flags: RigidBodyFlags, mass_properties: MassProperties, @@ -595,7 +597,7 @@ impl RigidBodyBuilder { /// Sets the initial translation of the rigid-body to be created. #[cfg(feature = "dim2")] - pub fn translation(mut self, x: f32, y: f32) -> Self { + pub fn translation(mut self, x: Real, y: Real) -> Self { self.position.translation.x = x; self.position.translation.y = y; self @@ -603,7 +605,7 @@ impl RigidBodyBuilder { /// Sets the initial translation of the rigid-body to be created. #[cfg(feature = "dim3")] - pub fn translation(mut self, x: f32, y: f32, z: f32) -> Self { + 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; @@ -611,13 +613,13 @@ impl RigidBodyBuilder { } /// Sets the initial orientation of the rigid-body to be created. - pub fn rotation(mut self, angle: AngVector) -> Self { + pub fn rotation(mut self, angle: AngVector) -> Self { self.position.rotation = Rotation::new(angle); self } /// Sets the initial position (translation and orientation) of the rigid-body to be created. - pub fn position(mut self, pos: Isometry) -> Self { + pub fn position(mut self, pos: Isometry) -> Self { self.position = pos; self } @@ -675,7 +677,7 @@ impl RigidBodyBuilder { /// will depends on the initial mass set by this method to which is added /// the contributions of all the colliders with non-zero density attached to /// this rigid-body. - pub fn mass(mut self, mass: f32, colliders_contribution_enabled: bool) -> Self { + pub fn mass(mut self, mass: Real, colliders_contribution_enabled: bool) -> Self { self.mass_properties.inv_mass = utils::inv(mass); self.flags.set( RigidBodyFlags::IGNORE_COLLIDER_MASS, @@ -696,7 +698,7 @@ impl RigidBodyBuilder { #[cfg(feature = "dim2")] pub fn principal_angular_inertia( mut self, - inertia: f32, + inertia: Real, colliders_contribution_enabled: bool, ) -> Self { self.mass_properties.inv_principal_inertia_sqrt = utils::inv(inertia); @@ -712,7 +714,7 @@ impl RigidBodyBuilder { /// Use `self.principal_angular_inertia` instead. #[cfg(feature = "dim2")] #[deprecated(note = "renamed to `principal_angular_inertia`.")] - pub fn principal_inertia(self, inertia: f32, colliders_contribution_enabled: bool) -> Self { + pub fn principal_inertia(self, inertia: Real, colliders_contribution_enabled: bool) -> Self { self.principal_angular_inertia(inertia, colliders_contribution_enabled) } @@ -731,7 +733,7 @@ impl RigidBodyBuilder { #[cfg(feature = "dim3")] pub fn principal_angular_inertia( mut self, - inertia: AngVector, + inertia: AngVector, colliders_contribution_enabled: AngVector, ) -> Self { self.mass_properties.inv_principal_inertia_sqrt = inertia.map(utils::inv); @@ -755,7 +757,7 @@ impl RigidBodyBuilder { #[deprecated(note = "renamed to `principal_angular_inertia`.")] pub fn principal_inertia( self, - inertia: AngVector, + inertia: AngVector, colliders_contribution_enabled: AngVector, ) -> Self { self.principal_angular_inertia(inertia, colliders_contribution_enabled) @@ -765,7 +767,7 @@ impl RigidBodyBuilder { /// /// The higher the linear damping factor is, the more quickly the rigid-body /// will slow-down its translational movement. - pub fn linear_damping(mut self, factor: f32) -> Self { + pub fn linear_damping(mut self, factor: Real) -> Self { self.linear_damping = factor; self } @@ -774,27 +776,27 @@ impl RigidBodyBuilder { /// /// The higher the angular damping factor is, the more quickly the rigid-body /// will slow-down its rotational movement. - pub fn angular_damping(mut self, factor: f32) -> Self { + pub fn angular_damping(mut self, factor: Real) -> Self { self.angular_damping = factor; self } /// Sets the initial linear velocity of the rigid-body to be created. #[cfg(feature = "dim2")] - pub fn linvel(mut self, x: f32, y: f32) -> Self { + 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: f32, y: f32, z: f32) -> Self { + pub fn linvel(mut self, x: Real, y: Real, z: Real) -> Self { self.linvel = Vector::new(x, y, z); self } /// Sets the initial angular velocity of the rigid-body to be created. - pub fn angvel(mut self, angvel: AngVector) -> Self { + pub fn angvel(mut self, angvel: AngVector) -> Self { self.angvel = angvel; self } @@ -845,16 +847,16 @@ impl RigidBodyBuilder { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct ActivationStatus { /// The threshold pseudo-kinetic energy bellow which the body can fall asleep. - pub threshold: f32, + pub threshold: Real, /// The current pseudo-kinetic energy of the body. - pub energy: f32, + pub energy: Real, /// Is this body already sleeping? pub sleeping: bool, } impl ActivationStatus { /// The default amount of energy bellow which a body can be put to sleep by nphysics. - pub fn default_threshold() -> f32 { + pub fn default_threshold() -> Real { 0.01 } -- cgit From 2231d0f6ea994cbe768ea3a9d60ad29d9a383c9b Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Wed, 6 Jan 2021 18:09:21 +0100 Subject: Add gravity scaling to rigid-bodies. --- src/dynamics/rigid_body.rs | 28 +++++++++++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) (limited to 'src/dynamics/rigid_body.rs') diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 683cca8..1574100 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -75,6 +75,7 @@ pub struct RigidBody { pub(crate) linacc: Vector, pub(crate) angacc: AngVector, pub(crate) colliders: Vec, + pub(crate) gravity_scale: Real, /// Whether or not this rigid-body is sleeping. pub activation: ActivationStatus, pub(crate) joint_graph_index: RigidBodyGraphIndex, @@ -102,6 +103,7 @@ impl RigidBody { angvel: na::zero(), linacc: Vector::zeros(), angacc: na::zero(), + gravity_scale: 1.0, linear_damping: 0.0, angular_damping: 0.0, colliders: Vec::new(), @@ -129,7 +131,7 @@ impl RigidBody { pub(crate) fn integrate_accelerations(&mut self, dt: Real, gravity: Vector) { if self.mass_properties.inv_mass != 0.0 { - self.linvel += (gravity + self.linacc) * dt; + self.linvel += (gravity * self.gravity_scale + self.linacc) * dt; self.angvel += self.angacc * dt; // Reset the accelerations. @@ -199,6 +201,21 @@ impl RigidBody { &self.predicted_position } + /// The scale factor applied to the gravity affecting this rigid-body. + pub fn gravity_scale(&self) -> Real { + self.gravity_scale + } + + /// Sets the gravity scale facter for this rigid-body. + pub fn set_gravity_scale(&mut self, scale: Real, wake_up: bool) { + if wake_up && self.activation.sleeping { + self.changes.insert(RigidBodyChanges::SLEEP); + self.activation.sleeping = false; + } + + self.gravity_scale = scale; + } + /// Adds a collider to this rigid-body. pub(crate) fn add_collider(&mut self, handle: ColliderHandle, coll: &Collider) { self.changes.set( @@ -552,6 +569,7 @@ pub struct RigidBodyBuilder { position: Isometry, linvel: Vector, angvel: AngVector, + gravity_scale: Real, linear_damping: Real, angular_damping: Real, body_status: BodyStatus, @@ -569,6 +587,7 @@ impl RigidBodyBuilder { position: Isometry::identity(), linvel: Vector::zeros(), angvel: na::zero(), + gravity_scale: 1.0, linear_damping: 0.0, angular_damping: 0.0, body_status, @@ -595,6 +614,12 @@ impl RigidBodyBuilder { Self::new(BodyStatus::Dynamic) } + /// 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; + self + } + /// Sets the initial translation of the rigid-body to be created. #[cfg(feature = "dim2")] pub fn translation(mut self, x: Real, y: Real) -> Self { @@ -825,6 +850,7 @@ impl RigidBodyBuilder { rb.mass_properties = self.mass_properties; rb.linear_damping = self.linear_damping; rb.angular_damping = self.angular_damping; + rb.gravity_scale = self.gravity_scale; rb.flags = self.flags; if self.can_sleep && self.sleeping { -- cgit From 0ade350b5f4b6e7c0c4116e1f4f2b30ab86b7e52 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Wed, 20 Jan 2021 16:33:42 +0100 Subject: Use newtypes for collider, rigid-body and joint handles. --- src/dynamics/rigid_body.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/dynamics/rigid_body.rs') diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 1574100..20d04b0 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -108,7 +108,7 @@ impl RigidBody { angular_damping: 0.0, colliders: Vec::new(), activation: ActivationStatus::new_active(), - joint_graph_index: InteractionGraph::<()>::invalid_graph_index(), + joint_graph_index: InteractionGraph::<(), ()>::invalid_graph_index(), active_island_id: 0, active_set_id: 0, active_set_offset: 0, @@ -122,7 +122,7 @@ impl RigidBody { pub(crate) fn reset_internal_references(&mut self) { self.colliders = Vec::new(); - self.joint_graph_index = InteractionGraph::<()>::invalid_graph_index(); + self.joint_graph_index = InteractionGraph::<(), ()>::invalid_graph_index(); self.active_island_id = 0; self.active_set_id = 0; self.active_set_offset = 0; -- cgit From 8f330b2a00610e5b68c1acd9208120e8f750c7aa Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Thu, 21 Jan 2021 14:58:40 +0100 Subject: Rotation locking: apply filter only to the world inertia properties to fix the multi-collider case. --- src/dynamics/rigid_body.rs | 194 ++++++++++++++++++++------------------------- 1 file changed, 86 insertions(+), 108 deletions(-) (limited to 'src/dynamics/rigid_body.rs') diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 20d04b0..9279e4f 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -31,10 +31,10 @@ bitflags::bitflags! { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// Flags affecting the behavior of the constraints solver for a given contact manifold. pub(crate) struct RigidBodyFlags: u8 { - const IGNORE_COLLIDER_MASS = 1 << 0; - const IGNORE_COLLIDER_ANGULAR_INERTIA_X = 1 << 1; - const IGNORE_COLLIDER_ANGULAR_INERTIA_Y = 1 << 2; - const IGNORE_COLLIDER_ANGULAR_INERTIA_Z = 1 << 3; + const TRANSLATION_LOCKED = 1 << 0; + const ROTATION_LOCKED_X = 1 << 1; + const ROTATION_LOCKED_Y = 1 << 2; + const ROTATION_LOCKED_Z = 1 << 3; } } @@ -62,8 +62,9 @@ pub struct RigidBody { pub(crate) mass_properties: MassProperties, /// The world-space center of mass of the rigid-body. pub world_com: Point, + pub effective_inv_mass: Real, /// The square-root of the inverse angular inertia tensor of the rigid-body. - pub world_inv_inertia_sqrt: AngularInertia, + pub effective_world_inv_inertia_sqrt: AngularInertia, /// The linear velocity of the rigid-body. pub(crate) linvel: Vector, /// The angular velocity of the rigid-body. @@ -98,7 +99,8 @@ impl RigidBody { predicted_position: Isometry::identity(), mass_properties: MassProperties::zero(), world_com: Point::origin(), - world_inv_inertia_sqrt: AngularInertia::zero(), + effective_inv_mass: 0.0, + effective_world_inv_inertia_sqrt: AngularInertia::zero(), linvel: Vector::zeros(), angvel: na::zero(), linacc: Vector::zeros(), @@ -130,14 +132,13 @@ impl RigidBody { } pub(crate) fn integrate_accelerations(&mut self, dt: Real, gravity: Vector) { - if self.mass_properties.inv_mass != 0.0 { + if self.effective_inv_mass != 0.0 { self.linvel += (gravity * self.gravity_scale + self.linacc) * dt; - self.angvel += self.angacc * dt; - - // Reset the accelerations. self.linacc = na::zero(); - self.angacc = na::zero(); } + + self.angvel += self.angacc * dt; + self.angacc = na::zero(); } /// The mass properties of this rigid-body. @@ -227,40 +228,10 @@ impl RigidBody { .mass_properties() .transform_by(coll.position_wrt_parent()); self.colliders.push(handle); - self.mass_properties += Self::filter_collider_mass_props(mass_properties, self.flags); + self.mass_properties += mass_properties; self.update_world_mass_properties(); } - fn filter_collider_mass_props( - mut props: MassProperties, - flags: RigidBodyFlags, - ) -> MassProperties { - if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_MASS) { - props.inv_mass = 0.0; - } - - #[cfg(feature = "dim2")] - { - if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z) { - props.inv_principal_inertia_sqrt = 0.0; - } - } - #[cfg(feature = "dim3")] - { - if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_X) { - props.inv_principal_inertia_sqrt.x = 0.0; - } - if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Y) { - props.inv_principal_inertia_sqrt.y = 0.0; - } - if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z) { - props.inv_principal_inertia_sqrt.z = 0.0; - } - } - - props - } - pub(crate) fn update_colliders_positions(&mut self, colliders: &mut ColliderSet) { for handle in &self.colliders { let collider = &mut colliders[*handle]; @@ -277,7 +248,7 @@ impl RigidBody { let mass_properties = coll .mass_properties() .transform_by(coll.position_wrt_parent()); - self.mass_properties -= Self::filter_collider_mass_props(mass_properties, self.flags); + self.mass_properties -= mass_properties; self.update_world_mass_properties(); } } @@ -458,9 +429,41 @@ impl RigidBody { pub(crate) fn update_world_mass_properties(&mut self) { self.world_com = self.mass_properties.world_com(&self.position); - self.world_inv_inertia_sqrt = self + self.effective_inv_mass = self.mass_properties.inv_mass; + self.effective_world_inv_inertia_sqrt = self .mass_properties .world_inv_inertia_sqrt(&self.position.rotation); + + // Take into account translation/rotation locking. + if self.flags.contains(RigidBodyFlags::TRANSLATION_LOCKED) { + self.effective_inv_mass = 0.0; + } + + #[cfg(feature = "dim2")] + { + if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Z) { + self.effective_world_inv_inertia_sqrt = 0.0; + } + } + #[cfg(feature = "dim3")] + { + if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_X) { + self.effective_world_inv_inertia_sqrt.m11 = 0.0; + self.effective_world_inv_inertia_sqrt.m12 = 0.0; + self.effective_world_inv_inertia_sqrt.m13 = 0.0; + } + + if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Y) { + self.effective_world_inv_inertia_sqrt.m22 = 0.0; + self.effective_world_inv_inertia_sqrt.m12 = 0.0; + self.effective_world_inv_inertia_sqrt.m23 = 0.0; + } + if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Z) { + self.effective_world_inv_inertia_sqrt.m33 = 0.0; + self.effective_world_inv_inertia_sqrt.m13 = 0.0; + self.effective_world_inv_inertia_sqrt.m23 = 0.0; + } + } } /* @@ -469,7 +472,7 @@ impl RigidBody { /// Applies a force at the center-of-mass of this rigid-body. pub fn apply_force(&mut self, force: Vector, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { - self.linacc += force * self.mass_properties.inv_mass; + self.linacc += force * self.effective_inv_mass; if wake_up { self.wake_up(true); @@ -480,7 +483,7 @@ impl RigidBody { /// Applies an impulse at the center-of-mass of this rigid-body. pub fn apply_impulse(&mut self, impulse: Vector, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { - self.linvel += impulse * self.mass_properties.inv_mass; + self.linvel += impulse * self.effective_inv_mass; if wake_up { self.wake_up(true); @@ -492,7 +495,8 @@ impl RigidBody { #[cfg(feature = "dim2")] pub fn apply_torque(&mut self, torque: Real, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { - self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque); + self.angacc += self.effective_world_inv_inertia_sqrt + * (self.effective_world_inv_inertia_sqrt * torque); if wake_up { self.wake_up(true); @@ -504,7 +508,8 @@ impl RigidBody { #[cfg(feature = "dim3")] pub fn apply_torque(&mut self, torque: Vector, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { - self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque); + self.angacc += self.effective_world_inv_inertia_sqrt + * (self.effective_world_inv_inertia_sqrt * torque); if wake_up { self.wake_up(true); @@ -516,8 +521,8 @@ impl RigidBody { #[cfg(feature = "dim2")] pub fn apply_torque_impulse(&mut self, torque_impulse: Real, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { - self.angvel += - self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse); + self.angvel += self.effective_world_inv_inertia_sqrt + * (self.effective_world_inv_inertia_sqrt * torque_impulse); if wake_up { self.wake_up(true); @@ -529,8 +534,8 @@ impl RigidBody { #[cfg(feature = "dim3")] pub fn apply_torque_impulse(&mut self, torque_impulse: Vector, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { - self.angvel += - self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse); + self.angvel += self.effective_world_inv_inertia_sqrt + * (self.effective_world_inv_inertia_sqrt * torque_impulse); if wake_up { self.wake_up(true); @@ -679,16 +684,28 @@ impl RigidBodyBuilder { } /// Prevents this rigid-body from rotating because of forces. - /// - /// This is equivalent to `self.principal_inertia(0.0, false)` (in 2D) or - /// `self.principal_inertia(Vector3::zeros(), Vector3::repeat(false))` (in 3D). - /// - /// See the documentation of [`RigidBodyBuilder::principal_inertia`] for more details. - pub fn lock_rotations(self) -> Self { - #[cfg(feature = "dim2")] - return self.principal_angular_inertia(0.0, false); - #[cfg(feature = "dim3")] - return self.principal_angular_inertia(Vector::zeros(), Vector::repeat(false)); + pub fn lock_rotations(mut self) -> Self { + self.flags.set(RigidBodyFlags::ROTATION_LOCKED_X, true); + self.flags.set(RigidBodyFlags::ROTATION_LOCKED_Y, true); + self.flags.set(RigidBodyFlags::ROTATION_LOCKED_Z, true); + self + } + + /// Only allow rotations of this rigid-body around specific coordinate axes. + #[cfg(feature = "dim3")] + pub fn restrict_rotations( + mut self, + allow_rotations_x: bool, + allow_rotations_y: bool, + allow_rotations_z: bool, + ) -> Self { + self.flags + .set(RigidBodyFlags::ROTATION_LOCKED_X, !allow_rotations_x); + self.flags + .set(RigidBodyFlags::ROTATION_LOCKED_Y, !allow_rotations_y); + self.flags + .set(RigidBodyFlags::ROTATION_LOCKED_Z, !allow_rotations_z); + self } /// Sets the mass of the rigid-body being built. @@ -705,42 +722,23 @@ impl RigidBodyBuilder { pub fn mass(mut self, mass: Real, colliders_contribution_enabled: bool) -> Self { self.mass_properties.inv_mass = utils::inv(mass); self.flags.set( - RigidBodyFlags::IGNORE_COLLIDER_MASS, + RigidBodyFlags::TRANSLATION_LOCKED, !colliders_contribution_enabled, ); self } /// Sets the angular inertia of this rigid-body. - /// - /// In order to lock the rotations of this rigid-body (by - /// making them kinematic), call `.principal_inertia(0.0, false)`. - /// - /// If `colliders_contribution_enabled` is `false`, then the principal inertia specified here - /// will be the final principal inertia of the rigid-body created by this builder. - /// If `colliders_contribution_enabled` is `true`, then the final principal of the rigid-body - /// will depend on the initial principal inertia set by this method to which is added - /// the contributions of all the colliders with non-zero density attached to this rigid-body. #[cfg(feature = "dim2")] - pub fn principal_angular_inertia( - mut self, - inertia: Real, - colliders_contribution_enabled: bool, - ) -> Self { + pub fn principal_angular_inertia(mut self, inertia: Real) -> Self { self.mass_properties.inv_principal_inertia_sqrt = utils::inv(inertia); - self.flags.set( - RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_X - | RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Y - | RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z, - !colliders_contribution_enabled, - ); self } /// Use `self.principal_angular_inertia` instead. #[cfg(feature = "dim2")] #[deprecated(note = "renamed to `principal_angular_inertia`.")] - pub fn principal_inertia(self, inertia: Real, colliders_contribution_enabled: bool) -> Self { - self.principal_angular_inertia(inertia, colliders_contribution_enabled) + pub fn principal_inertia(self, inertia: Real) -> Self { + self.principal_angular_inertia(inertia) } /// Sets the principal angular inertia of this rigid-body. @@ -756,36 +754,16 @@ impl RigidBodyBuilder { /// to which is added the contributions of all the colliders with non-zero density /// attached to this rigid-body. #[cfg(feature = "dim3")] - pub fn principal_angular_inertia( - mut self, - inertia: AngVector, - colliders_contribution_enabled: AngVector, - ) -> Self { + pub fn principal_angular_inertia(mut self, inertia: AngVector) -> Self { self.mass_properties.inv_principal_inertia_sqrt = inertia.map(utils::inv); - self.flags.set( - RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_X, - !colliders_contribution_enabled.x, - ); - self.flags.set( - RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Y, - !colliders_contribution_enabled.y, - ); - self.flags.set( - RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z, - !colliders_contribution_enabled.z, - ); self } /// Use `self.principal_angular_inertia` instead. #[cfg(feature = "dim3")] #[deprecated(note = "renamed to `principal_angular_inertia`.")] - pub fn principal_inertia( - self, - inertia: AngVector, - colliders_contribution_enabled: AngVector, - ) -> Self { - self.principal_angular_inertia(inertia, colliders_contribution_enabled) + pub fn principal_inertia(self, inertia: AngVector) -> Self { + self.principal_angular_inertia(inertia) } /// Sets the damping factor for the linear part of the rigid-body motion. -- cgit From 98d3980db7a9803f4ee965237599a87771a417d1 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Thu, 21 Jan 2021 16:03:27 +0100 Subject: Allow several rules for combining friction/restitution coefficients. --- src/dynamics/rigid_body.rs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'src/dynamics/rigid_body.rs') diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 9279e4f..85fdec9 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -62,8 +62,10 @@ pub struct RigidBody { pub(crate) mass_properties: MassProperties, /// The world-space center of mass of the rigid-body. pub world_com: Point, + /// The inverse mass taking into account translation locking. pub effective_inv_mass: Real, - /// The square-root of the inverse angular inertia tensor of the rigid-body. + /// The square-root of the world-space inverse angular inertia tensor of the rigid-body, + /// taking into account rotation locking. pub effective_world_inv_inertia_sqrt: AngularInertia, /// The linear velocity of the rigid-body. pub(crate) linvel: Vector, -- cgit From 8f7220f03d3c23574b9ece09d81d32e862f1b5c6 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Sun, 24 Jan 2021 11:13:44 +0100 Subject: Rename cdl to parry. --- src/dynamics/rigid_body.rs | 24 ++++-------------------- 1 file changed, 4 insertions(+), 20 deletions(-) (limited to 'src/dynamics/rigid_body.rs') diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 85fdec9..5fb6183 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -678,11 +678,9 @@ impl RigidBodyBuilder { } /// Prevents this rigid-body from translating because of forces. - /// - /// This is equivalent to `self.mass(0.0, false)`. See the - /// documentation of [`RigidBodyBuilder::mass`] for more details. - pub fn lock_translations(self) -> Self { - self.mass(0.0, false) + pub fn lock_translations(mut self) -> Self { + self.flags.set(RigidBodyFlags::TRANSLATION_LOCKED, true); + self } /// Prevents this rigid-body from rotating because of forces. @@ -711,22 +709,8 @@ impl RigidBodyBuilder { } /// Sets the mass of the rigid-body being built. - /// - /// In order to lock the translations of this rigid-body (by - /// making them kinematic), call `.mass(0.0, false)`. - /// - /// If `colliders_contribution_enabled` is `false`, then the mass specified here - /// will be the final mass of the rigid-body created by this builder. - /// If `colliders_contribution_enabled` is `true`, then the final mass of the rigid-body - /// will depends on the initial mass set by this method to which is added - /// the contributions of all the colliders with non-zero density attached to - /// this rigid-body. - pub fn mass(mut self, mass: Real, colliders_contribution_enabled: bool) -> Self { + pub fn mass(mut self, mass: Real) -> Self { self.mass_properties.inv_mass = utils::inv(mass); - self.flags.set( - RigidBodyFlags::TRANSLATION_LOCKED, - !colliders_contribution_enabled, - ); self } /// Sets the angular inertia of this rigid-body. -- cgit