aboutsummaryrefslogtreecommitdiff
path: root/src/dynamics
diff options
context:
space:
mode:
Diffstat (limited to 'src/dynamics')
-rw-r--r--src/dynamics/joint/joint.rs1
-rw-r--r--src/dynamics/joint/joint_set.rs32
-rw-r--r--src/dynamics/mass_properties.rs23
-rw-r--r--src/dynamics/mass_properties_capsule.rs23
-rw-r--r--src/dynamics/mass_properties_cone.rs29
-rw-r--r--src/dynamics/mass_properties_cylinder.rs40
-rw-r--r--src/dynamics/mass_properties_polygon.rs2
-rw-r--r--src/dynamics/mod.rs3
-rw-r--r--src/dynamics/rigid_body.rs107
-rw-r--r--src/dynamics/rigid_body_set.rs7
-rw-r--r--src/dynamics/solver/velocity_constraint.rs9
-rw-r--r--src/dynamics/solver/velocity_constraint_wide.rs11
-rw-r--r--src/dynamics/solver/velocity_ground_constraint.rs12
-rw-r--r--src/dynamics/solver/velocity_ground_constraint_wide.rs12
14 files changed, 257 insertions, 54 deletions
diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs
index 074f802..9fe6488 100644
--- a/src/dynamics/joint/joint.rs
+++ b/src/dynamics/joint/joint.rs
@@ -95,6 +95,7 @@ impl From<PrismaticJoint> for JointParams {
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone)]
/// A joint attached to two bodies.
pub struct Joint {
/// Handle to the first body attached to this joint.
diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs
index 2b1895f..5144d97 100644
--- a/src/dynamics/joint/joint_set.rs
+++ b/src/dynamics/joint/joint_set.rs
@@ -181,6 +181,38 @@ impl JointSet {
}
}
+ /// Removes a joint from this set.
+ ///
+ /// If `wake_up` is set to `true`, then the bodies attached to this joint will be
+ /// automatically woken up.
+ pub fn remove(
+ &mut self,
+ handle: JointHandle,
+ bodies: &mut RigidBodySet,
+ wake_up: bool,
+ ) -> Option<Joint> {
+ let id = self.joint_ids.remove(handle)?;
+ let endpoints = self.joint_graph.graph.edge_endpoints(id)?;
+
+ if wake_up {
+ // Wake-up the bodies attached to this joint.
+ if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.0) {
+ bodies.wake_up(*rb_handle, true);
+ }
+ if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.1) {
+ bodies.wake_up(*rb_handle, true);
+ }
+ }
+
+ let removed_joint = self.joint_graph.graph.remove_edge(id);
+
+ if let Some(edge) = self.joint_graph.graph.edge_weight(id) {
+ self.joint_ids[edge.handle] = id;
+ }
+
+ removed_joint
+ }
+
pub(crate) fn remove_rigid_body(
&mut self,
deleted_id: RigidBodyGraphIndex,
diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs
index 22e7da5..73e9b0d 100644
--- a/src/dynamics/mass_properties.rs
+++ b/src/dynamics/mass_properties.rs
@@ -25,8 +25,11 @@ pub struct MassProperties {
}
impl MassProperties {
+ /// Initializes the mass properties with the given center-of-mass, mass, and angular inertia.
+ ///
+ /// The center-of-mass is specified in the local-space of the rigid-body.
#[cfg(feature = "dim2")]
- pub(crate) fn new(local_com: Point<f32>, mass: f32, principal_inertia: f32) -> Self {
+ pub fn new(local_com: Point<f32>, mass: f32, principal_inertia: f32) -> Self {
let inv_mass = utils::inv(mass);
let inv_principal_inertia_sqrt = utils::inv(principal_inertia.sqrt());
Self {
@@ -36,13 +39,23 @@ impl MassProperties {
}
}
+ /// Initializes the mass properties from the given center-of-mass, mass, and principal angular inertia.
+ ///
+ /// The center-of-mass is specified in the local-space of the rigid-body.
+ /// The principal angular inertia are the angular inertia along the coordinate axes in the local-space
+ /// of the rigid-body.
#[cfg(feature = "dim3")]
- pub(crate) fn new(local_com: Point<f32>, mass: f32, principal_inertia: AngVector<f32>) -> Self {
+ pub fn new(local_com: Point<f32>, mass: f32, principal_inertia: AngVector<f32>) -> Self {
Self::with_principal_inertia_frame(local_com, mass, principal_inertia, Rotation::identity())
}
+ /// Initializes the mass properties from the given center-of-mass, mass, and principal angular inertia.
+ ///
+ /// The center-of-mass is specified in the local-space of the rigid-body.
+ /// The principal angular inertia are the angular inertia along the coordinate axes defined by
+ /// the `principal_inertia_local_frame` expressed in the local-space of the rigid-body.
#[cfg(feature = "dim3")]
- pub(crate) fn with_principal_inertia_frame(
+ pub fn with_principal_inertia_frame(
local_com: Point<f32>,
mass: f32,
principal_inertia: AngVector<f32>,
@@ -91,7 +104,7 @@ impl MassProperties {
}
#[cfg(feature = "dim3")]
- /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axii.
+ /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axes.
pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix3<f32> {
let inv_principal_inertia = self.inv_principal_inertia_sqrt.map(|e| e * e);
self.principal_inertia_local_frame.to_rotation_matrix()
@@ -103,7 +116,7 @@ impl MassProperties {
}
#[cfg(feature = "dim3")]
- /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axii.
+ /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axes.
pub fn reconstruct_inertia_matrix(&self) -> Matrix3<f32> {
let principal_inertia = self.inv_principal_inertia_sqrt.map(|e| utils::inv(e * e));
self.principal_inertia_local_frame.to_rotation_matrix()
diff --git a/src/dynamics/mass_properties_capsule.rs b/src/dynamics/mass_properties_capsule.rs
index 5f08958..3b1b214 100644
--- a/src/dynamics/mass_properties_capsule.rs
+++ b/src/dynamics/mass_properties_capsule.rs
@@ -1,30 +1,9 @@
use crate::dynamics::MassProperties;
#[cfg(feature = "dim3")]
use crate::geometry::Capsule;
-use crate::math::{Point, PrincipalAngularInertia, Vector};
+use crate::math::Point;
impl MassProperties {
- fn cylinder_y_volume_unit_inertia(
- half_height: f32,
- radius: f32,
- ) -> (f32, PrincipalAngularInertia<f32>) {
- #[cfg(feature = "dim2")]
- {
- Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height))
- }
-
- #[cfg(feature = "dim3")]
- {
- let volume = half_height * radius * radius * std::f32::consts::PI * 2.0;
- let sq_radius = radius * radius;
- let sq_height = half_height * half_height * 4.0;
- let off_principal = (sq_radius * 3.0 + sq_height) / 12.0;
-
- let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal);
- (volume, inertia)
- }
- }
-
pub(crate) fn from_capsule(density: f32, a: Point<f32>, b: Point<f32>, radius: f32) -> Self {
let half_height = (b - a).norm() / 2.0;
let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius);
diff --git a/src/dynamics/mass_properties_cone.rs b/src/dynamics/mass_properties_cone.rs
new file mode 100644
index 0000000..12f831f
--- /dev/null
+++ b/src/dynamics/mass_properties_cone.rs
@@ -0,0 +1,29 @@
+use crate::dynamics::MassProperties;
+use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector};
+
+impl MassProperties {
+ pub(crate) fn cone_y_volume_unit_inertia(
+ half_height: f32,
+ radius: f32,
+ ) -> (f32, PrincipalAngularInertia<f32>) {
+ let volume = radius * radius * std::f32::consts::PI * half_height * 2.0 / 3.0;
+ let sq_radius = radius * radius;
+ let sq_height = half_height * half_height * 4.0;
+ let off_principal = sq_radius * 3.0 / 20.0 + sq_height * 3.0 / 5.0;
+ let principal = sq_radius * 3.0 / 10.0;
+
+ (volume, Vector::new(off_principal, principal, off_principal))
+ }
+
+ pub(crate) fn from_cone(density: f32, half_height: f32, radius: f32) -> Self {
+ let (cyl_vol, cyl_unit_i) = Self::cone_y_volume_unit_inertia(half_height, radius);
+ let cyl_mass = cyl_vol * density;
+
+ Self::with_principal_inertia_frame(
+ Point::new(0.0, -half_height / 2.0, 0.0),
+ cyl_mass,
+ cyl_unit_i * cyl_mass,
+ Rotation::identity(),
+ )
+ }
+}
diff --git a/src/dynamics/mass_properties_cylinder.rs b/src/dynamics/mass_properties_cylinder.rs
new file mode 100644
index 0000000..7c8054a
--- /dev/null
+++ b/src/dynamics/mass_properties_cylinder.rs
@@ -0,0 +1,40 @@
+use crate::dynamics::MassProperties;
+#[cfg(feature = "dim3")]
+use crate::math::{Point, Rotation};
+use crate::math::{PrincipalAngularInertia, Vector};
+
+impl MassProperties {
+ pub(crate) fn cylinder_y_volume_unit_inertia(
+ half_height: f32,
+ radius: f32,
+ ) -> (f32, PrincipalAngularInertia<f32>) {
+ #[cfg(feature = "dim2")]
+ {
+ Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height))
+ }
+
+ #[cfg(feature = "dim3")]
+ {
+ let volume = half_height * radius * radius * std::f32::consts::PI * 2.0;
+ let sq_radius = radius * radius;
+ let sq_height = half_height * half_height * 4.0;
+ let off_principal = (sq_radius * 3.0 + sq_height) / 12.0;
+
+ let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal);
+ (volume, inertia)
+ }
+ }
+
+ #[cfg(feature = "dim3")]
+ pub(crate) fn from_cylinder(density: f32, half_height: f32, radius: f32) -> Self {
+ let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius);
+ let cyl_mass = cyl_vol * density;
+
+ Self::with_principal_inertia_frame(
+ Point::origin(),
+ cyl_mass,
+ cyl_unit_i * cyl_mass,
+ Rotation::identity(),
+ )
+ }
+}
diff --git a/src/dynamics/mass_properties_polygon.rs b/src/dynamics/mass_properties_polygon.rs
index c87e888..8b0b811 100644
--- a/src/dynamics/mass_properties_polygon.rs
+++ b/src/dynamics/mass_properties_polygon.rs
@@ -1,3 +1,5 @@
+#![allow(dead_code)] // TODO: remove this
+
use crate::dynamics::MassProperties;
use crate::math::Point;
diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs
index 4499d95..10cdd29 100644
--- a/src/dynamics/mod.rs
+++ b/src/dynamics/mod.rs
@@ -22,7 +22,10 @@ mod joint;
mod mass_properties;
mod mass_properties_ball;
mod mass_properties_capsule;
+#[cfg(feature = "dim3")]
+mod mass_properties_cone;
mod mass_properties_cuboid;
+mod mass_properties_cylinder;
#[cfg(feature = "dim2")]
mod mass_properties_polygon;
mod rigid_body;
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index d53e248..a1a23a0 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -27,7 +27,7 @@ pub enum BodyStatus {
/// A rigid body.
///
/// To create a new rigid-body, use the `RigidBodyBuilder` structure.
-#[derive(Debug)]
+#[derive(Debug, Clone)]
pub struct RigidBody {
/// The world-space position of the rigid-body.
pub position: Isometry<f32>,
@@ -42,6 +42,10 @@ pub struct RigidBody {
pub linvel: Vector<f32>,
/// The angular velocity of the rigid-body.
pub angvel: AngVector<f32>,
+ /// Damping factor for gradually slowing down the translational motion of the rigid-body.
+ pub linear_damping: f32,
+ /// Damping factor for gradually slowing down the angular motion of the rigid-body.
+ pub angular_damping: f32,
pub(crate) linacc: Vector<f32>,
pub(crate) angacc: AngVector<f32>,
pub(crate) colliders: Vec<ColliderHandle>,
@@ -54,20 +58,8 @@ pub struct RigidBody {
pub(crate) active_set_timestamp: u32,
/// The status of the body, governing how it is affected by external forces.
pub body_status: BodyStatus,
-}
-
-impl Clone for RigidBody {
- fn clone(&self) -> Self {
- Self {
- colliders: Vec::new(),
- joint_graph_index: RigidBodyGraphIndex::new(crate::INVALID_U32),
- active_island_id: crate::INVALID_USIZE,
- active_set_id: crate::INVALID_USIZE,
- active_set_offset: crate::INVALID_USIZE,
- active_set_timestamp: crate::INVALID_U32,
- ..*self
- }
- }
+ /// User-defined data associated to this rigid-body.
+ pub user_data: u128,
}
impl RigidBody {
@@ -82,6 +74,8 @@ impl RigidBody {
angvel: na::zero(),
linacc: Vector::zeros(),
angacc: na::zero(),
+ linear_damping: 0.0,
+ angular_damping: 0.0,
colliders: Vec::new(),
activation: ActivationStatus::new_active(),
joint_graph_index: InteractionGraph::<()>::invalid_graph_index(),
@@ -90,9 +84,19 @@ impl RigidBody {
active_set_offset: 0,
active_set_timestamp: 0,
body_status: BodyStatus::Dynamic,
+ user_data: 0,
}
}
+ pub(crate) fn reset_internal_references(&mut self) {
+ self.colliders = Vec::new();
+ self.joint_graph_index = InteractionGraph::<()>::invalid_graph_index();
+ self.active_island_id = 0;
+ self.active_set_id = 0;
+ self.active_set_offset = 0;
+ self.active_set_timestamp = 0;
+ }
+
pub(crate) fn integrate_accelerations(&mut self, dt: f32, gravity: Vector<f32>) {
if self.mass_properties.inv_mass != 0.0 {
self.linvel += (gravity + self.linacc) * dt;
@@ -218,7 +222,12 @@ impl RigidBody {
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) {
+ // 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);
+
self.position = self.integrate_velocity(dt) * self.position;
}
@@ -363,13 +372,17 @@ impl RigidBody {
}
}
-/// A builder for rigid-bodies.
+/// A builder for rigid-bodies.
pub struct RigidBodyBuilder {
position: Isometry<f32>,
linvel: Vector<f32>,
angvel: AngVector<f32>,
+ linear_damping: f32,
+ angular_damping: f32,
body_status: BodyStatus,
+ mass_properties: MassProperties,
can_sleep: bool,
+ user_data: u128,
}
impl RigidBodyBuilder {
@@ -379,8 +392,12 @@ impl RigidBodyBuilder {
position: Isometry::identity(),
linvel: Vector::zeros(),
angvel: na::zero(),
+ linear_damping: 0.0,
+ angular_damping: 0.0,
body_status,
+ mass_properties: MassProperties::zero(),
can_sleep: true,
+ user_data: 0,
}
}
@@ -428,6 +445,60 @@ impl RigidBodyBuilder {
self
}
+ /// An arbitrary user-defined 128-bit integer associated to the rigid-bodies built by this builder.
+ pub fn user_data(mut self, data: u128) -> Self {
+ self.user_data = data;
+ self
+ }
+
+ /// Sets the mass properties of the rigid-body being built.
+ ///
+ /// Note that the final mass properties of the rigid-bodies depends
+ /// on the initial mass-properties of the rigid-body (set by this method)
+ /// to which is added the contributions of all the colliders with non-zero density
+ /// attached to this rigid-body.
+ ///
+ /// Therefore, if you want your provided mass properties to be the final
+ /// mass properties of your rigid-body, don't attach colliders to it, or
+ /// only attach colliders with densities equal to zero.
+ pub fn mass_properties(mut self, props: MassProperties) -> Self {
+ self.mass_properties = props;
+ self
+ }
+
+ /// Sets the mass of the rigid-body being built.
+ ///
+ /// Note that the final mass of the rigid-bodies depends
+ /// on the initial mass of the rigid-body (set by this method)
+ /// to which is added the contributions of all the colliders with non-zero density
+ /// attached to this rigid-body.
+ ///
+ /// Therefore, if you want your provided mass to be the final
+ /// mass of your rigid-body, don't attach colliders to it, or
+ /// only attach colliders with densities equal to zero.
+ pub fn mass(mut self, mass: f32) -> Self {
+ self.mass_properties.inv_mass = crate::utils::inv(mass);
+ self
+ }
+
+ /// Sets the damping factor for the linear part of the rigid-body motion.
+ ///
+ /// 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 {
+ self.linear_damping = factor;
+ self
+ }
+
+ /// Sets the damping factor for the angular part of the rigid-body motion.
+ ///
+ /// 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 {
+ 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 {
@@ -462,6 +533,10 @@ impl RigidBodyBuilder {
rb.linvel = self.linvel;
rb.angvel = self.angvel;
rb.body_status = self.body_status;
+ rb.user_data = self.user_data;
+ rb.mass_properties = self.mass_properties;
+ rb.linear_damping = self.linear_damping;
+ rb.angular_damping = self.angular_damping;
if !self.can_sleep {
rb.activation.threshold = -1.0;
diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs
index 83f1c51..b857173 100644
--- a/src/dynamics/rigid_body_set.rs
+++ b/src/dynamics/rigid_body_set.rs
@@ -75,6 +75,7 @@ impl BodyPair {
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone)]
/// A set of rigid bodies that can be handled by a physics pipeline.
pub struct RigidBodySet {
// NOTE: the pub(crate) are needed by the broad phase
@@ -154,7 +155,11 @@ impl RigidBodySet {
}
/// Insert a rigid body into this set and retrieve its handle.
- pub fn insert(&mut self, rb: RigidBody) -> RigidBodyHandle {
+ pub fn insert(&mut self, mut rb: RigidBody) -> RigidBodyHandle {
+ // Make sure the internal links are reset, they may not be
+ // if this rigid-body was obtained by cloning another one.
+ rb.reset_internal_references();
+
let handle = self.bodies.insert(rb);
let rb = &mut self.bodies[handle];
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 190076d..948081d 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -238,8 +238,13 @@ impl VelocityConstraint {
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
- let rhs = (vel1 - vel2).dot(&force_dir1)
- + manifold_point.dist.max(0.0) * params.inv_dt();
+ let mut rhs = (vel1 - vel2).dot(&force_dir1);
+
+ if rhs <= -params.restitution_velocity_threshold {
+ rhs += manifold.restitution * rhs
+ }
+
+ rhs += manifold_point.dist.max(0.0) * params.inv_dt();
let impulse = manifold_points[k].impulse * warmstart_coeff;
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index f515d5e..5d8078a 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_constraint_wide.rs
@@ -104,6 +104,10 @@ impl WVelocityConstraint {
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]);
+ let restitution = SimdFloat::from(array![|ii| manifolds[ii].restitution; SIMD_WIDTH]);
+ let restitution_velocity_threshold =
+ SimdFloat::splat(params.restitution_velocity_threshold);
+
let warmstart_multiplier =
SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]);
let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff);
@@ -151,8 +155,11 @@ impl WVelocityConstraint {
let r = SimdFloat::splat(1.0)
/ (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
- let rhs =
- (vel1 - vel2).dot(&force_dir1) + dist.simd_max(SimdFloat::zero()) * inv_dt;
+ let mut rhs = (vel1 - vel2).dot(&force_dir1);
+ let use_restitution = rhs.simd_le(-restitution_velocity_threshold);
+ let rhs_with_restitution = rhs + rhs * restitution;
+ rhs = rhs_with_restitution.select(use_restitution, rhs);
+ rhs += dist.simd_max(SimdFloat::zero()) * inv_dt;
constraint.elements[k].normal_part = WVelocityConstraintElementPart {
gcross1,
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index d9229ff..d8ef8be 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -169,9 +169,15 @@ impl VelocityGroundConstraint {
.transform_vector(dp2.gcross(-force_dir1));
let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2));
- let rhs = -vel2.dot(&force_dir1)
- + vel1.dot(&force_dir1)
- + manifold_point.dist.max(0.0) * params.inv_dt();
+
+ let mut rhs = (vel1 - vel2).dot(&force_dir1);
+
+ if rhs <= -params.restitution_velocity_threshold {
+ rhs += manifold.restitution * rhs
+ }
+
+ rhs += manifold_point.dist.max(0.0) * params.inv_dt();
+
let impulse = manifold_points[k].impulse * warmstart_coeff;
constraint.elements[k].normal_part = VelocityGroundConstraintElementPart {
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index 18e9257..be01944 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs
@@ -110,6 +110,10 @@ impl WVelocityGroundConstraint {
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]);
+ let restitution = SimdFloat::from(array![|ii| manifolds[ii].restitution; SIMD_WIDTH]);
+ let restitution_velocity_threshold =
+ SimdFloat::splat(params.restitution_velocity_threshold);
+
let warmstart_multiplier =
SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]);
let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff);
@@ -154,9 +158,11 @@ impl WVelocityGroundConstraint {
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
let r = SimdFloat::splat(1.0) / (im2 + gcross2.gdot(gcross2));
- let rhs = -vel2.dot(&force_dir1)
- + vel1.dot(&force_dir1)
- + dist.simd_max(SimdFloat::zero()) * inv_dt;
+ let mut rhs = (vel1 - vel2).dot(&force_dir1);
+ let use_restitution = rhs.simd_le(-restitution_velocity_threshold);
+ let rhs_with_restitution = rhs + rhs * restitution;
+ rhs = rhs_with_restitution.select(use_restitution, rhs);
+ rhs += dist.simd_max(SimdFloat::zero()) * inv_dt;
constraint.elements[k].normal_part = WVelocityGroundConstraintElementPart {
gcross2,
generated by cgit (git 2.34.1) at 2026-06-30 19:36:25 +0000