nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Joint API and joint motors improvements

Browse Source
This commit is contained in:
Sébastien Crozet
2022-02-20 12:55:00 +01:00
committed by Sébastien Crozet
parent e740493b98
commit fb20d72ee2
108 changed files with 2650 additions and 1854 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/dynamics/solver/generic_velocity_constraint.rs
View File

@@ -261,7 +261,7 @@ impl GenericVelocityConstraint {
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
* (vel1 - vel2).dot(&force_dir1);
rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
rhs_wo_bias *= is_bouncy + is_resting;
let rhs_bias =
/* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0);

2
src/dynamics/solver/generic_velocity_ground_constraint.rs
View File

@@ -145,7 +145,7 @@ impl GenericVelocityGroundConstraint {
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
* (vel1 - vel2).dot(&force_dir1);
rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
rhs_wo_bias *= is_bouncy + is_resting ;
let rhs_bias =
/* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0);

2
src/dynamics/solver/island_solver.rs
View File

@@ -1,6 +1,6 @@
use super::VelocitySolver;
use crate::counters::Counters;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::data::{ComponentSet, ComponentSetMut};
use crate::dynamics::solver::{
AnyGenericVelocityConstraint, AnyJointVelocityConstraint, AnyVelocityConstraint,
SolverConstraints,

151
src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs
View File

@@ -1,7 +1,7 @@
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::dynamics::{GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, Multibody};
use crate::math::{Isometry, Real, DIM};
use crate::prelude::SPATIAL_DIM;
use na::{DVector, DVectorSlice, DVectorSliceMut};
@@ -25,6 +25,8 @@ pub struct JointGenericVelocityConstraint {
pub inv_lhs: Real,
pub rhs: Real,
pub rhs_wo_bias: Real,
pub cfm_coeff: Real,
pub cfm_gain: Real,
pub writeback_id: WritebackId,
}
@@ -52,6 +54,8 @@ impl JointGenericVelocityConstraint {
inv_lhs: 0.0,
rhs: 0.0,
rhs_wo_bias: 0.0,
cfm_coeff: 0.0,
cfm_gain: 0.0,
writeback_id: WritebackId::Dof(0),
}
}
@@ -65,7 +69,7 @@ impl JointGenericVelocityConstraint {
mb2: Option<(&Multibody, usize)>,
frame1: &Isometry<Real>,
frame2: &Isometry<Real>,
joint: &JointData,
joint: &GenericJoint,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
out: &mut [Self],
@@ -83,26 +87,10 @@ impl JointGenericVelocityConstraint {
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;
}
}
let start = len;
for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular_generic(
if (motor_axes >> DIM) & (1 << i) != 0 {
out[len] = builder.motor_angular_generic(
params,
jacobians,
j_id,
@@ -112,12 +100,12 @@ impl JointGenericVelocityConstraint {
mb1,
mb2,
i - DIM,
WritebackId::Dof(i),
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
);
len += 1;
}
}
for i in 0..DIM {
if motor_axes & (1 << i) != 0 {
out[len] = builder.motor_linear_generic(
@@ -137,10 +125,15 @@ impl JointGenericVelocityConstraint {
len += 1;
}
}
JointVelocityConstraintBuilder::finalize_generic_constraints(
jacobians,
&mut out[start..len],
);
let start = len;
for i in DIM..SPATIAL_DIM {
if (motor_axes >> DIM) & (1 << i) != 0 {
out[len] = builder.motor_angular_generic(
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular_generic(
params,
jacobians,
j_id,
@@ -150,16 +143,14 @@ impl JointGenericVelocityConstraint {
mb1,
mb2,
i - DIM,
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
WritebackId::Dof(i),
);
len += 1;
}
}
for i in 0..DIM {
if limit_axes & (1 << i) != 0 {
out[len] = builder.limit_linear_generic(
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_linear_generic(
params,
jacobians,
j_id,
@@ -169,8 +160,7 @@ impl JointGenericVelocityConstraint {
mb1,
mb2,
i,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
WritebackId::Dof(i),
);
len += 1;
}
@@ -194,8 +184,29 @@ impl JointGenericVelocityConstraint {
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;
}
}
JointVelocityConstraintBuilder::finalize_generic_constraints(jacobians, &mut out[..len]);
JointVelocityConstraintBuilder::finalize_generic_constraints(
jacobians,
&mut out[start..len],
);
len
}
@@ -273,7 +284,7 @@ impl JointGenericVelocityConstraint {
let dvel = self.rhs + (vel2 - vel1);
let total_impulse = na::clamp(
self.impulse + self.inv_lhs * dvel,
self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse),
self.impulse_bounds[0],
self.impulse_bounds[1],
);
@@ -316,6 +327,8 @@ pub struct JointGenericVelocityGroundConstraint {
pub inv_lhs: Real,
pub rhs: Real,
pub rhs_wo_bias: Real,
pub cfm_coeff: Real,
pub cfm_gain: Real,
pub writeback_id: WritebackId,
}
@@ -338,6 +351,8 @@ impl JointGenericVelocityGroundConstraint {
inv_lhs: 0.0,
rhs: 0.0,
rhs_wo_bias: 0.0,
cfm_coeff: 0.0,
cfm_gain: 0.0,
writeback_id: WritebackId::Dof(0),
}
}
@@ -350,7 +365,7 @@ impl JointGenericVelocityGroundConstraint {
mb2: (&Multibody, usize),
frame1: &Isometry<Real>,
frame2: &Isometry<Real>,
joint: &JointData,
joint: &GenericJoint,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
out: &mut [Self],
@@ -368,32 +383,20 @@ impl JointGenericVelocityGroundConstraint {
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;
}
}
let start = len;
for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular_generic_ground(
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,
WritebackId::Dof(i),
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
);
len += 1;
}
@@ -418,27 +421,30 @@ impl JointGenericVelocityGroundConstraint {
}
}
JointVelocityConstraintBuilder::finalize_generic_constraints_ground(
jacobians,
&mut out[start..len],
);
let start = len;
for i in DIM..SPATIAL_DIM {
if (motor_axes >> DIM) & (1 << i) != 0 {
out[len] = builder.motor_angular_generic_ground(
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular_generic_ground(
params,
jacobians,
j_id,
joint_id,
body1,
body2,
mb2,
i - DIM,
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
WritebackId::Dof(i),
);
len += 1;
}
}
for i in 0..DIM {
if limit_axes & (1 << i) != 0 {
out[len] = builder.limit_linear_generic_ground(
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_linear_generic_ground(
params,
jacobians,
j_id,
@@ -446,8 +452,7 @@ impl JointGenericVelocityGroundConstraint {
body1,
mb2,
i,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
WritebackId::Dof(i),
);
len += 1;
}
@@ -469,10 +474,26 @@ impl JointGenericVelocityGroundConstraint {
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;
}
}
JointVelocityConstraintBuilder::finalize_generic_constraints_ground(
jacobians,
&mut out[..len],
&mut out[start..len],
);
len
}
@@ -511,7 +532,7 @@ impl JointGenericVelocityGroundConstraint {
let dvel = self.rhs + vel2;
let total_impulse = na::clamp(
self.impulse + self.inv_lhs * dvel,
self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse),
self.impulse_bounds[0],
self.impulse_bounds[1],
);

98
src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs
View File

@@ -113,7 +113,7 @@ impl JointVelocityConstraintBuilder<Real> {
j.copy_from(&wj);
}
let rhs_wo_bias = (vel2 - vel1) * params.velocity_solve_fraction;
let rhs_wo_bias = vel2 - vel1;
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]);
@@ -133,6 +133,8 @@ impl JointVelocityConstraintBuilder<Real> {
inv_lhs: 0.0,
rhs: rhs_wo_bias,
rhs_wo_bias,
cfm_coeff: 0.0,
cfm_gain: 0.0,
writeback_id,
}
}
@@ -169,7 +171,7 @@ impl JointVelocityConstraintBuilder<Real> {
ang_jac2,
);
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.joint_erp_inv_dt();
let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
c.rhs += rhs_bias;
c
@@ -212,7 +214,7 @@ impl JointVelocityConstraintBuilder<Real> {
let min_enabled = dist < limits[0];
let max_enabled = limits[1] < dist;
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.joint_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 = [
@@ -265,20 +267,20 @@ impl JointVelocityConstraintBuilder<Real> {
);
let mut rhs_wo_bias = 0.0;
if motor_params.stiffness != 0.0 {
if motor_params.erp_inv_dt != 0.0 {
let dist = self.lin_err.dot(&lin_jac);
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
}
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;
}
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;
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
constraint.rhs = rhs_wo_bias;
constraint.rhs_wo_bias = rhs_wo_bias;
constraint.cfm_coeff = motor_params.cfm_coeff;
constraint.cfm_gain = motor_params.cfm_gain;
constraint
}
@@ -312,7 +314,7 @@ impl JointVelocityConstraintBuilder<Real> {
ang_jac,
);
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.joint_erp_inv_dt();
#[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")]
@@ -364,7 +366,7 @@ impl JointVelocityConstraintBuilder<Real> {
max_enabled as u32 as Real * Real::MAX,
];
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.joint_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;
@@ -409,22 +411,22 @@ impl JointVelocityConstraintBuilder<Real> {
);
let mut rhs_wo_bias = 0.0;
if motor_params.stiffness != 0.0 {
if motor_params.erp_inv_dt != 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;
rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt;
}
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;
}
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
rhs_wo_bias += dvel - motor_params.target_vel;
constraint.rhs_wo_bias = rhs_wo_bias;
constraint.rhs = rhs_wo_bias;
constraint.cfm_coeff = motor_params.cfm_coeff;
constraint.cfm_gain = motor_params.cfm_gain;
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
constraint
}
@@ -436,6 +438,11 @@ impl JointVelocityConstraintBuilder<Real> {
// TODO: orthogonalization doesn’t seem to give good results for multibodies?
const ORTHOGONALIZE: bool = false;
let len = constraints.len();
if len == 0 {
return;
}
let ndofs1 = constraints[0].ndofs1;
let ndofs2 = constraints[0].ndofs2;
@@ -449,8 +456,10 @@ impl JointVelocityConstraintBuilder<Real> {
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.
let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain;
let inv_dot_jj = crate::utils::simd_inv(dot_jj);
c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs.
c_j.cfm_gain = cfm_gain;
if c_j.impulse_bounds != [-Real::MAX, Real::MAX] {
// Don't remove constraints with limited forces from the others
@@ -510,7 +519,7 @@ impl JointVelocityConstraintBuilder<Real> {
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 rhs_wo_bias = vel2 - vel1;
let mj_lambda2 = mb2.solver_id;
@@ -524,6 +533,8 @@ impl JointVelocityConstraintBuilder<Real> {
inv_lhs: 0.0,
rhs: rhs_wo_bias,
rhs_wo_bias,
cfm_coeff: 0.0,
cfm_gain: 0.0,
writeback_id,
}
}
@@ -556,7 +567,7 @@ impl JointVelocityConstraintBuilder<Real> {
ang_jac2,
);
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.joint_erp_inv_dt();
let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
c.rhs += rhs_bias;
c
@@ -595,7 +606,7 @@ impl JointVelocityConstraintBuilder<Real> {
let min_enabled = dist < limits[0];
let max_enabled = limits[1] < dist;
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.joint_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 = [
@@ -645,20 +656,20 @@ impl JointVelocityConstraintBuilder<Real> {
);
let mut rhs_wo_bias = 0.0;
if motor_params.stiffness != 0.0 {
if motor_params.erp_inv_dt != 0.0 {
let dist = self.lin_err.dot(&lin_jac);
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
}
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;
}
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;
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
constraint.rhs = rhs_wo_bias;
constraint.rhs_wo_bias = rhs_wo_bias;
constraint.cfm_coeff = motor_params.cfm_coeff;
constraint.cfm_gain = motor_params.cfm_gain;
constraint
}
@@ -688,7 +699,7 @@ impl JointVelocityConstraintBuilder<Real> {
ang_jac,
);
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.joint_erp_inv_dt();
#[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")]
@@ -736,7 +747,7 @@ impl JointVelocityConstraintBuilder<Real> {
max_enabled as u32 as Real * Real::MAX,
];
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.joint_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;
@@ -778,22 +789,22 @@ impl JointVelocityConstraintBuilder<Real> {
);
let mut rhs = 0.0;
if motor_params.stiffness != 0.0 {
if motor_params.erp_inv_dt != 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;
rhs += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt;
}
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;
}
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
rhs += dvel - motor_params.target_vel;
constraint.rhs_wo_bias = rhs;
constraint.rhs = rhs;
constraint.cfm_coeff = motor_params.cfm_coeff;
constraint.cfm_gain = motor_params.cfm_gain;
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
constraint
}
@@ -805,6 +816,11 @@ impl JointVelocityConstraintBuilder<Real> {
// TODO: orthogonalization doesn’t seem to give good results for multibodies?
const ORTHOGONALIZE: bool = false;
let len = constraints.len();
if len == 0 {
return;
}
let ndofs2 = constraints[0].ndofs2;
// Use the modified Gramm-Schmidt orthogonalization.
@@ -815,8 +831,10 @@ impl JointVelocityConstraintBuilder<Real> {
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.
let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain;
let inv_dot_jj = crate::utils::simd_inv(dot_jj);
c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs.
c_j.cfm_gain = cfm_gain;
if c_j.impulse_bounds != [-Real::MAX, Real::MAX] {
// Don't remove constraints with limited forces from the others

307
src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
View File

@@ -1,6 +1,8 @@
use crate::dynamics::solver::joint_constraint::JointVelocityConstraintBuilder;
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{IntegrationParameters, JointData, JointGraphEdge, JointIndex};
use crate::dynamics::{
GenericJoint, IntegrationParameters, JointAxesMask, JointGraphEdge, JointIndex,
};
use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, Vector, DIM, SPATIAL_DIM};
use crate::utils::{WDot, WReal};
@@ -12,10 +14,9 @@ use {
#[derive(Copy, Clone, PartialEq, Debug)]
pub struct MotorParameters<N: WReal> {
pub stiffness: N,
pub damping: N,
pub gamma: N,
// pub keep_lhs: bool,
pub erp_inv_dt: N,
pub cfm_coeff: N,
pub cfm_gain: N,
pub target_pos: N,
pub target_vel: N,
pub max_impulse: N,
@@ -24,10 +25,9 @@ pub struct MotorParameters<N: WReal> {
impl<N: WReal> Default for MotorParameters<N> {
fn default() -> Self {
Self {
stiffness: N::zero(),
damping: N::zero(),
gamma: N::zero(),
// keep_lhs: true,
erp_inv_dt: N::zero(),
cfm_coeff: N::zero(),
cfm_gain: N::zero(),
target_pos: N::zero(),
target_vel: N::zero(),
max_impulse: N::zero(),
@@ -72,6 +72,8 @@ pub struct JointVelocityConstraint<N: WReal, const LANES: usize> {
pub inv_lhs: N,
pub rhs: N,
pub rhs_wo_bias: N,
pub cfm_gain: N,
pub cfm_coeff: N,
pub im1: Vector<N>,
pub im2: Vector<N>,
@@ -91,6 +93,8 @@ impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
ang_jac1: na::zero(),
ang_jac2: na::zero(),
inv_lhs: N::zero(),
cfm_gain: N::zero(),
cfm_coeff: N::zero(),
rhs: N::zero(),
rhs_wo_bias: N::zero(),
im1: na::zero(),
@@ -105,7 +109,7 @@ impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
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)
let total_impulse = (self.impulse + self.inv_lhs * (rhs - self.cfm_gain * self.impulse))
.simd_clamp(self.impulse_bounds[0], self.impulse_bounds[1]);
let delta_impulse = total_impulse - self.impulse;
self.impulse = total_impulse;
@@ -133,13 +137,14 @@ impl JointVelocityConstraint<Real, 1> {
body2: &SolverBody<Real, 1>,
frame1: &Isometry<Real>,
frame2: &Isometry<Real>,
joint: &JointData,
joint: &GenericJoint,
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 motor_axes = joint.motor_axes.bits() & !locked_axes;
let limit_axes = joint.limit_axes.bits() & !locked_axes;
let coupled_axes = joint.coupled_axes.bits();
let builder = JointVelocityConstraintBuilder::new(
frame1,
@@ -149,13 +154,62 @@ impl JointVelocityConstraint<Real, 1> {
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));
let start = len;
for i in DIM..SPATIAL_DIM {
if (motor_axes & !coupled_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 (motor_axes & !coupled_axes) & (1 << i) != 0 {
let limits = if limit_axes & (1 << i) != 0 {
Some([joint.limits[i].min, joint.limits[i].max])
} else {
None
};
out[len] = builder.motor_linear(
params,
[joint_id],
body1,
body2,
i,
&joint.motors[i].motor_params(params.dt),
limits,
WritebackId::Motor(i),
);
len += 1;
}
}
if (motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
// TODO: coupled angular motor constraint.
}
if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
// TODO: coupled linear limit constraint.
// out[len] = builder.motor_linear_coupled(
// params,
// [joint_id],
// body1,
// body2,
// limit_axes & coupled_axes,
// &joint.limits,
// WritebackId::Limit(0), // TODO: writeback
// );
// len += 1;
}
JointVelocityConstraintBuilder::finalize_constraints(&mut out[start..len]);
let start = len;
for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular(
@@ -169,52 +223,16 @@ impl JointVelocityConstraint<Real, 1> {
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),
);
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 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 {
if (limit_axes & !coupled_axes) & (1 << i) != 0 {
out[len] = builder.limit_angular(
params,
[joint_id],
@@ -227,8 +245,40 @@ impl JointVelocityConstraint<Real, 1> {
len += 1;
}
}
for i in 0..DIM {
if (limit_axes & !coupled_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;
}
}
if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
// TODO: coupled angular limit constraint.
}
if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
// TODO: coupled linear limit constraint.
out[len] = builder.limit_linear_coupled(
params,
[joint_id],
body1,
body2,
limit_axes & coupled_axes,
&joint.limits,
WritebackId::Limit(0), // TODO: writeback
);
len += 1;
}
JointVelocityConstraintBuilder::finalize_constraints(&mut out[start..len]);
JointVelocityConstraintBuilder::finalize_constraints(&mut out[..len]);
len
}
@@ -349,6 +399,8 @@ pub struct JointVelocityGroundConstraint<N: WReal, const LANES: usize> {
pub ang_jac2: AngVector<N>,
pub inv_lhs: N,
pub cfm_coeff: N,
pub cfm_gain: N,
pub rhs: N,
pub rhs_wo_bias: N,
@@ -367,6 +419,8 @@ impl<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> {
lin_jac: Vector::zeros(),
ang_jac2: na::zero(),
inv_lhs: N::zero(),
cfm_coeff: N::zero(),
cfm_gain: N::zero(),
rhs: N::zero(),
rhs_wo_bias: N::zero(),
im2: na::zero(),
@@ -379,7 +433,7 @@ impl<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> {
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)
let total_impulse = (self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse))
.simd_clamp(self.impulse_bounds[0], self.impulse_bounds[1]);
let delta_impulse = total_impulse - self.impulse;
self.impulse = total_impulse;
@@ -404,13 +458,14 @@ impl JointVelocityGroundConstraint<Real, 1> {
body2: &SolverBody<Real, 1>,
frame1: &Isometry<Real>,
frame2: &Isometry<Real>,
joint: &JointData,
joint: &GenericJoint,
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 locked_axes = joint.locked_axes.bits();
let motor_axes = joint.motor_axes.bits() & !locked_axes;
let limit_axes = joint.limit_axes.bits() & !locked_axes;
let coupled_axes = joint.coupled_axes.bits();
let builder = JointVelocityConstraintBuilder::new(
frame1,
@@ -420,19 +475,68 @@ impl JointVelocityGroundConstraint<Real, 1> {
locked_axes,
);
let start = len;
for i in DIM..SPATIAL_DIM {
if (motor_axes & !coupled_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 locked_axes & (1 << i) != 0 {
out[len] = builder.lock_linear_ground(
if (motor_axes & !coupled_axes) & (1 << i) != 0 {
let limits = if limit_axes & (1 << i) != 0 {
Some([joint.limits[i].min, joint.limits[i].max])
} else {
None
};
out[len] = builder.motor_linear_ground(
params,
[joint_id],
body1,
body2,
i,
WritebackId::Dof(i),
&joint.motors[i].motor_params(params.dt),
limits,
WritebackId::Motor(i),
);
len += 1;
}
}
if (motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
// TODO: coupled angular motor constraint.
}
if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
/*
// TODO: coupled linear motor constraint.
out[len] = builder.motor_linear_coupled_ground(
params,
[joint_id],
body1,
body2,
motor_axes & coupled_axes,
&joint.motors,
limit_axes & coupled_axes,
&joint.limits,
WritebackId::Limit(0), // TODO: writeback
);
len += 1;
*/
todo!()
}
JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[start..len]);
let start = len;
for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular_ground(
@@ -446,50 +550,22 @@ impl JointVelocityGroundConstraint<Real, 1> {
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(
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_linear_ground(
params,
[joint_id],
body1,
body2,
i,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
WritebackId::Dof(i),
);
len += 1;
}
}
for i in DIM..SPATIAL_DIM {
if limit_axes & (1 << i) != 0 {
if (limit_axes & !coupled_axes) & (1 << i) != 0 {
out[len] = builder.limit_angular_ground(
params,
[joint_id],
@@ -502,8 +578,39 @@ impl JointVelocityGroundConstraint<Real, 1> {
len += 1;
}
}
for i in 0..DIM {
if (limit_axes & !coupled_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;
}
}
if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
// TODO: coupled angular limit constraint.
}
if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
out[len] = builder.limit_linear_coupled_ground(
params,
[joint_id],
body1,
body2,
limit_axes & coupled_axes,
&joint.limits,
WritebackId::Limit(0), // TODO: writeback
);
len += 1;
}
JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[start..len]);
JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[..len]);
len
}

