From 5b80c4efbf93ad1294c9d3d390d8c8f090681b0e Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Wed, 10 Feb 2021 11:56:51 +0100 Subject: Start experimenting with a generic joint implementation for joint drives. --- .../generic_position_constraint.rs | 171 +++++++++++++++++++++ 1 file changed, 171 insertions(+) create mode 100644 src/dynamics/solver/joint_constraint/generic_position_constraint.rs (limited to 'src/dynamics/solver/joint_constraint/generic_position_constraint.rs') 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, + local_anchor2: Isometry, + local_com1: Point, + local_com2: Point, + im1: Real, + im2: Real, + ii1: AngularInertia, + ii2: AngularInertia, + + joint: GenericJoint, + + lin_impulse: Cell>, + ang_impulse: Cell>, +} + +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]) { + 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, + local_anchor2: Isometry, + local_com2: Point, + im2: Real, + ii2: AngularInertia, + 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]) { + 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; + } +} -- cgit From cc80e40067d100d0f519c9a20abb020726dd8514 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Thu, 11 Feb 2021 18:52:07 +0100 Subject: More experiments with the way the generic joint is stabilized. --- .../generic_position_constraint.rs | 57 +++++++++++++++++----- 1 file changed, 44 insertions(+), 13 deletions(-) (limited to 'src/dynamics/solver/joint_constraint/generic_position_constraint.rs') diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs index f5138ea..1e80311 100644 --- a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs @@ -1,4 +1,5 @@ use super::{GenericVelocityConstraint, GenericVelocityGroundConstraint}; +use crate::dynamics::solver::DeltaVel; use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody}; use crate::math::{ AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector, @@ -22,9 +23,6 @@ pub(crate) struct GenericPositionConstraint { ii2: AngularInertia, joint: GenericJoint, - - lin_impulse: Cell>, - ang_impulse: Cell>, } impl GenericPositionConstraint { @@ -58,7 +56,11 @@ impl GenericPositionConstraint { 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 delta_pos = Isometry::from_parts( + anchor2.translation * anchor1.translation.inverse(), + anchor2.rotation * anchor1.rotation.inverse(), + ); + let mass_matrix = GenericVelocityConstraint::compute_mass_matrix( &self.joint, self.im1, @@ -70,11 +72,15 @@ impl GenericPositionConstraint { 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 lin_dpos = delta_pos.translation.vector; + let ang_dpos = delta_pos.rotation.scaled_axis(); + let dpos = Vector6::new( + lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, ); + let err = dpos + - dpos + .sup(&self.joint.min_position) + .inf(&self.joint.max_position); let impulse = mass_matrix * err; let lin_impulse = impulse.xyz(); let ang_impulse = Vector3::new(impulse[3], impulse[4], impulse[5]); @@ -94,6 +100,15 @@ impl GenericPositionConstraint { positions[self.position1 as usize] = position1; positions[self.position2 as usize] = position2; } + + pub fn solve2( + &self, + params: &IntegrationParameters, + positions: &mut [Isometry], + dpos: &mut [DeltaVel], + ) { + return; + } } #[derive(Debug)] @@ -142,7 +157,10 @@ impl GenericPositionGroundConstraint { 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 delta_pos = Isometry::from_parts( + anchor2.translation * self.anchor1.translation.inverse(), + anchor2.rotation * self.anchor1.rotation.inverse(), + ); let mass_matrix = GenericVelocityGroundConstraint::compute_mass_matrix( &self.joint, self.im2, @@ -151,11 +169,15 @@ impl GenericPositionGroundConstraint { 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 lin_dpos = delta_pos.translation.vector; + let ang_dpos = delta_pos.rotation.scaled_axis(); + let dpos = Vector6::new( + lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, ); + let err = dpos + - dpos + .sup(&self.joint.min_position) + .inf(&self.joint.max_position); let impulse = mass_matrix * err; let lin_impulse = impulse.xyz(); let ang_impulse = Vector3::new(impulse[3], impulse[4], impulse[5]); @@ -168,4 +190,13 @@ impl GenericPositionGroundConstraint { positions[self.position2 as usize] = position2; } + + pub fn solve2( + &self, + params: &IntegrationParameters, + positions: &mut [Isometry], + dpos: &mut [DeltaVel], + ) { + return; + } } -- cgit From d9b6198fa0c7d933960030b7cff15cdaecb504e6 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Fri, 12 Feb 2021 16:00:57 +0100 Subject: Various generic joint fixes. --- .../generic_position_constraint.rs | 90 +--------------------- 1 file changed, 2 insertions(+), 88 deletions(-) (limited to 'src/dynamics/solver/joint_constraint/generic_position_constraint.rs') diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs index 1e80311..9d74bf3 100644 --- a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs @@ -48,57 +48,7 @@ impl GenericPositionConstraint { } 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 r1 = Point::from(anchor1.translation.vector) - position1 * self.local_com1; - let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2; - - let delta_pos = Isometry::from_parts( - anchor2.translation * anchor1.translation.inverse(), - anchor2.rotation * anchor1.rotation.inverse(), - ); - - let mass_matrix = GenericVelocityConstraint::compute_mass_matrix( - &self.joint, - self.im1, - self.im2, - self.ii1, - self.ii2, - r1, - r2, - false, - ); - - let lin_dpos = delta_pos.translation.vector; - let ang_dpos = delta_pos.rotation.scaled_axis(); - let dpos = Vector6::new( - lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, - ); - let err = dpos - - dpos - .sup(&self.joint.min_position) - .inf(&self.joint.max_position); - 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; + return; } pub fn solve2( @@ -152,43 +102,7 @@ impl GenericPositionGroundConstraint { } pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { - 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 = Isometry::from_parts( - anchor2.translation * self.anchor1.translation.inverse(), - anchor2.rotation * self.anchor1.rotation.inverse(), - ); - let mass_matrix = GenericVelocityGroundConstraint::compute_mass_matrix( - &self.joint, - self.im2, - self.ii2, - r2, - false, - ); - - let lin_dpos = delta_pos.translation.vector; - let ang_dpos = delta_pos.rotation.scaled_axis(); - let dpos = Vector6::new( - lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, - ); - let err = dpos - - dpos - .sup(&self.joint.min_position) - .inf(&self.joint.max_position); - 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; + return; } pub fn solve2( -- cgit From de39a41faa2ea722042231f91b5d579efdf1a02d Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 15 Feb 2021 11:20:09 +0100 Subject: Implement non-linear position stabilization for the generic constraint. --- .../generic_position_constraint.rs | 270 +++++++++++++++++++-- 1 file changed, 251 insertions(+), 19 deletions(-) (limited to 'src/dynamics/solver/joint_constraint/generic_position_constraint.rs') diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs index 9d74bf3..1502403 100644 --- a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs @@ -5,7 +5,7 @@ use crate::math::{ AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector, DIM, }; -use crate::utils::{WAngularInertia, WCross}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Vector3, Vector6}; // FIXME: review this code for the case where the center of masses are not the origin. @@ -48,16 +48,136 @@ impl GenericPositionConstraint { } pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { - return; - } + let mut position1 = positions[self.position1]; + let mut position2 = positions[self.position2]; + let mut params = *params; + params.joint_erp = 0.8; + + /* + * + * Translation part. + * + */ + { + let anchor1 = position1 * self.joint.local_anchor1; + let anchor2 = position2 * self.joint.local_anchor2; + let basis = anchor1.rotation; + let r1 = Point::from(anchor1.translation.vector) - position1 * self.local_com1; + let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2; + let mut min_pos_impulse = self.joint.min_pos_impulse.xyz(); + let mut max_pos_impulse = self.joint.max_pos_impulse.xyz(); + + let pos_rhs = GenericVelocityConstraint::compute_lin_position_error( + &anchor1, + &anchor2, + &basis, + &self.joint.min_position.xyz(), + &self.joint.max_position.xyz(), + ) * params.joint_erp; + + for i in 0..3 { + if pos_rhs[i] < 0.0 { + min_pos_impulse[i] = -Real::MAX; + } + if pos_rhs[i] > 0.0 { + max_pos_impulse[i] = Real::MAX; + } + } + + let rotmat = basis.to_rotation_matrix().into_inner(); + let rmat1 = r1.gcross_matrix() * rotmat; + let rmat2 = r2.gcross_matrix() * rotmat; + + // Will be actually inverted right after. + // TODO: we should keep the SdpMatrix3 type. + let delassus = (self.ii1.quadform(&rmat1).add_diagonal(self.im1) + + self.ii2.quadform(&rmat2).add_diagonal(self.im2)) + .into_matrix(); + + let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse, + &max_pos_impulse, + &mut Vector3::zeros(), + delassus, + ); + + let local_impulse = (inv_delassus * pos_rhs) + .inf(&max_pos_impulse) + .sup(&min_pos_impulse); + let impulse = basis * local_impulse; + + let rot1 = self.ii1.transform_vector(r1.gcross(impulse)); + let rot2 = self.ii2.transform_vector(r2.gcross(impulse)); + + position1.translation.vector += self.im1 * impulse; + position1.rotation = position1.rotation.append_axisangle_linearized(&rot1); + position2.translation.vector -= self.im2 * impulse; + position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2); + } + + /* + * + * Rotation part + * + */ + { + let anchor1 = position1 * self.joint.local_anchor1; + let anchor2 = position2 * self.joint.local_anchor2; + let basis = anchor1.rotation; + let mut min_pos_impulse = self + .joint + .min_pos_impulse + .fixed_rows::(DIM) + .into_owned(); + let mut max_pos_impulse = self + .joint + .max_pos_impulse + .fixed_rows::(DIM) + .into_owned(); + + let pos_rhs = GenericVelocityConstraint::compute_ang_position_error( + &anchor1, + &anchor2, + &basis, + &self.joint.min_position.fixed_rows::(DIM).into_owned(), + &self.joint.max_position.fixed_rows::(DIM).into_owned(), + ) * params.joint_erp; + + for i in 0..3 { + if pos_rhs[i] < 0.0 { + min_pos_impulse[i] = -Real::MAX; + } + if pos_rhs[i] > 0.0 { + max_pos_impulse[i] = Real::MAX; + } + } + + // Will be actually inverted right after. + // TODO: we should keep the SdpMatrix3 type. + let rotmat = basis.to_rotation_matrix().into_inner(); + let delassus = (self.ii1.quadform(&rotmat) + self.ii2.quadform(&rotmat)).into_matrix(); + + let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse, + &max_pos_impulse, + &mut Vector3::zeros(), + delassus, + ); + + let local_impulse = (inv_delassus * pos_rhs) + .inf(&max_pos_impulse) + .sup(&min_pos_impulse); + let impulse = basis * local_impulse; + + let rot1 = self.ii1.transform_vector(impulse); + let rot2 = self.ii2.transform_vector(impulse); + + position1.rotation = position1.rotation.append_axisangle_linearized(&rot1); + position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2); + } - pub fn solve2( - &self, - params: &IntegrationParameters, - positions: &mut [Isometry], - dpos: &mut [DeltaVel], - ) { - return; + positions[self.position1] = position1; + positions[self.position2] = position2; } } @@ -102,15 +222,127 @@ impl GenericPositionGroundConstraint { } pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { - return; - } + let mut position2 = positions[self.position2]; + let mut params = *params; + params.joint_erp = 0.8; + + /* + * + * Translation part. + * + */ + { + let anchor1 = self.anchor1; + let anchor2 = position2 * self.local_anchor2; + let basis = anchor1.rotation; + let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2; + let mut min_pos_impulse = self.joint.min_pos_impulse.xyz(); + let mut max_pos_impulse = self.joint.max_pos_impulse.xyz(); + + let pos_rhs = GenericVelocityConstraint::compute_lin_position_error( + &anchor1, + &anchor2, + &basis, + &self.joint.min_position.xyz(), + &self.joint.max_position.xyz(), + ) * params.joint_erp; + + for i in 0..3 { + if pos_rhs[i] < 0.0 { + min_pos_impulse[i] = -Real::MAX; + } + if pos_rhs[i] > 0.0 { + max_pos_impulse[i] = Real::MAX; + } + } + + let rotmat = basis.to_rotation_matrix().into_inner(); + let rmat2 = r2.gcross_matrix() * rotmat; + + // Will be actually inverted right after. + // TODO: we should keep the SdpMatrix3 type. + let delassus = self + .ii2 + .quadform(&rmat2) + .add_diagonal(self.im2) + .into_matrix(); + + let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse, + &max_pos_impulse, + &mut Vector3::zeros(), + delassus, + ); + + let local_impulse = (inv_delassus * pos_rhs) + .inf(&max_pos_impulse) + .sup(&min_pos_impulse); + let impulse = basis * local_impulse; + + let rot2 = self.ii2.transform_vector(r2.gcross(impulse)); + + position2.translation.vector -= self.im2 * impulse; + position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2); + } + + /* + * + * Rotation part + * + */ + { + let anchor1 = self.anchor1; + let anchor2 = position2 * self.local_anchor2; + let basis = anchor1.rotation; + let mut min_pos_impulse = self + .joint + .min_pos_impulse + .fixed_rows::(DIM) + .into_owned(); + let mut max_pos_impulse = self + .joint + .max_pos_impulse + .fixed_rows::(DIM) + .into_owned(); + + let pos_rhs = GenericVelocityConstraint::compute_ang_position_error( + &anchor1, + &anchor2, + &basis, + &self.joint.min_position.fixed_rows::(DIM).into_owned(), + &self.joint.max_position.fixed_rows::(DIM).into_owned(), + ) * params.joint_erp; + + for i in 0..3 { + if pos_rhs[i] < 0.0 { + min_pos_impulse[i] = -Real::MAX; + } + if pos_rhs[i] > 0.0 { + max_pos_impulse[i] = Real::MAX; + } + } + + // Will be actually inverted right after. + // TODO: we should keep the SdpMatrix3 type. + let rotmat = basis.to_rotation_matrix().into_inner(); + let delassus = self.ii2.quadform(&rotmat).into_matrix(); + + let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse, + &max_pos_impulse, + &mut Vector3::zeros(), + delassus, + ); + + let local_impulse = (inv_delassus * pos_rhs) + .inf(&max_pos_impulse) + .sup(&min_pos_impulse); + let impulse = basis * local_impulse; + let rot2 = self.ii2.transform_vector(impulse); + + position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2); + } - pub fn solve2( - &self, - params: &IntegrationParameters, - positions: &mut [Isometry], - dpos: &mut [DeltaVel], - ) { - return; + positions[self.position2] = position2; } } -- cgit From 4f8f8017f447fa5137fa5ed5fc3820faebb5c1bc Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 15 Feb 2021 16:44:55 +0100 Subject: Properly writeback the generic constrainst impulse when it comes from a revolute joint. --- src/dynamics/solver/joint_constraint/generic_position_constraint.rs | 2 -- 1 file changed, 2 deletions(-) (limited to 'src/dynamics/solver/joint_constraint/generic_position_constraint.rs') diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs index 1502403..be12258 100644 --- a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs @@ -152,7 +152,6 @@ impl GenericPositionConstraint { } } - // Will be actually inverted right after. // TODO: we should keep the SdpMatrix3 type. let rotmat = basis.to_rotation_matrix().into_inner(); let delassus = (self.ii1.quadform(&rotmat) + self.ii2.quadform(&rotmat)).into_matrix(); @@ -259,7 +258,6 @@ impl GenericPositionGroundConstraint { let rotmat = basis.to_rotation_matrix().into_inner(); let rmat2 = r2.gcross_matrix() * rotmat; - // Will be actually inverted right after. // TODO: we should keep the SdpMatrix3 type. let delassus = self .ii2 -- cgit