use super::multibody_link::{MultibodyLink, MultibodyLinkVec};
use super::multibody_workspace::MultibodyWorkspace;
use crate::dynamics::{RigidBodyHandle, RigidBodySet, RigidBodyType, RigidBodyVelocity};
#[cfg(feature = "dim3")]
use crate::math::Matrix;
use crate::math::{
AngDim, AngVector, Dim, Isometry, Jacobian, Point, Real, Vector, ANG_DIM, DIM, SPATIAL_DIM,
};
use crate::prelude::MultibodyJoint;
use crate::utils::{IndexMut2, SimdAngularInertia, SimdCross, SimdCrossMatrix};
use na::{
self, DMatrix, DVector, DVectorView, DVectorViewMut, Dyn, OMatrix, SMatrix, SVector,
StorageMut, LU,
};
#[repr(C)]
#[derive(Copy, Clone, Debug, Default)]
struct Force {
linear: Vector<Real>,
angular: AngVector<Real>,
}
impl Force {
fn new(linear: Vector<Real>, angular: AngVector<Real>) -> Self {
Self { linear, angular }
}
fn as_vector(&self) -> &SVector<Real, SPATIAL_DIM> {
unsafe { std::mem::transmute(self) }
}
}
#[cfg(feature = "dim2")]
fn concat_rb_mass_matrix(
