diff options
Diffstat (limited to 'src/dynamics/solver')
7 files changed, 1264 insertions, 4 deletions
diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs new file mode 100644 index 0000000..f5138ea --- /dev/null +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs @@ -0,0 +1,171 @@ +use super::{GenericVelocityConstraint, GenericVelocityGroundConstraint}; +use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody}; +use crate::math::{ + AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector, + DIM, +}; +use crate::utils::{WAngularInertia, WCross}; +use na::{Vector3, Vector6}; + +// FIXME: review this code for the case where the center of masses are not the origin. +#[derive(Debug)] +pub(crate) struct GenericPositionConstraint { + position1: usize, + position2: usize, + local_anchor1: Isometry<Real>, + local_anchor2: Isometry<Real>, + local_com1: Point<Real>, + local_com2: Point<Real>, + im1: Real, + im2: Real, + ii1: AngularInertia<Real>, + ii2: AngularInertia<Real>, + + joint: GenericJoint, + + lin_impulse: Cell<Vector3<Real>>, + ang_impulse: Cell<Vector3<Real>>, +} + +impl GenericPositionConstraint { + pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, joint: &GenericJoint) -> Self { + let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let im1 = rb1.effective_inv_mass; + let im2 = rb2.effective_inv_mass; + + Self { + local_anchor1: joint.local_anchor1, + local_anchor2: joint.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, + joint: *joint, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) { + 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 r1 = Point::from(anchor1.translation.vector) - position1 * self.local_com1; + let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2; + + let delta_pos = anchor1.inverse() * anchor2; + let mass_matrix = GenericVelocityConstraint::compute_mass_matrix( + &self.joint, + self.im1, + self.im2, + self.ii1, + self.ii2, + r1, + r2, + false, + ); + + let lin_err = delta_pos.translation.vector * params.joint_erp; + let ang_err = delta_pos.rotation.scaled_axis() * params.joint_erp; + let err = Vector6::new( + lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z, + ); + let impulse = mass_matrix * err; + let lin_impulse = impulse.xyz(); + let ang_impulse = Vector3::new(impulse[3], impulse[4], impulse[5]); + + position1.rotation = Rotation::new( + self.ii1 + .transform_vector(ang_impulse + r1.gcross(lin_impulse)), + ) * position1.rotation; + position2.rotation = Rotation::new( + self.ii2 + .transform_vector(-ang_impulse - r2.gcross(lin_impulse)), + ) * position2.rotation; + + position1.translation.vector += self.im1 * lin_impulse; + position2.translation.vector -= self.im2 * lin_impulse; + + positions[self.position1 as usize] = position1; + positions[self.position2 as usize] = position2; + } +} + +#[derive(Debug)] +pub(crate) struct GenericPositionGroundConstraint { + position2: usize, + anchor1: Isometry<Real>, + local_anchor2: Isometry<Real>, + local_com2: Point<Real>, + im2: Real, + ii2: AngularInertia<Real>, + joint: GenericJoint, +} + +impl GenericPositionGroundConstraint { + pub fn from_params( + rb1: &RigidBody, + rb2: &RigidBody, + joint: &GenericJoint, + flipped: bool, + ) -> Self { + let anchor1; + let local_anchor2; + + if flipped { + anchor1 = rb1.predicted_position * joint.local_anchor2; + local_anchor2 = joint.local_anchor1; + } else { + anchor1 = rb1.predicted_position * joint.local_anchor1; + local_anchor2 = joint.local_anchor2; + }; + + Self { + anchor1, + local_anchor2, + position2: rb2.active_set_offset, + im2: rb2.effective_inv_mass, + ii2: rb2.effective_world_inv_inertia_sqrt.squared(), + local_com2: rb2.mass_properties.local_com, + joint: *joint, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) { + let mut position2 = positions[self.position2 as usize]; + + let anchor2 = position2 * self.local_anchor2; + let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2; + + let delta_pos = self.anchor1.inverse() * anchor2; + let mass_matrix = GenericVelocityGroundConstraint::compute_mass_matrix( + &self.joint, + self.im2, + self.ii2, + r2, + false, + ); + + let lin_err = delta_pos.translation.vector * params.joint_erp; + let ang_err = Vector3::zeros(); // delta_pos.rotation.scaled_axis() * params.joint_erp; + let err = Vector6::new( + lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z, + ); + let impulse = mass_matrix * err; + let lin_impulse = impulse.xyz(); + let ang_impulse = Vector3::new(impulse[3], impulse[4], impulse[5]); + + position2.rotation = Rotation::new( + self.ii2 + .transform_vector(-ang_impulse - r2.gcross(lin_impulse)), + ) * position2.rotation; + position2.translation.vector -= self.im2 * lin_impulse; + + positions[self.position2 as usize] = position2; + } +} diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs new file mode 100644 index 0000000..9ceea67 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs @@ -0,0 +1,51 @@ +use super::{GenericPositionConstraint, GenericPositionGroundConstraint}; +use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody}; +use crate::math::{Isometry, Real, SIMD_WIDTH}; + +// TODO: this does not uses SIMD optimizations yet. +#[derive(Debug)] +pub(crate) struct WGenericPositionConstraint { + constraints: [GenericPositionConstraint; SIMD_WIDTH], +} + +impl WGenericPositionConstraint { + pub fn from_params( + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&GenericJoint; SIMD_WIDTH], + ) -> Self { + Self { + constraints: array![|ii| GenericPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH], + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) { + for constraint in &self.constraints { + constraint.solve(params, positions); + } + } +} + +#[derive(Debug)] +pub(crate) struct WGenericPositionGroundConstraint { + constraints: [GenericPositionGroundConstraint; SIMD_WIDTH], +} + +impl WGenericPositionGroundConstraint { + pub fn from_params( + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&GenericJoint; SIMD_WIDTH], + flipped: [bool; SIMD_WIDTH], + ) -> Self { + Self { + constraints: array![|ii| GenericPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH], + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) { + for constraint in &self.constraints { + constraint.solve(params, positions); + } + } +} diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs new file mode 100644 index 0000000..6f501d2 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs @@ -0,0 +1,460 @@ +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, +}; +use crate::math::{AngularInertia, Dim, Real, SpacialVector, Vector}; +use crate::parry::math::SpatialVector; +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 GenericVelocityConstraint { + mj_lambda1: usize, + mj_lambda2: usize, + + joint_id: JointIndex, + + impulse: SpacialVector<Real>, + max_positive_impulse: SpatialVector<Real>, + max_negative_impulse: SpatialVector<Real>, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix6<Real>, // FIXME: replace by Cholesky. + #[cfg(feature = "dim3")] + rhs: Vector6<Real>, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix3<Real>, // FIXME: replace by Cholesky. + #[cfg(feature = "dim2")] + rhs: Vector3<Real>, + + im1: Real, + im2: Real, + + ii1: AngularInertia<Real>, + ii2: AngularInertia<Real>, + + ii1_sqrt: AngularInertia<Real>, + ii2_sqrt: AngularInertia<Real>, + + r1: Vector<Real>, + r2: Vector<Real>, +} + +impl GenericVelocityConstraint { + #[inline(always)] + pub fn compute_mass_matrix( + joint: &GenericJoint, + im1: Real, + im2: Real, + ii1: AngularInertia<Real>, + ii2: AngularInertia<Real>, + r1: Vector<Real>, + r2: Vector<Real>, + velocity_solver: bool, + ) -> Matrix6<Real> { + 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::<U3, U3>(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11); + + // Adjust the mass matrix to take force limits into account. + // If a DoF has a force limit, then we need to make its + // constraint independent from the others because otherwise + // the force clamping will cause errors to propagate in the + // other constraints. + if velocity_solver { + for i in 0..6 { + if joint.max_negative_impulse[i] > -Real::MAX + || joint.max_positive_impulse[i] < Real::MAX + { + let diag = lhs[(i, i)]; + lhs.row_mut(i).fill(0.0); + lhs[(i, i)] = diag; + } + } + } else { + for i in 0..6 { + let diag = lhs[(i, i)]; + lhs.row_mut(i).fill(0.0); + lhs[(i, i)] = diag; + } + } + } + + // 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")] + return lhs.try_inverse().expect("Singular system."); + #[cfg(feature = "dim3")] + return lhs.cholesky().expect("Singular system.").inverse(); + } + + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + joint: &GenericJoint, + ) -> Self { + let anchor1 = rb1.position * joint.local_anchor1; + let anchor2 = rb2.position * joint.local_anchor2; + let im1 = rb1.effective_inv_mass; + let im2 = rb2.effective_inv_mass; + let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let r1 = anchor1.translation.vector - rb1.world_com.coords; + let r2 = anchor2.translation.vector - rb2.world_com.coords; + + let lin_dvel = -rb1.linvel - rb1.angvel.gcross(r1) + rb2.linvel + rb2.angvel.gcross(r2); + let ang_dvel = -rb1.angvel + rb2.angvel; + + let inv_lhs = Self::compute_mass_matrix(joint, im1, im2, ii1, ii2, r1, r2, true); + + #[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, + ); + + let impulse = (joint.impulse * params.warmstart_coeff) + .inf(&joint.max_positive_impulse) + .sup(&joint.max_negative_impulse); + + GenericVelocityConstraint { + joint_id, + mj_lambda1: rb1.active_set_offset, + mj_lambda2: rb2.active_set_offset, + im1, + im2, + ii1, + ii2, + ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, + ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + impulse, + max_positive_impulse: joint.max_positive_impulse, + max_negative_impulse: joint.max_negative_impulse, + inv_lhs, + r1, + r2, + rhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) { + 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::<Dim>(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = self.impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = self.impulse.fixed_rows::<U3>(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<Real>]) { + 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 dvel = Vector6::new( + dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, + ) + self.rhs; + + let new_impulse = (self.impulse + self.inv_lhs * dvel) + .sup(&self.max_negative_impulse) + .inf(&self.max_positive_impulse); + let effective_impulse = new_impulse - self.impulse; + self.impulse = new_impulse; + + let lin_impulse = effective_impulse.fixed_rows::<Dim>(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = effective_impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = effective_impulse.fixed_rows::<U3>(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::GenericJoint(fixed) = &mut joint.params { + fixed.impulse = self.impulse; + } + } +} + +#[derive(Debug)] +pub(crate) struct GenericVelocityGroundConstraint { + mj_lambda2: usize, + + joint_id: JointIndex, + + impulse: SpacialVector<Real>, + max_positive_impulse: SpatialVector<Real>, + max_negative_impulse: SpatialVector<Real>, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix6<Real>, // FIXME: replace by Cholesky. + #[cfg(feature = "dim3")] + rhs: Vector6<Real>, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix3<Real>, // FIXME: replace by Cholesky. + #[cfg(feature = "dim2")] + rhs: Vector3<Real>, + + im2: Real, + ii2: AngularInertia<Real>, + ii2_sqrt: AngularInertia<Real>, + r2: Vector<Real>, +} + +impl GenericVelocityGroundConstraint { + #[inline(always)] + pub fn compute_mass_matrix( + joint: &GenericJoint, + im2: Real, + ii2: AngularInertia<Real>, + r2: Vector<Real>, + velocity_solver: bool, + ) -> Matrix6<Real> { + 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::<U3, U3>(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11); + + // Adjust the mass matrix to take force limits into account. + // If a DoF has a force limit, then we need to make its + // constraint independent from the others because otherwise + // the force clamping will cause errors to propagate in the + // other constraints. + if velocity_solver { + for i in 0..6 { + if joint.max_negative_impulse[i] > -Real::MAX + || joint.max_positive_impulse[i] < Real::MAX + { + let diag = lhs[(i, i)]; + lhs.row_mut(i).fill(0.0); + lhs[(i, i)] = diag; + } + } + } + } + + // 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")] + return lhs.try_inverse().expect("Singular system."); + #[cfg(feature = "dim3")] + return lhs.cholesky().expect("Singular system.").inverse(); + } + + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + joint: &GenericJoint, + flipped: bool, + ) -> Self { + let (anchor1, anchor2) = if flipped { + ( + rb1.position * joint.local_anchor2, + rb2.position * joint.local_anchor1, + ) + } else { + ( + rb1.position * joint.local_anchor1, + rb2.position * joint.local_anchor2, + ) + }; + + let r1 = anchor1.translation.vector - rb1.world_com.coords; + let im2 = rb2.effective_inv_mass; + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let r2 = anchor2.translation.vector - rb2.world_com.coords; + + let inv_lhs = Self::compute_mass_matrix(joint, im2, ii2, r2, true); + + 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, + ); + + let impulse = (joint.impulse * params.warmstart_coeff) + .inf(&joint.max_positive_impulse) + .sup(&joint.max_negative_impulse); + + GenericVelocityGroundConstraint { + joint_id, + mj_lambda2: rb2.active_set_offset, + im2, + ii2, + ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + impulse, + max_positive_impulse: joint.max_positive_impulse, + max_negative_impulse: joint.max_negative_impulse, + inv_lhs, + r2, + rhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = self.impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = self.impulse.fixed_rows::<U3>(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<Real>]) { + 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 dvel = Vector6::new( + dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, + ) + self.rhs; + + let new_impulse = (self.impulse + self.inv_lhs * dvel) + .sup(&self.max_negative_impulse) + .inf(&self.max_positive_impulse); + let effective_impulse = new_impulse - self.impulse; + self.impulse = new_impulse; + + let lin_impulse = effective_impulse.fixed_rows::<Dim>(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = effective_impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = effective_impulse.fixed_rows::<U3>(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::GenericJoint(fixed) = &mut joint.params { + fixed.impulse = self.impulse; + } + } +} diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs new file mode 100644 index 0000000..8a6be8c --- /dev/null +++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs @@ -0,0 +1,472 @@ +use simba::simd::SimdValue; + +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, +}; +use crate::math::{ + AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, Real, SimdReal, SpacialVector, + Vector, SIMD_WIDTH, +}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +#[cfg(feature = "dim3")] +use na::{Cholesky, Matrix6, Vector6, U3}; +#[cfg(feature = "dim2")] +use { + na::{Matrix3, Vector3}, + parry::utils::SdpMatrix3, +}; + +#[derive(Debug)] +pub(crate) struct WGenericVelocityConstraint { + mj_lambda1: [usize; SIMD_WIDTH], + mj_lambda2: [usize; SIMD_WIDTH], + + joint_id: [JointIndex; SIMD_WIDTH], + + impulse: SpacialVector<SimdReal>, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix6<SimdReal>, // FIXME: replace by Cholesky. + #[cfg(feature = "dim3")] + rhs: Vector6<SimdReal>, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix3<SimdReal>, + #[cfg(feature = "dim2")] + rhs: Vector3<SimdReal>, + + im1: SimdReal, + im2: SimdReal, + + ii1: AngularInertia<SimdReal>, + ii2: AngularInertia<SimdReal>, + + ii1_sqrt: AngularInertia<SimdReal>, + ii2_sqrt: AngularInertia<SimdReal>, + + r1: Vector<SimdReal>, + r2: Vector<SimdReal>, +} + +impl WGenericVelocityConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&GenericJoint; 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::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); + let ii1_sqrt = AngularInertia::<SimdReal>::from( + array![|ii| rbs1[ii].effective_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::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::<SimdReal>::from( + array![|ii| rbs2[ii].effective_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::<U3, U3>(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::<U3, U3>(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, + ); + + WGenericVelocityConstraint { + joint_id, + mj_lambda1, + mj_lambda2, + im1, + im2, + ii1, + ii2, + ii1_sqrt, + ii2_sqrt, + impulse: impulse * SimdReal::splat(params.warmstart_coeff), + inv_lhs, + r1, + r2, + rhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) { + 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::<Dim>(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = self.impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned(); + + mj_lambda1.linear += lin_impulse * self.im1; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_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<Real>]) { + let mut mj_lambda1: DeltaVel<SimdReal> = 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<SimdReal> = 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 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) + |