Implement multibody joints and the new solver
This commit is contained in:
Sébastien Crozet
@@ -1,266 +0,0 @@
|
||||
use crate::dynamics::{
|
||||
BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
|
||||
};
|
||||
#[cfg(feature = "dim2")]
|
||||
use crate::math::SdpMatrix;
|
||||
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, UnitVector};
|
||||
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct BallPositionConstraint {
|
||||
position1: usize,
|
||||
position2: usize,
|
||||
|
||||
local_com1: Point<Real>,
|
||||
local_com2: Point<Real>,
|
||||
|
||||
im1: Real,
|
||||
im2: Real,
|
||||
|
||||
ii1: AngularInertia<Real>,
|
||||
ii2: AngularInertia<Real>,
|
||||
inv_ii1_ii2: AngularInertia<Real>,
|
||||
|
||||
local_anchor1: Point<Real>,
|
||||
local_anchor2: Point<Real>,
|
||||
|
||||
limits_enabled: bool,
|
||||
limits_angle: Real,
|
||||
limits_local_axis1: UnitVector<Real>,
|
||||
limits_local_axis2: UnitVector<Real>,
|
||||
}
|
||||
|
||||
impl BallPositionConstraint {
|
||||
pub fn from_params(
|
||||
rb1: (&RigidBodyMassProps, &RigidBodyIds),
|
||||
rb2: (&RigidBodyMassProps, &RigidBodyIds),
|
||||
cparams: &BallJoint,
|
||||
) -> Self {
|
||||
let (mprops1, ids1) = rb1;
|
||||
let (mprops2, ids2) = rb2;
|
||||
|
||||
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
|
||||
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
|
||||
let inv_ii1_ii2 = (ii1 + ii2).inverse();
|
||||
|
||||
Self {
|
||||
local_com1: mprops1.local_mprops.local_com,
|
||||
local_com2: mprops2.local_mprops.local_com,
|
||||
im1: mprops1.effective_inv_mass,
|
||||
im2: mprops2.effective_inv_mass,
|
||||
ii1,
|
||||
ii2,
|
||||
inv_ii1_ii2,
|
||||
local_anchor1: cparams.local_anchor1,
|
||||
local_anchor2: cparams.local_anchor2,
|
||||
position1: ids1.active_set_offset,
|
||||
position2: ids2.active_set_offset,
|
||||
limits_enabled: cparams.limits_enabled,
|
||||
limits_angle: cparams.limits_angle,
|
||||
limits_local_axis1: cparams.limits_local_axis1,
|
||||
limits_local_axis2: cparams.limits_local_axis2,
|
||||
}
|
||||
}
|
||||
|
||||
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 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;
|
||||
|
||||
/*
|
||||
* Limits part.
|
||||
*/
|
||||
if self.limits_enabled {
|
||||
let axis1 = position1 * self.limits_local_axis1;
|
||||
let axis2 = position2 * self.limits_local_axis2;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let axis_angle = Rotation::rotation_between_axis(&axis2, &axis1).axis_angle();
|
||||
#[cfg(feature = "dim3")]
|
||||
let axis_angle =
|
||||
Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle());
|
||||
|
||||
// TODO: handle the case where dot(axis1, axis2) = -1.0
|
||||
if let Some((axis, angle)) = axis_angle {
|
||||
if angle >= self.limits_angle {
|
||||
#[cfg(feature = "dim2")]
|
||||
let axis = axis[0];
|
||||
#[cfg(feature = "dim3")]
|
||||
let axis = axis.into_inner();
|
||||
let ang_error = angle - self.limits_angle;
|
||||
let ang_impulse = self
|
||||
.inv_ii1_ii2
|
||||
.transform_vector(axis * ang_error * 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
positions[self.position1 as usize] = position1;
|
||||
positions[self.position2 as usize] = position2;
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct BallPositionGroundConstraint {
|
||||
position2: usize,
|
||||
anchor1: Point<Real>,
|
||||
im2: Real,
|
||||
ii2: AngularInertia<Real>,
|
||||
local_anchor2: Point<Real>,
|
||||
local_com2: Point<Real>,
|
||||
|
||||
limits_enabled: bool,
|
||||
limits_angle: Real,
|
||||
limits_axis1: UnitVector<Real>,
|
||||
limits_local_axis2: UnitVector<Real>,
|
||||
}
|
||||
|
||||
impl BallPositionGroundConstraint {
|
||||
pub fn from_params(
|
||||
rb1: &RigidBodyPosition,
|
||||
rb2: (&RigidBodyMassProps, &RigidBodyIds),
|
||||
cparams: &BallJoint,
|
||||
flipped: bool,
|
||||
) -> Self {
|
||||
let poss1 = rb1;
|
||||
let (mprops2, ids2) = rb2;
|
||||
|
||||
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: poss1.next_position * cparams.local_anchor2,
|
||||
im2: mprops2.effective_inv_mass,
|
||||
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
|
||||
local_anchor2: cparams.local_anchor1,
|
||||
position2: ids2.active_set_offset,
|
||||
local_com2: mprops2.local_mprops.local_com,
|
||||
limits_enabled: cparams.limits_enabled,
|
||||
limits_angle: cparams.limits_angle,
|
||||
limits_axis1: poss1.next_position * cparams.limits_local_axis2,
|
||||
limits_local_axis2: cparams.limits_local_axis1,
|
||||
}
|
||||
} else {
|
||||
Self {
|
||||
anchor1: poss1.next_position * cparams.local_anchor1,
|
||||
im2: mprops2.effective_inv_mass,
|
||||
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
|
||||
local_anchor2: cparams.local_anchor2,
|
||||
position2: ids2.active_set_offset,
|
||||
local_com2: mprops2.local_mprops.local_com,
|
||||
limits_enabled: cparams.limits_enabled,
|
||||
limits_angle: cparams.limits_angle,
|
||||
limits_axis1: poss1.next_position * cparams.limits_local_axis1,
|
||||
limits_local_axis2: cparams.limits_local_axis2,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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 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;
|
||||
|
||||
/*
|
||||
* Limits part.
|
||||
*/
|
||||
if self.limits_enabled {
|
||||
let axis2 = position2 * self.limits_local_axis2;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let axis_angle =
|
||||
Rotation::rotation_between_axis(&axis2, &self.limits_axis1).axis_angle();
|
||||
#[cfg(feature = "dim3")]
|
||||
let axis_angle = Rotation::rotation_between_axis(&axis2, &self.limits_axis1)
|
||||
.and_then(|r| r.axis_angle());
|
||||
|
||||
// TODO: handle the case where dot(axis1, axis2) = -1.0
|
||||
if let Some((axis, angle)) = axis_angle {
|
||||
if angle >= self.limits_angle {
|
||||
#[cfg(feature = "dim2")]
|
||||
let axis = axis[0];
|
||||
#[cfg(feature = "dim3")]
|
||||
let axis = axis.into_inner();
|
||||
let ang_error = angle - self.limits_angle;
|
||||
let ang_correction = axis * ang_error * params.joint_erp;
|
||||
position2.rotation = Rotation::new(ang_correction) * position2.rotation;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
positions[self.position2 as usize] = position2;
|
||||
}
|
||||
}
|
||||
@@ -1,216 +0,0 @@
|
||||
use crate::dynamics::{
|
||||
BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
|
||||
};
|
||||
#[cfg(feature = "dim2")]
|
||||
use crate::math::SdpMatrix;
|
||||
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, SimdReal, 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<SimdReal>,
|
||||
local_com2: Point<SimdReal>,
|
||||
|
||||
im1: SimdReal,
|
||||
im2: SimdReal,
|
||||
|
||||
ii1: AngularInertia<SimdReal>,
|
||||
ii2: AngularInertia<SimdReal>,
|
||||
|
||||
local_anchor1: Point<SimdReal>,
|
||||
local_anchor2: Point<SimdReal>,
|
||||
}
|
||||
|
||||
impl WBallPositionConstraint {
|
||||
pub fn from_params(
|
||||
rbs1: (
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
rbs2: (
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
cparams: [&BallJoint; SIMD_WIDTH],
|
||||
) -> Self {
|
||||
let (mprops1, ids1) = rbs1;
|
||||
let (mprops2, ids2) = rbs2;
|
||||
|
||||
let local_com1 = Point::from(gather![|ii| mprops1[ii].local_mprops.local_com]);
|
||||
let local_com2 = Point::from(gather![|ii| mprops2[ii].local_mprops.local_com]);
|
||||
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
|
||||
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
|
||||
let ii1 = AngularInertia::<SimdReal>::from(gather![
|
||||
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
|
||||
])
|
||||
.squared();
|
||||
let ii2 = AngularInertia::<SimdReal>::from(gather![
|
||||
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
|
||||
])
|
||||
.squared();
|
||||
let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]);
|
||||
let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]);
|
||||
let position1 = gather![|ii| ids1[ii].active_set_offset];
|
||||
let position2 = gather![|ii| ids2[ii].active_set_offset];
|
||||
|
||||
Self {
|
||||
local_com1,
|
||||
local_com2,
|
||||
im1,
|
||||
im2,
|
||||
ii1,
|
||||
ii2,
|
||||
local_anchor1,
|
||||
local_anchor2,
|
||||
position1,
|
||||
position2,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
|
||||
let mut position1 = Isometry::from(gather![|ii| positions[self.position1[ii]]]);
|
||||
let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]);
|
||||
|
||||
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 * SimdReal::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<SimdReal>,
|
||||
im2: SimdReal,
|
||||
ii2: AngularInertia<SimdReal>,
|
||||
local_anchor2: Point<SimdReal>,
|
||||
local_com2: Point<SimdReal>,
|
||||
}
|
||||
|
||||
impl WBallPositionGroundConstraint {
|
||||
pub fn from_params(
|
||||
rbs1: [&RigidBodyPosition; SIMD_WIDTH],
|
||||
rbs2: (
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
cparams: [&BallJoint; SIMD_WIDTH],
|
||||
flipped: [bool; SIMD_WIDTH],
|
||||
) -> Self {
|
||||
let poss1 = rbs1;
|
||||
let (mprops2, ids2) = rbs2;
|
||||
|
||||
let position1 = Isometry::from(gather![|ii| poss1[ii].next_position]);
|
||||
let anchor1 = position1
|
||||
* Point::from(gather![|ii| if flipped[ii] {
|
||||
cparams[ii].local_anchor2
|
||||
} else {
|
||||
cparams[ii].local_anchor1
|
||||
}]);
|
||||
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
|
||||
let ii2 = AngularInertia::<SimdReal>::from(gather![
|
||||
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
|
||||
])
|
||||
.squared();
|
||||
let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
|
||||
cparams[ii].local_anchor1
|
||||
} else {
|
||||
cparams[ii].local_anchor2
|
||||
}]);
|
||||
let position2 = gather![|ii| ids2[ii].active_set_offset];
|
||||
let local_com2 = Point::from(gather![|ii| mprops2[ii].local_mprops.local_com]);
|
||||
|
||||
Self {
|
||||
anchor1,
|
||||
im2,
|
||||
ii2,
|
||||
local_anchor2,
|
||||
position2,
|
||||
local_com2,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
|
||||
let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]);
|
||||
|
||||
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 * SimdReal::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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,660 +0,0 @@
|
||||
use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{
|
||||
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
|
||||
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
|
||||
};
|
||||
use crate::math::{AngVector, AngularInertia, Real, Rotation, SdpMatrix, Vector};
|
||||
use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot};
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct BallVelocityConstraint {
|
||||
mj_lambda1: usize,
|
||||
mj_lambda2: usize,
|
||||
|
||||
joint_id: JointIndex,
|
||||
|
||||
rhs: Vector<Real>,
|
||||
impulse: Vector<Real>,
|
||||
|
||||
r1: Vector<Real>,
|
||||
r2: Vector<Real>,
|
||||
|
||||
inv_lhs: SdpMatrix<Real>,
|
||||
|
||||
motor_rhs: AngVector<Real>,
|
||||
motor_impulse: AngVector<Real>,
|
||||
motor_inv_lhs: Option<AngularInertia<Real>>,
|
||||
motor_max_impulse: Real,
|
||||
|
||||
limits_active: bool,
|
||||
limits_rhs: Real,
|
||||
limits_inv_lhs: Real,
|
||||
limits_impulse: Real,
|
||||
limits_axis: AngVector<Real>,
|
||||
|
||||
im1: Real,
|
||||
im2: Real,
|
||||
|
||||
ii1_sqrt: AngularInertia<Real>,
|
||||
ii2_sqrt: AngularInertia<Real>,
|
||||
}
|
||||
|
||||
impl BallVelocityConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
rb1: (
|
||||
&RigidBodyPosition,
|
||||
&RigidBodyVelocity,
|
||||
&RigidBodyMassProps,
|
||||
&RigidBodyIds,
|
||||
),
|
||||
rb2: (
|
||||
&RigidBodyPosition,
|
||||
&RigidBodyVelocity,
|
||||
&RigidBodyMassProps,
|
||||
&RigidBodyIds,
|
||||
),
|
||||
joint: &BallJoint,
|
||||
) -> Self {
|
||||
let (rb_pos1, rb_vels1, rb_mprops1, rb_ids1) = rb1;
|
||||
let (rb_pos2, rb_vels2, rb_mprops2, rb_ids2) = rb2;
|
||||
|
||||
let anchor_world1 = rb_pos1.position * joint.local_anchor1;
|
||||
let anchor_world2 = rb_pos2.position * joint.local_anchor2;
|
||||
let anchor1 = anchor_world1 - rb_mprops1.world_com;
|
||||
let anchor2 = anchor_world2 - rb_mprops2.world_com;
|
||||
|
||||
let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(anchor1);
|
||||
let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(anchor2);
|
||||
let im1 = rb_mprops1.effective_inv_mass;
|
||||
let im2 = rb_mprops2.effective_inv_mass;
|
||||
|
||||
let rhs = (vel2 - vel1) * params.velocity_solve_fraction
|
||||
+ (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
|
||||
|
||||
let lhs;
|
||||
let cmat1 = anchor1.gcross_matrix();
|
||||
let cmat2 = anchor2.gcross_matrix();
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
lhs = rb_mprops2
|
||||
.effective_world_inv_inertia_sqrt
|
||||
.squared()
|
||||
.quadform(&cmat2)
|
||||
.add_diagonal(im2)
|
||||
+ rb_mprops1
|
||||
.effective_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 = rb_mprops1.effective_world_inv_inertia_sqrt.squared();
|
||||
let ii2 = rb_mprops2.effective_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 inv_lhs = lhs.inverse_unchecked();
|
||||
|
||||
/*
|
||||
* Motor part.
|
||||
*/
|
||||
let mut motor_rhs = na::zero();
|
||||
let mut motor_inv_lhs = None;
|
||||
let motor_max_impulse = joint.motor_max_impulse;
|
||||
|
||||
if motor_max_impulse > 0.0 {
|
||||
let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
|
||||
params.dt,
|
||||
joint.motor_stiffness,
|
||||
joint.motor_damping,
|
||||
);
|
||||
|
||||
if stiffness != 0.0 {
|
||||
let dpos = rb_pos2.position.rotation
|
||||
* (rb_pos1.position.rotation * joint.motor_target_pos).inverse();
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
motor_rhs += dpos.angle() * stiffness;
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
motor_rhs += dpos.scaled_axis() * stiffness;
|
||||
}
|
||||
}
|
||||
|
||||
if damping != 0.0 {
|
||||
let curr_vel = rb_vels2.angvel - rb_vels1.angvel;
|
||||
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
if stiffness != 0.0 || damping != 0.0 {
|
||||
motor_inv_lhs = if keep_lhs {
|
||||
let ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared();
|
||||
let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
|
||||
Some(gamma / (ii1 + ii2))
|
||||
} else {
|
||||
Some(gamma)
|
||||
};
|
||||
motor_rhs /= gamma;
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
if stiffness != 0.0 || damping != 0.0 {
|
||||
motor_inv_lhs = if keep_lhs {
|
||||
let ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared();
|
||||
let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
|
||||
Some((ii1 + ii2).inverse_unchecked() * gamma)
|
||||
} else {
|
||||
Some(SdpMatrix::diagonal(gamma))
|
||||
};
|
||||
motor_rhs /= gamma;
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
|
||||
* params.warmstart_coeff;
|
||||
#[cfg(feature = "dim3")]
|
||||
let motor_impulse =
|
||||
joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff;
|
||||
|
||||
/*
|
||||
* Setup the limits constraint.
|
||||
*/
|
||||
let mut limits_active = false;
|
||||
let mut limits_rhs = 0.0;
|
||||
let mut limits_inv_lhs = 0.0;
|
||||
let mut limits_impulse = 0.0;
|
||||
let mut limits_axis = na::zero();
|
||||
|
||||
if joint.limits_enabled {
|
||||
let axis1 = rb_pos1.position * joint.limits_local_axis1;
|
||||
let axis2 = rb_pos2.position * joint.limits_local_axis2;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let axis_angle = Rotation::rotation_between_axis(&axis2, &axis1).axis_angle();
|
||||
#[cfg(feature = "dim3")]
|
||||
let axis_angle =
|
||||
Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle());
|
||||
|
||||
// TODO: handle the case where dot(axis1, axis2) = -1.0
|
||||
if let Some((axis, angle)) = axis_angle {
|
||||
if angle >= joint.limits_angle {
|
||||
#[cfg(feature = "dim2")]
|
||||
let axis = axis[0];
|
||||
#[cfg(feature = "dim3")]
|
||||
let axis = axis.into_inner();
|
||||
|
||||
limits_active = true;
|
||||
limits_rhs = (rb_vels2.angvel.gdot(axis) - rb_vels1.angvel.gdot(axis))
|
||||
* params.velocity_solve_fraction;
|
||||
|
||||
limits_rhs += (angle - joint.limits_angle) * params.velocity_based_erp_inv_dt();
|
||||
|
||||
let ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared();
|
||||
let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
|
||||
limits_inv_lhs = crate::utils::inv(
|
||||
axis.gdot(ii2.transform_vector(axis))
|
||||
+ axis.gdot(ii1.transform_vector(axis)),
|
||||
);
|
||||
|
||||
limits_impulse = joint.limits_impulse * params.warmstart_coeff;
|
||||
limits_axis = axis;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
BallVelocityConstraint {
|
||||
joint_id,
|
||||
mj_lambda1: rb_ids1.active_set_offset,
|
||||
mj_lambda2: rb_ids2.active_set_offset,
|
||||
im1,
|
||||
im2,
|
||||
impulse: joint.impulse * params.warmstart_coeff,
|
||||
r1: anchor1,
|
||||
r2: anchor2,
|
||||
rhs,
|
||||
inv_lhs,
|
||||
motor_rhs,
|
||||
motor_impulse,
|
||||
motor_inv_lhs,
|
||||
motor_max_impulse: joint.motor_max_impulse,
|
||||
ii1_sqrt: rb_mprops1.effective_world_inv_inertia_sqrt,
|
||||
ii2_sqrt: rb_mprops2.effective_world_inv_inertia_sqrt,
|
||||
limits_active,
|
||||
limits_axis,
|
||||
limits_rhs,
|
||||
limits_inv_lhs,
|
||||
limits_impulse,
|
||||
}
|
||||
}
|
||||
|
||||
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];
|
||||
|
||||
mj_lambda1.linear += self.im1 * self.impulse;
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(self.r1.gcross(self.impulse) + self.motor_impulse);
|
||||
mj_lambda2.linear -= self.im2 * self.impulse;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(self.r2.gcross(self.impulse) + self.motor_impulse);
|
||||
|
||||
/*
|
||||
* Warmstart limits.
|
||||
*/
|
||||
if self.limits_active {
|
||||
let limit_impulse1 = -self.limits_axis * self.limits_impulse;
|
||||
let limit_impulse2 = self.limits_axis * self.limits_impulse;
|
||||
mj_lambda1.angular += self.ii1_sqrt.transform_vector(limit_impulse1);
|
||||
mj_lambda2.angular += self.ii2_sqrt.transform_vector(limit_impulse2);
|
||||
}
|
||||
|
||||
mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
|
||||
let vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
|
||||
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.ii1_sqrt.transform_vector(self.r1.gcross(impulse));
|
||||
|
||||
mj_lambda2.linear -= self.im2 * impulse;
|
||||
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
|
||||
}
|
||||
|
||||
fn solve_limits(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
|
||||
if self.limits_active {
|
||||
let limits_torquedir1 = -self.limits_axis;
|
||||
let limits_torquedir2 = self.limits_axis;
|
||||
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let ang_dvel = limits_torquedir1.gdot(ang_vel1)
|
||||
+ limits_torquedir2.gdot(ang_vel2)
|
||||
+ self.limits_rhs;
|
||||
let new_impulse = (self.limits_impulse - ang_dvel * self.limits_inv_lhs).max(0.0);
|
||||
let dimpulse = new_impulse - self.limits_impulse;
|
||||
self.limits_impulse = new_impulse;
|
||||
|
||||
let ang_impulse1 = limits_torquedir1 * dimpulse;
|
||||
let ang_impulse2 = limits_torquedir2 * dimpulse;
|
||||
|
||||
mj_lambda1.angular += self.ii1_sqrt.transform_vector(ang_impulse1);
|
||||
mj_lambda2.angular += self.ii2_sqrt.transform_vector(ang_impulse2);
|
||||
}
|
||||
}
|
||||
|
||||
fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
|
||||
if let Some(motor_inv_lhs) = &self.motor_inv_lhs {
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let dangvel = (ang_vel2 - ang_vel1) + self.motor_rhs;
|
||||
|
||||
let new_impulse = self.motor_impulse + motor_inv_lhs.transform_vector(dangvel);
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let clamped_impulse =
|
||||
na::clamp(new_impulse, -self.motor_max_impulse, self.motor_max_impulse);
|
||||
#[cfg(feature = "dim3")]
|
||||
let clamped_impulse = new_impulse.cap_magnitude(self.motor_max_impulse);
|
||||
|
||||
let effective_impulse = clamped_impulse - self.motor_impulse;
|
||||
self.motor_impulse = clamped_impulse;
|
||||
|
||||
mj_lambda1.angular += self.ii1_sqrt.transform_vector(effective_impulse);
|
||||
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse);
|
||||
}
|
||||
}
|
||||
|
||||
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];
|
||||
|
||||
self.solve_limits(&mut mj_lambda1, &mut mj_lambda2);
|
||||
self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
|
||||
self.solve_motors(&mut mj_lambda1, &mut mj_lambda2);
|
||||
|
||||
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;
|
||||
ball.motor_impulse = self.motor_impulse;
|
||||
ball.limits_impulse = self.limits_impulse;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct BallVelocityGroundConstraint {
|
||||
mj_lambda2: usize,
|
||||
joint_id: JointIndex,
|
||||
r2: Vector<Real>,
|
||||
|
||||
rhs: Vector<Real>,
|
||||
impulse: Vector<Real>,
|
||||
inv_lhs: SdpMatrix<Real>,
|
||||
|
||||
motor_rhs: AngVector<Real>,
|
||||
motor_impulse: AngVector<Real>,
|
||||
motor_inv_lhs: Option<AngularInertia<Real>>,
|
||||
motor_max_impulse: Real,
|
||||
|
||||
limits_active: bool,
|
||||
limits_rhs: Real,
|
||||
limits_inv_lhs: Real,
|
||||
limits_impulse: Real,
|
||||
limits_axis: AngVector<Real>,
|
||||
|
||||
im2: Real,
|
||||
ii2_sqrt: AngularInertia<Real>,
|
||||
}
|
||||
|
||||
impl BallVelocityGroundConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
|
||||
rb2: (
|
||||
&RigidBodyPosition,
|
||||
&RigidBodyVelocity,
|
||||
&RigidBodyMassProps,
|
||||
&RigidBodyIds,
|
||||
),
|
||||
joint: &BallJoint,
|
||||
flipped: bool,
|
||||
) -> Self {
|
||||
let (rb_pos1, rb_vels1, rb_mprops1) = rb1;
|
||||
let (rb_pos2, rb_vels2, rb_mprops2, rb_ids2) = rb2;
|
||||
|
||||
let (anchor_world1, anchor_world2) = if flipped {
|
||||
(
|
||||
rb_pos1.position * joint.local_anchor2,
|
||||
rb_pos2.position * joint.local_anchor1,
|
||||
)
|
||||
} else {
|
||||
(
|
||||
rb_pos1.position * joint.local_anchor1,
|
||||
rb_pos2.position * joint.local_anchor2,
|
||||
)
|
||||
};
|
||||
|
||||
let anchor1 = anchor_world1 - rb_mprops1.world_com;
|
||||
let anchor2 = anchor_world2 - rb_mprops2.world_com;
|
||||
|
||||
let im2 = rb_mprops2.effective_inv_mass;
|
||||
let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(anchor1);
|
||||
let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(anchor2);
|
||||
|
||||
let rhs = (vel2 - vel1) * params.velocity_solve_fraction
|
||||
+ (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
|
||||
|
||||
let cmat2 = anchor2.gcross_matrix();
|
||||
|
||||
let lhs;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
lhs = rb_mprops2
|
||||
.effective_world_inv_inertia_sqrt
|
||||
.squared()
|
||||
.quadform(&cmat2)
|
||||
.add_diagonal(im2);
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let ii2 = rb_mprops2.effective_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();
|
||||
|
||||
/*
|
||||
* Motor part.
|
||||
*/
|
||||
let mut motor_rhs = na::zero();
|
||||
let mut motor_inv_lhs = None;
|
||||
let motor_max_impulse = joint.motor_max_impulse;
|
||||
|
||||
if motor_max_impulse > 0.0 {
|
||||
let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
|
||||
params.dt,
|
||||
joint.motor_stiffness,
|
||||
joint.motor_damping,
|
||||
);
|
||||
|
||||
if stiffness != 0.0 {
|
||||
let dpos = rb_pos2.position.rotation
|
||||
* (rb_pos1.position.rotation * joint.motor_target_pos).inverse();
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
motor_rhs += dpos.angle() * stiffness;
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
motor_rhs += dpos.scaled_axis() * stiffness;
|
||||
}
|
||||
}
|
||||
|
||||
if damping != 0.0 {
|
||||
let curr_vel = rb_vels2.angvel - rb_vels1.angvel;
|
||||
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
if stiffness != 0.0 || damping != 0.0 {
|
||||
motor_inv_lhs = if keep_lhs {
|
||||
let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
|
||||
Some(gamma / ii2)
|
||||
} else {
|
||||
Some(gamma)
|
||||
};
|
||||
motor_rhs /= gamma;
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
if stiffness != 0.0 || damping != 0.0 {
|
||||
motor_inv_lhs = if keep_lhs {
|
||||
let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
|
||||
Some(ii2.inverse_unchecked() * gamma)
|
||||
} else {
|
||||
Some(SdpMatrix::diagonal(gamma))
|
||||
};
|
||||
motor_rhs /= gamma;
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
|
||||
* params.warmstart_coeff;
|
||||
#[cfg(feature = "dim3")]
|
||||
let motor_impulse =
|
||||
joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff;
|
||||
|
||||
/*
|
||||
* Setup the limits constraint.
|
||||
*/
|
||||
let mut limits_active = false;
|
||||
let mut limits_rhs = 0.0;
|
||||
let mut limits_inv_lhs = 0.0;
|
||||
let mut limits_impulse = 0.0;
|
||||
let mut limits_axis = na::zero();
|
||||
|
||||
if joint.limits_enabled {
|
||||
let (axis1, axis2) = if flipped {
|
||||
(
|
||||
rb_pos1.position * joint.limits_local_axis2,
|
||||
rb_pos2.position * joint.limits_local_axis1,
|
||||
)
|
||||
} else {
|
||||
(
|
||||
rb_pos1.position * joint.limits_local_axis1,
|
||||
rb_pos2.position * joint.limits_local_axis2,
|
||||
)
|
||||
};
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let axis_angle = Rotation::rotation_between_axis(&axis2, &axis1).axis_angle();
|
||||
#[cfg(feature = "dim3")]
|
||||
let axis_angle =
|
||||
Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle());
|
||||
|
||||
// TODO: handle the case where dot(axis1, axis2) = -1.0
|
||||
if let Some((axis, angle)) = axis_angle {
|
||||
if angle >= joint.limits_angle {
|
||||
#[cfg(feature = "dim2")]
|
||||
let axis = axis[0];
|
||||
#[cfg(feature = "dim3")]
|
||||
let axis = axis.into_inner();
|
||||
|
||||
limits_active = true;
|
||||
limits_rhs = (rb_vels2.angvel.gdot(axis) - rb_vels1.angvel.gdot(axis))
|
||||
* params.velocity_solve_fraction;
|
||||
|
||||
limits_rhs += (angle - joint.limits_angle) * params.velocity_based_erp_inv_dt();
|
||||
|
||||
let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
|
||||
limits_inv_lhs = crate::utils::inv(axis.gdot(ii2.transform_vector(axis)));
|
||||
limits_impulse = joint.limits_impulse * params.warmstart_coeff;
|
||||
limits_axis = axis;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
BallVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2: rb_ids2.active_set_offset,
|
||||
im2,
|
||||
impulse: joint.impulse * params.warmstart_coeff,
|
||||
r2: anchor2,
|
||||
rhs,
|
||||
inv_lhs,
|
||||
motor_rhs,
|
||||
motor_impulse,
|
||||
motor_inv_lhs,
|
||||
motor_max_impulse: joint.motor_max_impulse,
|
||||
ii2_sqrt: rb_mprops2.effective_world_inv_inertia_sqrt,
|
||||
limits_active,
|
||||
limits_axis,
|
||||
limits_rhs,
|
||||
limits_inv_lhs,
|
||||
limits_impulse,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
mj_lambda2.linear -= self.im2 * self.impulse;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(self.r2.gcross(self.impulse) + self.motor_impulse);
|
||||
|
||||
/*
|
||||
* Warmstart limits.
|
||||
*/
|
||||
if self.limits_active {
|
||||
let limit_impulse2 = self.limits_axis * self.limits_impulse;
|
||||
mj_lambda2.angular += self.ii2_sqrt.transform_vector(limit_impulse2);
|
||||
}
|
||||
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
|
||||
let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let vel2 = mj_lambda2.linear + angvel.gcross(self.r2);
|
||||
let dvel = vel2 + self.rhs;
|
||||
|
||||
let impulse = self.inv_lhs * dvel;
|
||||
self.impulse += impulse;
|
||||
|
||||
mj_lambda2.linear -= self.im2 * impulse;
|
||||
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
|
||||
}
|
||||
|
||||
fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
|
||||
if self.limits_active {
|
||||
let limits_torquedir2 = self.limits_axis;
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let ang_dvel = limits_torquedir2.gdot(ang_vel2) + self.limits_rhs;
|
||||
let new_impulse = (self.limits_impulse - ang_dvel * self.limits_inv_lhs).max(0.0);
|
||||
let dimpulse = new_impulse - self.limits_impulse;
|
||||
self.limits_impulse = new_impulse;
|
||||
|
||||
let ang_impulse2 = limits_torquedir2 * dimpulse;
|
||||
mj_lambda2.angular += self.ii2_sqrt.transform_vector(ang_impulse2);
|
||||
}
|
||||
}
|
||||
|
||||
fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
|
||||
if let Some(motor_inv_lhs) = &self.motor_inv_lhs {
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let dangvel = ang_vel2 + self.motor_rhs;
|
||||
let new_impulse = self.motor_impulse + motor_inv_lhs.transform_vector(dangvel);
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let clamped_impulse =
|
||||
na::clamp(new_impulse, -self.motor_max_impulse, self.motor_max_impulse);
|
||||
#[cfg(feature = "dim3")]
|
||||
let clamped_impulse = new_impulse.cap_magnitude(self.motor_max_impulse);
|
||||
|
||||
let effective_impulse = clamped_impulse - self.motor_impulse;
|
||||
self.motor_impulse = clamped_impulse;
|
||||
|
||||
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
self.solve_limits(&mut mj_lambda2);
|
||||
self.solve_dofs(&mut mj_lambda2);
|
||||
self.solve_motors(&mut mj_lambda2);
|
||||
|
||||
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;
|
||||
ball.motor_impulse = self.motor_impulse;
|
||||
ball.limits_impulse = self.limits_impulse;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,359 +0,0 @@
|
||||
use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{
|
||||
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
|
||||
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
|
||||
};
|
||||
use crate::math::{
|
||||
AngVector, AngularInertia, Isometry, Point, Real, SdpMatrix, SimdReal, 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<SimdReal>,
|
||||
pub(crate) impulse: Vector<SimdReal>,
|
||||
|
||||
r1: Vector<SimdReal>,
|
||||
r2: Vector<SimdReal>,
|
||||
|
||||
inv_lhs: SdpMatrix<SimdReal>,
|
||||
|
||||
im1: SimdReal,
|
||||
im2: SimdReal,
|
||||
|
||||
ii1_sqrt: AngularInertia<SimdReal>,
|
||||
ii2_sqrt: AngularInertia<SimdReal>,
|
||||
}
|
||||
|
||||
impl WBallVelocityConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
rbs1: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
rbs2: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
cparams: [&BallJoint; SIMD_WIDTH],
|
||||
) -> Self {
|
||||
let (poss1, vels1, mprops1, ids1) = rbs1;
|
||||
let (poss2, vels2, mprops2, ids2) = rbs2;
|
||||
|
||||
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
|
||||
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
|
||||
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
|
||||
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
|
||||
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
|
||||
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|
||||
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
|
||||
]);
|
||||
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
|
||||
|
||||
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
|
||||
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
|
||||
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
|
||||
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
|
||||
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
|
||||
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|
||||
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
|
||||
]);
|
||||
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
|
||||
|
||||
let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]);
|
||||
let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]);
|
||||
let impulse = Vector::from(gather![|ii| cparams[ii].impulse]);
|
||||
|
||||
let anchor_world1 = position1 * local_anchor1;
|
||||
let anchor_world2 = position2 * local_anchor2;
|
||||
let anchor1 = anchor_world1 - world_com1;
|
||||
let anchor2 = anchor_world2 - world_com2;
|
||||
|
||||
let vel1: Vector<SimdReal> = linvel1 + angvel1.gcross(anchor1);
|
||||
let vel2: Vector<SimdReal> = linvel2 + angvel2.gcross(anchor2);
|
||||
let rhs = (vel2 - vel1) * SimdReal::splat(params.velocity_solve_fraction)
|
||||
+ (anchor_world2 - anchor_world1) * SimdReal::splat(params.velocity_based_erp_inv_dt());
|
||||
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 inv_lhs = lhs.inverse_unchecked();
|
||||
|
||||
WBallVelocityConstraint {
|
||||
joint_id,
|
||||
mj_lambda1,
|
||||
mj_lambda2,
|
||||
im1,
|
||||
im2,
|
||||
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
|
||||
r1: anchor1,
|
||||
r2: anchor2,
|
||||
rhs,
|
||||
inv_lhs,
|
||||
ii1_sqrt,
|
||||
ii2_sqrt,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda1 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
mj_lambda1.linear += self.impulse * self.im1;
|
||||
mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(self.impulse));
|
||||
mj_lambda2.linear -= self.impulse * self.im2;
|
||||
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.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<Real>]) {
|
||||
let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
|
||||
let vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
|
||||
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.ii1_sqrt.transform_vector(self.r1.gcross(impulse));
|
||||
|
||||
mj_lambda2.linear -= impulse * self.im2;
|
||||
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.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<SimdReal>,
|
||||
pub(crate) impulse: Vector<SimdReal>,
|
||||
r2: Vector<SimdReal>,
|
||||
inv_lhs: SdpMatrix<SimdReal>,
|
||||
im2: SimdReal,
|
||||
ii2_sqrt: AngularInertia<SimdReal>,
|
||||
}
|
||||
|
||||
impl WBallVelocityGroundConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
rbs1: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
),
|
||||
rbs2: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
cparams: [&BallJoint; SIMD_WIDTH],
|
||||
flipped: [bool; SIMD_WIDTH],
|
||||
) -> Self {
|
||||
let (poss1, vels1, mprops1) = rbs1;
|
||||
let (poss2, vels2, mprops2, ids2) = rbs2;
|
||||
|
||||
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
|
||||
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
|
||||
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
|
||||
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
|
||||
let local_anchor1 = Point::from(gather![|ii| if flipped[ii] {
|
||||
cparams[ii].local_anchor2
|
||||
} else {
|
||||
cparams[ii].local_anchor1
|
||||
}]);
|
||||
|
||||
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
|
||||
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
|
||||
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
|
||||
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
|
||||
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
|
||||
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|
||||
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
|
||||
]);
|
||||
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
|
||||
|
||||
let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
|
||||
cparams[ii].local_anchor1
|
||||
} else {
|
||||
cparams[ii].local_anchor2
|
||||
}]);
|
||||
let impulse = Vector::from(gather![|ii| cparams[ii].impulse]);
|
||||
|
||||
let anchor_world1 = position1 * local_anchor1;
|
||||
let anchor_world2 = position2 * local_anchor2;
|
||||
let anchor1 = anchor_world1 - world_com1;
|
||||
let anchor2 = anchor_world2 - world_com2;
|
||||
|
||||
let vel1: Vector<SimdReal> = linvel1 + angvel1.gcross(anchor1);
|
||||
let vel2: Vector<SimdReal> = linvel2 + angvel2.gcross(anchor2);
|
||||
let rhs = (vel2 - vel1) * SimdReal::splat(params.velocity_solve_fraction)
|
||||
+ (anchor_world2 - anchor_world1) * SimdReal::splat(params.velocity_based_erp_inv_dt());
|
||||
let lhs;
|
||||
|
||||
let cmat2 = anchor2.gcross_matrix();
|
||||
|
||||
#[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 * SimdReal::splat(params.warmstart_coeff),
|
||||
r2: anchor2,
|
||||
rhs,
|
||||
inv_lhs,
|
||||
ii2_sqrt,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
mj_lambda2.linear -= self.impulse * self.im2;
|
||||
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.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<Real>]) {
|
||||
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let vel2 = mj_lambda2.linear + angvel.gcross(self.r2);
|
||||
let dvel = vel2 + self.rhs;
|
||||
|
||||
let impulse = self.inv_lhs * dvel;
|
||||
self.impulse += impulse;
|
||||
|
||||
mj_lambda2.linear -= impulse * self.im2;
|
||||
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.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)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,151 +0,0 @@
|
||||
use crate::dynamics::{
|
||||
FixedJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
|
||||
};
|
||||
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation};
|
||||
use crate::utils::WAngularInertia;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct FixedPositionConstraint {
|
||||
position1: usize,
|
||||
position2: usize,
|
||||
local_frame1: Isometry<Real>,
|
||||
local_frame2: Isometry<Real>,
|
||||
local_com1: Point<Real>,
|
||||
local_com2: Point<Real>,
|
||||
im1: Real,
|
||||
im2: Real,
|
||||
ii1: AngularInertia<Real>,
|
||||
ii2: AngularInertia<Real>,
|
||||
|
||||
lin_inv_lhs: Real,
|
||||
ang_inv_lhs: AngularInertia<Real>,
|
||||
}
|
||||
|
||||
impl FixedPositionConstraint {
|
||||
pub fn from_params(
|
||||
rb1: (&RigidBodyMassProps, &RigidBodyIds),
|
||||
rb2: (&RigidBodyMassProps, &RigidBodyIds),
|
||||
cparams: &FixedJoint,
|
||||
) -> Self {
|
||||
let (mprops1, ids1) = rb1;
|
||||
let (mprops2, ids2) = rb2;
|
||||
|
||||
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
|
||||
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
|
||||
let im1 = mprops1.effective_inv_mass;
|
||||
let im2 = mprops2.effective_inv_mass;
|
||||
let lin_inv_lhs = 1.0 / (im1 + im2);
|
||||
let ang_inv_lhs = (ii1 + ii2).inverse();
|
||||
|
||||
Self {
|
||||
local_frame1: cparams.local_frame1,
|
||||
local_frame2: cparams.local_frame2,
|
||||
position1: ids1.active_set_offset,
|
||||
position2: ids2.active_set_offset,
|
||||
im1,
|
||||
im2,
|
||||
ii1,
|
||||
ii2,
|
||||
local_com1: mprops1.local_mprops.local_com,
|
||||
local_com2: mprops2.local_mprops.local_com,
|
||||
lin_inv_lhs,
|
||||
ang_inv_lhs,
|
||||
}
|
||||
}
|
||||
|
||||
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];
|
||||
|
||||
// Angular correction.
|
||||
let anchor1 = position1 * self.local_frame1;
|
||||
let anchor2 = position2 * self.local_frame2;
|
||||
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_frame1.translation.vector);
|
||||
let anchor2 = position2 * Point::from(self.local_frame2.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<Real>,
|
||||
local_frame2: Isometry<Real>,
|
||||
local_com2: Point<Real>,
|
||||
im2: Real,
|
||||
ii2: AngularInertia<Real>,
|
||||
impulse: Real,
|
||||
}
|
||||
|
||||
impl FixedPositionGroundConstraint {
|
||||
pub fn from_params(
|
||||
rb1: &RigidBodyPosition,
|
||||
rb2: (&RigidBodyMassProps, &RigidBodyIds),
|
||||
cparams: &FixedJoint,
|
||||
flipped: bool,
|
||||
) -> Self {
|
||||
let poss1 = rb1;
|
||||
let (mprops2, ids2) = rb2;
|
||||
|
||||
let anchor1;
|
||||
let local_frame2;
|
||||
|
||||
if flipped {
|
||||
anchor1 = poss1.next_position * cparams.local_frame2;
|
||||
local_frame2 = cparams.local_frame1;
|
||||
} else {
|
||||
anchor1 = poss1.next_position * cparams.local_frame1;
|
||||
local_frame2 = cparams.local_frame2;
|
||||
};
|
||||
|
||||
Self {
|
||||
anchor1,
|
||||
local_frame2,
|
||||
position2: ids2.active_set_offset,
|
||||
im2: mprops2.effective_inv_mass,
|
||||
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
|
||||
local_com2: mprops2.local_mprops.local_com,
|
||||
impulse: 0.0,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
|
||||
let mut position2 = positions[self.position2 as usize];
|
||||
|
||||
// Angular correction.
|
||||
let anchor2 = position2 * self.local_frame2;
|
||||
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_frame2.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;
|
||||
}
|
||||
}
|
||||
@@ -1,71 +0,0 @@
|
||||
use super::{FixedPositionConstraint, FixedPositionGroundConstraint};
|
||||
use crate::dynamics::{
|
||||
FixedJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
|
||||
};
|
||||
use crate::math::{Isometry, Real, SIMD_WIDTH};
|
||||
|
||||
// TODO: this does not uses SIMD optimizations yet.
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WFixedPositionConstraint {
|
||||
constraints: [FixedPositionConstraint; SIMD_WIDTH],
|
||||
}
|
||||
|
||||
impl WFixedPositionConstraint {
|
||||
pub fn from_params(
|
||||
rbs1: (
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
rbs2: (
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
cparams: [&FixedJoint; SIMD_WIDTH],
|
||||
) -> Self {
|
||||
Self {
|
||||
constraints: gather![|ii| FixedPositionConstraint::from_params(
|
||||
(rbs1.0[ii], rbs1.1[ii]),
|
||||
(rbs2.0[ii], rbs2.1[ii]),
|
||||
cparams[ii]
|
||||
)],
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
|
||||
for constraint in &self.constraints {
|
||||
constraint.solve(params, positions);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WFixedPositionGroundConstraint {
|
||||
constraints: [FixedPositionGroundConstraint; SIMD_WIDTH],
|
||||
}
|
||||
|
||||
impl WFixedPositionGroundConstraint {
|
||||
pub fn from_params(
|
||||
rbs1: [&RigidBodyPosition; SIMD_WIDTH],
|
||||
rbs2: (
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
cparams: [&FixedJoint; SIMD_WIDTH],
|
||||
flipped: [bool; SIMD_WIDTH],
|
||||
) -> Self {
|
||||
Self {
|
||||
constraints: gather![|ii| FixedPositionGroundConstraint::from_params(
|
||||
rbs1[ii],
|
||||
(rbs2.0[ii], rbs2.1[ii]),
|
||||
cparams[ii],
|
||||
flipped[ii]
|
||||
)],
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
|
||||
for constraint in &self.constraints {
|
||||
constraint.solve(params, positions);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,436 +0,0 @@
|
||||
use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{
|
||||
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
|
||||
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
|
||||
};
|
||||
use crate::math::{AngularInertia, Real, SpacialVector, Vector, DIM};
|
||||
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
|
||||
#[cfg(feature = "dim2")]
|
||||
use na::{Matrix3, Vector3};
|
||||
#[cfg(feature = "dim3")]
|
||||
use na::{Matrix6, Vector6};
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct FixedVelocityConstraint {
|
||||
mj_lambda1: usize,
|
||||
mj_lambda2: usize,
|
||||
|
||||
joint_id: JointIndex,
|
||||
|
||||
impulse: SpacialVector<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 FixedVelocityConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
rb1: (
|
||||
&RigidBodyPosition,
|
||||
&RigidBodyVelocity,
|
||||
&RigidBodyMassProps,
|
||||
&RigidBodyIds,
|
||||
),
|
||||
rb2: (
|
||||
&RigidBodyPosition,
|
||||
&RigidBodyVelocity,
|
||||
&RigidBodyMassProps,
|
||||
&RigidBodyIds,
|
||||
),
|
||||
cparams: &FixedJoint,
|
||||
) -> Self {
|
||||
let (poss1, vels1, mprops1, ids1) = rb1;
|
||||
let (poss2, vels2, mprops2, ids2) = rb2;
|
||||
|
||||
let anchor1 = poss1.position * cparams.local_frame1;
|
||||
let anchor2 = poss2.position * cparams.local_frame2;
|
||||
let im1 = mprops1.effective_inv_mass;
|
||||
let im2 = mprops2.effective_inv_mass;
|
||||
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
|
||||
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
|
||||
let r1 = anchor1.translation.vector - mprops1.world_com.coords;
|
||||
let r2 = anchor2.translation.vector - mprops2.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::<3, 3>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<3, 3>(3, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<3, 3>(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 =
|
||||
-vels1.linvel - vels1.angvel.gcross(r1) + vels2.linvel + vels2.angvel.gcross(r2);
|
||||
let ang_dvel = -vels1.angvel + vels2.angvel;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let mut rhs =
|
||||
Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * params.velocity_solve_fraction;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
let mut rhs = Vector6::new(
|
||||
lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
|
||||
) * params.velocity_solve_fraction;
|
||||
|
||||
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
|
||||
if velocity_based_erp_inv_dt != 0.0 {
|
||||
let lin_err = anchor2.translation.vector - anchor1.translation.vector;
|
||||
let ang_err = anchor2.rotation * anchor1.rotation.inverse();
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let ang_err = ang_err.angle();
|
||||
rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt;
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let ang_err = ang_err.scaled_axis();
|
||||
rhs += Vector6::new(
|
||||
lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
|
||||
) * velocity_based_erp_inv_dt;
|
||||
}
|
||||
}
|
||||
|
||||
FixedVelocityConstraint {
|
||||
joint_id,
|
||||
mj_lambda1: ids1.active_set_offset,
|
||||
mj_lambda2: ids2.active_set_offset,
|
||||
im1,
|
||||
im2,
|
||||
ii1,
|
||||
ii2,
|
||||
ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
|
||||
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
|
||||
impulse: cparams.impulse * params.warmstart_coeff,
|
||||
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::<3>(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 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::<DIM>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = impulse.fixed_rows::<3>(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<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 FixedVelocityGroundConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
|
||||
rb2: (
|
||||
&RigidBodyPosition,
|
||||
&RigidBodyVelocity,
|
||||
&RigidBodyMassProps,
|
||||
&RigidBodyIds,
|
||||
),
|
||||
cparams: &FixedJoint,
|
||||
flipped: bool,
|
||||
) -> Self {
|
||||
let (poss1, vels1, mprops1) = rb1;
|
||||
let (poss2, vels2, mprops2, ids2) = rb2;
|
||||
|
||||
let (anchor1, anchor2) = if flipped {
|
||||
(
|
||||
poss1.position * cparams.local_frame2,
|
||||
poss2.position * cparams.local_frame1,
|
||||
)
|
||||
} else {
|
||||
(
|
||||
poss1.position * cparams.local_frame1,
|
||||
poss2.position * cparams.local_frame2,
|
||||
)
|
||||
};
|
||||
|
||||
let r1 = anchor1.translation.vector - mprops1.world_com.coords;
|
||||
|
||||
let im2 = mprops2.effective_inv_mass;
|
||||
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
|
||||
let r2 = anchor2.translation.vector - mprops2.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::<3, 3>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<3, 3>(3, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<3, 3>(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 =
|
||||
vels2.linvel + vels2.angvel.gcross(r2) - vels1.linvel - vels1.angvel.gcross(r1);
|
||||
let ang_dvel = vels2.angvel - vels1.angvel;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let mut rhs =
|
||||
Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * params.velocity_solve_fraction;
|
||||
#[cfg(feature = "dim3")]
|
||||
let mut rhs = Vector6::new(
|
||||
lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
|
||||
) * params.velocity_solve_fraction;
|
||||
|
||||
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
|
||||
if velocity_based_erp_inv_dt != 0.0 {
|
||||
let lin_err = anchor2.translation.vector - anchor1.translation.vector;
|
||||
let ang_err = anchor2.rotation * anchor1.rotation.inverse();
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let ang_err = ang_err.angle();
|
||||
rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt;
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let ang_err = ang_err.scaled_axis();
|
||||
rhs += Vector6::new(
|
||||
lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
|
||||
) * velocity_based_erp_inv_dt;
|
||||
}
|
||||
}
|
||||
|
||||
FixedVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2: ids2.active_set_offset,
|
||||
im2,
|
||||
ii2,
|
||||
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
|
||||
impulse: cparams.impulse * params.warmstart_coeff,
|
||||
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::<3>(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 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::<DIM>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = impulse.fixed_rows::<3>(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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,539 +0,0 @@
|
||||
use simba::simd::SimdValue;
|
||||
|
||||
use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{
|
||||
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
|
||||
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
|
||||
};
|
||||
use crate::math::{
|
||||
AngVector, AngularInertia, CrossMatrix, Isometry, Point, Real, SimdReal, SpacialVector, Vector,
|
||||
DIM, SIMD_WIDTH,
|
||||
};
|
||||
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
|
||||
#[cfg(feature = "dim3")]
|
||||
use na::{Cholesky, Matrix6, Vector3, Vector6};
|
||||
#[cfg(feature = "dim2")]
|
||||
use {
|
||||
na::{Matrix3, Vector3},
|
||||
parry::utils::SdpMatrix3,
|
||||
};
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WFixedVelocityConstraint {
|
||||
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 WFixedVelocityConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
rbs1: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
rbs2: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
cparams: [&FixedJoint; SIMD_WIDTH],
|
||||
) -> Self {
|
||||
let (poss1, vels1, mprops1, ids1) = rbs1;
|
||||
let (poss2, vels2, mprops2, ids2) = rbs2;
|
||||
|
||||
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
|
||||
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
|
||||
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
|
||||
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
|
||||
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
|
||||
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|
||||
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
|
||||
]);
|
||||
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
|
||||
|
||||
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
|
||||
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
|
||||
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
|
||||
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
|
||||
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
|
||||
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|
||||
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
|
||||
]);
|
||||
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
|
||||
|
||||
let local_frame1 = Isometry::from(gather![|ii| cparams[ii].local_frame1]);
|
||||
let local_frame2 = Isometry::from(gather![|ii| cparams[ii].local_frame2]);
|
||||
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
|
||||
|
||||
let anchor1 = position1 * local_frame1;
|
||||
let anchor2 = position2 * local_frame2;
|
||||
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::<3, 3>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<3, 3>(3, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<3, 3>(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;
|
||||
|
||||
let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let mut rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * velocity_solve_fraction;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
let mut rhs = Vector6::new(
|
||||
lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
|
||||
) * velocity_solve_fraction;
|
||||
|
||||
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
|
||||
if velocity_based_erp_inv_dt != 0.0 {
|
||||
let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
|
||||
|
||||
let lin_err = anchor2.translation.vector - anchor1.translation.vector;
|
||||
let ang_err = anchor2.rotation * anchor1.rotation.inverse();
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let ang_err = ang_err.angle();
|
||||
rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt;
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
|
||||
rhs += Vector6::new(
|
||||
lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
|
||||
) * velocity_based_erp_inv_dt;
|
||||
}
|
||||
}
|
||||
|
||||
WFixedVelocityConstraint {
|
||||
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(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
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::<3>(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(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
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::<DIM>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = impulse.fixed_rows::<3>(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 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::FixedJoint(fixed) = &mut joint.params {
|
||||
fixed.impulse = self.impulse.extract(ii)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WFixedVelocityGroundConstraint {
|
||||
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>,
|
||||
|
||||
im2: SimdReal,
|
||||
ii2: AngularInertia<SimdReal>,
|
||||
ii2_sqrt: AngularInertia<SimdReal>,
|
||||
r2: Vector<SimdReal>,
|
||||
}
|
||||
|
||||
impl WFixedVelocityGroundConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
rbs1: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
),
|
||||
rbs2: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
cparams: [&FixedJoint; SIMD_WIDTH],
|
||||
flipped: [bool; SIMD_WIDTH],
|
||||
) -> Self {
|
||||
let (poss1, vels1, mprops1) = rbs1;
|
||||
let (poss2, vels2, mprops2, ids2) = rbs2;
|
||||
|
||||
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
|
||||
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
|
||||
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
|
||||
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
|
||||
|
||||
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
|
||||
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
|
||||
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
|
||||
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
|
||||
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
|
||||
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|
||||
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
|
||||
]);
|
||||
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
|
||||
|
||||
let local_frame1 = Isometry::from(gather![|ii| if flipped[ii] {
|
||||
cparams[ii].local_frame2
|
||||
} else {
|
||||
cparams[ii].local_frame1
|
||||
}]);
|
||||
let local_frame2 = Isometry::from(gather![|ii| if flipped[ii] {
|
||||
cparams[ii].local_frame1
|
||||
} else {
|
||||
cparams[ii].local_frame2
|
||||
}]);
|
||||
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
|
||||
|
||||
let anchor1 = position1 * local_frame1;
|
||||
let anchor2 = position2 * local_frame2;
|
||||
let ii2 = ii2_sqrt.squared();
|
||||
let r1 = anchor1.translation.vector - world_com1.coords;
|
||||
let r2 = anchor2.translation.vector - world_com2.coords;
|
||||
let rmat2: CrossMatrix<_> = 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();
|
||||
|
||||
lhs = Matrix6::zeros();
|
||||
lhs.fixed_slice_mut::<3, 3>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<3, 3>(3, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<3, 3>(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 = SdpMatrix3::new(m11, m12, m13, m22, m23, m33)
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let inv_lhs = lhs.inverse_unchecked().into_matrix(); // FIXME: don't do into_matrix?
|
||||
#[cfg(feature = "dim3")]
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let lin_dvel = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
|
||||
let ang_dvel = angvel2 - angvel1;
|
||||
|
||||
let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let mut rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * velocity_solve_fraction;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
let mut rhs = Vector6::new(
|
||||
lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
|
||||
) * velocity_solve_fraction;
|
||||
|
||||
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
|
||||
if velocity_based_erp_inv_dt != 0.0 {
|
||||
let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
|
||||
|
||||
let lin_err = anchor2.translation.vector - anchor1.translation.vector;
|
||||
let ang_err = anchor2.rotation * anchor1.rotation.inverse();
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let ang_err = ang_err.angle();
|
||||
rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt;
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
|
||||
rhs += Vector6::new(
|
||||
lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
|
||||
) * velocity_based_erp_inv_dt;
|
||||
}
|
||||
}
|
||||
|
||||
WFixedVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2,
|
||||
im2,
|
||||
ii2,
|
||||
ii2_sqrt,
|
||||
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
|
||||
inv_lhs,
|
||||
r2,
|
||||
rhs,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
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::<3>(3).into_owned();
|
||||
|
||||
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_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_lambda2: DeltaVel<SimdReal> = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
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::<DIM>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = impulse.fixed_rows::<3>(3).into_owned();
|
||||
|
||||
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_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
// FIXME: duplicated code with the non-ground constraint.
|
||||
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::FixedJoint(fixed) = &mut joint.params {
|
||||
fixed.impulse = self.impulse.extract(ii)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,346 +0,0 @@
|
||||
use super::{GenericVelocityConstraint, GenericVelocityGroundConstraint};
|
||||
use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{GenericJoint, IntegrationParameters};
|
||||
use crate::math::{
|
||||
AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector,
|
||||
DIM,
|
||||
};
|
||||
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.
|
||||
#[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,
|
||||
}
|
||||
|
||||
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.local_mprops.local_com,
|
||||
local_com2: rb2.local_mprops.local_com,
|
||||
joint: *joint,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
|
||||
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>(DIM)
|
||||
.into_owned();
|
||||
let mut max_pos_impulse = self
|
||||
.joint
|
||||
.max_pos_impulse
|
||||
.fixed_rows::<Dim>(DIM)
|
||||
.into_owned();
|
||||
|
||||
let pos_rhs = GenericVelocityConstraint::compute_ang_position_error(
|
||||
&anchor1,
|
||||
&anchor2,
|
||||
&basis,
|
||||
&self.joint.min_position.fixed_rows::<Dim>(DIM).into_owned(),
|
||||
&self.joint.max_position.fixed_rows::<Dim>(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;
|
||||
}
|
||||
}
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
positions[self.position1] = position1;
|
||||
positions[self.position2] = 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.local_mprops.local_com,
|
||||
joint: *joint,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
|
||||
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;
|
||||
|
||||
// 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>(DIM)
|
||||
.into_owned();
|
||||
let mut max_pos_impulse = self
|
||||
.joint
|
||||
.max_pos_impulse
|
||||
.fixed_rows::<Dim>(DIM)
|
||||
.into_owned();
|
||||
|
||||
let pos_rhs = GenericVelocityConstraint::compute_ang_position_error(
|
||||
&anchor1,
|
||||
&anchor2,
|
||||
&basis,
|
||||
&self.joint.min_position.fixed_rows::<Dim>(DIM).into_owned(),
|
||||
&self.joint.max_position.fixed_rows::<Dim>(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);
|
||||
}
|
||||
|
||||
positions[self.position2] = position2;
|
||||
}
|
||||
}
|
||||
@@ -1,60 +0,0 @@
|
||||
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: gather![|ii| GenericPositionConstraint::from_params(
|
||||
rbs1[ii],
|
||||
rbs2[ii],
|
||||
cparams[ii]
|
||||
)],
|
||||
}
|
||||
}
|
||||
|
||||
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: gather![|ii| GenericPositionGroundConstraint::from_params(
|
||||
rbs1[ii],
|
||||
rbs2[ii],
|
||||
cparams[ii],
|
||||
flipped[ii]
|
||||
)],
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
|
||||
for constraint in &self.constraints {
|
||||
constraint.solve(params, positions);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,706 +0,0 @@
|
||||
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)
|
||||
}
|
||||
}
|
||||
@@ -1,464 +0,0 @@
|
||||
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(gather![|ii| rbs1[ii].position]);
|
||||
let linvel1 = Vector::from(gather![|ii| *rbs1[ii].linvel()]);
|
||||
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| *rbs1[ii].angvel()]);
|
||||
let world_com1 = Point::from(gather![|ii| rbs1[ii].world_com]);
|
||||
let im1 = SimdReal::from(gather![|ii| rbs1[ii].effective_inv_mass]);
|
||||
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|
||||
|ii| rbs1[ii].effective_world_inv_inertia_sqrt
|
||||
]);
|
||||
let mj_lambda1 = gather![|ii| rbs1[ii].active_set_offset];
|
||||
|
||||
let position2 = Isometry::from(gather![|ii| rbs2[ii].position]);
|
||||
let linvel2 = Vector::from(gather![|ii| *rbs2[ii].linvel()]);
|
||||
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| *rbs2[ii].angvel()]);
|
||||
let world_com2 = Point::from(gather![|ii| rbs2[ii].world_com]);
|
||||
let im2 = SimdReal::from(gather![|ii| rbs2[ii].effective_inv_mass]);
|
||||
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|
||||
|ii| rbs2[ii].effective_world_inv_inertia_sqrt
|
||||
]);
|
||||
let mj_lambda2 = gather![|ii| rbs2[ii].active_set_offset];
|
||||
|
||||
let local_anchor1 = Isometry::from(gather![|ii| cparams[ii].local_anchor1]);
|
||||
let local_anchor2 = Isometry::from(gather![|ii| cparams[ii].local_anchor2]);
|
||||
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
|
||||
|
||||
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(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
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(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
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::<Dim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = 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 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::GenericJoint(fixed) = &mut joint.params {
|
||||
fixed.impulse = self.impulse.extract(ii)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WGenericVelocityGroundConstraint {
|
||||
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>,
|
||||
|
||||
im2: SimdReal,
|
||||
ii2: AngularInertia<SimdReal>,
|
||||
ii2_sqrt: AngularInertia<SimdReal>,
|
||||
r2: Vector<SimdReal>,
|
||||
}
|
||||
|
||||
impl WGenericVelocityGroundConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
rbs1: [&RigidBody; SIMD_WIDTH],
|
||||
rbs2: [&RigidBody; SIMD_WIDTH],
|
||||
cparams: [&GenericJoint; SIMD_WIDTH],
|
||||
flipped: [bool; SIMD_WIDTH],
|
||||
) -> Self {
|
||||
let position1 = Isometry::from(gather![|ii| rbs1[ii].position]);
|
||||
let linvel1 = Vector::from(gather![|ii| *rbs1[ii].linvel()]);
|
||||
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| *rbs1[ii].angvel()]);
|
||||
let world_com1 = Point::from(gather![|ii| rbs1[ii].world_com]);
|
||||
|
||||
let position2 = Isometry::from(gather![|ii| rbs2[ii].position]);
|
||||
let linvel2 = Vector::from(gather![|ii| *rbs2[ii].linvel()]);
|
||||
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| *rbs2[ii].angvel()]);
|
||||
let world_com2 = Point::from(gather![|ii| rbs2[ii].world_com]);
|
||||
let im2 = SimdReal::from(gather![|ii| rbs2[ii].effective_inv_mass]);
|
||||
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|
||||
|ii| rbs2[ii].effective_world_inv_inertia_sqrt
|
||||
]);
|
||||
let mj_lambda2 = gather![|ii| rbs2[ii].active_set_offset];
|
||||
|
||||
let local_anchor1 = Isometry::from(gather![|ii| if flipped[ii] {
|
||||
cparams[ii].local_anchor2
|
||||
} else {
|
||||
cparams[ii].local_anchor1
|
||||
}]);
|
||||
let local_anchor2 = Isometry::from(gather![|ii| if flipped[ii] {
|
||||
cparams[ii].local_anchor1
|
||||
} else {
|
||||
cparams[ii].local_anchor2
|
||||
}]);
|
||||
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
|
||||
|
||||
let anchor1 = position1 * local_anchor1;
|
||||
let anchor2 = position2 * local_anchor2;
|
||||
let ii2 = ii2_sqrt.squared();
|
||||
let r1 = anchor1.translation.vector - world_com1.coords;
|
||||
let r2 = anchor2.translation.vector - world_com2.coords;
|
||||
let rmat2: CrossMatrix<_> = 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();
|
||||
|
||||
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 = 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 = SdpMatrix3::new(m11, m12, m13, m22, m23, m33)
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let inv_lhs = lhs.inverse_unchecked().into_matrix(); // FIXME: don't do into_matrix?
|
||||
#[cfg(feature = "dim3")]
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let lin_dvel = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
|
||||
let ang_dvel = angvel2 - angvel1;
|
||||
|
||||
#[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,
|
||||
);
|
||||
|
||||
WGenericVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2,
|
||||
im2,
|
||||
ii2,
|
||||
ii2_sqrt,
|
||||
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
|
||||
inv_lhs,
|
||||
r2,
|
||||
rhs,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
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 -= 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_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_lambda2: DeltaVel<SimdReal> = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
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::<Dim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = impulse.fixed_rows::<U3>(3).into_owned();
|
||||
|
||||
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_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
// FIXME: duplicated code with the non-ground constraint.
|
||||
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::GenericJoint(fixed) = &mut joint.params {
|
||||
fixed.impulse = self.impulse.extract(ii)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,118 +1,155 @@
|
||||
use super::{
|
||||
BallVelocityConstraint, BallVelocityGroundConstraint, FixedVelocityConstraint,
|
||||
FixedVelocityGroundConstraint, PrismaticVelocityConstraint, PrismaticVelocityGroundConstraint,
|
||||
};
|
||||
#[cfg(feature = "dim3")]
|
||||
use super::{RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint};
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
use super::{
|
||||
WBallVelocityConstraint, WBallVelocityGroundConstraint, WFixedVelocityConstraint,
|
||||
WFixedVelocityGroundConstraint, WPrismaticVelocityConstraint,
|
||||
WPrismaticVelocityGroundConstraint,
|
||||
};
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
use super::{WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint};
|
||||
// use crate::dynamics::solver::joint_constraint::generic_velocity_constraint::{
|
||||
// GenericVelocityConstraint, GenericVelocityGroundConstraint,
|
||||
// };
|
||||
use crate::data::{BundleSet, ComponentSet};
|
||||
use crate::dynamics::solver::joint_constraint::joint_generic_velocity_constraint::{
|
||||
JointGenericVelocityConstraint, JointGenericVelocityGroundConstraint,
|
||||
};
|
||||
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
|
||||
JointVelocityConstraint, JointVelocityGroundConstraint, SolverBody,
|
||||
};
|
||||
use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{
|
||||
IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
|
||||
ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds,
|
||||
RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
|
||||
};
|
||||
use crate::math::Real;
|
||||
use crate::math::{Isometry, Real, SPATIAL_DIM};
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
use crate::math::SIMD_WIDTH;
|
||||
use crate::math::{SimdReal, SIMD_WIDTH};
|
||||
use crate::prelude::MultibodyJointSet;
|
||||
use na::DVector;
|
||||
|
||||
pub(crate) enum AnyJointVelocityConstraint {
|
||||
BallConstraint(BallVelocityConstraint),
|
||||
BallGroundConstraint(BallVelocityGroundConstraint),
|
||||
pub enum AnyJointVelocityConstraint {
|
||||
JointConstraint(JointVelocityConstraint<Real, 1>),
|
||||
JointGroundConstraint(JointVelocityGroundConstraint<Real, 1>),
|
||||
JointGenericConstraint(JointGenericVelocityConstraint),
|
||||
JointGenericGroundConstraint(JointGenericVelocityGroundConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WBallConstraint(WBallVelocityConstraint),
|
||||
JointConstraintSimd(JointVelocityConstraint<SimdReal, SIMD_WIDTH>),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WBallGroundConstraint(WBallVelocityGroundConstraint),
|
||||
FixedConstraint(FixedVelocityConstraint),
|
||||
FixedGroundConstraint(FixedVelocityGroundConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WFixedConstraint(WFixedVelocityConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WFixedGroundConstraint(WFixedVelocityGroundConstraint),
|
||||
// GenericConstraint(GenericVelocityConstraint),
|
||||
// GenericGroundConstraint(GenericVelocityGroundConstraint),
|
||||
// #[cfg(feature = "simd-is-enabled")]
|
||||
// WGenericConstraint(WGenericVelocityConstraint),
|
||||
// #[cfg(feature = "simd-is-enabled")]
|
||||
// WGenericGroundConstraint(WGenericVelocityGroundConstraint),
|
||||
PrismaticConstraint(PrismaticVelocityConstraint),
|
||||
PrismaticGroundConstraint(PrismaticVelocityGroundConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WPrismaticConstraint(WPrismaticVelocityConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WPrismaticGroundConstraint(WPrismaticVelocityGroundConstraint),
|
||||
#[cfg(feature = "dim3")]
|
||||
RevoluteConstraint(RevoluteVelocityConstraint),
|
||||
#[cfg(feature = "dim3")]
|
||||
RevoluteGroundConstraint(RevoluteVelocityGroundConstraint),
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WRevoluteConstraint(WRevoluteVelocityConstraint),
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WRevoluteGroundConstraint(WRevoluteVelocityGroundConstraint),
|
||||
#[allow(dead_code)] // The Empty variant is only used with parallel code.
|
||||
JointGroundConstraintSimd(JointVelocityGroundConstraint<SimdReal, SIMD_WIDTH>),
|
||||
Empty,
|
||||
}
|
||||
|
||||
impl AnyJointVelocityConstraint {
|
||||
#[cfg(feature = "parallel")]
|
||||
pub fn num_active_constraints(_: &Joint) -> usize {
|
||||
pub fn num_active_constraints(_: &ImpulseJoint) -> usize {
|
||||
1
|
||||
}
|
||||
|
||||
pub fn from_joint<Bodies>(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
joint: &Joint,
|
||||
joint: &ImpulseJoint,
|
||||
bodies: &Bodies,
|
||||
) -> Self
|
||||
where
|
||||
multibodies: &MultibodyJointSet,
|
||||
j_id: &mut usize,
|
||||
jacobians: &mut DVector<Real>,
|
||||
out: &mut Vec<Self>,
|
||||
) where
|
||||
Bodies: ComponentSet<RigidBodyPosition>
|
||||
+ ComponentSet<RigidBodyVelocity>
|
||||
+ ComponentSet<RigidBodyMassProps>
|
||||
+ ComponentSet<RigidBodyIds>,
|
||||
{
|
||||
let rb1 = (
|
||||
bodies.index(joint.body1.0),
|
||||
bodies.index(joint.body1.0),
|
||||
bodies.index(joint.body1.0),
|
||||
bodies.index(joint.body1.0),
|
||||
);
|
||||
let rb2 = (
|
||||
bodies.index(joint.body2.0),
|
||||
bodies.index(joint.body2.0),
|
||||
bodies.index(joint.body2.0),
|
||||
bodies.index(joint.body2.0),
|
||||
);
|
||||
let local_frame1 = joint.data.local_frame1;
|
||||
let local_frame2 = joint.data.local_frame2;
|
||||
let rb1: (
|
||||
&RigidBodyPosition,
|
||||
&RigidBodyVelocity,
|
||||
&RigidBodyMassProps,
|
||||
&RigidBodyIds,
|
||||
) = bodies.index_bundle(joint.body1.0);
|
||||
let rb2: (
|
||||
&RigidBodyPosition,
|
||||
&RigidBodyVelocity,
|
||||
&RigidBodyMassProps,
|
||||
&RigidBodyIds,
|
||||
) = bodies.index_bundle(joint.body2.0);
|
||||
|
||||
match &joint.params {
|
||||
JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallConstraint(
|
||||
BallVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
|
||||
),
|
||||
JointParams::FixedJoint(p) => AnyJointVelocityConstraint::FixedConstraint(
|
||||
FixedVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
|
||||
),
|
||||
JointParams::PrismaticJoint(p) => AnyJointVelocityConstraint::PrismaticConstraint(
|
||||
PrismaticVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
|
||||
),
|
||||
// JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericConstraint(
|
||||
// GenericVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
|
||||
// ),
|
||||
#[cfg(feature = "dim3")]
|
||||
JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteConstraint(
|
||||
RevoluteVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
|
||||
),
|
||||
let (rb_pos1, rb_vel1, rb_mprops1, rb_ids1) = rb1;
|
||||
let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rb2;
|
||||
let frame1 = rb_pos1.position * local_frame1;
|
||||
let frame2 = rb_pos2.position * local_frame2;
|
||||
|
||||
let body1 = SolverBody {
|
||||
linvel: rb_vel1.linvel,
|
||||
angvel: rb_vel1.angvel,
|
||||
im: rb_mprops1.effective_inv_mass,
|
||||
sqrt_ii: rb_mprops1.effective_world_inv_inertia_sqrt,
|
||||
world_com: rb_mprops1.world_com,
|
||||
mj_lambda: [rb_ids1.active_set_offset],
|
||||
};
|
||||
let body2 = SolverBody {
|
||||
linvel: rb_vel2.linvel,
|
||||
angvel: rb_vel2.angvel,
|
||||
im: rb_mprops2.effective_inv_mass,
|
||||
sqrt_ii: rb_mprops2.effective_world_inv_inertia_sqrt,
|
||||
world_com: rb_mprops2.world_com,
|
||||
mj_lambda: [rb_ids2.active_set_offset],
|
||||
};
|
||||
|
||||
let mb1 = multibodies
|
||||
.rigid_body_link(joint.body1)
|
||||
.map(|link| (&multibodies[link.multibody], link.id));
|
||||
let mb2 = multibodies
|
||||
.rigid_body_link(joint.body2)
|
||||
.map(|link| (&multibodies[link.multibody], link.id));
|
||||
|
||||
if mb1.is_some() || mb2.is_some() {
|
||||
let multibodies_ndof = mb1.map(|m| m.0.ndofs()).unwrap_or(SPATIAL_DIM)
|
||||
+ mb2.map(|m| m.0.ndofs()).unwrap_or(SPATIAL_DIM);
|
||||
|
||||
if multibodies_ndof == 0 {
|
||||
// Both multibodies are fixed, don’t generate any constraint.
|
||||
return;
|
||||
}
|
||||
|
||||
// For each solver contact we generate up to SPATIAL_DIM constraints, and each
|
||||
// constraints appends the multibodies jacobian and weighted jacobians.
|
||||
// Also note that for impulse_joints, the rigid-bodies will also add their jacobians
|
||||
// to the generic DVector.
|
||||
// TODO: is this count correct when we take both motors and limits into account?
|
||||
let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM;
|
||||
|
||||
if jacobians.nrows() < required_jacobian_len {
|
||||
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
|
||||
}
|
||||
|
||||
// TODO: find a way to avoid the temporary buffer.
|
||||
let mut out_tmp = [JointGenericVelocityConstraint::invalid(); 12];
|
||||
let out_tmp_len = JointGenericVelocityConstraint::lock_axes(
|
||||
params,
|
||||
joint_id,
|
||||
&body1,
|
||||
&body2,
|
||||
mb1,
|
||||
mb2,
|
||||
&frame1,
|
||||
&frame2,
|
||||
&joint.data,
|
||||
jacobians,
|
||||
j_id,
|
||||
&mut out_tmp,
|
||||
);
|
||||
|
||||
for c in out_tmp.into_iter().take(out_tmp_len) {
|
||||
out.push(AnyJointVelocityConstraint::JointGenericConstraint(c));
|
||||
}
|
||||
} else {
|
||||
// TODO: find a way to avoid the temporary buffer.
|
||||
let mut out_tmp = [JointVelocityConstraint::invalid(); 12];
|
||||
let out_tmp_len = JointVelocityConstraint::<Real, 1>::lock_axes(
|
||||
params,
|
||||
joint_id,
|
||||
&body1,
|
||||
&body2,
|
||||
&frame1,
|
||||
&frame2,
|
||||
&joint.data,
|
||||
&mut out_tmp,
|
||||
);
|
||||
|
||||
for c in out_tmp.into_iter().take(out_tmp_len) {
|
||||
out.push(AnyJointVelocityConstraint::JointConstraint(c));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -120,70 +157,96 @@ impl AnyJointVelocityConstraint {
|
||||
pub fn from_wide_joint<Bodies>(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
joints: [&Joint; SIMD_WIDTH],
|
||||
impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
|
||||
bodies: &Bodies,
|
||||
) -> Self
|
||||
where
|
||||
out: &mut Vec<Self>,
|
||||
) where
|
||||
Bodies: ComponentSet<RigidBodyPosition>
|
||||
+ ComponentSet<RigidBodyVelocity>
|
||||
+ ComponentSet<RigidBodyMassProps>
|
||||
+ ComponentSet<RigidBodyIds>,
|
||||
{
|
||||
let rbs1 = (
|
||||
gather![|ii| bodies.index(joints[ii].body1.0)],
|
||||
gather![|ii| bodies.index(joints[ii].body1.0)],
|
||||
gather![|ii| bodies.index(joints[ii].body1.0)],
|
||||
gather![|ii| bodies.index(joints[ii].body1.0)],
|
||||
let rbs1: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
) = (
|
||||
gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
|
||||
gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
|
||||
gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
|
||||
gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
|
||||
);
|
||||
let rbs2 = (
|
||||
gather![|ii| bodies.index(joints[ii].body2.0)],
|
||||
gather![|ii| bodies.index(joints[ii].body2.0)],
|
||||
gather![|ii| bodies.index(joints[ii].body2.0)],
|
||||
gather![|ii| bodies.index(joints[ii].body2.0)],
|
||||
let rbs2: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
) = (
|
||||
gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
|
||||
gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
|
||||
gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
|
||||
gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
|
||||
);
|
||||
|
||||
match &joints[0].params {
|
||||
JointParams::BallJoint(_) => {
|
||||
let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
|
||||
AnyJointVelocityConstraint::WBallConstraint(WBallVelocityConstraint::from_params(
|
||||
params, joint_id, rbs1, rbs2, joints,
|
||||
))
|
||||
}
|
||||
JointParams::FixedJoint(_) => {
|
||||
let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
|
||||
AnyJointVelocityConstraint::WFixedConstraint(WFixedVelocityConstraint::from_params(
|
||||
params, joint_id, rbs1, rbs2, joints,
|
||||
))
|
||||
}
|
||||
// JointParams::GenericJoint(_) => {
|
||||
// let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
|
||||
// AnyJointVelocityConstraint::WGenericConstraint(
|
||||
// WGenericVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
|
||||
// )
|
||||
// }
|
||||
JointParams::PrismaticJoint(_) => {
|
||||
let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
|
||||
AnyJointVelocityConstraint::WPrismaticConstraint(
|
||||
WPrismaticVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
|
||||
)
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
JointParams::RevoluteJoint(_) => {
|
||||
let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
|
||||
AnyJointVelocityConstraint::WRevoluteConstraint(
|
||||
WRevoluteVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
|
||||
)
|
||||
}
|
||||
let (rb_pos1, rb_vel1, rb_mprops1, rb_ids1) = rbs1;
|
||||
let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rbs2;
|
||||
let pos1: Isometry<SimdReal> = gather![|ii| rb_pos1[ii].position].into();
|
||||
let pos2: Isometry<SimdReal> = gather![|ii| rb_pos2[ii].position].into();
|
||||
|
||||
let local_frame1: Isometry<SimdReal> =
|
||||
gather![|ii| impulse_joints[ii].data.local_frame1].into();
|
||||
let local_frame2: Isometry<SimdReal> =
|
||||
gather![|ii| impulse_joints[ii].data.local_frame2].into();
|
||||
|
||||
let frame1 = pos1 * local_frame1;
|
||||
let frame2 = pos2 * local_frame2;
|
||||
|
||||
let body1: SolverBody<SimdReal, SIMD_WIDTH> = SolverBody {
|
||||
linvel: gather![|ii| rb_vel1[ii].linvel].into(),
|
||||
angvel: gather![|ii| rb_vel1[ii].angvel].into(),
|
||||
im: gather![|ii| rb_mprops1[ii].effective_inv_mass].into(),
|
||||
sqrt_ii: gather![|ii| rb_mprops1[ii].effective_world_inv_inertia_sqrt].into(),
|
||||
world_com: gather![|ii| rb_mprops1[ii].world_com].into(),
|
||||
mj_lambda: gather![|ii| rb_ids1[ii].active_set_offset],
|
||||
};
|
||||
let body2: SolverBody<SimdReal, SIMD_WIDTH> = SolverBody {
|
||||
linvel: gather![|ii| rb_vel2[ii].linvel].into(),
|
||||
angvel: gather![|ii| rb_vel2[ii].angvel].into(),
|
||||
im: gather![|ii| rb_mprops2[ii].effective_inv_mass].into(),
|
||||
sqrt_ii: gather![|ii| rb_mprops2[ii].effective_world_inv_inertia_sqrt].into(),
|
||||
world_com: gather![|ii| rb_mprops2[ii].world_com].into(),
|
||||
mj_lambda: gather![|ii| rb_ids2[ii].active_set_offset],
|
||||
};
|
||||
|
||||
// TODO: find a way to avoid the temporary buffer.
|
||||
let mut out_tmp = [JointVelocityConstraint::invalid(); 12];
|
||||
let out_tmp_len = JointVelocityConstraint::<SimdReal, SIMD_WIDTH>::lock_axes(
|
||||
params,
|
||||
joint_id,
|
||||
&body1,
|
||||
&body2,
|
||||
&frame1,
|
||||
&frame2,
|
||||
impulse_joints[0].data.locked_axes.bits(),
|
||||
&mut out_tmp,
|
||||
);
|
||||
|
||||
for c in out_tmp.into_iter().take(out_tmp_len) {
|
||||
out.push(AnyJointVelocityConstraint::JointConstraintSimd(c));
|
||||
}
|
||||
}
|
||||
|
||||
pub fn from_joint_ground<Bodies>(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
joint: &Joint,
|
||||
joint: &ImpulseJoint,
|
||||
bodies: &Bodies,
|
||||
) -> Self
|
||||
where
|
||||
multibodies: &MultibodyJointSet,
|
||||
j_id: &mut usize,
|
||||
jacobians: &mut DVector<Real>,
|
||||
out: &mut Vec<Self>,
|
||||
) where
|
||||
Bodies: ComponentSet<RigidBodyPosition>
|
||||
+ ComponentSet<RigidBodyType>
|
||||
+ ComponentSet<RigidBodyVelocity>
|
||||
@@ -195,36 +258,102 @@ impl AnyJointVelocityConstraint {
|
||||
let status2: &RigidBodyType = bodies.index(handle2.0);
|
||||
let flipped = !status2.is_dynamic();
|
||||
|
||||
if flipped {
|
||||
let (local_frame1, local_frame2) = if flipped {
|
||||
std::mem::swap(&mut handle1, &mut handle2);
|
||||
}
|
||||
(joint.data.local_frame2, joint.data.local_frame1)
|
||||
} else {
|
||||
(joint.data.local_frame1, joint.data.local_frame2)
|
||||
};
|
||||
|
||||
let rb1 = bodies.index_bundle(handle1.0);
|
||||
let rb2 = bodies.index_bundle(handle2.0);
|
||||
let rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps) =
|
||||
bodies.index_bundle(handle1.0);
|
||||
let rb2: (
|
||||
&RigidBodyPosition,
|
||||
&RigidBodyVelocity,
|
||||
&RigidBodyMassProps,
|
||||
&RigidBodyIds,
|
||||
) = bodies.index_bundle(handle2.0);
|
||||
|
||||
match &joint.params {
|
||||
JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallGroundConstraint(
|
||||
BallVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped),
|
||||
),
|
||||
JointParams::FixedJoint(p) => AnyJointVelocityConstraint::FixedGroundConstraint(
|
||||
FixedVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped),
|
||||
),
|
||||
// JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericGroundConstraint(
|
||||
// GenericVelocityGroundConstraint::from_params(
|
||||
// params, joint_id, rb1, rb2, p, flipped,
|
||||
// ),
|
||||
// ),
|
||||
JointParams::PrismaticJoint(p) => {
|
||||
AnyJointVelocityConstraint::PrismaticGroundConstraint(
|
||||
PrismaticVelocityGroundConstraint::from_params(
|
||||
params, joint_id, rb1, rb2, p, flipped,
|
||||
),
|
||||
)
|
||||
let (rb_pos1, rb_vel1, rb_mprops1) = rb1;
|
||||
let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rb2;
|
||||
let frame1 = rb_pos1.position * local_frame1;
|
||||
let frame2 = rb_pos2.position * local_frame2;
|
||||
|
||||
let body1 = SolverBody {
|
||||
linvel: rb_vel1.linvel,
|
||||
angvel: rb_vel1.angvel,
|
||||
im: rb_mprops1.effective_inv_mass,
|
||||
sqrt_ii: rb_mprops1.effective_world_inv_inertia_sqrt,
|
||||
world_com: rb_mprops1.world_com,
|
||||
mj_lambda: [crate::INVALID_USIZE],
|
||||
};
|
||||
let body2 = SolverBody {
|
||||
linvel: rb_vel2.linvel,
|
||||
angvel: rb_vel2.angvel,
|
||||
im: rb_mprops2.effective_inv_mass,
|
||||
sqrt_ii: rb_mprops2.effective_world_inv_inertia_sqrt,
|
||||
world_com: rb_mprops2.world_com,
|
||||
mj_lambda: [rb_ids2.active_set_offset],
|
||||
};
|
||||
|
||||
if let Some(mb2) = multibodies
|
||||
.rigid_body_link(handle2)
|
||||
.map(|link| (&multibodies[link.multibody], link.id))
|
||||
{
|
||||
let multibodies_ndof = mb2.0.ndofs();
|
||||
|
||||
if multibodies_ndof == 0 {
|
||||
// The multibody is fixed, don’t generate any constraint.
|
||||
return;
|
||||
}
|
||||
|
||||
// For each solver contact we generate up to SPATIAL_DIM constraints, and each
|
||||
// constraints appends the multibodies jacobian and weighted jacobians.
|
||||
// Also note that for impulse_joints, the rigid-bodies will also add their jacobians
|
||||
// to the generic DVector.
|
||||
// TODO: is this count correct when we take both motors and limits into account?
|
||||
let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM;
|
||||
|
||||
if jacobians.nrows() < required_jacobian_len {
|
||||
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
|
||||
}
|
||||
|
||||
// TODO: find a way to avoid the temporary buffer.
|
||||
let mut out_tmp = [JointGenericVelocityGroundConstraint::invalid(); 12];
|
||||
let out_tmp_len = JointGenericVelocityGroundConstraint::lock_axes(
|
||||
params,
|
||||
joint_id,
|
||||
&body1,
|
||||
&body2,
|
||||
mb2,
|
||||
&frame1,
|
||||
&frame2,
|
||||
&joint.data,
|
||||
jacobians,
|
||||
j_id,
|
||||
&mut out_tmp,
|
||||
);
|
||||
|
||||
for c in out_tmp.into_iter().take(out_tmp_len) {
|
||||
out.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(c));
|
||||
}
|
||||
} else {
|
||||
// TODO: find a way to avoid the temporary buffer.
|
||||
let mut out_tmp = [JointVelocityGroundConstraint::invalid(); 12];
|
||||
let out_tmp_len = JointVelocityGroundConstraint::<Real, 1>::lock_axes(
|
||||
params,
|
||||
joint_id,
|
||||
&body1,
|
||||
&body2,
|
||||
&frame1,
|
||||
&frame2,
|
||||
&joint.data,
|
||||
&mut out_tmp,
|
||||
);
|
||||
|
||||
for c in out_tmp.into_iter().take(out_tmp_len) {
|
||||
out.push(AnyJointVelocityConstraint::JointGroundConstraint(c));
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
JointParams::RevoluteJoint(p) => RevoluteVelocityGroundConstraint::from_params(
|
||||
params, joint_id, rb1, rb2, p, flipped,
|
||||
),
|
||||
}
|
||||
}
|
||||
|
||||
@@ -232,18 +361,18 @@ impl AnyJointVelocityConstraint {
|
||||
pub fn from_wide_joint_ground<Bodies>(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
joints: [&Joint; SIMD_WIDTH],
|
||||
impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
|
||||
bodies: &Bodies,
|
||||
) -> Self
|
||||
where
|
||||
out: &mut Vec<Self>,
|
||||
) where
|
||||
Bodies: ComponentSet<RigidBodyPosition>
|
||||
+ ComponentSet<RigidBodyType>
|
||||
+ ComponentSet<RigidBodyVelocity>
|
||||
+ ComponentSet<RigidBodyMassProps>
|
||||
+ ComponentSet<RigidBodyIds>,
|
||||
{
|
||||
let mut handles1 = gather![|ii| joints[ii].body1];
|
||||
let mut handles2 = gather![|ii| joints[ii].body2];
|
||||
let mut handles1 = gather![|ii| impulse_joints[ii].body1];
|
||||
let mut handles2 = gather![|ii| impulse_joints[ii].body2];
|
||||
let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
|
||||
let mut flipped = [false; SIMD_WIDTH];
|
||||
|
||||
@@ -254,197 +383,136 @@ impl AnyJointVelocityConstraint {
|
||||
}
|
||||
}
|
||||
|
||||
let rbs1 = (
|
||||
let local_frame1: Isometry<SimdReal> = gather![|ii| if flipped[ii] {
|
||||
impulse_joints[ii].data.local_frame2
|
||||
} else {
|
||||
impulse_joints[ii].data.local_frame1
|
||||
}]
|
||||
.into();
|
||||
let local_frame2: Isometry<SimdReal> = gather![|ii| if flipped[ii] {
|
||||
impulse_joints[ii].data.local_frame1
|
||||
} else {
|
||||
impulse_joints[ii].data.local_frame2
|
||||
}]
|
||||
.into();
|
||||
|
||||
let rbs1: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
) = (
|
||||
gather![|ii| bodies.index(handles1[ii].0)],
|
||||
gather![|ii| bodies.index(handles1[ii].0)],
|
||||
gather![|ii| bodies.index(handles1[ii].0)],
|
||||
);
|
||||
let rbs2 = (
|
||||
let rbs2: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
) = (
|
||||
gather![|ii| bodies.index(handles2[ii].0)],
|
||||
gather![|ii| bodies.index(handles2[ii].0)],
|
||||
gather![|ii| bodies.index(handles2[ii].0)],
|
||||
gather![|ii| bodies.index(handles2[ii].0)],
|
||||
);
|
||||
|
||||
match &joints[0].params {
|
||||
JointParams::BallJoint(_) => {
|
||||
let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
|
||||
AnyJointVelocityConstraint::WBallGroundConstraint(
|
||||
WBallVelocityGroundConstraint::from_params(
|
||||
params, joint_id, rbs1, rbs2, joints, flipped,
|
||||
),
|
||||
)
|
||||
}
|
||||
JointParams::FixedJoint(_) => {
|
||||
let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
|
||||
AnyJointVelocityConstraint::WFixedGroundConstraint(
|
||||
WFixedVelocityGroundConstraint::from_params(
|
||||
params, joint_id, rbs1, rbs2, joints, flipped,
|
||||
),
|
||||
)
|
||||
}
|
||||
// JointParams::GenericJoint(_) => {
|
||||
// let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
|
||||
// AnyJointVelocityConstraint::WGenericGroundConstraint(
|
||||
// WGenericVelocityGroundConstraint::from_params(
|
||||
// params, joint_id, rbs1, rbs2, joints, flipped,
|
||||
// ),
|
||||
// )
|
||||
// }
|
||||
JointParams::PrismaticJoint(_) => {
|
||||
let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
|
||||
AnyJointVelocityConstraint::WPrismaticGroundConstraint(
|
||||
WPrismaticVelocityGroundConstraint::from_params(
|
||||
params, joint_id, rbs1, rbs2, joints, flipped,
|
||||
),
|
||||
)
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
JointParams::RevoluteJoint(_) => {
|
||||
let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
|
||||
AnyJointVelocityConstraint::WRevoluteGroundConstraint(
|
||||
WRevoluteVelocityGroundConstraint::from_params(
|
||||
params, joint_id, rbs1, rbs2, joints, flipped,
|
||||
),
|
||||
)
|
||||
}
|
||||
let (rb_pos1, rb_vel1, rb_mprops1) = rbs1;
|
||||
let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rbs2;
|
||||
let pos1: Isometry<SimdReal> = gather![|ii| rb_pos1[ii].position].into();
|
||||
let pos2: Isometry<SimdReal> = gather![|ii| rb_pos2[ii].position].into();
|
||||
|
||||
let frame1 = pos1 * local_frame1;
|
||||
let frame2 = pos2 * local_frame2;
|
||||
|
||||
let body1: SolverBody<SimdReal, SIMD_WIDTH> = SolverBody {
|
||||
linvel: gather![|ii| rb_vel1[ii].linvel].into(),
|
||||
angvel: gather![|ii| rb_vel1[ii].angvel].into(),
|
||||
im: gather![|ii| rb_mprops1[ii].effective_inv_mass].into(),
|
||||
sqrt_ii: gather![|ii| rb_mprops1[ii].effective_world_inv_inertia_sqrt].into(),
|
||||
world_com: gather![|ii| rb_mprops1[ii].world_com].into(),
|
||||
mj_lambda: [crate::INVALID_USIZE; SIMD_WIDTH],
|
||||
};
|
||||
let body2: SolverBody<SimdReal, SIMD_WIDTH> = SolverBody {
|
||||
linvel: gather![|ii| rb_vel2[ii].linvel].into(),
|
||||
angvel: gather![|ii| rb_vel2[ii].angvel].into(),
|
||||
im: gather![|ii| rb_mprops2[ii].effective_inv_mass].into(),
|
||||
sqrt_ii: gather![|ii| rb_mprops2[ii].effective_world_inv_inertia_sqrt].into(),
|
||||
world_com: gather![|ii| rb_mprops2[ii].world_com].into(),
|
||||
mj_lambda: gather![|ii| rb_ids2[ii].active_set_offset],
|
||||
};
|
||||
|
||||
// TODO: find a way to avoid the temporary buffer.
|
||||
let mut out_tmp = [JointVelocityGroundConstraint::invalid(); 12];
|
||||
let out_tmp_len = JointVelocityGroundConstraint::<SimdReal, SIMD_WIDTH>::lock_axes(
|
||||
params,
|
||||
joint_id,
|
||||
&body1,
|
||||
&body2,
|
||||
&frame1,
|
||||
&frame2,
|
||||
impulse_joints[0].data.locked_axes.bits(),
|
||||
&mut out_tmp,
|
||||
);
|
||||
|
||||
for c in out_tmp.into_iter().take(out_tmp_len) {
|
||||
out.push(AnyJointVelocityConstraint::JointGroundConstraintSimd(c));
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
pub fn remove_bias_from_rhs(&mut self) {
|
||||
match self {
|
||||
AnyJointVelocityConstraint::BallConstraint(c) => c.warmstart(mj_lambdas),
|
||||
AnyJointVelocityConstraint::BallGroundConstraint(c) => c.warmstart(mj_lambdas),
|
||||
AnyJointVelocityConstraint::JointConstraint(c) => c.remove_bias_from_rhs(),
|
||||
AnyJointVelocityConstraint::JointGroundConstraint(c) => c.remove_bias_from_rhs(),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WBallConstraint(c) => c.warmstart(mj_lambdas),
|
||||
AnyJointVelocityConstraint::JointConstraintSimd(c) => c.remove_bias_from_rhs(),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WBallGroundConstraint(c) => c.warmstart(mj_lambdas),
|
||||
AnyJointVelocityConstraint::FixedConstraint(c) => c.warmstart(mj_lambdas),
|
||||
AnyJointVelocityConstraint::FixedGroundConstraint(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WFixedConstraint(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.warmstart(mj_lambdas),
|
||||
// AnyJointVelocityConstraint::GenericConstraint(c) => c.warmstart(mj_lambdas),
|
||||
// AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.warmstart(mj_lambdas),
|
||||
// #[cfg(feature = "simd-is-enabled")]
|
||||
// AnyJointVelocityConstraint::WGenericConstraint(c) => c.warmstart(mj_lambdas),
|
||||
// #[cfg(feature = "simd-is-enabled")]
|
||||
// AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.warmstart(mj_lambdas),
|
||||
AnyJointVelocityConstraint::PrismaticConstraint(c) => c.warmstart(mj_lambdas),
|
||||
AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WPrismaticConstraint(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WPrismaticGroundConstraint(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "dim3")]
|
||||
AnyJointVelocityConstraint::RevoluteConstraint(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "dim3")]
|
||||
AnyJointVelocityConstraint::RevoluteGroundConstraint(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WRevoluteConstraint(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WRevoluteGroundConstraint(c) => c.warmstart(mj_lambdas),
|
||||
AnyJointVelocityConstraint::JointGroundConstraintSimd(c) => c.remove_bias_from_rhs(),
|
||||
AnyJointVelocityConstraint::JointGenericConstraint(c) => c.remove_bias_from_rhs(),
|
||||
AnyJointVelocityConstraint::JointGenericGroundConstraint(c) => c.remove_bias_from_rhs(),
|
||||
AnyJointVelocityConstraint::Empty => unreachable!(),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
pub fn solve(
|
||||
&mut self,
|
||||
jacobians: &DVector<Real>,
|
||||
mj_lambdas: &mut [DeltaVel<Real>],
|
||||
generic_mj_lambdas: &mut DVector<Real>,
|
||||
) {
|
||||
match self {
|
||||
AnyJointVelocityConstraint::BallConstraint(c) => c.solve(mj_lambdas),
|
||||
AnyJointVelocityConstraint::BallGroundConstraint(c) => c.solve(mj_lambdas),
|
||||
AnyJointVelocityConstraint::JointConstraint(c) => c.solve(mj_lambdas),
|
||||
AnyJointVelocityConstraint::JointGroundConstraint(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WBallConstraint(c) => c.solve(mj_lambdas),
|
||||
AnyJointVelocityConstraint::JointConstraintSimd(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WBallGroundConstraint(c) => c.solve(mj_lambdas),
|
||||
AnyJointVelocityConstraint::FixedConstraint(c) => c.solve(mj_lambdas),
|
||||
AnyJointVelocityConstraint::FixedGroundConstraint(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WFixedConstraint(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.solve(mj_lambdas),
|
||||
// AnyJointVelocityConstraint::GenericConstraint(c) => c.solve(mj_lambdas),
|
||||
// AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.solve(mj_lambdas),
|
||||
// #[cfg(feature = "simd-is-enabled")]
|
||||
// AnyJointVelocityConstraint::WGenericConstraint(c) => c.solve(mj_lambdas),
|
||||
// #[cfg(feature = "simd-is-enabled")]
|
||||
// AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.solve(mj_lambdas),
|
||||
AnyJointVelocityConstraint::PrismaticConstraint(c) => c.solve(mj_lambdas),
|
||||
AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WPrismaticConstraint(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WPrismaticGroundConstraint(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "dim3")]
|
||||
AnyJointVelocityConstraint::RevoluteConstraint(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "dim3")]
|
||||
AnyJointVelocityConstraint::RevoluteGroundConstraint(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WRevoluteConstraint(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WRevoluteGroundConstraint(c) => c.solve(mj_lambdas),
|
||||
AnyJointVelocityConstraint::JointGroundConstraintSimd(c) => c.solve(mj_lambdas),
|
||||
AnyJointVelocityConstraint::JointGenericConstraint(c) => {
|
||||
c.solve(jacobians, mj_lambdas, generic_mj_lambdas)
|
||||
}
|
||||
AnyJointVelocityConstraint::JointGenericGroundConstraint(c) => {
|
||||
c.solve(jacobians, mj_lambdas, generic_mj_lambdas)
|
||||
}
|
||||
AnyJointVelocityConstraint::Empty => unreachable!(),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
match self {
|
||||
AnyJointVelocityConstraint::BallConstraint(c) => c.writeback_impulses(joints_all),
|
||||
|
||||
AnyJointVelocityConstraint::BallGroundConstraint(c) => c.writeback_impulses(joints_all),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WBallConstraint(c) => c.writeback_impulses(joints_all),
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WBallGroundConstraint(c) => {
|
||||
c.writeback_impulses(joints_all)
|
||||
}
|
||||
AnyJointVelocityConstraint::FixedConstraint(c) => c.writeback_impulses(joints_all),
|
||||
AnyJointVelocityConstraint::FixedGroundConstraint(c) => {
|
||||
AnyJointVelocityConstraint::JointConstraint(c) => c.writeback_impulses(joints_all),
|
||||
AnyJointVelocityConstraint::JointGroundConstraint(c) => {
|
||||
c.writeback_impulses(joints_all)
|
||||
}
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WFixedConstraint(c) => c.writeback_impulses(joints_all),
|
||||
AnyJointVelocityConstraint::JointConstraintSimd(c) => c.writeback_impulses(joints_all),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WFixedGroundConstraint(c) => {
|
||||
AnyJointVelocityConstraint::JointGroundConstraintSimd(c) => {
|
||||
c.writeback_impulses(joints_all)
|
||||
}
|
||||
// AnyJointVelocityConstraint::GenericConstraint(c) => c.writeback_impulses(joints_all),
|
||||
// AnyJointVelocityConstraint::GenericGroundConstraint(c) => {
|
||||
// c.writeback_impulses(joints_all)
|
||||
// }
|
||||
// #[cfg(feature = "simd-is-enabled")]
|
||||
// AnyJointVelocityConstraint::WGenericConstraint(c) => c.writeback_impulses(joints_all),
|
||||
// #[cfg(feature = "simd-is-enabled")]
|
||||
// AnyJointVelocityConstraint::WGenericGroundConstraint(c) => {
|
||||
// c.writeback_impulses(joints_all)
|
||||
// }
|
||||
AnyJointVelocityConstraint::PrismaticConstraint(c) => c.writeback_impulses(joints_all),
|
||||
AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => {
|
||||
AnyJointVelocityConstraint::JointGenericConstraint(c) => {
|
||||
c.writeback_impulses(joints_all)
|
||||
}
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WPrismaticConstraint(c) => c.writeback_impulses(joints_all),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WPrismaticGroundConstraint(c) => {
|
||||
c.writeback_impulses(joints_all)
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
AnyJointVelocityConstraint::RevoluteConstraint(c) => c.writeback_impulses(joints_all),
|
||||
#[cfg(feature = "dim3")]
|
||||
AnyJointVelocityConstraint::RevoluteGroundConstraint(c) => {
|
||||
c.writeback_impulses(joints_all)
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WRevoluteConstraint(c) => c.writeback_impulses(joints_all),
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WRevoluteGroundConstraint(c) => {
|
||||
AnyJointVelocityConstraint::JointGenericGroundConstraint(c) => {
|
||||
c.writeback_impulses(joints_all)
|
||||
}
|
||||
AnyJointVelocityConstraint::Empty => unreachable!(),
|
||||
|
||||
@@ -0,0 +1,529 @@
|
||||
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId;
|
||||
use crate::dynamics::solver::joint_constraint::{JointVelocityConstraintBuilder, SolverBody};
|
||||
use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{IntegrationParameters, JointData, JointGraphEdge, JointIndex, Multibody};
|
||||
use crate::math::{Isometry, Real, DIM};
|
||||
use crate::prelude::SPATIAL_DIM;
|
||||
use na::{DVector, DVectorSlice, DVectorSliceMut};
|
||||
|
||||
#[derive(Debug, Copy, Clone)]
|
||||
pub struct JointGenericVelocityConstraint {
|
||||
pub is_rigid_body1: bool,
|
||||
pub is_rigid_body2: bool,
|
||||
pub mj_lambda1: usize,
|
||||
pub mj_lambda2: usize,
|
||||
|
||||
pub ndofs1: usize,
|
||||
pub j_id1: usize,
|
||||
pub ndofs2: usize,
|
||||
pub j_id2: usize,
|
||||
|
||||
pub joint_id: JointIndex,
|
||||
|
||||
pub impulse: Real,
|
||||
pub impulse_bounds: [Real; 2],
|
||||
pub inv_lhs: Real,
|
||||
pub rhs: Real,
|
||||
pub rhs_wo_bias: Real,
|
||||
|
||||
pub writeback_id: WritebackId,
|
||||
}
|
||||
|
||||
impl JointGenericVelocityConstraint {
|
||||
pub fn invalid() -> Self {
|
||||
Self {
|
||||
is_rigid_body1: false,
|
||||
is_rigid_body2: false,
|
||||
mj_lambda1: 0,
|
||||
mj_lambda2: 0,
|
||||
ndofs1: 0,
|
||||
j_id1: 0,
|
||||
ndofs2: 0,
|
||||
j_id2: 0,
|
||||
joint_id: 0,
|
||||
impulse: 0.0,
|
||||
impulse_bounds: [-Real::MAX, Real::MAX],
|
||||
inv_lhs: 0.0,
|
||||
rhs: 0.0,
|
||||
rhs_wo_bias: 0.0,
|
||||
writeback_id: WritebackId::Dof(0),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn lock_axes(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
body1: &SolverBody<Real, 1>,
|
||||
body2: &SolverBody<Real, 1>,
|
||||
mb1: Option<(&Multibody, usize)>,
|
||||
mb2: Option<(&Multibody, usize)>,
|
||||
frame1: &Isometry<Real>,
|
||||
frame2: &Isometry<Real>,
|
||||
joint: &JointData,
|
||||
jacobians: &mut DVector<Real>,
|
||||
j_id: &mut usize,
|
||||
out: &mut [Self],
|
||||
) -> usize {
|
||||
let mut len = 0;
|
||||
let locked_axes = joint.locked_axes.bits();
|
||||
let motor_axes = joint.motor_axes.bits();
|
||||
let limit_axes = joint.limit_axes.bits();
|
||||
|
||||
let builder = JointVelocityConstraintBuilder::new(
|
||||
frame1,
|
||||
frame2,
|
||||
&body1.world_com,
|
||||
&body2.world_com,
|
||||
locked_axes,
|
||||
);
|
||||
|
||||
for i in 0..DIM {
|
||||
if locked_axes & (1 << i) != 0 {
|
||||
out[len] = builder.lock_linear_generic(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
body2,
|
||||
mb1,
|
||||
mb2,
|
||||
i,
|
||||
WritebackId::Dof(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
for i in DIM..SPATIAL_DIM {
|
||||
if locked_axes & (1 << i) != 0 {
|
||||
out[len] = builder.lock_angular_generic(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
body2,
|
||||
mb1,
|
||||
mb2,
|
||||
i - DIM,
|
||||
WritebackId::Dof(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
|
||||
for i in 0..DIM {
|
||||
if motor_axes & (1 << i) != 0 {
|
||||
out[len] = builder.motor_linear_generic(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
body2,
|
||||
mb1,
|
||||
mb2,
|
||||
// locked_ang_axes,
|
||||
i,
|
||||
&joint.motors[i].motor_params(params.dt),
|
||||
WritebackId::Motor(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
|
||||
for i in DIM..SPATIAL_DIM {
|
||||
if (motor_axes >> DIM) & (1 << i) != 0 {
|
||||
out[len] = builder.motor_angular_generic(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
body2,
|
||||
mb1,
|
||||
mb2,
|
||||
i - DIM,
|
||||
&joint.motors[i].motor_params(params.dt),
|
||||
WritebackId::Motor(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
|
||||
for i in 0..DIM {
|
||||
if limit_axes & (1 << i) != 0 {
|
||||
out[len] = builder.limit_linear_generic(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
body2,
|
||||
mb1,
|
||||
mb2,
|
||||
i,
|
||||
[joint.limits[i].min, joint.limits[i].max],
|
||||
WritebackId::Limit(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
|
||||
for i in DIM..SPATIAL_DIM {
|
||||
if limit_axes & (1 << i) != 0 {
|
||||
out[len] = builder.limit_angular_generic(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
body2,
|
||||
mb1,
|
||||
mb2,
|
||||
i - DIM,
|
||||
[joint.limits[i].min, joint.limits[i].max],
|
||||
WritebackId::Limit(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
|
||||
JointVelocityConstraintBuilder::finalize_generic_constraints(jacobians, &mut out[..len]);
|
||||
len
|
||||
}
|
||||
|
||||
fn wj_id1(&self) -> usize {
|
||||
self.j_id1 + self.ndofs1
|
||||
}
|
||||
|
||||
fn wj_id2(&self) -> usize {
|
||||
self.j_id2 + self.ndofs2
|
||||
}
|
||||
|
||||
fn mj_lambda1<'a>(
|
||||
&self,
|
||||
mj_lambdas: &'a [DeltaVel<Real>],
|
||||
generic_mj_lambdas: &'a DVector<Real>,
|
||||
) -> DVectorSlice<'a, Real> {
|
||||
if self.is_rigid_body1 {
|
||||
mj_lambdas[self.mj_lambda1].as_vector_slice()
|
||||
} else {
|
||||
generic_mj_lambdas.rows(self.mj_lambda1, self.ndofs1)
|
||||
}
|
||||
}
|
||||
|
||||
fn mj_lambda1_mut<'a>(
|
||||
&self,
|
||||
mj_lambdas: &'a mut [DeltaVel<Real>],
|
||||
generic_mj_lambdas: &'a mut DVector<Real>,
|
||||
) -> DVectorSliceMut<'a, Real> {
|
||||
if self.is_rigid_body1 {
|
||||
mj_lambdas[self.mj_lambda1].as_vector_slice_mut()
|
||||
} else {
|
||||
generic_mj_lambdas.rows_mut(self.mj_lambda1, self.ndofs1)
|
||||
}
|
||||
}
|
||||
|
||||
fn mj_lambda2<'a>(
|
||||
&self,
|
||||
mj_lambdas: &'a [DeltaVel<Real>],
|
||||
generic_mj_lambdas: &'a DVector<Real>,
|
||||
) -> DVectorSlice<'a, Real> {
|
||||
if self.is_rigid_body2 {
|
||||
mj_lambdas[self.mj_lambda2].as_vector_slice()
|
||||
} else {
|
||||
generic_mj_lambdas.rows(self.mj_lambda2, self.ndofs2)
|
||||
}
|
||||
}
|
||||
|
||||
fn mj_lambda2_mut<'a>(
|
||||
&self,
|
||||
mj_lambdas: &'a mut [DeltaVel<Real>],
|
||||
generic_mj_lambdas: &'a mut DVector<Real>,
|
||||
) -> DVectorSliceMut<'a, Real> {
|
||||
if self.is_rigid_body2 {
|
||||
mj_lambdas[self.mj_lambda2].as_vector_slice_mut()
|
||||
} else {
|
||||
generic_mj_lambdas.rows_mut(self.mj_lambda2, self.ndofs2)
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(
|
||||
&mut self,
|
||||
jacobians: &DVector<Real>,
|
||||
mj_lambdas: &mut [DeltaVel<Real>],
|
||||
generic_mj_lambdas: &mut DVector<Real>,
|
||||
) {
|
||||
let jacobians = jacobians.as_slice();
|
||||
|
||||
let mj_lambda1 = self.mj_lambda1(mj_lambdas, generic_mj_lambdas);
|
||||
let j1 = DVectorSlice::from_slice(&jacobians[self.j_id1..], self.ndofs1);
|
||||
let vel1 = j1.dot(&mj_lambda1);
|
||||
|
||||
let mj_lambda2 = self.mj_lambda2(mj_lambdas, generic_mj_lambdas);
|
||||
let j2 = DVectorSlice::from_slice(&jacobians[self.j_id2..], self.ndofs2);
|
||||
let vel2 = j2.dot(&mj_lambda2);
|
||||
|
||||
let dvel = self.rhs + (vel2 - vel1);
|
||||
let total_impulse = na::clamp(
|
||||
self.impulse + self.inv_lhs * dvel,
|
||||
self.impulse_bounds[0],
|
||||
self.impulse_bounds[1],
|
||||
);
|
||||
let delta_impulse = total_impulse - self.impulse;
|
||||
self.impulse = total_impulse;
|
||||
|
||||
let mut mj_lambda1 = self.mj_lambda1_mut(mj_lambdas, generic_mj_lambdas);
|
||||
let wj1 = DVectorSlice::from_slice(&jacobians[self.wj_id1()..], self.ndofs1);
|
||||
mj_lambda1.axpy(delta_impulse, &wj1, 1.0);
|
||||
|
||||
let mut mj_lambda2 = self.mj_lambda2_mut(mj_lambdas, generic_mj_lambdas);
|
||||
let wj2 = DVectorSlice::from_slice(&jacobians[self.wj_id2()..], self.ndofs2);
|
||||
mj_lambda2.axpy(-delta_impulse, &wj2, 1.0);
|
||||
}
|
||||
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
let joint = &mut joints_all[self.joint_id].weight;
|
||||
match self.writeback_id {
|
||||
WritebackId::Dof(i) => joint.impulses[i] = self.impulse,
|
||||
WritebackId::Limit(i) => joint.data.limits[i].impulse = self.impulse,
|
||||
WritebackId::Motor(i) => joint.data.motors[i].impulse = self.impulse,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn remove_bias_from_rhs(&mut self) {
|
||||
self.rhs = self.rhs_wo_bias;
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Copy, Clone)]
|
||||
pub struct JointGenericVelocityGroundConstraint {
|
||||
pub mj_lambda2: usize,
|
||||
pub ndofs2: usize,
|
||||
pub j_id2: usize,
|
||||
|
||||
pub joint_id: JointIndex,
|
||||
|
||||
pub impulse: Real,
|
||||
pub impulse_bounds: [Real; 2],
|
||||
pub inv_lhs: Real,
|
||||
pub rhs: Real,
|
||||
pub rhs_wo_bias: Real,
|
||||
|
||||
pub writeback_id: WritebackId,
|
||||
}
|
||||
|
||||
impl JointGenericVelocityGroundConstraint {
|
||||
pub fn invalid() -> Self {
|
||||
Self {
|
||||
mj_lambda2: crate::INVALID_USIZE,
|
||||
ndofs2: 0,
|
||||
j_id2: crate::INVALID_USIZE,
|
||||
joint_id: 0,
|
||||
impulse: 0.0,
|
||||
impulse_bounds: [-Real::MAX, Real::MAX],
|
||||
inv_lhs: 0.0,
|
||||
rhs: 0.0,
|
||||
rhs_wo_bias: 0.0,
|
||||
writeback_id: WritebackId::Dof(0),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn lock_axes(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
body1: &SolverBody<Real, 1>,
|
||||
body2: &SolverBody<Real, 1>,
|
||||
mb2: (&Multibody, usize),
|
||||
frame1: &Isometry<Real>,
|
||||
frame2: &Isometry<Real>,
|
||||
joint: &JointData,
|
||||
jacobians: &mut DVector<Real>,
|
||||
j_id: &mut usize,
|
||||
out: &mut [Self],
|
||||
) -> usize {
|
||||
let mut len = 0;
|
||||
let locked_axes = joint.locked_axes.bits();
|
||||
let motor_axes = joint.motor_axes.bits();
|
||||
let limit_axes = joint.limit_axes.bits();
|
||||
|
||||
let builder = JointVelocityConstraintBuilder::new(
|
||||
frame1,
|
||||
frame2,
|
||||
&body1.world_com,
|
||||
&body2.world_com,
|
||||
locked_axes,
|
||||
);
|
||||
|
||||
for i in 0..DIM {
|
||||
if locked_axes & (1 << i) != 0 {
|
||||
out[len] = builder.lock_linear_generic_ground(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
mb2,
|
||||
i,
|
||||
WritebackId::Dof(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
for i in DIM..SPATIAL_DIM {
|
||||
if locked_axes & (1 << i) != 0 {
|
||||
out[len] = builder.lock_angular_generic_ground(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
mb2,
|
||||
i - DIM,
|
||||
WritebackId::Dof(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
|
||||
for i in 0..DIM {
|
||||
if motor_axes & (1 << i) != 0 {
|
||||
out[len] = builder.motor_linear_generic_ground(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
body2,
|
||||
mb2,
|
||||
// locked_ang_axes,
|
||||
i,
|
||||
&joint.motors[i].motor_params(params.dt),
|
||||
WritebackId::Motor(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
|
||||
for i in DIM..SPATIAL_DIM {
|
||||
if (motor_axes >> DIM) & (1 << i) != 0 {
|
||||
out[len] = builder.motor_angular_generic_ground(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
body2,
|
||||
mb2,
|
||||
i - DIM,
|
||||
&joint.motors[i].motor_params(params.dt),
|
||||
WritebackId::Motor(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
|
||||
for i in 0..DIM {
|
||||
if limit_axes & (1 << i) != 0 {
|
||||
out[len] = builder.limit_linear_generic_ground(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
mb2,
|
||||
i,
|
||||
[joint.limits[i].min, joint.limits[i].max],
|
||||
WritebackId::Limit(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
|
||||
for i in DIM..SPATIAL_DIM {
|
||||
if limit_axes & (1 << i) != 0 {
|
||||
out[len] = builder.limit_angular_generic_ground(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
mb2,
|
||||
i - DIM,
|
||||
[joint.limits[i].min, joint.limits[i].max],
|
||||
WritebackId::Limit(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
|
||||
JointVelocityConstraintBuilder::finalize_generic_constraints_ground(
|
||||
jacobians,
|
||||
&mut out[..len],
|
||||
);
|
||||
len
|
||||
}
|
||||
|
||||
fn wj_id2(&self) -> usize {
|
||||
self.j_id2 + self.ndofs2
|
||||
}
|
||||
|
||||
fn mj_lambda2<'a>(
|
||||
&self,
|
||||
_mj_lambdas: &'a [DeltaVel<Real>],
|
||||
generic_mj_lambdas: &'a DVector<Real>,
|
||||
) -> DVectorSlice<'a, Real> {
|
||||
generic_mj_lambdas.rows(self.mj_lambda2, self.ndofs2)
|
||||
}
|
||||
|
||||
fn mj_lambda2_mut<'a>(
|
||||
&self,
|
||||
_mj_lambdas: &'a mut [DeltaVel<Real>],
|
||||
generic_mj_lambdas: &'a mut DVector<Real>,
|
||||
) -> DVectorSliceMut<'a, Real> {
|
||||
generic_mj_lambdas.rows_mut(self.mj_lambda2, self.ndofs2)
|
||||
}
|
||||
|
||||
pub fn solve(
|
||||
&mut self,
|
||||
jacobians: &DVector<Real>,
|
||||
mj_lambdas: &mut [DeltaVel<Real>],
|
||||
generic_mj_lambdas: &mut DVector<Real>,
|
||||
) {
|
||||
let jacobians = jacobians.as_slice();
|
||||
|
||||
let mj_lambda2 = self.mj_lambda2(mj_lambdas, generic_mj_lambdas);
|
||||
let j2 = DVectorSlice::from_slice(&jacobians[self.j_id2..], self.ndofs2);
|
||||
let vel2 = j2.dot(&mj_lambda2);
|
||||
|
||||
let dvel = self.rhs + vel2;
|
||||
let total_impulse = na::clamp(
|
||||
self.impulse + self.inv_lhs * dvel,
|
||||
self.impulse_bounds[0],
|
||||
self.impulse_bounds[1],
|
||||
);
|
||||
let delta_impulse = total_impulse - self.impulse;
|
||||
self.impulse = total_impulse;
|
||||
|
||||
let mut mj_lambda2 = self.mj_lambda2_mut(mj_lambdas, generic_mj_lambdas);
|
||||
let wj2 = DVectorSlice::from_slice(&jacobians[self.wj_id2()..], self.ndofs2);
|
||||
mj_lambda2.axpy(-delta_impulse, &wj2, 1.0);
|
||||
}
|
||||
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
// FIXME: impulse writeback isn’t supported yet for internal multibody_joint constraints.
|
||||
if self.joint_id != usize::MAX {
|
||||
let joint = &mut joints_all[self.joint_id].weight;
|
||||
match self.writeback_id {
|
||||
WritebackId::Dof(i) => joint.impulses[i] = self.impulse,
|
||||
WritebackId::Limit(i) => joint.data.limits[i].impulse = self.impulse,
|
||||
WritebackId::Motor(i) => joint.data.motors[i].impulse = self.impulse,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn remove_bias_from_rhs(&mut self) {
|
||||
self.rhs = self.rhs_wo_bias;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,853 @@
|
||||
use crate::dynamics::solver::joint_constraint::joint_generic_velocity_constraint::{
|
||||
JointGenericVelocityConstraint, JointGenericVelocityGroundConstraint,
|
||||
};
|
||||
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId;
|
||||
use crate::dynamics::solver::joint_constraint::{JointVelocityConstraintBuilder, SolverBody};
|
||||
use crate::dynamics::solver::MotorParameters;
|
||||
use crate::dynamics::{IntegrationParameters, JointIndex, Multibody};
|
||||
use crate::math::{Real, Vector, ANG_DIM, DIM, SPATIAL_DIM};
|
||||
use crate::utils::IndexMut2;
|
||||
use crate::utils::WDot;
|
||||
use na::{DVector, SVector};
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
use crate::utils::WAngularInertia;
|
||||
|
||||
impl SolverBody<Real, 1> {
|
||||
pub fn fill_jacobians(
|
||||
&self,
|
||||
unit_force: Vector<Real>,
|
||||
unit_torque: SVector<Real, ANG_DIM>,
|
||||
j_id: &mut usize,
|
||||
jacobians: &mut DVector<Real>,
|
||||
) -> Real {
|
||||
let wj_id = *j_id + SPATIAL_DIM;
|
||||
jacobians
|
||||
.fixed_rows_mut::<DIM>(*j_id)
|
||||
.copy_from(&unit_force);
|
||||
jacobians
|
||||
.fixed_rows_mut::<ANG_DIM>(*j_id + DIM)
|
||||
.copy_from(&unit_torque);
|
||||
|
||||
{
|
||||
let mut out_invm_j = jacobians.fixed_rows_mut::<SPATIAL_DIM>(wj_id);
|
||||
out_invm_j
|
||||
.fixed_rows_mut::<DIM>(0)
|
||||
.axpy(self.im, &unit_force, 0.0);
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
out_invm_j[DIM] *= self.sqrt_ii;
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
out_invm_j.fixed_rows_mut::<ANG_DIM>(DIM).gemv(
|
||||
1.0,
|
||||
&self.sqrt_ii.into_matrix(),
|
||||
&unit_torque,
|
||||
0.0,
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
*j_id += SPATIAL_DIM * 2;
|
||||
unit_force.dot(&self.linvel) + unit_torque.gdot(self.angvel)
|
||||
}
|
||||
}
|
||||
|
||||
impl JointVelocityConstraintBuilder<Real> {
|
||||
pub fn lock_jacobians_generic(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
jacobians: &mut DVector<Real>,
|
||||
j_id: &mut usize,
|
||||
joint_id: JointIndex,
|
||||
body1: &SolverBody<Real, 1>,
|
||||
body2: &SolverBody<Real, 1>,
|
||||
mb1: Option<(&Multibody, usize)>,
|
||||
mb2: Option<(&Multibody, usize)>,
|
||||
writeback_id: WritebackId,
|
||||
lin_jac: Vector<Real>,
|
||||
ang_jac1: SVector<Real, ANG_DIM>,
|
||||
ang_jac2: SVector<Real, ANG_DIM>,
|
||||
) -> JointGenericVelocityConstraint {
|
||||
let is_rigid_body1 = mb1.is_none();
|
||||
let is_rigid_body2 = mb2.is_none();
|
||||
|
||||
let ndofs1 = mb1.map(|(m, _)| m.ndofs()).unwrap_or(SPATIAL_DIM);
|
||||
let ndofs2 = mb2.map(|(m, _)| m.ndofs()).unwrap_or(SPATIAL_DIM);
|
||||
|
||||
let j_id1 = *j_id;
|
||||
let vel1 = if let Some((mb1, link_id1)) = mb1 {
|
||||
mb1.fill_jacobians(link_id1, lin_jac, ang_jac1, j_id, jacobians)
|
||||
.1
|
||||
} else {
|
||||
body1.fill_jacobians(lin_jac, ang_jac1, j_id, jacobians)
|
||||
};
|
||||
|
||||
let j_id2 = *j_id;
|
||||
let vel2 = if let Some((mb2, link_id2)) = mb2 {
|
||||
mb2.fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians)
|
||||
.1
|
||||
} else {
|
||||
body2.fill_jacobians(lin_jac, ang_jac2, j_id, jacobians)
|
||||
};
|
||||
|
||||
if is_rigid_body1 {
|
||||
let ang_j_id1 = j_id1 + DIM;
|
||||
let ang_wj_id1 = j_id1 + DIM + ndofs1;
|
||||
let (mut j, wj) = jacobians.rows_range_pair_mut(
|
||||
ang_j_id1..ang_j_id1 + ANG_DIM,
|
||||
ang_wj_id1..ang_wj_id1 + ANG_DIM,
|
||||
);
|
||||
j.copy_from(&wj);
|
||||
}
|
||||
|
||||
if is_rigid_body2 {
|
||||
let ang_j_id2 = j_id2 + DIM;
|
||||
let ang_wj_id2 = j_id2 + DIM + ndofs2;
|
||||
let (mut j, wj) = jacobians.rows_range_pair_mut(
|
||||
ang_j_id2..ang_j_id2 + ANG_DIM,
|
||||
ang_wj_id2..ang_wj_id2 + ANG_DIM,
|
||||
);
|
||||
j.copy_from(&wj);
|
||||
}
|
||||
|
||||
let rhs_wo_bias = (vel2 - vel1) * params.velocity_solve_fraction;
|
||||
|
||||
let mj_lambda1 = mb1.map(|m| m.0.solver_id).unwrap_or(body1.mj_lambda[0]);
|
||||
let mj_lambda2 = mb2.map(|m| m.0.solver_id).unwrap_or(body2.mj_lambda[0]);
|
||||
|
||||
JointGenericVelocityConstraint {
|
||||
is_rigid_body1,
|
||||
is_rigid_body2,
|
||||
mj_lambda1,
|
||||
mj_lambda2,
|
||||
ndofs1,
|
||||
j_id1,
|
||||
ndofs2,
|
||||
j_id2,
|
||||
joint_id,
|
||||
impulse: 0.0,
|
||||
impulse_bounds: [-Real::MAX, Real::MAX],
|
||||
inv_lhs: 0.0,
|
||||
rhs: rhs_wo_bias,
|
||||
rhs_wo_bias,
|
||||
writeback_id,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn lock_linear_generic(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
jacobians: &mut DVector<Real>,
|
||||
j_id: &mut usize,
|
||||
joint_id: JointIndex,
|
||||
body1: &SolverBody<Real, 1>,
|
||||
body2: &SolverBody<Real, 1>,
|
||||
mb1: Option<(&Multibody, usize)>,
|
||||
mb2: Option<(&Multibody, usize)>,
|
||||
locked_axis: usize,
|
||||
writeback_id: WritebackId,
|
||||
) -> JointGenericVelocityConstraint {
|
||||
let lin_jac = self.basis.column(locked_axis).into_owned();
|
||||
let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned();
|
||||
let ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned();
|
||||
|
||||
let mut c = self.lock_jacobians_generic(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
body2,
|
||||
mb1,
|
||||
mb2,
|
||||
writeback_id,
|
||||
lin_jac,
|
||||
ang_jac1,
|
||||
ang_jac2,
|
||||
);
|
||||
|
||||
let erp_inv_dt = params.erp_inv_dt();
|
||||
let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
|
||||
c.rhs += rhs_bias;
|
||||
c
|
||||
}
|
||||
|
||||
pub fn limit_linear_generic(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
jacobians: &mut DVector<Real>,
|
||||
j_id: &mut usize,
|
||||
joint_id: JointIndex,
|
||||
body1: &SolverBody<Real, 1>,
|
||||
body2: &SolverBody<Real, 1>,
|
||||
mb1: Option<(&Multibody, usize)>,
|
||||
mb2: Option<(&Multibody, usize)>,
|
||||
limited_axis: usize,
|
||||
limits: [Real; 2],
|
||||
writeback_id: WritebackId,
|
||||
) -> JointGenericVelocityConstraint {
|
||||
let lin_jac = self.basis.column(limited_axis).into_owned();
|
||||
let ang_jac1 = self.cmat1_basis.column(limited_axis).into_owned();
|
||||
let ang_jac2 = self.cmat2_basis.column(limited_axis).into_owned();
|
||||
|
||||
let mut constraint = self.lock_jacobians_generic(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
body2,
|
||||
mb1,
|
||||
mb2,
|
||||
writeback_id,
|
||||
lin_jac,
|
||||
ang_jac1,
|
||||
ang_jac2,
|
||||
);
|
||||
|
||||
let dist = self.lin_err.dot(&lin_jac);
|
||||
let min_enabled = dist < limits[0];
|
||||
let max_enabled = limits[1] < dist;
|
||||
|
||||
let erp_inv_dt = params.erp_inv_dt();
|
||||
let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
|
||||
constraint.rhs += rhs_bias;
|
||||
constraint.impulse_bounds = [
|
||||
min_enabled as u32 as Real * -Real::MAX,
|
||||
max_enabled as u32 as Real * Real::MAX,
|
||||
];
|
||||
|
||||
constraint
|
||||
}
|
||||
|
||||
pub fn motor_linear_generic(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
jacobians: &mut DVector<Real>,
|
||||
j_id: &mut usize,
|
||||
joint_id: JointIndex,
|
||||
body1: &SolverBody<Real, 1>,
|
||||
body2: &SolverBody<Real, 1>,
|
||||
mb1: Option<(&Multibody, usize)>,
|
||||
mb2: Option<(&Multibody, usize)>,
|
||||
motor_axis: usize,
|
||||
motor_params: &MotorParameters<Real>,
|
||||
writeback_id: WritebackId,
|
||||
) -> JointGenericVelocityConstraint {
|
||||
let lin_jac = self.basis.column(motor_axis).into_owned();
|
||||
let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned();
|
||||
let ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned();
|
||||
|
||||
// TODO: do we need the same trick as for the non-generic constraint?
|
||||
// if locked_ang_axes & (1 << motor_axis) != 0 {
|
||||
// // FIXME: check that this also works for cases
|
||||
// // whene not all the angular axes are locked.
|
||||
// constraint.ang_jac1.fill(0.0);
|
||||
// constraint.ang_jac2.fill(0.0);
|
||||
// }
|
||||
|
||||
let mut constraint = self.lock_jacobians_generic(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
body2,
|
||||
mb1,
|
||||
mb2,
|
||||
writeback_id,
|
||||
lin_jac,
|
||||
ang_jac1,
|
||||
ang_jac2,
|
||||
);
|
||||
|
||||
let mut rhs_wo_bias = 0.0;
|
||||
if motor_params.stiffness != 0.0 {
|
||||
let dist = self.lin_err.dot(&lin_jac);
|
||||
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
|
||||
}
|
||||
|
||||
if motor_params.damping != 0.0 {
|
||||
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
|
||||
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
|
||||
rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
|
||||
}
|
||||
|
||||
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
|
||||
constraint.rhs = rhs_wo_bias;
|
||||
constraint.rhs_wo_bias = rhs_wo_bias;
|
||||
constraint
|
||||
}
|
||||
|
||||
pub fn lock_angular_generic(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
jacobians: &mut DVector<Real>,
|
||||
j_id: &mut usize,
|
||||
joint_id: JointIndex,
|
||||
body1: &SolverBody<Real, 1>,
|
||||
body2: &SolverBody<Real, 1>,
|
||||
mb1: Option<(&Multibody, usize)>,
|
||||
mb2: Option<(&Multibody, usize)>,
|
||||
locked_axis: usize,
|
||||
writeback_id: WritebackId,
|
||||
) -> JointGenericVelocityConstraint {
|
||||
let ang_jac = self.ang_basis.column(locked_axis).into_owned();
|
||||
|
||||
let mut constraint = self.lock_jacobians_generic(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
body2,
|
||||
mb1,
|
||||
mb2,
|
||||
writeback_id,
|
||||
na::zero(),
|
||||
ang_jac,
|
||||
ang_jac,
|
||||
);
|
||||
|
||||
let erp_inv_dt = params.erp_inv_dt();
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs_bias = self.ang_err.im * erp_inv_dt;
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
|
||||
constraint.rhs += rhs_bias;
|
||||
constraint
|
||||
}
|
||||
|
||||
pub fn limit_angular_generic(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
jacobians: &mut DVector<Real>,
|
||||
j_id: &mut usize,
|
||||
joint_id: JointIndex,
|
||||
body1: &SolverBody<Real, 1>,
|
||||
body2: &SolverBody<Real, 1>,
|
||||
mb1: Option<(&Multibody, usize)>,
|
||||
mb2: Option<(&Multibody, usize)>,
|
||||
limited_axis: usize,
|
||||
limits: [Real; 2],
|
||||
writeback_id: WritebackId,
|
||||
) -> JointGenericVelocityConstraint {
|
||||
let ang_jac = self.ang_basis.column(limited_axis).into_owned();
|
||||
|
||||
let mut constraint = self.lock_jacobians_generic(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
body2,
|
||||
mb1,
|
||||
mb2,
|
||||
writeback_id,
|
||||
na::zero(),
|
||||
ang_jac,
|
||||
ang_jac,
|
||||
);
|
||||
|
||||
let s_limits = [(limits[0] / 2.0).sin(), (limits[1] / 2.0).sin()];
|
||||
#[cfg(feature = "dim2")]
|
||||
let s_ang = self.ang_err.im;
|
||||
#[cfg(feature = "dim3")]
|
||||
let s_ang = self.ang_err.imag()[limited_axis];
|
||||
let min_enabled = s_ang < s_limits[0];
|
||||
let max_enabled = s_limits[1] < s_ang;
|
||||
let impulse_bounds = [
|
||||
min_enabled as u32 as Real * -Real::MAX,
|
||||
max_enabled as u32 as Real * Real::MAX,
|
||||
];
|
||||
|
||||
let erp_inv_dt = params.erp_inv_dt();
|
||||
let rhs_bias =
|
||||
((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt;
|
||||
|
||||
constraint.rhs += rhs_bias;
|
||||
constraint.impulse_bounds = impulse_bounds;
|
||||
constraint
|
||||
}
|
||||
|
||||
pub fn motor_angular_generic(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
jacobians: &mut DVector<Real>,
|
||||
j_id: &mut usize,
|
||||
joint_id: JointIndex,
|
||||
body1: &SolverBody<Real, 1>,
|
||||
body2: &SolverBody<Real, 1>,
|
||||
mb1: Option<(&Multibody, usize)>,
|
||||
mb2: Option<(&Multibody, usize)>,
|
||||
_motor_axis: usize,
|
||||
motor_params: &MotorParameters<Real>,
|
||||
writeback_id: WritebackId,
|
||||
) -> JointGenericVelocityConstraint {
|
||||
// let mut ang_jac = self.ang_basis.column(motor_axis).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_jac = na::Vector1::new(1.0);
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_jac = self.basis.column(_motor_axis).into_owned();
|
||||
|
||||
let mut constraint = self.lock_jacobians_generic(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
body2,
|
||||
mb1,
|
||||
mb2,
|
||||
writeback_id,
|
||||
na::zero(),
|
||||
ang_jac,
|
||||
ang_jac,
|
||||
);
|
||||
|
||||
let mut rhs_wo_bias = 0.0;
|
||||
if motor_params.stiffness != 0.0 {
|
||||
#[cfg(feature = "dim2")]
|
||||
let s_ang_dist = self.ang_err.im;
|
||||
#[cfg(feature = "dim3")]
|
||||
let s_ang_dist = self.ang_err.imag()[_motor_axis];
|
||||
let s_target_ang = motor_params.target_pos.sin();
|
||||
rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.stiffness;
|
||||
}
|
||||
|
||||
if motor_params.damping != 0.0 {
|
||||
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
|
||||
rhs_wo_bias += (dvel - motor_params.target_vel * ang_jac.norm()) * motor_params.damping;
|
||||
}
|
||||
|
||||
constraint.rhs_wo_bias = rhs_wo_bias;
|
||||
constraint.rhs = rhs_wo_bias;
|
||||
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
|
||||
constraint
|
||||
}
|
||||
|
||||
pub fn finalize_generic_constraints(
|
||||
jacobians: &mut DVector<Real>,
|
||||
constraints: &mut [JointGenericVelocityConstraint],
|
||||
) {
|
||||
// TODO: orthogonalization doesn’t seem to give good results for multibodies?
|
||||
const ORTHOGONALIZE: bool = false;
|
||||
let len = constraints.len();
|
||||
let ndofs1 = constraints[0].ndofs1;
|
||||
let ndofs2 = constraints[0].ndofs2;
|
||||
|
||||
// Use the modified Gramm-Schmidt orthogonalization.
|
||||
for j in 0..len {
|
||||
let c_j = &mut constraints[j];
|
||||
|
||||
let jac_j1 = jacobians.rows(c_j.j_id1, ndofs1);
|
||||
let jac_j2 = jacobians.rows(c_j.j_id2, ndofs2);
|
||||
let w_jac_j1 = jacobians.rows(c_j.j_id1 + ndofs1, ndofs1);
|
||||
let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2);
|
||||
|
||||
let dot_jj = jac_j1.dot(&w_jac_j1) + jac_j2.dot(&w_jac_j2);
|
||||
let inv_dot_jj = crate::utils::inv(dot_jj);
|
||||
c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
|
||||
|
||||
if c_j.impulse_bounds != [-Real::MAX, Real::MAX] {
|
||||
// Don't remove constraints with limited forces from the others
|
||||
// because they may not deliver the necessary forces to fulfill
|
||||
// the removed parts of other constraints.
|
||||
continue;
|
||||
}
|
||||
|
||||
if !ORTHOGONALIZE {
|
||||
continue;
|
||||
}
|
||||
|
||||
for i in (j + 1)..len {
|
||||
let (c_i, c_j) = constraints.index_mut_const(i, j);
|
||||
|
||||
let jac_i1 = jacobians.rows(c_i.j_id1, ndofs1);
|
||||
let jac_i2 = jacobians.rows(c_i.j_id2, ndofs2);
|
||||
let w_jac_j1 = jacobians.rows(c_j.j_id1 + ndofs1, ndofs1);
|
||||
let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2);
|
||||
|
||||
let dot_ij = jac_i1.dot(&w_jac_j1) + jac_i2.dot(&w_jac_j2);
|
||||
let coeff = dot_ij * inv_dot_jj;
|
||||
|
||||
let (mut jac_i, jac_j) = jacobians.rows_range_pair_mut(
|
||||
c_i.j_id1..c_i.j_id1 + 2 * (ndofs1 + ndofs2),
|
||||
c_j.j_id1..c_j.j_id1 + 2 * (ndofs1 + ndofs2),
|
||||
);
|
||||
|
||||
jac_i.axpy(-coeff, &jac_j, 1.0);
|
||||
|
||||
c_i.rhs_wo_bias -= c_j.rhs_wo_bias * coeff;
|
||||
c_i.rhs -= c_j.rhs * coeff;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl JointVelocityConstraintBuilder<Real> {
|
||||
pub fn lock_jacobians_generic_ground(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
jacobians: &mut DVector<Real>,
|
||||
j_id: &mut usize,
|
||||
joint_id: JointIndex,
|
||||
body1: &SolverBody<Real, 1>,
|
||||
(mb2, link_id2): (&Multibody, usize),
|
||||
writeback_id: WritebackId,
|
||||
lin_jac: Vector<Real>,
|
||||
ang_jac1: SVector<Real, ANG_DIM>,
|
||||
ang_jac2: SVector<Real, ANG_DIM>,
|
||||
) -> JointGenericVelocityGroundConstraint {
|
||||
let ndofs2 = mb2.ndofs();
|
||||
|
||||
let vel1 = lin_jac.dot(&body1.linvel) + ang_jac1.gdot(body1.angvel);
|
||||
|
||||
let j_id2 = *j_id;
|
||||
let vel2 = mb2
|
||||
.fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians)
|
||||
.1;
|
||||
let rhs_wo_bias = (vel2 - vel1) * params.velocity_solve_fraction;
|
||||
|
||||
let mj_lambda2 = mb2.solver_id;
|
||||
|
||||
JointGenericVelocityGroundConstraint {
|
||||
mj_lambda2,
|
||||
ndofs2,
|
||||
j_id2,
|
||||
joint_id,
|
||||
impulse: 0.0,
|
||||
impulse_bounds: [-Real::MAX, Real::MAX],
|
||||
inv_lhs: 0.0,
|
||||
rhs: rhs_wo_bias,
|
||||
rhs_wo_bias,
|
||||
writeback_id,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn lock_linear_generic_ground(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
jacobians: &mut DVector<Real>,
|
||||
j_id: &mut usize,
|
||||
joint_id: JointIndex,
|
||||
body1: &SolverBody<Real, 1>,
|
||||
mb2: (&Multibody, usize),
|
||||
locked_axis: usize,
|
||||
writeback_id: WritebackId,
|
||||
) -> JointGenericVelocityGroundConstraint {
|
||||
let lin_jac = self.basis.column(locked_axis).into_owned();
|
||||
let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned();
|
||||
let ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned();
|
||||
|
||||
let mut c = self.lock_jacobians_generic_ground(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
mb2,
|
||||
writeback_id,
|
||||
lin_jac,
|
||||
ang_jac1,
|
||||
ang_jac2,
|
||||
);
|
||||
|
||||
let erp_inv_dt = params.erp_inv_dt();
|
||||
let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
|
||||
c.rhs += rhs_bias;
|
||||
c
|
||||
}
|
||||
|
||||
pub fn limit_linear_generic_ground(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
jacobians: &mut DVector<Real>,
|
||||
j_id: &mut usize,
|
||||
joint_id: JointIndex,
|
||||
body1: &SolverBody<Real, 1>,
|
||||
mb2: (&Multibody, usize),
|
||||
limited_axis: usize,
|
||||
limits: [Real; 2],
|
||||
writeback_id: WritebackId,
|
||||
) -> JointGenericVelocityGroundConstraint {
|
||||
let lin_jac = self.basis.column(limited_axis).into_owned();
|
||||
let ang_jac1 = self.cmat1_basis.column(limited_axis).into_owned();
|
||||
let ang_jac2 = self.cmat2_basis.column(limited_axis).into_owned();
|
||||
|
||||
let mut constraint = self.lock_jacobians_generic_ground(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
mb2,
|
||||
writeback_id,
|
||||
lin_jac,
|
||||
ang_jac1,
|
||||
ang_jac2,
|
||||
);
|
||||
|
||||
let dist = self.lin_err.dot(&lin_jac);
|
||||
let min_enabled = dist < limits[0];
|
||||
let max_enabled = limits[1] < dist;
|
||||
|
||||
let erp_inv_dt = params.erp_inv_dt();
|
||||
let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
|
||||
constraint.rhs += rhs_bias;
|
||||
constraint.impulse_bounds = [
|
||||
min_enabled as u32 as Real * -Real::MAX,
|
||||
max_enabled as u32 as Real * Real::MAX,
|
||||
];
|
||||
|
||||
constraint
|
||||
}
|
||||
|
||||
pub fn motor_linear_generic_ground(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
jacobians: &mut DVector<Real>,
|
||||
j_id: &mut usize,
|
||||
joint_id: JointIndex,
|
||||
body1: &SolverBody<Real, 1>,
|
||||
body2: &SolverBody<Real, 1>, // TODO: we shouldn’t need this.
|
||||
mb2: (&Multibody, usize),
|
||||
motor_axis: usize,
|
||||
motor_params: &MotorParameters<Real>,
|
||||
writeback_id: WritebackId,
|
||||
) -> JointGenericVelocityGroundConstraint {
|
||||
let lin_jac = self.basis.column(motor_axis).into_owned();
|
||||
let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned();
|
||||
let ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned();
|
||||
|
||||
// TODO: do we need the same trick as for the non-generic constraint?
|
||||
// if locked_ang_axes & (1 << motor_axis) != 0 {
|
||||
// // FIXME: check that this also works for cases
|
||||
// // whene not all the angular axes are locked.
|
||||
// constraint.ang_jac1.fill(0.0);
|
||||
// constraint.ang_jac2.fill(0.0);
|
||||
// }
|
||||
|
||||
let mut constraint = self.lock_jacobians_generic_ground(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
mb2,
|
||||
writeback_id,
|
||||
lin_jac,
|
||||
ang_jac1,
|
||||
ang_jac2,
|
||||
);
|
||||
|
||||
let mut rhs_wo_bias = 0.0;
|
||||
if motor_params.stiffness != 0.0 {
|
||||
let dist = self.lin_err.dot(&lin_jac);
|
||||
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
|
||||
}
|
||||
|
||||
if motor_params.damping != 0.0 {
|
||||
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
|
||||
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
|
||||
rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
|
||||
}
|
||||
|
||||
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
|
||||
constraint.rhs = rhs_wo_bias;
|
||||
constraint.rhs_wo_bias = rhs_wo_bias;
|
||||
constraint
|
||||
}
|
||||
|
||||
pub fn lock_angular_generic_ground(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
jacobians: &mut DVector<Real>,
|
||||
j_id: &mut usize,
|
||||
joint_id: JointIndex,
|
||||
body1: &SolverBody<Real, 1>,
|
||||
mb2: (&Multibody, usize),
|
||||
locked_axis: usize,
|
||||
writeback_id: WritebackId,
|
||||
) -> JointGenericVelocityGroundConstraint {
|
||||
let ang_jac = self.ang_basis.column(locked_axis).into_owned();
|
||||
|
||||
let mut constraint = self.lock_jacobians_generic_ground(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
mb2,
|
||||
writeback_id,
|
||||
na::zero(),
|
||||
ang_jac,
|
||||
ang_jac,
|
||||
);
|
||||
|
||||
let erp_inv_dt = params.erp_inv_dt();
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs_bias = self.ang_err.im * erp_inv_dt;
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
|
||||
constraint.rhs += rhs_bias;
|
||||
constraint
|
||||
}
|
||||
|
||||
pub fn limit_angular_generic_ground(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
jacobians: &mut DVector<Real>,
|
||||
j_id: &mut usize,
|
||||
joint_id: JointIndex,
|
||||
body1: &SolverBody<Real, 1>,
|
||||
mb2: (&Multibody, usize),
|
||||
limited_axis: usize,
|
||||
limits: [Real; 2],
|
||||
writeback_id: WritebackId,
|
||||
) -> JointGenericVelocityGroundConstraint {
|
||||
let ang_jac = self.ang_basis.column(limited_axis).into_owned();
|
||||
|
||||
let mut constraint = self.lock_jacobians_generic_ground(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
mb2,
|
||||
writeback_id,
|
||||
na::zero(),
|
||||
ang_jac,
|
||||
ang_jac,
|
||||
);
|
||||
|
||||
let s_limits = [(limits[0] / 2.0).sin(), (limits[1] / 2.0).sin()];
|
||||
#[cfg(feature = "dim2")]
|
||||
let s_ang = self.ang_err.im;
|
||||
#[cfg(feature = "dim3")]
|
||||
let s_ang = self.ang_err.imag()[limited_axis];
|
||||
let min_enabled = s_ang < s_limits[0];
|
||||
let max_enabled = s_limits[1] < s_ang;
|
||||
let impulse_bounds = [
|
||||
min_enabled as u32 as Real * -Real::MAX,
|
||||
max_enabled as u32 as Real * Real::MAX,
|
||||
];
|
||||
|
||||
let erp_inv_dt = params.erp_inv_dt();
|
||||
let rhs_bias =
|
||||
((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt;
|
||||
|
||||
constraint.rhs += rhs_bias;
|
||||
constraint.impulse_bounds = impulse_bounds;
|
||||
constraint
|
||||
}
|
||||
|
||||
pub fn motor_angular_generic_ground(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
jacobians: &mut DVector<Real>,
|
||||
j_id: &mut usize,
|
||||
joint_id: JointIndex,
|
||||
body1: &SolverBody<Real, 1>,
|
||||
body2: &SolverBody<Real, 1>, // TODO: we shouldn’t need this.
|
||||
mb2: (&Multibody, usize),
|
||||
_motor_axis: usize,
|
||||
motor_params: &MotorParameters<Real>,
|
||||
writeback_id: WritebackId,
|
||||
) -> JointGenericVelocityGroundConstraint {
|
||||
// let mut ang_jac = self.ang_basis.column(_motor_axis).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_jac = na::Vector1::new(1.0);
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_jac = self.basis.column(_motor_axis).into_owned();
|
||||
|
||||
let mut constraint = self.lock_jacobians_generic_ground(
|
||||
params,
|
||||
jacobians,
|
||||
j_id,
|
||||
joint_id,
|
||||
body1,
|
||||
mb2,
|
||||
writeback_id,
|
||||
na::zero(),
|
||||
ang_jac,
|
||||
ang_jac,
|
||||
);
|
||||
|
||||
let mut rhs = 0.0;
|
||||
if motor_params.stiffness != 0.0 {
|
||||
#[cfg(feature = "dim2")]
|
||||
let s_ang_dist = self.ang_err.im;
|
||||
#[cfg(feature = "dim3")]
|
||||
let s_ang_dist = self.ang_err.imag()[_motor_axis];
|
||||
let s_target_ang = motor_params.target_pos.sin();
|
||||
rhs += (s_ang_dist - s_target_ang) * motor_params.stiffness;
|
||||
}
|
||||
|
||||
if motor_params.damping != 0.0 {
|
||||
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
|
||||
rhs += (dvel - motor_params.target_vel * ang_jac.norm()) * motor_params.damping;
|
||||
}
|
||||
|
||||
constraint.rhs_wo_bias = rhs;
|
||||
constraint.rhs = rhs;
|
||||
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
|
||||
constraint
|
||||
}
|
||||
|
||||
pub fn finalize_generic_constraints_ground(
|
||||
jacobians: &mut DVector<Real>,
|
||||
constraints: &mut [JointGenericVelocityGroundConstraint],
|
||||
) {
|
||||
// TODO: orthogonalization doesn’t seem to give good results for multibodies?
|
||||
const ORTHOGONALIZE: bool = false;
|
||||
let len = constraints.len();
|
||||
let ndofs2 = constraints[0].ndofs2;
|
||||
|
||||
// Use the modified Gramm-Schmidt orthogonalization.
|
||||
for j in 0..len {
|
||||
let c_j = &mut constraints[j];
|
||||
|
||||
let jac_j2 = jacobians.rows(c_j.j_id2, ndofs2);
|
||||
let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2);
|
||||
|
||||
let dot_jj = jac_j2.dot(&w_jac_j2);
|
||||
let inv_dot_jj = crate::utils::inv(dot_jj);
|
||||
c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
|
||||
|
||||
if c_j.impulse_bounds != [-Real::MAX, Real::MAX] {
|
||||
// Don't remove constraints with limited forces from the others
|
||||
// because they may not deliver the necessary forces to fulfill
|
||||
// the removed parts of other constraints.
|
||||
continue;
|
||||
}
|
||||
|
||||
if !ORTHOGONALIZE {
|
||||
continue;
|
||||
}
|
||||
|
||||
for i in (j + 1)..len {
|
||||
let (c_i, c_j) = constraints.index_mut_const(i, j);
|
||||
|
||||
let jac_i2 = jacobians.rows(c_i.j_id2, ndofs2);
|
||||
let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2);
|
||||
|
||||
let dot_ij = jac_i2.dot(&w_jac_j2);
|
||||
let coeff = dot_ij * inv_dot_jj;
|
||||
|
||||
let (mut jac_i, jac_j) = jacobians.rows_range_pair_mut(
|
||||
c_i.j_id2..c_i.j_id2 + 2 * ndofs2,
|
||||
c_j.j_id2..c_j.j_id2 + 2 * ndofs2,
|
||||
);
|
||||
|
||||
jac_i.axpy(-coeff, &jac_j, 1.0);
|
||||
|
||||
c_i.rhs_wo_bias -= c_j.rhs_wo_bias * coeff;
|
||||
c_i.rhs -= c_j.rhs * coeff;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,280 +0,0 @@
|
||||
use super::{
|
||||
BallPositionConstraint, BallPositionGroundConstraint, FixedPositionConstraint,
|
||||
FixedPositionGroundConstraint, PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
|
||||
};
|
||||
#[cfg(feature = "dim3")]
|
||||
use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
|
||||
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
|
||||
use super::{WRevolutePositionConstraint, WRevolutePositionGroundConstraint};
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
use super::{
|
||||
WBallPositionConstraint, WBallPositionGroundConstraint, WFixedPositionConstraint,
|
||||
WFixedPositionGroundConstraint, WPrismaticPositionConstraint,
|
||||
WPrismaticPositionGroundConstraint,
|
||||
};
|
||||
use crate::data::{BundleSet, ComponentSet};
|
||||
use crate::dynamics::{
|
||||
IntegrationParameters, Joint, JointParams, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
|
||||
RigidBodyType,
|
||||
};
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
use crate::math::SIMD_WIDTH;
|
||||
use crate::math::{Isometry, Real};
|
||||
|
||||
pub(crate) enum AnyJointPositionConstraint {
|
||||
BallJoint(BallPositionConstraint),
|
||||
BallGroundConstraint(BallPositionGroundConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WBallJoint(WBallPositionConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WBallGroundConstraint(WBallPositionGroundConstraint),
|
||||
FixedJoint(FixedPositionConstraint),
|
||||
FixedGroundConstraint(FixedPositionGroundConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WFixedJoint(WFixedPositionConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WFixedGroundConstraint(WFixedPositionGroundConstraint),
|
||||
// GenericJoint(GenericPositionConstraint),
|
||||
// GenericGroundConstraint(GenericPositionGroundConstraint),
|
||||
// #[cfg(feature = "simd-is-enabled")]
|
||||
// WGenericJoint(WGenericPositionConstraint),
|
||||
// #[cfg(feature = "simd-is-enabled")]
|
||||
// WGenericGroundConstraint(WGenericPositionGroundConstraint),
|
||||
PrismaticJoint(PrismaticPositionConstraint),
|
||||
PrismaticGroundConstraint(PrismaticPositionGroundConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WPrismaticJoint(WPrismaticPositionConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WPrismaticGroundConstraint(WPrismaticPositionGroundConstraint),
|
||||
#[cfg(feature = "dim3")]
|
||||
RevoluteJoint(RevolutePositionConstraint),
|
||||
#[cfg(feature = "dim3")]
|
||||
RevoluteGroundConstraint(RevolutePositionGroundConstraint),
|
||||
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
|
||||
WRevoluteJoint(WRevolutePositionConstraint),
|
||||
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
|
||||
WRevoluteGroundConstraint(WRevolutePositionGroundConstraint),
|
||||
#[allow(dead_code)] // The Empty variant is only used with parallel code.
|
||||
Empty,
|
||||
}
|
||||
|
||||
impl AnyJointPositionConstraint {
|
||||
pub fn from_joint<Bodies>(joint: &Joint, bodies: &Bodies) -> Self
|
||||
where
|
||||
Bodies: ComponentSet<RigidBodyMassProps> + ComponentSet<RigidBodyIds>,
|
||||
{
|
||||
let rb1 = bodies.index_bundle(joint.body1.0);
|
||||
let rb2 = bodies.index_bundle(joint.body2.0);
|
||||
|
||||
match &joint.params {
|
||||
JointParams::BallJoint(p) => AnyJointPositionConstraint::BallJoint(
|
||||
BallPositionConstraint::from_params(rb1, rb2, p),
|
||||
),
|
||||
JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedJoint(
|
||||
FixedPositionConstraint::from_params(rb1, rb2, p),
|
||||
),
|
||||
// JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericJoint(
|
||||
// GenericPositionConstraint::from_params(rb1, rb2, p),
|
||||
// ),
|
||||
JointParams::PrismaticJoint(p) => AnyJointPositionConstraint::PrismaticJoint(
|
||||
PrismaticPositionConstraint::from_params(rb1, rb2, p),
|
||||
),
|
||||
#[cfg(feature = "dim3")]
|
||||
JointParams::RevoluteJoint(p) => AnyJointPositionConstraint::RevoluteJoint(
|
||||
RevolutePositionConstraint::from_params(rb1, rb2, p),
|
||||
),
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub fn from_wide_joint<Bodies>(joints: [&Joint; SIMD_WIDTH], bodies: &Bodies) -> Self
|
||||
where
|
||||
Bodies: ComponentSet<RigidBodyMassProps> + ComponentSet<RigidBodyIds>,
|
||||
{
|
||||
let rbs1 = (
|
||||
gather![|ii| bodies.index(joints[ii].body1.0)],
|
||||
gather![|ii| bodies.index(joints[ii].body1.0)],
|
||||
);
|
||||
let rbs2 = (
|
||||
gather![|ii| bodies.index(joints[ii].body2.0)],
|
||||
gather![|ii| bodies.index(joints[ii].body2.0)],
|
||||
);
|
||||
|
||||
match &joints[0].params {
|
||||
JointParams::BallJoint(_) => {
|
||||
let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
|
||||
AnyJointPositionConstraint::WBallJoint(WBallPositionConstraint::from_params(
|
||||
rbs1, rbs2, joints,
|
||||
))
|
||||
}
|
||||
JointParams::FixedJoint(_) => {
|
||||
let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
|
||||
AnyJointPositionConstraint::WFixedJoint(WFixedPositionConstraint::from_params(
|
||||
rbs1, rbs2, joints,
|
||||
))
|
||||
}
|
||||
// JointParams::GenericJoint(_) => {
|
||||
// let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
|
||||
// AnyJointPositionConstraint::WGenericJoint(WGenericPositionConstraint::from_params(
|
||||
// rbs1, rbs2, joints,
|
||||
// ))
|
||||
// }
|
||||
JointParams::PrismaticJoint(_) => {
|
||||
let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
|
||||
AnyJointPositionConstraint::WPrismaticJoint(
|
||||
WPrismaticPositionConstraint::from_params(rbs1, rbs2, joints),
|
||||
)
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
JointParams::RevoluteJoint(_) => {
|
||||
let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
|
||||
AnyJointPositionConstraint::WRevoluteJoint(
|
||||
WRevolutePositionConstraint::from_params(rbs1, rbs2, joints),
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn from_joint_ground<Bodies>(joint: &Joint, bodies: &Bodies) -> Self
|
||||
where
|
||||
Bodies: ComponentSet<RigidBodyType>
|
||||
+ ComponentSet<RigidBodyPosition>
|
||||
+ ComponentSet<RigidBodyMassProps>
|
||||
+ ComponentSet<RigidBodyIds>,
|
||||
{
|
||||
let mut handle1 = joint.body1;
|
||||
let mut handle2 = joint.body2;
|
||||
|
||||
let status2: &RigidBodyType = bodies.index(handle2.0);
|
||||
let flipped = !status2.is_dynamic();
|
||||
|
||||
if flipped {
|
||||
std::mem::swap(&mut handle1, &mut handle2);
|
||||
}
|
||||
|
||||
let rb1 = bodies.index(handle1.0);
|
||||
let rb2 = (bodies.index(handle2.0), bodies.index(handle2.0));
|
||||
|
||||
match &joint.params {
|
||||
JointParams::BallJoint(p) => AnyJointPositionConstraint::BallGroundConstraint(
|
||||
BallPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
|
||||
),
|
||||
JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedGroundConstraint(
|
||||
FixedPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
|
||||
),
|
||||
// JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericGroundConstraint(
|
||||
// GenericPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
|
||||
// ),
|
||||
JointParams::PrismaticJoint(p) => {
|
||||
AnyJointPositionConstraint::PrismaticGroundConstraint(
|
||||
PrismaticPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
|
||||
)
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
JointParams::RevoluteJoint(p) => AnyJointPositionConstraint::RevoluteGroundConstraint(
|
||||
RevolutePositionGroundConstraint::from_params(rb1, rb2, p, flipped),
|
||||
),
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub fn from_wide_joint_ground<Bodies>(joints: [&Joint; SIMD_WIDTH], bodies: &Bodies) -> Self
|
||||
where
|
||||
Bodies: ComponentSet<RigidBodyType>
|
||||
+ ComponentSet<RigidBodyPosition>
|
||||
+ ComponentSet<RigidBodyMassProps>
|
||||
+ ComponentSet<RigidBodyIds>,
|
||||
{
|
||||
let mut handles1 = gather![|ii| joints[ii].body1];
|
||||
let mut handles2 = gather![|ii| joints[ii].body2];
|
||||
let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
|
||||
|
||||
let mut flipped = [false; SIMD_WIDTH];
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
if !status2[ii].is_dynamic() {
|
||||
std::mem::swap(&mut handles1[ii], &mut handles2[ii]);
|
||||
flipped[ii] = true;
|
||||
}
|
||||
}
|
||||
|
||||
let rbs1 = gather![|ii| bodies.index(handles1[ii].0)];
|
||||
let rbs2 = (
|
||||
gather![|ii| bodies.index(handles2[ii].0)],
|
||||
gather![|ii| bodies.index(handles2[ii].0)],
|
||||
);
|
||||
|
||||
match &joints[0].params {
|
||||
JointParams::BallJoint(_) => {
|
||||
let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
|
||||
AnyJointPositionConstraint::WBallGroundConstraint(
|
||||
WBallPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
|
||||
)
|
||||
}
|
||||
JointParams::FixedJoint(_) => {
|
||||
let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
|
||||
AnyJointPositionConstraint::WFixedGroundConstraint(
|
||||
WFixedPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
|
||||
)
|
||||
}
|
||||
// JointParams::GenericJoint(_) => {
|
||||
// let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
|
||||
// AnyJointPositionConstraint::WGenericGroundConstraint(
|
||||
// WGenericPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
|
||||
// )
|
||||
// }
|
||||
JointParams::PrismaticJoint(_) => {
|
||||
let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
|
||||
AnyJointPositionConstraint::WPrismaticGroundConstraint(
|
||||
WPrismaticPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
|
||||
)
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
JointParams::RevoluteJoint(_) => {
|
||||
let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
|
||||
AnyJointPositionConstraint::WRevoluteGroundConstraint(
|
||||
WRevolutePositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
|
||||
match self {
|
||||
AnyJointPositionConstraint::BallJoint(c) => c.solve(params, positions),
|
||||
AnyJointPositionConstraint::BallGroundConstraint(c) => c.solve(params, positions),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointPositionConstraint::WBallJoint(c) => c.solve(params, positions),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointPositionConstraint::WBallGroundConstraint(c) => c.solve(params, positions),
|
||||
AnyJointPositionConstraint::FixedJoint(c) => c.solve(params, positions),
|
||||
AnyJointPositionConstraint::FixedGroundConstraint(c) => c.solve(params, positions),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointPositionConstraint::WFixedJoint(c) => c.solve(params, positions),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointPositionConstraint::WFixedGroundConstraint(c) => c.solve(params, positions),
|
||||
// AnyJointPositionConstraint::GenericJoint(c) => c.solve(params, positions),
|
||||
// AnyJointPositionConstraint::GenericGroundConstraint(c) => c.solve(params, positions),
|
||||
// #[cfg(feature = "simd-is-enabled")]
|
||||
// AnyJointPositionConstraint::WGenericJoint(c) => c.solve(params, positions),
|
||||
// #[cfg(feature = "simd-is-enabled")]
|
||||
// AnyJointPositionConstraint::WGenericGroundConstraint(c) => c.solve(params, positions),
|
||||
AnyJointPositionConstraint::PrismaticJoint(c) => c.solve(params, positions),
|
||||
AnyJointPositionConstraint::PrismaticGroundConstraint(c) => c.solve(params, positions),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointPositionConstraint::WPrismaticJoint(c) => c.solve(params, positions),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointPositionConstraint::WPrismaticGroundConstraint(c) => c.solve(params, positions),
|
||||
#[cfg(feature = "dim3")]
|
||||
AnyJointPositionConstraint::RevoluteJoint(c) => c.solve(params, positions),
|
||||
#[cfg(feature = "dim3")]
|
||||
AnyJointPositionConstraint::RevoluteGroundConstraint(c) => c.solve(params, positions),
|
||||
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
|
||||
AnyJointPositionConstraint::WRevoluteJoint(c) => c.solve(params, positions),
|
||||
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
|
||||
AnyJointPositionConstraint::WRevoluteGroundConstraint(c) => c.solve(params, positions),
|
||||
AnyJointPositionConstraint::Empty => unreachable!(),
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,608 @@
|
||||
use crate::dynamics::solver::joint_constraint::JointVelocityConstraintBuilder;
|
||||
use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{IntegrationParameters, JointData, JointGraphEdge, JointIndex};
|
||||
use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, Vector, DIM, SPATIAL_DIM};
|
||||
use crate::utils::{WDot, WReal};
|
||||
use simba::simd::SimdRealField;
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
use {
|
||||
crate::math::{SimdReal, SIMD_WIDTH},
|
||||
na::SimdValue,
|
||||
};
|
||||
|
||||
#[derive(Copy, Clone, PartialEq, Debug)]
|
||||
pub struct MotorParameters<N: SimdRealField> {
|
||||
pub stiffness: N,
|
||||
pub damping: N,
|
||||
pub gamma: N,
|
||||
// pub keep_lhs: bool,
|
||||
pub target_pos: N,
|
||||
pub target_vel: N,
|
||||
pub max_impulse: N,
|
||||
}
|
||||
|
||||
impl<N: SimdRealField> Default for MotorParameters<N> {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
stiffness: N::zero(),
|
||||
damping: N::zero(),
|
||||
gamma: N::zero(),
|
||||
// keep_lhs: true,
|
||||
target_pos: N::zero(),
|
||||
target_vel: N::zero(),
|
||||
max_impulse: N::zero(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Copy, Clone, PartialEq, Eq, Debug)]
|
||||
pub enum WritebackId {
|
||||
Dof(usize),
|
||||
Limit(usize),
|
||||
Motor(usize),
|
||||
}
|
||||
|
||||
// TODO: right now we only use this for impulse_joints.
|
||||
// However, it may actually be a good idea to use this everywhere in
|
||||
// the solver, to avoid fetching data from the rigid-body set
|
||||
// every time.
|
||||
#[derive(Copy, Clone)]
|
||||
pub struct SolverBody<N: SimdRealField, const LANES: usize> {
|
||||
pub linvel: Vector<N>,
|
||||
pub angvel: AngVector<N>,
|
||||
pub im: N,
|
||||
pub sqrt_ii: AngularInertia<N>,
|
||||
pub world_com: Point<N>,
|
||||
pub mj_lambda: [usize; LANES],
|
||||
}
|
||||
|
||||
#[derive(Debug, Copy, Clone)]
|
||||
pub struct JointVelocityConstraint<N: SimdRealField, const LANES: usize> {
|
||||
pub mj_lambda1: [usize; LANES],
|
||||
pub mj_lambda2: [usize; LANES],
|
||||
|
||||
pub joint_id: [JointIndex; LANES],
|
||||
|
||||
pub impulse: N,
|
||||
pub impulse_bounds: [N; 2],
|
||||
pub lin_jac: Vector<N>,
|
||||
pub ang_jac1: AngVector<N>,
|
||||
pub ang_jac2: AngVector<N>,
|
||||
|
||||
pub inv_lhs: N,
|
||||
pub rhs: N,
|
||||
pub rhs_wo_bias: N,
|
||||
|
||||
pub im1: N,
|
||||
pub im2: N,
|
||||
|
||||
pub writeback_id: WritebackId,
|
||||
}
|
||||
|
||||
impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
|
||||
pub fn invalid() -> Self {
|
||||
Self {
|
||||
mj_lambda1: [crate::INVALID_USIZE; LANES],
|
||||
mj_lambda2: [crate::INVALID_USIZE; LANES],
|
||||
joint_id: [crate::INVALID_USIZE; LANES],
|
||||
impulse: N::zero(),
|
||||
impulse_bounds: [N::zero(), N::zero()],
|
||||
lin_jac: Vector::zeros(),
|
||||
ang_jac1: na::zero(),
|
||||
ang_jac2: na::zero(),
|
||||
inv_lhs: N::zero(),
|
||||
rhs: N::zero(),
|
||||
rhs_wo_bias: N::zero(),
|
||||
im1: N::zero(),
|
||||
im2: N::zero(),
|
||||
writeback_id: WritebackId::Dof(0),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve_generic(&mut self, mj_lambda1: &mut DeltaVel<N>, mj_lambda2: &mut DeltaVel<N>) {
|
||||
let dlinvel = self.lin_jac.dot(&(mj_lambda2.linear - mj_lambda1.linear));
|
||||
let dangvel =
|
||||
self.ang_jac2.gdot(mj_lambda2.angular) - self.ang_jac1.gdot(mj_lambda1.angular);
|
||||
|
||||
let rhs = dlinvel + dangvel + self.rhs;
|
||||
let total_impulse = (self.impulse + self.inv_lhs * rhs)
|
||||
.simd_clamp(self.impulse_bounds[0], self.impulse_bounds[1]);
|
||||
let delta_impulse = total_impulse - self.impulse;
|
||||
self.impulse = total_impulse;
|
||||
|
||||
let lin_impulse = self.lin_jac * delta_impulse;
|
||||
let ang_impulse1 = self.ang_jac1 * delta_impulse;
|
||||
let ang_impulse2 = self.ang_jac2 * delta_impulse;
|
||||
|
||||
mj_lambda1.linear += lin_impulse * self.im1;
|
||||
mj_lambda1.angular += ang_impulse1;
|
||||
mj_lambda2.linear -= lin_impulse * self.im2;
|
||||
mj_lambda2.angular -= ang_impulse2;
|
||||
}
|
||||
|
||||
pub fn remove_bias_from_rhs(&mut self) {
|
||||
self.rhs = self.rhs_wo_bias;
|
||||
}
|
||||
}
|
||||
|
||||
impl JointVelocityConstraint<Real, 1> {
|
||||
pub fn lock_axes(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
body1: &SolverBody<Real, 1>,
|
||||
body2: &SolverBody<Real, 1>,
|
||||
frame1: &Isometry<Real>,
|
||||
frame2: &Isometry<Real>,
|
||||
joint: &JointData,
|
||||
out: &mut [Self],
|
||||
) -> usize {
|
||||
let mut len = 0;
|
||||
let locked_axes = joint.locked_axes.bits();
|
||||
let motor_axes = joint.motor_axes.bits();
|
||||
let limit_axes = joint.limit_axes.bits();
|
||||
|
||||
let builder = JointVelocityConstraintBuilder::new(
|
||||
frame1,
|
||||
frame2,
|
||||
&body1.world_com,
|
||||
&body2.world_com,
|
||||
locked_axes,
|
||||
);
|
||||
|
||||
for i in 0..DIM {
|
||||
if locked_axes & (1 << i) != 0 {
|
||||
out[len] =
|
||||
builder.lock_linear(params, [joint_id], body1, body2, i, WritebackId::Dof(i));
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
for i in DIM..SPATIAL_DIM {
|
||||
if locked_axes & (1 << i) != 0 {
|
||||
out[len] = builder.lock_angular(
|
||||
params,
|
||||
[joint_id],
|
||||
body1,
|
||||
body2,
|
||||
i - DIM,
|
||||
WritebackId::Dof(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
|
||||
for i in 0..DIM {
|
||||
if motor_axes & (1 << i) != 0 {
|
||||
out[len] = builder.motor_linear(
|
||||
params,
|
||||
[joint_id],
|
||||
body1,
|
||||
body2,
|
||||
locked_axes >> DIM,
|
||||
i,
|
||||
&joint.motors[i].motor_params(params.dt),
|
||||
WritebackId::Motor(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
for i in DIM..SPATIAL_DIM {
|
||||
if motor_axes & (1 << i) != 0 {
|
||||
out[len] = builder.motor_angular(
|
||||
[joint_id],
|
||||
body1,
|
||||
body2,
|
||||
i - DIM,
|
||||
&joint.motors[i].motor_params(params.dt),
|
||||
WritebackId::Motor(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
|
||||
for i in 0..DIM {
|
||||
if limit_axes & (1 << i) != 0 {
|
||||
out[len] = builder.limit_linear(
|
||||
params,
|
||||
[joint_id],
|
||||
body1,
|
||||
body2,
|
||||
i,
|
||||
[joint.limits[i].min, joint.limits[i].max],
|
||||
WritebackId::Limit(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
for i in DIM..SPATIAL_DIM {
|
||||
if limit_axes & (1 << i) != 0 {
|
||||
out[len] = builder.limit_angular(
|
||||
params,
|
||||
[joint_id],
|
||||
body1,
|
||||
body2,
|
||||
i - DIM,
|
||||
[joint.limits[i].min, joint.limits[i].max],
|
||||
WritebackId::Limit(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
|
||||
JointVelocityConstraintBuilder::finalize_constraints(&mut out[..len]);
|
||||
len
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1[0] as usize];
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2[0] as usize];
|
||||
|
||||
self.solve_generic(&mut mj_lambda1, &mut mj_lambda2);
|
||||
|
||||
mj_lambdas[self.mj_lambda1[0] as usize] = mj_lambda1;
|
||||
mj_lambdas[self.mj_lambda2[0] as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
let joint = &mut joints_all[self.joint_id[0]].weight;
|
||||
match self.writeback_id {
|
||||
WritebackId::Dof(i) => joint.impulses[i] = self.impulse,
|
||||
WritebackId::Limit(i) => joint.data.limits[i].impulse = self.impulse,
|
||||
WritebackId::Motor(i) => joint.data.motors[i].impulse = self.impulse,
|
||||
}
|
||||
}
|
||||
}
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
impl JointVelocityConstraint<SimdReal, SIMD_WIDTH> {
|
||||
pub fn lock_axes(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
body1: &SolverBody<SimdReal, SIMD_WIDTH>,
|
||||
body2: &SolverBody<SimdReal, SIMD_WIDTH>,
|
||||
frame1: &Isometry<SimdReal>,
|
||||
frame2: &Isometry<SimdReal>,
|
||||
locked_axes: u8,
|
||||
out: &mut [Self],
|
||||
) -> usize {
|
||||
let builder = JointVelocityConstraintBuilder::new(
|
||||
frame1,
|
||||
frame2,
|
||||
&body1.world_com,
|
||||
&body2.world_com,
|
||||
locked_axes,
|
||||
);
|
||||
|
||||
let mut len = 0;
|
||||
for i in 0..DIM {
|
||||
if locked_axes & (1 << i) != 0 {
|
||||
out[len] =
|
||||
builder.lock_linear(params, joint_id, body1, body2, i, WritebackId::Dof(i));
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
|
||||
for i in DIM..SPATIAL_DIM {
|
||||
if locked_axes & (1 << i) != 0 {
|
||||
out[len] = builder.lock_angular(
|
||||
params,
|
||||
joint_id,
|
||||
body1,
|
||||
body2,
|
||||
i - DIM,
|
||||
WritebackId::Dof(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
|
||||
JointVelocityConstraintBuilder::finalize_constraints(&mut out[..len]);
|
||||
len
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda1 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
self.solve_generic(&mut mj_lambda1, &mut mj_lambda2);
|
||||
|
||||
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);
|
||||
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]) {
|
||||
let impulses: [_; SIMD_WIDTH] = self.impulse.into();
|
||||
|
||||
// TODO: should we move the iteration on ii deeper in the mested match?
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
let joint = &mut joints_all[self.joint_id[ii]].weight;
|
||||
match self.writeback_id {
|
||||
WritebackId::Dof(i) => joint.impulses[i] = impulses[ii],
|
||||
WritebackId::Limit(i) => joint.data.limits[i].impulse = impulses[ii],
|
||||
WritebackId::Motor(i) => joint.data.motors[i].impulse = impulses[ii],
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Copy, Clone)]
|
||||
pub struct JointVelocityGroundConstraint<N: SimdRealField, const LANES: usize> {
|
||||
pub mj_lambda2: [usize; LANES],
|
||||
|
||||
pub joint_id: [JointIndex; LANES],
|
||||
|
||||
pub impulse: N,
|
||||
pub impulse_bounds: [N; 2],
|
||||
pub lin_jac: Vector<N>,
|
||||
pub ang_jac2: AngVector<N>,
|
||||
|
||||
pub inv_lhs: N,
|
||||
pub rhs: N,
|
||||
pub rhs_wo_bias: N,
|
||||
|
||||
pub im2: N,
|
||||
|
||||
pub writeback_id: WritebackId,
|
||||
}
|
||||
|
||||
impl<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> {
|
||||
pub fn invalid() -> Self {
|
||||
Self {
|
||||
mj_lambda2: [crate::INVALID_USIZE; LANES],
|
||||
joint_id: [crate::INVALID_USIZE; LANES],
|
||||
impulse: N::zero(),
|
||||
impulse_bounds: [N::zero(), N::zero()],
|
||||
lin_jac: Vector::zeros(),
|
||||
ang_jac2: na::zero(),
|
||||
inv_lhs: N::zero(),
|
||||
rhs: N::zero(),
|
||||
rhs_wo_bias: N::zero(),
|
||||
im2: N::zero(),
|
||||
writeback_id: WritebackId::Dof(0),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve_generic(&mut self, mj_lambda2: &mut DeltaVel<N>) {
|
||||
let dlinvel = mj_lambda2.linear;
|
||||
let dangvel = mj_lambda2.angular;
|
||||
|
||||
let dvel = self.lin_jac.dot(&dlinvel) + self.ang_jac2.gdot(dangvel) + self.rhs;
|
||||
let total_impulse = (self.impulse + self.inv_lhs * dvel)
|
||||
.simd_clamp(self.impulse_bounds[0], self.impulse_bounds[1]);
|
||||
let delta_impulse = total_impulse - self.impulse;
|
||||
self.impulse = total_impulse;
|
||||
|
||||
let lin_impulse = self.lin_jac * delta_impulse;
|
||||
let ang_impulse = self.ang_jac2 * delta_impulse;
|
||||
|
||||
mj_lambda2.linear -= lin_impulse * self.im2;
|
||||
mj_lambda2.angular -= ang_impulse;
|
||||
}
|
||||
|
||||
pub fn remove_bias_from_rhs(&mut self) {
|
||||
self.rhs = self.rhs_wo_bias;
|
||||
}
|
||||
}
|
||||
|
||||
impl JointVelocityGroundConstraint<Real, 1> {
|
||||
pub fn lock_axes(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
body1: &SolverBody<Real, 1>,
|
||||
body2: &SolverBody<Real, 1>,
|
||||
frame1: &Isometry<Real>,
|
||||
frame2: &Isometry<Real>,
|
||||
joint: &JointData,
|
||||
out: &mut [Self],
|
||||
) -> usize {
|
||||
let mut len = 0;
|
||||
let locked_axes = joint.locked_axes.bits() as u8;
|
||||
let motor_axes = joint.motor_axes.bits() as u8;
|
||||
let limit_axes = joint.limit_axes.bits() as u8;
|
||||
|
||||
let builder = JointVelocityConstraintBuilder::new(
|
||||
frame1,
|
||||
frame2,
|
||||
&body1.world_com,
|
||||
&body2.world_com,
|
||||
locked_axes,
|
||||
);
|
||||
|
||||
for i in 0..DIM {
|
||||
if locked_axes & (1 << i) != 0 {
|
||||
out[len] = builder.lock_linear_ground(
|
||||
params,
|
||||
[joint_id],
|
||||
body1,
|
||||
body2,
|
||||
i,
|
||||
WritebackId::Dof(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
for i in DIM..SPATIAL_DIM {
|
||||
if locked_axes & (1 << i) != 0 {
|
||||
out[len] = builder.lock_angular_ground(
|
||||
params,
|
||||
[joint_id],
|
||||
body1,
|
||||
body2,
|
||||
i - DIM,
|
||||
WritebackId::Dof(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
|
||||
for i in 0..DIM {
|
||||
if motor_axes & (1 << i) != 0 {
|
||||
out[len] = builder.motor_linear_ground(
|
||||
[joint_id],
|
||||
body1,
|
||||
body2,
|
||||
i,
|
||||
&joint.motors[i].motor_params(params.dt),
|
||||
WritebackId::Motor(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
for i in DIM..SPATIAL_DIM {
|
||||
if motor_axes & (1 << i) != 0 {
|
||||
out[len] = builder.motor_angular_ground(
|
||||
[joint_id],
|
||||
body1,
|
||||
body2,
|
||||
i - DIM,
|
||||
&joint.motors[i].motor_params(params.dt),
|
||||
WritebackId::Motor(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
|
||||
for i in 0..DIM {
|
||||
if limit_axes & (1 << i) != 0 {
|
||||
out[len] = builder.limit_linear_ground(
|
||||
params,
|
||||
[joint_id],
|
||||
body1,
|
||||
body2,
|
||||
i,
|
||||
[joint.limits[i].min, joint.limits[i].max],
|
||||
WritebackId::Limit(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
for i in DIM..SPATIAL_DIM {
|
||||
if limit_axes & (1 << i) != 0 {
|
||||
out[len] = builder.limit_angular_ground(
|
||||
params,
|
||||
[joint_id],
|
||||
body1,
|
||||
body2,
|
||||
i - DIM,
|
||||
[joint.limits[i].min, joint.limits[i].max],
|
||||
WritebackId::Limit(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
|
||||
JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[..len]);
|
||||
len
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2[0] as usize];
|
||||
self.solve_generic(&mut mj_lambda2);
|
||||
mj_lambdas[self.mj_lambda2[0] as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
let joint = &mut joints_all[self.joint_id[0]].weight;
|
||||
match self.writeback_id {
|
||||
WritebackId::Dof(i) => joint.impulses[i] = self.impulse,
|
||||
WritebackId::Limit(i) => joint.data.limits[i].impulse = self.impulse,
|
||||
WritebackId::Motor(i) => joint.data.motors[i].impulse = self.impulse,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
impl JointVelocityGroundConstraint<SimdReal, SIMD_WIDTH> {
|
||||
pub fn lock_axes(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
body1: &SolverBody<SimdReal, SIMD_WIDTH>,
|
||||
body2: &SolverBody<SimdReal, SIMD_WIDTH>,
|
||||
frame1: &Isometry<SimdReal>,
|
||||
frame2: &Isometry<SimdReal>,
|
||||
locked_axes: u8,
|
||||
out: &mut [Self],
|
||||
) -> usize {
|
||||
let mut len = 0;
|
||||
let builder = JointVelocityConstraintBuilder::new(
|
||||
frame1,
|
||||
frame2,
|
||||
&body1.world_com,
|
||||
&body2.world_com,
|
||||
locked_axes,
|
||||
);
|
||||
|
||||
for i in 0..DIM {
|
||||
if locked_axes & (1 << i) != 0 {
|
||||
out[len] = builder.lock_linear_ground(
|
||||
params,
|
||||
joint_id,
|
||||
body1,
|
||||
body2,
|
||||
i,
|
||||
WritebackId::Dof(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
for i in DIM..SPATIAL_DIM {
|
||||
if locked_axes & (1 << i) != 0 {
|
||||
out[len] = builder.lock_angular_ground(
|
||||
params,
|
||||
joint_id,
|
||||
body1,
|
||||
body2,
|
||||
i - DIM,
|
||||
WritebackId::Dof(i),
|
||||
);
|
||||
len += 1;
|
||||
}
|
||||
}
|
||||
|
||||
JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[..len]);
|
||||
len
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
self.solve_generic(&mut mj_lambda2);
|
||||
|
||||
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]) {
|
||||
let impulses: [_; SIMD_WIDTH] = self.impulse.into();
|
||||
|
||||
// TODO: should we move the iteration on ii deeper in the mested match?
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
let joint = &mut joints_all[self.joint_id[ii]].weight;
|
||||
match self.writeback_id {
|
||||
WritebackId::Dof(i) => joint.impulses[i] = impulses[ii],
|
||||
WritebackId::Limit(i) => joint.data.limits[i].impulse = impulses[ii],
|
||||
WritebackId::Motor(i) => joint.data.motors[i].impulse = impulses[ii],
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,699 @@
|
||||
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
|
||||
JointVelocityConstraint, JointVelocityGroundConstraint, WritebackId,
|
||||
};
|
||||
use crate::dynamics::solver::joint_constraint::SolverBody;
|
||||
use crate::dynamics::solver::MotorParameters;
|
||||
use crate::dynamics::{IntegrationParameters, JointIndex};
|
||||
use crate::math::{Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM};
|
||||
use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal};
|
||||
use na::SMatrix;
|
||||
use simba::simd::SimdRealField;
|
||||
|
||||
#[derive(Debug, Copy, Clone)]
|
||||
pub struct JointVelocityConstraintBuilder<N: SimdRealField> {
|
||||
pub basis: Matrix<N>,
|
||||
pub cmat1_basis: SMatrix<N, ANG_DIM, DIM>,
|
||||
pub cmat2_basis: SMatrix<N, ANG_DIM, DIM>,
|
||||
pub ang_basis: SMatrix<N, ANG_DIM, ANG_DIM>,
|
||||
pub lin_err: Vector<N>,
|
||||
pub ang_err: Rotation<N>,
|
||||
}
|
||||
|
||||
impl<N: WReal> JointVelocityConstraintBuilder<N> {
|
||||
pub fn new(
|
||||
frame1: &Isometry<N>,
|
||||
frame2: &Isometry<N>,
|
||||
world_com1: &Point<N>,
|
||||
world_com2: &Point<N>,
|
||||
locked_lin_axes: u8,
|
||||
) -> Self {
|
||||
let mut frame1 = *frame1;
|
||||
let basis = frame1.rotation.to_rotation_matrix().into_inner();
|
||||
let lin_err = frame2.translation.vector - frame1.translation.vector;
|
||||
|
||||
// Adjust the point of application of the force for the first body,
|
||||
// by snapping free axes to the second frame’s center (to account for
|
||||
// the allowed relative movement).
|
||||
{
|
||||
let mut new_center1 = frame2.translation.vector; // First, assume all dofs are free.
|
||||
|
||||
// Then snap the locked ones.
|
||||
for i in 0..DIM {
|
||||
if locked_lin_axes & (1 << i) != 0 {
|
||||
let axis = basis.column(i);
|
||||
new_center1 -= axis * lin_err.dot(&axis);
|
||||
}
|
||||
}
|
||||
frame1.translation.vector = new_center1;
|
||||
}
|
||||
|
||||
let r1 = frame1.translation.vector - world_com1.coords;
|
||||
let r2 = frame2.translation.vector - world_com2.coords;
|
||||
|
||||
let cmat1 = r1.gcross_matrix();
|
||||
let cmat2 = r2.gcross_matrix();
|
||||
|
||||
#[allow(unused_mut)] // The mut is needed for 3D
|
||||
let mut ang_basis = frame1.rotation.diff_conj1_2(&frame2.rotation).transpose();
|
||||
#[allow(unused_mut)] // The mut is needed for 3D
|
||||
let mut ang_err = frame1.rotation.inverse() * frame2.rotation;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let sgn = N::one().simd_copysign(frame1.rotation.dot(&frame2.rotation));
|
||||
ang_basis *= sgn;
|
||||
*ang_err.as_mut_unchecked() *= sgn;
|
||||
}
|
||||
|
||||
Self {
|
||||
basis,
|
||||
cmat1_basis: cmat1 * basis,
|
||||
cmat2_basis: cmat2 * basis,
|
||||
ang_basis,
|
||||
lin_err,
|
||||
ang_err,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn limit_linear<const LANES: usize>(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; LANES],
|
||||
body1: &SolverBody<N, LANES>,
|
||||
body2: &SolverBody<N, LANES>,
|
||||
limited_axis: usize,
|
||||
limits: [N; 2],
|
||||
writeback_id: WritebackId,
|
||||
) -> JointVelocityConstraint<N, LANES> {
|
||||
let zero = N::zero();
|
||||
let mut constraint =
|
||||
self.lock_linear(params, joint_id, body1, body2, limited_axis, writeback_id);
|
||||
|
||||
let dist = self.lin_err.dot(&constraint.lin_jac);
|
||||
let min_enabled = dist.simd_lt(limits[0]);
|
||||
let max_enabled = limits[1].simd_lt(dist);
|
||||
|
||||
let erp_inv_dt = N::splat(params.erp_inv_dt());
|
||||
let rhs_bias =
|
||||
((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) * erp_inv_dt;
|
||||
constraint.rhs = constraint.rhs_wo_bias + rhs_bias;
|
||||
constraint.impulse_bounds = [
|
||||
N::splat(-Real::INFINITY).select(min_enabled, zero),
|
||||
N::splat(Real::INFINITY).select(max_enabled, zero),
|
||||
];
|
||||
|
||||
constraint
|
||||
}
|
||||
|
||||
pub fn motor_linear<const LANES: usize>(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; LANES],
|
||||
body1: &SolverBody<N, LANES>,
|
||||
body2: &SolverBody<N, LANES>,
|
||||
_locked_ang_axes: u8,
|
||||
motor_axis: usize,
|
||||
motor_params: &MotorParameters<N>,
|
||||
writeback_id: WritebackId,
|
||||
) -> JointVelocityConstraint<N, LANES> {
|
||||
let mut constraint =
|
||||
self.lock_linear(params, joint_id, body1, body2, motor_axis, writeback_id);
|
||||
|
||||
// if locked_ang_axes & (1 << motor_axis) != 0 {
|
||||
// // FIXME: check that this also works for cases
|
||||
// // when not all the angular axes are locked.
|
||||
// constraint.ang_jac1 = na::zero();
|
||||
// constraint.ang_jac2 = na::zero();
|
||||
// }
|
||||
|
||||
let mut rhs_wo_bias = N::zero();
|
||||
if motor_params.stiffness != N::zero() {
|
||||
let dist = self.lin_err.dot(&constraint.lin_jac);
|
||||
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
|
||||
}
|
||||
|
||||
if motor_params.damping != N::zero() {
|
||||
let dvel = constraint.lin_jac.dot(&(body2.linvel - body1.linvel))
|
||||
+ (constraint.ang_jac2.gdot(body2.angvel) - constraint.ang_jac1.gdot(body1.angvel));
|
||||
rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
|
||||
}
|
||||
|
||||
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
|
||||
constraint.rhs = rhs_wo_bias;
|
||||
constraint.rhs_wo_bias = rhs_wo_bias;
|
||||
constraint
|
||||
}
|
||||
|
||||
pub fn lock_linear<const LANES: usize>(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; LANES],
|
||||
body1: &SolverBody<N, LANES>,
|
||||
body2: &SolverBody<N, LANES>,
|
||||
locked_axis: usize,
|
||||
writeback_id: WritebackId,
|
||||
) -> JointVelocityConstraint<N, LANES> {
|
||||
let lin_jac = self.basis.column(locked_axis).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let mut ang_jac1 = self.cmat1_basis[locked_axis];
|
||||
#[cfg(feature = "dim2")]
|
||||
let mut ang_jac2 = self.cmat2_basis[locked_axis];
|
||||
#[cfg(feature = "dim3")]
|
||||
let mut ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned();
|
||||
#[cfg(feature = "dim3")]
|
||||
let mut ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned();
|
||||
|
||||
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
|
||||
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
|
||||
let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
|
||||
|
||||
let erp_inv_dt = params.erp_inv_dt();
|
||||
let rhs_bias = lin_jac.dot(&self.lin_err) * N::splat(erp_inv_dt);
|
||||
|
||||
ang_jac1 = body1.sqrt_ii * ang_jac1;
|
||||
ang_jac2 = body2.sqrt_ii * ang_jac2;
|
||||
|
||||
JointVelocityConstraint {
|
||||
joint_id,
|
||||
mj_lambda1: body1.mj_lambda,
|
||||
mj_lambda2: body2.mj_lambda,
|
||||
im1: body1.im,
|
||||
im2: body2.im,
|
||||
impulse: N::zero(),
|
||||
impulse_bounds: [-N::splat(Real::MAX), N::splat(Real::MAX)],
|
||||
lin_jac,
|
||||
ang_jac1,
|
||||
ang_jac2,
|
||||
inv_lhs: N::zero(), // Will be set during ortogonalization.
|
||||
rhs: rhs_wo_bias + rhs_bias,
|
||||
rhs_wo_bias,
|
||||
writeback_id,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn limit_angular<const LANES: usize>(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; LANES],
|
||||
body1: &SolverBody<N, LANES>,
|
||||
body2: &SolverBody<N, LANES>,
|
||||
limited_axis: usize,
|
||||
limits: [N; 2],
|
||||
writeback_id: WritebackId,
|
||||
) -> JointVelocityConstraint<N, LANES> {
|
||||
let zero = N::zero();
|
||||
let half = N::splat(0.5);
|
||||
let s_limits = [(limits[0] * half).simd_sin(), (limits[1] * half).simd_sin()];
|
||||
#[cfg(feature = "dim2")]
|
||||
let s_ang = self.ang_err.im;
|
||||
#[cfg(feature = "dim3")]
|
||||
let s_ang = self.ang_err.imag()[limited_axis];
|
||||
let min_enabled = s_ang.simd_lt(s_limits[0]);
|
||||
let max_enabled = s_limits[1].simd_lt(s_ang);
|
||||
|
||||
let impulse_bounds = [
|
||||
N::splat(-Real::INFINITY).select(min_enabled, zero),
|
||||
N::splat(Real::INFINITY).select(max_enabled, zero),
|
||||
];
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_jac = self.ang_basis[limited_axis];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_jac = self.ang_basis.column(limited_axis).into_owned();
|
||||
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
|
||||
let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
|
||||
|
||||
let erp_inv_dt = params.erp_inv_dt();
|
||||
let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero)
|
||||
- (s_limits[0] - s_ang).simd_max(zero))
|
||||
* N::splat(erp_inv_dt);
|
||||
|
||||
let ang_jac1 = body1.sqrt_ii * ang_jac;
|
||||
let ang_jac2 = body2.sqrt_ii * ang_jac;
|
||||
|
||||
JointVelocityConstraint {
|
||||
joint_id,
|
||||
mj_lambda1: body1.mj_lambda,
|
||||
mj_lambda2: body2.mj_lambda,
|
||||
im1: body1.im,
|
||||
im2: body2.im,
|
||||
impulse: N::zero(),
|
||||
impulse_bounds,
|
||||
lin_jac: na::zero(),
|
||||
ang_jac1,
|
||||
ang_jac2,
|
||||
inv_lhs: N::zero(), // Will be set during ortogonalization.
|
||||
rhs: rhs_wo_bias + rhs_bias,
|
||||
rhs_wo_bias,
|
||||
writeback_id,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn motor_angular<const LANES: usize>(
|
||||
&self,
|
||||
joint_id: [JointIndex; LANES],
|
||||
body1: &SolverBody<N, LANES>,
|
||||
body2: &SolverBody<N, LANES>,
|
||||
_motor_axis: usize,
|
||||
motor_params: &MotorParameters<N>,
|
||||
writeback_id: WritebackId,
|
||||
) -> JointVelocityConstraint<N, LANES> {
|
||||
// let mut ang_jac = self.ang_basis.column(_motor_axis).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_jac = N::one();
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_jac = self.basis.column(_motor_axis).into_owned();
|
||||
|
||||
let mut rhs_wo_bias = N::zero();
|
||||
if motor_params.stiffness != N::zero() {
|
||||
#[cfg(feature = "dim2")]
|
||||
let s_ang_dist = self.ang_err.im;
|
||||
#[cfg(feature = "dim3")]
|
||||
let s_ang_dist = self.ang_err.imag()[_motor_axis];
|
||||
let s_target_ang = motor_params.target_pos.simd_sin();
|
||||
rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.stiffness;
|
||||
}
|
||||
|
||||
if motor_params.damping != N::zero() {
|
||||
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
|
||||
rhs_wo_bias +=
|
||||
(dvel - motor_params.target_vel/* * ang_jac.norm() */) * motor_params.damping;
|
||||
}
|
||||
|
||||
let ang_jac1 = body1.sqrt_ii * ang_jac;
|
||||
let ang_jac2 = body2.sqrt_ii * ang_jac;
|
||||
|
||||
JointVelocityConstraint {
|
||||
joint_id,
|
||||
mj_lambda1: body1.mj_lambda,
|
||||
mj_lambda2: body2.mj_lambda,
|
||||
im1: body1.im,
|
||||
im2: body2.im,
|
||||
impulse: N::zero(),
|
||||
impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse],
|
||||
lin_jac: na::zero(),
|
||||
ang_jac1,
|
||||
ang_jac2,
|
||||
inv_lhs: N::zero(), // Will be set during ortogonalization.
|
||||
rhs: rhs_wo_bias,
|
||||
rhs_wo_bias,
|
||||
writeback_id,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn lock_angular<const LANES: usize>(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; LANES],
|
||||
body1: &SolverBody<N, LANES>,
|
||||
body2: &SolverBody<N, LANES>,
|
||||
locked_axis: usize,
|
||||
writeback_id: WritebackId,
|
||||
) -> JointVelocityConstraint<N, LANES> {
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_jac = self.ang_basis[locked_axis];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_jac = self.ang_basis.column(locked_axis).into_owned();
|
||||
|
||||
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
|
||||
let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
|
||||
|
||||
let erp_inv_dt = params.erp_inv_dt();
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs_bias = self.ang_err.im * N::splat(erp_inv_dt);
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs_bias = self.ang_err.imag()[locked_axis] * N::splat(erp_inv_dt);
|
||||
|
||||
let ang_jac1 = body1.sqrt_ii * ang_jac;
|
||||
let ang_jac2 = body2.sqrt_ii * ang_jac;
|
||||
|
||||
JointVelocityConstraint {
|
||||
joint_id,
|
||||
mj_lambda1: body1.mj_lambda,
|
||||
mj_lambda2: body2.mj_lambda,
|
||||
im1: body1.im,
|
||||
im2: body2.im,
|
||||
impulse: N::zero(),
|
||||
impulse_bounds: [-N::splat(Real::MAX), N::splat(Real::MAX)],
|
||||
lin_jac: na::zero(),
|
||||
ang_jac1,
|
||||
ang_jac2,
|
||||
inv_lhs: N::zero(), // Will be set during ortogonalization.
|
||||
rhs: rhs_wo_bias + rhs_bias,
|
||||
rhs_wo_bias,
|
||||
writeback_id,
|
||||
}
|
||||
}
|
||||
|
||||
/// Orthogonalize the constraints and set their inv_lhs field.
|
||||
pub fn finalize_constraints<const LANES: usize>(
|
||||
constraints: &mut [JointVelocityConstraint<N, LANES>],
|
||||
) {
|
||||
let len = constraints.len();
|
||||
let imsum = constraints[0].im1 + constraints[0].im2;
|
||||
|
||||
// Use the modified Gram-Schmidt orthogonalization.
|
||||
for j in 0..len {
|
||||
let c_j = &mut constraints[j];
|
||||
let dot_jj = c_j.lin_jac.norm_squared() * imsum
|
||||
+ c_j.ang_jac1.gdot(c_j.ang_jac1)
|
||||
+ c_j.ang_jac2.gdot(c_j.ang_jac2);
|
||||
let inv_dot_jj = crate::utils::simd_inv(dot_jj);
|
||||
c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
|
||||
|
||||
if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] {
|
||||
// Don't remove constraints with limited forces from the others
|
||||
// because they may not deliver the necessary forces to fulfill
|
||||
// the removed parts of other constraints.
|
||||
continue;
|
||||
}
|
||||
|
||||
for i in (j + 1)..len {
|
||||
let (c_i, c_j) = constraints.index_mut_const(i, j);
|
||||
let dot_ij = c_i.lin_jac.dot(&c_j.lin_jac) * imsum
|
||||
+ c_i.ang_jac1.gdot(c_j.ang_jac1)
|
||||
+ c_i.ang_jac2.gdot(c_j.ang_jac2);
|
||||
let coeff = dot_ij * inv_dot_jj;
|
||||
|
||||
c_i.lin_jac -= c_j.lin_jac * coeff;
|
||||
c_i.ang_jac1 -= c_j.ang_jac1 * coeff;
|
||||
c_i.ang_jac2 -= c_j.ang_jac2 * coeff;
|
||||
c_i.rhs_wo_bias -= c_j.rhs_wo_bias * coeff;
|
||||
c_i.rhs -= c_j.rhs * coeff;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn limit_linear_ground<const LANES: usize>(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; LANES],
|
||||
body1: &SolverBody<N, LANES>,
|
||||
body2: &SolverBody<N, LANES>,
|
||||
limited_axis: usize,
|
||||
limits: [N; 2],
|
||||
writeback_id: WritebackId,
|
||||
) -> JointVelocityGroundConstraint<N, LANES> {
|
||||
let zero = N::zero();
|
||||
let lin_jac = self.basis.column(limited_axis).into_owned();
|
||||
let dist = self.lin_err.dot(&lin_jac);
|
||||
let min_enabled = dist.simd_lt(limits[0]);
|
||||
let max_enabled = limits[1].simd_lt(dist);
|
||||
|
||||
let impulse_bounds = [
|
||||
N::splat(-Real::INFINITY).select(min_enabled, zero),
|
||||
N::splat(Real::INFINITY).select(max_enabled, zero),
|
||||
];
|
||||
|
||||
let ang_jac1 = self.cmat1_basis.column(limited_axis).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let mut ang_jac2 = self.cmat2_basis[limited_axis];
|
||||
#[cfg(feature = "dim3")]
|
||||
let mut ang_jac2 = self.cmat2_basis.column(limited_axis).into_owned();
|
||||
|
||||
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
|
||||
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
|
||||
let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
|
||||
|
||||
let erp_inv_dt = params.erp_inv_dt();
|
||||
let rhs_bias = ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero))
|
||||
* N::splat(erp_inv_dt);
|
||||
|
||||
ang_jac2 = body2.sqrt_ii * ang_jac2;
|
||||
|
||||
JointVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2: body2.mj_lambda,
|
||||
im2: body2.im,
|
||||
impulse: zero,
|
||||
impulse_bounds,
|
||||
lin_jac,
|
||||
ang_jac2,
|
||||
inv_lhs: zero, // Will be set during ortogonalization.
|
||||
rhs: rhs_wo_bias + rhs_bias,
|
||||
rhs_wo_bias,
|
||||
writeback_id,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn motor_linear_ground<const LANES: usize>(
|
||||
&self,
|
||||
joint_id: [JointIndex; LANES],
|
||||
body1: &SolverBody<N, LANES>,
|
||||
body2: &SolverBody<N, LANES>,
|
||||
motor_axis: usize,
|
||||
motor_params: &MotorParameters<N>,
|
||||
writeback_id: WritebackId,
|
||||
) -> JointVelocityGroundConstraint<N, LANES> {
|
||||
let lin_jac = self.basis.column(motor_axis).into_owned();
|
||||
let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let mut ang_jac2 = self.cmat2_basis[motor_axis];
|
||||
#[cfg(feature = "dim3")]
|
||||
let mut ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned();
|
||||
|
||||
let mut rhs_wo_bias = N::zero();
|
||||
if motor_params.stiffness != N::zero() {
|
||||
let dist = self.lin_err.dot(&lin_jac);
|
||||
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
|
||||
}
|
||||
|
||||
if motor_params.damping != N::zero() {
|
||||
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
|
||||
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
|
||||
rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
|
||||
}
|
||||
|
||||
ang_jac2 = body2.sqrt_ii * ang_jac2;
|
||||
|
||||
JointVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2: body2.mj_lambda,
|
||||
im2: body2.im,
|
||||
impulse: N::zero(),
|
||||
impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse],
|
||||
lin_jac,
|
||||
ang_jac2,
|
||||
inv_lhs: N::zero(), // Will be set during ortogonalization.
|
||||
rhs: rhs_wo_bias,
|
||||
rhs_wo_bias,
|
||||
writeback_id,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn lock_linear_ground<const LANES: usize>(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; LANES],
|
||||
body1: &SolverBody<N, LANES>,
|
||||
body2: &SolverBody<N, LANES>,
|
||||
locked_axis: usize,
|
||||
writeback_id: WritebackId,
|
||||
) -> JointVelocityGroundConstraint<N, LANES> {
|
||||
let lin_jac = self.basis.column(locked_axis).into_owned();
|
||||
let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let mut ang_jac2 = self.cmat2_basis[locked_axis];
|
||||
#[cfg(feature = "dim3")]
|
||||
let mut ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned();
|
||||
|
||||
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
|
||||
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
|
||||
let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
|
||||
|
||||
let erp_inv_dt = params.erp_inv_dt();
|
||||
let rhs_bias = lin_jac.dot(&self.lin_err) * N::splat(erp_inv_dt);
|
||||
|
||||
ang_jac2 = body2.sqrt_ii * ang_jac2;
|
||||
|
||||
JointVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2: body2.mj_lambda,
|
||||
im2: body2.im,
|
||||
impulse: N::zero(),
|
||||
impulse_bounds: [-N::splat(Real::MAX), N::splat(Real::MAX)],
|
||||
lin_jac,
|
||||
ang_jac2,
|
||||
inv_lhs: N::zero(), // Will be set during ortogonalization.
|
||||
rhs: rhs_wo_bias + rhs_bias,
|
||||
rhs_wo_bias,
|
||||
writeback_id,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn motor_angular_ground<const LANES: usize>(
|
||||
&self,
|
||||
joint_id: [JointIndex; LANES],
|
||||
body1: &SolverBody<N, LANES>,
|
||||
body2: &SolverBody<N, LANES>,
|
||||
_motor_axis: usize,
|
||||
motor_params: &MotorParameters<N>,
|
||||
writeback_id: WritebackId,
|
||||
) -> JointVelocityGroundConstraint<N, LANES> {
|
||||
// let mut ang_jac = self.ang_basis.column(_motor_axis).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_jac = N::one();
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_jac = self.basis.column(_motor_axis).into_owned();
|
||||
|
||||
let mut rhs_wo_bias = N::zero();
|
||||
if motor_params.stiffness != N::zero() {
|
||||
#[cfg(feature = "dim2")]
|
||||
let s_ang_dist = self.ang_err.im;
|
||||
#[cfg(feature = "dim3")]
|
||||
let s_ang_dist = self.ang_err.imag()[_motor_axis];
|
||||
let s_target_ang = motor_params.target_pos.simd_sin();
|
||||
rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.stiffness;
|
||||
}
|
||||
|
||||
if motor_params.damping != N::zero() {
|
||||
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
|
||||
rhs_wo_bias +=
|
||||
(dvel - motor_params.target_vel/* * ang_jac.norm() */) * motor_params.damping;
|
||||
}
|
||||
|
||||
let ang_jac2 = body2.sqrt_ii * ang_jac;
|
||||
|
||||
JointVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2: body2.mj_lambda,
|
||||
im2: body2.im,
|
||||
impulse: N::zero(),
|
||||
impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse],
|
||||
lin_jac: na::zero(),
|
||||
ang_jac2,
|
||||
inv_lhs: N::zero(), // Will be set during ortogonalization.
|
||||
rhs: rhs_wo_bias,
|
||||
rhs_wo_bias,
|
||||
writeback_id,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn limit_angular_ground<const LANES: usize>(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; LANES],
|
||||
body1: &SolverBody<N, LANES>,
|
||||
body2: &SolverBody<N, LANES>,
|
||||
limited_axis: usize,
|
||||
limits: [N; 2],
|
||||
writeback_id: WritebackId,
|
||||
) -> JointVelocityGroundConstraint<N, LANES> {
|
||||
let zero = N::zero();
|
||||
let half = N::splat(0.5);
|
||||
let s_limits = [(limits[0] * half).simd_sin(), (limits[1] * half).simd_sin()];
|
||||
#[cfg(feature = "dim2")]
|
||||
let s_ang = self.ang_err.im;
|
||||
#[cfg(feature = "dim3")]
|
||||
let s_ang = self.ang_err.imag()[limited_axis];
|
||||
let min_enabled = s_ang.simd_lt(s_limits[0]);
|
||||
let max_enabled = s_limits[1].simd_lt(s_ang);
|
||||
|
||||
let impulse_bounds = [
|
||||
N::splat(-Real::INFINITY).select(min_enabled, zero),
|
||||
N::splat(Real::INFINITY).select(max_enabled, zero),
|
||||
];
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_jac = self.ang_basis[limited_axis];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_jac = self.ang_basis.column(limited_axis).into_owned();
|
||||
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
|
||||
let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
|
||||
|
||||
let erp_inv_dt = params.erp_inv_dt();
|
||||
let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero)
|
||||
- (s_limits[0] - s_ang).simd_max(zero))
|
||||
* N::splat(erp_inv_dt);
|
||||
|
||||
let ang_jac2 = body2.sqrt_ii * ang_jac;
|
||||
|
||||
JointVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2: body2.mj_lambda,
|
||||
im2: body2.im,
|
||||
impulse: zero,
|
||||
impulse_bounds,
|
||||
lin_jac: na::zero(),
|
||||
ang_jac2,
|
||||
inv_lhs: zero, // Will be set during ortogonalization.
|
||||
rhs: rhs_wo_bias + rhs_bias,
|
||||
rhs_wo_bias,
|
||||
writeback_id,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn lock_angular_ground<const LANES: usize>(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; LANES],
|
||||
body1: &SolverBody<N, LANES>,
|
||||
body2: &SolverBody<N, LANES>,
|
||||
locked_axis: usize,
|
||||
writeback_id: WritebackId,
|
||||
) -> JointVelocityGroundConstraint<N, LANES> {
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_jac = self.ang_basis[locked_axis];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_jac = self.ang_basis.column(locked_axis).into_owned();
|
||||
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
|
||||
let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
|
||||
|
||||
let erp_inv_dt = params.erp_inv_dt();
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs_bias = self.ang_err.im * N::splat(erp_inv_dt);
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs_bias = self.ang_err.imag()[locked_axis] * N::splat(erp_inv_dt);
|
||||
|
||||
let ang_jac2 = body2.sqrt_ii * ang_jac;
|
||||
|
||||
JointVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2: body2.mj_lambda,
|
||||
im2: body2.im,
|
||||
impulse: N::zero(),
|
||||
impulse_bounds: [-N::splat(Real::MAX), N::splat(Real::MAX)],
|
||||
lin_jac: na::zero(),
|
||||
ang_jac2,
|
||||
inv_lhs: N::zero(), // Will be set during ortogonalization.
|
||||
rhs: rhs_wo_bias + rhs_bias,
|
||||
rhs_wo_bias,
|
||||
writeback_id,
|
||||
}
|
||||
}
|
||||
|
||||
/// Orthogonalize the constraints and set their inv_lhs field.
|
||||
pub fn finalize_ground_constraints<const LANES: usize>(
|
||||
constraints: &mut [JointVelocityGroundConstraint<N, LANES>],
|
||||
) {
|
||||
let len = constraints.len();
|
||||
let imsum = constraints[0].im2;
|
||||
|
||||
// Use the modified Gram-Schmidt orthogonalization.
|
||||
for j in 0..len {
|
||||
let c_j = &mut constraints[j];
|
||||
let dot_jj = c_j.lin_jac.norm_squared() * imsum + c_j.ang_jac2.gdot(c_j.ang_jac2);
|
||||
let inv_dot_jj = crate::utils::simd_inv(dot_jj);
|
||||
c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
|
||||
|
||||
if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] {
|
||||
// Don't remove constraints with limited forces from the others
|
||||
// because they may not deliver the necessary forces to fulfill
|
||||
// the removed parts of other constraints.
|
||||
continue;
|
||||
}
|
||||
|
||||
for i in (j + 1)..len {
|
||||
let (c_i, c_j) = constraints.index_mut_const(i, j);
|
||||
let dot_ij =
|
||||
c_i.lin_jac.dot(&c_j.lin_jac) * imsum + c_i.ang_jac2.gdot(c_j.ang_jac2);
|
||||
let coeff = dot_ij * inv_dot_jj;
|
||||
|
||||
c_i.lin_jac -= c_j.lin_jac * coeff;
|
||||
c_i.ang_jac2 -= c_j.ang_jac2 * coeff;
|
||||
c_i.rhs_wo_bias -= c_j.rhs_wo_bias * coeff;
|
||||
c_i.rhs -= c_j.rhs * coeff;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,102 +1,13 @@
|
||||
pub(self) use ball_position_constraint::{BallPositionConstraint, BallPositionGroundConstraint};
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub(self) use ball_position_constraint_wide::{
|
||||
WBallPositionConstraint, WBallPositionGroundConstraint,
|
||||
};
|
||||
pub(self) use ball_velocity_constraint::{BallVelocityConstraint, BallVelocityGroundConstraint};
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub(self) use ball_velocity_constraint_wide::{
|
||||
WBallVelocityConstraint, WBallVelocityGroundConstraint,
|
||||
};
|
||||
pub(self) use fixed_position_constraint::{FixedPositionConstraint, FixedPositionGroundConstraint};
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub(self) use fixed_position_constraint_wide::{
|
||||
WFixedPositionConstraint, WFixedPositionGroundConstraint,
|
||||
};
|
||||
pub(self) use fixed_velocity_constraint::{FixedVelocityConstraint, FixedVelocityGroundConstraint};
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub(self) use fixed_velocity_constraint_wide::{
|
||||
WFixedVelocityConstraint, WFixedVelocityGroundConstraint,
|
||||
};
|
||||
// pub(self) use generic_position_constraint::{
|
||||
// GenericPositionConstraint, GenericPositionGroundConstraint,
|
||||
// };
|
||||
// #[cfg(feature = "simd-is-enabled")]
|
||||
// pub(self) use generic_position_constraint_wide::{
|
||||
// WGenericPositionConstraint, WGenericPositionGroundConstraint,
|
||||
// };
|
||||
// pub(self) use generic_velocity_constraint::{
|
||||
// GenericVelocityConstraint, GenericVelocityGroundConstraint,
|
||||
// };
|
||||
// #[cfg(feature = "simd-is-enabled")]
|
||||
// pub(self) use generic_velocity_constraint_wide::{
|
||||
// WGenericVelocityConstraint, WGenericVelocityGroundConstraint,
|
||||
// };
|
||||
pub use joint_velocity_constraint::{MotorParameters, SolverBody, WritebackId};
|
||||
|
||||
pub(crate) use joint_constraint::AnyJointVelocityConstraint;
|
||||
pub(crate) use joint_position_constraint::AnyJointPositionConstraint;
|
||||
pub(self) use prismatic_position_constraint::{
|
||||
PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
|
||||
};
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub(self) use prismatic_position_constraint_wide::{
|
||||
WPrismaticPositionConstraint, WPrismaticPositionGroundConstraint,
|
||||
};
|
||||
pub(self) use prismatic_velocity_constraint::{
|
||||
PrismaticVelocityConstraint, PrismaticVelocityGroundConstraint,
|
||||
};
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub(self) use prismatic_velocity_constraint_wide::{
|
||||
WPrismaticVelocityConstraint, WPrismaticVelocityGroundConstraint,
|
||||
};
|
||||
#[cfg(feature = "dim3")]
|
||||
pub(self) use revolute_position_constraint::{
|
||||
RevolutePositionConstraint, RevolutePositionGroundConstraint,
|
||||
};
|
||||
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
|
||||
pub(self) use revolute_position_constraint_wide::{
|
||||
WRevolutePositionConstraint, WRevolutePositionGroundConstraint,
|
||||
};
|
||||
#[cfg(feature = "dim3")]
|
||||
pub(self) use revolute_velocity_constraint::{
|
||||
RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint,
|
||||
};
|
||||
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
|
||||
pub(self) use revolute_velocity_constraint_wide::{
|
||||
WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint,
|
||||
pub use joint_constraint::AnyJointVelocityConstraint;
|
||||
pub use joint_generic_velocity_constraint::{
|
||||
JointGenericVelocityConstraint, JointGenericVelocityGroundConstraint,
|
||||
};
|
||||
pub use joint_velocity_constraint_builder::JointVelocityConstraintBuilder;
|
||||
|
||||
mod ball_position_constraint;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
mod ball_position_constraint_wide;
|
||||
mod ball_velocity_constraint;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
mod ball_velocity_constraint_wide;
|
||||
mod fixed_position_constraint;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
mod fixed_position_constraint_wide;
|
||||
mod fixed_velocity_constraint;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
mod fixed_velocity_constraint_wide;
|
||||
// mod generic_position_constraint;
|
||||
// #[cfg(feature = "simd-is-enabled")]
|
||||
// mod generic_position_constraint_wide;
|
||||
// mod generic_velocity_constraint;
|
||||
// #[cfg(feature = "simd-is-enabled")]
|
||||
// mod generic_velocity_constraint_wide;
|
||||
mod joint_constraint;
|
||||
mod joint_position_constraint;
|
||||
mod prismatic_position_constraint;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
mod prismatic_position_constraint_wide;
|
||||
mod prismatic_velocity_constraint;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
mod prismatic_velocity_constraint_wide;
|
||||
#[cfg(feature = "dim3")]
|
||||
mod revolute_position_constraint;
|
||||
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
|
||||
mod revolute_position_constraint_wide;
|
||||
#[cfg(feature = "dim3")]
|
||||
mod revolute_velocity_constraint;
|
||||
#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
|
||||
mod revolute_velocity_constraint_wide;
|
||||
mod joint_generic_velocity_constraint;
|
||||
mod joint_generic_velocity_constraint_builder;
|
||||
mod joint_velocity_constraint;
|
||||
mod joint_velocity_constraint_builder;
|
||||
|
||||
@@ -1,182 +0,0 @@
|
||||
use crate::dynamics::{
|
||||
IntegrationParameters, PrismaticJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
|
||||
};
|
||||
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector};
|
||||
use crate::utils::WAngularInertia;
|
||||
use na::Unit;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct PrismaticPositionConstraint {
|
||||
position1: usize,
|
||||
position2: usize,
|
||||
|
||||
im1: Real,
|
||||
im2: Real,
|
||||
|
||||
ii1: AngularInertia<Real>,
|
||||
ii2: AngularInertia<Real>,
|
||||
|
||||
lin_inv_lhs: Real,
|
||||
ang_inv_lhs: AngularInertia<Real>,
|
||||
|
||||
limits: [Real; 2],
|
||||
|
||||
local_frame1: Isometry<Real>,
|
||||
local_frame2: Isometry<Real>,
|
||||
|
||||
local_axis1: Unit<Vector<Real>>,
|
||||
local_axis2: Unit<Vector<Real>>,
|
||||
}
|
||||
|
||||
impl PrismaticPositionConstraint {
|
||||
pub fn from_params(
|
||||
rb1: (&RigidBodyMassProps, &RigidBodyIds),
|
||||
rb2: (&RigidBodyMassProps, &RigidBodyIds),
|
||||
cparams: &PrismaticJoint,
|
||||
) -> Self {
|
||||
let (mprops1, ids1) = rb1;
|
||||
let (mprops2, ids2) = rb2;
|
||||
|
||||
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
|
||||
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
|
||||
let im1 = mprops1.effective_inv_mass;
|
||||
let im2 = mprops2.effective_inv_mass;
|
||||
let lin_inv_lhs = 1.0 / (im1 + im2);
|
||||
let ang_inv_lhs = (ii1 + ii2).inverse();
|
||||
|
||||
Self {
|
||||
im1,
|
||||
im2,
|
||||
ii1,
|
||||
ii2,
|
||||
lin_inv_lhs,
|
||||
ang_inv_lhs,
|
||||
local_frame1: cparams.local_frame1(),
|
||||
local_frame2: cparams.local_frame2(),
|
||||
local_axis1: cparams.local_axis1,
|
||||
local_axis2: cparams.local_axis2,
|
||||
position1: ids1.active_set_offset,
|
||||
position2: ids2.active_set_offset,
|
||||
limits: cparams.limits,
|
||||
}
|
||||
}
|
||||
|
||||
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];
|
||||
|
||||
// Angular correction.
|
||||
let frame1 = position1 * self.local_frame1;
|
||||
let frame2 = position2 * self.local_frame2;
|
||||
let ang_err = frame2.rotation * frame1.rotation.inverse();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = self
|
||||
.ang_inv_lhs
|
||||
.transform_vector(ang_err.angle() * params.joint_erp);
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = self
|
||||
.ang_inv_lhs
|
||||
.transform_vector(ang_err.scaled_axis() * 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_frame1.translation.vector);
|
||||
let anchor2 = position2 * Point::from(self.local_frame2.translation.vector);
|
||||
let axis1 = position1 * self.local_axis1;
|
||||
let dpos = anchor2 - anchor1;
|
||||
let limit_err = dpos.dot(&axis1);
|
||||
let mut err = dpos - *axis1 * limit_err;
|
||||
|
||||
if limit_err < self.limits[0] {
|
||||
err += *axis1 * (limit_err - self.limits[0]);
|
||||
} else if limit_err > self.limits[1] {
|
||||
err += *axis1 * (limit_err - self.limits[1]);
|
||||
}
|
||||
|
||||
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 PrismaticPositionGroundConstraint {
|
||||
position2: usize,
|
||||
frame1: Isometry<Real>,
|
||||
local_frame2: Isometry<Real>,
|
||||
axis1: Unit<Vector<Real>>,
|
||||
local_axis2: Unit<Vector<Real>>,
|
||||
limits: [Real; 2],
|
||||
}
|
||||
|
||||
impl PrismaticPositionGroundConstraint {
|
||||
pub fn from_params(
|
||||
rb1: &RigidBodyPosition,
|
||||
rb2: (&RigidBodyMassProps, &RigidBodyIds),
|
||||
cparams: &PrismaticJoint,
|
||||
flipped: bool,
|
||||
) -> Self {
|
||||
let poss1 = rb1;
|
||||
let (_, ids2) = rb2;
|
||||
|
||||
let frame1;
|
||||
let local_frame2;
|
||||
let axis1;
|
||||
let local_axis2;
|
||||
|
||||
if flipped {
|
||||
frame1 = poss1.next_position * cparams.local_frame2();
|
||||
local_frame2 = cparams.local_frame1();
|
||||
axis1 = poss1.next_position * cparams.local_axis2;
|
||||
local_axis2 = cparams.local_axis1;
|
||||
} else {
|
||||
frame1 = poss1.next_position * cparams.local_frame1();
|
||||
local_frame2 = cparams.local_frame2();
|
||||
axis1 = poss1.next_position * cparams.local_axis1;
|
||||
local_axis2 = cparams.local_axis2;
|
||||
};
|
||||
|
||||
Self {
|
||||
frame1,
|
||||
local_frame2,
|
||||
axis1,
|
||||
local_axis2,
|
||||
position2: ids2.active_set_offset,
|
||||
limits: cparams.limits,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
|
||||
let mut position2 = positions[self.position2 as usize];
|
||||
|
||||
// Angular correction.
|
||||
let frame2 = position2 * self.local_frame2;
|
||||
let ang_err = frame2.rotation * self.frame1.rotation.inverse();
|
||||
position2.rotation = ang_err.powf(-params.joint_erp) * position2.rotation;
|
||||
|
||||
// Linear correction.
|
||||
let anchor1 = Point::from(self.frame1.translation.vector);
|
||||
let anchor2 = position2 * Point::from(self.local_frame2.translation.vector);
|
||||
let dpos = anchor2 - anchor1;
|
||||
let limit_err = dpos.dot(&self.axis1);
|
||||
let mut err = dpos - *self.axis1 * limit_err;
|
||||
|
||||
if limit_err < self.limits[0] {
|
||||
err += *self.axis1 * (limit_err - self.limits[0]);
|
||||
} else if limit_err > self.limits[1] {
|
||||
err += *self.axis1 * (limit_err - self.limits[1]);
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
@@ -1,71 +0,0 @@
|
||||
use super::{PrismaticPositionConstraint, PrismaticPositionGroundConstraint};
|
||||
use crate::dynamics::{
|
||||
IntegrationParameters, PrismaticJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
|
||||
};
|
||||
use crate::math::{Isometry, Real, SIMD_WIDTH};
|
||||
|
||||
// TODO: this does not uses SIMD optimizations yet.
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WPrismaticPositionConstraint {
|
||||
constraints: [PrismaticPositionConstraint; SIMD_WIDTH],
|
||||
}
|
||||
|
||||
impl WPrismaticPositionConstraint {
|
||||
pub fn from_params(
|
||||
rbs1: (
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
rbs2: (
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
cparams: [&PrismaticJoint; SIMD_WIDTH],
|
||||
) -> Self {
|
||||
Self {
|
||||
constraints: gather![|ii| PrismaticPositionConstraint::from_params(
|
||||
(rbs1.0[ii], rbs1.1[ii]),
|
||||
(rbs2.0[ii], rbs2.1[ii]),
|
||||
cparams[ii]
|
||||
)],
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
|
||||
for constraint in &self.constraints {
|
||||
constraint.solve(params, positions);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WPrismaticPositionGroundConstraint {
|
||||
constraints: [PrismaticPositionGroundConstraint; SIMD_WIDTH],
|
||||
}
|
||||
|
||||
impl WPrismaticPositionGroundConstraint {
|
||||
pub fn from_params(
|
||||
rbs1: [&RigidBodyPosition; SIMD_WIDTH],
|
||||
rbs2: (
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
cparams: [&PrismaticJoint; SIMD_WIDTH],
|
||||
flipped: [bool; SIMD_WIDTH],
|
||||
) -> Self {
|
||||
Self {
|
||||
constraints: gather![|ii| PrismaticPositionGroundConstraint::from_params(
|
||||
rbs1[ii],
|
||||
(rbs2.0[ii], rbs2.1[ii]),
|
||||
cparams[ii],
|
||||
flipped[ii]
|
||||
)],
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
|
||||
for constraint in &self.constraints {
|
||||
constraint.solve(params, positions);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,859 +0,0 @@
|
||||
use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{
|
||||
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBodyIds,
|
||||
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
|
||||
};
|
||||
use crate::math::{AngularInertia, Real, Vector};
|
||||
use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot};
|
||||
#[cfg(feature = "dim3")]
|
||||
use na::{Cholesky, Matrix3x2, Matrix5, Vector5};
|
||||
#[cfg(feature = "dim2")]
|
||||
use {
|
||||
na::{Matrix2, Vector2},
|
||||
parry::utils::SdpMatrix2,
|
||||
};
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
const LIN_IMPULSE_DIM: usize = 1;
|
||||
#[cfg(feature = "dim3")]
|
||||
const LIN_IMPULSE_DIM: usize = 2;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct PrismaticVelocityConstraint {
|
||||
mj_lambda1: usize,
|
||||
mj_lambda2: usize,
|
||||
|
||||
joint_id: JointIndex,
|
||||
|
||||
r1: Vector<Real>,
|
||||
r2: Vector<Real>,
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
inv_lhs: Matrix5<Real>,
|
||||
#[cfg(feature = "dim3")]
|
||||
rhs: Vector5<Real>,
|
||||
#[cfg(feature = "dim3")]
|
||||
impulse: Vector5<Real>,
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
inv_lhs: Matrix2<Real>,
|
||||
#[cfg(feature = "dim2")]
|
||||
rhs: Vector2<Real>,
|
||||
#[cfg(feature = "dim2")]
|
||||
impulse: Vector2<Real>,
|
||||
|
||||
motor_axis1: Vector<Real>,
|
||||
motor_axis2: Vector<Real>,
|
||||
motor_impulse: Real,
|
||||
motor_rhs: Real,
|
||||
motor_inv_lhs: Real,
|
||||
motor_max_impulse: Real,
|
||||
|
||||
limits_active: bool,
|
||||
limits_impulse: Real,
|
||||
/// World-coordinate direction of the limit force on rb2.
|
||||
/// The force direction on rb1 is opposite (Newton's third law)..
|
||||
limits_forcedir2: Vector<Real>,
|
||||
limits_rhs: Real,
|
||||
limits_inv_lhs: Real,
|
||||
/// min/max applied impulse due to limits
|
||||
limits_impulse_limits: (Real, Real),
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
basis1: Vector2<Real>,
|
||||
#[cfg(feature = "dim3")]
|
||||
basis1: Matrix3x2<Real>,
|
||||
|
||||
im1: Real,
|
||||
im2: Real,
|
||||
|
||||
ii1_sqrt: AngularInertia<Real>,
|
||||
ii2_sqrt: AngularInertia<Real>,
|
||||
}
|
||||
|
||||
impl PrismaticVelocityConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
rb1: (
|
||||
&RigidBodyPosition,
|
||||
&RigidBodyVelocity,
|
||||
&RigidBodyMassProps,
|
||||
&RigidBodyIds,
|
||||
),
|
||||
rb2: (
|
||||
&RigidBodyPosition,
|
||||
&RigidBodyVelocity,
|
||||
&RigidBodyMassProps,
|
||||
&RigidBodyIds,
|
||||
),
|
||||
joint: &PrismaticJoint,
|
||||
) -> Self {
|
||||
let (poss1, vels1, mprops1, ids1) = rb1;
|
||||
let (poss2, vels2, mprops2, ids2) = rb2;
|
||||
|
||||
// Linear part.
|
||||
let anchor1 = poss1.position * joint.local_anchor1;
|
||||
let anchor2 = poss2.position * joint.local_anchor2;
|
||||
let axis1 = poss1.position * joint.local_axis1;
|
||||
let axis2 = poss2.position * joint.local_axis2;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let basis1 = poss1.position * joint.basis1[0];
|
||||
#[cfg(feature = "dim3")]
|
||||
let basis1 = Matrix3x2::from_columns(&[
|
||||
poss1.position * joint.basis1[0],
|
||||
poss1.position * joint.basis1[1],
|
||||
]);
|
||||
|
||||
let im1 = mprops1.effective_inv_mass;
|
||||
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
|
||||
let r1 = anchor1 - mprops1.world_com;
|
||||
let r1_mat = r1.gcross_matrix();
|
||||
|
||||
let im2 = mprops2.effective_inv_mass;
|
||||
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
|
||||
let r2 = anchor2 - mprops2.world_com;
|
||||
let r2_mat = r2.gcross_matrix();
|
||||
|
||||
#[allow(unused_mut)] // For 2D.
|
||||
let mut lhs;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let r1_mat_b1 = r1_mat * basis1;
|
||||
let r2_mat_b1 = r2_mat * basis1;
|
||||
|
||||
lhs = Matrix5::zeros();
|
||||
let lhs00 = ii1.quadform3x2(&r1_mat_b1).add_diagonal(im1)
|
||||
+ ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2);
|
||||
let lhs10 = ii1 * r1_mat_b1 + ii2 * r2_mat_b1;
|
||||
let lhs11 = (ii1 + ii2).into_matrix();
|
||||
lhs.fixed_slice_mut::<2, 2>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<3, 2>(2, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<3, 3>(2, 2).copy_from(&lhs11);
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let b1r1 = basis1.dot(&r1_mat);
|
||||
let b2r2 = basis1.dot(&r2_mat);
|
||||
let m11 = im1 + im2 + b1r1 * ii1 * b1r1 + b2r2 * ii2 * b2r2;
|
||||
let m12 = basis1.dot(&r1_mat) * ii1 + basis1.dot(&r2_mat) * ii2;
|
||||
let m22 = ii1 + ii2;
|
||||
lhs = SdpMatrix2::new(m11, m12, m22);
|
||||
}
|
||||
|
||||
let anchor_linvel1 = vels1.linvel + vels1.angvel.gcross(r1);
|
||||
let anchor_linvel2 = vels2.linvel + vels2.angvel.gcross(r2);
|
||||
|
||||
// NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
|
||||
// for which a textbook inverse is still efficient.
|
||||
#[cfg(feature = "dim2")]
|
||||
let inv_lhs = lhs.inverse_unchecked().into_matrix();
|
||||
#[cfg(feature = "dim3")]
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
|
||||
let angvel_err = vels2.angvel - vels1.angvel;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let mut rhs = Vector2::new(linvel_err.x, angvel_err) * params.velocity_solve_fraction;
|
||||
#[cfg(feature = "dim3")]
|
||||
let mut rhs = Vector5::new(
|
||||
linvel_err.x,
|
||||
linvel_err.y,
|
||||
angvel_err.x,
|
||||
angvel_err.y,
|
||||
angvel_err.z,
|
||||
) * params.velocity_solve_fraction;
|
||||
|
||||
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
|
||||
if velocity_based_erp_inv_dt != 0.0 {
|
||||
let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
|
||||
|
||||
let frame1 = poss1.position * joint.local_frame1();
|
||||
let frame2 = poss2.position * joint.local_frame2();
|
||||
let ang_err = frame2.rotation * frame1.rotation.inverse();
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt;
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let ang_err = ang_err.scaled_axis();
|
||||
rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
|
||||
* velocity_based_erp_inv_dt;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Setup motor.
|
||||
*/
|
||||
let mut motor_rhs = 0.0;
|
||||
let mut motor_inv_lhs = 0.0;
|
||||
let gcross1 = r1.gcross(*axis1);
|
||||
let gcross2 = r2.gcross(*axis2);
|
||||
|
||||
let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
|
||||
params.dt,
|
||||
joint.motor_stiffness,
|
||||
joint.motor_damping,
|
||||
);
|
||||
|
||||
if stiffness != 0.0 {
|
||||
let dist = anchor2.coords.dot(&axis2) - anchor1.coords.dot(&axis1);
|
||||
motor_rhs += (dist - joint.motor_target_pos) * stiffness;
|
||||
}
|
||||
|
||||
if damping != 0.0 {
|
||||
let curr_vel = vels2.linvel.dot(&axis2) + vels2.angvel.gdot(gcross2)
|
||||
- vels1.linvel.dot(&axis1)
|
||||
- vels1.angvel.gdot(gcross1);
|
||||
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
|
||||
}
|
||||
|
||||
if stiffness != 0.0 || damping != 0.0 {
|
||||
motor_inv_lhs = if keep_lhs {
|
||||
let inv_projected_mass = crate::utils::inv(
|
||||
im1 + im2
|
||||
+ gcross1.gdot(ii1.transform_vector(gcross1))
|
||||
+ gcross2.gdot(ii2.transform_vector(gcross2)),
|
||||
);
|
||||
|
||||
gamma * inv_projected_mass
|
||||
} else {
|
||||
gamma
|
||||
};
|
||||
motor_rhs /= gamma;
|
||||
}
|
||||
|
||||
let motor_impulse = na::clamp(
|
||||
joint.motor_impulse,
|
||||
-joint.motor_max_impulse,
|
||||
joint.motor_max_impulse,
|
||||
);
|
||||
|
||||
// Setup limit constraint.
|
||||
let mut limits_active = false;
|
||||
let limits_forcedir2 = axis2.into_inner(); // hopefully axis1 is colinear with axis2
|
||||
let mut limits_rhs = 0.0;
|
||||
let mut limits_impulse = 0.0;
|
||||
let mut limits_inv_lhs = 0.0;
|
||||
let mut limits_impulse_limits = (0.0, 0.0);
|
||||
|
||||
if joint.limits_enabled {
|
||||
let danchor = anchor2 - anchor1;
|
||||
let dist = danchor.dot(&axis1);
|
||||
|
||||
// TODO: we should allow predictive constraint activation.
|
||||
|
||||
let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]);
|
||||
let min_enabled = dist < min_limit;
|
||||
let max_enabled = max_limit < dist;
|
||||
|
||||
limits_impulse_limits.0 = if max_enabled { -Real::INFINITY } else { 0.0 };
|
||||
limits_impulse_limits.1 = if min_enabled { Real::INFINITY } else { 0.0 };
|
||||
limits_active = min_enabled || max_enabled;
|
||||
|
||||
if limits_active {
|
||||
limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1))
|
||||
* params.velocity_solve_fraction;
|
||||
|
||||
limits_rhs += ((dist - max_limit).max(0.0) - (min_limit - dist).max(0.0))
|
||||
* velocity_based_erp_inv_dt;
|
||||
|
||||
limits_inv_lhs = crate::utils::inv(
|
||||
im1 + im2
|
||||
+ gcross1.gdot(ii1.transform_vector(gcross1))
|
||||
+ gcross2.gdot(ii2.transform_vector(gcross2)),
|
||||
);
|
||||
|
||||
limits_impulse = joint
|
||||
.limits_impulse
|
||||
.max(limits_impulse_limits.0)
|
||||
.min(limits_impulse_limits.1);
|
||||
}
|
||||
}
|
||||
|
||||
PrismaticVelocityConstraint {
|
||||
joint_id,
|
||||
mj_lambda1: ids1.active_set_offset,
|
||||
mj_lambda2: ids2.active_set_offset,
|
||||
im1,
|
||||
ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
|
||||
im2,
|
||||
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
|
||||
impulse: joint.impulse * params.warmstart_coeff,
|
||||
limits_active,
|
||||
limits_impulse: limits_impulse * params.warmstart_coeff,
|
||||
limits_forcedir2,
|
||||
limits_rhs,
|
||||
limits_inv_lhs,
|
||||
limits_impulse_limits,
|
||||
motor_rhs,
|
||||
motor_inv_lhs,
|
||||
motor_impulse,
|
||||
motor_axis1: *axis1,
|
||||
motor_axis2: *axis2,
|
||||
motor_max_impulse: joint.motor_max_impulse,
|
||||
basis1,
|
||||
inv_lhs,
|
||||
rhs,
|
||||
r1,
|
||||
r2,
|
||||
}
|
||||
}
|
||||
|
||||
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.basis1 * self.impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = self.impulse.y;
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = self.impulse.fixed_rows::<3>(2).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));
|
||||
|
||||
// Warmstart motors.
|
||||
if self.motor_impulse != 0.0 {
|
||||
let lin_impulse1 = self.motor_axis1 * self.motor_impulse;
|
||||
let lin_impulse2 = self.motor_axis2 * self.motor_impulse;
|
||||
|
||||
mj_lambda1.linear += lin_impulse1 * self.im1;
|
||||
mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1));
|
||||
|
||||
mj_lambda2.linear -= lin_impulse2 * self.im2;
|
||||
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2));
|
||||
}
|
||||
|
||||
// Warmstart limits.
|
||||
if self.limits_active {
|
||||
let limit_impulse1 = -self.limits_forcedir2 * self.limits_impulse;
|
||||
let limit_impulse2 = self.limits_forcedir2 * self.limits_impulse;
|
||||
mj_lambda1.linear += self.im1 * limit_impulse1;
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(self.r1.gcross(limit_impulse1));
|
||||
mj_lambda2.linear += self.im2 * limit_impulse2;
|
||||
mj_lambda2.angular += self
|
||||
.ii2_sqrt
|
||||
.transform_vector(self.r2.gcross(limit_impulse2));
|
||||
}
|
||||
|
||||
mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let lin_vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
|
||||
let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
|
||||
let lin_dvel = self.basis1.tr_mul(&(lin_vel2 - lin_vel1));
|
||||
let ang_dvel = ang_vel2 - ang_vel1;
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs;
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs =
|
||||
Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs;
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
self.impulse += impulse;
|
||||
let lin_impulse = self.basis1 * impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = impulse.y;
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = impulse.fixed_rows::<3>(2).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));
|
||||
}
|
||||
|
||||
fn solve_limits(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
|
||||
if self.limits_active {
|
||||
let limits_forcedir1 = -self.limits_forcedir2;
|
||||
let limits_forcedir2 = self.limits_forcedir2;
|
||||
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
|
||||
+ limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1)))
|
||||
+ self.limits_rhs;
|
||||
let new_impulse = (self.limits_impulse - lin_dvel * self.limits_inv_lhs)
|
||||
.max(self.limits_impulse_limits.0)
|
||||
.min(self.limits_impulse_limits.1);
|
||||
let dimpulse = new_impulse - self.limits_impulse;
|
||||
self.limits_impulse = new_impulse;
|
||||
|
||||
let lin_impulse1 = limits_forcedir1 * dimpulse;
|
||||
let lin_impulse2 = limits_forcedir2 * dimpulse;
|
||||
|
||||
mj_lambda1.linear += self.im1 * lin_impulse1;
|
||||
mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1));
|
||||
mj_lambda2.linear += self.im2 * lin_impulse2;
|
||||
mj_lambda2.angular += self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2));
|
||||
}
|
||||
}
|
||||
|
||||
fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
|
||||
if self.motor_inv_lhs != 0.0 {
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let dvel = self
|
||||
.motor_axis2
|
||||
.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
|
||||
- self
|
||||
.motor_axis1
|
||||
.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1)))
|
||||
+ self.motor_rhs;
|
||||
let new_impulse = na::clamp(
|
||||
self.motor_impulse + dvel * self.motor_inv_lhs,
|
||||
-self.motor_max_impulse,
|
||||
self.motor_max_impulse,
|
||||
);
|
||||
let dimpulse = new_impulse - self.motor_impulse;
|
||||
self.motor_impulse = new_impulse;
|
||||
|
||||
let lin_impulse1 = self.motor_axis1 * dimpulse;
|
||||
let lin_impulse2 = self.motor_axis2 * dimpulse;
|
||||
|
||||
mj_lambda1.linear += lin_impulse1 * self.im1;
|
||||
mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1));
|
||||
mj_lambda2.linear -= lin_impulse2 * self.im2;
|
||||
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2));
|
||||
}
|
||||
}
|
||||
|
||||
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];
|
||||
|
||||
self.solve_limits(&mut mj_lambda1, &mut mj_lambda2);
|
||||
self.solve_motors(&mut mj_lambda1, &mut mj_lambda2);
|
||||
self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
|
||||
|
||||
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::PrismaticJoint(revolute) = &mut joint.params {
|
||||
revolute.impulse = self.impulse;
|
||||
revolute.motor_impulse = self.motor_impulse;
|
||||
revolute.limits_impulse = self.limits_impulse;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct PrismaticVelocityGroundConstraint {
|
||||
mj_lambda2: usize,
|
||||
|
||||
joint_id: JointIndex,
|
||||
|
||||
r2: Vector<Real>,
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
inv_lhs: Matrix2<Real>,
|
||||
#[cfg(feature = "dim2")]
|
||||
rhs: Vector2<Real>,
|
||||
#[cfg(feature = "dim2")]
|
||||
impulse: Vector2<Real>,
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
inv_lhs: Matrix5<Real>,
|
||||
#[cfg(feature = "dim3")]
|
||||
rhs: Vector5<Real>,
|
||||
#[cfg(feature = "dim3")]
|
||||
impulse: Vector5<Real>,
|
||||
|
||||
limits_active: bool,
|
||||
limits_forcedir2: Vector<Real>,
|
||||
limits_impulse: Real,
|
||||
limits_rhs: Real,
|
||||
/// min/max applied impulse due to limits
|
||||
limits_impulse_limits: (Real, Real),
|
||||
|
||||
axis2: Vector<Real>,
|
||||
motor_impulse: Real,
|
||||
motor_rhs: Real,
|
||||
motor_inv_lhs: Real,
|
||||
motor_max_impulse: Real,
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
basis1: Vector2<Real>,
|
||||
#[cfg(feature = "dim3")]
|
||||
basis1: Matrix3x2<Real>,
|
||||
|
||||
im2: Real,
|
||||
ii2_sqrt: AngularInertia<Real>,
|
||||
}
|
||||
|
||||
impl PrismaticVelocityGroundConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
|
||||
rb2: (
|
||||
&RigidBodyPosition,
|
||||
&RigidBodyVelocity,
|
||||
&RigidBodyMassProps,
|
||||
&RigidBodyIds,
|
||||
),
|
||||
joint: &PrismaticJoint,
|
||||
flipped: bool,
|
||||
) -> Self {
|
||||
let (poss1, vels1, mprops1) = rb1;
|
||||
let (poss2, vels2, mprops2, ids2) = rb2;
|
||||
|
||||
let anchor2;
|
||||
let anchor1;
|
||||
let axis2;
|
||||
let axis1;
|
||||
let basis1;
|
||||
|
||||
if flipped {
|
||||
anchor2 = poss2.position * joint.local_anchor1;
|
||||
anchor1 = poss1.position * joint.local_anchor2;
|
||||
axis2 = poss2.position * joint.local_axis1;
|
||||
axis1 = poss1.position * joint.local_axis2;
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
basis1 = poss1.position * joint.basis2[0];
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
basis1 = Matrix3x2::from_columns(&[
|
||||
poss1.position * joint.basis2[0],
|
||||
poss1.position * joint.basis2[1],
|
||||
]);
|
||||
}
|
||||
} else {
|
||||
anchor2 = poss2.position * joint.local_anchor2;
|
||||
anchor1 = poss1.position * joint.local_anchor1;
|
||||
axis2 = poss2.position * joint.local_axis2;
|
||||
axis1 = poss1.position * joint.local_axis1;
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
basis1 = poss1.position * joint.basis1[0];
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
basis1 = Matrix3x2::from_columns(&[
|
||||
poss1.position * joint.basis1[0],
|
||||
poss1.position * joint.basis1[1],
|
||||
]);
|
||||
}
|
||||
};
|
||||
|
||||
// #[cfg(feature = "dim2")]
|
||||
// let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
|
||||
// .to_rotation_matrix()
|
||||
// .into_inner();
|
||||
// #[cfg(feature = "dim3")]
|
||||
// let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
|
||||
// .unwrap_or_else(Rotation::identity)
|
||||
// .to_rotation_matrix()
|
||||
// .into_inner();
|
||||
// let basis2 = r21 * basis1;
|
||||
// NOTE: we use basis2 := basis1 for now is that allows
|
||||
// simplifications of the computation without introducing
|
||||
// much instabilities.
|
||||
|
||||
let im2 = mprops2.effective_inv_mass;
|
||||
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
|
||||
let r1 = anchor1 - mprops1.world_com;
|
||||
let r2 = anchor2 - mprops2.world_com;
|
||||
let r2_mat = r2.gcross_matrix();
|
||||
|
||||
#[allow(unused_mut)] // For 2D.
|
||||
let mut lhs;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let r2_mat_b1 = r2_mat * basis1;
|
||||
|
||||
lhs = Matrix5::zeros();
|
||||
let lhs00 = ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2);
|
||||
let lhs10 = ii2 * r2_mat_b1;
|
||||
let lhs11 = ii2.into_matrix();
|
||||
lhs.fixed_slice_mut::<2, 2>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<3, 2>(2, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<3, 3>(2, 2).copy_from(&lhs11);
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let b2r2 = basis1.dot(&r2_mat);
|
||||
let m11 = im2 + b2r2 * ii2 * b2r2;
|
||||
let m12 = basis1.dot(&r2_mat) * ii2;
|
||||
let m22 = ii2;
|
||||
lhs = SdpMatrix2::new(m11, m12, m22);
|
||||
}
|
||||
|
||||
let anchor_linvel1 = vels1.linvel + vels1.angvel.gcross(r1);
|
||||
let anchor_linvel2 = vels2.linvel + vels2.angvel.gcross(r2);
|
||||
|
||||
// NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
|
||||
// for which a textbook inverse is still efficient.
|
||||
#[cfg(feature = "dim2")]
|
||||
let inv_lhs = lhs.inverse_unchecked().into_matrix();
|
||||
#[cfg(feature = "dim3")]
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
|
||||
let angvel_err = vels2.angvel - vels1.angvel;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let mut rhs = Vector2::new(linvel_err.x, angvel_err) * params.velocity_solve_fraction;
|
||||
#[cfg(feature = "dim3")]
|
||||
let mut rhs = Vector5::new(
|
||||
linvel_err.x,
|
||||
linvel_err.y,
|
||||
angvel_err.x,
|
||||
angvel_err.y,
|
||||
angvel_err.z,
|
||||
) * params.velocity_solve_fraction;
|
||||
|
||||
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
|
||||
if velocity_based_erp_inv_dt != 0.0 {
|
||||
let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
|
||||
|
||||
let (frame1, frame2);
|
||||
if flipped {
|
||||
frame1 = poss1.position * joint.local_frame2();
|
||||
frame2 = poss2.position * joint.local_frame1();
|
||||
} else {
|
||||
frame1 = poss1.position * joint.local_frame1();
|
||||
frame2 = poss2.position * joint.local_frame2();
|
||||
}
|
||||
|
||||
let ang_err = frame2.rotation * frame1.rotation.inverse();
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt;
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let ang_err = ang_err.scaled_axis();
|
||||
rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
|
||||
* velocity_based_erp_inv_dt;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Setup motor.
|
||||
*/
|
||||
let mut motor_rhs = 0.0;
|
||||
let mut motor_inv_lhs = 0.0;
|
||||
|
||||
let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
|
||||
params.dt,
|
||||
joint.motor_stiffness,
|
||||
joint.motor_damping,
|
||||
);
|
||||
|
||||
if stiffness != 0.0 {
|
||||
let dist = anchor2.coords.dot(&axis2) - anchor1.coords.dot(&axis1);
|
||||
motor_rhs += (dist - joint.motor_target_pos) * stiffness;
|
||||
}
|
||||
|
||||
if damping != 0.0 {
|
||||
let curr_vel = vels2.linvel.dot(&axis2) - vels1.linvel.dot(&axis1);
|
||||
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
|
||||
}
|
||||
|
||||
if stiffness != 0.0 || damping != 0.0 {
|
||||
motor_inv_lhs = if keep_lhs { gamma / im2 } else { gamma };
|
||||
motor_rhs /= gamma;
|
||||
}
|
||||
|
||||
let motor_impulse = na::clamp(
|
||||
joint.motor_impulse,
|
||||
-joint.motor_max_impulse,
|
||||
joint.motor_max_impulse,
|
||||
);
|
||||
|
||||
/*
|
||||
* Setup limit constraint.
|
||||
*/
|
||||
let mut limits_active = false;
|
||||
let limits_forcedir2 = axis2.into_inner();
|
||||
let mut limits_rhs = 0.0;
|
||||
let mut limits_impulse = 0.0;
|
||||
let mut limits_impulse_limits = (0.0, 0.0);
|
||||
|
||||
if joint.limits_enabled {
|
||||
let danchor = anchor2 - anchor1;
|
||||
let dist = danchor.dot(&axis1);
|
||||
|
||||
// TODO: we should allow predictive constraint activation.
|
||||
|
||||
let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]);
|
||||
let min_enabled = dist < min_limit;
|
||||
let max_enabled = max_limit < dist;
|
||||
|
||||
limits_impulse_limits.0 = if max_enabled { -Real::INFINITY } else { 0.0 };
|
||||
limits_impulse_limits.1 = if min_enabled { Real::INFINITY } else { 0.0 };
|
||||
|
||||
limits_active = min_enabled || max_enabled;
|
||||
if limits_active {
|
||||
limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1))
|
||||
* params.velocity_solve_fraction;
|
||||
|
||||
limits_rhs += ((dist - max_limit).max(0.0) - (min_limit - dist).max(0.0))
|
||||
* velocity_based_erp_inv_dt;
|
||||
|
||||
limits_impulse = joint
|
||||
.limits_impulse
|
||||
.max(limits_impulse_limits.0)
|
||||
.min(limits_impulse_limits.1);
|
||||
}
|
||||
}
|
||||
|
||||
PrismaticVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2: ids2.active_set_offset,
|
||||
im2,
|
||||
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
|
||||
impulse: joint.impulse * params.warmstart_coeff,
|
||||
limits_active,
|
||||
limits_forcedir2,
|
||||
limits_impulse: limits_impulse * params.warmstart_coeff,
|
||||
limits_rhs,
|
||||
limits_impulse_limits,
|
||||
motor_rhs,
|
||||
motor_inv_lhs,
|
||||
motor_impulse,
|
||||
motor_max_impulse: joint.motor_max_impulse,
|
||||
basis1,
|
||||
inv_lhs,
|
||||
rhs,
|
||||
r2,
|
||||
axis2: axis2.into_inner(),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = self.impulse.y;
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = self.impulse.fixed_rows::<3>(2).into_owned();
|
||||
|
||||
mj_lambda2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
// Warmstart motors.
|
||||
mj_lambda2.linear -= self.axis2 * (self.im2 * self.motor_impulse);
|
||||
|
||||
// Warmstart limits.
|
||||
mj_lambda2.linear += self.limits_forcedir2 * (self.im2 * self.limits_impulse);
|
||||
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
|
||||
let lin_dvel = self.basis1.tr_mul(&lin_vel2);
|
||||
let ang_dvel = ang_vel2;
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs;
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs =
|
||||
Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs;
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
self.impulse += impulse;
|
||||
let lin_impulse = self.basis1 * impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = impulse.y;
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = impulse.fixed_rows::<3>(2).into_owned();
|
||||
|
||||
mj_lambda2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
}
|
||||
|
||||
fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
|
||||
if self.limits_active {
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let lin_dvel = self
|
||||
.limits_forcedir2
|
||||
.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
|
||||
+ self.limits_rhs;
|
||||
let new_impulse = (self.limits_impulse - lin_dvel / self.im2)
|
||||
.max(self.limits_impulse_limits.0)
|
||||
.min(self.limits_impulse_limits.1);
|
||||
let dimpulse = new_impulse - self.limits_impulse;
|
||||
self.limits_impulse = new_impulse;
|
||||
|
||||
mj_lambda2.linear += self.limits_forcedir2 * (self.im2 * dimpulse);
|
||||
}
|
||||
}
|
||||
|
||||
fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
|
||||
if self.motor_inv_lhs != 0.0 {
|
||||
let lin_dvel = self.axis2.dot(&mj_lambda2.linear) + self.motor_rhs;
|
||||
let new_impulse = na::clamp(
|
||||
self.motor_impulse + lin_dvel * self.motor_inv_lhs,
|
||||
-self.motor_max_impulse,
|
||||
self.motor_max_impulse,
|
||||
);
|
||||
let dimpulse = new_impulse - self.motor_impulse;
|
||||
self.motor_impulse = new_impulse;
|
||||
|
||||
mj_lambda2.linear -= self.axis2 * (self.im2 * dimpulse);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
self.solve_limits(&mut mj_lambda2);
|
||||
self.solve_motors(&mut mj_lambda2);
|
||||
self.solve_dofs(&mut mj_lambda2);
|
||||
|
||||
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;
|
||||
if let JointParams::PrismaticJoint(revolute) = &mut joint.params {
|
||||
revolute.impulse = self.impulse;
|
||||
revolute.motor_impulse = self.motor_impulse;
|
||||
revolute.limits_impulse = self.limits_impulse;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,848 +0,0 @@
|
||||
use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
|
||||
|
||||
use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{
|
||||
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBodyIds,
|
||||
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
|
||||
};
|
||||
use crate::math::{
|
||||
AngVector, AngularInertia, Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH,
|
||||
};
|
||||
use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot};
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
use na::{Cholesky, Matrix3x2, Matrix5, Vector3, Vector5};
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
use {
|
||||
na::{Matrix2, Vector2},
|
||||
parry::utils::SdpMatrix2,
|
||||
};
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
const LIN_IMPULSE_DIM: usize = 1;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
const LIN_IMPULSE_DIM: usize = 2;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WPrismaticVelocityConstraint {
|
||||
mj_lambda1: [usize; SIMD_WIDTH],
|
||||
mj_lambda2: [usize; SIMD_WIDTH],
|
||||
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
|
||||
r1: Vector<SimdReal>,
|
||||
r2: Vector<SimdReal>,
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
inv_lhs: Matrix5<SimdReal>,
|
||||
#[cfg(feature = "dim3")]
|
||||
rhs: Vector5<SimdReal>,
|
||||
#[cfg(feature = "dim3")]
|
||||
impulse: Vector5<SimdReal>,
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
inv_lhs: Matrix2<SimdReal>,
|
||||
#[cfg(feature = "dim2")]
|
||||
rhs: Vector2<SimdReal>,
|
||||
#[cfg(feature = "dim2")]
|
||||
impulse: Vector2<SimdReal>,
|
||||
|
||||
limits_active: bool,
|
||||
limits_impulse: SimdReal,
|
||||
limits_forcedir2: Vector<SimdReal>,
|
||||
limits_rhs: SimdReal,
|
||||
limits_inv_lhs: SimdReal,
|
||||
limits_impulse_limits: (SimdReal, SimdReal),
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
basis1: Vector2<SimdReal>,
|
||||
#[cfg(feature = "dim3")]
|
||||
basis1: Matrix3x2<SimdReal>,
|
||||
|
||||
im1: SimdReal,
|
||||
im2: SimdReal,
|
||||
|
||||
ii1_sqrt: AngularInertia<SimdReal>,
|
||||
ii2_sqrt: AngularInertia<SimdReal>,
|
||||
}
|
||||
|
||||
impl WPrismaticVelocityConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
rbs1: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
rbs2: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
cparams: [&PrismaticJoint; SIMD_WIDTH],
|
||||
) -> Self {
|
||||
let (poss1, vels1, mprops1, ids1) = rbs1;
|
||||
let (poss2, vels2, mprops2, ids2) = rbs2;
|
||||
|
||||
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
|
||||
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
|
||||
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
|
||||
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
|
||||
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
|
||||
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|
||||
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
|
||||
]);
|
||||
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
|
||||
|
||||
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
|
||||
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
|
||||
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
|
||||
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
|
||||
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
|
||||
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|
||||
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
|
||||
]);
|
||||
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
|
||||
|
||||
let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]);
|
||||
let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]);
|
||||
let local_axis1 = Vector::from(gather![|ii| *cparams[ii].local_axis1]);
|
||||
let local_axis2 = Vector::from(gather![|ii| *cparams[ii].local_axis2]);
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let local_basis1 = [Vector::from(gather![|ii| cparams[ii].basis1[0]])];
|
||||
#[cfg(feature = "dim3")]
|
||||
let local_basis1 = [
|
||||
Vector::from(gather![|ii| cparams[ii].basis1[0]]),
|
||||
Vector::from(gather![|ii| cparams[ii].basis1[1]]),
|
||||
];
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let impulse = Vector2::from(gather![|ii| cparams[ii].impulse]);
|
||||
#[cfg(feature = "dim3")]
|
||||
let impulse = Vector5::from(gather![|ii| cparams[ii].impulse]);
|
||||
|
||||
let anchor1 = position1 * local_anchor1;
|
||||
let anchor2 = position2 * local_anchor2;
|
||||
let axis1 = position1 * local_axis1;
|
||||
let axis2 = position2 * local_axis2;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let basis1 = position1 * local_basis1[0];
|
||||
#[cfg(feature = "dim3")]
|
||||
let basis1 =
|
||||
Matrix3x2::from_columns(&[position1 * local_basis1[0], position1 * local_basis1[1]]);
|
||||
|
||||
// #[cfg(feature = "dim2")]
|
||||
// let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
|
||||
// .to_rotation_matrix()
|
||||
// .into_inner();
|
||||
// #[cfg(feature = "dim3")]
|
||||
// let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
|
||||
// .unwrap_or_else(Rotation::identity)
|
||||
// .to_rotation_matrix()
|
||||
// .into_inner();
|
||||
// let basis2 = r21 * basis1;
|
||||
// NOTE: we use basis2 := basis1 for now is that allows
|
||||
// simplifications of the computation without introducing
|
||||
// much instabilities.
|
||||
|
||||
let ii1 = ii1_sqrt.squared();
|
||||
let r1 = anchor1 - world_com1;
|
||||
let r1_mat = r1.gcross_matrix();
|
||||
|
||||
let ii2 = ii2_sqrt.squared();
|
||||
let r2 = anchor2 - world_com2;
|
||||
let r2_mat = r2.gcross_matrix();
|
||||
|
||||
#[allow(unused_mut)] // For 2D.
|
||||
let mut lhs;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let r1_mat_b1 = r1_mat * basis1;
|
||||
let r2_mat_b1 = r2_mat * basis1;
|
||||
|
||||
lhs = Matrix5::zeros();
|
||||
let lhs00 = ii1.quadform3x2(&r1_mat_b1).add_diagonal(im1)
|
||||
+ ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2);
|
||||
let lhs10 = ii1 * r1_mat_b1 + ii2 * r2_mat_b1;
|
||||
let lhs11 = (ii1 + ii2).into_matrix();
|
||||
lhs.fixed_slice_mut::<2, 2>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<3, 2>(2, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<3, 3>(2, 2).copy_from(&lhs11);
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let b1r1 = basis1.dot(&r1_mat);
|
||||
let b2r2 = basis1.dot(&r2_mat);
|
||||
let m11 = im1 + im2 + b1r1 * ii1 * b1r1 + b2r2 * ii2 * b2r2;
|
||||
let m12 = basis1.dot(&r1_mat) * ii1 + basis1.dot(&r2_mat) * ii2;
|
||||
let m22 = ii1 + ii2;
|
||||
lhs = SdpMatrix2::new(m11, m12, m22);
|
||||
}
|
||||
|
||||
let anchor_linvel1 = linvel1 + angvel1.gcross(r1);
|
||||
let anchor_linvel2 = linvel2 + angvel2.gcross(r2);
|
||||
|
||||
// NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
|
||||
// for which a textbook inverse is still efficient.
|
||||
#[cfg(feature = "dim2")]
|
||||
let inv_lhs = lhs.inverse_unchecked().into_matrix();
|
||||
#[cfg(feature = "dim3")]
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
|
||||
let angvel_err = angvel2 - angvel1;
|
||||
|
||||
let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let mut rhs = Vector2::new(linvel_err.x, angvel_err) * velocity_solve_fraction;
|
||||
#[cfg(feature = "dim3")]
|
||||
let mut rhs = Vector5::new(
|
||||
linvel_err.x,
|
||||
linvel_err.y,
|
||||
angvel_err.x,
|
||||
angvel_err.y,
|
||||
angvel_err.z,
|
||||
) * velocity_solve_fraction;
|
||||
|
||||
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
|
||||
if velocity_based_erp_inv_dt != 0.0 {
|
||||
let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
|
||||
|
||||
let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
|
||||
|
||||
let local_frame1 = Isometry::from(gather![|ii| cparams[ii].local_frame1()]);
|
||||
let local_frame2 = Isometry::from(gather![|ii| cparams[ii].local_frame2()]);
|
||||
|
||||
let frame1 = position1 * local_frame1;
|
||||
let frame2 = position2 * local_frame2;
|
||||
let ang_err = frame2.rotation * frame1.rotation.inverse();
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt;
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
|
||||
rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
|
||||
* velocity_based_erp_inv_dt;
|
||||
}
|
||||
}
|
||||
|
||||
// Setup limit constraint.
|
||||
let zero: SimdReal = na::zero();
|
||||
let limits_forcedir2 = axis2; // hopefully axis1 is colinear with axis2
|
||||
let mut limits_active = false;
|
||||
let mut limits_rhs = zero;
|
||||
let mut limits_impulse = zero;
|
||||
let mut limits_inv_lhs = zero;
|
||||
let mut limits_impulse_limits = (zero, zero);
|
||||
|
||||
let limits_enabled = SimdBool::from(gather![|ii| cparams[ii].limits_enabled]);
|
||||
if limits_enabled.any() {
|
||||
let danchor = anchor2 - anchor1;
|
||||
let dist = danchor.dot(&axis1);
|
||||
|
||||
// TODO: we should allow predictive constraint activation.
|
||||
|
||||
let min_limit = SimdReal::from(gather![|ii| cparams[ii].limits[0]]);
|
||||
let max_limit = SimdReal::from(gather![|ii| cparams[ii].limits[1]]);
|
||||
|
||||
let min_enabled = dist.simd_lt(min_limit);
|
||||
let max_enabled = dist.simd_gt(max_limit);
|
||||
|
||||
limits_impulse_limits.0 = SimdReal::splat(-Real::INFINITY).select(max_enabled, zero);
|
||||
limits_impulse_limits.1 = SimdReal::splat(Real::INFINITY).select(min_enabled, zero);
|
||||
|
||||
limits_active = (min_enabled | max_enabled).any();
|
||||
if limits_active {
|
||||
let gcross1 = r1.gcross(axis1);
|
||||
let gcross2 = r2.gcross(axis2);
|
||||
|
||||
limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1))
|
||||
* velocity_solve_fraction;
|
||||
|
||||
limits_rhs += ((dist - max_limit).simd_max(zero)
|
||||
- (min_limit - dist).simd_max(zero))
|
||||
* SimdReal::splat(velocity_based_erp_inv_dt);
|
||||
|
||||
limits_impulse = SimdReal::from(gather![|ii| cparams[ii].limits_impulse])
|
||||
.simd_max(limits_impulse_limits.0)
|
||||
.simd_min(limits_impulse_limits.1);
|
||||
|
||||
limits_inv_lhs = SimdReal::splat(1.0)
|
||||
/ (im1
|
||||
+ im2
|
||||
+ gcross1.gdot(ii1.transform_vector(gcross1))
|
||||
+ gcross2.gdot(ii2.transform_vector(gcross2)));
|
||||
}
|
||||
}
|
||||
|
||||
WPrismaticVelocityConstraint {
|
||||
joint_id,
|
||||
mj_lambda1,
|
||||
mj_lambda2,
|
||||
im1,
|
||||
ii1_sqrt,
|
||||
im2,
|
||||
ii2_sqrt,
|
||||
limits_active,
|
||||
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
|
||||
limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff),
|
||||
limits_forcedir2,
|
||||
limits_rhs,
|
||||
limits_inv_lhs,
|
||||
limits_impulse_limits,
|
||||
basis1,
|
||||
inv_lhs,
|
||||
rhs,
|
||||
r1,
|
||||
r2,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda1 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = self.impulse.y;
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = self.impulse.fixed_rows::<3>(2).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));
|
||||
|
||||
// Warmstart limits.
|
||||
if self.limits_active {
|
||||
let limit_impulse1 = -self.limits_forcedir2 * self.limits_impulse;
|
||||
let limit_impulse2 = self.limits_forcedir2 * self.limits_impulse;
|
||||
|
||||
mj_lambda1.linear += limit_impulse1 * self.im1;
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(self.r1.gcross(limit_impulse1));
|
||||
mj_lambda2.linear += limit_impulse2 * self.im2;
|
||||
mj_lambda2.angular += self
|
||||
.ii2_sqrt
|
||||
.transform_vector(self.r2.gcross(limit_impulse2));
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
fn solve_dofs(
|
||||
&mut self,
|
||||
mj_lambda1: &mut DeltaVel<SimdReal>,
|
||||
mj_lambda2: &mut DeltaVel<SimdReal>,
|
||||
) {
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let lin_vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
|
||||
let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
|
||||
let lin_dvel = self.basis1.tr_mul(&(lin_vel2 - lin_vel1));
|
||||
let ang_dvel = ang_vel2 - ang_vel1;
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs;
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs =
|
||||
Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs;
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
self.impulse += impulse;
|
||||
let lin_impulse = self.basis1 * impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = impulse.y;
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = impulse.fixed_rows::<3>(2).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));
|
||||
}
|
||||
|
||||
fn solve_limits(
|
||||
&mut self,
|
||||
mj_lambda1: &mut DeltaVel<SimdReal>,
|
||||
mj_lambda2: &mut DeltaVel<SimdReal>,
|
||||
) {
|
||||
if self.limits_active {
|
||||
let limits_forcedir1 = -self.limits_forcedir2;
|
||||
let limits_forcedir2 = self.limits_forcedir2;
|
||||
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
|
||||
+ limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1)))
|
||||
+ self.limits_rhs;
|
||||
let new_impulse = (self.limits_impulse - lin_dvel * self.limits_inv_lhs)
|
||||
.simd_max(self.limits_impulse_limits.0)
|
||||
.simd_min(self.limits_impulse_limits.1);
|
||||
let dimpulse = new_impulse - self.limits_impulse;
|
||||
self.limits_impulse = new_impulse;
|
||||
|
||||
let lin_impulse1 = limits_forcedir1 * dimpulse;
|
||||
let lin_impulse2 = limits_forcedir2 * dimpulse;
|
||||
|
||||
mj_lambda1.linear += lin_impulse1 * self.im1;
|
||||
mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1));
|
||||
mj_lambda2.linear += lin_impulse2 * self.im2;
|
||||
mj_lambda2.angular += self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2));
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda1 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
|
||||
self.solve_limits(&mut mj_lambda1, &mut mj_lambda2);
|
||||
|
||||
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::PrismaticJoint(rev) = &mut joint.params {
|
||||
rev.impulse = self.impulse.extract(ii);
|
||||
rev.limits_impulse = self.limits_impulse.extract(ii);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WPrismaticVelocityGroundConstraint {
|
||||
mj_lambda2: [usize; SIMD_WIDTH],
|
||||
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
|
||||
r2: Vector<SimdReal>,
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
inv_lhs: Matrix2<SimdReal>,
|
||||
#[cfg(feature = "dim2")]
|
||||
rhs: Vector2<SimdReal>,
|
||||
#[cfg(feature = "dim2")]
|
||||
impulse: Vector2<SimdReal>,
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
inv_lhs: Matrix5<SimdReal>,
|
||||
#[cfg(feature = "dim3")]
|
||||
rhs: Vector5<SimdReal>,
|
||||
#[cfg(feature = "dim3")]
|
||||
impulse: Vector5<SimdReal>,
|
||||
|
||||
limits_active: bool,
|
||||
limits_forcedir2: Vector<SimdReal>,
|
||||
limits_impulse: SimdReal,
|
||||
limits_rhs: SimdReal,
|
||||
limits_impulse_limits: (SimdReal, SimdReal),
|
||||
|
||||
axis2: Vector<SimdReal>,
|
||||
#[cfg(feature = "dim2")]
|
||||
basis1: Vector2<SimdReal>,
|
||||
#[cfg(feature = "dim3")]
|
||||
basis1: Matrix3x2<SimdReal>,
|
||||
|
||||
im2: SimdReal,
|
||||
ii2_sqrt: AngularInertia<SimdReal>,
|
||||
}
|
||||
|
||||
impl WPrismaticVelocityGroundConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
rbs1: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
),
|
||||
rbs2: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
cparams: [&PrismaticJoint; SIMD_WIDTH],
|
||||
flipped: [bool; SIMD_WIDTH],
|
||||
) -> Self {
|
||||
let (poss1, vels1, mprops1) = rbs1;
|
||||
let (poss2, vels2, mprops2, ids2) = rbs2;
|
||||
|
||||
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
|
||||
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
|
||||
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
|
||||
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
|
||||
|
||||
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
|
||||
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
|
||||
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
|
||||
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
|
||||
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
|
||||
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|
||||
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
|
||||
]);
|
||||
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let impulse = Vector2::from(gather![|ii| cparams[ii].impulse]);
|
||||
#[cfg(feature = "dim3")]
|
||||
let impulse = Vector5::from(gather![|ii| cparams[ii].impulse]);
|
||||
|
||||
let local_anchor1 = Point::from(gather![|ii| if flipped[ii] {
|
||||
cparams[ii].local_anchor2
|
||||
} else {
|
||||
cparams[ii].local_anchor1
|
||||
}]);
|
||||
let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
|
||||
cparams[ii].local_anchor1
|
||||
} else {
|
||||
cparams[ii].local_anchor2
|
||||
}]);
|
||||
let local_axis1 = Vector::from(gather![|ii| if flipped[ii] {
|
||||
*cparams[ii].local_axis2
|
||||
} else {
|
||||
*cparams[ii].local_axis1
|
||||
}]);
|
||||
let local_axis2 = Vector::from(gather![|ii| if flipped[ii] {
|
||||
*cparams[ii].local_axis1
|
||||
} else {
|
||||
*cparams[ii].local_axis2
|
||||
}]);
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let basis1 = position1
|
||||
* Vector::from(gather![|ii| if flipped[ii] {
|
||||
cparams[ii].basis2[0]
|
||||
} else {
|
||||
cparams[ii].basis1[0]
|
||||
}]);
|
||||
#[cfg(feature = "dim3")]
|
||||
let basis1 = Matrix3x2::from_columns(&[
|
||||
position1
|
||||
* Vector::from(gather![|ii| if flipped[ii] {
|
||||
cparams[ii].basis2[0]
|
||||
} else {
|
||||
cparams[ii].basis1[0]
|
||||
}]),
|
||||
position1
|
||||
* Vector::from(gather![|ii| if flipped[ii] {
|
||||
cparams[ii].basis2[1]
|
||||
} else {
|
||||
cparams[ii].basis1[1]
|
||||
}]),
|
||||
]);
|
||||
|
||||
let anchor1 = position1 * local_anchor1;
|
||||
let anchor2 = position2 * local_anchor2;
|
||||
let axis1 = position1 * local_axis1;
|
||||
let axis2 = position2 * local_axis2;
|
||||
|
||||
let ii2 = ii2_sqrt.squared();
|
||||
let r1 = anchor1 - world_com1;
|
||||
let r2 = anchor2 - world_com2;
|
||||
let r2_mat = r2.gcross_matrix();
|
||||
|
||||
#[allow(unused_mut)] // For 2D.
|
||||
let mut lhs;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let r2_mat_b1 = r2_mat * basis1;
|
||||
|
||||
lhs = Matrix5::zeros();
|
||||
let lhs00 = ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2);
|
||||
let lhs10 = ii2 * r2_mat_b1;
|
||||
let lhs11 = ii2.into_matrix();
|
||||
lhs.fixed_slice_mut::<2, 2>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<3, 2>(2, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<3, 3>(2, 2).copy_from(&lhs11);
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let b2r2 = basis1.dot(&r2_mat);
|
||||
let m11 = im2 + b2r2 * ii2 * b2r2;
|
||||
let m12 = basis1.dot(&r2_mat) * ii2;
|
||||
let m22 = ii2;
|
||||
lhs = SdpMatrix2::new(m11, m12, m22);
|
||||
}
|
||||
|
||||
let anchor_linvel1 = linvel1 + angvel1.gcross(r1);
|
||||
let anchor_linvel2 = linvel2 + angvel2.gcross(r2);
|
||||
|
||||
// NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
|
||||
// for which a textbook inverse is still efficient.
|
||||
#[cfg(feature = "dim2")]
|
||||
let inv_lhs = lhs.inverse_unchecked().into_matrix();
|
||||
#[cfg(feature = "dim3")]
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
|
||||
let angvel_err = angvel2 - angvel1;
|
||||
|
||||
let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let mut rhs = Vector2::new(linvel_err.x, angvel_err) * velocity_solve_fraction;
|
||||
#[cfg(feature = "dim3")]
|
||||
let mut rhs = Vector5::new(
|
||||
linvel_err.x,
|
||||
linvel_err.y,
|
||||
angvel_err.x,
|
||||
angvel_err.y,
|
||||
angvel_err.z,
|
||||
) * velocity_solve_fraction;
|
||||
|
||||
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
|
||||
if velocity_based_erp_inv_dt != 0.0 {
|
||||
let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
|
||||
|
||||
let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
|
||||
|
||||
let frame1 = position1
|
||||
* Isometry::from(gather![|ii| if flipped[ii] {
|
||||
cparams[ii].local_frame2()
|
||||
} else {
|
||||
cparams[ii].local_frame1()
|
||||
}]);
|
||||
let frame2 = position2
|
||||
* Isometry::from(gather![|ii| if flipped[ii] {
|
||||
cparams[ii].local_frame1()
|
||||
} else {
|
||||
cparams[ii].local_frame2()
|
||||
}]);
|
||||
|
||||
let ang_err = frame2.rotation * frame1.rotation.inverse();
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt;
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
|
||||
rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
|
||||
* velocity_based_erp_inv_dt;
|
||||
}
|
||||
}
|
||||
|
||||
// Setup limit constraint.
|
||||
let zero: SimdReal = na::zero();
|
||||
let limits_forcedir2 = axis2; // hopefully axis1 is colinear with axis2
|
||||
let mut limits_active = false;
|
||||
let mut limits_rhs = zero;
|
||||
let mut limits_impulse = zero;
|
||||
let mut limits_impulse_limits = (zero, zero);
|
||||
|
||||
let limits_enabled = SimdBool::from(gather![|ii| cparams[ii].limits_enabled]);
|
||||
if limits_enabled.any() {
|
||||
let danchor = anchor2 - anchor1;
|
||||
let dist = danchor.dot(&axis1);
|
||||
|
||||
// TODO: we should allow predictive constraint activation.
|
||||
let min_limit = SimdReal::from(gather![|ii| cparams[ii].limits[0]]);
|
||||
let max_limit = SimdReal::from(gather![|ii| cparams[ii].limits[1]]);
|
||||
|
||||
let min_enabled = dist.simd_lt(min_limit);
|
||||
let max_enabled = dist.simd_gt(max_limit);
|
||||
|
||||
limits_impulse_limits.0 = SimdReal::splat(-Real::INFINITY).select(max_enabled, zero);
|
||||
limits_impulse_limits.1 = SimdReal::splat(Real::INFINITY).select(min_enabled, zero);
|
||||
|
||||
limits_active = (min_enabled | max_enabled).any();
|
||||
if limits_active {
|
||||
limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1))
|
||||
* velocity_solve_fraction;
|
||||
|
||||
limits_rhs += ((dist - max_limit).simd_max(zero)
|
||||
- (min_limit - dist).simd_max(zero))
|
||||
* SimdReal::splat(velocity_based_erp_inv_dt);
|
||||
|
||||
limits_impulse = SimdReal::from(gather![|ii| cparams[ii].limits_impulse])
|
||||
.simd_max(limits_impulse_limits.0)
|
||||
.simd_min(limits_impulse_limits.1);
|
||||
}
|
||||
}
|
||||
|
||||
WPrismaticVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2,
|
||||
im2,
|
||||
ii2_sqrt,
|
||||
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
|
||||
limits_active,
|
||||
limits_forcedir2,
|
||||
limits_rhs,
|
||||
limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff),
|
||||
limits_impulse_limits,
|
||||
basis1,
|
||||
inv_lhs,
|
||||
rhs,
|
||||
r2,
|
||||
axis2,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = self.impulse.y;
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = self.impulse.fixed_rows::<3>(2).into_owned();
|
||||
|
||||
mj_lambda2.linear -= lin_impulse * self.im2;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
mj_lambda2.linear += self.limits_forcedir2 * (self.im2 * self.limits_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);
|
||||
}
|
||||
}
|
||||
|
||||
fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<SimdReal>) {
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
|
||||
let lin_dvel = self.basis1.tr_mul(&lin_vel2);
|
||||
let ang_dvel = ang_vel2;
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs;
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs =
|
||||
Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs;
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
self.impulse += impulse;
|
||||
let lin_impulse = self.basis1 * impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = impulse.y;
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = impulse.fixed_rows::<3>(2).into_owned();
|
||||
|
||||
mj_lambda2.linear -= lin_impulse * self.im2;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
}
|
||||
|
||||
fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<SimdReal>) {
|
||||
if self.limits_active {
|
||||
// FIXME: the transformation by ii2_sqrt could be avoided by
|
||||
// reusing some computations above.
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let lin_dvel = self
|
||||
.limits_forcedir2
|
||||
.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
|
||||
+ self.limits_rhs;
|
||||
let new_impulse = (self.limits_impulse - lin_dvel / self.im2)
|
||||
.simd_max(self.limits_impulse_limits.0)
|
||||
.simd_min(self.limits_impulse_limits.1);
|
||||
let dimpulse = new_impulse - self.limits_impulse;
|
||||
self.limits_impulse = new_impulse;
|
||||
|
||||
mj_lambda2.linear += self.limits_forcedir2 * (self.im2 * dimpulse);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
self.solve_dofs(&mut mj_lambda2);
|
||||
self.solve_limits(&mut mj_lambda2);
|
||||
|
||||
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::PrismaticJoint(rev) = &mut joint.params {
|
||||
rev.impulse = self.impulse.extract(ii);
|
||||
rev.limits_impulse = self.limits_impulse.extract(ii);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,295 +0,0 @@
|
||||
use crate::dynamics::{
|
||||
IntegrationParameters, RevoluteJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
|
||||
};
|
||||
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector};
|
||||
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
|
||||
use na::Unit;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct RevolutePositionConstraint {
|
||||
position1: usize,
|
||||
position2: usize,
|
||||
|
||||
local_com1: Point<Real>,
|
||||
local_com2: Point<Real>,
|
||||
|
||||
im1: Real,
|
||||
im2: Real,
|
||||
|
||||
ii1: AngularInertia<Real>,
|
||||
ii2: AngularInertia<Real>,
|
||||
|
||||
ang_inv_lhs: AngularInertia<Real>,
|
||||
|
||||
limits_enabled: bool,
|
||||
limits: [Real; 2],
|
||||
|
||||
motor_last_angle: Real,
|
||||
|
||||
local_anchor1: Point<Real>,
|
||||
local_anchor2: Point<Real>,
|
||||
|
||||
local_axis1: Unit<Vector<Real>>,
|
||||
local_axis2: Unit<Vector<Real>>,
|
||||
local_basis1: [Vector<Real>; 2],
|
||||
local_basis2: [Vector<Real>; 2],
|
||||
}
|
||||
|
||||
impl RevolutePositionConstraint {
|
||||
pub fn from_params(
|
||||
rb1: (&RigidBodyMassProps, &RigidBodyIds),
|
||||
rb2: (&RigidBodyMassProps, &RigidBodyIds),
|
||||
cparams: &RevoluteJoint,
|
||||
) -> Self {
|
||||
let (mprops1, ids1) = rb1;
|
||||
let (mprops2, ids2) = rb2;
|
||||
|
||||
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
|
||||
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
|
||||
let im1 = mprops1.effective_inv_mass;
|
||||
let im2 = mprops2.effective_inv_mass;
|
||||
let ang_inv_lhs = (ii1 + ii2).inverse();
|
||||
|
||||
Self {
|
||||
im1,
|
||||
im2,
|
||||
ii1,
|
||||
ii2,
|
||||
ang_inv_lhs,
|
||||
local_com1: mprops1.local_mprops.local_com,
|
||||
local_com2: mprops2.local_mprops.local_com,
|
||||
local_anchor1: cparams.local_anchor1,
|
||||
local_anchor2: cparams.local_anchor2,
|
||||
local_axis1: cparams.local_axis1,
|
||||
local_axis2: cparams.local_axis2,
|
||||
position1: ids1.active_set_offset,
|
||||
position2: ids2.active_set_offset,
|
||||
local_basis1: cparams.basis1,
|
||||
local_basis2: cparams.basis2,
|
||||
limits_enabled: cparams.limits_enabled,
|
||||
limits: cparams.limits,
|
||||
motor_last_angle: cparams.motor_last_angle,
|
||||
}
|
||||
}
|
||||
|
||||
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];
|
||||
|
||||
/*
|
||||
* Linear part.
|
||||
*/
|
||||
{
|
||||
let anchor1 = position1 * self.local_anchor1;
|
||||
let anchor2 = position2 * self.local_anchor2;
|
||||
|
||||
let r1 = anchor1 - position1 * self.local_com1;
|
||||
let r2 = anchor2 - position2 * self.local_com2;
|
||||
|
||||
// TODO: don't do the "to_matrix".
|
||||
let lhs = (self
|
||||
.ii2
|
||||
.quadform(&r2.gcross_matrix())
|
||||
.add_diagonal(self.im2)
|
||||
+ self
|
||||
.ii1
|
||||
.quadform(&r1.gcross_matrix())
|
||||
.add_diagonal(self.im1))
|
||||
.into_matrix();
|
||||
let inv_lhs = lhs.try_inverse().unwrap();
|
||||
|
||||
let delta_tra = anchor2 - anchor1;
|
||||
let lin_error = delta_tra * params.joint_erp;
|
||||
let lin_impulse = inv_lhs * lin_error;
|
||||
|
||||
let rot1 = self.ii1 * r1.gcross(lin_impulse);
|
||||
let rot2 = self.ii2 * r2.gcross(lin_impulse);
|
||||
position1.rotation = Rotation::new(rot1) * position1.rotation;
|
||||
position2.rotation = Rotation::new(-rot2) * position2.rotation;
|
||||
position1.translation.vector += self.im1 * lin_impulse;
|
||||
position2.translation.vector -= self.im2 * lin_impulse;
|
||||
}
|
||||
|
||||
/*
|
||||
* Angular part.
|
||||
*/
|
||||
{
|
||||
let axis1 = position1 * self.local_axis1;
|
||||
let axis2 = position2 * self.local_axis2;
|
||||
let delta_rot =
|
||||
Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity);
|
||||
let ang_error = delta_rot.scaled_axis() * params.joint_erp;
|
||||
let ang_impulse = self.ang_inv_lhs.transform_vector(ang_error);
|
||||
|
||||
position1.rotation =
|
||||
Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation;
|
||||
position2.rotation =
|
||||
Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation;
|
||||
}
|
||||
|
||||
/*
|
||||
* Limits part.
|
||||
*/
|
||||
if self.limits_enabled {
|
||||
let angle = RevoluteJoint::estimate_motor_angle_from_params(
|
||||
&(position1 * self.local_axis1),
|
||||
&(position1 * self.local_basis1[0]),
|
||||
&(position2 * self.local_basis2[0]),
|
||||
self.motor_last_angle,
|
||||
);
|
||||
let ang_error = (angle - self.limits[1]).max(0.0) - (self.limits[0] - angle).max(0.0);
|
||||
|
||||
if ang_error != 0.0 {
|
||||
let axis2 = position2 * self.local_axis2;
|
||||
let ang_impulse = self
|
||||
.ang_inv_lhs
|
||||
.transform_vector(*axis2 * ang_error * 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;
|
||||
}
|
||||
}
|
||||
|
||||
positions[self.position1 as usize] = position1;
|
||||
positions[self.position2 as usize] = position2;
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct RevolutePositionGroundConstraint {
|
||||
position2: usize,
|
||||
local_com2: Point<Real>,
|
||||
im2: Real,
|
||||
ii2: AngularInertia<Real>,
|
||||
anchor1: Point<Real>,
|
||||
local_anchor2: Point<Real>,
|
||||
axis1: Unit<Vector<Real>>,
|
||||
basis1: [Vector<Real>; 2],
|
||||
|
||||
limits_enabled: bool,
|
||||
limits: [Real; 2],
|
||||
|
||||
motor_last_angle: Real,
|
||||
|
||||
local_axis2: Unit<Vector<Real>>,
|
||||
local_basis2: [Vector<Real>; 2],
|
||||
}
|
||||
|
||||
impl RevolutePositionGroundConstraint {
|
||||
pub fn from_params(
|
||||
rb1: &RigidBodyPosition,
|
||||
rb2: (&RigidBodyMassProps, &RigidBodyIds),
|
||||
cparams: &RevoluteJoint,
|
||||
flipped: bool,
|
||||
) -> Self {
|
||||
let poss1 = rb1;
|
||||
let (mprops2, ids2) = rb2;
|
||||
|
||||
let anchor1;
|
||||
let local_anchor2;
|
||||
let axis1;
|
||||
let local_axis2;
|
||||
let basis1;
|
||||
let local_basis2;
|
||||
|
||||
if flipped {
|
||||
anchor1 = poss1.next_position * cparams.local_anchor2;
|
||||
local_anchor2 = cparams.local_anchor1;
|
||||
axis1 = poss1.next_position * cparams.local_axis2;
|
||||
local_axis2 = cparams.local_axis1;
|
||||
basis1 = [
|
||||
poss1.next_position * cparams.basis2[0],
|
||||
poss1.next_position * cparams.basis2[1],
|
||||
];
|
||||
local_basis2 = cparams.basis1;
|
||||
} else {
|
||||
anchor1 = poss1.next_position * cparams.local_anchor1;
|
||||
local_anchor2 = cparams.local_anchor2;
|
||||
axis1 = poss1.next_position * cparams.local_axis1;
|
||||
local_axis2 = cparams.local_axis2;
|
||||
basis1 = [
|
||||
poss1.next_position * cparams.basis1[0],
|
||||
poss1.next_position * cparams.basis1[1],
|
||||
];
|
||||
local_basis2 = cparams.basis2;
|
||||
};
|
||||
|
||||
Self {
|
||||
anchor1,
|
||||
local_anchor2,
|
||||
im2: mprops2.effective_inv_mass,
|
||||
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
|
||||
local_com2: mprops2.local_mprops.local_com,
|
||||
axis1,
|
||||
local_axis2,
|
||||
position2: ids2.active_set_offset,
|
||||
basis1,
|
||||
local_basis2,
|
||||
limits_enabled: cparams.limits_enabled,
|
||||
limits: cparams.limits,
|
||||
motor_last_angle: cparams.motor_last_angle,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
|
||||
let mut position2 = positions[self.position2 as usize];
|
||||
|
||||
/*
|
||||
* Linear part.
|
||||
*/
|
||||
{
|
||||
let anchor2 = position2 * self.local_anchor2;
|
||||
|
||||
let r2 = anchor2 - position2 * self.local_com2;
|
||||
// TODO: don't the the "to_matrix".
|
||||
let lhs = self
|
||||
.ii2
|
||||
.quadform(&r2.gcross_matrix())
|
||||
.add_diagonal(self.im2)
|
||||
.into_matrix();
|
||||
let inv_lhs = lhs.try_inverse().unwrap();
|
||||
|
||||
let delta_tra = anchor2 - self.anchor1;
|
||||
let lin_error = delta_tra * params.joint_erp;
|
||||
let lin_impulse = inv_lhs * lin_error;
|
||||
|
||||
let rot2 = self.ii2 * r2.gcross(lin_impulse);
|
||||
position2.rotation = Rotation::new(-rot2) * position2.rotation;
|
||||
position2.translation.vector -= self.im2 * lin_impulse;
|
||||
}
|
||||
|
||||
/*
|
||||
* Angular part.
|
||||
*/
|
||||
{
|
||||
let axis2 = position2 * self.local_axis2;
|
||||
let delta_rot = Rotation::rotation_between_axis(&self.axis1, &axis2)
|
||||
.unwrap_or_else(Rotation::identity);
|
||||
let ang_error = delta_rot.scaled_axis() * params.joint_erp;
|
||||
position2.rotation = Rotation::new(-ang_error) * position2.rotation;
|
||||
}
|
||||
|
||||
/*
|
||||
* Limits part.
|
||||
*/
|
||||
if self.limits_enabled {
|
||||
let angle = RevoluteJoint::estimate_motor_angle_from_params(
|
||||
&self.axis1,
|
||||
&self.basis1[0],
|
||||
&(position2 * self.local_basis2[0]),
|
||||
self.motor_last_angle,
|
||||
);
|
||||
let ang_error = (angle - self.limits[1]).max(0.0) - (self.limits[0] - angle).max(0.0);
|
||||
|
||||
if ang_error != 0.0 {
|
||||
let axis2 = position2 * self.local_axis2;
|
||||
position2.rotation =
|
||||
Rotation::new(*axis2 * -ang_error * params.joint_erp) * position2.rotation;
|
||||
}
|
||||
}
|
||||
|
||||
positions[self.position2 as usize] = position2;
|
||||
}
|
||||
}
|
||||
@@ -1,71 +0,0 @@
|
||||
use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
|
||||
use crate::dynamics::{
|
||||
IntegrationParameters, RevoluteJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
|
||||
};
|
||||
use crate::math::{Isometry, Real, SIMD_WIDTH};
|
||||
|
||||
// TODO: this does not uses SIMD optimizations yet.
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WRevolutePositionConstraint {
|
||||
constraints: [RevolutePositionConstraint; SIMD_WIDTH],
|
||||
}
|
||||
|
||||
impl WRevolutePositionConstraint {
|
||||
pub fn from_params(
|
||||
rbs1: (
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
rbs2: (
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
cparams: [&RevoluteJoint; SIMD_WIDTH],
|
||||
) -> Self {
|
||||
Self {
|
||||
constraints: gather![|ii| RevolutePositionConstraint::from_params(
|
||||
(rbs1.0[ii], rbs1.1[ii]),
|
||||
(rbs2.0[ii], rbs2.1[ii]),
|
||||
cparams[ii]
|
||||
)],
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
|
||||
for constraint in &self.constraints {
|
||||
constraint.solve(params, positions);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WRevolutePositionGroundConstraint {
|
||||
constraints: [RevolutePositionGroundConstraint; SIMD_WIDTH],
|
||||
}
|
||||
|
||||
impl WRevolutePositionGroundConstraint {
|
||||
pub fn from_params(
|
||||
rbs1: [&RigidBodyPosition; SIMD_WIDTH],
|
||||
rbs2: (
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
cparams: [&RevoluteJoint; SIMD_WIDTH],
|
||||
flipped: [bool; SIMD_WIDTH],
|
||||
) -> Self {
|
||||
Self {
|
||||
constraints: gather![|ii| RevolutePositionGroundConstraint::from_params(
|
||||
rbs1[ii],
|
||||
(rbs2.0[ii], rbs2.1[ii]),
|
||||
cparams[ii],
|
||||
flipped[ii]
|
||||
)],
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
|
||||
for constraint in &self.constraints {
|
||||
constraint.solve(params, positions);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,750 +0,0 @@
|
||||
use crate::dynamics::solver::{AnyJointVelocityConstraint, DeltaVel};
|
||||
use crate::dynamics::{
|
||||
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBodyIds,
|
||||
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
|
||||
};
|
||||
use crate::math::{AngularInertia, Real, Rotation, Vector};
|
||||
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
|
||||
use na::{Cholesky, Matrix3x2, Matrix5, UnitQuaternion, Vector5};
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct RevoluteVelocityConstraint {
|
||||
mj_lambda1: usize,
|
||||
mj_lambda2: usize,
|
||||
|
||||
joint_id: JointIndex,
|
||||
|
||||
r1: Vector<Real>,
|
||||
r2: Vector<Real>,
|
||||
|
||||
inv_lhs: Matrix5<Real>,
|
||||
rhs: Vector5<Real>,
|
||||
impulse: Vector5<Real>,
|
||||
|
||||
motor_inv_lhs: Real,
|
||||
motor_rhs: Real,
|
||||
motor_impulse: Real,
|
||||
motor_max_impulse: Real,
|
||||
motor_angle: Real, // Exists only to write it back into the joint.
|
||||
|
||||
motor_axis1: Vector<Real>,
|
||||
motor_axis2: Vector<Real>,
|
||||
|
||||
limits_active: bool,
|
||||
limits_impulse: Real,
|
||||
limits_rhs: Real,
|
||||
limits_inv_lhs: Real,
|
||||
limits_impulse_limits: (Real, Real),
|
||||
|
||||
basis1: Matrix3x2<Real>,
|
||||
basis2: Matrix3x2<Real>,
|
||||
|
||||
im1: Real,
|
||||
im2: Real,
|
||||
|
||||
ii1_sqrt: AngularInertia<Real>,
|
||||
ii2_sqrt: AngularInertia<Real>,
|
||||
}
|
||||
|
||||
impl RevoluteVelocityConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
rb1: (
|
||||
&RigidBodyPosition,
|
||||
&RigidBodyVelocity,
|
||||
&RigidBodyMassProps,
|
||||
&RigidBodyIds,
|
||||
),
|
||||
rb2: (
|
||||
&RigidBodyPosition,
|
||||
&RigidBodyVelocity,
|
||||
&RigidBodyMassProps,
|
||||
&RigidBodyIds,
|
||||
),
|
||||
joint: &RevoluteJoint,
|
||||
) -> Self {
|
||||
let (poss1, vels1, mprops1, ids1) = rb1;
|
||||
let (poss2, vels2, mprops2, ids2) = rb2;
|
||||
|
||||
// Linear part.
|
||||
let anchor1 = poss1.position * joint.local_anchor1;
|
||||
let anchor2 = poss2.position * joint.local_anchor2;
|
||||
let basis1 = Matrix3x2::from_columns(&[
|
||||
poss1.position * joint.basis1[0],
|
||||
poss1.position * joint.basis1[1],
|
||||
]);
|
||||
|
||||
let basis2 = Matrix3x2::from_columns(&[
|
||||
poss2.position * joint.basis2[0],
|
||||
poss2.position * joint.basis2[1],
|
||||
]);
|
||||
let basis_projection2 = basis2 * basis2.transpose();
|
||||
let basis2 = basis_projection2 * basis1;
|
||||
|
||||
let im1 = mprops1.effective_inv_mass;
|
||||
let im2 = mprops2.effective_inv_mass;
|
||||
|
||||
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
|
||||
let r1 = anchor1 - mprops1.world_com;
|
||||
let r1_mat = r1.gcross_matrix();
|
||||
|
||||
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
|
||||
let r2 = anchor2 - mprops2.world_com;
|
||||
let r2_mat = r2.gcross_matrix();
|
||||
|
||||
let mut lhs = Matrix5::zeros();
|
||||
|
||||
let lhs00 =
|
||||
ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1);
|
||||
let lhs10 = basis2.tr_mul(&(ii2 * r2_mat)) + basis1.tr_mul(&(ii1 * r1_mat));
|
||||
let lhs11 = (ii1.quadform3x2(&basis1) + ii2.quadform3x2(&basis2)).into_matrix();
|
||||
|
||||
// Note that Cholesky won't read the upper-right part
|
||||
// of lhs so we don't have to fill it.
|
||||
lhs.fixed_slice_mut::<3, 3>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<2, 3>(3, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<2, 2>(3, 3).copy_from(&lhs11);
|
||||
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let linvel_err =
|
||||
(vels2.linvel + vels2.angvel.gcross(r2)) - (vels1.linvel + vels1.angvel.gcross(r1));
|
||||
let angvel_err = basis2.tr_mul(&vels2.angvel) - basis1.tr_mul(&vels1.angvel);
|
||||
|
||||
let mut rhs = Vector5::new(
|
||||
linvel_err.x,
|
||||
linvel_err.y,
|
||||
linvel_err.z,
|
||||
angvel_err.x,
|
||||
angvel_err.y,
|
||||
) * params.velocity_solve_fraction;
|
||||
|
||||
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
|
||||
if velocity_based_erp_inv_dt != 0.0 {
|
||||
let lin_err = anchor2 - anchor1;
|
||||
|
||||
let axis1 = poss1.position * joint.local_axis1;
|
||||
let axis2 = poss2.position * joint.local_axis2;
|
||||
|
||||
let axis_error = axis1.cross(&axis2);
|
||||
let ang_err = (basis2.tr_mul(&axis_error) + basis1.tr_mul(&axis_error)) * 0.5;
|
||||
|
||||
rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y)
|
||||
* velocity_based_erp_inv_dt;
|
||||
}
|
||||
|
||||
/*
|
||||
* Motor.
|
||||
*/
|
||||
let motor_axis1 = poss1.position * *joint.local_axis1;
|
||||
let motor_axis2 = poss2.position * *joint.local_axis2;
|
||||
let mut motor_rhs = 0.0;
|
||||
let mut motor_inv_lhs = 0.0;
|
||||
let mut motor_angle = 0.0;
|
||||
let motor_max_impulse = joint.motor_max_impulse;
|
||||
|
||||
let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
|
||||
params.dt,
|
||||
joint.motor_stiffness,
|
||||
joint.motor_damping,
|
||||
);
|
||||
|
||||
if stiffness != 0.0 || joint.limits_enabled {
|
||||
motor_angle = joint.estimate_motor_angle(&poss1.position, &poss2.position);
|
||||
}
|
||||
|
||||
if stiffness != 0.0 {
|
||||
motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness;
|
||||
}
|
||||
|
||||
if damping != 0.0 {
|
||||
let curr_vel = vels2.angvel.dot(&motor_axis2) - vels1.angvel.dot(&motor_axis1);
|
||||
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
|
||||
}
|
||||
|
||||
if stiffness != 0.0 || damping != 0.0 {
|
||||
motor_inv_lhs = if keep_lhs {
|
||||
crate::utils::inv(
|
||||
motor_axis2.dot(&ii2.transform_vector(motor_axis2))
|
||||
+ motor_axis1.dot(&ii1.transform_vector(motor_axis1)),
|
||||
) * gamma
|
||||
} else {
|
||||
gamma
|
||||
};
|
||||
motor_rhs /= gamma;
|
||||
}
|
||||
|
||||
/*
|
||||
* Setup limit constraint.
|
||||
*/
|
||||
let mut limits_active = false;
|
||||
let mut limits_rhs = 0.0;
|
||||
let mut limits_inv_lhs = 0.0;
|
||||
let mut limits_impulse_limits = (0.0, 0.0);
|
||||
let mut limits_impulse = 0.0;
|
||||
|
||||
if joint.limits_enabled {
|
||||
// TODO: we should allow predictive constraint activation.
|
||||
|
||||
let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]);
|
||||
let min_enabled = motor_angle < min_limit;
|
||||
let max_enabled = max_limit < motor_angle;
|
||||
|
||||
limits_impulse_limits.0 = if max_enabled { -Real::INFINITY } else { 0.0 };
|
||||
limits_impulse_limits.1 = if min_enabled { Real::INFINITY } else { 0.0 };
|
||||
limits_active = min_enabled || max_enabled;
|
||||
|
||||
if limits_active {
|
||||
limits_rhs = (vels2.angvel.dot(&motor_axis2) - vels1.angvel.dot(&motor_axis1))
|
||||
* params.velocity_solve_fraction;
|
||||
|
||||
limits_rhs += ((motor_angle - max_limit).max(0.0)
|
||||
- (min_limit - motor_angle).max(0.0))
|
||||
* velocity_based_erp_inv_dt;
|
||||
|
||||
limits_inv_lhs = crate::utils::inv(
|
||||
motor_axis2.dot(&ii2.transform_vector(motor_axis2))
|
||||
+ motor_axis1.dot(&ii1.transform_vector(motor_axis1)),
|
||||
);
|
||||
|
||||
limits_impulse = joint
|
||||
.limits_impulse
|
||||
.max(limits_impulse_limits.0)
|
||||
.min(limits_impulse_limits.1)
|
||||
* params.warmstart_coeff;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Adjust the warmstart impulse.
|
||||
* If the velocity along the free axis is somewhat high,
|
||||
* we need to adjust the angular warmstart impulse because it
|
||||
* may have a direction that is too different than last frame,
|
||||
* making it counter-productive.
|
||||
*/
|
||||
let mut impulse = joint.impulse * params.warmstart_coeff;
|
||||
let axis_rot = Rotation::rotation_between(&joint.prev_axis1, &motor_axis1)
|
||||
.unwrap_or_else(UnitQuaternion::identity);
|
||||
let rotated_impulse = basis1.tr_mul(&(axis_rot * joint.world_ang_impulse));
|
||||
impulse[3] = rotated_impulse.x * params.warmstart_coeff;
|
||||
impulse[4] = rotated_impulse.y * params.warmstart_coeff;
|
||||
let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
|
||||
* params.warmstart_coeff;
|
||||
|
||||
RevoluteVelocityConstraint {
|
||||
joint_id,
|
||||
mj_lambda1: ids1.active_set_offset,
|
||||
mj_lambda2: ids2.active_set_offset,
|
||||
im1,
|
||||
ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
|
||||
basis1,
|
||||
basis2,
|
||||
im2,
|
||||
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
|
||||
impulse,
|
||||
inv_lhs,
|
||||
rhs,
|
||||
r1,
|
||||
r2,
|
||||
motor_rhs,
|
||||
motor_inv_lhs,
|
||||
motor_max_impulse,
|
||||
motor_axis1,
|
||||
motor_axis2,
|
||||
motor_impulse,
|
||||
motor_angle,
|
||||
limits_impulse,
|
||||
limits_impulse_limits,
|
||||
limits_active,
|
||||
limits_inv_lhs,
|
||||
limits_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_impulse1 = self.impulse.fixed_rows::<3>(0).into_owned();
|
||||
let lin_impulse2 = self.impulse.fixed_rows::<3>(0).into_owned();
|
||||
let ang_impulse1 = self.basis1 * self.impulse.fixed_rows::<2>(3).into_owned();
|
||||
let ang_impulse2 = self.basis2 * self.impulse.fixed_rows::<2>(3).into_owned();
|
||||
|
||||
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));
|
||||
|
||||
/*
|
||||
* Warmstart motor.
|
||||
*/
|
||||
if self.motor_inv_lhs != 0.0 {
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(self.motor_axis1 * self.motor_impulse);
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(self.motor_axis2 * self.motor_impulse);
|
||||
}
|
||||
|
||||
/*
|
||||
* Warmstart limits.
|
||||
*/
|
||||
if self.limits_active {
|
||||
let limit_impulse1 = -self.motor_axis2 * self.limits_impulse;
|
||||
let limit_impulse2 = self.motor_axis2 * self.limits_impulse;
|
||||
mj_lambda1.angular += self.ii1_sqrt.transform_vector(limit_impulse1);
|
||||
mj_lambda2.angular += self.ii2_sqrt.transform_vector(limit_impulse2);
|
||||
}
|
||||
|
||||
mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let lin_dvel = (mj_lambda2.linear + ang_vel2.gcross(self.r2))
|
||||
- (mj_lambda1.linear + ang_vel1.gcross(self.r1));
|
||||
let ang_dvel = self.basis2.tr_mul(&ang_vel2) - self.basis1.tr_mul(&ang_vel1);
|
||||
let rhs =
|
||||
Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
self.impulse += impulse;
|
||||
let lin_impulse1 = impulse.fixed_rows::<3>(0).into_owned();
|
||||
let lin_impulse2 = impulse.fixed_rows::<3>(0).into_owned();
|
||||
let ang_impulse1 = self.basis1 * impulse.fixed_rows::<2>(3).into_owned();
|
||||
let ang_impulse2 = self.basis2 * impulse.fixed_rows::<2>(3).into_owned();
|
||||
|
||||
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));
|
||||
}
|
||||
|
||||
fn solve_limits(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
|
||||
if self.limits_active {
|
||||
let limits_torquedir1 = -self.motor_axis2;
|
||||
let limits_torquedir2 = self.motor_axis2;
|
||||
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let ang_dvel = limits_torquedir1.dot(&ang_vel1)
|
||||
+ limits_torquedir2.dot(&ang_vel2)
|
||||
+ self.limits_rhs;
|
||||
let new_impulse = (self.limits_impulse - ang_dvel * self.limits_inv_lhs)
|
||||
.max(self.limits_impulse_limits.0)
|
||||
.min(self.limits_impulse_limits.1);
|
||||
let dimpulse = new_impulse - self.limits_impulse;
|
||||
self.limits_impulse = new_impulse;
|
||||
|
||||
let ang_impulse1 = limits_torquedir1 * dimpulse;
|
||||
let ang_impulse2 = limits_torquedir2 * dimpulse;
|
||||
|
||||
mj_lambda1.angular += self.ii1_sqrt.transform_vector(ang_impulse1);
|
||||
mj_lambda2.angular += self.ii2_sqrt.transform_vector(ang_impulse2);
|
||||
}
|
||||
}
|
||||
|
||||
fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
|
||||
if self.motor_inv_lhs != 0.0 {
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let ang_dvel = ang_vel2.dot(&self.motor_axis2) - ang_vel1.dot(&self.motor_axis1);
|
||||
let rhs = ang_dvel + self.motor_rhs;
|
||||
|
||||
let new_motor_impulse = na::clamp(
|
||||
self.motor_impulse + self.motor_inv_lhs * rhs,
|
||||
-self.motor_max_impulse,
|
||||
self.motor_max_impulse,
|
||||
);
|
||||
let impulse = new_motor_impulse - self.motor_impulse;
|
||||
self.motor_impulse = new_motor_impulse;
|
||||
|
||||
mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.motor_axis1 * impulse);
|
||||
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.motor_axis2 * impulse);
|
||||
}
|
||||
}
|
||||
|
||||
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];
|
||||
|
||||
self.solve_limits(&mut mj_lambda1, &mut mj_lambda2);
|
||||
self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
|
||||
self.solve_motors(&mut mj_lambda1, &mut mj_lambda2);
|
||||
|
||||
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::RevoluteJoint(revolute) = &mut joint.params {
|
||||
revolute.impulse = self.impulse;
|
||||
let rot_part = self.impulse.fixed_rows::<2>(3).into_owned();
|
||||
revolute.world_ang_impulse = self.basis1 * rot_part;
|
||||
revolute.prev_axis1 = self.motor_axis1;
|
||||
revolute.motor_last_angle = self.motor_angle;
|
||||
revolute.motor_impulse = self.motor_impulse;
|
||||
revolute.limits_impulse = self.limits_impulse;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct RevoluteVelocityGroundConstraint {
|
||||
mj_lambda2: usize,
|
||||
|
||||
joint_id: JointIndex,
|
||||
|
||||
r2: Vector<Real>,
|
||||
|
||||
inv_lhs: Matrix5<Real>,
|
||||
rhs: Vector5<Real>,
|
||||
impulse: Vector5<Real>,
|
||||
|
||||
motor_axis2: Vector<Real>,
|
||||
motor_inv_lhs: Real,
|
||||
motor_rhs: Real,
|
||||
motor_impulse: Real,
|
||||
motor_max_impulse: Real,
|
||||
motor_angle: Real, // Exists just for writing it into the joint.
|
||||
|
||||
limits_active: bool,
|
||||
limits_impulse: Real,
|
||||
limits_rhs: Real,
|
||||
limits_inv_lhs: Real,
|
||||
limits_impulse_limits: (Real, Real),
|
||||
|
||||
basis2: Matrix3x2<Real>,
|
||||
|
||||
im2: Real,
|
||||
|
||||
ii2_sqrt: AngularInertia<Real>,
|
||||
}
|
||||
|
||||
impl RevoluteVelocityGroundConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
|
||||
rb2: (
|
||||
&RigidBodyPosition,
|
||||
&RigidBodyVelocity,
|
||||
&RigidBodyMassProps,
|
||||
&RigidBodyIds,
|
||||
),
|
||||
joint: &RevoluteJoint,
|
||||
flipped: bool,
|
||||
) -> AnyJointVelocityConstraint {
|
||||
let (poss1, vels1, mprops1) = rb1;
|
||||
let (poss2, vels2, mprops2, ids2) = rb2;
|
||||
|
||||
let anchor2;
|
||||
let anchor1;
|
||||
let axis1;
|
||||
let axis2;
|
||||
let basis1;
|
||||
let basis2;
|
||||
|
||||
if flipped {
|
||||
axis1 = poss1.position * *joint.local_axis2;
|
||||
axis2 = poss2.position * *joint.local_axis1;
|
||||
anchor1 = poss1.position * joint.local_anchor2;
|
||||
anchor2 = poss2.position * joint.local_anchor1;
|
||||
basis1 = Matrix3x2::from_columns(&[
|
||||
poss1.position * joint.basis2[0],
|
||||
poss1.position * joint.basis2[1],
|
||||
]);
|
||||
basis2 = Matrix3x2::from_columns(&[
|
||||
poss2.position * joint.basis1[0],
|
||||
poss2.position * joint.basis1[1],
|
||||
]);
|
||||
} else {
|
||||
axis1 = poss1.position * *joint.local_axis1;
|
||||
axis2 = poss2.position * *joint.local_axis2;
|
||||
anchor1 = poss1.position * joint.local_anchor1;
|
||||
anchor2 = poss2.position * joint.local_anchor2;
|
||||
basis1 = Matrix3x2::from_columns(&[
|
||||
poss1.position * joint.basis1[0],
|
||||
poss1.position * joint.basis1[1],
|
||||
]);
|
||||
basis2 = Matrix3x2::from_columns(&[
|
||||
poss2.position * joint.basis2[0],
|
||||
poss2.position * joint.basis2[1],
|
||||
]);
|
||||
};
|
||||
|
||||
let basis_projection2 = basis2 * basis2.transpose();
|
||||
let basis2 = basis_projection2 * basis1;
|
||||
let im2 = mprops2.effective_inv_mass;
|
||||
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
|
||||
let r1 = anchor1 - mprops1.world_com;
|
||||
let r2 = anchor2 - mprops2.world_com;
|
||||
let r2_mat = r2.gcross_matrix();
|
||||
|
||||
let mut lhs = Matrix5::zeros();
|
||||
let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2);
|
||||
let lhs10 = basis2.tr_mul(&(ii2 * r2_mat));
|
||||
let lhs11 = ii2.quadform3x2(&basis2).into_matrix();
|
||||
|
||||
// Note that cholesky won't read the upper-right part
|
||||
// of lhs so we don't have to fill it.
|
||||
lhs.fixed_slice_mut::<3, 3>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<2, 3>(3, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<2, 2>(3, 3).copy_from(&lhs11);
|
||||
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let linvel_err =
|
||||
(vels2.linvel + vels2.angvel.gcross(r2)) - (vels1.linvel + vels1.angvel.gcross(r1));
|
||||
let angvel_err = basis2.tr_mul(&vels2.angvel) - basis1.tr_mul(&vels1.angvel);
|
||||
let mut rhs = Vector5::new(
|
||||
linvel_err.x,
|
||||
linvel_err.y,
|
||||
linvel_err.z,
|
||||
angvel_err.x,
|
||||
angvel_err.y,
|
||||
) * params.velocity_solve_fraction;
|
||||
|
||||
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
|
||||
if velocity_based_erp_inv_dt != 0.0 {
|
||||
let lin_err = anchor2 - anchor1;
|
||||
|
||||
let (axis1, axis2);
|
||||
if flipped {
|
||||
axis1 = poss1.position * joint.local_axis2;
|
||||
axis2 = poss2.position * joint.local_axis1;
|
||||
} else {
|
||||
axis1 = poss1.position * joint.local_axis1;
|
||||
axis2 = poss2.position * joint.local_axis2;
|
||||
}
|
||||
let axis_error = axis1.cross(&axis2);
|
||||
let ang_err = basis2.tr_mul(&axis_error);
|
||||
|
||||
rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y)
|
||||
* velocity_based_erp_inv_dt;
|
||||
}
|
||||
|
||||
/*
|
||||
* Motor part.
|
||||
*/
|
||||
let mut motor_rhs = 0.0;
|
||||
let mut motor_inv_lhs = 0.0;
|
||||
let mut motor_angle = 0.0;
|
||||
let motor_max_impulse = joint.motor_max_impulse;
|
||||
|
||||
let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
|
||||
params.dt,
|
||||
joint.motor_stiffness,
|
||||
joint.motor_damping,
|
||||
);
|
||||
|
||||
if stiffness != 0.0 || joint.limits_enabled {
|
||||
motor_angle = joint.estimate_motor_angle(&poss1.position, &poss2.position);
|
||||
}
|
||||
|
||||
if stiffness != 0.0 {
|
||||
motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness;
|
||||
}
|
||||
|
||||
if damping != 0.0 {
|
||||
let curr_vel = vels2.angvel.dot(&axis2) - vels1.angvel.dot(&axis1);
|
||||
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
|
||||
}
|
||||
|
||||
if stiffness != 0.0 || damping != 0.0 {
|
||||
motor_inv_lhs = if keep_lhs {
|
||||
crate::utils::inv(axis2.dot(&ii2.transform_vector(axis2))) * gamma
|
||||
} else {
|
||||
gamma
|
||||
};
|
||||
motor_rhs /= gamma;
|
||||
}
|
||||
|
||||
let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
|
||||
* params.warmstart_coeff;
|
||||
|
||||
/*
|
||||
* Setup limit constraint.
|
||||
*/
|
||||
let mut limits_active = false;
|
||||
let mut limits_rhs = 0.0;
|
||||
let mut limits_inv_lhs = 0.0;
|
||||
let mut limits_impulse_limits = (0.0, 0.0);
|
||||
let mut limits_impulse = 0.0;
|
||||
|
||||
if joint.limits_enabled {
|
||||
// TODO: we should allow predictive constraint activation.
|
||||
let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]);
|
||||
let min_enabled = motor_angle < min_limit;
|
||||
let max_enabled = max_limit < motor_angle;
|
||||
|
||||
limits_impulse_limits.0 = if max_enabled { -Real::INFINITY } else { 0.0 };
|
||||
limits_impulse_limits.1 = if min_enabled { Real::INFINITY } else { 0.0 };
|
||||
limits_active = min_enabled || max_enabled;
|
||||
|
||||
if limits_active {
|
||||
limits_rhs = (vels2.angvel.dot(&axis2) - vels1.angvel.dot(&axis1))
|
||||
* params.velocity_solve_fraction;
|
||||
|
||||
limits_rhs += ((motor_angle - max_limit).max(0.0)
|
||||
- (min_limit - motor_angle).max(0.0))
|
||||
* velocity_based_erp_inv_dt;
|
||||
|
||||
limits_inv_lhs = crate::utils::inv(axis2.dot(&ii2.transform_vector(axis2)));
|
||||
|
||||
limits_impulse = joint
|
||||
.limits_impulse
|
||||
.max(limits_impulse_limits.0)
|
||||
.min(limits_impulse_limits.1)
|
||||
* params.warmstart_coeff;
|
||||
}
|
||||
}
|
||||
|
||||
let result = RevoluteVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2: ids2.active_set_offset,
|
||||
im2,
|
||||
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
|
||||
impulse: joint.impulse * params.warmstart_coeff,
|
||||
basis2,
|
||||
inv_lhs,
|
||||
rhs,
|
||||
r2,
|
||||
motor_inv_lhs,
|
||||
motor_impulse,
|
||||
motor_axis2: axis2,
|
||||
motor_max_impulse,
|
||||
motor_rhs,
|
||||
motor_angle,
|
||||
limits_impulse,
|
||||
limits_impulse_limits,
|
||||
limits_active,
|
||||
limits_inv_lhs,
|
||||
limits_rhs,
|
||||
};
|
||||
|
||||
AnyJointVelocityConstraint::RevoluteGroundConstraint(result)
|
||||
}
|
||||
|
||||
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::<3>(0).into_owned();
|
||||
let ang_impulse = self.basis2 * self.impulse.fixed_rows::<2>(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));
|
||||
|
||||
/*
|
||||
* Motor
|
||||
*/
|
||||
if self.motor_inv_lhs != 0.0 {
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(self.motor_axis2 * self.motor_impulse);
|
||||
}
|
||||
|
||||
// Warmstart limits.
|
||||
if self.limits_active {
|
||||
let limit_impulse2 = self.motor_axis2 * self.limits_impulse;
|
||||
mj_lambda2.angular += self.ii2_sqrt.transform_vector(limit_impulse2);
|
||||
}
|
||||
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
|
||||
let ang_dvel = self.basis2.tr_mul(&ang_vel2);
|
||||
let rhs =
|
||||
Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
self.impulse += impulse;
|
||||
let lin_impulse = impulse.fixed_rows::<3>(0).into_owned();
|
||||
let ang_impulse = self.basis2 * impulse.fixed_rows::<2>(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));
|
||||
}
|
||||
|
||||
fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
|
||||
if self.limits_active {
|
||||
let limits_torquedir2 = self.motor_axis2;
|
||||
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let ang_dvel = limits_torquedir2.dot(&ang_vel2) + self.limits_rhs;
|
||||
let new_impulse = (self.limits_impulse - ang_dvel * self.limits_inv_lhs)
|
||||
.max(self.limits_impulse_limits.0)
|
||||
.min(self.limits_impulse_limits.1);
|
||||
let dimpulse = new_impulse - self.limits_impulse;
|
||||
self.limits_impulse = new_impulse;
|
||||
|
||||
let ang_impulse2 = limits_torquedir2 * dimpulse;
|
||||
mj_lambda2.angular += self.ii2_sqrt.transform_vector(ang_impulse2);
|
||||
}
|
||||
}
|
||||
|
||||
fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
|
||||
if self.motor_inv_lhs != 0.0 {
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let ang_dvel = ang_vel2.dot(&self.motor_axis2);
|
||||
let rhs = ang_dvel + self.motor_rhs;
|
||||
|
||||
let new_motor_impulse = na::clamp(
|
||||
self.motor_impulse + self.motor_inv_lhs * rhs,
|
||||
-self.motor_max_impulse,
|
||||
self.motor_max_impulse,
|
||||
);
|
||||
let impulse = new_motor_impulse - self.motor_impulse;
|
||||
self.motor_impulse = new_motor_impulse;
|
||||
|
||||
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.motor_axis2 * impulse);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
self.solve_limits(&mut mj_lambda2);
|
||||
self.solve_dofs(&mut mj_lambda2);
|
||||
self.solve_motors(&mut mj_lambda2);
|
||||
|
||||
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::RevoluteJoint(revolute) = &mut joint.params {
|
||||
revolute.impulse = self.impulse;
|
||||
revolute.motor_impulse = self.motor_impulse;
|
||||
revolute.motor_last_angle = self.motor_angle;
|
||||
revolute.limits_impulse = self.limits_impulse;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,527 +0,0 @@
|
||||
use simba::simd::SimdValue;
|
||||
|
||||
use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{
|
||||
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBodyIds,
|
||||
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
|
||||
};
|
||||
use crate::math::{
|
||||
AngVector, AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Vector, SIMD_WIDTH,
|
||||
};
|
||||
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
|
||||
use na::{Cholesky, Matrix3x2, Matrix5, Unit, Vector5};
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WRevoluteVelocityConstraint {
|
||||
mj_lambda1: [usize; SIMD_WIDTH],
|
||||
mj_lambda2: [usize; SIMD_WIDTH],
|
||||
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
|
||||
r1: Vector<SimdReal>,
|
||||
r2: Vector<SimdReal>,
|
||||
|
||||
inv_lhs: Matrix5<SimdReal>,
|
||||
rhs: Vector5<SimdReal>,
|
||||
impulse: Vector5<SimdReal>,
|
||||
|
||||
axis1: [Vector<Real>; SIMD_WIDTH],
|
||||
basis1: Matrix3x2<SimdReal>,
|
||||
basis2: Matrix3x2<SimdReal>,
|
||||
|
||||
im1: SimdReal,
|
||||
im2: SimdReal,
|
||||
|
||||
ii1_sqrt: AngularInertia<SimdReal>,
|
||||
ii2_sqrt: AngularInertia<SimdReal>,
|
||||
}
|
||||
|
||||
impl WRevoluteVelocityConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
rbs1: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
rbs2: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
joints: [&RevoluteJoint; SIMD_WIDTH],
|
||||
) -> Self {
|
||||
let (poss1, vels1, mprops1, ids1) = rbs1;
|
||||
let (poss2, vels2, mprops2, ids2) = rbs2;
|
||||
|
||||
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
|
||||
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
|
||||
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
|
||||
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
|
||||
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
|
||||
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|
||||
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
|
||||
]);
|
||||
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
|
||||
|
||||
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
|
||||
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
|
||||
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
|
||||
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
|
||||
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
|
||||
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|
||||
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
|
||||
]);
|
||||
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
|
||||
|
||||
let local_anchor1 = Point::from(gather![|ii| joints[ii].local_anchor1]);
|
||||
let local_anchor2 = Point::from(gather![|ii| joints[ii].local_anchor2]);
|
||||
let local_basis1 = [
|
||||
Vector::from(gather![|ii| joints[ii].basis1[0]]),
|
||||
Vector::from(gather![|ii| joints[ii].basis1[1]]),
|
||||
];
|
||||
let local_basis2 = [
|
||||
Vector::from(gather![|ii| joints[ii].basis2[0]]),
|
||||
Vector::from(gather![|ii| joints[ii].basis2[1]]),
|
||||
];
|
||||
let impulse = Vector5::from(gather![|ii| joints[ii].impulse]);
|
||||
|
||||
let anchor1 = position1 * local_anchor1;
|
||||
let anchor2 = position2 * local_anchor2;
|
||||
let basis1 =
|
||||
Matrix3x2::from_columns(&[position1 * local_basis1[0], position1 * local_basis1[1]]);
|
||||
let basis2 =
|
||||
Matrix3x2::from_columns(&[position2 * local_basis2[0], position2 * local_basis2[1]]);
|
||||
let basis_projection2 = basis2 * basis2.transpose();
|
||||
let basis2 = basis_projection2 * basis1;
|
||||
|
||||
let ii1 = ii1_sqrt.squared();
|
||||
let r1 = anchor1 - world_com1;
|
||||
let r1_mat = r1.gcross_matrix();
|
||||
|
||||
let ii2 = ii2_sqrt.squared();
|
||||
let r2 = anchor2 - world_com2;
|
||||
let r2_mat = r2.gcross_matrix();
|
||||
|
||||
let mut lhs = Matrix5::zeros();
|
||||
let lhs00 =
|
||||
ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1);
|
||||
let lhs10 = basis1.tr_mul(&(ii2 * r2_mat)) + basis2.tr_mul(&(ii1 * r1_mat));
|
||||
let lhs11 = (ii1.quadform3x2(&basis1) + ii2.quadform3x2(&basis2)).into_matrix();
|
||||
|
||||
// Note that Cholesky won't read the upper-right part
|
||||
// of lhs so we don't have to fill it.
|
||||
lhs.fixed_slice_mut::<3, 3>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<2, 3>(3, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<2, 2>(3, 3).copy_from(&lhs11);
|
||||
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let linvel_err = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
|
||||
let angvel_err = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1);
|
||||
|
||||
let mut rhs = Vector5::new(
|
||||
linvel_err.x,
|
||||
linvel_err.y,
|
||||
linvel_err.z,
|
||||
angvel_err.x,
|
||||
angvel_err.y,
|
||||
) * SimdReal::splat(params.velocity_solve_fraction);
|
||||
|
||||
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
|
||||
if velocity_based_erp_inv_dt != 0.0 {
|
||||
let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
|
||||
|
||||
let lin_err = anchor2 - anchor1;
|
||||
|
||||
let local_axis1 = Unit::<Vector<_>>::from(gather![|ii| joints[ii].local_axis1]);
|
||||
let local_axis2 = Unit::<Vector<_>>::from(gather![|ii| joints[ii].local_axis2]);
|
||||
|
||||
let axis1 = position1 * local_axis1;
|
||||
let axis2 = position2 * local_axis2;
|
||||
|
||||
let axis_error = axis1.cross(&axis2);
|
||||
let ang_err =
|
||||
(basis2.tr_mul(&axis_error) + basis1.tr_mul(&axis_error)) * SimdReal::splat(0.5);
|
||||
|
||||
rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y)
|
||||
* velocity_based_erp_inv_dt;
|
||||
}
|
||||
|
||||
/*
|
||||
* Adjust the warmstart impulse.
|
||||
* If the velocity along the free axis is somewhat high,
|
||||
* we need to adjust the angular warmstart impulse because it
|
||||
* may have a direction that is too different than last frame,
|
||||
* making it counter-productive.
|
||||
*/
|
||||
let warmstart_coeff = SimdReal::splat(params.warmstart_coeff);
|
||||
let mut impulse = impulse * warmstart_coeff;
|
||||
|
||||
let axis1 = gather![|ii| poss1[ii].position * *joints[ii].local_axis1];
|
||||
let rotated_impulse = Vector::from(gather![|ii| {
|
||||
let axis_rot = Rotation::rotation_between(&joints[ii].prev_axis1, &axis1[ii])
|
||||
.unwrap_or_else(Rotation::identity);
|
||||
axis_rot * joints[ii].world_ang_impulse
|
||||
}]);
|
||||
|
||||
let rotated_basis_impulse = basis1.tr_mul(&rotated_impulse);
|
||||
impulse[3] = rotated_basis_impulse.x * warmstart_coeff;
|
||||
impulse[4] = rotated_basis_impulse.y * warmstart_coeff;
|
||||
|
||||
WRevoluteVelocityConstraint {
|
||||
joint_id,
|
||||
mj_lambda1,
|
||||
mj_lambda2,
|
||||
im1,
|
||||
ii1_sqrt,
|
||||
axis1,
|
||||
basis1,
|
||||
basis2,
|
||||
im2,
|
||||
ii2_sqrt,
|
||||
impulse,
|
||||
inv_lhs,
|
||||
rhs,
|
||||
r1,
|
||||
r2,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda1 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
let lin_impulse1 = self.impulse.fixed_rows::<3>(0).into_owned();
|
||||
let lin_impulse2 = self.impulse.fixed_rows::<3>(0).into_owned();
|
||||
let ang_impulse1 = self.basis1 * self.impulse.fixed_rows::<2>(3).into_owned();
|
||||
let ang_impulse2 = self.basis2 * self.impulse.fixed_rows::<2>(3).into_owned();
|
||||
|
||||
mj_lambda1.linear += lin_impulse1 * self.im1;
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
|
||||
|
||||
mj_lambda2.linear -= lin_impulse2 * self.im2;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
|
||||
|
||||
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 {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let lin_dvel = (mj_lambda2.linear + ang_vel2.gcross(self.r2))
|
||||
- (mj_lambda1.linear + ang_vel1.gcross(self.r1));
|
||||
let ang_dvel = self.basis2.tr_mul(&ang_vel2) - self.basis1.tr_mul(&ang_vel1);
|
||||
let rhs =
|
||||
Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
self.impulse += impulse;
|
||||
let lin_impulse1 = impulse.fixed_rows::<3>(0).into_owned();
|
||||
let lin_impulse2 = impulse.fixed_rows::<3>(0).into_owned();
|
||||
let ang_impulse1 = self.basis1 * impulse.fixed_rows::<2>(3).into_owned();
|
||||
let ang_impulse2 = self.basis2 * impulse.fixed_rows::<2>(3).into_owned();
|
||||
|
||||
mj_lambda1.linear += lin_impulse1 * self.im1;
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
|
||||
|
||||
mj_lambda2.linear -= lin_impulse2 * self.im2;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
|
||||
|
||||
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]) {
|
||||
let rot_part = self.impulse.fixed_rows::<2>(3).into_owned();
|
||||
let world_ang_impulse = self.basis1 * rot_part;
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
let joint = &mut joints_all[self.joint_id[ii]].weight;
|
||||
if let JointParams::RevoluteJoint(rev) = &mut joint.params {
|
||||
rev.impulse = self.impulse.extract(ii);
|
||||
rev.world_ang_impulse = world_ang_impulse.extract(ii);
|
||||
rev.prev_axis1 = self.axis1[ii];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WRevoluteVelocityGroundConstraint {
|
||||
mj_lambda2: [usize; SIMD_WIDTH],
|
||||
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
|
||||
r2: Vector<SimdReal>,
|
||||
|
||||
inv_lhs: Matrix5<SimdReal>,
|
||||
rhs: Vector5<SimdReal>,
|
||||
impulse: Vector5<SimdReal>,
|
||||
|
||||
basis2: Matrix3x2<SimdReal>,
|
||||
|
||||
im2: SimdReal,
|
||||
|
||||
ii2_sqrt: AngularInertia<SimdReal>,
|
||||
}
|
||||
|
||||
impl WRevoluteVelocityGroundConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
rbs1: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
),
|
||||
rbs2: (
|
||||
[&RigidBodyPosition; SIMD_WIDTH],
|
||||
[&RigidBodyVelocity; SIMD_WIDTH],
|
||||
[&RigidBodyMassProps; SIMD_WIDTH],
|
||||
[&RigidBodyIds; SIMD_WIDTH],
|
||||
),
|
||||
joints: [&RevoluteJoint; SIMD_WIDTH],
|
||||
flipped: [bool; SIMD_WIDTH],
|
||||
) -> Self {
|
||||
let (poss1, vels1, mprops1) = rbs1;
|
||||
let (poss2, vels2, mprops2, ids2) = rbs2;
|
||||
|
||||
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
|
||||
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
|
||||
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
|
||||
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
|
||||
|
||||
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
|
||||
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
|
||||
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
|
||||
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
|
||||
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
|
||||
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|
||||
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
|
||||
]);
|
||||
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
|
||||
let impulse = Vector5::from(gather![|ii| joints[ii].impulse]);
|
||||
|
||||
let local_anchor1 = Point::from(gather![|ii| if flipped[ii] {
|
||||
joints[ii].local_anchor2
|
||||
} else {
|
||||
joints[ii].local_anchor1
|
||||
}]);
|
||||
let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
|
||||
joints[ii].local_anchor1
|
||||
} else {
|
||||
joints[ii].local_anchor2
|
||||
}]);
|
||||
let basis1 = Matrix3x2::from_columns(&[
|
||||
position1
|
||||
* Vector::from(gather![|ii| if flipped[ii] {
|
||||
joints[ii].basis2[0]
|
||||
} else {
|
||||
joints[ii].basis1[0]
|
||||
}]),
|
||||
position1
|
||||
* Vector::from(gather![|ii| if flipped[ii] {
|
||||
joints[ii].basis2[1]
|
||||
} else {
|
||||
joints[ii].basis1[1]
|
||||
}]),
|
||||
]);
|
||||
let basis2 = Matrix3x2::from_columns(&[
|
||||
position2
|
||||
* Vector::from(gather![|ii| if flipped[ii] {
|
||||
joints[ii].basis1[0]
|
||||
} else {
|
||||
joints[ii].basis2[0]
|
||||
}]),
|
||||
position2
|
||||
* Vector::from(gather![|ii| if flipped[ii] {
|
||||
joints[ii].basis1[1]
|
||||
} else {
|
||||
joints[ii].basis2[1]
|
||||
}]),
|
||||
]);
|
||||
let basis_projection2 = basis2 * basis2.transpose();
|
||||
let basis2 = basis_projection2 * basis1;
|
||||
|
||||
let anchor1 = position1 * local_anchor1;
|
||||
let anchor2 = position2 * local_anchor2;
|
||||
|
||||
let ii2 = ii2_sqrt.squared();
|
||||
let r1 = anchor1 - world_com1;
|
||||
let r2 = anchor2 - world_com2;
|
||||
let r2_mat = r2.gcross_matrix();
|
||||
|
||||
let mut lhs = Matrix5::zeros();
|
||||
let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2);
|
||||
let lhs10 = basis2.tr_mul(&(ii2 * r2_mat));
|
||||
let lhs11 = ii2.quadform3x2(&basis2).into_matrix();
|
||||
|
||||
// Note that cholesky won't read the upper-right part
|
||||
// of lhs so we don't have to fill it.
|
||||
lhs.fixed_slice_mut::<3, 3>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<2, 3>(3, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<2, 2>(3, 3).copy_from(&lhs11);
|
||||
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let linvel_err = (linvel2 + angvel2.gcross(r2)) - (linvel1 + angvel1.gcross(r1));
|
||||
let angvel_err = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1);
|
||||
|
||||
let mut rhs = Vector5::new(
|
||||
linvel_err.x,
|
||||
linvel_err.y,
|
||||
linvel_err.z,
|
||||
angvel_err.x,
|
||||
angvel_err.y,
|
||||
) * SimdReal::splat(params.velocity_solve_fraction);
|
||||
|
||||
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
|
||||
if velocity_based_erp_inv_dt != 0.0 {
|
||||
let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
|
||||
|
||||
let lin_err = anchor2 - anchor1;
|
||||
|
||||
let local_axis1 = Unit::<Vector<_>>::from(gather![|ii| if flipped[ii] {
|
||||
joints[ii].local_axis2
|
||||
} else {
|
||||
joints[ii].local_axis1
|
||||
}]);
|
||||
let local_axis2 = Unit::<Vector<_>>::from(gather![|ii| if flipped[ii] {
|
||||
joints[ii].local_axis1
|
||||
} else {
|
||||
joints[ii].local_axis2
|
||||
}]);
|
||||
let axis1 = position1 * local_axis1;
|
||||
let axis2 = position2 * local_axis2;
|
||||
|
||||
let axis_error = axis1.cross(&axis2);
|
||||
let ang_err = basis2.tr_mul(&axis_error) - basis1.tr_mul(&axis_error);
|
||||
|
||||
rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y)
|
||||
* velocity_based_erp_inv_dt;
|
||||
}
|
||||
|
||||
WRevoluteVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2,
|
||||
im2,
|
||||
ii2_sqrt,
|
||||
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
|
||||
basis2,
|
||||
inv_lhs,
|
||||
rhs,
|
||||
r2,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
let lin_impulse = self.impulse.fixed_rows::<3>(0).into_owned();
|
||||
let ang_impulse = self.basis2 * self.impulse.fixed_rows::<2>(3).into_owned();
|
||||
|
||||
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_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_lambda2 = DeltaVel {
|
||||
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
|
||||
angular: AngVector::from(gather![
|
||||
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
|
||||
]),
|
||||
};
|
||||
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
|
||||
let ang_dvel = self.basis2.tr_mul(&ang_vel2);
|
||||
let rhs =
|
||||
Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
self.impulse += impulse;
|
||||
let lin_impulse = impulse.fixed_rows::<3>(0).into_owned();
|
||||
let ang_impulse = self.basis2 * impulse.fixed_rows::<2>(3).into_owned();
|
||||
|
||||
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_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
// FIXME: duplicated code with the non-ground constraint.
|
||||
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::RevoluteJoint(rev) = &mut joint.params {
|
||||
rev.impulse = self.impulse.extract(ii)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user