diff options
Diffstat (limited to 'src/dynamics/solver/joint_constraint')
16 files changed, 4734 insertions, 0 deletions
diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs new file mode 100644 index 0000000..21a537e --- /dev/null +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -0,0 +1,165 @@ +use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; +#[cfg(feature = "dim2")] +use crate::math::SdpMatrix; +use crate::math::{AngularInertia, Isometry, Point, Rotation}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; + +#[derive(Debug)] +pub(crate) struct BallPositionConstraint { + position1: usize, + position2: usize, + + local_com1: Point<f32>, + local_com2: Point<f32>, + + im1: f32, + im2: f32, + + ii1: AngularInertia<f32>, + ii2: AngularInertia<f32>, + + local_anchor1: Point<f32>, + local_anchor2: Point<f32>, +} + +impl BallPositionConstraint { + pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &BallJoint) -> Self { + Self { + local_com1: rb1.mass_properties.local_com, + local_com2: rb2.mass_properties.local_com, + im1: rb1.mass_properties.inv_mass, + im2: rb2.mass_properties.inv_mass, + ii1: rb1.world_inv_inertia_sqrt.squared(), + ii2: rb2.world_inv_inertia_sqrt.squared(), + local_anchor1: cparams.local_anchor1, + local_anchor2: cparams.local_anchor2, + position1: rb1.active_set_offset, + position2: rb2.active_set_offset, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) { + 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; + + positions[self.position1 as usize] = position1; + positions[self.position2 as usize] = position2; + } +} + +#[derive(Debug)] +pub(crate) struct BallPositionGroundConstraint { + position2: usize, + anchor1: Point<f32>, + im2: f32, + ii2: AngularInertia<f32>, + local_anchor2: Point<f32>, + local_com2: Point<f32>, +} + +impl BallPositionGroundConstraint { + pub fn from_params( + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &BallJoint, + flipped: bool, + ) -> Self { + 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: rb1.predicted_position * cparams.local_anchor2, + im2: rb2.mass_properties.inv_mass, + ii2: rb2.world_inv_inertia_sqrt.squared(), + local_anchor2: cparams.local_anchor1, + position2: rb2.active_set_offset, + local_com2: rb2.mass_properties.local_com, + } + } else { + Self { + anchor1: rb1.predicted_position * cparams.local_anchor1, + im2: rb2.mass_properties.inv_mass, + ii2: rb2.world_inv_inertia_sqrt.squared(), + local_anchor2: cparams.local_anchor2, + position2: rb2.active_set_offset, + local_com2: rb2.mass_properties.local_com, + } + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) { + 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; + 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 new file mode 100644 index 0000000..c552d57 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -0,0 +1,199 @@ +use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; +#[cfg(feature = "dim2")] +use crate::math::SdpMatrix; +use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdFloat, 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<SimdFloat>, + local_com2: Point<SimdFloat>, + + im1: SimdFloat, + im2: SimdFloat, + + ii1: AngularInertia<SimdFloat>, + ii2: AngularInertia<SimdFloat>, + + local_anchor1: Point<SimdFloat>, + local_anchor2: Point<SimdFloat>, +} + +impl WBallPositionConstraint { + pub fn from_params( + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&BallJoint; SIMD_WIDTH], + ) -> Self { + let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]); + let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]); + let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1 = AngularInertia::<SimdFloat>::from( + array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ) + .squared(); + let ii2 = AngularInertia::<SimdFloat>::from( + array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ) + .squared(); + let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); + let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); + let position1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + Self { + local_com1, + local_com2, + im1, + im2, + ii1, + ii2, + local_anchor1, + local_anchor2, + position1, + position2, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) { + let mut position1 = Isometry::from(array![|ii| positions[self.position1[ii]]; SIMD_WIDTH]); + let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]); + + 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 * SimdFloat::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<SimdFloat>, + im2: SimdFloat, + ii2: AngularInertia<SimdFloat>, + local_anchor2: Point<SimdFloat>, + local_com2: Point<SimdFloat>, +} + +impl WBallPositionGroundConstraint { + pub fn from_params( + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&BallJoint; SIMD_WIDTH], + flipped: [bool; SIMD_WIDTH], + ) -> Self { + let position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]); + let anchor1 = position1 + * Point::from(array![|ii| if flipped[ii] { + cparams[ii].local_anchor2 + } else { + cparams[ii].local_anchor1 + }; SIMD_WIDTH]); + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2 = AngularInertia::<SimdFloat>::from( + array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ) + .squared(); + let local_anchor2 = Point::from(array![|ii| if flipped[ii] { + cparams[ii].local_anchor1 + } else { + cparams[ii].local_anchor2 + }; SIMD_WIDTH]); + let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]); + + Self { + anchor1, + im2, + ii2, + local_anchor2, + position2, + local_com2, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) { + let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]); + + 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 * SimdFloat::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 new file mode 100644 index 0000000..97ba244 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -0,0 +1,238 @@ +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, +}; +use crate::math::{SdpMatrix, Vector}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; + +#[derive(Debug)] +pub(crate) struct BallVelocityConstraint { + mj_lambda1: usize, + mj_lambda2: usize, + + joint_id: JointIndex, + + rhs: Vector<f32>, + pub(crate) impulse: Vector<f32>, + + gcross1: Vector<f32>, + gcross2: Vector<f32>, + + inv_lhs: SdpMatrix<f32>, + + im1: f32, + im2: f32, +} + +impl BallVelocityConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &BallJoint, + ) -> Self { + let anchor1 = rb1.position * cparams.local_anchor1 - rb1.world_com; + let anchor2 = rb2.position * cparams.local_anchor2 - rb2.world_com; + + let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1); + let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2); + let im1 = rb1.mass_properties.inv_mass; + let im2 = rb2.mass_properties.inv_mass; + + let rhs = -(vel1 - vel2); + let lhs; + + let cmat1 = anchor1.gcross_matrix(); + let cmat2 = anchor2.gcross_matrix(); + + #[cfg(feature = "dim3")] + { + lhs = rb2 + .world_inv_inertia_sqrt + .squared() + .quadform(&cmat2) + .add_diagonal(im2) + + rb1 + .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 = rb1.world_inv_inertia_sqrt.squared(); + let ii2 = rb2.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 gcross1 = rb1.world_inv_inertia_sqrt.transform_lin_vector(anchor1); + let gcross2 = rb2.world_inv_inertia_sqrt.transform_lin_vector(anchor2); + + let inv_lhs = lhs.inverse_unchecked(); + + BallVelocityConstraint { + joint_id, + mj_lambda1: rb1.active_set_offset, + mj_lambda2: rb2.active_set_offset, + im1, + im2, + impulse: cparams.impulse * params.warmstart_coeff, + gcross1, + gcross2, + rhs, + inv_lhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) { + 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.gcross1.gcross(self.impulse); + mj_lambda2.linear -= self.im2 * self.impulse; + mj_lambda2.angular -= self.gcross2.gcross(self.impulse); + + mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let vel1 = mj_lambda1.linear + mj_lambda1.angular.gcross(self.gcross1); + let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); + 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.gcross1.gcross(impulse); + + mj_lambda2.linear -= self.im2 * impulse; + mj_lambda2.angular -= self.gcross2.gcross(impulse); + + 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 + } + } +} + +#[derive(Debug)] +pub(crate) struct BallVelocityGroundConstraint { + mj_lambda2: usize, + joint_id: JointIndex, + rhs: Vector<f32>, + impulse: Vector<f32>, + gcross2: Vector<f32>, + inv_lhs: SdpMatrix<f32>, + im2: f32, +} + +impl BallVelocityGroundConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &BallJoint, + flipped: bool, + ) -> Self { + let (anchor1, anchor2) = if flipped { + ( + rb1.position * cparams.local_anchor2 - rb1.world_com, + rb2.position * cparams.local_anchor1 - rb2.world_com, + ) + } else { + ( + rb1.position * cparams.local_anchor1 - rb1.world_com, + rb2.position * cparams.local_anchor2 - rb2.world_com, + ) + }; + + let im2 = rb2.mass_properties.inv_mass; + let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1); + let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2); + let rhs = vel2 - vel1; + + let cmat2 = anchor2.gcross_matrix(); + let gcross2 = rb2.world_inv_inertia_sqrt.transform_lin_vector(anchor2); + + let lhs; + + #[cfg(feature = "dim3")] + { + lhs = rb2 + .world_inv_inertia_sqrt + .squared() + .quadform(&cmat2) + .add_diagonal(im2); + } + + #[cfg(feature = "dim2")] + { + let ii2 = rb2.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(); + + BallVelocityGroundConstraint { + joint_id, + mj_lambda2: rb2.active_set_offset, + im2, + impulse: cparams.impulse * params.warmstart_coeff, + gcross2, + rhs, + inv_lhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + mj_lambda2.linear -= self.im2 * self.impulse; + mj_lambda2.angular -= self.gcross2.gcross(self.impulse); + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); + let dvel = vel2 + self.rhs; + + let impulse = self.inv_lhs * dvel; + self.impulse += impulse; + + mj_lambda2.linear -= self.im2 * impulse; + mj_lambda2.angular -= self.gcross2.gcross(impulse); + + 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 + } + } +} diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs new file mode 100644 index 0000000..b96f3b8 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs @@ -0,0 +1,329 @@ +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, +}; +use crate::math::{ + AngVector, AngularInertia, Isometry, Point, SdpMatrix, SimdFloat, 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<SimdFloat>, + pub(crate) impulse: Vector<SimdFloat>, + + gcross1: Vector<SimdFloat>, + gcross2: Vector<SimdFloat>, + + inv_lhs: SdpMatrix<SimdFloat>, + + im1: SimdFloat, + im2: SimdFloat, +} + +impl WBallVelocityConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&BallJoint; SIMD_WIDTH], + ) -> Self { + let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1_sqrt = AngularInertia::<SimdFloat>::from( + array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + + let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::<SimdFloat>::from( + array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); + let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); + let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + + let anchor1 = position1 * local_anchor1 - world_com1; + let anchor2 = position2 * local_anchor2 - world_com2; + + let vel1: Vector<SimdFloat> = linvel1 + angvel1.gcross(anchor1); + let vel2: Vector<SimdFloat> = linvel2 + angvel2.gcross(anchor2); + let rhs = -(vel1 - vel2); + 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 gcross1 = ii1_sqrt.transform_lin_vector(anchor1); + let gcross2 = ii2_sqrt.transform_lin_vector(anchor2); + + let inv_lhs = lhs.inverse_unchecked(); + + WBallVelocityConstraint { + joint_id, + mj_lambda1, + mj_lambda2, + im1, + im2, + impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + gcross1, + gcross2, + rhs, + inv_lhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) { + let mut mj_lambda1 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + mj_lambda1.linear += self.impulse * self.im1; + mj_lambda1.angular += self.gcross1.gcross(self.impulse); + mj_lambda2.linear -= self.impulse * self.im2; + mj_lambda2.angular -= self.gcross2.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<f32>]) { + let mut mj_lambda1: DeltaVel<SimdFloat> = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let vel1 = mj_lambda1.linear + mj_lambda1.angular.gcross(self.gcross1); + let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); + 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.gcross1.gcross(impulse); + + mj_lambda2.linear -= impulse * self.im2; + mj_lambda2.angular -= self.gcross2.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<SimdFloat>, + pub(crate) impulse: Vector<SimdFloat>, + gcross2: Vector<SimdFloat>, + inv_lhs: SdpMatrix<SimdFloat>, + im2: SimdFloat, +} + +impl WBallVelocityGroundConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&BallJoint; SIMD_WIDTH], + flipped: [bool; SIMD_WIDTH], + ) -> Self { + let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + let local_anchor1 = Point::from( + array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], + ); + + let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::<SimdFloat>::from( + array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let local_anchor2 = Point::from( + array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH], + ); + let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + + let anchor1 = position1 * local_anchor1 - world_com1; + let anchor2 = position2 * local_anchor2 - world_com2; + |