707 lines
25 KiB
Rust
707 lines
25 KiB
Rust
use crate::dynamics::solver::DeltaVel;
|
|
use crate::dynamics::{
|
|
GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
|
|
};
|
|
use crate::math::{AngularInertia, Dim, Isometry, Real, Rotation, SpacialVector, Vector, DIM};
|
|
use crate::na::UnitQuaternion;
|
|
use crate::parry::math::{AngDim, SpatialVector};
|
|
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
|
|
#[cfg(feature = "dim3")]
|
|
use na::{Matrix3, Matrix6, Vector3, Vector6, U3};
|
|
#[cfg(feature = "dim2")]
|
|
use na::{Matrix3, Vector3};
|
|
|
|
#[derive(Debug)]
|
|
pub(crate) struct GenericVelocityConstraint {
|
|
mj_lambda1: usize,
|
|
mj_lambda2: usize,
|
|
|
|
joint_id: JointIndex,
|
|
|
|
inv_lhs_lin: Matrix3<Real>,
|
|
inv_lhs_ang: Matrix3<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>,
|
|
basis1: Rotation<Real>,
|
|
basis2: Rotation<Real>,
|
|
dependant_set_mask: SpacialVector<Real>,
|
|
|
|
vel: GenericConstraintPart,
|
|
}
|
|
|
|
impl GenericVelocityConstraint {
|
|
pub fn compute_velocity_error(
|
|
min_velocity: &SpatialVector<Real>,
|
|
max_velocity: &SpatialVector<Real>,
|
|
r1: &Vector<Real>,
|
|
r2: &Vector<Real>,
|
|
basis1: &Rotation<Real>,
|
|
basis2: &Rotation<Real>,
|
|
rb1: &RigidBody,
|
|
rb2: &RigidBody,
|
|
) -> SpatialVector<Real> {
|
|
let lin_dvel = basis1.inverse_transform_vector(&(-rb1.linvel() - rb1.angvel().gcross(*r1)))
|
|
+ basis2.inverse_transform_vector(&(rb2.linvel() + rb2.angvel().gcross(*r2)));
|
|
let ang_dvel = basis1.inverse_transform_vector(&-rb1.angvel)
|
|
+ basis2.inverse_transform_vector(&rb2.angvel);
|
|
|
|
let min_linvel = min_velocity.xyz();
|
|
let min_angvel = min_velocity.fixed_rows::<AngDim>(DIM).into_owned();
|
|
let max_linvel = max_velocity.xyz();
|
|
let max_angvel = max_velocity.fixed_rows::<AngDim>(DIM).into_owned();
|
|
let lin_rhs = lin_dvel - lin_dvel.sup(&min_linvel).inf(&max_linvel);
|
|
let ang_rhs = ang_dvel - ang_dvel.sup(&min_angvel).inf(&max_angvel);
|
|
|
|
#[cfg(feature = "dim2")]
|
|
return Vector3::new(lin_rhs.x, lin_rhs.y, ang_rhs);
|
|
|
|
#[cfg(feature = "dim3")]
|
|
return Vector6::new(
|
|
lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y, ang_rhs.z,
|
|
);
|
|
}
|
|
|
|
pub fn compute_lin_position_error(
|
|
anchor1: &Isometry<Real>,
|
|
anchor2: &Isometry<Real>,
|
|
basis: &Rotation<Real>,
|
|
min_position: &Vector<Real>,
|
|
max_position: &Vector<Real>,
|
|
) -> Vector<Real> {
|
|
let dpos = anchor2.translation.vector - anchor1.translation.vector;
|
|
let local_dpos = basis.inverse_transform_vector(&dpos);
|
|
local_dpos - local_dpos.sup(min_position).inf(max_position)
|
|
}
|
|
|
|
pub fn compute_ang_position_error(
|
|
anchor1: &Isometry<Real>,
|
|
anchor2: &Isometry<Real>,
|
|
basis: &Rotation<Real>,
|
|
min_position: &Vector<Real>,
|
|
max_position: &Vector<Real>,
|
|
) -> Vector<Real> {
|
|
let drot = anchor2.rotation * anchor1.rotation.inverse();
|
|
let local_drot_diff = basis.inverse_transform_vector(&drot.scaled_axis());
|
|
|
|
let error = local_drot_diff - local_drot_diff.sup(min_position).inf(max_position);
|
|
let error_code =
|
|
(error[0] == 0.0) as usize + (error[1] == 0.0) as usize + (error[2] == 0.0) as usize;
|
|
|
|
if error_code == 1 {
|
|
// Align two axes.
|
|
let constrained_axis = error.iamin();
|
|
let axis1 = anchor1
|
|
.rotation
|
|
.to_rotation_matrix()
|
|
.into_inner()
|
|
.column(constrained_axis)
|
|
.into_owned();
|
|
let axis2 = anchor2
|
|
.rotation
|
|
.to_rotation_matrix()
|
|
.into_inner()
|
|
.column(constrained_axis)
|
|
.into_owned();
|
|
let rot_cross = UnitQuaternion::rotation_between(&axis1, &axis2)
|
|
.unwrap_or(UnitQuaternion::identity());
|
|
let local_drot_diff = basis.inverse_transform_vector(&rot_cross.scaled_axis());
|
|
local_drot_diff - local_drot_diff.sup(min_position).inf(max_position)
|
|
} else {
|
|
error
|
|
}
|
|
}
|
|
|
|
pub fn invert_partial_delassus_matrix(
|
|
min_impulse: &Vector<Real>,
|
|
max_impulse: &Vector<Real>,
|
|
dependant_set_mask: &mut Vector<Real>,
|
|
mut delassus: na::Matrix3<Real>,
|
|
) -> na::Matrix3<Real> {
|
|
// Adjust the Delassus 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.
|
|
for i in 0..3 {
|
|
if min_impulse[i] > -Real::MAX || max_impulse[i] < Real::MAX {
|
|
let diag = delassus[(i, i)];
|
|
delassus.column_mut(i).fill(0.0);
|
|
delassus.row_mut(i).fill(0.0);
|
|
delassus[(i, i)] = diag;
|
|
dependant_set_mask[i] = 0.0;
|
|
} else {
|
|
dependant_set_mask[i] = 1.0;
|
|
}
|
|
}
|
|
|
|
delassus.try_inverse().unwrap()
|
|
}
|
|
|
|
pub fn compute_position_error(
|
|
joint: &GenericJoint,
|
|
anchor1: &Isometry<Real>,
|
|
anchor2: &Isometry<Real>,
|
|
basis: &Rotation<Real>,
|
|
) -> SpatialVector<Real> {
|
|
let delta_pos = Isometry::from_parts(
|
|
anchor2.translation * anchor1.translation.inverse(),
|
|
anchor2.rotation * anchor1.rotation.inverse(),
|
|
);
|
|
let lin_dpos = basis.inverse_transform_vector(&delta_pos.translation.vector);
|
|
let ang_dpos = basis.inverse_transform_vector(&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 error = dpos - dpos.sup(&joint.min_position).inf(&joint.max_position);
|
|
let error_code =
|
|
(error[3] == 0.0) as usize + (error[4] == 0.0) as usize + (error[5] == 0.0) as usize;
|
|
|
|
match error_code {
|
|
1 => {
|
|
let constrained_axis = error.rows(3, 3).iamin();
|
|
let axis1 = anchor1
|
|
.rotation
|
|
.to_rotation_matrix()
|
|
.into_inner()
|
|
.column(constrained_axis)
|
|
.into_owned();
|
|
let axis2 = anchor2
|
|
.rotation
|
|
.to_rotation_matrix()
|
|
.into_inner()
|
|
.column(constrained_axis)
|
|
.into_owned();
|
|
let rot_cross = UnitQuaternion::rotation_between(&axis1, &axis2)
|
|
.unwrap_or(UnitQuaternion::identity());
|
|
let ang_dpos = basis.inverse_transform_vector(&rot_cross.scaled_axis());
|
|
let dpos = Vector6::new(
|
|
lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z,
|
|
);
|
|
|
|
dpos - dpos.sup(&joint.min_position).inf(&joint.max_position)
|
|
}
|
|
_ => error,
|
|
}
|
|
}
|
|
|
|
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 basis1 = anchor1.rotation;
|
|
let basis2 = anchor2.rotation;
|
|
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 mut min_impulse = joint.min_impulse;
|
|
let mut max_impulse = joint.max_impulse;
|
|
let mut min_pos_impulse = joint.min_pos_impulse;
|
|
let mut max_pos_impulse = joint.max_pos_impulse;
|
|
let mut min_velocity = joint.min_velocity;
|
|
let mut max_velocity = joint.max_velocity;
|
|
let mut dependant_set_mask = SpacialVector::repeat(1.0);
|
|
|
|
let pos_rhs = Self::compute_position_error(joint, &anchor1, &anchor2, &basis1)
|
|
* params.inv_dt()
|
|
* params.joint_erp;
|
|
|
|
for i in 0..6 {
|
|
if pos_rhs[i] < 0.0 {
|
|
min_impulse[i] = -Real::MAX;
|
|
min_pos_impulse[i] = -Real::MAX;
|
|
min_velocity[i] = 0.0;
|
|
}
|
|
if pos_rhs[i] > 0.0 {
|
|
max_impulse[i] = Real::MAX;
|
|
max_pos_impulse[i] = Real::MAX;
|
|
max_velocity[i] = 0.0;
|
|
}
|
|
}
|
|
|
|
let rhs = Self::compute_velocity_error(
|
|
&min_velocity,
|
|
&max_velocity,
|
|
&r1,
|
|
&r2,
|
|
&basis1,
|
|
&basis2,
|
|
rb1,
|
|
rb2,
|
|
);
|
|
let rhs_lin = rhs.xyz();
|
|
let rhs_ang = rhs.fixed_rows::<Dim>(DIM).into();
|
|
|
|
// TODO: we should keep the SdpMatrix3 type.
|
|
let rotmat1 = basis1.to_rotation_matrix().into_inner();
|
|
let rotmat2 = basis2.to_rotation_matrix().into_inner();
|
|
let rmat1 = r1.gcross_matrix() * rotmat1;
|
|
let rmat2 = r2.gcross_matrix() * rotmat2;
|
|
let delassus00 = (ii1.quadform(&rmat1).add_diagonal(im1)
|
|
+ ii2.quadform(&rmat2).add_diagonal(im2))
|
|
.into_matrix();
|
|
let delassus11 = (ii1.quadform(&rotmat1) + ii2.quadform(&rotmat2)).into_matrix();
|
|
|
|
let inv_lhs_lin = GenericVelocityConstraint::invert_partial_delassus_matrix(
|
|
&min_pos_impulse.xyz(),
|
|
&max_pos_impulse.xyz(),
|
|
&mut Vector3::zeros(),
|
|
delassus00,
|
|
);
|
|
let inv_lhs_ang = GenericVelocityConstraint::invert_partial_delassus_matrix(
|
|
&min_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(),
|
|
&max_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(),
|
|
&mut Vector3::zeros(),
|
|
delassus11,
|
|
);
|
|
|
|
let impulse = (joint.impulse * params.warmstart_coeff)
|
|
.inf(&max_impulse)
|
|
.sup(&min_impulse);
|
|
|
|
let lin_impulse = impulse.xyz();
|
|
let ang_impulse = impulse.fixed_rows::<Dim>(DIM).into_owned();
|
|
let min_lin_impulse = min_impulse.xyz();
|
|
let min_ang_impulse = min_impulse.fixed_rows::<Dim>(DIM).into_owned();
|
|
let max_lin_impulse = max_impulse.xyz();
|
|
let max_ang_impulse = max_impulse.fixed_rows::<Dim>(DIM).into_owned();
|
|
|
|
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,
|
|
inv_lhs_lin,
|
|
inv_lhs_ang,
|
|
r1,
|
|
r2,
|
|
basis1,
|
|
basis2,
|
|
dependant_set_mask,
|
|
vel: GenericConstraintPart {
|
|
lin_impulse,
|
|
ang_impulse,
|
|
min_lin_impulse,
|
|
min_ang_impulse,
|
|
max_lin_impulse,
|
|
max_ang_impulse,
|
|
rhs_lin,
|
|
rhs_ang,
|
|
},
|
|
}
|
|
}
|
|
|
|
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_impulse1 = self.basis1 * self.vel.lin_impulse;
|
|
let ang_impulse1 = self.basis1 * self.vel.ang_impulse;
|
|
let lin_impulse2 = self.basis2 * self.vel.lin_impulse;
|
|
let ang_impulse2 = self.basis2 * self.vel.ang_impulse;
|
|
|
|
mj_lambda1.linear += self.im1 * lin_impulse1;
|
|
mj_lambda1.angular += self
|
|
.ii1_sqrt
|
|
.transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
|
|
|
|
mj_lambda2.linear -= self.im2 * lin_impulse2;
|
|
mj_lambda2.angular -= self
|
|
.ii2_sqrt
|
|
.transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
|
|
|
|
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 (lin_imp, ang_imp) = self.vel.solve(self, &mut mj_lambda1, &mut mj_lambda2);
|
|
self.vel.lin_impulse = lin_imp;
|
|
self.vel.ang_impulse = ang_imp;
|
|
|
|
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;
|
|
match &mut joint.params {
|
|
JointParams::GenericJoint(out) => {
|
|
out.impulse[0] = self.vel.lin_impulse.x;
|
|
out.impulse[1] = self.vel.lin_impulse.y;
|
|
out.impulse[2] = self.vel.lin_impulse.z;
|
|
out.impulse[3] = self.vel.ang_impulse.x;
|
|
out.impulse[4] = self.vel.ang_impulse.y;
|
|
out.impulse[5] = self.vel.ang_impulse.z;
|
|
}
|
|
JointParams::RevoluteJoint(out) => {
|
|
out.impulse[0] = self.vel.lin_impulse.x;
|
|
out.impulse[1] = self.vel.lin_impulse.y;
|
|
out.impulse[2] = self.vel.lin_impulse.z;
|
|
out.motor_impulse = self.vel.ang_impulse.x;
|
|
out.impulse[3] = self.vel.ang_impulse.y;
|
|
out.impulse[4] = self.vel.ang_impulse.z;
|
|
}
|
|
_ => unimplemented!(),
|
|
}
|
|
}
|
|
}
|
|
|
|
#[derive(Debug)]
|
|
pub(crate) struct GenericVelocityGroundConstraint {
|
|
mj_lambda2: usize,
|
|
|
|
joint_id: JointIndex,
|
|
|
|
inv_lhs_lin: Matrix3<Real>,
|
|
inv_lhs_ang: Matrix3<Real>,
|
|
|
|
im2: Real,
|
|
ii2: AngularInertia<Real>,
|
|
ii2_sqrt: AngularInertia<Real>,
|
|
r2: Vector<Real>,
|
|
basis: Rotation<Real>,
|
|
|
|
dependant_set_mask: SpacialVector<Real>,
|
|
|
|
vel: GenericConstraintPart,
|
|
}
|
|
|
|
impl GenericVelocityGroundConstraint {
|
|
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 basis = anchor2.rotation;
|
|
let im2 = rb2.effective_inv_mass;
|
|
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 mut min_impulse = joint.min_impulse;
|
|
let mut max_impulse = joint.max_impulse;
|
|
let mut min_pos_impulse = joint.min_pos_impulse;
|
|
let mut max_pos_impulse = joint.max_pos_impulse;
|
|
let mut min_velocity = joint.min_velocity;
|
|
let mut max_velocity = joint.max_velocity;
|
|
let mut dependant_set_mask = SpacialVector::repeat(1.0);
|
|
|
|
let pos_rhs =
|
|
GenericVelocityConstraint::compute_position_error(joint, &anchor1, &anchor2, &basis)
|
|
* params.inv_dt()
|
|
* params.joint_erp;
|
|
|
|
for i in 0..6 {
|
|
if pos_rhs[i] < 0.0 {
|
|
min_impulse[i] = -Real::MAX;
|
|
min_pos_impulse[i] = -Real::MAX;
|
|
min_velocity[i] = 0.0;
|
|
}
|
|
if pos_rhs[i] > 0.0 {
|
|
max_impulse[i] = Real::MAX;
|
|
max_pos_impulse[i] = Real::MAX;
|
|
max_velocity[i] = 0.0;
|
|
}
|
|
}
|
|
|
|
let rhs = GenericVelocityConstraint::compute_velocity_error(
|
|
&min_velocity,
|
|
&max_velocity,
|
|
&r1,
|
|
&r2,
|
|
&basis,
|
|
&basis,
|
|
rb1,
|
|
rb2,
|
|
);
|
|
let rhs_lin = rhs.xyz();
|
|
let rhs_ang = rhs.fixed_rows::<Dim>(DIM).into_owned();
|
|
|
|
// TODO: we should keep the SdpMatrix3 type.
|
|
let rotmat = basis.to_rotation_matrix().into_inner();
|
|
let rmat2 = r2.gcross_matrix() * rotmat;
|
|
let delassus00 = ii2.quadform(&rmat2).add_diagonal(im2).into_matrix();
|
|
let delassus11 = ii2.quadform(&rotmat).into_matrix();
|
|
|
|
let inv_lhs_lin = GenericVelocityConstraint::invert_partial_delassus_matrix(
|
|
&min_pos_impulse.xyz(),
|
|
&max_pos_impulse.xyz(),
|
|
&mut Vector3::zeros(),
|
|
delassus00,
|
|
);
|
|
let inv_lhs_ang = GenericVelocityConstraint::invert_partial_delassus_matrix(
|
|
&min_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(),
|
|
&max_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(),
|
|
&mut Vector3::zeros(),
|
|
delassus11,
|
|
);
|
|
|
|
let impulse = (joint.impulse * params.warmstart_coeff)
|
|
.inf(&max_impulse)
|
|
.sup(&min_impulse);
|
|
|
|
let lin_impulse = impulse.xyz();
|
|
let ang_impulse = impulse.fixed_rows::<Dim>(DIM).into_owned();
|
|
let min_lin_impulse = min_impulse.xyz();
|
|
let min_ang_impulse = min_impulse.fixed_rows::<Dim>(DIM).into_owned();
|
|
let max_lin_impulse = max_impulse.xyz();
|
|
let max_ang_impulse = max_impulse.fixed_rows::<Dim>(DIM).into_owned();
|
|
|
|
GenericVelocityGroundConstraint {
|
|
joint_id,
|
|
mj_lambda2: rb2.active_set_offset,
|
|
im2,
|
|
ii2,
|
|
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
|
|
inv_lhs_lin,
|
|
inv_lhs_ang,
|
|
r2,
|
|
basis,
|
|
vel: GenericConstraintPart {
|
|
lin_impulse,
|
|
ang_impulse,
|
|
min_lin_impulse,
|
|
min_ang_impulse,
|
|
max_lin_impulse,
|
|
max_ang_impulse,
|
|
rhs_lin,
|
|
rhs_ang,
|
|
},
|
|
dependant_set_mask,
|
|
}
|
|
}
|
|
|
|
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
|
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
|
|
|
let lin_impulse = self.basis * self.vel.lin_impulse;
|
|
#[cfg(feature = "dim2")]
|
|
let ang_impulse = self.basis * self.vel.impulse[2];
|
|
#[cfg(feature = "dim3")]
|
|
let ang_impulse = self.basis * self.vel.ang_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_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 (lin_imp, ang_imp) = self.vel.solve_ground(self, &mut mj_lambda2);
|
|
self.vel.lin_impulse = lin_imp;
|
|
self.vel.ang_impulse = ang_imp;
|
|
|
|
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
|
}
|
|
|
|
// TODO: 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;
|
|
match &mut joint.params {
|
|
JointParams::GenericJoint(out) => {
|
|
out.impulse[0] = self.vel.lin_impulse.x;
|
|
out.impulse[1] = self.vel.lin_impulse.y;
|
|
out.impulse[2] = self.vel.lin_impulse.z;
|
|
out.impulse[3] = self.vel.ang_impulse.x;
|
|
out.impulse[4] = self.vel.ang_impulse.y;
|
|
out.impulse[5] = self.vel.ang_impulse.z;
|
|
}
|
|
JointParams::RevoluteJoint(out) => {
|
|
out.impulse[0] = self.vel.lin_impulse.x;
|
|
out.impulse[1] = self.vel.lin_impulse.y;
|
|
out.impulse[2] = self.vel.lin_impulse.z;
|
|
out.motor_impulse = self.vel.ang_impulse.x;
|
|
out.impulse[3] = self.vel.ang_impulse.y;
|
|
out.impulse[4] = self.vel.ang_impulse.z;
|
|
}
|
|
_ => unimplemented!(),
|
|
}
|
|
}
|
|
}
|
|
|
|
#[derive(Debug)]
|
|
struct GenericConstraintPart {
|
|
lin_impulse: Vector3<Real>,
|
|
max_lin_impulse: Vector3<Real>,
|
|
min_lin_impulse: Vector3<Real>,
|
|
rhs_lin: Vector3<Real>,
|
|
|
|
ang_impulse: Vector3<Real>,
|
|
max_ang_impulse: Vector3<Real>,
|
|
min_ang_impulse: Vector3<Real>,
|
|
rhs_ang: Vector3<Real>,
|
|
}
|
|
|
|
impl GenericConstraintPart {
|
|
fn solve(
|
|
&self,
|
|
parent: &GenericVelocityConstraint,
|
|
mj_lambda1: &mut DeltaVel<Real>,
|
|
mj_lambda2: &mut DeltaVel<Real>,
|
|
) -> (Vector3<Real>, Vector3<Real>) {
|
|
let new_lin_impulse;
|
|
let new_ang_impulse;
|
|
|
|
/*
|
|
*
|
|
* Solve translations.
|
|
*
|
|
*/
|
|
{
|
|
let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
|
let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
|
|
|
let dvel = parent
|
|
.basis1
|
|
.inverse_transform_vector(&(-mj_lambda1.linear - ang_vel1.gcross(parent.r1)))
|
|
+ parent
|
|
.basis2
|
|
.inverse_transform_vector(&(mj_lambda2.linear + ang_vel2.gcross(parent.r2)));
|
|
|
|
let err = dvel + self.rhs_lin;
|
|
|
|
new_lin_impulse = (self.lin_impulse + parent.inv_lhs_lin * err)
|
|
.sup(&self.min_lin_impulse)
|
|
.inf(&self.max_lin_impulse);
|
|
let effective_impulse1 = parent.basis1 * (new_lin_impulse - self.lin_impulse);
|
|
let effective_impulse2 = parent.basis2 * (new_lin_impulse - self.lin_impulse);
|
|
|
|
mj_lambda1.linear += parent.im1 * effective_impulse1;
|
|
mj_lambda1.angular += parent
|
|
.ii1_sqrt
|
|
.transform_vector(parent.r1.gcross(effective_impulse1));
|
|
|
|
mj_lambda2.linear -= parent.im2 * effective_impulse2;
|
|
mj_lambda2.angular -= parent
|
|
.ii2_sqrt
|
|
.transform_vector(parent.r2.gcross(effective_impulse2));
|
|
}
|
|
|
|
/*
|
|
*
|
|
* Solve rotations.
|
|
*
|
|
*/
|
|
{
|
|
let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
|
let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
|
|
|
let dvel = parent.basis2.inverse_transform_vector(&ang_vel2)
|
|
- parent.basis1.inverse_transform_vector(&ang_vel1);
|
|
let err = dvel + self.rhs_ang;
|
|
|
|
new_ang_impulse = (self.ang_impulse + parent.inv_lhs_ang * err)
|
|
.sup(&self.min_ang_impulse)
|
|
.inf(&self.max_ang_impulse);
|
|
let effective_impulse1 = parent.basis1 * (new_ang_impulse - self.ang_impulse);
|
|
let effective_impulse2 = parent.basis2 * (new_ang_impulse - self.ang_impulse);
|
|
|
|
mj_lambda1.angular += parent.ii1_sqrt.transform_vector(effective_impulse1);
|
|
mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse2);
|
|
}
|
|
|
|
(new_lin_impulse, new_ang_impulse)
|
|
}
|
|
|
|
fn solve_ground(
|
|
&self,
|
|
parent: &GenericVelocityGroundConstraint,
|
|
mj_lambda2: &mut DeltaVel<Real>,
|
|
) -> (Vector3<Real>, Vector3<Real>) {
|
|
let new_lin_impulse;
|
|
let new_ang_impulse;
|
|
|
|
/*
|
|
*
|
|
* Solve translations.
|
|
*
|
|
*/
|
|
{
|
|
let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
|
|
|
let dvel = parent
|
|
.basis
|
|
.inverse_transform_vector(&(mj_lambda2.linear + ang_vel2.gcross(parent.r2)));
|
|
|
|
let err = dvel + self.rhs_lin;
|
|
|
|
new_lin_impulse = (self.lin_impulse + parent.inv_lhs_lin * err)
|
|
.sup(&self.min_lin_impulse)
|
|
.inf(&self.max_lin_impulse);
|
|
let effective_impulse = parent.basis * (new_lin_impulse - self.lin_impulse);
|
|
|
|
mj_lambda2.linear -= parent.im2 * effective_impulse;
|
|
mj_lambda2.angular -= parent
|
|
.ii2_sqrt
|
|
.transform_vector(parent.r2.gcross(effective_impulse));
|
|
}
|
|
|
|
/*
|
|
*
|
|
* Solve rotations.
|
|
*
|
|
*/
|
|
{
|
|
let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
|
|
|
let dvel = parent.basis.inverse_transform_vector(&ang_vel2);
|
|
let err = dvel + self.rhs_ang;
|
|
|
|
new_ang_impulse = (self.ang_impulse + parent.inv_lhs_ang * err)
|
|
.sup(&self.min_ang_impulse)
|
|
.inf(&self.max_ang_impulse);
|
|
let effective_impulse = parent.basis * (new_ang_impulse - self.ang_impulse);
|
|
|
|
mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse);
|
|
}
|
|
|
|
(new_lin_impulse, new_ang_impulse)
|
|
}
|
|
}
|