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 --- .../joint_constraint/ball_position_constraint.rs | 266 ------- .../ball_position_constraint_wide.rs | 216 ------ .../joint_constraint/ball_velocity_constraint.rs | 660 ---------------- .../ball_velocity_constraint_wide.rs | 359 --------- .../joint_constraint/fixed_position_constraint.rs | 151 ---- .../fixed_position_constraint_wide.rs | 71 -- .../joint_constraint/fixed_velocity_constraint.rs | 436 ----------- .../fixed_velocity_constraint_wide.rs | 539 ------------- .../generic_position_constraint.rs | 346 --------- .../generic_position_constraint_wide.rs | 60 -- .../generic_velocity_constraint.rs | 706 ----------------- .../generic_velocity_constraint_wide.rs | 464 ----------- .../solver/joint_constraint/joint_constraint.rs | 710 +++++++++-------- .../joint_generic_velocity_constraint.rs | 529 +++++++++++++ .../joint_generic_velocity_constraint_builder.rs | 853 ++++++++++++++++++++ .../joint_constraint/joint_position_constraint.rs | 280 ------- .../joint_constraint/joint_velocity_constraint.rs | 608 +++++++++++++++ .../joint_velocity_constraint_builder.rs | 699 +++++++++++++++++ src/dynamics/solver/joint_constraint/mod.rs | 107 +-- .../prismatic_position_constraint.rs | 182 ----- .../prismatic_position_constraint_wide.rs | 71 -- .../prismatic_velocity_constraint.rs | 859 --------------------- .../prismatic_velocity_constraint_wide.rs | 848 -------------------- .../revolute_position_constraint.rs | 295 ------- .../revolute_position_constraint_wide.rs | 71 -- .../revolute_velocity_constraint.rs | 750 ------------------ .../revolute_velocity_constraint_wide.rs | 527 ------------- 27 files changed, 3087 insertions(+), 8576 deletions(-) delete mode 100644 src/dynamics/solver/joint_constraint/ball_position_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/fixed_position_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/generic_position_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs delete mode 100644 src/dynamics/solver/joint_constraint/joint_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs delete mode 100644 src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/revolute_position_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs delete mode 100644 src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs (limited to 'src/dynamics/solver/joint_constraint') diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs deleted file mode 100644 index 0dfdb86..0000000 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ /dev/null @@ -1,266 +0,0 @@ -use crate::dynamics::{ - BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, -}; -#[cfg(feature = "dim2")] -use crate::math::SdpMatrix; -use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, UnitVector}; -use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; - -#[derive(Debug)] -pub(crate) struct BallPositionConstraint { - position1: usize, - position2: usize, - - local_com1: Point, - local_com2: Point, - - im1: Real, - im2: Real, - - ii1: AngularInertia, - ii2: AngularInertia, - inv_ii1_ii2: AngularInertia, - - local_anchor1: Point, - local_anchor2: Point, - - limits_enabled: bool, - limits_angle: Real, - limits_local_axis1: UnitVector, - limits_local_axis2: UnitVector, -} - -impl BallPositionConstraint { - pub fn from_params( - rb1: (&RigidBodyMassProps, &RigidBodyIds), - rb2: (&RigidBodyMassProps, &RigidBodyIds), - cparams: &BallJoint, - ) -> Self { - let (mprops1, ids1) = rb1; - let (mprops2, ids2) = rb2; - - let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); - let inv_ii1_ii2 = (ii1 + ii2).inverse(); - - Self { - local_com1: mprops1.local_mprops.local_com, - local_com2: mprops2.local_mprops.local_com, - im1: mprops1.effective_inv_mass, - im2: mprops2.effective_inv_mass, - ii1, - ii2, - inv_ii1_ii2, - local_anchor1: cparams.local_anchor1, - local_anchor2: cparams.local_anchor2, - position1: ids1.active_set_offset, - position2: ids2.active_set_offset, - limits_enabled: cparams.limits_enabled, - limits_angle: cparams.limits_angle, - limits_local_axis1: cparams.limits_local_axis1, - limits_local_axis2: cparams.limits_local_axis2, - } - } - - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { - let mut position1 = positions[self.position1 as usize]; - let mut position2 = positions[self.position2 as usize]; - - let anchor1 = position1 * self.local_anchor1; - let anchor2 = position2 * self.local_anchor2; - - let com1 = position1 * self.local_com1; - let com2 = position2 * self.local_com2; - - let err = anchor1 - anchor2; - - let centered_anchor1 = anchor1 - com1; - let centered_anchor2 = anchor2 - com2; - - let cmat1 = centered_anchor1.gcross_matrix(); - let cmat2 = centered_anchor2.gcross_matrix(); - - // NOTE: the -cmat1 is just a simpler way of doing cmat1.transpose() - // because it is anti-symmetric. - #[cfg(feature = "dim3")] - let lhs = self.ii1.quadform(&cmat1).add_diagonal(self.im1) - + self.ii2.quadform(&cmat2).add_diagonal(self.im2); - - // In 2D we just unroll the computation because - // it's just easier that way. It is also - // faster because in 2D lhs will be symmetric. - #[cfg(feature = "dim2")] - let lhs = { - let m11 = - self.im1 + self.im2 + cmat1.x * cmat1.x * self.ii1 + cmat2.x * cmat2.x * self.ii2; - let m12 = cmat1.x * cmat1.y * self.ii1 + cmat2.x * cmat2.y * self.ii2; - let m22 = - self.im1 + self.im2 + cmat1.y * cmat1.y * self.ii1 + cmat2.y * cmat2.y * self.ii2; - SdpMatrix::new(m11, m12, m22) - }; - - let inv_lhs = lhs.inverse_unchecked(); - let impulse = inv_lhs * -(err * params.joint_erp); - - position1.translation.vector += self.im1 * impulse; - position2.translation.vector -= self.im2 * impulse; - - let angle1 = self.ii1.transform_vector(centered_anchor1.gcross(impulse)); - let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse)); - - position1.rotation = Rotation::new(angle1) * position1.rotation; - position2.rotation = Rotation::new(angle2) * position2.rotation; - - /* - * Limits part. - */ - if self.limits_enabled { - let axis1 = position1 * self.limits_local_axis1; - let axis2 = position2 * self.limits_local_axis2; - - #[cfg(feature = "dim2")] - let axis_angle = Rotation::rotation_between_axis(&axis2, &axis1).axis_angle(); - #[cfg(feature = "dim3")] - let axis_angle = - Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle()); - - // TODO: handle the case where dot(axis1, axis2) = -1.0 - if let Some((axis, angle)) = axis_angle { - if angle >= self.limits_angle { - #[cfg(feature = "dim2")] - let axis = axis[0]; - #[cfg(feature = "dim3")] - let axis = axis.into_inner(); - let ang_error = angle - self.limits_angle; - let ang_impulse = self - .inv_ii1_ii2 - .transform_vector(axis * ang_error * params.joint_erp); - - position1.rotation = - Rotation::new(self.ii1.transform_vector(-ang_impulse)) * position1.rotation; - position2.rotation = - Rotation::new(self.ii2.transform_vector(ang_impulse)) * position2.rotation; - } - } - } - - positions[self.position1 as usize] = position1; - positions[self.position2 as usize] = position2; - } -} - -#[derive(Debug)] -pub(crate) struct BallPositionGroundConstraint { - position2: usize, - anchor1: Point, - im2: Real, - ii2: AngularInertia, - local_anchor2: Point, - local_com2: Point, - - limits_enabled: bool, - limits_angle: Real, - limits_axis1: UnitVector, - limits_local_axis2: UnitVector, -} - -impl BallPositionGroundConstraint { - pub fn from_params( - rb1: &RigidBodyPosition, - rb2: (&RigidBodyMassProps, &RigidBodyIds), - cparams: &BallJoint, - flipped: bool, - ) -> Self { - let poss1 = rb1; - let (mprops2, ids2) = rb2; - - if flipped { - // Note the only thing that is flipped here - // are the local_anchors. The rb1 and rb2 have - // already been flipped by the caller. - Self { - anchor1: poss1.next_position * cparams.local_anchor2, - im2: mprops2.effective_inv_mass, - ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), - local_anchor2: cparams.local_anchor1, - position2: ids2.active_set_offset, - local_com2: mprops2.local_mprops.local_com, - limits_enabled: cparams.limits_enabled, - limits_angle: cparams.limits_angle, - limits_axis1: poss1.next_position * cparams.limits_local_axis2, - limits_local_axis2: cparams.limits_local_axis1, - } - } else { - Self { - anchor1: poss1.next_position * cparams.local_anchor1, - im2: mprops2.effective_inv_mass, - ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), - local_anchor2: cparams.local_anchor2, - position2: ids2.active_set_offset, - local_com2: mprops2.local_mprops.local_com, - limits_enabled: cparams.limits_enabled, - limits_angle: cparams.limits_angle, - limits_axis1: poss1.next_position * cparams.limits_local_axis1, - limits_local_axis2: cparams.limits_local_axis2, - } - } - } - - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { - let mut position2 = positions[self.position2 as usize]; - - let anchor2 = position2 * self.local_anchor2; - let com2 = position2 * self.local_com2; - - let err = self.anchor1 - anchor2; - let centered_anchor2 = anchor2 - com2; - let cmat2 = centered_anchor2.gcross_matrix(); - - #[cfg(feature = "dim3")] - let lhs = self.ii2.quadform(&cmat2).add_diagonal(self.im2); - - #[cfg(feature = "dim2")] - let lhs = { - let m11 = self.im2 + cmat2.x * cmat2.x * self.ii2; - let m12 = cmat2.x * cmat2.y * self.ii2; - let m22 = self.im2 + cmat2.y * cmat2.y * self.ii2; - SdpMatrix::new(m11, m12, m22) - }; - - let inv_lhs = lhs.inverse_unchecked(); - let impulse = inv_lhs * -(err * params.joint_erp); - position2.translation.vector -= self.im2 * impulse; - - let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse)); - position2.rotation = Rotation::new(angle2) * position2.rotation; - - /* - * Limits part. - */ - if self.limits_enabled { - let axis2 = position2 * self.limits_local_axis2; - - #[cfg(feature = "dim2")] - let axis_angle = - Rotation::rotation_between_axis(&axis2, &self.limits_axis1).axis_angle(); - #[cfg(feature = "dim3")] - let axis_angle = Rotation::rotation_between_axis(&axis2, &self.limits_axis1) - .and_then(|r| r.axis_angle()); - - // TODO: handle the case where dot(axis1, axis2) = -1.0 - if let Some((axis, angle)) = axis_angle { - if angle >= self.limits_angle { - #[cfg(feature = "dim2")] - let axis = axis[0]; - #[cfg(feature = "dim3")] - let axis = axis.into_inner(); - let ang_error = angle - self.limits_angle; - let ang_correction = axis * ang_error * params.joint_erp; - position2.rotation = Rotation::new(ang_correction) * position2.rotation; - } - } - } - - positions[self.position2 as usize] = position2; - } -} diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs deleted file mode 100644 index ea462ed..0000000 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ /dev/null @@ -1,216 +0,0 @@ -use crate::dynamics::{ - BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, -}; -#[cfg(feature = "dim2")] -use crate::math::SdpMatrix; -use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, SimdReal, SIMD_WIDTH}; -use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; -use simba::simd::SimdValue; - -#[derive(Debug)] -pub(crate) struct WBallPositionConstraint { - position1: [usize; SIMD_WIDTH], - position2: [usize; SIMD_WIDTH], - - local_com1: Point, - local_com2: Point, - - im1: SimdReal, - im2: SimdReal, - - ii1: AngularInertia, - ii2: AngularInertia, - - local_anchor1: Point, - local_anchor2: Point, -} - -impl WBallPositionConstraint { - pub fn from_params( - rbs1: ( - [&RigidBodyMassProps; SIMD_WIDTH], - [&RigidBodyIds; SIMD_WIDTH], - ), - rbs2: ( - [&RigidBodyMassProps; SIMD_WIDTH], - [&RigidBodyIds; SIMD_WIDTH], - ), - cparams: [&BallJoint; SIMD_WIDTH], - ) -> Self { - let (mprops1, ids1) = rbs1; - let (mprops2, ids2) = rbs2; - - let local_com1 = Point::from(gather![|ii| mprops1[ii].local_mprops.local_com]); - let local_com2 = Point::from(gather![|ii| mprops2[ii].local_mprops.local_com]); - let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]); - let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); - let ii1 = AngularInertia::::from(gather![ - |ii| mprops1[ii].effective_world_inv_inertia_sqrt - ]) - .squared(); - let ii2 = AngularInertia::::from(gather![ - |ii| mprops2[ii].effective_world_inv_inertia_sqrt - ]) - .squared(); - let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]); - let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]); - let position1 = gather![|ii| ids1[ii].active_set_offset]; - let position2 = gather![|ii| ids2[ii].active_set_offset]; - - Self { - local_com1, - local_com2, - im1, - im2, - ii1, - ii2, - local_anchor1, - local_anchor2, - position1, - position2, - } - } - - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { - let mut position1 = Isometry::from(gather![|ii| positions[self.position1[ii]]]); - let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]); - - let anchor1 = position1 * self.local_anchor1; - let anchor2 = position2 * self.local_anchor2; - - let com1 = position1 * self.local_com1; - let com2 = position2 * self.local_com2; - - let err = anchor1 - anchor2; - - let centered_anchor1 = anchor1 - com1; - let centered_anchor2 = anchor2 - com2; - - let cmat1 = centered_anchor1.gcross_matrix(); - let cmat2 = centered_anchor2.gcross_matrix(); - - // NOTE: the -cmat1 is just a simpler way of doing cmat1.transpose() - // because it is anti-symmetric. - #[cfg(feature = "dim3")] - let lhs = self.ii1.quadform(&cmat1).add_diagonal(self.im1) - + self.ii2.quadform(&cmat2).add_diagonal(self.im2); - - // In 2D we just unroll the computation because - // it's just easier that way. - #[cfg(feature = "dim2")] - let lhs = { - let m11 = - self.im1 + self.im2 + cmat1.x * cmat1.x * self.ii1 + cmat2.x * cmat2.x * self.ii2; - let m12 = cmat1.x * cmat1.y * self.ii1 + cmat2.x * cmat2.y * self.ii2; - let m22 = - self.im1 + self.im2 + cmat1.y * cmat1.y * self.ii1 + cmat2.y * cmat2.y * self.ii2; - SdpMatrix::new(m11, m12, m22) - }; - - let inv_lhs = lhs.inverse_unchecked(); - let impulse = inv_lhs * -(err * SimdReal::splat(params.joint_erp)); - - position1.translation.vector += impulse * self.im1; - position2.translation.vector -= impulse * self.im2; - - let angle1 = self.ii1.transform_vector(centered_anchor1.gcross(impulse)); - let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse)); - - position1.rotation = Rotation::new(angle1) * position1.rotation; - position2.rotation = Rotation::new(angle2) * position2.rotation; - - for ii in 0..SIMD_WIDTH { - positions[self.position1[ii]] = position1.extract(ii); - } - for ii in 0..SIMD_WIDTH { - positions[self.position2[ii]] = position2.extract(ii); - } - } -} - -#[derive(Debug)] -pub(crate) struct WBallPositionGroundConstraint { - position2: [usize; SIMD_WIDTH], - anchor1: Point, - im2: SimdReal, - ii2: AngularInertia, - local_anchor2: Point, - local_com2: Point, -} - -impl WBallPositionGroundConstraint { - pub fn from_params( - rbs1: [&RigidBodyPosition; SIMD_WIDTH], - rbs2: ( - [&RigidBodyMassProps; SIMD_WIDTH], - [&RigidBodyIds; SIMD_WIDTH], - ), - cparams: [&BallJoint; SIMD_WIDTH], - flipped: [bool; SIMD_WIDTH], - ) -> Self { - let poss1 = rbs1; - let (mprops2, ids2) = rbs2; - - let position1 = Isometry::from(gather![|ii| poss1[ii].next_position]); - let anchor1 = position1 - * Point::from(gather![|ii| if flipped[ii] { - cparams[ii].local_anchor2 - } else { - cparams[ii].local_anchor1 - }]); - let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); - let ii2 = AngularInertia::::from(gather![ - |ii| mprops2[ii].effective_world_inv_inertia_sqrt - ]) - .squared(); - let local_anchor2 = Point::from(gather![|ii| if flipped[ii] { - cparams[ii].local_anchor1 - } else { - cparams[ii].local_anchor2 - }]); - let position2 = gather![|ii| ids2[ii].active_set_offset]; - let local_com2 = Point::from(gather![|ii| mprops2[ii].local_mprops.local_com]); - - Self { - anchor1, - im2, - ii2, - local_anchor2, - position2, - local_com2, - } - } - - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { - let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]); - - let anchor2 = position2 * self.local_anchor2; - let com2 = position2 * self.local_com2; - - let err = self.anchor1 - anchor2; - let centered_anchor2 = anchor2 - com2; - let cmat2 = centered_anchor2.gcross_matrix(); - - #[cfg(feature = "dim3")] - let lhs = self.ii2.quadform(&cmat2).add_diagonal(self.im2); - - #[cfg(feature = "dim2")] - let lhs = { - let m11 = self.im2 + cmat2.x * cmat2.x * self.ii2; - let m12 = cmat2.x * cmat2.y * self.ii2; - let m22 = self.im2 + cmat2.y * cmat2.y * self.ii2; - SdpMatrix::new(m11, m12, m22) - }; - - let inv_lhs = lhs.inverse_unchecked(); - let impulse = inv_lhs * -(err * SimdReal::splat(params.joint_erp)); - position2.translation.vector -= impulse * self.im2; - - let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse)); - position2.rotation = Rotation::new(angle2) * position2.rotation; - - for ii in 0..SIMD_WIDTH { - positions[self.position2[ii]] = position2.extract(ii); - } - } -} diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs deleted file mode 100644 index 5abd726..0000000 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ /dev/null @@ -1,660 +0,0 @@ -use crate::dynamics::solver::DeltaVel; -use crate::dynamics::{ - BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds, - RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, -}; -use crate::math::{AngVector, AngularInertia, Real, Rotation, SdpMatrix, Vector}; -use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot}; - -#[derive(Debug)] -pub(crate) struct BallVelocityConstraint { - mj_lambda1: usize, - mj_lambda2: usize, - - joint_id: JointIndex, - - rhs: Vector, - impulse: Vector, - - r1: Vector, - r2: Vector, - - inv_lhs: SdpMatrix, - - motor_rhs: AngVector, - motor_impulse: AngVector, - motor_inv_lhs: Option>, - motor_max_impulse: Real, - - limits_active: bool, - limits_rhs: Real, - limits_inv_lhs: Real, - limits_impulse: Real, - limits_axis: AngVector, - - im1: Real, - im2: Real, - - ii1_sqrt: AngularInertia, - ii2_sqrt: AngularInertia, -} - -impl BallVelocityConstraint { - pub fn from_params( - params: &IntegrationParameters, - joint_id: JointIndex, - rb1: ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyIds, - ), - rb2: ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyIds, - ), - joint: &BallJoint, - ) -> Self { - let (rb_pos1, rb_vels1, rb_mprops1, rb_ids1) = rb1; - let (rb_pos2, rb_vels2, rb_mprops2, rb_ids2) = rb2; - - let anchor_world1 = rb_pos1.position * joint.local_anchor1; - let anchor_world2 = rb_pos2.position * joint.local_anchor2; - let anchor1 = anchor_world1 - rb_mprops1.world_com; - let anchor2 = anchor_world2 - rb_mprops2.world_com; - - let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(anchor1); - let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(anchor2); - let im1 = rb_mprops1.effective_inv_mass; - let im2 = rb_mprops2.effective_inv_mass; - - let rhs = (vel2 - vel1) * params.velocity_solve_fraction - + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt(); - - let lhs; - let cmat1 = anchor1.gcross_matrix(); - let cmat2 = anchor2.gcross_matrix(); - - #[cfg(feature = "dim3")] - { - lhs = rb_mprops2 - .effective_world_inv_inertia_sqrt - .squared() - .quadform(&cmat2) - .add_diagonal(im2) - + rb_mprops1 - .effective_world_inv_inertia_sqrt - .squared() - .quadform(&cmat1) - .add_diagonal(im1); - } - - // In 2D we just unroll the computation because - // it's just easier that way. - #[cfg(feature = "dim2")] - { - let ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared(); - let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2; - let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2; - let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2; - lhs = SdpMatrix::new(m11, m12, m22) - } - - let inv_lhs = lhs.inverse_unchecked(); - - /* - * Motor part. - */ - let mut motor_rhs = na::zero(); - let mut motor_inv_lhs = None; - let motor_max_impulse = joint.motor_max_impulse; - - if motor_max_impulse > 0.0 { - let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients( - params.dt, - joint.motor_stiffness, - joint.motor_damping, - ); - - if stiffness != 0.0 { - let dpos = rb_pos2.position.rotation - * (rb_pos1.position.rotation * joint.motor_target_pos).inverse(); - #[cfg(feature = "dim2")] - { - motor_rhs += dpos.angle() * stiffness; - } - #[cfg(feature = "dim3")] - { - motor_rhs += dpos.scaled_axis() * stiffness; - } - } - - if damping != 0.0 { - let curr_vel = rb_vels2.angvel - rb_vels1.angvel; - motor_rhs += (curr_vel - joint.motor_target_vel) * damping; - } - - #[cfg(feature = "dim2")] - if stiffness != 0.0 || damping != 0.0 { - motor_inv_lhs = if keep_lhs { - let ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared(); - Some(gamma / (ii1 + ii2)) - } else { - Some(gamma) - }; - motor_rhs /= gamma; - } - - #[cfg(feature = "dim3")] - if stiffness != 0.0 || damping != 0.0 { - motor_inv_lhs = if keep_lhs { - let ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared(); - Some((ii1 + ii2).inverse_unchecked() * gamma) - } else { - Some(SdpMatrix::diagonal(gamma)) - }; - motor_rhs /= gamma; - } - } - - #[cfg(feature = "dim2")] - let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse) - * params.warmstart_coeff; - #[cfg(feature = "dim3")] - let motor_impulse = - joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff; - - /* - * Setup the limits constraint. - */ - let mut limits_active = false; - let mut limits_rhs = 0.0; - let mut limits_inv_lhs = 0.0; - let mut limits_impulse = 0.0; - let mut limits_axis = na::zero(); - - if joint.limits_enabled { - let axis1 = rb_pos1.position * joint.limits_local_axis1; - let axis2 = rb_pos2.position * joint.limits_local_axis2; - - #[cfg(feature = "dim2")] - let axis_angle = Rotation::rotation_between_axis(&axis2, &axis1).axis_angle(); - #[cfg(feature = "dim3")] - let axis_angle = - Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle()); - - // TODO: handle the case where dot(axis1, axis2) = -1.0 - if let Some((axis, angle)) = axis_angle { - if angle >= joint.limits_angle { - #[cfg(feature = "dim2")] - let axis = axis[0]; - #[cfg(feature = "dim3")] - let axis = axis.into_inner(); - - limits_active = true; - limits_rhs = (rb_vels2.angvel.gdot(axis) - rb_vels1.angvel.gdot(axis)) - * params.velocity_solve_fraction; - - limits_rhs += (angle - joint.limits_angle) * params.velocity_based_erp_inv_dt(); - - let ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared(); - limits_inv_lhs = crate::utils::inv( - axis.gdot(ii2.transform_vector(axis)) - + axis.gdot(ii1.transform_vector(axis)), - ); - - limits_impulse = joint.limits_impulse * params.warmstart_coeff; - limits_axis = axis; - } - } - } - - BallVelocityConstraint { - joint_id, - mj_lambda1: rb_ids1.active_set_offset, - mj_lambda2: rb_ids2.active_set_offset, - im1, - im2, - impulse: joint.impulse * params.warmstart_coeff, - r1: anchor1, - r2: anchor2, - rhs, - inv_lhs, - motor_rhs, - motor_impulse, - motor_inv_lhs, - motor_max_impulse: joint.motor_max_impulse, - ii1_sqrt: rb_mprops1.effective_world_inv_inertia_sqrt, - ii2_sqrt: rb_mprops2.effective_world_inv_inertia_sqrt, - limits_active, - limits_axis, - limits_rhs, - limits_inv_lhs, - limits_impulse, - } - } - - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - - mj_lambda1.linear += self.im1 * self.impulse; - mj_lambda1.angular += self - .ii1_sqrt - .transform_vector(self.r1.gcross(self.impulse) + self.motor_impulse); - mj_lambda2.linear -= self.im2 * self.impulse; - mj_lambda2.angular -= self - .ii2_sqrt - .transform_vector(self.r2.gcross(self.impulse) + self.motor_impulse); - - /* - * Warmstart limits. - */ - if self.limits_active { - let limit_impulse1 = -self.limits_axis * self.limits_impulse; - let limit_impulse2 = self.limits_axis * self.limits_impulse; - mj_lambda1.angular += self.ii1_sqrt.transform_vector(limit_impulse1); - mj_lambda2.angular += self.ii2_sqrt.transform_vector(limit_impulse2); - } - - mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; - mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; - } - - fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { - let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); - let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - let vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1); - let vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2); - let dvel = -vel1 + vel2 + self.rhs; - - let impulse = self.inv_lhs * dvel; - self.impulse += impulse; - - mj_lambda1.linear += self.im1 * impulse; - mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(impulse)); - - mj_lambda2.linear -= self.im2 * impulse; - mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse)); - } - - fn solve_limits(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { - if self.limits_active { - let limits_torquedir1 = -self.limits_axis; - let limits_torquedir2 = self.limits_axis; - - let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); - let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - - let ang_dvel = limits_torquedir1.gdot(ang_vel1) - + limits_torquedir2.gdot(ang_vel2) - + self.limits_rhs; - let new_impulse = (self.limits_impulse - ang_dvel * self.limits_inv_lhs).max(0.0); - let dimpulse = new_impulse - self.limits_impulse; - self.limits_impulse = new_impulse; - - let ang_impulse1 = limits_torquedir1 * dimpulse; - let ang_impulse2 = limits_torquedir2 * dimpulse; - - mj_lambda1.angular += self.ii1_sqrt.transform_vector(ang_impulse1); - mj_lambda2.angular += self.ii2_sqrt.transform_vector(ang_impulse2); - } - } - - fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { - if let Some(motor_inv_lhs) = &self.motor_inv_lhs { - let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); - let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - - let dangvel = (ang_vel2 - ang_vel1) + self.motor_rhs; - - let new_impulse = self.motor_impulse + motor_inv_lhs.transform_vector(dangvel); - - #[cfg(feature = "dim2")] - let clamped_impulse = - na::clamp(new_impulse, -self.motor_max_impulse, self.motor_max_impulse); - #[cfg(feature = "dim3")] - let clamped_impulse = new_impulse.cap_magnitude(self.motor_max_impulse); - - let effective_impulse = clamped_impulse - self.motor_impulse; - self.motor_impulse = clamped_impulse; - - mj_lambda1.angular += self.ii1_sqrt.transform_vector(effective_impulse); - mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse); - } - } - - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - - self.solve_limits(&mut mj_lambda1, &mut mj_lambda2); - self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2); - self.solve_motors(&mut mj_lambda1, &mut mj_lambda2); - - mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; - mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; - } - - pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { - let joint = &mut joints_all[self.joint_id].weight; - if let JointParams::BallJoint(ball) = &mut joint.params { - ball.impulse = self.impulse; - ball.motor_impulse = self.motor_impulse; - ball.limits_impulse = self.limits_impulse; - } - } -} - -#[derive(Debug)] -pub(crate) struct BallVelocityGroundConstraint { - mj_lambda2: usize, - joint_id: JointIndex, - r2: Vector, - - rhs: Vector, - impulse: Vector, - inv_lhs: SdpMatrix, - - motor_rhs: AngVector, - motor_impulse: AngVector, - motor_inv_lhs: Option>, - motor_max_impulse: Real, - - limits_active: bool, - limits_rhs: Real, - limits_inv_lhs: Real, - limits_impulse: Real, - limits_axis: AngVector, - - im2: Real, - ii2_sqrt: AngularInertia, -} - -impl BallVelocityGroundConstraint { - pub fn from_params( - params: &IntegrationParameters, - joint_id: JointIndex, - rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps), - rb2: ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyIds, - ), - joint: &BallJoint, - flipped: bool, - ) -> Self { - let (rb_pos1, rb_vels1, rb_mprops1) = rb1; - let (rb_pos2, rb_vels2, rb_mprops2, rb_ids2) = rb2; - - let (anchor_world1, anchor_world2) = if flipped { - ( - rb_pos1.position * joint.local_anchor2, - rb_pos2.position * joint.local_anchor1, - ) - } else { - ( - rb_pos1.position * joint.local_anchor1, - rb_pos2.position * joint.local_anchor2, - ) - }; - - let anchor1 = anchor_world1 - rb_mprops1.world_com; - let anchor2 = anchor_world2 - rb_mprops2.world_com; - - let im2 = rb_mprops2.effective_inv_mass; - let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(anchor1); - let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(anchor2); - - let rhs = (vel2 - vel1) * params.velocity_solve_fraction - + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt(); - - let cmat2 = anchor2.gcross_matrix(); - - let lhs; - - #[cfg(feature = "dim3")] - { - lhs = rb_mprops2 - .effective_world_inv_inertia_sqrt - .squared() - .quadform(&cmat2) - .add_diagonal(im2); - } - - #[cfg(feature = "dim2")] - { - let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared(); - let m11 = im2 + cmat2.x * cmat2.x * ii2; - let m12 = cmat2.x * cmat2.y * ii2; - let m22 = im2 + cmat2.y * cmat2.y * ii2; - lhs = SdpMatrix::new(m11, m12, m22) - } - - let inv_lhs = lhs.inverse_unchecked(); - - /* - * Motor part. - */ - let mut motor_rhs = na::zero(); - let mut motor_inv_lhs = None; - let motor_max_impulse = joint.motor_max_impulse; - - if motor_max_impulse > 0.0 { - let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients( - params.dt, - joint.motor_stiffness, - joint.motor_damping, - ); - - if stiffness != 0.0 { - let dpos = rb_pos2.position.rotation - * (rb_pos1.position.rotation * joint.motor_target_pos).inverse(); - #[cfg(feature = "dim2")] - { - motor_rhs += dpos.angle() * stiffness; - } - #[cfg(feature = "dim3")] - { - motor_rhs += dpos.scaled_axis() * stiffness; - } - } - - if damping != 0.0 { - let curr_vel = rb_vels2.angvel - rb_vels1.angvel; - motor_rhs += (curr_vel - joint.motor_target_vel) * damping; - } - - #[cfg(feature = "dim2")] - if stiffness != 0.0 || damping != 0.0 { - motor_inv_lhs = if keep_lhs { - let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared(); - Some(gamma / ii2) - } else { - Some(gamma) - }; - motor_rhs /= gamma; - } - - #[cfg(feature = "dim3")] - if stiffness != 0.0 || damping != 0.0 { - motor_inv_lhs = if keep_lhs { - let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared(); - Some(ii2.inverse_unchecked() * gamma) - } else { - Some(SdpMatrix::diagonal(gamma)) - }; - motor_rhs /= gamma; - } - } - - #[cfg(feature = "dim2")] - let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse) - * params.warmstart_coeff; - #[cfg(feature = "dim3")] - let motor_impulse = - joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff; - - /* - * Setup the limits constraint. - */ - let mut limits_active = false; - let mut limits_rhs = 0.0; - let mut limits_inv_lhs = 0.0; - let mut limits_impulse = 0.0; - let mut limits_axis = na::zero(); - - if joint.limits_enabled { - let (axis1, axis2) = if flipped { - ( - rb_pos1.position * joint.limits_local_axis2, - rb_pos2.position * joint.limits_local_axis1, - ) - } else { - ( - rb_pos1.position * joint.limits_local_axis1, - rb_pos2.position * joint.limits_local_axis2, - ) - }; - - #[cfg(feature = "dim2")] - let axis_angle = Rotation::rotation_between_axis(&axis2, &axis1).axis_angle(); - #[cfg(feature = "dim3")] - let axis_angle = - Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle()); - - // TODO: handle the case where dot(axis1, axis2) = -1.0 - if let Some((axis, angle)) = axis_angle { - if angle >= joint.limits_angle { - #[cfg(feature = "dim2")] - let axis = axis[0]; - #[cfg(feature = "dim3")] - let axis = axis.into_inner(); - - limits_active = true; - limits_rhs = (rb_vels2.angvel.gdot(axis) - rb_vels1.angvel.gdot(axis)) - * params.velocity_solve_fraction; - - limits_rhs += (angle - joint.limits_angle) * params.velocity_based_erp_inv_dt(); - - let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared(); - limits_inv_lhs = crate::utils::inv(axis.gdot(ii2.transform_vector(axis))); - limits_impulse = joint.limits_impulse * params.warmstart_coeff; - limits_axis = axis; - } - } - } - - BallVelocityGroundConstraint { - joint_id, - mj_lambda2: rb_ids2.active_set_offset, - im2, - impulse: joint.impulse * params.warmstart_coeff, - r2: anchor2, - rhs, - inv_lhs, - motor_rhs, - motor_impulse, - motor_inv_lhs, - motor_max_impulse: joint.motor_max_impulse, - ii2_sqrt: rb_mprops2.effective_world_inv_inertia_sqrt, - limits_active, - limits_axis, - limits_rhs, - limits_inv_lhs, - limits_impulse, - } - } - - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - mj_lambda2.linear -= self.im2 * self.impulse; - mj_lambda2.angular -= self - .ii2_sqrt - .transform_vector(self.r2.gcross(self.impulse) + self.motor_impulse); - - /* - * Warmstart limits. - */ - if self.limits_active { - let limit_impulse2 = self.limits_axis * self.limits_impulse; - mj_lambda2.angular += self.ii2_sqrt.transform_vector(limit_impulse2); - } - - mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; - } - - fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel) { - let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - let vel2 = mj_lambda2.linear + angvel.gcross(self.r2); - let dvel = vel2 + self.rhs; - - let impulse = self.inv_lhs * dvel; - self.impulse += impulse; - - mj_lambda2.linear -= self.im2 * impulse; - mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse)); - } - - fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel) { - if self.limits_active { - let limits_torquedir2 = self.limits_axis; - let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - - let ang_dvel = limits_torquedir2.gdot(ang_vel2) + self.limits_rhs; - let new_impulse = (self.limits_impulse - ang_dvel * self.limits_inv_lhs).max(0.0); - let dimpulse = new_impulse - self.limits_impulse; - self.limits_impulse = new_impulse; - - let ang_impulse2 = limits_torquedir2 * dimpulse; - mj_lambda2.angular += self.ii2_sqrt.transform_vector(ang_impulse2); - } - } - - fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel) { - if let Some(motor_inv_lhs) = &self.motor_inv_lhs { - let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - - let dangvel = ang_vel2 + self.motor_rhs; - let new_impulse = self.motor_impulse + motor_inv_lhs.transform_vector(dangvel); - - #[cfg(feature = "dim2")] - let clamped_impulse = - na::clamp(new_impulse, -self.motor_max_impulse, self.motor_max_impulse); - #[cfg(feature = "dim3")] - let clamped_impulse = new_impulse.cap_magnitude(self.motor_max_impulse); - - let effective_impulse = clamped_impulse - self.motor_impulse; - self.motor_impulse = clamped_impulse; - - mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse); - } - } - - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - - self.solve_limits(&mut mj_lambda2); - self.solve_dofs(&mut mj_lambda2); - self.solve_motors(&mut mj_lambda2); - - mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; - } - - // FIXME: duplicated code with the non-ground constraint. - pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { - let joint = &mut joints_all[self.joint_id].weight; - if let JointParams::BallJoint(ball) = &mut joint.params { - ball.impulse = self.impulse; - ball.motor_impulse = self.motor_impulse; - ball.limits_impulse = self.limits_impulse; - } - } -} diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs deleted file mode 100644 index ee465cd..0000000 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs +++ /dev/null @@ -1,359 +0,0 @@ -use crate::dynamics::solver::DeltaVel; -use crate::dynamics::{ - BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds, - RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, -}; -use crate::math::{ - AngVector, AngularInertia, Isometry, Point, Real, SdpMatrix, SimdReal, Vector, SIMD_WIDTH, -}; -use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; -use simba::simd::SimdValue; - -#[derive(Debug)] -pub(crate) struct WBallVelocityConstraint { - mj_lambda1: [usize; SIMD_WIDTH], - mj_lambda2: [usize; SIMD_WIDTH], - - joint_id: [JointIndex; SIMD_WIDTH], - - rhs: Vector, - pub(crate) impulse: Vector, - - r1: Vector, - r2: Vector, - - inv_lhs: SdpMatrix, - - im1: SimdReal, - im2: SimdReal, - - ii1_sqrt: AngularInertia, - ii2_sqrt: AngularInertia, -} - -impl WBallVelocityConstraint { - pub fn from_params( - params: &IntegrationParameters, - joint_id: [JointIndex; SIMD_WIDTH], - rbs1: ( - [&RigidBodyPosition; SIMD_WIDTH], - [&RigidBodyVelocity; SIMD_WIDTH], - [&RigidBodyMassProps; SIMD_WIDTH], - [&RigidBodyIds; SIMD_WIDTH], - ), - rbs2: ( - [&RigidBodyPosition; SIMD_WIDTH], - [&RigidBodyVelocity; SIMD_WIDTH], - [&RigidBodyMassProps; SIMD_WIDTH], - [&RigidBodyIds; SIMD_WIDTH], - ), - cparams: [&BallJoint; SIMD_WIDTH], - ) -> Self { - let (poss1, vels1, mprops1, ids1) = rbs1; - let (poss2, vels2, mprops2, ids2) = rbs2; - - let position1 = Isometry::from(gather![|ii| poss1[ii].position]); - let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); - let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); - let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); - let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]); - let ii1_sqrt = AngularInertia::::from(gather![ - |ii| mprops1[ii].effective_world_inv_inertia_sqrt - ]); - let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset]; - - let position2 = Isometry::from(gather![|ii| poss2[ii].position]); - let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); - let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); - let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); - let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); - let ii2_sqrt = AngularInertia::::from(gather![ - |ii| mprops2[ii].effective_world_inv_inertia_sqrt - ]); - let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; - - let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]); - let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]); - let impulse = Vector::from(gather![|ii| cparams[ii].impulse]); - - let anchor_world1 = position1 * local_anchor1; - let anchor_world2 = position2 * local_anchor2; - let anchor1 = anchor_world1 - world_com1; - let anchor2 = anchor_world2 - world_com2; - - let vel1: Vector = linvel1 + angvel1.gcross(anchor1); - let vel2: Vector = linvel2 + angvel2.gcross(anchor2); - let rhs = (vel2 - vel1) * SimdReal::splat(params.velocity_solve_fraction) - + (anchor_world2 - anchor_world1) * SimdReal::splat(params.velocity_based_erp_inv_dt()); - let lhs; - - let cmat1 = anchor1.gcross_matrix(); - let cmat2 = anchor2.gcross_matrix(); - - #[cfg(feature = "dim3")] - { - lhs = ii2_sqrt.squared().quadform(&cmat2).add_diagonal(im2) - + ii1_sqrt.squared().quadform(&cmat1).add_diagonal(im1); - } - - // In 2D we just unroll the computation because - // it's just easier that way. - #[cfg(feature = "dim2")] - { - let ii1 = ii1_sqrt.squared(); - let ii2 = ii2_sqrt.squared(); - let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2; - let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2; - let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2; - lhs = SdpMatrix::new(m11, m12, m22) - } - - let inv_lhs = lhs.inverse_unchecked(); - - WBallVelocityConstraint { - joint_id, - mj_lambda1, - mj_lambda2, - im1, - im2, - impulse: impulse * SimdReal::splat(params.warmstart_coeff), - r1: anchor1, - r2: anchor2, - rhs, - inv_lhs, - ii1_sqrt, - ii2_sqrt, - } - } - - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda1 = DeltaVel { - linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), - angular: AngVector::from(gather![ - |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular - ]), - }; - let mut mj_lambda2 = DeltaVel { - linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), - angular: AngVector::from(gather![ - |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular - ]), - }; - - mj_lambda1.linear += self.impulse * self.im1; - mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(self.impulse)); - mj_lambda2.linear -= self.impulse * self.im2; - mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse)); - - for ii in 0..SIMD_WIDTH { - mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); - mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); - } - for ii in 0..SIMD_WIDTH { - mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); - mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); - } - } - - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda1: DeltaVel = DeltaVel { - linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), - angular: AngVector::from(gather![ - |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular - ]), - }; - let mut mj_lambda2: DeltaVel = DeltaVel { - linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), - angular: AngVector::from(gather![ - |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular - ]), - }; - - let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); - let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - let vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1); - let vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2); - let dvel = -vel1 + vel2 + self.rhs; - - let impulse = self.inv_lhs * dvel; - self.impulse += impulse; - - mj_lambda1.linear += impulse * self.im1; - mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(impulse)); - - mj_lambda2.linear -= impulse * self.im2; - mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse)); - - for ii in 0..SIMD_WIDTH { - mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); - mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); - } - for ii in 0..SIMD_WIDTH { - mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); - mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); - } - } - - pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { - for ii in 0..SIMD_WIDTH { - let joint = &mut joints_all[self.joint_id[ii]].weight; - if let JointParams::BallJoint(ball) = &mut joint.params { - ball.impulse = self.impulse.extract(ii) - } - } - } -} - -#[derive(Debug)] -pub(crate) struct WBallVelocityGroundConstraint { - mj_lambda2: [usize; SIMD_WIDTH], - joint_id: [JointIndex; SIMD_WIDTH], - rhs: Vector, - pub(crate) impulse: Vector, - r2: Vector, - inv_lhs: SdpMatrix, - im2: SimdReal, - ii2_sqrt: AngularInertia, -} - -impl WBallVelocityGroundConstraint { - pub fn from_params( - params: &IntegrationParameters, - joint_id: [JointIndex; SIMD_WIDTH], - rbs1: ( - [&RigidBodyPosition; SIMD_WIDTH], - [&RigidBodyVelocity; SIMD_WIDTH], - [&RigidBodyMassProps; SIMD_WIDTH], - ), - rbs2: ( - [&RigidBodyPosition; SIMD_WIDTH], - [&RigidBodyVelocity; SIMD_WIDTH], - [&RigidBodyMassProps; SIMD_WIDTH], - [&RigidBodyIds; SIMD_WIDTH], - ), - cparams: [&BallJoint; SIMD_WIDTH], - flipped: [bool; SIMD_WIDTH], - ) -> Self { - let (poss1, vels1, mprops1) = rbs1; - let (poss2, vels2, mprops2, ids2) = rbs2; - - let position1 = Isometry::from(gather![|ii| poss1[ii].position]); - let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); - let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); - let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); - let local_anchor1 = Point::from(gather![|ii| if flipped[ii] { - cparams[ii].local_anchor2 - } else { - cparams[ii].local_anchor1 - }]); - - let position2 = Isometry::from(gather![|ii| poss2[ii].position]); - let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); - let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); - let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); - let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); - let ii2_sqrt = AngularInertia::::from(gather![ - |ii| mprops2[ii].effective_world_inv_inertia_sqrt - ]); - let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; - - let local_anchor2 = Point::from(gather![|ii| if flipped[ii] { - cparams[ii].local_anchor1 - } else { - cparams[ii].local_anchor2 - }]); - let impulse = Vector::from(gather![|ii| cparams[ii].impulse]); - - let anchor_world1 = position1 * local_anchor1; - let anchor_world2 = position2 * local_anchor2; - let anchor1 = anchor_world1 - world_com1; - let anchor2 = anchor_world2 - world_com2; - - let vel1: Vector = linvel1 + angvel1.gcross(anchor1); - let vel2: Vector = linvel2 + angvel2.gcross(anchor2); - let rhs = (vel2 - vel1) * SimdReal::splat(params.velocity_solve_fraction) - + (anchor_world2 - anchor_world1) * SimdReal::splat(params.velocity_based_erp_inv_dt()); - let lhs; - - let cmat2 = anchor2.gcross_matrix(); - - #[cfg(feature = "dim3")] - { - lhs = ii2_sqrt.squared().quadform(&cmat2).add_diagonal(im2); - } - - // In 2D we just unroll the computation because - // it's just easier that way. - #[cfg(feature = "dim2")] - { - let ii2 = ii2_sqrt.squared(); - let m11 = im2 + cmat2.x * cmat2.x * ii2; - let m12 = cmat2.x * cmat2.y * ii2; - let m22 = im2 + cmat2.y * cmat2.y * ii2; - lhs = SdpMatrix::new(m11, m12, m22) - } - - let inv_lhs = lhs.inverse_unchecked(); - - WBallVelocityGroundConstraint { - joint_id, - mj_lambda2, - im2, - impulse: impulse * SimdReal::splat(params.warmstart_coeff), - r2: anchor2, - rhs, - inv_lhs, - ii2_sqrt, - } - } - - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda2 = DeltaVel { - linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), - angular: AngVector::from(gather![ - |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular - ]), - }; - - mj_lambda2.linear -= self.impulse * self.im2; - mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse)); - - for ii in 0..SIMD_WIDTH { - mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); - mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); - } - } - - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda2: DeltaVel = DeltaVel { - linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), - angular: AngVector::from(gather![ - |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular - ]), - }; - - let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - let vel2 = mj_lambda2.linear + angvel.gcross(self.r2); - let dvel = vel2 + self.rhs; - - let impulse = self.inv_lhs * dvel; - self.impulse += impulse; - - mj_lambda2.linear -= impulse * self.im2; - mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse)); - - for ii in 0..SIMD_WIDTH { - mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); - mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); - } - } - - pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { - for ii in 0..SIMD_WIDTH { - let joint = &mut joints_all[self.joint_id[ii]].weight; - if let JointParams::BallJoint(ball) = &mut joint.params { - ball.impulse = self.impulse.extract(ii) - } - } - } -} diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs deleted file mode 100644 index 3ab13f7..0000000 --- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs +++ /dev/null @@ -1,151 +0,0 @@ -use crate::dynamics::{ - FixedJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, -}; -use crate::math::{AngularInertia, Isometry, Point, Real, Rotation}; -use crate::utils::WAngularInertia; - -#[derive(Debug)] -pub(crate) struct FixedPositionConstraint { - position1: usize, - position2: usize, - local_frame1: Isometry, - local_frame2: Isometry, - local_com1: Point, - local_com2: Point, - im1: Real, - im2: Real, - ii1: AngularInertia, - ii2: AngularInertia, - - lin_inv_lhs: Real, - ang_inv_lhs: AngularInertia, -} - -impl FixedPositionConstraint { - pub fn from_params( - rb1: (&RigidBodyMassProps, &RigidBodyIds), - rb2: (&RigidBodyMassProps, &RigidBodyIds), - cparams: &FixedJoint, - ) -> Self { - let (mprops1, ids1) = rb1; - let (mprops2, ids2) = rb2; - - let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); - let im1 = mprops1.effective_inv_mass; - let im2 = mprops2.effective_inv_mass; - let lin_inv_lhs = 1.0 / (im1 + im2); - let ang_inv_lhs = (ii1 + ii2).inverse(); - - Self { - local_frame1: cparams.local_frame1, - local_frame2: cparams.local_frame2, - position1: ids1.active_set_offset, - position2: ids2.active_set_offset, - im1, - im2, - ii1, - ii2, - local_com1: mprops1.local_mprops.local_com, - local_com2: mprops2.local_mprops.local_com, - lin_inv_lhs, - ang_inv_lhs, - } - } - - pub fn solve(&self, pa