405
src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
View File

@@ -3,8 +3,8 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
};
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::dynamics::{IntegrationParameters, JointAxesMask, JointIndex, JointLimits, JointMotor};
use crate::math::{AngVector, Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM};
use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal};
use na::SMatrix;
@@ -92,10 +92,12 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
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 erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
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.cfm_coeff = cfm_coeff;
constraint.impulse_bounds = [
N::splat(-Real::INFINITY).select(min_enabled, zero),
N::splat(Real::INFINITY).select(max_enabled, zero),
@@ -104,39 +106,114 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
constraint
}
pub fn limit_linear_coupled<const LANES: usize>(
&self,
params: &IntegrationParameters,
joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>,
limited_coupled_axes: u8,
limits: &[JointLimits<N>],
writeback_id: WritebackId,
) -> JointVelocityConstraint<N, LANES> {
let zero = N::zero();
let mut lin_jac = Vector::zeros();
let mut ang_jac1: AngVector<N> = na::zero();
let mut ang_jac2: AngVector<N> = na::zero();
let mut limit = N::zero();
for i in 0..DIM {
if limited_coupled_axes & (1 << i) != 0 {
let coeff = self.basis.column(i).dot(&self.lin_err);
lin_jac += self.basis.column(i) * coeff;
#[cfg(feature = "dim2")]
{
ang_jac1 += self.cmat1_basis[i] * coeff;
ang_jac2 += self.cmat2_basis[i] * coeff;
}
#[cfg(feature = "dim3")]
{
ang_jac1 += self.cmat1_basis.column(i) * coeff;
ang_jac2 += self.cmat2_basis.column(i) * coeff;
}
limit += limits[i].max * limits[i].max;
}
}
limit = limit.simd_sqrt();
let dist = lin_jac.norm();
let inv_dist = crate::utils::simd_inv(dist);
lin_jac *= inv_dist;
ang_jac1 *= inv_dist;
ang_jac2 *= inv_dist;
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt());
ang_jac1 = body1.sqrt_ii * ang_jac1;
ang_jac2 = body2.sqrt_ii * ang_jac2;
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt;
let rhs = rhs_wo_bias + rhs_bias;
let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)];
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,
ang_jac1,
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs,
rhs_wo_bias,
writeback_id,
}
}
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>,
limits: Option<[N; 2]>,
writeback_id: WritebackId,
) -> JointVelocityConstraint<N, LANES> {
let inv_dt = N::splat(params.inv_dt());
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() {
if motor_params.erp_inv_dt != N::zero() {
let dist = self.lin_err.dot(&constraint.lin_jac);
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
}
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;
}
let mut target_vel = motor_params.target_vel;
if let Some(limits) = limits {
let dist = self.lin_err.dot(&constraint.lin_jac);
target_vel =
target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt);
};
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 - target_vel;
constraint.cfm_coeff = motor_params.cfm_coeff;
constraint.cfm_gain = motor_params.cfm_gain;
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
constraint.rhs = rhs_wo_bias;
constraint.rhs_wo_bias = rhs_wo_bias;
@@ -164,10 +241,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
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 rhs_wo_bias = dvel;
let erp_inv_dt = params.erp_inv_dt();
let rhs_bias = lin_jac.dot(&self.lin_err) * N::splat(erp_inv_dt);
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
ang_jac1 = body1.sqrt_ii * ang_jac1;
ang_jac2 = body2.sqrt_ii * ang_jac2;
@@ -184,6 +262,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
ang_jac1,
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
writeback_id,
@@ -220,12 +300,13 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
#[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 rhs_wo_bias = dvel;
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero)
- (s_limits[0] - s_ang).simd_max(zero))
* N::splat(erp_inv_dt);
* erp_inv_dt;
let ang_jac1 = body1.sqrt_ii * ang_jac;
let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -242,6 +323,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
ang_jac1,
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
writeback_id,
@@ -264,20 +347,17 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let ang_jac = self.basis.column(_motor_axis).into_owned();
let mut rhs_wo_bias = N::zero();
if motor_params.stiffness != N::zero() {
if motor_params.erp_inv_dt != 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;
rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt;
}
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 dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
rhs_wo_bias += dvel - motor_params.target_vel;
let ang_jac1 = body1.sqrt_ii * ang_jac;
let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -294,6 +374,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
ang_jac1,
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff: motor_params.cfm_coeff,
cfm_gain: motor_params.cfm_gain,
rhs: rhs_wo_bias,
rhs_wo_bias,
writeback_id,
@@ -315,13 +397,14 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
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 rhs_wo_bias = dvel;
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
#[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * N::splat(erp_inv_dt);
let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")]
let rhs_bias = self.ang_err.imag()[locked_axis] * N::splat(erp_inv_dt);
let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
let ang_jac1 = body1.sqrt_ii * ang_jac;
let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -338,6 +421,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
ang_jac1,
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
writeback_id,
@@ -349,6 +434,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
constraints: &mut [JointVelocityConstraint<N, LANES>],
) {
let len = constraints.len();
if len == 0 {
return;
}
let imsum = constraints[0].im1 + constraints[0].im2;
// Use the modified Gram-Schmidt orthogonalization.
@@ -357,8 +447,10 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
+ c_j.ang_jac1.gdot(c_j.ang_jac1)
+ c_j.ang_jac2.gdot(c_j.ang_jac2);
let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain;
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.
c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs.
c_j.cfm_gain = cfm_gain;
if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] {
// Don't remove constraints with limited forces from the others
@@ -369,6 +461,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
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(&imsum.component_mul(&c_j.lin_jac))
+ c_i.ang_jac1.gdot(c_j.ang_jac1)
+ c_i.ang_jac2.gdot(c_j.ang_jac2);
@@ -396,6 +489,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
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);
@@ -412,11 +506,12 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
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 rhs_wo_bias = dvel;
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);
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
let rhs_bias =
((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) * erp_inv_dt;
ang_jac2 = body2.sqrt_ii * ang_jac2;
@@ -429,21 +524,97 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
lin_jac,
ang_jac2,
inv_lhs: zero, // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
writeback_id,
}
}
pub fn limit_linear_coupled_ground<const LANES: usize>(
&self,
params: &IntegrationParameters,
joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>,
limited_coupled_axes: u8,
limits: &[JointLimits<N>],
writeback_id: WritebackId,
) -> JointVelocityGroundConstraint<N, LANES> {
let zero = N::zero();
let mut lin_jac = Vector::zeros();
let mut ang_jac1: AngVector<N> = na::zero();
let mut ang_jac2: AngVector<N> = na::zero();
let mut limit = N::zero();
for i in 0..DIM {
if limited_coupled_axes & (1 << i) != 0 {
let coeff = self.basis.column(i).dot(&self.lin_err);
lin_jac += self.basis.column(i) * coeff;
#[cfg(feature = "dim2")]
{
ang_jac1 += self.cmat1_basis[i] * coeff;
ang_jac2 += self.cmat2_basis[i] * coeff;
}
#[cfg(feature = "dim3")]
{
ang_jac1 += self.cmat1_basis.column(i) * coeff;
ang_jac2 += self.cmat2_basis.column(i) * coeff;
}
limit += limits[i].max * limits[i].max;
}
}
limit = limit.simd_sqrt();
let dist = lin_jac.norm();
let inv_dist = crate::utils::simd_inv(dist);
lin_jac *= inv_dist;
ang_jac1 *= inv_dist;
ang_jac2 *= inv_dist;
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt());
ang_jac2 = body2.sqrt_ii * ang_jac2;
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt;
let rhs = rhs_wo_bias + rhs_bias;
let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)];
JointVelocityGroundConstraint {
joint_id,
mj_lambda2: body2.mj_lambda,
im2: body2.im,
impulse: N::zero(),
impulse_bounds,
lin_jac,
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs,
rhs_wo_bias,
writeback_id,
}
}
pub fn motor_linear_ground<const LANES: usize>(
&self,
params: &IntegrationParameters,
joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>,
motor_axis: usize,
motor_params: &MotorParameters<N>,
limits: Option<[N; 2]>,
writeback_id: WritebackId,
) -> JointVelocityGroundConstraint<N, LANES> {
let inv_dt = N::splat(params.inv_dt());
let lin_jac = self.basis.column(motor_axis).into_owned();
let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned();
#[cfg(feature = "dim2")]
@@ -452,16 +623,21 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
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() {
if motor_params.erp_inv_dt != N::zero() {
let dist = self.lin_err.dot(&lin_jac);
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
}
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;
}
let mut target_vel = motor_params.target_vel;
if let Some(limits) = limits {
let dist = self.lin_err.dot(&lin_jac);
target_vel =
target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt);
};
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
rhs_wo_bias += dvel - target_vel;
ang_jac2 = body2.sqrt_ii * ang_jac2;
@@ -474,12 +650,89 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
lin_jac,
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff: motor_params.cfm_coeff,
cfm_gain: motor_params.cfm_gain,
rhs: rhs_wo_bias,
rhs_wo_bias,
writeback_id,
}
}
pub fn motor_linear_coupled_ground<const LANES: usize>(
&self,
params: &IntegrationParameters,
joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>,
motor_coupled_axes: u8,
motors: &[MotorParameters<N>],
limited_coupled_axes: u8,
limits: &[JointLimits<N>],
writeback_id: WritebackId,
) -> JointVelocityGroundConstraint<N, LANES> {
todo!()
/*
let zero = N::zero();
let mut lin_jac = Vector::zeros();
let mut ang_jac1: AngVector<N> = na::zero();
let mut ang_jac2: AngVector<N> = na::zero();
let mut limit = N::zero();
for i in 0..DIM {
if limited_coupled_axes & (1 << i) != 0 {
let coeff = self.basis.column(i).dot(&self.lin_err);
lin_jac += self.basis.column(i) * coeff;
#[cfg(feature = "dim2")]
{
ang_jac1 += self.cmat1_basis[i] * coeff;
ang_jac2 += self.cmat2_basis[i] * coeff;
}
#[cfg(feature = "dim3")]
{
ang_jac1 += self.cmat1_basis.column(i) * coeff;
ang_jac2 += self.cmat2_basis.column(i) * coeff;
}
limit += limits[i].max * limits[i].max;
}
}
limit = limit.simd_sqrt();
let dist = lin_jac.norm();
let inv_dist = crate::utils::simd_inv(dist);
lin_jac *= inv_dist;
ang_jac1 *= inv_dist;
ang_jac2 *= inv_dist;
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt());
ang_jac2 = body2.sqrt_ii * ang_jac2;
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt;
let rhs = rhs_wo_bias + rhs_bias;
let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)];
JointVelocityGroundConstraint {
joint_id,
mj_lambda2: body2.mj_lambda,
im2: body2.im,
impulse: N::zero(),
impulse_bounds,
lin_jac,
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs,
rhs_wo_bias,
writeback_id,
}
*/
}
pub fn lock_linear_ground<const LANES: usize>(
&self,
params: &IntegrationParameters,
@@ -498,10 +751,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
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 rhs_wo_bias = dvel;
let erp_inv_dt = params.erp_inv_dt();
let rhs_bias = lin_jac.dot(&self.lin_err) * N::splat(erp_inv_dt);
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
ang_jac2 = body2.sqrt_ii * ang_jac2;
@@ -514,6 +768,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
lin_jac,
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
writeback_id,
@@ -536,20 +792,17 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let ang_jac = self.basis.column(_motor_axis).into_owned();
let mut rhs_wo_bias = N::zero();
if motor_params.stiffness != N::zero() {
if motor_params.erp_inv_dt != 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;
rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt;
}
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 dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
rhs_wo_bias += dvel - motor_params.target_vel;
let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -562,6 +815,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
lin_jac: na::zero(),
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff: motor_params.cfm_coeff,
cfm_gain: motor_params.cfm_gain,
rhs: rhs_wo_bias,
rhs_wo_bias,
writeback_id,
@@ -598,12 +853,13 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
#[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 rhs_wo_bias = dvel;
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero)
- (s_limits[0] - s_ang).simd_max(zero))
* N::splat(erp_inv_dt);
* erp_inv_dt;
let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -616,6 +872,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
lin_jac: na::zero(),
ang_jac2,
inv_lhs: zero, // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
writeback_id,
@@ -636,13 +894,14 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
#[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 rhs_wo_bias = dvel;
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
#[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * N::splat(erp_inv_dt);
let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")]
let rhs_bias = self.ang_err.imag()[locked_axis] * N::splat(erp_inv_dt);
let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -655,6 +914,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
lin_jac: na::zero(),
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
writeback_id,
@@ -666,6 +927,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
constraints: &mut [JointVelocityGroundConstraint<N, LANES>],
) {
let len = constraints.len();
if len == 0 {
return;
}
let imsum = constraints[0].im2;
// Use the modified Gram-Schmidt orthogonalization.
@@ -673,18 +939,23 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let c_j = &mut constraints[j];
let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
+ c_j.ang_jac2.gdot(c_j.ang_jac2);
let inv_dot_jj = crate::utils::simd_inv(dot_jj);
let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain;
let inv_dot_jj = crate::utils::simd_inv(dot_jj + cfm_gain);
c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
c_j.cfm_gain = cfm_gain;
if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] {
if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)]
|| c_j.cfm_gain != N::zero()
{
// 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 {
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(&imsum.component_mul(&c_j.lin_jac))
+ c_i.ang_jac2.gdot(c_j.ang_jac2);
let coeff = dot_ij * inv_dot_jj;

18
src/dynamics/solver/velocity_constraint.rs
View File

@@ -5,7 +5,7 @@ use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{WAngularInertia, WBasis, WCross, WDot, WReal};
use crate::utils::{self, WAngularInertia, WBasis, WCross, WDot, WReal};
use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart};
@@ -239,10 +239,11 @@ impl VelocityConstraint {
.transform_vector(dp2.gcross(-force_dir1));
let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
let projected_mass = 1.0
/ (force_dir1.dot(&imsum.component_mul(&force_dir1))
let projected_mass = utils::inv(
force_dir1.dot(&imsum.component_mul(&force_dir1))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
+ gcross2.gdot(gcross2),
);
let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
let is_resting = 1.0 - is_bouncy;
@@ -250,7 +251,7 @@ impl VelocityConstraint {
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
* (vel1 - vel2).dot(&force_dir1);
rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
rhs_wo_bias *= is_bouncy + is_resting;
let rhs_bias = /* is_resting
* */ erp_inv_dt
* (manifold_point.dist + params.allowed_linear_error).min(0.0);
@@ -285,8 +286,11 @@ impl VelocityConstraint {
constraint.elements[k].tangent_part.gcross1[j] = gcross1;
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.rhs[j] = rhs;
constraint.elements[k].tangent_part.r[j] =
if cfg!(feature = "dim2") { 1.0 / r } else { r };
constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
utils::inv(r)
} else {
r
};
}
#[cfg(feature = "dim3")]

14
src/dynamics/solver/velocity_constraint_wide.rs
View File

@@ -9,7 +9,7 @@ use crate::math::{
};
#[cfg(feature = "dim2")]
use crate::utils::WBasis;
use crate::utils::{WAngularInertia, WCross, WDot};
use crate::utils::{self, WAngularInertia, WCross, WDot};
use num::Zero;
use simba::simd::{SimdPartialOrd, SimdValue};
@@ -47,7 +47,6 @@ impl WVelocityConstraint {
}
let inv_dt = SimdReal::splat(params.inv_dt());
let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
@@ -135,16 +134,17 @@ impl WVelocityConstraint {
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
let imsum = im1 + im2;
let projected_mass = SimdReal::splat(1.0)
/ (force_dir1.dot(&imsum.component_mul(&force_dir1))
let projected_mass = utils::simd_inv(
force_dir1.dot(&imsum.component_mul(&force_dir1))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
+ gcross2.gdot(gcross2),
);
let projected_velocity = (vel1 - vel2).dot(&force_dir1);
let mut rhs_wo_bias =
(SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting * velocity_solve_fraction;
rhs_wo_bias *= is_bouncy + is_resting;
let rhs_bias = (dist + allowed_lin_err).simd_min(SimdReal::zero())
* (erp_inv_dt/* * is_resting */);
@@ -174,7 +174,7 @@ impl WVelocityConstraint {
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.rhs[j] = rhs;
constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
SimdReal::splat(1.0) / r
utils::simd_inv(r)
} else {
r
};

18
src/dynamics/solver/velocity_ground_constraint.rs
View File

@@ -5,7 +5,7 @@ use super::{
use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
#[cfg(feature = "dim2")]
use crate::utils::WBasis;
use crate::utils::{WAngularInertia, WCross, WDot};
use crate::utils::{self, WAngularInertia, WCross, WDot};
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
@@ -153,9 +153,10 @@ impl VelocityGroundConstraint {
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-force_dir1));
let projected_mass = 1.0
/ (force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+ gcross2.gdot(gcross2));
let projected_mass = utils::inv(
force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+ gcross2.gdot(gcross2),
);
let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
let is_resting = 1.0 - is_bouncy;
@@ -163,7 +164,7 @@ impl VelocityGroundConstraint {
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
* (vel1 - vel2).dot(&force_dir1);
rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
rhs_wo_bias *= is_bouncy + is_resting;
let rhs_bias = /* is_resting
* */ erp_inv_dt
* (manifold_point.dist + params.allowed_linear_error).min(0.0);
@@ -194,8 +195,11 @@ impl VelocityGroundConstraint {
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.rhs[j] = rhs;
constraint.elements[k].tangent_part.r[j] =
if cfg!(feature = "dim2") { 1.0 / r } else { r };
constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
utils::inv(r)
} else {
r
};
}
#[cfg(feature = "dim3")]

12
src/dynamics/solver/velocity_ground_constraint_wide.rs
View File

@@ -10,7 +10,7 @@ use crate::math::{
};
#[cfg(feature = "dim2")]
use crate::utils::WBasis;
use crate::utils::{WAngularInertia, WCross, WDot};
use crate::utils::{self, WAngularInertia, WCross, WDot};
use num::Zero;
use simba::simd::{SimdPartialOrd, SimdValue};
@@ -42,7 +42,6 @@ impl WVelocityGroundConstraint {
+ ComponentSet<RigidBodyMassProps>,
{
let inv_dt = SimdReal::splat(params.inv_dt());
let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
@@ -142,14 +141,15 @@ impl WVelocityGroundConstraint {
{
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
let projected_mass = SimdReal::splat(1.0)
/ (force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2));
let projected_mass = utils::simd_inv(
force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2),
);
let projected_velocity = (vel1 - vel2).dot(&force_dir1);
let mut rhs_wo_bias =
(SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting * velocity_solve_fraction;
rhs_wo_bias *= is_bouncy + is_resting;
let rhs_bias = (dist + allowed_lin_err).simd_min(SimdReal::zero())
* (erp_inv_dt/* * is_resting */);
@@ -174,7 +174,7 @@ impl WVelocityGroundConstraint {
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.rhs[j] = rhs;
constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
SimdReal::splat(1.0) / r
utils::simd_inv(r)
} else {
r
};