use std::any::Any;
use std::ops::MulAssign;
use crate::joint::Joint;
use crate::math::{
AngularDim, Dim, Force, ForceType, Inertia, Isometry, Jacobian, Point, SpatialMatrix,
Translation, Vector, Velocity, DIM,
};
use crate::object::{
ActivationStatus, Body, BodyHandle, BodyPart, BodyPartHandle, BodyStatus, BodyUpdateStatus,
ColliderHandle, MultibodyLink, MultibodyLinkVec,
};
use crate::solver::{
ConstraintSet, ForceDirection, IntegrationParameters, NonlinearSORProx, SORProx,
};
use na::{self, DMatrix, DVector, DVectorSlice, DVectorSliceMut, Dynamic, MatrixMN, RealField, LU};
use ncollide::shape::DeformationsType;
use crate::utils::{GeneralizedCross, IndexMut2};
pub struct Multibody<N: RealField> {
rbs: MultibodyLinkVec<N>,
velocities: DVector<N>,
damping: DVector<N>,
accelerations: DVector<N>,
forces: DVector<N>,
impulses: DVector<N>,
body_jacobians: Vec<Jacobian<N>>,
augmented_mass: DMatrix<N>,
inv_augmented_mass: LU<N, Dynamic, Dynamic>,
status: BodyStatus,
gravity_enabled: bool,
update_status: BodyUpdateStatus,
activation: ActivationStatus<N>,
ndofs: usize,
companion_id: usize,
user_data: Option<Box<dyn Any + Send + Sync>>,
workspace: MultibodyWorkspace<N>,
coriolis_v: Vec<MatrixMN<N, Dim, Dynamic>>,
coriolis_w: Vec<MatrixMN<N, AngularDim, Dynamic>>,
i_coriolis_dt: Jacobian<N>,
solver_workspace: Option<SolverWorkspace<N, (), ()>>,
}
impl<N: RealField> Multibody<N> {
fn new() -> Self {
Multibody {
rbs: MultibodyLinkVec(Vec::new()),
velocities: DVector::zeros(0),
forces: DVector::zeros(0),
damping: DVector::zeros(0),
accelerations: DVector::zeros(0),
impulses: DVector::zeros(0),
body_jacobians: Vec::new(),
augmented_mass: DMatrix::zeros(0, 0),
inv_augmented_mass: LU::new(DMatrix::zeros(0, 0)),
status: BodyStatus::Dynamic,
update_status: BodyUpdateStatus::all(),
gravity_enabled: true,
activation: ActivationStatus::new_active(),
ndofs: 0,
companion_id: 0,
workspace: MultibodyWorkspace::new(),
coriolis_v: Vec::new(),
coriolis_w: Vec::new(),
i_coriolis_dt: Jacobian::zeros(0),
solver_workspace: Some(SolverWorkspace::new()),
user_data: None,
}
}
user_data_accessors!();
#[inline]
pub fn root(&self) -> &MultibodyLink<N> {
&self.rbs[0]
}
#[inline]
pub fn root_mut(&mut self) -> &mut MultibodyLink<N> {
&mut self.rbs[0]
}
#[inline]
pub fn link(&self, id: usize) -> Option<&MultibodyLink<N>> {
self.rbs.get(id)
}
#[inline]
pub fn link_mut(&mut self, id: usize) -> Option<&mut MultibodyLink<N>> {
self.rbs.get_mut(id)
}
pub fn links_with_name<'a>(
&'a self,
name: &'a str,
) -> impl Iterator<Item = (usize, &'a MultibodyLink<N>)> {
self.rbs
.iter()
.enumerate()
.filter(move |(_i, l)| l.name == name)
}
pub fn num_links(&self) -> usize {
self.rbs.len()
}
pub fn links(&self) -> impl Iterator<Item = &MultibodyLink<N>> {
self.rbs.iter()
}
pub fn links_mut(&mut self) -> impl Iterator<Item = &mut MultibodyLink<N>> {
self.rbs.iter_mut()
}
#[inline]
pub fn damping(&self) -> &DVector<N> {
&self.damping
}
#[inline]
pub fn damping_mut(&mut self) -> &mut DVector<N> {
&mut self.damping
}
#[inline]
pub fn set_link_mass(&mut self, link_id: usize, mass: N) {
self.update_status.set_local_inertia_changed(true);
self.rbs[link_id].local_inertia.linear = mass;
}
#[inline]
#[cfg(feature = "dim3")]
pub fn set_link_angular_inertia(&mut self, link_id: usize, angular_inertia: na::Matrix3<N>) {
self.update_status.set_local_inertia_changed(true);
self.rbs[link_id].local_inertia.angular = angular_inertia;
}
#[inline]
#[cfg(feature = "dim2")]
pub fn set_link_angular_inertia(&mut self, link_id: usize, angular_inertia: N) {
self.update_status.set_local_inertia_changed(true);
self.rbs[link_id].local_inertia.angular = angular_inertia;
}
fn add_link(
&mut self,
parent: Option<usize>,
mut dof: Box<dyn Joint<N>>,
parent_shift: Vector<N>,
body_shift: Vector<N>,
local_inertia: Inertia<N>,
local_com: Point<N>,
) -> &mut MultibodyLink<N> {
assert!(
parent.is_none() || !self.rbs.is_empty(),
"Multibody::build_body: invalid parent id."
);
let assembly_id = self.velocities.len();
let impulse_id = self.impulses.len();
let internal_id = self.rbs.len();
let ndofs = dof.ndofs();
let nimpulses = dof.nimpulses();
self.grow_buffers(ndofs, nimpulses);
self.ndofs += ndofs;
dof.default_damping(&mut self.damping.rows_mut(assembly_id, ndofs));
dof.update_jacobians(&body_shift, &self.velocities.as_slice()[assembly_id..]);
let local_to_parent = dof.body_to_parent(&parent_shift, &body_shift);
let local_to_world;
let parent_to_world;
let parent_internal_id;
if let Some(parent) = parent {
parent_internal_id = parent;
let parent_rb = &mut self.rbs[parent_internal_id];
parent_rb.is_leaf = false;
parent_to_world = parent_rb.local_to_world;
local_to_world = parent_rb.local_to_world * local_to_parent;
} else {
parent_internal_id = 0;
parent_to_world = Isometry::identity();
local_to_world = local_to_parent;
}
let rb = MultibodyLink::new(
internal_id,
assembly_id,
impulse_id,
parent_internal_id,
dof,
parent_shift,
body_shift,
parent_to_world,
local_to_world,
local_to_parent,
local_inertia,
local_com,
);
self.rbs.push(rb);
self.workspace.resize(self.rbs.len(), self.ndofs);
&mut self.rbs[internal_id]
}
fn grow_buffers(&mut self, ndofs: usize, nimpulses: usize) {
let len = self.velocities.len();
self.velocities
.resize_vertically_mut(len + ndofs, N::zero());
self.forces.resize_vertically_mut(len + ndofs, N::zero());
self.damping.resize_vertically_mut(len + ndofs, N::zero());
self.accelerations
.resize_vertically_mut(len + ndofs, N::zero());
self.body_jacobians.push(Jacobian::zeros(0));
let len = self.impulses.len();
self.impulses
.resize_vertically_mut(len + nimpulses, N::zero());
}
fn update_acceleration(&mut self, gravity: &Vector<N>) {
if self.status != BodyStatus::Dynamic {
return;
}
self.accelerations.fill(N::zero());
for i in 0..self.rbs.len() {
let external_forces;
{
let rb = &self.rbs[i];
let mut acc = rb.velocity_dot_wrt_joint;
if i != 0 {
let parent_id = rb.parent_internal_id;
let parent_rb = &self.rbs[parent_id];
let parent_vel = &parent_rb.velocity;
acc += self.workspace.accs[parent_id];
acc.linear += parent_vel
.angular_vector()
.gcross(&rb.velocity_wrt_joint.linear);
#[cfg(feature = "dim3")]
{
acc.angular += parent_vel.angular.cross(&rb.velocity_wrt_joint.angular);
}
let shift = rb.center_of_mass() - parent_rb.center_of_mass();
let dvel = rb.velocity.linear - parent_rb.velocity.linear;
acc.linear += parent_rb.velocity.angular_vector().gcross(&dvel);
acc.linear += self.workspace.accs[parent_id]
.angular_vector()
.gcross(&shift);
}
self.workspace.accs[i] = acc;
let gravity_force = if self.gravity_enabled {
gravity * rb.inertia.mass()
} else {
Vector::zeros()
};
let gyroscopic;
#[cfg(feature = "dim3")]
{
gyroscopic = rb
.velocity
.angular
.cross(&(rb.inertia.angular * rb.velocity.angular));
}
#[cfg(feature = "dim2")]
{
gyroscopic = N::zero();
}
external_forces = Force::new(gravity_force, -gyroscopic) - rb.inertia * acc;
self.accelerations.gemv_tr(
N::one(),
&self.body_jacobians[i],
external_forces.as_vector(),
N::one(),
);
}
}
self.accelerations.axpy(N::one(), &self.forces, N::one());
self.accelerations
.cmpy(-N::one(), &self.damping, &self.velocities, N::one());
assert!(self.inv_augmented_mass.solve_mut(&mut self.accelerations));
}
fn update_dynamics(&mut self, dt: N) {
if !self.update_status.inertia_needs_update() {
return;
}
let rb = &mut self.rbs[0];
let velocity_wrt_joint = rb
.dof
.jacobian_mul_coordinates(&self.velocities.as_slice()[rb.assembly_id..]);
let velocity_dot_wrt_joint = rb
.dof
.jacobian_dot_mul_coordinates(&self.velocities.as_slice()[rb.assembly_id..]);
rb.velocity_dot_wrt_joint = velocity_dot_wrt_joint;
rb.velocity_wrt_joint = velocity_wrt_joint;
rb.velocity = rb.velocity_wrt_joint;
for i in 1..self.rbs.len() {
let (rb, parent_rb) = self.rbs.get_mut_with_parent(i);
let velocity_wrt_joint = rb
.dof
.jacobian_mul_coordinates(&self.velocities.as_slice()[rb.assembly_id..]);
let velocity_dot_wrt_joint = rb
.dof
.jacobian_dot_mul_coordinates(&self.velocities.as_slice()[rb.assembly_id..]);
rb.velocity_dot_wrt_joint =
velocity_dot_wrt_joint.transformed(&parent_rb.local_to_world);
rb.velocity_wrt_joint = velocity_wrt_joint.transformed(&parent_rb.local_to_world);
rb.velocity = parent_rb.velocity + rb.velocity_wrt_joint;
let shift = rb.center_of_mass() - parent_rb.center_of_mass();
rb.velocity.linear += parent_rb.velocity.angular_vector().gcross(&shift);
}
if self.status != BodyStatus::Dynamic {
return;
}
if !self.is_active() {
self.activate();
}
self.update_inertias(dt);
}
fn update_body_jacobians(&mut self) {
for i in 0..self.rbs.len() {
let rb = &self.rbs[i];
if self.body_jacobians[i].ncols() != self.ndofs {
self.body_jacobians[i] = Jacobian::zeros(self.ndofs);
}
if i != 0 {
let parent_id = rb.parent_internal_id;
let parent_rb = &self.rbs[parent_id];
let (rb_j, parent_j) = self.body_jacobians.index_mut_const(i, parent_id);
rb_j.copy_from(&parent_j);
{
let mut rb_j_v = rb_j.fixed_rows_mut::<Dim>(0);
let parent_j_w = parent_j.fixed_rows::<AngularDim>(DIM);
let shift_tr =
(rb.center_of_mass() - parent_rb.center_of_mass()).gcross_matrix_tr();
rb_j_v.gemm(N::one(), &shift_tr, &parent_j_w, N::one());
}
} else {
self.body_jacobians[i].fill(N::zero());
}
let ndofs = rb.dof.ndofs();
let mut tmp = SpatialMatrix::zeros();
let mut rb_joint_j = tmp.columns_mut(0, ndofs);
let mut rb_j_part = self.body_jacobians[i].columns_mut(rb.assembly_id, ndofs);
rb.dof.jacobian(&rb.parent_to_world, &mut rb_joint_j);
rb_j_part += rb_joint_j;
}
}
fn update_inertias(&mut self, dt: N) {
if self.augmented_mass.ncols() != self.ndofs {
self.augmented_mass = DMatrix::zeros(self.ndofs, self.ndofs);
} else {
self.augmented_mass.fill(N::zero());
}
for i in 0..self.rbs.len() {
let mut rb = &mut self.rbs[i];
rb.inertia = rb.local_inertia.transformed(&rb.local_to_world);
}
if self.coriolis_v.len() != self.rbs.len() {
self.coriolis_v.resize(
self.rbs.len(),
MatrixMN::<N, Dim, Dynamic>::zeros(self.ndofs),
);
self.coriolis_w.resize(
self.rbs.len(),
MatrixMN::<N, AngularDim, Dynamic>::zeros(self.ndofs),
);
self.i_coriolis_dt = Jacobian::zeros(self.ndofs);
}
for i in 0..self.rbs.len() {
let rb = &self.rbs[i];
let body_jacobian = &self.body_jacobians[i];
#[allow(unused_mut)]
let mut augmented_inertia = rb.inertia;
#[cfg(feature = "dim3")]
{
let ang_inertia = rb.inertia.angular;
let gyroscopic_matrix = rb.velocity.angular.gcross_matrix() * ang_inertia
- (ang_inertia * rb.velocity.angular).gcross_matrix();
augmented_inertia.angular += gyroscopic_matrix * dt;
}
self.augmented_mass.quadform(
N::one(),
&augmented_inertia.to_matrix(),
body_jacobian,
N::one(),
);
let rb_j = &self.body_jacobians[i];
let rb_j_v = rb_j.fixed_rows::<Dim>(0);
let ndofs = rb.dof.ndofs();
if i != 0 {
let parent_id = rb.parent_internal_id;
let parent_rb = &self.rbs[parent_id];
let parent_j = &self.body_jacobians[parent_id];
let parent_j_v = parent_j.fixed_rows::<Dim>(0);
let parent_j_w = parent_j.fixed_rows::<AngularDim>(DIM);
let parent_w = parent_rb.velocity.angular_vector().gcross_matrix();
let (coriolis_v, parent_coriolis_v) = self.coriolis_v.index_mut_const(i, parent_id);
let (coriolis_w, parent_coriolis_w) = self.coriolis_w.index_mut_const(i, parent_id);
coriolis_v.copy_from(&parent_coriolis_v);
coriolis_w.copy_from(&parent_coriolis_w);
let shift_tr =
(rb.center_of_mass() - parent_rb.center_of_mass()).gcross_matrix_tr();
coriolis_v.gemm(N::one(), &shift_tr, &parent_coriolis_w, N::one());
let dvel_tr = (rb.velocity.linear - parent_rb.velocity.linear).gcross_matrix_tr();
coriolis_v.gemm(N::one(), &dvel_tr, &parent_j_w, N::one());
coriolis_v.gemm(
N::one(),
&rb.velocity_wrt_joint.linear.gcross_matrix_tr(),
&parent_j_w,
N::one(),
);
coriolis_v.gemm(N::one(), &parent_w, &rb_j_v, N::one());
coriolis_v.gemm(-N::one(), &parent_w, &parent_j_v, N::one());
#[cfg(feature = "dim3")]
{
let vel_wrt_joint_w = rb.velocity_wrt_joint.angular_vector().gcross_matrix();
coriolis_w.gemm(-N::one(), &vel_wrt_joint_w, &parent_j_w, N::one());
}
{
let mut coriolis_v_part = coriolis_v.columns_mut(rb.assembly_id, ndofs);
let mut tmp1 = SpatialMatrix::zeros();
let mut rb_joint_j = tmp1.columns_mut(0, ndofs);
rb.dof.jacobian(&parent_rb.local_to_world, &mut rb_joint_j);
let rb_joint_j_v = rb_joint_j.fixed_rows::<Dim>(0);
coriolis_v_part.gemm(N::one(), &parent_w, &rb_joint_j_v, N::one());
#[cfg(feature = "dim3")]
{
let rb_joint_j_w = rb_joint_j.fixed_rows::<AngularDim>(DIM);
let mut coriolis_w_part = coriolis_w.columns_mut(rb.assembly_id, ndofs);
coriolis_w_part.gemm(N::one(), &parent_w, &rb_joint_j_w, N::one());
}
}
} else {
self.coriolis_v[i].fill(N::zero());
self.coriolis_w[i].fill(N::zero());
}
let coriolis_v = &mut self.coriolis_v[i];
let coriolis_w = &mut self.coriolis_w[i];
{
let mut tmp1 = SpatialMatrix::zeros();
let mut tmp2 = SpatialMatrix::zeros();
let mut rb_joint_j_dot = tmp1.columns_mut(0, ndofs);
let mut rb_joint_j_dot_veldiff = tmp2.columns_mut(0, ndofs);
rb.dof
.jacobian_dot(&rb.parent_to_world, &mut rb_joint_j_dot);
rb.dof.jacobian_dot_veldiff_mul_coordinates(
&rb.parent_to_world,
&self.velocities.as_slice()[rb.assembly_id..],
&mut rb_joint_j_dot_veldiff,
);
let rb_joint_j_v_dot = rb_joint_j_dot.fixed_rows::<Dim>(0);
let rb_joint_j_w_dot = rb_joint_j_dot.fixed_rows::<AngularDim>(DIM);
let rb_joint_j_v_dot_veldiff = rb_joint_j_dot_veldiff.fixed_rows::<Dim>(0);
let rb_joint_j_w_dot_veldiff = rb_joint_j_dot_veldiff.fixed_rows::<AngularDim>(DIM);
let mut coriolis_v_part = coriolis_v.columns_mut(rb.assembly_id, ndofs);
let mut coriolis_w_part = coriolis_w.columns_mut(rb.assembly_id, ndofs);
coriolis_v_part += rb_joint_j_v_dot;
coriolis_w_part += rb_joint_j_w_dot;
coriolis_v_part += rb_joint_j_v_dot_veldiff;
coriolis_w_part += rb_joint_j_w_dot_veldiff;
}
{
let mut i_coriolis_dt_v = self.i_coriolis_dt.fixed_rows_mut::<Dim>(0);
i_coriolis_dt_v.copy_from(coriolis_v);
i_coriolis_dt_v *= rb.inertia.linear * dt;
}
{
let mut i_coriolis_dt_w = self.i_coriolis_dt.fixed_rows_mut::<AngularDim>(DIM);
i_coriolis_dt_w.gemm(dt, rb.inertia.angular_matrix(), &coriolis_w, N::zero());
}
self.augmented_mass
.gemm_tr(N::one(), &rb_j, &self.i_coriolis_dt, N::one());
}
for i in 0..self.ndofs {
self.augmented_mass[(i, i)] += self.damping[i] * dt;
}
self.inv_augmented_mass = LU::new(self.augmented_mass.clone());
}
#[inline]
pub fn joint_velocity(&self, link: &MultibodyLink<N>) -> DVectorSlice<N> {
let ndofs = link.dof.ndofs();
DVectorSlice::from_slice(
&self.velocities.as_slice()[link.assembly_id..link.assembly_id + ndofs],
ndofs,
)
}
pub fn link_jacobian_mul_force(
&self,
link: &MultibodyLink<N>,
force: &Force<N>,
out: &mut [N],
) {
let mut out = DVectorSliceMut::from_slice(out, self.ndofs);
self.body_jacobians[link.internal_id].tr_mul_to(force.as_vector(), &mut out);
}
pub fn inv_mass_mul_link_force(
&self,
link: &MultibodyLink<N>,
force: &Force<N>,
out: &mut [N],
) {
let mut out = DVectorSliceMut::from_slice(out, self.ndofs);
self.body_jacobians[link.internal_id].tr_mul_to(force.as_vector(), &mut out);
assert!(self.inv_augmented_mass.solve_mut(&mut out));
}
pub fn inv_mass_mul_unit_joint_force(
&self,
link: &MultibodyLink<N>,
dof_id: usize,
force: N,
out: &mut [N],
) {
let mut out = DVectorSliceMut::from_slice(out, self.ndofs);
out.fill(N::zero());
out[link.assembly_id + dof_id] = force;
assert!(self.inv_augmented_mass.solve_mut(&mut out));
}
pub fn inv_mass_mul_joint_force(
&self,
link: &MultibodyLink<N>,
force: DVectorSlice<N>,
out: &mut [N],
) {
let ndofs = link.dof.ndofs();
let mut out = DVectorSliceMut::from_slice(out, self.ndofs);
out.fill(N::zero());
out.rows_mut(link.assembly_id, ndofs).copy_from(&force);
assert!(self.inv_augmented_mass.solve_mut(&mut out));
}
pub fn augmented_mass(&self) -> &DMatrix<N> {
&self.augmented_mass
}
#[inline]
pub fn joint_velocity_mut(&mut self, id: usize) -> DVectorSliceMut<N> {
self.update_status.set_velocity_changed(true);
let ndofs;
let i;
{
let link = self.link(id).expect("Invalid multibody link handle.");
ndofs = link.dof.ndofs();
i = link.assembly_id;
}
DVectorSliceMut::from_slice(&mut self.velocities.as_mut_slice()[i..i + ndofs], ndofs)
}
#[inline]
pub fn generalized_force(&self) -> &DVector<N> {
&self.forces
}
#[inline]
pub fn generalized_force_mut(&mut self) -> &mut DVector<N> {
&mut self.forces
}
#[inline]
pub(crate) fn impulses(&self) -> &[N] {
self.impulses.as_slice()
}
}
struct MultibodyWorkspace<N: RealField> {
accs: Vec<Velocity<N>>,
ndofs_vec: DVector<N>,
}
impl<N: RealField> MultibodyWorkspace<N> {
pub fn new() -> Self {
MultibodyWorkspace {
accs: Vec::new(),
ndofs_vec: DVector::zeros(0),
}
}
pub fn resize(&mut self, nlinks: usize, ndofs: usize) {
self.accs.resize(nlinks, Velocity::zero());
self.ndofs_vec = DVector::zeros(ndofs)
}
}
struct SolverWorkspace<N: RealField, Handle: BodyHandle, CollHandle: ColliderHandle> {
jacobians: DVector<N>,
constraints: ConstraintSet<N, Handle, CollHandle, usize>,
}
impl<N: RealField, Handle: BodyHandle, CollHandle: ColliderHandle>
SolverWorkspace<N, Handle, CollHandle>
{
pub fn new() -> Self {
SolverWorkspace {
jacobians: DVector::zeros(0),
constraints: ConstraintSet::new(),
}
}
pub fn resize(&mut self, nconstraints: usize, ndofs: usize) {
let j_len = nconstraints * ndofs * 2;
if self.jacobians.len() != j_len {
self.jacobians = DVector::zeros(j_len);
}
}
}
impl<N: RealField> Body<N> for Multibody<N> {
#[inline]
fn part(&self, id: usize) -> Option<&dyn BodyPart<N>> {
self.link(id).map(|l| l as &dyn BodyPart<N>)
}
#[inline]
fn deformed_positions(&self) -> Option<(DeformationsType, &[N])> {
None
}
#[inline]
fn deformed_positions_mut(&mut self) -> Option<(DeformationsType, &mut [N])> {
None
}
fn clear_update_flags(&mut self) {
self.update_status.clear();
}
fn update_status(&self) -> BodyUpdateStatus {
self.update_status
}
#[inline]
fn integrate(&mut self, parameters: &IntegrationParameters<N>) {
self.update_status.set_position_changed(true);
for rb in self.rbs.iter_mut() {
rb.dof
.integrate(parameters, &self.velocities.as_slice()[rb.assembly_id..])
}
}
fn apply_displacement(&mut self, disp: &[N]) {
self.update_status.set_position_changed(true);
for rb in self.rbs.iter_mut() {
rb.dof.apply_displacement(&disp[rb.assembly_id..])
}
self.update_kinematics();
}
fn clear_forces(&mut self) {
self.forces.fill(N::zero())
}
fn update_kinematics(&mut self) {
if !self.update_status.position_changed() {
return;
}
{
let rb = &mut self.rbs[0];
rb.dof
.update_jacobians(&rb.body_shift, &self.velocities.as_slice());
rb.local_to_parent = rb.dof.body_to_parent(&rb.parent_shift, &rb.body_shift);
rb.local_to_world = rb.local_to_parent;
rb.com = rb.local_to_world * rb.local_com;
}
for i in 1..self.rbs.len() {
let (rb, parent_rb) = self.rbs.get_mut_with_parent(i);
rb.dof.update_jacobians(
&rb.body_shift,
&self.velocities.as_slice()[rb.assembly_id..],
);
rb.local_to_parent = rb.dof.body_to_parent(&rb.parent_shift, &rb.body_shift);
rb.local_to_world = parent_rb.local_to_world * rb.local_to_parent;
rb.parent_to_world = parent_rb.local_to_world;
rb.com = rb.local_to_world * rb.local_com;
}
self.update_body_jacobians();
}
fn update_dynamics(&mut self, dt: N) {
self.update_dynamics(dt)
}
fn update_acceleration(&mut self, gravity: &Vector<N>, _: &IntegrationParameters<N>) {
self.update_acceleration(gravity)
}
#[inline]
fn generalized_acceleration(&self) -> DVectorSlice<N> {
self.accelerations.rows(0, self.ndofs)
}
#[inline]
fn generalized_velocity(&self) -> DVectorSlice<N> {
self.velocities.rows(0, self.ndofs)
}
#[inline]
fn generalized_velocity_mut(&mut self) -> DVectorSliceMut<N> {
self.update_status.set_velocity_changed(true);
self.velocities.rows_mut(0, self.ndofs)
}
#[inline]
fn gravity_enabled(&self) -> bool {
self.gravity_enabled
}
#[inline]
fn enable_gravity(&mut self, enabled: bool) {
self.gravity_enabled = enabled
}
#[inline]
fn activation_status(&self) -> &ActivationStatus<N> {
&self.activation
}
#[inline]
fn activate_with_energy(&mut self, energy: N) {
self.activation.set_energy(energy)
}
#[inline]
fn deactivate(&mut self) {
self.update_status.clear();
self.activation.set_energy(N::zero());
self.velocities.fill(N::zero());
}
#[inline]
fn set_deactivation_threshold(&mut self, threshold: Option<N>) {
self.activation.set_deactivation_threshold(threshold)
}
#[inline]
fn set_status(&mut self, status: BodyStatus) {
self.update_status.set_status_changed(true);
self.status = status
}
#[inline]
fn status(&self) -> BodyStatus {
self.status
}
#[inline]
fn companion_id(&self) -> usize {
self.companion_id
}
#[inline]
fn set_companion_id(&mut self, id: usize) {
self.companion_id = id
}
#[inline]
fn ndofs(&self) -> usize {
self.ndofs
}
#[inline]
fn world_point_at_material_point(&self, part: &dyn BodyPart<N>, point: &Point<N>) -> Point<N> {
let link = part
.downcast_ref::<MultibodyLink<N>>()
.expect("The provided body part must be a multibody link");
link.local_to_world * point
}
#[inline]
fn position_at_material_point(&self, part: &dyn BodyPart<N>, point: &Point<N>) -> Isometry<N> {
let link = part
.downcast_ref::<MultibodyLink<N>>()
.expect("The provided body part must be a multibody link");
link.local_to_world * Translation::from(point.coords)
}
#[inline]
fn material_point_at_world_point(&self, part: &dyn BodyPart<N>, point: &Point<N>) -> Point<N> {
let link = part
.downcast_ref::<MultibodyLink<N>>()
.expect("The provided body part must be a multibody link");
link.local_to_world.inverse_transform_point(point)
}
fn fill_constraint_geometry(
&self,
part: &dyn BodyPart<N>,
ndofs: usize,
point: &Point<N>,
force_dir: &ForceDirection<N>,
j_id: usize,
wj_id: usize,
jacobians: &mut [N],
inv_r: &mut N,
ext_vels: Option<&DVectorSlice<N>>,
out_vel: Option<&mut N>,
) {
let link = part
.downcast_ref::<MultibodyLink<N>>()
.expect("The provided body part must be a multibody link");
let pos = point - link.com.coords;
let force = force_dir.at_point(&pos);
match self.status() {
BodyStatus::Dynamic => {
self.link_jacobian_mul_force(link, &force, &mut jacobians[j_id..]);
for i in 0..ndofs {
jacobians[wj_id + i] = jacobians[j_id + i];
}
{
let mut out = DVectorSliceMut::from_slice(&mut jacobians[wj_id..], self.ndofs);
assert!(self.inv_augmented_mass.solve_mut(&mut out))
}
let j = DVectorSlice::from_slice(&jacobians[j_id..], ndofs);
let invm_j = DVectorSlice::from_slice(&jacobians[wj_id..], ndofs);
*inv_r += j.dot(&invm_j);
if let Some(out_vel) = out_vel {
*out_vel += j.dot(&self.generalized_velocity());
if let Some(ext_vels) = ext_vels {
*out_vel += j.dot(ext_vels)
}
}
}
BodyStatus::Kinematic => {
if let Some(out_vel) = out_vel {
*out_vel += force.as_vector().dot(&link.velocity.as_vector())
}
}
BodyStatus::Static | BodyStatus::Disabled => {}
}
}
#[inline]
fn has_active_internal_constraints(&mut self) -> bool {
self.links()
.any(|link| link.joint().num_velocity_constraints() != 0)
}
#[inline]
fn setup_internal_velocity_constraints(
&mut self,
ext_vels: &DVectorSlice<N>,
parameters: &IntegrationParameters<N>,
) {
let mut ground_j_id = 0;
let mut workspace = self.solver_workspace.take().unwrap();
for c in &workspace.constraints.velocity.unilateral_ground {
self.impulses[c.impulse_id] = c.impulse;
}
for c in &workspace.constraints.velocity.bilateral_ground {
self.impulses[c.impulse_id] = c.impulse;
}
workspace.constraints.clear();
let nconstraints = self
.rbs
.iter()
.map(|l| l.joint().num_velocity_constraints())
.sum();
workspace.resize(nconstraints, self.ndofs);
for link in self.rbs.iter() {
link.joint().velocity_constraints(
parameters,
self,
&link,
0,
0,
ext_vels.as_slice(),
&mut ground_j_id,
workspace.jacobians.as_mut_slice(),
&mut workspace.constraints,
);
}
self.solver_workspace = Some(workspace);
}
#[inline]
fn warmstart_internal_velocity_constraints(&mut self, dvels: &mut DVectorSliceMut<N>) {
let workspace = self.solver_workspace.as_mut().unwrap();
for c in &mut workspace.constraints.velocity.unilateral_ground {
let dim = Dynamic::new(c.ndofs);
SORProx::warmstart_unilateral_ground(c, workspace.jacobians.as_slice(), dvels, dim)
}
for c in &mut workspace.constraints.velocity.bilateral_ground {
let dim = Dynamic::new(c.ndofs);
SORProx::warmstart_bilateral_ground(c, workspace.jacobians.as_slice(), dvels, dim)
}
}
#[inline]
fn step_solve_internal_velocity_constraints(&mut self, dvels: &mut DVectorSliceMut<N>) {
let workspace = self.solver_workspace.as_mut().unwrap();
for c in &mut workspace.constraints.velocity.unilateral_ground {
let dim = Dynamic::new(c.ndofs);
SORProx::solve_unilateral_ground(c, workspace.jacobians.as_slice(), dvels, dim)
}
for c in &mut workspace.constraints.velocity.bilateral_ground {
let dim = Dynamic::new(c.ndofs);
SORProx::solve_bilateral_ground(c, &[], workspace.jacobians.as_slice(), dvels, dim)
}
}
#[inline]
fn step_solve_internal_position_constraints(&mut self, parameters: &IntegrationParameters<N>) {
let mut workspace = self.solver_workspace.take().unwrap();
let jacobians = &mut workspace.jacobians;
for i in 0..self.rbs.len() {
for j in 0..self.rbs[i].joint().num_position_constraints() {
let link = &self.rbs[i];
let c = link.joint().position_constraint(
j,
self,
link,
BodyPartHandle((), i),
0,
jacobians.as_mut_slice(),
);
if let Some(c) = c {
let rhs = NonlinearSORProx::clamp_rhs(c.rhs, c.is_angular, parameters);
if rhs < N::zero() {
let impulse = -rhs * c.r;
jacobians.rows_mut(c.wj_id1, c.dim1).mul_assign(impulse);
self.apply_displacement(jacobians.rows(c.wj_id1, c.dim1).as_slice());
}
}
}
}
self.solver_workspace = Some(workspace);
self.update_kinematics();
}
#[inline]
fn add_local_inertia_and_com(&mut self, part_id: usize, com: Point<N>, inertia: Inertia<N>) {
self.update_status.set_local_inertia_changed(true);
let mut link = &mut self.rbs[part_id];
let mass_sum = link.inertia.linear + inertia.linear;
if !mass_sum.is_zero() {
link.local_com =
(link.local_com * link.inertia.linear + com.coords * inertia.linear) / mass_sum;
link.com = link.local_to_world * link.local_com;
} else {
link.local_com = Point::origin();
link.com = link.local_to_world.translation.vector.into();
}
link.local_inertia += inertia;
}
#[inline]
fn velocity_at_point(&self, part_id: usize, point: &Point<N>) -> Velocity<N> {
let link = &self.rbs[part_id];
let pos = point - link.com;
link.velocity.shift(&pos)
}
fn apply_force(
&mut self,
part_id: usize,
force: &Force<N>,
force_type: ForceType,
auto_wake_up: bool,
) {
if self.status != BodyStatus::Dynamic {
return;
}
if auto_wake_up {
self.activate()
}
match force_type {
ForceType::Force => self.forces.gemv_tr(
N::one(),
&self.body_jacobians[part_id],
force.as_vector(),
N::one(),
),
ForceType::Impulse => {
self.update_status.set_velocity_changed(true);
let dvel = &mut self.workspace.ndofs_vec;
dvel.gemv_tr(
N::one(),
&self.body_jacobians[part_id],
force.as_vector(),
N::zero(),
);
let _ = self.inv_augmented_mass.solve_mut(dvel);
self.velocities.axpy(N::one(), dvel, N::one());
}
ForceType::AccelerationChange => {
let force = self.rbs[part_id].inertia * *force;
self.forces.gemv_tr(
N::one(),
&self.body_jacobians[part_id],
force.as_vector(),
N::one(),
);
}
ForceType::VelocityChange => {
self.update_status.set_velocity_changed(true);
self.velocities.gemv_tr(
N::one(),
&self.body_jacobians[part_id],
force.as_vector(),
N::one(),
)
}
}
}
fn apply_local_force(
&mut self,
part_id: usize,
force: &Force<N>,
force_type: ForceType,
auto_wake_up: bool,
) {
let world_force = force.transform_by(&self.rbs[part_id].local_to_world);
self.apply_force(part_id, &world_force, force_type, auto_wake_up);
}
fn apply_force_at_point(
&mut self,
part_id: usize,
force: &Vector<N>,
point: &Point<N>,
force_type: ForceType,
auto_wake_up: bool,
) {
let force = Force::linear_at_point(*force, &(point - self.rbs[part_id].com.coords));
self.apply_force(part_id, &force, force_type, auto_wake_up)
}
fn apply_local_force_at_point(
&mut self,
part_id: usize,
force: &Vector<N>,
point: &Point<N>,
force_type: ForceType,
auto_wake_up: bool,
) {
self.apply_force_at_point(
part_id,
&(self.rbs[part_id].local_to_world * force),
point,
force_type,
auto_wake_up,
)
}
fn apply_force_at_local_point(
&mut self,
part_id: usize,
force: &Vector<N>,
point: &Point<N>,
force_type: ForceType,
auto_wake_up: bool,
) {
self.apply_force_at_point(
part_id,
force,
&(self.rbs[part_id].local_to_world * point),
force_type,
auto_wake_up,
)
}
fn apply_local_force_at_local_point(
&mut self,
part_id: usize,
force: &Vector<N>,
point: &Point<N>,
force_type: ForceType,
auto_wake_up: bool,
) {
self.apply_force_at_point(
part_id,
&(self.rbs[part_id].local_to_world * force),
&(self.rbs[part_id].local_to_world * point),
force_type,
auto_wake_up,
)
}
}
pub struct MultibodyDesc<N: RealField> {
name: String,
children: Vec<MultibodyDesc<N>>,
joint: Box<dyn Joint<N>>,
velocity: Velocity<N>,
local_inertia: Inertia<N>,
local_center_of_mass: Point<N>,
body_shift: Vector<N>,
parent_shift: Vector<N>,
}
impl<N: RealField> MultibodyDesc<N> {
pub fn new<J: Joint<N>>(joint: J) -> Self {
MultibodyDesc {
name: String::new(),
joint: Box::new(joint),
children: Vec::new(),
velocity: Velocity::zero(),
local_inertia: Inertia::zero(),
local_center_of_mass: Point::origin(),
body_shift: Vector::zeros(),
parent_shift: Vector::zeros(),
}
}
pub fn add_child<J: Joint<N>>(&mut self, joint: J) -> &mut MultibodyDesc<N> {
let child = MultibodyDesc::new(joint);
self.children.push(child);
self.children.last_mut().unwrap()
}
pub fn set_joint<J: Joint<N>>(&mut self, joint: J) -> &mut Self {
self.joint = Box::new(joint);
self
}
#[cfg(feature = "dim2")]
desc_custom_setters!(
self.angular_inertia,
set_angular_inertia,
angular_inertia: N | { self.local_inertia.angular = angular_inertia }
);
#[cfg(feature = "dim3")]
desc_custom_setters!(
self.angular_inertia,
set_angular_inertia,
angular_inertia: na::Matrix3<N> | { self.local_inertia.angular = angular_inertia }
);
desc_custom_setters!(
self.mass,
set_mass,
mass: N | { self.local_inertia.linear = mass }
);
desc_setters!(
name, set_name, name: String
parent_shift, set_parent_shift, parent_shift: Vector<N>
body_shift, set_body_shift, body_shift: Vector<N>
velocity, set_velocity, velocity: Velocity<N>
local_inertia, set_local_inertia, local_inertia: Inertia<N>
local_center_of_mass, set_local_center_of_mass, local_center_of_mass: Point<N>
);
#[cfg(feature = "dim2")]
desc_custom_getters!(self.get_angular_inertia: N | { self.local_inertia.angular });
#[cfg(feature = "dim3")]
desc_custom_getters!(
self.get_angular_inertia: &na::Matrix3<N> | { &self.local_inertia.angular }
);
desc_custom_getters!(
self.get_mass: N | { self.local_inertia.linear }
self.get_name: &str | { &self.name }
);
desc_getters!(
[ref] get_parent_shift -> parent_shift: Vector<N>
[ref] get_body_shift -> body_shift: Vector<N>
[ref] get_velocity -> velocity: Velocity<N>
[ref] get_local_inertia -> local_inertia: Inertia<N>
[ref] get_local_center_of_mass -> local_center_of_mass: Point<N>
);
pub fn build(&self) -> Multibody<N> {
let mut multibody = Multibody::new();
let _ = self.do_build_with_parent(&mut multibody, None);
multibody
}
pub fn build_with_parent<'m>(
&self,
multibody: &'m mut Multibody<N>,
parent_id: usize,
) -> &'m mut MultibodyLink<N> {
self.do_build_with_parent(multibody, Some(parent_id))
}
fn do_build_with_parent<'m>(
&self,
multibody: &'m mut Multibody<N>,
parent_id: Option<usize>,
) -> &'m mut MultibodyLink<N> {
let mut link = multibody.add_link(
parent_id,
self.joint.clone(),
self.parent_shift,
self.body_shift,
self.local_inertia,
self.local_center_of_mass,
);
link.velocity = self.velocity;
link.name = self.name.clone();
let me = link.link_id();
for child in &self.children {
let _ = child.do_build_with_parent(multibody, Some(me));
}
multibody.link_mut(me).unwrap()
}
}