From 754a48b7ff6d8c58b1ee08651e60112900b60455 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Tue, 25 Aug 2020 22:10:25 +0200 Subject: First public release of Rapier. --- .../joint_constraint/ball_position_constraint.rs | 165 +++++ .../ball_position_constraint_wide.rs | 199 ++++++ .../joint_constraint/ball_velocity_constraint.rs | 238 +++++++ .../ball_velocity_constraint_wide.rs | 329 ++++++++++ .../joint_constraint/fixed_position_constraint.rs | 139 +++++ .../joint_constraint/fixed_velocity_constraint.rs | 370 +++++++++++ .../fixed_velocity_constraint_wide.rs | 472 ++++++++++++++ .../solver/joint_constraint/joint_constraint.rs | 340 ++++++++++ .../joint_constraint/joint_position_constraint.rs | 169 +++++ src/dynamics/solver/joint_constraint/mod.rs | 65 ++ .../prismatic_position_constraint.rs | 170 +++++ .../prismatic_velocity_constraint.rs | 558 +++++++++++++++++ .../prismatic_velocity_constraint_wide.rs | 687 +++++++++++++++++++++ .../revolute_position_constraint.rs | 142 +++++ .../revolute_velocity_constraint.rs | 294 +++++++++ .../revolute_velocity_constraint_wide.rs | 397 ++++++++++++ 16 files changed, 4734 insertions(+) create mode 100644 src/dynamics/solver/joint_constraint/ball_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/fixed_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/mod.rs create mode 100644 src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/revolute_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs create 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 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, + local_com2: Point, + + im1: f32, + im2: f32, + + ii1: AngularInertia, + ii2: AngularInertia, + + local_anchor1: Point, + local_anchor2: Point, +} + +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]) { + 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, + im2: f32, + ii2: AngularInertia, + local_anchor2: Point, + local_com2: Point, +} + +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]) { + 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, + local_com2: Point, + + im1: SimdFloat, + im2: SimdFloat, + + ii1: AngularInertia, + ii2: AngularInertia, + + local_anchor1: Point, + local_anchor2: Point, +} + +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::::from( + array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ) + .squared(); + let ii2 = AngularInertia::::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]) { + 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, + im2: SimdFloat, + ii2: AngularInertia, + local_anchor2: Point, + local_com2: Point, +} + +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::::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]) { + 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, + pub(crate) impulse: Vector, + + gcross1: Vector, + gcross2: Vector, + + inv_lhs: SdpMatrix, + + 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]) { + 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]) { + 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, + impulse: Vector, + gcross2: Vector, + inv_lhs: SdpMatrix, + 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]) { + 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]) { + 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, + pub(crate) impulse: Vector, + + gcross1: Vector, + gcross2: Vector, + + inv_lhs: SdpMatrix, + + 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::::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::::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::::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::::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 = linvel1 + angvel1.gcross(anchor1); + let vel2: Vector = 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]) { + 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]) { + let mut mj_lambda1: DeltaVel = 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 = 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, + pub(crate) impulse: Vector, + gcross2: Vector, + inv_lhs: SdpMatrix, + 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::::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::::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::::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; + + let vel1: Vector = linvel1 + angvel1.gcross(anchor1); + let vel2: Vector = linvel2 + angvel2.gcross(anchor2); + let rhs = vel2 - vel1; + let lhs; + + let cmat2 = anchor2.gcross_matrix(); + let gcross2 = ii2_sqrt.transform_lin_vector(anchor2); + + #[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 * SimdFloat::splat(params.warmstart_coeff), + gcross2, + rhs, + inv_lhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + 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_lambda2.linear -= self.impulse * self.im2; + mj_lambda2.angular -= self.gcross2.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( + 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 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 -= impulse * self.im2; + mj_lambda2.angular -= self.gcross2.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 new file mode 100644 index 0000000..24001cd --- /dev/null +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs @@ -0,0 +1,139 @@ +use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody}; +use crate::math::{AngularInertia, Isometry, Point, Rotation}; +use crate::utils::WAngularInertia; + +#[derive(Debug)] +pub(crate) struct FixedPositionConstraint { + position1: usize, + position2: usize, + local_anchor1: Isometry, + local_anchor2: Isometry, + local_com1: Point, + local_com2: Point, + im1: f32, + im2: f32, + ii1: AngularInertia, + ii2: AngularInertia, + + lin_inv_lhs: f32, + ang_inv_lhs: AngularInertia, +} + +impl FixedPositionConstraint { + pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &FixedJoint) -> Self { + let ii1 = rb1.world_inv_inertia_sqrt.squared(); + let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let im1 = rb1.mass_properties.inv_mass; + let im2 = rb2.mass_properties.inv_mass; + let lin_inv_lhs = 1.0 / (im1 + im2); + let ang_inv_lhs = (ii1 + ii2).inverse(); + + Self { + local_anchor1: cparams.local_anchor1, + local_anchor2: cparams.local_anchor2, + position1: rb1.active_set_offset, + position2: rb2.active_set_offset, + im1, + im2, + ii1, + ii2, + local_com1: rb1.mass_properties.local_com, + local_com2: rb2.mass_properties.local_com, + lin_inv_lhs, + ang_inv_lhs, + } + } + + 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]; + + // Angular correction. + let anchor1 = position1 * self.local_anchor1; + let anchor2 = position2 * self.local_anchor2; + let ang_err = anchor2.rotation * anchor1.rotation.inverse(); + #[cfg(feature = "dim3")] + let ang_impulse = self + .ang_inv_lhs + .transform_vector(ang_err.scaled_axis() * params.joint_erp); + #[cfg(feature = "dim2")] + let ang_impulse = self + .ang_inv_lhs + .transform_vector(ang_err.angle() * 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; + + // Linear correction. + let anchor1 = position1 * Point::from(self.local_anchor1.translation.vector); + let anchor2 = position2 * Point::from(self.local_anchor2.translation.vector); + let err = anchor2 - anchor1; + let impulse = err * (self.lin_inv_lhs * params.joint_erp); + position1.translation.vector += self.im1 * impulse; + position2.translation.vector -= self.im2 * impulse; + + positions[self.position1 as usize] = position1; + positions[self.position2 as usize] = position2; + } +} + +#[derive(Debug)] +pub(crate) struct FixedPositionGroundConstraint { + position2: usize, + anchor1: Isometry, + local_anchor2: Isometry, + local_com2: Point, + im2: f32, + ii2: AngularInertia, + impulse: f32, +} + +impl FixedPositionGroundConstraint { + pub fn from_params( + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &FixedJoint, + flipped: bool, + ) -> Self { + let anchor1; + let local_anchor2; + + if flipped { + anchor1 = rb1.predicted_position * cparams.local_anchor2; + local_anchor2 = cparams.local_anchor1; + } else { + anchor1 = rb1.predicted_position * cparams.local_anchor1; + local_anchor2 = cparams.local_anchor2; + }; + + Self { + anchor1, + local_anchor2, + position2: rb2.active_set_offset, + im2: rb2.mass_properties.inv_mass, + ii2: rb2.world_inv_inertia_sqrt.squared(), + local_com2: rb2.mass_properties.local_com, + impulse: 0.0, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + let mut position2 = positions[self.position2 as usize]; + + // Angular correction. + let anchor2 = position2 * self.local_anchor2; + let ang_err = anchor2.rotation * self.anchor1.rotation.inverse(); + position2.rotation = ang_err.powf(-params.joint_erp) * position2.rotation; + + // Linear correction. + let anchor1 = Point::from(self.anchor1.translation.vector); + let anchor2 = position2 * Point::from(self.local_anchor2.translation.vector); + let err = anchor2 - anchor1; + // NOTE: no need to divide by im2 just to multiply right after. + let impulse = err * params.joint_erp; + position2.translation.vector -= impulse; + + positions[self.position2 as usize] = position2; + } +} diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs new file mode 100644 index 0000000..e4187c8 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs @@ -0,0 +1,370 @@ +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, +}; +use crate::math::{AngularInertia, Dim, SpacialVector, Vector}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +#[cfg(feature = "dim2")] +use na::{Matrix3, Vector3}; +#[cfg(feature = "dim3")] +use na::{Matrix6, Vector6, U3}; + +#[derive(Debug)] +pub(crate) struct FixedVelocityConstraint { + mj_lambda1: usize, + mj_lambda2: usize, + + joint_id: JointIndex, + + impulse: SpacialVector, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix6, // FIXME: replace by Cholesky. + #[cfg(feature = "dim3")] + rhs: Vector6, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix3, // FIXME: replace by Cholesky. + #[cfg(feature = "dim2")] + rhs: Vector3, + + im1: f32, + im2: f32, + + ii1: AngularInertia, + ii2: AngularInertia, + + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, + + r1: Vector, + r2: Vector, +} + +impl FixedVelocityConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &FixedJoint, + ) -> Self { + let anchor1 = rb1.position * cparams.local_anchor1; + let anchor2 = rb2.position * cparams.local_anchor2; + let im1 = rb1.mass_properties.inv_mass; + let im2 = rb2.mass_properties.inv_mass; + let ii1 = rb1.world_inv_inertia_sqrt.squared(); + let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let r1 = anchor1.translation.vector - rb1.world_com.coords; + let r2 = anchor2.translation.vector - rb2.world_com.coords; + let rmat1 = r1.gcross_matrix(); + let rmat2 = r2.gcross_matrix(); + + #[allow(unused_mut)] // For 2D + let mut lhs; + + #[cfg(feature = "dim3")] + { + let lhs00 = + ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2); + let lhs10 = ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2); + let lhs11 = (ii1 + ii2).into_matrix(); + + // Note that Cholesky only reads the lower-triangular part of the matrix + // so we don't need to fill lhs01. + lhs = Matrix6::zeros(); + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(3, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::(3, 3).copy_from(&lhs11); + } + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + { + let m11 = im1 + im2 + rmat1.x * rmat1.x * ii1 + rmat2.x * rmat2.x * ii2; + let m12 = rmat1.x * rmat1.y * ii1 + rmat2.x * rmat2.y * ii2; + let m22 = im1 + im2 + rmat1.y * rmat1.y * ii1 + rmat2.y * rmat2.y * ii2; + let m13 = rmat1.x * ii1 + rmat2.x * ii2; + let m23 = rmat1.y * ii1 + rmat2.y * ii2; + let m33 = ii1 + ii2; + lhs = Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33) + } + + // NOTE: we don't use cholesky in 2D because we only have a 3x3 matrix + // for which a textbook inverse is still efficient. + #[cfg(feature = "dim2")] + let inv_lhs = lhs.try_inverse().expect("Singular system."); + #[cfg(feature = "dim3")] + let inv_lhs = lhs.cholesky().expect("Singular system.").inverse(); + + let lin_dvel = -rb1.linvel - rb1.angvel.gcross(r1) + rb2.linvel + rb2.angvel.gcross(r2); + let ang_dvel = -rb1.angvel + rb2.angvel; + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, + ); + + FixedVelocityConstraint { + joint_id, + mj_lambda1: rb1.active_set_offset, + mj_lambda2: rb2.active_set_offset, + im1, + im2, + ii1, + ii2, + ii1_sqrt: rb1.world_inv_inertia_sqrt, + ii2_sqrt: rb2.world_inv_inertia_sqrt, + impulse: cparams.impulse * params.warmstart_coeff, + inv_lhs, + r1, + r2, + rhs, + } + } + + 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]; + + let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = self.impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = self.impulse.fixed_rows::(3).into_owned(); + + mj_lambda1.linear += self.im1 * lin_impulse; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_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]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1) + + mj_lambda2.linear + + ang_vel2.gcross(self.r2); + let dangvel = -ang_vel1 + ang_vel2; + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, + ) + self.rhs; + + let impulse = self.inv_lhs * rhs; + self.impulse += impulse; + let lin_impulse = impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = impulse.fixed_rows::(3).into_owned(); + + mj_lambda1.linear += self.im1 * lin_impulse; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_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::FixedJoint(fixed) = &mut joint.params { + fixed.impulse = self.impulse; + } + } +} + +#[derive(Debug)] +pub(crate) struct FixedVelocityGroundConstraint { + mj_lambda2: usize, + + joint_id: JointIndex, + + impulse: SpacialVector, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix6, // FIXME: replace by Cholesky. + #[cfg(feature = "dim3")] + rhs: Vector6, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix3, // FIXME: replace by Cholesky. + #[cfg(feature = "dim2")] + rhs: Vector3, + + im2: f32, + ii2: AngularInertia, + ii2_sqrt: AngularInertia, + r2: Vector, +} + +impl FixedVelocityGroundConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &FixedJoint, + flipped: bool, + ) -> Self { + let (anchor1, anchor2) = if flipped { + ( + rb1.position * cparams.local_anchor2, + rb2.position * cparams.local_anchor1, + ) + } else { + ( + rb1.position * cparams.local_anchor1, + rb2.position * cparams.local_anchor2, + ) + }; + + let r1 = anchor1.translation.vector - rb1.world_com.coords; + + let im2 = rb2.mass_properties.inv_mass; + let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let r2 = anchor2.translation.vector - rb2.world_com.coords; + let rmat2 = r2.gcross_matrix(); + + #[allow(unused_mut)] // For 2D. + let mut lhs; + + #[cfg(feature = "dim3")] + { + let lhs00 = ii2.quadform(&rmat2).add_diagonal(im2); + let lhs10 = ii2.transform_matrix(&rmat2); + let lhs11 = ii2.into_matrix(); + + // Note that Cholesky only reads the lower-triangular part of the matrix + // so we don't need to fill lhs01. + lhs = Matrix6::zeros(); + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(3, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::(3, 3).copy_from(&lhs11); + } + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + { + let m11 = im2 + rmat2.x * rmat2.x * ii2; + let m12 = rmat2.x * rmat2.y * ii2; + let m22 = im2 + rmat2.y * rmat2.y * ii2; + let m13 = rmat2.x * ii2; + let m23 = rmat2.y * ii2; + let m33 = ii2; + lhs = Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33) + } + + #[cfg(feature = "dim2")] + let inv_lhs = lhs.try_inverse().expect("Singular system."); + #[cfg(feature = "dim3")] + let inv_lhs = lhs.cholesky().expect("Singular system.").inverse(); + + let lin_dvel = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1); + let ang_dvel = rb2.angvel - rb1.angvel; + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, + ); + + FixedVelocityGroundConstraint { + joint_id, + mj_lambda2: rb2.active_set_offset, + im2, + ii2, + ii2_sqrt: rb2.world_inv_inertia_sqrt, + impulse: cparams.impulse * params.warmstart_coeff, + inv_lhs, + r2, + rhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = self.impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = self.impulse.fixed_rows::(3).into_owned(); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2); + let dangvel = ang_vel2; + #[cfg(feature = "dim2")] + let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, + ) + self.rhs; + + let impulse = self.inv_lhs * rhs; + + self.impulse += impulse; + let lin_impulse = impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = impulse.fixed_rows::(3).into_owned(); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_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::FixedJoint(fixed) = &mut joint.params { + fixed.impulse = self.impulse; + } + } +} diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs new file mode 100644 index 0000000..7c87b2c --- /dev/null +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs @@ -0,0 +1,472 @@ +use simba::simd::SimdValue; + +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, +}; +use crate::math::{ + AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, SimdFloat, SpacialVector, Vector, + SIMD_WIDTH, +}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +#[cfg(feature = "dim3")] +use na::{Cholesky, Matrix6, Vector6, U3}; +#[cfg(feature = "dim2")] +use { + crate::utils::SdpMatrix3, + na::{Matrix3, Vector3}, +}; + +#[derive(Debug)] +pub(crate) struct WFixedVelocityConstraint { + mj_lambda1: [usize; SIMD_WIDTH], + mj_lambda2: [usize; SIMD_WIDTH], + + joint_id: [JointIndex; SIMD_WIDTH], + + impulse: SpacialVector, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix6, // FIXME: replace by Cholesky. + #[cfg(feature = "dim3")] + rhs: Vector6, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix3, + #[cfg(feature = "dim2")] + rhs: Vector3, + + im1: SimdFloat, + im2: SimdFloat, + + ii1: AngularInertia, + ii2: AngularInertia, + + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, + + r1: Vector, + r2: Vector, +} + +impl WFixedVelocityConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&FixedJoint; 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::::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::::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::::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::::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 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); + let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); + let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + + let anchor1 = position1 * local_anchor1; + let anchor2 = position2 * local_anchor2; + let ii1 = ii1_sqrt.squared(); + let ii2 = ii2_sqrt.squared(); + let r1 = anchor1.translation.vector - world_com1.coords; + let r2 = anchor2.translation.vector - world_com2.coords; + let rmat1: CrossMatrix<_> = r1.gcross_matrix(); + let rmat2: CrossMatrix<_> = r2.gcross_matrix(); + + #[allow(unused_mut)] // For 2D. + let mut lhs; + + #[cfg(feature = "dim3")] + { + let lhs00 = + ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2); + let lhs10 = ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2); + let lhs11 = (ii1 + ii2).into_matrix(); + + // Note that Cholesky only reads the lower-triangular part of the matrix + // so we don't need to fill lhs01. + lhs = Matrix6::zeros(); + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(3, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::(3, 3).copy_from(&lhs11); + } + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + { + let m11 = im1 + im2 + rmat1.x * rmat1.x * ii1 + rmat2.x * rmat2.x * ii2; + let m12 = rmat1.x * rmat1.y * ii1 + rmat2.x * rmat2.y * ii2; + let m22 = im1 + im2 + rmat1.y * rmat1.y * ii1 + rmat2.y * rmat2.y * ii2; + let m13 = rmat1.x * ii1 + rmat2.x * ii2; + let m23 = rmat1.y * ii1 + rmat2.y * ii2; + let m33 = ii1 + ii2; + lhs = SdpMatrix3::new(m11, m12, m13, m22, m23, m33) + } + + // NOTE: we don't use cholesky in 2D because we only have a 3x3 matrix + // for which a textbook inverse is still efficient. + #[cfg(feature = "dim2")] + let inv_lhs = lhs.inverse_unchecked().into_matrix(); // FIXME: don't extract the matrix? + #[cfg(feature = "dim3")] + let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); + + let lin_dvel = -linvel1 - angvel1.gcross(r1) + linvel2 + angvel2.gcross(r2); + let ang_dvel = -angvel1 + angvel2; + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, + ); + + WFixedVelocityConstraint { + joint_id, + mj_lambda1, + mj_lambda2, + im1, + im2, + ii1, + ii2, + ii1_sqrt, + ii2_sqrt, + impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + inv_lhs, + r1, + r2, + rhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + 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], + ), + }; + + let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); + #[cfg(fe