From f74b8401ad9ef50b8cdbf1f43a2b21f6c42b0ebc Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 2 Jan 2022 14:47:40 +0100 Subject: Implement multibody joints and the new solver --- src/dynamics/joint/generic_joint.rs | 144 ------------------------------------ 1 file changed, 144 deletions(-) delete mode 100644 src/dynamics/joint/generic_joint.rs (limited to 'src/dynamics/joint/generic_joint.rs') diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs deleted file mode 100644 index 78f1e84..0000000 --- a/src/dynamics/joint/generic_joint.rs +++ /dev/null @@ -1,144 +0,0 @@ -use crate::dynamics::{BallJoint, FixedJoint, PrismaticJoint, RevoluteJoint}; -use crate::math::{Isometry, Real, SpacialVector}; -use crate::na::{Rotation3, UnitQuaternion}; - -#[derive(Copy, Clone, Debug, PartialEq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// A joint that prevents all relative movement between two bodies. -/// -/// Given two frames of references, this joint aims to ensure these frame always coincide in world-space. -pub struct GenericJoint { - /// 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, - /// 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, - /// 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`. - /// This combines both linear and angular impulses: - /// - In 2D, `impulse.xy()` gives the linear impulse, and `impulse.z` the angular impulse. - /// - In 3D, `impulse.xyz()` gives the linear impulse, and `(impulse[3], impulse[4], impulse[5])` the angular impulse. - pub impulse: SpacialVector, - - pub min_position: SpacialVector, - pub max_position: SpacialVector, - pub min_velocity: SpacialVector, - pub max_velocity: SpacialVector, - /// The minimum negative impulse the joint can apply on each DoF. Must be <= 0.0 - pub min_impulse: SpacialVector, - /// The maximum positive impulse the joint can apply on each DoF. Must be >= 0.0 - pub max_impulse: SpacialVector, - /// The minimum negative position impulse the joint can apply on each DoF. Must be <= 0.0 - pub min_pos_impulse: SpacialVector, - /// The maximum positive position impulse the joint can apply on each DoF. Must be >= 0.0 - pub max_pos_impulse: SpacialVector, -} - -impl GenericJoint { - /// Creates a new fixed joint from the frames of reference of both bodies. - pub fn new(local_anchor1: Isometry, local_anchor2: Isometry) -> Self { - Self { - local_anchor1, - local_anchor2, - impulse: SpacialVector::zeros(), - min_position: SpacialVector::zeros(), - max_position: SpacialVector::zeros(), - min_velocity: SpacialVector::zeros(), - max_velocity: SpacialVector::zeros(), - min_impulse: SpacialVector::repeat(-Real::MAX), - max_impulse: SpacialVector::repeat(Real::MAX), - min_pos_impulse: SpacialVector::repeat(-Real::MAX), - max_pos_impulse: SpacialVector::repeat(Real::MAX), - } - } - - pub fn set_dof_vel(&mut self, dof: u8, target_vel: Real, max_force: Real) { - self.min_velocity[dof as usize] = target_vel; - self.max_velocity[dof as usize] = target_vel; - self.min_impulse[dof as usize] = -max_force; - self.max_impulse[dof as usize] = max_force; - } - - pub fn free_dof(&mut self, dof: u8) { - self.min_position[dof as usize] = -Real::MAX; - self.max_position[dof as usize] = Real::MAX; - self.min_velocity[dof as usize] = -Real::MAX; - self.max_velocity[dof as usize] = Real::MAX; - self.min_impulse[dof as usize] = 0.0; - self.max_impulse[dof as usize] = 0.0; - self.min_pos_impulse[dof as usize] = 0.0; - self.max_pos_impulse[dof as usize] = 0.0; - } - - pub fn set_dof_limits(&mut self, dof: u8, min: Real, max: Real) { - self.min_position[dof as usize] = min; - self.max_position[dof as usize] = max; - } -} - -impl From for GenericJoint { - fn from(joint: RevoluteJoint) -> Self { - let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]]; - let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]]; - let quat1 = UnitQuaternion::from_basis_unchecked(&basis1); - let quat2 = UnitQuaternion::from_basis_unchecked(&basis2); - let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1); - let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2); - - let mut result = Self::new(local_anchor1, local_anchor2); - result.free_dof(3); - - if joint.motor_damping != 0.0 { - result.set_dof_vel(3, joint.motor_target_vel, joint.motor_max_impulse); - } - - result.impulse[0] = joint.impulse[0]; - result.impulse[1] = joint.impulse[1]; - result.impulse[2] = joint.impulse[2]; - result.impulse[3] = joint.motor_impulse; - result.impulse[4] = joint.impulse[3]; - result.impulse[5] = joint.impulse[4]; - - result - } -} - -impl From for GenericJoint { - fn from(joint: BallJoint) -> Self { - let local_anchor1 = Isometry::new(joint.local_anchor1.coords, na::zero()); - let local_anchor2 = Isometry::new(joint.local_anchor2.coords, na::zero()); - - let mut result = Self::new(local_anchor1, local_anchor2); - result.impulse[0] = joint.impulse[0]; - result.impulse[1] = joint.impulse[1]; - result.impulse[2] = joint.impulse[2]; - result.free_dof(3); - result.free_dof(4); - result.free_dof(5); - result - } -} - -impl From for GenericJoint { - fn from(joint: PrismaticJoint) -> Self { - let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]]; - let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]]; - let quat1 = UnitQuaternion::from_basis_unchecked(&basis1); - let quat2 = UnitQuaternion::from_basis_unchecked(&basis2); - let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1); - let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2); - - let mut result = Self::new(local_anchor1, local_anchor2); - result.free_dof(0); - result.set_dof_limits(0, joint.limits[0], joint.limits[1]); - result - } -} - -impl From for GenericJoint { - fn from(joint: FixedJoint) -> Self { - Self::new(joint.local_anchor1, joint.local_anchor2) - } -} -- cgit